diff --git a/cartographer/internal/mapping/global_trajectory_builder.cc b/cartographer/internal/mapping/global_trajectory_builder.cc index 10a5c5b..6bce4f0 100644 --- a/cartographer/internal/mapping/global_trajectory_builder.cc +++ b/cartographer/internal/mapping/global_trajectory_builder.cc @@ -128,13 +128,12 @@ class GlobalTrajectoryBuilder : public mapping::TrajectoryBuilderInterface { } // namespace std::unique_ptr CreateGlobalTrajectoryBuilder2D( - std::unique_ptr - local_trajectory_builder, + std::unique_ptr local_trajectory_builder, const int trajectory_id, mapping::PoseGraph2D* const pose_graph, const TrajectoryBuilderInterface::LocalSlamResultCallback& local_slam_result_callback) { - return common::make_unique>( + return common::make_unique< + GlobalTrajectoryBuilder>( std::move(local_trajectory_builder), trajectory_id, pose_graph, local_slam_result_callback); } diff --git a/cartographer/internal/mapping/global_trajectory_builder.h b/cartographer/internal/mapping/global_trajectory_builder.h index 3aeae5d..e582311 100644 --- a/cartographer/internal/mapping/global_trajectory_builder.h +++ b/cartographer/internal/mapping/global_trajectory_builder.h @@ -19,7 +19,7 @@ #include -#include "cartographer/internal/mapping_2d/local_trajectory_builder.h" +#include "cartographer/internal/mapping_2d/local_trajectory_builder_2d.h" #include "cartographer/internal/mapping_3d/local_trajectory_builder.h" #include "cartographer/mapping/local_slam_result_data.h" #include "cartographer/mapping/trajectory_builder_interface.h" @@ -30,8 +30,7 @@ namespace cartographer { namespace mapping { std::unique_ptr CreateGlobalTrajectoryBuilder2D( - std::unique_ptr - local_trajectory_builder, + std::unique_ptr local_trajectory_builder, const int trajectory_id, mapping::PoseGraph2D* const pose_graph, const TrajectoryBuilderInterface::LocalSlamResultCallback& local_slam_result_callback); diff --git a/cartographer/internal/mapping_2d/local_trajectory_builder.cc b/cartographer/internal/mapping_2d/local_trajectory_builder_2d.cc similarity index 84% rename from cartographer/internal/mapping_2d/local_trajectory_builder.cc rename to cartographer/internal/mapping_2d/local_trajectory_builder_2d.cc index d76cd5c..a04782b 100644 --- a/cartographer/internal/mapping_2d/local_trajectory_builder.cc +++ b/cartographer/internal/mapping_2d/local_trajectory_builder_2d.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/internal/mapping_2d/local_trajectory_builder.h" +#include "cartographer/internal/mapping_2d/local_trajectory_builder_2d.h" #include #include @@ -23,10 +23,10 @@ #include "cartographer/sensor/range_data.h" namespace cartographer { -namespace mapping_2d { +namespace mapping { -LocalTrajectoryBuilder::LocalTrajectoryBuilder( - const mapping::proto::LocalTrajectoryBuilderOptions2D& options) +LocalTrajectoryBuilder2D::LocalTrajectoryBuilder2D( + const proto::LocalTrajectoryBuilderOptions2D& options) : options_(options), active_submaps_(options.submaps_options()), motion_filter_(options_.motion_filter_options()), @@ -34,10 +34,10 @@ LocalTrajectoryBuilder::LocalTrajectoryBuilder( options_.real_time_correlative_scan_matcher_options()), ceres_scan_matcher_(options_.ceres_scan_matcher_options()) {} -LocalTrajectoryBuilder::~LocalTrajectoryBuilder() {} +LocalTrajectoryBuilder2D::~LocalTrajectoryBuilder2D() {} sensor::RangeData -LocalTrajectoryBuilder::TransformToGravityAlignedFrameAndFilter( +LocalTrajectoryBuilder2D::TransformToGravityAlignedFrameAndFilter( const transform::Rigid3f& transform_to_gravity_aligned_frame, const sensor::RangeData& range_data) const { const sensor::RangeData cropped = @@ -50,10 +50,10 @@ LocalTrajectoryBuilder::TransformToGravityAlignedFrameAndFilter( sensor::VoxelFilter(options_.voxel_filter_size()).Filter(cropped.misses)}; } -std::unique_ptr LocalTrajectoryBuilder::ScanMatch( +std::unique_ptr LocalTrajectoryBuilder2D::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. @@ -80,9 +80,9 @@ std::unique_ptr LocalTrajectoryBuilder::ScanMatch( return pose_observation; } -std::unique_ptr -LocalTrajectoryBuilder::AddRangeData(const common::Time time, - const sensor::TimedRangeData& range_data) { +std::unique_ptr +LocalTrajectoryBuilder2D::AddRangeData( + const common::Time time, const sensor::TimedRangeData& range_data) { // Initialize extrapolator now if we do not ever use an IMU. if (!options_.use_imu_data()) { InitializeExtrapolator(time); @@ -155,8 +155,8 @@ LocalTrajectoryBuilder::AddRangeData(const common::Time time, return nullptr; } -std::unique_ptr -LocalTrajectoryBuilder::AddAccumulatedRangeData( +std::unique_ptr +LocalTrajectoryBuilder2D::AddAccumulatedRangeData( const common::Time time, const sensor::RangeData& gravity_aligned_range_data, const transform::Rigid3d& gravity_alignment) { @@ -193,8 +193,8 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData( std::move(insertion_result)}); } -std::unique_ptr -LocalTrajectoryBuilder::InsertIntoSubmap( +std::unique_ptr +LocalTrajectoryBuilder2D::InsertIntoSubmap( const common::Time time, const sensor::RangeData& range_data_in_local, const sensor::RangeData& gravity_aligned_range_data, const transform::Rigid3d& pose_estimate, @@ -205,9 +205,8 @@ 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); @@ -218,25 +217,24 @@ LocalTrajectoryBuilder::InsertIntoSubmap( adaptive_voxel_filter.Filter(gravity_aligned_range_data.returns); return common::make_unique(InsertionResult{ - std::make_shared( - mapping::TrajectoryNode::Data{ - time, - gravity_alignment, - filtered_gravity_aligned_point_cloud, - {}, // 'high_resolution_point_cloud' is only used in 3D. - {}, // 'low_resolution_point_cloud' is only used in 3D. - {}, // 'rotational_scan_matcher_histogram' is only used in 3D. - pose_estimate}), + std::make_shared(TrajectoryNode::Data{ + time, + gravity_alignment, + filtered_gravity_aligned_point_cloud, + {}, // 'high_resolution_point_cloud' is only used in 3D. + {}, // 'low_resolution_point_cloud' is only used in 3D. + {}, // 'rotational_scan_matcher_histogram' is only used in 3D. + pose_estimate}), std::move(insertion_submaps)}); } -void LocalTrajectoryBuilder::AddImuData(const sensor::ImuData& imu_data) { +void LocalTrajectoryBuilder2D::AddImuData(const sensor::ImuData& imu_data) { CHECK(options_.use_imu_data()) << "An unexpected IMU packet was added."; InitializeExtrapolator(imu_data.time); extrapolator_->AddImuData(imu_data); } -void LocalTrajectoryBuilder::AddOdometryData( +void LocalTrajectoryBuilder2D::AddOdometryData( const sensor::OdometryData& odometry_data) { if (extrapolator_ == nullptr) { // Until we've initialized the extrapolator we cannot add odometry data. @@ -246,7 +244,7 @@ void LocalTrajectoryBuilder::AddOdometryData( extrapolator_->AddOdometryData(odometry_data); } -void LocalTrajectoryBuilder::InitializeExtrapolator(const common::Time time) { +void LocalTrajectoryBuilder2D::InitializeExtrapolator(const common::Time time) { if (extrapolator_ != nullptr) { return; } @@ -255,11 +253,11 @@ void LocalTrajectoryBuilder::InitializeExtrapolator(const common::Time time) { // in time and thus the last two are used. constexpr double kExtrapolationEstimationTimeSec = 0.001; // TODO(gaschler): Consider using InitializeWithImu as 3D does. - extrapolator_ = common::make_unique( + extrapolator_ = common::make_unique( ::cartographer::common::FromSeconds(kExtrapolationEstimationTimeSec), options_.imu_gravity_time_constant()); extrapolator_->AddPose(time, transform::Rigid3d::Identity()); } -} // namespace mapping_2d +} // namespace mapping } // namespace cartographer diff --git a/cartographer/internal/mapping_2d/local_trajectory_builder.h b/cartographer/internal/mapping_2d/local_trajectory_builder_2d.h similarity index 81% rename from cartographer/internal/mapping_2d/local_trajectory_builder.h rename to cartographer/internal/mapping_2d/local_trajectory_builder_2d.h index 0f8e1d6..2f9c61f 100644 --- a/cartographer/internal/mapping_2d/local_trajectory_builder.h +++ b/cartographer/internal/mapping_2d/local_trajectory_builder_2d.h @@ -23,8 +23,8 @@ #include "cartographer/internal/mapping/motion_filter.h" #include "cartographer/mapping/pose_extrapolator.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/scan_matching/ceres_scan_matcher_2d.h" +#include "cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_2d.h" #include "cartographer/mapping_2d/submap_2d.h" #include "cartographer/sensor/imu_data.h" #include "cartographer/sensor/odometry_data.h" @@ -33,16 +33,16 @@ #include "cartographer/transform/rigid_transform.h" namespace cartographer { -namespace mapping_2d { +namespace mapping { // Wires up the local SLAM stack (i.e. pose extrapolator, scan matching, etc.) // without loop closure. // TODO(gaschler): Add test for this class similar to the 3D test. -class LocalTrajectoryBuilder { +class LocalTrajectoryBuilder2D { public: struct InsertionResult { - std::shared_ptr constant_data; - std::vector> insertion_submaps; + std::shared_ptr constant_data; + std::vector> insertion_submaps; }; struct MatchingResult { common::Time time; @@ -52,12 +52,12 @@ class LocalTrajectoryBuilder { std::unique_ptr insertion_result; }; - explicit LocalTrajectoryBuilder( - const mapping::proto::LocalTrajectoryBuilderOptions2D& options); - ~LocalTrajectoryBuilder(); + explicit LocalTrajectoryBuilder2D( + const proto::LocalTrajectoryBuilderOptions2D& options); + ~LocalTrajectoryBuilder2D(); - LocalTrajectoryBuilder(const LocalTrajectoryBuilder&) = delete; - LocalTrajectoryBuilder& operator=(const LocalTrajectoryBuilder&) = delete; + LocalTrajectoryBuilder2D(const LocalTrajectoryBuilder2D&) = delete; + LocalTrajectoryBuilder2D& operator=(const LocalTrajectoryBuilder2D&) = delete; // Returns 'MatchingResult' when range data accumulation completed, // otherwise 'nullptr'. Range data must be approximately horizontal @@ -92,21 +92,21 @@ class LocalTrajectoryBuilder { // Lazily constructs a PoseExtrapolator. void InitializeExtrapolator(common::Time time); - const mapping::proto::LocalTrajectoryBuilderOptions2D options_; - mapping::ActiveSubmaps2D active_submaps_; + const proto::LocalTrajectoryBuilderOptions2D options_; + ActiveSubmaps2D active_submaps_; - mapping::MotionFilter motion_filter_; - scan_matching::RealTimeCorrelativeScanMatcher + MotionFilter motion_filter_; + scan_matching::RealTimeCorrelativeScanMatcher2D real_time_correlative_scan_matcher_; - scan_matching::CeresScanMatcher ceres_scan_matcher_; + scan_matching::CeresScanMatcher2D ceres_scan_matcher_; - std::unique_ptr extrapolator_; + std::unique_ptr extrapolator_; int num_accumulated_ = 0; sensor::RangeData accumulated_range_data_; }; -} // namespace mapping_2d +} // namespace mapping } // namespace cartographer #endif // CARTOGRAPHER_INTERNAL_MAPPING_2D_LOCAL_TRAJECTORY_BUILDER_H_ 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_2d.h similarity index 81% rename from cartographer/internal/mapping_2d/scan_matching/occupied_space_cost_function.h rename to cartographer/internal/mapping_2d/scan_matching/occupied_space_cost_function_2d.h index deb2b8c..2a13985 100644 --- a/cartographer/internal/mapping_2d/scan_matching/occupied_space_cost_function.h +++ b/cartographer/internal/mapping_2d/scan_matching/occupied_space_cost_function_2d.h @@ -26,22 +26,22 @@ #include "ceres/cubic_interpolation.h" namespace cartographer { -namespace mapping_2d { +namespace mapping { namespace scan_matching { // Computes a cost for matching the 'point_cloud' to the 'probability_grid' with // a 'pose'. The cost increases when points fall into less occupied space, i.e. // at pixels with lower values. -class OccupiedSpaceCostFunction { +class OccupiedSpaceCostFunction2D { public: static ceres::CostFunction* CreateAutoDiffCostFunction( const double scaling_factor, const sensor::PointCloud& point_cloud, - const mapping::ProbabilityGrid& probability_grid) { - return new ceres::AutoDiffCostFunction( - new OccupiedSpaceCostFunction(scaling_factor, point_cloud, - probability_grid), + new OccupiedSpaceCostFunction2D(scaling_factor, point_cloud, + probability_grid), point_cloud.size()); } @@ -55,7 +55,7 @@ class OccupiedSpaceCostFunction { const GridArrayAdapter adapter(probability_grid_); ceres::BiCubicInterpolator interpolator(adapter); - const mapping::MapLimits& limits = probability_grid_.limits(); + const 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,13 +79,13 @@ class OccupiedSpaceCostFunction { public: enum { DATA_DIMENSION = 1 }; - explicit GridArrayAdapter(const mapping::ProbabilityGrid& probability_grid) + explicit GridArrayAdapter(const ProbabilityGrid& probability_grid) : probability_grid_(probability_grid) {} void GetValue(const int row, const int column, double* const value) const { if (row < kPadding || column < kPadding || row >= NumRows() - kPadding || column >= NumCols() - kPadding) { - *value = mapping::kMinProbability; + *value = kMinProbability; } else { *value = static_cast(probability_grid_.GetProbability( Eigen::Array2i(column - kPadding, row - kPadding))); @@ -103,27 +103,27 @@ class OccupiedSpaceCostFunction { } private: - const mapping::ProbabilityGrid& probability_grid_; + const ProbabilityGrid& probability_grid_; }; - OccupiedSpaceCostFunction(const double scaling_factor, - const sensor::PointCloud& point_cloud, - const mapping::ProbabilityGrid& probability_grid) + OccupiedSpaceCostFunction2D(const double scaling_factor, + const sensor::PointCloud& point_cloud, + const ProbabilityGrid& probability_grid) : scaling_factor_(scaling_factor), point_cloud_(point_cloud), probability_grid_(probability_grid) {} - OccupiedSpaceCostFunction(const OccupiedSpaceCostFunction&) = delete; - OccupiedSpaceCostFunction& operator=(const OccupiedSpaceCostFunction&) = + OccupiedSpaceCostFunction2D(const OccupiedSpaceCostFunction2D&) = delete; + OccupiedSpaceCostFunction2D& operator=(const OccupiedSpaceCostFunction2D&) = delete; const double scaling_factor_; const sensor::PointCloud& point_cloud_; - const mapping::ProbabilityGrid& probability_grid_; + const ProbabilityGrid& probability_grid_; }; } // namespace scan_matching -} // namespace mapping_2d +} // namespace mapping } // namespace cartographer #endif // CARTOGRAPHER_INTERNAL_MAPPING_2D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTION_H_ diff --git a/cartographer/internal/mapping_3d/local_trajectory_builder.cc b/cartographer/internal/mapping_3d/local_trajectory_builder.cc index de418ef..d9ef256 100644 --- a/cartographer/internal/mapping_3d/local_trajectory_builder.cc +++ b/cartographer/internal/mapping_3d/local_trajectory_builder.cc @@ -20,7 +20,7 @@ #include "cartographer/common/make_unique.h" #include "cartographer/common/time.h" -#include "cartographer/mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.pb.h" +#include "cartographer/mapping/scan_matching/proto/real_time_correlative_scan_matcher_options.pb.h" #include "cartographer/mapping_3d/proto/local_trajectory_builder_options_3d.pb.h" #include "cartographer/mapping_3d/proto/submaps_options_3d.pb.h" #include "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.pb.h" diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index 6fee0d1..aa70120 100644 --- a/cartographer/mapping/map_builder.cc +++ b/cartographer/mapping/map_builder.cc @@ -25,7 +25,7 @@ #include "cartographer/common/make_unique.h" #include "cartographer/common/time.h" #include "cartographer/internal/mapping/global_trajectory_builder.h" -#include "cartographer/internal/mapping_2d/local_trajectory_builder.h" +#include "cartographer/internal/mapping_2d/local_trajectory_builder_2d.h" #include "cartographer/internal/mapping_3d/local_trajectory_builder.h" #include "cartographer/mapping/collated_trajectory_builder.h" #include "cartographer/sensor/collator.h" @@ -94,12 +94,10 @@ int MapBuilder::AddTrajectoryBuilder( trajectory_id, pose_graph_3d_.get(), local_slam_result_callback))); } else { - std::unique_ptr - local_trajectory_builder; + std::unique_ptr local_trajectory_builder; if (trajectory_options.has_trajectory_builder_2d_options()) { - local_trajectory_builder = - common::make_unique( - trajectory_options.trajectory_builder_2d_options()); + local_trajectory_builder = common::make_unique( + trajectory_options.trajectory_builder_2d_options()); } trajectory_builders_.push_back( common::make_unique( diff --git a/cartographer/mapping/pose_graph/constraint_builder.cc b/cartographer/mapping/pose_graph/constraint_builder.cc index d598f03..31c5644 100644 --- a/cartographer/mapping/pose_graph/constraint_builder.cc +++ b/cartographer/mapping/pose_graph/constraint_builder.cc @@ -16,8 +16,8 @@ #include "cartographer/mapping/pose_graph/constraint_builder.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/scan_matching/ceres_scan_matcher_2d.h" +#include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_2d.h" #include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h" #include "cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h" #include "cartographer/sensor/voxel_filter.h" @@ -41,11 +41,11 @@ proto::ConstraintBuilderOptions CreateConstraintBuilderOptions( parameter_dictionary->GetDouble("loop_closure_rotation_weight")); options.set_log_matches(parameter_dictionary->GetBool("log_matches")); *options.mutable_fast_correlative_scan_matcher_options() = - mapping_2d::scan_matching::CreateFastCorrelativeScanMatcherOptions( + scan_matching::CreateFastCorrelativeScanMatcherOptions2D( parameter_dictionary->GetDictionary("fast_correlative_scan_matcher") .get()); *options.mutable_ceres_scan_matcher_options() = - mapping_2d::scan_matching::CreateCeresScanMatcherOptions( + scan_matching::CreateCeresScanMatcherOptions2D( parameter_dictionary->GetDictionary("ceres_scan_matcher").get()); *options.mutable_fast_correlative_scan_matcher_options_3d() = mapping_3d::scan_matching::CreateFastCorrelativeScanMatcherOptions( diff --git a/cartographer/mapping/pose_graph/proto/constraint_builder_options.proto b/cartographer/mapping/pose_graph/proto/constraint_builder_options.proto index c2c357b..985d945 100644 --- a/cartographer/mapping/pose_graph/proto/constraint_builder_options.proto +++ b/cartographer/mapping/pose_graph/proto/constraint_builder_options.proto @@ -16,8 +16,8 @@ syntax = "proto3"; package cartographer.mapping.pose_graph.proto; -import "cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options.proto"; -import "cartographer/mapping_2d/scan_matching/proto/fast_correlative_scan_matcher_options.proto"; +import "cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options_2d.proto"; +import "cartographer/mapping_2d/scan_matching/proto/fast_correlative_scan_matcher_options_2d.proto"; import "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.proto"; import "cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options.proto"; @@ -48,13 +48,12 @@ message ConstraintBuilderOptions { bool log_matches = 8; // Options for the internally used scan matchers. - mapping_2d.scan_matching.proto.FastCorrelativeScanMatcherOptions + mapping.scan_matching.proto.FastCorrelativeScanMatcherOptions2D fast_correlative_scan_matcher_options = 9; - mapping_2d.scan_matching.proto.CeresScanMatcherOptions + mapping.scan_matching.proto.CeresScanMatcherOptions2D ceres_scan_matcher_options = 11; mapping_3d.scan_matching.proto.FastCorrelativeScanMatcherOptions fast_correlative_scan_matcher_options_3d = 10; - mapping_3d.scan_matching.proto.CeresScanMatcherOptions ceres_scan_matcher_options_3d = 12; } diff --git a/cartographer/mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.proto b/cartographer/mapping/scan_matching/proto/real_time_correlative_scan_matcher_options.proto similarity index 95% rename from cartographer/mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.proto rename to cartographer/mapping/scan_matching/proto/real_time_correlative_scan_matcher_options.proto index 0bd6c4e..4ddaef3 100644 --- a/cartographer/mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.proto +++ b/cartographer/mapping/scan_matching/proto/real_time_correlative_scan_matcher_options.proto @@ -14,7 +14,7 @@ syntax = "proto3"; -package cartographer.mapping_2d.scan_matching.proto; +package cartographer.mapping.scan_matching.proto; message RealTimeCorrelativeScanMatcherOptions { // Minimum linear search window in which the best possible scan alignment diff --git a/cartographer/mapping/scan_matching/real_time_correlative_scan_matcher.cc b/cartographer/mapping/scan_matching/real_time_correlative_scan_matcher.cc new file mode 100644 index 0000000..3709c28 --- /dev/null +++ b/cartographer/mapping/scan_matching/real_time_correlative_scan_matcher.cc @@ -0,0 +1,26 @@ +#include "cartographer/mapping/scan_matching/real_time_correlative_scan_matcher.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { + +proto::RealTimeCorrelativeScanMatcherOptions +CreateRealTimeCorrelativeScanMatcherOptions( + common::LuaParameterDictionary* const parameter_dictionary) { + proto::RealTimeCorrelativeScanMatcherOptions options; + options.set_linear_search_window( + parameter_dictionary->GetDouble("linear_search_window")); + options.set_angular_search_window( + parameter_dictionary->GetDouble("angular_search_window")); + options.set_translation_delta_cost_weight( + parameter_dictionary->GetDouble("translation_delta_cost_weight")); + options.set_rotation_delta_cost_weight( + parameter_dictionary->GetDouble("rotation_delta_cost_weight")); + CHECK_GE(options.translation_delta_cost_weight(), 0.); + CHECK_GE(options.rotation_delta_cost_weight(), 0.); + return options; +} + +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer diff --git a/cartographer/mapping/scan_matching/real_time_correlative_scan_matcher.h b/cartographer/mapping/scan_matching/real_time_correlative_scan_matcher.h new file mode 100644 index 0000000..56c9f01 --- /dev/null +++ b/cartographer/mapping/scan_matching/real_time_correlative_scan_matcher.h @@ -0,0 +1,36 @@ + +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_H_ +#define CARTOGRAPHER_MAPPING_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_H_ + +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/mapping/scan_matching/proto/real_time_correlative_scan_matcher_options.pb.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { + +proto::RealTimeCorrelativeScanMatcherOptions +CreateRealTimeCorrelativeScanMatcherOptions( + common::LuaParameterDictionary* const parameter_dictionary); + +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_H_ diff --git a/cartographer/mapping/trajectory_builder_interface.h b/cartographer/mapping/trajectory_builder_interface.h index fa9002d..2cf93f7 100644 --- a/cartographer/mapping/trajectory_builder_interface.h +++ b/cartographer/mapping/trajectory_builder_interface.h @@ -107,7 +107,7 @@ class TrajectoryBuilderInterface { const sensor::LandmarkData& landmark_data) = 0; // Allows to directly add local SLAM results to the 'PoseGraph'. Note that it // is invalid to add local SLAM results for a trajectory that has a - // 'LocalTrajectoryBuilder'. + // 'LocalTrajectoryBuilder2D/3D'. virtual void AddLocalSlamResultData( std::unique_ptr local_slam_result_data) = 0; }; diff --git a/cartographer/mapping_2d/local_trajectory_builder_options_2d.cc b/cartographer/mapping_2d/local_trajectory_builder_options_2d.cc index 6bd04cb..e64c7c0 100644 --- a/cartographer/mapping_2d/local_trajectory_builder_options_2d.cc +++ b/cartographer/mapping_2d/local_trajectory_builder_options_2d.cc @@ -17,8 +17,8 @@ #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/scan_matching/ceres_scan_matcher_2d.h" +#include "cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_2d.h" #include "cartographer/mapping_2d/submap_2d.h" #include "cartographer/sensor/voxel_filter.h" @@ -49,12 +49,12 @@ proto::LocalTrajectoryBuilderOptions2D CreateLocalTrajectoryBuilderOptions2D( ->GetDictionary("loop_closure_adaptive_voxel_filter") .get()); *options.mutable_real_time_correlative_scan_matcher_options() = - mapping_2d::scan_matching::CreateRealTimeCorrelativeScanMatcherOptions( + mapping::scan_matching::CreateRealTimeCorrelativeScanMatcherOptions( parameter_dictionary ->GetDictionary("real_time_correlative_scan_matcher") .get()); *options.mutable_ceres_scan_matcher_options() = - mapping_2d::scan_matching::CreateCeresScanMatcherOptions( + mapping::scan_matching::CreateCeresScanMatcherOptions2D( parameter_dictionary->GetDictionary("ceres_scan_matcher").get()); *options.mutable_motion_filter_options() = mapping::CreateMotionFilterOptions( parameter_dictionary->GetDictionary("motion_filter").get()); diff --git a/cartographer/mapping_2d/pose_graph/constraint_builder.cc b/cartographer/mapping_2d/pose_graph/constraint_builder_2d.cc similarity index 85% rename from cartographer/mapping_2d/pose_graph/constraint_builder.cc rename to cartographer/mapping_2d/pose_graph/constraint_builder_2d.cc index 0b7dd54..d6e1857 100644 --- a/cartographer/mapping_2d/pose_graph/constraint_builder.cc +++ b/cartographer/mapping_2d/pose_graph/constraint_builder_2d.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/mapping_2d/pose_graph/constraint_builder.h" +#include "cartographer/mapping_2d/pose_graph/constraint_builder_2d.h" #include #include @@ -29,8 +29,8 @@ #include "cartographer/common/make_unique.h" #include "cartographer/common/math.h" #include "cartographer/common/thread_pool.h" -#include "cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options.pb.h" -#include "cartographer/mapping_2d/scan_matching/proto/fast_correlative_scan_matcher_options.pb.h" +#include "cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options_2d.pb.h" +#include "cartographer/mapping_2d/scan_matching/proto/fast_correlative_scan_matcher_options_2d.pb.h" #include "cartographer/metrics/counter.h" #include "cartographer/metrics/gauge.h" #include "cartographer/metrics/histogram.h" @@ -38,7 +38,7 @@ #include "glog/logging.h" namespace cartographer { -namespace mapping_2d { +namespace mapping { namespace pose_graph { static auto* kConstraintsSearchedMetric = metrics::Counter::Null(); @@ -49,19 +49,19 @@ static auto* kQueueLengthMetric = metrics::Gauge::Null(); static auto* kConstraintScoresMetric = metrics::Histogram::Null(); static auto* kGlobalConstraintScoresMetric = metrics::Histogram::Null(); -transform::Rigid2d ComputeSubmapPose(const mapping::Submap2D& submap) { +transform::Rigid2d ComputeSubmapPose(const Submap2D& submap) { return transform::Project2D(submap.local_pose()); } -ConstraintBuilder::ConstraintBuilder( - const mapping::pose_graph::proto::ConstraintBuilderOptions& options, +ConstraintBuilder2D::ConstraintBuilder2D( + const pose_graph::proto::ConstraintBuilderOptions& options, common::ThreadPool* const thread_pool) : options_(options), thread_pool_(thread_pool), sampler_(options.sampling_ratio()), ceres_scan_matcher_(options.ceres_scan_matcher_options()) {} -ConstraintBuilder::~ConstraintBuilder() { +ConstraintBuilder2D::~ConstraintBuilder2D() { common::MutexLocker locker(&mutex_); CHECK_EQ(constraints_.size(), 0) << "WhenDone() was not called"; CHECK_EQ(pending_computations_.size(), 0); @@ -69,10 +69,9 @@ ConstraintBuilder::~ConstraintBuilder() { CHECK(when_done_ == nullptr); } -void ConstraintBuilder::MaybeAddConstraint( - const mapping::SubmapId& submap_id, const mapping::Submap2D* const submap, - const mapping::NodeId& node_id, - const mapping::TrajectoryNode::Data* const constant_data, +void ConstraintBuilder2D::MaybeAddConstraint( + const SubmapId& submap_id, const Submap2D* const submap, + const NodeId& node_id, const TrajectoryNode::Data* const constant_data, const transform::Rigid2d& initial_relative_pose) { if (initial_relative_pose.translation().norm() > options_.max_constraint_distance()) { @@ -95,10 +94,9 @@ void ConstraintBuilder::MaybeAddConstraint( } } -void ConstraintBuilder::MaybeAddGlobalConstraint( - const mapping::SubmapId& submap_id, const mapping::Submap2D* const submap, - const mapping::NodeId& node_id, - const mapping::TrajectoryNode::Data* const constant_data) { +void ConstraintBuilder2D::MaybeAddGlobalConstraint( + const SubmapId& submap_id, const Submap2D* const submap, + const NodeId& node_id, const TrajectoryNode::Data* const constant_data) { common::MutexLocker locker(&mutex_); constraints_.emplace_back(); kQueueLengthMetric->Set(constraints_.size()); @@ -114,13 +112,13 @@ void ConstraintBuilder::MaybeAddGlobalConstraint( }); } -void ConstraintBuilder::NotifyEndOfNode() { +void ConstraintBuilder2D::NotifyEndOfNode() { common::MutexLocker locker(&mutex_); ++current_computation_; } -void ConstraintBuilder::WhenDone( - const std::function& callback) { +void ConstraintBuilder2D::WhenDone( + const std::function& callback) { common::MutexLocker locker(&mutex_); CHECK(when_done_ == nullptr); when_done_ = @@ -131,9 +129,8 @@ void ConstraintBuilder::WhenDone( [this, current_computation] { FinishComputation(current_computation); }); } -void ConstraintBuilder::ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( - const mapping::SubmapId& submap_id, - const mapping::ProbabilityGrid* const submap, +void ConstraintBuilder2D::ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( + const SubmapId& submap_id, const ProbabilityGrid* const submap, const std::function& work_item) { if (submap_scan_matchers_[submap_id].fast_correlative_scan_matcher != nullptr) { @@ -147,11 +144,10 @@ void ConstraintBuilder::ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( } } -void ConstraintBuilder::ConstructSubmapScanMatcher( - const mapping::SubmapId& submap_id, - const mapping::ProbabilityGrid* const submap) { +void ConstraintBuilder2D::ConstructSubmapScanMatcher( + const SubmapId& submap_id, const ProbabilityGrid* const submap) { auto submap_scan_matcher = - common::make_unique( + common::make_unique( *submap, options_.fast_correlative_scan_matcher_options()); common::MutexLocker locker(&mutex_); submap_scan_matchers_[submap_id] = {submap, std::move(submap_scan_matcher)}; @@ -162,8 +158,8 @@ void ConstraintBuilder::ConstructSubmapScanMatcher( submap_queued_work_items_.erase(submap_id); } -const ConstraintBuilder::SubmapScanMatcher* -ConstraintBuilder::GetSubmapScanMatcher(const mapping::SubmapId& submap_id) { +const ConstraintBuilder2D::SubmapScanMatcher* +ConstraintBuilder2D::GetSubmapScanMatcher(const SubmapId& submap_id) { common::MutexLocker locker(&mutex_); const SubmapScanMatcher* submap_scan_matcher = &submap_scan_matchers_[submap_id]; @@ -171,12 +167,12 @@ ConstraintBuilder::GetSubmapScanMatcher(const mapping::SubmapId& submap_id) { return submap_scan_matcher; } -void ConstraintBuilder::ComputeConstraint( - 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, +void ConstraintBuilder2D::ComputeConstraint( + const SubmapId& submap_id, const Submap2D* const submap, + const NodeId& node_id, bool match_full_submap, + const TrajectoryNode::Data* const constant_data, const transform::Rigid2d& initial_relative_pose, - std::unique_ptr* constraint) { + std::unique_ptr* constraint) { const transform::Rigid2d initial_pose = ComputeSubmapPose(*submap) * initial_relative_pose; const SubmapScanMatcher* const submap_scan_matcher = @@ -262,7 +258,7 @@ void ConstraintBuilder::ComputeConstraint( } } -void ConstraintBuilder::FinishComputation(const int computation_index) { +void ConstraintBuilder2D::FinishComputation(const int computation_index) { Result result; std::unique_ptr> callback; { @@ -295,7 +291,7 @@ void ConstraintBuilder::FinishComputation(const int computation_index) { } } -int ConstraintBuilder::GetNumFinishedNodes() { +int ConstraintBuilder2D::GetNumFinishedNodes() { common::MutexLocker locker(&mutex_); if (pending_computations_.empty()) { return current_computation_; @@ -303,13 +299,13 @@ int ConstraintBuilder::GetNumFinishedNodes() { return pending_computations_.begin()->first; } -void ConstraintBuilder::DeleteScanMatcher(const mapping::SubmapId& submap_id) { +void ConstraintBuilder2D::DeleteScanMatcher(const SubmapId& submap_id) { common::MutexLocker locker(&mutex_); CHECK(pending_computations_.empty()); submap_scan_matchers_.erase(submap_id); } -void ConstraintBuilder::RegisterMetrics(metrics::FamilyFactory* factory) { +void ConstraintBuilder2D::RegisterMetrics(metrics::FamilyFactory* factory) { auto* counts = factory->NewCounterFamily( "/mapping_2d/pose_graph/constraint_builder/constraints", "Constraints computed"); @@ -333,5 +329,5 @@ void ConstraintBuilder::RegisterMetrics(metrics::FamilyFactory* factory) { } } // namespace pose_graph -} // namespace mapping_2d +} // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping_2d/pose_graph/constraint_builder.h b/cartographer/mapping_2d/pose_graph/constraint_builder_2d.h similarity index 71% rename from cartographer/mapping_2d/pose_graph/constraint_builder.h rename to cartographer/mapping_2d/pose_graph/constraint_builder_2d.h index 68f3940..93b0254 100644 --- a/cartographer/mapping_2d/pose_graph/constraint_builder.h +++ b/cartographer/mapping_2d/pose_graph/constraint_builder_2d.h @@ -14,8 +14,8 @@ * limitations under the License. */ -#ifndef CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_CONSTRAINT_BUILDER_H_ -#define CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_CONSTRAINT_BUILDER_H_ +#ifndef CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_CONSTRAINT_BUILDER_2D_H_ +#define CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_CONSTRAINT_BUILDER_2D_H_ #include #include @@ -32,20 +32,20 @@ #include "cartographer/common/thread_pool.h" #include "cartographer/mapping/pose_graph/proto/constraint_builder_options.pb.h" #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/scan_matching/ceres_scan_matcher_2d.h" +#include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_2d.h" #include "cartographer/mapping_2d/submap_2d.h" #include "cartographer/metrics/family_factory.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/voxel_filter.h" namespace cartographer { -namespace mapping_2d { +namespace mapping { namespace pose_graph { // Returns (map <- submap) where 'submap' is a coordinate system at the origin // of the Submap. -transform::Rigid2d ComputeSubmapPose(const mapping::Submap2D& submap); +transform::Rigid2d ComputeSubmapPose(const Submap2D& submap); // Asynchronously computes constraints. // @@ -55,18 +55,18 @@ transform::Rigid2d ComputeSubmapPose(const mapping::Submap2D& submap); // MaybeAdd(Global)Constraint()/WhenDone() cycle can follow. // // This class is thread-safe. -class ConstraintBuilder { +class ConstraintBuilder2D { public: - using Constraint = mapping::PoseGraphInterface::Constraint; + using Constraint = PoseGraphInterface::Constraint; using Result = std::vector; - ConstraintBuilder( - const mapping::pose_graph::proto::ConstraintBuilderOptions& options, + ConstraintBuilder2D( + const pose_graph::proto::ConstraintBuilderOptions& options, common::ThreadPool* thread_pool); - ~ConstraintBuilder(); + ~ConstraintBuilder2D(); - ConstraintBuilder(const ConstraintBuilder&) = delete; - ConstraintBuilder& operator=(const ConstraintBuilder&) = delete; + ConstraintBuilder2D(const ConstraintBuilder2D&) = delete; + ConstraintBuilder2D& operator=(const ConstraintBuilder2D&) = delete; // Schedules exploring a new constraint between 'submap' identified by // 'submap_id', and the 'compressed_point_cloud' for 'node_id'. The @@ -74,11 +74,10 @@ 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 mapping::Submap2D* submap, - const mapping::NodeId& node_id, - const mapping::TrajectoryNode::Data* const constant_data, - const transform::Rigid2d& initial_relative_pose); + void MaybeAddConstraint(const SubmapId& submap_id, const Submap2D* submap, + const NodeId& node_id, + const TrajectoryNode::Data* const constant_data, + const transform::Rigid2d& initial_relative_pose); // Schedules exploring a new constraint between 'submap' identified by // 'submap_id' and the 'compressed_point_cloud' for 'node_id'. @@ -87,9 +86,8 @@ 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 mapping::Submap2D* submap, - const mapping::NodeId& node_id, - const mapping::TrajectoryNode::Data* const constant_data); + const SubmapId& submap_id, const Submap2D* submap, const NodeId& node_id, + const TrajectoryNode::Data* const constant_data); // Must be called after all computations related to one node have been added. void NotifyEndOfNode(); @@ -102,48 +100,47 @@ class ConstraintBuilder { int GetNumFinishedNodes(); // Delete data related to 'submap_id'. - void DeleteScanMatcher(const mapping::SubmapId& submap_id); + void DeleteScanMatcher(const SubmapId& submap_id); static void RegisterMetrics(metrics::FamilyFactory* family_factory); private: struct SubmapScanMatcher { - const mapping::ProbabilityGrid* probability_grid; - std::unique_ptr + const ProbabilityGrid* probability_grid; + std::unique_ptr fast_correlative_scan_matcher; }; // 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 mapping::ProbabilityGrid* submap, + const SubmapId& submap_id, const 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 mapping::ProbabilityGrid* submap) + void ConstructSubmapScanMatcher(const SubmapId& submap_id, + const ProbabilityGrid* submap) EXCLUDES(mutex_); // Returns the scan matcher for a submap, which has to exist. - const SubmapScanMatcher* GetSubmapScanMatcher( - const mapping::SubmapId& submap_id) EXCLUDES(mutex_); + const SubmapScanMatcher* GetSubmapScanMatcher(const SubmapId& submap_id) + EXCLUDES(mutex_); // Runs in a background thread and does computations for an additional // 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 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, - std::unique_ptr* constraint) EXCLUDES(mutex_); + void ComputeConstraint(const SubmapId& submap_id, const Submap2D* submap, + const NodeId& node_id, bool match_full_submap, + const TrajectoryNode::Data* const constant_data, + const transform::Rigid2d& initial_relative_pose, + std::unique_ptr* constraint) + EXCLUDES(mutex_); // Decrements the 'pending_computations_' count. If all computations are done, // runs the 'when_done_' callback and resets the state. void FinishComputation(int computation_index) EXCLUDES(mutex_); - const mapping::pose_graph::proto::ConstraintBuilderOptions options_; + const pose_graph::proto::ConstraintBuilderOptions options_; common::ThreadPool* thread_pool_; common::Mutex mutex_; @@ -165,23 +162,23 @@ class ConstraintBuilder { std::deque> constraints_ GUARDED_BY(mutex_); // Map of already constructed scan matchers by 'submap_id'. - std::map submap_scan_matchers_ + std::map submap_scan_matchers_ GUARDED_BY(mutex_); // Map by 'submap_id' of scan matchers under construction, and the work // to do once construction is done. - std::map>> + std::map>> submap_queued_work_items_ GUARDED_BY(mutex_); common::FixedRatioSampler sampler_; - scan_matching::CeresScanMatcher ceres_scan_matcher_; + scan_matching::CeresScanMatcher2D ceres_scan_matcher_; // Histogram of scan matcher scores. common::Histogram score_histogram_ GUARDED_BY(mutex_); }; } // namespace pose_graph -} // namespace mapping_2d +} // namespace mapping } // namespace cartographer -#endif // CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_CONSTRAINT_BUILDER_H_ +#endif // CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_CONSTRAINT_BUILDER_2D_H_ diff --git a/cartographer/mapping_2d/pose_graph/landmark_cost_function.h b/cartographer/mapping_2d/pose_graph/landmark_cost_function_2d.h similarity index 85% rename from cartographer/mapping_2d/pose_graph/landmark_cost_function.h rename to cartographer/mapping_2d/pose_graph/landmark_cost_function_2d.h index 238015c..1d92b09 100644 --- a/cartographer/mapping_2d/pose_graph/landmark_cost_function.h +++ b/cartographer/mapping_2d/pose_graph/landmark_cost_function_2d.h @@ -14,49 +14,50 @@ * limitations under the License. */ -#ifndef CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_LANDMARK_COST_FUNCTION_H_ -#define CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_LANDMARK_COST_FUNCTION_H_ +#ifndef CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_LANDMARK_COST_FUNCTION_2D_H_ +#define CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_LANDMARK_COST_FUNCTION_2D_H_ #include "Eigen/Core" #include "Eigen/Geometry" #include "cartographer/mapping/pose_graph/cost_helpers.h" #include "cartographer/mapping/pose_graph_interface.h" -#include "cartographer/mapping_2d/pose_graph/optimization_problem.h" +#include "cartographer/mapping_2d/pose_graph/optimization_problem_2d.h" #include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/transform.h" #include "ceres/ceres.h" #include "ceres/jet.h" namespace cartographer { -namespace mapping_2d { +namespace mapping { namespace pose_graph { // Cost function measuring the weighted error between the observed pose given by // the landmark measurement and the linearly interpolated pose of embedded in 3D // space node poses. -class LandmarkCostFunction { +class LandmarkCostFunction2D { public: using LandmarkObservation = - mapping::PoseGraphInterface::LandmarkNode::LandmarkObservation; + PoseGraphInterface::LandmarkNode::LandmarkObservation; static ceres::CostFunction* CreateAutoDiffCostFunction( - const LandmarkObservation& observation, const NodeData& prev_node, - const NodeData& next_node) { + const LandmarkObservation& observation, + const OptimizationProblem2D::NodeData& prev_node, + const OptimizationProblem2D::NodeData& next_node) { return new ceres::AutoDiffCostFunction< - LandmarkCostFunction, 6 /* residuals */, + LandmarkCostFunction2D, 6 /* residuals */, 3 /* previous node pose variables */, 3 /* next node pose variables */, 4 /* landmark rotation variables */, 3 /* landmark translation variables */>( - new LandmarkCostFunction(observation, prev_node, next_node)); + new LandmarkCostFunction2D(observation, prev_node, next_node)); } template bool operator()(const T* const prev_node_pose, const T* const next_node_pose, const T* const landmark_rotation, const T* const landmark_translation, T* const e) const { - using mapping::pose_graph::ComputeUnscaledError; - using mapping::pose_graph::ScaleError; - using mapping::pose_graph::SlerpQuaternions; + using pose_graph::ComputeUnscaledError; + using pose_graph::ScaleError; + using pose_graph::SlerpQuaternions; const std::array interpolated_pose_translation{ {prev_node_pose[0] + @@ -102,8 +103,9 @@ class LandmarkCostFunction { } private: - LandmarkCostFunction(const LandmarkObservation& observation, - const NodeData& prev_node, const NodeData& next_node) + LandmarkCostFunction2D(const LandmarkObservation& observation, + const OptimizationProblem2D::NodeData& prev_node, + const OptimizationProblem2D::NodeData& next_node) : landmark_to_tracking_transform_( observation.landmark_to_tracking_transform), prev_node_gravity_alignment_(prev_node.gravity_alignment), @@ -123,7 +125,7 @@ class LandmarkCostFunction { }; } // namespace pose_graph -} // namespace mapping_2d +} // namespace mapping } // namespace cartographer -#endif // CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_LANDMARK_COST_FUNCTION_H_ +#endif // CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_LANDMARK_COST_FUNCTION_2D_H_ diff --git a/cartographer/mapping_2d/pose_graph/landmark_cost_function_test.cc b/cartographer/mapping_2d/pose_graph/landmark_cost_function_2d_test.cc similarity index 90% rename from cartographer/mapping_2d/pose_graph/landmark_cost_function_test.cc rename to cartographer/mapping_2d/pose_graph/landmark_cost_function_2d_test.cc index 03be67d..8d3220d 100644 --- a/cartographer/mapping_2d/pose_graph/landmark_cost_function_test.cc +++ b/cartographer/mapping_2d/pose_graph/landmark_cost_function_2d_test.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/mapping_2d/pose_graph/landmark_cost_function.h" +#include "cartographer/mapping_2d/pose_graph/landmark_cost_function_2d.h" #include @@ -23,7 +23,7 @@ #include "gtest/gtest.h" namespace cartographer { -namespace mapping_2d { +namespace mapping { namespace pose_graph { namespace { @@ -31,18 +31,18 @@ using ::testing::DoubleEq; using ::testing::ElementsAre; using LandmarkObservation = - mapping::PoseGraphInterface::LandmarkNode::LandmarkObservation; + PoseGraphInterface::LandmarkNode::LandmarkObservation; TEST(LandmarkCostFunctionTest, SmokeTest) { - NodeData prev_node; + OptimizationProblem2D::NodeData prev_node; prev_node.time = common::FromUniversal(0); prev_node.gravity_alignment = Eigen::Quaterniond::Identity(); - NodeData next_node; + OptimizationProblem2D::NodeData next_node; next_node.time = common::FromUniversal(10); next_node.gravity_alignment = Eigen::Quaterniond::Identity(); std::unique_ptr cost_function( - LandmarkCostFunction::CreateAutoDiffCostFunction( + LandmarkCostFunction2D::CreateAutoDiffCostFunction( LandmarkObservation{ 0 /* trajectory ID */, common::FromUniversal(5) /* time */, @@ -73,5 +73,5 @@ TEST(LandmarkCostFunctionTest, SmokeTest) { } // namespace } // namespace pose_graph -} // namespace mapping_2d +} // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping_2d/pose_graph/optimization_problem.cc b/cartographer/mapping_2d/pose_graph/optimization_problem_2d.cc similarity index 84% rename from cartographer/mapping_2d/pose_graph/optimization_problem.cc rename to cartographer/mapping_2d/pose_graph/optimization_problem_2d.cc index 329949f..b6792eb 100644 --- a/cartographer/mapping_2d/pose_graph/optimization_problem.cc +++ b/cartographer/mapping_2d/pose_graph/optimization_problem_2d.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/mapping_2d/pose_graph/optimization_problem.h" +#include "cartographer/mapping_2d/pose_graph/optimization_problem_2d.h" #include #include @@ -28,21 +28,22 @@ #include "cartographer/common/histogram.h" #include "cartographer/common/math.h" #include "cartographer/mapping/pose_graph/ceres_pose.h" -#include "cartographer/mapping_2d/pose_graph/landmark_cost_function.h" -#include "cartographer/mapping_2d/pose_graph/spa_cost_function.h" +#include "cartographer/mapping_2d/pose_graph/landmark_cost_function_2d.h" +#include "cartographer/mapping_2d/pose_graph/spa_cost_function_2d.h" #include "cartographer/sensor/odometry_data.h" #include "cartographer/transform/transform.h" #include "ceres/ceres.h" #include "glog/logging.h" namespace cartographer { -namespace mapping_2d { +namespace mapping { namespace pose_graph { - namespace { using ::cartographer::mapping::pose_graph::CeresPose; using LandmarkNode = ::cartographer::mapping::PoseGraphInterface::LandmarkNode; +using NodeData = OptimizationProblem2D::NodeData; +using SubmapData = OptimizationProblem2D::SubmapData; // Converts a pose into the 3 optimization variable format used for Ceres: // translation in x and y, followed by the rotation angle representing the @@ -73,8 +74,8 @@ transform::Rigid3d GetInitialLandmarkPose( void AddLandmarkCostFunctions( const std::map& landmark_nodes, - const mapping::MapById& node_data, - mapping::MapById>* C_nodes, + const MapById& node_data, + MapById>* C_nodes, std::map* C_landmarks, ceres::Problem* problem) { for (const auto& landmark_node : landmark_nodes) { for (const auto& observation : landmark_node.second.landmark_observations) { @@ -110,7 +111,7 @@ void AddLandmarkCostFunctions( problem)); } problem->AddResidualBlock( - LandmarkCostFunction::CreateAutoDiffCostFunction( + LandmarkCostFunction2D::CreateAutoDiffCostFunction( observation, prev->data, next->data), nullptr /* loss function */, C_nodes->at(prev->id).data(), C_nodes->at(next->id).data(), C_landmarks->at(landmark_id).rotation(), @@ -121,23 +122,23 @@ void AddLandmarkCostFunctions( } // namespace -OptimizationProblem::OptimizationProblem( - const mapping::pose_graph::proto::OptimizationProblemOptions& options) +OptimizationProblem2D::OptimizationProblem2D( + const pose_graph::proto::OptimizationProblemOptions& options) : options_(options) {} -OptimizationProblem::~OptimizationProblem() {} +OptimizationProblem2D::~OptimizationProblem2D() {} -void OptimizationProblem::AddImuData(const int trajectory_id, - const sensor::ImuData& imu_data) { +void OptimizationProblem2D::AddImuData(const int trajectory_id, + const sensor::ImuData& imu_data) { imu_data_.Append(trajectory_id, imu_data); } -void OptimizationProblem::AddOdometryData( +void OptimizationProblem2D::AddOdometryData( const int trajectory_id, const sensor::OdometryData& odometry_data) { odometry_data_.Append(trajectory_id, odometry_data); } -void OptimizationProblem::AddTrajectoryNode( +void OptimizationProblem2D::AddTrajectoryNode( const int trajectory_id, const common::Time time, const transform::Rigid2d& initial_pose, const transform::Rigid2d& pose, const Eigen::Quaterniond& gravity_alignment) { @@ -145,41 +146,41 @@ void OptimizationProblem::AddTrajectoryNode( NodeData{time, initial_pose, pose, gravity_alignment}); } -void OptimizationProblem::InsertTrajectoryNode( - const mapping::NodeId& node_id, const common::Time time, +void OptimizationProblem2D::InsertTrajectoryNode( + const NodeId& node_id, const common::Time time, const transform::Rigid2d& initial_pose, const transform::Rigid2d& pose, const Eigen::Quaterniond& gravity_alignment) { node_data_.Insert(node_id, NodeData{time, initial_pose, pose, gravity_alignment}); } -void OptimizationProblem::TrimTrajectoryNode(const mapping::NodeId& node_id) { +void OptimizationProblem2D::TrimTrajectoryNode(const NodeId& node_id) { imu_data_.Trim(node_data_, node_id); odometry_data_.Trim(node_data_, node_id); node_data_.Trim(node_id); } -void OptimizationProblem::AddSubmap( +void OptimizationProblem2D::AddSubmap( const int trajectory_id, const transform::Rigid2d& global_submap_pose) { submap_data_.Append(trajectory_id, SubmapData{global_submap_pose}); } -void OptimizationProblem::InsertSubmap( - const mapping::SubmapId& submap_id, - const transform::Rigid2d& global_submap_pose) { +void OptimizationProblem2D::InsertSubmap( + const SubmapId& submap_id, const transform::Rigid2d& global_submap_pose) { submap_data_.Insert(submap_id, SubmapData{global_submap_pose}); } -void OptimizationProblem::TrimSubmap(const mapping::SubmapId& submap_id) { +void OptimizationProblem2D::TrimSubmap(const SubmapId& submap_id) { submap_data_.Trim(submap_id); } -void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) { +void OptimizationProblem2D::SetMaxNumIterations( + const int32 max_num_iterations) { options_.mutable_ceres_solver_options()->set_max_num_iterations( max_num_iterations); } -void OptimizationProblem::Solve( +void OptimizationProblem2D::Solve( const std::vector& constraints, const std::set& frozen_trajectories, const std::map& landmark_nodes) { @@ -193,8 +194,8 @@ void OptimizationProblem::Solve( // Set the starting point. // TODO(hrapp): Move ceres data into SubmapData. - mapping::MapById> C_submaps; - mapping::MapById> C_nodes; + MapById> C_submaps; + MapById> C_nodes; std::map C_landmarks; bool first_submap = true; for (const auto& submap_id_data : submap_data_) { @@ -222,7 +223,7 @@ void OptimizationProblem::Solve( // Add cost functions for intra- and inter-submap constraints. for (const Constraint& constraint : constraints) { problem.AddResidualBlock( - SpaCostFunction::CreateAutoDiffCostFunction(constraint.pose), + SpaCostFunction2D::CreateAutoDiffCostFunction(constraint.pose), // Only loop closure constraints should have a loss function. constraint.tag == Constraint::INTER_SUBMAP ? new ceres::HuberLoss(options_.huber_scale()) @@ -245,10 +246,10 @@ void OptimizationProblem::Solve( auto prev_node_it = node_it; for (++node_it; node_it != trajectory_end; ++node_it) { - const mapping::NodeId first_node_id = prev_node_it->id; + const NodeId first_node_id = prev_node_it->id; const NodeData& first_node_data = prev_node_it->data; prev_node_it = node_it; - const mapping::NodeId second_node_id = node_it->id; + const NodeId second_node_id = node_it->id; const NodeData& second_node_data = node_it->data; if (second_node_id.node_index != first_node_id.node_index + 1) { @@ -258,7 +259,7 @@ void OptimizationProblem::Solve( const transform::Rigid3d relative_pose = ComputeRelativePose(trajectory_id, first_node_data, second_node_data); problem.AddResidualBlock( - SpaCostFunction::CreateAutoDiffCostFunction(Constraint::Pose{ + SpaCostFunction2D::CreateAutoDiffCostFunction(Constraint::Pose{ relative_pose, options_.consecutive_node_translation_weight(), options_.consecutive_node_rotation_weight()}), nullptr /* loss function */, C_nodes.at(first_node_id).data(), @@ -288,32 +289,31 @@ void OptimizationProblem::Solve( } } -const mapping::MapById& -OptimizationProblem::node_data() const { +const MapById& OptimizationProblem2D::node_data() const { return node_data_; } -const mapping::MapById& -OptimizationProblem::submap_data() const { +const MapById& OptimizationProblem2D::submap_data() + const { return submap_data_; } const std::map& -OptimizationProblem::landmark_data() const { +OptimizationProblem2D::landmark_data() const { return landmark_data_; } -const sensor::MapByTime& OptimizationProblem::imu_data() +const sensor::MapByTime& OptimizationProblem2D::imu_data() const { return imu_data_; } const sensor::MapByTime& -OptimizationProblem::odometry_data() const { +OptimizationProblem2D::odometry_data() const { return odometry_data_; } -std::unique_ptr OptimizationProblem::InterpolateOdometry( +std::unique_ptr OptimizationProblem2D::InterpolateOdometry( const int trajectory_id, const common::Time time) const { const auto it = odometry_data_.lower_bound(trajectory_id, time); if (it == odometry_data_.EndOfTrajectory(trajectory_id)) { @@ -332,7 +332,7 @@ std::unique_ptr OptimizationProblem::InterpolateOdometry( .transform); } -transform::Rigid3d OptimizationProblem::ComputeRelativePose( +transform::Rigid3d OptimizationProblem2D::ComputeRelativePose( const int trajectory_id, const NodeData& first_node_data, const NodeData& second_node_data) const { if (odometry_data_.HasTrajectory(trajectory_id)) { @@ -352,5 +352,5 @@ transform::Rigid3d OptimizationProblem::ComputeRelativePose( } } // namespace pose_graph -} // namespace mapping_2d +} // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping_2d/pose_graph/optimization_problem.h b/cartographer/mapping_2d/pose_graph/optimization_problem_2d.h similarity index 72% rename from cartographer/mapping_2d/pose_graph/optimization_problem.h rename to cartographer/mapping_2d/pose_graph/optimization_problem_2d.h index 68ab147..cace99d 100644 --- a/cartographer/mapping_2d/pose_graph/optimization_problem.h +++ b/cartographer/mapping_2d/pose_graph/optimization_problem_2d.h @@ -14,8 +14,8 @@ * limitations under the License. */ -#ifndef CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_OPTIMIZATION_PROBLEM_H_ -#define CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_OPTIMIZATION_PROBLEM_H_ +#ifndef CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_OPTIMIZATION_PROBLEM_2D_H_ +#define CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_OPTIMIZATION_PROBLEM_2D_H_ #include #include @@ -36,32 +36,32 @@ #include "cartographer/transform/timestamped_transform.h" namespace cartographer { -namespace mapping_2d { +namespace mapping { namespace pose_graph { -struct NodeData { - common::Time time; - transform::Rigid2d initial_pose; - transform::Rigid2d pose; - Eigen::Quaterniond gravity_alignment; -}; - -struct SubmapData { - transform::Rigid2d global_pose; -}; - // Implements the SPA loop closure method. -class OptimizationProblem { +class OptimizationProblem2D { public: - using Constraint = mapping::PoseGraphInterface::Constraint; - using LandmarkNode = mapping::PoseGraphInterface::LandmarkNode; + using Constraint = PoseGraphInterface::Constraint; + using LandmarkNode = PoseGraphInterface::LandmarkNode; - explicit OptimizationProblem( - const mapping::pose_graph::proto::OptimizationProblemOptions& options); - ~OptimizationProblem(); + struct NodeData { + common::Time time; + transform::Rigid2d initial_pose; + transform::Rigid2d pose; + Eigen::Quaterniond gravity_alignment; + }; - OptimizationProblem(const OptimizationProblem&) = delete; - OptimizationProblem& operator=(const OptimizationProblem&) = delete; + struct SubmapData { + transform::Rigid2d global_pose; + }; + + explicit OptimizationProblem2D( + const pose_graph::proto::OptimizationProblemOptions& options); + ~OptimizationProblem2D(); + + OptimizationProblem2D(const OptimizationProblem2D&) = delete; + OptimizationProblem2D& operator=(const OptimizationProblem2D&) = delete; void AddImuData(int trajectory_id, const sensor::ImuData& imu_data); void AddOdometryData(int trajectory_id, @@ -70,16 +70,16 @@ class OptimizationProblem { const transform::Rigid2d& initial_pose, const transform::Rigid2d& pose, const Eigen::Quaterniond& gravity_alignment); - void InsertTrajectoryNode(const mapping::NodeId& node_id, common::Time time, + void InsertTrajectoryNode(const NodeId& node_id, common::Time time, const transform::Rigid2d& initial_pose, const transform::Rigid2d& pose, const Eigen::Quaterniond& gravity_alignment); - void TrimTrajectoryNode(const mapping::NodeId& node_id); + void TrimTrajectoryNode(const NodeId& node_id); void AddSubmap(int trajectory_id, const transform::Rigid2d& global_submap_pose); - void InsertSubmap(const mapping::SubmapId& submap_id, + void InsertSubmap(const SubmapId& submap_id, const transform::Rigid2d& global_submap_pose); - void TrimSubmap(const mapping::SubmapId& submap_id); + void TrimSubmap(const SubmapId& submap_id); void SetMaxNumIterations(int32 max_num_iterations); @@ -88,8 +88,8 @@ class OptimizationProblem { const std::set& frozen_trajectories, const std::map& landmark_nodes); - const mapping::MapById& node_data() const; - const mapping::MapById& submap_data() const; + const MapById& node_data() const; + const MapById& submap_data() const; const std::map& landmark_data() const; const sensor::MapByTime& imu_data() const; const sensor::MapByTime& odometry_data() const; @@ -102,16 +102,16 @@ class OptimizationProblem { int trajectory_id, const NodeData& first_node_data, const NodeData& second_node_data) const; - mapping::pose_graph::proto::OptimizationProblemOptions options_; - mapping::MapById node_data_; - mapping::MapById submap_data_; + pose_graph::proto::OptimizationProblemOptions options_; + MapById node_data_; + MapById submap_data_; std::map landmark_data_; sensor::MapByTime imu_data_; sensor::MapByTime odometry_data_; }; } // namespace pose_graph -} // namespace mapping_2d +} // namespace mapping } // namespace cartographer -#endif // CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_OPTIMIZATION_PROBLEM_H_ +#endif // CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_OPTIMIZATION_PROBLEM_2D_H_ diff --git a/cartographer/mapping_2d/pose_graph/spa_cost_function.h b/cartographer/mapping_2d/pose_graph/spa_cost_function_2d.h similarity index 73% rename from cartographer/mapping_2d/pose_graph/spa_cost_function.h rename to cartographer/mapping_2d/pose_graph/spa_cost_function_2d.h index 25ee9d6..bb80284 100644 --- a/cartographer/mapping_2d/pose_graph/spa_cost_function.h +++ b/cartographer/mapping_2d/pose_graph/spa_cost_function_2d.h @@ -14,8 +14,8 @@ * limitations under the License. */ -#ifndef CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_SPA_COST_FUNCTION_H_ -#define CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_SPA_COST_FUNCTION_H_ +#ifndef CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_SPA_COST_FUNCTION_2D_H_ +#define CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_SPA_COST_FUNCTION_2D_H_ #include @@ -30,25 +30,23 @@ #include "ceres/jet.h" namespace cartographer { -namespace mapping_2d { +namespace mapping { namespace pose_graph { -class SpaCostFunction { +class SpaCostFunction2D { public: - using Constraint = mapping::PoseGraph::Constraint; - static ceres::CostFunction* CreateAutoDiffCostFunction( - const Constraint::Pose& pose) { - return new ceres::AutoDiffCostFunction( - new SpaCostFunction(pose)); + new SpaCostFunction2D(pose)); } template bool operator()(const T* const c_i, const T* const c_j, T* e) const { - using mapping::pose_graph::ComputeUnscaledError; - using mapping::pose_graph::ScaleError; + using pose_graph::ComputeUnscaledError; + using pose_graph::ScaleError; const std::array error = ScaleError( ComputeUnscaledError(transform::Project2D(pose_.zbar_ij), c_i, c_j), @@ -58,13 +56,14 @@ class SpaCostFunction { } private: - explicit SpaCostFunction(const Constraint::Pose& pose) : pose_(pose) {} + explicit SpaCostFunction2D(const PoseGraph::Constraint::Pose& pose) + : pose_(pose) {} - const Constraint::Pose pose_; + const PoseGraph::Constraint::Pose pose_; }; } // namespace pose_graph -} // namespace mapping_2d +} // namespace mapping } // namespace cartographer -#endif // CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_SPA_COST_FUNCTION_H_ +#endif // CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_SPA_COST_FUNCTION_2D_H_ diff --git a/cartographer/mapping_2d/pose_graph_2d.cc b/cartographer/mapping_2d/pose_graph_2d.cc index ec38597..053825f 100644 --- a/cartographer/mapping_2d/pose_graph_2d.cc +++ b/cartographer/mapping_2d/pose_graph_2d.cc @@ -86,9 +86,8 @@ std::vector PoseGraph2D::InitializeGlobalSubmapPoses( optimization_problem_.AddSubmap( trajectory_id, first_submap_pose * - mapping_2d::pose_graph::ComputeSubmapPose(*insertion_submaps[0]) - .inverse() * - mapping_2d::pose_graph::ComputeSubmapPose(*insertion_submaps[1])); + pose_graph::ComputeSubmapPose(*insertion_submaps[0]).inverse() * + pose_graph::ComputeSubmapPose(*insertion_submaps[1])); return {last_submap_id, SubmapId{trajectory_id, last_submap_id.submap_index + 1}}; } @@ -244,8 +243,7 @@ void PoseGraph2D::ComputeConstraintsForNode( transform::Rigid3d::Rotation(constant_data->gravity_alignment.inverse())); const transform::Rigid2d optimized_pose = optimization_problem_.submap_data().at(matching_id).global_pose * - mapping_2d::pose_graph::ComputeSubmapPose(*insertion_submaps.front()) - .inverse() * + pose_graph::ComputeSubmapPose(*insertion_submaps.front()).inverse() * pose; optimization_problem_.AddTrajectoryNode( matching_id.trajectory_id, constant_data->time, pose, optimized_pose, @@ -257,9 +255,7 @@ void PoseGraph2D::ComputeConstraintsForNode( CHECK(submap_data_.at(submap_id).state == SubmapState::kActive); submap_data_.at(submap_id).node_ids.emplace(node_id); const transform::Rigid2d constraint_transform = - mapping_2d::pose_graph::ComputeSubmapPose(*insertion_submaps[i]) - .inverse() * - pose; + pose_graph::ComputeSubmapPose(*insertion_submaps[i]).inverse() * pose; constraints_.push_back(Constraint{submap_id, node_id, {transform::Embed3D(constraint_transform), @@ -325,7 +321,7 @@ void PoseGraph2D::UpdateTrajectoryConnectivity(const Constraint& constraint) { void PoseGraph2D::HandleWorkQueue() { constraint_builder_.WhenDone( - [this](const mapping_2d::pose_graph::ConstraintBuilder::Result& result) { + [this](const pose_graph::ConstraintBuilder2D::Result& result) { { common::MutexLocker locker(&mutex_); constraints_.insert(constraints_.end(), result.begin(), result.end()); @@ -386,8 +382,8 @@ void PoseGraph2D::WaitForAllComputations() { } std::cout << "\r\x1b[KOptimizing: Done. " << std::endl; constraint_builder_.WhenDone( - [this, ¬ification]( - const mapping_2d::pose_graph::ConstraintBuilder::Result& result) { + [this, + ¬ification](const pose_graph::ConstraintBuilder2D::Result& result) { common::MutexLocker locker(&mutex_); constraints_.insert(constraints_.end(), result.begin(), result.end()); notification = true; @@ -440,7 +436,8 @@ void PoseGraph2D::AddSubmapFromProto( submap_data_.at(submap_id).submap = submap_ptr; // Immediately show the submap at the 'global_submap_pose'. global_submap_poses_.Insert( - submap_id, mapping_2d::pose_graph::SubmapData{global_submap_pose_2d}); + submap_id, + pose_graph::OptimizationProblem2D::SubmapData{global_submap_pose_2d}); AddWorkItem([this, submap_id, global_submap_pose_2d]() REQUIRES(mutex_) { CHECK_EQ(frozen_trajectories_.count(submap_id.trajectory_id), 1); submap_data_.at(submap_id).state = SubmapState::kFinished; @@ -728,7 +725,7 @@ PoseGraph2D::GetAllSubmapPoses() { } transform::Rigid3d PoseGraph2D::ComputeLocalToGlobalTransform( - const MapById& + const MapById& global_submap_poses, const int trajectory_id) const { auto begin_it = global_submap_poses.BeginOfTrajectory(trajectory_id); diff --git a/cartographer/mapping_2d/pose_graph_2d.h b/cartographer/mapping_2d/pose_graph_2d.h index a104937..92d30d6 100644 --- a/cartographer/mapping_2d/pose_graph_2d.h +++ b/cartographer/mapping_2d/pose_graph_2d.h @@ -35,8 +35,8 @@ #include "cartographer/mapping/pose_graph.h" #include "cartographer/mapping/pose_graph_trimmer.h" #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/pose_graph/constraint_builder_2d.h" +#include "cartographer/mapping_2d/pose_graph/optimization_problem_2d.h" #include "cartographer/mapping_2d/submap_2d.h" #include "cartographer/sensor/fixed_frame_pose_data.h" #include "cartographer/sensor/landmark_data.h" @@ -190,7 +190,7 @@ class PoseGraph2D : public PoseGraph { // Computes the local to global map frame transform based on the given // 'global_submap_poses'. transform::Rigid3d ComputeLocalToGlobalTransform( - const MapById& + const MapById& global_submap_poses, int trajectory_id) const REQUIRES(mutex_); @@ -230,9 +230,8 @@ class PoseGraph2D : public PoseGraph { void DispatchOptimization() REQUIRES(mutex_); // Current optimization problem. - mapping_2d::pose_graph::OptimizationProblem optimization_problem_; - mapping_2d::pose_graph::ConstraintBuilder constraint_builder_ - GUARDED_BY(mutex_); + pose_graph::OptimizationProblem2D optimization_problem_; + pose_graph::ConstraintBuilder2D constraint_builder_ GUARDED_BY(mutex_); std::vector constraints_ GUARDED_BY(mutex_); // Submaps get assigned an ID and state as soon as they are seen, even @@ -244,8 +243,8 @@ class PoseGraph2D : public PoseGraph { int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0; // Global submap poses currently used for displaying data. - MapById global_submap_poses_ - GUARDED_BY(mutex_); + MapById + global_submap_poses_ GUARDED_BY(mutex_); // Global landmark poses with all observations. std::map diff --git a/cartographer/mapping_2d/proto/local_trajectory_builder_options_2d.proto b/cartographer/mapping_2d/proto/local_trajectory_builder_options_2d.proto index d344649..5829384 100644 --- a/cartographer/mapping_2d/proto/local_trajectory_builder_options_2d.proto +++ b/cartographer/mapping_2d/proto/local_trajectory_builder_options_2d.proto @@ -19,8 +19,8 @@ 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_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"; +import "cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options_2d.proto"; +import "cartographer/mapping/scan_matching/proto/real_time_correlative_scan_matcher_options.proto"; message LocalTrajectoryBuilderOptions2D { // Rangefinder points outside these ranges will be dropped. @@ -51,10 +51,9 @@ message LocalTrajectoryBuilderOptions2D { // 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; - cartographer.mapping_2d.scan_matching.proto - .RealTimeCorrelativeScanMatcherOptions - real_time_correlative_scan_matcher_options = 7; - cartographer.mapping_2d.scan_matching.proto.CeresScanMatcherOptions + cartographer.mapping.scan_matching.proto.RealTimeCorrelativeScanMatcherOptions + real_time_correlative_scan_matcher_options = 7; + cartographer.mapping.scan_matching.proto.CeresScanMatcherOptions2D ceres_scan_matcher_options = 8; MotionFilterOptions motion_filter_options = 13; diff --git a/cartographer/mapping_2d/scan_matching/ceres_scan_matcher.cc b/cartographer/mapping_2d/scan_matching/ceres_scan_matcher_2d.cc similarity index 75% rename from cartographer/mapping_2d/scan_matching/ceres_scan_matcher.cc rename to cartographer/mapping_2d/scan_matching/ceres_scan_matcher_2d.cc index d87008e..e98eb6d 100644 --- a/cartographer/mapping_2d/scan_matching/ceres_scan_matcher.cc +++ b/cartographer/mapping_2d/scan_matching/ceres_scan_matcher_2d.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h" +#include "cartographer/mapping_2d/scan_matching/ceres_scan_matcher_2d.h" #include #include @@ -22,21 +22,21 @@ #include "Eigen/Core" #include "cartographer/common/ceres_solver_options.h" #include "cartographer/common/lua_parameter_dictionary.h" -#include "cartographer/internal/mapping_2d/scan_matching/occupied_space_cost_function.h" +#include "cartographer/internal/mapping_2d/scan_matching/occupied_space_cost_function_2d.h" #include "cartographer/mapping_2d/probability_grid.h" -#include "cartographer/mapping_2d/scan_matching/rotation_delta_cost_functor.h" -#include "cartographer/mapping_2d/scan_matching/translation_delta_cost_functor.h" +#include "cartographer/mapping_2d/scan_matching/rotation_delta_cost_functor_2d.h" +#include "cartographer/mapping_2d/scan_matching/translation_delta_cost_functor_2d.h" #include "cartographer/transform/transform.h" #include "ceres/ceres.h" #include "glog/logging.h" namespace cartographer { -namespace mapping_2d { +namespace mapping { namespace scan_matching { -proto::CeresScanMatcherOptions CreateCeresScanMatcherOptions( +proto::CeresScanMatcherOptions2D CreateCeresScanMatcherOptions2D( common::LuaParameterDictionary* const parameter_dictionary) { - proto::CeresScanMatcherOptions options; + proto::CeresScanMatcherOptions2D options; options.set_occupied_space_weight( parameter_dictionary->GetDouble("occupied_space_weight")); options.set_translation_weight( @@ -49,41 +49,41 @@ proto::CeresScanMatcherOptions CreateCeresScanMatcherOptions( return options; } -CeresScanMatcher::CeresScanMatcher( - const proto::CeresScanMatcherOptions& options) +CeresScanMatcher2D::CeresScanMatcher2D( + const proto::CeresScanMatcherOptions2D& options) : options_(options), ceres_solver_options_( common::CreateCeresSolverOptions(options.ceres_solver_options())) { ceres_solver_options_.linear_solver_type = ceres::DENSE_QR; } -CeresScanMatcher::~CeresScanMatcher() {} +CeresScanMatcher2D::~CeresScanMatcher2D() {} -void CeresScanMatcher::Match(const Eigen::Vector2d& target_translation, - const transform::Rigid2d& initial_pose_estimate, - const sensor::PointCloud& point_cloud, - const mapping::ProbabilityGrid& probability_grid, - transform::Rigid2d* const pose_estimate, - ceres::Solver::Summary* const summary) const { +void CeresScanMatcher2D::Match(const Eigen::Vector2d& target_translation, + const transform::Rigid2d& initial_pose_estimate, + const sensor::PointCloud& point_cloud, + const ProbabilityGrid& probability_grid, + transform::Rigid2d* const pose_estimate, + ceres::Solver::Summary* const summary) const { double ceres_pose_estimate[3] = {initial_pose_estimate.translation().x(), initial_pose_estimate.translation().y(), initial_pose_estimate.rotation().angle()}; ceres::Problem problem; CHECK_GT(options_.occupied_space_weight(), 0.); problem.AddResidualBlock( - OccupiedSpaceCostFunction::CreateAutoDiffCostFunction( + OccupiedSpaceCostFunction2D::CreateAutoDiffCostFunction( options_.occupied_space_weight() / std::sqrt(static_cast(point_cloud.size())), point_cloud, probability_grid), nullptr /* loss function */, ceres_pose_estimate); CHECK_GT(options_.translation_weight(), 0.); problem.AddResidualBlock( - TranslationDeltaCostFunctor::CreateAutoDiffCostFunction( + TranslationDeltaCostFunctor2D::CreateAutoDiffCostFunction( options_.translation_weight(), target_translation), nullptr /* loss function */, ceres_pose_estimate); CHECK_GT(options_.rotation_weight(), 0.); problem.AddResidualBlock( - RotationDeltaCostFunctor::CreateAutoDiffCostFunction( + RotationDeltaCostFunctor2D::CreateAutoDiffCostFunction( options_.rotation_weight(), ceres_pose_estimate[2]), nullptr /* loss function */, ceres_pose_estimate); @@ -94,5 +94,5 @@ void CeresScanMatcher::Match(const Eigen::Vector2d& target_translation, } } // namespace scan_matching -} // namespace mapping_2d +} // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h b/cartographer/mapping_2d/scan_matching/ceres_scan_matcher_2d.h similarity index 77% rename from cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h rename to cartographer/mapping_2d/scan_matching/ceres_scan_matcher_2d.h index 5858865..5b124e1 100644 --- a/cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h +++ b/cartographer/mapping_2d/scan_matching/ceres_scan_matcher_2d.h @@ -14,8 +14,8 @@ * limitations under the License. */ -#ifndef CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_CERES_SCAN_MATCHER_H_ -#define CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_CERES_SCAN_MATCHER_H_ +#ifndef CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_CERES_SCAN_MATCHER_2D_H_ +#define CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_CERES_SCAN_MATCHER_2D_H_ #include #include @@ -23,25 +23,25 @@ #include "Eigen/Core" #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/mapping_2d/probability_grid.h" -#include "cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options.pb.h" +#include "cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options_2d.pb.h" #include "cartographer/sensor/point_cloud.h" #include "ceres/ceres.h" namespace cartographer { -namespace mapping_2d { +namespace mapping { namespace scan_matching { -proto::CeresScanMatcherOptions CreateCeresScanMatcherOptions( +proto::CeresScanMatcherOptions2D CreateCeresScanMatcherOptions2D( common::LuaParameterDictionary* parameter_dictionary); // Align scans with an existing map using Ceres. -class CeresScanMatcher { +class CeresScanMatcher2D { public: - explicit CeresScanMatcher(const proto::CeresScanMatcherOptions& options); - virtual ~CeresScanMatcher(); + explicit CeresScanMatcher2D(const proto::CeresScanMatcherOptions2D& options); + virtual ~CeresScanMatcher2D(); - CeresScanMatcher(const CeresScanMatcher&) = delete; - CeresScanMatcher& operator=(const CeresScanMatcher&) = delete; + CeresScanMatcher2D(const CeresScanMatcher2D&) = delete; + CeresScanMatcher2D& operator=(const CeresScanMatcher2D&) = delete; // Aligns 'point_cloud' within the 'probability_grid' given an // 'initial_pose_estimate' and returns a 'pose_estimate' and the solver @@ -49,17 +49,17 @@ class CeresScanMatcher { void Match(const Eigen::Vector2d& target_translation, const transform::Rigid2d& initial_pose_estimate, const sensor::PointCloud& point_cloud, - const mapping::ProbabilityGrid& probability_grid, + const ProbabilityGrid& probability_grid, transform::Rigid2d* pose_estimate, ceres::Solver::Summary* summary) const; private: - const proto::CeresScanMatcherOptions options_; + const proto::CeresScanMatcherOptions2D options_; ceres::Solver::Options ceres_solver_options_; }; } // namespace scan_matching -} // namespace mapping_2d +} // namespace mapping } // namespace cartographer -#endif // CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_CERES_SCAN_MATCHER_H_ +#endif // CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_CERES_SCAN_MATCHER_2D_H_ diff --git a/cartographer/mapping_2d/scan_matching/ceres_scan_matcher_test.cc b/cartographer/mapping_2d/scan_matching/ceres_scan_matcher_2d_test.cc similarity index 85% rename from cartographer/mapping_2d/scan_matching/ceres_scan_matcher_test.cc rename to cartographer/mapping_2d/scan_matching/ceres_scan_matcher_2d_test.cc index 7c2119c..d5b49c6 100644 --- a/cartographer/mapping_2d/scan_matching/ceres_scan_matcher_test.cc +++ b/cartographer/mapping_2d/scan_matching/ceres_scan_matcher_2d_test.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h" +#include "cartographer/mapping_2d/scan_matching/ceres_scan_matcher_2d.h" #include @@ -28,18 +28,18 @@ #include "gtest/gtest.h" namespace cartographer { -namespace mapping_2d { +namespace mapping { namespace scan_matching { namespace { class CeresScanMatcherTest : public ::testing::Test { protected: CeresScanMatcherTest() - : probability_grid_(mapping::MapLimits(1., Eigen::Vector2d(10., 10.), - mapping::CellLimits(20, 20))) { + : probability_grid_( + MapLimits(1., Eigen::Vector2d(10., 10.), CellLimits(20, 20))) { probability_grid_.SetProbability( probability_grid_.limits().GetCellIndex(Eigen::Vector2f(-3.5f, 2.5f)), - mapping::kMaxProbability); + kMaxProbability); point_cloud_.emplace_back(-3.f, 2.f, 0.f); @@ -54,9 +54,9 @@ class CeresScanMatcherTest : public ::testing::Test { num_threads = 1, }, })text"); - const proto::CeresScanMatcherOptions options = - CreateCeresScanMatcherOptions(parameter_dictionary.get()); - ceres_scan_matcher_ = common::make_unique(options); + const proto::CeresScanMatcherOptions2D options = + CreateCeresScanMatcherOptions2D(parameter_dictionary.get()); + ceres_scan_matcher_ = common::make_unique(options); } void TestFromInitialPose(const transform::Rigid2d& initial_pose) { @@ -73,9 +73,9 @@ class CeresScanMatcherTest : public ::testing::Test { << "\nExpected: " << transform::ToProto(expected_pose).DebugString(); } - mapping::ProbabilityGrid probability_grid_; + ProbabilityGrid probability_grid_; sensor::PointCloud point_cloud_; - std::unique_ptr ceres_scan_matcher_; + std::unique_ptr ceres_scan_matcher_; }; TEST_F(CeresScanMatcherTest, testPerfectEstimate) { @@ -96,5 +96,5 @@ TEST_F(CeresScanMatcherTest, testOptimizeAlongXY) { } // namespace } // namespace scan_matching -} // namespace mapping_2d +} // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping_2d/scan_matching/correlative_scan_matcher.cc b/cartographer/mapping_2d/scan_matching/correlative_scan_matcher_2d.cc similarity index 95% rename from cartographer/mapping_2d/scan_matching/correlative_scan_matcher.cc rename to cartographer/mapping_2d/scan_matching/correlative_scan_matcher_2d.cc index 9c053e4..82fdece 100644 --- a/cartographer/mapping_2d/scan_matching/correlative_scan_matcher.cc +++ b/cartographer/mapping_2d/scan_matching/correlative_scan_matcher_2d.cc @@ -14,14 +14,14 @@ * limitations under the License. */ -#include "cartographer/mapping_2d/scan_matching/correlative_scan_matcher.h" +#include "cartographer/mapping_2d/scan_matching/correlative_scan_matcher_2d.h" #include #include "cartographer/common/math.h" namespace cartographer { -namespace mapping_2d { +namespace mapping { namespace scan_matching { SearchParameters::SearchParameters(const double linear_search_window, @@ -71,7 +71,7 @@ SearchParameters::SearchParameters(const int num_linear_perturbations, } void SearchParameters::ShrinkToFit(const std::vector& scans, - const mapping::CellLimits& cell_limits) { + const 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,8 +109,7 @@ std::vector GenerateRotatedScans( } std::vector DiscretizeScans( - const mapping::MapLimits& map_limits, - const std::vector& scans, + const MapLimits& map_limits, const std::vector& scans, const Eigen::Translation2f& initial_translation) { std::vector discrete_scans; discrete_scans.reserve(scans.size()); @@ -128,5 +127,5 @@ std::vector DiscretizeScans( } } // namespace scan_matching -} // namespace mapping_2d +} // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping_2d/scan_matching/correlative_scan_matcher.h b/cartographer/mapping_2d/scan_matching/correlative_scan_matcher_2d.h similarity index 93% rename from cartographer/mapping_2d/scan_matching/correlative_scan_matcher.h rename to cartographer/mapping_2d/scan_matching/correlative_scan_matcher_2d.h index d2fbf5f..ab082f9 100644 --- a/cartographer/mapping_2d/scan_matching/correlative_scan_matcher.h +++ b/cartographer/mapping_2d/scan_matching/correlative_scan_matcher_2d.h @@ -14,8 +14,8 @@ * limitations under the License. */ -#ifndef CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_CORRELATIVE_SCAN_MATCHER_H_ -#define CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_CORRELATIVE_SCAN_MATCHER_H_ +#ifndef CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_CORRELATIVE_SCAN_MATCHER_2D_H_ +#define CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_CORRELATIVE_SCAN_MATCHER_2D_H_ #include @@ -26,7 +26,7 @@ #include "cartographer/sensor/point_cloud.h" namespace cartographer { -namespace mapping_2d { +namespace mapping { namespace scan_matching { typedef std::vector DiscreteScan; @@ -50,7 +50,7 @@ struct SearchParameters { // Tightens the search window as much as possible. void ShrinkToFit(const std::vector& scans, - const mapping::CellLimits& cell_limits); + const CellLimits& cell_limits); int num_angular_perturbations; double angular_perturbation_step_size; @@ -67,8 +67,7 @@ std::vector GenerateRotatedScans( // Translates and discretizes the rotated scans into a vector of integer // indices. std::vector DiscretizeScans( - const mapping::MapLimits& map_limits, - const std::vector& scans, + const MapLimits& map_limits, const std::vector& scans, const Eigen::Translation2f& initial_translation); // A possible solution. @@ -104,7 +103,7 @@ struct Candidate { }; } // namespace scan_matching -} // namespace mapping_2d +} // namespace mapping } // namespace cartographer -#endif // CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_CORRELATIVE_SCAN_MATCHER_H_ +#endif // CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_CORRELATIVE_SCAN_MATCHER_2D_H_ 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 05b07ac..0e756e7 100644 --- a/cartographer/mapping_2d/scan_matching/correlative_scan_matcher_test.cc +++ b/cartographer/mapping_2d/scan_matching/correlative_scan_matcher_test.cc @@ -14,13 +14,13 @@ * limitations under the License. */ -#include "cartographer/mapping_2d/scan_matching/correlative_scan_matcher.h" +#include "cartographer/mapping_2d/scan_matching/correlative_scan_matcher_2d.h" #include "cartographer/sensor/point_cloud.h" #include "gtest/gtest.h" namespace cartographer { -namespace mapping_2d { +namespace mapping { namespace scan_matching { namespace { @@ -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 mapping::MapLimits map_limits(0.05, Eigen::Vector2d(0.05, 0.25), - mapping::CellLimits(6, 6)); + const MapLimits map_limits(0.05, Eigen::Vector2d(0.05, 0.25), + CellLimits(6, 6)); const std::vector scans = GenerateRotatedScans(point_cloud, SearchParameters(0, 0, 0., 0.)); const std::vector discrete_scans = @@ -98,5 +98,5 @@ TEST(DiscretizeScans, DiscretizeScans) { } // namespace } // namespace scan_matching -} // namespace mapping_2d +} // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.cc b/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_2d.cc similarity index 91% rename from cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.cc rename to cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_2d.cc index ec7bddc..87d0666 100644 --- a/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.cc +++ b/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_2d.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h" +#include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_2d.h" #include #include @@ -30,9 +30,8 @@ #include "glog/logging.h" namespace cartographer { -namespace mapping_2d { +namespace mapping { namespace scan_matching { - namespace { // A collection of values which can be added and later removed, and the maximum @@ -75,10 +74,10 @@ class SlidingWindowMaximum { } // namespace -proto::FastCorrelativeScanMatcherOptions -CreateFastCorrelativeScanMatcherOptions( +proto::FastCorrelativeScanMatcherOptions2D +CreateFastCorrelativeScanMatcherOptions2D( common::LuaParameterDictionary* const parameter_dictionary) { - proto::FastCorrelativeScanMatcherOptions options; + proto::FastCorrelativeScanMatcherOptions2D options; options.set_linear_search_window( parameter_dictionary->GetDouble("linear_search_window")); options.set_angular_search_window( @@ -89,9 +88,8 @@ CreateFastCorrelativeScanMatcherOptions( } PrecomputationGrid::PrecomputationGrid( - const mapping::ProbabilityGrid& probability_grid, - const mapping::CellLimits& limits, const int width, - std::vector* reusable_intermediate_grid) + const ProbabilityGrid& probability_grid, const 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), @@ -160,9 +158,9 @@ PrecomputationGrid::PrecomputationGrid( } uint8 PrecomputationGrid::ComputeCellValue(const float probability) const { - const int cell_value = common::RoundToInt( - (probability - mapping::kMinProbability) * - (255.f / (mapping::kMaxProbability - mapping::kMinProbability))); + const int cell_value = + common::RoundToInt((probability - kMinProbability) * + (255.f / (kMaxProbability - kMinProbability))); CHECK_GE(cell_value, 0); CHECK_LE(cell_value, 255); return cell_value; @@ -171,13 +169,13 @@ uint8 PrecomputationGrid::ComputeCellValue(const float probability) const { class PrecomputationGridStack { public: PrecomputationGridStack( - const mapping::ProbabilityGrid& probability_grid, - const proto::FastCorrelativeScanMatcherOptions& options) { + const ProbabilityGrid& probability_grid, + const proto::FastCorrelativeScanMatcherOptions2D& 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 mapping::CellLimits limits = probability_grid.limits().cell_limits(); + const 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,17 +195,17 @@ class PrecomputationGridStack { std::vector precomputation_grids_; }; -FastCorrelativeScanMatcher::FastCorrelativeScanMatcher( - const mapping::ProbabilityGrid& probability_grid, - const proto::FastCorrelativeScanMatcherOptions& options) +FastCorrelativeScanMatcher2D::FastCorrelativeScanMatcher2D( + const ProbabilityGrid& probability_grid, + const proto::FastCorrelativeScanMatcherOptions2D& options) : options_(options), limits_(probability_grid.limits()), precomputation_grid_stack_( new PrecomputationGridStack(probability_grid, options)) {} -FastCorrelativeScanMatcher::~FastCorrelativeScanMatcher() {} +FastCorrelativeScanMatcher2D::~FastCorrelativeScanMatcher2D() {} -bool FastCorrelativeScanMatcher::Match( +bool FastCorrelativeScanMatcher2D::Match( const transform::Rigid2d& initial_pose_estimate, const sensor::PointCloud& point_cloud, const float min_score, float* score, transform::Rigid2d* pose_estimate) const { @@ -219,7 +217,7 @@ bool FastCorrelativeScanMatcher::Match( pose_estimate); } -bool FastCorrelativeScanMatcher::MatchFullSubmap( +bool FastCorrelativeScanMatcher2D::MatchFullSubmap( const sensor::PointCloud& point_cloud, float min_score, float* score, transform::Rigid2d* pose_estimate) const { // Compute a search window around the center of the submap that includes it @@ -236,7 +234,7 @@ bool FastCorrelativeScanMatcher::MatchFullSubmap( min_score, score, pose_estimate); } -bool FastCorrelativeScanMatcher::MatchWithSearchParameters( +bool FastCorrelativeScanMatcher2D::MatchWithSearchParameters( SearchParameters search_parameters, const transform::Rigid2d& initial_pose_estimate, const sensor::PointCloud& point_cloud, float min_score, float* score, @@ -274,7 +272,7 @@ bool FastCorrelativeScanMatcher::MatchWithSearchParameters( } std::vector -FastCorrelativeScanMatcher::ComputeLowestResolutionCandidates( +FastCorrelativeScanMatcher2D::ComputeLowestResolutionCandidates( const std::vector& discrete_scans, const SearchParameters& search_parameters) const { std::vector lowest_resolution_candidates = @@ -286,7 +284,7 @@ FastCorrelativeScanMatcher::ComputeLowestResolutionCandidates( } std::vector -FastCorrelativeScanMatcher::GenerateLowestResolutionCandidates( +FastCorrelativeScanMatcher2D::GenerateLowestResolutionCandidates( const SearchParameters& search_parameters) const { const int linear_step_size = 1 << precomputation_grid_stack_->max_depth(); int num_candidates = 0; @@ -323,7 +321,7 @@ FastCorrelativeScanMatcher::GenerateLowestResolutionCandidates( return candidates; } -void FastCorrelativeScanMatcher::ScoreCandidates( +void FastCorrelativeScanMatcher2D::ScoreCandidates( const PrecomputationGrid& precomputation_grid, const std::vector& discrete_scans, const SearchParameters& search_parameters, @@ -343,7 +341,7 @@ void FastCorrelativeScanMatcher::ScoreCandidates( std::sort(candidates->begin(), candidates->end(), std::greater()); } -Candidate FastCorrelativeScanMatcher::BranchAndBound( +Candidate FastCorrelativeScanMatcher2D::BranchAndBound( const std::vector& discrete_scans, const SearchParameters& search_parameters, const std::vector& candidates, const int candidate_depth, @@ -389,5 +387,5 @@ Candidate FastCorrelativeScanMatcher::BranchAndBound( } } // namespace scan_matching -} // namespace mapping_2d +} // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h b/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_2d.h similarity index 85% rename from cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h rename to cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_2d.h index a92de8f..a3c210f 100644 --- a/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h +++ b/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_2d.h @@ -22,8 +22,8 @@ // precomputation done for a given map. However, this map is immutable after // construction. -#ifndef CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_H_ -#define CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_H_ +#ifndef CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_2D_H_ +#define CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_2D_H_ #include #include @@ -32,16 +32,16 @@ #include "cartographer/common/port.h" #include "cartographer/mapping/probability_values.h" #include "cartographer/mapping_2d/probability_grid.h" -#include "cartographer/mapping_2d/scan_matching/correlative_scan_matcher.h" -#include "cartographer/mapping_2d/scan_matching/proto/fast_correlative_scan_matcher_options.pb.h" +#include "cartographer/mapping_2d/scan_matching/correlative_scan_matcher_2d.h" +#include "cartographer/mapping_2d/scan_matching/proto/fast_correlative_scan_matcher_options_2d.pb.h" #include "cartographer/sensor/point_cloud.h" namespace cartographer { -namespace mapping_2d { +namespace mapping { namespace scan_matching { -proto::FastCorrelativeScanMatcherOptions -CreateFastCorrelativeScanMatcherOptions( +proto::FastCorrelativeScanMatcherOptions2D +CreateFastCorrelativeScanMatcherOptions2D( common::LuaParameterDictionary* parameter_dictionary); // A precomputed grid that contains in each cell (x0, y0) the maximum @@ -49,8 +49,8 @@ CreateFastCorrelativeScanMatcherOptions( // y0 <= y < y0. class PrecomputationGrid { public: - PrecomputationGrid(const mapping::ProbabilityGrid& probability_grid, - const mapping::CellLimits& limits, int width, + PrecomputationGrid(const ProbabilityGrid& probability_grid, + const CellLimits& limits, int width, std::vector* reusable_intermediate_grid); // Returns a value between 0 and 255 to represent probabilities between @@ -74,9 +74,8 @@ class PrecomputationGrid { // Maps values from [0, 255] to [kMinProbability, kMaxProbability]. static float ToProbability(float value) { - return mapping::kMinProbability + - value * - ((mapping::kMaxProbability - mapping::kMinProbability) / 255.f); + return kMinProbability + + value * ((kMaxProbability - kMinProbability) / 255.f); } private: @@ -87,7 +86,7 @@ class PrecomputationGrid { const Eigen::Array2i offset_; // Size of the precomputation grid. - const mapping::CellLimits wide_limits_; + const CellLimits wide_limits_; // Probabilites mapped to 0 to 255. std::vector cells_; @@ -96,15 +95,15 @@ class PrecomputationGrid { class PrecomputationGridStack; // An implementation of "Real-Time Correlative Scan Matching" by Olson. -class FastCorrelativeScanMatcher { +class FastCorrelativeScanMatcher2D { public: - FastCorrelativeScanMatcher( - const mapping::ProbabilityGrid& probability_grid, - const proto::FastCorrelativeScanMatcherOptions& options); - ~FastCorrelativeScanMatcher(); + FastCorrelativeScanMatcher2D( + const ProbabilityGrid& probability_grid, + const proto::FastCorrelativeScanMatcherOptions2D& options); + ~FastCorrelativeScanMatcher2D(); - FastCorrelativeScanMatcher(const FastCorrelativeScanMatcher&) = delete; - FastCorrelativeScanMatcher& operator=(const FastCorrelativeScanMatcher&) = + FastCorrelativeScanMatcher2D(const FastCorrelativeScanMatcher2D&) = delete; + FastCorrelativeScanMatcher2D& operator=(const FastCorrelativeScanMatcher2D&) = delete; // Aligns 'point_cloud' within the 'probability_grid' given an @@ -145,13 +144,13 @@ class FastCorrelativeScanMatcher { const std::vector& candidates, int candidate_depth, float min_score) const; - const proto::FastCorrelativeScanMatcherOptions options_; - mapping::MapLimits limits_; + const proto::FastCorrelativeScanMatcherOptions2D options_; + MapLimits limits_; std::unique_ptr precomputation_grid_stack_; }; } // namespace scan_matching -} // namespace mapping_2d +} // namespace mapping } // namespace cartographer -#endif // CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_H_ +#endif // CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_2D_H_ diff --git a/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_test.cc b/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_2d_test.cc similarity index 81% rename from cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_test.cc rename to cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_2d_test.cc index 4503ff1..2e00f87 100644 --- a/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_test.cc +++ b/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_2d_test.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h" +#include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_2d.h" #include #include @@ -30,7 +30,7 @@ #include "gtest/gtest.h" namespace cartographer { -namespace mapping_2d { +namespace mapping { namespace scan_matching { namespace { @@ -39,10 +39,10 @@ TEST(PrecomputationGridTest, CorrectValues) { // represented by uint8 values. std::mt19937 prng(42); std::uniform_int_distribution distribution(0, 255); - 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))) { + 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))) { 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 : mapping::XYIndexRangeIterator( - probability_grid.limits().cell_limits())) { + for (const Eigen::Array2i& xy_index : + 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); - mapping::ProbabilityGrid probability_grid(mapping::MapLimits( - 0.05, Eigen::Vector2d(0.1, 0.1), mapping::CellLimits(4, 4))); + ProbabilityGrid probability_grid( + MapLimits(0.05, Eigen::Vector2d(0.1, 0.1), CellLimits(4, 4))); for (const Eigen::Array2i& xy_index : - mapping::XYIndexRangeIterator(probability_grid.limits().cell_limits())) { + 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 : mapping::XYIndexRangeIterator( - probability_grid.limits().cell_limits())) { + for (const Eigen::Array2i& xy_index : + 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) { @@ -104,8 +104,9 @@ TEST(PrecomputationGridTest, TinyProbabilityGrid) { } } -proto::FastCorrelativeScanMatcherOptions -CreateFastCorrelativeScanMatcherTestOptions(const int branch_and_bound_depth) { +proto::FastCorrelativeScanMatcherOptions2D +CreateFastCorrelativeScanMatcherTestOptions2D( + const int branch_and_bound_depth) { auto parameter_dictionary = common::MakeDictionary(R"text( return { @@ -113,7 +114,7 @@ CreateFastCorrelativeScanMatcherTestOptions(const int branch_and_bound_depth) { angular_search_window = 1., branch_and_bound_depth = )text" + std::to_string(branch_and_bound_depth) + "}"); - return CreateFastCorrelativeScanMatcherOptions(parameter_dictionary.get()); + return CreateFastCorrelativeScanMatcherOptions2D(parameter_dictionary.get()); } mapping::proto::RangeDataInserterOptions2D @@ -130,10 +131,10 @@ CreateRangeDataInserterTestOptions2D() { TEST(FastCorrelativeScanMatcherTest, CorrectPose) { std::mt19937 prng(42); std::uniform_real_distribution distribution(-1.f, 1.f); - mapping::RangeDataInserter2D range_data_inserter( + RangeDataInserter2D range_data_inserter( CreateRangeDataInserterTestOptions2D()); constexpr float kMinScore = 0.1f; - const auto options = CreateFastCorrelativeScanMatcherTestOptions(3); + const auto options = CreateFastCorrelativeScanMatcherTestOptions2D(3); sensor::PointCloud point_cloud; point_cloud.emplace_back(-2.5f, 0.5f, 0.f); @@ -148,8 +149,8 @@ TEST(FastCorrelativeScanMatcherTest, CorrectPose) { {2. * distribution(prng), 2. * distribution(prng)}, 0.5 * distribution(prng)); - mapping::ProbabilityGrid probability_grid(mapping::MapLimits( - 0.05, Eigen::Vector2d(5., 5.), mapping::CellLimits(200, 200))); + ProbabilityGrid probability_grid( + MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(200, 200))); range_data_inserter.Insert( sensor::RangeData{ Eigen::Vector3f(expected_pose.translation().x(), @@ -160,8 +161,8 @@ TEST(FastCorrelativeScanMatcherTest, CorrectPose) { &probability_grid); probability_grid.FinishUpdate(); - FastCorrelativeScanMatcher fast_correlative_scan_matcher(probability_grid, - options); + FastCorrelativeScanMatcher2D fast_correlative_scan_matcher(probability_grid, + options); transform::Rigid2d pose_estimate; float score; EXPECT_TRUE(fast_correlative_scan_matcher.Match( @@ -178,10 +179,10 @@ TEST(FastCorrelativeScanMatcherTest, CorrectPose) { TEST(FastCorrelativeScanMatcherTest, FullSubmapMatching) { std::mt19937 prng(42); std::uniform_real_distribution distribution(-1.f, 1.f); - mapping::RangeDataInserter2D range_data_inserter( + RangeDataInserter2D range_data_inserter( CreateRangeDataInserterTestOptions2D()); constexpr float kMinScore = 0.1f; - const auto options = CreateFastCorrelativeScanMatcherTestOptions(6); + const auto options = CreateFastCorrelativeScanMatcherTestOptions2D(6); sensor::PointCloud unperturbed_point_cloud; unperturbed_point_cloud.emplace_back(-2.5f, 0.5f, 0.f); @@ -202,8 +203,8 @@ TEST(FastCorrelativeScanMatcherTest, FullSubmapMatching) { 0.5 * distribution(prng)) * perturbation.inverse(); - mapping::ProbabilityGrid probability_grid(mapping::MapLimits( - 0.05, Eigen::Vector2d(5., 5.), mapping::CellLimits(200, 200))); + ProbabilityGrid probability_grid( + MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(200, 200))); range_data_inserter.Insert( sensor::RangeData{ transform::Embed3D(expected_pose * perturbation).translation(), @@ -213,8 +214,8 @@ TEST(FastCorrelativeScanMatcherTest, FullSubmapMatching) { &probability_grid); probability_grid.FinishUpdate(); - FastCorrelativeScanMatcher fast_correlative_scan_matcher(probability_grid, - options); + FastCorrelativeScanMatcher2D fast_correlative_scan_matcher(probability_grid, + options); transform::Rigid2d pose_estimate; float score; EXPECT_TRUE(fast_correlative_scan_matcher.MatchFullSubmap( @@ -229,5 +230,5 @@ TEST(FastCorrelativeScanMatcherTest, FullSubmapMatching) { } // namespace } // namespace scan_matching -} // namespace mapping_2d +} // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping_2d/scan_matching/fast_global_localizer.cc b/cartographer/mapping_2d/scan_matching/fast_global_localizer.cc index fa245c2..02c4f5f 100644 --- a/cartographer/mapping_2d/scan_matching/fast_global_localizer.cc +++ b/cartographer/mapping_2d/scan_matching/fast_global_localizer.cc @@ -19,16 +19,13 @@ #include "glog/logging.h" namespace cartographer { -namespace mapping_2d { +namespace mapping { namespace scan_matching { bool PerformGlobalLocalization( - const float cutoff, - const cartographer::sensor::AdaptiveVoxelFilter& voxel_filter, - const std::vector< - cartographer::mapping_2d::scan_matching::FastCorrelativeScanMatcher*>& - matchers, - const cartographer::sensor::PointCloud& point_cloud, + const float cutoff, const sensor::AdaptiveVoxelFilter& voxel_filter, + const std::vector& matchers, + const sensor::PointCloud& point_cloud, transform::Rigid2d* const best_pose_estimate, float* const best_score) { CHECK(best_pose_estimate != nullptr) << "Need a non-null output_pose_estimate!"; @@ -57,5 +54,5 @@ bool PerformGlobalLocalization( } } // namespace scan_matching -} // namespace mapping_2d +} // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping_2d/scan_matching/fast_global_localizer.h b/cartographer/mapping_2d/scan_matching/fast_global_localizer.h index 6fec202..e15f204 100644 --- a/cartographer/mapping_2d/scan_matching/fast_global_localizer.h +++ b/cartographer/mapping_2d/scan_matching/fast_global_localizer.h @@ -20,11 +20,11 @@ #include #include "Eigen/Geometry" -#include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h" +#include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_2d.h" #include "cartographer/sensor/voxel_filter.h" namespace cartographer { -namespace mapping_2d { +namespace mapping { namespace scan_matching { // Perform global localization against the provided 'matchers'. The 'cutoff' @@ -37,15 +37,13 @@ namespace scan_matching { // should not be trusted if the function returns false. The 'cutoff' and // 'best_score' are in the range [0., 1.]. bool PerformGlobalLocalization( - float cutoff, const cartographer::sensor::AdaptiveVoxelFilter& voxel_filter, - const std::vector< - cartographer::mapping_2d::scan_matching::FastCorrelativeScanMatcher*>& - matchers, - const cartographer::sensor::PointCloud& point_cloud, + float cutoff, const sensor::AdaptiveVoxelFilter& voxel_filter, + const std::vector& matchers, + const sensor::PointCloud& point_cloud, transform::Rigid2d* best_pose_estimate, float* best_score); } // namespace scan_matching -} // namespace mapping_2d +} // namespace mapping } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_FAST_GLOBAL_LOCALIZER_H_ diff --git a/cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options.proto b/cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options_2d.proto similarity index 92% rename from cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options.proto rename to cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options_2d.proto index 20f0a75..f1bcf48 100644 --- a/cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options.proto +++ b/cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options_2d.proto @@ -14,12 +14,12 @@ syntax = "proto3"; -package cartographer.mapping_2d.scan_matching.proto; +package cartographer.mapping.scan_matching.proto; import "cartographer/common/proto/ceres_solver_options.proto"; // NEXT ID: 10 -message CeresScanMatcherOptions { +message CeresScanMatcherOptions2D { // Scaling parameters for each cost functor. double occupied_space_weight = 1; double translation_weight = 2; diff --git a/cartographer/mapping_2d/scan_matching/proto/fast_correlative_scan_matcher_options.proto b/cartographer/mapping_2d/scan_matching/proto/fast_correlative_scan_matcher_options_2d.proto similarity index 90% rename from cartographer/mapping_2d/scan_matching/proto/fast_correlative_scan_matcher_options.proto rename to cartographer/mapping_2d/scan_matching/proto/fast_correlative_scan_matcher_options_2d.proto index 41cef1b..3e5fe37 100644 --- a/cartographer/mapping_2d/scan_matching/proto/fast_correlative_scan_matcher_options.proto +++ b/cartographer/mapping_2d/scan_matching/proto/fast_correlative_scan_matcher_options_2d.proto @@ -14,9 +14,9 @@ syntax = "proto3"; -package cartographer.mapping_2d.scan_matching.proto; +package cartographer.mapping.scan_matching.proto; -message FastCorrelativeScanMatcherOptions { +message FastCorrelativeScanMatcherOptions2D { // Minimum linear search window in which the best possible scan alignment // will be found. double linear_search_window = 3; 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_2d.cc similarity index 81% rename from cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.cc rename to cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_2d.cc index 31f75c9..2e93689 100644 --- a/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.cc +++ b/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_2d.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.h" +#include "cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_2d.h" #include #include @@ -30,32 +30,15 @@ #include "glog/logging.h" namespace cartographer { -namespace mapping_2d { +namespace mapping { namespace scan_matching { -proto::RealTimeCorrelativeScanMatcherOptions -CreateRealTimeCorrelativeScanMatcherOptions( - common::LuaParameterDictionary* const parameter_dictionary) { - proto::RealTimeCorrelativeScanMatcherOptions options; - options.set_linear_search_window( - parameter_dictionary->GetDouble("linear_search_window")); - options.set_angular_search_window( - parameter_dictionary->GetDouble("angular_search_window")); - options.set_translation_delta_cost_weight( - parameter_dictionary->GetDouble("translation_delta_cost_weight")); - options.set_rotation_delta_cost_weight( - parameter_dictionary->GetDouble("rotation_delta_cost_weight")); - CHECK_GE(options.translation_delta_cost_weight(), 0.); - CHECK_GE(options.rotation_delta_cost_weight(), 0.); - return options; -} - -RealTimeCorrelativeScanMatcher::RealTimeCorrelativeScanMatcher( +RealTimeCorrelativeScanMatcher2D::RealTimeCorrelativeScanMatcher2D( const proto::RealTimeCorrelativeScanMatcherOptions& options) : options_(options) {} std::vector -RealTimeCorrelativeScanMatcher::GenerateExhaustiveSearchCandidates( +RealTimeCorrelativeScanMatcher2D::GenerateExhaustiveSearchCandidates( const SearchParameters& search_parameters) const { int num_candidates = 0; for (int scan_index = 0; scan_index != search_parameters.num_scans; @@ -88,10 +71,10 @@ RealTimeCorrelativeScanMatcher::GenerateExhaustiveSearchCandidates( return candidates; } -double RealTimeCorrelativeScanMatcher::Match( +double RealTimeCorrelativeScanMatcher2D::Match( const transform::Rigid2d& initial_pose_estimate, const sensor::PointCloud& point_cloud, - const mapping::ProbabilityGrid& probability_grid, + const ProbabilityGrid& probability_grid, transform::Rigid2d* pose_estimate) const { CHECK_NOTNULL(pose_estimate); @@ -124,8 +107,8 @@ double RealTimeCorrelativeScanMatcher::Match( return best_candidate.score; } -void RealTimeCorrelativeScanMatcher::ScoreCandidates( - const mapping::ProbabilityGrid& probability_grid, +void RealTimeCorrelativeScanMatcher2D::ScoreCandidates( + const ProbabilityGrid& probability_grid, const std::vector& discrete_scans, const SearchParameters& search_parameters, std::vector* const candidates) const { @@ -152,5 +135,5 @@ void RealTimeCorrelativeScanMatcher::ScoreCandidates( } } // namespace scan_matching -} // namespace mapping_2d +} // namespace mapping } // namespace cartographer 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_2d.h similarity index 80% rename from cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.h rename to cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_2d.h index 5437f60..884617a 100644 --- a/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.h +++ b/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_2d.h @@ -33,43 +33,40 @@ // This can be made even faster by transforming the scan exactly once over some // discretized range. -#ifndef CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_H_ -#define CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_H_ +#ifndef CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_2D_H_ +#define CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_2D_H_ #include #include #include #include "Eigen/Core" +#include "cartographer/mapping/scan_matching/proto/real_time_correlative_scan_matcher_options.pb.h" +#include "cartographer/mapping/scan_matching/real_time_correlative_scan_matcher.h" #include "cartographer/mapping_2d/probability_grid.h" -#include "cartographer/mapping_2d/scan_matching/correlative_scan_matcher.h" -#include "cartographer/mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.pb.h" +#include "cartographer/mapping_2d/scan_matching/correlative_scan_matcher_2d.h" namespace cartographer { -namespace mapping_2d { +namespace mapping { namespace scan_matching { -proto::RealTimeCorrelativeScanMatcherOptions -CreateRealTimeCorrelativeScanMatcherOptions( - common::LuaParameterDictionary* const parameter_dictionary); - // An implementation of "Real-Time Correlative Scan Matching" by Olson. -class RealTimeCorrelativeScanMatcher { +class RealTimeCorrelativeScanMatcher2D { public: - explicit RealTimeCorrelativeScanMatcher( + explicit RealTimeCorrelativeScanMatcher2D( const proto::RealTimeCorrelativeScanMatcherOptions& options); - RealTimeCorrelativeScanMatcher(const RealTimeCorrelativeScanMatcher&) = + RealTimeCorrelativeScanMatcher2D(const RealTimeCorrelativeScanMatcher2D&) = delete; - RealTimeCorrelativeScanMatcher& operator=( - const RealTimeCorrelativeScanMatcher&) = delete; + RealTimeCorrelativeScanMatcher2D& operator=( + const RealTimeCorrelativeScanMatcher2D&) = delete; // Aligns 'point_cloud' within the 'probability_grid' given an // 'initial_pose_estimate' then updates 'pose_estimate' with the result and // returns the score. double Match(const transform::Rigid2d& initial_pose_estimate, const sensor::PointCloud& point_cloud, - const mapping::ProbabilityGrid& probability_grid, + const ProbabilityGrid& probability_grid, transform::Rigid2d* pose_estimate) const; // Computes the score for each Candidate in a collection. The cost is computed @@ -77,7 +74,7 @@ class RealTimeCorrelativeScanMatcher { // http://ceres-solver.org/modeling.html // // Visible for testing. - void ScoreCandidates(const mapping::ProbabilityGrid& probability_grid, + void ScoreCandidates(const ProbabilityGrid& probability_grid, const std::vector& discrete_scans, const SearchParameters& search_parameters, std::vector* candidates) const; @@ -90,7 +87,7 @@ class RealTimeCorrelativeScanMatcher { }; } // namespace scan_matching -} // namespace mapping_2d +} // namespace mapping } // namespace cartographer -#endif // CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_H_ +#endif // CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_2D_H_ 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_2d_test.cc similarity index 87% rename from cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_test.cc rename to cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_2d_test.cc index d802370..2ff8f9c 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_2d_test.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.h" +#include "cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_2d.h" #include #include @@ -29,15 +29,15 @@ #include "gtest/gtest.h" namespace cartographer { -namespace mapping_2d { +namespace mapping { namespace scan_matching { namespace { class RealTimeCorrelativeScanMatcherTest : public ::testing::Test { protected: RealTimeCorrelativeScanMatcherTest() - : probability_grid_(mapping::MapLimits(0.05, Eigen::Vector2d(0.05, 0.25), - mapping::CellLimits(6, 6))) { + : probability_grid_( + MapLimits(0.05, Eigen::Vector2d(0.05, 0.25), CellLimits(6, 6))) { { auto parameter_dictionary = common::MakeDictionary( "return { " @@ -45,9 +45,8 @@ class RealTimeCorrelativeScanMatcherTest : public ::testing::Test { "hit_probability = 0.7, " "miss_probability = 0.4, " "}"); - range_data_inserter_ = common::make_unique( - mapping::CreateRangeDataInserterOptions2D( - parameter_dictionary.get())); + range_data_inserter_ = common::make_unique( + CreateRangeDataInserterOptions2D(parameter_dictionary.get())); } point_cloud_.emplace_back(0.025f, 0.175f, 0.f); point_cloud_.emplace_back(-0.025f, 0.175f, 0.f); @@ -69,16 +68,16 @@ class RealTimeCorrelativeScanMatcherTest : public ::testing::Test { "rotation_delta_cost_weight = 0., " "}"); real_time_correlative_scan_matcher_ = - common::make_unique( + common::make_unique( CreateRealTimeCorrelativeScanMatcherOptions( parameter_dictionary.get())); } } - mapping::ProbabilityGrid probability_grid_; - std::unique_ptr range_data_inserter_; + ProbabilityGrid probability_grid_; + std::unique_ptr range_data_inserter_; sensor::PointCloud point_cloud_; - std::unique_ptr + std::unique_ptr real_time_correlative_scan_matcher_; }; @@ -121,5 +120,5 @@ TEST_F(RealTimeCorrelativeScanMatcherTest, } // namespace } // namespace scan_matching -} // namespace mapping_2d +} // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping_2d/scan_matching/rotation_delta_cost_functor.h b/cartographer/mapping_2d/scan_matching/rotation_delta_cost_functor_2d.h similarity index 72% rename from cartographer/mapping_2d/scan_matching/rotation_delta_cost_functor.h rename to cartographer/mapping_2d/scan_matching/rotation_delta_cost_functor_2d.h index fcfbc3b..17d4837 100644 --- a/cartographer/mapping_2d/scan_matching/rotation_delta_cost_functor.h +++ b/cartographer/mapping_2d/scan_matching/rotation_delta_cost_functor_2d.h @@ -14,24 +14,24 @@ * limitations under the License. */ -#ifndef CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_H_ -#define CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_H_ +#ifndef CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_2D_H_ +#define CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_2D_H_ #include "Eigen/Core" namespace cartographer { -namespace mapping_2d { +namespace mapping { namespace scan_matching { // Computes the cost of rotating 'pose' to 'target_angle'. Cost increases with // the solution's distance from 'target_angle'. -class RotationDeltaCostFunctor { +class RotationDeltaCostFunctor2D { public: static ceres::CostFunction* CreateAutoDiffCostFunction( const double scaling_factor, const double target_angle) { return new ceres::AutoDiffCostFunction< - RotationDeltaCostFunctor, 1 /* residuals */, 3 /* pose variables */>( - new RotationDeltaCostFunctor(scaling_factor, target_angle)); + RotationDeltaCostFunctor2D, 1 /* residuals */, 3 /* pose variables */>( + new RotationDeltaCostFunctor2D(scaling_factor, target_angle)); } template @@ -41,19 +41,20 @@ class RotationDeltaCostFunctor { } private: - explicit RotationDeltaCostFunctor(const double scaling_factor, - const double target_angle) + explicit RotationDeltaCostFunctor2D(const double scaling_factor, + const double target_angle) : scaling_factor_(scaling_factor), angle_(target_angle) {} - RotationDeltaCostFunctor(const RotationDeltaCostFunctor&) = delete; - RotationDeltaCostFunctor& operator=(const RotationDeltaCostFunctor&) = delete; + RotationDeltaCostFunctor2D(const RotationDeltaCostFunctor2D&) = delete; + RotationDeltaCostFunctor2D& operator=(const RotationDeltaCostFunctor2D&) = + delete; const double scaling_factor_; const double angle_; }; } // namespace scan_matching -} // namespace mapping_2d +} // namespace mapping } // namespace cartographer -#endif // CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_H_ +#endif // CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_2D_H_ diff --git a/cartographer/mapping_2d/scan_matching/translation_delta_cost_functor.h b/cartographer/mapping_2d/scan_matching/translation_delta_cost_functor_2d.h similarity index 71% rename from cartographer/mapping_2d/scan_matching/translation_delta_cost_functor.h rename to cartographer/mapping_2d/scan_matching/translation_delta_cost_functor_2d.h index e3810d2..ee9bfc0 100644 --- a/cartographer/mapping_2d/scan_matching/translation_delta_cost_functor.h +++ b/cartographer/mapping_2d/scan_matching/translation_delta_cost_functor_2d.h @@ -14,24 +14,25 @@ * limitations under the License. */ -#ifndef CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_H_ -#define CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_H_ +#ifndef CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_2D_H_ +#define CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_2D_H_ #include "Eigen/Core" namespace cartographer { -namespace mapping_2d { +namespace mapping { namespace scan_matching { // Computes the cost of translating 'pose' to 'target_translation'. // Cost increases with the solution's distance from 'target_translation'. -class TranslationDeltaCostFunctor { +class TranslationDeltaCostFunctor2D { public: static ceres::CostFunction* CreateAutoDiffCostFunction( const double scaling_factor, const Eigen::Vector2d& target_translation) { - return new ceres::AutoDiffCostFunction< - TranslationDeltaCostFunctor, 2 /* residuals */, 3 /* pose variables */>( - new TranslationDeltaCostFunctor(scaling_factor, target_translation)); + return new ceres::AutoDiffCostFunction( + new TranslationDeltaCostFunctor2D(scaling_factor, target_translation)); } template @@ -42,17 +43,17 @@ class TranslationDeltaCostFunctor { } private: - // Constructs a new TranslationDeltaCostFunctor from the given + // Constructs a new TranslationDeltaCostFunctor2D from the given // 'target_translation' (x, y). - explicit TranslationDeltaCostFunctor( + explicit TranslationDeltaCostFunctor2D( const double scaling_factor, const Eigen::Vector2d& target_translation) : scaling_factor_(scaling_factor), x_(target_translation.x()), y_(target_translation.y()) {} - TranslationDeltaCostFunctor(const TranslationDeltaCostFunctor&) = delete; - TranslationDeltaCostFunctor& operator=(const TranslationDeltaCostFunctor&) = - delete; + TranslationDeltaCostFunctor2D(const TranslationDeltaCostFunctor2D&) = delete; + TranslationDeltaCostFunctor2D& operator=( + const TranslationDeltaCostFunctor2D&) = delete; const double scaling_factor_; const double x_; @@ -60,7 +61,7 @@ class TranslationDeltaCostFunctor { }; } // namespace scan_matching -} // namespace mapping_2d +} // namespace mapping } // namespace cartographer -#endif // CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_H_ +#endif // CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_2D_H_ diff --git a/cartographer/mapping_3d/local_trajectory_builder_options_3d.cc b/cartographer/mapping_3d/local_trajectory_builder_options_3d.cc index ac68d3d..858f32d 100644 --- a/cartographer/mapping_3d/local_trajectory_builder_options_3d.cc +++ b/cartographer/mapping_3d/local_trajectory_builder_options_3d.cc @@ -17,7 +17,7 @@ #include "cartographer/mapping_3d/local_trajectory_builder_options_3d.h" #include "cartographer/internal/mapping/motion_filter.h" -#include "cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.h" +#include "cartographer/mapping/scan_matching/real_time_correlative_scan_matcher.h" #include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h" #include "cartographer/mapping_3d/submap_3d.h" #include "cartographer/sensor/voxel_filter.h" @@ -48,7 +48,7 @@ proto::LocalTrajectoryBuilderOptions3D CreateLocalTrajectoryBuilderOptions3D( options.set_use_online_correlative_scan_matching( parameter_dictionary->GetBool("use_online_correlative_scan_matching")); *options.mutable_real_time_correlative_scan_matcher_options() = - mapping_2d::scan_matching::CreateRealTimeCorrelativeScanMatcherOptions( + mapping::scan_matching::CreateRealTimeCorrelativeScanMatcherOptions( parameter_dictionary ->GetDictionary("real_time_correlative_scan_matcher") .get()); diff --git a/cartographer/mapping_3d/proto/local_trajectory_builder_options_3d.proto b/cartographer/mapping_3d/proto/local_trajectory_builder_options_3d.proto index cd60674..bb923a2 100644 --- a/cartographer/mapping_3d/proto/local_trajectory_builder_options_3d.proto +++ b/cartographer/mapping_3d/proto/local_trajectory_builder_options_3d.proto @@ -18,7 +18,7 @@ package cartographer.mapping.proto; import "cartographer/mapping/proto/motion_filter_options.proto"; import "cartographer/sensor/proto/adaptive_voxel_filter_options.proto"; -import "cartographer/mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.proto"; +import "cartographer/mapping/scan_matching/proto/real_time_correlative_scan_matcher_options.proto"; import "cartographer/mapping_3d/proto/submaps_options_3d.proto"; import "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.proto"; @@ -45,7 +45,7 @@ message LocalTrajectoryBuilderOptions3D { // 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 = 13; - mapping_2d.scan_matching.proto.RealTimeCorrelativeScanMatcherOptions + mapping.scan_matching.proto.RealTimeCorrelativeScanMatcherOptions real_time_correlative_scan_matcher_options = 14; mapping_3d.scan_matching.proto.CeresScanMatcherOptions ceres_scan_matcher_options = 6; diff --git a/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h b/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h index 233e5f0..d3f03d3 100644 --- a/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h +++ b/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h @@ -15,7 +15,7 @@ */ // This is an implementation of a 3D branch-and-bound algorithm similar to -// mapping_2d::FastCorrelativeScanMatcher. +// FastCorrelativeScanMatcher2D. #ifndef CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_H_ #define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_H_ @@ -26,7 +26,7 @@ #include "Eigen/Core" #include "cartographer/common/port.h" #include "cartographer/mapping/trajectory_node.h" -#include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h" +#include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_2d.h" #include "cartographer/mapping_3d/hybrid_grid.h" #include "cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options.pb.h" #include "cartographer/mapping_3d/scan_matching/rotational_scan_matcher.h" diff --git a/cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher.cc b/cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher.cc index 798e978..4b3a600 100644 --- a/cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher.cc +++ b/cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher.cc @@ -28,8 +28,8 @@ namespace mapping_3d { namespace scan_matching { RealTimeCorrelativeScanMatcher::RealTimeCorrelativeScanMatcher( - const mapping_2d::scan_matching::proto:: - RealTimeCorrelativeScanMatcherOptions& options) + const mapping::scan_matching::proto::RealTimeCorrelativeScanMatcherOptions& + options) : options_(options) {} float RealTimeCorrelativeScanMatcher::Match( diff --git a/cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher.h b/cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher.h index a016585..578d873 100644 --- a/cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher.h +++ b/cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher.h @@ -22,7 +22,7 @@ #include #include "Eigen/Core" -#include "cartographer/mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.pb.h" +#include "cartographer/mapping/scan_matching/proto/real_time_correlative_scan_matcher_options.pb.h" #include "cartographer/mapping_3d/hybrid_grid.h" #include "cartographer/sensor/point_cloud.h" @@ -33,7 +33,7 @@ namespace scan_matching { class RealTimeCorrelativeScanMatcher { public: explicit RealTimeCorrelativeScanMatcher( - const mapping_2d::scan_matching::proto:: + const mapping::scan_matching::proto:: RealTimeCorrelativeScanMatcherOptions& options); RealTimeCorrelativeScanMatcher(const RealTimeCorrelativeScanMatcher&) = @@ -56,7 +56,7 @@ class RealTimeCorrelativeScanMatcher { const sensor::PointCloud& transformed_point_cloud, const transform::Rigid3f& transform) const; - const mapping_2d::scan_matching::proto::RealTimeCorrelativeScanMatcherOptions + const mapping::scan_matching::proto::RealTimeCorrelativeScanMatcherOptions options_; }; diff --git a/cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher_test.cc b/cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher_test.cc index c4bb9ab..f72bbb2 100644 --- a/cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher_test.cc +++ b/cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher_test.cc @@ -20,7 +20,7 @@ #include "Eigen/Core" #include "cartographer/common/lua_parameter_dictionary_test_helpers.h" -#include "cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.h" +#include "cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_2d.h" #include "cartographer/mapping_3d/hybrid_grid.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/transform/rigid_transform.h" @@ -58,9 +58,8 @@ class RealTimeCorrelativeScanMatcherTest : public ::testing::Test { })text"); real_time_correlative_scan_matcher_.reset( new RealTimeCorrelativeScanMatcher( - mapping_2d::scan_matching:: - CreateRealTimeCorrelativeScanMatcherOptions( - parameter_dictionary.get()))); + mapping::scan_matching::CreateRealTimeCorrelativeScanMatcherOptions( + parameter_dictionary.get()))); } void TestFromInitialPose(const transform::Rigid3d& initial_pose) { diff --git a/cartographer/metrics/register.cc b/cartographer/metrics/register.cc index 18bb7d5..deeaf0d 100644 --- a/cartographer/metrics/register.cc +++ b/cartographer/metrics/register.cc @@ -16,14 +16,14 @@ #include "cartographer/metrics/register.h" -#include "cartographer/mapping_2d/pose_graph/constraint_builder.h" +#include "cartographer/mapping_2d/pose_graph/constraint_builder_2d.h" #include "cartographer/mapping_3d/pose_graph/constraint_builder.h" namespace cartographer { namespace metrics { void RegisterAllMetrics(FamilyFactory* registry) { - mapping_2d::pose_graph::ConstraintBuilder::RegisterMetrics(registry); + mapping::pose_graph::ConstraintBuilder2D::RegisterMetrics(registry); mapping_3d::pose_graph::ConstraintBuilder::RegisterMetrics(registry); }