Added 2D visualization of Kinect data. 3D visualization to follow soon.

qgroundcontrol.pri

@@ -74,17 +74,30 @@ macx {
     # Copy audio files if needed
     QMAKE_PRE_LINK += cp -r $$BASEDIR/audio $$DESTDIR/qgroundcontrol.app/Contents/MacOs/.
 
-    exists(/opt/local/lib/osg):exists("/opt/local/lib/osgEarth") {
-    message("Building support for OSGEARTH")
-    DEPENDENCIES_PRESENT += osgearth
-    LIBS += -L/opt/local/lib/
-    INCLUDEPATH += /opt/local/include
-    # Include OpenSceneGraph and osgEarth libraries
-    LIBS += -losg \
-        -losgViewer \
-        -losgEarth
+    exists(/usr/include/osg) {
+    message("Building support for OpenSceneGraph")
+    DEPENDENCIES_PRESENT += osg
+    # Include OpenSceneGraph libraries
+    LIBS += -losg
     DEFINES += QGC_OSG_ENABLED
     }
+
+    exists(/usr/include/osgEarth) {
+    message("Building support for osgEarth")
+    DEPENDENCIES_PRESENT += osgearth
+    # Include osgEarth libraries
+    LIBS += -losgViewer \
+            -losgEarth
+    DEFINES += QGC_OSGEARTH_ENABLED
+    }
+
+    exists(/usr/local/include/libfreenect) {
+    message("Building support for libfreenect")
+    DEPENDENCIES_PRESENT += libfreenect
+    # Include libfreenect libraries
+    LIBS += -lfreenect
+    DEFINES += QGC_LIBFREENECT_ENABLED
+    }
 }
 
 # GNU/Linux
@@ -127,24 +140,32 @@ linux-g++ {
         -lSDL \
         -lSDLmain
 
-exists(/usr/include/osg) {
-message("Building support for OpenSceneGraph")
-DEPENDENCIES_PRESENT += osg
-# Include OpenSceneGraph libraries
-LIBS += -losg
-DEFINES += QGC_OSG_ENABLED
-}
+    exists(/usr/include/osg) {
+    message("Building support for OpenSceneGraph")
+    DEPENDENCIES_PRESENT += osg
+    # Include OpenSceneGraph libraries
+    LIBS += -losg
+    DEFINES += QGC_OSG_ENABLED
+    }
 
-exists(/usr/include/osgEarth) {
-message("Building support for osgEarth")
-DEPENDENCIES_PRESENT += osgearth
-# Include osgEarth libraries
-LIBS += -losgViewer \
-        -losgEarth
-DEFINES += QGC_OSGEARTH_ENABLED
-}
+    exists(/usr/include/osgEarth) {
+    message("Building support for osgEarth")
+    DEPENDENCIES_PRESENT += osgearth
+    # Include osgEarth libraries
+    LIBS += -losgViewer \
+            -losgEarth
+    DEFINES += QGC_OSGEARTH_ENABLED
+    }
 
-QMAKE_CXXFLAGS += -Wl,-E
+    exists(/usr/local/include/libfreenect) {
+    message("Building support for libfreenect")
+    DEPENDENCIES_PRESENT += libfreenect
+    # Include libfreenect libraries
+    LIBS += -lfreenect
+    DEFINES += QGC_LIBFREENECT_ENABLED
+    }
+
+    QMAKE_CXXFLAGS += -Wl,-E
 
         #-lflite_cmu_us_rms \
         #-lflite_cmu_us_slt \
@@ -188,22 +209,31 @@ linux-g++-64 {
         -lSDL \
         -lSDLmain
 
-exists(/usr/include/osg) {
-message("Building support for OpenSceneGraph")
-DEPENDENCIES_PRESENT += osg
-# Include OpenSceneGraph libraries
-LIBS += -losg
-DEFINES += QGC_OSG_ENABLED
-}
+    exists(/usr/include/osg) {
+    message("Building support for OpenSceneGraph")
+    DEPENDENCIES_PRESENT += osg
+    # Include OpenSceneGraph libraries
+    LIBS += -losg
+    DEFINES += QGC_OSG_ENABLED
+    }
+
+    exists(/usr/include/osgEarth) {
+    message("Building support for osgEarth")
+    DEPENDENCIES_PRESENT += osgearth
+    # Include osgEarth libraries
+    LIBS += -losgViewer \
+            -losgEarth
+    DEFINES += QGC_OSGEARTH_ENABLED
+    }
+
+    exists(/usr/local/include/libfreenect) {
+    message("Building support for libfreenect")
+    DEPENDENCIES_PRESENT += libfreenect
+    # Include libfreenect libraries
+    LIBS += -lfreenect
+    DEFINES += QGC_LIBFREENECT_ENABLED
+    }
 
-exists(/usr/include/osgEarth) {
-message("Building support for osgEarth")
-DEPENDENCIES_PRESENT += osgearth
-# Include osgEarth libraries
-LIBS += -losgViewer \
-        -losgEarth
-DEFINES += QGC_OSGEARTH_ENABLED
-}
 }
 
 

qgroundcontrol.pro

@@ -47,7 +47,6 @@ MAVLINK_CONF = ""
 # if the variable MAVLINK_CONF contains the name of an
 # additional project, QGroundControl includes the support
 # of custom MAVLink messages of this project
-
 exists(user_config.pri) { 
     include(user_config.pri)
     message("----- USING CUSTOM USER QGROUNDCONTROL CONFIG FROM user_config.pri -----")
@@ -139,7 +138,8 @@ FORMS += src/ui/MainWindow.ui \
     src/ui/QGCPxImuFirmwareUpdate.ui \
     src/ui/QGCDataPlot2D.ui \
     src/ui/QGCRemoteControlView.ui
-    #src/ui/WaypointGlobalView.ui
+
+# src/ui/WaypointGlobalView.ui
 INCLUDEPATH += src \
     src/ui \
     src/ui/linechart \
@@ -221,26 +221,31 @@ HEADERS += src/MG.h \
     src/ui/linechart/IncrementalPlot.h \
     src/ui/map/Waypoint2DIcon.h \
     src/ui/map/MAV2DIcon.h \
-    src/ui/QGCRemoteControlView.h \
-    #src/ui/WaypointGlobalView.h \
+    src/ui/QGCRemoteControlView.h \ # src/ui/WaypointGlobalView.h \
     src/ui/RadioCalibration/RadioCalibrationData.h \
     src/ui/RadioCalibration/RadioCalibrationWindow.h \
     src/ui/RadioCalibration/AirfoilServoCalibrator.h \
     src/ui/RadioCalibration/SwitchCalibrator.h \
     src/ui/RadioCalibration/CurveCalibrator.h \
     src/ui/RadioCalibration/AbstractCalibrator.h \
-    src/comm/QGCMAVLink.h
+    src/comm/QGCMAVLink.h \
+    src/ui/map3D/ImageWindowGeode.h
+contains(DEPENDENCIES_PRESENT, osg) { 
+    message("Including headers for OpenSceneGraph")
     
-contains(DEPENDENCIES_PRESENT, osgearth) {
-message("Including headers for OSGEARTH")
-# Enable only if OpenSceneGraph is available
-HEADERS += src/ui/map3D/Q3DWidget.h \
-    src/ui/map3D/PixhawkCheetahGeode.h \
-    src/ui/map3D/Pixhawk3DWidget.h \
-    src/ui/map3D/Q3DWidgetFactory.h \
-    src/ui/map3D/GCManipulator.h
+    # Enable only if OpenSceneGraph is available
+    HEADERS += src/ui/map3D/Q3DWidget.h \
+        src/ui/map3D/PixhawkCheetahGeode.h \
+        src/ui/map3D/Pixhawk3DWidget.h \
+        src/ui/map3D/Q3DWidgetFactory.h \
+        src/ui/map3D/GCManipulator.h
 }
+contains(DEPENDENCIES_PRESENT, libfreenect) { 
+    message("Including headers for libfreenect")
     
+    # Enable only if libfreenect is available
+    HEADERS += src/input/Freenect.h
+}
 SOURCES += src/main.cc \
     src/Core.cc \
     src/uas/UASManager.cc \
@@ -309,17 +314,23 @@ SOURCES += src/main.cc \
     src/ui/RadioCalibration/SwitchCalibrator.cc \
     src/ui/RadioCalibration/CurveCalibrator.cc \
     src/ui/RadioCalibration/AbstractCalibrator.cc \
-    src/ui/RadioCalibration/RadioCalibrationData.cc \
-    #src/ui/WaypointGlobalView.cc \
+    src/ui/RadioCalibration/RadioCalibrationData.cc \ # src/ui/WaypointGlobalView.cc \
+    src/ui/map3D/ImageWindowGeode.cc
+contains(DEPENDENCIES_PRESENT, osg) { 
+    message("Including headers for OpenSceneGraph")
+    
+    # Enable only if OpenSceneGraph is available
+    SOURCES += src/ui/map3D/Q3DWidget.cc \
+        src/ui/map3D/PixhawkCheetahGeode.cc \
+        src/ui/map3D/Pixhawk3DWidget.cc \
+        src/ui/map3D/Q3DWidgetFactory.cc \
+        src/ui/map3D/GCManipulator.cc
+}
+contains(DEPENDENCIES_PRESENT, libfreenect) { 
+    message("Including headers for libfreenect")
     
-contains(DEPENDENCIES_PRESENT, osgearth) {
-message("Including sources for OSGEARTH")
-# Enable only if OpenSceneGraph is available
-SOURCES += src/ui/map3D/Q3DWidget.cc \
-    src/ui/map3D/PixhawkCheetahGeode.cc \
-    src/ui/map3D/Pixhawk3DWidget.cc \
-    src/ui/map3D/Q3DWidgetFactory.cc \
-    src/ui/map3D/GCManipulator.cc
+    # Enable only if libfreenect is available
+    SOURCES += src/input/Freenect.cc
 }
 RESOURCES += mavground.qrc
 

src/input/Freenect.cc

@@ -0,0 +1,170 @@
+#include "Freenect.h"
+
+#include <string.h>
+#include <QDebug>
+
+Freenect::Freenect()
+    : context(NULL)
+    , device(NULL)
+    , tiltAngle(0)
+{
+
+}
+
+Freenect::~Freenect()
+{
+    if (device != NULL)
+    {
+        freenect_stop_depth(device);
+        freenect_stop_rgb(device);
+    }
+
+    freenect_close_device(device);
+
+    freenect_shutdown(context);
+}
+
+bool
+Freenect::init(int userDeviceNumber)
+{
+    if (freenect_init(&context, NULL) < 0)
+    {
+        return false;
+    }
+
+    freenect_set_log_level(context, FREENECT_LOG_DEBUG);
+
+    if (freenect_num_devices(context) < 1)
+    {
+        return false;
+    }
+
+    if (freenect_open_device(context, &device, userDeviceNumber) < 0)
+    {
+        return false;
+    }
+
+    freenect_set_user(device, this);
+
+    memset(rgb, 0, FREENECT_RGB_SIZE);
+    memset(depth, 0, FREENECT_DEPTH_SIZE);
+
+    // set Kinect parameters
+    if (freenect_set_tilt_degs(device, tiltAngle) != 0)
+    {
+        return false;
+    }
+    if (freenect_set_led(device, LED_RED) != 0)
+    {
+        return false;
+    }
+    if (freenect_set_rgb_format(device, FREENECT_FORMAT_RGB) != 0)
+    {
+        return false;
+    }
+    if (freenect_set_depth_format(device, FREENECT_FORMAT_11_BIT) != 0)
+    {
+        return false;
+    }
+    freenect_set_rgb_callback(device, rgbCallback);
+    freenect_set_depth_callback(device, depthCallback);
+
+    if (freenect_start_rgb(device) != 0)
+    {
+        return false;
+    }
+    if (freenect_start_depth(device) != 0)
+    {
+        return false;
+    }
+
+    thread.reset(new FreenectThread(device));
+    thread->start();
+
+    return true;
+}
+
+bool
+Freenect::process(void)
+{
+    if (freenect_process_events(context) < 0)
+    {
+        return false;
+    }
+
+    freenect_get_raw_accel(device, &ax, &ay, &az);
+    freenect_get_mks_accel(device, &dx, &dy, &dz);
+
+    return true;
+}
+
+QSharedPointer<QByteArray>
+Freenect::getRgbData(void)
+{
+    QMutexLocker locker(&rgbMutex);
+    return QSharedPointer<QByteArray>(new QByteArray(rgb, FREENECT_RGB_SIZE));
+}
+
+QSharedPointer<QByteArray>
+Freenect::getDepthData(void)
+{
+    QMutexLocker locker(&depthMutex);
+    return QSharedPointer<QByteArray>(new QByteArray(depth, FREENECT_DEPTH_SIZE));
+}
+
+int
+Freenect::getTiltAngle(void) const
+{
+    return tiltAngle;
+}
+
+void
+Freenect::setTiltAngle(int angle)
+{
+    if (angle > 30)
+    {
+        angle = 30;
+    }
+    if (angle < -30)
+    {
+        angle = -30;
+    }
+
+    tiltAngle = angle;
+}
+
+Freenect::FreenectThread::FreenectThread(freenect_device* _device)
+{
+    device = _device;
+}
+
+void
+Freenect::FreenectThread::run(void)
+{
+    Freenect* freenect = static_cast<Freenect *>(freenect_get_user(device));
+    while (1)
+    {
+        freenect->process();
+    }
+}
+
+void
+Freenect::rgbCallback(freenect_device* device, freenect_pixel* rgb,
+                      unsigned int timestamp)
+{
+    Freenect* freenect = static_cast<Freenect *>(freenect_get_user(device));
+
+    QMutexLocker locker(&freenect->rgbMutex);
+    memcpy(freenect->rgb, rgb, FREENECT_RGB_SIZE);
+}
+
+void
+Freenect::depthCallback(freenect_device* device, void* depth,
+                        unsigned int timestamp)
+{
+    Freenect* freenect = static_cast<Freenect *>(freenect_get_user(device));
+    freenect_depth* data = reinterpret_cast<freenect_depth *>(depth);
+
+    QMutexLocker locker(&freenect->depthMutex);
+    memcpy(freenect->depth, data, FREENECT_DEPTH_SIZE);
+}

src/input/Freenect.h

@@ -0,0 +1,61 @@
+#ifndef FREENECT_H
+#define FREENECT_H
+
+#include <libfreenect.h>
+#include <QMutex>
+#include <QScopedPointer>
+#include <QSharedPointer>
+#include <QThread>
+
+class Freenect
+{
+public:
+    Freenect();
+    ~Freenect();
+
+    bool init(int userDeviceNumber = 0);
+    bool process(void);
+
+    QSharedPointer<QByteArray> getRgbData(void);
+    QSharedPointer<QByteArray> getDepthData(void);
+
+    int getTiltAngle(void) const;
+    void setTiltAngle(int angle);
+
+private:
+    static void rgbCallback(freenect_device* device, freenect_pixel* rgb,
+                            unsigned int timestamp);
+    static void depthCallback(freenect_device* device, void* depth,
+                              unsigned int timestamp);
+
+    freenect_context* context;
+    freenect_device* device;
+
+    class FreenectThread : public QThread
+    {
+    public:
+        explicit FreenectThread(freenect_device* _device);
+
+    protected:
+        virtual void run(void);
+
+        freenect_device* device;
+    };
+    QScopedPointer<FreenectThread> thread;
+
+    // tilt angle of Kinect camera
+    int tiltAngle;
+
+    // rgbd data
+    char rgb[FREENECT_RGB_SIZE];
+    QMutex rgbMutex;
+
+    char depth[FREENECT_DEPTH_SIZE];
+    QMutex depthMutex;
+
+    // accelerometer data
+    short ax, ay, az;
+    double dx, dy, dz;
+};
+
+#endif // FREENECT_H

src/ui/map3D/ImageWindowGeode.cc

@@ -0,0 +1,115 @@
+///*=====================================================================
+//
+//QGroundControl Open Source Ground Control Station
+//
+//(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+//
+//This file is part of the QGROUNDCONTROL project
+//
+//    QGROUNDCONTROL is free software: you can redistribute it and/or modify
+//    it under the terms of the GNU General Public License as published by
+//    the Free Software Foundation, either version 3 of the License, or
+//    (at your option) any later version.
+//
+//    QGROUNDCONTROL is distributed in the hope that it will be useful,
+//    but WITHOUT ANY WARRANTY; without even the implied warranty of
+//    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//    GNU General Public License for more details.
+//
+//    You should have received a copy of the GNU General Public License
+//    along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
+//
+//======================================================================*/
+
+/**
+ * @file
+ *   @brief Definition of the class ImageWindowGeode.
+ *
+ *   @author Lionel Heng <hengli@student.ethz.ch>
+ *
+ */
+
+#include "ImageWindowGeode.h"
+
+ImageWindowGeode::ImageWindowGeode(const QString& caption,
+                                   const osg::Vec4& backgroundColor,
+                                   osg::ref_ptr<osg::Image>& image)
+    : border(5)
+{
+    // image
+    osg::ref_ptr<osg::Geometry> imageGeometry = new osg::Geometry;
+    imageVertices = new osg::Vec3Array(4);
+
+    osg::ref_ptr<osg::Vec2Array> textureCoords = new osg::Vec2Array;
+    textureCoords->push_back(osg::Vec2(0.0f, 1.0f));
+    textureCoords->push_back(osg::Vec2(1.0f, 1.0f));
+    textureCoords->push_back(osg::Vec2(1.0f, 0.0f));
+    textureCoords->push_back(osg::Vec2(0.0f, 0.0f));
+
+    osg::ref_ptr<osg::Vec4Array> imageColors(new osg::Vec4Array);
+    imageColors->push_back(osg::Vec4(1.0f, 1.0f, 1.0f, 1.0f));
+    imageGeometry->setColorArray(imageColors);
+    imageGeometry->setColorBinding(osg::Geometry::BIND_OVERALL);
+
+    imageGeometry->setVertexArray(imageVertices);
+    imageGeometry->setTexCoordArray(0, textureCoords);
+
+    imageGeometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::POLYGON,
+                                                       0, imageVertices->size()));
+
+    osg::ref_ptr<osg::Texture2D> texture = new osg::Texture2D;
+    texture->setDataVariance(osg::Object::DYNAMIC);
+    texture->setImage(image);
+    texture->setResizeNonPowerOfTwoHint(false);
+
+    imageGeometry->getOrCreateStateSet()->
+            setTextureAttributeAndModes(0, texture, osg::StateAttribute::ON);
+    imageGeometry->setUseDisplayList(false);
+
+    // background
+    osg::ref_ptr<osg::Geometry> backgroundGeometry = new osg::Geometry;
+    backgroundVertices = new osg::Vec3Array(4);
+    backgroundGeometry->setVertexArray(backgroundVertices);
+    backgroundGeometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::POLYGON,
+                                                            0, backgroundVertices->size()));
+    osg::ref_ptr<osg::Vec4Array> backgroundColors(new osg::Vec4Array);
+    backgroundColors->push_back(backgroundColor);
+    backgroundGeometry->setColorArray(backgroundColors);
+    backgroundGeometry->setColorBinding(osg::Geometry::BIND_OVERALL);
+    backgroundGeometry->setUseDisplayList(false);
+
+    // caption
+    text = new osgText::Text;
+    text->setText(caption.toStdString().c_str());
+    text->setCharacterSize(11);
+    text->setFont("images/Vera.ttf");
+    text->setAxisAlignment(osgText::Text::SCREEN);
+    text->setColor(osg::Vec4(1.0f, 1.0f, 1.0f, 1.0f));
+
+    addDrawable(imageGeometry);
+    addDrawable(backgroundGeometry);
+    addDrawable(text);
+
+    setAttributes(0, 0, 0, 0);
+}
+
+void
+ImageWindowGeode::setAttributes(int x, int y, int width, int height)
+{
+    int imageWidth = width - border * 2;
+    int imageHeight = height - border * 2 - 15;
+    int imageXPosition = x + border;
+    int imageYPosition = y + border;
+
+    imageVertices->at(0) = osg::Vec3(imageXPosition, imageYPosition, 0);
+    imageVertices->at(1) = osg::Vec3(imageXPosition + imageWidth, imageYPosition, 0);
+    imageVertices->at(2) = osg::Vec3(imageXPosition + imageWidth, imageYPosition + imageHeight, 0);
+    imageVertices->at(3) = osg::Vec3(imageXPosition, imageYPosition + imageHeight, 0);
+
+    text->setPosition(osg::Vec3(imageXPosition, imageYPosition + imageHeight + 5, 0));
+
+    backgroundVertices->at(0) = osg::Vec3(x, y, -1);
+    backgroundVertices->at(1) = osg::Vec3(x + width, y, -1);
+    backgroundVertices->at(2) = osg::Vec3(x + width, y + height, -1);
+    backgroundVertices->at(3) = osg::Vec3(x, y + height, -1);
+}

src/ui/map3D/ImageWindowGeode.h

@@ -0,0 +1,56 @@
+///*=====================================================================
+//
+//QGroundControl Open Source Ground Control Station
+//
+//(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+//
+//This file is part of the QGROUNDCONTROL project
+//
+//    QGROUNDCONTROL is free software: you can redistribute it and/or modify
+//    it under the terms of the GNU General Public License as published by
+//    the Free Software Foundation, either version 3 of the License, or
+//    (at your option) any later version.
+//
+//    QGROUNDCONTROL is distributed in the hope that it will be useful,
+//    but WITHOUT ANY WARRANTY; without even the implied warranty of
+//    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//    GNU General Public License for more details.
+//
+//    You should have received a copy of the GNU General Public License
+//    along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
+//
+//======================================================================*/
+
+/**
+ * @file
+ *   @brief Definition of the class ImageWindowGeode.
+ *
+ *   @author Lionel Heng <hengli@student.ethz.ch>
+ *
+ */
+
+#ifndef IMAGEWINDOWGEODE_H
+#define IMAGEWINDOWGEODE_H
+
+#include <osg/Geode>
+#include <osg/Geometry>
+#include <osgText/Text>
+#include <QString>
+
+class ImageWindowGeode : public osg::Geode
+{
+public:
+    ImageWindowGeode(const QString& caption, const osg::Vec4& backgroundColor,
+                     osg::ref_ptr<osg::Image>& image);
+
+    void setAttributes(int x, int y, int width, int height);
+
+private:
+    int border;
+
+    osg::ref_ptr<osg::Vec3Array> imageVertices;
+    osg::ref_ptr<osg::Vec3Array> backgroundVertices;
+    osg::ref_ptr<osgText::Text> text;
+};
+
+#endif // IMAGEWINDOWGEODE_H

src/ui/map3D/Pixhawk3DWidget.cc

@@ -34,6 +34,7 @@
 #include <sstream>
 
 #include <osg/Geode>
+#include <osg/Image>
 #include <osg/LineWidth>
 #include <osg/ShapeDrawable>
 
@@ -49,6 +50,8 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
      , displayTrail(false)
      , displayTarget(false)
      , displayWaypoints(true)
+     , displayRGBD2D(false)
+     , displayRGBD3D(false)
      , followCamera(true)
      , lastRobotX(0.0f)
      , lastRobotY(0.0f)
@@ -81,6 +84,15 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
     waypointsNode = createWaypoints();
     rollingMap->addChild(waypointsNode);
 
+#ifdef QGC_LIBFREENECT_ENABLED
+    // generate RGBD model
+    freenect.reset(new Freenect());
+    freenect->init();
+
+    rgbdNode = createRGBD();
+    egocentricMap->addChild(rgbdNode);
+#endif
+
     setupHUD();
 
     buildLayout();
@@ -273,16 +285,21 @@ Pixhawk3DWidget::display(void)
                                          robotPitch, osg::Vec3f(1.0f, 0.0f, 0.0f),
                                          robotRoll, osg::Vec3f(0.0f, 1.0f, 0.0f)));
 
-    updateHUD(robotX, robotY, robotZ, robotRoll, robotPitch, robotYaw);
     updateTrail(robotX, robotY, robotZ);
     updateTarget();
     updateWaypoints();
+#ifdef QGC_LIBFREENECT_ENABLED
+    updateRGBD();
+#endif
+    updateHUD(robotX, robotY, robotZ, robotRoll, robotPitch, robotYaw);
 
     // set node visibility
     rollingMap->setChildValue(gridNode, displayGrid);
     rollingMap->setChildValue(trailNode, displayTrail);
     rollingMap->setChildValue(targetNode, displayTarget);
     rollingMap->setChildValue(waypointsNode, displayWaypoints);
+    hudGroup->setChildValue(rgb2DGeode, displayRGBD2D);
+    hudGroup->setChildValue(depth2DGeode, displayRGBD2D);
 
     lastRobotX = robotX;
     lastRobotY = robotY;
@@ -290,6 +307,22 @@ Pixhawk3DWidget::display(void)
 }
 
 void
+Pixhawk3DWidget::keyPressEvent(QKeyEvent* event)
+{
+    if (!event->text().isEmpty())
+    {
+        switch (*(event->text().toAscii().data()))
+        {
+        case '1':
+            displayRGBD2D = !displayRGBD2D;
+            break;
+        }
+    }
+
+    Q3DWidget::keyPressEvent(event);
+}
+
+void
 Pixhawk3DWidget::mousePressEvent(QMouseEvent* event)
 {
     if (event->button() == Qt::LeftButton && targetButton->isChecked())
@@ -425,72 +458,106 @@ Pixhawk3DWidget::createWaypoints(void)
     return group;
 }
 
+#ifdef QGC_LIBFREENECT_ENABLED
+osg::ref_ptr<osg::Geode>
+Pixhawk3DWidget::createRGBD(void)
+{
+    return osg::ref_ptr<osg::Geode>(new osg::Geode);
+}
+#endif
+
 void
 Pixhawk3DWidget::setupHUD(void)
 {
-    osg::ref_ptr<osg::Vec3Array> hudBackgroundVertices(new osg::Vec3Array);
-    hudBackgroundVertices->push_back(osg::Vec3(0, height(), -1));
-    hudBackgroundVertices->push_back(osg::Vec3(width(), height(), -1));
-    hudBackgroundVertices->push_back(osg::Vec3(width(), height() - 30, -1));
-    hudBackgroundVertices->push_back(osg::Vec3(0, height() - 30, -1));
-    hudBackgroundVertices->push_back(osg::Vec3(0, 0, -1));
-    hudBackgroundVertices->push_back(osg::Vec3(width(), 0, -1));
-    hudBackgroundVertices->push_back(osg::Vec3(width(), 25, -1));
-    hudBackgroundVertices->push_back(osg::Vec3(0, 25, -1));
-
-    osg::ref_ptr<osg::DrawElementsUInt> hudTopBackgroundIndices(
-            new osg::DrawElementsUInt(osg::PrimitiveSet::POLYGON, 0));
-    hudTopBackgroundIndices->push_back(0);
-    hudTopBackgroundIndices->push_back(1);
-    hudTopBackgroundIndices->push_back(2);
-    hudTopBackgroundIndices->push_back(3);
-
-    osg::ref_ptr<osg::DrawElementsUInt> hudBottomBackgroundIndices(
-            new osg::DrawElementsUInt(osg::PrimitiveSet::POLYGON, 0));
-    hudBottomBackgroundIndices->push_back(4);
-    hudBottomBackgroundIndices->push_back(5);
-    hudBottomBackgroundIndices->push_back(6);
-    hudBottomBackgroundIndices->push_back(7);
-
     osg::ref_ptr<osg::Vec4Array> hudColors(new osg::Vec4Array);
     hudColors->push_back(osg::Vec4(0.0f, 0.0f, 0.0f, 0.2f));
 
     hudBackgroundGeometry = new osg::Geometry;
-    hudBackgroundGeometry->addPrimitiveSet(hudTopBackgroundIndices);
-    hudBackgroundGeometry->addPrimitiveSet(hudBottomBackgroundIndices);
-    hudBackgroundGeometry->setVertexArray(hudBackgroundVertices);
+    hudBackgroundGeometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::POLYGON,
+                                                               0, 4));
+    hudBackgroundGeometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::POLYGON,
+                                                               4, 4));
     hudBackgroundGeometry->setColorArray(hudColors);
     hudBackgroundGeometry->setColorBinding(osg::Geometry::BIND_OVERALL);
-
-    hudGeode->addDrawable(hudBackgroundGeometry);
+    hudBackgroundGeometry->setUseDisplayList(false);
 
     statusText = new osgText::Text;
     statusText->setCharacterSize(11);
     statusText->setFont("images/Vera.ttf");
     statusText->setAxisAlignment(osgText::Text::SCREEN);
-    statusText->setPosition(osg::Vec3(10, height() - 10, -1.5));
     statusText->setColor(osg::Vec4(255, 255, 255, 1));
 
-    hudGeode->addDrawable(statusText);
+    resizeHUD();
+
+    osg::ref_ptr<osg::Geode> statusGeode = new osg::Geode;
+    statusGeode->addDrawable(hudBackgroundGeometry);
+    statusGeode->addDrawable(statusText);
+    hudGroup->addChild(statusGeode);
+
+    rgbImage = new osg::Image;
+    rgb2DGeode = new ImageWindowGeode("RGB Image",
+                                      osg::Vec4(0.0f, 0.0f, 0.1f, 1.0f),
+                                      rgbImage);
+    hudGroup->addChild(rgb2DGeode);
+
+    depthImage = new osg::Image;
+    depthImage->allocateImage(640, 480, 1, GL_RGB, GL_UNSIGNED_BYTE);
+    depth2DGeode = new ImageWindowGeode("Depth Image",
+                                        osg::Vec4(0.0f, 0.0f, 0.1f, 1.0f),
+                                        depthImage);
+    hudGroup->addChild(depth2DGeode);
+
+    for (int i = 0; i < 2048; ++i)
+    {
+        float v = static_cast<float>(i) / 2048.0f;
+        v = powf(v, 3.0f) * 6.0f;
+        gammaLookup[i] = static_cast<unsigned short>(v * 6.0f * 256.0f);
+    }
 }
 
 void
-Pixhawk3DWidget::updateHUD(float robotX, float robotY, float robotZ,
-                         float robotRoll, float robotPitch, float robotYaw)
+Pixhawk3DWidget::resizeHUD(void)
 {
-    osg::ref_ptr<osg::Vec3Array> hudBackgroundVertices(new osg::Vec3Array);
-    hudBackgroundVertices->push_back(osg::Vec3(0, height(), -1));
-    hudBackgroundVertices->push_back(osg::Vec3(width(), height(), -1));
-    hudBackgroundVertices->push_back(osg::Vec3(width(), height() - 30, -1));
-    hudBackgroundVertices->push_back(osg::Vec3(0, height() - 30, -1));
-    hudBackgroundVertices->push_back(osg::Vec3(0, 0, -1));
-    hudBackgroundVertices->push_back(osg::Vec3(width(), 0, -1));
-    hudBackgroundVertices->push_back(osg::Vec3(width(), 25, -1));
-    hudBackgroundVertices->push_back(osg::Vec3(0, 25, -1));
-    hudBackgroundGeometry->setVertexArray(hudBackgroundVertices);
+    int topHUDHeight = 30;
+    int bottomHUDHeight = 25;
+
+    osg::Vec3Array* vertices = static_cast<osg::Vec3Array*>(hudBackgroundGeometry->getVertexArray());
+    if (vertices == NULL || vertices->size() != 8)
+    {
+        osg::ref_ptr<osg::Vec3Array> newVertices = new osg::Vec3Array(8);
+        hudBackgroundGeometry->setVertexArray(newVertices);
+
+        vertices = static_cast<osg::Vec3Array*>(hudBackgroundGeometry->getVertexArray());
+    }
+
+    (*vertices)[0] = osg::Vec3(0, height(), -1);
+    (*vertices)[1] = osg::Vec3(width(), height(), -1);
+    (*vertices)[2] = osg::Vec3(width(), height() - topHUDHeight, -1);
+    (*vertices)[3] = osg::Vec3(0, height() - topHUDHeight, -1);
+    (*vertices)[4] = osg::Vec3(0, 0, -1);
+    (*vertices)[5] = osg::Vec3(width(), 0, -1);
+    (*vertices)[6] = osg::Vec3(width(), bottomHUDHeight, -1);
+    (*vertices)[7] = osg::Vec3(0, bottomHUDHeight, -1);
 
     statusText->setPosition(osg::Vec3(10, height() - 20, -1.5));
 
+    if (rgb2DGeode.valid() && depth2DGeode.valid())
+    {
+        int windowWidth = (width() - 20) / 2;
+        int windowHeight = 3 * windowWidth / 4;
+        rgb2DGeode->setAttributes(10, (height() - windowHeight) / 2,
+                                  windowWidth, windowHeight);
+        depth2DGeode->setAttributes(width() / 2, (height() - windowHeight) / 2,
+                                    windowWidth, windowHeight);
+    }
+}
+
+void
+Pixhawk3DWidget::updateHUD(float robotX, float robotY, float robotZ,
+                           float robotRoll, float robotPitch, float robotYaw)
+{
+    resizeHUD();
+
     std::pair<double,double> cursorPosition =
             getGlobalCursorPosition(getMouseX(), getMouseY(), -robotZ);
 
@@ -506,6 +573,62 @@ Pixhawk3DWidget::updateHUD(float robotX, float robotY, float robotZ,
             " Cursor [" << cursorPosition.first <<
             " " << cursorPosition.second << "]";
     statusText->setText(oss.str());
+
+    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();
+
+        unsigned short* src = reinterpret_cast<unsigned short *>(depth->data());
+        unsigned char* dst = depthImage->data();
+        for (int i = 0; i < depthImage->s() * depthImage->t(); ++i)
+        {
+            unsigned short pval = gammaLookup[src[i]];
+            unsigned short lb = pval & 0xFF;
+            switch (pval >> 8)
+            {
+            case 0:
+                dst[3 * i] = 255;
+                dst[3 * i + 1] = 255 - lb;
+                dst[3 * i + 2] = 255 - lb;
+                break;
+            case 1:
+                dst[3 * i] = 255;
+                dst[3 * i + 1] = lb;
+                dst[3 * i + 2] = 0;
+                break;
+            case 2:
+                dst[3 * i] = 255 - lb;
+                dst[3 * i + 1] = 255;
+                dst[3 * i + 2] = 0;
+                break;
+            case 3:
+                dst[3 * i] = 0;
+                dst[3 * i + 1] = 255;
+                dst[3 * i + 2] = lb;
+                break;
+            case 4:
+                dst[3 * i] = 0;
+                dst[3 * i + 1] = 255 - lb;
+                dst[3 * i + 2] = 255;
+                break;
+            case 5:
+                dst[3 * i] = 0;
+                dst[3 * i + 1] = 0;
+                dst[3 * i + 2] = 255 - lb;
+                break;
+            default:
+                dst[3 * i] = 0;
+                dst[3 * i + 1] = 0;
+                dst[3 * i + 2] = 0;
+                break;
+            }
+        }
+        depthImage->dirty();
+    }
 }
 
 void
@@ -641,6 +764,14 @@ Pixhawk3DWidget::updateWaypoints(void)
     }
 }
 
+#ifdef QGC_LIBFREENECT_ENABLED
+void
+Pixhawk3DWidget::updateRGBD(void)
+{
+    rgb = freenect->getRgbData();
+    depth = freenect->getDepthData();
+}
+#endif
 
 void
 Pixhawk3DWidget::markTarget(void)

src/ui/map3D/Pixhawk3DWidget.h

@@ -33,11 +33,17 @@
 #define PIXHAWK3DWIDGET_H
 
 #include <osgText/Text>
-
 #ifdef QGC_OSGEARTH_ENABLED
 #include <osgEarth/MapNode>
 #endif
 
+#ifdef QGC_OSG_ENABLED
+#include "ImageWindowGeode.h"
+#endif
+#ifdef QGC_LIBFREENECT_ENABLED
+#include "Freenect.h"
+#endif
+
 #include "Q3DWidget.h"
 
 class UASInterface;
@@ -69,6 +75,7 @@ private slots:
 
 protected:
     virtual void display(void);
+    virtual void keyPressEvent(QKeyEvent* event);
     virtual void mousePressEvent(QMouseEvent* event);
 
     UASInterface* uas;
@@ -84,13 +91,21 @@ private:
     osg::ref_ptr<osg::Node> createTarget(void);
     osg::ref_ptr<osg::Group> createWaypoints(void);
 
+#ifdef QGC_LIBFREENECT_ENABLED
+    osg::ref_ptr<osg::Geode> createRGBD(void);
+#endif
+
     void setupHUD(void);
+    void resizeHUD(void);
 
     void updateHUD(float robotX, float robotY, float robotZ,
                    float robotRoll, float robotPitch, float robotYaw);
     void updateTrail(float robotX, float robotY, float robotZ);
     void updateTarget(void);
     void updateWaypoints(void);
+#ifdef QGC_LIBFREENECT_ENABLED
+    void updateRGBD(void);
+#endif
 
     void markTarget(void);
 
@@ -98,6 +113,8 @@ private:
     bool displayTrail;
     bool displayTarget;
     bool displayWaypoints;
+    bool displayRGBD2D;
+    bool displayRGBD3D;
 
     bool followCamera;
 
@@ -106,6 +123,10 @@ private:
 
     osg::ref_ptr<osg::Geometry> hudBackgroundGeometry;
     osg::ref_ptr<osgText::Text> statusText;
+    osg::ref_ptr<ImageWindowGeode> rgb2DGeode;
+    osg::ref_ptr<ImageWindowGeode> depth2DGeode;
+    osg::ref_ptr<osg::Image> rgbImage;
+    osg::ref_ptr<osg::Image> depthImage;
     osg::ref_ptr<osg::Geode> gridNode;
     osg::ref_ptr<osg::Geode> trailNode;
     osg::ref_ptr<osg::Geometry> trailGeometry;
@@ -116,6 +137,13 @@ private:
     osg::ref_ptr<osg::Geode> targetNode;
     osg::ref_ptr<osg::PositionAttitudeTransform> targetPosition;
     osg::ref_ptr<osg::Group> waypointsNode;
+#ifdef QGC_LIBFREENECT_ENABLED
+    osg::ref_ptr<osg::Geode> rgbdNode;
+    QScopedPointer<Freenect> freenect;
+#endif
+    QSharedPointer<QByteArray> rgb;
+    QSharedPointer<QByteArray> depth;
+    unsigned short gammaLookup[2048];
 
     QPushButton* targetButton;
 

src/ui/map3D/Q3DWidget.cc

@@ -47,7 +47,7 @@ Q3DWidget::Q3DWidget(QWidget* parent)
     , egocentricMap(new osg::Switch())
     , robotPosition(new osg::PositionAttitudeTransform())
     , robotAttitude(new osg::PositionAttitudeTransform())
-    , hudGeode(new osg::Geode())
+    , hudGroup(new osg::Switch())
     , hudProjectionMatrix(new osg::Projection)
 {
     // set initial camera parameters
@@ -159,11 +159,12 @@ Q3DWidget::createHUD(void)
     hudModelViewMatrix->setReferenceFrame(osg::Transform::ABSOLUTE_RF);
 
     hudProjectionMatrix->addChild(hudModelViewMatrix);
-    hudModelViewMatrix->addChild(hudGeode);
+    hudModelViewMatrix->addChild(hudGroup);
 
     osg::ref_ptr<osg::StateSet> hudStateSet(new osg::StateSet);
-    hudGeode->setStateSet(hudStateSet);
+    hudGroup->setStateSet(hudStateSet);
     hudStateSet->setMode(GL_DEPTH_TEST, osg::StateAttribute::OFF);
+    hudStateSet->setMode(GL_LIGHTING, osg::StateAttribute::OFF);
     hudStateSet->setRenderingHint(osg::StateSet::TRANSPARENT_BIN);
     hudStateSet->setRenderBinDetails(11, "RenderBin");
 

src/ui/map3D/Q3DWidget.h

@@ -249,7 +249,7 @@ protected:
     osg::ref_ptr<osg::PositionAttitudeTransform> robotPosition;
     osg::ref_ptr<osg::PositionAttitudeTransform> robotAttitude;
 
-    osg::ref_ptr<osg::Geode> hudGeode; /**< A geode which contains renderable HUD objects. */
+    osg::ref_ptr<osg::Switch> hudGroup; /**< A group which contains renderable HUD objects. */
     osg::ref_ptr<osg::Projection> hudProjectionMatrix; /**< An orthographic projection matrix for HUD display. */
 
     osg::ref_ptr<osgViewer::GraphicsWindowEmbedded> osgGW; /**< A class which manages OSG graphics windows and events. */