Merge branch 'dev' of pixhawk.ethz.ch:qgroundcontrol into dev

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<QVector3D>
-Freenect::getPointCloudData(void)
+Freenect::get3DPointCloudData(void)
 {
     QMutexLocker locker(&depthMutex);
 
@@ -200,6 +215,49 @@ Freenect::getPointCloudData(void)
     return pointCloud;
 }
 
+QVector<Freenect::Vector6D>
+Freenect::get6DPointCloudData(void)
+{
+    QVector<QVector3D> rawPointCloud = get3DPointCloudData();
+
+    QVector<Freenect::Vector6D> 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<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];
+
+            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)

src/input/Freenect.h

@@ -2,6 +2,7 @@
 #define FREENECT_H
 
 #include <libfreenect/libfreenect.h>
+#include <QMatrix4x4>
 #include <QMutex>
 #include <QScopedPointer>
 #include <QSharedPointer>
@@ -22,7 +23,18 @@ public:
     QSharedPointer<QByteArray> getRgbData(void);
     QSharedPointer<QByteArray> getRawDepthData(void);
     QSharedPointer<QByteArray> getColoredDepthData(void);
-    QVector<QVector3D> getPointCloudData(void);
+    QVector<QVector3D> get3DPointCloudData(void);
+
+    typedef struct
+    {
+        double x;
+        double y;
+        double z;
+        unsigned char r;
+        unsigned char g;
+        unsigned char b;
+    } Vector6D;
+    QVector<Vector6D> 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

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<osg::Vec4Array*>(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<int>(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<int>(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)

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<QByteArray> rgb;
     QSharedPointer<QByteArray> coloredDepth;
-    QVector<QVector3D> pointCloud;
+    QVector<Freenect::Vector6D> pointCloud;
 
     QVector< osg::ref_ptr<osg::Node> > vehicleModels;