Add intensity cost function to the Ceres scan matcher. (#1761)

Not yet used.

Signed-off-by: Wolfgang Hess <whess@lyft.com>
master
Wolfgang Hess 2020-10-14 18:04:12 +02:00 committed by GitHub
parent cad3378929
commit 81d34ef185
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
6 changed files with 93 additions and 18 deletions

View File

@ -86,9 +86,11 @@ std::unique_ptr<transform::Rigid3d> LocalTrajectoryBuilder3D::ScanMatch(
(matching_submap->local_pose().inverse() * pose_prediction).translation(), (matching_submap->local_pose().inverse() * pose_prediction).translation(),
initial_ceres_pose, initial_ceres_pose,
{{&high_resolution_point_cloud_in_tracking, {{&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, {&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); &pose_observation_in_submap, &summary);
kCeresScanMatcherCostMetric->Observe(summary.final_cost); kCeresScanMatcherCostMetric->Observe(summary.final_cost);
const double residual_distance = (pose_observation_in_submap.translation() - const double residual_distance = (pose_observation_in_submap.translation() -

View File

@ -23,6 +23,7 @@
#include "absl/memory/memory.h" #include "absl/memory/memory.h"
#include "cartographer/common/ceres_solver_options.h" #include "cartographer/common/ceres_solver_options.h"
#include "cartographer/mapping/internal/3d/rotation_parameterization.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/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/rotation_delta_cost_functor_3d.h"
#include "cartographer/mapping/internal/3d/scan_matching/translation_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( options.add_occupied_space_weight(
parameter_dictionary->GetDouble(lua_identifier)); 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( options.set_translation_weight(
parameter_dictionary->GetDouble("translation_weight")); parameter_dictionary->GetDouble("translation_weight"));
options.set_rotation_weight( options.set_rotation_weight(
@ -71,10 +90,10 @@ CeresScanMatcher3D::CeresScanMatcher3D(
void CeresScanMatcher3D::Match( void CeresScanMatcher3D::Match(
const Eigen::Vector3d& target_translation, const Eigen::Vector3d& target_translation,
const transform::Rigid3d& initial_pose_estimate, const transform::Rigid3d& initial_pose_estimate,
const std::vector<PointCloudAndHybridGridPointers>& const std::vector<PointCloudAndHybridGridsPointers>&
point_clouds_and_hybrid_grids, point_clouds_and_hybrid_grids,
transform::Rigid3d* const pose_estimate, transform::Rigid3d* const pose_estimate,
ceres::Solver::Summary* const summary) { ceres::Solver::Summary* const summary) const {
ceres::Problem problem; ceres::Problem problem;
optimization::CeresPose ceres_pose( optimization::CeresPose ceres_pose(
initial_pose_estimate, nullptr /* translation_parameterization */, 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) { for (size_t i = 0; i != point_clouds_and_hybrid_grids.size(); ++i) {
CHECK_GT(options_.occupied_space_weight(i), 0.); CHECK_GT(options_.occupied_space_weight(i), 0.);
const sensor::PointCloud& point_cloud = const sensor::PointCloud& point_cloud =
*point_clouds_and_hybrid_grids[i].first; *point_clouds_and_hybrid_grids[i].point_cloud;
const HybridGrid& hybrid_grid = *point_clouds_and_hybrid_grids[i].second; const HybridGrid& hybrid_grid =
*point_clouds_and_hybrid_grids[i].hybrid_grid;
problem.AddResidualBlock( problem.AddResidualBlock(
OccupiedSpaceCostFunction3D::CreateAutoDiffCostFunction( OccupiedSpaceCostFunction3D::CreateAutoDiffCostFunction(
options_.occupied_space_weight(i) / options_.occupied_space_weight(i) /
@ -100,7 +120,25 @@ void CeresScanMatcher3D::Match(
point_cloud, hybrid_grid), point_cloud, hybrid_grid),
nullptr /* loss function */, ceres_pose.translation(), nullptr /* loss function */, ceres_pose.translation(),
ceres_pose.rotation()); 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<double>(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.); CHECK_GT(options_.translation_weight(), 0.);
problem.AddResidualBlock( problem.AddResidualBlock(
TranslationDeltaCostFunctor3D::CreateAutoDiffCostFunction( TranslationDeltaCostFunctor3D::CreateAutoDiffCostFunction(

View File

@ -34,8 +34,11 @@ namespace scan_matching {
proto::CeresScanMatcherOptions3D CreateCeresScanMatcherOptions3D( proto::CeresScanMatcherOptions3D CreateCeresScanMatcherOptions3D(
common::LuaParameterDictionary* parameter_dictionary); common::LuaParameterDictionary* parameter_dictionary);
using PointCloudAndHybridGridPointers = struct PointCloudAndHybridGridsPointers {
std::pair<const sensor::PointCloud*, const HybridGrid*>; 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. // This scan matcher uses Ceres to align scans with an existing map.
class CeresScanMatcher3D { class CeresScanMatcher3D {
@ -50,10 +53,10 @@ class CeresScanMatcher3D {
// 'summary'. // 'summary'.
void Match(const Eigen::Vector3d& target_translation, void Match(const Eigen::Vector3d& target_translation,
const transform::Rigid3d& initial_pose_estimate, const transform::Rigid3d& initial_pose_estimate,
const std::vector<PointCloudAndHybridGridPointers>& const std::vector<PointCloudAndHybridGridsPointers>&
point_clouds_and_hybrid_grids, point_clouds_and_hybrid_grids,
transform::Rigid3d* pose_estimate, transform::Rigid3d* pose_estimate,
ceres::Solver::Summary* summary); ceres::Solver::Summary* summary) const;
private: private:
const proto::CeresScanMatcherOptions3D options_; const proto::CeresScanMatcherOptions3D options_;

View File

@ -35,21 +35,35 @@ class CeresScanMatcher3DTest : public ::testing::Test {
protected: protected:
CeresScanMatcher3DTest() CeresScanMatcher3DTest()
: hybrid_grid_(1.f), : hybrid_grid_(1.f),
intensity_hybrid_grid_(1.f),
expected_pose_( expected_pose_(
transform::Rigid3d::Translation(Eigen::Vector3d(-1., 0., 0.))) { transform::Rigid3d::Translation(Eigen::Vector3d(-1., 0., 0.))) {
std::vector<sensor::RangefinderPoint> points;
std::vector<float> intensities;
for (const Eigen::Vector3f& point : for (const Eigen::Vector3f& point :
{Eigen::Vector3f(-3.f, 2.f, 0.f), Eigen::Vector3f(-4.f, 2.f, 0.f), {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(-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(-6.f, 3.f, 1.f), Eigen::Vector3f(-6.f, 4.f, 2.f),
Eigen::Vector3f(-7.f, 3.f, 1.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_.SetProbability(
hybrid_grid_.GetCellIndex(expected_pose_.cast<float>() * point), 1.); hybrid_grid_.GetCellIndex(expected_pose_.cast<float>() * point), 1.);
intensity_hybrid_grid_.AddIntensity(
intensity_hybrid_grid_.GetCellIndex(expected_pose_.cast<float>() *
point),
50);
} }
point_cloud_ = sensor::PointCloud(points, intensities);
auto parameter_dictionary = common::MakeDictionary(R"text( auto parameter_dictionary = common::MakeDictionary(R"text(
return { return {
occupied_space_weight_0 = 1., occupied_space_weight_0 = 1.,
intensity_cost_function_options_0 = {
weight = 0.5,
huber_scale = 55,
intensity_threshold = 100,
},
translation_weight = 0.01, translation_weight = 0.01,
rotation_weight = 0.1, rotation_weight = 0.1,
only_optimize_yaw = false, only_optimize_yaw = false,
@ -67,14 +81,20 @@ class CeresScanMatcher3DTest : public ::testing::Test {
transform::Rigid3d pose; transform::Rigid3d pose;
ceres::Solver::Summary summary; ceres::Solver::Summary summary;
ceres_scan_matcher_->Match(initial_pose.translation(), initial_pose,
{{&point_cloud_, &hybrid_grid_}}, &pose, 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); &summary);
EXPECT_NEAR(0., summary.final_cost, 1e-2) << summary.FullReport(); EXPECT_NEAR(0., summary.final_cost, 1e-2) << summary.FullReport();
EXPECT_THAT(pose, transform::IsNearly(expected_pose_, 3e-2)); EXPECT_THAT(pose, transform::IsNearly(expected_pose_, 3e-2));
} }
HybridGrid hybrid_grid_; HybridGrid hybrid_grid_;
IntensityHybridGrid intensity_hybrid_grid_;
transform::Rigid3d expected_pose_; transform::Rigid3d expected_pose_;
sensor::PointCloud point_cloud_; sensor::PointCloud point_cloud_;
proto::CeresScanMatcherOptions3D options_; proto::CeresScanMatcherOptions3D options_;

View File

@ -267,9 +267,11 @@ void ConstraintBuilder3D::ComputeConstraint(
ceres_scan_matcher_.Match(match_result->pose_estimate.translation(), ceres_scan_matcher_.Match(match_result->pose_estimate.translation(),
match_result->pose_estimate, match_result->pose_estimate,
{{&constant_data->high_resolution_point_cloud, {{&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, {&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_transform, &unused_summary);
constraint->reset(new Constraint{ constraint->reset(new Constraint{

View File

@ -18,9 +18,16 @@ package cartographer.mapping.scan_matching.proto;
import "cartographer/common/proto/ceres_solver_options.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 { message CeresScanMatcherOptions3D {
// Scaling parameters for each cost functor. // Scaling parameters for each occupied space cost functor.
repeated double occupied_space_weight = 1; repeated double occupied_space_weight = 1;
double translation_weight = 2; double translation_weight = 2;
double rotation_weight = 3; double rotation_weight = 3;
@ -31,4 +38,7 @@ message CeresScanMatcherOptions3D {
// Configure the Ceres solver. See the Ceres documentation for more // Configure the Ceres solver. See the Ceres documentation for more
// information: https://code.google.com/p/ceres-solver/ // information: https://code.google.com/p/ceres-solver/
common.proto.CeresSolverOptions ceres_solver_options = 6; common.proto.CeresSolverOptions ceres_solver_options = 6;
// Scaling parameters for each intensity cost functor.
repeated IntensityCostFunctionOptions intensity_cost_function_options = 7;
} }