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 -----")
@@ -79,10 +78,10 @@ contains(MAVLINK_CONF, ualberta) {
     INCLUDEPATH += $$BASEDIR/../mavlink/include/ualberta
     DEFINES += QGC_USE_UALBERTA_MESSAGES
 }
-contains(MAVLINK_CONF, ardupilotmega) {
+contains(MAVLINK_CONF, ardupilotmega) { 
     # Remove the default set - it is included anyway
     INCLUDEPATH -= $$BASEDIR/../mavlink/include/common
-
+    
     # UALBERTA SPECIAL MESSAGES
     INCLUDEPATH += $$BASEDIR/../mavlink/include/ardupilotmega
     DEFINES += QGC_USE_ARDUPILOTMEGA_MESSAGES
@@ -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
-
-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
+    src/comm/QGCMAVLink.h \
+    src/ui/map3D/ImageWindowGeode.h
+contains(DEPENDENCIES_PRESENT, osg) { 
+    message("Including headers for OpenSceneGraph")
+    
+    # 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 \
-
-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
+    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")
+    
+    # Enable only if libfreenect is available
+    SOURCES += src/input/Freenect.cc
 }
 RESOURCES += mavground.qrc
 

src/input/Freenect.cc

+#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

+#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

+///*=====================================================================
+//
+//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

+///*=====================================================================
+//
+//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.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)