Added visualization of point cloud data from Kinect camera. Colorized point...

Added visualization of point cloud data from Kinect camera. Colorized point cloud visualization to follow soon.

qgroundcontrol.pri

@@ -189,7 +189,7 @@ linux-g++ {
     }
 
     exists(/usr/local/include/libfreenect) {
-    message("Building suplocport for libfreenect")
+    message("Building support for libfreenect")
     DEPENDENCIES_PRESENT += libfreenect
     INCLUDEPATH += /usr/include/libusb-1.0
     # Include libfreenect libraries

src/input/Freenect.cc

@@ -9,12 +9,51 @@ Freenect::Freenect()
     , device(NULL)
     , tiltAngle(0)
 {
+    // default rgb camera parameters
+    rgbCameraParameters.cx = 3.2894272028759258e+02;
+    rgbCameraParameters.cy = 2.6748068171871557e+02;
+    rgbCameraParameters.fx = 5.2921508098293293e+02;
+    rgbCameraParameters.fy = 5.2556393630057437e+02;
+    rgbCameraParameters.k[0] = 2.6451622333009589e-01;
+    rgbCameraParameters.k[1] = -8.3990749424620825e-01;
+    rgbCameraParameters.k[2] = -1.9922302173693159e-03;
+    rgbCameraParameters.k[3] = 1.4371995932897616e-03;
+    rgbCameraParameters.k[4] = 9.1192465078713847e-01;
+
+    // default depth camera parameters
+    depthCameraParameters.cx = 3.3930780975300314e+02;
+    depthCameraParameters.cy = 2.4273913761751615e+02;
+    depthCameraParameters.fx = 5.9421434211923247e+02;
+    depthCameraParameters.fy = 5.9104053696870778e+02;
+    depthCameraParameters.k[0] = -2.6386489753128833e-01;
+    depthCameraParameters.k[1] = 9.9966832163729757e-01;
+    depthCameraParameters.k[2] = -7.6275862143610667e-04;
+    depthCameraParameters.k[3] = 5.0350940090814270e-03;
+    depthCameraParameters.k[4] = -1.3053628089976321e+00;
+
+    // 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;
+        }
+    }
 }
 
 Freenect::~Freenect()
@@ -120,15 +159,6 @@ Freenect::getRawDepthData(void)
             new QByteArray(depth, FREENECT_DEPTH_SIZE));
 }
 
-QSharedPointer<QByteArray>
-Freenect::getDistanceData(void)
-{
-    QMutexLocker locker(&distanceMutex);
-    return QSharedPointer<QByteArray>(
-            new QByteArray(reinterpret_cast<char *>(distance),
-                           FREENECT_FRAME_PIX * sizeof(float)));
-}
-
 QSharedPointer<QByteArray>
 Freenect::getColoredDepthData(void)
 {
@@ -137,6 +167,34 @@ Freenect::getColoredDepthData(void)
             new QByteArray(coloredDepth, FREENECT_RGB_SIZE));
 }
 
+QVector<QVector3D>
+Freenect::getPointCloudData(void)
+{
+    QMutexLocker locker(&depthMutex);
+
+    QVector<QVector3D> pointCloud;
+
+    unsigned short* data = reinterpret_cast<unsigned short*>(depth);
+    for (int i = 0; i < FREENECT_FRAME_PIX; ++i)
+    {
+        if (data[i] <= 2048)
+        {
+            // see www.ros.org/wiki/kinect_node for details
+            double range = 1.0f / (-0.00307f * static_cast<float>(data[i]) + 3.33f);
+
+            if (range > 0.0f)
+            {
+                QVector3D ray = depthProjectionMatrix[i];
+                ray *= range;
+
+                pointCloud.push_back(QVector3D(ray.x(), ray.y(), ray.z()));
+            }
+        }
+    }
+
+    return pointCloud;
+}
+
 int
 Freenect::getTiltAngle(void) const
 {
@@ -173,6 +231,43 @@ Freenect::FreenectThread::run(void)
     }
 }
 
+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::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::rgbCallback(freenect_device* device, freenect_pixel* rgb, uint32_t timestamp)
 {
@@ -183,7 +278,7 @@ Freenect::rgbCallback(freenect_device* device, freenect_pixel* rgb, uint32_t tim
 }
 
 void
-Freenect::depthCallback(freenect_device* device, freenect_depth* depth, uint32_t timestamp)
+Freenect::depthCallback(freenect_device* device, void* depth, uint32_t timestamp)
 {
     Freenect* freenect = static_cast<Freenect *>(freenect_get_user(device));
     freenect_depth* data = reinterpret_cast<freenect_depth *>(depth);
@@ -191,13 +286,6 @@ Freenect::depthCallback(freenect_device* device, freenect_depth* depth, uint32_t
     QMutexLocker depthLocker(&freenect->depthMutex);
     memcpy(freenect->depth, data, FREENECT_DEPTH_SIZE);
 
-    QMutexLocker distanceLocker(&freenect->distanceMutex);
-    for (int i = 0; i < FREENECT_FRAME_PIX; ++i)
-    {
-        freenect->distance[i] =
-                100.f / (-0.00307f * static_cast<float>(data[i]) + 3.33f);
-    }
-
     QMutexLocker coloredDepthLocker(&freenect->coloredDepthMutex);
     unsigned short* src = reinterpret_cast<unsigned short *>(data);
     unsigned char* dst = reinterpret_cast<unsigned char *>(freenect->coloredDepth);

src/input/Freenect.h

@@ -6,6 +6,8 @@
 #include <QScopedPointer>
 #include <QSharedPointer>
 #include <QThread>
+#include <QVector2D>
+#include <QVector3D>
 
 class Freenect
 {
@@ -18,15 +20,34 @@ public:
 
     QSharedPointer<QByteArray> getRgbData(void);
     QSharedPointer<QByteArray> getRawDepthData(void);
-    QSharedPointer<QByteArray> getDistanceData(void);
     QSharedPointer<QByteArray> getColoredDepthData(void);
+    QVector<QVector3D> getPointCloudData(void);
 
     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 rectifyPoint(const QVector2D& originalPoint, QVector2D& rectifiedPoint,
+                      const IntrinsicCameraParameters& params);
+    void projectPixelTo3DRay(const QVector2D& pixel, QVector3D& ray,
+                             const IntrinsicCameraParameters& params);
+
     static void rgbCallback(freenect_device* device, freenect_pixel* rgb, uint32_t timestamp);
-    static void depthCallback(freenect_device* device, freenect_depth* depth, uint32_t timestamp);
+    static void depthCallback(freenect_device* device, void* depth, uint32_t timestamp);
 
     freenect_context* context;
     freenect_device* device;
@@ -43,6 +64,9 @@ private:
     };
     QScopedPointer<FreenectThread> thread;
 
+    IntrinsicCameraParameters rgbCameraParameters;
+    IntrinsicCameraParameters depthCameraParameters;
+
     // tilt angle of Kinect camera
     int tiltAngle;
 
@@ -53,9 +77,6 @@ private:
     char depth[FREENECT_DEPTH_SIZE];
     QMutex depthMutex;
 
-    float distance[FREENECT_FRAME_PIX];
-    QMutex distanceMutex;
-
     char coloredDepth[FREENECT_RGB_SIZE];
     QMutex coloredDepthMutex;
 
@@ -65,6 +86,8 @@ private:
 
     // gamma map
     unsigned short gammaTable[2048];
+
+    QVector3D depthProjectionMatrix[FREENECT_FRAME_PIX];
 };
 
 #endif // FREENECT_H

src/ui/map3D/Pixhawk3DWidget.cc

@@ -224,8 +224,7 @@ Pixhawk3DWidget::findVehicleModels(void)
 
         if (node)
         {
-
-        nodes.push_back(node);
+            nodes.push_back(node);
         }
         else
         {
@@ -809,12 +808,175 @@ Pixhawk3DWidget::updateWaypoints(void)
     }
 }
 
+float colormap_jet[128][3] =
+{
+                {0.0f,0.0f,0.53125f},
+                {0.0f,0.0f,0.5625f},
+                {0.0f,0.0f,0.59375f},
+                {0.0f,0.0f,0.625f},
+                {0.0f,0.0f,0.65625f},
+                {0.0f,0.0f,0.6875f},
+                {0.0f,0.0f,0.71875f},
+                {0.0f,0.0f,0.75f},
+                {0.0f,0.0f,0.78125f},
+                {0.0f,0.0f,0.8125f},
+                {0.0f,0.0f,0.84375f},
+                {0.0f,0.0f,0.875f},
+                {0.0f,0.0f,0.90625f},
+                {0.0f,0.0f,0.9375f},
+                {0.0f,0.0f,0.96875f},
+                {0.0f,0.0f,1.0f},
+                {0.0f,0.03125f,1.0f},
+                {0.0f,0.0625f,1.0f},
+                {0.0f,0.09375f,1.0f},
+                {0.0f,0.125f,1.0f},
+                {0.0f,0.15625f,1.0f},
+                {0.0f,0.1875f,1.0f},
+                {0.0f,0.21875f,1.0f},
+                {0.0f,0.25f,1.0f},
+                {0.0f,0.28125f,1.0f},
+                {0.0f,0.3125f,1.0f},
+                {0.0f,0.34375f,1.0f},
+                {0.0f,0.375f,1.0f},
+                {0.0f,0.40625f,1.0f},
+                {0.0f,0.4375f,1.0f},
+                {0.0f,0.46875f,1.0f},
+                {0.0f,0.5f,1.0f},
+                {0.0f,0.53125f,1.0f},
+                {0.0f,0.5625f,1.0f},
+                {0.0f,0.59375f,1.0f},
+                {0.0f,0.625f,1.0f},
+                {0.0f,0.65625f,1.0f},
+                {0.0f,0.6875f,1.0f},
+                {0.0f,0.71875f,1.0f},
+                {0.0f,0.75f,1.0f},
+                {0.0f,0.78125f,1.0f},
+                {0.0f,0.8125f,1.0f},
+                {0.0f,0.84375f,1.0f},
+                {0.0f,0.875f,1.0f},
+                {0.0f,0.90625f,1.0f},
+                {0.0f,0.9375f,1.0f},
+                {0.0f,0.96875f,1.0f},
+                {0.0f,1.0f,1.0f},
+                {0.03125f,1.0f,0.96875f},
+                {0.0625f,1.0f,0.9375f},
+                {0.09375f,1.0f,0.90625f},
+                {0.125f,1.0f,0.875f},
+                {0.15625f,1.0f,0.84375f},
+                {0.1875f,1.0f,0.8125f},
+                {0.21875f,1.0f,0.78125f},
+                {0.25f,1.0f,0.75f},
+                {0.28125f,1.0f,0.71875f},
+                {0.3125f,1.0f,0.6875f},
+                {0.34375f,1.0f,0.65625f},
+                {0.375f,1.0f,0.625f},
+                {0.40625f,1.0f,0.59375f},
+                {0.4375f,1.0f,0.5625f},
+                {0.46875f,1.0f,0.53125f},
+                {0.5f,1.0f,0.5f},
+                {0.53125f,1.0f,0.46875f},
+                {0.5625f,1.0f,0.4375f},
+                {0.59375f,1.0f,0.40625f},
+                {0.625f,1.0f,0.375f},
+                {0.65625f,1.0f,0.34375f},
+                {0.6875f,1.0f,0.3125f},
+                {0.71875f,1.0f,0.28125f},
+                {0.75f,1.0f,0.25f},
+                {0.78125f,1.0f,0.21875f},
+                {0.8125f,1.0f,0.1875f},
+                {0.84375f,1.0f,0.15625f},
+                {0.875f,1.0f,0.125f},
+                {0.90625f,1.0f,0.09375f},
+                {0.9375f,1.0f,0.0625f},
+                {0.96875f,1.0f,0.03125f},
+                {1.0f,1.0f,0.0f},
+                {1.0f,0.96875f,0.0f},
+                {1.0f,0.9375f,0.0f},
+                {1.0f,0.90625f,0.0f},
+                {1.0f,0.875f,0.0f},
+                {1.0f,0.84375f,0.0f},
+                {1.0f,0.8125f,0.0f},
+                {1.0f,0.78125f,0.0f},
+                {1.0f,0.75f,0.0f},
+                {1.0f,0.71875f,0.0f},
+                {1.0f,0.6875f,0.0f},
+                {1.0f,0.65625f,0.0f},
+                {1.0f,0.625f,0.0f},
+                {1.0f,0.59375f,0.0f},
+                {1.0f,0.5625f,0.0f},
+                {1.0f,0.53125f,0.0f},
+                {1.0f,0.5f,0.0f},
+                {1.0f,0.46875f,0.0f},
+                {1.0f,0.4375f,0.0f},
+                {1.0f,0.40625f,0.0f},
+                {1.0f,0.375f,0.0f},
+                {1.0f,0.34375f,0.0f},
+                {1.0f,0.3125f,0.0f},
+                {1.0f,0.28125f,0.0f},
+                {1.0f,0.25f,0.0f},
+                {1.0f,0.21875f,0.0f},
+                {1.0f,0.1875f,0.0f},
+                {1.0f,0.15625f,0.0f},
+                {1.0f,0.125f,0.0f},
+                {1.0f,0.09375f,0.0f},
+                {1.0f,0.0625f,0.0f},
+                {1.0f,0.03125f,0.0f},
+                {1.0f,0.0f,0.0f},
+                {0.96875f,0.0f,0.0f},
+                {0.9375f,0.0f,0.0f},
+                {0.90625f,0.0f,0.0f},
+                {0.875f,0.0f,0.0f},
+                {0.84375f,0.0f,0.0f},
+                {0.8125f,0.0f,0.0f},
+                {0.78125f,0.0f,0.0f},
+                {0.75f,0.0f,0.0f},
+                {0.71875f,0.0f,0.0f},
+                {0.6875f,0.0f,0.0f},
+                {0.65625f,0.0f,0.0f},
+                {0.625f,0.0f,0.0f},
+                {0.59375f,0.0f,0.0f},
+                {0.5625f,0.0f,0.0f},
+                {0.53125f,0.0f,0.0f},
+                {0.5f,0.0f,0.0f}
+};
+
 #ifdef QGC_LIBFREENECT_ENABLED
 void
 Pixhawk3DWidget::updateRGBD(void)
 {
     rgb = freenect->getRgbData();
     coloredDepth = freenect->getColoredDepthData();
+    pointCloud = freenect->getPointCloudData();
+
+    osg::Geometry* geometry = rgbd3DNode->getDrawable(0)->asGeometry();
+
+    osg::Vec3Array* vertices = static_cast<osg::Vec3Array*>(geometry->getVertexArray());
+    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();
+        (*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 (geometry->getNumPrimitiveSets() == 0)
+    {
+        geometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::POINTS,
+                                                      0, pointCloud.size()));
+    }
+    else
+    {
+        osg::DrawArrays* drawarrays = static_cast<osg::DrawArrays*>(geometry->getPrimitiveSet(0));
+        drawarrays->setCount(pointCloud.size());
+    }
 }
 #endif
 

src/ui/map3D/Pixhawk3DWidget.h

@@ -139,6 +139,7 @@ private:
 #endif
     QSharedPointer<QByteArray> rgb;
     QSharedPointer<QByteArray> coloredDepth;
+    QVector<QVector3D> pointCloud;
 
     QVector< osg::ref_ptr<osg::Node> > vehicleModels;