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

Follow googlecartographer/cartographer#498. (#495)

master
Wolfgang Hess 2017-09-01 16:15:16 +02:00 committed by GitHub
parent 0305ac67d8
commit 44b9e7b531
1 changed files with 0 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
cartographer_ros/cartographer_ros/map_builder_bridge.cc
View File

@ -191,10 +191,6 @@ visualization_msgs::MarkerArray MapBuilderBridge::GetTrajectoryNodeList() {
if (node.trimmed()) { if (node.trimmed()) {
continue; continue;
} }
// In 2D, the pose in node.pose is xy-aligned. Multiplying by
// node.constant_data->tracking_to_pose would give the full orientation,
// but that is not needed here since we are only interested in the
// translational part.
const ::geometry_msgs::Point node_point = const ::geometry_msgs::Point node_point =
ToGeometryMsgPoint(node.pose.translation()); ToGeometryMsgPoint(node.pose.translation());
marker.points.push_back(node_point); marker.points.push_back(node_point);
@ -284,14 +280,10 @@ visualization_msgs::MarkerArray MapBuilderBridge::GetConstraintList() {
all_submap_data[constraint.submap_id.trajectory_id] all_submap_data[constraint.submap_id.trajectory_id]
[constraint.submap_id.submap_index]; [constraint.submap_id.submap_index];
const auto& submap_pose = submap_data.pose; const auto& submap_pose = submap_data.pose;
// Similar to GetTrajectoryNodeList(), we can skip multiplying with
// node.constant_data->tracking_to_pose.
const auto& trajectory_node_pose = const auto& trajectory_node_pose =
all_trajectory_nodes[constraint.node_id.trajectory_id] all_trajectory_nodes[constraint.node_id.trajectory_id]
[constraint.node_id.node_index] [constraint.node_id.node_index]
.pose; .pose;
// Similar to GetTrajectoryNodeList(), we can skip multiplying with
// node.constant_data->tracking_to_pose.
const cartographer::transform::Rigid3d constraint_pose = const cartographer::transform::Rigid3d constraint_pose =
submap_pose * constraint.pose.zbar_ij; submap_pose * constraint.pose.zbar_ij;