Merge pull request #569 from kwaegel/coordSystemPose_pr

Added updateCoordinateSystemPose(..) to PCLVisualizer.

visualization/include/pcl/visualization/pcl_visualizer.h

@@ -521,6 +521,18 @@ namespace pcl
         bool
         updateShapePose (const std::string &id, const Eigen::Affine3f& pose);
 
+        /** \brief Set the pose of an existing coordinate system.
+          *
+          * Returns false if the coordinate system doesn't exist, true if the pose was successfully
+          * updated.
+          *
+          * \param[in] id the point cloud object id (i.e., given on \a addCoordinateSystem etc.)
+          * \param[in] pose the new pose
+          * \return false if no coordinate system with the specified ID was found
+          */
+        bool
+        updateCoordinateSystemPose (const std::string &id, const Eigen::Affine3f& pose);
+
         /** \brief Set the pose of an existing point cloud.
           *
           * Returns false if the point cloud doesn't exist, true if the pose was successfully

visualization/src/pcl_visualizer.cpp

@@ -1683,6 +1683,29 @@ pcl::visualization::PCLVisualizer::updateShapePose (const std::string &id, const
   return (true);
 }
 
+/////////////////////////////////////////////////////////////////////////////////////////////
+bool
+pcl::visualization::PCLVisualizer::updateCoordinateSystemPose (const std::string &id, const Eigen::Affine3f& pose)
+{
+  ShapeActorMap::iterator am_it = coordinate_actor_map_->find (id);
+
+  vtkLODActor* actor;
+
+  if (am_it == coordinate_actor_map_->end ())
+    return (false);
+  else
+    actor = vtkLODActor::SafeDownCast (am_it->second);
+
+  vtkSmartPointer<vtkMatrix4x4> matrix = vtkSmartPointer<vtkMatrix4x4>::New ();
+
+  convertToVtkMatrix (pose.matrix (), matrix);
+
+  actor->SetUserMatrix (matrix);
+  actor->Modified ();
+
+  return (true);
+}
+
 /////////////////////////////////////////////////////////////////////////////////////////////
 bool
 pcl::visualization::PCLVisualizer::updatePointCloudPose (const std::string &id, const Eigen::Affine3f& pose)