Re-implemented 3D imagery feature in OSG. Still a few outstanding issues to resolve.

qgroundcontrol.pro

@@ -230,8 +230,6 @@ HEADERS += src/MG.h \
     src/ui/RadioCalibration/CurveCalibrator.h \
     src/ui/RadioCalibration/AbstractCalibrator.h \
     src/comm/QGCMAVLink.h
-
-
 contains(DEPENDENCIES_PRESENT, osg) { 
     message("Including headers for OpenSceneGraph")
     
@@ -242,23 +240,25 @@ contains(DEPENDENCIES_PRESENT, osg) {
         src/ui/map3D/QOSGWidget.h \
         src/ui/map3D/PixhawkCheetahGeode.h \
         src/ui/map3D/Pixhawk3DWidget.h \
-        src/ui/map3D/Q3DWidgetFactory.h
+        src/ui/map3D/Q3DWidgetFactory.h \
-
+        src/ui/map3D/WebImageCache.h \
-contains(DEPENDENCIES_PRESENT, osgearth) { 
+        src/ui/map3D/WebImage.h \
-    message("Including headers for OSGEARTH")
+        src/ui/map3D/TextureCache.h \
-    
+        src/ui/map3D/Texture.h \
-    # Enable only if OpenSceneGraph is available
+        src/ui/map3D/Imagery.h
-    HEADERS += src/ui/map3D/QMap3D.h
+    contains(DEPENDENCIES_PRESENT, osgearth) { 
-}
+        message("Including headers for OSGEARTH")
+        
+        # Enable only if OpenSceneGraph is available
+        HEADERS += src/ui/map3D/QMap3D.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 \
@@ -328,38 +328,35 @@ SOURCES += src/main.cc \
     src/ui/RadioCalibration/CurveCalibrator.cc \
     src/ui/RadioCalibration/AbstractCalibrator.cc \
     src/ui/RadioCalibration/RadioCalibrationData.cc
-
 contains(DEPENDENCIES_PRESENT, osg) { 
     message("Including sources for OpenSceneGraph")
     
     # Enable only if OpenSceneGraph is available
     SOURCES += src/ui/map3D/Q3DWidget.cc \
-    src/ui/map3D/ImageWindowGeode.cc \
+        src/ui/map3D/ImageWindowGeode.cc \
-    src/ui/map3D/GCManipulator.cc \
+        src/ui/map3D/GCManipulator.cc \
-    src/ui/map3D/QOSGWidget.cc \
+        src/ui/map3D/QOSGWidget.cc \
         src/ui/map3D/PixhawkCheetahGeode.cc \
         src/ui/map3D/Pixhawk3DWidget.cc \
-        src/ui/map3D/Q3DWidgetFactory.cc
+        src/ui/map3D/Q3DWidgetFactory.cc \
-
+        src/ui/map3D/WebImageCache.cc \
-
+        src/ui/map3D/WebImage.cc \
-
+        src/ui/map3D/TextureCache.cc \
-contains(DEPENDENCIES_PRESENT, osgearth) { 
+        src/ui/map3D/Texture.cc \
-    message("Including sources for osgEarth")
+        src/ui/map3D/Imagery.cc
-    
+    contains(DEPENDENCIES_PRESENT, osgearth) { 
-    # Enable only if OpenSceneGraph is available
+        message("Including sources for osgEarth")
-    SOURCES += src/ui/map3D/QMap3D.cc
+        
-
+        # Enable only if OpenSceneGraph is available
-}
+        SOURCES += src/ui/map3D/QMap3D.cc
+    }
 }
-
+contains(DEPENDENCIES_PRESENT, libfreenect) { 
-contains(DEPENDENCIES_PRESENT, libfreenect) {
     message("Including sources for libfreenect")
-
+    
     # Enable only if libfreenect is available
     SOURCES += src/input/Freenect.cc
 }
-
-
 RESOURCES += mavground.qrc
 
 # Include RT-LAB Library

src/ui/map3D/Imagery.cc

@@ -46,6 +46,12 @@ Imagery::Imagery()
 
 }
 
+Imagery::ImageryType
+Imagery::getImageryType(void) const
+{
+    return currentImageryType;
+}
+
 void
 Imagery::setImageryType(ImageryType type)
 {
@@ -62,7 +68,6 @@ Imagery::setOffset(double xOffset, double yOffset)
 void
 Imagery::prefetch2D(double windowWidth, double windowHeight,
                     double zoom, double xOrigin, double yOrigin,
-                    double viewXOffset, double viewYOffset,
                     const QString& utmZone)
 {
     double tileResolution;
@@ -88,10 +93,10 @@ Imagery::prefetch2D(double windowWidth, double windowHeight,
     int zoomLevel;
 
     tileBounds(tileResolution,
-               xOrigin + viewXOffset - windowWidth / 2.0 / zoom,
+               xOrigin - windowWidth / 2.0 / zoom * 1.5,
-               yOrigin + viewYOffset - windowHeight / 2.0 / zoom,
+               yOrigin - windowHeight / 2.0 / zoom * 1.5,
-               xOrigin + viewXOffset + windowWidth / 2.0 / zoom,
+               xOrigin + windowWidth / 2.0 / zoom * 1.5,
-               yOrigin + viewYOffset + windowHeight / 2.0 / zoom, utmZone,
+               yOrigin + windowHeight / 2.0 / zoom * 1.5, utmZone,
                minTileX, minTileY, maxTileX, maxTileY, zoomLevel);
 
     for (int r = minTileY; r <= maxTileY; ++r)
@@ -108,7 +113,6 @@ Imagery::prefetch2D(double windowWidth, double windowHeight,
 void
 Imagery::draw2D(double windowWidth, double windowHeight,
                 double zoom, double xOrigin, double yOrigin,
-                double viewXOffset, double viewYOffset,
                 const QString& utmZone)
 {
     double tileResolution;
@@ -134,12 +138,17 @@ Imagery::draw2D(double windowWidth, double windowHeight,
     int zoomLevel;
 
     tileBounds(tileResolution,
-               xOrigin + viewXOffset - windowWidth / 2.0 / zoom * 1.5,
+               xOrigin - windowWidth / 2.0 / zoom * 1.5,
-               yOrigin + viewYOffset - windowHeight / 2.0 / zoom * 1.5,
+               yOrigin - windowHeight / 2.0 / zoom * 1.5,
-               xOrigin + viewXOffset + windowWidth / 2.0 / zoom * 1.5,
+               xOrigin + windowWidth / 2.0 / zoom * 1.5,
-               yOrigin + viewYOffset + windowHeight / 2.0 / zoom * 1.5, utmZone,
+               yOrigin + windowHeight / 2.0 / zoom * 1.5, utmZone,
                minTileX, minTileY, maxTileX, maxTileY, zoomLevel);
 
+    if (getNumDrawables() > 0)
+    {
+        removeDrawables(0, getNumDrawables());
+    }
+
     for (int r = minTileY; r <= maxTileY; ++r)
     {
         for (int c = minTileX; c <= maxTileX; ++c)
@@ -152,10 +161,11 @@ Imagery::draw2D(double windowWidth, double windowHeight,
             TexturePtr t = textureCache->get(tileURL);
             if (!t.isNull())
             {
-                t->draw(x1 - xOrigin, y1 - yOrigin,
+                addDrawable(t->draw(y1 - yOrigin, x1 - xOrigin,
-                        x2 - xOrigin, y2 - yOrigin,
+                                    y2 - yOrigin, x2 - xOrigin,
-                        x3 - xOrigin, y3 - yOrigin,
+                                    y3 - yOrigin, x3 - xOrigin,
-                        x4 - xOrigin, y4 - yOrigin, true);
+                                    y4 - yOrigin, x4 - xOrigin,
+                                    true));
             }
         }
     }
@@ -164,17 +174,14 @@ Imagery::draw2D(double windowWidth, double windowHeight,
 void
 Imagery::prefetch3D(double radius, double tileResolution,
                     double xOrigin, double yOrigin,
-                    double viewXOffset, double viewYOffset,
+                    const QString& utmZone)
-                    const QString& utmZone, bool useHeightModel)
 {
     int minTileX, minTileY, maxTileX, maxTileY;
     int zoomLevel;
 
     tileBounds(tileResolution,
-               xOrigin + viewXOffset - radius,
+               xOrigin - radius, yOrigin - radius,
-               yOrigin + viewYOffset - radius,
+               xOrigin + radius, yOrigin + radius, utmZone,
-               xOrigin + viewXOffset + radius,
-               yOrigin + viewYOffset + radius, utmZone,
                minTileX, minTileY, maxTileX, maxTileY, zoomLevel);
 
     for (int r = minTileY; r <= maxTileY; ++r)
@@ -183,7 +190,7 @@ Imagery::prefetch3D(double radius, double tileResolution,
         {
             QString url = getTileLocation(c, r, zoomLevel, tileResolution);
 
-            TexturePtr t = textureCache->get(url, useHeightModel);
+            TexturePtr t = textureCache->get(url);
         }
     }
 }
@@ -191,19 +198,21 @@ Imagery::prefetch3D(double radius, double tileResolution,
 void
 Imagery::draw3D(double radius, double tileResolution,
                 double xOrigin, double yOrigin,
-                double viewXOffset, double viewYOffset,
+                const QString& utmZone)
-                const QString& utmZone, bool useHeightModel)
 {
     int minTileX, minTileY, maxTileX, maxTileY;
     int zoomLevel;
 
     tileBounds(tileResolution,
-               xOrigin + viewXOffset - radius,
+               xOrigin - radius, yOrigin - radius,
-               yOrigin + viewYOffset - radius,
+               xOrigin + radius, yOrigin + radius, utmZone,
-               xOrigin + viewXOffset + radius,
-               yOrigin + viewYOffset + radius, utmZone,
                minTileX, minTileY, maxTileX, maxTileY, zoomLevel);
 
+    if (getNumDrawables() > 0)
+    {
+        removeDrawables(0, getNumDrawables());
+    }
+
     for (int r = minTileY; r <= maxTileY; ++r)
     {
         for (int c = minTileX; c <= maxTileX; ++c)
@@ -213,14 +222,15 @@ Imagery::draw3D(double radius, double tileResolution,
             double x1, y1, x2, y2, x3, y3, x4, y4;
             imageBounds(c, r, tileResolution, x1, y1, x2, y2, x3, y3, x4, y4);
 
-            TexturePtr t = textureCache->get(tileURL, useHeightModel);
+            TexturePtr t = textureCache->get(tileURL);
 
             if (!t.isNull())
             {
-                t->draw(x1 - xOrigin, y1 - yOrigin,
+                addDrawable(t->draw(y1 - yOrigin, x1 - xOrigin,
-                        x2 - xOrigin, y2 - yOrigin,
+                                    y2 - yOrigin, x2 - xOrigin,
-                        x3 - xOrigin, y3 - yOrigin,
+                                    y3 - yOrigin, x3 - xOrigin,
-                        x4 - xOrigin, y4 - yOrigin, true);
+                                    y4 - yOrigin, x4 - xOrigin,
+                                    true));
             }
         }
     }
@@ -257,8 +267,7 @@ Imagery::imageBounds(int tileX, int tileY, double tileResolution,
         LLtoUTM(lat2, lon2, x3, y3, utmZone);
         LLtoUTM(lat2, lon1, x4, y4, utmZone);
     }
-    else if (currentImageryType == SWISSTOPO_SATELLITE ||
+    else if (currentImageryType == SWISSTOPO_SATELLITE)
-             currentImageryType == SWISSTOPO_SATELLITE_3D)
     {
         double utmMultiplier = tileResolution * 200.0;
         double minX = tileX * utmMultiplier;
@@ -295,8 +304,7 @@ Imagery::tileBounds(double tileResolution,
         UTMtoTile(maxUtmX, maxUtmY, utmZone, tileResolution,
                   maxTileX, minTileY, zoomLevel);
     }
-    else if (currentImageryType == SWISSTOPO_SATELLITE ||
+    else if (currentImageryType == SWISSTOPO_SATELLITE)
-             currentImageryType == SWISSTOPO_SATELLITE_3D)
     {
         double utmMultiplier = tileResolution * 200;
 
@@ -555,7 +563,7 @@ Imagery::UTMtoLL(double utmNorthing, double utmEasting, const QString& utmZone,
                  * D * D * D * D * D / 120.0) / cos(phi1Rad);
     longitude = LongOrigin + longitude / M_PI * 180.0;
 }
-
+#include <QDebug>
 QString
 Imagery::getTileLocation(int tileX, int tileY, int zoomLevel,
                          double tileResolution) const
@@ -572,7 +580,7 @@ Imagery::getTileLocation(int tileX, int tileY, int zoomLevel,
         oss << "http://khm.google.com/vt/lbw/lyrs=y&x=" << tileX
             << "&y=" << tileY << "&z=" << zoomLevel;
         break;
-    case SWISSTOPO_SATELLITE: case SWISSTOPO_SATELLITE_3D:
+    case SWISSTOPO_SATELLITE:
         oss << "../map/eth_zurich_swissimage_025/200/color/" << tileY
             << "/tile-";
         if (tileResolution < 1.0)

src/ui/map3D/Imagery.h

@@ -32,44 +32,42 @@ This file is part of the QGROUNDCONTROL project
 #ifndef IMAGERY_H
 #define IMAGERY_H
 
+#include <osg/Geode>
 #include <QScopedPointer>
 #include <QString>
 
 #include "TextureCache.h"
 
-class Imagery
+class Imagery : public osg::Geode
 {
 public:
     enum ImageryType
     {
-        GOOGLE_MAP = 0,
+        BLANK_MAP = 0,
-        GOOGLE_SATELLITE = 1,
+        GOOGLE_MAP = 1,
-        SWISSTOPO_SATELLITE = 2,
+        GOOGLE_SATELLITE = 2,
-        SWISSTOPO_SATELLITE_3D = 3
+        SWISSTOPO_SATELLITE = 3
     };
 
     Imagery();
 
+    ImageryType getImageryType(void) const;
     void setImageryType(ImageryType type);
     void setOffset(double xOffset, double yOffset);
 
     void prefetch2D(double windowWidth, double windowHeight,
                     double zoom, double xOrigin, double yOrigin,
-                    double viewXOffset, double viewYOffset,
                     const QString& utmZone);
     void draw2D(double windowWidth, double windowHeight,
                 double zoom, double xOrigin, double yOrigin,
-                double viewXOffset, double viewYOffset,
                 const QString& utmZone);
 
     void prefetch3D(double radius, double tileResolution,
                     double xOrigin, double yOrigin,
-                    double viewXOffset, double viewYOffset,
+                    const QString& utmZone);
-                    const QString& utmZone, bool useHeightModel);
     void draw3D(double radius, double tileResolution,
                 double xOrigin, double yOrigin,
-                double viewXOffset, double viewYOffset,
+                const QString& utmZone);
-                const QString& utmZone, bool useHeightModel);
 
     bool update(void);
 

src/ui/map3D/Pixhawk3DWidget.cc

@@ -49,6 +49,7 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
      , uas(NULL)
      , displayGrid(true)
      , displayTrail(false)
+     , displayImagery(true)
      , displayTarget(false)
      , displayWaypoints(true)
      , displayRGBD2D(false)
@@ -74,11 +75,9 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
     trailNode = createTrail();
     rollingMap->addChild(trailNode);
 
-#ifdef QGC_OSGEARTH_ENABLED
     // generate map model
     mapNode = createMap();
-    root->addChild(mapNode);
+    allocentricMap->addChild(mapNode);
-#endif
 
     // generate target model
     allocentricMap->addChild(createTarget());
@@ -172,6 +171,12 @@ Pixhawk3DWidget::showWaypoints(int state)
 }
 
 void
+Pixhawk3DWidget::selectMapSource(int index)
+{
+    mapNode->setImageryType(static_cast<Imagery::ImageryType>(index));
+}
+
+void
 Pixhawk3DWidget::selectVehicleModel(int index)
 {
     egocentricMap->removeChild(vehicleModel);
@@ -205,7 +210,7 @@ Pixhawk3DWidget::toggleFollowCamera(int32_t state)
         followCamera = false;
     }
 }
-#include <osgDB/WriteFile>
+
 QVector< osg::ref_ptr<osg::Node> >
 Pixhawk3DWidget::findVehicleModels(void)
 {
@@ -273,6 +278,12 @@ Pixhawk3DWidget::buildLayout(void)
     waypointsCheckBox->setText("Waypoints");
     waypointsCheckBox->setChecked(displayWaypoints);
 
+    QLabel* mapLabel = new QLabel("Map", this);
+    QComboBox* mapComboBox = new QComboBox(this);
+    mapComboBox->addItem("None");
+    mapComboBox->addItem("Map (Google)");
+    mapComboBox->addItem("Satellite (Google)");
+
     QLabel* modelLabel = new QLabel("Vehicle Model", this);
     QComboBox* modelComboBox = new QComboBox(this);
     for (int i = 0; i < vehicleModels.size(); ++i)
@@ -299,12 +310,14 @@ Pixhawk3DWidget::buildLayout(void)
     layout->addWidget(trailCheckBox, 1, 1);
     layout->addWidget(waypointsCheckBox, 1, 2);
     layout->addItem(new QSpacerItem(10, 0, QSizePolicy::Expanding, QSizePolicy::Expanding), 1, 3);
-    layout->addWidget(modelLabel, 1, 4);
+    layout->addWidget(mapLabel, 1, 4);
-    layout->addWidget(modelComboBox, 1, 5);
+    layout->addWidget(mapComboBox, 1, 5);
-    layout->addWidget(targetButton, 1, 6);
+    layout->addWidget(modelLabel, 1, 6);
-    layout->addItem(new QSpacerItem(10, 0, QSizePolicy::Expanding, QSizePolicy::Expanding), 1, 7);
+    layout->addWidget(modelComboBox, 1, 7);
-    layout->addWidget(recenterButton, 1, 8);
+    layout->addWidget(targetButton, 1, 8);
-    layout->addWidget(followCameraCheckBox, 1, 9);
+    layout->addItem(new QSpacerItem(10, 0, QSizePolicy::Expanding, QSizePolicy::Expanding), 1, 9);
+    layout->addWidget(recenterButton, 1, 10);
+    layout->addWidget(followCameraCheckBox, 1, 11);
     layout->setRowStretch(0, 100);
     layout->setRowStretch(1, 1);
     setLayout(layout);
@@ -315,6 +328,8 @@ Pixhawk3DWidget::buildLayout(void)
             this, SLOT(showTrail(int)));
     connect(waypointsCheckBox, SIGNAL(stateChanged(int)),
             this, SLOT(showWaypoints(int)));
+    connect(mapComboBox, SIGNAL(currentIndexChanged(int)),
+            this, SLOT(selectMapSource(int)));
     connect(modelComboBox, SIGNAL(currentIndexChanged(int)),
             this, SLOT(selectVehicleModel(int)));
     connect(recenterButton, SIGNAL(clicked()), this, SLOT(recenter()));
@@ -362,20 +377,42 @@ Pixhawk3DWidget::display(void)
                                          robotPitch, osg::Vec3f(1.0f, 0.0f, 0.0f),
                                          robotRoll, osg::Vec3f(0.0f, 1.0f, 0.0f)));
 
-    updateTrail(robotX, robotY, robotZ);
+    if (displayTrail)
-    updateTarget();
+    {
-    updateWaypoints();
+        updateTrail(robotX, robotY, robotZ);
+    }
+
+    if (displayImagery)
+    {
+        updateImagery();
+    }
+
+    if (displayTarget)
+    {
+        updateTarget();
+    }
+
+    if (displayWaypoints)
+    {
+        updateWaypoints();
+    }
+
 #ifdef QGC_LIBFREENECT_ENABLED
-    updateRGBD();
+    if (displayRGBD2D || displayRGBD3D)
+    {
+        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);
     egocentricMap->setChildValue(rgbd3DNode, displayRGBD3D);
+    allocentricMap->setChildValue(mapNode, displayImagery);
     hudGroup->setChildValue(rgb2DGeode, displayRGBD2D);
     hudGroup->setChildValue(depth2DGeode, displayRGBD2D);
 
@@ -400,6 +437,9 @@ Pixhawk3DWidget::keyPressEvent(QKeyEvent* event)
         case 'c': case 'C':
             enableRGBDColor = !enableRGBDColor;
             break;
+        case 'i': case 'I':
+            displayImagery = !displayImagery;
+            break;
         }
     }
 
@@ -512,16 +552,11 @@ Pixhawk3DWidget::createTrail(void)
     return geode;
 }
 
-#ifdef QGC_OSGEARTH_ENABLED
+osg::ref_ptr<Imagery>
-osg::ref_ptr<osgEarth::MapNode>
 Pixhawk3DWidget::createMap(void)
 {
-    osg::ref_ptr<osg::Node> model = osgDB::readNodeFile("map.earth");
+    return osg::ref_ptr<Imagery>(new Imagery());
-    osg::ref_ptr<osgEarth::MapNode> node = osgEarth::MapNode::findMapNode(model);
-
-    return node;
 }
-#endif
 
 osg::ref_ptr<osg::Node>
 Pixhawk3DWidget::createTarget(void)
@@ -731,6 +766,74 @@ Pixhawk3DWidget::updateTrail(float robotX, float robotY, float robotZ)
 }
 
 void
+Pixhawk3DWidget::updateImagery(void)
+{
+    char zone[5] = "32T";
+
+    double viewingRadius = cameraManipulator->getDistance() * 10.0;
+    if (viewingRadius < 100.0)
+    {
+        viewingRadius = 100.0;
+    }
+
+    double minResolution = 0.25;
+    double centerResolution = cameraManipulator->getDistance() / 25.0;
+    double maxResolution = 1048576.0;
+
+    Imagery::ImageryType imageryType = mapNode->getImageryType();
+    switch (imageryType)
+    {
+    case Imagery::GOOGLE_MAP:
+        minResolution = 0.25;
+        break;
+    case Imagery::GOOGLE_SATELLITE:
+        minResolution = 0.5;
+        break;
+    case Imagery::SWISSTOPO_SATELLITE:
+        minResolution = 0.25;
+        maxResolution = 0.25;
+        break;
+    default: {}
+    }
+
+    double resolution = minResolution;
+    while (resolution * 2.0 < centerResolution)
+    {
+        resolution *= 2.0;
+    }
+    if (resolution > maxResolution)
+    {
+        resolution = maxResolution;
+    }
+
+    mapNode->draw3D(viewingRadius,
+                    resolution,
+                    cameraManipulator->getCenter().y(),
+                    cameraManipulator->getCenter().x(),
+                    zone);
+
+    // prefetch map tiles
+    if (resolution / 2.0 >= minResolution)
+    {
+        mapNode->prefetch3D(viewingRadius / 2.0,
+                            resolution / 2.0,
+                            cameraManipulator->getCenter().y(),
+                            cameraManipulator->getCenter().x(),
+                            zone);
+    }
+    if (resolution * 2.0 <= maxResolution)
+    {
+        mapNode->prefetch3D(viewingRadius * 2.0,
+                            resolution * 2.0,
+                            cameraManipulator->getCenter().y(),
+                            cameraManipulator->getCenter().x(),
+                            zone);
+    }
+
+    mapNode->update();
+}
+
+void
 Pixhawk3DWidget::updateTarget(void)
 {
     static double radius = 0.2;

src/ui/map3D/Pixhawk3DWidget.h

@@ -33,10 +33,8 @@
 #define PIXHAWK3DWIDGET_H
 
 #include <osgText/Text>
-#ifdef QGC_OSGEARTH_ENABLED
-#include <osgEarth/MapNode>
-#endif
 
+#include "Imagery.h"
 #include "ImageWindowGeode.h"
 
 #ifdef QGC_LIBFREENECT_ENABLED
@@ -65,6 +63,7 @@ private slots:
     void showGrid(int state);
     void showTrail(int state);
     void showWaypoints(int state);
+    void selectMapSource(int index);
     void selectVehicleModel(int index);
     void recenter(void);
     void toggleFollowCamera(int state);
@@ -81,11 +80,7 @@ protected:
 private:
     osg::ref_ptr<osg::Geode> createGrid(void);
     osg::ref_ptr<osg::Geode> createTrail(void);
-
+    osg::ref_ptr<Imagery> createMap(void);
-#ifdef QGC_OSGEARTH_ENABLED
-    osg::ref_ptr<osgEarth::MapNode> createMap(void);
-#endif
-
     osg::ref_ptr<osg::Node> createTarget(void);
     osg::ref_ptr<osg::Group> createWaypoints(void);
     osg::ref_ptr<osg::Geode> createRGBD3D(void);
@@ -96,6 +91,7 @@ private:
     void updateHUD(float robotX, float robotY, float robotZ,
                    float robotRoll, float robotPitch, float robotYaw);
     void updateTrail(float robotX, float robotY, float robotZ);
+    void updateImagery(void);
     void updateTarget(void);
     void updateWaypoints(void);
 #ifdef QGC_LIBFREENECT_ENABLED
@@ -106,6 +102,7 @@ private:
 
     bool displayGrid;
     bool displayTrail;
+    bool displayImagery;
     bool displayTarget;
     bool displayWaypoints;
     bool displayRGBD2D;
@@ -128,9 +125,7 @@ private:
     osg::ref_ptr<osg::Geode> trailNode;
     osg::ref_ptr<osg::Geometry> trailGeometry;
     osg::ref_ptr<osg::DrawArrays> trailDrawArrays;
-#ifdef QGC_OSGEARTH_ENABLED
+    osg::ref_ptr<Imagery> mapNode;
-    osg::ref_ptr<osgEarth::MapNode> mapNode;
-#endif
     osg::ref_ptr<osg::Geode> targetNode;
     osg::ref_ptr<osg::PositionAttitudeTransform> targetPosition;
     osg::ref_ptr<osg::Group> waypointsNode;

src/ui/map3D/Texture.cc

@@ -33,18 +33,39 @@ This file is part of the QGROUNDCONTROL project
 
 #include "Texture.h"
 
-Texture::Texture()
+Texture::Texture(unsigned int _id)
-    : _is3D(false)
+    : id(_id)
+    , texture2D(new osg::Texture2D)
+    , geometry(new osg::Geometry)
 {
     texture2D->setFilter(osg::Texture::MIN_FILTER, osg::Texture::NEAREST);
     texture2D->setFilter(osg::Texture::MAG_FILTER, osg::Texture::NEAREST);
-    GLuint id;
+
-    glGenTextures(1, &id);
+    texture2D->setDataVariance(osg::Object::DYNAMIC);
-    t->setID(id);
+    texture2D->setResizeNonPowerOfTwoHint(false);
-    glBindTexture(GL_TEXTURE_2D, id);
+
-    glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
+    osg::ref_ptr<osg::Image> image = new osg::Image;
-    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
+    texture2D->setImage(image);
-    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
+
+    osg::ref_ptr<osg::Vec2Array> vertices(new osg::Vec2Array(4));
+    geometry->setVertexArray(vertices);
+
+    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));
+    geometry->setTexCoordArray(id, textureCoords);
+
+    geometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::LINES,
+                                                  0, 4));
+
+    osg::ref_ptr<osg::Vec4Array> colors(new osg::Vec4Array);
+    colors->push_back(osg::Vec4(0.0f, 0.0f, 1.0f, 1.0f));
+    geometry->setColorArray(colors);
+    geometry->setColorBinding(osg::Geometry::BIND_OVERALL);
+
+    geometry->setUseDisplayList(false);
 }
 
 const QString&
@@ -54,63 +75,32 @@ Texture::getSourceURL(void) const
 }
 
 void
-Texture::setId(unsigned int _id)
-{
-    id = _id;
-}
-
-void
 Texture::sync(const WebImagePtr& image)
 {
     state = static_cast<State>(image->getState());
 
     if (image->getState() != WebImage::UNINITIALIZED &&
-        (sourceURL != image->getSourceURL() ||
+        sourceURL != image->getSourceURL())
-         _is3D != image->is3D()))
     {
         sourceURL = image->getSourceURL();
-        _is3D = image->is3D();
     }
 
     if (image->getState() == WebImage::READY && image->getSyncFlag())
     {
         image->setSyncFlag(false);
 
-        if (image->getWidth() != imageWidth ||
+        if (texture2D->getImage() != NULL)
-            image->getHeight() != imageHeight)
         {
-            imageWidth = image->getWidth();
+            texture2D->getImage()->setImage(image->getWidth(),
-            textureWidth = 32;
+                                            image->getHeight(),
-            while (textureWidth < imageWidth)
+                                            1,
-            {
+                                            GL_RGB,
-                textureWidth *= 2;
+                                            GL_RGB,
-            }
+                                            GL_UNSIGNED_BYTE,
-            imageHeight = image->getHeight();
+                                            image->getImageData(),
-            textureHeight = 32;
+                                            osg::Image::NO_DELETE);
-            while (textureHeight < imageHeight)
+            texture2D->getImage()->dirty();
-            {
-                textureHeight *= 2;
-            }
-
-            maxU = static_cast<double>(imageWidth)
-                   / static_cast<double>(textureWidth);
-            maxV = static_cast<double>(imageHeight)
-                   / static_cast<double>(textureHeight);
-
-            osg::ref_ptr<osg::Image> image;
-            image->setImage(textureWidth, textureHeight, 8, 3, GL_RGBA, GL_UNSIGNED_BYTES, NULL, osg::Image::USE_NEW_DELETE);
-
-            texture2D->
-            glBindTexture(GL_TEXTURE_2D, id);
-            glTexImage2D(GL_TEXTURE_2D, 0, 3, textureWidth, textureHeight,
-                         0, GL_RGBA, GL_UNSIGNED_BYTE, NULL);
         }
-
-        glBindTexture(GL_TEXTURE_2D, id);
-        glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, imageWidth, imageHeight,
-                        GL_RGBA, GL_UNSIGNED_BYTE, image->getImageData());
-
-        heightModel = image->getHeightModel();
     }
 }
 
@@ -126,126 +116,45 @@ Texture::draw(float x1, float y1, float x2, float y2,
               float x3, float y3, float x4, float y4,
               bool smoothInterpolation) const
 {
-    osg::ref_ptr<osg::Geometry> geometry(new osg::Geometry);
+    osg::Vec2Array* vertices =
-    osg::ref_ptr<osg::StateSet> stateset(new osg::StateSet);
+            static_cast<osg::Vec2Array*>(geometry->getVertexArray());
+    (*vertices)[0].set(x1, y1);
+    (*vertices)[1].set(x2, y2);
+    (*vertices)[2].set(x3, y3);
+    (*vertices)[3].set(x4, y4);
+
+    osg::DrawArrays* drawarrays =
+            static_cast<osg::DrawArrays*>(geometry->getPrimitiveSet(0));
+    osg::Vec4Array* colors =
+            static_cast<osg::Vec4Array*>(geometry->getColorArray());
 
     if (state == REQUESTED)
     {
-        osg::ref_ptr<osg::Vec2Array> vertices(new osg::Vec2Array);
+        drawarrays->set(osg::PrimitiveSet::LINES, 0, 4);
-        vertices->push_back(osg::Vec2(x1, y1));
+        (*colors)[0].set(0.0f, 0.0f, 1.0f, 1.0f);
-        vertices->push_back(osg::Vec2(x2, y2));
-        vertices->push_back(osg::Vec2(x3, y3));
-        vertices->push_back(osg::Vec2(x4, y4));
-
-        geometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::LINES,
-                                                      0, vertices->size()));
-
-        geometry->setVertexArray(vertices);
-
-        osg::ref_ptr<osg::Vec4Array> color(new osg::Vec4Array);
-        color->push_back(osg::Vec4(0.0f, 0.0f, 1.0f, 1.0f));
-        geometry->setColorArray(color);
-        geometry->setColorBinding(osg::Geometry::BIND_OVERALL);
         
+        geometry->getOrCreateStateSet()->
+                setTextureAttributeAndModes(id, texture2D, osg::StateAttribute::OFF);
+
         return geometry;
     }
 
-    stateset->setTextureAttributeAndModes(id, texture2D);
-
-    float dx, dy;
     if (smoothInterpolation)
     {
         texture2D->setFilter(osg::Texture::MIN_FILTER, osg::Texture::LINEAR);
         texture2D->setFilter(osg::Texture::MAG_FILTER, osg::Texture::LINEAR);
-        dx = 1.0f / (2.0f * textureWidth);
-        dy = 1.0f / (2.0f * textureHeight);
     }
     else
     {
         texture2D->setFilter(osg::Texture::MIN_FILTER, osg::Texture::NEAREST);
         texture2D->setFilter(osg::Texture::MAG_FILTER, osg::Texture::NEAREST);
-        dx = 0.0f;
-        dy = 0.0f;
     }
 
-    glColor3f(1.0f, 1.0f, 1.0f);
+    drawarrays->set(osg::PrimitiveSet::POLYGON, 0, 4);
-    if (!_is3D)
+    (*colors)[0].set(1.0f, 1.0f, 1.0f, 1.0f);
-    {
-        osg::ref_ptr<osg::Vec2Array> tc = new osg::Vec2Array;
-
-        geometry->setTexCoordArray(id, tc);
-        tc->push_back(osg::Vec2(dx, maxV - dy));
-        tc->push_back(osg::Vec2(maxU - dx, maxV - dy));
-        tc->push_back(osg::Vec2(maxU - dx, dy));
-        tc->push_back(osg::Vec2(dx, dy));
-
-        glBegin(GL_QUADS);
-        glTexCoord2f(dx, maxV - dy);
-        glVertex3f(x1, y1, 0.0f);
-        glTexCoord2f(maxU - dx, maxV - dy);
-        glVertex3f(x2, y2, 0.0f);
-        glTexCoord2f(maxU - dx, dy);
-        glVertex3f(x3, y3, 0.0f);
-        glTexCoord2f(dx, dy);
-        glVertex3f(x4, y4, 0.0f);
-        glEnd();
-    }
-    else
-    {
-        float scaleX = 1.0f / static_cast<float>(heightModel.size() - 1);
 
-        for (int32_t i = 0; i < heightModel.size() - 1; ++i)
+    geometry->getOrCreateStateSet()->
-        {
+            setTextureAttributeAndModes(id, texture2D, osg::StateAttribute::ON);
-            float scaleI = scaleX * static_cast<float>(i);
-
-            float scaleY =
-                    1.0f / static_cast<float>(heightModel[i].size() - 1);
-
-            float x1i = x1 + scaleI * (x4 - x1);
-            float x1f = x2 + scaleI * (x3 - x2);
-            float x2i = x1i + scaleX * (x4 - x1);
-            float x2f = x1f + scaleX * (x3 - x2);
-
-            for (int32_t j = 0; j < heightModel[i].size() - 1; ++j)
-            {
-                float scaleJ = scaleY * static_cast<float>(j);
-
-                float y1i = y1 + scaleJ * (y2 - y1);
-                float y1f = y4 + scaleJ * (y3 - y4);
-                float y2i = y1i + scaleY * (y2 - y1);
-                float y2f = y1f + scaleY * (y3 - y4);
-
-                float nx1 = x1i + scaleJ * (x1f - x1i);
-                float nx2 = x1i + (scaleJ + scaleY) * (x1f - x1i);
-                float nx3 = x2i + (scaleJ + scaleY) * (x2f - x2i);
-                float nx4  = x2i + scaleJ * (x2f - x2i);
-                float ny1 = y1i + scaleI * (y1f - y1i);
-                float ny2 = y2i + scaleI * (y2f - y2i);
-                float ny3 = y2i + (scaleI + scaleX) * (y2f - y2i);
-                float ny4 = y1i + (scaleI + scaleX) * (y1f - y1i);
-
-                glBegin(GL_QUADS);
-                glTexCoord2f(dx + scaleJ * (maxU - dx * 2.0f),
-                             dy + (1.0f - scaleI) * (maxV - dy * 2.0f));
-                glVertex3f(nx1, ny1, -static_cast<float>(heightModel[i][j]));
-                glTexCoord2f(dx + (scaleJ + scaleY) * (maxU - dx * 2.0f),
-                             dy + (1.0f - scaleI) * (maxV - dy * 2.0f));
-                glVertex3f(nx2, ny2, -static_cast<float>(heightModel[i][j + 1]));
-                glTexCoord2f(dx + (scaleJ + scaleY) * (maxU - dx * 2.0f),
-                             dy + (1.0f - scaleI - scaleX) * (maxV - dy * 2.0f));
-                glVertex3f(nx3, ny3, -static_cast<float>(heightModel[i + 1][j + 1]));
-                glTexCoord2f(dx + scaleJ * (maxU - dx * 2.0f),
-                             dy + (1.0f - scaleI - scaleX) * (maxV - dy * 2.0f));
-                glVertex3f(nx4, ny4, -static_cast<float>(heightModel[i + 1][j]));
-
-                glEnd();
-            }
-        }
-    }
-}
 
-bool
+    return geometry;
-Texture::is3D(void) const
-{
-    return _is3D;
 }

src/ui/map3D/Texture.h

@@ -32,14 +32,10 @@ This file is part of the QGROUNDCONTROL project
 #ifndef TEXTURE_H
 #define TEXTURE_H
 
-#if (defined __APPLE__) & (defined __MACH__)
-#include <OpenGL/gl.h>
-#else
-#include <GL/gl.h>
-#endif
 #include <inttypes.h>
 #include <osg/ref_ptr>
 #include <osg/Geometry>
+#include <osg/Texture2D>
 #include <QSharedPointer>
 
 #include "WebImage.h"
@@ -47,7 +43,7 @@ This file is part of the QGROUNDCONTROL project
 class Texture
 {
 public:
-    Texture();
+    explicit Texture(unsigned int _id);
 
     const QString& getSourceURL(void) const;
 
@@ -61,8 +57,6 @@ public:
                                      float x3, float y3, float x4, float y4,
                                      bool smoothInterpolation) const;
 
-    bool is3D(void) const;
-
 private:
     enum State
     {
@@ -75,18 +69,7 @@ private:
     QString sourceURL;
     unsigned int id;
     osg::ref_ptr<osg::Texture2D> texture2D;
-
+    osg::ref_ptr<osg::Geometry> geometry;
-    int32_t textureWidth;
-    int32_t textureHeight;
-
-    int32_t imageWidth;
-    int32_t imageHeight;
-
-    bool _is3D;
-    QVector< QVector<int32_t> > heightModel;
-
-    float maxU;
-    float maxV;
 };
 
 typedef QSharedPointer<Texture> TexturePtr;

src/ui/map3D/TextureCache.cc

@@ -37,28 +37,26 @@ TextureCache::TextureCache(uint32_t _cacheSize)
 {
     for (uint32_t i = 0; i < cacheSize; ++i)
     {
-        TexturePtr t(new Texture);
+        TexturePtr t(new Texture(i));
-        t->setId(i);
 
         textures.push_back(t);
     }
 }
 
 TexturePtr
-TextureCache::get(const QString& tileURL, bool useHeightModel)
+TextureCache::get(const QString& tileURL)
 {
-    QPair<TexturePtr, int32_t> p1 = lookup(tileURL, useHeightModel);
+    QPair<TexturePtr, int32_t> p1 = lookup(tileURL);
     if (!p1.first.isNull())
     {
         return p1.first;
     }
 
-    QPair<WebImagePtr, int32_t> p2 =
+    QPair<WebImagePtr, int32_t> p2 = imageCache->lookup(tileURL);
-            imageCache->lookup(tileURL, useHeightModel);
     if (!p2.first.isNull())
     {
         textures[p2.second]->sync(p2.first);
-        p1 = lookup(tileURL, useHeightModel);
+        p1 = lookup(tileURL);
 
         return p1.first;
     }
@@ -79,12 +77,11 @@ TextureCache::sync(void)
 }
 
 QPair<TexturePtr, int32_t>
-TextureCache::lookup(const QString& tileURL, bool useHeightModel)
+TextureCache::lookup(const QString& tileURL)
 {
     for (int32_t i = 0; i < textures.size(); ++i)
     {
-        if (textures[i]->getSourceURL() == tileURL &&
+        if (textures[i]->getSourceURL() == tileURL)
-            textures[i]->is3D() == useHeightModel)
         {
             return qMakePair(textures[i], i);
         }

src/ui/map3D/TextureCache.h

@@ -42,13 +42,12 @@ class TextureCache
 public:
     explicit TextureCache(uint32_t cacheSize);
 
-    TexturePtr get(const QString& tileURL, bool useHeightModel = false);
+    TexturePtr get(const QString& tileURL);
 
     void sync(void);
 
 private:
-    QPair<TexturePtr, int32_t> lookup(const QString& tileURL,
+    QPair<TexturePtr, int32_t> lookup(const QString& tileURL);
-                                      bool useHeightModel);
 
     bool requireSync(void) const;
 

src/ui/map3D/WebImage.cc

@@ -39,7 +39,6 @@ WebImage::WebImage()
     , sourceURL("")
     , image(0)
     , lastReference(0)
-    , _is3D(false)
     , syncFlag(false)
 {
 
@@ -52,7 +51,6 @@ WebImage::clear(void)
     sourceURL.clear();
     state = WebImage::UNINITIALIZED;
     lastReference = 0;
-    heightModel.clear();
 }
 
 WebImage::State
@@ -79,18 +77,12 @@ WebImage::setSourceURL(const QString& url)
     sourceURL = url;
 }
 
-const uint8_t*
+uchar*
 WebImage::getImageData(void) const
 {
     return image->scanLine(0);
 }
 
-const QVector< QVector<int32_t> >&
-WebImage::getHeightModel(void) const
-{
-    return heightModel;
-}
-
 bool
 WebImage::setData(const QByteArray& data)
 {
@@ -131,90 +123,32 @@ WebImage::setData(const QString& filename)
     }
 }
 
-bool
+int
-WebImage::setData(const QString& imageFilename, const QString& heightFilename)
-{
-    QFile heightFile(heightFilename);
-
-    QImage tempImage;
-    if (tempImage.load(imageFilename) && heightFile.open(QIODevice::ReadOnly))
-    {
-        if (image.isNull())
-        {
-            image.reset(new QImage);
-        }
-        *image = QGLWidget::convertToGLFormat(tempImage);
-
-        QDataStream heightDataStream(&heightFile);
-
-        // read in width and height values for height map
-        char header[8];
-        heightDataStream.readRawData(header, 8);
-
-        int32_t height = *(reinterpret_cast<int32_t *>(header));
-        int32_t width = *(reinterpret_cast<int32_t *>(header + 4));
-
-        char buffer[height * width * sizeof(int32_t)];
-        heightDataStream.readRawData(buffer, height * width * sizeof(int32_t));
-
-        heightModel.clear();
-        for (int32_t i = 0; i < height; ++i)
-        {
-            QVector<int32_t> scanline;
-            for (int32_t j = 0; j < width; ++j)
-            {    
-                int32_t n = *(reinterpret_cast<int32_t *>(buffer
-                                                          + (i * height + j)
-                                                          * sizeof(int32_t)));
-                scanline.push_back(n);
-            }
-            heightModel.push_back(scanline);
-        }
-
-        heightFile.close();
-
-        _is3D = true;
-
-        return true;
-    }
-    else
-    {
-        return false;
-    }
-}
-
-int32_t
 WebImage::getWidth(void) const
 {
     return image->width();
 }
 
-int32_t
+int
 WebImage::getHeight(void) const
 {
     return image->height();
 }
 
-int32_t
+int
 WebImage::getByteCount(void) const
 {
     return image->byteCount();
 }
 
-bool
+ulong
-WebImage::is3D(void) const
-{
-    return _is3D;
-}
-
-uint64_t
 WebImage::getLastReference(void) const
 {
     return lastReference;
 }
 
 void
-WebImage::setLastReference(uint64_t value)
+WebImage::setLastReference(ulong value)
 {
     lastReference = value;
 }

src/ui/map3D/WebImage.h

@@ -57,20 +57,16 @@ public:
     const QString& getSourceURL(void) const;
     void setSourceURL(const QString& url);
 
-    const uint8_t* getImageData(void) const;
+    uchar* getImageData(void) const;
-    const QVector< QVector<int32_t> >& getHeightModel(void) const;
     bool setData(const QByteArray& data);
     bool setData(const QString& filename);
-    bool setData(const QString& imageFilename, const QString& heightFilename);
 
-    int32_t getWidth(void) const;
+    int getWidth(void) const;
-    int32_t getHeight(void) const;
+    int getHeight(void) const;
-    int32_t getByteCount(void) const;
+    int getByteCount(void) const;
 
-    bool is3D(void) const;
+    ulong getLastReference(void) const;
-
+    void setLastReference(ulong value);
-    uint64_t getLastReference(void) const;
-    void setLastReference(uint64_t value);
 
     bool getSyncFlag(void) const;
     void setSyncFlag(bool onoff);
@@ -79,9 +75,7 @@ private:
     State state;
     QString sourceURL;
     QScopedPointer<QImage> image;
-    QVector< QVector<int32_t> > heightModel;
+    ulong lastReference;
-    uint64_t lastReference;
-    bool _is3D;
     bool syncFlag;
 };
 

src/ui/map3D/WebImageCache.cc

@@ -52,15 +52,14 @@ WebImageCache::WebImageCache(QObject* parent, uint32_t _cacheSize)
 }
 
 QPair<WebImagePtr, int32_t>
-WebImageCache::lookup(const QString& url, bool useHeightModel)
+WebImageCache::lookup(const QString& url)
 {
     QPair<WebImagePtr, int32_t> cacheEntry;
 
     for (int32_t i = 0; i < webImages.size(); ++i)
     {
         if (webImages[i]->getState() != WebImage::UNINITIALIZED &&
-            webImages[i]->getSourceURL() == url &&
+            webImages[i]->getSourceURL() == url)
-            webImages[i]->is3D() == useHeightModel)
         {
             cacheEntry.first = webImages[i];
             cacheEntry.second = i;
@@ -111,22 +110,7 @@ WebImageCache::lookup(const QString& url, bool useHeightModel)
             }
             else
             {
-                bool success;
+                if (cacheEntry.first->setData(url))
-
-                if (useHeightModel)
-                {
-                    QString heightURL = url;
-                    heightURL.replace("color", "dom");
-                    heightURL.replace(".jpg", ".txt");
-
-                    success = cacheEntry.first->setData(url, heightURL);
-                }
-                else
-                {
-                    success = cacheEntry.first->setData(url);
-                }
-
-                if (success)
                 {
                     cacheEntry.first->setSyncFlag(true);
                     cacheEntry.first->setState(WebImage::READY);

src/ui/map3D/WebImageCache.h

@@ -45,8 +45,7 @@ class WebImageCache : public QObject
 public:
     WebImageCache(QObject* parent, uint32_t cacheSize);
 
-    QPair<WebImagePtr, int32_t> lookup(const QString& url,
+    QPair<WebImagePtr, int32_t> lookup(const QString& url);
-                                       bool useHeightModel);
 
     WebImagePtr at(int32_t index) const;