12345qiupeng
/
cartographer_ros
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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.
master
Wolfgang Hess 2017-07-24 15:19:43 +02:00 committed by GitHub
parent 3dceac9a46
commit e6d214c51d
5 changed files with 12 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
cartographer_ros/cartographer_ros/map_builder_bridge.cc
View File

@ -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 {

5
cartographer_ros/cartographer_ros/node.h
View File

@ -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<string> ComputeExpectedTopics(
const TrajectoryOptions& options,

3
cartographer_ros/cartographer_ros/offline_node_main.cc
View File

@ -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

18
cartographer_rviz/cartographer_rviz/drawable_submap.cc
View File

@ -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<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();
}

1
cartographer_rviz/cartographer_rviz/drawable_submap.h
View File

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