From 04c2a1a91a92f53bda6480e934da005e59e535ff Mon Sep 17 00:00:00 2001 From: hengli Date: Tue, 30 Nov 2010 16:09:01 +0100 Subject: [PATCH] Added visualization of point cloud data from Kinect camera. Colorized point cloud visualization to follow soon. --- qgroundcontrol.pri | 2 +- src/input/Freenect.cc | 122 +++++++++++++++++++---- src/input/Freenect.h | 33 ++++++- src/ui/map3D/Pixhawk3DWidget.cc | 166 +++++++++++++++++++++++++++++++- src/ui/map3D/Pixhawk3DWidget.h | 1 + 5 files changed, 299 insertions(+), 25 deletions(-) diff --git a/qgroundcontrol.pri b/qgroundcontrol.pri index 8103c9ea9..d4bcfcf57 100644 --- a/qgroundcontrol.pri +++ b/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 diff --git a/src/input/Freenect.cc b/src/input/Freenect.cc index a60f094f8..dbde5a83b 100644 --- a/src/input/Freenect.cc +++ b/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(i) / 2048.0f; v = powf(v, 3.0f) * 6.0f; gammaTable[i] = static_cast(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 -Freenect::getDistanceData(void) -{ - QMutexLocker locker(&distanceMutex); - return QSharedPointer( - new QByteArray(reinterpret_cast(distance), - FREENECT_FRAME_PIX * sizeof(float))); -} - QSharedPointer Freenect::getColoredDepthData(void) { @@ -137,6 +167,34 @@ Freenect::getColoredDepthData(void) new QByteArray(coloredDepth, FREENECT_RGB_SIZE)); } +QVector +Freenect::getPointCloudData(void) +{ + QMutexLocker locker(&depthMutex); + + QVector pointCloud; + + unsigned short* data = reinterpret_cast(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(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_get_user(device)); freenect_depth* data = reinterpret_cast(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(data[i]) + 3.33f); - } - QMutexLocker coloredDepthLocker(&freenect->coloredDepthMutex); unsigned short* src = reinterpret_cast(data); unsigned char* dst = reinterpret_cast(freenect->coloredDepth); diff --git a/src/input/Freenect.h b/src/input/Freenect.h index e3e3925c5..c375a7380 100644 --- a/src/input/Freenect.h +++ b/src/input/Freenect.h @@ -6,6 +6,8 @@ #include #include #include +#include +#include class Freenect { @@ -18,15 +20,34 @@ public: QSharedPointer getRgbData(void); QSharedPointer getRawDepthData(void); - QSharedPointer getDistanceData(void); QSharedPointer getColoredDepthData(void); + QVector 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 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 diff --git a/src/ui/map3D/Pixhawk3DWidget.cc b/src/ui/map3D/Pixhawk3DWidget.cc index af91684a1..8a15cde4d 100644 --- a/src/ui/map3D/Pixhawk3DWidget.cc +++ b/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(geometry->getVertexArray()); + osg::Vec4Array* colors = static_cast(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(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(geometry->getPrimitiveSet(0)); + drawarrays->setCount(pointCloud.size()); + } } #endif diff --git a/src/ui/map3D/Pixhawk3DWidget.h b/src/ui/map3D/Pixhawk3DWidget.h index be4e66d29..0d8074093 100644 --- a/src/ui/map3D/Pixhawk3DWidget.h +++ b/src/ui/map3D/Pixhawk3DWidget.h @@ -139,6 +139,7 @@ private: #endif QSharedPointer rgb; QSharedPointer coloredDepth; + QVector pointCloud; QVector< osg::ref_ptr > vehicleModels; -- 2.22.0