Fixed bugs in 3D imagery. Still more unresolved bugs related to tile resolution.

src/comm/MAVLinkSimulationLink.cc

@@ -389,6 +389,10 @@ void MAVLinkSimulationLink::mainloop()
         y = (y < -5.0f) ? -5.0f : y;
         z = (z < -3.0f) ? -3.0f : z;
 
+        // position at Pixhawk lab @ ETHZ
+        x += 5247273.0f;
+        y += 465955.0f;
+
         // Send back new setpoint
         mavlink_message_t ret;
         mavlink_msg_local_position_setpoint_pack(systemId, componentId, &ret, spX, spY, spZ, spYaw);

src/ui/map3D/Imagery.cc

@@ -214,6 +214,7 @@ Imagery::draw3D(double radius, double imageResolution,
               centerTileX, centerTileY, zoomLevel);
     UTMtoTile(maxX, maxY, utmZone, imageResolution,
               maxTileX, minTileY, zoomLevel);
+
     if (maxTileX - minTileX + 1 > 14)
     {
         minTileX = centerTileX - 7;
@@ -235,6 +236,7 @@ Imagery::draw3D(double radius, double imageResolution,
             imageBounds(c, r, imageResolution, x1, y1, x2, y2, x3, y3, x4, y4);
 
             TexturePtr t = textureCache->get(tileURL);
+
             if (!t.isNull())
             {
                 t->draw(x1 - xOrigin, y1 - yOrigin,
@@ -249,6 +251,8 @@ Imagery::draw3D(double radius, double imageResolution,
 bool
 Imagery::update(void)
 {
+    textureCache->sync();
+
     return true;
 }
 
@@ -257,7 +261,7 @@ Imagery::imageBounds(int32_t x, int32_t y, double imageResolution,
                      double& x1, double& y1, double& x2, double& y2,
                      double& x3, double& y3, double& x4, double& y4)
 {
-    int32_t zoomLevel = 17 - static_cast<int32_t>(rint(log2(imageResolution)));
+    int32_t zoomLevel = 19 - static_cast<int32_t>(rint(log2(imageResolution)));
     int32_t numTiles = static_cast<int32_t>(exp2(static_cast<double>(zoomLevel)));
 
     double lon1 = 360.0 * (static_cast<double>(x)
@@ -273,10 +277,10 @@ Imagery::imageBounds(int32_t x, int32_t y, double imageResolution,
                                   * 2.0 * M_PI - M_PI);
 
     QString utmZone;
-    LLtoUTM(lat1, lon1, y1, x1, utmZone);
-    LLtoUTM(lat1, lon2, y2, x2, utmZone);
-    LLtoUTM(lat2, lon2, y3, x3, utmZone);
-    LLtoUTM(lat2, lon1, y4, x4, utmZone);
+    LLtoUTM(lat2, lon1, x1, y1, utmZone);
+    LLtoUTM(lat2, lon2, x2, y2, utmZone);
+    LLtoUTM(lat1, lon2, x3, y3, utmZone);
+    LLtoUTM(lat1, lon1, x4, y4, utmZone);
 }
 
 double
@@ -302,7 +306,7 @@ Imagery::UTMtoTile(double northing, double easting, const QString& utmZone,
 
     UTMtoLL(northing, easting, utmZone, latitude, longitude);
 
-    zoomLevel = 17 - static_cast<int32_t>(rint(log2(imageResolution)));
+    zoomLevel = 19 - static_cast<int32_t>(rint(log2(imageResolution)));
     int32_t numTiles = static_cast<int32_t>(exp2(static_cast<double>(zoomLevel)));
 
     double x = longitude / 180.0;

src/ui/map3D/QMap3DWidget.cc

@@ -51,7 +51,7 @@ QMap3DWidget::QMap3DWidget(QWidget* parent)
      , updateLastUnlockedPose(true)
      , displayTarget(false)
      , displayWaypoints(true)
-     , imagery(new Imagery)
+     , imagery(0)
 {
     setFocusPolicy(Qt::StrongFocus);
 
@@ -223,7 +223,7 @@ QMap3DWidget::displayHandler(void)
 
     if (displayImagery)
     {
-        drawImagery(robotX, robotY, "32N", true);
+        drawImagery(robotX, robotY, "32T", true);
     }
 
     glPopMatrix();
@@ -389,9 +389,15 @@ QMap3DWidget::timer(void* clientData)
 void
 QMap3DWidget::timerHandler(void)
 {
+    if (imagery.isNull())
+    {
+        imagery.reset(new Imagery);
+    }
+
     double timeLapsed = getTime() - lastRedrawTime;
     if (timeLapsed > 0.1)
     {
+        imagery->update();
         forceRedraw();
         lastRedrawTime = getTime();
     }
@@ -598,6 +604,8 @@ QMap3DWidget::drawImagery(double originX, double originY, const QString& zone,
     glPushMatrix();
     glEnable(GL_BLEND);
 
+    glTranslatef(0, 0, 0.1);
+
     CameraPose camPose = getCameraPose();
     double viewingRadius = camPose.distance / 4000.0 * 3000.0;
     if (viewingRadius < 100.0)
@@ -605,8 +613,8 @@ QMap3DWidget::drawImagery(double originX, double originY, const QString& zone,
         viewingRadius = 100.0;
     }
 
-    double minResolution = 0.25;
-    double centerResolution = camPose.distance / 160.0 * 0.25;
+    double minResolution = 0.125;
+    double centerResolution = camPose.distance / 160.0;
     double maxResolution = 2.0;
 
     double resolution = minResolution;
@@ -619,7 +627,7 @@ QMap3DWidget::drawImagery(double originX, double originY, const QString& zone,
         resolution = maxResolution;
     }
 
-    imagery->draw3D(viewingRadius, resolution, originX, originY, camPose.yOffset, camPose.xOffset, zone);
+    imagery->draw3D(viewingRadius, resolution, originX, originY, camPose.xOffset, camPose.yOffset, zone);
 
     if (prefetch)
     {
@@ -627,13 +635,13 @@ QMap3DWidget::drawImagery(double originX, double originY, const QString& zone,
         {
             imagery->prefetch3D(viewingRadius / 2.0, resolution / 2.0,
                                 originX, originY,
-                                camPose.yOffset, camPose.xOffset, zone);
+                                camPose.xOffset, camPose.yOffset, zone);
         }
         if (resolution * 2.0 <= maxResolution)
         {
             imagery->prefetch3D(viewingRadius * 2.0, resolution * 2.0,
                                 originX, originY,
-                                camPose.yOffset, camPose.xOffset, zone);
+                                camPose.xOffset, camPose.yOffset, zone);
         }
     }
 

src/ui/map3D/Texture.cc

@@ -55,13 +55,12 @@ Texture::sync(const WebImagePtr& image)
 
             glBindTexture(GL_TEXTURE_2D, id);
             glTexImage2D(GL_TEXTURE_2D, 0, 3, textureWidth, textureHeight,
-                         0, GL_RGB, GL_UNSIGNED_BYTE, NULL);
+                         0, GL_RGBA, GL_UNSIGNED_BYTE, NULL);
         }
 
         glBindTexture(GL_TEXTURE_2D, id);
         glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, imageWidth, imageHeight,
-                        GL_RGB, GL_UNSIGNED_BYTE, image->getData());
-
+                        GL_RGBA, GL_UNSIGNED_BYTE, image->getData());
     }
 }
 

src/ui/map3D/TextureCache.cc

@@ -7,6 +7,7 @@ TextureCache::TextureCache(uint32_t _cacheSize)
     for (uint32_t i = 0; i < cacheSize; ++i)
     {
         TexturePtr t(new Texture);
+
         GLuint id;
         glGenTextures(1, &id);
         t->setID(id);

src/ui/map3D/WebImage.cc

 #include "WebImage.h"
 
+#include <QDebug>
+#include <QGLWidget>
+
 WebImage::WebImage()
     : state(WebImage::UNINITIALIZED)
     , sourceURL("")
@@ -46,13 +49,25 @@ WebImage::setSourceURL(const QString& url)
 const uint8_t*
 WebImage::getData(void) const
 {
-    return image->bits();
+    return image->scanLine(0);
 }
 
 void
 WebImage::setData(const QByteArray& data)
 {
-    image->loadFromData(data);
+    QImage tempImage;
+    if (tempImage.loadFromData(data))
+    {
+        if (image.isNull())
+        {
+            image = QSharedPointer<QImage>(new QImage);
+        }
+        *image = QGLWidget::convertToGLFormat(tempImage);
+    }
+    else
+    {
+        qDebug() << "# WARNING: cannot load image data for" << sourceURL;
+    }
 }
 
 int32_t
@@ -67,6 +82,12 @@ WebImage::getHeight(void) const
     return image->height();
 }
 
+int32_t
+WebImage::getByteCount(void) const
+{
+    return image->byteCount();
+}
+
 uint64_t
 WebImage::getLastReference(void) const
 {

src/ui/map3D/WebImage.h

@@ -3,6 +3,7 @@
 
 #include <inttypes.h>
 #include <QImage>
+#include <QScopedPointer>
 #include <QSharedPointer>
 
 class WebImage
@@ -30,6 +31,7 @@ public:
 
     int32_t getWidth(void) const;
     int32_t getHeight(void) const;
+    int32_t getByteCount(void) const;
 
     uint64_t getLastReference(void) const;
     void setLastReference(uint64_t value);