Skip to content

Commit e96598c

Browse files
wohewohe
authored
Merge branch 'master' into master
2 parents 7cf62b3 + 81d34ef commit e96598c

File tree

6 files changed

+93
-18
lines changed

6 files changed

+93
-18
lines changed

‎cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -86,9 +86,11 @@ std::unique_ptr<transform::Rigid3d> LocalTrajectoryBuilder3D::ScanMatch(
8686
(matching_submap->local_pose().inverse() * pose_prediction).translation(),
8787
initial_ceres_pose,
8888
{{&high_resolution_point_cloud_in_tracking,
89-
&matching_submap->high_resolution_hybrid_grid()},
89+
&matching_submap->high_resolution_hybrid_grid(),
90+
/*intensity_hybrid_grid=*/nullptr},
9091
{&low_resolution_point_cloud_in_tracking,
91-
&matching_submap->low_resolution_hybrid_grid()}},
92+
&matching_submap->low_resolution_hybrid_grid(),
93+
/*intensity_hybrid_grid=*/nullptr}},
9294
&pose_observation_in_submap, &summary);
9395
kCeresScanMatcherCostMetric->Observe(summary.final_cost);
9496
const double residual_distance = (pose_observation_in_submap.translation() -

‎cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.cc

Lines changed: 42 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -23,6 +23,7 @@
2323
#include "absl/memory/memory.h"
2424
#include "cartographer/common/ceres_solver_options.h"
2525
#include "cartographer/mapping/internal/3d/rotation_parameterization.h"
26+
#include "cartographer/mapping/internal/3d/scan_matching/intensity_cost_function_3d.h"
2627
#include "cartographer/mapping/internal/3d/scan_matching/occupied_space_cost_function_3d.h"
2728
#include "cartographer/mapping/internal/3d/scan_matching/rotation_delta_cost_functor_3d.h"
2829
#include "cartographer/mapping/internal/3d/scan_matching/translation_delta_cost_functor_3d.h"
@@ -48,6 +49,24 @@ proto::CeresScanMatcherOptions3D CreateCeresScanMatcherOptions3D(
4849
options.add_occupied_space_weight(
4950
parameter_dictionary->GetDouble(lua_identifier));
5051
}
52+
for (int i = 0;; ++i) {
53+
const std::string lua_identifier =
54+
"intensity_cost_function_options_" + std::to_string(i);
55+
if (!parameter_dictionary->HasKey(lua_identifier)) {
56+
break;
57+
}
58+
const auto intensity_cost_function_options_dictionary =
59+
parameter_dictionary->GetDictionary(lua_identifier);
60+
auto* intensity_cost_function_options =
61+
options.add_intensity_cost_function_options();
62+
intensity_cost_function_options->set_weight(
63+
intensity_cost_function_options_dictionary->GetDouble("weight"));
64+
intensity_cost_function_options->set_huber_scale(
65+
intensity_cost_function_options_dictionary->GetDouble("huber_scale"));
66+
intensity_cost_function_options->set_intensity_threshold(
67+
intensity_cost_function_options_dictionary->GetDouble(
68+
"intensity_threshold"));
69+
}
5170
options.set_translation_weight(
5271
parameter_dictionary->GetDouble("translation_weight"));
5372
options.set_rotation_weight(
@@ -71,10 +90,10 @@ CeresScanMatcher3D::CeresScanMatcher3D(
7190
void CeresScanMatcher3D::Match(
7291
const Eigen::Vector3d& target_translation,
7392
const transform::Rigid3d& initial_pose_estimate,
74-
const std::vector<PointCloudAndHybridGridPointers>&
93+
const std::vector<PointCloudAndHybridGridsPointers>&
7594
point_clouds_and_hybrid_grids,
7695
transform::Rigid3d* const pose_estimate,
77-
ceres::Solver::Summary* const summary) {
96+
ceres::Solver::Summary* const summary) const {
7897
ceres::Problem problem;
7998
optimization::CeresPose ceres_pose(
8099
initial_pose_estimate, nullptr /* translation_parameterization */,
@@ -91,16 +110,35 @@ void CeresScanMatcher3D::Match(
91110
for (size_t i = 0; i != point_clouds_and_hybrid_grids.size(); ++i) {
92111
CHECK_GT(options_.occupied_space_weight(i), 0.);
93112
const sensor::PointCloud& point_cloud =
94-
*point_clouds_and_hybrid_grids[i].first;
95-
const HybridGrid& hybrid_grid = *point_clouds_and_hybrid_grids[i].second;
113+
*point_clouds_and_hybrid_grids[i].point_cloud;
114+
const HybridGrid& hybrid_grid =
115+
*point_clouds_and_hybrid_grids[i].hybrid_grid;
96116
problem.AddResidualBlock(
97117
OccupiedSpaceCostFunction3D::CreateAutoDiffCostFunction(
98118
options_.occupied_space_weight(i) /
99119
std::sqrt(static_cast<double>(point_cloud.size())),
100120
point_cloud, hybrid_grid),
101121
nullptr /* loss function */, ceres_pose.translation(),
102122
ceres_pose.rotation());
123+
if (point_clouds_and_hybrid_grids[i].intensity_hybrid_grid) {
124+
CHECK_GT(options_.intensity_cost_function_options(i).huber_scale(), 0.);
125+
CHECK_GT(options_.intensity_cost_function_options(i).weight(), 0.);
126+
CHECK_GT(
127+
options_.intensity_cost_function_options(i).intensity_threshold(), 0);
128+
const IntensityHybridGrid& intensity_hybrid_grid =
129+
*point_clouds_and_hybrid_grids[i].intensity_hybrid_grid;
130+
problem.AddResidualBlock(
131+
IntensityCostFunction3D::CreateAutoDiffCostFunction(
132+
options_.intensity_cost_function_options(i).weight() /
133+
std::sqrt(static_cast<double>(point_cloud.size())),
134+
options_.intensity_cost_function_options(i).intensity_threshold(),
135+
point_cloud, intensity_hybrid_grid),
136+
new ceres::HuberLoss(
137+
options_.intensity_cost_function_options(i).huber_scale()),
138+
ceres_pose.translation(), ceres_pose.rotation());
139+
}
103140
}
141+
104142
CHECK_GT(options_.translation_weight(), 0.);
105143
problem.AddResidualBlock(
106144
TranslationDeltaCostFunctor3D::CreateAutoDiffCostFunction(

‎cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.h

Lines changed: 7 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -34,8 +34,11 @@ namespace scan_matching {
3434
proto::CeresScanMatcherOptions3D CreateCeresScanMatcherOptions3D(
3535
common::LuaParameterDictionary* parameter_dictionary);
3636

37-
using PointCloudAndHybridGridPointers =
38-
std::pair<const sensor::PointCloud*, const HybridGrid*>;
37+
struct PointCloudAndHybridGridsPointers {
38+
const sensor::PointCloud* point_cloud;
39+
const HybridGrid* hybrid_grid;
40+
const IntensityHybridGrid* intensity_hybrid_grid; // optional
41+
};
3942

4043
// This scan matcher uses Ceres to align scans with an existing map.
4144
class CeresScanMatcher3D {
@@ -50,10 +53,10 @@ class CeresScanMatcher3D {
5053
// 'summary'.
5154
void Match(const Eigen::Vector3d& target_translation,
5255
const transform::Rigid3d& initial_pose_estimate,
53-
const std::vector<PointCloudAndHybridGridPointers>&
56+
const std::vector<PointCloudAndHybridGridsPointers>&
5457
point_clouds_and_hybrid_grids,
5558
transform::Rigid3d* pose_estimate,
56-
ceres::Solver::Summary* summary);
59+
ceres::Solver::Summary* summary) const;
5760

5861
private:
5962
const proto::CeresScanMatcherOptions3D options_;

‎cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d_test.cc

Lines changed: 24 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -35,21 +35,35 @@ class CeresScanMatcher3DTest : public ::testing::Test {
3535
protected:
3636
CeresScanMatcher3DTest()
3737
: hybrid_grid_(1.f),
38+
intensity_hybrid_grid_(1.f),
3839
expected_pose_(
3940
transform::Rigid3d::Translation(Eigen::Vector3d(-1., 0., 0.))) {
41+
std::vector<sensor::RangefinderPoint> points;
42+
std::vector<float> intensities;
4043
for (const Eigen::Vector3f& point :
4144
{Eigen::Vector3f(-3.f, 2.f, 0.f), Eigen::Vector3f(-4.f, 2.f, 0.f),
4245
Eigen::Vector3f(-5.f, 2.f, 0.f), Eigen::Vector3f(-6.f, 2.f, 0.f),
4346
Eigen::Vector3f(-6.f, 3.f, 1.f), Eigen::Vector3f(-6.f, 4.f, 2.f),
4447
Eigen::Vector3f(-7.f, 3.f, 1.f)}) {
45-
point_cloud_.push_back({point});
48+
points.push_back({point});
49+
intensities.push_back(50);
4650
hybrid_grid_.SetProbability(
4751
hybrid_grid_.GetCellIndex(expected_pose_.cast<float>() * point), 1.);
52+
intensity_hybrid_grid_.AddIntensity(
53+
intensity_hybrid_grid_.GetCellIndex(expected_pose_.cast<float>() *
54+
point),
55+
50);
4856
}
57+
point_cloud_ = sensor::PointCloud(points, intensities);
4958

5059
auto parameter_dictionary = common::MakeDictionary(R"text(
5160
return {
5261
occupied_space_weight_0 = 1.,
62+
intensity_cost_function_options_0 = {
63+
weight = 0.5,
64+
huber_scale = 55,
65+
intensity_threshold = 100,
66+
},
5367
translation_weight = 0.01,
5468
rotation_weight = 0.1,
5569
only_optimize_yaw = false,
@@ -67,14 +81,20 @@ class CeresScanMatcher3DTest : public ::testing::Test {
6781
transform::Rigid3d pose;
6882

6983
ceres::Solver::Summary summary;
70-
ceres_scan_matcher_->Match(initial_pose.translation(), initial_pose,
71-
{{&point_cloud_, &hybrid_grid_}}, &pose,
72-
&summary);
84+
85+
IntensityHybridGrid* intensity_hybrid_grid_ptr =
86+
point_cloud_.intensities().empty() ? nullptr : &intensity_hybrid_grid_;
87+
88+
ceres_scan_matcher_->Match(
89+
initial_pose.translation(), initial_pose,
90+
{{&point_cloud_, &hybrid_grid_, intensity_hybrid_grid_ptr}}, &pose,
91+
&summary);
7392
EXPECT_NEAR(0., summary.final_cost, 1e-2) << summary.FullReport();
7493
EXPECT_THAT(pose, transform::IsNearly(expected_pose_, 3e-2));
7594
}
7695

7796
HybridGrid hybrid_grid_;
97+
IntensityHybridGrid intensity_hybrid_grid_;
7898
transform::Rigid3d expected_pose_;
7999
sensor::PointCloud point_cloud_;
80100
proto::CeresScanMatcherOptions3D options_;

‎cartographer/mapping/internal/constraints/constraint_builder_3d.cc

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -267,9 +267,11 @@ void ConstraintBuilder3D::ComputeConstraint(
267267
ceres_scan_matcher_.Match(match_result->pose_estimate.translation(),
268268
match_result->pose_estimate,
269269
{{&constant_data->high_resolution_point_cloud,
270-
submap_scan_matcher.high_resolution_hybrid_grid},
270+
submap_scan_matcher.high_resolution_hybrid_grid,
271+
/*intensity_hybrid_grid=*/nullptr},
271272
{&constant_data->low_resolution_point_cloud,
272-
submap_scan_matcher.low_resolution_hybrid_grid}},
273+
submap_scan_matcher.low_resolution_hybrid_grid,
274+
/*intensity_hybrid_grid=*/nullptr}},
273275
&constraint_transform, &unused_summary);
274276

275277
constraint->reset(new Constraint{

‎cartographer/mapping/proto/scan_matching/ceres_scan_matcher_options_3d.proto

Lines changed: 12 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -18,9 +18,16 @@ package cartographer.mapping.scan_matching.proto;
1818

1919
import "cartographer/common/proto/ceres_solver_options.proto";
2020

21-
// NEXT ID: 7
21+
message IntensityCostFunctionOptions {
22+
double weight = 1;
23+
double huber_scale = 2;
24+
// Ignore ranges with intensity above this threshold.
25+
float intensity_threshold = 3;
26+
}
27+
28+
// NEXT ID: 8
2229
message CeresScanMatcherOptions3D {
23-
// Scaling parameters for each cost functor.
30+
// Scaling parameters for each occupied space cost functor.
2431
repeated double occupied_space_weight = 1;
2532
double translation_weight = 2;
2633
double rotation_weight = 3;
@@ -31,4 +38,7 @@ message CeresScanMatcherOptions3D {
3138
// Configure the Ceres solver. See the Ceres documentation for more
3239
// information: https://code.google.com/p/ceres-solver/
3340
common.proto.CeresSolverOptions ceres_solver_options = 6;
41+
42+
// Scaling parameters for each intensity cost functor.
43+
repeated IntensityCostFunctionOptions intensity_cost_function_options = 7;
3444
}

0 commit comments

Comments
 (0)