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

Follow googlecartographer/cartographer#603. (#550)

master
Christoph Schütte 2017-10-19 15:39:23 +02:00 committed by GitHub
parent b9dbfc6664
commit adbaeb4fe6
1 changed files with 14 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
cartographer_ros/cartographer_ros/map_builder_bridge.cc
View File

@ -150,7 +150,8 @@ cartographer_ros_msgs::SubmapList MapBuilderBridge::GetSubmapList() {
cartographer_ros_msgs::SubmapList submap_list; cartographer_ros_msgs::SubmapList submap_list;
submap_list.header.stamp = ::ros::Time::now(); submap_list.header.stamp = ::ros::Time::now();
submap_list.header.frame_id = node_options_.map_frame; submap_list.header.frame_id = node_options_.map_frame;
for (const auto& submap_id_data : map_builder_.sparse_pose_graph()->GetAllSubmapData()) { for (const auto &submap_id_data :
map_builder_.sparse_pose_graph()->GetAllSubmapData()) {
cartographer_ros_msgs::SubmapEntry submap_entry; cartographer_ros_msgs::SubmapEntry submap_entry;
submap_entry.trajectory_id = submap_id_data.id.trajectory_id; submap_entry.trajectory_id = submap_id_data.id.trajectory_id;
submap_entry.submap_index = submap_id_data.id.submap_index; submap_entry.submap_index = submap_id_data.id.submap_index;
@ -192,22 +193,18 @@ MapBuilderBridge::GetTrajectoryStates() {
visualization_msgs::MarkerArray MapBuilderBridge::GetTrajectoryNodeList() { visualization_msgs::MarkerArray MapBuilderBridge::GetTrajectoryNodeList() {
visualization_msgs::MarkerArray trajectory_node_list; visualization_msgs::MarkerArray trajectory_node_list;
const auto all_trajectory_nodes = const auto nodes = map_builder_.sparse_pose_graph()->GetTrajectoryNodes();
map_builder_.sparse_pose_graph()->GetTrajectoryNodes(); for (const int trajectory_id : nodes.trajectory_ids()) {
for (int trajectory_id = 0;
trajectory_id < static_cast<int>(all_trajectory_nodes.size());
++trajectory_id) {
const auto& single_trajectory_nodes = all_trajectory_nodes[trajectory_id];
visualization_msgs::Marker marker = visualization_msgs::Marker marker =
CreateTrajectoryMarker(trajectory_id, node_options_.map_frame); CreateTrajectoryMarker(trajectory_id, node_options_.map_frame);
for (const auto& node : single_trajectory_nodes) { for (const auto& node_id_data : nodes.trajectory(trajectory_id)) {
if (node.constant_data == nullptr) { if (node_id_data.data.constant_data == nullptr) {
PushAndResetLineMarker(&marker, &trajectory_node_list.markers); PushAndResetLineMarker(&marker, &trajectory_node_list.markers);
continue; continue;
} }
const ::geometry_msgs::Point node_point = const ::geometry_msgs::Point node_point =
ToGeometryMsgPoint(node.global_pose.translation()); ToGeometryMsgPoint(node_id_data.data.global_pose.translation());
marker.points.push_back(node_point); marker.points.push_back(node_point);
// Work around the 16384 point limit in RViz by splitting the // Work around the 16384 point limit in RViz by splitting the
// trajectory into multiple markers. // trajectory into multiple markers.
@ -252,10 +249,9 @@ visualization_msgs::MarkerArray MapBuilderBridge::GetConstraintList() {
residual_inter_marker.ns = "Inter residuals"; residual_inter_marker.ns = "Inter residuals";
residual_inter_marker.pose.position.z = 0.1; residual_inter_marker.pose.position.z = 0.1;
const auto all_trajectory_nodes = const auto trajectory_nodes =
map_builder_.sparse_pose_graph()->GetTrajectoryNodes(); map_builder_.sparse_pose_graph()->GetTrajectoryNodes();
const auto submap_data = const auto submap_data = map_builder_.sparse_pose_graph()->GetAllSubmapData();
map_builder_.sparse_pose_graph()->GetAllSubmapData();
const auto constraints = map_builder_.sparse_pose_graph()->constraints(); const auto constraints = map_builder_.sparse_pose_graph()->constraints();
for (const auto& constraint : constraints) { for (const auto& constraint : constraints) {
@ -294,10 +290,11 @@ visualization_msgs::MarkerArray MapBuilderBridge::GetConstraintList() {
continue; continue;
} }
const auto& submap_pose = submap_it->data.pose; const auto& submap_pose = submap_it->data.pose;
const auto& trajectory_node_pose = const auto node_it = trajectory_nodes.find(constraint.node_id);
all_trajectory_nodes[constraint.node_id.trajectory_id] if (node_it == trajectory_nodes.end()) {
[constraint.node_id.node_index] continue;
.global_pose; }
const auto& trajectory_node_pose = node_it->data.global_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;