Improved rendering performance for Kinect data.

src/input/Freenect.cc

@@ -39,6 +39,11 @@ 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>)
 {
 
 }
@@ -166,33 +171,38 @@ QSharedPointer<QByteArray>
 Freenect::getRgbData(void)
 {
     QMutexLocker locker(&rgbMutex);
-    return QSharedPointer<QByteArray>(
-            new QByteArray(rgb, FREENECT_VIDEO_RGB_SIZE));
+    rgbData->clear();
+    rgbData->append(rgb, FREENECT_VIDEO_RGB_SIZE);
+
+    return rgbData;
 }
 
 QSharedPointer<QByteArray>
 Freenect::getRawDepthData(void)
 {
     QMutexLocker locker(&depthMutex);
-    return QSharedPointer<QByteArray>(
-            new QByteArray(depth, FREENECT_DEPTH_11BIT_SIZE));
+    rawDepthData->clear();
+    rawDepthData->append(depth, FREENECT_DEPTH_11BIT_SIZE);
+
+    return rawDepthData;
 }
 
 QSharedPointer<QByteArray>
 Freenect::getColoredDepthData(void)
 {
     QMutexLocker locker(&coloredDepthMutex);
-    return QSharedPointer<QByteArray>(
-            new QByteArray(coloredDepth, FREENECT_VIDEO_RGB_SIZE));
+    coloredDepthData->clear();
+    coloredDepthData->append(coloredDepth, FREENECT_VIDEO_RGB_SIZE);
+
+    return coloredDepthData;
 }
 
-QVector<QVector3D>
+QSharedPointer< QVector<QVector3D> >
 Freenect::get3DPointCloudData(void)
 {
     QMutexLocker locker(&depthMutex);
 
-    QVector<QVector3D> pointCloud;
-
+    pointCloud3D->clear();
     unsigned short* data = reinterpret_cast<unsigned short*>(depth);
     for (int i = 0; i < FREENECT_FRAME_PIX; ++i)
     {
@@ -206,28 +216,27 @@ Freenect::get3DPointCloudData(void)
                 QVector3D ray = depthProjectionMatrix[i];
                 ray *= range;
 
-                pointCloud.push_back(QVector3D(ray.x(), ray.y(), ray.z()));
+                pointCloud3D->push_back(QVector3D(ray.x(), ray.y(), ray.z()));
             }
         }
     }
 
-    return pointCloud;
+    return pointCloud3D;
 }
 
-QVector<Freenect::Vector6D>
+QSharedPointer< QVector<Freenect::Vector6D> >
 Freenect::get6DPointCloudData(void)
 {
-    QVector<QVector3D> rawPointCloud = get3DPointCloudData();
-
-    QVector<Freenect::Vector6D> pointCloud;
+    get3DPointCloudData();
 
-    for (int i = 0; i < rawPointCloud.size(); ++i)
+    pointCloud6D->clear();
+    for (int i = 0; i < pointCloud3D->size(); ++i)
     {
         Vector6D point;
 
-        point.x = rawPointCloud.at(i).x();
-        point.y = rawPointCloud.at(i).y();
-        point.z = rawPointCloud.at(i).z();
+        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);
 
@@ -250,11 +259,11 @@ Freenect::get6DPointCloudData(void)
             point.g = pixel[1];
             point.b = pixel[2];
 
-            pointCloud.push_back(point);
+            pointCloud6D->push_back(point);
         }
     }
 
-    return pointCloud;
+    return pointCloud6D;
 }
 
 int

src/input/Freenect.h

@@ -54,7 +54,7 @@ public:
     QSharedPointer<QByteArray> getRgbData(void);
     QSharedPointer<QByteArray> getRawDepthData(void);
     QSharedPointer<QByteArray> getColoredDepthData(void);
-    QVector<QVector3D> get3DPointCloudData(void);
+    QSharedPointer< QVector<QVector3D> > get3DPointCloudData(void);
 
     typedef struct
     {
@@ -65,7 +65,7 @@ public:
         unsigned char g;
         unsigned char b;
     } Vector6D;
-    QVector<Vector6D> get6DPointCloudData(void);
+    QSharedPointer< QVector<Vector6D> > get6DPointCloudData();
 
     int getTiltAngle(void) const;
     void setTiltAngle(int angle);
@@ -142,6 +142,13 @@ private:
 
     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

src/ui/map3D/Pixhawk3DWidget.cc

@@ -975,21 +975,6 @@ Pixhawk3DWidget::updateHUD(double robotX, double robotY, double robotZ,
 
     scaleGeode->update(height(), cameraParams.cameraFov,
                        cameraManipulator->getDistance(), darkBackground);
-
-    if (!rgb.isNull())
-    {
-        rgbImage->setImage(640, 480, 1,
-                           GL_RGB, GL_RGB, GL_UNSIGNED_BYTE,
-                           reinterpret_cast<unsigned char *>(rgb->data()),
-                           osg::Image::NO_DELETE);
-        rgbImage->dirty();
-
-        depthImage->setImage(640, 480, 1,
-                             GL_RGB, GL_RGB, GL_UNSIGNED_BYTE,
-                             reinterpret_cast<unsigned char *>(coloredDepth->data()),
-                             osg::Image::NO_DELETE);
-        depthImage->dirty();
-    }
 }
 
 void
@@ -1260,26 +1245,45 @@ float colormap_jet[128][3] =
 void
 Pixhawk3DWidget::updateRGBD(void)
 {
-    rgb = freenect->getRgbData();
-    coloredDepth = freenect->getColoredDepthData();
-    pointCloud = freenect->get6DPointCloudData();
+    QSharedPointer<QByteArray> rgb = freenect->getRgbData();
+    if (!rgb.isNull())
+    {
+        rgbImage->setImage(640, 480, 1,
+                           GL_RGB, GL_RGB, GL_UNSIGNED_BYTE,
+                           reinterpret_cast<unsigned char *>(rgb->data()),
+                           osg::Image::NO_DELETE);
+        rgbImage->dirty();
+    }
+
+    QSharedPointer<QByteArray> coloredDepth = freenect->getColoredDepthData();
+    if (!coloredDepth.isNull())
+    {
+        depthImage->setImage(640, 480, 1,
+                             GL_RGB, GL_RGB, GL_UNSIGNED_BYTE,
+                             reinterpret_cast<unsigned char *>(coloredDepth->data()),
+                             osg::Image::NO_DELETE);
+        depthImage->dirty();
+    }
+
+    QSharedPointer< QVector<Freenect::Vector6D> > pointCloud =
+            freenect->get6DPointCloudData();
 
     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)
+    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->at(i).x;
+        double y = pointCloud->at(i).y;
+        double z = pointCloud->at(i).z;
         (*vertices)[i].set(x, z, -y);
 
         if (enableRGBDColor)
         {
-            (*colors)[i].set(pointCloud[i].r / 255.0f,
-                             pointCloud[i].g / 255.0f,
-                             pointCloud[i].b / 255.0f,
+            (*colors)[i].set(pointCloud->at(i).r / 255.0f,
+                             pointCloud->at(i).g / 255.0f,
+                             pointCloud->at(i).b / 255.0f,
                              1.0f);
         }
         else
@@ -1296,12 +1300,12 @@ Pixhawk3DWidget::updateRGBD(void)
     if (geometry->getNumPrimitiveSets() == 0)
     {
         geometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::POINTS,
-                                                      0, pointCloud.size()));
+                                                      0, pointCloud->size()));
     }
     else
     {
         osg::DrawArrays* drawarrays = static_cast<osg::DrawArrays*>(geometry->getPrimitiveSet(0));
-        drawarrays->setCount(pointCloud.size());
+        drawarrays->setCount(pointCloud->size());
     }
 }
 #endif

src/ui/map3D/Pixhawk3DWidget.h

@@ -157,11 +157,8 @@ private:
     osg::ref_ptr<osg::Geode> rgbd3DNode;
 #ifdef QGC_LIBFREENECT_ENABLED
     QScopedPointer<Freenect> freenect;
-    QVector<Freenect::Vector6D> pointCloud;
 #endif
     bool enableFreenect;
-    QSharedPointer<QByteArray> rgb;
-    QSharedPointer<QByteArray> coloredDepth;
 
     QVector< osg::ref_ptr<osg::Node> > vehicleModels;