diff --git a/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc b/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc index 361f724cf0..a9bf65f3ae 100644 --- a/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc +++ b/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc @@ -86,9 +86,11 @@ std::unique_ptr LocalTrajectoryBuilder3D::ScanMatch( (matching_submap->local_pose().inverse() * pose_prediction).translation(), initial_ceres_pose, {{&high_resolution_point_cloud_in_tracking, - &matching_submap->high_resolution_hybrid_grid()}, + &matching_submap->high_resolution_hybrid_grid(), + /*intensity_hybrid_grid=*/nullptr}, {&low_resolution_point_cloud_in_tracking, - &matching_submap->low_resolution_hybrid_grid()}}, + &matching_submap->low_resolution_hybrid_grid(), + /*intensity_hybrid_grid=*/nullptr}}, &pose_observation_in_submap, &summary); kCeresScanMatcherCostMetric->Observe(summary.final_cost); const double residual_distance = (pose_observation_in_submap.translation() - diff --git a/cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.cc b/cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.cc index 7eb8fb5629..2c107532ba 100644 --- a/cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.cc +++ b/cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.cc @@ -23,6 +23,7 @@ #include "absl/memory/memory.h" #include "cartographer/common/ceres_solver_options.h" #include "cartographer/mapping/internal/3d/rotation_parameterization.h" +#include "cartographer/mapping/internal/3d/scan_matching/intensity_cost_function_3d.h" #include "cartographer/mapping/internal/3d/scan_matching/occupied_space_cost_function_3d.h" #include "cartographer/mapping/internal/3d/scan_matching/rotation_delta_cost_functor_3d.h" #include "cartographer/mapping/internal/3d/scan_matching/translation_delta_cost_functor_3d.h" @@ -48,6 +49,24 @@ proto::CeresScanMatcherOptions3D CreateCeresScanMatcherOptions3D( options.add_occupied_space_weight( parameter_dictionary->GetDouble(lua_identifier)); } + for (int i = 0;; ++i) { + const std::string lua_identifier = + "intensity_cost_function_options_" + std::to_string(i); + if (!parameter_dictionary->HasKey(lua_identifier)) { + break; + } + const auto intensity_cost_function_options_dictionary = + parameter_dictionary->GetDictionary(lua_identifier); + auto* intensity_cost_function_options = + options.add_intensity_cost_function_options(); + intensity_cost_function_options->set_weight( + intensity_cost_function_options_dictionary->GetDouble("weight")); + intensity_cost_function_options->set_huber_scale( + intensity_cost_function_options_dictionary->GetDouble("huber_scale")); + intensity_cost_function_options->set_intensity_threshold( + intensity_cost_function_options_dictionary->GetDouble( + "intensity_threshold")); + } options.set_translation_weight( parameter_dictionary->GetDouble("translation_weight")); options.set_rotation_weight( @@ -71,10 +90,10 @@ CeresScanMatcher3D::CeresScanMatcher3D( void CeresScanMatcher3D::Match( const Eigen::Vector3d& target_translation, const transform::Rigid3d& initial_pose_estimate, - const std::vector& + const std::vector& point_clouds_and_hybrid_grids, transform::Rigid3d* const pose_estimate, - ceres::Solver::Summary* const summary) { + ceres::Solver::Summary* const summary) const { ceres::Problem problem; optimization::CeresPose ceres_pose( initial_pose_estimate, nullptr /* translation_parameterization */, @@ -91,8 +110,9 @@ void CeresScanMatcher3D::Match( for (size_t i = 0; i != point_clouds_and_hybrid_grids.size(); ++i) { CHECK_GT(options_.occupied_space_weight(i), 0.); const sensor::PointCloud& point_cloud = - *point_clouds_and_hybrid_grids[i].first; - const HybridGrid& hybrid_grid = *point_clouds_and_hybrid_grids[i].second; + *point_clouds_and_hybrid_grids[i].point_cloud; + const HybridGrid& hybrid_grid = + *point_clouds_and_hybrid_grids[i].hybrid_grid; problem.AddResidualBlock( OccupiedSpaceCostFunction3D::CreateAutoDiffCostFunction( options_.occupied_space_weight(i) / @@ -100,7 +120,25 @@ void CeresScanMatcher3D::Match( point_cloud, hybrid_grid), nullptr /* loss function */, ceres_pose.translation(), ceres_pose.rotation()); + if (point_clouds_and_hybrid_grids[i].intensity_hybrid_grid) { + CHECK_GT(options_.intensity_cost_function_options(i).huber_scale(), 0.); + CHECK_GT(options_.intensity_cost_function_options(i).weight(), 0.); + CHECK_GT( + options_.intensity_cost_function_options(i).intensity_threshold(), 0); + const IntensityHybridGrid& intensity_hybrid_grid = + *point_clouds_and_hybrid_grids[i].intensity_hybrid_grid; + problem.AddResidualBlock( + IntensityCostFunction3D::CreateAutoDiffCostFunction( + options_.intensity_cost_function_options(i).weight() / + std::sqrt(static_cast(point_cloud.size())), + options_.intensity_cost_function_options(i).intensity_threshold(), + point_cloud, intensity_hybrid_grid), + new ceres::HuberLoss( + options_.intensity_cost_function_options(i).huber_scale()), + ceres_pose.translation(), ceres_pose.rotation()); + } } + CHECK_GT(options_.translation_weight(), 0.); problem.AddResidualBlock( TranslationDeltaCostFunctor3D::CreateAutoDiffCostFunction( diff --git a/cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.h b/cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.h index ffbaa0cdcc..17c6066b4b 100644 --- a/cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.h +++ b/cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.h @@ -34,8 +34,11 @@ namespace scan_matching { proto::CeresScanMatcherOptions3D CreateCeresScanMatcherOptions3D( common::LuaParameterDictionary* parameter_dictionary); -using PointCloudAndHybridGridPointers = - std::pair; +struct PointCloudAndHybridGridsPointers { + const sensor::PointCloud* point_cloud; + const HybridGrid* hybrid_grid; + const IntensityHybridGrid* intensity_hybrid_grid; // optional +}; // This scan matcher uses Ceres to align scans with an existing map. class CeresScanMatcher3D { @@ -50,10 +53,10 @@ class CeresScanMatcher3D { // 'summary'. void Match(const Eigen::Vector3d& target_translation, const transform::Rigid3d& initial_pose_estimate, - const std::vector& + const std::vector& point_clouds_and_hybrid_grids, transform::Rigid3d* pose_estimate, - ceres::Solver::Summary* summary); + ceres::Solver::Summary* summary) const; private: const proto::CeresScanMatcherOptions3D options_; diff --git a/cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d_test.cc b/cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d_test.cc index 52d40258ab..68a67703e0 100644 --- a/cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d_test.cc +++ b/cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d_test.cc @@ -35,21 +35,35 @@ class CeresScanMatcher3DTest : public ::testing::Test { protected: CeresScanMatcher3DTest() : hybrid_grid_(1.f), + intensity_hybrid_grid_(1.f), expected_pose_( transform::Rigid3d::Translation(Eigen::Vector3d(-1., 0., 0.))) { + std::vector points; + std::vector intensities; for (const Eigen::Vector3f& point : {Eigen::Vector3f(-3.f, 2.f, 0.f), Eigen::Vector3f(-4.f, 2.f, 0.f), Eigen::Vector3f(-5.f, 2.f, 0.f), Eigen::Vector3f(-6.f, 2.f, 0.f), Eigen::Vector3f(-6.f, 3.f, 1.f), Eigen::Vector3f(-6.f, 4.f, 2.f), Eigen::Vector3f(-7.f, 3.f, 1.f)}) { - point_cloud_.push_back({point}); + points.push_back({point}); + intensities.push_back(50); hybrid_grid_.SetProbability( hybrid_grid_.GetCellIndex(expected_pose_.cast() * point), 1.); + intensity_hybrid_grid_.AddIntensity( + intensity_hybrid_grid_.GetCellIndex(expected_pose_.cast() * + point), + 50); } + point_cloud_ = sensor::PointCloud(points, intensities); auto parameter_dictionary = common::MakeDictionary(R"text( return { occupied_space_weight_0 = 1., + intensity_cost_function_options_0 = { + weight = 0.5, + huber_scale = 55, + intensity_threshold = 100, + }, translation_weight = 0.01, rotation_weight = 0.1, only_optimize_yaw = false, @@ -67,14 +81,20 @@ class CeresScanMatcher3DTest : public ::testing::Test { transform::Rigid3d pose; ceres::Solver::Summary summary; - ceres_scan_matcher_->Match(initial_pose.translation(), initial_pose, - {{&point_cloud_, &hybrid_grid_}}, &pose, - &summary); + + IntensityHybridGrid* intensity_hybrid_grid_ptr = + point_cloud_.intensities().empty() ? nullptr : &intensity_hybrid_grid_; + + ceres_scan_matcher_->Match( + initial_pose.translation(), initial_pose, + {{&point_cloud_, &hybrid_grid_, intensity_hybrid_grid_ptr}}, &pose, + &summary); EXPECT_NEAR(0., summary.final_cost, 1e-2) << summary.FullReport(); EXPECT_THAT(pose, transform::IsNearly(expected_pose_, 3e-2)); } HybridGrid hybrid_grid_; + IntensityHybridGrid intensity_hybrid_grid_; transform::Rigid3d expected_pose_; sensor::PointCloud point_cloud_; proto::CeresScanMatcherOptions3D options_; diff --git a/cartographer/mapping/internal/constraints/constraint_builder_3d.cc b/cartographer/mapping/internal/constraints/constraint_builder_3d.cc index ca9ff71682..87b3742b53 100644 --- a/cartographer/mapping/internal/constraints/constraint_builder_3d.cc +++ b/cartographer/mapping/internal/constraints/constraint_builder_3d.cc @@ -267,9 +267,11 @@ void ConstraintBuilder3D::ComputeConstraint( ceres_scan_matcher_.Match(match_result->pose_estimate.translation(), match_result->pose_estimate, {{&constant_data->high_resolution_point_cloud, - submap_scan_matcher.high_resolution_hybrid_grid}, + submap_scan_matcher.high_resolution_hybrid_grid, + /*intensity_hybrid_grid=*/nullptr}, {&constant_data->low_resolution_point_cloud, - submap_scan_matcher.low_resolution_hybrid_grid}}, + submap_scan_matcher.low_resolution_hybrid_grid, + /*intensity_hybrid_grid=*/nullptr}}, &constraint_transform, &unused_summary); constraint->reset(new Constraint{ diff --git a/cartographer/mapping/proto/scan_matching/ceres_scan_matcher_options_3d.proto b/cartographer/mapping/proto/scan_matching/ceres_scan_matcher_options_3d.proto index 8c9030a83a..0f80184159 100644 --- a/cartographer/mapping/proto/scan_matching/ceres_scan_matcher_options_3d.proto +++ b/cartographer/mapping/proto/scan_matching/ceres_scan_matcher_options_3d.proto @@ -18,9 +18,16 @@ package cartographer.mapping.scan_matching.proto; import "cartographer/common/proto/ceres_solver_options.proto"; -// NEXT ID: 7 +message IntensityCostFunctionOptions { + double weight = 1; + double huber_scale = 2; + // Ignore ranges with intensity above this threshold. + float intensity_threshold = 3; +} + +// NEXT ID: 8 message CeresScanMatcherOptions3D { - // Scaling parameters for each cost functor. + // Scaling parameters for each occupied space cost functor. repeated double occupied_space_weight = 1; double translation_weight = 2; double rotation_weight = 3; @@ -31,4 +38,7 @@ message CeresScanMatcherOptions3D { // Configure the Ceres solver. See the Ceres documentation for more // information: https://code.google.com/p/ceres-solver/ common.proto.CeresSolverOptions ceres_solver_options = 6; + + // Scaling parameters for each intensity cost functor. + repeated IntensityCostFunctionOptions intensity_cost_function_options = 7; }