mapping_2d/scan_matching/ceres_scan_matcher.cc
Go to the documentation of this file.
1 /*
2  * Copyright 2016 The Cartographer Authors
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16 
18 
19 #include <utility>
20 #include <vector>
21 
22 #include "Eigen/Core"
30 #include "ceres/ceres.h"
31 #include "glog/logging.h"
32 
33 namespace cartographer {
34 namespace mapping_2d {
35 namespace scan_matching {
36 
37 proto::CeresScanMatcherOptions CreateCeresScanMatcherOptions(
38  common::LuaParameterDictionary* const parameter_dictionary) {
39  proto::CeresScanMatcherOptions options;
40  options.set_occupied_space_weight(
41  parameter_dictionary->GetDouble("occupied_space_weight"));
42  options.set_translation_weight(
43  parameter_dictionary->GetDouble("translation_weight"));
44  options.set_rotation_weight(
45  parameter_dictionary->GetDouble("rotation_weight"));
46  *options.mutable_ceres_solver_options() =
48  parameter_dictionary->GetDictionary("ceres_solver_options").get());
49  return options;
50 }
51 
53  const proto::CeresScanMatcherOptions& options)
54  : options_(options),
55  ceres_solver_options_(
56  common::CreateCeresSolverOptions(options.ceres_solver_options())) {
57  ceres_solver_options_.linear_solver_type = ceres::DENSE_QR;
58 }
59 
61 
62 void CeresScanMatcher::Match(const transform::Rigid2d& previous_pose,
63  const transform::Rigid2d& initial_pose_estimate,
64  const sensor::PointCloud& point_cloud,
65  const ProbabilityGrid& probability_grid,
66  transform::Rigid2d* const pose_estimate,
67  ceres::Solver::Summary* const summary) const {
68  double ceres_pose_estimate[3] = {initial_pose_estimate.translation().x(),
69  initial_pose_estimate.translation().y(),
70  initial_pose_estimate.rotation().angle()};
71  ceres::Problem problem;
72  CHECK_GT(options_.occupied_space_weight(), 0.);
73  problem.AddResidualBlock(
74  new ceres::AutoDiffCostFunction<OccupiedSpaceCostFunctor, ceres::DYNAMIC,
75  3>(
77  options_.occupied_space_weight() /
78  std::sqrt(static_cast<double>(point_cloud.size())),
79  point_cloud, probability_grid),
80  point_cloud.size()),
81  nullptr, ceres_pose_estimate);
82  CHECK_GT(options_.translation_weight(), 0.);
83  problem.AddResidualBlock(
84  new ceres::AutoDiffCostFunction<TranslationDeltaCostFunctor, 2, 3>(
85  new TranslationDeltaCostFunctor(options_.translation_weight(),
86  previous_pose)),
87  nullptr, ceres_pose_estimate);
88  CHECK_GT(options_.rotation_weight(), 0.);
89  problem.AddResidualBlock(
90  new ceres::AutoDiffCostFunction<RotationDeltaCostFunctor, 1, 3>(
91  new RotationDeltaCostFunctor(options_.rotation_weight(),
92  ceres_pose_estimate[2])),
93  nullptr, ceres_pose_estimate);
94 
95  ceres::Solve(ceres_solver_options_, &problem, summary);
96 
97  *pose_estimate = transform::Rigid2d(
98  {ceres_pose_estimate[0], ceres_pose_estimate[1]}, ceres_pose_estimate[2]);
99 }
100 
101 } // namespace scan_matching
102 } // namespace mapping_2d
103 } // namespace cartographer
const Vector & translation() const
proto::RangeDataInserterOptions options_
Rigid2< double > Rigid2d
proto::CeresScanMatcherOptions CreateCeresScanMatcherOptions(common::LuaParameterDictionary *const parameter_dictionary)
ceres::Solver::Options CreateCeresSolverOptions(const proto::CeresSolverOptions &proto)
std::vector< Eigen::Vector3f > PointCloud
Definition: point_cloud.h:30
void Match(const transform::Rigid2d &previous_pose, const transform::Rigid2d &initial_pose_estimate, const sensor::PointCloud &point_cloud, const ProbabilityGrid &probability_grid, transform::Rigid2d *pose_estimate, ceres::Solver::Summary *summary) const
proto::CeresSolverOptions CreateCeresSolverOptionsProto(common::LuaParameterDictionary *parameter_dictionary)
std::unique_ptr< LuaParameterDictionary > GetDictionary(const string &key)


cartographer
Author(s):
autogenerated on Mon Jun 10 2019 12:51:38