From 7d13383dec3828940c1bdcc7d8b3c901c853311a Mon Sep 17 00:00:00 2001 From: Alexander Belyaev <32522095+pifon2a@users.noreply.github.com> Date: Wed, 21 Feb 2018 14:24:12 +0100 Subject: [PATCH] Remove 'mapping_3d' namespace. (#922) (#925) Remove 'mapping_3d' namespace. (#922) It is removed from everywhere but 'scan_matching', 'pose_graph' subfolders of 'mapping_3d'. [Code structure RFC](https://github.com/pifon2a/rfcs/blob/e11bca586f8cc733063ea9c94bdade71086ed9ea/text/0000-code-structure.md) --- .../mapping_3d/local_trajectory_builder.cc | 13 ++-- .../mapping_3d/local_trajectory_builder.h | 12 ++-- .../local_trajectory_builder_test.cc | 10 +-- .../occupied_space_cost_function.h | 4 +- .../io/hybrid_grid_points_processor.cc | 9 ++- .../io/hybrid_grid_points_processor.h | 10 +-- .../io/outlier_removing_points_processor.h | 2 +- cartographer/io/submap_painter.cc | 4 +- cartographer/io/xray_points_processor.cc | 4 +- cartographer/io/xray_points_processor.h | 2 +- .../mapping/local_slam_result_data.cc | 2 +- cartographer/mapping/local_slam_result_data.h | 5 +- cartographer/mapping/proto/submap.proto | 6 +- .../proto/trajectory_builder_options.proto | 5 +- .../mapping/trajectory_builder_interface.cc | 4 +- cartographer/mapping_3d/hybrid_grid.h | 20 +++--- cartographer/mapping_3d/hybrid_grid_test.cc | 40 +++++------ ...=> local_trajectory_builder_options_3d.cc} | 20 +++--- ... => local_trajectory_builder_options_3d.h} | 14 ++-- .../pose_graph/constraint_builder.cc | 9 +-- .../pose_graph/constraint_builder.h | 14 ++-- .../pose_graph/optimization_problem.cc | 4 +- cartographer/mapping_3d/pose_graph_3d.cc | 16 ++--- cartographer/mapping_3d/pose_graph_3d.h | 25 +++---- .../mapping_3d/proto/hybrid_grid.proto | 2 +- ...local_trajectory_builder_options_3d.proto} | 10 +-- ...o => range_data_inserter_options_3d.proto} | 4 +- ...options.proto => submaps_options_3d.proto} | 8 +-- ..._inserter.cc => range_data_inserter_3d.cc} | 27 ++++---- ...ta_inserter.h => range_data_inserter_3d.h} | 25 +++---- ...test.cc => range_data_inserter_3d_test.cc} | 30 ++++---- .../mapping_3d/rotation_parameterization.h | 4 +- .../scan_matching/ceres_scan_matcher.cc | 5 +- .../scan_matching/ceres_scan_matcher.h | 2 +- .../scan_matching/ceres_scan_matcher_test.cc | 2 +- .../fast_correlative_scan_matcher.cc | 6 +- .../fast_correlative_scan_matcher.h | 6 +- .../fast_correlative_scan_matcher_test.cc | 13 ++-- .../scan_matching/interpolated_grid.h | 4 +- .../scan_matching/interpolated_grid_test.cc | 2 +- .../scan_matching/low_resolution_matcher.cc | 3 +- .../scan_matching/low_resolution_matcher.h | 3 +- .../scan_matching/precomputation_grid.cc | 6 +- .../scan_matching/precomputation_grid.h | 7 +- .../scan_matching/precomputation_grid_test.cc | 2 +- .../real_time_correlative_scan_matcher.cc | 5 +- .../real_time_correlative_scan_matcher.h | 4 +- ...real_time_correlative_scan_matcher_test.cc | 2 +- .../mapping_3d/{submaps.cc => submap_3d.cc} | 68 +++++++++---------- .../mapping_3d/{submaps.h => submap_3d.h} | 51 +++++++------- .../{submaps_test.cc => submap_3d_test.cc} | 17 ++--- cartographer/sensor/compressed_point_cloud.cc | 2 +- cartographer/sensor/voxel_filter.cc | 3 +- cartographer/sensor/voxel_filter.h | 2 +- cartographer_grpc/map_builder_context.cc | 11 ++- cartographer_grpc/map_builder_context.h | 4 +- 56 files changed, 296 insertions(+), 298 deletions(-) rename cartographer/mapping_3d/{local_trajectory_builder_options.cc => local_trajectory_builder_options_3d.cc} (84%) rename cartographer/mapping_3d/{local_trajectory_builder_options.h => local_trajectory_builder_options_3d.h} (86%) rename cartographer/mapping_3d/proto/{local_trajectory_builder_options.proto => local_trajectory_builder_options_3d.proto} (91%) rename cartographer/mapping_3d/proto/{range_data_inserter_options.proto => range_data_inserter_options_3d.proto} (93%) rename cartographer/mapping_3d/proto/{submaps_options.proto => submaps_options_3d.proto} (90%) rename cartographer/mapping_3d/{range_data_inserter.cc => range_data_inserter_3d.cc} (82%) rename cartographer/mapping_3d/{range_data_inserter.h => range_data_inserter_3d.h} (66%) rename cartographer/mapping_3d/{range_data_inserter_test.cc => range_data_inserter_3d_test.cc} (75%) rename cartographer/mapping_3d/{submaps.cc => submap_3d.cc} (85%) rename cartographer/mapping_3d/{submaps.h => submap_3d.h} (70%) rename cartographer/mapping_3d/{submaps_test.cc => submap_3d_test.cc} (79%) diff --git a/cartographer/internal/mapping_3d/local_trajectory_builder.cc b/cartographer/internal/mapping_3d/local_trajectory_builder.cc index 0845144..de418ef 100644 --- a/cartographer/internal/mapping_3d/local_trajectory_builder.cc +++ b/cartographer/internal/mapping_3d/local_trajectory_builder.cc @@ -21,8 +21,8 @@ #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" -#include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h" -#include "cartographer/mapping_3d/proto/submaps_options.pb.h" +#include "cartographer/mapping_3d/proto/local_trajectory_builder_options_3d.pb.h" +#include "cartographer/mapping_3d/proto/submaps_options_3d.pb.h" #include "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.pb.h" #include "cartographer/mapping_3d/scan_matching/rotational_scan_matcher.h" #include "glog/logging.h" @@ -31,7 +31,7 @@ namespace cartographer { namespace mapping_3d { LocalTrajectoryBuilder::LocalTrajectoryBuilder( - const proto::LocalTrajectoryBuilderOptions& options) + const mapping::proto::LocalTrajectoryBuilderOptions3D& options) : options_(options), active_submaps_(options.submaps_options()), motion_filter_(options.motion_filter_options()), @@ -142,7 +142,7 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData( const transform::Rigid3d pose_prediction = extrapolator_->ExtrapolatePose(time); - std::shared_ptr matching_submap = + std::shared_ptr matching_submap = active_submaps_.submaps().front(); transform::Rigid3d initial_ceres_pose = matching_submap->local_pose().inverse() * pose_prediction; @@ -223,8 +223,9 @@ LocalTrajectoryBuilder::InsertIntoSubmap( } // Querying the active submaps must be done here before calling // InsertRangeData() since the queried values are valid for next insertion. - std::vector> insertion_submaps; - for (const std::shared_ptr& submap : active_submaps_.submaps()) { + std::vector> insertion_submaps; + for (const std::shared_ptr& submap : + active_submaps_.submaps()) { insertion_submaps.push_back(submap); } active_submaps_.InsertRangeData(filtered_range_data_in_local, diff --git a/cartographer/internal/mapping_3d/local_trajectory_builder.h b/cartographer/internal/mapping_3d/local_trajectory_builder.h index cf78a7c..76dff5a 100644 --- a/cartographer/internal/mapping_3d/local_trajectory_builder.h +++ b/cartographer/internal/mapping_3d/local_trajectory_builder.h @@ -22,10 +22,10 @@ #include "cartographer/common/time.h" #include "cartographer/internal/mapping/motion_filter.h" #include "cartographer/mapping/pose_extrapolator.h" -#include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h" +#include "cartographer/mapping_3d/proto/local_trajectory_builder_options_3d.pb.h" #include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h" #include "cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher.h" -#include "cartographer/mapping_3d/submaps.h" +#include "cartographer/mapping_3d/submap_3d.h" #include "cartographer/sensor/imu_data.h" #include "cartographer/sensor/odometry_data.h" #include "cartographer/sensor/range_data.h" @@ -41,7 +41,7 @@ class LocalTrajectoryBuilder { public: struct InsertionResult { std::shared_ptr constant_data; - std::vector> insertion_submaps; + std::vector> insertion_submaps; }; struct MatchingResult { common::Time time; @@ -52,7 +52,7 @@ class LocalTrajectoryBuilder { }; explicit LocalTrajectoryBuilder( - const proto::LocalTrajectoryBuilderOptions& options); + const mapping::proto::LocalTrajectoryBuilderOptions3D& options); ~LocalTrajectoryBuilder(); LocalTrajectoryBuilder(const LocalTrajectoryBuilder&) = delete; @@ -80,8 +80,8 @@ class LocalTrajectoryBuilder { const transform::Rigid3d& pose_estimate, const Eigen::Quaterniond& gravity_alignment); - const proto::LocalTrajectoryBuilderOptions options_; - ActiveSubmaps active_submaps_; + const mapping::proto::LocalTrajectoryBuilderOptions3D options_; + mapping::ActiveSubmaps3D active_submaps_; mapping::MotionFilter motion_filter_; std::unique_ptr diff --git a/cartographer/internal/mapping_3d/local_trajectory_builder_test.cc b/cartographer/internal/mapping_3d/local_trajectory_builder_test.cc index b45a7eb..960f91a 100644 --- a/cartographer/internal/mapping_3d/local_trajectory_builder_test.cc +++ b/cartographer/internal/mapping_3d/local_trajectory_builder_test.cc @@ -23,7 +23,7 @@ #include "cartographer/common/lua_parameter_dictionary_test_helpers.h" #include "cartographer/common/time.h" #include "cartographer/mapping_3d/hybrid_grid.h" -#include "cartographer/mapping_3d/local_trajectory_builder_options.h" +#include "cartographer/mapping_3d/local_trajectory_builder_options_3d.h" #include "cartographer/sensor/range_data.h" #include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/rigid_transform_test_helpers.h" @@ -44,7 +44,8 @@ class LocalTrajectoryBuilderTest : public ::testing::Test { void SetUp() override { GenerateBubbles(); } - proto::LocalTrajectoryBuilderOptions CreateTrajectoryBuilderOptions() { + mapping::proto::LocalTrajectoryBuilderOptions3D + CreateTrajectoryBuilderOptions3D() { auto parameter_dictionary = common::MakeDictionary(R"text( return { min_range = 0.5, @@ -107,7 +108,8 @@ class LocalTrajectoryBuilderTest : public ::testing::Test { }, } )text"); - return CreateLocalTrajectoryBuilderOptions(parameter_dictionary.get()); + return mapping::CreateLocalTrajectoryBuilderOptions3D( + parameter_dictionary.get()); } void GenerateBubbles() { @@ -270,7 +272,7 @@ class LocalTrajectoryBuilderTest : public ::testing::Test { TEST_F(LocalTrajectoryBuilderTest, MoveInsideCubeUsingOnlyCeresScanMatcher) { local_trajectory_builder_.reset( - new LocalTrajectoryBuilder(CreateTrajectoryBuilderOptions())); + new LocalTrajectoryBuilder(CreateTrajectoryBuilderOptions3D())); VerifyAccuracy(GenerateCorkscrewTrajectory(), 1e-1); } diff --git a/cartographer/internal/mapping_3d/scan_matching/occupied_space_cost_function.h b/cartographer/internal/mapping_3d/scan_matching/occupied_space_cost_function.h index c735832..5aff992 100644 --- a/cartographer/internal/mapping_3d/scan_matching/occupied_space_cost_function.h +++ b/cartographer/internal/mapping_3d/scan_matching/occupied_space_cost_function.h @@ -35,7 +35,7 @@ class OccupiedSpaceCostFunction { public: static ceres::CostFunction* CreateAutoDiffCostFunction( const double scaling_factor, const sensor::PointCloud& point_cloud, - const HybridGrid& hybrid_grid) { + const mapping::HybridGrid& hybrid_grid) { return new ceres::AutoDiffCostFunction< OccupiedSpaceCostFunction, ceres::DYNAMIC /* residuals */, 3 /* translation variables */, 4 /* rotation variables */>( @@ -56,7 +56,7 @@ class OccupiedSpaceCostFunction { private: OccupiedSpaceCostFunction(const double scaling_factor, const sensor::PointCloud& point_cloud, - const HybridGrid& hybrid_grid) + const mapping::HybridGrid& hybrid_grid) : scaling_factor_(scaling_factor), point_cloud_(point_cloud), interpolated_grid_(hybrid_grid) {} diff --git a/cartographer/io/hybrid_grid_points_processor.cc b/cartographer/io/hybrid_grid_points_processor.cc index 77d972a..308fc5f 100644 --- a/cartographer/io/hybrid_grid_points_processor.cc +++ b/cartographer/io/hybrid_grid_points_processor.cc @@ -9,7 +9,7 @@ #include "cartographer/io/points_batch.h" #include "cartographer/io/points_processor.h" #include "cartographer/mapping_3d/hybrid_grid.h" -#include "cartographer/mapping_3d/range_data_inserter.h" +#include "cartographer/mapping_3d/range_data_inserter_3d.h" #include "cartographer/sensor/range_data.h" #include "glog/logging.h" @@ -18,7 +18,7 @@ namespace io { HybridGridPointsProcessor::HybridGridPointsProcessor( const double voxel_size, - const mapping_3d::proto::RangeDataInserterOptions& + const mapping::proto::RangeDataInserterOptions3D& range_data_inserter_options, std::unique_ptr file_writer, PointsProcessor* const next) : next_(next), @@ -33,7 +33,7 @@ HybridGridPointsProcessor::FromDictionary( PointsProcessor* const next) { return common::make_unique( dictionary->GetDouble("voxel_size"), - mapping_3d::CreateRangeDataInserterOptions( + mapping::CreateRangeDataInserterOptions3D( dictionary->GetDictionary("range_data_inserter").get()), file_writer_factory(dictionary->GetString("filename")), next); } @@ -45,8 +45,7 @@ void HybridGridPointsProcessor::Process(std::unique_ptr batch) { } PointsProcessor::FlushResult HybridGridPointsProcessor::Flush() { - const mapping_3d::proto::HybridGrid hybrid_grid_proto = - hybrid_grid_.ToProto(); + const mapping::proto::HybridGrid hybrid_grid_proto = hybrid_grid_.ToProto(); std::string serialized; hybrid_grid_proto.SerializeToString(&serialized); file_writer_->Write(serialized.data(), serialized.size()); diff --git a/cartographer/io/hybrid_grid_points_processor.h b/cartographer/io/hybrid_grid_points_processor.h index 800bbb7..451c948 100644 --- a/cartographer/io/hybrid_grid_points_processor.h +++ b/cartographer/io/hybrid_grid_points_processor.h @@ -10,8 +10,8 @@ #include "cartographer/io/points_batch.h" #include "cartographer/io/points_processor.h" #include "cartographer/mapping_3d/hybrid_grid.h" -#include "cartographer/mapping_3d/proto/range_data_inserter_options.pb.h" -#include "cartographer/mapping_3d/range_data_inserter.h" +#include "cartographer/mapping_3d/proto/range_data_inserter_options_3d.pb.h" +#include "cartographer/mapping_3d/range_data_inserter_3d.h" namespace cartographer { namespace io { @@ -24,7 +24,7 @@ class HybridGridPointsProcessor : public PointsProcessor { constexpr static const char* kConfigurationFileActionName = "write_hybrid_grid"; HybridGridPointsProcessor(double voxel_size, - const mapping_3d::proto::RangeDataInserterOptions& + const mapping::proto::RangeDataInserterOptions3D& range_data_inserter_options, std::unique_ptr file_writer, PointsProcessor* next); @@ -44,8 +44,8 @@ class HybridGridPointsProcessor : public PointsProcessor { private: PointsProcessor* const next_; - mapping_3d::RangeDataInserter range_data_inserter_; - mapping_3d::HybridGrid hybrid_grid_; + mapping::RangeDataInserter3D range_data_inserter_; + mapping::HybridGrid hybrid_grid_; std::unique_ptr file_writer_; }; diff --git a/cartographer/io/outlier_removing_points_processor.h b/cartographer/io/outlier_removing_points_processor.h index 5f233e6..60ed6d7 100644 --- a/cartographer/io/outlier_removing_points_processor.h +++ b/cartographer/io/outlier_removing_points_processor.h @@ -78,7 +78,7 @@ class OutlierRemovingPointsProcessor : public PointsProcessor { const double voxel_size_; PointsProcessor* const next_; State state_; - mapping_3d::HybridGridBase voxels_; + mapping::HybridGridBase voxels_; }; } // namespace io diff --git a/cartographer/io/submap_painter.cc b/cartographer/io/submap_painter.cc index 5cc46ce..8feee2f 100644 --- a/cartographer/io/submap_painter.cc +++ b/cartographer/io/submap_painter.cc @@ -17,7 +17,7 @@ #include "cartographer/io/submap_painter.h" #include "cartographer/mapping_2d/submap_2d.h" -#include "cartographer/mapping_3d/submaps.h" +#include "cartographer/mapping_3d/submap_3d.h" namespace cartographer { namespace io { @@ -114,7 +114,7 @@ void FillSubmapSlice( ::cartographer::mapping::proto::SubmapQuery::Response response; ::cartographer::transform::Rigid3d local_pose; if (proto.has_submap_3d()) { - ::cartographer::mapping_3d::Submap submap(proto.submap_3d()); + mapping::Submap3D submap(proto.submap_3d()); local_pose = submap.local_pose(); submap.ToResponseProto(global_submap_pose, &response); } else { diff --git a/cartographer/io/xray_points_processor.cc b/cartographer/io/xray_points_processor.cc index 3bf277c..08827d5 100644 --- a/cartographer/io/xray_points_processor.cc +++ b/cartographer/io/xray_points_processor.cc @@ -111,7 +111,7 @@ XRayPointsProcessor::XRayPointsProcessor( transform_(transform) { for (size_t i = 0; i < (floors_.empty() ? 1 : floors.size()); ++i) { aggregations_.emplace_back( - Aggregation{mapping_3d::HybridGridBase(voxel_size), {}}); + Aggregation{mapping::HybridGridBase(voxel_size), {}}); } } @@ -161,7 +161,7 @@ void XRayPointsProcessor::WriteVoxels(const Aggregation& aggregation, const int xsize = bounding_box_.sizes()[1] + 1; const int ysize = bounding_box_.sizes()[2] + 1; PixelDataMatrix pixel_data_matrix = PixelDataMatrix(ysize, xsize); - for (mapping_3d::HybridGridBase::Iterator it(aggregation.voxels); + for (mapping::HybridGridBase::Iterator it(aggregation.voxels); !it.Done(); it.Next()) { const Eigen::Array3i cell_index = it.GetCellIndex(); const Eigen::Array2i pixel = voxel_index_to_pixel(cell_index); diff --git a/cartographer/io/xray_points_processor.h b/cartographer/io/xray_points_processor.h index 99bc49b..5029718 100644 --- a/cartographer/io/xray_points_processor.h +++ b/cartographer/io/xray_points_processor.h @@ -66,7 +66,7 @@ class XRayPointsProcessor : public PointsProcessor { }; struct Aggregation { - mapping_3d::HybridGridBase voxels; + mapping::HybridGridBase voxels; std::map, ColumnData> column_data; }; diff --git a/cartographer/mapping/local_slam_result_data.cc b/cartographer/mapping/local_slam_result_data.cc index c8b29c2..6fcea8e 100644 --- a/cartographer/mapping/local_slam_result_data.cc +++ b/cartographer/mapping/local_slam_result_data.cc @@ -49,7 +49,7 @@ void LocalSlamResult2D::AddToPoseGraph(int trajectory_id, LocalSlamResult3D::LocalSlamResult3D( const std::string& sensor_id, common::Time time, std::shared_ptr node_data, - const std::vector>& + const std::vector>& insertion_submaps) : LocalSlamResultData(sensor_id, time), node_data_(node_data), diff --git a/cartographer/mapping/local_slam_result_data.h b/cartographer/mapping/local_slam_result_data.h index f350f67..4bb9880 100644 --- a/cartographer/mapping/local_slam_result_data.h +++ b/cartographer/mapping/local_slam_result_data.h @@ -58,8 +58,7 @@ class LocalSlamResult3D : public LocalSlamResultData { LocalSlamResult3D( const std::string& sensor_id, common::Time time, std::shared_ptr node_data, - const std::vector>& - insertion_submaps); + const std::vector>& insertion_submaps); void AddToTrajectoryBuilder( TrajectoryBuilderInterface* const trajectory_builder) override; @@ -67,7 +66,7 @@ class LocalSlamResult3D : public LocalSlamResultData { private: std::shared_ptr node_data_; - std::vector> insertion_submaps_; + std::vector> insertion_submaps_; }; } // namespace mapping diff --git a/cartographer/mapping/proto/submap.proto b/cartographer/mapping/proto/submap.proto index 40ead20..ac5e052 100644 --- a/cartographer/mapping/proto/submap.proto +++ b/cartographer/mapping/proto/submap.proto @@ -25,7 +25,7 @@ message Submap2D { transform.proto.Rigid3d local_pose = 1; int32 num_range_data = 2; bool finished = 3; - mapping.proto.ProbabilityGrid probability_grid = 4; + ProbabilityGrid probability_grid = 4; } // Serialized state of a mapping_3d::Submap. @@ -33,6 +33,6 @@ message Submap3D { transform.proto.Rigid3d local_pose = 1; int32 num_range_data = 2; bool finished = 3; - mapping_3d.proto.HybridGrid high_resolution_hybrid_grid = 4; - mapping_3d.proto.HybridGrid low_resolution_hybrid_grid = 5; + HybridGrid high_resolution_hybrid_grid = 4; + HybridGrid low_resolution_hybrid_grid = 5; } diff --git a/cartographer/mapping/proto/trajectory_builder_options.proto b/cartographer/mapping/proto/trajectory_builder_options.proto index 0b05091..08c4ebf 100644 --- a/cartographer/mapping/proto/trajectory_builder_options.proto +++ b/cartographer/mapping/proto/trajectory_builder_options.proto @@ -16,7 +16,7 @@ syntax = "proto3"; import "cartographer/transform/proto/transform.proto"; import "cartographer/mapping_2d/proto/local_trajectory_builder_options_2d.proto"; -import "cartographer/mapping_3d/proto/local_trajectory_builder_options.proto"; +import "cartographer/mapping_3d/proto/local_trajectory_builder_options_3d.proto"; package cartographer.mapping.proto; @@ -28,8 +28,7 @@ message InitialTrajectoryPose { message TrajectoryBuilderOptions { LocalTrajectoryBuilderOptions2D trajectory_builder_2d_options = 1; - mapping_3d.proto.LocalTrajectoryBuilderOptions - trajectory_builder_3d_options = 2; + LocalTrajectoryBuilderOptions3D trajectory_builder_3d_options = 2; bool pure_localization = 3; InitialTrajectoryPose initial_trajectory_pose = 4; } diff --git a/cartographer/mapping/trajectory_builder_interface.cc b/cartographer/mapping/trajectory_builder_interface.cc index 5f016bb..1f0c13c 100644 --- a/cartographer/mapping/trajectory_builder_interface.cc +++ b/cartographer/mapping/trajectory_builder_interface.cc @@ -18,7 +18,7 @@ #include "cartographer/mapping/local_slam_result_data.h" #include "cartographer/mapping_2d/local_trajectory_builder_options_2d.h" -#include "cartographer/mapping_3d/local_trajectory_builder_options.h" +#include "cartographer/mapping_3d/local_trajectory_builder_options_3d.h" namespace cartographer { namespace mapping { @@ -30,7 +30,7 @@ proto::TrajectoryBuilderOptions CreateTrajectoryBuilderOptions( CreateLocalTrajectoryBuilderOptions2D( parameter_dictionary->GetDictionary("trajectory_builder_2d").get()); *options.mutable_trajectory_builder_3d_options() = - mapping_3d::CreateLocalTrajectoryBuilderOptions( + CreateLocalTrajectoryBuilderOptions3D( parameter_dictionary->GetDictionary("trajectory_builder_3d").get()); options.set_pure_localization( parameter_dictionary->GetBool("pure_localization")); diff --git a/cartographer/mapping_3d/hybrid_grid.h b/cartographer/mapping_3d/hybrid_grid.h index 8c6c651..b5fa75e 100644 --- a/cartographer/mapping_3d/hybrid_grid.h +++ b/cartographer/mapping_3d/hybrid_grid.h @@ -33,7 +33,7 @@ #include "glog/logging.h" namespace cartographer { -namespace mapping_3d { +namespace mapping { // Converts an 'index' with each dimension from 0 to 2^'bits' - 1 to a flat // z-major index. @@ -475,20 +475,20 @@ class HybridGrid : public HybridGridBase { // SetProbability does some error checking for us. SetProbability(Eigen::Vector3i(proto.x_indices(i), proto.y_indices(i), proto.z_indices(i)), - mapping::ValueToProbability(proto.values(i))); + ValueToProbability(proto.values(i))); } } // Sets the probability of the cell at 'index' to the given 'probability'. void SetProbability(const Eigen::Array3i& index, const float probability) { - *mutable_value(index) = mapping::ProbabilityToValue(probability); + *mutable_value(index) = ProbabilityToValue(probability); } // Finishes the update sequence. void FinishUpdate() { while (!update_indices_.empty()) { - DCHECK_GE(*update_indices_.back(), mapping::kUpdateMarker); - *update_indices_.back() -= mapping::kUpdateMarker; + DCHECK_GE(*update_indices_.back(), kUpdateMarker); + *update_indices_.back() -= kUpdateMarker; update_indices_.pop_back(); } } @@ -502,20 +502,20 @@ class HybridGrid : public HybridGridBase { // will be set to probability corresponding to 'odds'. bool ApplyLookupTable(const Eigen::Array3i& index, const std::vector& table) { - DCHECK_EQ(table.size(), mapping::kUpdateMarker); + DCHECK_EQ(table.size(), kUpdateMarker); uint16* const cell = mutable_value(index); - if (*cell >= mapping::kUpdateMarker) { + if (*cell >= kUpdateMarker) { return false; } update_indices_.push_back(cell); *cell = table[*cell]; - DCHECK_GE(*cell, mapping::kUpdateMarker); + DCHECK_GE(*cell, kUpdateMarker); return true; } // Returns the probability of the cell with 'index'. float GetProbability(const Eigen::Array3i& index) const { - return mapping::ValueToProbability(value(index)); + return ValueToProbability(value(index)); } // Returns true if the probability at the specified 'index' is known. @@ -540,7 +540,7 @@ class HybridGrid : public HybridGridBase { std::vector update_indices_; }; -} // namespace mapping_3d +} // namespace mapping } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_3D_HYBRID_GRID_H_ diff --git a/cartographer/mapping_3d/hybrid_grid_test.cc b/cartographer/mapping_3d/hybrid_grid_test.cc index 3cec073..aba7c75 100644 --- a/cartographer/mapping_3d/hybrid_grid_test.cc +++ b/cartographer/mapping_3d/hybrid_grid_test.cc @@ -23,7 +23,7 @@ #include "gmock/gmock.h" namespace cartographer { -namespace mapping_3d { +namespace mapping { namespace { TEST(HybridGridTest, ApplyOdds) { @@ -40,35 +40,30 @@ TEST(HybridGridTest, ApplyOdds) { hybrid_grid.SetProbability(Eigen::Array3i(1, 0, 1), 0.5f); - hybrid_grid.ApplyLookupTable( - Eigen::Array3i(1, 0, 1), - mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.9f))); + hybrid_grid.ApplyLookupTable(Eigen::Array3i(1, 0, 1), + ComputeLookupTableToApplyOdds(Odds(0.9f))); hybrid_grid.FinishUpdate(); EXPECT_GT(hybrid_grid.GetProbability(Eigen::Array3i(1, 0, 1)), 0.5f); hybrid_grid.SetProbability(Eigen::Array3i(0, 1, 0), 0.5f); - hybrid_grid.ApplyLookupTable( - Eigen::Array3i(0, 1, 0), - mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.1f))); + hybrid_grid.ApplyLookupTable(Eigen::Array3i(0, 1, 0), + ComputeLookupTableToApplyOdds(Odds(0.1f))); hybrid_grid.FinishUpdate(); EXPECT_LT(hybrid_grid.GetProbability(Eigen::Array3i(0, 1, 0)), 0.5f); // Tests adding odds to an unknown cell. - hybrid_grid.ApplyLookupTable( - Eigen::Array3i(1, 1, 1), - mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.42f))); + hybrid_grid.ApplyLookupTable(Eigen::Array3i(1, 1, 1), + ComputeLookupTableToApplyOdds(Odds(0.42f))); EXPECT_NEAR(hybrid_grid.GetProbability(Eigen::Array3i(1, 1, 1)), 0.42f, 1e-4); // Tests that further updates are ignored if FinishUpdate() isn't called. - hybrid_grid.ApplyLookupTable( - Eigen::Array3i(1, 1, 1), - mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.9f))); + hybrid_grid.ApplyLookupTable(Eigen::Array3i(1, 1, 1), + ComputeLookupTableToApplyOdds(Odds(0.9f))); EXPECT_NEAR(hybrid_grid.GetProbability(Eigen::Array3i(1, 1, 1)), 0.42f, 1e-4); hybrid_grid.FinishUpdate(); - hybrid_grid.ApplyLookupTable( - Eigen::Array3i(1, 1, 1), - mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.9f))); + hybrid_grid.ApplyLookupTable(Eigen::Array3i(1, 1, 1), + ComputeLookupTableToApplyOdds(Odds(0.9f))); EXPECT_GT(hybrid_grid.GetProbability(Eigen::Array3i(1, 1, 1)), 0.42f); } @@ -77,10 +72,10 @@ TEST(HybridGridTest, GetProbability) { hybrid_grid.SetProbability( hybrid_grid.GetCellIndex(Eigen::Vector3f(0.f, 1.f, 1.f)), - mapping::kMaxProbability); + kMaxProbability); EXPECT_NEAR(hybrid_grid.GetProbability( hybrid_grid.GetCellIndex(Eigen::Vector3f(0.f, 1.f, 1.f))), - mapping::kMaxProbability, 1e-6); + kMaxProbability, 1e-6); for (const Eigen::Array3i& index : {hybrid_grid.GetCellIndex(Eigen::Vector3f(0.f, 2.f, 1.f)), hybrid_grid.GetCellIndex(Eigen::Vector3f(1.f, 1.f, 1.f)), @@ -129,8 +124,8 @@ class RandomHybridGridTest : public ::testing::Test { public: RandomHybridGridTest() : hybrid_grid_(2.f), values_() { std::mt19937 rng(1285120005); - std::uniform_real_distribution value_distribution( - mapping::kMinProbability, mapping::kMaxProbability); + std::uniform_real_distribution value_distribution(kMinProbability, + kMaxProbability); std::uniform_int_distribution xyz_distribution(-3000, 2999); for (int i = 0; i < 10000; ++i) { const auto x = xyz_distribution(rng); @@ -156,8 +151,7 @@ class RandomHybridGridTest : public ::testing::Test { TEST_F(RandomHybridGridTest, TestIteration) { for (auto it = HybridGrid::Iterator(hybrid_grid_); !it.Done(); it.Next()) { const Eigen::Array3i cell_index = it.GetCellIndex(); - const float iterator_probability = - mapping::ValueToProbability(it.GetValue()); + const float iterator_probability = ValueToProbability(it.GetValue()); EXPECT_EQ(iterator_probability, hybrid_grid_.GetProbability(cell_index)); const std::tuple key = std::make_tuple(cell_index[0], cell_index[1], cell_index[2]); @@ -228,5 +222,5 @@ TEST_F(RandomHybridGridTest, FromProto) { } } // namespace -} // namespace mapping_3d +} // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping_3d/local_trajectory_builder_options.cc b/cartographer/mapping_3d/local_trajectory_builder_options_3d.cc similarity index 84% rename from cartographer/mapping_3d/local_trajectory_builder_options.cc rename to cartographer/mapping_3d/local_trajectory_builder_options_3d.cc index b0cadb9..ac68d3d 100644 --- a/cartographer/mapping_3d/local_trajectory_builder_options.cc +++ b/cartographer/mapping_3d/local_trajectory_builder_options_3d.cc @@ -14,21 +14,21 @@ * limitations under the License. */ -#include "cartographer/mapping_3d/local_trajectory_builder_options.h" +#include "cartographer/mapping_3d/local_trajectory_builder_options_3d.h" #include "cartographer/internal/mapping/motion_filter.h" #include "cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.h" #include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h" -#include "cartographer/mapping_3d/submaps.h" +#include "cartographer/mapping_3d/submap_3d.h" #include "cartographer/sensor/voxel_filter.h" #include "glog/logging.h" namespace cartographer { -namespace mapping_3d { +namespace mapping { -proto::LocalTrajectoryBuilderOptions CreateLocalTrajectoryBuilderOptions( - common::LuaParameterDictionary* const parameter_dictionary) { - proto::LocalTrajectoryBuilderOptions options; +proto::LocalTrajectoryBuilderOptions3D CreateLocalTrajectoryBuilderOptions3D( + common::LuaParameterDictionary* parameter_dictionary) { + proto::LocalTrajectoryBuilderOptions3D options; options.set_min_range(parameter_dictionary->GetDouble("min_range")); options.set_max_range(parameter_dictionary->GetDouble("max_range")); options.set_num_accumulated_range_data( @@ -53,18 +53,18 @@ proto::LocalTrajectoryBuilderOptions CreateLocalTrajectoryBuilderOptions( ->GetDictionary("real_time_correlative_scan_matcher") .get()); *options.mutable_ceres_scan_matcher_options() = - scan_matching::CreateCeresScanMatcherOptions( + mapping_3d::scan_matching::CreateCeresScanMatcherOptions( parameter_dictionary->GetDictionary("ceres_scan_matcher").get()); - *options.mutable_motion_filter_options() = mapping::CreateMotionFilterOptions( + *options.mutable_motion_filter_options() = CreateMotionFilterOptions( parameter_dictionary->GetDictionary("motion_filter").get()); options.set_imu_gravity_time_constant( parameter_dictionary->GetDouble("imu_gravity_time_constant")); options.set_rotational_histogram_size( parameter_dictionary->GetInt("rotational_histogram_size")); - *options.mutable_submaps_options() = mapping_3d::CreateSubmapsOptions( + *options.mutable_submaps_options() = CreateSubmapsOptions3D( parameter_dictionary->GetDictionary("submaps").get()); return options; } -} // namespace mapping_3d +} // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping_3d/local_trajectory_builder_options.h b/cartographer/mapping_3d/local_trajectory_builder_options_3d.h similarity index 86% rename from cartographer/mapping_3d/local_trajectory_builder_options.h rename to cartographer/mapping_3d/local_trajectory_builder_options_3d.h index 96d6508..7c0dc5c 100644 --- a/cartographer/mapping_3d/local_trajectory_builder_options.h +++ b/cartographer/mapping_3d/local_trajectory_builder_options_3d.h @@ -14,19 +14,19 @@ * limitations under the License. */ -#ifndef CARTOGRAPHER_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_ -#define CARTOGRAPHER_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_ +#ifndef CARTOGRAPHER_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_OPTIONS_3D_H_ +#define CARTOGRAPHER_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_OPTIONS_3D_H_ #include "cartographer/common/lua_parameter_dictionary.h" -#include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h" +#include "cartographer/mapping_3d/proto/local_trajectory_builder_options_3d.pb.h" namespace cartographer { -namespace mapping_3d { +namespace mapping { -proto::LocalTrajectoryBuilderOptions CreateLocalTrajectoryBuilderOptions( +proto::LocalTrajectoryBuilderOptions3D CreateLocalTrajectoryBuilderOptions3D( common::LuaParameterDictionary* parameter_dictionary); -} // namespace mapping_3d +} // namespace mapping } // namespace cartographer -#endif // CARTOGRAPHER_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_ +#endif // CARTOGRAPHER_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_OPTIONS_3D_H_ diff --git a/cartographer/mapping_3d/pose_graph/constraint_builder.cc b/cartographer/mapping_3d/pose_graph/constraint_builder.cc index 49f5099..73f1bcd 100644 --- a/cartographer/mapping_3d/pose_graph/constraint_builder.cc +++ b/cartographer/mapping_3d/pose_graph/constraint_builder.cc @@ -55,7 +55,7 @@ ConstraintBuilder::~ConstraintBuilder() { } void ConstraintBuilder::MaybeAddConstraint( - const mapping::SubmapId& submap_id, const Submap* const submap, + const mapping::SubmapId& submap_id, const mapping::Submap3D* const submap, const mapping::NodeId& node_id, const mapping::TrajectoryNode::Data* const constant_data, const std::vector& submap_nodes, @@ -82,7 +82,7 @@ void ConstraintBuilder::MaybeAddConstraint( } void ConstraintBuilder::MaybeAddGlobalConstraint( - const mapping::SubmapId& submap_id, const Submap* const submap, + const mapping::SubmapId& submap_id, const mapping::Submap3D* const submap, const mapping::NodeId& node_id, const mapping::TrajectoryNode::Data* const constant_data, const std::vector& submap_nodes, @@ -123,7 +123,8 @@ void ConstraintBuilder::WhenDone( void ConstraintBuilder::ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( const mapping::SubmapId& submap_id, const std::vector& submap_nodes, - const Submap* const submap, const std::function& work_item) { + const mapping::Submap3D* const submap, + const std::function& work_item) { if (submap_scan_matchers_[submap_id].fast_correlative_scan_matcher != nullptr) { thread_pool_->Schedule(work_item); @@ -140,7 +141,7 @@ void ConstraintBuilder::ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( void ConstraintBuilder::ConstructSubmapScanMatcher( const mapping::SubmapId& submap_id, const std::vector& submap_nodes, - const Submap* const submap) { + const mapping::Submap3D* const submap) { auto submap_scan_matcher = common::make_unique( submap->high_resolution_hybrid_grid(), diff --git a/cartographer/mapping_3d/pose_graph/constraint_builder.h b/cartographer/mapping_3d/pose_graph/constraint_builder.h index 43bf519..5157163 100644 --- a/cartographer/mapping_3d/pose_graph/constraint_builder.h +++ b/cartographer/mapping_3d/pose_graph/constraint_builder.h @@ -36,7 +36,7 @@ #include "cartographer/mapping/trajectory_node.h" #include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h" #include "cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h" -#include "cartographer/mapping_3d/submaps.h" +#include "cartographer/mapping_3d/submap_3d.h" #include "cartographer/sensor/compressed_point_cloud.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/voxel_filter.h" @@ -75,7 +75,7 @@ class ConstraintBuilder { // The pointees of 'submap' and 'compressed_point_cloud' must stay valid until // all computations are finished. void MaybeAddConstraint( - const mapping::SubmapId& submap_id, const Submap* submap, + const mapping::SubmapId& submap_id, const mapping::Submap3D* submap, const mapping::NodeId& node_id, const mapping::TrajectoryNode::Data* const constant_data, const std::vector& submap_nodes, @@ -92,7 +92,7 @@ class ConstraintBuilder { // The pointees of 'submap' and 'compressed_point_cloud' must stay valid until // all computations are finished. void MaybeAddGlobalConstraint( - const mapping::SubmapId& submap_id, const Submap* submap, + const mapping::SubmapId& submap_id, const mapping::Submap3D* submap, const mapping::NodeId& node_id, const mapping::TrajectoryNode::Data* const constant_data, const std::vector& submap_nodes, @@ -114,8 +114,8 @@ class ConstraintBuilder { private: struct SubmapScanMatcher { - const HybridGrid* high_resolution_hybrid_grid; - const HybridGrid* low_resolution_hybrid_grid; + const mapping::HybridGrid* high_resolution_hybrid_grid; + const mapping::HybridGrid* low_resolution_hybrid_grid; std::unique_ptr fast_correlative_scan_matcher; }; @@ -125,14 +125,14 @@ class ConstraintBuilder { void ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( const mapping::SubmapId& submap_id, const std::vector& submap_nodes, - const Submap* submap, const std::function& work_item) + const mapping::Submap3D* submap, const std::function& work_item) REQUIRES(mutex_); // Constructs the scan matcher for a 'submap', then schedules its work items. void ConstructSubmapScanMatcher( const mapping::SubmapId& submap_id, const std::vector& submap_nodes, - const Submap* submap) EXCLUDES(mutex_); + const mapping::Submap3D* submap) EXCLUDES(mutex_); // Returns the scan matcher for a submap, which has to exist. const SubmapScanMatcher* GetSubmapScanMatcher( diff --git a/cartographer/mapping_3d/pose_graph/optimization_problem.cc b/cartographer/mapping_3d/pose_graph/optimization_problem.cc index c1f7748..d8669f2 100644 --- a/cartographer/mapping_3d/pose_graph/optimization_problem.cc +++ b/cartographer/mapping_3d/pose_graph/optimization_problem.cc @@ -279,7 +279,7 @@ void OptimizationProblem::Solve( CeresPose(submap_id_data.data.global_pose, translation_parameterization(), common::make_unique>(), + mapping::ConstantYawQuaternionPlus, 4, 2>>(), &problem)); problem.SetParameterBlockConstant( C_submaps.at(submap_id_data.id).translation()); @@ -499,7 +499,7 @@ void OptimizationProblem::Solve( Eigen::Vector3d::UnitZ())), nullptr, common::make_unique>(), + mapping::YawOnlyQuaternionPlus, 4, 1>>(), &problem)); fixed_frame_pose_initialized = true; } diff --git a/cartographer/mapping_3d/pose_graph_3d.cc b/cartographer/mapping_3d/pose_graph_3d.cc index 4f12816..53e1a6c 100644 --- a/cartographer/mapping_3d/pose_graph_3d.cc +++ b/cartographer/mapping_3d/pose_graph_3d.cc @@ -55,8 +55,7 @@ PoseGraph3D::~PoseGraph3D() { std::vector PoseGraph3D::InitializeGlobalSubmapPoses( const int trajectory_id, const common::Time time, - const std::vector>& - insertion_submaps) { + const std::vector>& insertion_submaps) { CHECK(!insertion_submaps.empty()); const auto& submap_data = optimization_problem_.submap_data(); if (insertion_submaps.size() == 1) { @@ -102,8 +101,7 @@ std::vector PoseGraph3D::InitializeGlobalSubmapPoses( NodeId PoseGraph3D::AddNode( std::shared_ptr constant_data, const int trajectory_id, - const std::vector>& - insertion_submaps) { + const std::vector>& insertion_submaps) { const transform::Rigid3d optimized_pose( GetLocalToGlobalTransform(trajectory_id) * constant_data->local_pose); @@ -255,7 +253,7 @@ void PoseGraph3D::ComputeConstraintsForOldNodes(const SubmapId& submap_id) { void PoseGraph3D::ComputeConstraintsForNode( const NodeId& node_id, - std::vector> insertion_submaps, + std::vector> insertion_submaps, const bool newly_finished_submap) { const auto& constant_data = trajectory_nodes_.at(node_id).constant_data; const std::vector submap_ids = InitializeGlobalSubmapPoses( @@ -446,8 +444,8 @@ void PoseGraph3D::AddSubmapFromProto( const SubmapId submap_id = {submap.submap_id().trajectory_id(), submap.submap_id().submap_index()}; - std::shared_ptr submap_ptr = - std::make_shared(submap.submap_3d()); + std::shared_ptr submap_ptr = + std::make_shared(submap.submap_3d()); common::MutexLocker locker(&mutex_); AddTrajectoryIfNeeded(submap_id.trajectory_id); @@ -483,7 +481,7 @@ void PoseGraph3D::AddNodeFromProto(const transform::Rigid3d& global_pose, } void PoseGraph3D::SetTrajectoryDataFromProto( - const mapping::proto::TrajectoryData& data) { + const proto::TrajectoryData& data) { TrajectoryData trajectory_data; trajectory_data.gravity_constant = data.gravity_constant(); trajectory_data.imu_calibration = { @@ -680,7 +678,7 @@ PoseGraph3D::GetFixedFramePoseData() { return optimization_problem_.fixed_frame_pose_data(); } -std::map +std::map PoseGraph3D::GetTrajectoryData() { common::MutexLocker locker(&mutex_); return optimization_problem_.trajectory_data(); diff --git a/cartographer/mapping_3d/pose_graph_3d.h b/cartographer/mapping_3d/pose_graph_3d.h index e218cef..046fbe8 100644 --- a/cartographer/mapping_3d/pose_graph_3d.h +++ b/cartographer/mapping_3d/pose_graph_3d.h @@ -37,7 +37,7 @@ #include "cartographer/mapping/trajectory_connectivity_state.h" #include "cartographer/mapping_3d/pose_graph/constraint_builder.h" #include "cartographer/mapping_3d/pose_graph/optimization_problem.h" -#include "cartographer/mapping_3d/submaps.h" +#include "cartographer/mapping_3d/submap_3d.h" #include "cartographer/sensor/fixed_frame_pose_data.h" #include "cartographer/sensor/landmark_data.h" #include "cartographer/sensor/odometry_data.h" @@ -70,10 +70,11 @@ class PoseGraph3D : public PoseGraph { // node data was inserted into the 'insertion_submaps'. If // 'insertion_submaps.front().finished()' is 'true', data was inserted into // this submap for the last time. - NodeId AddNode(std::shared_ptr constant_data, - int trajectory_id, - const std::vector>& - insertion_submaps) EXCLUDES(mutex_); + NodeId AddNode( + std::shared_ptr constant_data, + int trajectory_id, + const std::vector>& insertion_submaps) + EXCLUDES(mutex_); void AddImuData(int trajectory_id, const sensor::ImuData& imu_data) override EXCLUDES(mutex_); @@ -94,10 +95,10 @@ class PoseGraph3D : public PoseGraph { void AddSubmapFromProto(const transform::Rigid3d& global_submap_pose, const proto::Submap& submap) override; void AddNodeFromProto(const transform::Rigid3d& global_pose, - const mapping::proto::Node& node) override; + const proto::Node& node) override; void SetTrajectoryDataFromProto(const proto::TrajectoryData& data) override; - void AddNodeToSubmap(const mapping::NodeId& node_id, - const mapping::SubmapId& submap_id) override; + void AddNodeToSubmap(const NodeId& node_id, + const SubmapId& submap_id) override; void AddSerializedConstraints( const std::vector& constraints) override; void AddTrimmer(std::unique_ptr trimmer) override; @@ -137,7 +138,7 @@ class PoseGraph3D : public PoseGraph { // Likewise, all new nodes are matched against submaps which are finished. enum class SubmapState { kActive, kFinished }; struct SubmapData { - std::shared_ptr submap; + std::shared_ptr submap; // IDs of the nodes that were inserted into this map together with // constraints for them. They are not to be matched again when this submap @@ -157,13 +158,13 @@ class PoseGraph3D : public PoseGraph { // 'insertion_submaps'. Returns the IDs for the 'insertion_submaps'. std::vector InitializeGlobalSubmapPoses( int trajectory_id, const common::Time time, - const std::vector>& - insertion_submaps) REQUIRES(mutex_); + const std::vector>& insertion_submaps) + REQUIRES(mutex_); // Adds constraints for a node, and starts scan matching in the background. void ComputeConstraintsForNode( const NodeId& node_id, - std::vector> insertion_submaps, + std::vector> insertion_submaps, bool newly_finished_submap) REQUIRES(mutex_); // Computes constraints for a node and submap pair. diff --git a/cartographer/mapping_3d/proto/hybrid_grid.proto b/cartographer/mapping_3d/proto/hybrid_grid.proto index feb968d..b53f366 100644 --- a/cartographer/mapping_3d/proto/hybrid_grid.proto +++ b/cartographer/mapping_3d/proto/hybrid_grid.proto @@ -14,7 +14,7 @@ syntax = "proto3"; -package cartographer.mapping_3d.proto; +package cartographer.mapping.proto; message HybridGrid { float resolution = 1; diff --git a/cartographer/mapping_3d/proto/local_trajectory_builder_options.proto b/cartographer/mapping_3d/proto/local_trajectory_builder_options_3d.proto similarity index 91% rename from cartographer/mapping_3d/proto/local_trajectory_builder_options.proto rename to cartographer/mapping_3d/proto/local_trajectory_builder_options_3d.proto index 1db0dcd..cd60674 100644 --- a/cartographer/mapping_3d/proto/local_trajectory_builder_options.proto +++ b/cartographer/mapping_3d/proto/local_trajectory_builder_options_3d.proto @@ -14,16 +14,16 @@ syntax = "proto3"; -package cartographer.mapping_3d.proto; +package cartographer.mapping.proto; import "cartographer/mapping/proto/motion_filter_options.proto"; import "cartographer/sensor/proto/adaptive_voxel_filter_options.proto"; import "cartographer/mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.proto"; -import "cartographer/mapping_3d/proto/submaps_options.proto"; +import "cartographer/mapping_3d/proto/submaps_options_3d.proto"; import "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.proto"; // NEXT ID: 18 -message LocalTrajectoryBuilderOptions { +message LocalTrajectoryBuilderOptions3D { // Rangefinder points outside these ranges will be dropped. float min_range = 1; float max_range = 2; @@ -47,7 +47,7 @@ message LocalTrajectoryBuilderOptions { bool use_online_correlative_scan_matching = 13; mapping_2d.scan_matching.proto.RealTimeCorrelativeScanMatcherOptions real_time_correlative_scan_matcher_options = 14; - scan_matching.proto.CeresScanMatcherOptions + mapping_3d.scan_matching.proto.CeresScanMatcherOptions ceres_scan_matcher_options = 6; mapping.proto.MotionFilterOptions motion_filter_options = 7; @@ -62,5 +62,5 @@ message LocalTrajectoryBuilderOptions { // Number of histogram buckets for the rotational scan matcher. int32 rotational_histogram_size = 17; - SubmapsOptions submaps_options = 8; + SubmapsOptions3D submaps_options = 8; } diff --git a/cartographer/mapping_3d/proto/range_data_inserter_options.proto b/cartographer/mapping_3d/proto/range_data_inserter_options_3d.proto similarity index 93% rename from cartographer/mapping_3d/proto/range_data_inserter_options.proto rename to cartographer/mapping_3d/proto/range_data_inserter_options_3d.proto index b421c57..5fcf198 100644 --- a/cartographer/mapping_3d/proto/range_data_inserter_options.proto +++ b/cartographer/mapping_3d/proto/range_data_inserter_options_3d.proto @@ -14,9 +14,9 @@ syntax = "proto3"; -package cartographer.mapping_3d.proto; +package cartographer.mapping.proto; -message RangeDataInserterOptions { +message RangeDataInserterOptions3D { // Probability change for a hit (this will be converted to odds and therefore // must be greater than 0.5). double hit_probability = 1; diff --git a/cartographer/mapping_3d/proto/submaps_options.proto b/cartographer/mapping_3d/proto/submaps_options_3d.proto similarity index 90% rename from cartographer/mapping_3d/proto/submaps_options.proto rename to cartographer/mapping_3d/proto/submaps_options_3d.proto index fd1cd92..cf703a7 100644 --- a/cartographer/mapping_3d/proto/submaps_options.proto +++ b/cartographer/mapping_3d/proto/submaps_options_3d.proto @@ -14,11 +14,11 @@ syntax = "proto3"; -import "cartographer/mapping_3d/proto/range_data_inserter_options.proto"; +import "cartographer/mapping_3d/proto/range_data_inserter_options_3d.proto"; -package cartographer.mapping_3d.proto; +package cartographer.mapping.proto; -message SubmapsOptions { +message SubmapsOptions3D { // Resolution of the 'high_resolution' map in meters used for local SLAM and // loop closure. double high_resolution = 1; @@ -36,5 +36,5 @@ message SubmapsOptions { // matched against, then while being matched. int32 num_range_data = 2; - RangeDataInserterOptions range_data_inserter_options = 3; + RangeDataInserterOptions3D range_data_inserter_options = 3; } diff --git a/cartographer/mapping_3d/range_data_inserter.cc b/cartographer/mapping_3d/range_data_inserter_3d.cc similarity index 82% rename from cartographer/mapping_3d/range_data_inserter.cc rename to cartographer/mapping_3d/range_data_inserter_3d.cc index dc29901..b601067 100644 --- a/cartographer/mapping_3d/range_data_inserter.cc +++ b/cartographer/mapping_3d/range_data_inserter_3d.cc @@ -14,15 +14,14 @@ * limitations under the License. */ -#include "cartographer/mapping_3d/range_data_inserter.h" +#include "cartographer/mapping_3d/range_data_inserter_3d.h" #include "Eigen/Core" #include "cartographer/mapping/probability_values.h" #include "glog/logging.h" namespace cartographer { -namespace mapping_3d { - +namespace mapping { namespace { void InsertMissesIntoGrid(const std::vector& miss_table, @@ -54,9 +53,9 @@ void InsertMissesIntoGrid(const std::vector& miss_table, } // namespace -proto::RangeDataInserterOptions CreateRangeDataInserterOptions( +proto::RangeDataInserterOptions3D CreateRangeDataInserterOptions3D( common::LuaParameterDictionary* parameter_dictionary) { - proto::RangeDataInserterOptions options; + proto::RangeDataInserterOptions3D options; options.set_hit_probability( parameter_dictionary->GetDouble("hit_probability")); options.set_miss_probability( @@ -68,16 +67,16 @@ proto::RangeDataInserterOptions CreateRangeDataInserterOptions( return options; } -RangeDataInserter::RangeDataInserter( - const proto::RangeDataInserterOptions& options) +RangeDataInserter3D::RangeDataInserter3D( + const proto::RangeDataInserterOptions3D& options) : options_(options), - hit_table_(mapping::ComputeLookupTableToApplyOdds( - mapping::Odds(options_.hit_probability()))), - miss_table_(mapping::ComputeLookupTableToApplyOdds( - mapping::Odds(options_.miss_probability()))) {} + hit_table_( + ComputeLookupTableToApplyOdds(Odds(options_.hit_probability()))), + miss_table_( + ComputeLookupTableToApplyOdds(Odds(options_.miss_probability()))) {} -void RangeDataInserter::Insert(const sensor::RangeData& range_data, - HybridGrid* hybrid_grid) const { +void RangeDataInserter3D::Insert(const sensor::RangeData& range_data, + HybridGrid* hybrid_grid) const { CHECK_NOTNULL(hybrid_grid); for (const Eigen::Vector3f& hit : range_data.returns) { @@ -92,5 +91,5 @@ void RangeDataInserter::Insert(const sensor::RangeData& range_data, hybrid_grid->FinishUpdate(); } -} // namespace mapping_3d +} // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping_3d/range_data_inserter.h b/cartographer/mapping_3d/range_data_inserter_3d.h similarity index 66% rename from cartographer/mapping_3d/range_data_inserter.h rename to cartographer/mapping_3d/range_data_inserter_3d.h index d03ae3a..9af26f5 100644 --- a/cartographer/mapping_3d/range_data_inserter.h +++ b/cartographer/mapping_3d/range_data_inserter_3d.h @@ -14,38 +14,39 @@ * limitations under the License. */ -#ifndef CARTOGRAPHER_MAPPING_3D_RANGE_DATA_INSERTER_H_ -#define CARTOGRAPHER_MAPPING_3D_RANGE_DATA_INSERTER_H_ +#ifndef CARTOGRAPHER_MAPPING_3D_RANGE_DATA_INSERTER_3D_H_ +#define CARTOGRAPHER_MAPPING_3D_RANGE_DATA_INSERTER_3D_H_ #include "cartographer/mapping_3d/hybrid_grid.h" -#include "cartographer/mapping_3d/proto/range_data_inserter_options.pb.h" +#include "cartographer/mapping_3d/proto/range_data_inserter_options_3d.pb.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/range_data.h" namespace cartographer { -namespace mapping_3d { +namespace mapping { -proto::RangeDataInserterOptions CreateRangeDataInserterOptions( +proto::RangeDataInserterOptions3D CreateRangeDataInserterOptions3D( common::LuaParameterDictionary* parameter_dictionary); -class RangeDataInserter { +class RangeDataInserter3D { public: - explicit RangeDataInserter(const proto::RangeDataInserterOptions& options); + explicit RangeDataInserter3D( + const proto::RangeDataInserterOptions3D& options); - RangeDataInserter(const RangeDataInserter&) = delete; - RangeDataInserter& operator=(const RangeDataInserter&) = delete; + RangeDataInserter3D(const RangeDataInserter3D&) = delete; + RangeDataInserter3D& operator=(const RangeDataInserter3D&) = delete; // Inserts 'range_data' into 'hybrid_grid'. void Insert(const sensor::RangeData& range_data, HybridGrid* hybrid_grid) const; private: - const proto::RangeDataInserterOptions options_; + const proto::RangeDataInserterOptions3D options_; const std::vector hit_table_; const std::vector miss_table_; }; -} // namespace mapping_3d +} // namespace mapping } // namespace cartographer -#endif // CARTOGRAPHER_MAPPING_3D_RANGE_DATA_INSERTER_H_ +#endif // CARTOGRAPHER_MAPPING_3D_RANGE_DATA_INSERTER_3D_H_ diff --git a/cartographer/mapping_3d/range_data_inserter_test.cc b/cartographer/mapping_3d/range_data_inserter_3d_test.cc similarity index 75% rename from cartographer/mapping_3d/range_data_inserter_test.cc rename to cartographer/mapping_3d/range_data_inserter_3d_test.cc index e970247..e652d30 100644 --- a/cartographer/mapping_3d/range_data_inserter_test.cc +++ b/cartographer/mapping_3d/range_data_inserter_3d_test.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/mapping_3d/range_data_inserter.h" +#include "cartographer/mapping_3d/range_data_inserter_3d.h" #include #include @@ -23,20 +23,20 @@ #include "gmock/gmock.h" namespace cartographer { -namespace mapping_3d { +namespace mapping { namespace { -class RangeDataInserterTest : public ::testing::Test { +class RangeDataInserter3DTest : public ::testing::Test { protected: - RangeDataInserterTest() : hybrid_grid_(1.f) { + RangeDataInserter3DTest() : hybrid_grid_(1.f) { auto parameter_dictionary = common::MakeDictionary( "return { " "hit_probability = 0.7, " "miss_probability = 0.4, " "num_free_space_voxels = 1000, " "}"); - options_ = CreateRangeDataInserterOptions(parameter_dictionary.get()); - range_data_inserter_.reset(new RangeDataInserter(options_)); + options_ = CreateRangeDataInserterOptions3D(parameter_dictionary.get()); + range_data_inserter_.reset(new RangeDataInserter3D(options_)); } void InsertPointCloud() { @@ -57,15 +57,15 @@ class RangeDataInserterTest : public ::testing::Test { hybrid_grid_.GetCellIndex(Eigen::Vector3f(x, y, z))); } - const proto::RangeDataInserterOptions& options() const { return options_; } + const proto::RangeDataInserterOptions3D& options() const { return options_; } private: HybridGrid hybrid_grid_; - std::unique_ptr range_data_inserter_; - proto::RangeDataInserterOptions options_; + std::unique_ptr range_data_inserter_; + proto::RangeDataInserterOptions3D options_; }; -TEST_F(RangeDataInserterTest, InsertPointCloud) { +TEST_F(RangeDataInserter3DTest, InsertPointCloud) { InsertPointCloud(); EXPECT_NEAR(options().miss_probability(), GetProbability(0.f, 0.f, -4.f), 1e-4); @@ -85,7 +85,7 @@ TEST_F(RangeDataInserterTest, InsertPointCloud) { } } -TEST_F(RangeDataInserterTest, ProbabilityProgression) { +TEST_F(RangeDataInserter3DTest, ProbabilityProgression) { InsertPointCloud(); EXPECT_NEAR(options().hit_probability(), GetProbability(-2.f, 0.f, 4.f), 1e-4); @@ -97,11 +97,11 @@ TEST_F(RangeDataInserterTest, ProbabilityProgression) { for (int i = 0; i < 1000; ++i) { InsertPointCloud(); } - EXPECT_NEAR(mapping::kMaxProbability, GetProbability(-2.f, 0.f, 4.f), 1e-3); - EXPECT_NEAR(mapping::kMinProbability, GetProbability(-2.f, 0.f, 3.f), 1e-3); - EXPECT_NEAR(mapping::kMinProbability, GetProbability(0.f, 0.f, -3.f), 1e-3); + EXPECT_NEAR(kMaxProbability, GetProbability(-2.f, 0.f, 4.f), 1e-3); + EXPECT_NEAR(kMinProbability, GetProbability(-2.f, 0.f, 3.f), 1e-3); + EXPECT_NEAR(kMinProbability, GetProbability(0.f, 0.f, -3.f), 1e-3); } } // namespace -} // namespace mapping_3d +} // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping_3d/rotation_parameterization.h b/cartographer/mapping_3d/rotation_parameterization.h index b23678d..c554287 100644 --- a/cartographer/mapping_3d/rotation_parameterization.h +++ b/cartographer/mapping_3d/rotation_parameterization.h @@ -22,7 +22,7 @@ #include "ceres/rotation.h" namespace cartographer { -namespace mapping_3d { +namespace mapping { struct YawOnlyQuaternionPlus { template @@ -61,7 +61,7 @@ struct ConstantYawQuaternionPlus { } }; -} // namespace mapping_3d +} // namespace mapping } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_3D_ROTATION_PARAMETERIZATION_H_ diff --git a/cartographer/mapping_3d/scan_matching/ceres_scan_matcher.cc b/cartographer/mapping_3d/scan_matching/ceres_scan_matcher.cc index a504cc6..945fb08 100644 --- a/cartographer/mapping_3d/scan_matching/ceres_scan_matcher.cc +++ b/cartographer/mapping_3d/scan_matching/ceres_scan_matcher.cc @@ -80,7 +80,7 @@ void CeresScanMatcher::Match(const Eigen::Vector3d& target_translation, options_.only_optimize_yaw() ? std::unique_ptr( common::make_unique>()) + mapping::YawOnlyQuaternionPlus, 4, 1>>()) : std::unique_ptr( common::make_unique()), &problem); @@ -91,7 +91,8 @@ void CeresScanMatcher::Match(const Eigen::Vector3d& target_translation, CHECK_GT(options_.occupied_space_weight(i), 0.); const sensor::PointCloud& point_cloud = *point_clouds_and_hybrid_grids[i].first; - const HybridGrid& hybrid_grid = *point_clouds_and_hybrid_grids[i].second; + const mapping::HybridGrid& hybrid_grid = + *point_clouds_and_hybrid_grids[i].second; problem.AddResidualBlock( OccupiedSpaceCostFunction::CreateAutoDiffCostFunction( options_.occupied_space_weight(i) / diff --git a/cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h b/cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h index b7ce2d5..45969e1 100644 --- a/cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h +++ b/cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h @@ -35,7 +35,7 @@ proto::CeresScanMatcherOptions CreateCeresScanMatcherOptions( common::LuaParameterDictionary* parameter_dictionary); using PointCloudAndHybridGridPointers = - std::pair; + std::pair; // This scan matcher uses Ceres to align scans with an existing map. class CeresScanMatcher { diff --git a/cartographer/mapping_3d/scan_matching/ceres_scan_matcher_test.cc b/cartographer/mapping_3d/scan_matching/ceres_scan_matcher_test.cc index 4d8656f..8391d7c 100644 --- a/cartographer/mapping_3d/scan_matching/ceres_scan_matcher_test.cc +++ b/cartographer/mapping_3d/scan_matching/ceres_scan_matcher_test.cc @@ -74,7 +74,7 @@ class CeresScanMatcherTest : public ::testing::Test { EXPECT_THAT(pose, transform::IsNearly(expected_pose_, 3e-2)); } - HybridGrid hybrid_grid_; + mapping::HybridGrid hybrid_grid_; transform::Rigid3d expected_pose_; sensor::PointCloud point_cloud_; proto::CeresScanMatcherOptions options_; diff --git a/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc b/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc index 2ae4424..dc00dfe 100644 --- a/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc +++ b/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc @@ -58,7 +58,7 @@ CreateFastCorrelativeScanMatcherOptions( class PrecomputationGridStack { public: PrecomputationGridStack( - const HybridGrid& hybrid_grid, + const mapping::HybridGrid& hybrid_grid, const proto::FastCorrelativeScanMatcherOptions& options) { CHECK_GE(options.branch_and_bound_depth(), 1); CHECK_GE(options.full_resolution_depth(), 1); @@ -142,8 +142,8 @@ std::vector> HistogramsAtAnglesFromNodes( } // namespace FastCorrelativeScanMatcher::FastCorrelativeScanMatcher( - const HybridGrid& hybrid_grid, - const HybridGrid* const low_resolution_hybrid_grid, + const mapping::HybridGrid& hybrid_grid, + const mapping::HybridGrid* const low_resolution_hybrid_grid, const std::vector& nodes, const proto::FastCorrelativeScanMatcherOptions& options) : options_(options), diff --git a/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h b/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h index 071aec3..233e5f0 100644 --- a/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h +++ b/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h @@ -57,8 +57,8 @@ class FastCorrelativeScanMatcher { }; FastCorrelativeScanMatcher( - const HybridGrid& hybrid_grid, - const HybridGrid* low_resolution_hybrid_grid, + const mapping::HybridGrid& hybrid_grid, + const mapping::HybridGrid* low_resolution_hybrid_grid, const std::vector& nodes, const proto::FastCorrelativeScanMatcherOptions& options); ~FastCorrelativeScanMatcher(); @@ -132,7 +132,7 @@ class FastCorrelativeScanMatcher { const float resolution_; const int width_in_voxels_; std::unique_ptr precomputation_grid_stack_; - const HybridGrid* const low_resolution_hybrid_grid_; + const mapping::HybridGrid* const low_resolution_hybrid_grid_; RotationalScanMatcher rotational_scan_matcher_; }; diff --git a/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_test.cc b/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_test.cc index 5e4e9ff..7919523 100644 --- a/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_test.cc +++ b/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_test.cc @@ -23,7 +23,7 @@ #include "cartographer/common/lua_parameter_dictionary_test_helpers.h" #include "cartographer/common/make_unique.h" -#include "cartographer/mapping_3d/range_data_inserter.h" +#include "cartographer/mapping_3d/range_data_inserter_3d.h" #include "cartographer/transform/rigid_transform_test_helpers.h" #include "cartographer/transform/transform.h" #include "gtest/gtest.h" @@ -81,7 +81,7 @@ class FastCorrelativeScanMatcherTest : public ::testing::Test { return CreateFastCorrelativeScanMatcherOptions(parameter_dictionary.get()); } - static mapping_3d::proto::RangeDataInserterOptions + static mapping::proto::RangeDataInserterOptions3D CreateRangeDataInserterTestOptions() { auto parameter_dictionary = common::MakeDictionary( "return { " @@ -89,13 +89,14 @@ class FastCorrelativeScanMatcherTest : public ::testing::Test { "miss_probability = 0.4, " "num_free_space_voxels = 5, " "}"); - return CreateRangeDataInserterOptions(parameter_dictionary.get()); + return mapping::CreateRangeDataInserterOptions3D( + parameter_dictionary.get()); } std::unique_ptr GetFastCorrelativeScanMatcher( const proto::FastCorrelativeScanMatcherOptions& options, const transform::Rigid3f& pose) { - hybrid_grid_ = common::make_unique(0.05f); + hybrid_grid_ = common::make_unique(0.05f); range_data_inserter_.Insert( sensor::RangeData{pose.translation(), sensor::TransformPointCloud(point_cloud_, pose), @@ -125,10 +126,10 @@ class FastCorrelativeScanMatcherTest : public ::testing::Test { std::mt19937 prng_ = std::mt19937(42); std::uniform_real_distribution distribution_ = std::uniform_real_distribution(-1.f, 1.f); - RangeDataInserter range_data_inserter_; + mapping::RangeDataInserter3D range_data_inserter_; const proto::FastCorrelativeScanMatcherOptions options_; sensor::PointCloud point_cloud_; - std::unique_ptr hybrid_grid_; + std::unique_ptr hybrid_grid_; }; constexpr float kMinScore = 0.1f; diff --git a/cartographer/mapping_3d/scan_matching/interpolated_grid.h b/cartographer/mapping_3d/scan_matching/interpolated_grid.h index 32b8b33..ec9c313 100644 --- a/cartographer/mapping_3d/scan_matching/interpolated_grid.h +++ b/cartographer/mapping_3d/scan_matching/interpolated_grid.h @@ -34,7 +34,7 @@ namespace scan_matching { // continuously differentiable. class InterpolatedGrid { public: - explicit InterpolatedGrid(const HybridGrid& hybrid_grid) + explicit InterpolatedGrid(const mapping::HybridGrid& hybrid_grid) : hybrid_grid_(hybrid_grid) {} InterpolatedGrid(const InterpolatedGrid&) = delete; @@ -145,7 +145,7 @@ class InterpolatedGrid { return CenterOfLowerVoxel(jet_x.a, jet_y.a, jet_z.a); } - const HybridGrid& hybrid_grid_; + const mapping::HybridGrid& hybrid_grid_; }; } // namespace scan_matching diff --git a/cartographer/mapping_3d/scan_matching/interpolated_grid_test.cc b/cartographer/mapping_3d/scan_matching/interpolated_grid_test.cc index 4f8893b..e7b8615 100644 --- a/cartographer/mapping_3d/scan_matching/interpolated_grid_test.cc +++ b/cartographer/mapping_3d/scan_matching/interpolated_grid_test.cc @@ -43,7 +43,7 @@ class InterpolatedGridTest : public ::testing::Test { hybrid_grid_.GetCellIndex(Eigen::Vector3f(x, y, z))); } - HybridGrid hybrid_grid_; + mapping::HybridGrid hybrid_grid_; InterpolatedGrid interpolated_grid_; }; diff --git a/cartographer/mapping_3d/scan_matching/low_resolution_matcher.cc b/cartographer/mapping_3d/scan_matching/low_resolution_matcher.cc index a1b5a65..6c31072 100644 --- a/cartographer/mapping_3d/scan_matching/low_resolution_matcher.cc +++ b/cartographer/mapping_3d/scan_matching/low_resolution_matcher.cc @@ -21,7 +21,8 @@ namespace mapping_3d { namespace scan_matching { std::function CreateLowResolutionMatcher( - const HybridGrid* low_resolution_grid, const sensor::PointCloud* points) { + const mapping::HybridGrid* low_resolution_grid, + const sensor::PointCloud* points) { return [=](const transform::Rigid3f& pose) { float score = 0.f; for (const Eigen::Vector3f& point : diff --git a/cartographer/mapping_3d/scan_matching/low_resolution_matcher.h b/cartographer/mapping_3d/scan_matching/low_resolution_matcher.h index c6a804c..4453571 100644 --- a/cartographer/mapping_3d/scan_matching/low_resolution_matcher.h +++ b/cartographer/mapping_3d/scan_matching/low_resolution_matcher.h @@ -28,7 +28,8 @@ namespace mapping_3d { namespace scan_matching { std::function CreateLowResolutionMatcher( - const HybridGrid* low_resolution_grid, const sensor::PointCloud* points); + const mapping::HybridGrid* low_resolution_grid, + const sensor::PointCloud* points); } // namespace scan_matching } // namespace mapping_3d diff --git a/cartographer/mapping_3d/scan_matching/precomputation_grid.cc b/cartographer/mapping_3d/scan_matching/precomputation_grid.cc index fe16e4e..6c2eef9 100644 --- a/cartographer/mapping_3d/scan_matching/precomputation_grid.cc +++ b/cartographer/mapping_3d/scan_matching/precomputation_grid.cc @@ -47,9 +47,11 @@ Eigen::Array3i CellIndexAtHalfResolution(const Eigen::Array3i& cell_index) { } // namespace -PrecomputationGrid ConvertToPrecomputationGrid(const HybridGrid& hybrid_grid) { +PrecomputationGrid ConvertToPrecomputationGrid( + const mapping::HybridGrid& hybrid_grid) { PrecomputationGrid result(hybrid_grid.resolution()); - for (auto it = HybridGrid::Iterator(hybrid_grid); !it.Done(); it.Next()) { + for (auto it = mapping::HybridGrid::Iterator(hybrid_grid); !it.Done(); + it.Next()) { const int cell_value = common::RoundToInt( (mapping::ValueToProbability(it.GetValue()) - mapping::kMinProbability) * diff --git a/cartographer/mapping_3d/scan_matching/precomputation_grid.h b/cartographer/mapping_3d/scan_matching/precomputation_grid.h index 37ffa08..f6f45fe 100644 --- a/cartographer/mapping_3d/scan_matching/precomputation_grid.h +++ b/cartographer/mapping_3d/scan_matching/precomputation_grid.h @@ -23,10 +23,10 @@ namespace cartographer { namespace mapping_3d { namespace scan_matching { -class PrecomputationGrid : public HybridGridBase { +class PrecomputationGrid : public mapping::HybridGridBase { public: explicit PrecomputationGrid(const float resolution) - : HybridGridBase(resolution) {} + : mapping::HybridGridBase(resolution) {} // Maps values from [0, 255] to [kMinProbability, kMaxProbability]. static float ToProbability(float value) { @@ -38,7 +38,8 @@ class PrecomputationGrid : public HybridGridBase { // Converts a HybridGrid to a PrecomputationGrid representing the same data, // but only using 8 bit instead of 2 x 16 bit. -PrecomputationGrid ConvertToPrecomputationGrid(const HybridGrid& hybrid_grid); +PrecomputationGrid ConvertToPrecomputationGrid( + const mapping::HybridGrid& hybrid_grid); // Returns a grid of the same resolution containing the maximum value of // original voxels in 'grid'. This maximum is over the 8 voxels that have diff --git a/cartographer/mapping_3d/scan_matching/precomputation_grid_test.cc b/cartographer/mapping_3d/scan_matching/precomputation_grid_test.cc index 73d5242..111cb06 100644 --- a/cartographer/mapping_3d/scan_matching/precomputation_grid_test.cc +++ b/cartographer/mapping_3d/scan_matching/precomputation_grid_test.cc @@ -29,7 +29,7 @@ namespace scan_matching { namespace { TEST(PrecomputedGridGeneratorTest, TestAgainstNaiveAlgorithm) { - HybridGrid hybrid_grid(2.f); + mapping::HybridGrid hybrid_grid(2.f); std::mt19937 rng(23847); std::uniform_int_distribution coordinate_distribution(-50, 49); diff --git a/cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher.cc b/cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher.cc index 3ca643f..798e978 100644 --- a/cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher.cc +++ b/cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher.cc @@ -34,7 +34,8 @@ RealTimeCorrelativeScanMatcher::RealTimeCorrelativeScanMatcher( float RealTimeCorrelativeScanMatcher::Match( const transform::Rigid3d& initial_pose_estimate, - const sensor::PointCloud& point_cloud, const HybridGrid& hybrid_grid, + const sensor::PointCloud& point_cloud, + const mapping::HybridGrid& hybrid_grid, transform::Rigid3d* pose_estimate) const { CHECK_NOTNULL(pose_estimate); float best_score = -1.f; @@ -96,7 +97,7 @@ RealTimeCorrelativeScanMatcher::GenerateExhaustiveSearchTransforms( } float RealTimeCorrelativeScanMatcher::ScoreCandidate( - const HybridGrid& hybrid_grid, + const mapping::HybridGrid& hybrid_grid, const sensor::PointCloud& transformed_point_cloud, const transform::Rigid3f& transform) const { float score = 0.f; diff --git a/cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher.h b/cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher.h index bdab139..a016585 100644 --- a/cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher.h +++ b/cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher.h @@ -46,13 +46,13 @@ class RealTimeCorrelativeScanMatcher { // returns the score. float Match(const transform::Rigid3d& initial_pose_estimate, const sensor::PointCloud& point_cloud, - const HybridGrid& hybrid_grid, + const mapping::HybridGrid& hybrid_grid, transform::Rigid3d* pose_estimate) const; private: std::vector GenerateExhaustiveSearchTransforms( float resolution, const sensor::PointCloud& point_cloud) const; - float ScoreCandidate(const HybridGrid& hybrid_grid, + float ScoreCandidate(const mapping::HybridGrid& hybrid_grid, const sensor::PointCloud& transformed_point_cloud, const transform::Rigid3f& transform) const; diff --git a/cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher_test.cc b/cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher_test.cc index 07d69f3..c4bb9ab 100644 --- a/cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher_test.cc +++ b/cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher_test.cc @@ -72,7 +72,7 @@ class RealTimeCorrelativeScanMatcherTest : public ::testing::Test { EXPECT_THAT(pose, transform::IsNearly(expected_pose_, 1e-3)); } - HybridGrid hybrid_grid_; + mapping::HybridGrid hybrid_grid_; transform::Rigid3d expected_pose_; sensor::PointCloud point_cloud_; std::unique_ptr diff --git a/cartographer/mapping_3d/submaps.cc b/cartographer/mapping_3d/submap_3d.cc similarity index 85% rename from cartographer/mapping_3d/submaps.cc rename to cartographer/mapping_3d/submap_3d.cc index 22acaea..02e590a 100644 --- a/cartographer/mapping_3d/submaps.cc +++ b/cartographer/mapping_3d/submap_3d.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/mapping_3d/submaps.h" +#include "cartographer/mapping_3d/submap_3d.h" #include #include @@ -24,8 +24,7 @@ #include "glog/logging.h" namespace cartographer { -namespace mapping_3d { - +namespace mapping { namespace { struct PixelData { @@ -69,7 +68,7 @@ std::vector AccumulatePixelData( pixel.min_z = std::min(pixel.min_z, voxel_index_and_probability[2]); pixel.max_z = std::max(pixel.max_z, voxel_index_and_probability[2]); const float probability = - mapping::ValueToProbability(voxel_index_and_probability[3]); + ValueToProbability(voxel_index_and_probability[3]); pixel.probability_sum += probability; pixel.max_probability = std::max(pixel.max_probability, probability); } @@ -88,7 +87,7 @@ std::vector ExtractVoxelData( constexpr float kXrayObstructedCellProbabilityLimit = 0.501f; for (auto it = HybridGrid::Iterator(hybrid_grid); !it.Done(); it.Next()) { const uint16 probability_value = it.GetValue(); - const float probability = mapping::ValueToProbability(probability_value); + const float probability = ValueToProbability(probability_value); if (probability < kXrayObstructedCellProbabilityLimit) { // We ignore non-obstructed cells. continue; @@ -133,11 +132,10 @@ std::string ComputePixelValues( const float free_space_weight = kFreeSpaceWeight * free_space; const float total_weight = pixel.count + free_space_weight; const float free_space_probability = 1.f - pixel.max_probability; - const float average_probability = mapping::ClampProbability( + const float average_probability = ClampProbability( (pixel.probability_sum + free_space_probability * free_space_weight) / total_weight); - const int delta = - 128 - mapping::ProbabilityToLogOddsInteger(average_probability); + const int delta = 128 - ProbabilityToLogOddsInteger(average_probability); const uint8 alpha = delta > 0 ? 0 : -delta; const uint8 value = delta > 0 ? delta : 0; cell_data.push_back(value); // value @@ -148,7 +146,7 @@ std::string ComputePixelValues( void AddToTextureProto( const HybridGrid& hybrid_grid, const transform::Rigid3d& global_submap_pose, - mapping::proto::SubmapQuery::Response::SubmapTexture* const texture) { + proto::SubmapQuery::Response::SubmapTexture* const texture) { // Generate an X-ray view through the 'hybrid_grid', aligned to the // xy-plane in the global map frame. const float resolution = hybrid_grid.resolution(); @@ -180,9 +178,9 @@ void AddToTextureProto( } // namespace -proto::SubmapsOptions CreateSubmapsOptions( +proto::SubmapsOptions3D CreateSubmapsOptions3D( common::LuaParameterDictionary* parameter_dictionary) { - proto::SubmapsOptions options; + proto::SubmapsOptions3D options; options.set_high_resolution( parameter_dictionary->GetDouble("high_resolution")); options.set_high_resolution_max_range( @@ -191,22 +189,22 @@ proto::SubmapsOptions CreateSubmapsOptions( options.set_num_range_data( parameter_dictionary->GetNonNegativeInt("num_range_data")); *options.mutable_range_data_inserter_options() = - CreateRangeDataInserterOptions( + CreateRangeDataInserterOptions3D( parameter_dictionary->GetDictionary("range_data_inserter").get()); CHECK_GT(options.num_range_data(), 0); return options; } -Submap::Submap(const float high_resolution, const float low_resolution, - const transform::Rigid3d& local_submap_pose) - : mapping::Submap(local_submap_pose), +Submap3D::Submap3D(const float high_resolution, const float low_resolution, + const transform::Rigid3d& local_submap_pose) + : Submap(local_submap_pose), high_resolution_hybrid_grid_( common::make_unique(high_resolution)), low_resolution_hybrid_grid_( common::make_unique(low_resolution)) {} -Submap::Submap(const mapping::proto::Submap3D& proto) - : mapping::Submap(transform::ToRigid3(proto.local_pose())), +Submap3D::Submap3D(const proto::Submap3D& proto) + : Submap(transform::ToRigid3(proto.local_pose())), high_resolution_hybrid_grid_( common::make_unique(proto.high_resolution_hybrid_grid())), low_resolution_hybrid_grid_( @@ -215,8 +213,8 @@ Submap::Submap(const mapping::proto::Submap3D& proto) SetFinished(proto.finished()); } -void Submap::ToProto(mapping::proto::Submap* const proto, - bool include_probability_grid_data) const { +void Submap3D::ToProto(proto::Submap* const proto, + bool include_probability_grid_data) const { auto* const submap_3d = proto->mutable_submap_3d(); *submap_3d->mutable_local_pose() = transform::ToProto(local_pose()); submap_3d->set_num_range_data(num_range_data()); @@ -229,7 +227,7 @@ void Submap::ToProto(mapping::proto::Submap* const proto, } } -void Submap::UpdateFromProto(const mapping::proto::Submap& proto) { +void Submap3D::UpdateFromProto(const proto::Submap& proto) { CHECK(proto.has_submap_3d()); const auto& submap_3d = proto.submap_3d(); SetNumRangeData(submap_3d.num_range_data()); @@ -250,9 +248,9 @@ void Submap::UpdateFromProto(const mapping::proto::Submap& proto) { } } -void Submap::ToResponseProto( +void Submap3D::ToResponseProto( const transform::Rigid3d& global_submap_pose, - mapping::proto::SubmapQuery::Response* const response) const { + proto::SubmapQuery::Response* const response) const { response->set_submap_version(num_range_data()); AddToTextureProto(*high_resolution_hybrid_grid_, global_submap_pose, @@ -261,9 +259,9 @@ void Submap::ToResponseProto( response->add_textures()); } -void Submap::InsertRangeData(const sensor::RangeData& range_data, - const RangeDataInserter& range_data_inserter, - const int high_resolution_max_range) { +void Submap3D::InsertRangeData(const sensor::RangeData& range_data, + const RangeDataInserter3D& range_data_inserter, + const int high_resolution_max_range) { CHECK(!finished()); const sensor::RangeData transformed_range_data = sensor::TransformRangeData( range_data, local_pose().inverse().cast()); @@ -276,12 +274,12 @@ void Submap::InsertRangeData(const sensor::RangeData& range_data, SetNumRangeData(num_range_data() + 1); } -void Submap::Finish() { +void Submap3D::Finish() { CHECK(!finished()); SetFinished(true); } -ActiveSubmaps::ActiveSubmaps(const proto::SubmapsOptions& options) +ActiveSubmaps3D::ActiveSubmaps3D(const proto::SubmapsOptions3D& options) : options_(options), range_data_inserter_(options.range_data_inserter_options()) { // We always want to have at least one submap which we can return and will @@ -292,13 +290,13 @@ ActiveSubmaps::ActiveSubmaps(const proto::SubmapsOptions& options) AddSubmap(transform::Rigid3d::Identity()); } -std::vector> ActiveSubmaps::submaps() const { +std::vector> ActiveSubmaps3D::submaps() const { return submaps_; } -int ActiveSubmaps::matching_index() const { return matching_submap_index_; } +int ActiveSubmaps3D::matching_index() const { return matching_submap_index_; } -void ActiveSubmaps::InsertRangeData( +void ActiveSubmaps3D::InsertRangeData( const sensor::RangeData& range_data, const Eigen::Quaterniond& gravity_alignment) { for (auto& submap : submaps_) { @@ -311,17 +309,17 @@ void ActiveSubmaps::InsertRangeData( } } -void ActiveSubmaps::AddSubmap(const transform::Rigid3d& local_submap_pose) { +void ActiveSubmaps3D::AddSubmap(const transform::Rigid3d& local_submap_pose) { if (submaps_.size() > 1) { submaps_.front()->Finish(); ++matching_submap_index_; submaps_.erase(submaps_.begin()); } - submaps_.emplace_back(new Submap(options_.high_resolution(), - options_.low_resolution(), - local_submap_pose)); + submaps_.emplace_back(new Submap3D(options_.high_resolution(), + options_.low_resolution(), + local_submap_pose)); LOG(INFO) << "Added submap " << matching_submap_index_ + submaps_.size(); } -} // namespace mapping_3d +} // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping_3d/submaps.h b/cartographer/mapping_3d/submap_3d.h similarity index 70% rename from cartographer/mapping_3d/submaps.h rename to cartographer/mapping_3d/submap_3d.h index 6d33a6b..153b888 100644 --- a/cartographer/mapping_3d/submaps.h +++ b/cartographer/mapping_3d/submap_3d.h @@ -14,8 +14,8 @@ * limitations under the License. */ -#ifndef CARTOGRAPHER_MAPPING_3D_SUBMAPS_H_ -#define CARTOGRAPHER_MAPPING_3D_SUBMAPS_H_ +#ifndef CARTOGRAPHER_MAPPING_3D_SUBMAP_3D_H_ +#define CARTOGRAPHER_MAPPING_3D_SUBMAP_3D_H_ #include #include @@ -28,31 +28,30 @@ #include "cartographer/mapping/proto/submap_visualization.pb.h" #include "cartographer/mapping/submaps.h" #include "cartographer/mapping_3d/hybrid_grid.h" -#include "cartographer/mapping_3d/proto/submaps_options.pb.h" -#include "cartographer/mapping_3d/range_data_inserter.h" +#include "cartographer/mapping_3d/proto/submaps_options_3d.pb.h" +#include "cartographer/mapping_3d/range_data_inserter_3d.h" #include "cartographer/sensor/range_data.h" #include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/transform.h" namespace cartographer { -namespace mapping_3d { +namespace mapping { -proto::SubmapsOptions CreateSubmapsOptions( +proto::SubmapsOptions3D CreateSubmapsOptions3D( common::LuaParameterDictionary* parameter_dictionary); -class Submap : public mapping::Submap { +class Submap3D : public Submap { public: - Submap(float high_resolution, float low_resolution, - const transform::Rigid3d& local_submap_pose); - explicit Submap(const mapping::proto::Submap3D& proto); + Submap3D(float high_resolution, float low_resolution, + const transform::Rigid3d& local_submap_pose); + explicit Submap3D(const proto::Submap3D& proto); - void ToProto(mapping::proto::Submap* proto, + void ToProto(proto::Submap* proto, bool include_probability_grid_data) const override; - void UpdateFromProto(const mapping::proto::Submap& proto) override; + void UpdateFromProto(const proto::Submap& proto) override; - void ToResponseProto( - const transform::Rigid3d& global_submap_pose, - mapping::proto::SubmapQuery::Response* response) const override; + void ToResponseProto(const transform::Rigid3d& global_submap_pose, + proto::SubmapQuery::Response* response) const override; const HybridGrid& high_resolution_hybrid_grid() const { return *high_resolution_hybrid_grid_; @@ -64,7 +63,7 @@ class Submap : public mapping::Submap { // Insert 'range_data' into this submap using 'range_data_inserter'. The // submap must not be finished yet. void InsertRangeData(const sensor::RangeData& range_data, - const RangeDataInserter& range_data_inserter, + const RangeDataInserter3D& range_data_inserter, int high_resolution_max_range); void Finish(); @@ -82,12 +81,12 @@ class Submap : public mapping::Submap { // considered initialized: the old submap is no longer changed, the "new" submap // is now the "old" submap and is used for scan-to-map matching. Moreover, a // "new" submap gets created. The "old" submap is forgotten by this object. -class ActiveSubmaps { +class ActiveSubmaps3D { public: - explicit ActiveSubmaps(const proto::SubmapsOptions& options); + explicit ActiveSubmaps3D(const proto::SubmapsOptions3D& options); - ActiveSubmaps(const ActiveSubmaps&) = delete; - ActiveSubmaps& operator=(const ActiveSubmaps&) = delete; + ActiveSubmaps3D(const ActiveSubmaps3D&) = delete; + ActiveSubmaps3D& operator=(const ActiveSubmaps3D&) = delete; // Returns the index of the newest initialized Submap which can be // used for scan-to-map matching. @@ -99,18 +98,18 @@ class ActiveSubmaps { void InsertRangeData(const sensor::RangeData& range_data, const Eigen::Quaterniond& gravity_alignment); - std::vector> submaps() const; + std::vector> submaps() const; private: void AddSubmap(const transform::Rigid3d& local_submap_pose); - const proto::SubmapsOptions options_; + const proto::SubmapsOptions3D options_; int matching_submap_index_ = 0; - std::vector> submaps_; - RangeDataInserter range_data_inserter_; + std::vector> submaps_; + RangeDataInserter3D range_data_inserter_; }; -} // namespace mapping_3d +} // namespace mapping } // namespace cartographer -#endif // CARTOGRAPHER_MAPPING_3D_SUBMAPS_H_ +#endif // CARTOGRAPHER_MAPPING_3D_SUBMAP_3D_H_ diff --git a/cartographer/mapping_3d/submaps_test.cc b/cartographer/mapping_3d/submap_3d_test.cc similarity index 79% rename from cartographer/mapping_3d/submaps_test.cc rename to cartographer/mapping_3d/submap_3d_test.cc index 36776bc..e272e31 100644 --- a/cartographer/mapping_3d/submaps_test.cc +++ b/cartographer/mapping_3d/submap_3d_test.cc @@ -14,24 +14,25 @@ * limitations under the License. */ -#include "cartographer/mapping_3d/submaps.h" +#include "cartographer/mapping_3d/submap_3d.h" #include "cartographer/transform/transform.h" #include "gmock/gmock.h" namespace cartographer { -namespace mapping_3d { +namespace mapping { namespace { TEST(SubmapsTest, ToFromProto) { - const Submap expected(0.05, 0.25, - transform::Rigid3d(Eigen::Vector3d(1., 2., 0.), - Eigen::Quaterniond(0., 0., 0., 1.))); - mapping::proto::Submap proto; + const Submap3D expected( + 0.05, 0.25, + transform::Rigid3d(Eigen::Vector3d(1., 2., 0.), + Eigen::Quaterniond(0., 0., 0., 1.))); + proto::Submap proto; expected.ToProto(&proto, true /* include_probability_grid_data */); EXPECT_FALSE(proto.has_submap_2d()); EXPECT_TRUE(proto.has_submap_3d()); - const auto actual = Submap(proto.submap_3d()); + const auto actual = Submap3D(proto.submap_3d()); EXPECT_TRUE(expected.local_pose().translation().isApprox( actual.local_pose().translation(), 1e-6)); EXPECT_TRUE(expected.local_pose().rotation().isApprox( @@ -43,5 +44,5 @@ TEST(SubmapsTest, ToFromProto) { } } // namespace -} // namespace mapping_3d +} // namespace mapping } // namespace cartographer diff --git a/cartographer/sensor/compressed_point_cloud.cc b/cartographer/sensor/compressed_point_cloud.cc index f98c619..63f03d6 100644 --- a/cartographer/sensor/compressed_point_cloud.cc +++ b/cartographer/sensor/compressed_point_cloud.cc @@ -103,7 +103,7 @@ CompressedPointCloud::CompressedPointCloud(const PointCloud& point_cloud) Eigen::Array3i point; int index; }; - using Blocks = mapping_3d::HybridGridBase>; + using Blocks = mapping::HybridGridBase>; Blocks blocks(kPrecision); int num_blocks = 0; CHECK_LE(point_cloud.size(), std::numeric_limits::max()); diff --git a/cartographer/sensor/voxel_filter.cc b/cartographer/sensor/voxel_filter.cc index ab4c63c..fe496a0 100644 --- a/cartographer/sensor/voxel_filter.cc +++ b/cartographer/sensor/voxel_filter.cc @@ -78,8 +78,7 @@ PointCloud AdaptivelyVoxelFiltered( template PointCloudType FilterPointCloudUsingVoxels( - const PointCloudType& point_cloud, - mapping_3d::HybridGridBase* voxels) { + const PointCloudType& point_cloud, mapping::HybridGridBase* voxels) { PointCloudType results; for (const typename PointCloudType::value_type& point : point_cloud) { auto* const value = diff --git a/cartographer/sensor/voxel_filter.h b/cartographer/sensor/voxel_filter.h index 8ecb23e..da7d192 100644 --- a/cartographer/sensor/voxel_filter.h +++ b/cartographer/sensor/voxel_filter.h @@ -43,7 +43,7 @@ class VoxelFilter { TimedPointCloud Filter(const TimedPointCloud& timed_point_cloud); private: - mapping_3d::HybridGridBase voxels_; + mapping::HybridGridBase voxels_; }; proto::AdaptiveVoxelFilterOptions CreateAdaptiveVoxelFilterOptions( diff --git a/cartographer_grpc/map_builder_context.cc b/cartographer_grpc/map_builder_context.cc index 081c514..b6ec4bc 100644 --- a/cartographer_grpc/map_builder_context.cc +++ b/cartographer_grpc/map_builder_context.cc @@ -103,23 +103,23 @@ MapBuilderContext::UpdateSubmap2D( return submap_2d_ptr; } -std::shared_ptr +std::shared_ptr MapBuilderContext::UpdateSubmap3D( const cartographer::mapping::proto::Submap& proto) { CHECK(proto.has_submap_3d()); cartographer::mapping::SubmapId submap_id{proto.submap_id().trajectory_id(), proto.submap_id().submap_index()}; - std::shared_ptr submap_3d_ptr; + std::shared_ptr submap_3d_ptr; auto submap_it = unfinished_submaps_.find(submap_id); if (submap_it == unfinished_submaps_.end()) { // Seeing a submap for the first time it should never be finished. CHECK(!proto.submap_3d().finished()); submap_3d_ptr = - std::make_shared(proto.submap_3d()); + std::make_shared(proto.submap_3d()); unfinished_submaps_.Insert(submap_id, submap_3d_ptr); submap_it = unfinished_submaps_.find(submap_id); } else { - submap_3d_ptr = std::dynamic_pointer_cast( + submap_3d_ptr = std::dynamic_pointer_cast( submap_it->data); CHECK(submap_3d_ptr); @@ -157,8 +157,7 @@ MapBuilderContext::ProcessLocalSlamResultData( cartographer::mapping::FromProto(proto.node_data())), submaps); } else { - std::vector> - submaps; + std::vector> submaps; for (const auto& submap_proto : proto.submaps()) { submaps.push_back(UpdateSubmap3D(submap_proto)); } diff --git a/cartographer_grpc/map_builder_context.h b/cartographer_grpc/map_builder_context.h index 8ebb207..411ee05 100644 --- a/cartographer_grpc/map_builder_context.h +++ b/cartographer_grpc/map_builder_context.h @@ -18,7 +18,7 @@ #define CARTOGRAPHER_GRPC_MAP_BUILDER_CONTEXT_H #include "cartographer/mapping_2d/submap_2d.h" -#include "cartographer/mapping_3d/submaps.h" +#include "cartographer/mapping_3d/submap_3d.h" #include "cartographer_grpc/map_builder_context_interface.h" namespace cartographer_grpc { @@ -53,7 +53,7 @@ class MapBuilderContext : public MapBuilderContextInterface { private: std::shared_ptr UpdateSubmap2D( const cartographer::mapping::proto::Submap& proto); - std::shared_ptr UpdateSubmap3D( + std::shared_ptr UpdateSubmap3D( const cartographer::mapping::proto::Submap& proto); MapBuilderServer* map_builder_server_;