12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc
View File

@ -86,9 +86,11 @@ std::unique_ptr<transform::Rigid3d> 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() -

46
cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.cc
View File

@ -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<PointCloudAndHybridGridPointers>&
const std::vector<PointCloudAndHybridGridsPointers>&
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<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.);
problem.AddResidualBlock(
TranslationDeltaCostFunctor3D::CreateAutoDiffCostFunction(

11
cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.h
View File

@ -34,8 +34,11 @@ namespace scan_matching {
proto::CeresScanMatcherOptions3D CreateCeresScanMatcherOptions3D(
common::LuaParameterDictionary* parameter_dictionary);
using PointCloudAndHybridGridPointers =
std::pair<const sensor::PointCloud*, const HybridGrid*>;
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<PointCloudAndHybridGridPointers>&
const std::vector<PointCloudAndHybridGridsPointers>&
point_clouds_and_hybrid_grids,
transform::Rigid3d* pose_estimate,
ceres::Solver::Summary* summary);
ceres::Solver::Summary* summary) const;
private:
const proto::CeresScanMatcherOptions3D options_;

28
cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d_test.cc
View File

@ -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<sensor::RangefinderPoint> points;
std::vector<float> 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<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(
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_;

6
cartographer/mapping/internal/constraints/constraint_builder_3d.cc
View File

@ -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{

14
cartographer/mapping/proto/scan_matching/ceres_scan_matcher_options_3d.proto
View File

@ -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;
}