Skip to content
GitLab
Commit 56473f8c authored by Bryant's avatar Bryant
Browse files

Remove last remnants of Kinect support.

parent a72573de
Hide whitespace changes
Inline Side-by-side
data/kinect.cal deleted 100644 → 0
View file @ a72573de
[rgb]
principal_point\x=314.70228964
principal_point\y=264.30478827
focal_length\x=527.91246131
focal_length\y=527.91246131
distortion\k1=0.20496745
distortion\k2=-0.36341243
distortion\k3=0.00000000
distortion\k4=0.00000000
distortion\k5=0.00000000
[depth]
principal_point\x=311.88621344
principal_point\y=247.63447078
focal_length\x=593.89813561
focal_length\y=593.89813561
distortion\k1=0.00000000
distortion\k2=0.00000000
distortion\k3=0.00000000
distortion\k4=0.00000000
distortion\k5=0.00000000
[transform]
R11=0.999982
R12=0.000556
R13=0.005927
R21=-0.000563
R22=0.999999
R23=0.001235
R31=-0.005926
R32=-0.001239
R33=0.999982
Tx=-0.024287
Ty=0.001018
Tz=-0.015195
baseline=0.06061
disparity_offset=1092.3403
src/input/Freenect.cc deleted 100644 → 0
View file @ a72573de
/*=====================================================================
QGroundControl Open Source Ground Control Station
(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
This file is part of the QGROUNDCONTROL project
QGROUNDCONTROL is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
QGROUNDCONTROL is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
======================================================================*/
/**
* @file
* @brief Definition of the class Freenect.
*
* @author Lionel Heng <hengli@student.ethz.ch>
*
*/
#include "Freenect.h"
#include <cmath>
#include <string.h>
#include <QSettings>
Freenect::Freenect()
: context(NULL)
, device(NULL)
, tiltAngle(0)
, rgbData(new QByteArray)
, rawDepthData(new QByteArray)
, coloredDepthData(new QByteArray)
, pointCloud3D(new QVector<QVector3D>)
, pointCloud6D(new QVector<Vector6D>)
{
}
Freenect::~Freenect()
{
if (device != NULL) {
freenect_stop_depth(device);
freenect_stop_video(device);
}
freenect_close_device(device);
freenect_shutdown(context);
}
bool
Freenect::init(int userDeviceNumber)
{
// read in settings
readConfigFile();
// populate gamma lookup table
for (int i = 0; i < 2048; ++i) {
float v = static_cast<float>(i) / 2048.0f;
v = powf(v, 3.0f) * 6.0f;
gammaTable[i] = static_cast<unsigned short>(v * 6.0f * 256.0f);
}
// populate depth projection matrix
for (int i = 0; i < FREENECT_FRAME_H; ++i) {
for (int j = 0; j < FREENECT_FRAME_W; ++j) {
QVector2D originalPoint(j, i);
QVector2D rectifiedPoint;
rectifyPoint(originalPoint, rectifiedPoint, depthCameraParameters);
QVector3D rectifiedRay;
projectPixelTo3DRay(rectifiedPoint, rectifiedRay, depthCameraParameters);
depthProjectionMatrix[i * FREENECT_FRAME_W + j] = rectifiedRay;
rectifyPoint(originalPoint, rectifiedPoint, rgbCameraParameters);
rgbRectificationMap[i * FREENECT_FRAME_W + j] = rectifiedPoint;
}
}
if (freenect_init(&context, NULL) < 0) {
return false;
}
freenect_set_log_level(context, FREENECT_LOG_DEBUG);
if (freenect_num_devices(context) < 1) {
return false;
}
if (freenect_open_device(context, &device, userDeviceNumber) < 0) {
return false;
}
freenect_set_user(device, this);
memset(rgb, 0, FREENECT_VIDEO_RGB_SIZE);
memset(depth, 0, FREENECT_DEPTH_11BIT_SIZE);
// set Kinect parameters
if (freenect_set_tilt_degs(device, tiltAngle) != 0) {
return false;
}
if (freenect_set_led(device, LED_RED) != 0) {
return false;
}
if (freenect_set_video_format(device, FREENECT_VIDEO_RGB) != 0) {
return false;
}
if (freenect_set_depth_format(device, FREENECT_DEPTH_11BIT) != 0) {
return false;
}
freenect_set_video_callback(device, videoCallback);
freenect_set_depth_callback(device, depthCallback);
if (freenect_start_depth(device) != 0) {
return false;
}
if (freenect_start_video(device) != 0) {
return false;
}
thread.reset(new FreenectThread(device));
thread->start();
return true;
}
bool
Freenect::process(void)
{
if (freenect_process_events(context) < 0) {
return false;
}
freenect_raw_tilt_state* state;
freenect_update_tilt_state(device);
state = freenect_get_tilt_state(device);
freenect_get_mks_accel(state, &ax, &ay, &az);
return true;
}
QSharedPointer<QByteArray>
Freenect::getRgbData(void)
{
QMutexLocker locker(&rgbMutex);
rgbData->clear();
rgbData->append(rgb, FREENECT_VIDEO_RGB_SIZE);
return rgbData;
}
QSharedPointer<QByteArray>
Freenect::getRawDepthData(void)
{
QMutexLocker locker(&depthMutex);
rawDepthData->clear();
rawDepthData->append(depth, FREENECT_DEPTH_11BIT_SIZE);
return rawDepthData;
}
QSharedPointer<QByteArray>
Freenect::getColoredDepthData(void)
{
QMutexLocker locker(&coloredDepthMutex);
coloredDepthData->clear();
coloredDepthData->append(coloredDepth, FREENECT_VIDEO_RGB_SIZE);
return coloredDepthData;
}
QSharedPointer< QVector<QVector3D> >
Freenect::get3DPointCloudData(void)
{
QMutexLocker locker(&depthMutex);
pointCloud3D->clear();
unsigned short* data = reinterpret_cast<unsigned short*>(depth);
for (int i = 0; i < FREENECT_FRAME_PIX; ++i) {
if (data[i] > 0 && data[i] <= 2048) {
double range = baseline * depthCameraParameters.fx
/ (1.0 / 8.0 * (disparityOffset
- static_cast<double>(data[i])));
if (range > 0.0f) {
QVector3D ray = depthProjectionMatrix[i];
ray *= range;
pointCloud3D->push_back(QVector3D(ray.x(), ray.y(), ray.z()));
}
}
}
return pointCloud3D;
}
QSharedPointer< QVector<Freenect::Vector6D> >
Freenect::get6DPointCloudData(void)
{
get3DPointCloudData();
pointCloud6D->clear();
for (int i = 0; i < pointCloud3D->size(); ++i) {
Vector6D point;
point.x = pointCloud3D->at(i).x();
point.y = pointCloud3D->at(i).y();
point.z = pointCloud3D->at(i).z();
QVector4D transformedPoint = transformMatrix * QVector4D(point.x, point.y, point.z, 1.0);
float iz = 1.0 / transformedPoint.z();
QVector2D rectifiedPoint(transformedPoint.x() * iz * rgbCameraParameters.fx + rgbCameraParameters.cx,
transformedPoint.y() * iz * rgbCameraParameters.fy + rgbCameraParameters.cy);
QVector2D originalPoint;
unrectifyPoint(rectifiedPoint, originalPoint, rgbCameraParameters);
if (originalPoint.x() >= 0.0 && originalPoint.x() < FREENECT_FRAME_W &&
originalPoint.y() >= 0.0 && originalPoint.y() < FREENECT_FRAME_H) {
int x = static_cast<int>(originalPoint.x());
int y = static_cast<int>(originalPoint.y());
unsigned char* pixel = reinterpret_cast<unsigned char*>(rgb)
+ (y * FREENECT_FRAME_W + x) * 3;
point.r = pixel[0];
point.g = pixel[1];
point.b = pixel[2];
pointCloud6D->push_back(point);
}
}
return pointCloud6D;
}
int
Freenect::getTiltAngle(void) const
{
return tiltAngle;
}
void
Freenect::setTiltAngle(int angle)
{
if (angle > 30) {
angle = 30;
}
if (angle < -30) {
angle = -30;
}
tiltAngle = angle;
}
Freenect::FreenectThread::FreenectThread(freenect_device* _device)
{
device = _device;
}
void
Freenect::FreenectThread::run(void)
{
Freenect* freenect = static_cast<Freenect *>(freenect_get_user(device));
while (1) {
freenect->process();
}
}
void
Freenect::readConfigFile(void)
{
QSettings settings("src/data/kinect.cal", QSettings::IniFormat, 0);
rgbCameraParameters.cx = settings.value("rgb/principal_point/x").toDouble();
rgbCameraParameters.cy = settings.value("rgb/principal_point/y").toDouble();
rgbCameraParameters.fx = settings.value("rgb/focal_length/x").toDouble();
rgbCameraParameters.fy = settings.value("rgb/focal_length/y").toDouble();
rgbCameraParameters.k[0] = settings.value("rgb/distortion/k1").toDouble();
rgbCameraParameters.k[1] = settings.value("rgb/distortion/k2").toDouble();
rgbCameraParameters.k[2] = settings.value("rgb/distortion/k3").toDouble();
rgbCameraParameters.k[3] = settings.value("rgb/distortion/k4").toDouble();
rgbCameraParameters.k[4] = settings.value("rgb/distortion/k5").toDouble();
depthCameraParameters.cx = settings.value("depth/principal_point/x").toDouble();
depthCameraParameters.cy = settings.value("depth/principal_point/y").toDouble();
depthCameraParameters.fx = settings.value("depth/focal_length/x").toDouble();
depthCameraParameters.fy = settings.value("depth/focal_length/y").toDouble();
depthCameraParameters.k[0] = settings.value("depth/distortion/k1").toDouble();
depthCameraParameters.k[1] = settings.value("depth/distortion/k2").toDouble();
depthCameraParameters.k[2] = settings.value("depth/distortion/k3").toDouble();
depthCameraParameters.k[3] = settings.value("depth/distortion/k4").toDouble();
depthCameraParameters.k[4] = settings.value("depth/distortion/k5").toDouble();
transformMatrix = QMatrix4x4(settings.value("transform/R11").toDouble(),
settings.value("transform/R12").toDouble(),
settings.value("transform/R13").toDouble(),
settings.value("transform/Tx").toDouble(),
settings.value("transform/R21").toDouble(),
settings.value("transform/R22").toDouble(),
settings.value("transform/R23").toDouble(),
settings.value("transform/Ty").toDouble(),
settings.value("transform/R31").toDouble(),
settings.value("transform/R32").toDouble(),
settings.value("transform/R33").toDouble(),
settings.value("transform/Tz").toDouble(),
0.0, 0.0, 0.0, 1.0);
transformMatrix = transformMatrix.inverted();
baseline = settings.value("transform/baseline").toDouble();
disparityOffset = settings.value("transform/disparity_offset").toDouble();
}
void
Freenect::rectifyPoint(const QVector2D& originalPoint,
QVector2D& rectifiedPoint,
const IntrinsicCameraParameters& params)
{
double x = (originalPoint.x() - params.cx) / params.fx;
double y = (originalPoint.y() - params.cy) / params.fy;
double x0 = x;
double y0 = y;
// eliminate lens distortion iteratively
for (int i = 0; i < 4; ++i) {
double r2 = x * x + y * y;
// tangential distortion vector [dx dy]
double dx = 2 * params.k[2] * x * y + params.k[3] * (r2 + 2 * x * x);
double dy = params.k[2] * (r2 + 2 * y * y) + 2 * params.k[3] * x * y;
double icdist = 1.0 / (1.0 + r2 * (params.k[0] + r2 * (params.k[1] + r2 * params.k[4])));
x = (x0 - dx) * icdist;
y = (y0 - dy) * icdist;
}
rectifiedPoint.setX(x * params.fx + params.cx);
rectifiedPoint.setY(y * params.fy + params.cy);
}
void
Freenect::unrectifyPoint(const QVector2D& rectifiedPoint,
QVector2D& originalPoint,
const IntrinsicCameraParameters& params)
{
double x = (rectifiedPoint.x() - params.cx) / params.fx;
double y = (rectifiedPoint.y() - params.cy) / params.fy;
double r2 = x * x + y * y;
// tangential distortion vector [dx dy]
double dx = 2 * params.k[2] * x * y + params.k[3] * (r2 + 2 * x * x);
double dy = params.k[2] * (r2 + 2 * y * y) + 2 * params.k[3] * x * y;
double cdist = 1.0 + r2 * (params.k[0] + r2 * (params.k[1] + r2 * params.k[4]));
x = x * cdist + dx;
y = y * cdist + dy;
originalPoint.setX(x * params.fx + params.cx);
originalPoint.setY(y * params.fy + params.cy);
}
void
Freenect::projectPixelTo3DRay(const QVector2D& pixel, QVector3D& ray,
const IntrinsicCameraParameters& params)
{
ray.setX((pixel.x() - params.cx) / params.fx);
ray.setY((pixel.y() - params.cy) / params.fy);
ray.setZ(1.0);
}
void
Freenect::videoCallback(freenect_device* device, void* video, uint32_t timestamp)
{
Freenect* freenect = static_cast<Freenect *>(freenect_get_user(device));
QMutexLocker locker(&freenect->rgbMutex);
memcpy(freenect->rgb, video, FREENECT_VIDEO_RGB_SIZE);
}
void
Freenect::depthCallback(freenect_device* device, void* depth, uint32_t timestamp)
{
Freenect* freenect = static_cast<Freenect *>(freenect_get_user(device));
uint16_t* data = reinterpret_cast<uint16_t *>(depth);
QMutexLocker depthLocker(&freenect->depthMutex);
memcpy(freenect->depth, data, FREENECT_DEPTH_11BIT_SIZE);
QMutexLocker coloredDepthLocker(&freenect->coloredDepthMutex);
unsigned short* src = reinterpret_cast<unsigned short *>(data);
unsigned char* dst = reinterpret_cast<unsigned char *>(freenect->coloredDepth);
for (int i = 0; i < FREENECT_FRAME_PIX; ++i) {
unsigned short pval = freenect->gammaTable[src[i]];
unsigned short lb = pval & 0xFF;
switch (pval >> 8) {
case 0:
dst[3 * i] = 255;
dst[3 * i + 1] = 255 - lb;
dst[3 * i + 2] = 255 - lb;
break;
case 1:
dst[3 * i] = 255;
dst[3 * i + 1] = lb;
dst[3 * i + 2] = 0;
break;
case 2:
dst[3 * i] = 255 - lb;
dst[3 * i + 1] = 255;
dst[3 * i + 2] = 0;
break;
case 3:
dst[3 * i] = 0;
dst[3 * i + 1] = 255;
dst[3 * i + 2] = lb;
break;
case 4:
dst[3 * i] = 0;
dst[3 * i + 1] = 255 - lb;
dst[3 * i + 2] = 255;
break;
case 5:
dst[3 * i] = 0;
dst[3 * i + 1] = 0;
dst[3 * i + 2] = 255 - lb;
break;
default:
dst[3 * i] = 0;
dst[3 * i + 1] = 0;
dst[3 * i + 2] = 0;
break;
}
}
}
src/input/Freenect.h deleted 100644 → 0
View file @ a72573de
/*=====================================================================
QGroundControl Open Source Ground Control Station
(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
This file is part of the QGROUNDCONTROL project
QGROUNDCONTROL is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
QGROUNDCONTROL is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
======================================================================*/
/**
* @file
* @brief Definition of the class Freenect.
*
* @author Lionel Heng <hengli@student.ethz.ch>
*
*/
#ifndef FREENECT_H
#define FREENECT_H
#include <libfreenect/libfreenect.h>
#include <QMatrix4x4>
#include <QMutex>
#include <QScopedPointer>
#include <QSharedPointer>
#include <QThread>
#include <QVector>
#include <QVector2D>
#include <QVector3D>
class Freenect
{
public:
Freenect();
~Freenect();
bool init(int userDeviceNumber = 0);
bool process(void);
QSharedPointer<QByteArray> getRgbData(void);
QSharedPointer<QByteArray> getRawDepthData(void);
QSharedPointer<QByteArray> getColoredDepthData(void);
QSharedPointer< QVector<QVector3D> > get3DPointCloudData(void);
typedef struct {
double x;
double y;
double z;
unsigned char r;
unsigned char g;
unsigned char b;
} Vector6D;
QSharedPointer< QVector<Vector6D> > get6DPointCloudData();
int getTiltAngle(void) const;
void setTiltAngle(int angle);
private:
typedef struct {
// coordinates of principal point
double cx;
double cy;
// focal length in pixels
double fx;
double fy;
// distortion parameters
double k[5];
} IntrinsicCameraParameters;
void readConfigFile(void);
void rectifyPoint(const QVector2D& originalPoint,
QVector2D& rectifiedPoint,
const IntrinsicCameraParameters& params);
void unrectifyPoint(const QVector2D& rectifiedPoint,
QVector2D& originalPoint,
const IntrinsicCameraParameters& params);
void projectPixelTo3DRay(const QVector2D& pixel, QVector3D& ray,
const IntrinsicCameraParameters& params);
static void videoCallback(freenect_device* device, void* video, uint32_t timestamp);
static void depthCallback(freenect_device* device, void* depth, uint32_t timestamp);
freenect_context* context;
freenect_device* device;
class FreenectThread : public QThread
{
public:
explicit FreenectThread(freenect_device* _device);
protected:
virtual void run(void);
freenect_device* device;
};
QScopedPointer<FreenectThread> thread;
IntrinsicCameraParameters rgbCameraParameters;
IntrinsicCameraParameters depthCameraParameters;
QMatrix4x4 transformMatrix;
double baseline;
double disparityOffset;
// tilt angle of Kinect camera
int tiltAngle;
// rgbd data
char rgb[FREENECT_VIDEO_RGB_SIZE];
QMutex rgbMutex;
char depth[FREENECT_DEPTH_11BIT_SIZE];
QMutex depthMutex;
char coloredDepth[FREENECT_VIDEO_RGB_SIZE];
QMutex coloredDepthMutex;
// accelerometer data
double ax, ay, az;
double dx, dy, dz;
// gamma map
unsigned short gammaTable[2048];
QVector3D depthProjectionMatrix[FREENECT_FRAME_PIX];
QVector2D rgbRectificationMap[FREENECT_FRAME_PIX];
// variables for use outside class
QSharedPointer<QByteArray> rgbData;
QSharedPointer<QByteArray> rawDepthData;
QSharedPointer<QByteArray> coloredDepthData;
QSharedPointer< QVector<QVector3D> > pointCloud3D;
QSharedPointer< QVector<Vector6D> > pointCloud6D;
};
#endif // FREENECT_H
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment