Fixed issue with transform from screen to world coordinates.

src/ui/map3D/Pixhawk3DWidget.cc

@@ -38,10 +38,6 @@
 #include <osg/LineWidth>
 #include <osg/ShapeDrawable>
 
-#include <osgEarth/Map>
-#include <osgEarth/MapNode>
-#include <osgEarthDrivers/gdal/GDALOptions>
-
 #include "PixhawkCheetahGeode.h"
 #include "UASManager.h"
 #include "UASInterface.h"
@@ -60,9 +56,6 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
     init(15.0f);
     setCameraParams(0.5f, 30.0f, 0.01f, 10000.0f);
 
-    osg::Node* imagery = osgDB::readNodeFile("/home/hengli/swissimage.earth");
-    root->addChild(imagery);
-
     // generate Pixhawk Cheetah model
     egocentricMap->addChild(PixhawkCheetahGeode::instance());
 
@@ -74,6 +67,10 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
     trailNode = createTrail();
     rollingMap->addChild(trailNode);
 
+    // generate map model
+    mapNode = createMap();
+    root->addChild(mapNode);
+
     // generate target model
     targetNode = createTarget();
     rollingMap->addChild(targetNode);
@@ -85,7 +82,7 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
     setupHUD();
 
     setDisplayFunc(display, this);
-//    setMouseFunc(mouse, this);
+    setMouseFunc(mouse, this);
     addTimerFunc(100, timer, this);
 
     buildLayout();
@@ -114,6 +111,10 @@ Pixhawk3DWidget::buildLayout(void)
     waypointsCheckBox->setText("Waypoints");
     waypointsCheckBox->setChecked(displayWaypoints);
 
+    targetButton = new QPushButton(this);
+    targetButton->setCheckable(true);
+    targetButton->setIcon(QIcon(QString::fromUtf8(":/images/status/weather-clear.svg")));
+
     QPushButton* recenterButton = new QPushButton(this);
     recenterButton->setText("Recenter Camera");
 
@@ -127,19 +128,21 @@ Pixhawk3DWidget::buildLayout(void)
     layout->addWidget(gridCheckBox, 1, 0);
     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->addItem(new QSpacerItem(20, 0, QSizePolicy::Expanding, QSizePolicy::Expanding), 1, 3);
+    layout->addWidget(targetButton, 1, 4);
+    layout->addItem(new QSpacerItem(20, 0, QSizePolicy::Expanding, QSizePolicy::Expanding), 1, 5);
+    layout->addWidget(recenterButton, 1, 6);
+    layout->addWidget(lockCameraCheckBox, 1, 7);
     layout->setRowStretch(0, 100);
     layout->setRowStretch(1, 1);
-    //layout->setColumnStretch(0, 1);
-    layout->setColumnStretch(2, 50);
     setLayout(layout);
 
     connect(gridCheckBox, SIGNAL(stateChanged(int)),
             this, SLOT(showGrid(int)));
     connect(trailCheckBox, SIGNAL(stateChanged(int)),
             this, SLOT(showTrail(int)));
+    connect(waypointsCheckBox, SIGNAL(stateChanged(int)),
+            this, SLOT(showWaypoints(int)));
     connect(recenterButton, SIGNAL(clicked()), this, SLOT(recenterCamera()));
     connect(lockCameraCheckBox, SIGNAL(stateChanged(int)),
             this, SLOT(toggleLockCamera(int)));
@@ -174,6 +177,7 @@ Pixhawk3DWidget::displayHandler(void)
 
     updateHUD(robotX, robotY, robotZ, robotRoll, robotPitch, robotYaw);
     updateTrail(robotX, robotY, robotZ);
+    updateTarget(robotX, robotY, robotZ);
     updateWaypoints();
 
     // set node visibility
@@ -195,12 +199,10 @@ void
 Pixhawk3DWidget::mouseHandler(Qt::MouseButton button, MouseState state,
                               int32_t x, int32_t y)
 {
-    if (button == Qt::RightButton && state == MOUSE_STATE_DOWN)
+    if (button == Qt::LeftButton && state == MOUSE_STATE_DOWN &&
+        targetButton->isChecked())
     {
-        QMenu menu(this);
-        QAction* targetAction = menu.addAction(tr("Mark as Target"));
-        connect(targetAction, SIGNAL(triggered()), this, SLOT(markTarget()));
-        menu.exec(mapToGlobal(QPoint(x, y)));
+        markTarget();
     }
 }
 
@@ -249,35 +251,6 @@ void Pixhawk3DWidget::setActiveUAS(UASInterface* uas)
     this->uas = uas;
 }
 
-void
-Pixhawk3DWidget::markTarget(void)
-{
-    std::pair<float,float> mouseWorldCoords =
-            getGlobalCursorPosition(getLastMouseX(), getLastMouseY());
-
-    float robotX = 0.0f, robotY = 0.0f, robotZ = 0.0f;
-    if (uas != NULL)
-    {
-        robotX = uas->getLocalX();
-        robotY = uas->getLocalY();
-        robotZ = uas->getLocalZ();
-    }
-
-    targetPosition.x() = mouseWorldCoords.first + robotX;
-    targetPosition.y() = mouseWorldCoords.second + robotY;
-    targetPosition.z() = robotZ;
-
-    displayTarget = true;
-
-    if (uas)
-    {
-        uas->setTargetPosition(targetPosition.x(),
-                               targetPosition.y(),
-                               targetPosition.z(),
-                               0.0f);
-    }
-}
-
 void
 Pixhawk3DWidget::showGrid(int32_t state)
 {
@@ -309,6 +282,19 @@ Pixhawk3DWidget::showTrail(int32_t state)
     }
 }
 
+void
+Pixhawk3DWidget::showWaypoints(int state)
+{
+    if (state == Qt::Checked)
+    {
+        displayWaypoints = true;
+    }
+    else
+    {
+        displayWaypoints = false;
+    }
+}
+
 void
 Pixhawk3DWidget::recenterCamera(void)
 {
@@ -397,20 +383,29 @@ Pixhawk3DWidget::createTrail(void)
     return geode;
 }
 
+osg::ref_ptr<osgEarth::MapNode>
+Pixhawk3DWidget::createMap(void)
+{
+    osg::ref_ptr<osg::Node> model = osgDB::readNodeFile("map.earth");
+    osg::ref_ptr<osgEarth::MapNode> node = osgEarth::MapNode::findMapNode(model);
+
+    return node;
+}
+
 osg::ref_ptr<osg::Group>
 Pixhawk3DWidget::createTarget(void)
 {
-    osg::ref_ptr<osg::Group> geode(new osg::Group());
+    osg::ref_ptr<osg::Group> group(new osg::Group());
 
-    return geode;
+    return group;
 }
 
 osg::ref_ptr<osg::Group>
 Pixhawk3DWidget::createWaypoints(void)
 {
-    osg::ref_ptr<osg::Group> geode(new osg::Group());
+    osg::ref_ptr<osg::Group> group(new osg::Group());
 
-    return geode;
+    return group;
 }
 
 void
@@ -421,19 +416,31 @@ Pixhawk3DWidget::setupHUD(void)
     hudBackgroundVertices->push_back(osg::Vec3(width(), height(), -1));
     hudBackgroundVertices->push_back(osg::Vec3(width(), height() - 30, -1));
     hudBackgroundVertices->push_back(osg::Vec3(0, height() - 30, -1));
+    hudBackgroundVertices->push_back(osg::Vec3(0, 0, -1));
+    hudBackgroundVertices->push_back(osg::Vec3(width(), 0, -1));
+    hudBackgroundVertices->push_back(osg::Vec3(width(), 25, -1));
+    hudBackgroundVertices->push_back(osg::Vec3(0, 25, -1));
+
+    osg::ref_ptr<osg::DrawElementsUInt> hudTopBackgroundIndices(
+            new osg::DrawElementsUInt(osg::PrimitiveSet::POLYGON, 0));
+    hudTopBackgroundIndices->push_back(0);
+    hudTopBackgroundIndices->push_back(1);
+    hudTopBackgroundIndices->push_back(2);
+    hudTopBackgroundIndices->push_back(3);
 
-    osg::ref_ptr<osg::DrawElementsUInt> hudBackgroundIndices(
+    osg::ref_ptr<osg::DrawElementsUInt> hudBottomBackgroundIndices(
             new osg::DrawElementsUInt(osg::PrimitiveSet::POLYGON, 0));
-    hudBackgroundIndices->push_back(0);
-    hudBackgroundIndices->push_back(1);
-    hudBackgroundIndices->push_back(2);
-    hudBackgroundIndices->push_back(3);
+    hudBottomBackgroundIndices->push_back(4);
+    hudBottomBackgroundIndices->push_back(5);
+    hudBottomBackgroundIndices->push_back(6);
+    hudBottomBackgroundIndices->push_back(7);
 
     osg::ref_ptr<osg::Vec4Array> hudColors(new osg::Vec4Array);
     hudColors->push_back(osg::Vec4(0.0f, 0.0f, 0.0f, 0.2f));
 
     hudBackgroundGeometry = new osg::Geometry;
-    hudBackgroundGeometry->addPrimitiveSet(hudBackgroundIndices);
+    hudBackgroundGeometry->addPrimitiveSet(hudTopBackgroundIndices);
+    hudBackgroundGeometry->addPrimitiveSet(hudBottomBackgroundIndices);
     hudBackgroundGeometry->setVertexArray(hudBackgroundVertices);
     hudBackgroundGeometry->setColorArray(hudColors);
     hudBackgroundGeometry->setColorBinding(osg::Geometry::BIND_OVERALL);
@@ -459,12 +466,16 @@ Pixhawk3DWidget::updateHUD(float robotX, float robotY, float robotZ,
     hudBackgroundVertices->push_back(osg::Vec3(width(), height(), -1));
     hudBackgroundVertices->push_back(osg::Vec3(width(), height() - 30, -1));
     hudBackgroundVertices->push_back(osg::Vec3(0, height() - 30, -1));
+    hudBackgroundVertices->push_back(osg::Vec3(0, 0, -1));
+    hudBackgroundVertices->push_back(osg::Vec3(width(), 0, -1));
+    hudBackgroundVertices->push_back(osg::Vec3(width(), 25, -1));
+    hudBackgroundVertices->push_back(osg::Vec3(0, 25, -1));
     hudBackgroundGeometry->setVertexArray(hudBackgroundVertices);
 
     statusText->setPosition(osg::Vec3(10, height() - 20, -1.5));
 
-    std::pair<float,float> cursorPosition =
-            getGlobalCursorPosition(getMouseX(), getMouseY());
+    std::pair<double,double> cursorPosition =
+            getGlobalCursorPosition(getMouseX(), getMouseY(), -robotZ);
 
     std::ostringstream oss;
     oss.setf(std::ios::fixed, std::ios::floatfield);
@@ -546,6 +557,11 @@ Pixhawk3DWidget::updateTarget(float robotX, float robotY, float robotZ)
         expand = false;
     }
 
+    if (targetNode->getNumChildren() > 0)
+    {
+        targetNode->removeChild(0, targetNode->getNumChildren());
+    }
+
     osg::ref_ptr<osg::ShapeDrawable> sd = new osg::ShapeDrawable;
     osg::ref_ptr<osg::Sphere> sphere = new osg::Sphere;
     sphere->setRadius(radius);
@@ -618,3 +634,33 @@ Pixhawk3DWidget::updateWaypoints(void)
         }
     }
 }
+
+
+void
+Pixhawk3DWidget::markTarget(void)
+{
+    float robotZ = 0.0f;
+    if (uas != NULL)
+    {
+        robotZ = uas->getLocalZ();
+    }
+
+    std::pair<double,double> mouseWorldCoords =
+            getGlobalCursorPosition(getLastMouseX(), getLastMouseY(), -robotZ);
+
+    targetPosition.x() = mouseWorldCoords.first;
+    targetPosition.y() = mouseWorldCoords.second;
+    targetPosition.z() = robotZ;
+
+    displayTarget = true;
+
+    if (uas)
+    {
+        uas->setTargetPosition(targetPosition.x(),
+                               targetPosition.y(),
+                               targetPosition.z(),
+                               0.0f);
+    }
+
+    targetButton->setChecked(false);
+}

src/ui/map3D/Pixhawk3DWidget.h

@@ -33,6 +33,7 @@
 #define PIXHAWK3DWIDGET_H
 
 #include <osgText/Text>
+#include <osgEarth/MapNode>
 
 #include "Q3DWidget.h"
 
@@ -66,11 +67,11 @@ public:
 
 public slots:
     void setActiveUAS(UASInterface* uas);
-    void markTarget(void);
 
 private slots:
     void showGrid(int state);
     void showTrail(int state);
+    void showWaypoints(int state);
     void recenterCamera(void);
     void toggleLockCamera(int state);
 
@@ -80,6 +81,7 @@ protected:
 private:
     osg::ref_ptr<osg::Geode> createGrid(void);
     osg::ref_ptr<osg::Geode> createTrail(void);
+    osg::ref_ptr<osgEarth::MapNode> createMap(void);
     osg::ref_ptr<osg::Group> createTarget(void);
     osg::ref_ptr<osg::Group> createWaypoints(void);
 
@@ -91,6 +93,8 @@ private:
     void updateTarget(float robotX, float robotY, float robotZ);
     void updateWaypoints(void);
 
+    void markTarget(void);
+
     double lastRedrawTime;
 
     bool displayGrid;
@@ -98,13 +102,6 @@ private:
     bool displayTarget;
     bool displayWaypoints;
 
-    typedef struct
-    {
-        float x;
-        float y;
-        float z;
-    } Pose3D;
-
     bool lockCamera;
 
     osg::ref_ptr<osg::Vec3Array> trailVertices;
@@ -118,8 +115,11 @@ private:
     osg::ref_ptr<osg::Geode> trailNode;
     osg::ref_ptr<osg::Geometry> trailGeometry;
     osg::ref_ptr<osg::DrawArrays> trailDrawArrays;
+    osg::ref_ptr<osgEarth::MapNode> mapNode;
     osg::ref_ptr<osg::Group> targetNode;
     osg::ref_ptr<osg::Group> waypointsNode;
+
+    QPushButton* targetButton;
 };
 
 #endif // PIXHAWK3DWIDGET_H

src/ui/map3D/Q3DWidget.cc

@@ -262,24 +262,29 @@ Q3DWidget::addTimerFunc(uint32_t msecs, void(*func)(void *),
     QTimer::singleShot(msecs, this, SLOT(userTimer()));
 }
 
-std::pair<float,float>
-Q3DWidget::getGlobalCursorPosition(int32_t mouseX, int32_t mouseY)
+std::pair<double,double>
+Q3DWidget::getGlobalCursorPosition(int32_t mouseX, int32_t mouseY,
+                                   double z)
 {
     osgUtil::LineSegmentIntersector::Intersections intersections;
 
     // normalize cursor position to value between -1 and 1
-    float x = -1.0f + static_cast<float>(2 * mouseX)
-              / static_cast<float>(width());
-    float y = -1.0f + static_cast<float>(2 * (height() - mouseY))
-              / static_cast<float>(height());
+    double x = -1.0f + static_cast<double>(2 * mouseX)
+              / static_cast<double>(width());
+    double y = -1.0f + static_cast<double>(2 * (height() - mouseY))
+              / static_cast<double>(height());
+
+    osg::Matrixd m = getCamera()->getViewMatrix()
+                     * getCamera()->getProjectionMatrix();
+    osg::Matrixd invM = osg::Matrixd::inverse(m);
 
-    osg::Matrix pm = getCamera()->getProjectionMatrix();
-    osg::Matrix vm = getCamera()->getViewMatrix();
+    osg::Vec3d nearPoint = osg::Vec3d(x, y, -1.0) * invM;
+    osg::Vec3d farPoint = osg::Vec3d(x, y, 1.0) * invM;
 
     osg::ref_ptr<osg::LineSegment> line =
-            projectNormalizedXYIntoObjectCoordinates(pm, vm, x, y);
+            new osg::LineSegment(nearPoint, farPoint);
 
-    osg::Plane p(line->start() - line->end(), 0.0);
+    osg::Plane p(osg::Vec3d(0.0, 0.0, 1.0), osg::Vec3d(0.0, 0.0, z));
 
     osg::Vec3d projectedPoint;
     getPlaneLineIntersection(p.asVec4(), *line, projectedPoint);
@@ -618,24 +623,6 @@ Q3DWidget::convertKey(int key) const
     }
 }
 
-osg::ref_ptr<osg::LineSegment>
-Q3DWidget::projectNormalizedXYIntoObjectCoordinates(
-        const osg::Matrix& projectionMatrix,
-        const osg::Matrix& viewMatrix,
-        float x,
-        float y) const
-{
-    osg::Matrix matrix = viewMatrix * projectionMatrix;
-    osg::Matrix inverseVP;
-    inverseVP.invert(matrix);
-
-    osg::Vec3 nearPoint = osg::Vec3(x, y, -1.0f) * inverseVP;
-    osg::Vec3 farPoint = osg::Vec3(x, y, 1.0f) * inverseVP;
-
-    return osg::ref_ptr<osg::LineSegment>(
-            new osg::LineSegment(nearPoint, farPoint));
-}
-
 bool
 Q3DWidget::getPlaneLineIntersection(const osg::Vec4d& plane,
                                     const osg::LineSegment& line,

src/ui/map3D/Q3DWidget.h

@@ -73,8 +73,9 @@ public:
     void addTimerFunc(uint msecs, void(*func)(void *),
                       void* clientData);
 
-    std::pair<float,float> getGlobalCursorPosition(int32_t mouseX,
-                                                   int32_t mouseY);
+    std::pair<double,double> getGlobalCursorPosition(int32_t mouseX,
+                                                     int32_t mouseY,
+                                                     double z);
 
 protected slots:
     void redraw(void);
@@ -104,11 +105,6 @@ protected:
     float r2d(float angle);
     float d2r(float angle);
     osgGA::GUIEventAdapter::KeySymbol convertKey(int key) const;
-    osg::ref_ptr<osg::LineSegment> projectNormalizedXYIntoObjectCoordinates(
-            const osg::Matrix& projectionMatrix,
-            const osg::Matrix& viewMatrix,
-            float x,
-            float y) const;
     bool getPlaneLineIntersection(const osg::Vec4d& plane,
                                   const osg::LineSegment& line,
                                   osg::Vec3d& isect);

src/ui/map3D/Q3DWidgetFactory.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 Q3DWidgetFactory.
+ *
+ *   @author Lionel Heng <hengli@student.ethz.ch>
+ *
+ */
+
 #include "Q3DWidgetFactory.h"
 
 #include "Pixhawk3DWidget.h"