From e75e023ce25368c50efc8e4bfd99c9c7168ae044 Mon Sep 17 00:00:00 2001 From: Alexander Belyaev <32522095+pifon2a@users.noreply.github.com> Date: Mon, 26 Feb 2018 14:26:41 +0100 Subject: [PATCH] Purge 'mapping_3d::'. (#927) (#928) [Code structure RFC](https://github.com/googlecartographer/rfcs/blob/master/text/0016-code-structure.md) --- .../mapping/global_trajectory_builder.cc | 7 +- .../mapping/global_trajectory_builder.h | 5 +- ...tion.h => acceleration_cost_function_3d.h} | 36 ++-- ...lder.cc => local_trajectory_builder_3d.cc} | 37 +++-- ...uilder.h => local_trajectory_builder_3d.h} | 28 ++-- ...cc => local_trajectory_builder_3d_test.cc} | 12 +- ...function.h => rotation_cost_function_3d.h} | 10 +- ...on.h => occupied_space_cost_function_3d.h} | 27 +-- cartographer/mapping/map_builder.cc | 10 +- .../mapping/pose_graph/constraint_builder.cc | 8 +- .../proto/constraint_builder_options.proto | 8 +- cartographer/mapping/proto/submap.proto | 2 +- .../pose_graph/landmark_cost_function_2d.h | 4 - cartographer/mapping_2d/pose_graph_2d_test.cc | 2 +- .../correlative_scan_matcher_2d.cc | 6 +- .../correlative_scan_matcher_2d.h | 20 +-- .../correlative_scan_matcher_test.cc | 6 +- .../fast_correlative_scan_matcher_2d.cc | 49 +++--- .../fast_correlative_scan_matcher_2d.h | 28 ++-- .../fast_correlative_scan_matcher_2d_test.cc | 12 +- .../real_time_correlative_scan_matcher_2d.cc | 16 +- .../real_time_correlative_scan_matcher_2d.h | 12 +- ...l_time_correlative_scan_matcher_2d_test.cc | 8 +- cartographer/mapping_3d/imu_integration.h | 4 +- .../local_trajectory_builder_options_3d.cc | 4 +- ...nt_builder.cc => constraint_builder_3d.cc} | 78 ++++----- ...aint_builder.h => constraint_builder_3d.h} | 94 +++++------ ...function.h => landmark_cost_function_3d.h} | 34 ++-- ...t.cc => landmark_cost_function_3d_test.cc} | 16 +- ..._problem.cc => optimization_problem_3d.cc} | 111 +++++++------ ...on_problem.h => optimization_problem_3d.h} | 73 ++++----- ...est.cc => optimization_problem_3d_test.cc} | 69 ++++---- ...cost_function.h => spa_cost_function_3d.h} | 28 ++-- cartographer/mapping_3d/pose_graph_3d.cc | 16 +- cartographer/mapping_3d/pose_graph_3d.h | 15 +- .../local_trajectory_builder_options_3d.proto | 4 +- ...an_matcher.cc => ceres_scan_matcher_3d.cc} | 46 +++--- ...scan_matcher.h => ceres_scan_matcher_3d.h} | 26 +-- ..._test.cc => ceres_scan_matcher_3d_test.cc} | 32 ++-- ...cc => fast_correlative_scan_matcher_3d.cc} | 154 +++++++++--------- ...r.h => fast_correlative_scan_matcher_3d.h} | 86 +++++----- ... fast_correlative_scan_matcher_3d_test.cc} | 74 ++++----- .../scan_matching/interpolated_grid.h | 8 +- .../scan_matching/interpolated_grid_test.cc | 6 +- .../scan_matching/low_resolution_matcher.cc | 7 +- .../scan_matching/low_resolution_matcher.h | 7 +- ...tion_grid.cc => precomputation_grid_3d.cc} | 33 ++-- ...tation_grid.h => precomputation_grid_3d.h} | 32 ++-- ...test.cc => precomputation_grid_3d_test.cc} | 18 +- ...to => ceres_scan_matcher_options_3d.proto} | 4 +- ...correlative_scan_matcher_options_3d.proto} | 4 +- ... real_time_correlative_scan_matcher_3d.cc} | 22 ++- ...> real_time_correlative_scan_matcher_3d.h} | 35 ++-- ..._time_correlative_scan_matcher_3d_test.cc} | 32 ++-- ...tor.h => rotation_delta_cost_functor_3d.h} | 28 ++-- ...=> rotation_delta_cost_functor_3d_test.cc} | 14 +- .../scan_matching/rotational_scan_matcher.cc | 4 +- .../scan_matching/rotational_scan_matcher.h | 4 +- .../rotational_scan_matcher_test.cc | 8 +- ....h => translation_delta_cost_functor_3d.h} | 26 +-- cartographer/metrics/register.cc | 4 +- 61 files changed, 791 insertions(+), 822 deletions(-) rename cartographer/internal/mapping_3d/{acceleration_cost_function.h => acceleration_cost_function_3d.h} (78%) rename cartographer/internal/mapping_3d/{local_trajectory_builder.cc => local_trajectory_builder_3d.cc} (92%) rename cartographer/internal/mapping_3d/{local_trajectory_builder.h => local_trajectory_builder_3d.h} (87%) rename cartographer/internal/mapping_3d/{local_trajectory_builder_test.cc => local_trajectory_builder_3d_test.cc} (97%) rename cartographer/internal/mapping_3d/{rotation_cost_function.h => rotation_cost_function_3d.h} (97%) rename cartographer/internal/mapping_3d/scan_matching/{occupied_space_cost_function.h => occupied_space_cost_function_3d.h} (80%) rename cartographer/mapping_3d/pose_graph/{constraint_builder.cc => constraint_builder_3d.cc} (86%) rename cartographer/mapping_3d/pose_graph/{constraint_builder.h => constraint_builder_3d.h} (70%) rename cartographer/mapping_3d/pose_graph/{landmark_cost_function.h => landmark_cost_function_3d.h} (85%) rename cartographer/mapping_3d/pose_graph/{landmark_cost_function_test.cc => landmark_cost_function_3d_test.cc} (89%) rename cartographer/mapping_3d/pose_graph/{optimization_problem.cc => optimization_problem_3d.cc} (88%) rename cartographer/mapping_3d/pose_graph/{optimization_problem.h => optimization_problem_3d.h} (70%) rename cartographer/mapping_3d/pose_graph/{optimization_problem_test.cc => optimization_problem_3d_test.cc} (76%) rename cartographer/mapping_3d/pose_graph/{spa_cost_function.h => spa_cost_function_3d.h} (74%) rename cartographer/mapping_3d/scan_matching/{ceres_scan_matcher.cc => ceres_scan_matcher_3d.cc} (77%) rename cartographer/mapping_3d/scan_matching/{ceres_scan_matcher.h => ceres_scan_matcher_3d.h} (79%) rename cartographer/mapping_3d/scan_matching/{ceres_scan_matcher_test.cc => ceres_scan_matcher_3d_test.cc} (84%) rename cartographer/mapping_3d/scan_matching/{fast_correlative_scan_matcher.cc => fast_correlative_scan_matcher_3d.cc} (79%) rename cartographer/mapping_3d/scan_matching/{fast_correlative_scan_matcher.h => fast_correlative_scan_matcher_3d.h} (63%) rename cartographer/mapping_3d/scan_matching/{fast_correlative_scan_matcher_test.cc => fast_correlative_scan_matcher_3d_test.cc} (74%) rename cartographer/mapping_3d/scan_matching/{precomputation_grid.cc => precomputation_grid_3d.cc} (73%) rename cartographer/mapping_3d/scan_matching/{precomputation_grid.h => precomputation_grid_3d.h} (68%) rename cartographer/mapping_3d/scan_matching/{precomputation_grid_test.cc => precomputation_grid_3d_test.cc} (85%) rename cartographer/mapping_3d/scan_matching/proto/{ceres_scan_matcher_options.proto => ceres_scan_matcher_options_3d.proto} (92%) rename cartographer/mapping_3d/scan_matching/proto/{fast_correlative_scan_matcher_options.proto => fast_correlative_scan_matcher_options_3d.proto} (94%) rename cartographer/mapping_3d/scan_matching/{real_time_correlative_scan_matcher.cc => real_time_correlative_scan_matcher_3d.cc} (88%) rename cartographer/mapping_3d/scan_matching/{real_time_correlative_scan_matcher.h => real_time_correlative_scan_matcher_3d.h} (73%) rename cartographer/mapping_3d/scan_matching/{real_time_correlative_scan_matcher_test.cc => real_time_correlative_scan_matcher_3d_test.cc} (81%) rename cartographer/mapping_3d/scan_matching/{rotation_delta_cost_functor.h => rotation_delta_cost_functor_3d.h} (80%) rename cartographer/mapping_3d/scan_matching/{rotation_delta_cost_functor_test.cc => rotation_delta_cost_functor_3d_test.cc} (89%) rename cartographer/mapping_3d/scan_matching/{translation_delta_cost_functor.h => translation_delta_cost_functor_3d.h} (80%) diff --git a/cartographer/internal/mapping/global_trajectory_builder.cc b/cartographer/internal/mapping/global_trajectory_builder.cc index 6bce4f0..a4e7be5 100644 --- a/cartographer/internal/mapping/global_trajectory_builder.cc +++ b/cartographer/internal/mapping/global_trajectory_builder.cc @@ -139,13 +139,12 @@ std::unique_ptr CreateGlobalTrajectoryBuilder2D( } std::unique_ptr CreateGlobalTrajectoryBuilder3D( - std::unique_ptr - local_trajectory_builder, + std::unique_ptr local_trajectory_builder, const int trajectory_id, mapping::PoseGraph3D* 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 e582311..8d24dd3 100644 --- a/cartographer/internal/mapping/global_trajectory_builder.h +++ b/cartographer/internal/mapping/global_trajectory_builder.h @@ -20,7 +20,7 @@ #include #include "cartographer/internal/mapping_2d/local_trajectory_builder_2d.h" -#include "cartographer/internal/mapping_3d/local_trajectory_builder.h" +#include "cartographer/internal/mapping_3d/local_trajectory_builder_3d.h" #include "cartographer/mapping/local_slam_result_data.h" #include "cartographer/mapping/trajectory_builder_interface.h" #include "cartographer/mapping_2d/pose_graph_2d.h" @@ -36,8 +36,7 @@ std::unique_ptr CreateGlobalTrajectoryBuilder2D( local_slam_result_callback); std::unique_ptr CreateGlobalTrajectoryBuilder3D( - std::unique_ptr - local_trajectory_builder, + std::unique_ptr local_trajectory_builder, const int trajectory_id, mapping::PoseGraph3D* const pose_graph, const TrajectoryBuilderInterface::LocalSlamResultCallback& local_slam_result_callback); diff --git a/cartographer/internal/mapping_3d/acceleration_cost_function.h b/cartographer/internal/mapping_3d/acceleration_cost_function_3d.h similarity index 78% rename from cartographer/internal/mapping_3d/acceleration_cost_function.h rename to cartographer/internal/mapping_3d/acceleration_cost_function_3d.h index 8c90eb0..bf37801 100644 --- a/cartographer/internal/mapping_3d/acceleration_cost_function.h +++ b/cartographer/internal/mapping_3d/acceleration_cost_function_3d.h @@ -14,17 +14,17 @@ * limitations under the License. */ -#ifndef CARTOGRAPHER_INTERNAL_MAPPING_3D_ACCELERATION_COST_FUNCTION_H_ -#define CARTOGRAPHER_INTERNAL_MAPPING_3D_ACCELERATION_COST_FUNCTION_H_ +#ifndef CARTOGRAPHER_INTERNAL_MAPPING_3D_ACCELERATION_COST_FUNCTION_3D_H_ +#define CARTOGRAPHER_INTERNAL_MAPPING_3D_ACCELERATION_COST_FUNCTION_3D_H_ #include "Eigen/Core" #include "Eigen/Geometry" namespace cartographer { -namespace mapping_3d { +namespace mapping { // Penalizes differences between IMU data and optimized accelerations. -class AccelerationCostFunction { +class AccelerationCostFunction3D { public: static ceres::CostFunction* CreateAutoDiffCostFunction( const double scaling_factor, @@ -32,12 +32,13 @@ class AccelerationCostFunction { const double first_delta_time_seconds, const double second_delta_time_seconds) { return new ceres::AutoDiffCostFunction< - AccelerationCostFunction, 3 /* residuals */, 4 /* rotation variables */, + AccelerationCostFunction3D, 3 /* residuals */, + 4 /* rotation variables */, 3 /* position variables */, 3 /* position variables */, 3 /* position variables */, - 3 /* position variables */, 1 /* gravity variables */, - 4 /* rotation variables */>(new AccelerationCostFunction( - scaling_factor, delta_velocity_imu_frame, first_delta_time_seconds, - second_delta_time_seconds)); + 1 /* gravity variables */, 4 /* rotation variables */>( + new AccelerationCostFunction3D(scaling_factor, delta_velocity_imu_frame, + first_delta_time_seconds, + second_delta_time_seconds)); } template @@ -71,17 +72,18 @@ class AccelerationCostFunction { } private: - AccelerationCostFunction(const double scaling_factor, - const Eigen::Vector3d& delta_velocity_imu_frame, - const double first_delta_time_seconds, - const double second_delta_time_seconds) + AccelerationCostFunction3D(const double scaling_factor, + const Eigen::Vector3d& delta_velocity_imu_frame, + const double first_delta_time_seconds, + const double second_delta_time_seconds) : scaling_factor_(scaling_factor), delta_velocity_imu_frame_(delta_velocity_imu_frame), first_delta_time_seconds_(first_delta_time_seconds), second_delta_time_seconds_(second_delta_time_seconds) {} - AccelerationCostFunction(const AccelerationCostFunction&) = delete; - AccelerationCostFunction& operator=(const AccelerationCostFunction&) = delete; + AccelerationCostFunction3D(const AccelerationCostFunction3D&) = delete; + AccelerationCostFunction3D& operator=(const AccelerationCostFunction3D&) = + delete; template static Eigen::Quaternion ToEigen(const T* const quaternion) { @@ -95,7 +97,7 @@ class AccelerationCostFunction { const double second_delta_time_seconds_; }; -} // namespace mapping_3d +} // namespace mapping } // namespace cartographer -#endif // CARTOGRAPHER_INTERNAL_MAPPING_3D_ACCELERATION_COST_FUNCTION_H_ +#endif // CARTOGRAPHER_INTERNAL_MAPPING_3D_ACCELERATION_COST_FUNCTION_3D_H_ diff --git a/cartographer/internal/mapping_3d/local_trajectory_builder.cc b/cartographer/internal/mapping_3d/local_trajectory_builder_3d.cc similarity index 92% rename from cartographer/internal/mapping_3d/local_trajectory_builder.cc rename to cartographer/internal/mapping_3d/local_trajectory_builder_3d.cc index d9ef256..a1bbe5f 100644 --- a/cartographer/internal/mapping_3d/local_trajectory_builder.cc +++ b/cartographer/internal/mapping_3d/local_trajectory_builder_3d.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/internal/mapping_3d/local_trajectory_builder.h" +#include "cartographer/internal/mapping_3d/local_trajectory_builder_3d.h" #include @@ -23,28 +23,29 @@ #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" +#include "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options_3d.pb.h" #include "cartographer/mapping_3d/scan_matching/rotational_scan_matcher.h" #include "glog/logging.h" namespace cartographer { -namespace mapping_3d { +namespace mapping { -LocalTrajectoryBuilder::LocalTrajectoryBuilder( +LocalTrajectoryBuilder3D::LocalTrajectoryBuilder3D( const mapping::proto::LocalTrajectoryBuilderOptions3D& options) : options_(options), active_submaps_(options.submaps_options()), motion_filter_(options.motion_filter_options()), real_time_correlative_scan_matcher_( - common::make_unique( + common::make_unique( options_.real_time_correlative_scan_matcher_options())), - ceres_scan_matcher_(common::make_unique( - options_.ceres_scan_matcher_options())), + ceres_scan_matcher_( + common::make_unique( + options_.ceres_scan_matcher_options())), accumulated_range_data_{Eigen::Vector3f::Zero(), {}, {}} {} -LocalTrajectoryBuilder::~LocalTrajectoryBuilder() {} +LocalTrajectoryBuilder3D::~LocalTrajectoryBuilder3D() {} -void LocalTrajectoryBuilder::AddImuData(const sensor::ImuData& imu_data) { +void LocalTrajectoryBuilder3D::AddImuData(const sensor::ImuData& imu_data) { if (extrapolator_ != nullptr) { extrapolator_->AddImuData(imu_data); return; @@ -58,9 +59,9 @@ void LocalTrajectoryBuilder::AddImuData(const sensor::ImuData& imu_data) { options_.imu_gravity_time_constant(), imu_data); } -std::unique_ptr -LocalTrajectoryBuilder::AddRangeData(const common::Time time, - const sensor::TimedRangeData& range_data) { +std::unique_ptr +LocalTrajectoryBuilder3D::AddRangeData( + const common::Time time, const sensor::TimedRangeData& range_data) { if (extrapolator_ == nullptr) { // Until we've initialized the extrapolator with our first IMU message, we // cannot compute the orientation of the rangefinder. @@ -130,8 +131,8 @@ LocalTrajectoryBuilder::AddRangeData(const common::Time time, return nullptr; } -std::unique_ptr -LocalTrajectoryBuilder::AddAccumulatedRangeData( +std::unique_ptr +LocalTrajectoryBuilder3D::AddAccumulatedRangeData( const common::Time time, const sensor::RangeData& filtered_range_data_in_tracking) { if (filtered_range_data_in_tracking.returns.empty()) { @@ -199,7 +200,7 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData( std::move(insertion_result)}); } -void LocalTrajectoryBuilder::AddOdometryData( +void LocalTrajectoryBuilder3D::AddOdometryData( const sensor::OdometryData& odometry_data) { if (extrapolator_ == nullptr) { // Until we've initialized the extrapolator we cannot add odometry data. @@ -209,8 +210,8 @@ void LocalTrajectoryBuilder::AddOdometryData( extrapolator_->AddOdometryData(odometry_data); } -std::unique_ptr -LocalTrajectoryBuilder::InsertIntoSubmap( +std::unique_ptr +LocalTrajectoryBuilder3D::InsertIntoSubmap( const common::Time time, const sensor::RangeData& filtered_range_data_in_local, const sensor::RangeData& filtered_range_data_in_tracking, @@ -249,5 +250,5 @@ LocalTrajectoryBuilder::InsertIntoSubmap( std::move(insertion_submaps)}); } -} // namespace mapping_3d +} // namespace mapping } // namespace cartographer diff --git a/cartographer/internal/mapping_3d/local_trajectory_builder.h b/cartographer/internal/mapping_3d/local_trajectory_builder_3d.h similarity index 87% rename from cartographer/internal/mapping_3d/local_trajectory_builder.h rename to cartographer/internal/mapping_3d/local_trajectory_builder_3d.h index 76dff5a..7f2d456 100644 --- a/cartographer/internal/mapping_3d/local_trajectory_builder.h +++ b/cartographer/internal/mapping_3d/local_trajectory_builder_3d.h @@ -14,8 +14,8 @@ * limitations under the License. */ -#ifndef CARTOGRAPHER_INTERNAL_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_H_ -#define CARTOGRAPHER_INTERNAL_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_H_ +#ifndef CARTOGRAPHER_INTERNAL_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_3D_H_ +#define CARTOGRAPHER_INTERNAL_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_3D_H_ #include @@ -23,8 +23,8 @@ #include "cartographer/internal/mapping/motion_filter.h" #include "cartographer/mapping/pose_extrapolator.h" #include "cartographer/mapping_3d/proto/local_trajectory_builder_options_3d.pb.h" -#include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h" -#include "cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher.h" +#include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher_3d.h" +#include "cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher_3d.h" #include "cartographer/mapping_3d/submap_3d.h" #include "cartographer/sensor/imu_data.h" #include "cartographer/sensor/odometry_data.h" @@ -33,11 +33,11 @@ #include "cartographer/transform/rigid_transform.h" namespace cartographer { -namespace mapping_3d { +namespace mapping { // Wires up the local SLAM stack (i.e. pose extrapolator, scan matching, etc.) // without loop closure. -class LocalTrajectoryBuilder { +class LocalTrajectoryBuilder3D { public: struct InsertionResult { std::shared_ptr constant_data; @@ -51,12 +51,12 @@ class LocalTrajectoryBuilder { std::unique_ptr insertion_result; }; - explicit LocalTrajectoryBuilder( + explicit LocalTrajectoryBuilder3D( const mapping::proto::LocalTrajectoryBuilderOptions3D& options); - ~LocalTrajectoryBuilder(); + ~LocalTrajectoryBuilder3D(); - LocalTrajectoryBuilder(const LocalTrajectoryBuilder&) = delete; - LocalTrajectoryBuilder& operator=(const LocalTrajectoryBuilder&) = delete; + LocalTrajectoryBuilder3D(const LocalTrajectoryBuilder3D&) = delete; + LocalTrajectoryBuilder3D& operator=(const LocalTrajectoryBuilder3D&) = delete; void AddImuData(const sensor::ImuData& imu_data); // Returns 'MatchingResult' when range data accumulation completed, @@ -84,9 +84,9 @@ class LocalTrajectoryBuilder { mapping::ActiveSubmaps3D active_submaps_; mapping::MotionFilter motion_filter_; - std::unique_ptr + std::unique_ptr real_time_correlative_scan_matcher_; - std::unique_ptr ceres_scan_matcher_; + std::unique_ptr ceres_scan_matcher_; std::unique_ptr extrapolator_; @@ -94,7 +94,7 @@ class LocalTrajectoryBuilder { sensor::RangeData accumulated_range_data_; }; -} // namespace mapping_3d +} // namespace mapping } // namespace cartographer -#endif // CARTOGRAPHER_INTERNAL_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_H_ +#endif // CARTOGRAPHER_INTERNAL_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_3D_H_ diff --git a/cartographer/internal/mapping_3d/local_trajectory_builder_test.cc b/cartographer/internal/mapping_3d/local_trajectory_builder_3d_test.cc similarity index 97% rename from cartographer/internal/mapping_3d/local_trajectory_builder_test.cc rename to cartographer/internal/mapping_3d/local_trajectory_builder_3d_test.cc index 960f91a..ba8bf8a 100644 --- a/cartographer/internal/mapping_3d/local_trajectory_builder_test.cc +++ b/cartographer/internal/mapping_3d/local_trajectory_builder_3d_test.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/internal/mapping_3d/local_trajectory_builder.h" +#include "cartographer/internal/mapping_3d/local_trajectory_builder_3d.h" #include #include @@ -32,7 +32,7 @@ #include "gmock/gmock.h" namespace cartographer { -namespace mapping_3d { +namespace mapping { namespace { class LocalTrajectoryBuilderTest : public ::testing::Test { @@ -253,7 +253,7 @@ class LocalTrajectoryBuilderTest : public ::testing::Test { for (const TrajectoryNode& node : expected_trajectory) { AddLinearOnlyImuObservation(node.time, node.pose); const auto range_data = GenerateRangeData(node.pose); - const std::unique_ptr + const std::unique_ptr matching_result = local_trajectory_builder_->AddRangeData( node.time, sensor::TimedRangeData{ range_data.origin, range_data.returns, {}}); @@ -266,16 +266,16 @@ class LocalTrajectoryBuilderTest : public ::testing::Test { } } - std::unique_ptr local_trajectory_builder_; + std::unique_ptr local_trajectory_builder_; std::vector bubbles_; }; TEST_F(LocalTrajectoryBuilderTest, MoveInsideCubeUsingOnlyCeresScanMatcher) { local_trajectory_builder_.reset( - new LocalTrajectoryBuilder(CreateTrajectoryBuilderOptions3D())); + new LocalTrajectoryBuilder3D(CreateTrajectoryBuilderOptions3D())); VerifyAccuracy(GenerateCorkscrewTrajectory(), 1e-1); } } // namespace -} // namespace mapping_3d +} // namespace mapping } // namespace cartographer diff --git a/cartographer/internal/mapping_3d/rotation_cost_function.h b/cartographer/internal/mapping_3d/rotation_cost_function_3d.h similarity index 97% rename from cartographer/internal/mapping_3d/rotation_cost_function.h rename to cartographer/internal/mapping_3d/rotation_cost_function_3d.h index dc49a28..78bdefe 100644 --- a/cartographer/internal/mapping_3d/rotation_cost_function.h +++ b/cartographer/internal/mapping_3d/rotation_cost_function_3d.h @@ -14,14 +14,14 @@ * limitations under the License. */ -#ifndef CARTOGRAPHER_INTERNAL_MAPPING_3D_ROTATION_COST_FUNCTION_H_ -#define CARTOGRAPHER_INTERNAL_MAPPING_3D_ROTATION_COST_FUNCTION_H_ +#ifndef CARTOGRAPHER_INTERNAL_MAPPING_3D_ROTATION_COST_FUNCTION_3D_H_ +#define CARTOGRAPHER_INTERNAL_MAPPING_3D_ROTATION_COST_FUNCTION_3D_H_ #include "Eigen/Core" #include "Eigen/Geometry" namespace cartographer { -namespace mapping_3d { +namespace mapping { // Penalizes differences between IMU data and optimized orientations. class RotationCostFunction { @@ -67,7 +67,7 @@ class RotationCostFunction { const Eigen::Quaterniond delta_rotation_imu_frame_; }; -} // namespace mapping_3d +} // namespace mapping } // namespace cartographer -#endif // CARTOGRAPHER_INTERNAL_MAPPING_3D_ROTATION_COST_FUNCTION_H_ +#endif // CARTOGRAPHER_INTERNAL_MAPPING_3D_ROTATION_COST_FUNCTION_3D_H_ diff --git a/cartographer/internal/mapping_3d/scan_matching/occupied_space_cost_function.h b/cartographer/internal/mapping_3d/scan_matching/occupied_space_cost_function_3d.h similarity index 80% rename from cartographer/internal/mapping_3d/scan_matching/occupied_space_cost_function.h rename to cartographer/internal/mapping_3d/scan_matching/occupied_space_cost_function_3d.h index 5aff992..b32db99 100644 --- a/cartographer/internal/mapping_3d/scan_matching/occupied_space_cost_function.h +++ b/cartographer/internal/mapping_3d/scan_matching/occupied_space_cost_function_3d.h @@ -14,8 +14,8 @@ * limitations under the License. */ -#ifndef CARTOGRAPHER_INTERNAL_MAPPING_3D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTION_H_ -#define CARTOGRAPHER_INTERNAL_MAPPING_3D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTION_H_ +#ifndef CARTOGRAPHER_INTERNAL_MAPPING_3D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTION_3D_H_ +#define CARTOGRAPHER_INTERNAL_MAPPING_3D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTION_3D_H_ #include "Eigen/Core" #include "cartographer/mapping_3d/hybrid_grid.h" @@ -25,21 +25,22 @@ #include "cartographer/transform/transform.h" namespace cartographer { -namespace mapping_3d { +namespace mapping { namespace scan_matching { // Computes a cost for matching the 'point_cloud' to the 'hybrid_grid' with a // 'translation' and 'rotation'. The cost increases when points fall into less // occupied space, i.e. at voxels with lower values. -class OccupiedSpaceCostFunction { +class OccupiedSpaceCostFunction3D { public: static ceres::CostFunction* CreateAutoDiffCostFunction( const double scaling_factor, const sensor::PointCloud& point_cloud, const mapping::HybridGrid& hybrid_grid) { return new ceres::AutoDiffCostFunction< - OccupiedSpaceCostFunction, ceres::DYNAMIC /* residuals */, + OccupiedSpaceCostFunction3D, ceres::DYNAMIC /* residuals */, 3 /* translation variables */, 4 /* rotation variables */>( - new OccupiedSpaceCostFunction(scaling_factor, point_cloud, hybrid_grid), + new OccupiedSpaceCostFunction3D(scaling_factor, point_cloud, + hybrid_grid), point_cloud.size()); } @@ -54,15 +55,15 @@ class OccupiedSpaceCostFunction { } private: - OccupiedSpaceCostFunction(const double scaling_factor, - const sensor::PointCloud& point_cloud, - const mapping::HybridGrid& hybrid_grid) + OccupiedSpaceCostFunction3D(const double scaling_factor, + const sensor::PointCloud& point_cloud, + const mapping::HybridGrid& hybrid_grid) : scaling_factor_(scaling_factor), point_cloud_(point_cloud), interpolated_grid_(hybrid_grid) {} - OccupiedSpaceCostFunction(const OccupiedSpaceCostFunction&) = delete; - OccupiedSpaceCostFunction& operator=(const OccupiedSpaceCostFunction&) = + OccupiedSpaceCostFunction3D(const OccupiedSpaceCostFunction3D&) = delete; + OccupiedSpaceCostFunction3D& operator=(const OccupiedSpaceCostFunction3D&) = delete; template @@ -84,7 +85,7 @@ class OccupiedSpaceCostFunction { }; } // namespace scan_matching -} // namespace mapping_3d +} // namespace mapping } // namespace cartographer -#endif // CARTOGRAPHER_INTERNAL_MAPPING_3D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTION_H_ +#endif // CARTOGRAPHER_INTERNAL_MAPPING_3D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTION_3D_H_ diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index aa70120..99a5b17 100644 --- a/cartographer/mapping/map_builder.cc +++ b/cartographer/mapping/map_builder.cc @@ -26,7 +26,7 @@ #include "cartographer/common/time.h" #include "cartographer/internal/mapping/global_trajectory_builder.h" #include "cartographer/internal/mapping_2d/local_trajectory_builder_2d.h" -#include "cartographer/internal/mapping_3d/local_trajectory_builder.h" +#include "cartographer/internal/mapping_3d/local_trajectory_builder_3d.h" #include "cartographer/mapping/collated_trajectory_builder.h" #include "cartographer/sensor/collator.h" #include "cartographer/sensor/range_data.h" @@ -80,12 +80,10 @@ int MapBuilder::AddTrajectoryBuilder( LocalSlamResultCallback local_slam_result_callback) { const int trajectory_id = trajectory_builders_.size(); if (options_.use_trajectory_builder_3d()) { - std::unique_ptr - local_trajectory_builder; + std::unique_ptr local_trajectory_builder; if (trajectory_options.has_trajectory_builder_3d_options()) { - local_trajectory_builder = - common::make_unique( - trajectory_options.trajectory_builder_3d_options()); + local_trajectory_builder = common::make_unique( + trajectory_options.trajectory_builder_3d_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 31c5644..cec0e2e 100644 --- a/cartographer/mapping/pose_graph/constraint_builder.cc +++ b/cartographer/mapping/pose_graph/constraint_builder.cc @@ -18,8 +18,8 @@ #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/mapping_3d/scan_matching/ceres_scan_matcher_3d.h" +#include "cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_3d.h" #include "cartographer/sensor/voxel_filter.h" namespace cartographer { @@ -48,12 +48,12 @@ proto::ConstraintBuilderOptions CreateConstraintBuilderOptions( scan_matching::CreateCeresScanMatcherOptions2D( parameter_dictionary->GetDictionary("ceres_scan_matcher").get()); *options.mutable_fast_correlative_scan_matcher_options_3d() = - mapping_3d::scan_matching::CreateFastCorrelativeScanMatcherOptions( + scan_matching::CreateFastCorrelativeScanMatcherOptions3D( parameter_dictionary ->GetDictionary("fast_correlative_scan_matcher_3d") .get()); *options.mutable_ceres_scan_matcher_options_3d() = - mapping_3d::scan_matching::CreateCeresScanMatcherOptions( + scan_matching::CreateCeresScanMatcherOptions3D( parameter_dictionary->GetDictionary("ceres_scan_matcher_3d").get()); return options; } diff --git a/cartographer/mapping/pose_graph/proto/constraint_builder_options.proto b/cartographer/mapping/pose_graph/proto/constraint_builder_options.proto index 985d945..594a4c6 100644 --- a/cartographer/mapping/pose_graph/proto/constraint_builder_options.proto +++ b/cartographer/mapping/pose_graph/proto/constraint_builder_options.proto @@ -18,8 +18,8 @@ package cartographer.mapping.pose_graph.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"; +import "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options_3d.proto"; +import "cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options_3d.proto"; message ConstraintBuilderOptions { // A constraint will be added if the proportion of added constraints to @@ -52,8 +52,8 @@ message ConstraintBuilderOptions { fast_correlative_scan_matcher_options = 9; mapping.scan_matching.proto.CeresScanMatcherOptions2D ceres_scan_matcher_options = 11; - mapping_3d.scan_matching.proto.FastCorrelativeScanMatcherOptions + mapping.scan_matching.proto.FastCorrelativeScanMatcherOptions3D fast_correlative_scan_matcher_options_3d = 10; - mapping_3d.scan_matching.proto.CeresScanMatcherOptions + mapping.scan_matching.proto.CeresScanMatcherOptions3D ceres_scan_matcher_options_3d = 12; } diff --git a/cartographer/mapping/proto/submap.proto b/cartographer/mapping/proto/submap.proto index ac5e052..09fbbab 100644 --- a/cartographer/mapping/proto/submap.proto +++ b/cartographer/mapping/proto/submap.proto @@ -28,7 +28,7 @@ message Submap2D { ProbabilityGrid probability_grid = 4; } -// Serialized state of a mapping_3d::Submap. +// Serialized state of a Submap3D. message Submap3D { transform.proto.Rigid3d local_pose = 1; int32 num_range_data = 2; diff --git a/cartographer/mapping_2d/pose_graph/landmark_cost_function_2d.h b/cartographer/mapping_2d/pose_graph/landmark_cost_function_2d.h index 1d92b09..98dd7d2 100644 --- a/cartographer/mapping_2d/pose_graph/landmark_cost_function_2d.h +++ b/cartographer/mapping_2d/pose_graph/landmark_cost_function_2d.h @@ -55,10 +55,6 @@ class LandmarkCostFunction2D { 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 pose_graph::ComputeUnscaledError; - using pose_graph::ScaleError; - using pose_graph::SlerpQuaternions; - const std::array interpolated_pose_translation{ {prev_node_pose[0] + interpolation_parameter_ * (next_node_pose[0] - prev_node_pose[0]), diff --git a/cartographer/mapping_2d/pose_graph_2d_test.cc b/cartographer/mapping_2d/pose_graph_2d_test.cc index c080887..8366b98 100644 --- a/cartographer/mapping_2d/pose_graph_2d_test.cc +++ b/cartographer/mapping_2d/pose_graph_2d_test.cc @@ -40,7 +40,7 @@ class PoseGraph2DTest : public ::testing::Test { PoseGraph2DTest() : thread_pool_(1) { // Builds a wavy, irregularly circular point cloud that is unique // rotationally. This gives us good rotational texture and avoids any - // possibility of the CeresScanMatcher preferring free space (> + // possibility of the CeresScanMatcher2D preferring free space (> // kMinProbability) to unknown space (== kMinProbability). for (float t = 0.f; t < 2.f * M_PI; t += 0.005f) { const float r = (std::sin(20.f * t) + 2.f) * std::sin(t + 2.f); diff --git a/cartographer/mapping_2d/scan_matching/correlative_scan_matcher_2d.cc b/cartographer/mapping_2d/scan_matching/correlative_scan_matcher_2d.cc index 82fdece..d7b5de6 100644 --- a/cartographer/mapping_2d/scan_matching/correlative_scan_matcher_2d.cc +++ b/cartographer/mapping_2d/scan_matching/correlative_scan_matcher_2d.cc @@ -70,7 +70,7 @@ SearchParameters::SearchParameters(const int num_linear_perturbations, } } -void SearchParameters::ShrinkToFit(const std::vector& scans, +void SearchParameters::ShrinkToFit(const std::vector& scans, const CellLimits& cell_limits) { CHECK_EQ(scans.size(), num_scans); CHECK_EQ(linear_bounds.size(), num_scans); @@ -108,10 +108,10 @@ std::vector GenerateRotatedScans( return rotated_scans; } -std::vector DiscretizeScans( +std::vector DiscretizeScans( const MapLimits& map_limits, const std::vector& scans, const Eigen::Translation2f& initial_translation) { - std::vector discrete_scans; + std::vector discrete_scans; discrete_scans.reserve(scans.size()); for (const sensor::PointCloud& scan : scans) { discrete_scans.emplace_back(); diff --git a/cartographer/mapping_2d/scan_matching/correlative_scan_matcher_2d.h b/cartographer/mapping_2d/scan_matching/correlative_scan_matcher_2d.h index ab082f9..515402e 100644 --- a/cartographer/mapping_2d/scan_matching/correlative_scan_matcher_2d.h +++ b/cartographer/mapping_2d/scan_matching/correlative_scan_matcher_2d.h @@ -29,7 +29,7 @@ namespace cartographer { namespace mapping { namespace scan_matching { -typedef std::vector DiscreteScan; +typedef std::vector DiscreteScan2D; // Describes the search space. struct SearchParameters { @@ -49,7 +49,7 @@ struct SearchParameters { double angular_perturbation_step_size, double resolution); // Tightens the search window as much as possible. - void ShrinkToFit(const std::vector& scans, + void ShrinkToFit(const std::vector& scans, const CellLimits& cell_limits); int num_angular_perturbations; @@ -66,15 +66,15 @@ std::vector GenerateRotatedScans( // Translates and discretizes the rotated scans into a vector of integer // indices. -std::vector DiscretizeScans( +std::vector DiscretizeScans( const MapLimits& map_limits, const std::vector& scans, const Eigen::Translation2f& initial_translation); // A possible solution. -struct Candidate { - Candidate(const int init_scan_index, const int init_x_index_offset, - const int init_y_index_offset, - const SearchParameters& search_parameters) +struct Candidate2D { + Candidate2D(const int init_scan_index, const int init_x_index_offset, + const int init_y_index_offset, + const SearchParameters& search_parameters) : scan_index(init_scan_index), x_index_offset(init_x_index_offset), y_index_offset(init_y_index_offset), @@ -90,7 +90,7 @@ struct Candidate { int x_index_offset = 0; int y_index_offset = 0; - // Pose of this Candidate relative to the initial pose. + // Pose of this Candidate2D relative to the initial pose. double x = 0.; double y = 0.; double orientation = 0.; @@ -98,8 +98,8 @@ struct Candidate { // Score, higher is better. float score = 0.f; - bool operator<(const Candidate& other) const { return score < other.score; } - bool operator>(const Candidate& other) const { return score > other.score; } + bool operator<(const Candidate2D& other) const { return score < other.score; } + bool operator>(const Candidate2D& other) const { return score > other.score; } }; } // namespace scan_matching 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 0e756e7..1fa9072 100644 --- a/cartographer/mapping_2d/scan_matching/correlative_scan_matcher_test.cc +++ b/cartographer/mapping_2d/scan_matching/correlative_scan_matcher_test.cc @@ -42,7 +42,7 @@ TEST(SearchParameters, Construction) { TEST(Candidate, Construction) { const SearchParameters search_parameters(4, 5, 0.03, 0.05); - const Candidate candidate(3, 4, -5, search_parameters); + const Candidate2D candidate(3, 4, -5, search_parameters); EXPECT_EQ(3, candidate.scan_index); EXPECT_EQ(4, candidate.x_index_offset); EXPECT_EQ(-5, candidate.y_index_offset); @@ -51,7 +51,7 @@ TEST(Candidate, Construction) { EXPECT_NEAR(-0.06, candidate.orientation, 1e-9); EXPECT_NEAR(0., candidate.score, 1e-9); - Candidate bigger_candidate(3, 4, 5, search_parameters); + Candidate2D bigger_candidate(3, 4, 5, search_parameters); bigger_candidate.score = 1.; EXPECT_LT(candidate, bigger_candidate); } @@ -83,7 +83,7 @@ TEST(DiscretizeScans, DiscretizeScans) { CellLimits(6, 6)); const std::vector scans = GenerateRotatedScans(point_cloud, SearchParameters(0, 0, 0., 0.)); - const std::vector discrete_scans = + const std::vector discrete_scans = DiscretizeScans(map_limits, scans, Eigen::Translation2f::Identity()); EXPECT_EQ(1, discrete_scans.size()); EXPECT_EQ(7, discrete_scans[0].size()); diff --git a/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_2d.cc b/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_2d.cc index 87d0666..3cba7af 100644 --- a/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_2d.cc +++ b/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_2d.cc @@ -87,7 +87,7 @@ CreateFastCorrelativeScanMatcherOptions2D( return options; } -PrecomputationGrid::PrecomputationGrid( +PrecomputationGrid2D::PrecomputationGrid2D( const ProbabilityGrid& probability_grid, const CellLimits& limits, const int width, std::vector* reusable_intermediate_grid) : offset_(-width + 1, -width + 1), @@ -157,7 +157,7 @@ PrecomputationGrid::PrecomputationGrid( } } -uint8 PrecomputationGrid::ComputeCellValue(const float probability) const { +uint8 PrecomputationGrid2D::ComputeCellValue(const float probability) const { const int cell_value = common::RoundToInt((probability - kMinProbability) * (255.f / (kMaxProbability - kMinProbability))); @@ -185,14 +185,14 @@ class PrecomputationGridStack { } } - const PrecomputationGrid& Get(int index) { + const PrecomputationGrid2D& Get(int index) { return precomputation_grids_[index]; } int max_depth() const { return precomputation_grids_.size() - 1; } private: - std::vector precomputation_grids_; + std::vector precomputation_grids_; }; FastCorrelativeScanMatcher2D::FastCorrelativeScanMatcher2D( @@ -249,15 +249,15 @@ bool FastCorrelativeScanMatcher2D::MatchWithSearchParameters( initial_rotation.cast().angle(), Eigen::Vector3f::UnitZ()))); const std::vector rotated_scans = GenerateRotatedScans(rotated_point_cloud, search_parameters); - const std::vector discrete_scans = DiscretizeScans( + const std::vector discrete_scans = DiscretizeScans( limits_, rotated_scans, Eigen::Translation2f(initial_pose_estimate.translation().x(), initial_pose_estimate.translation().y())); search_parameters.ShrinkToFit(discrete_scans, limits_.cell_limits()); - const std::vector lowest_resolution_candidates = + const std::vector lowest_resolution_candidates = ComputeLowestResolutionCandidates(discrete_scans, search_parameters); - const Candidate best_candidate = BranchAndBound( + const Candidate2D best_candidate = BranchAndBound( discrete_scans, search_parameters, lowest_resolution_candidates, precomputation_grid_stack_->max_depth(), min_score); if (best_candidate.score > min_score) { @@ -271,11 +271,11 @@ bool FastCorrelativeScanMatcher2D::MatchWithSearchParameters( return false; } -std::vector +std::vector FastCorrelativeScanMatcher2D::ComputeLowestResolutionCandidates( - const std::vector& discrete_scans, + const std::vector& discrete_scans, const SearchParameters& search_parameters) const { - std::vector lowest_resolution_candidates = + std::vector lowest_resolution_candidates = GenerateLowestResolutionCandidates(search_parameters); ScoreCandidates( precomputation_grid_stack_->Get(precomputation_grid_stack_->max_depth()), @@ -283,7 +283,7 @@ FastCorrelativeScanMatcher2D::ComputeLowestResolutionCandidates( return lowest_resolution_candidates; } -std::vector +std::vector FastCorrelativeScanMatcher2D::GenerateLowestResolutionCandidates( const SearchParameters& search_parameters) const { const int linear_step_size = 1 << precomputation_grid_stack_->max_depth(); @@ -301,7 +301,7 @@ FastCorrelativeScanMatcher2D::GenerateLowestResolutionCandidates( num_candidates += num_lowest_resolution_linear_x_candidates * num_lowest_resolution_linear_y_candidates; } - std::vector candidates; + std::vector candidates; candidates.reserve(num_candidates); for (int scan_index = 0; scan_index != search_parameters.num_scans; ++scan_index) { @@ -322,11 +322,11 @@ FastCorrelativeScanMatcher2D::GenerateLowestResolutionCandidates( } void FastCorrelativeScanMatcher2D::ScoreCandidates( - const PrecomputationGrid& precomputation_grid, - const std::vector& discrete_scans, + const PrecomputationGrid2D& precomputation_grid, + const std::vector& discrete_scans, const SearchParameters& search_parameters, - std::vector* const candidates) const { - for (Candidate& candidate : *candidates) { + std::vector* const candidates) const { + for (Candidate2D& candidate : *candidates) { int sum = 0; for (const Eigen::Array2i& xy_index : discrete_scans[candidate.scan_index]) { @@ -335,29 +335,30 @@ void FastCorrelativeScanMatcher2D::ScoreCandidates( xy_index.y() + candidate.y_index_offset); sum += precomputation_grid.GetValue(proposed_xy_index); } - candidate.score = PrecomputationGrid::ToProbability( + candidate.score = PrecomputationGrid2D::ToProbability( sum / static_cast(discrete_scans[candidate.scan_index].size())); } - std::sort(candidates->begin(), candidates->end(), std::greater()); + std::sort(candidates->begin(), candidates->end(), + std::greater()); } -Candidate FastCorrelativeScanMatcher2D::BranchAndBound( - const std::vector& discrete_scans, +Candidate2D FastCorrelativeScanMatcher2D::BranchAndBound( + const std::vector& discrete_scans, const SearchParameters& search_parameters, - const std::vector& candidates, const int candidate_depth, + const std::vector& candidates, const int candidate_depth, float min_score) const { if (candidate_depth == 0) { // Return the best candidate. return *candidates.begin(); } - Candidate best_high_resolution_candidate(0, 0, 0, search_parameters); + Candidate2D best_high_resolution_candidate(0, 0, 0, search_parameters); best_high_resolution_candidate.score = min_score; - for (const Candidate& candidate : candidates) { + for (const Candidate2D& candidate : candidates) { if (candidate.score <= min_score) { break; } - std::vector higher_resolution_candidates; + std::vector higher_resolution_candidates; const int half_width = 1 << (candidate_depth - 1); for (int x_offset : {0, half_width}) { if (candidate.x_index_offset + x_offset > diff --git a/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_2d.h b/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_2d.h index a3c210f..819f373 100644 --- a/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_2d.h +++ b/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_2d.h @@ -47,11 +47,11 @@ CreateFastCorrelativeScanMatcherOptions2D( // A precomputed grid that contains in each cell (x0, y0) the maximum // probability in the width x width area defined by x0 <= x < x0 + width and // y0 <= y < y0. -class PrecomputationGrid { +class PrecomputationGrid2D { public: - PrecomputationGrid(const ProbabilityGrid& probability_grid, - const CellLimits& limits, int width, - std::vector* reusable_intermediate_grid); + PrecomputationGrid2D(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 // kMinProbability and kMaxProbability. @@ -130,19 +130,19 @@ class FastCorrelativeScanMatcher2D { const transform::Rigid2d& initial_pose_estimate, const sensor::PointCloud& point_cloud, float min_score, float* score, transform::Rigid2d* pose_estimate) const; - std::vector ComputeLowestResolutionCandidates( - const std::vector& discrete_scans, + std::vector ComputeLowestResolutionCandidates( + const std::vector& discrete_scans, const SearchParameters& search_parameters) const; - std::vector GenerateLowestResolutionCandidates( + std::vector GenerateLowestResolutionCandidates( const SearchParameters& search_parameters) const; - void ScoreCandidates(const PrecomputationGrid& precomputation_grid, - const std::vector& discrete_scans, + void ScoreCandidates(const PrecomputationGrid2D& precomputation_grid, + const std::vector& discrete_scans, const SearchParameters& search_parameters, - std::vector* const candidates) const; - Candidate BranchAndBound(const std::vector& discrete_scans, - const SearchParameters& search_parameters, - const std::vector& candidates, - int candidate_depth, float min_score) const; + std::vector* const candidates) const; + Candidate2D BranchAndBound(const std::vector& discrete_scans, + const SearchParameters& search_parameters, + const std::vector& candidates, + int candidate_depth, float min_score) const; const proto::FastCorrelativeScanMatcherOptions2D options_; MapLimits limits_; diff --git a/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_2d_test.cc b/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_2d_test.cc index 2e00f87..118fef2 100644 --- a/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_2d_test.cc +++ b/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_2d_test.cc @@ -44,12 +44,12 @@ TEST(PrecomputationGridTest, CorrectValues) { for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(Eigen::Array2i(50, 50), Eigen::Array2i(249, 249))) { probability_grid.SetProbability( - xy_index, PrecomputationGrid::ToProbability(distribution(prng))); + xy_index, PrecomputationGrid2D::ToProbability(distribution(prng))); } std::vector reusable_intermediate_grid; for (const int width : {1, 2, 3, 8}) { - PrecomputationGrid precomputation_grid( + PrecomputationGrid2D precomputation_grid( probability_grid, probability_grid.limits().cell_limits(), width, &reusable_intermediate_grid); for (const Eigen::Array2i& xy_index : @@ -63,7 +63,7 @@ TEST(PrecomputationGridTest, CorrectValues) { } } EXPECT_NEAR(max_score, - PrecomputationGrid::ToProbability( + PrecomputationGrid2D::ToProbability( precomputation_grid.GetValue(xy_index)), 1e-4); } @@ -78,12 +78,12 @@ TEST(PrecomputationGridTest, TinyProbabilityGrid) { for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(probability_grid.limits().cell_limits())) { probability_grid.SetProbability( - xy_index, PrecomputationGrid::ToProbability(distribution(prng))); + xy_index, PrecomputationGrid2D::ToProbability(distribution(prng))); } std::vector reusable_intermediate_grid; for (const int width : {1, 2, 3, 8, 200}) { - PrecomputationGrid precomputation_grid( + PrecomputationGrid2D precomputation_grid( probability_grid, probability_grid.limits().cell_limits(), width, &reusable_intermediate_grid); for (const Eigen::Array2i& xy_index : @@ -97,7 +97,7 @@ TEST(PrecomputationGridTest, TinyProbabilityGrid) { } } EXPECT_NEAR(max_score, - PrecomputationGrid::ToProbability( + PrecomputationGrid2D::ToProbability( precomputation_grid.GetValue(xy_index)), 1e-4); } diff --git a/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_2d.cc b/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_2d.cc index 2e93689..aeeb607 100644 --- a/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_2d.cc +++ b/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_2d.cc @@ -37,7 +37,7 @@ RealTimeCorrelativeScanMatcher2D::RealTimeCorrelativeScanMatcher2D( const proto::RealTimeCorrelativeScanMatcherOptions& options) : options_(options) {} -std::vector +std::vector RealTimeCorrelativeScanMatcher2D::GenerateExhaustiveSearchCandidates( const SearchParameters& search_parameters) const { int num_candidates = 0; @@ -51,7 +51,7 @@ RealTimeCorrelativeScanMatcher2D::GenerateExhaustiveSearchCandidates( search_parameters.linear_bounds[scan_index].min_y + 1); num_candidates += num_linear_x_candidates * num_linear_y_candidates; } - std::vector candidates; + std::vector candidates; candidates.reserve(num_candidates); for (int scan_index = 0; scan_index != search_parameters.num_scans; ++scan_index) { @@ -89,16 +89,16 @@ double RealTimeCorrelativeScanMatcher2D::Match( const std::vector rotated_scans = GenerateRotatedScans(rotated_point_cloud, search_parameters); - const std::vector discrete_scans = DiscretizeScans( + const std::vector discrete_scans = DiscretizeScans( probability_grid.limits(), rotated_scans, Eigen::Translation2f(initial_pose_estimate.translation().x(), initial_pose_estimate.translation().y())); - std::vector candidates = + std::vector candidates = GenerateExhaustiveSearchCandidates(search_parameters); ScoreCandidates(probability_grid, discrete_scans, search_parameters, &candidates); - const Candidate& best_candidate = + const Candidate2D& best_candidate = *std::max_element(candidates.begin(), candidates.end()); *pose_estimate = transform::Rigid2d( {initial_pose_estimate.translation().x() + best_candidate.x, @@ -109,10 +109,10 @@ double RealTimeCorrelativeScanMatcher2D::Match( void RealTimeCorrelativeScanMatcher2D::ScoreCandidates( const ProbabilityGrid& probability_grid, - const std::vector& discrete_scans, + const std::vector& discrete_scans, const SearchParameters& search_parameters, - std::vector* const candidates) const { - for (Candidate& candidate : *candidates) { + std::vector* const candidates) const { + for (Candidate2D& candidate : *candidates) { candidate.score = 0.f; for (const Eigen::Array2i& xy_index : discrete_scans[candidate.scan_index]) { diff --git a/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_2d.h b/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_2d.h index 884617a..fcad1c1 100644 --- a/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_2d.h +++ b/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_2d.h @@ -69,18 +69,18 @@ class RealTimeCorrelativeScanMatcher2D { const ProbabilityGrid& probability_grid, transform::Rigid2d* pose_estimate) const; - // Computes the score for each Candidate in a collection. The cost is computed - // as the sum of probabilities, different from the Ceres CostFunctions: - // http://ceres-solver.org/modeling.html + // Computes the score for each Candidate2D in a collection. The cost is + // computed as the sum of probabilities, different from the Ceres + // CostFunctions: http://ceres-solver.org/modeling.html // // Visible for testing. void ScoreCandidates(const ProbabilityGrid& probability_grid, - const std::vector& discrete_scans, + const std::vector& discrete_scans, const SearchParameters& search_parameters, - std::vector* candidates) const; + std::vector* candidates) const; private: - std::vector GenerateExhaustiveSearchCandidates( + std::vector GenerateExhaustiveSearchCandidates( const SearchParameters& search_parameters) const; const proto::RealTimeCorrelativeScanMatcherOptions options_; diff --git a/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_2d_test.cc b/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_2d_test.cc index 2ff8f9c..0b25c10 100644 --- a/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_2d_test.cc +++ b/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_2d_test.cc @@ -85,9 +85,9 @@ TEST_F(RealTimeCorrelativeScanMatcherTest, ScorePerfectHighResolutionCandidate) { const std::vector scans = GenerateRotatedScans(point_cloud_, SearchParameters(0, 0, 0., 0.)); - const std::vector discrete_scans = DiscretizeScans( + const std::vector discrete_scans = DiscretizeScans( probability_grid_.limits(), scans, Eigen::Translation2f::Identity()); - std::vector candidates; + std::vector candidates; candidates.emplace_back(0, 0, 0, SearchParameters(0, 0, 0., 0.)); real_time_correlative_scan_matcher_->ScoreCandidates( probability_grid_, discrete_scans, SearchParameters(0, 0, 0., 0.), @@ -103,9 +103,9 @@ TEST_F(RealTimeCorrelativeScanMatcherTest, ScorePartiallyCorrectHighResolutionCandidate) { const std::vector scans = GenerateRotatedScans(point_cloud_, SearchParameters(0, 0, 0., 0.)); - const std::vector discrete_scans = DiscretizeScans( + const std::vector discrete_scans = DiscretizeScans( probability_grid_.limits(), scans, Eigen::Translation2f::Identity()); - std::vector candidates; + std::vector candidates; candidates.emplace_back(0, 0, 1, SearchParameters(0, 0, 0., 0.)); real_time_correlative_scan_matcher_->ScoreCandidates( probability_grid_, discrete_scans, SearchParameters(0, 0, 0., 0.), diff --git a/cartographer/mapping_3d/imu_integration.h b/cartographer/mapping_3d/imu_integration.h index 4358e1a..65f316c 100644 --- a/cartographer/mapping_3d/imu_integration.h +++ b/cartographer/mapping_3d/imu_integration.h @@ -28,7 +28,7 @@ #include "glog/logging.h" namespace cartographer { -namespace mapping_3d { +namespace mapping { template struct IntegrateImuResult { @@ -92,7 +92,7 @@ IntegrateImuResult IntegrateImu(const RangeType& imu_data, start_time, end_time, it); } -} // namespace mapping_3d +} // namespace mapping } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_3D_IMU_INTEGRATION_H_ diff --git a/cartographer/mapping_3d/local_trajectory_builder_options_3d.cc b/cartographer/mapping_3d/local_trajectory_builder_options_3d.cc index 858f32d..134fc72 100644 --- a/cartographer/mapping_3d/local_trajectory_builder_options_3d.cc +++ b/cartographer/mapping_3d/local_trajectory_builder_options_3d.cc @@ -18,7 +18,7 @@ #include "cartographer/internal/mapping/motion_filter.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/scan_matching/ceres_scan_matcher_3d.h" #include "cartographer/mapping_3d/submap_3d.h" #include "cartographer/sensor/voxel_filter.h" #include "glog/logging.h" @@ -53,7 +53,7 @@ proto::LocalTrajectoryBuilderOptions3D CreateLocalTrajectoryBuilderOptions3D( ->GetDictionary("real_time_correlative_scan_matcher") .get()); *options.mutable_ceres_scan_matcher_options() = - mapping_3d::scan_matching::CreateCeresScanMatcherOptions( + mapping::scan_matching::CreateCeresScanMatcherOptions3D( parameter_dictionary->GetDictionary("ceres_scan_matcher").get()); *options.mutable_motion_filter_options() = CreateMotionFilterOptions( parameter_dictionary->GetDictionary("motion_filter").get()); diff --git a/cartographer/mapping_3d/pose_graph/constraint_builder.cc b/cartographer/mapping_3d/pose_graph/constraint_builder_3d.cc similarity index 86% rename from cartographer/mapping_3d/pose_graph/constraint_builder.cc rename to cartographer/mapping_3d/pose_graph/constraint_builder_3d.cc index 5106a8f..03f5972 100644 --- a/cartographer/mapping_3d/pose_graph/constraint_builder.cc +++ b/cartographer/mapping_3d/pose_graph/constraint_builder_3d.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/mapping_3d/pose_graph/constraint_builder.h" +#include "cartographer/mapping_3d/pose_graph/constraint_builder_3d.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_3d/scan_matching/proto/ceres_scan_matcher_options.pb.h" -#include "cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options.pb.h" +#include "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options_3d.pb.h" +#include "cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options_3d.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_3d { +namespace mapping { namespace pose_graph { static auto* kConstraintsSearchedMetric = metrics::Counter::Null(); @@ -55,15 +55,15 @@ static auto* kGlobalConstraintRotationalScoresMetric = static auto* kGlobalConstraintLowResolutionScoresMetric = metrics::Histogram::Null(); -ConstraintBuilder::ConstraintBuilder( - const mapping::pose_graph::proto::ConstraintBuilderOptions& options, +ConstraintBuilder3D::ConstraintBuilder3D( + const 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_3d()) {} -ConstraintBuilder::~ConstraintBuilder() { +ConstraintBuilder3D::~ConstraintBuilder3D() { common::MutexLocker locker(&mutex_); CHECK_EQ(constraints_.size(), 0) << "WhenDone() was not called"; CHECK_EQ(pending_computations_.size(), 0); @@ -71,11 +71,10 @@ ConstraintBuilder::~ConstraintBuilder() { CHECK(when_done_ == nullptr); } -void ConstraintBuilder::MaybeAddConstraint( - const mapping::SubmapId& submap_id, const mapping::Submap3D* const submap, - const mapping::NodeId& node_id, - const mapping::TrajectoryNode::Data* const constant_data, - const std::vector& submap_nodes, +void ConstraintBuilder3D::MaybeAddConstraint( + const SubmapId& submap_id, const Submap3D* const submap, + const NodeId& node_id, const TrajectoryNode::Data* const constant_data, + const std::vector& submap_nodes, const transform::Rigid3d& global_node_pose, const transform::Rigid3d& global_submap_pose) { if ((global_node_pose.translation() - global_submap_pose.translation()) @@ -99,11 +98,10 @@ void ConstraintBuilder::MaybeAddConstraint( } } -void ConstraintBuilder::MaybeAddGlobalConstraint( - const mapping::SubmapId& submap_id, const mapping::Submap3D* const submap, - const mapping::NodeId& node_id, - const mapping::TrajectoryNode::Data* const constant_data, - const std::vector& submap_nodes, +void ConstraintBuilder3D::MaybeAddGlobalConstraint( + const SubmapId& submap_id, const Submap3D* const submap, + const NodeId& node_id, const TrajectoryNode::Data* const constant_data, + const std::vector& submap_nodes, const Eigen::Quaterniond& global_node_rotation, const Eigen::Quaterniond& global_submap_rotation) { common::MutexLocker locker(&mutex_); @@ -122,13 +120,13 @@ void ConstraintBuilder::MaybeAddGlobalConstraint( }); } -void ConstraintBuilder::NotifyEndOfNode() { +void ConstraintBuilder3D::NotifyEndOfNode() { common::MutexLocker locker(&mutex_); ++current_computation_; } -void ConstraintBuilder::WhenDone( - const std::function& callback) { +void ConstraintBuilder3D::WhenDone( + const std::function& callback) { common::MutexLocker locker(&mutex_); CHECK(when_done_ == nullptr); when_done_ = @@ -139,11 +137,9 @@ void ConstraintBuilder::WhenDone( [this, current_computation] { FinishComputation(current_computation); }); } -void ConstraintBuilder::ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( - const mapping::SubmapId& submap_id, - const std::vector& submap_nodes, - const mapping::Submap3D* const submap, - const std::function& work_item) { +void ConstraintBuilder3D::ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( + const SubmapId& submap_id, const std::vector& submap_nodes, + const Submap3D* const submap, const std::function& work_item) { if (submap_scan_matchers_[submap_id].fast_correlative_scan_matcher != nullptr) { thread_pool_->Schedule(work_item); @@ -157,12 +153,11 @@ void ConstraintBuilder::ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( } } -void ConstraintBuilder::ConstructSubmapScanMatcher( - const mapping::SubmapId& submap_id, - const std::vector& submap_nodes, - const mapping::Submap3D* const submap) { +void ConstraintBuilder3D::ConstructSubmapScanMatcher( + const SubmapId& submap_id, const std::vector& submap_nodes, + const Submap3D* const submap) { auto submap_scan_matcher = - common::make_unique( + common::make_unique( submap->high_resolution_hybrid_grid(), &submap->low_resolution_hybrid_grid(), submap_nodes, options_.fast_correlative_scan_matcher_options_3d()); @@ -177,8 +172,8 @@ void ConstraintBuilder::ConstructSubmapScanMatcher( submap_queued_work_items_.erase(submap_id); } -const ConstraintBuilder::SubmapScanMatcher* -ConstraintBuilder::GetSubmapScanMatcher(const mapping::SubmapId& submap_id) { +const ConstraintBuilder3D::SubmapScanMatcher* +ConstraintBuilder3D::GetSubmapScanMatcher(const SubmapId& submap_id) { common::MutexLocker locker(&mutex_); const SubmapScanMatcher* submap_scan_matcher = &submap_scan_matchers_[submap_id]; @@ -186,10 +181,9 @@ ConstraintBuilder::GetSubmapScanMatcher(const mapping::SubmapId& submap_id) { return submap_scan_matcher; } -void ConstraintBuilder::ComputeConstraint( - const mapping::SubmapId& submap_id, const mapping::NodeId& node_id, - bool match_full_submap, - const mapping::TrajectoryNode::Data* const constant_data, +void ConstraintBuilder3D::ComputeConstraint( + const SubmapId& submap_id, const NodeId& node_id, bool match_full_submap, + const TrajectoryNode::Data* const constant_data, const transform::Rigid3d& global_node_pose, const transform::Rigid3d& global_submap_pose, std::unique_ptr* constraint) { @@ -199,7 +193,7 @@ void ConstraintBuilder::ComputeConstraint( // The 'constraint_transform' (submap i <- node j) is computed from: // - a 'high_resolution_point_cloud' in node j and // - the initial guess 'initial_pose' (submap i <- node j). - std::unique_ptr + std::unique_ptr match_result; // Compute 'pose_estimate' in three stages: @@ -293,7 +287,7 @@ void ConstraintBuilder::ComputeConstraint( } } -void ConstraintBuilder::FinishComputation(const int computation_index) { +void ConstraintBuilder3D::FinishComputation(const int computation_index) { Result result; std::unique_ptr> callback; { @@ -330,7 +324,7 @@ void ConstraintBuilder::FinishComputation(const int computation_index) { } } -int ConstraintBuilder::GetNumFinishedNodes() { +int ConstraintBuilder3D::GetNumFinishedNodes() { common::MutexLocker locker(&mutex_); if (pending_computations_.empty()) { return current_computation_; @@ -338,13 +332,13 @@ int ConstraintBuilder::GetNumFinishedNodes() { return pending_computations_.begin()->first; } -void ConstraintBuilder::DeleteScanMatcher(const mapping::SubmapId& submap_id) { +void ConstraintBuilder3D::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 ConstraintBuilder3D::RegisterMetrics(metrics::FamilyFactory* factory) { auto* counts = factory->NewCounterFamily( "/mapping_3d/pose_graph/constraint_builder/constraints", "Constraints computed"); @@ -378,5 +372,5 @@ void ConstraintBuilder::RegisterMetrics(metrics::FamilyFactory* factory) { } } // namespace pose_graph -} // namespace mapping_3d +} // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping_3d/pose_graph/constraint_builder.h b/cartographer/mapping_3d/pose_graph/constraint_builder_3d.h similarity index 70% rename from cartographer/mapping_3d/pose_graph/constraint_builder.h rename to cartographer/mapping_3d/pose_graph/constraint_builder_3d.h index 5df4760..9b66f2f 100644 --- a/cartographer/mapping_3d/pose_graph/constraint_builder.h +++ b/cartographer/mapping_3d/pose_graph/constraint_builder_3d.h @@ -14,8 +14,8 @@ * limitations under the License. */ -#ifndef CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_CONSTRAINT_BUILDER_H_ -#define CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_CONSTRAINT_BUILDER_H_ +#ifndef CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_CONSTRAINT_BUILDER_3D_H_ +#define CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_CONSTRAINT_BUILDER_3D_H_ #include #include @@ -34,8 +34,8 @@ #include "cartographer/mapping/pose_graph/proto/constraint_builder_options.pb.h" #include "cartographer/mapping/pose_graph_interface.h" #include "cartographer/mapping/trajectory_node.h" -#include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h" -#include "cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h" +#include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher_3d.h" +#include "cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_3d.h" #include "cartographer/mapping_3d/submap_3d.h" #include "cartographer/metrics/family_factory.h" #include "cartographer/sensor/compressed_point_cloud.h" @@ -43,7 +43,7 @@ #include "cartographer/sensor/voxel_filter.h" namespace cartographer { -namespace mapping_3d { +namespace mapping { namespace pose_graph { // Asynchronously computes constraints. @@ -54,18 +54,17 @@ namespace pose_graph { // MaybeAdd(Global)Constraint()/WhenDone() cycle can follow. // // This class is thread-safe. -class ConstraintBuilder { +class ConstraintBuilder3D { public: using Constraint = mapping::PoseGraphInterface::Constraint; using Result = std::vector; - ConstraintBuilder( - const mapping::pose_graph::proto::ConstraintBuilderOptions& options, - common::ThreadPool* thread_pool); - ~ConstraintBuilder(); + ConstraintBuilder3D(const proto::ConstraintBuilderOptions& options, + common::ThreadPool* thread_pool); + ~ConstraintBuilder3D(); - ConstraintBuilder(const ConstraintBuilder&) = delete; - ConstraintBuilder& operator=(const ConstraintBuilder&) = delete; + ConstraintBuilder3D(const ConstraintBuilder3D&) = delete; + ConstraintBuilder3D& operator=(const ConstraintBuilder3D&) = delete; // Schedules exploring a new constraint between 'submap' identified by // 'submap_id', and the 'compressed_point_cloud' for 'node_id'. @@ -75,13 +74,12 @@ 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::Submap3D* submap, - const mapping::NodeId& node_id, - const mapping::TrajectoryNode::Data* const constant_data, - const std::vector& submap_nodes, - const transform::Rigid3d& global_node_pose, - const transform::Rigid3d& global_submap_pose); + void MaybeAddConstraint(const SubmapId& submap_id, const Submap3D* submap, + const NodeId& node_id, + const TrajectoryNode::Data* const constant_data, + const std::vector& submap_nodes, + const transform::Rigid3d& global_node_pose, + const transform::Rigid3d& global_submap_pose); // Schedules exploring a new constraint between 'submap' identified by // 'submap_id' and the 'compressed_point_cloud' for 'node_id'. @@ -93,10 +91,9 @@ 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::Submap3D* submap, - const mapping::NodeId& node_id, - const mapping::TrajectoryNode::Data* const constant_data, - const std::vector& submap_nodes, + const SubmapId& submap_id, const Submap3D* submap, const NodeId& node_id, + const TrajectoryNode::Data* const constant_data, + const std::vector& submap_nodes, const Eigen::Quaterniond& global_node_rotation, const Eigen::Quaterniond& global_submap_rotation); @@ -111,52 +108,51 @@ 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::HybridGrid* high_resolution_hybrid_grid; - const mapping::HybridGrid* low_resolution_hybrid_grid; - std::unique_ptr + const HybridGrid* high_resolution_hybrid_grid; + const HybridGrid* low_resolution_hybrid_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 std::vector& submap_nodes, - const mapping::Submap3D* submap, const std::function& work_item) - REQUIRES(mutex_); + const SubmapId& submap_id, + const std::vector& submap_nodes, const Submap3D* submap, + const std::function& work_item) REQUIRES(mutex_); // Constructs the scan matcher for a 'submap', then schedules its work items. void ConstructSubmapScanMatcher( - const mapping::SubmapId& submap_id, - const std::vector& submap_nodes, - const mapping::Submap3D* submap) EXCLUDES(mutex_); + const SubmapId& submap_id, + const std::vector& submap_nodes, const Submap3D* 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. // As output, it may create a new Constraint in 'constraint'. - void ComputeConstraint( - const mapping::SubmapId& submap_id, const mapping::NodeId& node_id, - bool match_full_submap, - const mapping::TrajectoryNode::Data* const constant_data, - const transform::Rigid3d& global_node_pose, - const transform::Rigid3d& global_submap_pose, - std::unique_ptr* constraint) EXCLUDES(mutex_); + void ComputeConstraint(const SubmapId& submap_id, const NodeId& node_id, + bool match_full_submap, + const TrajectoryNode::Data* const constant_data, + const transform::Rigid3d& global_node_pose, + const transform::Rigid3d& global_submap_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 proto::ConstraintBuilderOptions options_; common::ThreadPool* thread_pool_; common::Mutex mutex_; @@ -178,16 +174,16 @@ 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::CeresScanMatcher3D ceres_scan_matcher_; // Histograms of scan matcher scores. common::Histogram score_histogram_ GUARDED_BY(mutex_); @@ -196,7 +192,7 @@ class ConstraintBuilder { }; } // namespace pose_graph -} // namespace mapping_3d +} // namespace mapping } // namespace cartographer -#endif // CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_CONSTRAINT_BUILDER_H_ +#endif // CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_CONSTRAINT_BUILDER_3D_H_ diff --git a/cartographer/mapping_3d/pose_graph/landmark_cost_function.h b/cartographer/mapping_3d/pose_graph/landmark_cost_function_3d.h similarity index 85% rename from cartographer/mapping_3d/pose_graph/landmark_cost_function.h rename to cartographer/mapping_3d/pose_graph/landmark_cost_function_3d.h index 46a028f..ebaa415 100644 --- a/cartographer/mapping_3d/pose_graph/landmark_cost_function.h +++ b/cartographer/mapping_3d/pose_graph/landmark_cost_function_3d.h @@ -14,42 +14,43 @@ * limitations under the License. */ -#ifndef CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_LANDMARK_COST_FUNCTION_H_ -#define CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_LANDMARK_COST_FUNCTION_H_ +#ifndef CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_LANDMARK_COST_FUNCTION_3D_H_ +#define CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_LANDMARK_COST_FUNCTION_3D_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_3d/pose_graph/optimization_problem.h" +#include "cartographer/mapping_3d/pose_graph/optimization_problem_3d.h" #include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/transform.h" #include "ceres/ceres.h" #include "ceres/jet.h" namespace cartographer { -namespace mapping_3d { +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. -class LandmarkCostFunction { +class LandmarkCostFunction3D { 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 OptimizationProblem3D::NodeData& prev_node, + const OptimizationProblem3D::NodeData& next_node) { return new ceres::AutoDiffCostFunction< - LandmarkCostFunction, 6 /* residuals */, + LandmarkCostFunction3D, 6 /* residuals */, 4 /* previous node rotation variables */, 3 /* previous node translation variables */, 4 /* next node rotation variables */, 3 /* next node translation variables */, 4 /* landmark rotation variables */, 3 /* landmark translation variables */>( - new LandmarkCostFunction(observation, prev_node, next_node)); + new LandmarkCostFunction3D(observation, prev_node, next_node)); } template @@ -59,10 +60,6 @@ class LandmarkCostFunction { const T* const next_node_translation, 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; - const std::array interpolated_pose_translation{ {prev_node_translation[0] + interpolation_parameter_ * @@ -87,8 +84,9 @@ class LandmarkCostFunction { } private: - LandmarkCostFunction(const LandmarkObservation& observation, - const NodeData& prev_node, const NodeData& next_node) + LandmarkCostFunction3D(const LandmarkObservation& observation, + const OptimizationProblem3D::NodeData& prev_node, + const OptimizationProblem3D::NodeData& next_node) : landmark_to_tracking_transform_( observation.landmark_to_tracking_transform), translation_weight_(observation.translation_weight), @@ -104,7 +102,7 @@ class LandmarkCostFunction { }; } // namespace pose_graph -} // namespace mapping_3d +} // namespace mapping } // namespace cartographer -#endif // CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_LANDMARK_COST_FUNCTION_H_ +#endif // CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_LANDMARK_COST_FUNCTION_3D_H_ diff --git a/cartographer/mapping_3d/pose_graph/landmark_cost_function_test.cc b/cartographer/mapping_3d/pose_graph/landmark_cost_function_3d_test.cc similarity index 89% rename from cartographer/mapping_3d/pose_graph/landmark_cost_function_test.cc rename to cartographer/mapping_3d/pose_graph/landmark_cost_function_3d_test.cc index c124c6b..0e2aa49 100644 --- a/cartographer/mapping_3d/pose_graph/landmark_cost_function_test.cc +++ b/cartographer/mapping_3d/pose_graph/landmark_cost_function_3d_test.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/mapping_3d/pose_graph/landmark_cost_function.h" +#include "cartographer/mapping_3d/pose_graph/landmark_cost_function_3d.h" #include @@ -23,7 +23,7 @@ #include "gtest/gtest.h" namespace cartographer { -namespace mapping_3d { +namespace mapping { namespace pose_graph { namespace { @@ -31,16 +31,16 @@ using ::testing::DoubleEq; using ::testing::ElementsAre; using LandmarkObservation = - mapping::PoseGraphInterface::LandmarkNode::LandmarkObservation; + PoseGraphInterface::LandmarkNode::LandmarkObservation; -TEST(LandmarkCostFunctionTest, SmokeTest) { - NodeData prev_node; +TEST(LandmarkCostFunction3DTest, SmokeTest) { + OptimizationProblem3D::NodeData prev_node; prev_node.time = common::FromUniversal(0); - NodeData next_node; + OptimizationProblem3D::NodeData next_node; next_node.time = common::FromUniversal(10); std::unique_ptr cost_function( - LandmarkCostFunction::CreateAutoDiffCostFunction( + LandmarkCostFunction3D::CreateAutoDiffCostFunction( LandmarkObservation{ 0 /* trajectory ID */, common::FromUniversal(5) /* time */, @@ -73,5 +73,5 @@ TEST(LandmarkCostFunctionTest, SmokeTest) { } // namespace } // namespace pose_graph -} // namespace mapping_3d +} // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping_3d/pose_graph/optimization_problem.cc b/cartographer/mapping_3d/pose_graph/optimization_problem_3d.cc similarity index 88% rename from cartographer/mapping_3d/pose_graph/optimization_problem.cc rename to cartographer/mapping_3d/pose_graph/optimization_problem_3d.cc index d8669f2..835d52b 100644 --- a/cartographer/mapping_3d/pose_graph/optimization_problem.cc +++ b/cartographer/mapping_3d/pose_graph/optimization_problem_3d.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/mapping_3d/pose_graph/optimization_problem.h" +#include "cartographer/mapping_3d/pose_graph/optimization_problem_3d.h" #include #include @@ -30,12 +30,12 @@ #include "cartographer/common/make_unique.h" #include "cartographer/common/math.h" #include "cartographer/common/time.h" -#include "cartographer/internal/mapping_3d/acceleration_cost_function.h" -#include "cartographer/internal/mapping_3d/rotation_cost_function.h" +#include "cartographer/internal/mapping_3d/acceleration_cost_function_3d.h" +#include "cartographer/internal/mapping_3d/rotation_cost_function_3d.h" #include "cartographer/mapping/pose_graph/ceres_pose.h" #include "cartographer/mapping_3d/imu_integration.h" -#include "cartographer/mapping_3d/pose_graph/landmark_cost_function.h" -#include "cartographer/mapping_3d/pose_graph/spa_cost_function.h" +#include "cartographer/mapping_3d/pose_graph/landmark_cost_function_3d.h" +#include "cartographer/mapping_3d/pose_graph/spa_cost_function_3d.h" #include "cartographer/mapping_3d/rotation_parameterization.h" #include "cartographer/transform/timestamped_transform.h" #include "cartographer/transform/transform.h" @@ -45,14 +45,15 @@ #include "glog/logging.h" namespace cartographer { -namespace mapping_3d { +namespace mapping { namespace pose_graph { namespace { -using ::cartographer::mapping::pose_graph::CeresPose; using LandmarkNode = ::cartographer::mapping::PoseGraphInterface::LandmarkNode; using TrajectoryData = ::cartographer::mapping::PoseGraphInterface::TrajectoryData; +using NodeData = OptimizationProblem3D::NodeData; +using SubmapData = OptimizationProblem3D::SubmapData; // For odometry. std::unique_ptr Interpolate( @@ -116,8 +117,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) { @@ -153,7 +154,7 @@ void AddLandmarkCostFunctions( problem)); } problem->AddResidualBlock( - LandmarkCostFunction::CreateAutoDiffCostFunction( + LandmarkCostFunction3D::CreateAutoDiffCostFunction( observation, prev->data, next->data), nullptr /* loss function */, C_nodes->at(prev->id).rotation(), C_nodes->at(prev->id).translation(), C_nodes->at(next->id).rotation(), @@ -166,30 +167,29 @@ void AddLandmarkCostFunctions( } // namespace -OptimizationProblem::OptimizationProblem( - const mapping::pose_graph::proto::OptimizationProblemOptions& options, - FixZ fix_z) +OptimizationProblem3D::OptimizationProblem3D( + const pose_graph::proto::OptimizationProblemOptions& options, FixZ fix_z) : options_(options), fix_z_(fix_z) {} -OptimizationProblem::~OptimizationProblem() {} +OptimizationProblem3D::~OptimizationProblem3D() {} -void OptimizationProblem::AddImuData(const int trajectory_id, - const sensor::ImuData& imu_data) { +void OptimizationProblem3D::AddImuData(const int trajectory_id, + const sensor::ImuData& imu_data) { imu_data_.Append(trajectory_id, imu_data); } -void OptimizationProblem::AddOdometryData( +void OptimizationProblem3D::AddOdometryData( const int trajectory_id, const sensor::OdometryData& odometry_data) { odometry_data_.Append(trajectory_id, odometry_data); } -void OptimizationProblem::AddFixedFramePoseData( +void OptimizationProblem3D::AddFixedFramePoseData( const int trajectory_id, const sensor::FixedFramePoseData& fixed_frame_pose_data) { fixed_frame_pose_data_.Append(trajectory_id, fixed_frame_pose_data); } -void OptimizationProblem::AddTrajectoryNode( +void OptimizationProblem3D::AddTrajectoryNode( const int trajectory_id, const common::Time time, const transform::Rigid3d& local_pose, const transform::Rigid3d& global_pose) { @@ -197,20 +197,20 @@ void OptimizationProblem::AddTrajectoryNode( trajectory_data_[trajectory_id]; } -void OptimizationProblem::SetTrajectoryData( +void OptimizationProblem3D::SetTrajectoryData( int trajectory_id, const TrajectoryData& trajectory_data) { trajectory_data_[trajectory_id] = trajectory_data; } -void OptimizationProblem::InsertTrajectoryNode( - const mapping::NodeId& node_id, const common::Time time, +void OptimizationProblem3D::InsertTrajectoryNode( + const NodeId& node_id, const common::Time time, const transform::Rigid3d& local_pose, const transform::Rigid3d& global_pose) { node_data_.Insert(node_id, NodeData{time, local_pose, global_pose}); trajectory_data_[node_id.trajectory_id]; } -void OptimizationProblem::TrimTrajectoryNode(const mapping::NodeId& node_id) { +void OptimizationProblem3D::TrimTrajectoryNode(const NodeId& node_id) { imu_data_.Trim(node_data_, node_id); odometry_data_.Trim(node_data_, node_id); fixed_frame_pose_data_.Trim(node_data_, node_id); @@ -220,27 +220,27 @@ void OptimizationProblem::TrimTrajectoryNode(const mapping::NodeId& node_id) { } } -void OptimizationProblem::AddSubmap( +void OptimizationProblem3D::AddSubmap( const int trajectory_id, const transform::Rigid3d& global_submap_pose) { submap_data_.Append(trajectory_id, SubmapData{global_submap_pose}); } -void OptimizationProblem::InsertSubmap( - const mapping::SubmapId& submap_id, - const transform::Rigid3d& global_submap_pose) { +void OptimizationProblem3D::InsertSubmap( + const SubmapId& submap_id, const transform::Rigid3d& global_submap_pose) { submap_data_.Insert(submap_id, SubmapData{global_submap_pose}); } -void OptimizationProblem::TrimSubmap(const mapping::SubmapId& submap_id) { +void OptimizationProblem3D::TrimSubmap(const SubmapId& submap_id) { submap_data_.Trim(submap_id); } -void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) { +void OptimizationProblem3D::SetMaxNumIterations( + const int32 max_num_iterations) { options_.mutable_ceres_solver_options()->set_max_num_iterations( max_num_iterations); } -void OptimizationProblem::Solve( +void OptimizationProblem3D::Solve( const std::vector& constraints, const std::set& frozen_trajectories, const std::map& landmark_nodes) { @@ -262,9 +262,9 @@ void OptimizationProblem::Solve( // Set the starting point. CHECK(!submap_data_.empty()); - CHECK(submap_data_.Contains(mapping::SubmapId{0, 0})); - mapping::MapById C_submaps; - mapping::MapById C_nodes; + CHECK(submap_data_.Contains(SubmapId{0, 0})); + MapById C_submaps; + MapById C_nodes; std::map C_landmarks; bool first_submap = true; for (const auto& submap_id_data : submap_data_) { @@ -279,7 +279,7 @@ void OptimizationProblem::Solve( CeresPose(submap_id_data.data.global_pose, translation_parameterization(), common::make_unique>(), + ConstantYawQuaternionPlus, 4, 2>>(), &problem)); problem.SetParameterBlockConstant( C_submaps.at(submap_id_data.id).translation()); @@ -315,7 +315,7 @@ void OptimizationProblem::Solve( // Add cost functions for intra- and inter-submap constraints. for (const Constraint& constraint : constraints) { problem.AddResidualBlock( - SpaCostFunction::CreateAutoDiffCostFunction(constraint.pose), + SpaCostFunction3D::CreateAutoDiffCostFunction(constraint.pose), // Only loop closure constraints should have a loss function. constraint.tag == Constraint::INTER_SUBMAP ? new ceres::HuberLoss(options_.huber_scale()) @@ -350,10 +350,10 @@ void OptimizationProblem::Solve( auto imu_it = imu_data.begin(); 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) { @@ -372,7 +372,7 @@ void OptimizationProblem::Solve( const auto next_node_it = std::next(node_it); if (next_node_it != trajectory_end && next_node_it->id.node_index == second_node_id.node_index + 1) { - const mapping::NodeId third_node_id = next_node_it->id; + const NodeId third_node_id = next_node_it->id; const NodeData& third_node_data = next_node_it->data; const common::Time first_time = first_node_data.time; const common::Time second_time = second_node_data.time; @@ -395,7 +395,7 @@ void OptimizationProblem::Solve( result_to_first_center.delta_rotation) * result_center_to_center.delta_velocity; problem.AddResidualBlock( - AccelerationCostFunction::CreateAutoDiffCostFunction( + AccelerationCostFunction3D::CreateAutoDiffCostFunction( options_.acceleration_weight(), delta_velocity, common::ToSeconds(first_duration), common::ToSeconds(second_duration)), @@ -430,10 +430,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) { @@ -443,7 +443,7 @@ void OptimizationProblem::Solve( const transform::Rigid3d relative_pose = ComputeRelativePose( trajectory_id, first_node_data, second_node_data); problem.AddResidualBlock( - SpaCostFunction::CreateAutoDiffCostFunction(Constraint::Pose{ + SpaCostFunction3D::CreateAutoDiffCostFunction(Constraint::Pose{ relative_pose, options_.consecutive_node_translation_weight(), options_.consecutive_node_rotation_weight()}), nullptr /* loss function */, C_nodes.at(first_node_id).rotation(), @@ -467,7 +467,7 @@ void OptimizationProblem::Solve( const TrajectoryData& trajectory_data = trajectory_data_.at(trajectory_id); bool fixed_frame_pose_initialized = false; for (; node_it != trajectory_end; ++node_it) { - const mapping::NodeId node_id = node_it->id; + const NodeId node_id = node_it->id; const NodeData& node_data = node_it->data; const std::unique_ptr fixed_frame_pose = @@ -499,13 +499,13 @@ void OptimizationProblem::Solve( Eigen::Vector3d::UnitZ())), nullptr, common::make_unique>(), + YawOnlyQuaternionPlus, 4, 1>>(), &problem)); fixed_frame_pose_initialized = true; } problem.AddResidualBlock( - SpaCostFunction::CreateAutoDiffCostFunction(constraint_pose), + SpaCostFunction3D::CreateAutoDiffCostFunction(constraint_pose), nullptr /* loss function */, C_fixed_frames.at(trajectory_id).rotation(), C_fixed_frames.at(trajectory_id).translation(), @@ -553,42 +553,41 @@ void OptimizationProblem::Solve( } } -const mapping::MapById& -OptimizationProblem::node_data() const { +const MapById& OptimizationProblem3D::node_data() const { return node_data_; } -const mapping::MapById& -OptimizationProblem::submap_data() const { +const MapById& OptimizationProblem3D::submap_data() + const { return submap_data_; } const std::map& -OptimizationProblem::landmark_data() const { +OptimizationProblem3D::landmark_data() const { return landmark_data_; } -const sensor::MapByTime& OptimizationProblem::imu_data() +const sensor::MapByTime& OptimizationProblem3D::imu_data() const { return imu_data_; } const sensor::MapByTime& -OptimizationProblem::odometry_data() const { +OptimizationProblem3D::odometry_data() const { return odometry_data_; } const sensor::MapByTime& -OptimizationProblem::fixed_frame_pose_data() const { +OptimizationProblem3D::fixed_frame_pose_data() const { return fixed_frame_pose_data_; } -const std::map& OptimizationProblem::trajectory_data() +const std::map& OptimizationProblem3D::trajectory_data() const { return trajectory_data_; } -transform::Rigid3d OptimizationProblem::ComputeRelativePose( +transform::Rigid3d OptimizationProblem3D::ComputeRelativePose( const int trajectory_id, const NodeData& first_node_data, const NodeData& second_node_data) const { if (odometry_data_.HasTrajectory(trajectory_id)) { @@ -604,5 +603,5 @@ transform::Rigid3d OptimizationProblem::ComputeRelativePose( } } // namespace pose_graph -} // namespace mapping_3d +} // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping_3d/pose_graph/optimization_problem.h b/cartographer/mapping_3d/pose_graph/optimization_problem_3d.h similarity index 70% rename from cartographer/mapping_3d/pose_graph/optimization_problem.h rename to cartographer/mapping_3d/pose_graph/optimization_problem_3d.h index 582bc7f..8eb9eb5 100644 --- a/cartographer/mapping_3d/pose_graph/optimization_problem.h +++ b/cartographer/mapping_3d/pose_graph/optimization_problem_3d.h @@ -14,8 +14,8 @@ * limitations under the License. */ -#ifndef CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_OPTIMIZATION_PROBLEM_H_ -#define CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_OPTIMIZATION_PROBLEM_H_ +#ifndef CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_OPTIMIZATION_PROBLEM_3D_H_ +#define CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_OPTIMIZATION_PROBLEM_3D_H_ #include #include @@ -37,34 +37,33 @@ #include "cartographer/transform/transform_interpolation_buffer.h" namespace cartographer { -namespace mapping_3d { +namespace mapping { namespace pose_graph { -struct NodeData { - common::Time time; - transform::Rigid3d local_pose; - transform::Rigid3d global_pose; -}; - -struct SubmapData { - transform::Rigid3d global_pose; -}; - // Implements the SPA loop closure method. -class OptimizationProblem { +class OptimizationProblem3D { public: - using Constraint = mapping::PoseGraphInterface::Constraint; - using LandmarkNode = mapping::PoseGraphInterface::LandmarkNode; + using Constraint = PoseGraphInterface::Constraint; + using LandmarkNode = PoseGraphInterface::LandmarkNode; + + struct NodeData { + common::Time time; + transform::Rigid3d local_pose; + transform::Rigid3d global_pose; + }; + + struct SubmapData { + transform::Rigid3d global_pose; + }; enum class FixZ { kYes, kNo }; - OptimizationProblem( - const mapping::pose_graph::proto::OptimizationProblemOptions& options, - FixZ fix_z); - ~OptimizationProblem(); + OptimizationProblem3D( + const pose_graph::proto::OptimizationProblemOptions& options, FixZ fix_z); + ~OptimizationProblem3D(); - OptimizationProblem(const OptimizationProblem&) = delete; - OptimizationProblem& operator=(const OptimizationProblem&) = delete; + OptimizationProblem3D(const OptimizationProblem3D&) = delete; + OptimizationProblem3D& operator=(const OptimizationProblem3D&) = delete; void AddImuData(int trajectory_id, const sensor::ImuData& imu_data); void AddOdometryData(int trajectory_id, @@ -77,16 +76,16 @@ class OptimizationProblem { const transform::Rigid3d& global_pose); void SetTrajectoryData( int trajectory_id, - const mapping::PoseGraphInterface::TrajectoryData& trajectory_data); - void InsertTrajectoryNode(const mapping::NodeId& node_id, common::Time time, + const PoseGraphInterface::TrajectoryData& trajectory_data); + void InsertTrajectoryNode(const NodeId& node_id, common::Time time, const transform::Rigid3d& local_pose, const transform::Rigid3d& global_pose); - void TrimTrajectoryNode(const mapping::NodeId& node_id); + void TrimTrajectoryNode(const NodeId& node_id); void AddSubmap(int trajectory_id, const transform::Rigid3d& global_submap_pose); - void InsertSubmap(const mapping::SubmapId& submap_id, + void InsertSubmap(const SubmapId& submap_id, const transform::Rigid3d& global_submap_pose); - void TrimSubmap(const mapping::SubmapId& submap_id); + void TrimSubmap(const SubmapId& submap_id); void SetMaxNumIterations(int32 max_num_iterations); @@ -95,15 +94,15 @@ 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; const sensor::MapByTime& fixed_frame_pose_data() const; - const std::map& - trajectory_data() const; + const std::map& trajectory_data() + const; private: // Uses odometry if available, otherwise the local SLAM results. @@ -111,19 +110,19 @@ class OptimizationProblem { int trajectory_id, const NodeData& first_node_data, const NodeData& second_node_data) const; - mapping::pose_graph::proto::OptimizationProblemOptions options_; + pose_graph::proto::OptimizationProblemOptions options_; FixZ fix_z_; - mapping::MapById node_data_; - mapping::MapById submap_data_; + MapById node_data_; + MapById submap_data_; std::map landmark_data_; sensor::MapByTime imu_data_; sensor::MapByTime odometry_data_; sensor::MapByTime fixed_frame_pose_data_; - std::map trajectory_data_; + std::map trajectory_data_; }; } // namespace pose_graph -} // namespace mapping_3d +} // namespace mapping } // namespace cartographer -#endif // CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_OPTIMIZATION_PROBLEM_H_ +#endif // CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_OPTIMIZATION_PROBLEM_3D_H_ diff --git a/cartographer/mapping_3d/pose_graph/optimization_problem_test.cc b/cartographer/mapping_3d/pose_graph/optimization_problem_3d_test.cc similarity index 76% rename from cartographer/mapping_3d/pose_graph/optimization_problem_test.cc rename to cartographer/mapping_3d/pose_graph/optimization_problem_3d_test.cc index a0695d5..13a55e1 100644 --- a/cartographer/mapping_3d/pose_graph/optimization_problem_test.cc +++ b/cartographer/mapping_3d/pose_graph/optimization_problem_3d_test.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/mapping_3d/pose_graph/optimization_problem.h" +#include "cartographer/mapping_3d/pose_graph/optimization_problem_3d.h" #include @@ -27,17 +27,18 @@ #include "gmock/gmock.h" namespace cartographer { -namespace mapping_3d { +namespace mapping { namespace pose_graph { namespace { -class OptimizationProblemTest : public ::testing::Test { +class OptimizationProblem3DTest : public ::testing::Test { protected: - OptimizationProblemTest() - : optimization_problem_(CreateOptions(), OptimizationProblem::FixZ::kNo), + OptimizationProblem3DTest() + : optimization_problem_(CreateOptions(), + OptimizationProblem3D::FixZ::kNo), rng_(45387) {} - mapping::pose_graph::proto::OptimizationProblemOptions CreateOptions() { + pose_graph::proto::OptimizationProblemOptions CreateOptions() { auto parameter_dictionary = common::MakeDictionary(R"text( return { acceleration_weight = 1e-4, @@ -54,7 +55,7 @@ class OptimizationProblemTest : public ::testing::Test { num_threads = 4, }, })text"); - return mapping::pose_graph::CreateOptimizationProblemOptions( + return pose_graph::CreateOptimizationProblemOptions( parameter_dictionary.get()); } @@ -90,7 +91,7 @@ class OptimizationProblemTest : public ::testing::Test { Eigen::Vector3d(0., 0., rz))); } - OptimizationProblem optimization_problem_; + OptimizationProblem3D optimization_problem_; std::mt19937 rng_; }; @@ -102,7 +103,7 @@ transform::Rigid3d AddNoise(const transform::Rigid3d& transform, noisy_rotation); } -TEST_F(OptimizationProblemTest, ReducesNoise) { +TEST_F(OptimizationProblem3DTest, ReducesNoise) { constexpr int kNumNodes = 100; const transform::Rigid3d kSubmap0Transform = transform::Rigid3d::Identity(); const transform::Rigid3d kSubmap2Transform = transform::Rigid3d::Rotation( @@ -130,24 +131,24 @@ TEST_F(OptimizationProblemTest, ReducesNoise) { now += common::FromSeconds(0.01); } - std::vector constraints; + std::vector constraints; for (int j = 0; j != kNumNodes; ++j) { - constraints.push_back(OptimizationProblem::Constraint{ - mapping::SubmapId{kTrajectoryId, 0}, mapping::NodeId{kTrajectoryId, j}, - OptimizationProblem::Constraint::Pose{ + constraints.push_back(OptimizationProblem3D::Constraint{ + SubmapId{kTrajectoryId, 0}, NodeId{kTrajectoryId, j}, + OptimizationProblem3D::Constraint::Pose{ AddNoise(test_data[j].ground_truth_pose, test_data[j].noise), 1., 1.}}); // We add an additional independent, but equally noisy observation. - constraints.push_back(OptimizationProblem::Constraint{ - mapping::SubmapId{kTrajectoryId, 1}, mapping::NodeId{kTrajectoryId, j}, - OptimizationProblem::Constraint::Pose{ + constraints.push_back(OptimizationProblem3D::Constraint{ + SubmapId{kTrajectoryId, 1}, NodeId{kTrajectoryId, j}, + OptimizationProblem3D::Constraint::Pose{ AddNoise(test_data[j].ground_truth_pose, RandomYawOnlyTransform(0.2, 0.3)), 1., 1.}}); // We add very noisy data with a low weight to verify it is mostly ignored. - constraints.push_back(OptimizationProblem::Constraint{ - mapping::SubmapId{kTrajectoryId, 2}, mapping::NodeId{kTrajectoryId, j}, - OptimizationProblem::Constraint::Pose{ + constraints.push_back(OptimizationProblem3D::Constraint{ + SubmapId{kTrajectoryId, 2}, NodeId{kTrajectoryId, j}, + OptimizationProblem3D::Constraint::Pose{ kSubmap2Transform.inverse() * test_data[j].ground_truth_pose * RandomTransform(1e3, 3.), 1e-9, 1e-9}}); @@ -157,13 +158,13 @@ TEST_F(OptimizationProblemTest, ReducesNoise) { double rotation_error_before = 0.; const auto& node_data = optimization_problem_.node_data(); for (int j = 0; j != kNumNodes; ++j) { - translation_error_before += (test_data[j].ground_truth_pose.translation() - - node_data.at(mapping::NodeId{kTrajectoryId, j}) - .global_pose.translation()) - .norm(); - rotation_error_before += transform::GetAngle( - test_data[j].ground_truth_pose.inverse() * - node_data.at(mapping::NodeId{kTrajectoryId, j}).global_pose); + translation_error_before += + (test_data[j].ground_truth_pose.translation() - + node_data.at(NodeId{kTrajectoryId, j}).global_pose.translation()) + .norm(); + rotation_error_before += + transform::GetAngle(test_data[j].ground_truth_pose.inverse() * + node_data.at(NodeId{kTrajectoryId, j}).global_pose); } optimization_problem_.AddSubmap(kTrajectoryId, kSubmap0Transform); @@ -175,13 +176,13 @@ TEST_F(OptimizationProblemTest, ReducesNoise) { double translation_error_after = 0.; double rotation_error_after = 0.; for (int j = 0; j != kNumNodes; ++j) { - translation_error_after += (test_data[j].ground_truth_pose.translation() - - node_data.at(mapping::NodeId{kTrajectoryId, j}) - .global_pose.translation()) - .norm(); - rotation_error_after += transform::GetAngle( - test_data[j].ground_truth_pose.inverse() * - node_data.at(mapping::NodeId{kTrajectoryId, j}).global_pose); + translation_error_after += + (test_data[j].ground_truth_pose.translation() - + node_data.at(NodeId{kTrajectoryId, j}).global_pose.translation()) + .norm(); + rotation_error_after += + transform::GetAngle(test_data[j].ground_truth_pose.inverse() * + node_data.at(NodeId{kTrajectoryId, j}).global_pose); } EXPECT_GT(0.8 * translation_error_before, translation_error_after); @@ -190,5 +191,5 @@ TEST_F(OptimizationProblemTest, ReducesNoise) { } // namespace } // namespace pose_graph -} // namespace mapping_3d +} // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping_3d/pose_graph/spa_cost_function.h b/cartographer/mapping_3d/pose_graph/spa_cost_function_3d.h similarity index 74% rename from cartographer/mapping_3d/pose_graph/spa_cost_function.h rename to cartographer/mapping_3d/pose_graph/spa_cost_function_3d.h index da3ca5d..72d8b14 100644 --- a/cartographer/mapping_3d/pose_graph/spa_cost_function.h +++ b/cartographer/mapping_3d/pose_graph/spa_cost_function_3d.h @@ -14,8 +14,8 @@ * limitations under the License. */ -#ifndef CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_SPA_COST_FUNCTION_H_ -#define CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_SPA_COST_FUNCTION_H_ +#ifndef CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_SPA_COST_FUNCTION_3D_H_ +#define CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_SPA_COST_FUNCTION_3D_H_ #include @@ -30,28 +30,23 @@ #include "ceres/jet.h" namespace cartographer { -namespace mapping_3d { +namespace mapping { namespace pose_graph { -class SpaCostFunction { +class SpaCostFunction3D { public: - using Constraint = mapping::PoseGraph::Constraint; - static ceres::CostFunction* CreateAutoDiffCostFunction( - const Constraint::Pose& pose) { + const PoseGraph::Constraint::Pose& pose) { return new ceres::AutoDiffCostFunction< - SpaCostFunction, 6 /* residuals */, 4 /* rotation variables */, + SpaCostFunction3D, 6 /* residuals */, 4 /* rotation variables */, 3 /* translation variables */, 4 /* rotation variables */, - 3 /* translation variables */>(new SpaCostFunction(pose)); + 3 /* translation variables */>(new SpaCostFunction3D(pose)); } template bool operator()(const T* const c_i_rotation, const T* const c_i_translation, const T* const c_j_rotation, const T* const c_j_translation, T* const e) const { - using mapping::pose_graph::ComputeUnscaledError; - using mapping::pose_graph::ScaleError; - const std::array error = ScaleError( ComputeUnscaledError(pose_.zbar_ij, c_i_rotation, c_i_translation, c_j_rotation, c_j_translation), @@ -61,13 +56,14 @@ class SpaCostFunction { } private: - explicit SpaCostFunction(const Constraint::Pose& pose) : pose_(pose) {} + explicit SpaCostFunction3D(const PoseGraph::Constraint::Pose& pose) + : pose_(pose) {} - const Constraint::Pose pose_; + const PoseGraph::Constraint::Pose pose_; }; } // namespace pose_graph -} // namespace mapping_3d +} // namespace mapping } // namespace cartographer -#endif // CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_SPA_COST_FUNCTION_H_ +#endif // CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_SPA_COST_FUNCTION_3D_H_ diff --git a/cartographer/mapping_3d/pose_graph_3d.cc b/cartographer/mapping_3d/pose_graph_3d.cc index 53e1a6c..1df7216 100644 --- a/cartographer/mapping_3d/pose_graph_3d.cc +++ b/cartographer/mapping_3d/pose_graph_3d.cc @@ -42,9 +42,8 @@ namespace mapping { PoseGraph3D::PoseGraph3D(const proto::PoseGraphOptions& options, common::ThreadPool* thread_pool) : options_(options), - optimization_problem_( - options_.optimization_problem_options(), - mapping_3d::pose_graph::OptimizationProblem::FixZ::kNo), + optimization_problem_(options_.optimization_problem_options(), + pose_graph::OptimizationProblem3D::FixZ::kNo), constraint_builder_(options_.constraint_builder_options(), thread_pool) {} PoseGraph3D::~PoseGraph3D() { @@ -340,7 +339,7 @@ void PoseGraph3D::UpdateTrajectoryConnectivity(const Constraint& constraint) { void PoseGraph3D::HandleWorkQueue() { constraint_builder_.WhenDone( - [this](const mapping_3d::pose_graph::ConstraintBuilder::Result& result) { + [this](const pose_graph::ConstraintBuilder3D::Result& result) { { common::MutexLocker locker(&mutex_); constraints_.insert(constraints_.end(), result.begin(), result.end()); @@ -401,8 +400,8 @@ void PoseGraph3D::WaitForAllComputations() { } std::cout << "\r\x1b[KOptimizing: Done. " << std::endl; constraint_builder_.WhenDone( - [this, ¬ification]( - const mapping_3d::pose_graph::ConstraintBuilder::Result& result) { + [this, + ¬ification](const pose_graph::ConstraintBuilder3D::Result& result) { common::MutexLocker locker(&mutex_); constraints_.insert(constraints_.end(), result.begin(), result.end()); notification = true; @@ -453,7 +452,8 @@ void PoseGraph3D::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_3d::pose_graph::SubmapData{global_submap_pose}); + submap_id, + pose_graph::OptimizationProblem3D::SubmapData{global_submap_pose}); AddWorkItem([this, submap_id, global_submap_pose]() REQUIRES(mutex_) { CHECK_EQ(frozen_trajectories_.count(submap_id.trajectory_id), 1); submap_data_.at(submap_id).state = SubmapState::kFinished; @@ -759,7 +759,7 @@ PoseGraph3D::GetAllSubmapPoses() { } transform::Rigid3d PoseGraph3D::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_3d/pose_graph_3d.h b/cartographer/mapping_3d/pose_graph_3d.h index 046fbe8..5bef40f 100644 --- a/cartographer/mapping_3d/pose_graph_3d.h +++ b/cartographer/mapping_3d/pose_graph_3d.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_3d/pose_graph/constraint_builder.h" -#include "cartographer/mapping_3d/pose_graph/optimization_problem.h" +#include "cartographer/mapping_3d/pose_graph/constraint_builder_3d.h" +#include "cartographer/mapping_3d/pose_graph/optimization_problem_3d.h" #include "cartographer/mapping_3d/submap_3d.h" #include "cartographer/sensor/fixed_frame_pose_data.h" #include "cartographer/sensor/landmark_data.h" @@ -190,7 +190,7 @@ class PoseGraph3D : 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_); @@ -234,9 +234,8 @@ class PoseGraph3D : public PoseGraph { void DispatchOptimization() REQUIRES(mutex_); // Current optimization problem. - mapping_3d::pose_graph::OptimizationProblem optimization_problem_; - mapping_3d::pose_graph::ConstraintBuilder constraint_builder_ - GUARDED_BY(mutex_); + pose_graph::OptimizationProblem3D optimization_problem_; + pose_graph::ConstraintBuilder3D 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 @@ -248,8 +247,8 @@ class PoseGraph3D : 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_3d/proto/local_trajectory_builder_options_3d.proto b/cartographer/mapping_3d/proto/local_trajectory_builder_options_3d.proto index bb923a2..c173266 100644 --- a/cartographer/mapping_3d/proto/local_trajectory_builder_options_3d.proto +++ b/cartographer/mapping_3d/proto/local_trajectory_builder_options_3d.proto @@ -20,7 +20,7 @@ import "cartographer/mapping/proto/motion_filter_options.proto"; import "cartographer/sensor/proto/adaptive_voxel_filter_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"; +import "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options_3d.proto"; // NEXT ID: 18 message LocalTrajectoryBuilderOptions3D { @@ -47,7 +47,7 @@ message LocalTrajectoryBuilderOptions3D { bool use_online_correlative_scan_matching = 13; mapping.scan_matching.proto.RealTimeCorrelativeScanMatcherOptions real_time_correlative_scan_matcher_options = 14; - mapping_3d.scan_matching.proto.CeresScanMatcherOptions + mapping.scan_matching.proto.CeresScanMatcherOptions3D ceres_scan_matcher_options = 6; mapping.proto.MotionFilterOptions motion_filter_options = 7; diff --git a/cartographer/mapping_3d/scan_matching/ceres_scan_matcher.cc b/cartographer/mapping_3d/scan_matching/ceres_scan_matcher_3d.cc similarity index 77% rename from cartographer/mapping_3d/scan_matching/ceres_scan_matcher.cc rename to cartographer/mapping_3d/scan_matching/ceres_scan_matcher_3d.cc index 945fb08..c2904bc 100644 --- a/cartographer/mapping_3d/scan_matching/ceres_scan_matcher.cc +++ b/cartographer/mapping_3d/scan_matching/ceres_scan_matcher_3d.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h" +#include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher_3d.h" #include #include @@ -22,23 +22,23 @@ #include "cartographer/common/ceres_solver_options.h" #include "cartographer/common/make_unique.h" -#include "cartographer/internal/mapping_3d/scan_matching/occupied_space_cost_function.h" +#include "cartographer/internal/mapping_3d/scan_matching/occupied_space_cost_function_3d.h" #include "cartographer/mapping/pose_graph/ceres_pose.h" #include "cartographer/mapping_3d/rotation_parameterization.h" -#include "cartographer/mapping_3d/scan_matching/rotation_delta_cost_functor.h" -#include "cartographer/mapping_3d/scan_matching/translation_delta_cost_functor.h" +#include "cartographer/mapping_3d/scan_matching/rotation_delta_cost_functor_3d.h" +#include "cartographer/mapping_3d/scan_matching/translation_delta_cost_functor_3d.h" #include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/transform.h" #include "ceres/ceres.h" #include "glog/logging.h" namespace cartographer { -namespace mapping_3d { +namespace mapping { namespace scan_matching { -proto::CeresScanMatcherOptions CreateCeresScanMatcherOptions( +proto::CeresScanMatcherOptions3D CreateCeresScanMatcherOptions3D( common::LuaParameterDictionary* const parameter_dictionary) { - proto::CeresScanMatcherOptions options; + proto::CeresScanMatcherOptions3D options; for (int i = 0;; ++i) { const std::string lua_identifier = "occupied_space_weight_" + std::to_string(i); @@ -60,27 +60,28 @@ proto::CeresScanMatcherOptions CreateCeresScanMatcherOptions( return options; } -CeresScanMatcher::CeresScanMatcher( - const proto::CeresScanMatcherOptions& options) +CeresScanMatcher3D::CeresScanMatcher3D( + const proto::CeresScanMatcherOptions3D& options) : options_(options), ceres_solver_options_( common::CreateCeresSolverOptions(options.ceres_solver_options())) { ceres_solver_options_.linear_solver_type = ceres::DENSE_QR; } -void CeresScanMatcher::Match(const Eigen::Vector3d& target_translation, - const transform::Rigid3d& initial_pose_estimate, - const std::vector& - point_clouds_and_hybrid_grids, - transform::Rigid3d* const pose_estimate, - ceres::Solver::Summary* const summary) { +void CeresScanMatcher3D::Match( + const Eigen::Vector3d& target_translation, + const transform::Rigid3d& initial_pose_estimate, + const std::vector& + point_clouds_and_hybrid_grids, + transform::Rigid3d* const pose_estimate, + ceres::Solver::Summary* const summary) { ceres::Problem problem; - mapping::pose_graph::CeresPose ceres_pose( + pose_graph::CeresPose ceres_pose( initial_pose_estimate, nullptr /* translation_parameterization */, options_.only_optimize_yaw() ? std::unique_ptr( common::make_unique>()) + YawOnlyQuaternionPlus, 4, 1>>()) : std::unique_ptr( common::make_unique()), &problem); @@ -91,10 +92,9 @@ void CeresScanMatcher::Match(const Eigen::Vector3d& target_translation, CHECK_GT(options_.occupied_space_weight(i), 0.); const sensor::PointCloud& point_cloud = *point_clouds_and_hybrid_grids[i].first; - const mapping::HybridGrid& hybrid_grid = - *point_clouds_and_hybrid_grids[i].second; + const HybridGrid& hybrid_grid = *point_clouds_and_hybrid_grids[i].second; problem.AddResidualBlock( - OccupiedSpaceCostFunction::CreateAutoDiffCostFunction( + OccupiedSpaceCostFunction3D::CreateAutoDiffCostFunction( options_.occupied_space_weight(i) / std::sqrt(static_cast(point_cloud.size())), point_cloud, hybrid_grid), @@ -103,12 +103,12 @@ void CeresScanMatcher::Match(const Eigen::Vector3d& target_translation, } CHECK_GT(options_.translation_weight(), 0.); problem.AddResidualBlock( - TranslationDeltaCostFunctor::CreateAutoDiffCostFunction( + TranslationDeltaCostFunctor3D::CreateAutoDiffCostFunction( options_.translation_weight(), target_translation), nullptr /* loss function */, ceres_pose.translation()); CHECK_GT(options_.rotation_weight(), 0.); problem.AddResidualBlock( - RotationDeltaCostFunctor::CreateAutoDiffCostFunction( + RotationDeltaCostFunctor3D::CreateAutoDiffCostFunction( options_.rotation_weight(), initial_pose_estimate.rotation()), nullptr /* loss function */, ceres_pose.rotation()); @@ -118,5 +118,5 @@ void CeresScanMatcher::Match(const Eigen::Vector3d& target_translation, } } // namespace scan_matching -} // namespace mapping_3d +} // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h b/cartographer/mapping_3d/scan_matching/ceres_scan_matcher_3d.h similarity index 79% rename from cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h rename to cartographer/mapping_3d/scan_matching/ceres_scan_matcher_3d.h index 45969e1..58708f7 100644 --- a/cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h +++ b/cartographer/mapping_3d/scan_matching/ceres_scan_matcher_3d.h @@ -14,8 +14,8 @@ * limitations under the License. */ -#ifndef CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_CERES_SCAN_MATCHER_H_ -#define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_CERES_SCAN_MATCHER_H_ +#ifndef CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_CERES_SCAN_MATCHER_3D_H_ +#define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_CERES_SCAN_MATCHER_3D_H_ #include #include @@ -23,27 +23,27 @@ #include "Eigen/Core" #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/mapping_3d/hybrid_grid.h" -#include "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.pb.h" +#include "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options_3d.pb.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/transform/rigid_transform.h" namespace cartographer { -namespace mapping_3d { +namespace mapping { namespace scan_matching { -proto::CeresScanMatcherOptions CreateCeresScanMatcherOptions( +proto::CeresScanMatcherOptions3D CreateCeresScanMatcherOptions3D( common::LuaParameterDictionary* parameter_dictionary); using PointCloudAndHybridGridPointers = - std::pair; + std::pair; // This scan matcher uses Ceres to align scans with an existing map. -class CeresScanMatcher { +class CeresScanMatcher3D { public: - explicit CeresScanMatcher(const proto::CeresScanMatcherOptions& options); + explicit CeresScanMatcher3D(const proto::CeresScanMatcherOptions3D& options); - CeresScanMatcher(const CeresScanMatcher&) = delete; - CeresScanMatcher& operator=(const CeresScanMatcher&) = delete; + CeresScanMatcher3D(const CeresScanMatcher3D&) = delete; + CeresScanMatcher3D& operator=(const CeresScanMatcher3D&) = delete; // Aligns 'point_clouds' within the 'hybrid_grids' given an // 'initial_pose_estimate' and returns a 'pose_estimate' and the solver @@ -56,12 +56,12 @@ class CeresScanMatcher { ceres::Solver::Summary* summary); private: - const proto::CeresScanMatcherOptions options_; + const proto::CeresScanMatcherOptions3D options_; ceres::Solver::Options ceres_solver_options_; }; } // namespace scan_matching -} // namespace mapping_3d +} // namespace mapping } // namespace cartographer -#endif // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_CERES_SCAN_MATCHER_H_ +#endif // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_CERES_SCAN_MATCHER_3D_H_ diff --git a/cartographer/mapping_3d/scan_matching/ceres_scan_matcher_test.cc b/cartographer/mapping_3d/scan_matching/ceres_scan_matcher_3d_test.cc similarity index 84% rename from cartographer/mapping_3d/scan_matching/ceres_scan_matcher_test.cc rename to cartographer/mapping_3d/scan_matching/ceres_scan_matcher_3d_test.cc index 8391d7c..7b50c2e 100644 --- a/cartographer/mapping_3d/scan_matching/ceres_scan_matcher_test.cc +++ b/cartographer/mapping_3d/scan_matching/ceres_scan_matcher_3d_test.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h" +#include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher_3d.h" #include @@ -27,13 +27,13 @@ #include "gtest/gtest.h" namespace cartographer { -namespace mapping_3d { +namespace mapping { namespace scan_matching { namespace { -class CeresScanMatcherTest : public ::testing::Test { +class CeresScanMatcher3DTest : public ::testing::Test { protected: - CeresScanMatcherTest() + CeresScanMatcher3DTest() : hybrid_grid_(1.f), expected_pose_( transform::Rigid3d::Translation(Eigen::Vector3d(-1., 0., 0.))) { @@ -59,8 +59,8 @@ class CeresScanMatcherTest : public ::testing::Test { num_threads = 1, }, })text"); - options_ = CreateCeresScanMatcherOptions(parameter_dictionary.get()); - ceres_scan_matcher_.reset(new CeresScanMatcher(options_)); + options_ = CreateCeresScanMatcherOptions3D(parameter_dictionary.get()); + ceres_scan_matcher_.reset(new CeresScanMatcher3D(options_)); } void TestFromInitialPose(const transform::Rigid3d& initial_pose) { @@ -74,35 +74,35 @@ class CeresScanMatcherTest : public ::testing::Test { EXPECT_THAT(pose, transform::IsNearly(expected_pose_, 3e-2)); } - mapping::HybridGrid hybrid_grid_; + HybridGrid hybrid_grid_; transform::Rigid3d expected_pose_; sensor::PointCloud point_cloud_; - proto::CeresScanMatcherOptions options_; - std::unique_ptr ceres_scan_matcher_; + proto::CeresScanMatcherOptions3D options_; + std::unique_ptr ceres_scan_matcher_; }; -TEST_F(CeresScanMatcherTest, PerfectEstimate) { +TEST_F(CeresScanMatcher3DTest, PerfectEstimate) { TestFromInitialPose( transform::Rigid3d::Translation(Eigen::Vector3d(-1., 0., 0.))); } -TEST_F(CeresScanMatcherTest, AlongX) { - ceres_scan_matcher_.reset(new CeresScanMatcher(options_)); +TEST_F(CeresScanMatcher3DTest, AlongX) { + ceres_scan_matcher_.reset(new CeresScanMatcher3D(options_)); TestFromInitialPose( transform::Rigid3d::Translation(Eigen::Vector3d(-0.8, 0., 0.))); } -TEST_F(CeresScanMatcherTest, AlongZ) { +TEST_F(CeresScanMatcher3DTest, AlongZ) { TestFromInitialPose( transform::Rigid3d::Translation(Eigen::Vector3d(-1., 0., -0.2))); } -TEST_F(CeresScanMatcherTest, AlongXYZ) { +TEST_F(CeresScanMatcher3DTest, AlongXYZ) { TestFromInitialPose( transform::Rigid3d::Translation(Eigen::Vector3d(-0.9, -0.2, 0.2))); } -TEST_F(CeresScanMatcherTest, FullPoseCorrection) { +TEST_F(CeresScanMatcher3DTest, FullPoseCorrection) { // We try to find the rotation around z... const auto additional_transform = transform::Rigid3d::Rotation( Eigen::AngleAxisd(0.05, Eigen::Vector3d(0., 0., 1.))); @@ -117,5 +117,5 @@ TEST_F(CeresScanMatcherTest, FullPoseCorrection) { } // namespace } // namespace scan_matching -} // namespace mapping_3d +} // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc b/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_3d.cc similarity index 79% rename from cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc rename to cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_3d.cc index dc00dfe..11c582e 100644 --- a/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc +++ b/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_3d.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h" +#include "cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_3d.h" #include #include @@ -25,19 +25,19 @@ #include "cartographer/common/make_unique.h" #include "cartographer/common/math.h" #include "cartographer/mapping_3d/scan_matching/low_resolution_matcher.h" -#include "cartographer/mapping_3d/scan_matching/precomputation_grid.h" -#include "cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options.pb.h" +#include "cartographer/mapping_3d/scan_matching/precomputation_grid_3d.h" +#include "cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options_3d.pb.h" #include "cartographer/transform/transform.h" #include "glog/logging.h" namespace cartographer { -namespace mapping_3d { +namespace mapping { namespace scan_matching { -proto::FastCorrelativeScanMatcherOptions -CreateFastCorrelativeScanMatcherOptions( +proto::FastCorrelativeScanMatcherOptions3D +CreateFastCorrelativeScanMatcherOptions3D( common::LuaParameterDictionary* const parameter_dictionary) { - proto::FastCorrelativeScanMatcherOptions options; + proto::FastCorrelativeScanMatcherOptions3D options; options.set_branch_and_bound_depth( parameter_dictionary->GetInt("branch_and_bound_depth")); options.set_full_resolution_depth( @@ -58,8 +58,8 @@ CreateFastCorrelativeScanMatcherOptions( class PrecomputationGridStack { public: PrecomputationGridStack( - const mapping::HybridGrid& hybrid_grid, - const proto::FastCorrelativeScanMatcherOptions& options) { + const HybridGrid& hybrid_grid, + const proto::FastCorrelativeScanMatcherOptions3D& options) { CHECK_GE(options.branch_and_bound_depth(), 1); CHECK_GE(options.full_resolution_depth(), 1); precomputation_grids_.reserve(options.branch_and_bound_depth()); @@ -80,29 +80,29 @@ class PrecomputationGridStack { } } - const PrecomputationGrid& Get(int depth) const { + const PrecomputationGrid3D& Get(int depth) const { return precomputation_grids_.at(depth); } int max_depth() const { return precomputation_grids_.size() - 1; } private: - std::vector precomputation_grids_; + std::vector precomputation_grids_; }; -struct DiscreteScan { +struct DiscreteScan3D { transform::Rigid3f pose; // Contains a vector of discretized scans for each 'depth'. std::vector> cell_indices_per_depth; float rotational_score; }; -struct Candidate { - Candidate(const int scan_index, const Eigen::Array3i& offset) +struct Candidate3D { + Candidate3D(const int scan_index, const Eigen::Array3i& offset) : scan_index(scan_index), offset(offset) {} - static Candidate Unsuccessful() { - return Candidate(0, Eigen::Array3i::Zero()); + static Candidate3D Unsuccessful() { + return Candidate3D(0, Eigen::Array3i::Zero()); } // Index into the discrete scans vectors. @@ -119,14 +119,14 @@ struct Candidate { // Score of the low resolution matcher. float low_resolution_score = 0.f; - bool operator<(const Candidate& other) const { return score < other.score; } - bool operator>(const Candidate& other) const { return score > other.score; } + bool operator<(const Candidate3D& other) const { return score < other.score; } + bool operator>(const Candidate3D& other) const { return score > other.score; } }; namespace { std::vector> HistogramsAtAnglesFromNodes( - const std::vector& nodes) { + const std::vector& nodes) { std::vector> histograms_at_angles; for (const auto& node : nodes) { histograms_at_angles.emplace_back( @@ -141,11 +141,11 @@ std::vector> HistogramsAtAnglesFromNodes( } // namespace -FastCorrelativeScanMatcher::FastCorrelativeScanMatcher( - const mapping::HybridGrid& hybrid_grid, - const mapping::HybridGrid* const low_resolution_hybrid_grid, - const std::vector& nodes, - const proto::FastCorrelativeScanMatcherOptions& options) +FastCorrelativeScanMatcher3D::FastCorrelativeScanMatcher3D( + const HybridGrid& hybrid_grid, + const HybridGrid* const low_resolution_hybrid_grid, + const std::vector& nodes, + const proto::FastCorrelativeScanMatcherOptions3D& options) : options_(options), resolution_(hybrid_grid.resolution()), width_in_voxels_(hybrid_grid.grid_size()), @@ -154,14 +154,13 @@ FastCorrelativeScanMatcher::FastCorrelativeScanMatcher( low_resolution_hybrid_grid_(low_resolution_hybrid_grid), rotational_scan_matcher_(HistogramsAtAnglesFromNodes(nodes)) {} -FastCorrelativeScanMatcher::~FastCorrelativeScanMatcher() {} +FastCorrelativeScanMatcher3D::~FastCorrelativeScanMatcher3D() {} -std::unique_ptr -FastCorrelativeScanMatcher::Match( +std::unique_ptr +FastCorrelativeScanMatcher3D::Match( const transform::Rigid3d& global_node_pose, const transform::Rigid3d& global_submap_pose, - const mapping::TrajectoryNode::Data& constant_data, - const float min_score) const { + const TrajectoryNode::Data& constant_data, const float min_score) const { const auto low_resolution_matcher = scan_matching::CreateLowResolutionMatcher( low_resolution_hybrid_grid_, &constant_data.low_resolution_point_cloud); const SearchParameters search_parameters{ @@ -176,12 +175,11 @@ FastCorrelativeScanMatcher::Match( constant_data.gravity_alignment, min_score); } -std::unique_ptr -FastCorrelativeScanMatcher::MatchFullSubmap( +std::unique_ptr +FastCorrelativeScanMatcher3D::MatchFullSubmap( const Eigen::Quaterniond& global_node_rotation, const Eigen::Quaterniond& global_submap_rotation, - const mapping::TrajectoryNode::Data& constant_data, - const float min_score) const { + const TrajectoryNode::Data& constant_data, const float min_score) const { float max_point_distance = 0.f; for (const Eigen::Vector3f& point : constant_data.high_resolution_point_cloud) { @@ -203,22 +201,22 @@ FastCorrelativeScanMatcher::MatchFullSubmap( constant_data.gravity_alignment, min_score); } -std::unique_ptr -FastCorrelativeScanMatcher::MatchWithSearchParameters( - const FastCorrelativeScanMatcher::SearchParameters& search_parameters, +std::unique_ptr +FastCorrelativeScanMatcher3D::MatchWithSearchParameters( + const FastCorrelativeScanMatcher3D::SearchParameters& search_parameters, const transform::Rigid3f& global_node_pose, const transform::Rigid3f& global_submap_pose, const sensor::PointCloud& point_cloud, const Eigen::VectorXf& rotational_scan_matcher_histogram, const Eigen::Quaterniond& gravity_alignment, const float min_score) const { - const std::vector discrete_scans = GenerateDiscreteScans( + const std::vector discrete_scans = GenerateDiscreteScans( search_parameters, point_cloud, rotational_scan_matcher_histogram, gravity_alignment, global_node_pose, global_submap_pose); - const std::vector lowest_resolution_candidates = + const std::vector lowest_resolution_candidates = ComputeLowestResolutionCandidates(search_parameters, discrete_scans); - const Candidate best_candidate = BranchAndBound( + const Candidate3D best_candidate = BranchAndBound( search_parameters, discrete_scans, lowest_resolution_candidates, precomputation_grid_stack_->max_depth(), min_score); if (best_candidate.score > min_score) { @@ -231,12 +229,13 @@ FastCorrelativeScanMatcher::MatchWithSearchParameters( return nullptr; } -DiscreteScan FastCorrelativeScanMatcher::DiscretizeScan( - const FastCorrelativeScanMatcher::SearchParameters& search_parameters, +DiscreteScan3D FastCorrelativeScanMatcher3D::DiscretizeScan( + const FastCorrelativeScanMatcher3D::SearchParameters& search_parameters, const sensor::PointCloud& point_cloud, const transform::Rigid3f& pose, const float rotational_score) const { std::vector> cell_indices_per_depth; - const PrecomputationGrid& original_grid = precomputation_grid_stack_->Get(0); + const PrecomputationGrid3D& original_grid = + precomputation_grid_stack_->Get(0); std::vector full_resolution_cell_indices; for (const Eigen::Vector3f& point : sensor::TransformPointCloud(point_cloud, pose)) { @@ -272,17 +271,17 @@ DiscreteScan FastCorrelativeScanMatcher::DiscretizeScan( low_resolution_cell_at_start - low_resolution_search_window_start); } } - return DiscreteScan{pose, cell_indices_per_depth, rotational_score}; + return DiscreteScan3D{pose, cell_indices_per_depth, rotational_score}; } -std::vector FastCorrelativeScanMatcher::GenerateDiscreteScans( - const FastCorrelativeScanMatcher::SearchParameters& search_parameters, +std::vector FastCorrelativeScanMatcher3D::GenerateDiscreteScans( + const FastCorrelativeScanMatcher3D::SearchParameters& search_parameters, const sensor::PointCloud& point_cloud, const Eigen::VectorXf& rotational_scan_matcher_histogram, const Eigen::Quaterniond& gravity_alignment, const transform::Rigid3f& global_node_pose, const transform::Rigid3f& global_submap_pose) const { - std::vector result; + std::vector result; // We set this value to something on the order of resolution to make sure that // the std::acos() below is defined. float max_scan_range = 3.f * resolution_; @@ -326,9 +325,9 @@ std::vector FastCorrelativeScanMatcher::GenerateDiscreteScans( return result; } -std::vector -FastCorrelativeScanMatcher::GenerateLowestResolutionCandidates( - const FastCorrelativeScanMatcher::SearchParameters& search_parameters, +std::vector +FastCorrelativeScanMatcher3D::GenerateLowestResolutionCandidates( + const FastCorrelativeScanMatcher3D::SearchParameters& search_parameters, const int num_discrete_scans) const { const int linear_step_size = 1 << precomputation_grid_stack_->max_depth(); const int num_lowest_resolution_linear_xy_candidates = @@ -341,7 +340,7 @@ FastCorrelativeScanMatcher::GenerateLowestResolutionCandidates( num_discrete_scans * common::Power(num_lowest_resolution_linear_xy_candidates, 2) * num_lowest_resolution_linear_z_candidates; - std::vector candidates; + std::vector candidates; candidates.reserve(num_candidates); for (int scan_index = 0; scan_index != num_discrete_scans; ++scan_index) { for (int z = -search_parameters.linear_z_window_size; @@ -361,14 +360,14 @@ FastCorrelativeScanMatcher::GenerateLowestResolutionCandidates( return candidates; } -void FastCorrelativeScanMatcher::ScoreCandidates( - const int depth, const std::vector& discrete_scans, - std::vector* const candidates) const { +void FastCorrelativeScanMatcher3D::ScoreCandidates( + const int depth, const std::vector& discrete_scans, + std::vector* const candidates) const { const int reduction_exponent = std::max(0, depth - options_.full_resolution_depth() + 1); - for (Candidate& candidate : *candidates) { + for (Candidate3D& candidate : *candidates) { int sum = 0; - const DiscreteScan& discrete_scan = discrete_scans[candidate.scan_index]; + const DiscreteScan3D& discrete_scan = discrete_scans[candidate.scan_index]; const Eigen::Array3i offset(candidate.offset[0] >> reduction_exponent, candidate.offset[1] >> reduction_exponent, candidate.offset[2] >> reduction_exponent); @@ -378,18 +377,19 @@ void FastCorrelativeScanMatcher::ScoreCandidates( const Eigen::Array3i proposed_cell_index = cell_index + offset; sum += precomputation_grid_stack_->Get(depth).value(proposed_cell_index); } - candidate.score = PrecomputationGrid::ToProbability( + candidate.score = PrecomputationGrid3D::ToProbability( sum / static_cast(discrete_scan.cell_indices_per_depth[depth].size())); } - std::sort(candidates->begin(), candidates->end(), std::greater()); + std::sort(candidates->begin(), candidates->end(), + std::greater()); } -std::vector -FastCorrelativeScanMatcher::ComputeLowestResolutionCandidates( - const FastCorrelativeScanMatcher::SearchParameters& search_parameters, - const std::vector& discrete_scans) const { - std::vector lowest_resolution_candidates = +std::vector +FastCorrelativeScanMatcher3D::ComputeLowestResolutionCandidates( + const FastCorrelativeScanMatcher3D::SearchParameters& search_parameters, + const std::vector& discrete_scans) const { + std::vector lowest_resolution_candidates = GenerateLowestResolutionCandidates(search_parameters, discrete_scans.size()); ScoreCandidates(precomputation_grid_stack_->max_depth(), discrete_scans, @@ -397,48 +397,48 @@ FastCorrelativeScanMatcher::ComputeLowestResolutionCandidates( return lowest_resolution_candidates; } -transform::Rigid3f FastCorrelativeScanMatcher::GetPoseFromCandidate( - const std::vector& discrete_scans, - const Candidate& candidate) const { +transform::Rigid3f FastCorrelativeScanMatcher3D::GetPoseFromCandidate( + const std::vector& discrete_scans, + const Candidate3D& candidate) const { return transform::Rigid3f::Translation( resolution_ * candidate.offset.matrix().cast()) * discrete_scans[candidate.scan_index].pose; } -Candidate FastCorrelativeScanMatcher::BranchAndBound( - const FastCorrelativeScanMatcher::SearchParameters& search_parameters, - const std::vector& discrete_scans, - const std::vector& candidates, const int candidate_depth, +Candidate3D FastCorrelativeScanMatcher3D::BranchAndBound( + const FastCorrelativeScanMatcher3D::SearchParameters& search_parameters, + const std::vector& discrete_scans, + const std::vector& candidates, const int candidate_depth, float min_score) const { if (candidate_depth == 0) { - for (const Candidate& candidate : candidates) { + for (const Candidate3D& candidate : candidates) { if (candidate.score <= min_score) { // Return if the candidate is bad because the following candidate will // not have better score. - return Candidate::Unsuccessful(); + return Candidate3D::Unsuccessful(); } const float low_resolution_score = (*search_parameters.low_resolution_matcher)( GetPoseFromCandidate(discrete_scans, candidate)); if (low_resolution_score >= options_.min_low_resolution_score()) { // We found the best candidate that passes the matching function. - Candidate best_candidate = candidate; + Candidate3D best_candidate = candidate; best_candidate.low_resolution_score = low_resolution_score; return best_candidate; } } // All candidates have good scores but none passes the matching function. - return Candidate::Unsuccessful(); + return Candidate3D::Unsuccessful(); } - Candidate best_high_resolution_candidate = Candidate::Unsuccessful(); + Candidate3D best_high_resolution_candidate = Candidate3D::Unsuccessful(); best_high_resolution_candidate.score = min_score; - for (const Candidate& candidate : candidates) { + for (const Candidate3D& candidate : candidates) { if (candidate.score <= min_score) { break; } - std::vector higher_resolution_candidates; + std::vector higher_resolution_candidates; const int half_width = 1 << (candidate_depth - 1); for (int z : {0, half_width}) { if (candidate.offset.z() + z > search_parameters.linear_z_window_size) { @@ -471,5 +471,5 @@ Candidate FastCorrelativeScanMatcher::BranchAndBound( } } // namespace scan_matching -} // namespace mapping_3d +} // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h b/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_3d.h similarity index 63% rename from cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h rename to cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_3d.h index d3f03d3..ab14ea1 100644 --- a/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h +++ b/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_3d.h @@ -17,8 +17,8 @@ // This is an implementation of a 3D branch-and-bound algorithm similar to // FastCorrelativeScanMatcher2D. -#ifndef CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_H_ -#define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_H_ +#ifndef CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_3D_H_ +#define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_3D_H_ #include #include @@ -28,26 +28,26 @@ #include "cartographer/mapping/trajectory_node.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/proto/fast_correlative_scan_matcher_options_3d.pb.h" #include "cartographer/mapping_3d/scan_matching/rotational_scan_matcher.h" #include "cartographer/sensor/point_cloud.h" namespace cartographer { -namespace mapping_3d { +namespace mapping { namespace scan_matching { -proto::FastCorrelativeScanMatcherOptions -CreateFastCorrelativeScanMatcherOptions( +proto::FastCorrelativeScanMatcherOptions3D +CreateFastCorrelativeScanMatcherOptions3D( common::LuaParameterDictionary* parameter_dictionary); class PrecomputationGridStack; -struct DiscreteScan; -struct Candidate; +struct DiscreteScan3D; +struct Candidate3D; // Used to compute scores between 0 and 1 how well the given pose matches. using MatchingFunction = std::function; -class FastCorrelativeScanMatcher { +class FastCorrelativeScanMatcher3D { public: struct Result { float score; @@ -56,25 +56,24 @@ class FastCorrelativeScanMatcher { float low_resolution_score; }; - FastCorrelativeScanMatcher( - const mapping::HybridGrid& hybrid_grid, - const mapping::HybridGrid* low_resolution_hybrid_grid, - const std::vector& nodes, - const proto::FastCorrelativeScanMatcherOptions& options); - ~FastCorrelativeScanMatcher(); + FastCorrelativeScanMatcher3D( + const HybridGrid& hybrid_grid, + const HybridGrid* low_resolution_hybrid_grid, + const std::vector& nodes, + const proto::FastCorrelativeScanMatcherOptions3D& options); + ~FastCorrelativeScanMatcher3D(); - FastCorrelativeScanMatcher(const FastCorrelativeScanMatcher&) = delete; - FastCorrelativeScanMatcher& operator=(const FastCorrelativeScanMatcher&) = + FastCorrelativeScanMatcher3D(const FastCorrelativeScanMatcher3D&) = delete; + FastCorrelativeScanMatcher3D& operator=(const FastCorrelativeScanMatcher3D&) = delete; // Aligns the node with the given 'constant_data' within the 'hybrid_grid' // given 'global_node_pose' and 'global_submap_pose'. 'Result' is only // returned if a score above 'min_score' (excluding equality) is possible. - std::unique_ptr Match( - const transform::Rigid3d& global_node_pose, - const transform::Rigid3d& global_submap_pose, - const mapping::TrajectoryNode::Data& constant_data, - float min_score) const; + std::unique_ptr Match(const transform::Rigid3d& global_node_pose, + const transform::Rigid3d& global_submap_pose, + const TrajectoryNode::Data& constant_data, + float min_score) const; // Aligns the node with the given 'constant_data' within the 'hybrid_grid' // given rotations which are expected to be approximately gravity aligned. @@ -83,8 +82,7 @@ class FastCorrelativeScanMatcher { std::unique_ptr MatchFullSubmap( const Eigen::Quaterniond& global_node_rotation, const Eigen::Quaterniond& global_submap_rotation, - const mapping::TrajectoryNode::Data& constant_data, - float min_score) const; + const TrajectoryNode::Data& constant_data, float min_score) const; private: struct SearchParameters { @@ -101,43 +99,43 @@ class FastCorrelativeScanMatcher { const sensor::PointCloud& point_cloud, const Eigen::VectorXf& rotational_scan_matcher_histogram, const Eigen::Quaterniond& gravity_alignment, float min_score) const; - DiscreteScan DiscretizeScan(const SearchParameters& search_parameters, - const sensor::PointCloud& point_cloud, - const transform::Rigid3f& pose, - float rotational_score) const; - std::vector GenerateDiscreteScans( + DiscreteScan3D DiscretizeScan(const SearchParameters& search_parameters, + const sensor::PointCloud& point_cloud, + const transform::Rigid3f& pose, + float rotational_score) const; + std::vector GenerateDiscreteScans( const SearchParameters& search_parameters, const sensor::PointCloud& point_cloud, const Eigen::VectorXf& rotational_scan_matcher_histogram, const Eigen::Quaterniond& gravity_alignment, const transform::Rigid3f& global_node_pose, const transform::Rigid3f& global_submap_pose) const; - std::vector GenerateLowestResolutionCandidates( + std::vector GenerateLowestResolutionCandidates( const SearchParameters& search_parameters, int num_discrete_scans) const; void ScoreCandidates(int depth, - const std::vector& discrete_scans, - std::vector* const candidates) const; - std::vector ComputeLowestResolutionCandidates( + const std::vector& discrete_scans, + std::vector* const candidates) const; + std::vector ComputeLowestResolutionCandidates( const SearchParameters& search_parameters, - const std::vector& discrete_scans) const; - Candidate BranchAndBound(const SearchParameters& search_parameters, - const std::vector& discrete_scans, - const std::vector& candidates, - int candidate_depth, float min_score) const; + const std::vector& discrete_scans) const; + Candidate3D BranchAndBound(const SearchParameters& search_parameters, + const std::vector& discrete_scans, + const std::vector& candidates, + int candidate_depth, float min_score) const; transform::Rigid3f GetPoseFromCandidate( - const std::vector& discrete_scans, - const Candidate& candidate) const; + const std::vector& discrete_scans, + const Candidate3D& candidate) const; - const proto::FastCorrelativeScanMatcherOptions options_; + const proto::FastCorrelativeScanMatcherOptions3D options_; const float resolution_; const int width_in_voxels_; std::unique_ptr precomputation_grid_stack_; - const mapping::HybridGrid* const low_resolution_hybrid_grid_; + const HybridGrid* const low_resolution_hybrid_grid_; RotationalScanMatcher rotational_scan_matcher_; }; } // namespace scan_matching -} // namespace mapping_3d +} // namespace mapping } // namespace cartographer -#endif // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_H_ +#endif // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_3D_H_ diff --git a/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_test.cc b/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_3d_test.cc similarity index 74% rename from cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_test.cc rename to cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_3d_test.cc index 7919523..9b678c9 100644 --- a/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_test.cc +++ b/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_3d_test.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h" +#include "cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_3d.h" #include #include @@ -29,15 +29,15 @@ #include "gtest/gtest.h" namespace cartographer { -namespace mapping_3d { +namespace mapping { namespace scan_matching { namespace { -class FastCorrelativeScanMatcherTest : public ::testing::Test { +class FastCorrelativeScanMatcher3DTest : public ::testing::Test { protected: - FastCorrelativeScanMatcherTest() - : range_data_inserter_(CreateRangeDataInserterTestOptions()), - options_(CreateFastCorrelativeScanMatcherTestOptions(5)) {} + FastCorrelativeScanMatcher3DTest() + : range_data_inserter_(CreateRangeDataInserterTestOptions3D()), + options_(CreateFastCorrelativeScanMatcher3DTestOptions3D(5)) {} void SetUp() override { point_cloud_ = { @@ -59,8 +59,8 @@ class FastCorrelativeScanMatcherTest : public ::testing::Test { Eigen::AngleAxisf(theta, Eigen::Vector3f::UnitZ())); } - static proto::FastCorrelativeScanMatcherOptions - CreateFastCorrelativeScanMatcherTestOptions( + static proto::FastCorrelativeScanMatcherOptions3D + CreateFastCorrelativeScanMatcher3DTestOptions3D( const int branch_and_bound_depth) { auto parameter_dictionary = common::MakeDictionary( "return {" @@ -78,25 +78,25 @@ class FastCorrelativeScanMatcherTest : public ::testing::Test { "linear_z_search_window = 0.8, " "angular_search_window = 0.3, " "}"); - return CreateFastCorrelativeScanMatcherOptions(parameter_dictionary.get()); + return CreateFastCorrelativeScanMatcherOptions3D( + parameter_dictionary.get()); } static mapping::proto::RangeDataInserterOptions3D - CreateRangeDataInserterTestOptions() { + CreateRangeDataInserterTestOptions3D() { auto parameter_dictionary = common::MakeDictionary( "return { " "hit_probability = 0.7, " "miss_probability = 0.4, " "num_free_space_voxels = 5, " "}"); - return mapping::CreateRangeDataInserterOptions3D( - parameter_dictionary.get()); + return CreateRangeDataInserterOptions3D(parameter_dictionary.get()); } - std::unique_ptr GetFastCorrelativeScanMatcher( - const proto::FastCorrelativeScanMatcherOptions& options, + std::unique_ptr GetFastCorrelativeScanMatcher( + const proto::FastCorrelativeScanMatcherOptions3D& options, const transform::Rigid3f& pose) { - hybrid_grid_ = common::make_unique(0.05f); + hybrid_grid_ = common::make_unique(0.05f); range_data_inserter_.Insert( sensor::RangeData{pose.translation(), sensor::TransformPointCloud(point_cloud_, pose), @@ -104,44 +104,44 @@ class FastCorrelativeScanMatcherTest : public ::testing::Test { hybrid_grid_.get()); hybrid_grid_->FinishUpdate(); - return common::make_unique( + return common::make_unique( *hybrid_grid_, hybrid_grid_.get(), - std::vector( - {{std::make_shared( + std::vector( + {{std::make_shared( CreateConstantData(point_cloud_)), pose.cast()}}), options); } - mapping::TrajectoryNode::Data CreateConstantData( + TrajectoryNode::Data CreateConstantData( const sensor::PointCloud& low_resolution_point_cloud) { - return mapping::TrajectoryNode::Data{common::FromUniversal(0), - Eigen::Quaterniond::Identity(), - {}, - point_cloud_, - low_resolution_point_cloud, - Eigen::VectorXf::Zero(10)}; + return TrajectoryNode::Data{common::FromUniversal(0), + Eigen::Quaterniond::Identity(), + {}, + point_cloud_, + low_resolution_point_cloud, + Eigen::VectorXf::Zero(10)}; } std::mt19937 prng_ = std::mt19937(42); std::uniform_real_distribution distribution_ = std::uniform_real_distribution(-1.f, 1.f); - mapping::RangeDataInserter3D range_data_inserter_; - const proto::FastCorrelativeScanMatcherOptions options_; + RangeDataInserter3D range_data_inserter_; + const proto::FastCorrelativeScanMatcherOptions3D options_; sensor::PointCloud point_cloud_; - std::unique_ptr hybrid_grid_; + std::unique_ptr hybrid_grid_; }; constexpr float kMinScore = 0.1f; -TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatch) { +TEST_F(FastCorrelativeScanMatcher3DTest, CorrectPoseForMatch) { for (int i = 0; i != 20; ++i) { const auto expected_pose = GetRandomPose(); - std::unique_ptr fast_correlative_scan_matcher( + std::unique_ptr fast_correlative_scan_matcher( GetFastCorrelativeScanMatcher(options_, expected_pose)); - const std::unique_ptr result = + const std::unique_ptr result = fast_correlative_scan_matcher->Match( transform::Rigid3d::Identity(), transform::Rigid3d::Identity(), CreateConstantData(point_cloud_), kMinScore); @@ -154,7 +154,7 @@ TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatch) { << "Actual: " << transform::ToProto(result->pose_estimate).DebugString() << "\nExpected: " << transform::ToProto(expected_pose).DebugString(); - const std::unique_ptr + const std::unique_ptr low_resolution_result = fast_correlative_scan_matcher->Match( transform::Rigid3d::Identity(), transform::Rigid3d::Identity(), CreateConstantData({Eigen::Vector3f(42.f, 42.f, 42.f)}), kMinScore); @@ -163,13 +163,13 @@ TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatch) { } } -TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatchFullSubmap) { +TEST_F(FastCorrelativeScanMatcher3DTest, CorrectPoseForMatchFullSubmap) { const auto expected_pose = GetRandomPose(); - std::unique_ptr fast_correlative_scan_matcher( + std::unique_ptr fast_correlative_scan_matcher( GetFastCorrelativeScanMatcher(options_, expected_pose)); - const std::unique_ptr result = + const std::unique_ptr result = fast_correlative_scan_matcher->MatchFullSubmap( Eigen::Quaterniond::Identity(), Eigen::Quaterniond::Identity(), CreateConstantData(point_cloud_), kMinScore); @@ -182,7 +182,7 @@ TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatchFullSubmap) { << "Actual: " << transform::ToProto(result->pose_estimate).DebugString() << "\nExpected: " << transform::ToProto(expected_pose).DebugString(); - const std::unique_ptr + const std::unique_ptr low_resolution_result = fast_correlative_scan_matcher->MatchFullSubmap( Eigen::Quaterniond::Identity(), Eigen::Quaterniond::Identity(), CreateConstantData({Eigen::Vector3f(42.f, 42.f, 42.f)}), kMinScore); @@ -192,5 +192,5 @@ TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatchFullSubmap) { } // namespace } // namespace scan_matching -} // namespace mapping_3d +} // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping_3d/scan_matching/interpolated_grid.h b/cartographer/mapping_3d/scan_matching/interpolated_grid.h index ec9c313..e88e508 100644 --- a/cartographer/mapping_3d/scan_matching/interpolated_grid.h +++ b/cartographer/mapping_3d/scan_matching/interpolated_grid.h @@ -22,7 +22,7 @@ #include "cartographer/mapping_3d/hybrid_grid.h" namespace cartographer { -namespace mapping_3d { +namespace mapping { namespace scan_matching { // Interpolates between HybridGrid probability voxels. We use the tricubic @@ -34,7 +34,7 @@ namespace scan_matching { // continuously differentiable. class InterpolatedGrid { public: - explicit InterpolatedGrid(const mapping::HybridGrid& hybrid_grid) + explicit InterpolatedGrid(const HybridGrid& hybrid_grid) : hybrid_grid_(hybrid_grid) {} InterpolatedGrid(const InterpolatedGrid&) = delete; @@ -145,11 +145,11 @@ class InterpolatedGrid { return CenterOfLowerVoxel(jet_x.a, jet_y.a, jet_z.a); } - const mapping::HybridGrid& hybrid_grid_; + const HybridGrid& hybrid_grid_; }; } // namespace scan_matching -} // namespace mapping_3d +} // namespace mapping } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_INTERPOLATED_GRID_H_ diff --git a/cartographer/mapping_3d/scan_matching/interpolated_grid_test.cc b/cartographer/mapping_3d/scan_matching/interpolated_grid_test.cc index e7b8615..4981d14 100644 --- a/cartographer/mapping_3d/scan_matching/interpolated_grid_test.cc +++ b/cartographer/mapping_3d/scan_matching/interpolated_grid_test.cc @@ -21,7 +21,7 @@ #include "gtest/gtest.h" namespace cartographer { -namespace mapping_3d { +namespace mapping { namespace scan_matching { namespace { @@ -43,7 +43,7 @@ class InterpolatedGridTest : public ::testing::Test { hybrid_grid_.GetCellIndex(Eigen::Vector3f(x, y, z))); } - mapping::HybridGrid hybrid_grid_; + HybridGrid hybrid_grid_; InterpolatedGrid interpolated_grid_; }; @@ -85,5 +85,5 @@ TEST_F(InterpolatedGridTest, MonotonicBehaviorBetweenGridPointsInX) { } // namespace } // namespace scan_matching -} // namespace mapping_3d +} // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping_3d/scan_matching/low_resolution_matcher.cc b/cartographer/mapping_3d/scan_matching/low_resolution_matcher.cc index 6c31072..f9cfd39 100644 --- a/cartographer/mapping_3d/scan_matching/low_resolution_matcher.cc +++ b/cartographer/mapping_3d/scan_matching/low_resolution_matcher.cc @@ -17,12 +17,11 @@ #include "cartographer/mapping_3d/scan_matching/low_resolution_matcher.h" namespace cartographer { -namespace mapping_3d { +namespace mapping { namespace scan_matching { std::function CreateLowResolutionMatcher( - const mapping::HybridGrid* low_resolution_grid, - const sensor::PointCloud* points) { + const HybridGrid* low_resolution_grid, const sensor::PointCloud* points) { return [=](const transform::Rigid3f& pose) { float score = 0.f; for (const Eigen::Vector3f& point : @@ -36,5 +35,5 @@ std::function CreateLowResolutionMatcher( } } // namespace scan_matching -} // namespace mapping_3d +} // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping_3d/scan_matching/low_resolution_matcher.h b/cartographer/mapping_3d/scan_matching/low_resolution_matcher.h index 4453571..2bf23e3 100644 --- a/cartographer/mapping_3d/scan_matching/low_resolution_matcher.h +++ b/cartographer/mapping_3d/scan_matching/low_resolution_matcher.h @@ -24,15 +24,14 @@ #include "cartographer/transform/rigid_transform.h" namespace cartographer { -namespace mapping_3d { +namespace mapping { namespace scan_matching { std::function CreateLowResolutionMatcher( - const mapping::HybridGrid* low_resolution_grid, - const sensor::PointCloud* points); + const HybridGrid* low_resolution_grid, const sensor::PointCloud* points); } // namespace scan_matching -} // namespace mapping_3d +} // namespace mapping } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_LOW_RESOLUTION_MATCHER_H_ diff --git a/cartographer/mapping_3d/scan_matching/precomputation_grid.cc b/cartographer/mapping_3d/scan_matching/precomputation_grid_3d.cc similarity index 73% rename from cartographer/mapping_3d/scan_matching/precomputation_grid.cc rename to cartographer/mapping_3d/scan_matching/precomputation_grid_3d.cc index 6c2eef9..614be10 100644 --- a/cartographer/mapping_3d/scan_matching/precomputation_grid.cc +++ b/cartographer/mapping_3d/scan_matching/precomputation_grid_3d.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/mapping_3d/scan_matching/precomputation_grid.h" +#include "cartographer/mapping_3d/scan_matching/precomputation_grid_3d.h" #include @@ -24,9 +24,8 @@ #include "glog/logging.h" namespace cartographer { -namespace mapping_3d { +namespace mapping { namespace scan_matching { - namespace { // C++11 defines that integer division rounds towards zero. For index math, we @@ -47,15 +46,13 @@ Eigen::Array3i CellIndexAtHalfResolution(const Eigen::Array3i& cell_index) { } // namespace -PrecomputationGrid ConvertToPrecomputationGrid( - const mapping::HybridGrid& hybrid_grid) { - PrecomputationGrid result(hybrid_grid.resolution()); - for (auto it = mapping::HybridGrid::Iterator(hybrid_grid); !it.Done(); - it.Next()) { +PrecomputationGrid3D ConvertToPrecomputationGrid( + const HybridGrid& hybrid_grid) { + PrecomputationGrid3D result(hybrid_grid.resolution()); + for (auto it = HybridGrid::Iterator(hybrid_grid); !it.Done(); it.Next()) { const int cell_value = common::RoundToInt( - (mapping::ValueToProbability(it.GetValue()) - - mapping::kMinProbability) * - (255.f / (mapping::kMaxProbability - mapping::kMinProbability))); + (ValueToProbability(it.GetValue()) - kMinProbability) * + (255.f / (kMaxProbability - kMinProbability))); CHECK_GE(cell_value, 0); CHECK_LE(cell_value, 255); *result.mutable_value(it.GetCellIndex()) = cell_value; @@ -63,18 +60,18 @@ PrecomputationGrid ConvertToPrecomputationGrid( return result; } -PrecomputationGrid PrecomputeGrid(const PrecomputationGrid& grid, - const bool half_resolution, - const Eigen::Array3i& shift) { - PrecomputationGrid result(grid.resolution()); - for (auto it = PrecomputationGrid::Iterator(grid); !it.Done(); it.Next()) { +PrecomputationGrid3D PrecomputeGrid(const PrecomputationGrid3D& grid, + const bool half_resolution, + const Eigen::Array3i& shift) { + PrecomputationGrid3D result(grid.resolution()); + for (auto it = PrecomputationGrid3D::Iterator(grid); !it.Done(); it.Next()) { for (int i = 0; i != 8; ++i) { // We use this value to update 8 values in the resulting grid, at // position (x - {0, 'shift'}, y - {0, 'shift'}, z - {0, 'shift'}). // If 'shift' is 2 ** (depth - 1), where depth 0 is the original grid, // this results in precomputation grids analogous to the 2D case. const Eigen::Array3i cell_index = - it.GetCellIndex() - shift * PrecomputationGrid::GetOctant(i); + it.GetCellIndex() - shift * PrecomputationGrid3D::GetOctant(i); auto* const cell_value = result.mutable_value( half_resolution ? CellIndexAtHalfResolution(cell_index) : cell_index); *cell_value = std::max(it.GetValue(), *cell_value); @@ -84,5 +81,5 @@ PrecomputationGrid PrecomputeGrid(const PrecomputationGrid& grid, } } // namespace scan_matching -} // namespace mapping_3d +} // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping_3d/scan_matching/precomputation_grid.h b/cartographer/mapping_3d/scan_matching/precomputation_grid_3d.h similarity index 68% rename from cartographer/mapping_3d/scan_matching/precomputation_grid.h rename to cartographer/mapping_3d/scan_matching/precomputation_grid_3d.h index f6f45fe..f1d8686 100644 --- a/cartographer/mapping_3d/scan_matching/precomputation_grid.h +++ b/cartographer/mapping_3d/scan_matching/precomputation_grid_3d.h @@ -14,32 +14,30 @@ * limitations under the License. */ -#ifndef CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_PRECOMPUTATION_GRID_H_ -#define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_PRECOMPUTATION_GRID_H_ +#ifndef CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_PRECOMPUTATION_GRID_3D_H_ +#define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_PRECOMPUTATION_GRID_3D_H_ #include "cartographer/mapping_3d/hybrid_grid.h" namespace cartographer { -namespace mapping_3d { +namespace mapping { namespace scan_matching { -class PrecomputationGrid : public mapping::HybridGridBase { +class PrecomputationGrid3D : public HybridGridBase { public: - explicit PrecomputationGrid(const float resolution) - : mapping::HybridGridBase(resolution) {} + explicit PrecomputationGrid3D(const float resolution) + : HybridGridBase(resolution) {} // 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); } }; -// Converts a HybridGrid to a PrecomputationGrid representing the same data, +// Converts a HybridGrid to a PrecomputationGrid3D representing the same data, // but only using 8 bit instead of 2 x 16 bit. -PrecomputationGrid ConvertToPrecomputationGrid( - const mapping::HybridGrid& hybrid_grid); +PrecomputationGrid3D ConvertToPrecomputationGrid(const HybridGrid& hybrid_grid); // Returns a grid of the same resolution containing the maximum value of // original voxels in 'grid'. This maximum is over the 8 voxels that have @@ -47,12 +45,12 @@ PrecomputationGrid ConvertToPrecomputationGrid( // If 'shift' is 2 ** (depth - 1), where depth 0 is the original grid, and this // is using the precomputed grid of one depth before, this results in // precomputation grids analogous to the 2D case. -PrecomputationGrid PrecomputeGrid(const PrecomputationGrid& grid, - bool half_resolution, - const Eigen::Array3i& shift); +PrecomputationGrid3D PrecomputeGrid(const PrecomputationGrid3D& grid, + bool half_resolution, + const Eigen::Array3i& shift); } // namespace scan_matching -} // namespace mapping_3d +} // namespace mapping } // namespace cartographer -#endif // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_PRECOMPUTATION_GRID_H_ +#endif // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_PRECOMPUTATION_GRID_3D_H_ diff --git a/cartographer/mapping_3d/scan_matching/precomputation_grid_test.cc b/cartographer/mapping_3d/scan_matching/precomputation_grid_3d_test.cc similarity index 85% rename from cartographer/mapping_3d/scan_matching/precomputation_grid_test.cc rename to cartographer/mapping_3d/scan_matching/precomputation_grid_3d_test.cc index 111cb06..3f2463e 100644 --- a/cartographer/mapping_3d/scan_matching/precomputation_grid_test.cc +++ b/cartographer/mapping_3d/scan_matching/precomputation_grid_3d_test.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/mapping_3d/scan_matching/precomputation_grid.h" +#include "cartographer/mapping_3d/scan_matching/precomputation_grid_3d.h" #include #include @@ -24,17 +24,17 @@ #include "gmock/gmock.h" namespace cartographer { -namespace mapping_3d { +namespace mapping { namespace scan_matching { namespace { -TEST(PrecomputedGridGeneratorTest, TestAgainstNaiveAlgorithm) { - mapping::HybridGrid hybrid_grid(2.f); +TEST(PrecomputedGridGenerator3DTest, TestAgainstNaiveAlgorithm) { + HybridGrid hybrid_grid(2.f); std::mt19937 rng(23847); std::uniform_int_distribution coordinate_distribution(-50, 49); - std::uniform_real_distribution value_distribution( - mapping::kMinProbability, mapping::kMaxProbability); + std::uniform_real_distribution value_distribution(kMinProbability, + kMaxProbability); for (int i = 0; i < 1000; ++i) { const auto x = coordinate_distribution(rng); const auto y = coordinate_distribution(rng); @@ -43,7 +43,7 @@ TEST(PrecomputedGridGeneratorTest, TestAgainstNaiveAlgorithm) { hybrid_grid.SetProbability(cell_index, value_distribution(rng)); } - std::vector precomputed_grids; + std::vector precomputed_grids; for (int depth = 0; depth <= 3; ++depth) { if (depth == 0) { precomputed_grids.push_back(ConvertToPrecomputationGrid(hybrid_grid)); @@ -69,7 +69,7 @@ TEST(PrecomputedGridGeneratorTest, TestAgainstNaiveAlgorithm) { } EXPECT_NEAR(max_probability, - PrecomputationGrid::ToProbability( + PrecomputationGrid3D::ToProbability( precomputed_grids.back().value(Eigen::Array3i(x, y, z))), 1e-2); } @@ -78,5 +78,5 @@ TEST(PrecomputedGridGeneratorTest, TestAgainstNaiveAlgorithm) { } // namespace } // namespace scan_matching -} // namespace mapping_3d +} // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.proto b/cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options_3d.proto similarity index 92% rename from cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.proto rename to cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options_3d.proto index fc590cc..8c9030a 100644 --- a/cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.proto +++ b/cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options_3d.proto @@ -14,12 +14,12 @@ syntax = "proto3"; -package cartographer.mapping_3d.scan_matching.proto; +package cartographer.mapping.scan_matching.proto; import "cartographer/common/proto/ceres_solver_options.proto"; // NEXT ID: 7 -message CeresScanMatcherOptions { +message CeresScanMatcherOptions3D { // Scaling parameters for each cost functor. repeated double occupied_space_weight = 1; double translation_weight = 2; diff --git a/cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options.proto b/cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options_3d.proto similarity index 94% rename from cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options.proto rename to cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options_3d.proto index 4baa066..b6330f1 100644 --- a/cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options.proto +++ b/cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options_3d.proto @@ -14,9 +14,9 @@ syntax = "proto3"; -package cartographer.mapping_3d.scan_matching.proto; +package cartographer.mapping.scan_matching.proto; -message FastCorrelativeScanMatcherOptions { +message FastCorrelativeScanMatcherOptions3D { // Number of precomputed grids to use. int32 branch_and_bound_depth = 2; 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_3d.cc similarity index 88% rename from cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher.cc rename to cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher_3d.cc index 4b3a600..482e1e9 100644 --- a/cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher.cc +++ b/cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher_3d.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher.h" +#include "cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher_3d.h" #include @@ -24,18 +24,16 @@ #include "glog/logging.h" namespace cartographer { -namespace mapping_3d { +namespace mapping { namespace scan_matching { -RealTimeCorrelativeScanMatcher::RealTimeCorrelativeScanMatcher( - const mapping::scan_matching::proto::RealTimeCorrelativeScanMatcherOptions& - options) +RealTimeCorrelativeScanMatcher3D::RealTimeCorrelativeScanMatcher3D( + const proto::RealTimeCorrelativeScanMatcherOptions& options) : options_(options) {} -float RealTimeCorrelativeScanMatcher::Match( +float RealTimeCorrelativeScanMatcher3D::Match( const transform::Rigid3d& initial_pose_estimate, - const sensor::PointCloud& point_cloud, - const mapping::HybridGrid& hybrid_grid, + const sensor::PointCloud& point_cloud, const HybridGrid& hybrid_grid, transform::Rigid3d* pose_estimate) const { CHECK_NOTNULL(pose_estimate); float best_score = -1.f; @@ -55,7 +53,7 @@ float RealTimeCorrelativeScanMatcher::Match( } std::vector -RealTimeCorrelativeScanMatcher::GenerateExhaustiveSearchTransforms( +RealTimeCorrelativeScanMatcher3D::GenerateExhaustiveSearchTransforms( const float resolution, const sensor::PointCloud& point_cloud) const { std::vector result; const int linear_window_size = @@ -96,8 +94,8 @@ RealTimeCorrelativeScanMatcher::GenerateExhaustiveSearchTransforms( return result; } -float RealTimeCorrelativeScanMatcher::ScoreCandidate( - const mapping::HybridGrid& hybrid_grid, +float RealTimeCorrelativeScanMatcher3D::ScoreCandidate( + const HybridGrid& hybrid_grid, const sensor::PointCloud& transformed_point_cloud, const transform::Rigid3f& transform) const { float score = 0.f; @@ -115,5 +113,5 @@ float RealTimeCorrelativeScanMatcher::ScoreCandidate( } } // namespace scan_matching -} // namespace mapping_3d +} // namespace mapping } // namespace cartographer 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_3d.h similarity index 73% rename from cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher.h rename to cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher_3d.h index 578d873..bdedfc6 100644 --- a/cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher.h +++ b/cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher_3d.h @@ -14,10 +14,8 @@ * limitations under the License. */ -// A voxel accurate scan matcher, exhaustively evaluating the scan matching -// search space. -#ifndef CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_H_ -#define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_H_ +#ifndef CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_3D_H_ +#define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_3D_H_ #include @@ -27,41 +25,42 @@ #include "cartographer/sensor/point_cloud.h" namespace cartographer { -namespace mapping_3d { +namespace mapping { namespace scan_matching { -class RealTimeCorrelativeScanMatcher { +// A voxel accurate scan matcher, exhaustively evaluating the scan matching +// search space. +class RealTimeCorrelativeScanMatcher3D { public: - explicit RealTimeCorrelativeScanMatcher( - const mapping::scan_matching::proto:: - RealTimeCorrelativeScanMatcherOptions& options); + explicit RealTimeCorrelativeScanMatcher3D( + const scan_matching::proto::RealTimeCorrelativeScanMatcherOptions& + options); - RealTimeCorrelativeScanMatcher(const RealTimeCorrelativeScanMatcher&) = + RealTimeCorrelativeScanMatcher3D(const RealTimeCorrelativeScanMatcher3D&) = delete; - RealTimeCorrelativeScanMatcher& operator=( - const RealTimeCorrelativeScanMatcher&) = delete; + RealTimeCorrelativeScanMatcher3D& operator=( + const RealTimeCorrelativeScanMatcher3D&) = delete; // Aligns 'point_cloud' within the 'hybrid_grid' given an // 'initial_pose_estimate' then updates 'pose_estimate' with the result and // returns the score. float Match(const transform::Rigid3d& initial_pose_estimate, const sensor::PointCloud& point_cloud, - const mapping::HybridGrid& hybrid_grid, + const HybridGrid& hybrid_grid, transform::Rigid3d* pose_estimate) const; private: std::vector GenerateExhaustiveSearchTransforms( float resolution, const sensor::PointCloud& point_cloud) const; - float ScoreCandidate(const mapping::HybridGrid& hybrid_grid, + float ScoreCandidate(const HybridGrid& hybrid_grid, const sensor::PointCloud& transformed_point_cloud, const transform::Rigid3f& transform) const; - const mapping::scan_matching::proto::RealTimeCorrelativeScanMatcherOptions - options_; + const proto::RealTimeCorrelativeScanMatcherOptions options_; }; } // namespace scan_matching -} // namespace mapping_3d +} // namespace mapping } // namespace cartographer -#endif // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_H_ +#endif // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_3D_H_ 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_3d_test.cc similarity index 81% rename from cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher_test.cc rename to cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher_3d_test.cc index f72bbb2..d94675b 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_3d_test.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher.h" +#include "cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher_3d.h" #include @@ -29,13 +29,13 @@ #include "gtest/gtest.h" namespace cartographer { -namespace mapping_3d { +namespace mapping { namespace scan_matching { namespace { -class RealTimeCorrelativeScanMatcherTest : public ::testing::Test { +class RealTimeCorrelativeScanMatcher3DTest : public ::testing::Test { protected: - RealTimeCorrelativeScanMatcherTest() + RealTimeCorrelativeScanMatcher3DTest() : hybrid_grid_(0.1f), expected_pose_(Eigen::Vector3d(-1., 0., 0.), Eigen::Quaterniond::Identity()) { @@ -57,8 +57,8 @@ class RealTimeCorrelativeScanMatcherTest : public ::testing::Test { rotation_delta_cost_weight = 1., })text"); real_time_correlative_scan_matcher_.reset( - new RealTimeCorrelativeScanMatcher( - mapping::scan_matching::CreateRealTimeCorrelativeScanMatcherOptions( + new RealTimeCorrelativeScanMatcher3D( + CreateRealTimeCorrelativeScanMatcherOptions( parameter_dictionary.get()))); } @@ -71,46 +71,46 @@ class RealTimeCorrelativeScanMatcherTest : public ::testing::Test { EXPECT_THAT(pose, transform::IsNearly(expected_pose_, 1e-3)); } - mapping::HybridGrid hybrid_grid_; + HybridGrid hybrid_grid_; transform::Rigid3d expected_pose_; sensor::PointCloud point_cloud_; - std::unique_ptr + std::unique_ptr real_time_correlative_scan_matcher_; }; -TEST_F(RealTimeCorrelativeScanMatcherTest, PerfectEstimate) { +TEST_F(RealTimeCorrelativeScanMatcher3DTest, PerfectEstimate) { TestFromInitialPose( transform::Rigid3d::Translation(Eigen::Vector3d(-1., 0., 0.))); } -TEST_F(RealTimeCorrelativeScanMatcherTest, AlongX) { +TEST_F(RealTimeCorrelativeScanMatcher3DTest, AlongX) { TestFromInitialPose( transform::Rigid3d::Translation(Eigen::Vector3d(-0.8, 0., 0.))); } -TEST_F(RealTimeCorrelativeScanMatcherTest, AlongZ) { +TEST_F(RealTimeCorrelativeScanMatcher3DTest, AlongZ) { TestFromInitialPose( transform::Rigid3d::Translation(Eigen::Vector3d(-1., 0., -0.2))); } -TEST_F(RealTimeCorrelativeScanMatcherTest, AlongXYZ) { +TEST_F(RealTimeCorrelativeScanMatcher3DTest, AlongXYZ) { TestFromInitialPose( transform::Rigid3d::Translation(Eigen::Vector3d(-0.9, -0.2, 0.2))); } -TEST_F(RealTimeCorrelativeScanMatcherTest, RotationAroundX) { +TEST_F(RealTimeCorrelativeScanMatcher3DTest, RotationAroundX) { TestFromInitialPose(transform::Rigid3d( Eigen::Vector3d(-1., 0., 0.), Eigen::AngleAxisd(0.8 / 180. * M_PI, Eigen::Vector3d(1., 0., 0.)))); } -TEST_F(RealTimeCorrelativeScanMatcherTest, RotationAroundY) { +TEST_F(RealTimeCorrelativeScanMatcher3DTest, RotationAroundY) { TestFromInitialPose(transform::Rigid3d( Eigen::Vector3d(-1., 0., 0.), Eigen::AngleAxisd(0.8 / 180. * M_PI, Eigen::Vector3d(0., 1., 0.)))); } -TEST_F(RealTimeCorrelativeScanMatcherTest, RotationAroundYZ) { +TEST_F(RealTimeCorrelativeScanMatcher3DTest, RotationAroundYZ) { TestFromInitialPose(transform::Rigid3d( Eigen::Vector3d(-1., 0., 0.), Eigen::AngleAxisd(0.8 / 180. * M_PI, Eigen::Vector3d(0., 1., 1.)))); @@ -118,5 +118,5 @@ TEST_F(RealTimeCorrelativeScanMatcherTest, RotationAroundYZ) { } // namespace } // namespace scan_matching -} // namespace mapping_3d +} // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping_3d/scan_matching/rotation_delta_cost_functor.h b/cartographer/mapping_3d/scan_matching/rotation_delta_cost_functor_3d.h similarity index 80% rename from cartographer/mapping_3d/scan_matching/rotation_delta_cost_functor.h rename to cartographer/mapping_3d/scan_matching/rotation_delta_cost_functor_3d.h index f6a3377..5c83398 100644 --- a/cartographer/mapping_3d/scan_matching/rotation_delta_cost_functor.h +++ b/cartographer/mapping_3d/scan_matching/rotation_delta_cost_functor_3d.h @@ -14,8 +14,8 @@ * limitations under the License. */ -#ifndef CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_H_ -#define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_H_ +#ifndef CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_3D_H_ +#define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_3D_H_ #include @@ -24,19 +24,19 @@ #include "ceres/rotation.h" namespace cartographer { -namespace mapping_3d { +namespace mapping { namespace scan_matching { // Computes the cost of rotating 'rotation_quaternion' to 'target_rotation'. // Cost increases with the solution's distance from 'target_rotation'. -class RotationDeltaCostFunctor { +class RotationDeltaCostFunctor3D { public: static ceres::CostFunction* CreateAutoDiffCostFunction( const double scaling_factor, const Eigen::Quaterniond& target_rotation) { - return new ceres::AutoDiffCostFunction( - new RotationDeltaCostFunctor(scaling_factor, target_rotation)); + new RotationDeltaCostFunctor3D(scaling_factor, target_rotation)); } template @@ -56,9 +56,10 @@ class RotationDeltaCostFunctor { } private: - // Constructs a new RotationDeltaCostFunctor from the given 'target_rotation'. - explicit RotationDeltaCostFunctor(const double scaling_factor, - const Eigen::Quaterniond& target_rotation) + // Constructs a new RotationDeltaCostFunctor3D from the given + // 'target_rotation'. + explicit RotationDeltaCostFunctor3D(const double scaling_factor, + const Eigen::Quaterniond& target_rotation) : scaling_factor_(scaling_factor) { target_rotation_inverse_[0] = target_rotation.w(); target_rotation_inverse_[1] = -target_rotation.x(); @@ -66,15 +67,16 @@ class RotationDeltaCostFunctor { target_rotation_inverse_[3] = -target_rotation.z(); } - RotationDeltaCostFunctor(const RotationDeltaCostFunctor&) = delete; - RotationDeltaCostFunctor& operator=(const RotationDeltaCostFunctor&) = delete; + RotationDeltaCostFunctor3D(const RotationDeltaCostFunctor3D&) = delete; + RotationDeltaCostFunctor3D& operator=(const RotationDeltaCostFunctor3D&) = + delete; const double scaling_factor_; double target_rotation_inverse_[4]; }; } // namespace scan_matching -} // namespace mapping_3d +} // namespace mapping } // namespace cartographer -#endif // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_H_ +#endif // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_3D_H_ diff --git a/cartographer/mapping_3d/scan_matching/rotation_delta_cost_functor_test.cc b/cartographer/mapping_3d/scan_matching/rotation_delta_cost_functor_3d_test.cc similarity index 89% rename from cartographer/mapping_3d/scan_matching/rotation_delta_cost_functor_test.cc rename to cartographer/mapping_3d/scan_matching/rotation_delta_cost_functor_3d_test.cc index b4adca7..fe4bf90 100644 --- a/cartographer/mapping_3d/scan_matching/rotation_delta_cost_functor_test.cc +++ b/cartographer/mapping_3d/scan_matching/rotation_delta_cost_functor_3d_test.cc @@ -14,12 +14,12 @@ * limitations under the License. */ -#include "cartographer/mapping_3d/scan_matching/rotation_delta_cost_functor.h" +#include "cartographer/mapping_3d/scan_matching/rotation_delta_cost_functor_3d.h" #include "gtest/gtest.h" namespace cartographer { -namespace mapping_3d { +namespace mapping { namespace scan_matching { namespace { @@ -29,8 +29,8 @@ double ComputeRotationDeltaSquaredCost( const Eigen::Quaterniond& rotation, const double scaling_factor, const Eigen::Quaterniond& target_rotation) { std::unique_ptr cost_function( - RotationDeltaCostFunctor::CreateAutoDiffCostFunction(scaling_factor, - target_rotation)); + RotationDeltaCostFunctor3D::CreateAutoDiffCostFunction(scaling_factor, + target_rotation)); const std::array parameter_quaternion = { {rotation.w(), rotation.x(), rotation.y(), rotation.z()}}; const std::vector parameters = {parameter_quaternion.data()}; @@ -44,7 +44,7 @@ double ComputeRotationDeltaSquaredCost( return sum_of_squares; } -TEST(RotationDeltaCostFunctorTest, SameRotationGivesZeroCost) { +TEST(RotationDeltaCostFunctor3DTest, SameRotationGivesZeroCost) { EXPECT_NEAR( 0., ComputeRotationDeltaSquaredCost(Eigen::Quaterniond::Identity(), 1.0, @@ -57,7 +57,7 @@ TEST(RotationDeltaCostFunctorTest, SameRotationGivesZeroCost) { kPrecision); } -TEST(RotationDeltaCostFunctorTest, ComputesCorrectCost) { +TEST(RotationDeltaCostFunctor3DTest, ComputesCorrectCost) { double scaling_factor = 1.2; double angle = 0.8; Eigen::Quaterniond rotation( @@ -81,5 +81,5 @@ TEST(RotationDeltaCostFunctorTest, ComputesCorrectCost) { } // namespace } // namespace scan_matching -} // namespace mapping_3d +} // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping_3d/scan_matching/rotational_scan_matcher.cc b/cartographer/mapping_3d/scan_matching/rotational_scan_matcher.cc index 26a3385..b3ebd04 100644 --- a/cartographer/mapping_3d/scan_matching/rotational_scan_matcher.cc +++ b/cartographer/mapping_3d/scan_matching/rotational_scan_matcher.cc @@ -23,7 +23,7 @@ #include "cartographer/common/port.h" namespace cartographer { -namespace mapping_3d { +namespace mapping { namespace scan_matching { namespace { @@ -190,5 +190,5 @@ std::vector RotationalScanMatcher::Match( } } // namespace scan_matching -} // namespace mapping_3d +} // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping_3d/scan_matching/rotational_scan_matcher.h b/cartographer/mapping_3d/scan_matching/rotational_scan_matcher.h index 65e0013..31897e9 100644 --- a/cartographer/mapping_3d/scan_matching/rotational_scan_matcher.h +++ b/cartographer/mapping_3d/scan_matching/rotational_scan_matcher.h @@ -23,7 +23,7 @@ #include "cartographer/sensor/point_cloud.h" namespace cartographer { -namespace mapping_3d { +namespace mapping { namespace scan_matching { class RotationalScanMatcher { @@ -51,7 +51,7 @@ class RotationalScanMatcher { }; } // namespace scan_matching -} // namespace mapping_3d +} // namespace mapping } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_ROTATIONAL_SCAN_MATCHER_H_ diff --git a/cartographer/mapping_3d/scan_matching/rotational_scan_matcher_test.cc b/cartographer/mapping_3d/scan_matching/rotational_scan_matcher_test.cc index 08ae9ad..077ba7a 100644 --- a/cartographer/mapping_3d/scan_matching/rotational_scan_matcher_test.cc +++ b/cartographer/mapping_3d/scan_matching/rotational_scan_matcher_test.cc @@ -21,11 +21,11 @@ #include "gtest/gtest.h" namespace cartographer { -namespace mapping_3d { +namespace mapping { namespace scan_matching { namespace { -TEST(RotationalScanMatcherTest, OnlySameHistogramIsScoreOne) { +TEST(RotationalScanMatcher3DTest, OnlySameHistogramIsScoreOne) { Eigen::VectorXf histogram(7); histogram << 1.f, 43.f, 0.5f, 0.3123f, 23.f, 42.f, 0.f; RotationalScanMatcher matcher({{histogram, 0.f}}); @@ -35,7 +35,7 @@ TEST(RotationalScanMatcherTest, OnlySameHistogramIsScoreOne) { EXPECT_GT(1.f, scores[1]); } -TEST(RotationalScanMatcherTest, InterpolatesAsExpected) { +TEST(RotationalScanMatcher3DTest, InterpolatesAsExpected) { constexpr int kNumBuckets = 10; constexpr float kAnglePerBucket = M_PI / kNumBuckets; constexpr float kNoInitialRotation = 0.f; @@ -67,5 +67,5 @@ TEST(RotationalScanMatcherTest, InterpolatesAsExpected) { } // namespace } // namespace scan_matching -} // namespace mapping_3d +} // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping_3d/scan_matching/translation_delta_cost_functor.h b/cartographer/mapping_3d/scan_matching/translation_delta_cost_functor_3d.h similarity index 80% rename from cartographer/mapping_3d/scan_matching/translation_delta_cost_functor.h rename to cartographer/mapping_3d/scan_matching/translation_delta_cost_functor_3d.h index 3a76e40..d00a1c2 100644 --- a/cartographer/mapping_3d/scan_matching/translation_delta_cost_functor.h +++ b/cartographer/mapping_3d/scan_matching/translation_delta_cost_functor_3d.h @@ -14,25 +14,25 @@ * limitations under the License. */ -#ifndef CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_H_ -#define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_H_ +#ifndef CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_3D_H_ +#define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_3D_H_ #include "Eigen/Core" namespace cartographer { -namespace mapping_3d { +namespace mapping { namespace scan_matching { // Computes the cost of translating 'translation' to 'target_translation'. // Cost increases with the solution's distance from 'target_translation'. -class TranslationDeltaCostFunctor { +class TranslationDeltaCostFunctor3D { public: static ceres::CostFunction* CreateAutoDiffCostFunction( const double scaling_factor, const Eigen::Vector3d& target_translation) { - return new ceres::AutoDiffCostFunction( - new TranslationDeltaCostFunctor(scaling_factor, target_translation)); + new TranslationDeltaCostFunctor3D(scaling_factor, target_translation)); } template @@ -44,18 +44,18 @@ class TranslationDeltaCostFunctor { } private: - // Constructs a new TranslationDeltaCostFunctor from the given + // Constructs a new TranslationDeltaCostFunctor3D from the given // 'target_translation'. - explicit TranslationDeltaCostFunctor( + explicit TranslationDeltaCostFunctor3D( const double scaling_factor, const Eigen::Vector3d& target_translation) : scaling_factor_(scaling_factor), x_(target_translation.x()), y_(target_translation.y()), z_(target_translation.z()) {} - TranslationDeltaCostFunctor(const TranslationDeltaCostFunctor&) = delete; - TranslationDeltaCostFunctor& operator=(const TranslationDeltaCostFunctor&) = - delete; + TranslationDeltaCostFunctor3D(const TranslationDeltaCostFunctor3D&) = delete; + TranslationDeltaCostFunctor3D& operator=( + const TranslationDeltaCostFunctor3D&) = delete; const double scaling_factor_; const double x_; @@ -64,7 +64,7 @@ class TranslationDeltaCostFunctor { }; } // namespace scan_matching -} // namespace mapping_3d +} // namespace mapping } // namespace cartographer -#endif // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_H_ +#endif // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_3D_H_ diff --git a/cartographer/metrics/register.cc b/cartographer/metrics/register.cc index deeaf0d..0e690c6 100644 --- a/cartographer/metrics/register.cc +++ b/cartographer/metrics/register.cc @@ -17,14 +17,14 @@ #include "cartographer/metrics/register.h" #include "cartographer/mapping_2d/pose_graph/constraint_builder_2d.h" -#include "cartographer/mapping_3d/pose_graph/constraint_builder.h" +#include "cartographer/mapping_3d/pose_graph/constraint_builder_3d.h" namespace cartographer { namespace metrics { void RegisterAllMetrics(FamilyFactory* registry) { mapping::pose_graph::ConstraintBuilder2D::RegisterMetrics(registry); - mapping_3d::pose_graph::ConstraintBuilder::RegisterMetrics(registry); + mapping::pose_graph::ConstraintBuilder3D::RegisterMetrics(registry); } } // namespace metrics