diff --git a/cartographer/mapping_2d/global_trajectory_builder.cc b/cartographer/mapping_2d/global_trajectory_builder.cc index 17bc9ed..3d92b8f 100644 --- a/cartographer/mapping_2d/global_trajectory_builder.cc +++ b/cartographer/mapping_2d/global_trajectory_builder.cc @@ -38,11 +38,8 @@ void GlobalTrajectoryBuilder::AddRangefinderData( return; } sparse_pose_graph_->AddScan( - insertion_result->time, insertion_result->tracking_to_tracking_2d, - insertion_result->range_data_in_tracking_2d, - insertion_result->filtered_point_cloud_in_tracking_2d, - insertion_result->pose_estimate_2d, trajectory_id_, - insertion_result->insertion_submaps); + insertion_result->constant_data, insertion_result->pose_estimate_2d, + trajectory_id_, insertion_result->insertion_submaps); } void GlobalTrajectoryBuilder::AddSensorData(const sensor::ImuData& imu_data) { diff --git a/cartographer/mapping_2d/local_trajectory_builder.cc b/cartographer/mapping_2d/local_trajectory_builder.cc index a0648b2..ba2680c 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.cc +++ b/cartographer/mapping_2d/local_trajectory_builder.cc @@ -17,6 +17,7 @@ #include "cartographer/mapping_2d/local_trajectory_builder.h" #include +#include #include "cartographer/common/make_unique.h" #include "cartographer/sensor/range_data.h" @@ -186,10 +187,16 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData( const sensor::PointCloud filtered_point_cloud_in_tracking_2d = adaptive_voxel_filter.Filter(range_data_in_tracking_2d.returns); - return common::make_unique( - InsertionResult{time, std::move(insertion_submaps), - tracking_to_tracking_2d, range_data_in_tracking_2d, - pose_estimate_2d, filtered_point_cloud_in_tracking_2d}); + return common::make_unique(InsertionResult{ + std::make_shared( + mapping::TrajectoryNode::Data{ + time, + {}, // 'range_data' is only used in 3D. + filtered_point_cloud_in_tracking_2d, + {}, // 'high_resolution_point_cloud' is only used in 3D. + {}, // 'low_resolution_point_cloud' is only used in 3D. + tracking_to_tracking_2d}), + pose_estimate_2d, std::move(insertion_submaps)}); } const mapping::PoseEstimate& LocalTrajectoryBuilder::pose_estimate() const { diff --git a/cartographer/mapping_2d/local_trajectory_builder.h b/cartographer/mapping_2d/local_trajectory_builder.h index 81cbc5a..b980c29 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.h +++ b/cartographer/mapping_2d/local_trajectory_builder.h @@ -41,12 +41,9 @@ namespace mapping_2d { class LocalTrajectoryBuilder { public: struct InsertionResult { - common::Time time; - std::vector> insertion_submaps; - transform::Rigid3d tracking_to_tracking_2d; - sensor::RangeData range_data_in_tracking_2d; + std::shared_ptr constant_data; transform::Rigid2d pose_estimate_2d; - sensor::PointCloud filtered_point_cloud_in_tracking_2d; + std::vector> insertion_submaps; }; explicit LocalTrajectoryBuilder( diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index eed759f..62907fc 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -94,9 +94,7 @@ std::vector SparsePoseGraph::GrowSubmapTransformsAsNeeded( } void SparsePoseGraph::AddScan( - common::Time time, const transform::Rigid3d& tracking_to_pose, - const sensor::RangeData& range_data_in_pose, - const sensor::PointCloud& filtered_point_cloud, + std::shared_ptr constant_data, const transform::Rigid2d& pose, const int trajectory_id, const std::vector>& insertion_submaps) { const transform::Rigid3d optimized_pose( @@ -104,16 +102,7 @@ void SparsePoseGraph::AddScan( common::MutexLocker locker(&mutex_); trajectory_nodes_.Append( - trajectory_id, - mapping::TrajectoryNode{ - std::make_shared( - mapping::TrajectoryNode::Data{time, - Compress(range_data_in_pose), - filtered_point_cloud, - {}, - {}, - tracking_to_pose}), - optimized_pose}); + trajectory_id, mapping::TrajectoryNode{constant_data, optimized_pose}); ++num_trajectory_nodes_; trajectory_connectivity_.Add(trajectory_id); diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h index b940666..eaa9968 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.h +++ b/cartographer/mapping_2d/sparse_pose_graph.h @@ -64,16 +64,13 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { SparsePoseGraph(const SparsePoseGraph&) = delete; SparsePoseGraph& operator=(const SparsePoseGraph&) = delete; - // Adds a new 'range_data_in_pose' observation at 'time', and a 'pose' that - // will later be optimized. The 'tracking_to_pose' is remembered so that the - // optimized pose can be embedded into 3D. The 'pose' was determined by scan - // matching against the 'insertion_submaps.front()' and the scan was inserted - // into the 'insertion_submaps'. If 'insertion_submaps.front().finished()' is + // Adds a new node with 'constant_data' and a 'pose' that will later be + // optimized. The 'pose' was determined by scan matching against + // 'insertion_submaps.front()' and the scan was inserted into the + // 'insertion_submaps'. If 'insertion_submaps.front().finished()' is // 'true', this submap was inserted into for the last time. void AddScan( - common::Time time, const transform::Rigid3d& tracking_to_pose, - const sensor::RangeData& range_data_in_pose, - const sensor::PointCloud& filtered_point_cloud, + std::shared_ptr constant_data, const transform::Rigid2d& pose, int trajectory_id, const std::vector>& insertion_submaps) EXCLUDES(mutex_); diff --git a/cartographer/mapping_2d/sparse_pose_graph_test.cc b/cartographer/mapping_2d/sparse_pose_graph_test.cc index f92cd47..f7216e4 100644 --- a/cartographer/mapping_2d/sparse_pose_graph_test.cc +++ b/cartographer/mapping_2d/sparse_pose_graph_test.cc @@ -158,8 +158,14 @@ class SparsePoseGraphTest : public ::testing::Test { range_data, transform::Embed3D(pose_estimate.cast()))); sparse_pose_graph_->AddScan( - common::FromUniversal(0), transform::Rigid3d::Identity(), range_data, - range_data.returns, pose_estimate, kTrajectoryId, insertion_submaps); + std::make_shared( + mapping::TrajectoryNode::Data{common::FromUniversal(0), + Compress(range_data), + range_data.returns, + {}, + {}, + transform::Rigid3d::Identity()}), + pose_estimate, kTrajectoryId, insertion_submaps); } void MoveRelative(const transform::Rigid2d& movement) { diff --git a/cartographer/mapping_3d/global_trajectory_builder.cc b/cartographer/mapping_3d/global_trajectory_builder.cc index ae709a4..24062a6 100644 --- a/cartographer/mapping_3d/global_trajectory_builder.cc +++ b/cartographer/mapping_3d/global_trajectory_builder.cc @@ -31,17 +31,14 @@ GlobalTrajectoryBuilder::~GlobalTrajectoryBuilder() {} void GlobalTrajectoryBuilder::AddRangefinderData( const common::Time time, const Eigen::Vector3f& origin, const sensor::PointCloud& ranges) { - auto insertion_result = + std::unique_ptr insertion_result = local_trajectory_builder_.AddRangefinderData(time, origin, ranges); if (insertion_result == nullptr) { return; } sparse_pose_graph_->AddScan( - insertion_result->time, insertion_result->range_data_in_tracking, - insertion_result->high_resolution_point_cloud, - insertion_result->low_resolution_point_cloud, - insertion_result->pose_observation, trajectory_id_, - insertion_result->insertion_submaps); + insertion_result->constant_data, insertion_result->pose_observation, + trajectory_id_, insertion_result->insertion_submaps); } void GlobalTrajectoryBuilder::AddSensorData(const sensor::ImuData& imu_data) { diff --git a/cartographer/mapping_3d/local_trajectory_builder.cc b/cartographer/mapping_3d/local_trajectory_builder.cc index d49a544..41e2349 100644 --- a/cartographer/mapping_3d/local_trajectory_builder.cc +++ b/cartographer/mapping_3d/local_trajectory_builder.cc @@ -16,6 +16,8 @@ #include "cartographer/mapping_3d/local_trajectory_builder.h" +#include + #include "cartographer/common/make_unique.h" #include "cartographer/common/time.h" #include "cartographer/mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.pb.h" @@ -201,9 +203,16 @@ LocalTrajectoryBuilder::InsertIntoSubmap( pose_observation.cast()), extrapolator_->gravity_orientation()); return std::unique_ptr(new InsertionResult{ - time, range_data_in_tracking, high_resolution_point_cloud, - low_resolution_point_cloud, pose_observation, - std::move(insertion_submaps)}); + + std::make_shared( + mapping::TrajectoryNode::Data{ + time, + sensor::Compress(range_data_in_tracking), + {}, // 'filtered_point_cloud' is only used in 2D. + high_resolution_point_cloud, + low_resolution_point_cloud, + transform::Rigid3d::Identity()}), + pose_observation, std::move(insertion_submaps)}); } } // namespace mapping_3d diff --git a/cartographer/mapping_3d/local_trajectory_builder.h b/cartographer/mapping_3d/local_trajectory_builder.h index 413f120..5fd029d 100644 --- a/cartographer/mapping_3d/local_trajectory_builder.h +++ b/cartographer/mapping_3d/local_trajectory_builder.h @@ -41,13 +41,7 @@ namespace mapping_3d { class LocalTrajectoryBuilder { public: struct InsertionResult { - common::Time time; - sensor::RangeData range_data_in_tracking; - - // Used for loop closure in 3D. - sensor::PointCloud high_resolution_point_cloud; - sensor::PointCloud low_resolution_point_cloud; - + std::shared_ptr constant_data; transform::Rigid3d pose_observation; std::vector> insertion_submaps; }; diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index a1ec50c..a5196c0 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -93,25 +93,14 @@ std::vector SparsePoseGraph::GrowSubmapTransformsAsNeeded( } void SparsePoseGraph::AddScan( - common::Time time, const sensor::RangeData& range_data_in_tracking, - const sensor::PointCloud& high_resolution_point_cloud, - const sensor::PointCloud& low_resolution_point_cloud, + std::shared_ptr constant_data, const transform::Rigid3d& pose, const int trajectory_id, const std::vector>& insertion_submaps) { const transform::Rigid3d optimized_pose( GetLocalToGlobalTransform(trajectory_id) * pose); common::MutexLocker locker(&mutex_); trajectory_nodes_.Append( - trajectory_id, mapping::TrajectoryNode{ - std::make_shared( - mapping::TrajectoryNode::Data{ - time, - sensor::Compress(range_data_in_tracking), - {}, - high_resolution_point_cloud, - low_resolution_point_cloud, - transform::Rigid3d::Identity()}), - optimized_pose}); + trajectory_id, mapping::TrajectoryNode{constant_data, optimized_pose}); ++num_trajectory_nodes_; trajectory_connectivity_.Add(trajectory_id); diff --git a/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/mapping_3d/sparse_pose_graph.h index 060c4e9..a821fe2 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.h +++ b/cartographer/mapping_3d/sparse_pose_graph.h @@ -64,15 +64,13 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { SparsePoseGraph(const SparsePoseGraph&) = delete; SparsePoseGraph& operator=(const SparsePoseGraph&) = delete; - // Adds a new 'range_data_in_tracking' observation at 'time', and a 'pose' - // that will later be optimized. The 'pose' was determined by scan matching - // against 'insertion_submaps.front()' and the scan was inserted into the - // 'insertion_submaps'. If 'insertion_submaps.front().finished()' is 'true', - // this submap was inserted into for the last time. + // Adds a new node with 'constant_data' and a 'pose' that will later be + // optimized. The 'pose' was determined by scan matching against + // 'insertion_submaps.front()' and the scan was inserted into the + // 'insertion_submaps'. If 'insertion_submaps.front().finished()' is + // 'true', this submap was inserted into for the last time. void AddScan( - common::Time time, const sensor::RangeData& range_data_in_tracking, - const sensor::PointCloud& high_resolution_point_cloud, - const sensor::PointCloud& low_resolution_point_cloud, + std::shared_ptr constant_data, const transform::Rigid3d& pose, int trajectory_id, const std::vector>& insertion_submaps) EXCLUDES(mutex_);