parent
227e46c285
commit
4cd812d806
|
@ -112,10 +112,10 @@ cartographer_ros_msgs::SubmapList MapBuilderBridge::GetSubmapList() {
|
||||||
for (int trajectory_id = 0;
|
for (int trajectory_id = 0;
|
||||||
trajectory_id < map_builder_.num_trajectory_builders();
|
trajectory_id < map_builder_.num_trajectory_builders();
|
||||||
++trajectory_id) {
|
++trajectory_id) {
|
||||||
|
const std::vector<cartographer::transform::Rigid3d> submap_transforms =
|
||||||
|
map_builder_.sparse_pose_graph()->GetSubmapTransforms(trajectory_id);
|
||||||
const cartographer::mapping::Submaps* submaps =
|
const cartographer::mapping::Submaps* submaps =
|
||||||
map_builder_.GetTrajectoryBuilder(trajectory_id)->submaps();
|
map_builder_.GetTrajectoryBuilder(trajectory_id)->submaps();
|
||||||
const std::vector<cartographer::transform::Rigid3d> submap_transforms =
|
|
||||||
map_builder_.sparse_pose_graph()->GetSubmapTransforms(submaps);
|
|
||||||
CHECK_LE(submap_transforms.size(), submaps->size());
|
CHECK_LE(submap_transforms.size(), submaps->size());
|
||||||
|
|
||||||
cartographer_ros_msgs::TrajectorySubmapList trajectory_submap_list;
|
cartographer_ros_msgs::TrajectorySubmapList trajectory_submap_list;
|
||||||
|
@ -172,7 +172,7 @@ MapBuilderBridge::GetTrajectoryStates() {
|
||||||
trajectory_states[trajectory_id] = {
|
trajectory_states[trajectory_id] = {
|
||||||
pose_estimate,
|
pose_estimate,
|
||||||
map_builder_.sparse_pose_graph()->GetLocalToGlobalTransform(
|
map_builder_.sparse_pose_graph()->GetLocalToGlobalTransform(
|
||||||
trajectory_builder->submaps()),
|
trajectory_id),
|
||||||
sensor_bridge.tf_bridge().LookupToTracking(pose_estimate.time,
|
sensor_bridge.tf_bridge().LookupToTracking(pose_estimate.time,
|
||||||
options_.published_frame)};
|
options_.published_frame)};
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in New Issue