diff --git a/src/input/Freenect.cc b/src/input/Freenect.cc index 6ad5c1f..01f751b 100644 --- a/src/input/Freenect.cc +++ b/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) + , pointCloud6D(new QVector) { } @@ -166,33 +171,38 @@ QSharedPointer Freenect::getRgbData(void) { QMutexLocker locker(&rgbMutex); - return QSharedPointer( - new QByteArray(rgb, FREENECT_VIDEO_RGB_SIZE)); + rgbData->clear(); + rgbData->append(rgb, FREENECT_VIDEO_RGB_SIZE); + + return rgbData; } QSharedPointer Freenect::getRawDepthData(void) { QMutexLocker locker(&depthMutex); - return QSharedPointer( - new QByteArray(depth, FREENECT_DEPTH_11BIT_SIZE)); + rawDepthData->clear(); + rawDepthData->append(depth, FREENECT_DEPTH_11BIT_SIZE); + + return rawDepthData; } QSharedPointer Freenect::getColoredDepthData(void) { QMutexLocker locker(&coloredDepthMutex); - return QSharedPointer( - new QByteArray(coloredDepth, FREENECT_VIDEO_RGB_SIZE)); + coloredDepthData->clear(); + coloredDepthData->append(coloredDepth, FREENECT_VIDEO_RGB_SIZE); + + return coloredDepthData; } -QVector +QSharedPointer< QVector > Freenect::get3DPointCloudData(void) { QMutexLocker locker(&depthMutex); - QVector pointCloud; - + pointCloud3D->clear(); unsigned short* data = reinterpret_cast(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 +QSharedPointer< QVector > Freenect::get6DPointCloudData(void) { - QVector rawPointCloud = get3DPointCloudData(); - - QVector 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 diff --git a/src/input/Freenect.h b/src/input/Freenect.h index 6f8f391..0995715 100644 --- a/src/input/Freenect.h +++ b/src/input/Freenect.h @@ -54,7 +54,7 @@ public: QSharedPointer getRgbData(void); QSharedPointer getRawDepthData(void); QSharedPointer getColoredDepthData(void); - QVector get3DPointCloudData(void); + QSharedPointer< QVector > get3DPointCloudData(void); typedef struct { @@ -65,7 +65,7 @@ public: unsigned char g; unsigned char b; } Vector6D; - QVector get6DPointCloudData(void); + QSharedPointer< QVector > 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 rgbData; + QSharedPointer rawDepthData; + QSharedPointer coloredDepthData; + QSharedPointer< QVector > pointCloud3D; + QSharedPointer< QVector > pointCloud6D; }; #endif // FREENECT_H diff --git a/src/ui/map3D/Pixhawk3DWidget.cc b/src/ui/map3D/Pixhawk3DWidget.cc index afda24a..d0a5796 100644 --- a/src/ui/map3D/Pixhawk3DWidget.cc +++ b/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(rgb->data()), - osg::Image::NO_DELETE); - rgbImage->dirty(); - - depthImage->setImage(640, 480, 1, - GL_RGB, GL_RGB, GL_UNSIGNED_BYTE, - reinterpret_cast(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 rgb = freenect->getRgbData(); + if (!rgb.isNull()) + { + rgbImage->setImage(640, 480, 1, + GL_RGB, GL_RGB, GL_UNSIGNED_BYTE, + reinterpret_cast(rgb->data()), + osg::Image::NO_DELETE); + rgbImage->dirty(); + } + + QSharedPointer coloredDepth = freenect->getColoredDepthData(); + if (!coloredDepth.isNull()) + { + depthImage->setImage(640, 480, 1, + GL_RGB, GL_RGB, GL_UNSIGNED_BYTE, + reinterpret_cast(coloredDepth->data()), + osg::Image::NO_DELETE); + depthImage->dirty(); + } + + QSharedPointer< QVector > pointCloud = + freenect->get6DPointCloudData(); osg::Geometry* geometry = rgbd3DNode->getDrawable(0)->asGeometry(); osg::Vec3Array* vertices = static_cast(geometry->getVertexArray()); osg::Vec4Array* colors = static_cast(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(geometry->getPrimitiveSet(0)); - drawarrays->setCount(pointCloud.size()); + drawarrays->setCount(pointCloud->size()); } } #endif diff --git a/src/ui/map3D/Pixhawk3DWidget.h b/src/ui/map3D/Pixhawk3DWidget.h index 0cc499a..2d3b1a5 100644 --- a/src/ui/map3D/Pixhawk3DWidget.h +++ b/src/ui/map3D/Pixhawk3DWidget.h @@ -157,11 +157,8 @@ private: osg::ref_ptr rgbd3DNode; #ifdef QGC_LIBFREENECT_ENABLED QScopedPointer freenect; - QVector pointCloud; #endif bool enableFreenect; - QSharedPointer rgb; - QSharedPointer coloredDepth; QVector< osg::ref_ptr > vehicleModels;