From 6354c28aa740774dda187d5dfc69be5d8bb4ebe5 Mon Sep 17 00:00:00 2001
From: Lionel Heng <hengli@student.ethz.ch>
Date: Sat, 10 Dec 2011 22:27:54 +0100
Subject: [PATCH] QGC now receives and displays RGBD images via extended
 MAVLINK messages over UDP.

---
 src/uas/UAS.cc                  |  4 ++++
 src/uas/UAS.h                   |  5 +++++
 src/uas/UASInterface.h          |  1 +
 src/ui/map3D/Pixhawk3DWidget.cc | 48 +++++++++++++++++++++++++++--------------
 4 files changed, 42 insertions(+), 16 deletions(-)

diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc
index e14eb5a..707076d 100644
--- a/src/uas/UAS.cc
+++ b/src/uas/UAS.cc
@@ -983,6 +983,10 @@ void UAS::receiveExtendedMessage(LinkInterface* link, std::tr1::shared_ptr<googl
     {
         pointCloud.CopyFrom(*message);
     }
+    else if (message->GetTypeName() == rgbdImage.GetTypeName())
+    {
+        rgbdImage.CopyFrom(*message);
+    }
 }
 
 #endif
diff --git a/src/uas/UAS.h b/src/uas/UAS.h
index b7583d9..f0d9b29 100644
--- a/src/uas/UAS.h
+++ b/src/uas/UAS.h
@@ -132,6 +132,10 @@ public:
     px::PointCloudXYZRGB getPointCloud() const {
         return pointCloud;
     }
+
+    px::RGBDImage getRGBDImage() const {
+        return rgbdImage;
+    }
 #endif
 
     friend class UASWaypointManager;
@@ -216,6 +220,7 @@ protected: //COMMENTS FOR TEST UNIT
 
 #ifdef QGC_PROTOBUF_ENABLED
     px::PointCloudXYZRGB pointCloud;
+    px::RGBDImage rgbdImage;
 #endif
 
     QMap<int, QMap<QString, QVariant>* > parameters; ///< All parameters
diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h
index c2ccd1a..07e193f 100644
--- a/src/uas/UASInterface.h
+++ b/src/uas/UASInterface.h
@@ -96,6 +96,7 @@ public:
 
 #ifdef QGC_PROTOBUF_ENABLED
     virtual px::PointCloudXYZRGB getPointCloud() const = 0;
+    virtual px::RGBDImage getRGBDImage() const = 0;
 #endif
 
     virtual bool isArmed() const = 0;
diff --git a/src/ui/map3D/Pixhawk3DWidget.cc b/src/ui/map3D/Pixhawk3DWidget.cc
index 1c08d03..4890b3a 100644
--- a/src/ui/map3D/Pixhawk3DWidget.cc
+++ b/src/ui/map3D/Pixhawk3DWidget.cc
@@ -1219,25 +1219,41 @@ float colormap_jet[128][3] = {
 void
 Pixhawk3DWidget::updateRGBD(double robotX, double robotY, double robotZ)
 {
+    px::RGBDImage rgbdImage = uas->getRGBDImage();
     px::PointCloudXYZRGB pointCloud = uas->getPointCloud();
 
-//    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();
-//    }
+    rgbImage->setImage(rgbdImage.cols(), rgbdImage.rows(), 1,
+                       GL_LUMINANCE, GL_LUMINANCE, GL_UNSIGNED_BYTE,
+                       reinterpret_cast<unsigned char *>(&(*(rgbdImage.mutable_imagedata1()))[0]),
+                       osg::Image::NO_DELETE);
+    rgbImage->dirty();
+
+    QByteArray coloredDepth(rgbdImage.cols() * rgbdImage.rows() * 3, 0);
+    for (uint32_t r = 0; r < rgbdImage.rows(); ++r)
+    {
+        const float* depth = reinterpret_cast<const float*>(rgbdImage.imagedata2().c_str() + r * rgbdImage.step2());
+        uint8_t* pixel = reinterpret_cast<uint8_t*>(coloredDepth.data()) + r * rgbdImage.cols() * 3;
+        for (uint32_t c = 0; c < rgbdImage.cols(); ++c)
+        {
+            if (depth[c] != 0)
+            {
+                int idx = fminf(depth[c], 10.0f) / 10.0f * 127.0f;
+                idx = 127 - idx;
+
+                pixel[0] = colormap_jet[idx][2] * 255.0f;
+                pixel[1] = colormap_jet[idx][1] * 255.0f;
+                pixel[2] = colormap_jet[idx][0] * 255.0f;
+            }
 
-//    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();
-//    }
+            pixel += 3;
+        }
+    }
+
+    depthImage->setImage(rgbdImage.cols(), rgbdImage.rows(), 1,
+                         GL_RGB, GL_RGB, GL_UNSIGNED_BYTE,
+                         reinterpret_cast<unsigned char *>(coloredDepth.data()),
+                         osg::Image::NO_DELETE);
+    depthImage->dirty();
 
     osg::Geometry* geometry = rgbd3DNode->getDrawable(0)->asGeometry();