diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc
index e14eb5a6d6d2fdc46b949679695c67041dc180b3..707076dc1a8156c984927a180ef8c8b8a55c30a2 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 b7583d9de7be04ebbbc675e66dcea70b71c9d137..f0d9b29186b6958ffcdf6c7312b9eb11c4199358 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 c2ccd1a1629528e4e76657a4277165f81bdb44b4..07e193f97f0396351c324f7a254b3745d03c0a8a 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 1c08d0315200270fd4e71d0ca6830a86c7e92370..4890b3acfa5d6344f7ed52c7f751b2bd8b47123d 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();