mapping_2d/scan_matching/real_time_correlative_scan_matcher.h
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 
17 // This is an implementation of the algorithm described in "Real-Time
18 // Correlative Scan Matching" by Olson.
19 //
20 // The correlative scan matching algorithm is exhaustively evaluating the scan
21 // matching search space. As described by the paper, the basic steps are:
22 //
23 // 1) Evaluate the probability p(z|xi, m) over the entire 3D search window using
24 // the low-resolution table.
25 // 2) Find the best voxel in the low-resolution 3D space that has not already
26 // been considered. Denote this value as Li. If Li < Hbest, terminate: Hbest is
27 // the best scan matching alignment.
28 // 3) Evaluate the search volume inside voxel i using the high resolution table.
29 // Suppose the log-likelihood of this voxel is Hi. Note that Hi <= Li since the
30 // low-resolution map overestimates the log likelihoods. If Hi > Hbest, set
31 // Hbest = Hi.
32 //
33 // This can be made even faster by transforming the scan exactly once over some
34 // discretized range.
35 
36 #ifndef CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_H_
37 #define CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_H_
38 
39 #include <iostream>
40 #include <memory>
41 #include <vector>
42 
43 #include "Eigen/Core"
46 #include "cartographer/mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.pb.h"
47 
48 namespace cartographer {
49 namespace mapping_2d {
50 namespace scan_matching {
51 
52 proto::RealTimeCorrelativeScanMatcherOptions
54  common::LuaParameterDictionary* const parameter_dictionary);
55 
56 // An implementation of "Real-Time Correlative Scan Matching" by Olson.
58  public:
60  const proto::RealTimeCorrelativeScanMatcherOptions& options);
61 
63  delete;
65  const RealTimeCorrelativeScanMatcher&) = delete;
66 
67  // Aligns 'point_cloud' within the 'probability_grid' given an
68  // 'initial_pose_estimate' then updates 'pose_estimate' with the result and
69  // returns the score.
70  double Match(const transform::Rigid2d& initial_pose_estimate,
71  const sensor::PointCloud& point_cloud,
72  const ProbabilityGrid& probability_grid,
73  transform::Rigid2d* pose_estimate) const;
74 
75  // Computes the score for each Candidate in a collection. The cost is computed
76  // as the sum of probabilities, different from the Ceres CostFunctions:
77  // http://ceres-solver.org/modeling.html
78  //
79  // Visible for testing.
80  void ScoreCandidates(const ProbabilityGrid& probability_grid,
81  const std::vector<DiscreteScan>& discrete_scans,
82  const SearchParameters& search_parameters,
83  std::vector<Candidate>* candidates) const;
84 
85  private:
86  std::vector<Candidate> GenerateExhaustiveSearchCandidates(
87  const SearchParameters& search_parameters) const;
88 
89  const proto::RealTimeCorrelativeScanMatcherOptions options_;
90 };
91 
92 } // namespace scan_matching
93 } // namespace mapping_2d
94 } // namespace cartographer
95 
96 #endif // CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_H_
void ScoreCandidates(const ProbabilityGrid &probability_grid, const std::vector< DiscreteScan > &discrete_scans, const SearchParameters &search_parameters, std::vector< Candidate > *candidates) const
RealTimeCorrelativeScanMatcher & operator=(const RealTimeCorrelativeScanMatcher &)=delete
proto::RealTimeCorrelativeScanMatcherOptions CreateRealTimeCorrelativeScanMatcherOptions(common::LuaParameterDictionary *const parameter_dictionary)
std::vector< Eigen::Vector3f > PointCloud
Definition: point_cloud.h:30
double Match(const transform::Rigid2d &initial_pose_estimate, const sensor::PointCloud &point_cloud, const ProbabilityGrid &probability_grid, transform::Rigid2d *pose_estimate) const
std::vector< Candidate > GenerateExhaustiveSearchCandidates(const SearchParameters &search_parameters) const


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