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

Follow googlecartographer/cartographer#677. (#595) 

[RFC=0001](https://github.com/googlecartographer/rfcs/blob/master/text/0001-renaming-sparse-pose-graph.md)
master
Wolfgang Hess 2017-11-15 12:57:50 +01:00 committed by Wally B. Feed
parent 7931d1e53c
commit 6b68522fcc
2 changed files with 8 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
cartographer_ros/cartographer_ros/map_builder_bridge.cc
View File

@ -106,7 +106,7 @@ void MapBuilderBridge::FinishTrajectory(const int trajectory_id) {
void MapBuilderBridge::RunFinalOptimization() { void MapBuilderBridge::RunFinalOptimization() {
LOG(INFO) << "Running final trajectory optimization..."; LOG(INFO) << "Running final trajectory optimization...";
map_builder_.sparse_pose_graph()->RunFinalOptimization(); map_builder_.pose_graph()->RunFinalOptimization();
} }
void MapBuilderBridge::SerializeState(const std::string& filename) { void MapBuilderBridge::SerializeState(const std::string& filename) {
@ -151,7 +151,7 @@ cartographer_ros_msgs::SubmapList MapBuilderBridge::GetSubmapList() {
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 : for (const auto& submap_id_data :
map_builder_.sparse_pose_graph()->GetAllSubmapData()) { map_builder_.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;
@ -184,8 +184,7 @@ MapBuilderBridge::GetTrajectoryStates() {
CHECK_EQ(trajectory_options_.count(trajectory_id), 1); CHECK_EQ(trajectory_options_.count(trajectory_id), 1);
trajectory_states[trajectory_id] = { trajectory_states[trajectory_id] = {
pose_estimate, pose_estimate,
map_builder_.sparse_pose_graph()->GetLocalToGlobalTransform( map_builder_.pose_graph()->GetLocalToGlobalTransform(trajectory_id),
trajectory_id),
sensor_bridge.tf_bridge().LookupToTracking( sensor_bridge.tf_bridge().LookupToTracking(
pose_estimate.time, pose_estimate.time,
trajectory_options_[trajectory_id].published_frame), trajectory_options_[trajectory_id].published_frame),
@ -196,7 +195,7 @@ 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 nodes = map_builder_.sparse_pose_graph()->GetTrajectoryNodes(); const auto nodes = map_builder_.pose_graph()->GetTrajectoryNodes();
for (const int trajectory_id : nodes.trajectory_ids()) { for (const int trajectory_id : nodes.trajectory_ids()) {
visualization_msgs::Marker marker = visualization_msgs::Marker marker =
CreateTrajectoryMarker(trajectory_id, node_options_.map_frame); CreateTrajectoryMarker(trajectory_id, node_options_.map_frame);
@ -252,16 +251,15 @@ 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 trajectory_nodes = const auto trajectory_nodes = map_builder_.pose_graph()->GetTrajectoryNodes();
map_builder_.sparse_pose_graph()->GetTrajectoryNodes(); const auto submap_data = map_builder_.pose_graph()->GetAllSubmapData();
const auto submap_data = map_builder_.sparse_pose_graph()->GetAllSubmapData(); const auto constraints = map_builder_.pose_graph()->constraints();
const auto constraints = map_builder_.sparse_pose_graph()->constraints();
for (const auto& constraint : constraints) { for (const auto& constraint : constraints) {
visualization_msgs::Marker *constraint_marker, *residual_marker; visualization_msgs::Marker *constraint_marker, *residual_marker;
std_msgs::ColorRGBA color_constraint, color_residual; std_msgs::ColorRGBA color_constraint, color_residual;
if (constraint.tag == if (constraint.tag ==
cartographer::mapping::SparsePoseGraph::Constraint::INTRA_SUBMAP) { cartographer::mapping::PoseGraph::Constraint::INTRA_SUBMAP) {
constraint_marker = &constraint_intra_marker; constraint_marker = &constraint_intra_marker;
residual_marker = &residual_intra_marker; residual_marker = &residual_intra_marker;
// Color mapping for submaps of various trajectories - add trajectory id // Color mapping for submaps of various trajectories - add trajectory id

1
cartographer_ros/cartographer_ros/node.cc
View File

@ -27,7 +27,6 @@
#include "cartographer/common/port.h" #include "cartographer/common/port.h"
#include "cartographer/common/time.h" #include "cartographer/common/time.h"
#include "cartographer/mapping/proto/submap_visualization.pb.h" #include "cartographer/mapping/proto/submap_visualization.pb.h"
#include "cartographer/mapping/sparse_pose_graph.h"
#include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/point_cloud.h"
#include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/rigid_transform.h"
#include "cartographer/transform/transform.h" #include "cartographer/transform/transform.h"