From e6d214c51d7b569fadb29ce4223d621b7dc3ebd6 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Mon, 24 Jul 2017 15:19:43 +0200 Subject: [PATCH] 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/map_builder_bridge.cc | 6 +++--- cartographer_ros/cartographer_ros/node.h | 5 ++--- .../cartographer_ros/offline_node_main.cc | 3 ++- .../cartographer_rviz/drawable_submap.cc | 18 +++++------------- .../cartographer_rviz/drawable_submap.h | 1 - 5 files changed, 12 insertions(+), 21 deletions(-) diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.cc b/cartographer_ros/cartographer_ros/map_builder_bridge.cc index a7c7f6f..1fa937d 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.cc +++ b/cartographer_ros/cartographer_ros/map_builder_bridge.cc @@ -255,9 +255,9 @@ 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( - constraint.submap_id.submap_index + - constraint.submap_id.trajectory_id + 25)); + color_constraint = ToMessage( + cartographer::io::GetColor(constraint.submap_id.submap_index + + constraint.submap_id.trajectory_id + 25)); color_residual.a = 1.0; color_residual.r = 1.0; } else { diff --git a/cartographer_ros/cartographer_ros/node.h b/cartographer_ros/cartographer_ros/node.h index 9c15896..81bb7fd 100644 --- a/cartographer_ros/cartographer_ros/node.h +++ b/cartographer_ros/cartographer_ros/node.h @@ -71,9 +71,8 @@ class Node { bool HandleFinishTrajectory( cartographer_ros_msgs::FinishTrajectory::Request& request, cartographer_ros_msgs::FinishTrajectory::Response& response); - bool HandleWriteState( - cartographer_ros_msgs::WriteState::Request& request, - cartographer_ros_msgs::WriteState::Response& response); + bool HandleWriteState(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 ComputeExpectedTopics( const TrajectoryOptions& options, diff --git a/cartographer_ros/cartographer_ros/offline_node_main.cc b/cartographer_ros/cartographer_ros/offline_node_main.cc index ce2b958..4034ab2 100644 --- a/cartographer_ros/cartographer_ros/offline_node_main.cc +++ b/cartographer_ros/cartographer_ros/offline_node_main.cc @@ -273,7 +273,8 @@ void Run(const std::vector& 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 diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.cc b/cartographer_rviz/cartographer_rviz/drawable_submap.cc index b5d529c..2027c2b 100644 --- a/cartographer_rviz/cartographer_rviz/drawable_submap.cc +++ b/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 - // for this submap. - UpdateTransform(); + display_context_->queueRender(); } 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 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(); } diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.h b/cartographer_rviz/cartographer_rviz/drawable_submap.h index 9e5a71b..8574077 100644 --- a/cartographer_rviz/cartographer_rviz/drawable_submap.h +++ b/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_;