Latest working version of 3D imagery overlay. Unresolved errors with Google...

Latest working version of 3D imagery overlay. Unresolved errors with Google URLs for satellite imagery.

src/ui/map3D/Imagery.cc

@@ -7,6 +7,7 @@ const double WGS84_A = 6378137.0;
 const double WGS84_ECCSQ = 0.00669437999013;
 
 Imagery::Imagery()
+    : textureCache(new TextureCache(1000))
 {
 
 }
@@ -137,10 +138,13 @@ Imagery::draw2D(double windowWidth, double windowHeight,
             imageBounds(c, r, imageResolution, x1, y1, x2, y2, x3, y3, x4, y4);
 
             TexturePtr t = textureCache->get(tileURL);
-            t->draw(x1 - xOrigin, y1 - yOrigin,
-                    x2 - xOrigin, y2 - yOrigin,
-                    x3 - xOrigin, y3 - yOrigin,
-                    x4 - xOrigin, y4 - yOrigin, true);
+            if (!t.isNull())
+            {
+                t->draw(x1 - xOrigin, y1 - yOrigin,
+                        x2 - xOrigin, y2 - yOrigin,
+                        x3 - xOrigin, y3 - yOrigin,
+                        x4 - xOrigin, y4 - yOrigin, true);
+            }
         }
     }
 }
@@ -231,10 +235,13 @@ Imagery::draw3D(double radius, double imageResolution,
             imageBounds(c, r, imageResolution, x1, y1, x2, y2, x3, y3, x4, y4);
 
             TexturePtr t = textureCache->get(tileURL);
-            t->draw(x1 - xOrigin, y1 - yOrigin,
-                    x2 - xOrigin, y2 - yOrigin,
-                    x3 - xOrigin, y3 - yOrigin,
-                    x4 - xOrigin, y4 - yOrigin, true);
+            if (!t.isNull())
+            {
+                t->draw(x1 - xOrigin, y1 - yOrigin,
+                        x2 - xOrigin, y2 - yOrigin,
+                        x3 - xOrigin, y3 - yOrigin,
+                        x4 - xOrigin, y4 - yOrigin, true);
+            }
         }
     }
 }
@@ -499,18 +506,18 @@ Imagery::getTileURL(int32_t x, int32_t y, int32_t zoomLevel)
     switch (currentImageryType)
     {
     case MAP:
-        oss << "http://mt1.google.com/mt?&x=" << x << "&y=" << y <<
-                "&z=" << zoomLevel;
+        oss << "http://mt0.google.com/vt/lyrs=m@120&x=" << x << "&y=" << y
+            << "&z=" << zoomLevel;
         break;
     case SATELLITE:
-        oss << "http://khm.google.com/kh?&x=" << x << "&y=" << y <<
-                "&zoom=" << zoomLevel;
+        oss << "http://khm.google.com/vt/lbw/lyrs=y&x=" << x << "&y=" << y
+            << "&z=" << zoomLevel;
         break;
     default:
         {};
     }
 
-    QString url;
+    QString url(oss.str().c_str());
 
     return url;
 }

src/ui/map3D/QMap3DWidget.cc

@@ -45,11 +45,13 @@ QMap3DWidget::QMap3DWidget(QWidget* parent)
      , uas(NULL)
      , lastRedrawTime(0.0)
      , displayGrid(true)
+     , displayImagery(false)
      , displayTrail(false)
      , lockCamera(true)
      , updateLastUnlockedPose(true)
      , displayTarget(false)
      , displayWaypoints(true)
+     , imagery(new Imagery)
 {
     setFocusPolicy(Qt::StrongFocus);
 
@@ -88,6 +90,14 @@ QMap3DWidget::buildLayout(void)
     waypointsCheckBox->setText("Waypoints");
     waypointsCheckBox->setChecked(displayWaypoints);
 
+    QLabel* imageryLabel = new QLabel(this);
+    imageryLabel->setText("Imagery");
+
+    QComboBox* imageryComboBox = new QComboBox(this);
+    imageryComboBox->addItem("None");
+    imageryComboBox->addItem("Map (Google)");
+    imageryComboBox->addItem("Satellite (Google)");
+
     QPushButton* recenterButton = new QPushButton(this);
     recenterButton->setText("Recenter Camera");
 
@@ -105,18 +115,23 @@ QMap3DWidget::buildLayout(void)
     layout->addWidget(trailCheckBox, 1, 1);
     layout->addWidget(waypointsCheckBox, 1, 2);
     layout->addItem(new QSpacerItem(0, 0, QSizePolicy::Expanding, QSizePolicy::Expanding), 1, 3);
-    layout->addWidget(recenterButton, 1, 4);
-    layout->addWidget(lockCameraCheckBox, 1, 5);
+    layout->addWidget(imageryLabel, 1, 4);
+    layout->addWidget(imageryComboBox, 1, 5);
+    layout->addItem(new QSpacerItem(0, 0, QSizePolicy::Expanding, QSizePolicy::Expanding), 1, 6);
+    layout->addWidget(recenterButton, 1, 7);
+    layout->addWidget(lockCameraCheckBox, 1, 8);
     layout->setRowStretch(0, 100);
     layout->setRowStretch(1, 1);
-    //layout->setColumnStretch(0, 1);
-    layout->setColumnStretch(2, 50);
+    layout->setColumnStretch(3, 50);
+    layout->setColumnStretch(6, 50);
     setLayout(layout);
 
     connect(gridCheckBox, SIGNAL(stateChanged(int)),
             this, SLOT(showGrid(int)));
     connect(trailCheckBox, SIGNAL(stateChanged(int)),
             this, SLOT(showTrail(int)));
+    connect(imageryComboBox, SIGNAL(currentIndexChanged(const QString &)),
+            this, SLOT(showImagery(const QString &)));
     connect(recenterButton, SIGNAL(clicked()), this, SLOT(recenterCamera()));
     connect(lockCameraCheckBox, SIGNAL(stateChanged(int)),
             this, SLOT(toggleLockCamera(int)));
@@ -206,6 +221,11 @@ QMap3DWidget::displayHandler(void)
         drawWaypoints();
     }
 
+    if (displayImagery)
+    {
+        drawImagery(robotX, robotY, "32N", true);
+    }
+
     glPopMatrix();
 
     // switch to 2D
@@ -441,6 +461,28 @@ QMap3DWidget::showGrid(int32_t state)
     }
 }
 
+void
+QMap3DWidget::showImagery(const QString& text)
+{
+    if (text.compare("None") == 0)
+    {
+        displayImagery = false;
+    }
+    else
+    {
+        if (text.compare("Map (Google)") == 0)
+        {
+            imagery->setImageryType(Imagery::MAP);
+        }
+        else if (text.compare("Satellite (Google)") == 0)
+        {
+            imagery->setImageryType(Imagery::SATELLITE);
+        }
+        displayImagery = true;
+    }
+}
+
+
 void
 QMap3DWidget::showTrail(int32_t state)
 {
@@ -549,6 +591,56 @@ QMap3DWidget::drawGrid(void)
     glPopMatrix();
 }
 
+void
+QMap3DWidget::drawImagery(double originX, double originY, const QString& zone,
+                          bool prefetch)
+{
+    glPushMatrix();
+    glEnable(GL_BLEND);
+
+    CameraPose camPose = getCameraPose();
+    double viewingRadius = camPose.distance / 4000.0 * 3000.0;
+    if (viewingRadius < 100.0)
+    {
+        viewingRadius = 100.0;
+    }
+
+    double minResolution = 0.25;
+    double centerResolution = camPose.distance / 160.0 * 0.25;
+    double maxResolution = 2.0;
+
+    double resolution = minResolution;
+    while (resolution * 2.0 < centerResolution)
+    {
+        resolution *= 2.0;
+    }
+    if (resolution > maxResolution)
+    {
+        resolution = maxResolution;
+    }
+
+    imagery->draw3D(viewingRadius, resolution, originX, originY, camPose.yOffset, camPose.xOffset, zone);
+
+    if (prefetch)
+    {
+        if (resolution / 2.0 >= minResolution)
+        {
+            imagery->prefetch3D(viewingRadius / 2.0, resolution / 2.0,
+                                originX, originY,
+                                camPose.yOffset, camPose.xOffset, zone);
+        }
+        if (resolution * 2.0 <= maxResolution)
+        {
+            imagery->prefetch3D(viewingRadius * 2.0, resolution * 2.0,
+                                originX, originY,
+                                camPose.yOffset, camPose.xOffset, zone);
+        }
+    }
+
+    glDisable(GL_BLEND);
+    glPopMatrix();
+}
+
 void
 QMap3DWidget::drawTrail(float x, float y, float z)
 {

src/ui/map3D/QMap3DWidget.h

@@ -34,6 +34,7 @@ This file is part of the QGROUNDCONTROL project
 
 #include <QLabel>
 
+#include "Imagery.h"
 #include "Q3DWidget.h"
 
 class CheetahModel;
@@ -72,23 +73,28 @@ public slots:
 private slots:
     void showGrid(int state);
     void showTrail(int state);
+    void showImagery(const QString& text);
     void recenterCamera(void);
     void toggleLockCamera(int state);
 
 protected:
     UASInterface* uas;
-    void paintText(QString text, QColor color, float fontSize, float refX, float refY, QPainter* painter);
+    void paintText(QString text, QColor color, float fontSize,
+                   float refX, float refY, QPainter* painter);
     void drawWaypoints();
 
 private:
     void drawPlatform(float roll, float pitch, float yaw);
     void drawGrid(void);
+    void drawImagery(double originX, double originY, const QString& zone,
+                     bool prefetch);
     void drawTrail(float x, float y, float z);
     void drawTarget(float x, float y, float z);
 
     double lastRedrawTime;
 
     bool displayGrid;
+    bool displayImagery;
     bool displayTrail;
 
     typedef struct
@@ -110,6 +116,7 @@ private:
     Pose3D targetPosition;
 
     QScopedPointer<CheetahModel> cheetahModel;
+    QScopedPointer<Imagery> imagery;
 };
 
 #endif // QMAP3DWIDGET_H

src/ui/map3D/Texture.cc

@@ -5,8 +5,8 @@ Texture::Texture()
 
 }
 
-QString
-Texture::getSourceURL(void)
+const QString&
+Texture::getSourceURL(void) const
 {
     return sourceURL;
 }

src/ui/map3D/Texture.h

@@ -16,7 +16,7 @@ class Texture
 public:
     Texture();
 
-    QString getSourceURL(void);
+    const QString& getSourceURL(void) const;
 
     void setID(GLuint id);
 

src/ui/map3D/TextureCache.cc

@@ -4,10 +4,9 @@ TextureCache::TextureCache(uint32_t _cacheSize)
     : cacheSize(_cacheSize)
     , imageCache(new WebImageCache(0, cacheSize))
 {
-    textures.resize(cacheSize);
-    TexturePtr t;
-    foreach(t, textures)
+    for (uint32_t i = 0; i < cacheSize; ++i)
     {
+        TexturePtr t(new Texture);
         GLuint id;
         glGenTextures(1, &id);
         t->setID(id);
@@ -15,6 +14,8 @@ TextureCache::TextureCache(uint32_t _cacheSize)
         glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
         glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
         glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
+
+        textures.push_back(t);
     }
 }
 

src/ui/map3D/WebImage.cc

@@ -2,6 +2,8 @@
 
 WebImage::WebImage()
     : state(WebImage::UNINITIALIZED)
+    , sourceURL("")
+    , image(new QImage)
     , lastReference(0)
     , syncFlag(false)
 {
@@ -29,7 +31,7 @@ WebImage::setState(State state)
     this->state = state;
 }
 
-QString
+const QString&
 WebImage::getSourceURL(void) const
 {
     return sourceURL;
@@ -47,6 +49,12 @@ WebImage::getData(void) const
     return image->bits();
 }
 
+void
+WebImage::setData(const QByteArray& data)
+{
+    image->loadFromData(data);
+}
+
 int32_t
 WebImage::getWidth(void) const
 {

src/ui/map3D/WebImage.h

@@ -22,10 +22,12 @@ public:
     State getState(void) const;
     void setState(State state);
 
-    QString getSourceURL(void) const;
+    const QString& getSourceURL(void) const;
     void setSourceURL(const QString& url);
 
     const uint8_t* getData(void) const;
+    void setData(const QByteArray& data);
+
     int32_t getWidth(void) const;
     int32_t getHeight(void) const;
 

src/ui/map3D/WebImageCache.cc

@@ -9,7 +9,12 @@ WebImageCache::WebImageCache(QObject* parent, uint32_t _cacheSize)
     , currentReference(0)
     , networkManager(new QNetworkAccessManager)
 {
-    webImages.resize(cacheSize);
+    for (uint32_t i = 0; i < cacheSize; ++i)
+    {
+        WebImagePtr image(new WebImage);
+
+        webImages.push_back(image);
+    }
 
     connect(networkManager.data(), SIGNAL(finished(QNetworkReply*)),
             this, SLOT(downloadFinished(QNetworkReply*)));
@@ -104,9 +109,16 @@ WebImageCache::downloadFinished(QNetworkReply* reply)
         return;
     }
 
-    QByteArray imageData = reply->readAll();
-    QPixmap pixmap;
-    pixmap.loadFromData(imageData);
+    WebImagePtr image;
+    foreach(image, webImages)
+    {
+        if (reply->url().toString() == image->getSourceURL())
+        {
+            image->setData(reply->readAll());
+            image->setSyncFlag(true);
+            image->setState(WebImage::READY);
 
-    // set image, needsSync to true, and state to READY
+            return;
+        }
+    }
 }