From f8dc89d8ffa158e20cfdc9a3fc89ef0bb261d4ba Mon Sep 17 00:00:00 2001 From: Alexander Belyaev <32522095+pifon2a@users.noreply.github.com> Date: Wed, 21 Feb 2018 12:41:14 +0100 Subject: [PATCH] Remove 'mapping_2d' namespace. (#922) It is removed from everywhere but 'scan_matching', 'pose_graph' subfolders of 'mapping_2d'. [Code structure RFC](https://github.com/pifon2a/rfcs/blob/e11bca586f8cc733063ea9c94bdade71086ed9ea/text/0000-code-structure.md) --- .../mapping_2d/local_trajectory_builder.cc | 9 +-- .../mapping_2d/local_trajectory_builder.h | 12 ++-- .../occupied_space_cost_function.h | 12 ++-- .../io/probability_grid_points_processor.cc | 23 ++++---- .../io/probability_grid_points_processor.h | 15 +++-- cartographer/io/submap_painter.cc | 5 +- cartographer/io/submap_painter.h | 4 +- .../mapping/local_slam_result_data.cc | 2 +- cartographer/mapping/local_slam_result_data.h | 5 +- cartographer/mapping/proto/submap.proto | 4 +- .../proto/trajectory_builder_options.proto | 5 +- .../mapping/trajectory_builder_interface.cc | 4 +- ...=> local_trajectory_builder_options_2d.cc} | 18 +++--- ... => local_trajectory_builder_options_2d.h} | 14 ++--- cartographer/mapping_2d/map_limits.h | 4 +- cartographer/mapping_2d/map_limits_test.cc | 4 +- .../pose_graph/constraint_builder.cc | 14 +++-- .../pose_graph/constraint_builder.h | 19 +++---- cartographer/mapping_2d/pose_graph_2d.cc | 12 ++-- cartographer/mapping_2d/pose_graph_2d.h | 19 ++++--- cartographer/mapping_2d/pose_graph_2d_test.cc | 12 ++-- cartographer/mapping_2d/probability_grid.cc | 32 +++++------ cartographer/mapping_2d/probability_grid.h | 4 +- .../mapping_2d/probability_grid_test.cc | 38 ++++++------- .../mapping_2d/proto/cell_limits.proto | 2 +- ...local_trajectory_builder_options_2d.proto} | 20 +++---- .../mapping_2d/proto/map_limits.proto | 2 +- .../mapping_2d/proto/probability_grid.proto | 2 +- ...o => range_data_inserter_options_2d.proto} | 4 +- ...options.proto => submaps_options_2d.proto} | 8 +-- ..._inserter.cc => range_data_inserter_2d.cc} | 27 ++++----- ...ta_inserter.h => range_data_inserter_2d.h} | 25 +++++---- ...test.cc => range_data_inserter_2d_test.cc} | 26 ++++----- cartographer/mapping_2d/ray_casting.cc | 5 +- cartographer/mapping_2d/ray_casting.h | 4 +- .../scan_matching/ceres_scan_matcher.cc | 2 +- .../scan_matching/ceres_scan_matcher.h | 2 +- .../scan_matching/ceres_scan_matcher_test.cc | 6 +- .../scan_matching/correlative_scan_matcher.cc | 5 +- .../scan_matching/correlative_scan_matcher.h | 5 +- .../correlative_scan_matcher_test.cc | 4 +- .../fast_correlative_scan_matcher.cc | 11 ++-- .../fast_correlative_scan_matcher.h | 10 ++-- .../fast_correlative_scan_matcher_test.cc | 44 ++++++++------- .../real_time_correlative_scan_matcher.cc | 4 +- .../real_time_correlative_scan_matcher.h | 4 +- ...real_time_correlative_scan_matcher_test.cc | 15 ++--- .../mapping_2d/{submaps.cc => submap_2d.cc} | 56 +++++++++---------- .../mapping_2d/{submaps.h => submap_2d.h} | 49 ++++++++-------- .../{submaps_test.cc => submap_2d_test.cc} | 23 ++++---- cartographer/mapping_2d/xy_index.h | 4 +- cartographer/mapping_2d/xy_index_test.cc | 4 +- cartographer/mapping_3d/submaps.h | 1 - cartographer_grpc/map_builder_context.cc | 11 ++-- cartographer_grpc/map_builder_context.h | 4 +- 55 files changed, 337 insertions(+), 342 deletions(-) rename cartographer/mapping_2d/{local_trajectory_builder_options.cc => local_trajectory_builder_options_2d.cc} (87%) rename cartographer/mapping_2d/{local_trajectory_builder_options.h => local_trajectory_builder_options_2d.h} (86%) rename cartographer/mapping_2d/proto/{local_trajectory_builder_options.proto => local_trajectory_builder_options_2d.proto} (82%) rename cartographer/mapping_2d/proto/{range_data_inserter_options.proto => range_data_inserter_options_2d.proto} (93%) rename cartographer/mapping_2d/proto/{submaps_options.proto => submaps_options_2d.proto} (87%) rename cartographer/mapping_2d/{range_data_inserter.cc => range_data_inserter_2d.cc} (72%) rename cartographer/mapping_2d/{range_data_inserter.h => range_data_inserter_2d.h} (69%) rename cartographer/mapping_2d/{range_data_inserter_test.cc => range_data_inserter_2d_test.cc} (88%) rename cartographer/mapping_2d/{submaps.cc => submap_2d.cc} (80%) rename cartographer/mapping_2d/{submaps.h => submap_2d.h} (68%) rename cartographer/mapping_2d/{submaps_test.cc => submap_2d_test.cc} (85%) diff --git a/cartographer/internal/mapping_2d/local_trajectory_builder.cc b/cartographer/internal/mapping_2d/local_trajectory_builder.cc index 076f497..d76cd5c 100644 --- a/cartographer/internal/mapping_2d/local_trajectory_builder.cc +++ b/cartographer/internal/mapping_2d/local_trajectory_builder.cc @@ -26,7 +26,7 @@ namespace cartographer { namespace mapping_2d { LocalTrajectoryBuilder::LocalTrajectoryBuilder( - const proto::LocalTrajectoryBuilderOptions& options) + const mapping::proto::LocalTrajectoryBuilderOptions2D& options) : options_(options), active_submaps_(options.submaps_options()), motion_filter_(options_.motion_filter_options()), @@ -53,7 +53,7 @@ LocalTrajectoryBuilder::TransformToGravityAlignedFrameAndFilter( std::unique_ptr LocalTrajectoryBuilder::ScanMatch( const common::Time time, const transform::Rigid2d& pose_prediction, const sensor::RangeData& gravity_aligned_range_data) { - std::shared_ptr matching_submap = + std::shared_ptr matching_submap = active_submaps_.submaps().front(); // The online correlative scan matcher will refine the initial estimate for // the Ceres scan matcher. @@ -205,8 +205,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(range_data_in_local); diff --git a/cartographer/internal/mapping_2d/local_trajectory_builder.h b/cartographer/internal/mapping_2d/local_trajectory_builder.h index 670df42..0f8e1d6 100644 --- a/cartographer/internal/mapping_2d/local_trajectory_builder.h +++ b/cartographer/internal/mapping_2d/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_2d/proto/local_trajectory_builder_options.pb.h" +#include "cartographer/mapping_2d/proto/local_trajectory_builder_options_2d.pb.h" #include "cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h" #include "cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.h" -#include "cartographer/mapping_2d/submaps.h" +#include "cartographer/mapping_2d/submap_2d.h" #include "cartographer/sensor/imu_data.h" #include "cartographer/sensor/odometry_data.h" #include "cartographer/sensor/range_data.h" @@ -42,7 +42,7 @@ class LocalTrajectoryBuilder { public: struct InsertionResult { std::shared_ptr constant_data; - std::vector> insertion_submaps; + std::vector> insertion_submaps; }; struct MatchingResult { common::Time time; @@ -53,7 +53,7 @@ class LocalTrajectoryBuilder { }; explicit LocalTrajectoryBuilder( - const proto::LocalTrajectoryBuilderOptions& options); + const mapping::proto::LocalTrajectoryBuilderOptions2D& options); ~LocalTrajectoryBuilder(); LocalTrajectoryBuilder(const LocalTrajectoryBuilder&) = delete; @@ -92,8 +92,8 @@ class LocalTrajectoryBuilder { // Lazily constructs a PoseExtrapolator. void InitializeExtrapolator(common::Time time); - const proto::LocalTrajectoryBuilderOptions options_; - ActiveSubmaps active_submaps_; + const mapping::proto::LocalTrajectoryBuilderOptions2D options_; + mapping::ActiveSubmaps2D active_submaps_; mapping::MotionFilter motion_filter_; scan_matching::RealTimeCorrelativeScanMatcher diff --git a/cartographer/internal/mapping_2d/scan_matching/occupied_space_cost_function.h b/cartographer/internal/mapping_2d/scan_matching/occupied_space_cost_function.h index f2b7b15..deb2b8c 100644 --- a/cartographer/internal/mapping_2d/scan_matching/occupied_space_cost_function.h +++ b/cartographer/internal/mapping_2d/scan_matching/occupied_space_cost_function.h @@ -36,7 +36,7 @@ class OccupiedSpaceCostFunction { public: static ceres::CostFunction* CreateAutoDiffCostFunction( const double scaling_factor, const sensor::PointCloud& point_cloud, - const ProbabilityGrid& probability_grid) { + const mapping::ProbabilityGrid& probability_grid) { return new ceres::AutoDiffCostFunction( @@ -55,7 +55,7 @@ class OccupiedSpaceCostFunction { const GridArrayAdapter adapter(probability_grid_); ceres::BiCubicInterpolator interpolator(adapter); - const MapLimits& limits = probability_grid_.limits(); + const mapping::MapLimits& limits = probability_grid_.limits(); for (size_t i = 0; i < point_cloud_.size(); ++i) { // Note that this is a 2D point. The third component is a scaling factor. @@ -79,7 +79,7 @@ class OccupiedSpaceCostFunction { public: enum { DATA_DIMENSION = 1 }; - explicit GridArrayAdapter(const ProbabilityGrid& probability_grid) + explicit GridArrayAdapter(const mapping::ProbabilityGrid& probability_grid) : probability_grid_(probability_grid) {} void GetValue(const int row, const int column, double* const value) const { @@ -103,12 +103,12 @@ class OccupiedSpaceCostFunction { } private: - const ProbabilityGrid& probability_grid_; + const mapping::ProbabilityGrid& probability_grid_; }; OccupiedSpaceCostFunction(const double scaling_factor, const sensor::PointCloud& point_cloud, - const ProbabilityGrid& probability_grid) + const mapping::ProbabilityGrid& probability_grid) : scaling_factor_(scaling_factor), point_cloud_(point_cloud), probability_grid_(probability_grid) {} @@ -119,7 +119,7 @@ class OccupiedSpaceCostFunction { const double scaling_factor_; const sensor::PointCloud& point_cloud_; - const ProbabilityGrid& probability_grid_; + const mapping::ProbabilityGrid& probability_grid_; }; } // namespace scan_matching diff --git a/cartographer/io/probability_grid_points_processor.cc b/cartographer/io/probability_grid_points_processor.cc index c6e3ed9..799abda 100644 --- a/cartographer/io/probability_grid_points_processor.cc +++ b/cartographer/io/probability_grid_points_processor.cc @@ -30,7 +30,7 @@ namespace io { namespace { void DrawTrajectoriesIntoImage( - const mapping_2d::ProbabilityGrid& probability_grid, + const mapping::ProbabilityGrid& probability_grid, const Eigen::Array2i& offset, const std::vector& trajectories, cairo_surface_t* cairo_surface) { @@ -57,7 +57,7 @@ uint8 ProbabilityToColor(float probability_from_grid) { ProbabilityGridPointsProcessor::ProbabilityGridPointsProcessor( const double resolution, - const mapping_2d::proto::RangeDataInserterOptions& + const mapping::proto::RangeDataInserterOptions2D& range_data_inserter_options, const DrawTrajectories& draw_trajectories, std::unique_ptr file_writer, @@ -82,7 +82,7 @@ ProbabilityGridPointsProcessor::FromDictionary( : DrawTrajectories::kNo; return common::make_unique( dictionary->GetDouble("resolution"), - mapping_2d::CreateRangeDataInserterOptions( + mapping::CreateRangeDataInserterOptions2D( dictionary->GetDictionary("range_data_inserter").get()), draw_trajectories, file_writer_factory(dictionary->GetString("filename") + ".png"), @@ -125,9 +125,8 @@ PointsProcessor::FlushResult ProbabilityGridPointsProcessor::Flush() { } std::unique_ptr DrawProbabilityGrid( - const mapping_2d::ProbabilityGrid& probability_grid, - Eigen::Array2i* offset) { - mapping_2d::CellLimits cell_limits; + const mapping::ProbabilityGrid& probability_grid, Eigen::Array2i* offset) { + mapping::CellLimits cell_limits; probability_grid.ComputeCroppedLimits(offset, &cell_limits); if (cell_limits.num_x_cells == 0 || cell_limits.num_y_cells == 0) { LOG(WARNING) << "Not writing output: empty probability grid"; @@ -136,7 +135,7 @@ std::unique_ptr DrawProbabilityGrid( auto image = common::make_unique(cell_limits.num_x_cells, cell_limits.num_y_cells); for (const Eigen::Array2i& xy_index : - cartographer::mapping_2d::XYIndexRangeIterator(cell_limits)) { + mapping::XYIndexRangeIterator(cell_limits)) { const Eigen::Array2i index = xy_index + *offset; constexpr uint8 kUnknownValue = 128; const uint8 value = @@ -149,14 +148,14 @@ std::unique_ptr DrawProbabilityGrid( return image; } -mapping_2d::ProbabilityGrid CreateProbabilityGrid(const double resolution) { +mapping::ProbabilityGrid CreateProbabilityGrid(const double resolution) { constexpr int kInitialProbabilityGridSize = 100; Eigen::Vector2d max = 0.5 * kInitialProbabilityGridSize * resolution * Eigen::Vector2d::Ones(); - return mapping_2d::ProbabilityGrid(cartographer::mapping_2d::MapLimits( - resolution, max, - mapping_2d::CellLimits(kInitialProbabilityGridSize, - kInitialProbabilityGridSize))); + return mapping::ProbabilityGrid( + mapping::MapLimits(resolution, max, + mapping::CellLimits(kInitialProbabilityGridSize, + kInitialProbabilityGridSize))); } } // namespace io diff --git a/cartographer/io/probability_grid_points_processor.h b/cartographer/io/probability_grid_points_processor.h index 091e4ef..fac51b3 100644 --- a/cartographer/io/probability_grid_points_processor.h +++ b/cartographer/io/probability_grid_points_processor.h @@ -25,8 +25,8 @@ #include "cartographer/io/points_processor.h" #include "cartographer/mapping/proto/trajectory.pb.h" #include "cartographer/mapping_2d/probability_grid.h" -#include "cartographer/mapping_2d/proto/range_data_inserter_options.pb.h" -#include "cartographer/mapping_2d/range_data_inserter.h" +#include "cartographer/mapping_2d/proto/range_data_inserter_options_2d.pb.h" +#include "cartographer/mapping_2d/range_data_inserter_2d.h" namespace cartographer { namespace io { @@ -42,7 +42,7 @@ class ProbabilityGridPointsProcessor : public PointsProcessor { enum class DrawTrajectories { kNo, kYes }; ProbabilityGridPointsProcessor( double resolution, - const mapping_2d::proto::RangeDataInserterOptions& + const mapping::proto::RangeDataInserterOptions2D& range_data_inserter_options, const DrawTrajectories& draw_trajectories, std::unique_ptr file_writer, @@ -68,19 +68,18 @@ class ProbabilityGridPointsProcessor : public PointsProcessor { const std::vector trajectories_; std::unique_ptr file_writer_; PointsProcessor* const next_; - mapping_2d::RangeDataInserter range_data_inserter_; - mapping_2d::ProbabilityGrid probability_grid_; + mapping::RangeDataInserter2D range_data_inserter_; + mapping::ProbabilityGrid probability_grid_; }; // Draws 'probability_grid' into an image and fills in 'offset' with the cropped // map limits. Returns 'nullptr' if probability_grid was empty. std::unique_ptr DrawProbabilityGrid( - const mapping_2d::ProbabilityGrid& probability_grid, - Eigen::Array2i* offset); + const mapping::ProbabilityGrid& probability_grid, Eigen::Array2i* offset); // Create an initially empty probability grid with 'resolution' and a small // size, suitable for a PointsBatchProcessor. -mapping_2d::ProbabilityGrid CreateProbabilityGrid(const double resolution); +mapping::ProbabilityGrid CreateProbabilityGrid(const double resolution); } // namespace io } // namespace cartographer diff --git a/cartographer/io/submap_painter.cc b/cartographer/io/submap_painter.cc index 6c446d8..5cc46ce 100644 --- a/cartographer/io/submap_painter.cc +++ b/cartographer/io/submap_painter.cc @@ -16,6 +16,9 @@ #include "cartographer/io/submap_painter.h" +#include "cartographer/mapping_2d/submap_2d.h" +#include "cartographer/mapping_3d/submaps.h" + namespace cartographer { namespace io { namespace { @@ -115,7 +118,7 @@ void FillSubmapSlice( local_pose = submap.local_pose(); submap.ToResponseProto(global_submap_pose, &response); } else { - ::cartographer::mapping_2d::Submap submap(proto.submap_2d()); + ::cartographer::mapping::Submap2D submap(proto.submap_2d()); local_pose = submap.local_pose(); submap.ToResponseProto(global_submap_pose, &response); } diff --git a/cartographer/io/submap_painter.h b/cartographer/io/submap_painter.h index f9d5d36..9bbcab9 100644 --- a/cartographer/io/submap_painter.h +++ b/cartographer/io/submap_painter.h @@ -21,9 +21,7 @@ #include "cairo/cairo.h" #include "cartographer/io/image.h" #include "cartographer/mapping/id.h" -#include "cartographer/mapping/proto/submap.pb.h" -#include "cartographer/mapping_2d/submaps.h" -#include "cartographer/mapping_3d/submaps.h" +#include "cartographer/mapping/proto/serialization.pb.h" #include "cartographer/transform/rigid_transform.h" namespace cartographer { diff --git a/cartographer/mapping/local_slam_result_data.cc b/cartographer/mapping/local_slam_result_data.cc index 8c661a3..c8b29c2 100644 --- a/cartographer/mapping/local_slam_result_data.cc +++ b/cartographer/mapping/local_slam_result_data.cc @@ -27,7 +27,7 @@ LocalSlamResultData::LocalSlamResultData(const std::string& sensor_id, LocalSlamResult2D::LocalSlamResult2D( 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 c0255b4..f350f67 100644 --- a/cartographer/mapping/local_slam_result_data.h +++ b/cartographer/mapping/local_slam_result_data.h @@ -42,8 +42,7 @@ class LocalSlamResult2D : public LocalSlamResultData { LocalSlamResult2D( 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; @@ -51,7 +50,7 @@ class LocalSlamResult2D : public LocalSlamResultData { private: std::shared_ptr node_data_; - std::vector> insertion_submaps_; + std::vector> insertion_submaps_; }; class LocalSlamResult3D : public LocalSlamResultData { diff --git a/cartographer/mapping/proto/submap.proto b/cartographer/mapping/proto/submap.proto index 57925d2..40ead20 100644 --- a/cartographer/mapping/proto/submap.proto +++ b/cartographer/mapping/proto/submap.proto @@ -20,12 +20,12 @@ import "cartographer/mapping_2d/proto/probability_grid.proto"; import "cartographer/mapping_3d/proto/hybrid_grid.proto"; import "cartographer/transform/proto/transform.proto"; -// Serialized state of a mapping_2d::Submap. +// Serialized state of a Submap2D. message Submap2D { transform.proto.Rigid3d local_pose = 1; int32 num_range_data = 2; bool finished = 3; - mapping_2d.proto.ProbabilityGrid probability_grid = 4; + mapping.proto.ProbabilityGrid probability_grid = 4; } // Serialized state of a mapping_3d::Submap. diff --git a/cartographer/mapping/proto/trajectory_builder_options.proto b/cartographer/mapping/proto/trajectory_builder_options.proto index 0b26119..0b05091 100644 --- a/cartographer/mapping/proto/trajectory_builder_options.proto +++ b/cartographer/mapping/proto/trajectory_builder_options.proto @@ -15,7 +15,7 @@ syntax = "proto3"; import "cartographer/transform/proto/transform.proto"; -import "cartographer/mapping_2d/proto/local_trajectory_builder_options.proto"; +import "cartographer/mapping_2d/proto/local_trajectory_builder_options_2d.proto"; import "cartographer/mapping_3d/proto/local_trajectory_builder_options.proto"; package cartographer.mapping.proto; @@ -27,8 +27,7 @@ message InitialTrajectoryPose { } message TrajectoryBuilderOptions { - mapping_2d.proto.LocalTrajectoryBuilderOptions - trajectory_builder_2d_options = 1; + LocalTrajectoryBuilderOptions2D trajectory_builder_2d_options = 1; mapping_3d.proto.LocalTrajectoryBuilderOptions trajectory_builder_3d_options = 2; bool pure_localization = 3; diff --git a/cartographer/mapping/trajectory_builder_interface.cc b/cartographer/mapping/trajectory_builder_interface.cc index 29789f9..5f016bb 100644 --- a/cartographer/mapping/trajectory_builder_interface.cc +++ b/cartographer/mapping/trajectory_builder_interface.cc @@ -17,7 +17,7 @@ #include "cartographer/mapping/trajectory_builder_interface.h" #include "cartographer/mapping/local_slam_result_data.h" -#include "cartographer/mapping_2d/local_trajectory_builder_options.h" +#include "cartographer/mapping_2d/local_trajectory_builder_options_2d.h" #include "cartographer/mapping_3d/local_trajectory_builder_options.h" namespace cartographer { @@ -27,7 +27,7 @@ proto::TrajectoryBuilderOptions CreateTrajectoryBuilderOptions( common::LuaParameterDictionary* const parameter_dictionary) { proto::TrajectoryBuilderOptions options; *options.mutable_trajectory_builder_2d_options() = - mapping_2d::CreateLocalTrajectoryBuilderOptions( + CreateLocalTrajectoryBuilderOptions2D( parameter_dictionary->GetDictionary("trajectory_builder_2d").get()); *options.mutable_trajectory_builder_3d_options() = mapping_3d::CreateLocalTrajectoryBuilderOptions( diff --git a/cartographer/mapping_2d/local_trajectory_builder_options.cc b/cartographer/mapping_2d/local_trajectory_builder_options_2d.cc similarity index 87% rename from cartographer/mapping_2d/local_trajectory_builder_options.cc rename to cartographer/mapping_2d/local_trajectory_builder_options_2d.cc index 40eda3e..6bd04cb 100644 --- a/cartographer/mapping_2d/local_trajectory_builder_options.cc +++ b/cartographer/mapping_2d/local_trajectory_builder_options_2d.cc @@ -14,20 +14,20 @@ * limitations under the License. */ -#include "cartographer/mapping_2d/local_trajectory_builder_options.h" +#include "cartographer/mapping_2d/local_trajectory_builder_options_2d.h" #include "cartographer/internal/mapping/motion_filter.h" #include "cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h" #include "cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.h" -#include "cartographer/mapping_2d/submaps.h" +#include "cartographer/mapping_2d/submap_2d.h" #include "cartographer/sensor/voxel_filter.h" namespace cartographer { -namespace mapping_2d { +namespace mapping { -proto::LocalTrajectoryBuilderOptions CreateLocalTrajectoryBuilderOptions( +proto::LocalTrajectoryBuilderOptions2D CreateLocalTrajectoryBuilderOptions2D( common::LuaParameterDictionary* const parameter_dictionary) { - proto::LocalTrajectoryBuilderOptions options; + proto::LocalTrajectoryBuilderOptions2D options; options.set_min_range(parameter_dictionary->GetDouble("min_range")); options.set_max_range(parameter_dictionary->GetDouble("max_range")); options.set_min_z(parameter_dictionary->GetDouble("min_z")); @@ -49,22 +49,22 @@ proto::LocalTrajectoryBuilderOptions CreateLocalTrajectoryBuilderOptions( ->GetDictionary("loop_closure_adaptive_voxel_filter") .get()); *options.mutable_real_time_correlative_scan_matcher_options() = - scan_matching::CreateRealTimeCorrelativeScanMatcherOptions( + mapping_2d::scan_matching::CreateRealTimeCorrelativeScanMatcherOptions( parameter_dictionary ->GetDictionary("real_time_correlative_scan_matcher") .get()); *options.mutable_ceres_scan_matcher_options() = - scan_matching::CreateCeresScanMatcherOptions( + mapping_2d::scan_matching::CreateCeresScanMatcherOptions( parameter_dictionary->GetDictionary("ceres_scan_matcher").get()); *options.mutable_motion_filter_options() = mapping::CreateMotionFilterOptions( parameter_dictionary->GetDictionary("motion_filter").get()); options.set_imu_gravity_time_constant( parameter_dictionary->GetDouble("imu_gravity_time_constant")); - *options.mutable_submaps_options() = CreateSubmapsOptions( + *options.mutable_submaps_options() = CreateSubmapsOptions2D( parameter_dictionary->GetDictionary("submaps").get()); options.set_use_imu_data(parameter_dictionary->GetBool("use_imu_data")); return options; } -} // namespace mapping_2d +} // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping_2d/local_trajectory_builder_options.h b/cartographer/mapping_2d/local_trajectory_builder_options_2d.h similarity index 86% rename from cartographer/mapping_2d/local_trajectory_builder_options.h rename to cartographer/mapping_2d/local_trajectory_builder_options_2d.h index 7f6eb87..68d0f4a 100644 --- a/cartographer/mapping_2d/local_trajectory_builder_options.h +++ b/cartographer/mapping_2d/local_trajectory_builder_options_2d.h @@ -14,19 +14,19 @@ * limitations under the License. */ -#ifndef CARTOGRAPHER_MAPPING_2D_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_ -#define CARTOGRAPHER_MAPPING_2D_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_ +#ifndef CARTOGRAPHER_MAPPING_2D_LOCAL_TRAJECTORY_BUILDER_OPTIONS_2D_H_ +#define CARTOGRAPHER_MAPPING_2D_LOCAL_TRAJECTORY_BUILDER_OPTIONS_2D_H_ #include "cartographer/common/lua_parameter_dictionary.h" -#include "cartographer/mapping_2d/proto/local_trajectory_builder_options.pb.h" +#include "cartographer/mapping_2d/proto/local_trajectory_builder_options_2d.pb.h" namespace cartographer { -namespace mapping_2d { +namespace mapping { -proto::LocalTrajectoryBuilderOptions CreateLocalTrajectoryBuilderOptions( +proto::LocalTrajectoryBuilderOptions2D CreateLocalTrajectoryBuilderOptions2D( common::LuaParameterDictionary* parameter_dictionary); -} // namespace mapping_2d +} // namespace mapping } // namespace cartographer -#endif // CARTOGRAPHER_MAPPING_2D_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_ +#endif // CARTOGRAPHER_MAPPING_2D_LOCAL_TRAJECTORY_BUILDER_OPTIONS_2D_H_ diff --git a/cartographer/mapping_2d/map_limits.h b/cartographer/mapping_2d/map_limits.h index 8f4a033..a91fa06 100644 --- a/cartographer/mapping_2d/map_limits.h +++ b/cartographer/mapping_2d/map_limits.h @@ -33,7 +33,7 @@ #include "glog/logging.h" namespace cartographer { -namespace mapping_2d { +namespace mapping { // Defines the limits of a grid map. This class must remain inlined for // performance reasons. @@ -97,7 +97,7 @@ inline proto::MapLimits ToProto(const MapLimits& map_limits) { return result; } -} // namespace mapping_2d +} // namespace mapping } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_2D_MAP_LIMITS_H_ diff --git a/cartographer/mapping_2d/map_limits_test.cc b/cartographer/mapping_2d/map_limits_test.cc index 350c585..c3a414e 100644 --- a/cartographer/mapping_2d/map_limits_test.cc +++ b/cartographer/mapping_2d/map_limits_test.cc @@ -19,7 +19,7 @@ #include "gtest/gtest.h" namespace cartographer { -namespace mapping_2d { +namespace mapping { namespace { TEST(MapLimitsTest, ToProto) { @@ -63,5 +63,5 @@ TEST(MapLimitsTest, ConstructAndGet) { } } // namespace -} // namespace mapping_2d +} // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping_2d/pose_graph/constraint_builder.cc b/cartographer/mapping_2d/pose_graph/constraint_builder.cc index 1940be7..7d0e0c7 100644 --- a/cartographer/mapping_2d/pose_graph/constraint_builder.cc +++ b/cartographer/mapping_2d/pose_graph/constraint_builder.cc @@ -38,7 +38,7 @@ namespace cartographer { namespace mapping_2d { namespace pose_graph { -transform::Rigid2d ComputeSubmapPose(const Submap& submap) { +transform::Rigid2d ComputeSubmapPose(const mapping::Submap2D& submap) { return transform::Project2D(submap.local_pose()); } @@ -59,7 +59,7 @@ ConstraintBuilder::~ConstraintBuilder() { } void ConstraintBuilder::MaybeAddConstraint( - const mapping::SubmapId& submap_id, const Submap* const submap, + const mapping::SubmapId& submap_id, const mapping::Submap2D* const submap, const mapping::NodeId& node_id, const mapping::TrajectoryNode::Data* const constant_data, const transform::Rigid2d& initial_relative_pose) { @@ -84,7 +84,7 @@ void ConstraintBuilder::MaybeAddConstraint( } void ConstraintBuilder::MaybeAddGlobalConstraint( - const mapping::SubmapId& submap_id, const Submap* const submap, + const mapping::SubmapId& submap_id, const mapping::Submap2D* const submap, const mapping::NodeId& node_id, const mapping::TrajectoryNode::Data* const constant_data) { common::MutexLocker locker(&mutex_); @@ -119,7 +119,8 @@ void ConstraintBuilder::WhenDone( } void ConstraintBuilder::ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( - const mapping::SubmapId& submap_id, const ProbabilityGrid* const submap, + const mapping::SubmapId& submap_id, + const mapping::ProbabilityGrid* const submap, const std::function& work_item) { if (submap_scan_matchers_[submap_id].fast_correlative_scan_matcher != nullptr) { @@ -134,7 +135,8 @@ void ConstraintBuilder::ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( } void ConstraintBuilder::ConstructSubmapScanMatcher( - const mapping::SubmapId& submap_id, const ProbabilityGrid* const submap) { + const mapping::SubmapId& submap_id, + const mapping::ProbabilityGrid* const submap) { auto submap_scan_matcher = common::make_unique( *submap, options_.fast_correlative_scan_matcher_options()); @@ -157,7 +159,7 @@ ConstraintBuilder::GetSubmapScanMatcher(const mapping::SubmapId& submap_id) { } void ConstraintBuilder::ComputeConstraint( - const mapping::SubmapId& submap_id, const Submap* const submap, + const mapping::SubmapId& submap_id, const mapping::Submap2D* const submap, const mapping::NodeId& node_id, bool match_full_submap, const mapping::TrajectoryNode::Data* const constant_data, const transform::Rigid2d& initial_relative_pose, diff --git a/cartographer/mapping_2d/pose_graph/constraint_builder.h b/cartographer/mapping_2d/pose_graph/constraint_builder.h index 8a65816..f8abf65 100644 --- a/cartographer/mapping_2d/pose_graph/constraint_builder.h +++ b/cartographer/mapping_2d/pose_graph/constraint_builder.h @@ -34,9 +34,7 @@ #include "cartographer/mapping/pose_graph_interface.h" #include "cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h" #include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h" -#include "cartographer/mapping_2d/submaps.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_2d/submap_2d.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/voxel_filter.h" @@ -46,7 +44,7 @@ namespace pose_graph { // Returns (map <- submap) where 'submap' is a coordinate system at the origin // of the Submap. -transform::Rigid2d ComputeSubmapPose(const Submap& submap); +transform::Rigid2d ComputeSubmapPose(const mapping::Submap2D& submap); // Asynchronously computes constraints. // @@ -76,7 +74,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::Submap2D* submap, const mapping::NodeId& node_id, const mapping::TrajectoryNode::Data* const constant_data, const transform::Rigid2d& initial_relative_pose); @@ -88,7 +86,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::Submap2D* submap, const mapping::NodeId& node_id, const mapping::TrajectoryNode::Data* const constant_data); @@ -107,7 +105,7 @@ class ConstraintBuilder { private: struct SubmapScanMatcher { - const ProbabilityGrid* probability_grid; + const mapping::ProbabilityGrid* probability_grid; std::unique_ptr fast_correlative_scan_matcher; }; @@ -115,12 +113,13 @@ class ConstraintBuilder { // Either schedules the 'work_item', or if needed, schedules the scan matcher // construction and queues the 'work_item'. void ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( - const mapping::SubmapId& submap_id, const ProbabilityGrid* submap, + const mapping::SubmapId& submap_id, + const mapping::ProbabilityGrid* 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 ProbabilityGrid* submap) + const mapping::ProbabilityGrid* submap) EXCLUDES(mutex_); // Returns the scan matcher for a submap, which has to exist. @@ -131,7 +130,7 @@ class ConstraintBuilder { // constraint, assuming 'submap' and 'compressed_point_cloud' do not change // anymore. As output, it may create a new Constraint in 'constraint'. void ComputeConstraint( - const mapping::SubmapId& submap_id, const Submap* submap, + const mapping::SubmapId& submap_id, const mapping::Submap2D* submap, const mapping::NodeId& node_id, bool match_full_submap, const mapping::TrajectoryNode::Data* const constant_data, const transform::Rigid2d& initial_relative_pose, diff --git a/cartographer/mapping_2d/pose_graph_2d.cc b/cartographer/mapping_2d/pose_graph_2d.cc index e02226f..ec38597 100644 --- a/cartographer/mapping_2d/pose_graph_2d.cc +++ b/cartographer/mapping_2d/pose_graph_2d.cc @@ -53,8 +53,7 @@ PoseGraph2D::~PoseGraph2D() { std::vector PoseGraph2D::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) { @@ -103,8 +102,7 @@ std::vector PoseGraph2D::InitializeGlobalSubmapPoses( NodeId PoseGraph2D::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); @@ -234,7 +232,7 @@ void PoseGraph2D::ComputeConstraintsForOldNodes(const SubmapId& submap_id) { void PoseGraph2D::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( @@ -431,8 +429,8 @@ void PoseGraph2D::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_2d()); + std::shared_ptr submap_ptr = + std::make_shared(submap.submap_2d()); const transform::Rigid2d global_submap_pose_2d = transform::Project2D(global_submap_pose); diff --git a/cartographer/mapping_2d/pose_graph_2d.h b/cartographer/mapping_2d/pose_graph_2d.h index 9ae47ef..a104937 100644 --- a/cartographer/mapping_2d/pose_graph_2d.h +++ b/cartographer/mapping_2d/pose_graph_2d.h @@ -37,7 +37,7 @@ #include "cartographer/mapping/trajectory_connectivity_state.h" #include "cartographer/mapping_2d/pose_graph/constraint_builder.h" #include "cartographer/mapping_2d/pose_graph/optimization_problem.h" -#include "cartographer/mapping_2d/submaps.h" +#include "cartographer/mapping_2d/submap_2d.h" #include "cartographer/sensor/fixed_frame_pose_data.h" #include "cartographer/sensor/landmark_data.h" #include "cartographer/sensor/odometry_data.h" @@ -71,10 +71,11 @@ class PoseGraph2D : 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_); @@ -137,7 +138,7 @@ class PoseGraph2D : 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 PoseGraph2D : 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_2d/pose_graph_2d_test.cc b/cartographer/mapping_2d/pose_graph_2d_test.cc index 76be41f..c080887 100644 --- a/cartographer/mapping_2d/pose_graph_2d_test.cc +++ b/cartographer/mapping_2d/pose_graph_2d_test.cc @@ -24,8 +24,8 @@ #include "cartographer/common/make_unique.h" #include "cartographer/common/thread_pool.h" #include "cartographer/common/time.h" -#include "cartographer/mapping_2d/range_data_inserter.h" -#include "cartographer/mapping_2d/submaps.h" +#include "cartographer/mapping_2d/range_data_inserter_2d.h" +#include "cartographer/mapping_2d/submap_2d.h" #include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/rigid_transform_test_helpers.h" #include "cartographer/transform/transform.h" @@ -58,8 +58,8 @@ class PoseGraph2DTest : public ::testing::Test { miss_probability = 0.495, }, })text"); - active_submaps_ = common::make_unique( - mapping_2d::CreateSubmapsOptions(parameter_dictionary.get())); + active_submaps_ = common::make_unique( + mapping::CreateSubmapsOptions2D(parameter_dictionary.get())); } { @@ -145,7 +145,7 @@ class PoseGraph2DTest : public ::testing::Test { const sensor::PointCloud new_point_cloud = sensor::TransformPointCloud( point_cloud_, transform::Embed3D(current_pose_.inverse().cast())); - std::vector> insertion_submaps; + std::vector> insertion_submaps; for (const auto& submap : active_submaps_->submaps()) { insertion_submaps.push_back(submap); } @@ -178,7 +178,7 @@ class PoseGraph2DTest : public ::testing::Test { } sensor::PointCloud point_cloud_; - std::unique_ptr active_submaps_; + std::unique_ptr active_submaps_; common::ThreadPool thread_pool_; std::unique_ptr pose_graph_; transform::Rigid2d current_pose_; diff --git a/cartographer/mapping_2d/probability_grid.cc b/cartographer/mapping_2d/probability_grid.cc index 8fd341e..472bc43 100644 --- a/cartographer/mapping_2d/probability_grid.cc +++ b/cartographer/mapping_2d/probability_grid.cc @@ -5,7 +5,7 @@ #include "cartographer/mapping/probability_values.h" namespace cartographer { -namespace mapping_2d { +namespace mapping { namespace { // Converts a 'cell_index' into an index into 'cells_'. @@ -20,7 +20,7 @@ ProbabilityGrid::ProbabilityGrid(const MapLimits& limits) : limits_(limits), cells_( limits_.cell_limits().num_x_cells * limits_.cell_limits().num_y_cells, - mapping::kUnknownProbabilityValue) {} + kUnknownProbabilityValue) {} ProbabilityGrid::ProbabilityGrid(const proto::ProbabilityGrid& proto) : limits_(proto.limits()), cells_() { @@ -40,8 +40,8 @@ ProbabilityGrid::ProbabilityGrid(const proto::ProbabilityGrid& proto) // Finishes the update sequence. void ProbabilityGrid::FinishUpdate() { while (!update_indices_.empty()) { - DCHECK_GE(cells_[update_indices_.back()], mapping::kUpdateMarker); - cells_[update_indices_.back()] -= mapping::kUpdateMarker; + DCHECK_GE(cells_[update_indices_.back()], kUpdateMarker); + cells_[update_indices_.back()] -= kUpdateMarker; update_indices_.pop_back(); } } @@ -51,8 +51,8 @@ void ProbabilityGrid::FinishUpdate() { void ProbabilityGrid::SetProbability(const Eigen::Array2i& cell_index, const float probability) { uint16& cell = cells_[ToFlatIndex(cell_index, limits_)]; - CHECK_EQ(cell, mapping::kUnknownProbabilityValue); - cell = mapping::ProbabilityToValue(probability); + CHECK_EQ(cell, kUnknownProbabilityValue); + cell = ProbabilityToValue(probability); known_cells_box_.extend(cell_index.matrix()); } @@ -65,15 +65,15 @@ void ProbabilityGrid::SetProbability(const Eigen::Array2i& cell_index, // will be set to probability corresponding to 'odds'. bool ProbabilityGrid::ApplyLookupTable(const Eigen::Array2i& cell_index, const std::vector& table) { - DCHECK_EQ(table.size(), mapping::kUpdateMarker); + DCHECK_EQ(table.size(), kUpdateMarker); const int flat_index = ToFlatIndex(cell_index, limits_); uint16* cell = &cells_[flat_index]; - if (*cell >= mapping::kUpdateMarker) { + if (*cell >= kUpdateMarker) { return false; } update_indices_.push_back(flat_index); *cell = table[*cell]; - DCHECK_GE(*cell, mapping::kUpdateMarker); + DCHECK_GE(*cell, kUpdateMarker); known_cells_box_.extend(cell_index.matrix()); return true; } @@ -81,17 +81,15 @@ bool ProbabilityGrid::ApplyLookupTable(const Eigen::Array2i& cell_index, // Returns the probability of the cell with 'cell_index'. float ProbabilityGrid::GetProbability(const Eigen::Array2i& cell_index) const { if (limits_.Contains(cell_index)) { - return mapping::ValueToProbability( - cells_[ToFlatIndex(cell_index, limits_)]); + return ValueToProbability(cells_[ToFlatIndex(cell_index, limits_)]); } - return mapping::kMinProbability; + return kMinProbability; } // Returns true if the probability at the specified index is known. bool ProbabilityGrid::IsKnown(const Eigen::Array2i& cell_index) const { return limits_.Contains(cell_index) && - cells_[ToFlatIndex(cell_index, limits_)] != - mapping::kUnknownProbabilityValue; + cells_[ToFlatIndex(cell_index, limits_)] != kUnknownProbabilityValue; } // Fills in 'offset' and 'limits' to define a subregion of that contains all @@ -126,7 +124,7 @@ void ProbabilityGrid::GrowLimits(const Eigen::Vector2f& point) { const int offset = x_offset + stride * y_offset; const int new_size = new_limits.cell_limits().num_x_cells * new_limits.cell_limits().num_y_cells; - std::vector new_cells(new_size, mapping::kUnknownProbabilityValue); + std::vector new_cells(new_size, kUnknownProbabilityValue); for (int i = 0; i < limits_.cell_limits().num_y_cells; ++i) { for (int j = 0; j < limits_.cell_limits().num_x_cells; ++j) { new_cells[offset + j + i * stride] = @@ -143,7 +141,7 @@ void ProbabilityGrid::GrowLimits(const Eigen::Vector2f& point) { proto::ProbabilityGrid ProbabilityGrid::ToProto() const { proto::ProbabilityGrid result; - *result.mutable_limits() = cartographer::mapping_2d::ToProto(limits_); + *result.mutable_limits() = mapping::ToProto(limits_); result.mutable_cells()->Reserve(cells_.size()); for (const auto& cell : cells_) { result.mutable_cells()->Add(cell); @@ -160,5 +158,5 @@ proto::ProbabilityGrid ProbabilityGrid::ToProto() const { return result; } -} // namespace mapping_2d +} // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping_2d/probability_grid.h b/cartographer/mapping_2d/probability_grid.h index 1963ec7..38ffe4a 100644 --- a/cartographer/mapping_2d/probability_grid.h +++ b/cartographer/mapping_2d/probability_grid.h @@ -25,7 +25,7 @@ #include "cartographer/mapping_2d/xy_index.h" namespace cartographer { -namespace mapping_2d { +namespace mapping { // Represents a 2D grid of probabilities. class ProbabilityGrid { @@ -81,7 +81,7 @@ class ProbabilityGrid { Eigen::AlignedBox2i known_cells_box_; }; -} // namespace mapping_2d +} // namespace mapping } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_2D_PROBABILITY_GRID_H_ diff --git a/cartographer/mapping_2d/probability_grid_test.cc b/cartographer/mapping_2d/probability_grid_test.cc index c8a1a75..b9530ea 100644 --- a/cartographer/mapping_2d/probability_grid_test.cc +++ b/cartographer/mapping_2d/probability_grid_test.cc @@ -22,7 +22,7 @@ #include "gtest/gtest.h" namespace cartographer { -namespace mapping_2d { +namespace mapping { namespace { using Eigen::Array2i; @@ -75,35 +75,29 @@ TEST(ProbabilityGridTest, ApplyOdds) { probability_grid.SetProbability(Array2i(1, 0), 0.5); - probability_grid.ApplyLookupTable( - Array2i(1, 0), - mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.9))); + probability_grid.ApplyLookupTable(Array2i(1, 0), + ComputeLookupTableToApplyOdds(Odds(0.9))); probability_grid.FinishUpdate(); EXPECT_GT(probability_grid.GetProbability(Array2i(1, 0)), 0.5); probability_grid.SetProbability(Array2i(0, 1), 0.5); - - probability_grid.ApplyLookupTable( - Array2i(0, 1), - mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.1))); + probability_grid.ApplyLookupTable(Array2i(0, 1), + ComputeLookupTableToApplyOdds(Odds(0.1))); probability_grid.FinishUpdate(); EXPECT_LT(probability_grid.GetProbability(Array2i(0, 1)), 0.5); // Tests adding odds to an unknown cell. - probability_grid.ApplyLookupTable( - Array2i(1, 1), - mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.42))); + probability_grid.ApplyLookupTable(Array2i(1, 1), + ComputeLookupTableToApplyOdds(Odds(0.42))); EXPECT_NEAR(probability_grid.GetProbability(Array2i(1, 1)), 0.42, 1e-4); // Tests that further updates are ignored if FinishUpdate() isn't called. - probability_grid.ApplyLookupTable( - Array2i(1, 1), - mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.9))); + probability_grid.ApplyLookupTable(Array2i(1, 1), + ComputeLookupTableToApplyOdds(Odds(0.9))); EXPECT_NEAR(probability_grid.GetProbability(Array2i(1, 1)), 0.42, 1e-4); probability_grid.FinishUpdate(); - probability_grid.ApplyLookupTable( - Array2i(1, 1), - mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.9))); + probability_grid.ApplyLookupTable(Array2i(1, 1), + ComputeLookupTableToApplyOdds(Odds(0.9))); EXPECT_GT(probability_grid.GetProbability(Array2i(1, 1)), 0.42); } @@ -120,10 +114,10 @@ TEST(ProbabilityGridTest, GetProbability) { ASSERT_EQ(2, cell_limits.num_y_cells); probability_grid.SetProbability(limits.GetCellIndex(Vector2f(-0.5f, 0.5f)), - mapping::kMaxProbability); + kMaxProbability); EXPECT_NEAR(probability_grid.GetProbability( limits.GetCellIndex(Vector2f(-0.5f, 0.5f))), - mapping::kMaxProbability, 1e-6); + kMaxProbability, 1e-6); for (const Array2i& xy_index : {limits.GetCellIndex(Vector2f(-0.5f, 1.5f)), limits.GetCellIndex(Vector2f(0.5f, 0.5f)), limits.GetCellIndex(Vector2f(0.5f, 1.5f))}) { @@ -165,8 +159,8 @@ TEST(ProbabilityGridTest, GetCellIndex) { TEST(ProbabilityGridTest, CorrectCropping) { // Create a probability grid with random values. std::mt19937 rng(42); - std::uniform_real_distribution value_distribution( - mapping::kMinProbability, mapping::kMaxProbability); + std::uniform_real_distribution value_distribution(kMinProbability, + kMaxProbability); ProbabilityGrid probability_grid( MapLimits(0.05, Eigen::Vector2d(10., 10.), CellLimits(400, 400))); for (const Array2i& xy_index : @@ -182,5 +176,5 @@ TEST(ProbabilityGridTest, CorrectCropping) { } } // namespace -} // namespace mapping_2d +} // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping_2d/proto/cell_limits.proto b/cartographer/mapping_2d/proto/cell_limits.proto index 72416bf..6cee237 100644 --- a/cartographer/mapping_2d/proto/cell_limits.proto +++ b/cartographer/mapping_2d/proto/cell_limits.proto @@ -14,7 +14,7 @@ syntax = "proto3"; -package cartographer.mapping_2d.proto; +package cartographer.mapping.proto; message CellLimits { int32 num_x_cells = 1; diff --git a/cartographer/mapping_2d/proto/local_trajectory_builder_options.proto b/cartographer/mapping_2d/proto/local_trajectory_builder_options_2d.proto similarity index 82% rename from cartographer/mapping_2d/proto/local_trajectory_builder_options.proto rename to cartographer/mapping_2d/proto/local_trajectory_builder_options_2d.proto index 9cbac89..d344649 100644 --- a/cartographer/mapping_2d/proto/local_trajectory_builder_options.proto +++ b/cartographer/mapping_2d/proto/local_trajectory_builder_options_2d.proto @@ -14,15 +14,15 @@ syntax = "proto3"; -package cartographer.mapping_2d.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/proto/submaps_options.proto"; +import "cartographer/mapping_2d/proto/submaps_options_2d.proto"; import "cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options.proto"; import "cartographer/mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.proto"; -message LocalTrajectoryBuilderOptions { +message LocalTrajectoryBuilderOptions2D { // Rangefinder points outside these ranges will be dropped. float min_range = 14; float max_range = 15; @@ -41,8 +41,7 @@ message LocalTrajectoryBuilderOptions { float voxel_filter_size = 3; // Voxel filter used to compute a sparser point cloud for matching. - sensor.proto.AdaptiveVoxelFilterOptions - adaptive_voxel_filter_options = 6; + sensor.proto.AdaptiveVoxelFilterOptions adaptive_voxel_filter_options = 6; // Voxel filter used to compute a sparser point cloud for finding loop // closures. @@ -52,11 +51,12 @@ message LocalTrajectoryBuilderOptions { // Whether to solve the online scan matching first using the correlative scan // matcher to generate a good starting point for Ceres. bool use_online_correlative_scan_matching = 5; - scan_matching.proto.RealTimeCorrelativeScanMatcherOptions - real_time_correlative_scan_matcher_options = 7; - scan_matching.proto.CeresScanMatcherOptions + cartographer.mapping_2d.scan_matching.proto + .RealTimeCorrelativeScanMatcherOptions + real_time_correlative_scan_matcher_options = 7; + cartographer.mapping_2d.scan_matching.proto.CeresScanMatcherOptions ceres_scan_matcher_options = 8; - mapping.proto.MotionFilterOptions motion_filter_options = 13; + MotionFilterOptions motion_filter_options = 13; // Time constant in seconds for the orientation moving average based on // observed gravity via the IMU. It should be chosen so that the error @@ -66,7 +66,7 @@ message LocalTrajectoryBuilderOptions { // constant is increased) is balanced. double imu_gravity_time_constant = 17; - mapping_2d.proto.SubmapsOptions submaps_options = 11; + SubmapsOptions2D submaps_options = 11; // True if IMU data should be expected and used. bool use_imu_data = 12; diff --git a/cartographer/mapping_2d/proto/map_limits.proto b/cartographer/mapping_2d/proto/map_limits.proto index e011a65..592f3b7 100644 --- a/cartographer/mapping_2d/proto/map_limits.proto +++ b/cartographer/mapping_2d/proto/map_limits.proto @@ -17,7 +17,7 @@ syntax = "proto3"; import "cartographer/mapping_2d/proto/cell_limits.proto"; import "cartographer/transform/proto/transform.proto"; -package cartographer.mapping_2d.proto; +package cartographer.mapping.proto; message MapLimits { double resolution = 1; diff --git a/cartographer/mapping_2d/proto/probability_grid.proto b/cartographer/mapping_2d/proto/probability_grid.proto index c8f774e..700e31c 100644 --- a/cartographer/mapping_2d/proto/probability_grid.proto +++ b/cartographer/mapping_2d/proto/probability_grid.proto @@ -16,7 +16,7 @@ syntax = "proto3"; import "cartographer/mapping_2d/proto/map_limits.proto"; -package cartographer.mapping_2d.proto; +package cartographer.mapping.proto; message ProbabilityGrid { message CellBox { diff --git a/cartographer/mapping_2d/proto/range_data_inserter_options.proto b/cartographer/mapping_2d/proto/range_data_inserter_options_2d.proto similarity index 93% rename from cartographer/mapping_2d/proto/range_data_inserter_options.proto rename to cartographer/mapping_2d/proto/range_data_inserter_options_2d.proto index db84c6e..67c6288 100644 --- a/cartographer/mapping_2d/proto/range_data_inserter_options.proto +++ b/cartographer/mapping_2d/proto/range_data_inserter_options_2d.proto @@ -14,9 +14,9 @@ syntax = "proto3"; -package cartographer.mapping_2d.proto; +package cartographer.mapping.proto; -message RangeDataInserterOptions { +message RangeDataInserterOptions2D { // 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_2d/proto/submaps_options.proto b/cartographer/mapping_2d/proto/submaps_options_2d.proto similarity index 87% rename from cartographer/mapping_2d/proto/submaps_options.proto rename to cartographer/mapping_2d/proto/submaps_options_2d.proto index 787e50e..d8fa1f4 100644 --- a/cartographer/mapping_2d/proto/submaps_options.proto +++ b/cartographer/mapping_2d/proto/submaps_options_2d.proto @@ -14,11 +14,11 @@ syntax = "proto3"; -import "cartographer/mapping_2d/proto/range_data_inserter_options.proto"; +import "cartographer/mapping_2d/proto/range_data_inserter_options_2d.proto"; -package cartographer.mapping_2d.proto; +package cartographer.mapping.proto; -message SubmapsOptions { +message SubmapsOptions2D { // Resolution of the map in meters. double resolution = 1; @@ -27,5 +27,5 @@ message SubmapsOptions { // matched against, then while being matched. int32 num_range_data = 3; - RangeDataInserterOptions range_data_inserter_options = 5; + RangeDataInserterOptions2D range_data_inserter_options = 5; } diff --git a/cartographer/mapping_2d/range_data_inserter.cc b/cartographer/mapping_2d/range_data_inserter_2d.cc similarity index 72% rename from cartographer/mapping_2d/range_data_inserter.cc rename to cartographer/mapping_2d/range_data_inserter_2d.cc index 24b5486..4bbe288 100644 --- a/cartographer/mapping_2d/range_data_inserter.cc +++ b/cartographer/mapping_2d/range_data_inserter_2d.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/mapping_2d/range_data_inserter.h" +#include "cartographer/mapping_2d/range_data_inserter_2d.h" #include @@ -26,11 +26,11 @@ #include "glog/logging.h" namespace cartographer { -namespace mapping_2d { +namespace mapping { -proto::RangeDataInserterOptions CreateRangeDataInserterOptions( +proto::RangeDataInserterOptions2D CreateRangeDataInserterOptions2D( common::LuaParameterDictionary* const parameter_dictionary) { - proto::RangeDataInserterOptions options; + proto::RangeDataInserterOptions2D options; options.set_hit_probability( parameter_dictionary->GetDouble("hit_probability")); options.set_miss_probability( @@ -44,16 +44,17 @@ proto::RangeDataInserterOptions CreateRangeDataInserterOptions( return options; } -RangeDataInserter::RangeDataInserter( - const proto::RangeDataInserterOptions& options) +RangeDataInserter2D::RangeDataInserter2D( + const proto::RangeDataInserterOptions2D& 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, - ProbabilityGrid* const probability_grid) const { +void RangeDataInserter2D::Insert( + const sensor::RangeData& range_data, + ProbabilityGrid* const probability_grid) const { // By not finishing the update after hits are inserted, we give hits priority // (i.e. no hits will be ignored because of a miss in the same cell). CastRays(range_data, hit_table_, miss_table_, options_.insert_free_space(), @@ -61,5 +62,5 @@ void RangeDataInserter::Insert(const sensor::RangeData& range_data, probability_grid->FinishUpdate(); } -} // namespace mapping_2d +} // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping_2d/range_data_inserter.h b/cartographer/mapping_2d/range_data_inserter_2d.h similarity index 69% rename from cartographer/mapping_2d/range_data_inserter.h rename to cartographer/mapping_2d/range_data_inserter_2d.h index 2b55103..5211a67 100644 --- a/cartographer/mapping_2d/range_data_inserter.h +++ b/cartographer/mapping_2d/range_data_inserter_2d.h @@ -14,8 +14,8 @@ * limitations under the License. */ -#ifndef CARTOGRAPHER_MAPPING_2D_RANGE_DATA_INSERTER_H_ -#define CARTOGRAPHER_MAPPING_2D_RANGE_DATA_INSERTER_H_ +#ifndef CARTOGRAPHER_MAPPING_2D_RANGE_DATA_INSERTER_2D_H_ +#define CARTOGRAPHER_MAPPING_2D_RANGE_DATA_INSERTER_2D_H_ #include #include @@ -23,35 +23,36 @@ #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/port.h" #include "cartographer/mapping_2d/probability_grid.h" -#include "cartographer/mapping_2d/proto/range_data_inserter_options.pb.h" +#include "cartographer/mapping_2d/proto/range_data_inserter_options_2d.pb.h" #include "cartographer/mapping_2d/xy_index.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/range_data.h" namespace cartographer { -namespace mapping_2d { +namespace mapping { -proto::RangeDataInserterOptions CreateRangeDataInserterOptions( +proto::RangeDataInserterOptions2D CreateRangeDataInserterOptions2D( common::LuaParameterDictionary* parameter_dictionary); -class RangeDataInserter { +class RangeDataInserter2D { public: - explicit RangeDataInserter(const proto::RangeDataInserterOptions& options); + explicit RangeDataInserter2D( + const proto::RangeDataInserterOptions2D& options); - RangeDataInserter(const RangeDataInserter&) = delete; - RangeDataInserter& operator=(const RangeDataInserter&) = delete; + RangeDataInserter2D(const RangeDataInserter2D&) = delete; + RangeDataInserter2D& operator=(const RangeDataInserter2D&) = delete; // Inserts 'range_data' into 'probability_grid'. void Insert(const sensor::RangeData& range_data, ProbabilityGrid* probability_grid) const; private: - const proto::RangeDataInserterOptions options_; + const proto::RangeDataInserterOptions2D options_; const std::vector hit_table_; const std::vector miss_table_; }; -} // namespace mapping_2d +} // namespace mapping } // namespace cartographer -#endif // CARTOGRAPHER_MAPPING_2D_RANGE_DATA_INSERTER_H_ +#endif // CARTOGRAPHER_MAPPING_2D_RANGE_DATA_INSERTER_2D_H_ diff --git a/cartographer/mapping_2d/range_data_inserter_test.cc b/cartographer/mapping_2d/range_data_inserter_2d_test.cc similarity index 88% rename from cartographer/mapping_2d/range_data_inserter_test.cc rename to cartographer/mapping_2d/range_data_inserter_2d_test.cc index 33c6303..d53f6d4 100644 --- a/cartographer/mapping_2d/range_data_inserter_test.cc +++ b/cartographer/mapping_2d/range_data_inserter_2d_test.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/mapping_2d/range_data_inserter.h" +#include "cartographer/mapping_2d/range_data_inserter_2d.h" #include @@ -26,12 +26,12 @@ #include "gmock/gmock.h" namespace cartographer { -namespace mapping_2d { +namespace mapping { namespace { -class RangeDataInserterTest : public ::testing::Test { +class RangeDataInserterTest2D : public ::testing::Test { protected: - RangeDataInserterTest() + RangeDataInserterTest2D() : probability_grid_( MapLimits(1., Eigen::Vector2d(1., 5.), CellLimits(5, 5))) { auto parameter_dictionary = common::MakeDictionary( @@ -40,8 +40,8 @@ class RangeDataInserterTest : public ::testing::Test { "hit_probability = 0.7, " "miss_probability = 0.4, " "}"); - options_ = CreateRangeDataInserterOptions(parameter_dictionary.get()); - range_data_inserter_ = common::make_unique(options_); + options_ = CreateRangeDataInserterOptions2D(parameter_dictionary.get()); + range_data_inserter_ = common::make_unique(options_); } void InsertPointCloud() { @@ -57,11 +57,11 @@ class RangeDataInserterTest : public ::testing::Test { } ProbabilityGrid probability_grid_; - std::unique_ptr range_data_inserter_; - proto::RangeDataInserterOptions options_; + std::unique_ptr range_data_inserter_; + proto::RangeDataInserterOptions2D options_; }; -TEST_F(RangeDataInserterTest, InsertPointCloud) { +TEST_F(RangeDataInserterTest2D, InsertPointCloud) { InsertPointCloud(); EXPECT_NEAR(1., probability_grid_.limits().max().x(), 1e-9); @@ -101,7 +101,7 @@ TEST_F(RangeDataInserterTest, InsertPointCloud) { } } -TEST_F(RangeDataInserterTest, ProbabilityProgression) { +TEST_F(RangeDataInserterTest2D, ProbabilityProgression) { InsertPointCloud(); EXPECT_NEAR( options_.hit_probability(), @@ -118,17 +118,17 @@ TEST_F(RangeDataInserterTest, ProbabilityProgression) { InsertPointCloud(); } EXPECT_NEAR( - mapping::kMaxProbability, + kMaxProbability, probability_grid_.GetProbability(probability_grid_.limits().GetCellIndex( Eigen::Vector2f(-3.5f, 0.5f))), 1e-3); EXPECT_NEAR( - mapping::kMinProbability, + kMinProbability, probability_grid_.GetProbability(probability_grid_.limits().GetCellIndex( Eigen::Vector2f(-2.5f, 0.5f))), 1e-3); } } // namespace -} // namespace mapping_2d +} // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping_2d/ray_casting.cc b/cartographer/mapping_2d/ray_casting.cc index b81a591..9d9b3f8 100644 --- a/cartographer/mapping_2d/ray_casting.cc +++ b/cartographer/mapping_2d/ray_casting.cc @@ -17,8 +17,7 @@ #include "cartographer/mapping_2d/ray_casting.h" namespace cartographer { -namespace mapping_2d { - +namespace mapping { namespace { // Factor for subpixel accuracy of start and end point. @@ -203,5 +202,5 @@ void CastRays(const sensor::RangeData& range_data, } } -} // namespace mapping_2d +} // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping_2d/ray_casting.h b/cartographer/mapping_2d/ray_casting.h index e8a4482..f9fe956 100644 --- a/cartographer/mapping_2d/ray_casting.h +++ b/cartographer/mapping_2d/ray_casting.h @@ -26,7 +26,7 @@ #include "cartographer/transform/transform.h" namespace cartographer { -namespace mapping_2d { +namespace mapping { // For each ray in 'range_data', inserts hits and misses into // 'probability_grid'. Hits are handled before misses. @@ -35,7 +35,7 @@ void CastRays(const sensor::RangeData& range_data, const std::vector& miss_table, bool insert_free_space, ProbabilityGrid* probability_grid); -} // namespace mapping_2d +} // namespace mapping } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_2D_RAY_CASTING_H_ diff --git a/cartographer/mapping_2d/scan_matching/ceres_scan_matcher.cc b/cartographer/mapping_2d/scan_matching/ceres_scan_matcher.cc index 889feb6..d87008e 100644 --- a/cartographer/mapping_2d/scan_matching/ceres_scan_matcher.cc +++ b/cartographer/mapping_2d/scan_matching/ceres_scan_matcher.cc @@ -62,7 +62,7 @@ CeresScanMatcher::~CeresScanMatcher() {} void CeresScanMatcher::Match(const Eigen::Vector2d& target_translation, const transform::Rigid2d& initial_pose_estimate, const sensor::PointCloud& point_cloud, - const ProbabilityGrid& probability_grid, + const mapping::ProbabilityGrid& probability_grid, transform::Rigid2d* const pose_estimate, ceres::Solver::Summary* const summary) const { double ceres_pose_estimate[3] = {initial_pose_estimate.translation().x(), diff --git a/cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h b/cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h index 1d89dab..5858865 100644 --- a/cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h +++ b/cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h @@ -49,7 +49,7 @@ class CeresScanMatcher { void Match(const Eigen::Vector2d& target_translation, const transform::Rigid2d& initial_pose_estimate, const sensor::PointCloud& point_cloud, - const ProbabilityGrid& probability_grid, + const mapping::ProbabilityGrid& probability_grid, transform::Rigid2d* pose_estimate, ceres::Solver::Summary* summary) const; diff --git a/cartographer/mapping_2d/scan_matching/ceres_scan_matcher_test.cc b/cartographer/mapping_2d/scan_matching/ceres_scan_matcher_test.cc index f21a9ed..7c2119c 100644 --- a/cartographer/mapping_2d/scan_matching/ceres_scan_matcher_test.cc +++ b/cartographer/mapping_2d/scan_matching/ceres_scan_matcher_test.cc @@ -35,8 +35,8 @@ namespace { class CeresScanMatcherTest : public ::testing::Test { protected: CeresScanMatcherTest() - : probability_grid_( - MapLimits(1., Eigen::Vector2d(10., 10.), CellLimits(20, 20))) { + : probability_grid_(mapping::MapLimits(1., Eigen::Vector2d(10., 10.), + mapping::CellLimits(20, 20))) { probability_grid_.SetProbability( probability_grid_.limits().GetCellIndex(Eigen::Vector2f(-3.5f, 2.5f)), mapping::kMaxProbability); @@ -73,7 +73,7 @@ class CeresScanMatcherTest : public ::testing::Test { << "\nExpected: " << transform::ToProto(expected_pose).DebugString(); } - ProbabilityGrid probability_grid_; + mapping::ProbabilityGrid probability_grid_; sensor::PointCloud point_cloud_; std::unique_ptr ceres_scan_matcher_; }; diff --git a/cartographer/mapping_2d/scan_matching/correlative_scan_matcher.cc b/cartographer/mapping_2d/scan_matching/correlative_scan_matcher.cc index 2f6cda4..9c053e4 100644 --- a/cartographer/mapping_2d/scan_matching/correlative_scan_matcher.cc +++ b/cartographer/mapping_2d/scan_matching/correlative_scan_matcher.cc @@ -71,7 +71,7 @@ SearchParameters::SearchParameters(const int num_linear_perturbations, } void SearchParameters::ShrinkToFit(const std::vector& scans, - const CellLimits& cell_limits) { + const mapping::CellLimits& cell_limits) { CHECK_EQ(scans.size(), num_scans); CHECK_EQ(linear_bounds.size(), num_scans); for (int i = 0; i != num_scans; ++i) { @@ -109,7 +109,8 @@ std::vector GenerateRotatedScans( } std::vector DiscretizeScans( - const MapLimits& map_limits, const std::vector& scans, + const mapping::MapLimits& map_limits, + const std::vector& scans, const Eigen::Translation2f& initial_translation) { std::vector discrete_scans; discrete_scans.reserve(scans.size()); diff --git a/cartographer/mapping_2d/scan_matching/correlative_scan_matcher.h b/cartographer/mapping_2d/scan_matching/correlative_scan_matcher.h index 2a36e46..d2fbf5f 100644 --- a/cartographer/mapping_2d/scan_matching/correlative_scan_matcher.h +++ b/cartographer/mapping_2d/scan_matching/correlative_scan_matcher.h @@ -50,7 +50,7 @@ struct SearchParameters { // Tightens the search window as much as possible. void ShrinkToFit(const std::vector& scans, - const CellLimits& cell_limits); + const mapping::CellLimits& cell_limits); int num_angular_perturbations; double angular_perturbation_step_size; @@ -67,7 +67,8 @@ std::vector GenerateRotatedScans( // Translates and discretizes the rotated scans into a vector of integer // indices. std::vector DiscretizeScans( - const MapLimits& map_limits, const std::vector& scans, + const mapping::MapLimits& map_limits, + const std::vector& scans, const Eigen::Translation2f& initial_translation); // A possible solution. diff --git a/cartographer/mapping_2d/scan_matching/correlative_scan_matcher_test.cc b/cartographer/mapping_2d/scan_matching/correlative_scan_matcher_test.cc index 93f44e0..05b07ac 100644 --- a/cartographer/mapping_2d/scan_matching/correlative_scan_matcher_test.cc +++ b/cartographer/mapping_2d/scan_matching/correlative_scan_matcher_test.cc @@ -79,8 +79,8 @@ TEST(DiscretizeScans, DiscretizeScans) { point_cloud.emplace_back(-0.125f, 0.125f, 0.f); point_cloud.emplace_back(-0.125f, 0.075f, 0.f); point_cloud.emplace_back(-0.125f, 0.025f, 0.f); - const MapLimits map_limits(0.05, Eigen::Vector2d(0.05, 0.25), - CellLimits(6, 6)); + const mapping::MapLimits map_limits(0.05, Eigen::Vector2d(0.05, 0.25), + mapping::CellLimits(6, 6)); const std::vector scans = GenerateRotatedScans(point_cloud, SearchParameters(0, 0, 0., 0.)); const std::vector discrete_scans = diff --git a/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.cc b/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.cc index 823394b..ec7bddc 100644 --- a/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.cc +++ b/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.cc @@ -89,8 +89,9 @@ CreateFastCorrelativeScanMatcherOptions( } PrecomputationGrid::PrecomputationGrid( - const ProbabilityGrid& probability_grid, const CellLimits& limits, - const int width, std::vector* reusable_intermediate_grid) + const mapping::ProbabilityGrid& probability_grid, + const mapping::CellLimits& limits, const int width, + std::vector* reusable_intermediate_grid) : offset_(-width + 1, -width + 1), wide_limits_(limits.num_x_cells + width - 1, limits.num_y_cells + width - 1), @@ -170,13 +171,13 @@ uint8 PrecomputationGrid::ComputeCellValue(const float probability) const { class PrecomputationGridStack { public: PrecomputationGridStack( - const ProbabilityGrid& probability_grid, + const mapping::ProbabilityGrid& probability_grid, const proto::FastCorrelativeScanMatcherOptions& options) { CHECK_GE(options.branch_and_bound_depth(), 1); const int max_width = 1 << (options.branch_and_bound_depth() - 1); precomputation_grids_.reserve(options.branch_and_bound_depth()); std::vector reusable_intermediate_grid; - const CellLimits limits = probability_grid.limits().cell_limits(); + const mapping::CellLimits limits = probability_grid.limits().cell_limits(); reusable_intermediate_grid.reserve((limits.num_x_cells + max_width - 1) * limits.num_y_cells); for (int i = 0; i != options.branch_and_bound_depth(); ++i) { @@ -197,7 +198,7 @@ class PrecomputationGridStack { }; FastCorrelativeScanMatcher::FastCorrelativeScanMatcher( - const ProbabilityGrid& probability_grid, + const mapping::ProbabilityGrid& probability_grid, const proto::FastCorrelativeScanMatcherOptions& options) : options_(options), limits_(probability_grid.limits()), diff --git a/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h b/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h index e0f343e..a92de8f 100644 --- a/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h +++ b/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h @@ -49,8 +49,8 @@ CreateFastCorrelativeScanMatcherOptions( // y0 <= y < y0. class PrecomputationGrid { public: - PrecomputationGrid(const ProbabilityGrid& probability_grid, - const CellLimits& limits, int width, + PrecomputationGrid(const mapping::ProbabilityGrid& probability_grid, + const mapping::CellLimits& limits, int width, std::vector* reusable_intermediate_grid); // Returns a value between 0 and 255 to represent probabilities between @@ -87,7 +87,7 @@ class PrecomputationGrid { const Eigen::Array2i offset_; // Size of the precomputation grid. - const CellLimits wide_limits_; + const mapping::CellLimits wide_limits_; // Probabilites mapped to 0 to 255. std::vector cells_; @@ -99,7 +99,7 @@ class PrecomputationGridStack; class FastCorrelativeScanMatcher { public: FastCorrelativeScanMatcher( - const ProbabilityGrid& probability_grid, + const mapping::ProbabilityGrid& probability_grid, const proto::FastCorrelativeScanMatcherOptions& options); ~FastCorrelativeScanMatcher(); @@ -146,7 +146,7 @@ class FastCorrelativeScanMatcher { int candidate_depth, float min_score) const; const proto::FastCorrelativeScanMatcherOptions options_; - MapLimits limits_; + mapping::MapLimits limits_; std::unique_ptr precomputation_grid_stack_; }; diff --git a/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_test.cc b/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_test.cc index d04d447..4503ff1 100644 --- a/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_test.cc +++ b/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_test.cc @@ -24,7 +24,7 @@ #include "cartographer/common/lua_parameter_dictionary_test_helpers.h" #include "cartographer/mapping_2d/probability_grid.h" -#include "cartographer/mapping_2d/range_data_inserter.h" +#include "cartographer/mapping_2d/range_data_inserter_2d.h" #include "cartographer/transform/rigid_transform_test_helpers.h" #include "cartographer/transform/transform.h" #include "gtest/gtest.h" @@ -39,10 +39,10 @@ TEST(PrecomputationGridTest, CorrectValues) { // represented by uint8 values. std::mt19937 prng(42); std::uniform_int_distribution distribution(0, 255); - ProbabilityGrid probability_grid( - MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(250, 250))); - for (const Eigen::Array2i& xy_index : - XYIndexRangeIterator(Eigen::Array2i(50, 50), Eigen::Array2i(249, 249))) { + mapping::ProbabilityGrid probability_grid(mapping::MapLimits( + 0.05, Eigen::Vector2d(5., 5.), mapping::CellLimits(250, 250))); + for (const Eigen::Array2i& xy_index : mapping::XYIndexRangeIterator( + Eigen::Array2i(50, 50), Eigen::Array2i(249, 249))) { probability_grid.SetProbability( xy_index, PrecomputationGrid::ToProbability(distribution(prng))); } @@ -52,8 +52,8 @@ TEST(PrecomputationGridTest, CorrectValues) { PrecomputationGrid precomputation_grid( probability_grid, probability_grid.limits().cell_limits(), width, &reusable_intermediate_grid); - for (const Eigen::Array2i& xy_index : - XYIndexRangeIterator(probability_grid.limits().cell_limits())) { + for (const Eigen::Array2i& xy_index : mapping::XYIndexRangeIterator( + probability_grid.limits().cell_limits())) { float max_score = -std::numeric_limits::infinity(); for (int y = 0; y != width; ++y) { for (int x = 0; x != width; ++x) { @@ -73,10 +73,10 @@ TEST(PrecomputationGridTest, CorrectValues) { TEST(PrecomputationGridTest, TinyProbabilityGrid) { std::mt19937 prng(42); std::uniform_int_distribution distribution(0, 255); - ProbabilityGrid probability_grid( - MapLimits(0.05, Eigen::Vector2d(0.1, 0.1), CellLimits(4, 4))); + mapping::ProbabilityGrid probability_grid(mapping::MapLimits( + 0.05, Eigen::Vector2d(0.1, 0.1), mapping::CellLimits(4, 4))); for (const Eigen::Array2i& xy_index : - XYIndexRangeIterator(probability_grid.limits().cell_limits())) { + mapping::XYIndexRangeIterator(probability_grid.limits().cell_limits())) { probability_grid.SetProbability( xy_index, PrecomputationGrid::ToProbability(distribution(prng))); } @@ -86,8 +86,8 @@ TEST(PrecomputationGridTest, TinyProbabilityGrid) { PrecomputationGrid precomputation_grid( probability_grid, probability_grid.limits().cell_limits(), width, &reusable_intermediate_grid); - for (const Eigen::Array2i& xy_index : - XYIndexRangeIterator(probability_grid.limits().cell_limits())) { + for (const Eigen::Array2i& xy_index : mapping::XYIndexRangeIterator( + probability_grid.limits().cell_limits())) { float max_score = -std::numeric_limits::infinity(); for (int y = 0; y != width; ++y) { for (int x = 0; x != width; ++x) { @@ -116,21 +116,22 @@ CreateFastCorrelativeScanMatcherTestOptions(const int branch_and_bound_depth) { return CreateFastCorrelativeScanMatcherOptions(parameter_dictionary.get()); } -mapping_2d::proto::RangeDataInserterOptions -CreateRangeDataInserterTestOptions() { +mapping::proto::RangeDataInserterOptions2D +CreateRangeDataInserterTestOptions2D() { auto parameter_dictionary = common::MakeDictionary(R"text( return { insert_free_space = true, hit_probability = 0.7, miss_probability = 0.4, })text"); - return mapping_2d::CreateRangeDataInserterOptions(parameter_dictionary.get()); + return mapping::CreateRangeDataInserterOptions2D(parameter_dictionary.get()); } TEST(FastCorrelativeScanMatcherTest, CorrectPose) { std::mt19937 prng(42); std::uniform_real_distribution distribution(-1.f, 1.f); - RangeDataInserter range_data_inserter(CreateRangeDataInserterTestOptions()); + mapping::RangeDataInserter2D range_data_inserter( + CreateRangeDataInserterTestOptions2D()); constexpr float kMinScore = 0.1f; const auto options = CreateFastCorrelativeScanMatcherTestOptions(3); @@ -147,8 +148,8 @@ TEST(FastCorrelativeScanMatcherTest, CorrectPose) { {2. * distribution(prng), 2. * distribution(prng)}, 0.5 * distribution(prng)); - ProbabilityGrid probability_grid( - MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(200, 200))); + mapping::ProbabilityGrid probability_grid(mapping::MapLimits( + 0.05, Eigen::Vector2d(5., 5.), mapping::CellLimits(200, 200))); range_data_inserter.Insert( sensor::RangeData{ Eigen::Vector3f(expected_pose.translation().x(), @@ -177,7 +178,8 @@ TEST(FastCorrelativeScanMatcherTest, CorrectPose) { TEST(FastCorrelativeScanMatcherTest, FullSubmapMatching) { std::mt19937 prng(42); std::uniform_real_distribution distribution(-1.f, 1.f); - RangeDataInserter range_data_inserter(CreateRangeDataInserterTestOptions()); + mapping::RangeDataInserter2D range_data_inserter( + CreateRangeDataInserterTestOptions2D()); constexpr float kMinScore = 0.1f; const auto options = CreateFastCorrelativeScanMatcherTestOptions(6); @@ -200,8 +202,8 @@ TEST(FastCorrelativeScanMatcherTest, FullSubmapMatching) { 0.5 * distribution(prng)) * perturbation.inverse(); - ProbabilityGrid probability_grid( - MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(200, 200))); + mapping::ProbabilityGrid probability_grid(mapping::MapLimits( + 0.05, Eigen::Vector2d(5., 5.), mapping::CellLimits(200, 200))); range_data_inserter.Insert( sensor::RangeData{ transform::Embed3D(expected_pose * perturbation).translation(), diff --git a/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.cc b/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.cc index 38493a5..31f75c9 100644 --- a/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.cc +++ b/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.cc @@ -91,7 +91,7 @@ RealTimeCorrelativeScanMatcher::GenerateExhaustiveSearchCandidates( double RealTimeCorrelativeScanMatcher::Match( const transform::Rigid2d& initial_pose_estimate, const sensor::PointCloud& point_cloud, - const ProbabilityGrid& probability_grid, + const mapping::ProbabilityGrid& probability_grid, transform::Rigid2d* pose_estimate) const { CHECK_NOTNULL(pose_estimate); @@ -125,7 +125,7 @@ double RealTimeCorrelativeScanMatcher::Match( } void RealTimeCorrelativeScanMatcher::ScoreCandidates( - const ProbabilityGrid& probability_grid, + const mapping::ProbabilityGrid& probability_grid, const std::vector& discrete_scans, const SearchParameters& search_parameters, std::vector* const candidates) const { diff --git a/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.h b/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.h index a28c4e7..5437f60 100644 --- a/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.h +++ b/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.h @@ -69,7 +69,7 @@ class RealTimeCorrelativeScanMatcher { // returns the score. double Match(const transform::Rigid2d& initial_pose_estimate, const sensor::PointCloud& point_cloud, - const ProbabilityGrid& probability_grid, + const mapping::ProbabilityGrid& probability_grid, transform::Rigid2d* pose_estimate) const; // Computes the score for each Candidate in a collection. The cost is computed @@ -77,7 +77,7 @@ class RealTimeCorrelativeScanMatcher { // http://ceres-solver.org/modeling.html // // Visible for testing. - void ScoreCandidates(const ProbabilityGrid& probability_grid, + void ScoreCandidates(const mapping::ProbabilityGrid& probability_grid, const std::vector& discrete_scans, const SearchParameters& search_parameters, std::vector* candidates) const; diff --git a/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_test.cc b/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_test.cc index a099f49..d802370 100644 --- a/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_test.cc +++ b/cartographer/mapping_2d/scan_matching/real_time_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_2d/probability_grid.h" -#include "cartographer/mapping_2d/range_data_inserter.h" +#include "cartographer/mapping_2d/range_data_inserter_2d.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/transform/transform.h" #include "gtest/gtest.h" @@ -36,8 +36,8 @@ namespace { class RealTimeCorrelativeScanMatcherTest : public ::testing::Test { protected: RealTimeCorrelativeScanMatcherTest() - : probability_grid_( - MapLimits(0.05, Eigen::Vector2d(0.05, 0.25), CellLimits(6, 6))) { + : probability_grid_(mapping::MapLimits(0.05, Eigen::Vector2d(0.05, 0.25), + mapping::CellLimits(6, 6))) { { auto parameter_dictionary = common::MakeDictionary( "return { " @@ -45,8 +45,9 @@ class RealTimeCorrelativeScanMatcherTest : public ::testing::Test { "hit_probability = 0.7, " "miss_probability = 0.4, " "}"); - range_data_inserter_ = common::make_unique( - CreateRangeDataInserterOptions(parameter_dictionary.get())); + range_data_inserter_ = common::make_unique( + mapping::CreateRangeDataInserterOptions2D( + parameter_dictionary.get())); } point_cloud_.emplace_back(0.025f, 0.175f, 0.f); point_cloud_.emplace_back(-0.025f, 0.175f, 0.f); @@ -74,8 +75,8 @@ class RealTimeCorrelativeScanMatcherTest : public ::testing::Test { } } - ProbabilityGrid probability_grid_; - std::unique_ptr range_data_inserter_; + mapping::ProbabilityGrid probability_grid_; + std::unique_ptr range_data_inserter_; sensor::PointCloud point_cloud_; std::unique_ptr real_time_correlative_scan_matcher_; diff --git a/cartographer/mapping_2d/submaps.cc b/cartographer/mapping_2d/submap_2d.cc similarity index 80% rename from cartographer/mapping_2d/submaps.cc rename to cartographer/mapping_2d/submap_2d.cc index 9fc5598..1d2d1d6 100644 --- a/cartographer/mapping_2d/submaps.cc +++ b/cartographer/mapping_2d/submap_2d.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/mapping_2d/submaps.h" +#include "cartographer/mapping_2d/submap_2d.h" #include #include @@ -28,7 +28,7 @@ #include "glog/logging.h" namespace cartographer { -namespace mapping_2d { +namespace mapping { ProbabilityGrid ComputeCroppedProbabilityGrid( const ProbabilityGrid& probability_grid) { @@ -49,33 +49,33 @@ ProbabilityGrid ComputeCroppedProbabilityGrid( return cropped_grid; } -proto::SubmapsOptions CreateSubmapsOptions( +proto::SubmapsOptions2D CreateSubmapsOptions2D( common::LuaParameterDictionary* const parameter_dictionary) { - proto::SubmapsOptions options; + proto::SubmapsOptions2D options; options.set_resolution(parameter_dictionary->GetDouble("resolution")); options.set_num_range_data( parameter_dictionary->GetNonNegativeInt("num_range_data")); *options.mutable_range_data_inserter_options() = - CreateRangeDataInserterOptions( + CreateRangeDataInserterOptions2D( parameter_dictionary->GetDictionary("range_data_inserter").get()); CHECK_GT(options.num_range_data(), 0); return options; } -Submap::Submap(const MapLimits& limits, const Eigen::Vector2f& origin) - : mapping::Submap(transform::Rigid3d::Translation( +Submap2D::Submap2D(const MapLimits& limits, const Eigen::Vector2f& origin) + : Submap(transform::Rigid3d::Translation( Eigen::Vector3d(origin.x(), origin.y(), 0.))), probability_grid_(limits) {} -Submap::Submap(const mapping::proto::Submap2D& proto) - : mapping::Submap(transform::ToRigid3(proto.local_pose())), +Submap2D::Submap2D(const proto::Submap2D& proto) + : Submap(transform::ToRigid3(proto.local_pose())), probability_grid_(ProbabilityGrid(proto.probability_grid())) { SetNumRangeData(proto.num_range_data()); SetFinished(proto.finished()); } -void Submap::ToProto(mapping::proto::Submap* const proto, - bool include_probability_grid_data) const { +void Submap2D::ToProto(proto::Submap* const proto, + bool include_probability_grid_data) const { auto* const submap_2d = proto->mutable_submap_2d(); *submap_2d->mutable_local_pose() = transform::ToProto(local_pose()); submap_2d->set_num_range_data(num_range_data()); @@ -85,7 +85,7 @@ void Submap::ToProto(mapping::proto::Submap* const proto, } } -void Submap::UpdateFromProto(const mapping::proto::Submap& proto) { +void Submap2D::UpdateFromProto(const proto::Submap& proto) { CHECK(proto.has_submap_2d()); const auto& submap_2d = proto.submap_2d(); SetNumRangeData(submap_2d.num_range_data()); @@ -95,9 +95,9 @@ void Submap::UpdateFromProto(const mapping::proto::Submap& proto) { } } -void Submap::ToResponseProto( +void Submap2D::ToResponseProto( const transform::Rigid3d&, - mapping::proto::SubmapQuery::Response* const response) const { + proto::SubmapQuery::Response* const response) const { response->set_submap_version(num_range_data()); Eigen::Array2i offset; @@ -114,7 +114,7 @@ void Submap::ToResponseProto( // is currently white, so walls will look too gray. This should be hard to // detect visually for the user, though. const int delta = - 128 - mapping::ProbabilityToLogOddsInteger( + 128 - ProbabilityToLogOddsInteger( probability_grid_.GetProbability(xy_index + offset)); const uint8 alpha = delta > 0 ? 0 : -delta; const uint8 value = delta > 0 ? delta : 0; @@ -126,7 +126,7 @@ void Submap::ToResponseProto( cells.push_back(0); // alpha } } - mapping::proto::SubmapQuery::Response::SubmapTexture* const texture = + proto::SubmapQuery::Response::SubmapTexture* const texture = response->add_textures(); common::FastGzipString(cells, texture->mutable_cells()); @@ -143,20 +143,20 @@ void Submap::ToResponseProto( transform::Rigid3d::Translation(Eigen::Vector3d(max_x, max_y, 0.))); } -void Submap::InsertRangeData(const sensor::RangeData& range_data, - const RangeDataInserter& range_data_inserter) { +void Submap2D::InsertRangeData(const sensor::RangeData& range_data, + const RangeDataInserter2D& range_data_inserter) { CHECK(!finished()); range_data_inserter.Insert(range_data, &probability_grid_); SetNumRangeData(num_range_data() + 1); } -void Submap::Finish() { +void Submap2D::Finish() { CHECK(!finished()); probability_grid_ = ComputeCroppedProbabilityGrid(probability_grid_); SetFinished(true); } -ActiveSubmaps::ActiveSubmaps(const proto::SubmapsOptions& options) +ActiveSubmaps2D::ActiveSubmaps2D(const proto::SubmapsOptions2D& options) : options_(options), range_data_inserter_(options.range_data_inserter_options()) { // We always want to have at least one likelihood field which we can return, @@ -164,7 +164,7 @@ ActiveSubmaps::ActiveSubmaps(const proto::SubmapsOptions& options) AddSubmap(Eigen::Vector2f::Zero()); } -void ActiveSubmaps::InsertRangeData(const sensor::RangeData& range_data) { +void ActiveSubmaps2D::InsertRangeData(const sensor::RangeData& range_data) { for (auto& submap : submaps_) { submap->InsertRangeData(range_data, range_data_inserter_); } @@ -173,27 +173,27 @@ void ActiveSubmaps::InsertRangeData(const sensor::RangeData& range_data) { } } -std::vector> ActiveSubmaps::submaps() const { +std::vector> ActiveSubmaps2D::submaps() const { return submaps_; } -int ActiveSubmaps::matching_index() const { return matching_submap_index_; } +int ActiveSubmaps2D::matching_index() const { return matching_submap_index_; } -void ActiveSubmaps::FinishSubmap() { - Submap* submap = submaps_.front().get(); +void ActiveSubmaps2D::FinishSubmap() { + Submap2D* submap = submaps_.front().get(); submap->Finish(); ++matching_submap_index_; submaps_.erase(submaps_.begin()); } -void ActiveSubmaps::AddSubmap(const Eigen::Vector2f& origin) { +void ActiveSubmaps2D::AddSubmap(const Eigen::Vector2f& origin) { if (submaps_.size() > 1) { // This will crop the finished Submap before inserting a new Submap to // reduce peak memory usage a bit. FinishSubmap(); } constexpr int kInitialSubmapSize = 100; - submaps_.push_back(common::make_unique( + submaps_.push_back(common::make_unique( MapLimits(options_.resolution(), origin.cast() + 0.5 * kInitialSubmapSize * options_.resolution() * @@ -203,5 +203,5 @@ void ActiveSubmaps::AddSubmap(const Eigen::Vector2f& origin) { LOG(INFO) << "Added submap " << matching_submap_index_ + submaps_.size(); } -} // namespace mapping_2d +} // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping_2d/submaps.h b/cartographer/mapping_2d/submap_2d.h similarity index 68% rename from cartographer/mapping_2d/submaps.h rename to cartographer/mapping_2d/submap_2d.h index 67da5d8..2793aaa 100644 --- a/cartographer/mapping_2d/submaps.h +++ b/cartographer/mapping_2d/submap_2d.h @@ -14,8 +14,8 @@ * limitations under the License. */ -#ifndef CARTOGRAPHER_MAPPING_2D_SUBMAPS_H_ -#define CARTOGRAPHER_MAPPING_2D_SUBMAPS_H_ +#ifndef CARTOGRAPHER_MAPPING_2D_SUBMAP_2D_H_ +#define CARTOGRAPHER_MAPPING_2D_SUBMAP_2D_H_ #include #include @@ -28,39 +28,38 @@ #include "cartographer/mapping/trajectory_node.h" #include "cartographer/mapping_2d/map_limits.h" #include "cartographer/mapping_2d/probability_grid.h" -#include "cartographer/mapping_2d/proto/submaps_options.pb.h" -#include "cartographer/mapping_2d/range_data_inserter.h" +#include "cartographer/mapping_2d/proto/submaps_options_2d.pb.h" +#include "cartographer/mapping_2d/range_data_inserter_2d.h" #include "cartographer/sensor/range_data.h" #include "cartographer/transform/rigid_transform.h" namespace cartographer { -namespace mapping_2d { +namespace mapping { ProbabilityGrid ComputeCroppedProbabilityGrid( const ProbabilityGrid& probability_grid); -proto::SubmapsOptions CreateSubmapsOptions( +proto::SubmapsOptions2D CreateSubmapsOptions2D( common::LuaParameterDictionary* parameter_dictionary); -class Submap : public mapping::Submap { +class Submap2D : public Submap { public: - Submap(const MapLimits& limits, const Eigen::Vector2f& origin); - explicit Submap(const mapping::proto::Submap2D& proto); + Submap2D(const MapLimits& limits, const Eigen::Vector2f& origin); + explicit Submap2D(const proto::Submap2D& 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 ProbabilityGrid& probability_grid() const { return probability_grid_; } // 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 RangeDataInserter2D& range_data_inserter); void Finish(); private: @@ -76,12 +75,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 ActiveSubmaps2D { public: - explicit ActiveSubmaps(const proto::SubmapsOptions& options); + explicit ActiveSubmaps2D(const proto::SubmapsOptions2D& options); - ActiveSubmaps(const ActiveSubmaps&) = delete; - ActiveSubmaps& operator=(const ActiveSubmaps&) = delete; + ActiveSubmaps2D(const ActiveSubmaps2D&) = delete; + ActiveSubmaps2D& operator=(const ActiveSubmaps2D&) = delete; // Returns the index of the newest initialized Submap which can be // used for scan-to-map matching. @@ -90,19 +89,19 @@ class ActiveSubmaps { // Inserts 'range_data' into the Submap collection. void InsertRangeData(const sensor::RangeData& range_data); - std::vector> submaps() const; + std::vector> submaps() const; private: void FinishSubmap(); void AddSubmap(const Eigen::Vector2f& origin); - const proto::SubmapsOptions options_; + const proto::SubmapsOptions2D options_; int matching_submap_index_ = 0; - std::vector> submaps_; - RangeDataInserter range_data_inserter_; + std::vector> submaps_; + RangeDataInserter2D range_data_inserter_; }; -} // namespace mapping_2d +} // namespace mapping } // namespace cartographer -#endif // CARTOGRAPHER_MAPPING_2D_SUBMAPS_H_ +#endif // CARTOGRAPHER_MAPPING_2D_SUBMAP_2D_H_ diff --git a/cartographer/mapping_2d/submaps_test.cc b/cartographer/mapping_2d/submap_2d_test.cc similarity index 85% rename from cartographer/mapping_2d/submaps_test.cc rename to cartographer/mapping_2d/submap_2d_test.cc index 9981489..075e1d0 100644 --- a/cartographer/mapping_2d/submaps_test.cc +++ b/cartographer/mapping_2d/submap_2d_test.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/mapping_2d/submaps.h" +#include "cartographer/mapping_2d/submap_2d.h" #include #include @@ -28,10 +28,10 @@ #include "gmock/gmock.h" namespace cartographer { -namespace mapping_2d { +namespace mapping { namespace { -TEST(SubmapsTest, TheRightNumberOfRangeDataAreInserted) { +TEST(Submap2DTest, TheRightNumberOfRangeDataAreInserted) { constexpr int kNumRangeData = 10; auto parameter_dictionary = common::MakeDictionary( "return {" @@ -45,8 +45,8 @@ TEST(SubmapsTest, TheRightNumberOfRangeDataAreInserted) { "miss_probability = 0.495, " "}," "}"); - ActiveSubmaps submaps{CreateSubmapsOptions(parameter_dictionary.get())}; - std::set> all_submaps; + ActiveSubmaps2D submaps{CreateSubmapsOptions2D(parameter_dictionary.get())}; + std::set> all_submaps; for (int i = 0; i != 1000; ++i) { submaps.InsertRangeData({Eigen::Vector3f::Zero(), {}, {}}); // Except for the first, maps should only be returned after enough range @@ -68,14 +68,15 @@ TEST(SubmapsTest, TheRightNumberOfRangeDataAreInserted) { EXPECT_EQ(correct_num_range_data, all_submaps.size() - 2); } -TEST(SubmapsTest, ToFromProto) { - Submap expected(MapLimits(1., Eigen::Vector2d(2., 3.), CellLimits(100, 110)), - Eigen::Vector2f(4.f, 5.f)); - mapping::proto::Submap proto; +TEST(Submap2DTest, ToFromProto) { + Submap2D expected( + MapLimits(1., Eigen::Vector2d(2., 3.), CellLimits(100, 110)), + Eigen::Vector2f(4.f, 5.f)); + proto::Submap proto; expected.ToProto(&proto, true /* include_probability_grid_data */); EXPECT_TRUE(proto.has_submap_2d()); EXPECT_FALSE(proto.has_submap_3d()); - const auto actual = Submap(proto.submap_2d()); + const auto actual = Submap2D(proto.submap_2d()); EXPECT_TRUE(expected.local_pose().translation().isApprox( actual.local_pose().translation(), 1e-6)); EXPECT_TRUE(expected.local_pose().rotation().isApprox( @@ -91,5 +92,5 @@ TEST(SubmapsTest, ToFromProto) { } } // namespace -} // namespace mapping_2d +} // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping_2d/xy_index.h b/cartographer/mapping_2d/xy_index.h index fae2252..1e11629 100644 --- a/cartographer/mapping_2d/xy_index.h +++ b/cartographer/mapping_2d/xy_index.h @@ -29,7 +29,7 @@ #include "glog/logging.h" namespace cartographer { -namespace mapping_2d { +namespace mapping { struct CellLimits { CellLimits() = default; @@ -108,7 +108,7 @@ class XYIndexRangeIterator Eigen::Array2i xy_index_; }; -} // namespace mapping_2d +} // namespace mapping } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_2D_XY_INDEX_H_ diff --git a/cartographer/mapping_2d/xy_index_test.cc b/cartographer/mapping_2d/xy_index_test.cc index 05f9b43..ad750e0 100644 --- a/cartographer/mapping_2d/xy_index_test.cc +++ b/cartographer/mapping_2d/xy_index_test.cc @@ -19,7 +19,7 @@ #include "gtest/gtest.h" namespace cartographer { -namespace mapping_2d { +namespace mapping { namespace { TEST(XYIndexTest, CellLimitsToProto) { @@ -57,5 +57,5 @@ TEST(XYIndexTest, XYIndexRangeIterator) { } } // namespace -} // namespace mapping_2d +} // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping_3d/submaps.h b/cartographer/mapping_3d/submaps.h index 51212f9..6d33a6b 100644 --- a/cartographer/mapping_3d/submaps.h +++ b/cartographer/mapping_3d/submaps.h @@ -27,7 +27,6 @@ #include "cartographer/mapping/proto/serialization.pb.h" #include "cartographer/mapping/proto/submap_visualization.pb.h" #include "cartographer/mapping/submaps.h" -#include "cartographer/mapping_2d/range_data_inserter.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" diff --git a/cartographer_grpc/map_builder_context.cc b/cartographer_grpc/map_builder_context.cc index b176f53..081c514 100644 --- a/cartographer_grpc/map_builder_context.cc +++ b/cartographer_grpc/map_builder_context.cc @@ -70,22 +70,22 @@ void MapBuilderContext::NotifyFinishTrajectory(int trajectory_id) { map_builder_server_->NotifyFinishTrajectory(trajectory_id); } -std::shared_ptr +std::shared_ptr MapBuilderContext::UpdateSubmap2D( const cartographer::mapping::proto::Submap& proto) { CHECK(proto.has_submap_2d()); cartographer::mapping::SubmapId submap_id{proto.submap_id().trajectory_id(), proto.submap_id().submap_index()}; - std::shared_ptr submap_2d_ptr; + std::shared_ptr submap_2d_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_2d().finished()); submap_2d_ptr = - std::make_shared(proto.submap_2d()); + std::make_shared(proto.submap_2d()); unfinished_submaps_.Insert(submap_id, submap_2d_ptr); } else { - submap_2d_ptr = std::dynamic_pointer_cast( + submap_2d_ptr = std::dynamic_pointer_cast( submap_it->data); CHECK(submap_2d_ptr); submap_2d_ptr->UpdateFromProto(proto); @@ -146,8 +146,7 @@ MapBuilderContext::ProcessLocalSlamResultData( CHECK_GE(proto.submaps().size(), 1); CHECK(proto.submaps(0).has_submap_2d() || proto.submaps(0).has_submap_3d()); if (proto.submaps(0).has_submap_2d()) { - std::vector> - submaps; + std::vector> submaps; for (const auto& submap_proto : proto.submaps()) { submaps.push_back(UpdateSubmap2D(submap_proto)); } diff --git a/cartographer_grpc/map_builder_context.h b/cartographer_grpc/map_builder_context.h index c27fa2b..8ebb207 100644 --- a/cartographer_grpc/map_builder_context.h +++ b/cartographer_grpc/map_builder_context.h @@ -17,7 +17,7 @@ #ifndef CARTOGRAPHER_GRPC_MAP_BUILDER_CONTEXT_H #define CARTOGRAPHER_GRPC_MAP_BUILDER_CONTEXT_H -#include "cartographer/mapping_2d/submaps.h" +#include "cartographer/mapping_2d/submap_2d.h" #include "cartographer/mapping_3d/submaps.h" #include "cartographer_grpc/map_builder_context_interface.h" @@ -51,7 +51,7 @@ class MapBuilderContext : public MapBuilderContextInterface { local_slam_result_data) override; private: - std::shared_ptr UpdateSubmap2D( + std::shared_ptr UpdateSubmap2D( const cartographer::mapping::proto::Submap& proto); std::shared_ptr UpdateSubmap3D( const cartographer::mapping::proto::Submap& proto);