From 5d25e982c657707c3397b69f2a80c4cf91460325 Mon Sep 17 00:00:00 2001
From: hengli <hengli@student.ethz.ch>
Date: Tue, 7 Dec 2010 10:36:30 +0100
Subject: [PATCH] Improved rendering performance for Kinect data.
---
src/input/Freenect.cc | 51 +++++++++++++++++++++---------------
src/input/Freenect.h | 11 ++++++--
src/ui/map3D/Pixhawk3DWidget.cc | 58 ++++++++++++++++++++++-------------------
src/ui/map3D/Pixhawk3DWidget.h | 3 ---
4 files changed, 70 insertions(+), 53 deletions(-)
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<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
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<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
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<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
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<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;