RVIZ: added OrbitOrientedViewController plugin, the camera will follow the orientation of the target frame

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@1554 f169173b-cf89-36c8-b27e-44dbe73f0c83

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 )

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_ */