Merged

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

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

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

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<QVector3D>
-Freenect::getPointCloudData(void)
+Freenect::get3DPointCloudData(void)
 {
     QMutexLocker locker(&depthMutex);
 
@@ -200,6 +215,49 @@ Freenect::getPointCloudData(void)
     return pointCloud;
 }
 
+QVector<Freenect::Vector6D>
+Freenect::get6DPointCloudData(void)
+{
+    QVector<QVector3D> rawPointCloud = get3DPointCloudData();
+
+    QVector<Freenect::Vector6D> 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<int>(originalPoint.x());
+            int y = static_cast<int>(originalPoint.y());
+            unsigned char* pixel = reinterpret_cast<unsigned char*>(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)

src/input/Freenect.h

@@ -2,6 +2,7 @@
 #define FREENECT_H
 
 #include <libfreenect/libfreenect.h>
+#include <QMatrix4x4>
 #include <QMutex>
 #include <QScopedPointer>
 #include <QSharedPointer>
@@ -22,7 +23,18 @@ public:
     QSharedPointer<QByteArray> getRgbData(void);
     QSharedPointer<QByteArray> getRawDepthData(void);
     QSharedPointer<QByteArray> getColoredDepthData(void);
-    QVector<QVector3D> getPointCloudData(void);
+    QVector<QVector3D> get3DPointCloudData(void);
+
+    typedef struct
+    {
+        double x;
+        double y;
+        double z;
+        unsigned char r;
+        unsigned char g;
+        unsigned char b;
+    } Vector6D;
+    QVector<Vector6D> 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

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<osg::Vec4Array*>(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<int>(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<int>(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)

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<osg::Geode> rgbd3DNode;
 #ifdef QGC_LIBFREENECT_ENABLED
     QScopedPointer<Freenect> freenect;
+    QVector<Freenect::Vector6D> pointCloud;
 #endif
     QSharedPointer<QByteArray> rgb;
     QSharedPointer<QByteArray> coloredDepth;
-    QVector<QVector3D> pointCloud;
 
     QVector< osg::ref_ptr<osg::Node> > vehicleModels;