diff --git a/CMakeLists.txt b/CMakeLists.txt
index bab99311..dda57a7e 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -105,11 +105,12 @@ SET(Libraries
 ## RVIZ plugin
 qt4_wrap_cpp(MOC_FILES
   src/rviz/MapCloudDisplay.h
+  src/rviz/OrbitOrientedViewController.h
 )
 
 # tf:message_filters, mixing boost and Qt signals
 set_property(
-   SOURCE src/rviz/MapCloudDisplay.cpp
+   SOURCE src/rviz/MapCloudDisplay.cpp src/rviz/OrbitOrientedViewController.cpp
    PROPERTY COMPILE_DEFINITIONS QT_NO_KEYWORDS
    )
    
@@ -120,6 +121,7 @@ add_library(rtabmap_ros
    src/nodelets/point_cloud_xyzrgb.cpp 
    src/MsgConversion.cpp
    src/rviz/MapCloudDisplay.cpp
+   src/rviz/OrbitOrientedViewController.cpp
    ${MOC_FILES}
 )
 target_link_libraries(rtabmap_ros
diff --git a/plugin_description.xml b/plugin_description.xml
index 61f19678..482090f0 100644
--- a/plugin_description.xml
+++ b/plugin_description.xml
@@ -6,4 +6,11 @@
       Displays graph point clouds from rtabmap/MapData messages.
     </description>
   </class>
+  <class name="rtabmap/OrbitOriented"
+         type="rtabmap::OrbitOrientedViewController"
+         base_class_type="rviz::ViewController">
+    <description>
+      Camera orbit with orientation.
+    </description>
+  </class>
 </library>
diff --git a/src/rviz/OrbitOrientedViewController.cpp b/src/rviz/OrbitOrientedViewController.cpp
new file mode 100644
index 00000000..8ea363ee
--- /dev/null
+++ b/src/rviz/OrbitOrientedViewController.cpp
@@ -0,0 +1,56 @@
+/*
+ * OrbitOrientedViewController.cpp
+ *
+ *  Created on: 2014-07-13
+ *      Author: mathieu
+ */
+
+#include "OrbitOrientedViewController.h"
+
+#include <OgreCamera.h>
+#include <OgreQuaternion.h>
+#include <OgreSceneManager.h>
+#include <OgreSceneNode.h>
+#include <OgreVector3.h>
+#include <OgreViewport.h>
+
+#include "rviz/properties/float_property.h"
+#include "rviz/properties/vector_property.h"
+#include "rviz/ogre_helpers/shape.h"
+
+namespace rtabmap
+{
+
+void OrbitOrientedViewController::updateCamera()
+{
+	float distance = distance_property_->getFloat();
+	float yaw = yaw_property_->getFloat();
+	float pitch = pitch_property_->getFloat();
+
+	Ogre::Matrix3 rot;
+	reference_orientation_.ToRotationMatrix(rot);
+	Ogre::Radian rollTarget, pitchTarget, yawTarget;
+	rot.ToEulerAnglesXYZ(yawTarget, pitchTarget, rollTarget);
+
+	yaw += rollTarget.valueRadians();
+	pitch += pitchTarget.valueRadians();
+
+	Ogre::Vector3 focal_point = focal_point_property_->getVector();
+
+	float x = distance * cos( yaw ) * cos( pitch ) + focal_point.x;
+	float y = distance * sin( yaw ) * cos( pitch ) + focal_point.y;
+	float z = distance *              sin( pitch ) + focal_point.z;
+
+	Ogre::Vector3 pos( x, y, z );
+
+	camera_->setPosition(pos);
+	camera_->setFixedYawAxis(true, target_scene_node_->getOrientation() * Ogre::Vector3::UNIT_Z);
+	camera_->setDirection(target_scene_node_->getOrientation() * (focal_point - pos));
+
+	focal_shape_->setPosition( focal_point );
+}
+
+}
+
+#include <pluginlib/class_list_macros.h>
+PLUGINLIB_EXPORT_CLASS( rtabmap::OrbitOrientedViewController, rviz::ViewController )
diff --git a/src/rviz/OrbitOrientedViewController.h b/src/rviz/OrbitOrientedViewController.h
new file mode 100644
index 00000000..691ab2de
--- /dev/null
+++ b/src/rviz/OrbitOrientedViewController.h
@@ -0,0 +1,31 @@
+/*
+ * OrbitOrientedViewController.h
+ *
+ *  Created on: 2014-07-13
+ *      Author: mathieu
+ */
+
+#ifndef ORBITORIENTEDVIEWCONTROLLER_H_
+#define ORBITORIENTEDVIEWCONTROLLER_H_
+
+#include "rviz/default_plugin/view_controllers/orbit_view_controller.h"
+
+namespace rtabmap
+{
+
+class OrbitOrientedViewController: public rviz::OrbitViewController
+{
+Q_OBJECT
+public:
+	OrbitOrientedViewController() {}
+	virtual ~OrbitOrientedViewController() {}
+
+protected:
+  virtual void updateCamera();
+};
+
+}
+
+
+
+#endif /* ORBITORIENTEDVIEWCONTROLLER_H_ */