Added vehicle model selection to 3D view perspective.

qgroundcontrol.pri

@@ -86,7 +86,8 @@ macx {
     DEPENDENCIES_PRESENT += osgearth
     # Include osgEarth libraries
     LIBS += -losgViewer \
-            -losgEarth
+            -losgEarth \
+            -losgEarthUtil
     DEFINES += QGC_OSGEARTH_ENABLED
     }
 
@@ -155,7 +156,8 @@ linux-g++ {
     DEPENDENCIES_PRESENT += osgearth
     # Include osgEarth libraries
     LIBS += -losgViewer \
-            -losgEarth
+            -losgEarth \
+            -losgEarthUtil
     DEFINES += QGC_OSGEARTH_ENABLED
     }
 
@@ -227,7 +229,8 @@ linux-g++-64 {
     DEPENDENCIES_PRESENT += osgearth
     # Include osgEarth libraries
     LIBS += -losgViewer \
-            -losgEarth
+            -losgEarth \
+            -losgEarthUtil
     DEFINES += QGC_OSGEARTH_ENABLED
     }
 

src/input/Freenect.cc

 #include "Freenect.h"
 
+#include <cmath>
 #include <string.h>
 #include <QDebug>
 
@@ -8,7 +9,12 @@ Freenect::Freenect()
     , device(NULL)
     , tiltAngle(0)
 {
-
+    for (int i = 0; i < 2048; ++i)
+    {
+        float v = static_cast<float>(i) / 2048.0f;
+        v = powf(v, 3.0f) * 6.0f;
+        gammaTable[i] = static_cast<unsigned short>(v * 6.0f * 256.0f);
+    }
 }
 
 Freenect::~Freenect()
@@ -102,14 +108,33 @@ QSharedPointer<QByteArray>
 Freenect::getRgbData(void)
 {
     QMutexLocker locker(&rgbMutex);
-    return QSharedPointer<QByteArray>(new QByteArray(rgb, FREENECT_RGB_SIZE));
+    return QSharedPointer<QByteArray>(
+            new QByteArray(rgb, FREENECT_RGB_SIZE));
 }
 
 QSharedPointer<QByteArray>
-Freenect::getDepthData(void)
+Freenect::getRawDepthData(void)
 {
     QMutexLocker locker(&depthMutex);
-    return QSharedPointer<QByteArray>(new QByteArray(depth, FREENECT_DEPTH_SIZE));
+    return QSharedPointer<QByteArray>(
+            new QByteArray(depth, FREENECT_DEPTH_SIZE));
+}
+
+QSharedPointer<QByteArray>
+Freenect::getDistanceData(void)
+{
+    QMutexLocker locker(&distanceMutex);
+    return QSharedPointer<QByteArray>(
+            new QByteArray(reinterpret_cast<char *>(distance),
+                           FREENECT_FRAME_PIX * sizeof(float)));
+}
+
+QSharedPointer<QByteArray>
+Freenect::getColoredDepthData(void)
+{
+    QMutexLocker locker(&coloredDepthMutex);
+    return QSharedPointer<QByteArray>(
+            new QByteArray(coloredDepth, FREENECT_RGB_SIZE));
 }
 
 int
@@ -165,6 +190,60 @@ Freenect::depthCallback(freenect_device* device, void* depth,
     Freenect* freenect = static_cast<Freenect *>(freenect_get_user(device));
     freenect_depth* data = reinterpret_cast<freenect_depth *>(depth);
 
-    QMutexLocker locker(&freenect->depthMutex);
+    QMutexLocker depthLocker(&freenect->depthMutex);
     memcpy(freenect->depth, data, FREENECT_DEPTH_SIZE);
+
+    QMutexLocker distanceLocker(&freenect->distanceMutex);
+    for (int i = 0; i < FREENECT_FRAME_PIX; ++i)
+    {
+        freenect->distance[i] =
+                100.f / (-0.00307f * static_cast<float>(data[i]) + 3.33f);
+    }
+
+    QMutexLocker coloredDepthLocker(&freenect->coloredDepthMutex);
+    unsigned short* src = reinterpret_cast<unsigned short *>(data);
+    unsigned char* dst = reinterpret_cast<unsigned char *>(freenect->coloredDepth);
+    for (int i = 0; i < FREENECT_FRAME_PIX; ++i)
+    {
+        unsigned short pval = freenect->gammaTable[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;
+        }
+    }
 }

src/input/Freenect.h

@@ -17,7 +17,9 @@ public:
     bool process(void);
 
     QSharedPointer<QByteArray> getRgbData(void);
-    QSharedPointer<QByteArray> getDepthData(void);
+    QSharedPointer<QByteArray> getRawDepthData(void);
+    QSharedPointer<QByteArray> getDistanceData(void);
+    QSharedPointer<QByteArray> getColoredDepthData(void);
 
     int getTiltAngle(void) const;
     void setTiltAngle(int angle);
@@ -53,9 +55,18 @@ private:
     char depth[FREENECT_DEPTH_SIZE];
     QMutex depthMutex;
 
+    float distance[FREENECT_FRAME_PIX];
+    QMutex distanceMutex;
+
+    char coloredDepth[FREENECT_RGB_SIZE];
+    QMutex coloredDepthMutex;
+
     // accelerometer data
     short ax, ay, az;
     double dx, dy, dz;
+
+    // gamma map
+    unsigned short gammaTable[2048];
 };
 
 #endif // FREENECT_H

src/ui/MainWindow.cc

@@ -47,7 +47,7 @@ This file is part of the QGROUNDCONTROL project
 #include "GAudioOutput.h"
 
 #ifdef QGC_OSG_ENABLED
-#include "QMap3D.h"
+#include "Q3DWidgetFactory.h"
 #endif
 
 // FIXME Move
@@ -136,8 +136,8 @@ void MainWindow::buildWidgets()
     protocolWidget    = new XMLCommProtocolWidget(this);
     dataplotWidget    = new QGCDataPlot2D(this);
     #ifdef QGC_OSG_ENABLED
-    //_3DWidget         = Q3DWidgetFactory::get(QGC_MAP3D_OSGEARTH);
-    _3DWidget = new QMap3D(this);
+    _3DWidget         = Q3DWidgetFactory::get("PIXHAWK");
+    //_3DWidget = Q3DWidgetFactory::get("MAP3D");
     #endif
 
     // Dock widgets
@@ -672,7 +672,7 @@ void MainWindow::loadPixhawkView()
     clearView();
     // Engineer view, used in EMAV2009
 
-        #ifdef QGC_OSG_ENABLED
+#ifdef QGC_OSG_ENABLED
     // 3D map
     if (_3DWidget)
     {

src/ui/MainWindow.h

@@ -167,7 +167,7 @@ protected:
     QPointer<XMLCommProtocolWidget> protocolWidget;
     QPointer<QGCDataPlot2D> dataplotWidget;
     #ifdef QGC_OSG_ENABLED
-    QPointer<QMap3D> _3DWidget;
+    QPointer<QWidget> _3DWidget;
     #endif
     // Dock widgets
     QPointer<QDockWidget> controlDockWidget;

src/ui/map3D/Pixhawk3DWidget.h

@@ -37,9 +37,8 @@
 #include <osgEarth/MapNode>
 #endif
 
-#ifdef QGC_OSG_ENABLED
 #include "ImageWindowGeode.h"
-#endif
+
 #ifdef QGC_LIBFREENECT_ENABLED
 #include "Freenect.h"
 #endif
@@ -59,10 +58,6 @@ public:
     explicit Pixhawk3DWidget(QWidget* parent = 0);
     ~Pixhawk3DWidget();
 
-    void buildLayout(void);
-
-    double getTime(void) const;
-
 public slots:
     void setActiveUAS(UASInterface* uas);
 
@@ -70,10 +65,13 @@ private slots:
     void showGrid(int state);
     void showTrail(int state);
     void showWaypoints(int state);
+    void selectVehicleModel(int index);
     void recenter(void);
     void toggleFollowCamera(int state);
 
 protected:
+    QVector< osg::ref_ptr<osg::Node> > findVehicleModels(void);
+    void buildLayout(void);
     virtual void display(void);
     virtual void keyPressEvent(QKeyEvent* event);
     virtual void mousePressEvent(QMouseEvent* event);
@@ -90,10 +88,7 @@ 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
+    osg::ref_ptr<osg::Geode> createRGBD3D(void);
 
     void setupHUD(void);
     void resizeHUD(void);
@@ -121,6 +116,7 @@ private:
     osg::ref_ptr<osg::Vec3Array> trailVertices;
     QVarLengthArray<osg::Vec3, 10000> trail;
 
+    osg::ref_ptr<osg::Node> vehicleModel;
     osg::ref_ptr<osg::Geometry> hudBackgroundGeometry;
     osg::ref_ptr<osgText::Text> statusText;
     osg::ref_ptr<ImageWindowGeode> rgb2DGeode;
@@ -137,13 +133,14 @@ private:
     osg::ref_ptr<osg::Geode> targetNode;
     osg::ref_ptr<osg::PositionAttitudeTransform> targetPosition;
     osg::ref_ptr<osg::Group> waypointsNode;
+    osg::ref_ptr<osg::Geode> rgbd3DNode;
 #ifdef QGC_LIBFREENECT_ENABLED
-    osg::ref_ptr<osg::Geode> rgbdNode;
     QScopedPointer<Freenect> freenect;
 #endif
     QSharedPointer<QByteArray> rgb;
-    QSharedPointer<QByteArray> depth;
-    unsigned short gammaLookup[2048];
+    QSharedPointer<QByteArray> coloredDepth;
+
+    QVector< osg::ref_ptr<osg::Node> > vehicleModels;
 
     QPushButton* targetButton;
 

src/ui/map3D/PixhawkCheetahGeode.cc

@@ -59358,6 +59358,7 @@ osg::ref_ptr<osg::Geode>
 PixhawkCheetahGeode::create(float red, float green, float blue)
 {
     osg::ref_ptr<osg::Geode> geode(new osg::Geode());
+    geode->setName("Pixhawk Bravo");
     osg::ref_ptr<osg::Geometry> geometry(new osg::Geometry());
     geode->addDrawable(geometry.get());
 

src/ui/map3D/Q3DWidgetFactory.cc

@@ -32,16 +32,21 @@ This file is part of the QGROUNDCONTROL project
 #include "Q3DWidgetFactory.h"
 
 #include "Pixhawk3DWidget.h"
+#include "QMap3D.h"
 
-QPointer<Q3DWidget>
+QPointer<QWidget>
 Q3DWidgetFactory::get(const std::string& type)
 {
     if (type == "PIXHAWK")
     {
-        return QPointer<Q3DWidget>(new Pixhawk3DWidget);
+        return QPointer<QWidget>(new Pixhawk3DWidget);
+    }
+    else if (type == "MAP3D")
+    {
+        return QPointer<QWidget>(new QMap3D);
     }
     else
     {
-        return QPointer<Q3DWidget>(new Q3DWidget);
+        return QPointer<QWidget>(new Q3DWidget);
     }
 }

src/ui/map3D/Q3DWidgetFactory.h

@@ -50,7 +50,7 @@ public:
      * @return A smart pointer to the Q3DWidget instance.
      */
 
-    static QPointer<Q3DWidget> get(const std::string& type);
+    static QPointer<QWidget> get(const std::string& type);
 };
 
 #endif // Q3DWIDGETFACTORY_H