Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025 #ifndef CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_2D_H_
00026 #define CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_2D_H_
00027
00028 #include <memory>
00029 #include <vector>
00030
00031 #include "Eigen/Core"
00032 #include "cartographer/common/port.h"
00033 #include "cartographer/mapping/2d/grid_2d.h"
00034 #include "cartographer/mapping/internal/2d/scan_matching/correlative_scan_matcher_2d.h"
00035 #include "cartographer/mapping/proto/scan_matching/fast_correlative_scan_matcher_options_2d.pb.h"
00036 #include "cartographer/sensor/point_cloud.h"
00037
00038 namespace cartographer {
00039 namespace mapping {
00040 namespace scan_matching {
00041
00042 proto::FastCorrelativeScanMatcherOptions2D
00043 CreateFastCorrelativeScanMatcherOptions2D(
00044 common::LuaParameterDictionary* parameter_dictionary);
00045
00046
00047
00048
00049 class PrecomputationGrid2D {
00050 public:
00051 PrecomputationGrid2D(const Grid2D& grid, const CellLimits& limits, int width,
00052 std::vector<float>* reusable_intermediate_grid);
00053
00054
00055
00056 int GetValue(const Eigen::Array2i& xy_index) const {
00057 const Eigen::Array2i local_xy_index = xy_index - offset_;
00058
00059
00060
00061
00062
00063 if (static_cast<unsigned>(local_xy_index.x()) >=
00064 static_cast<unsigned>(wide_limits_.num_x_cells) ||
00065 static_cast<unsigned>(local_xy_index.y()) >=
00066 static_cast<unsigned>(wide_limits_.num_y_cells)) {
00067 return 0;
00068 }
00069 const int stride = wide_limits_.num_x_cells;
00070 return cells_[local_xy_index.x() + local_xy_index.y() * stride];
00071 }
00072
00073
00074 float ToScore(float value) const {
00075 return min_score_ + value * ((max_score_ - min_score_) / 255.f);
00076 }
00077
00078 private:
00079 uint8 ComputeCellValue(float probability) const;
00080
00081
00082
00083 const Eigen::Array2i offset_;
00084
00085
00086 const CellLimits wide_limits_;
00087
00088 const float min_score_;
00089 const float max_score_;
00090
00091
00092 std::vector<uint8> cells_;
00093 };
00094
00095 class PrecomputationGridStack2D {
00096 public:
00097 PrecomputationGridStack2D(
00098 const Grid2D& grid,
00099 const proto::FastCorrelativeScanMatcherOptions2D& options);
00100
00101 const PrecomputationGrid2D& Get(int index) {
00102 return precomputation_grids_[index];
00103 }
00104
00105 int max_depth() const { return precomputation_grids_.size() - 1; }
00106
00107 private:
00108 std::vector<PrecomputationGrid2D> precomputation_grids_;
00109 };
00110
00111
00112 class FastCorrelativeScanMatcher2D {
00113 public:
00114 FastCorrelativeScanMatcher2D(
00115 const Grid2D& grid,
00116 const proto::FastCorrelativeScanMatcherOptions2D& options);
00117 ~FastCorrelativeScanMatcher2D();
00118
00119 FastCorrelativeScanMatcher2D(const FastCorrelativeScanMatcher2D&) = delete;
00120 FastCorrelativeScanMatcher2D& operator=(const FastCorrelativeScanMatcher2D&) =
00121 delete;
00122
00123
00124
00125
00126
00127 bool Match(const transform::Rigid2d& initial_pose_estimate,
00128 const sensor::PointCloud& point_cloud, float min_score,
00129 float* score, transform::Rigid2d* pose_estimate) const;
00130
00131
00132
00133
00134
00135 bool MatchFullSubmap(const sensor::PointCloud& point_cloud, float min_score,
00136 float* score, transform::Rigid2d* pose_estimate) const;
00137
00138 private:
00139
00140
00141
00142 bool MatchWithSearchParameters(
00143 SearchParameters search_parameters,
00144 const transform::Rigid2d& initial_pose_estimate,
00145 const sensor::PointCloud& point_cloud, float min_score, float* score,
00146 transform::Rigid2d* pose_estimate) const;
00147 std::vector<Candidate2D> ComputeLowestResolutionCandidates(
00148 const std::vector<DiscreteScan2D>& discrete_scans,
00149 const SearchParameters& search_parameters) const;
00150 std::vector<Candidate2D> GenerateLowestResolutionCandidates(
00151 const SearchParameters& search_parameters) const;
00152 void ScoreCandidates(const PrecomputationGrid2D& precomputation_grid,
00153 const std::vector<DiscreteScan2D>& discrete_scans,
00154 const SearchParameters& search_parameters,
00155 std::vector<Candidate2D>* const candidates) const;
00156 Candidate2D BranchAndBound(const std::vector<DiscreteScan2D>& discrete_scans,
00157 const SearchParameters& search_parameters,
00158 const std::vector<Candidate2D>& candidates,
00159 int candidate_depth, float min_score) const;
00160
00161 const proto::FastCorrelativeScanMatcherOptions2D options_;
00162 MapLimits limits_;
00163 std::unique_ptr<PrecomputationGridStack2D> precomputation_grid_stack_;
00164 };
00165
00166 }
00167 }
00168 }
00169
00170 #endif // CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_2D_H_