diff --git a/qgroundcontrol.pri b/qgroundcontrol.pri index e40b106103e7d98039183efec7b7c2d0c2cc8af3..b32b120c959a5ffcf87e3826802ac4a195b0e33a 100644 --- a/qgroundcontrol.pri +++ b/qgroundcontrol.pri @@ -189,6 +189,7 @@ linux-g++ { } exists(/usr/local/include/libfreenect) { + exists(/usr/local/include/libfreenect/libfreenect.h) { message("Building support for libfreenect") DEPENDENCIES_PRESENT += libfreenect INCLUDEPATH += /usr/include/libusb-1.0 diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index 741c2bb7b51dcecef51fc82663e29aa3f365fe61..ef2e0d790619a34ba4e1491c1a1123a4ef84e518 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -258,7 +258,7 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) //if () // If a new loss was detected or we just hit one 128th packet step - if (lastLoss != totalLossCounter || (totalReceiveCounter == 128)) + if (lastLoss != totalLossCounter || (totalReceiveCounter % 64 == 0)) { // Calculate new loss ratio // Receive loss @@ -325,6 +325,8 @@ void MAVLinkProtocol::sendMessage(LinkInterface* link, mavlink_message_t message { // Create buffer uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + // Rewriting header to ensure correct link ID is set + if (link->getId() != 0) mavlink_finalize_message_chan(&message, this->getSystemId(), this->getComponentId(), link->getId(), message.len); // Write message into buffer, prepending start sign int len = mavlink_msg_to_send_buffer(buffer, &message); // If link is connected diff --git a/src/comm/MAVLinkXMLParser.cc b/src/comm/MAVLinkXMLParser.cc index 3d7068a261753bd68c0c2f670b5dc6551c008d2a..8c2e6df1efc04f89a322b488c6134c3702b62dac 100644 --- a/src/comm/MAVLinkXMLParser.cc +++ b/src/comm/MAVLinkXMLParser.cc @@ -341,7 +341,8 @@ bool MAVLinkXMLParser::generate() QString decode("static inline void mavlink_msg_%1_decode(const mavlink_message_t* msg, %2* %1)\n{\n%3}\n"); QString pack("static inline uint16_t mavlink_msg_%1_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg%2)\n{\n\tuint16_t i = 0;\n\tmsg->msgid = MAVLINK_MSG_ID_%3;\n\n%4\n\treturn mavlink_finalize_message(msg, system_id, component_id, i);\n}\n\n"); - QString compactSend("#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS\n\nstatic inline void mavlink_msg_%3_send(%1 chan%5)\n{\n\t%2 msg;\n\tmavlink_msg_%3_pack(mavlink_system.sysid, mavlink_system.compid, &msg%4);\n\tmavlink_send_uart(chan, &msg);\n}\n\n#endif"); + QString packChan("static inline uint16_t mavlink_msg_%1_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg%2)\n{\n\tuint16_t i = 0;\n\tmsg->msgid = MAVLINK_MSG_ID_%3;\n\n%4\n\treturn mavlink_finalize_message_chan(msg, system_id, component_id, chan, i);\n}\n\n"); + QString compactSend("#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS\n\nstatic inline void mavlink_msg_%3_send(%1 chan%5)\n{\n\t%2 msg;\n\tmavlink_msg_%3_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg%4);\n\tmavlink_send_uart(chan, &msg);\n}\n\n#endif"); //QString compactStructSend = "#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS\n\nstatic inline void mavlink_msg_%3_struct_send(%1 chan%5)\n{\n\t%2 msg;\n\tmavlink_msg_%3_encode(mavlink_system.sysid, mavlink_system.compid, &msg%4);\n\tmavlink_send_uart(chan, &msg);\n}\n\n#endif"; QString unpacking; QString prepends; @@ -506,10 +507,11 @@ bool MAVLinkXMLParser::generate() cStruct = cStruct.arg(cStructName, cStructLines); lcmStructDefs.append("\n").append(cStruct).append("\n"); pack = pack.arg(messageName, packParameters, messageName.toUpper(), packLines); + packChan = packChan.arg(messageName, packParameters, messageName.toUpper(), packLines); encode = encode.arg(messageName).arg(cStructName).arg(packArguments); decode = decode.arg(messageName).arg(cStructName).arg(decodeLines); compactSend = compactSend.arg(channelType, messageType, messageName, sendArguments, packParameters); - QString cFile = "// MESSAGE " + messageName.toUpper() + " PACKING\n\n" + idDefine + "\n\n" + cStruct + "\n\n" + arrayDefines + "\n\n" + commentContainer.arg(messageName.toLower(), commentLines) + pack + encode + "\n" + compactSend + "\n" + "// MESSAGE " + messageName.toUpper() + " UNPACKING\n\n" + unpacking + decode; + QString cFile = "// MESSAGE " + messageName.toUpper() + " PACKING\n\n" + idDefine + "\n\n" + cStruct + "\n\n" + arrayDefines + "\n\n" + commentContainer.arg(messageName.toLower(), commentLines) + pack + packChan + encode + "\n" + compactSend + "\n" + "// MESSAGE " + messageName.toUpper() + " UNPACKING\n\n" + unpacking + decode; cFiles.append(qMakePair(QString("mavlink_msg_%1.h").arg(messageName), cFile)); } // Check if tag = message } // Check if e = NULL diff --git a/src/input/Freenect.cc b/src/input/Freenect.cc index 55951f94ddd21d7cdc899c6fb7315ef1013acb21..0b5c3edbf4cc322b405bf943506cf1cb1780c96d 100644 --- a/src/input/Freenect.cc +++ b/src/input/Freenect.cc @@ -30,6 +30,18 @@ Freenect::Freenect() depthCameraParameters.k[3] = 5.0350940090814270e-03; depthCameraParameters.k[4] = -1.3053628089976321e+00; + // relative rotation/translation between cameras with depth camera as reference + transformMatrix = QMatrix4x4(9.9984628826577793e-01, 1.2635359098409581e-03, + -1.7487233004436643e-02, 1.9985242312092553e-02, + -1.4779096108364480e-03, 9.9992385683542895e-01, + -1.2251380107679535e-02, -7.4423738761617583e-04, + 1.7470421412464927e-02, 1.2275341476520762e-02, + 9.9977202419716948e-01, -1.0916736334336222e-02, + 0.0, 0.0, 0.0, 1.0); + + // relative rotation/translation between cameras with rgb camera as reference + transformMatrix = transformMatrix.transposed(); + // populate gamma lookup table for (int i = 0; i < 2048; ++i) { @@ -51,6 +63,9 @@ Freenect::Freenect() projectPixelTo3DRay(rectifiedPoint, rectifiedRay, depthCameraParameters); depthProjectionMatrix[i * FREENECT_FRAME_W + j] = rectifiedRay; + + rectifyPoint(originalPoint, rectifiedPoint, rgbCameraParameters); + rgbRectificationMap[i * FREENECT_FRAME_W + j] = rectifiedPoint; } } } @@ -173,7 +188,7 @@ Freenect::getColoredDepthData(void) } QVector -Freenect::getPointCloudData(void) +Freenect::get3DPointCloudData(void) { QMutexLocker locker(&depthMutex); @@ -200,6 +215,49 @@ Freenect::getPointCloudData(void) return pointCloud; } +QVector +Freenect::get6DPointCloudData(void) +{ + QVector rawPointCloud = get3DPointCloudData(); + + QVector pointCloud; + + for (int i = 0; i < rawPointCloud.size(); ++i) + { + Vector6D point; + + point.x = rawPointCloud[i].x(); + point.y = rawPointCloud[i].y(); + point.z = rawPointCloud[i].z(); + + QVector4D transformedPoint = transformMatrix * QVector4D(point.x, point.y, point.z, 1.0); + + float iz = 1.0 / transformedPoint.z(); + QVector2D rectifiedPoint(transformedPoint.x() * iz * rgbCameraParameters.fx + rgbCameraParameters.cx, + transformedPoint.y() * iz * rgbCameraParameters.fy + rgbCameraParameters.cy); + + QVector2D originalPoint; + unrectifyPoint(rectifiedPoint, originalPoint, rgbCameraParameters); + + if (originalPoint.x() >= 0.0 && originalPoint.x() < FREENECT_FRAME_W && + originalPoint.y() >= 0.0 && originalPoint.y() < FREENECT_FRAME_H) + { + int x = static_cast(originalPoint.x()); + int y = static_cast(originalPoint.y()); + unsigned char* pixel = reinterpret_cast(rgb) + + (y * FREENECT_FRAME_W + x) * 3; + + point.r = pixel[0]; + point.g = pixel[1]; + point.b = pixel[2]; + + pointCloud.push_back(point); + } + } + + return pointCloud; +} + int Freenect::getTiltAngle(void) const { @@ -237,7 +295,8 @@ Freenect::FreenectThread::run(void) } void -Freenect::rectifyPoint(const QVector2D& originalPoint, QVector2D& rectifiedPoint, +Freenect::rectifyPoint(const QVector2D& originalPoint, + QVector2D& rectifiedPoint, const IntrinsicCameraParameters& params) { double x = (originalPoint.x() - params.cx) / params.fx; @@ -264,6 +323,28 @@ Freenect::rectifyPoint(const QVector2D& originalPoint, QVector2D& rectifiedPoint rectifiedPoint.setY(y * params.fy + params.cy); } +void +Freenect::unrectifyPoint(const QVector2D& rectifiedPoint, + QVector2D& originalPoint, + const IntrinsicCameraParameters& params) +{ + double x = (rectifiedPoint.x() - params.cx) / params.fx; + double y = (rectifiedPoint.y() - params.cy) / params.fy; + + 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 cdist = 1.0 + r2 * (params.k[0] + r2 * (params.k[1] + r2 * params.k[4])); + x = x * cdist + dx; + y = y * cdist + dy; + + originalPoint.setX(x * params.fx + params.cx); + originalPoint.setY(y * params.fy + params.cy); +} + void Freenect::projectPixelTo3DRay(const QVector2D& pixel, QVector3D& ray, const IntrinsicCameraParameters& params) diff --git a/src/input/Freenect.h b/src/input/Freenect.h index 7424addd1c14eb259d18044d21a4b0c32b6b12b7..216b4d49c8c4830f5e01d8558e73b0d5f7ed6854 100644 --- a/src/input/Freenect.h +++ b/src/input/Freenect.h @@ -2,6 +2,7 @@ #define FREENECT_H #include +#include #include #include #include @@ -22,7 +23,18 @@ public: QSharedPointer getRgbData(void); QSharedPointer getRawDepthData(void); QSharedPointer getColoredDepthData(void); - QVector getPointCloudData(void); + QVector get3DPointCloudData(void); + + typedef struct + { + double x; + double y; + double z; + unsigned char r; + unsigned char g; + unsigned char b; + } Vector6D; + QVector get6DPointCloudData(void); int getTiltAngle(void) const; void setTiltAngle(int angle); @@ -43,8 +55,12 @@ private: } IntrinsicCameraParameters; - void rectifyPoint(const QVector2D& originalPoint, QVector2D& rectifiedPoint, + void rectifyPoint(const QVector2D& originalPoint, + QVector2D& rectifiedPoint, const IntrinsicCameraParameters& params); + void unrectifyPoint(const QVector2D& rectifiedPoint, + QVector2D& originalPoint, + const IntrinsicCameraParameters& params); void projectPixelTo3DRay(const QVector2D& pixel, QVector3D& ray, const IntrinsicCameraParameters& params); @@ -69,6 +85,8 @@ private: IntrinsicCameraParameters rgbCameraParameters; IntrinsicCameraParameters depthCameraParameters; + QMatrix4x4 transformMatrix; + // tilt angle of Kinect camera int tiltAngle; @@ -90,6 +108,7 @@ private: unsigned short gammaTable[2048]; QVector3D depthProjectionMatrix[FREENECT_FRAME_PIX]; + QVector2D rgbRectificationMap[FREENECT_FRAME_PIX]; }; #endif // FREENECT_H diff --git a/src/ui/map3D/Pixhawk3DWidget.cc b/src/ui/map3D/Pixhawk3DWidget.cc index fa31c2f126bf738aa311b716c2c24ec66408f272..03657627460a1c39e2d110eb079b7bea7c6b1b0e 100644 --- a/src/ui/map3D/Pixhawk3DWidget.cc +++ b/src/ui/map3D/Pixhawk3DWidget.cc @@ -53,6 +53,7 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent) , displayWaypoints(true) , displayRGBD2D(false) , displayRGBD3D(false) + , enableRGBDColor(true) , followCamera(true) , lastRobotX(0.0f) , lastRobotY(0.0f) @@ -228,7 +229,7 @@ Pixhawk3DWidget::findVehicleModels(void) } else { - printf(QString("ERROR: Could not load file " + directory.absoluteFilePath(files[i]) + "\n").toStdString().c_str()); + printf("%s\n", QString("ERROR: Could not load file " + directory.absoluteFilePath(files[i]) + "\n").toStdString().c_str()); } } @@ -396,6 +397,9 @@ Pixhawk3DWidget::keyPressEvent(QKeyEvent* event) case '2': displayRGBD3D = !displayRGBD3D; break; + case 'c': case 'C': + enableRGBDColor = !enableRGBDColor; + break; } } @@ -946,7 +950,7 @@ Pixhawk3DWidget::updateRGBD(void) { rgb = freenect->getRgbData(); coloredDepth = freenect->getColoredDepthData(); - pointCloud = freenect->getPointCloudData(); + pointCloud = freenect->get6DPointCloudData(); osg::Geometry* geometry = rgbd3DNode->getDrawable(0)->asGeometry(); @@ -954,17 +958,27 @@ Pixhawk3DWidget::updateRGBD(void) 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(); + 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 (enableRGBDColor) + { + (*colors)[i].set(pointCloud[i].r / 255.0f, + pointCloud[i].g / 255.0f, + pointCloud[i].b / 255.0f, + 1.0f); + } + else + { + 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) diff --git a/src/ui/map3D/Pixhawk3DWidget.h b/src/ui/map3D/Pixhawk3DWidget.h index 0d8074093da1fd326aaa5c66e1f5062bec56a97a..d4377ca193457607a6b5aa8c43d3ca61381a58eb 100644 --- a/src/ui/map3D/Pixhawk3DWidget.h +++ b/src/ui/map3D/Pixhawk3DWidget.h @@ -110,6 +110,7 @@ private: bool displayWaypoints; bool displayRGBD2D; bool displayRGBD3D; + bool enableRGBDColor; bool followCamera; @@ -136,10 +137,10 @@ private: osg::ref_ptr rgbd3DNode; #ifdef QGC_LIBFREENECT_ENABLED QScopedPointer freenect; + QVector pointCloud; #endif QSharedPointer rgb; QSharedPointer coloredDepth; - QVector pointCloud; QVector< osg::ref_ptr > vehicleModels;