Fix flickering submaps. (#440)

The 'slice_pose' must be updated together with the texture. Right
now it is updated before the texture so the submap will be shown
in the wrong pose for a short time.

The fix is to move the handling of poses over to Ogre:
The 'scene_node_' now transforms both the shown submap and the axes
into the submap frame. The 'submap_node_' applies the 'slice_pose'
so that the textured rectangle is shown in the correct pose in
the submap frame.

cartographer_ros/cartographer_ros/map_builder_bridge.cc

@@ -255,8 +255,8 @@ visualization_msgs::MarkerArray MapBuilderBridge::GetConstraintList() {
       // Color mapping for submaps of various trajectories - add trajectory id
       // to ensure different starting colors. Also add a fixed offset of 25
       // to avoid having identical colors as trajectories.
-      color_constraint = ToMessage(cartographer::io::GetColor(
+      color_constraint = ToMessage(
-          constraint.submap_id.submap_index +
+          cartographer::io::GetColor(constraint.submap_id.submap_index +
                                      constraint.submap_id.trajectory_id + 25));
       color_residual.a = 1.0;
       color_residual.r = 1.0;

cartographer_ros/cartographer_ros/node.h

@@ -71,8 +71,7 @@ class Node {
   bool HandleFinishTrajectory(
       cartographer_ros_msgs::FinishTrajectory::Request& request,
       cartographer_ros_msgs::FinishTrajectory::Response& response);
-  bool HandleWriteState(
+  bool HandleWriteState(cartographer_ros_msgs::WriteState::Request& request,
-      cartographer_ros_msgs::WriteState::Request& request,
                         cartographer_ros_msgs::WriteState::Response& response);
   // Returns the set of topic names we want to subscribe to.
   std::unordered_set<string> ComputeExpectedTopics(

cartographer_ros/cartographer_ros/offline_node_main.cc

@@ -273,7 +273,8 @@ void Run(const std::vector<string>& bag_filenames) {
             << (cpu_timespec.tv_sec + 1e-9 * cpu_timespec.tv_nsec) << " s";
 #endif
 
-  node.map_builder_bridge()->SerializeState(bag_filenames.front() + ".pbstream");
+  node.map_builder_bridge()->SerializeState(bag_filenames.front() +
+                                            ".pbstream");
 }
 
 }  // namespace

cartographer_rviz/cartographer_rviz/drawable_submap.cc

@@ -130,10 +130,10 @@ void DrawableSubmap::Update(
   ::cartographer::common::MutexLocker locker(&mutex_);
   metadata_version_ = metadata.submap_version;
   pose_ = ::cartographer_ros::ToRigid3d(metadata.pose);
+  scene_node_->setPosition(ToOgre(pose_.translation()));
+  scene_node_->setOrientation(ToOgre(pose_.rotation()));
   if (submap_texture_ != nullptr) {
-    // We have to update the transform since we are already displaying a texture
+    display_context_->queueRender();
-    // for this submap.
-    UpdateTransform();
   }
   visibility_->setName(
       QString("%1.%2").arg(id_.submap_index).arg(metadata_version_));
@@ -197,7 +197,8 @@ void DrawableSubmap::SetAlpha(const double current_tracking_z) {
 
 void DrawableSubmap::UpdateSceneNode() {
   ::cartographer::common::MutexLocker locker(&mutex_);
-  UpdateTransform();
+  submap_node_->setPosition(ToOgre(submap_texture_->slice_pose.translation()));
+  submap_node_->setOrientation(ToOgre(submap_texture_->slice_pose.rotation()));
   // The call to Ogre's loadRawData below does not work with an RG texture,
   // therefore we create an RGB one whose blue channel is always 0.
   std::vector<char> rgb;
@@ -250,16 +251,7 @@ void DrawableSubmap::UpdateSceneNode() {
 
   texture_unit->setTextureName(texture_->getName());
   texture_unit->setTextureFiltering(Ogre::TFO_NONE);
-}
 
-void DrawableSubmap::UpdateTransform() {
-  CHECK(submap_texture_ != nullptr);
-  const ::cartographer::transform::Rigid3d pose =
-      pose_ * submap_texture_->slice_pose;
-  submap_node_->setPosition(ToOgre(pose.translation()));
-  submap_node_->setOrientation(ToOgre(pose.rotation()));
-  pose_axes_.setPosition(ToOgre(pose_.translation()));
-  pose_axes_.setOrientation(ToOgre(pose_.rotation()));
   display_context_->queueRender();
 }
 

cartographer_rviz/cartographer_rviz/drawable_submap.h

@@ -91,7 +91,6 @@ class DrawableSubmap : public QObject {
   void ToggleVisibility();
 
  private:
-  void UpdateTransform();
   float UpdateAlpha(float target_alpha);
 
   const ::cartographer::mapping::SubmapId id_;