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 #ifndef CARTOGRAPHER_MAPPING_INTERNAL_3D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_3D_H_
00021 #define CARTOGRAPHER_MAPPING_INTERNAL_3D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_3D_H_
00022
00023 #include <memory>
00024 #include <vector>
00025
00026 #include "Eigen/Core"
00027 #include "cartographer/common/port.h"
00028 #include "cartographer/mapping/3d/hybrid_grid.h"
00029 #include "cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d.h"
00030 #include "cartographer/mapping/internal/3d/scan_matching/precomputation_grid_3d.h"
00031 #include "cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher.h"
00032 #include "cartographer/mapping/proto/scan_matching/fast_correlative_scan_matcher_options_3d.pb.h"
00033 #include "cartographer/mapping/trajectory_node.h"
00034 #include "cartographer/sensor/point_cloud.h"
00035
00036 namespace cartographer {
00037 namespace mapping {
00038 namespace scan_matching {
00039
00040 proto::FastCorrelativeScanMatcherOptions3D
00041 CreateFastCorrelativeScanMatcherOptions3D(
00042 common::LuaParameterDictionary* parameter_dictionary);
00043
00044 class PrecomputationGridStack3D {
00045 public:
00046 PrecomputationGridStack3D(
00047 const HybridGrid& hybrid_grid,
00048 const proto::FastCorrelativeScanMatcherOptions3D& options);
00049
00050 const PrecomputationGrid3D& Get(int depth) const {
00051 return precomputation_grids_.at(depth);
00052 }
00053
00054 int max_depth() const { return precomputation_grids_.size() - 1; }
00055
00056 private:
00057 std::vector<PrecomputationGrid3D> precomputation_grids_;
00058 };
00059
00060 struct DiscreteScan3D;
00061 struct Candidate3D;
00062
00063
00064 using MatchingFunction = std::function<float(const transform::Rigid3f&)>;
00065
00066 class FastCorrelativeScanMatcher3D {
00067 public:
00068 struct Result {
00069 float score;
00070 transform::Rigid3d pose_estimate;
00071 float rotational_score;
00072 float low_resolution_score;
00073 };
00074
00075 FastCorrelativeScanMatcher3D(
00076 const HybridGrid& hybrid_grid,
00077 const HybridGrid* low_resolution_hybrid_grid,
00078 const Eigen::VectorXf* rotational_scan_matcher_histogram,
00079 const proto::FastCorrelativeScanMatcherOptions3D& options);
00080 ~FastCorrelativeScanMatcher3D();
00081
00082 FastCorrelativeScanMatcher3D(const FastCorrelativeScanMatcher3D&) = delete;
00083 FastCorrelativeScanMatcher3D& operator=(const FastCorrelativeScanMatcher3D&) =
00084 delete;
00085
00086
00087
00088
00089 std::unique_ptr<Result> Match(const transform::Rigid3d& global_node_pose,
00090 const transform::Rigid3d& global_submap_pose,
00091 const TrajectoryNode::Data& constant_data,
00092 float min_score) const;
00093
00094
00095
00096
00097
00098 std::unique_ptr<Result> MatchFullSubmap(
00099 const Eigen::Quaterniond& global_node_rotation,
00100 const Eigen::Quaterniond& global_submap_rotation,
00101 const TrajectoryNode::Data& constant_data, float min_score) const;
00102
00103 private:
00104 struct SearchParameters {
00105 const int linear_xy_window_size;
00106 const int linear_z_window_size;
00107 const double angular_search_window;
00108 const MatchingFunction* const low_resolution_matcher;
00109 };
00110
00111 std::unique_ptr<Result> MatchWithSearchParameters(
00112 const SearchParameters& search_parameters,
00113 const transform::Rigid3f& global_node_pose,
00114 const transform::Rigid3f& global_submap_pose,
00115 const sensor::PointCloud& point_cloud,
00116 const Eigen::VectorXf& rotational_scan_matcher_histogram,
00117 const Eigen::Quaterniond& gravity_alignment, float min_score) const;
00118 DiscreteScan3D DiscretizeScan(const SearchParameters& search_parameters,
00119 const sensor::PointCloud& point_cloud,
00120 const transform::Rigid3f& pose,
00121 float rotational_score) const;
00122 std::vector<DiscreteScan3D> GenerateDiscreteScans(
00123 const SearchParameters& search_parameters,
00124 const sensor::PointCloud& point_cloud,
00125 const Eigen::VectorXf& rotational_scan_matcher_histogram,
00126 const Eigen::Quaterniond& gravity_alignment,
00127 const transform::Rigid3f& global_node_pose,
00128 const transform::Rigid3f& global_submap_pose) const;
00129 std::vector<Candidate3D> GenerateLowestResolutionCandidates(
00130 const SearchParameters& search_parameters, int num_discrete_scans) const;
00131 void ScoreCandidates(int depth,
00132 const std::vector<DiscreteScan3D>& discrete_scans,
00133 std::vector<Candidate3D>* const candidates) const;
00134 std::vector<Candidate3D> ComputeLowestResolutionCandidates(
00135 const SearchParameters& search_parameters,
00136 const std::vector<DiscreteScan3D>& discrete_scans) const;
00137 Candidate3D BranchAndBound(const SearchParameters& search_parameters,
00138 const std::vector<DiscreteScan3D>& discrete_scans,
00139 const std::vector<Candidate3D>& candidates,
00140 int candidate_depth, float min_score) const;
00141 transform::Rigid3f GetPoseFromCandidate(
00142 const std::vector<DiscreteScan3D>& discrete_scans,
00143 const Candidate3D& candidate) const;
00144
00145 const proto::FastCorrelativeScanMatcherOptions3D options_;
00146 const float resolution_;
00147 const int width_in_voxels_;
00148 std::unique_ptr<PrecomputationGridStack3D> precomputation_grid_stack_;
00149 const HybridGrid* const low_resolution_hybrid_grid_;
00150 RotationalScanMatcher rotational_scan_matcher_;
00151 };
00152
00153 }
00154 }
00155 }
00156
00157 #endif // CARTOGRAPHER_MAPPING_INTERNAL_3D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_3D_H_