diff --git a/qgroundcontrol.pri b/qgroundcontrol.pri
index 1638f810b15fba4bbd3c0c5676e51124186cd526..b7d88c2dc2d5bd72835caab4b8afbbdd815d5244 100644
--- a/qgroundcontrol.pri
+++ b/qgroundcontrol.pri
@@ -234,10 +234,9 @@ message("Compiling for linux 32")
             -losgGA \
             -losgDB \
             -losgText \
+            -losgQt \
             -lOpenThreads
 
-            #-losgQt \
-
     DEFINES += QGC_OSG_ENABLED
     }
 
@@ -318,10 +317,9 @@ linux-g++-64 {
             -losgGA \
             -losgDB \
             -losgText \
+            -losgQt \
             -lOpenThreads
 
-#            -losgQt \
-
     DEFINES += QGC_OSG_ENABLED
     }
 
diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro
index f93cab1095e1b93c44322252928d5e5f992c3214..dec0ea3d4803a7b452f5deb604643ef6ad8db06a 100644
--- a/qgroundcontrol.pro
+++ b/qgroundcontrol.pro
@@ -387,7 +387,8 @@ contains(DEPENDENCIES_PRESENT, protobuf):contains(MAVLINK_CONF, pixhawk) {
     message("Including headers for Protocol Buffers")
 
     # Enable only if protobuf is available
-    HEADERS += ../mavlink/include/pixhawk/pixhawk.pb.h
+    HEADERS += ../mavlink/include/pixhawk/pixhawk.pb.h \
+               src/ui/map3D/ObstacleGroupNode.h
 }
 contains(DEPENDENCIES_PRESENT, libfreenect) { 
     message("Including headers for libfreenect")
@@ -527,7 +528,8 @@ contains(DEPENDENCIES_PRESENT, protobuf):contains(MAVLINK_CONF, pixhawk) {
     message("Including sources for Protocol Buffers")
 
     # Enable only if protobuf is available
-    SOURCES += ../mavlink/src/pixhawk/pixhawk.pb.cc
+    SOURCES += ../mavlink/src/pixhawk/pixhawk.pb.cc \
+               src/ui/map3D/ObstacleGroupNode.cc
 }
 contains(DEPENDENCIES_PRESENT, libfreenect) { 
     message("Including sources for libfreenect")
diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc
index 7c8aa430c37feb121c48b19bc687dac1de4be34d..d28952f80d0a22526acd22b3c146dcd3150678cb 100644
--- a/src/uas/UAS.cc
+++ b/src/uas/UAS.cc
@@ -961,6 +961,11 @@ void UAS::receiveExtendedMessage(LinkInterface* link, std::tr1::shared_ptr<googl
         rgbdImage.CopyFrom(*message);
         emit rgbdImageChanged(this);
     }
+    else if (message->GetTypeName() == obstacleList.GetTypeName())
+    {
+        obstacleList.CopyFrom(*message);
+        emit obstacleListChanged(this);
+    }
 }
 
 #endif
diff --git a/src/uas/UAS.h b/src/uas/UAS.h
index 093122db15a3754f54f54ab965c46111cb69a40f..76d5c068a7eb18c34303b43315a3f8c09b7ffbd4 100644
--- a/src/uas/UAS.h
+++ b/src/uas/UAS.h
@@ -136,6 +136,10 @@ public:
     px::RGBDImage getRGBDImage() const {
         return rgbdImage;
     }
+
+    px::ObstacleList getObstacleList() const {
+        return obstacleList;
+    }
 #endif
 
     friend class UASWaypointManager;
@@ -223,6 +227,7 @@ protected: //COMMENTS FOR TEST UNIT
 #ifdef QGC_PROTOBUF_ENABLED
     px::PointCloudXYZRGB pointCloud;
     px::RGBDImage rgbdImage;
+    px::ObstacleList obstacleList;
 #endif
 
     QMap<int, QMap<QString, QVariant>* > parameters; ///< All parameters
@@ -556,10 +561,14 @@ signals:
     void imageStarted(quint64 timestamp);
     /** @brief A new camera image has arrived */
     void imageReady(UASInterface* uas);
+#ifdef QGC_PROTOBUF_ENABLED
     /** @brief Point cloud data has been changed */
     void pointCloudChanged(UASInterface* uas);
     /** @brief RGBD image data has been changed */
     void rgbdImageChanged(UASInterface* uas);
+    /** @brief Obstacle list data has been changed */
+    void obstacleListChanged(UASInterface* uas);
+#endif
     /** @brief HIL controls have changed */
     void hilControlsChanged(uint64_t time, float rollAilerons, float pitchElevator, float yawRudder, float throttle, uint8_t systemMode, uint8_t navMode);
 
diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h
index 07e193f97f0396351c324f7a254b3745d03c0a8a..798f1ba7dcde84e7a04ecb7e89d79c604b49e74a 100644
--- a/src/uas/UASInterface.h
+++ b/src/uas/UASInterface.h
@@ -97,6 +97,7 @@ public:
 #ifdef QGC_PROTOBUF_ENABLED
     virtual px::PointCloudXYZRGB getPointCloud() const = 0;
     virtual px::RGBDImage getRGBDImage() const = 0;
+    virtual px::ObstacleList getObstacleList() const = 0;
 #endif
 
     virtual bool isArmed() const = 0;
diff --git a/src/ui/map3D/ObstacleGroupNode.cc b/src/ui/map3D/ObstacleGroupNode.cc
new file mode 100644
index 0000000000000000000000000000000000000000..72120fcea2d58a73c79272beeb396bdd1ef6e7d0
--- /dev/null
+++ b/src/ui/map3D/ObstacleGroupNode.cc
@@ -0,0 +1,89 @@
+/*=====================================================================
+
+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 ObstacleGroupNode.
+ *
+ *   @author Lionel Heng <hengli@inf.ethz.ch>
+ *
+ */
+
+#include "ObstacleGroupNode.h"
+
+#include <osg/PositionAttitudeTransform>
+#include <osg/ShapeDrawable>
+
+#include "Imagery.h"
+
+ObstacleGroupNode::ObstacleGroupNode()
+{
+
+}
+
+void
+ObstacleGroupNode::init(void)
+{
+
+}
+
+void
+ObstacleGroupNode::update(MAV_FRAME frame, UASInterface *uas)
+{
+    if (!uas || frame == MAV_FRAME_GLOBAL)
+    {
+        return;
+    }
+
+    double robotX = uas->getLocalX();
+    double robotY = uas->getLocalY();
+    double robotZ = uas->getLocalZ();
+
+    if (getNumChildren() > 0)
+    {
+        removeChild(0, getNumChildren());
+    }
+
+    osg::ref_ptr<osg::Geode> geode = new osg::Geode;
+
+    px::ObstacleList obstacleList = uas->getObstacleList();
+
+    for (int i = 0; i < obstacleList.obstacles_size(); ++i)
+    {
+        const px::Obstacle& obs = obstacleList.obstacles(i);
+
+        osg::Vec3d obsPos(obs.y() - robotY, obs.x() - robotX, robotZ - obs.z());
+
+        osg::ref_ptr<osg::Box> box =
+            new osg::Box(obsPos, obs.width(), obs.width(), obs.height());
+        osg::ref_ptr<osg::ShapeDrawable> sd = new osg::ShapeDrawable(box);
+
+        sd->getOrCreateStateSet()->setMode(GL_BLEND, osg::StateAttribute::ON);
+        sd->setColor(osg::Vec4(0.0f, 0.0f, 1.0f, 1.0f));
+
+        geode->addDrawable(sd);
+    }
+
+    addChild(geode);
+}
+
diff --git a/src/ui/map3D/ObstacleGroupNode.h b/src/ui/map3D/ObstacleGroupNode.h
new file mode 100644
index 0000000000000000000000000000000000000000..0c965cef0d94ee150b1d14ec3715dbae3255ebef
--- /dev/null
+++ b/src/ui/map3D/ObstacleGroupNode.h
@@ -0,0 +1,48 @@
+/*=====================================================================
+
+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 ObstacleGroupNode.
+ *
+ *   @author Lionel Heng <hengli@inf.ethz.ch>
+ *
+ */
+
+#ifndef OBSTACLEGROUPNODE_H
+#define OBSTACLEGROUPNODE_H
+
+#include <osg/Group>
+
+#include "UASInterface.h"
+
+class ObstacleGroupNode : public osg::Group
+{
+public:
+    ObstacleGroupNode();
+
+    void init(void);
+    void update(MAV_FRAME frame, UASInterface* uas);
+};
+
+#endif // OBSTACLEGROUPNODE_H
diff --git a/src/ui/map3D/Pixhawk3DWidget.cc b/src/ui/map3D/Pixhawk3DWidget.cc
index b212e5102a3274ad6270fa8f6bb0008fb9873350..4a51f9aa7d75a0e2fa39b5a3615a14df54b5d344 100644
--- a/src/ui/map3D/Pixhawk3DWidget.cc
+++ b/src/ui/map3D/Pixhawk3DWidget.cc
@@ -25,7 +25,7 @@
  * @file
  *   @brief Definition of the class Pixhawk3DWidget.
  *
- *   @author Lionel Heng <hengli@student.ethz.ch>
+ *   @author Lionel Heng <hengli@inf.ethz.ch>
  *
  */
 
@@ -60,6 +60,7 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
     , displayWaypoints(true)
     , displayRGBD2D(false)
     , displayRGBD3D(true)
+    , displayObstacleList(true)
     , enableRGBDColor(false)
     , enableTarget(false)
     , followCamera(true)
@@ -98,7 +99,13 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
 
     // generate RGBD model
     rgbd3DNode = createRGBD3D();
-    egocentricMap->addChild(rgbd3DNode);
+    rollingMap->addChild(rgbd3DNode);
+
+#ifdef QGC_PROTOBUF_ENABLED
+    obstacleGroupNode = new ObstacleGroupNode;
+    obstacleGroupNode->init();
+    rollingMap->addChild(obstacleGroupNode);
+#endif
 
     setupHUD();
 
@@ -123,7 +130,8 @@ Pixhawk3DWidget::~Pixhawk3DWidget()
 void
 Pixhawk3DWidget::setActiveUAS(UASInterface* uas)
 {
-    if (this->uas != NULL && this->uas != uas) {
+    if (this->uas != NULL && this->uas != uas)
+    {
         // Disconnect any previously connected active MAV
         //disconnect(uas, SIGNAL(valueChanged(UASInterface*,QString,double,quint64)), this, SLOT(updateValue(UASInterface*,QString,double,quint64)));
     }
@@ -134,9 +142,12 @@ Pixhawk3DWidget::setActiveUAS(UASInterface* uas)
 void
 Pixhawk3DWidget::selectFrame(QString text)
 {
-    if (text.compare("Global") == 0) {
+    if (text.compare("Global") == 0)
+    {
         frame = MAV_FRAME_GLOBAL;
-    } else if (text.compare("Local") == 0) {
+    }
+    else if (text.compare("Local") == 0)
+    {
         frame = MAV_FRAME_LOCAL_NED;
     }
 
@@ -148,9 +159,12 @@ Pixhawk3DWidget::selectFrame(QString text)
 void
 Pixhawk3DWidget::showGrid(int32_t state)
 {
-    if (state == Qt::Checked) {
+    if (state == Qt::Checked)
+    {
         displayGrid = true;
-    } else {
+    }
+    else
+    {
         displayGrid = false;
     }
 }
@@ -158,13 +172,17 @@ Pixhawk3DWidget::showGrid(int32_t state)
 void
 Pixhawk3DWidget::showTrail(int32_t state)
 {
-    if (state == Qt::Checked) {
-        if (!displayTrail) {
+    if (state == Qt::Checked)
+    {
+        if (!displayTrail)
+        {
             trail.clear();
         }
 
         displayTrail = true;
-    } else {
+    }
+    else
+    {
         displayTrail = false;
     }
 }
@@ -172,9 +190,12 @@ Pixhawk3DWidget::showTrail(int32_t state)
 void
 Pixhawk3DWidget::showWaypoints(int state)
 {
-    if (state == Qt::Checked) {
+    if (state == Qt::Checked)
+    {
         displayWaypoints = true;
-    } else {
+    }
+    else
+    {
         displayWaypoints = false;
     }
 }
@@ -184,9 +205,12 @@ Pixhawk3DWidget::selectMapSource(int index)
 {
     mapNode->setImageryType(static_cast<Imagery::ImageryType>(index));
 
-    if (mapNode->getImageryType() == Imagery::BLANK_MAP) {
+    if (mapNode->getImageryType() == Imagery::BLANK_MAP)
+    {
         displayImagery = false;
-    } else {
+    }
+    else
+    {
         displayImagery = true;
     }
 }
@@ -211,9 +235,12 @@ Pixhawk3DWidget::recenter(void)
 void
 Pixhawk3DWidget::toggleFollowCamera(int32_t state)
 {
-    if (state == Qt::Checked) {
+    if (state == Qt::Checked)
+    {
         followCamera = true;
-    } else {
+    }
+    else
+    {
         followCamera = false;
     }
 }
@@ -221,63 +248,76 @@ Pixhawk3DWidget::toggleFollowCamera(int32_t state)
 void
 Pixhawk3DWidget::selectTarget(void)
 {
-    if (uas) {
-        if (frame == MAV_FRAME_GLOBAL) {
-            double altitude = uas->getAltitude();
-
-            std::pair<double,double> cursorWorldCoords =
-                getGlobalCursorPosition(getMouseX(), getMouseY(), altitude);
+    if (!uas)
+    {
+        return;
+    }
 
-            target.set(cursorWorldCoords.first, cursorWorldCoords.second);
-        } else if (frame == MAV_FRAME_LOCAL_NED) {
-            double z = uas->getLocalZ();
+    if (frame == MAV_FRAME_GLOBAL)
+    {
+        double altitude = uas->getAltitude();
 
-            std::pair<double,double> cursorWorldCoords =
-                getGlobalCursorPosition(getMouseX(), getMouseY(), -z);
+        std::pair<double,double> cursorWorldCoords =
+            getGlobalCursorPosition(getMouseX(), getMouseY(), altitude);
 
-            target.set(cursorWorldCoords.first, cursorWorldCoords.second);
-        }
+        target.set(cursorWorldCoords.first, cursorWorldCoords.second);
+    }
+    else if (frame == MAV_FRAME_LOCAL_NED)
+    {
+        double z = uas->getLocalZ();
 
-        uas->setTargetPosition(target.x(), target.y(), 0.0, 0.0);
+        std::pair<double,double> cursorWorldCoords =
+            getGlobalCursorPosition(getMouseX(), getMouseY(), -z);
 
-        enableTarget = true;
+        target.set(cursorWorldCoords.first, cursorWorldCoords.second);
     }
+
+    uas->setTargetPosition(target.x(), target.y(), 0.0, 0.0);
+
+    enableTarget = true;
 }
 
 void
 Pixhawk3DWidget::insertWaypoint(void)
 {
-    if (uas) {
-        Waypoint* wp = NULL;
-        if (frame == MAV_FRAME_GLOBAL) {
-            double latitude = uas->getLatitude();
-            double longitude = uas->getLongitude();
-            double altitude = uas->getAltitude();
-            double x, y;
-            QString utmZone;
-            Imagery::LLtoUTM(latitude, longitude, x, y, utmZone);
-
-            std::pair<double,double> cursorWorldCoords =
-                getGlobalCursorPosition(getMouseX(), getMouseY(), altitude);
-
-            Imagery::UTMtoLL(cursorWorldCoords.first, cursorWorldCoords.second, utmZone,
-                             latitude, longitude);
-
-            wp = new Waypoint(0, longitude, latitude, altitude);
-        } else if (frame == MAV_FRAME_LOCAL_NED) {
-            double z = uas->getLocalZ();
-
-            std::pair<double,double> cursorWorldCoords =
-                getGlobalCursorPosition(getMouseX(), getMouseY(), -z);
-
-            wp = new Waypoint(0, cursorWorldCoords.first,
-                              cursorWorldCoords.second, z);
-        }
+    if (!uas)
+    {
+        return;
+    }
 
-        if (wp) {
-            wp->setFrame(frame);
-            uas->getWaypointManager()->addWaypointEditable(wp);
-        }
+    Waypoint* wp = NULL;
+    if (frame == MAV_FRAME_GLOBAL)
+    {
+        double latitude = uas->getLatitude();
+        double longitude = uas->getLongitude();
+        double altitude = uas->getAltitude();
+        double x, y;
+        QString utmZone;
+        Imagery::LLtoUTM(latitude, longitude, x, y, utmZone);
+
+        std::pair<double,double> cursorWorldCoords =
+            getGlobalCursorPosition(getMouseX(), getMouseY(), altitude);
+
+        Imagery::UTMtoLL(cursorWorldCoords.first, cursorWorldCoords.second, utmZone,
+                         latitude, longitude);
+
+        wp = new Waypoint(0, longitude, latitude, altitude);
+    }
+    else if (frame == MAV_FRAME_LOCAL_NED)
+    {
+        double z = uas->getLocalZ();
+
+        std::pair<double,double> cursorWorldCoords =
+            getGlobalCursorPosition(getMouseX(), getMouseY(), -z);
+
+        wp = new Waypoint(0, cursorWorldCoords.first,
+                          cursorWorldCoords.second, z);
+    }
+
+    if (wp)
+    {
+        wp->setFrame(frame);
+        uas->getWaypointManager()->addWaypointEditable(wp);
     }
 }
 
@@ -290,45 +330,52 @@ Pixhawk3DWidget::moveWaypoint(void)
 void
 Pixhawk3DWidget::setWaypoint(void)
 {
-    if (uas) {
-        const QVector<Waypoint *> waypoints =
-            uas->getWaypointManager()->getWaypointEditableList();
-        Waypoint* waypoint = waypoints.at(selectedWpIndex);
-
-        if (frame == MAV_FRAME_GLOBAL) {
-            double latitude = uas->getLatitude();
-            double longitude = uas->getLongitude();
-            double altitude = uas->getAltitude();
-            double x, y;
-            QString utmZone;
-            Imagery::LLtoUTM(latitude, longitude, x, y, utmZone);
-
-            std::pair<double,double> cursorWorldCoords =
-                getGlobalCursorPosition(getMouseX(), getMouseY(), altitude);
-
-            Imagery::UTMtoLL(cursorWorldCoords.first, cursorWorldCoords.second, utmZone,
-                             latitude, longitude);
-
-            waypoint->setX(longitude);
-            waypoint->setY(latitude);
-            waypoint->setZ(altitude);
-        } else if (frame == MAV_FRAME_LOCAL_NED) {
-            double z = uas->getLocalZ();
-
-            std::pair<double,double> cursorWorldCoords =
-                getGlobalCursorPosition(getMouseX(), getMouseY(), -z);
-
-            waypoint->setX(cursorWorldCoords.first);
-            waypoint->setY(cursorWorldCoords.second);
-            waypoint->setZ(z);
-        }
+    if (!uas)
+    {
+        return;
+    }
+
+    const QVector<Waypoint *> waypoints =
+        uas->getWaypointManager()->getWaypointEditableList();
+    Waypoint* waypoint = waypoints.at(selectedWpIndex);
+
+    if (frame == MAV_FRAME_GLOBAL)
+    {
+        double latitude = uas->getLatitude();
+        double longitude = uas->getLongitude();
+        double altitude = uas->getAltitude();
+        double x, y;
+        QString utmZone;
+        Imagery::LLtoUTM(latitude, longitude, x, y, utmZone);
+
+        std::pair<double,double> cursorWorldCoords =
+            getGlobalCursorPosition(getMouseX(), getMouseY(), altitude);
+
+        Imagery::UTMtoLL(cursorWorldCoords.first, cursorWorldCoords.second, utmZone,
+                         latitude, longitude);
+
+        waypoint->setX(longitude);
+        waypoint->setY(latitude);
+        waypoint->setZ(altitude);
+    }
+    else if (frame == MAV_FRAME_LOCAL_NED)
+    {
+        double z = uas->getLocalZ();
+
+        std::pair<double,double> cursorWorldCoords =
+            getGlobalCursorPosition(getMouseX(), getMouseY(), -z);
+
+        waypoint->setX(cursorWorldCoords.first);
+        waypoint->setY(cursorWorldCoords.second);
+        waypoint->setZ(z);
     }
 }
 
 void
 Pixhawk3DWidget::deleteWaypoint(void)
 {
-    if (uas) {
+    if (uas)
+    {
         uas->getWaypointManager()->removeWaypoint(selectedWpIndex);
     }
 }
@@ -336,26 +383,34 @@ Pixhawk3DWidget::deleteWaypoint(void)
 void
 Pixhawk3DWidget::setWaypointAltitude(void)
 {
-    if (uas) {
-        bool ok;
-        const QVector<Waypoint *> waypoints =
-            uas->getWaypointManager()->getWaypointEditableList();
-        Waypoint* waypoint = waypoints.at(selectedWpIndex);
+    if (!uas)
+    {
+        return;
+    }
 
-        double altitude = waypoint->getZ();
-        if (frame == MAV_FRAME_LOCAL_NED) {
-            altitude = -altitude;
-        }
+    bool ok;
+    const QVector<Waypoint *> waypoints =
+        uas->getWaypointManager()->getWaypointEditableList();
+    Waypoint* waypoint = waypoints.at(selectedWpIndex);
 
-        double newAltitude =
-            QInputDialog::getDouble(this, tr("Set altitude of waypoint %1").arg(selectedWpIndex),
-                                    tr("Altitude (m):"), waypoint->getZ(), -1000.0, 1000.0, 1, &ok);
-        if (ok) {
-            if (frame == MAV_FRAME_GLOBAL) {
-                waypoint->setZ(newAltitude);
-            } else if (frame == MAV_FRAME_LOCAL_NED) {
-                waypoint->setZ(-newAltitude);
-            }
+    double altitude = waypoint->getZ();
+    if (frame == MAV_FRAME_LOCAL_NED)
+    {
+        altitude = -altitude;
+    }
+
+    double newAltitude =
+        QInputDialog::getDouble(this, tr("Set altitude of waypoint %1").arg(selectedWpIndex),
+                                tr("Altitude (m):"), waypoint->getZ(), -1000.0, 1000.0, 1, &ok);
+    if (ok)
+    {
+        if (frame == MAV_FRAME_GLOBAL)
+        {
+            waypoint->setZ(newAltitude);
+        }
+        else if (frame == MAV_FRAME_LOCAL_NED)
+        {
+            waypoint->setZ(-newAltitude);
         }
     }
 }
@@ -363,10 +418,12 @@ Pixhawk3DWidget::setWaypointAltitude(void)
 void
 Pixhawk3DWidget::clearAllWaypoints(void)
 {
-    if (uas) {
+    if (uas)
+    {
         const QVector<Waypoint *> waypoints =
             uas->getWaypointManager()->getWaypointEditableList();
-        for (int i = waypoints.size() - 1; i >= 0; --i) {
+        for (int i = waypoints.size() - 1; i >= 0; --i)
+        {
             uas->getWaypointManager()->removeWaypoint(i);
         }
     }
@@ -393,13 +450,17 @@ Pixhawk3DWidget::findVehicleModels(void)
     nodes.push_back(sphereGeode);
 
     // add all other models in folder
-    for (int i = 0; i < files.size(); ++i) {
+    for (int i = 0; i < files.size(); ++i)
+    {
         osg::ref_ptr<osg::Node> node =
             osgDB::readNodeFile(directory.absoluteFilePath(files[i]).toStdString().c_str());
 
-        if (node) {
+        if (node)
+        {
             nodes.push_back(node);
-        } else {
+        }
+        else
+        {
             printf("%s\n", QString("ERROR: Could not load file " + directory.absoluteFilePath(files[i]) + "\n").toStdString().c_str());
         }
     }
@@ -457,7 +518,8 @@ Pixhawk3DWidget::buildLayout(void)
 
     QLabel* modelLabel = new QLabel("Vehicle", this);
     QComboBox* modelComboBox = new QComboBox(this);
-    for (int i = 0; i < vehicleModels.size(); ++i) {
+    for (int i = 0; i < vehicleModels.size(); ++i)
+    {
         modelComboBox->addItem(vehicleModels[i]->getName().c_str());
     }
 
@@ -508,7 +570,8 @@ Pixhawk3DWidget::buildLayout(void)
 void
 Pixhawk3DWidget::display(void)
 {
-    if (uas == NULL) {
+    if (!uas)
+    {
         return;
     }
 
@@ -516,7 +579,8 @@ Pixhawk3DWidget::display(void)
     QString utmZone;
     getPose(robotX, robotY, robotZ, robotRoll, robotPitch, robotYaw, utmZone);
 
-    if (lastRobotX == 0.0f && lastRobotY == 0.0f && lastRobotZ == 0.0f) {
+    if (lastRobotX == 0.0f && lastRobotY == 0.0f && lastRobotZ == 0.0f)
+    {
         lastRobotX = robotX;
         lastRobotY = robotY;
         lastRobotZ = robotZ;
@@ -524,7 +588,8 @@ Pixhawk3DWidget::display(void)
         recenterCamera(robotY, robotX, -robotZ);
     }
 
-    if (followCamera) {
+    if (followCamera)
+    {
         double dx = robotY - lastRobotY;
         double dy = robotX - lastRobotX;
         double dz = lastRobotZ - robotZ;
@@ -537,26 +602,36 @@ Pixhawk3DWidget::display(void)
                                          robotPitch, osg::Vec3d(1.0f, 0.0f, 0.0f),
                                          robotRoll, osg::Vec3d(0.0f, 1.0f, 0.0f)));
 
-    if (displayTrail) {
+    if (displayTrail)
+    {
         updateTrail(robotX, robotY, robotZ);
     }
 
-    if (frame == MAV_FRAME_GLOBAL && displayImagery) {
+    if (frame == MAV_FRAME_GLOBAL && displayImagery)
+    {
         updateImagery(robotX, robotY, robotZ, utmZone);
     }
 
-    if (displayWaypoints) {
+    if (displayWaypoints)
+    {
         updateWaypoints();
     }
 
-    if (enableTarget) {
+    if (enableTarget)
+    {
         updateTarget(robotX, robotY);
     }
 
 #ifdef QGC_PROTOBUF_ENABLED
-    if (displayRGBD2D || displayRGBD3D) {
+    if (displayRGBD2D || displayRGBD3D)
+    {
         updateRGBD(robotX, robotY, robotZ);
     }
+
+    if (displayObstacleList)
+    {
+        updateObstacles();
+    }
 #endif
 
     updateHUD(robotX, robotY, robotZ, robotRoll, robotPitch, robotYaw, utmZone);
@@ -568,7 +643,10 @@ Pixhawk3DWidget::display(void)
     rollingMap->setChildValue(mapNode, displayImagery);
     rollingMap->setChildValue(waypointGroupNode, displayWaypoints);
     rollingMap->setChildValue(targetNode, enableTarget);
-    egocentricMap->setChildValue(rgbd3DNode, displayRGBD3D);
+#ifdef QGC_PROTOBUF_ENABLED
+    rollingMap->setChildValue(obstacleGroupNode, displayObstacleList);
+#endif
+    rollingMap->setChildValue(rgbd3DNode, displayRGBD3D);
     hudGroup->setChildValue(rgb2DGeode, displayRGBD2D);
     hudGroup->setChildValue(depth2DGeode, displayRGBD2D);
 
@@ -582,8 +660,10 @@ Pixhawk3DWidget::display(void)
 void
 Pixhawk3DWidget::keyPressEvent(QKeyEvent* event)
 {
-    if (!event->text().isEmpty()) {
-        switch (*(event->text().toAscii().data())) {
+    if (!event->text().isEmpty())
+    {
+        switch (*(event->text().toAscii().data()))
+        {
         case '1':
             displayRGBD2D = !displayRGBD2D;
             break;
@@ -594,6 +674,10 @@ Pixhawk3DWidget::keyPressEvent(QKeyEvent* event)
         case 'C':
             enableRGBDColor = !enableRGBDColor;
             break;
+        case 'o':
+        case 'O':
+            displayObstacleList = !displayObstacleList;
+            break;
         }
     }
 
@@ -603,19 +687,25 @@ Pixhawk3DWidget::keyPressEvent(QKeyEvent* event)
 void
 Pixhawk3DWidget::mousePressEvent(QMouseEvent* event)
 {
-    if (event->button() == Qt::LeftButton) {
-        if (mode == MOVE_WAYPOINT_MODE) {
+    if (event->button() == Qt::LeftButton)
+    {
+        if (mode == MOVE_WAYPOINT_MODE)
+        {
             setWaypoint();
             mode = DEFAULT_MODE;
 
             return;
         }
 
-        if (event->modifiers() == Qt::ShiftModifier) {
+        if (event->modifiers() == Qt::ShiftModifier)
+        {
             selectedWpIndex = findWaypoint(event->x(), event->y());
-            if (selectedWpIndex == -1) {
+            if (selectedWpIndex == -1)
+            {
                 showInsertWaypointMenu(event->globalPos());
-            } else {
+            }
+            else
+            {
                 showEditWaypointMenu(event->globalPos());
             }
 
@@ -631,24 +721,30 @@ Pixhawk3DWidget::getPose(double& x, double& y, double& z,
                          double& roll, double& pitch, double& yaw,
                          QString& utmZone)
 {
-    if (uas) {
-        if (frame == MAV_FRAME_GLOBAL) {
-            double latitude = uas->getLatitude();
-            double longitude = uas->getLongitude();
-            double altitude = uas->getAltitude();
-
-            Imagery::LLtoUTM(latitude, longitude, x, y, utmZone);
-            z = -altitude;
-        } else if (frame == MAV_FRAME_LOCAL_NED) {
-            x = uas->getLocalX();
-            y = uas->getLocalY();
-            z = uas->getLocalZ();
-        }
+    if (!uas)
+    {
+        return;
+    }
 
-        roll = uas->getRoll();
-        pitch = uas->getPitch();
-        yaw = uas->getYaw();
+    if (frame == MAV_FRAME_GLOBAL)
+    {
+        double latitude = uas->getLatitude();
+        double longitude = uas->getLongitude();
+        double altitude = uas->getAltitude();
+
+        Imagery::LLtoUTM(latitude, longitude, x, y, utmZone);
+        z = -altitude;
+    }
+    else if (frame == MAV_FRAME_LOCAL_NED)
+    {
+        x = uas->getLocalX();
+        y = uas->getLocalY();
+        z = uas->getLocalZ();
     }
+
+    roll = uas->getRoll();
+    pitch = uas->getPitch();
+    yaw = uas->getYaw();
 }
 
 void
@@ -663,23 +759,25 @@ void
 Pixhawk3DWidget::getPosition(double& x, double& y, double& z,
                              QString& utmZone)
 {
-    if (uas)
+    if (!uas)
     {
-        if (frame == MAV_FRAME_GLOBAL)
-        {
-            double latitude = uas->getLatitude();
-            double longitude = uas->getLongitude();
-            double altitude = uas->getAltitude();
+        return;
+    }
 
-            Imagery::LLtoUTM(latitude, longitude, x, y, utmZone);
-            z = -altitude;
-        }
-        else if (frame == MAV_FRAME_LOCAL_NED)
-        {
-            x = uas->getLocalX();
-            y = uas->getLocalY();
-            z = uas->getLocalZ();
-        }
+    if (frame == MAV_FRAME_GLOBAL)
+    {
+        double latitude = uas->getLatitude();
+        double longitude = uas->getLongitude();
+        double altitude = uas->getAltitude();
+
+        Imagery::LLtoUTM(latitude, longitude, x, y, utmZone);
+        z = -altitude;
+    }
+    else if (frame == MAV_FRAME_LOCAL_NED)
+    {
+        x = uas->getLocalX();
+        y = uas->getLocalY();
+        z = uas->getLocalZ();
     }
 }
 
@@ -850,7 +948,7 @@ Pixhawk3DWidget::setupHUD(void)
 
     statusText = new osgText::Text;
     statusText->setCharacterSize(11);
-    statusText->setFont("images/Vera.ttf");
+    statusText->setFont(font);
     statusText->setAxisAlignment(osgText::Text::SCREEN);
     statusText->setColor(osg::Vec4(255, 255, 255, 1));
 
@@ -1059,7 +1157,7 @@ Pixhawk3DWidget::updateImagery(double originX, double originY, double originZ,
         maxResolution = 0.25;
         break;
     default:
-    {}
+        {}
     }
 
     double resolution = minResolution;
@@ -1295,7 +1393,8 @@ Pixhawk3DWidget::updateRGBD(double robotX, double robotY, double robotZ)
 
     osg::Vec3Array* vertices = static_cast<osg::Vec3Array*>(geometry->getVertexArray());
     osg::Vec4Array* colors = static_cast<osg::Vec4Array*>(geometry->getColorArray());
-    for (int i = 0; i < pointCloud.points_size(); ++i) {
+    for (int i = 0; i < pointCloud.points_size(); ++i)
+    {
         const px::PointCloudXYZRGB_PointXYZRGB& p = pointCloud.points(i);
 
         double x = p.x() - robotX;
@@ -1305,7 +1404,8 @@ Pixhawk3DWidget::updateRGBD(double robotX, double robotY, double robotZ)
 
         (*vertices)[i].set(y, x, -z);
 
-        if (enableRGBDColor) {
+        if (enableRGBDColor)
+        {
             float rgb = p.rgb();
 
             float b = *(reinterpret_cast<unsigned char*>(&rgb)) / 255.0f;
@@ -1313,7 +1413,9 @@ Pixhawk3DWidget::updateRGBD(double robotX, double robotY, double robotZ)
             float r = *(2 + reinterpret_cast<unsigned char*>(&rgb)) / 255.0f;
 
             (*colors)[i].set(r, g, b, 1.0f);
-        } else {
+        }
+        else
+        {
             double dist = sqrt(x * x + y * y + z * z);
             int colorIndex = static_cast<int>(fmin(dist / 7.0 * 127.0, 127.0));
             (*colors)[i].set(colormap_jet[colorIndex][0],
@@ -1323,28 +1425,43 @@ Pixhawk3DWidget::updateRGBD(double robotX, double robotY, double robotZ)
         }
     }
 
-    if (geometry->getNumPrimitiveSets() == 0) {
+    if (geometry->getNumPrimitiveSets() == 0)
+    {
         geometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::POINTS,
                                   0, pointCloud.points_size()));
-    } else {
+    }
+    else
+    {
         osg::DrawArrays* drawarrays = static_cast<osg::DrawArrays*>(geometry->getPrimitiveSet(0));
         drawarrays->setCount(pointCloud.points_size());
     }
 }
+
+void
+Pixhawk3DWidget::updateObstacles(void)
+{
+    obstacleGroupNode->update(frame, uas);
+}
+
 #endif
 
 int
 Pixhawk3DWidget::findWaypoint(int mouseX, int mouseY)
 {
-    if (getSceneData() != NULL) {
+    if (getSceneData())
+    {
         osgUtil::LineSegmentIntersector::Intersections intersections;
 
-        if (computeIntersections(mouseX, height() - mouseY, intersections)) {
+        if (computeIntersections(mouseX, height() - mouseY, intersections))
+        {
             for (osgUtil::LineSegmentIntersector::Intersections::iterator
-                    it = intersections.begin(); it != intersections.end(); it++) {
-                for (uint i = 0 ; i < it->nodePath.size(); ++i) {
+                    it = intersections.begin(); it != intersections.end(); it++)
+            {
+                for (uint i = 0 ; i < it->nodePath.size(); ++i)
+                {
                     std::string nodeName = it->nodePath[i]->getName();
-                    if (nodeName.substr(0, 2).compare("wp") == 0) {
+                    if (nodeName.substr(0, 2).compare("wp") == 0)
+                    {
                         return atoi(nodeName.substr(2).c_str());
                     }
                 }
@@ -1358,15 +1475,20 @@ Pixhawk3DWidget::findWaypoint(int mouseX, int mouseY)
 bool
 Pixhawk3DWidget::findTarget(int mouseX, int mouseY)
 {
-    if (getSceneData() != NULL) {
+    if (getSceneData())
+    {
         osgUtil::LineSegmentIntersector::Intersections intersections;
 
-        if (computeIntersections(mouseX, height() - mouseY, intersections)) {
+        if (computeIntersections(mouseX, height() - mouseY, intersections))
+        {
             for (osgUtil::LineSegmentIntersector::Intersections::iterator
-                    it = intersections.begin(); it != intersections.end(); it++) {
-                for (uint i = 0 ; i < it->nodePath.size(); ++i) {
+                    it = intersections.begin(); it != intersections.end(); it++)
+            {
+                for (uint i = 0 ; i < it->nodePath.size(); ++i)
+                {
                     std::string nodeName = it->nodePath[i]->getName();
-                    if (nodeName.compare("Target") == 0) {
+                    if (nodeName.compare("Target") == 0)
+                    {
                         return true;
                     }
                 }
diff --git a/src/ui/map3D/Pixhawk3DWidget.h b/src/ui/map3D/Pixhawk3DWidget.h
index 6773bab0063b4c97f383748dcf8b89b5beefbb52..3ce5ceef2c0339f896ad4527ad9b3bc6e2914653 100644
--- a/src/ui/map3D/Pixhawk3DWidget.h
+++ b/src/ui/map3D/Pixhawk3DWidget.h
@@ -25,7 +25,7 @@
  * @file
  *   @brief Definition of the class Pixhawk3DWidget.
  *
- *   @author Lionel Heng <hengli@student.ethz.ch>
+ *   @author Lionel Heng <hengli@inf.ethz.ch>
  *
  */
 
@@ -38,6 +38,9 @@
 #include "Imagery.h"
 #include "ImageWindowGeode.h"
 #include "WaypointGroupNode.h"
+#ifdef QGC_PROTOBUF_ENABLED
+    #include "ObstacleGroupNode.h"
+#endif
 
 #include "Q3DWidget.h"
 
@@ -113,6 +116,7 @@ private:
     void updateTarget(double robotX, double robotY);
 #ifdef QGC_PROTOBUF_ENABLED
     void updateRGBD(double robotX, double robotY, double robotZ);
+    void updateObstacles(void);
 #endif
 
     int findWaypoint(int mouseX, int mouseY);
@@ -133,6 +137,7 @@ private:
     bool displayWaypoints;
     bool displayRGBD2D;
     bool displayRGBD3D;
+    bool displayObstacleList;
     bool enableRGBDColor;
     bool enableTarget;
 
@@ -157,6 +162,9 @@ private:
     osg::ref_ptr<WaypointGroupNode> waypointGroupNode;
     osg::ref_ptr<osg::Node> targetNode;
     osg::ref_ptr<osg::Geode> rgbd3DNode;
+#ifdef QGC_PROTOBUF_ENABLED
+    osg::ref_ptr<ObstacleGroupNode> obstacleGroupNode;
+#endif
 
     QVector< osg::ref_ptr<osg::Node> > vehicleModels;
 
diff --git a/src/ui/map3D/Q3DWidget.cc b/src/ui/map3D/Q3DWidget.cc
index a14fd11fe6a356e24cfe784ec15d248db42d810f..af37b350590b21dbe014110f8a7d243023df67c4 100644
--- a/src/ui/map3D/Q3DWidget.cc
+++ b/src/ui/map3D/Q3DWidget.cc
@@ -35,6 +35,7 @@ This file is part of the QGROUNDCONTROL project
 #include <osg/Geometry>
 #include <osg/LineWidth>
 #include <osg/MatrixTransform>
+#include <osgQt/QFontImplementation>
 #ifdef Q_OS_MACX
 #include <Carbon/Carbon.h>
 #endif
@@ -57,6 +58,10 @@ Q3DWidget::Q3DWidget(QWidget* parent)
     cameraParams.minClipRange = 1.0f;
     cameraParams.maxClipRange = 10000.0f;
 
+    osg::ref_ptr<osgText::Font::FontImplementation> fontImpl;
+    fontImpl = new osgQt::QFontImplementation(QFont(":/general/vera.ttf"));
+    font = new osgText::Font(fontImpl);
+
     osgGW = new osgViewer::GraphicsWindowEmbedded(0, 0, width(), height());
 
     setThreadingModel(osgViewer::Viewer::SingleThreaded);
diff --git a/src/ui/map3D/Q3DWidget.h b/src/ui/map3D/Q3DWidget.h
index 1b4c83cbf7df47d717030479b0a8b6f63c4d444b..f3b257a33fe339597eeccdf5c08a3d7f81558eb1 100644
--- a/src/ui/map3D/Q3DWidget.h
+++ b/src/ui/map3D/Q3DWidget.h
@@ -38,6 +38,7 @@ This file is part of the QGROUNDCONTROL project
 #include <osg/LineSegment>
 #include <osg/PositionAttitudeTransform>
 #include <osgGA/TrackballManipulator>
+#include <osgText/Font>
 #include <osgViewer/Viewer>
 
 #include "GCManipulator.h"
@@ -259,6 +260,8 @@ protected:
 
     CameraParams cameraParams; /**< Struct representing camera parameters. */
     float fps;
+
+    osg::ref_ptr<osgText::Font> font;
 };
 
 #endif // Q3DWIDGET_H
diff --git a/src/ui/map3D/WaypointGroupNode.cc b/src/ui/map3D/WaypointGroupNode.cc
index 4d56ac4888a04bbf08b1f56d611d76c2f4fd8a27..246bf040712b9d9fda38d769b55f3158426a5e3d 100644
--- a/src/ui/map3D/WaypointGroupNode.cc
+++ b/src/ui/map3D/WaypointGroupNode.cc
@@ -25,7 +25,7 @@ This file is part of the QGROUNDCONTROL project
  * @file
  *   @brief Definition of the class WaypointGroupNode.
  *
- *   @author Lionel Heng <hengli@student.ethz.ch>
+ *   @author Lionel Heng <hengli@inf.ethz.ch>
  *
  */
 
@@ -51,102 +51,115 @@ WaypointGroupNode::init(void)
 void
 WaypointGroupNode::update(MAV_FRAME frame, UASInterface *uas)
 {
-    if (uas) {
-        double robotX = 0.0;
-        double robotY = 0.0;
-        double robotZ = 0.0;
-        if (frame == MAV_FRAME_GLOBAL) {
-            double latitude = uas->getLatitude();
-            double longitude = uas->getLongitude();
-            double altitude = uas->getAltitude();
-
-            QString utmZone;
-            Imagery::LLtoUTM(latitude, longitude, robotX, robotY, utmZone);
-            robotZ = -altitude;
-        } else if (frame == MAV_FRAME_LOCAL_NED) {
-            robotX = uas->getLocalX();
-            robotY = uas->getLocalY();
-            robotZ = uas->getLocalZ();
-        }
-
-        if (getNumChildren() > 0) {
-            removeChild(0, getNumChildren());
-        }
+    if (!uas)
+    {
+        return;
+    }
 
-        const QVector<Waypoint *>& list = uas->getWaypointManager()->getWaypointEditableList();
+    double robotX = 0.0;
+    double robotY = 0.0;
+    double robotZ = 0.0;
+    if (frame == MAV_FRAME_GLOBAL)
+    {
+        double latitude = uas->getLatitude();
+        double longitude = uas->getLongitude();
+        double altitude = uas->getAltitude();
 
-        for (int i = 0; i < list.size(); i++) {
-            Waypoint* wp = list.at(i);
+        QString utmZone;
+        Imagery::LLtoUTM(latitude, longitude, robotX, robotY, utmZone);
+        robotZ = -altitude;
+    }
+    else if (frame == MAV_FRAME_LOCAL_NED)
+    {
+        robotX = uas->getLocalX();
+        robotY = uas->getLocalY();
+        robotZ = uas->getLocalZ();
+    }
 
-            double wpX, wpY, wpZ;
-            getPosition(wp, wpX, wpY, wpZ);
+    if (getNumChildren() > 0)
+    {
+        removeChild(0, getNumChildren());
+    }
 
-            osg::ref_ptr<osg::ShapeDrawable> sd = new osg::ShapeDrawable;
-            osg::ref_ptr<osg::Cylinder> cylinder =
-                new osg::Cylinder(osg::Vec3d(0.0, 0.0, -wpZ / 2.0),
-                                  wp->getAcceptanceRadius(),
-                                  fabs(wpZ));
+    const QVector<Waypoint *>& list = uas->getWaypointManager()->getWaypointEditableList();
 
-            sd->setShape(cylinder);
-            sd->getOrCreateStateSet()->setMode(GL_BLEND, osg::StateAttribute::ON);
+    for (int i = 0; i < list.size(); i++)
+    {
+        Waypoint* wp = list.at(i);
 
-            if (wp->getCurrent()) {
-                sd->setColor(osg::Vec4(1.0f, 0.3f, 0.3f, 0.5f));
-            } else {
-                sd->setColor(osg::Vec4(0.0f, 1.0f, 0.0f, 0.5f));
-            }
+        double wpX, wpY, wpZ;
+        getPosition(wp, wpX, wpY, wpZ);
 
-            osg::ref_ptr<osg::Geode> geode = new osg::Geode;
-            geode->addDrawable(sd);
+        osg::ref_ptr<osg::ShapeDrawable> sd = new osg::ShapeDrawable;
+        osg::ref_ptr<osg::Cylinder> cylinder =
+            new osg::Cylinder(osg::Vec3d(0.0, 0.0, -wpZ / 2.0),
+                              wp->getAcceptanceRadius(),
+                              fabs(wpZ));
 
-            char wpLabel[10];
-            sprintf(wpLabel, "wp%d", i);
-            geode->setName(wpLabel);
+        sd->setShape(cylinder);
+        sd->getOrCreateStateSet()->setMode(GL_BLEND, osg::StateAttribute::ON);
 
-            if (i < list.size() - 1) {
-                double nextWpX, nextWpY, nextWpZ;
-                getPosition(list.at(i + 1), nextWpX, nextWpY, nextWpZ);
+        if (wp->getCurrent())
+        {
+            sd->setColor(osg::Vec4(1.0f, 0.3f, 0.3f, 0.5f));
+        }
+        else
+        {
+            sd->setColor(osg::Vec4(0.0f, 1.0f, 0.0f, 0.5f));
+        }
 
-                osg::ref_ptr<osg::Geometry> geometry = new osg::Geometry;
-                osg::ref_ptr<osg::Vec3dArray> vertices = new osg::Vec3dArray;
-                vertices->push_back(osg::Vec3d(0.0, 0.0, -wpZ));
-                vertices->push_back(osg::Vec3d(nextWpY - wpY,
-                                               nextWpX - wpX,
-                                               -nextWpZ));
-                geometry->setVertexArray(vertices);
+        osg::ref_ptr<osg::Geode> geode = new osg::Geode;
+        geode->addDrawable(sd);
 
-                osg::ref_ptr<osg::Vec4Array> colors = new osg::Vec4Array;
-                colors->push_back(osg::Vec4(0.0f, 1.0f, 0.0f, 0.5f));
-                geometry->setColorArray(colors);
-                geometry->setColorBinding(osg::Geometry::BIND_OVERALL);
+        char wpLabel[10];
+        sprintf(wpLabel, "wp%d", i);
+        geode->setName(wpLabel);
 
-                geometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::LINES, 0, 2));
+        if (i < list.size() - 1)
+        {
+            double nextWpX, nextWpY, nextWpZ;
+            getPosition(list.at(i + 1), nextWpX, nextWpY, nextWpZ);
 
-                osg::ref_ptr<osg::LineWidth> linewidth(new osg::LineWidth());
-                linewidth->setWidth(2.0f);
-                geometry->getOrCreateStateSet()->setAttributeAndModes(linewidth, osg::StateAttribute::ON);
-                geometry->getOrCreateStateSet()->setMode(GL_LIGHTING, osg::StateAttribute::OFF);
+            osg::ref_ptr<osg::Geometry> geometry = new osg::Geometry;
+            osg::ref_ptr<osg::Vec3dArray> vertices = new osg::Vec3dArray;
+            vertices->push_back(osg::Vec3d(0.0, 0.0, -wpZ));
+            vertices->push_back(osg::Vec3d(nextWpY - wpY,
+                                           nextWpX - wpX,
+                                           -nextWpZ));
+            geometry->setVertexArray(vertices);
 
-                geode->addDrawable(geometry);
-            }
+            osg::ref_ptr<osg::Vec4Array> colors = new osg::Vec4Array;
+            colors->push_back(osg::Vec4(0.0f, 1.0f, 0.0f, 0.5f));
+            geometry->setColorArray(colors);
+            geometry->setColorBinding(osg::Geometry::BIND_OVERALL);
 
-            osg::ref_ptr<osg::PositionAttitudeTransform> pat =
-                new osg::PositionAttitudeTransform;
+            geometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::LINES, 0, 2));
 
-            pat->setPosition(osg::Vec3d(wpY - robotY,
-                                        wpX - robotX,
-                                        robotZ));
+            osg::ref_ptr<osg::LineWidth> linewidth(new osg::LineWidth());
+            linewidth->setWidth(2.0f);
+            geometry->getOrCreateStateSet()->setAttributeAndModes(linewidth, osg::StateAttribute::ON);
+            geometry->getOrCreateStateSet()->setMode(GL_LIGHTING, osg::StateAttribute::OFF);
 
-            addChild(pat);
-            pat->addChild(geode);
+            geode->addDrawable(geometry);
         }
+
+        osg::ref_ptr<osg::PositionAttitudeTransform> pat =
+            new osg::PositionAttitudeTransform;
+
+        pat->setPosition(osg::Vec3d(wpY - robotY,
+                                    wpX - robotX,
+                                    robotZ));
+
+        addChild(pat);
+        pat->addChild(geode);
     }
 }
 
 void
 WaypointGroupNode::getPosition(Waypoint* wp, double &x, double &y, double &z)
 {
-    if (wp->getFrame() == MAV_FRAME_GLOBAL) {
+    if (wp->getFrame() == MAV_FRAME_GLOBAL)
+    {
         double latitude = wp->getY();
         double longitude = wp->getX();
         double altitude = wp->getZ();
@@ -154,7 +167,9 @@ WaypointGroupNode::getPosition(Waypoint* wp, double &x, double &y, double &z)
         QString utmZone;
         Imagery::LLtoUTM(latitude, longitude, x, y, utmZone);
         z = -altitude;
-    } else if (wp->getFrame() == MAV_FRAME_LOCAL_NED) {
+    }
+    else if (wp->getFrame() == MAV_FRAME_LOCAL_NED)
+    {
         x = wp->getX();
         y = wp->getY();
         z = wp->getZ();
diff --git a/src/ui/map3D/WaypointGroupNode.h b/src/ui/map3D/WaypointGroupNode.h
index 1c1c4de8ec46e55c2f8ccb0bdfd1bd1e8d360f91..e4df6049df80eabd5918fd8ca95a452460727123 100644
--- a/src/ui/map3D/WaypointGroupNode.h
+++ b/src/ui/map3D/WaypointGroupNode.h
@@ -25,7 +25,7 @@ This file is part of the QGROUNDCONTROL project
  * @file
  *   @brief Definition of the class WaypointGroupNode.
  *
- *   @author Lionel Heng <hengli@student.ethz.ch>
+ *   @author Lionel Heng <hengli@inf.ethz.ch>
  *
  */