diff --git a/src/input/Freenect.cc b/src/input/Freenect.cc index 55951f94ddd21d7cdc899c6fb7315ef1013acb21..0b5c3edbf4cc322b405bf943506cf1cb1780c96d 100644 --- a/src/input/Freenect.cc +++ b/src/input/Freenect.cc @@ -30,6 +30,18 @@ Freenect::Freenect() depthCameraParameters.k[3] = 5.0350940090814270e-03; depthCameraParameters.k[4] = -1.3053628089976321e+00; + // relative rotation/translation between cameras with depth camera as reference + transformMatrix = QMatrix4x4(9.9984628826577793e-01, 1.2635359098409581e-03, + -1.7487233004436643e-02, 1.9985242312092553e-02, + -1.4779096108364480e-03, 9.9992385683542895e-01, + -1.2251380107679535e-02, -7.4423738761617583e-04, + 1.7470421412464927e-02, 1.2275341476520762e-02, + 9.9977202419716948e-01, -1.0916736334336222e-02, + 0.0, 0.0, 0.0, 1.0); + + // relative rotation/translation between cameras with rgb camera as reference + transformMatrix = transformMatrix.transposed(); + // populate gamma lookup table for (int i = 0; i < 2048; ++i) { @@ -51,6 +63,9 @@ Freenect::Freenect() projectPixelTo3DRay(rectifiedPoint, rectifiedRay, depthCameraParameters); depthProjectionMatrix[i * FREENECT_FRAME_W + j] = rectifiedRay; + + rectifyPoint(originalPoint, rectifiedPoint, rgbCameraParameters); + rgbRectificationMap[i * FREENECT_FRAME_W + j] = rectifiedPoint; } } } @@ -173,7 +188,7 @@ Freenect::getColoredDepthData(void) } QVector -Freenect::getPointCloudData(void) +Freenect::get3DPointCloudData(void) { QMutexLocker locker(&depthMutex); @@ -200,6 +215,49 @@ Freenect::getPointCloudData(void) return pointCloud; } +QVector +Freenect::get6DPointCloudData(void) +{ + QVector rawPointCloud = get3DPointCloudData(); + + QVector pointCloud; + + for (int i = 0; i < rawPointCloud.size(); ++i) + { + Vector6D point; + + point.x = rawPointCloud[i].x(); + point.y = rawPointCloud[i].y(); + point.z = rawPointCloud[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(originalPoint.x()); + int y = static_cast(originalPoint.y()); + unsigned char* pixel = reinterpret_cast(rgb) + + (y * FREENECT_FRAME_W + x) * 3; + + point.r = pixel[0]; + point.g = pixel[1]; + point.b = pixel[2]; + + pointCloud.push_back(point); + } + } + + return pointCloud; +} + int Freenect::getTiltAngle(void) const { @@ -237,7 +295,8 @@ Freenect::FreenectThread::run(void) } void -Freenect::rectifyPoint(const QVector2D& originalPoint, QVector2D& rectifiedPoint, +Freenect::rectifyPoint(const QVector2D& originalPoint, + QVector2D& rectifiedPoint, const IntrinsicCameraParameters& params) { double x = (originalPoint.x() - params.cx) / params.fx; @@ -264,6 +323,28 @@ Freenect::rectifyPoint(const QVector2D& originalPoint, QVector2D& rectifiedPoint 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) diff --git a/src/input/Freenect.h b/src/input/Freenect.h index 7424addd1c14eb259d18044d21a4b0c32b6b12b7..216b4d49c8c4830f5e01d8558e73b0d5f7ed6854 100644 --- a/src/input/Freenect.h +++ b/src/input/Freenect.h @@ -2,6 +2,7 @@ #define FREENECT_H #include +#include #include #include #include @@ -22,7 +23,18 @@ public: QSharedPointer getRgbData(void); QSharedPointer getRawDepthData(void); QSharedPointer getColoredDepthData(void); - QVector getPointCloudData(void); + QVector get3DPointCloudData(void); + + typedef struct + { + double x; + double y; + double z; + unsigned char r; + unsigned char g; + unsigned char b; + } Vector6D; + QVector get6DPointCloudData(void); int getTiltAngle(void) const; void setTiltAngle(int angle); @@ -43,8 +55,12 @@ private: } IntrinsicCameraParameters; - void rectifyPoint(const QVector2D& originalPoint, QVector2D& rectifiedPoint, + 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); @@ -69,6 +85,8 @@ private: IntrinsicCameraParameters rgbCameraParameters; IntrinsicCameraParameters depthCameraParameters; + QMatrix4x4 transformMatrix; + // tilt angle of Kinect camera int tiltAngle; @@ -90,6 +108,7 @@ private: unsigned short gammaTable[2048]; QVector3D depthProjectionMatrix[FREENECT_FRAME_PIX]; + QVector2D rgbRectificationMap[FREENECT_FRAME_PIX]; }; #endif // FREENECT_H diff --git a/src/ui/map3D/Pixhawk3DWidget.cc b/src/ui/map3D/Pixhawk3DWidget.cc index 8a15cde4de99db874c70df04e58cecd85b7b49c2..c9350784ce7dbd5f6e5e6e7c5e45887f7b6e7ff1 100644 --- a/src/ui/map3D/Pixhawk3DWidget.cc +++ b/src/ui/map3D/Pixhawk3DWidget.cc @@ -53,6 +53,7 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent) , displayWaypoints(true) , displayRGBD2D(false) , displayRGBD3D(false) + , enableRGBDColor(true) , followCamera(true) , lastRobotX(0.0f) , lastRobotY(0.0f) @@ -228,7 +229,7 @@ Pixhawk3DWidget::findVehicleModels(void) } else { - printf(QString("ERROR: Could not load file " + directory.absoluteFilePath(files[i]) + "\n").toStdString().c_str()); + printf("%s\n", QString("ERROR: Could not load file " + directory.absoluteFilePath(files[i]) + "\n").toStdString().c_str()); } } @@ -396,6 +397,9 @@ Pixhawk3DWidget::keyPressEvent(QKeyEvent* event) case '2': displayRGBD3D = !displayRGBD3D; break; + case 'c': case 'C': + enableRGBDColor = !enableRGBDColor; + break; } } @@ -946,7 +950,7 @@ Pixhawk3DWidget::updateRGBD(void) { rgb = freenect->getRgbData(); coloredDepth = freenect->getColoredDepthData(); - pointCloud = freenect->getPointCloudData(); + pointCloud = freenect->get6DPointCloudData(); osg::Geometry* geometry = rgbd3DNode->getDrawable(0)->asGeometry(); @@ -954,17 +958,27 @@ Pixhawk3DWidget::updateRGBD(void) osg::Vec4Array* colors = static_cast(geometry->getColorArray()); for (int i = 0; i < pointCloud.size(); ++i) { - double x = pointCloud[i].x(); - double y = pointCloud[i].y(); - double z = pointCloud[i].z(); + double x = pointCloud[i].x; + double y = pointCloud[i].y; + double z = pointCloud[i].z; (*vertices)[i].set(x, z, -y); - double dist = sqrt(x * x + y * y + z * z); - int colorIndex = static_cast(fmin(dist / 7.0 * 127.0, 127.0)); - (*colors)[i].set(colormap_jet[colorIndex][0], - colormap_jet[colorIndex][1], - colormap_jet[colorIndex][2], - 1.0f); + if (enableRGBDColor) + { + (*colors)[i].set(pointCloud[i].r / 255.0f, + pointCloud[i].g / 255.0f, + pointCloud[i].b / 255.0f, + 1.0f); + } + else + { + double dist = sqrt(x * x + y * y + z * z); + int colorIndex = static_cast(fmin(dist / 7.0 * 127.0, 127.0)); + (*colors)[i].set(colormap_jet[colorIndex][0], + colormap_jet[colorIndex][1], + colormap_jet[colorIndex][2], + 1.0f); + } } if (geometry->getNumPrimitiveSets() == 0) diff --git a/src/ui/map3D/Pixhawk3DWidget.h b/src/ui/map3D/Pixhawk3DWidget.h index 0d8074093da1fd326aaa5c66e1f5062bec56a97a..fe6073fc70fc0e6a0fa844137607b5e608c3aeb0 100644 --- a/src/ui/map3D/Pixhawk3DWidget.h +++ b/src/ui/map3D/Pixhawk3DWidget.h @@ -110,6 +110,7 @@ private: bool displayWaypoints; bool displayRGBD2D; bool displayRGBD3D; + bool enableRGBDColor; bool followCamera; @@ -139,7 +140,7 @@ private: #endif QSharedPointer rgb; QSharedPointer coloredDepth; - QVector pointCloud; + QVector pointCloud; QVector< osg::ref_ptr > vehicleModels;