#pragma once #include "../utils/rigid3.h" #include "../utils/vector_point_type.h" #include "voxel_map_interface.h" #include "options.h" #define FAST_MODE_BASE 0 #define FAST_MODE_INIT 1 // init_mode, first match #define FAST_MODE_GUESS 2 // full_mode, before ceres_match #define FAST_MODE_GROUND 4 // count ground score #define FAST_MODE_NOYAW 8 // do not search yaw #define FAST_MODE_DEBUG 16 namespace juefx { struct DiscreteScan; struct Candidate; class FastVoxelMatcher { public: explicit FastVoxelMatcher(const FastVoxelMatcherOption &options); ~FastVoxelMatcher(); FastVoxelMatcher(const FastVoxelMatcher &) = delete; FastVoxelMatcher &operator=(const FastVoxelMatcher &) = delete; struct Result { float score; float low_resolution_score; Rigid3f pose_estimate; }; struct Param { int8_t mode; uint8_t start_depth; uint16_t xy_window_size; uint16_t z_window_size; uint8_t resolution_cm; uint8_t resolution_z_cm; uint8_t yaw_window_size; float yaw_step; float resolution; float resolution_z; }; Param GetMatchParam(const VoxelMap &voxel_map, float movement, float rotation, int mode = FAST_MODE_BASE) const; std::unique_ptr<Result> Match(const Rigid3f &init_pose, const VectorCloud &point_cloud, const VoxelMap &voxel_map, const Param ¶m) const; private: std::vector<DiscreteScan> GenerateDiscreteScans( const Rigid3f &init_pose, const VectorCloud &point_cloud, const VoxelMap &voxel_map, const Param ¶m) const; DiscreteScan CreateDiscreteScan(const int index, const Rigid3f &trans_pose, const VectorCloud &point_cloud, const VoxelMap &voxel_map) const; std::vector<Candidate> GenerateLowestResolutionCandidates( const VoxelMap &voxel_map, const std::vector<DiscreteScan> &discrete_scans, const Param ¶m) const; void ScoreCandidates(const VoxelMap &voxel_map, std::vector<Candidate> *const candidates, const FastVoxelMatcher::Param ¶m, bool low_resolution = false) const; Candidate BranchAndBound(const VoxelMap &voxel_map, const std::vector<Candidate> &candidates, const Param ¶m, float min_score) const; const FastVoxelMatcherOption options_; }; }