#include "voxel_map_matcher.h" #include "carto/transform/transform.h" #include "utils/timer.h" #include "utils/util.hpp" #include "utils/voxel_filter.h" #include "voxel_map/voxel_map_interface.h" #include "voxel_map/ceres_voxel_matcher.h" #include "voxel_map/fast_voxel_matcher.h" namespace juefx { /////////////////////////////////////////////////////////// // method inline float ceres_match(CeresVoxelMatcher *matcher, const VoxelMap *map, const VectorCloud &vector_cloud, const Rigid3d &predict_pose, Rigid3d &estimate_pose) { int ceres_type = CERES_MODE_BASE; #ifdef VOXEL_MAP_DEBUG ceres_type |= CERES_MODE_LOSS | CERES_MODE_DEBUG; #endif const VectorCloud &low_cloud = vector_cloud; const VectorCloud &high_cloud = vector_cloud; ceres::Solver::Summary summary; Eigen::Vector4d loss = matcher->Match( predict_pose, high_cloud, low_cloud, *map, &estimate_pose, &summary, ceres_type); return loss[0]; } inline float fast_match(FastVoxelMatcher *matcher, const VoxelMap *map, const FastVoxelMatcherOption& fast_options, const VectorCloud &vector_cloud, const GuessPose guess_pose, const Rigid3d &predict_pose, Rigid3d &estimate_pose) { int mode = FAST_MODE_GROUND; if (guess_pose.precision_type == MATCH_PRECISION_INIT) mode |= FAST_MODE_INIT; else if (guess_pose.precision_type == MATCH_PRECISION_FULL) mode |= FAST_MODE_GUESS; const auto param = matcher->GetMatchParam(*map, guess_pose.movement, guess_pose.rotation, mode); #ifdef VOXEL_MAP_DEBUG printf("fast_match Search param: mode %d, start_depth %u, window xy %u / z %u / yaw %u, yaw step %f\n", param.mode, param.start_depth, param.xy_window_size, param.z_window_size, param.yaw_window_size, param.yaw_step); #endif auto result = matcher->Match(predict_pose.cast<float>(), vector_cloud, *map, param); if (result != NULL) { estimate_pose = result->pose_estimate.cast<double>(); #ifdef VOXEL_MAP_DEBUG char buf[200]; // add by weiji: Trans from estimate to predict // FIXME transform here is diffirent from transform in base frame Rigid3d delta_pose = predict_pose.inverse() * estimate_pose; auto delta_rpy = cartographer::transform::RotationQuaternionToEulerVector(delta_pose.rotation()); snprintf(buf, 200, "{ pos: %.3f %.3f %.3f, euler: %.5f %.5f %.5f }", delta_pose.translation().x(), delta_pose.translation().y(), delta_pose.translation().z(), delta_rpy.x(), delta_rpy.y(), delta_rpy.z()); printf("fast_match return score: %f, low_score: %f, delta: %s\n", result->score, result->low_resolution_score, buf); #endif return result->score; } return -1.f; } //////////////////////////////////////////////////////////// // Ceres voxel map matcher CeresVoxelMapMatcher::CeresVoxelMapMatcher(const MapMatcherOption &option, const CeresVoxelMatcherOption ceres_options) : MapMatcherInterface(option) { ceres_options_ = ceres_options; matcher_ = new CeresVoxelMatcher(ceres_options); } CeresVoxelMapMatcher::~CeresVoxelMapMatcher() { delete matcher_; } float CeresVoxelMapMatcher::InnerMatch(const VectorCloud &vector_cloud, const GuessPose guess_pose, const Rigid3d &predict_pose, Rigid3d &estimate_pose){ return ceres_match(matcher_, map_, vector_cloud, predict_pose, estimate_pose); } //////////////////////////////////////////////////////////// // Fast voxel map matcher FastVoxelMapMatcher::FastVoxelMapMatcher(const MapMatcherOption &option, const FastVoxelMatcherOption fast_options) : MapMatcherInterface(option) { fast_options_ = fast_options; matcher_ = new FastVoxelMatcher(fast_options); } FastVoxelMapMatcher::~FastVoxelMapMatcher() { delete matcher_; } float FastVoxelMapMatcher::InnerMatch(const VectorCloud &vector_cloud, const GuessPose guess_pose, const Rigid3d &predict_pose, Rigid3d &estimate_pose) { float score = fast_match(matcher_, map_, fast_options_, vector_cloud, guess_pose, predict_pose, estimate_pose); return score >= fast_options_.accepted_score? score: -1; } //////////////////////////////////////////////////////////// // Voxel map matcher VoxelMapMatcher::VoxelMapMatcher(const VoxelMapMatcherOption &options) : MapMatcherInterface(options.option) { ceres_options_ = options.ceres_option; fast_options_ = options.fast_option; ceres_matcher_ = new CeresVoxelMatcher(ceres_options_); fast_matcher_ = new FastVoxelMatcher(fast_options_); } VoxelMapMatcher::~VoxelMapMatcher() { delete ceres_matcher_; delete fast_matcher_; } float VoxelMapMatcher::InnerMatch(const VectorCloud &vector_cloud, const GuessPose guess_pose, const Rigid3d &predict_pose, Rigid3d &estimate_pose) { float score = 0; Rigid3d result_pose; switch (guess_pose.precision_type) { case MATCH_PRECISION_FAST: score = fast_match(fast_matcher_, map_, fast_options_, vector_cloud, guess_pose, predict_pose, estimate_pose); return (score >= fast_options_.accepted_score || score < 0) ? score : -score; case MATCH_PRECISION_HIGH: return ceres_match(ceres_matcher_, map_, vector_cloud, predict_pose, estimate_pose); case MATCH_PRECISION_FULL: case MATCH_PRECISION_INIT: score = fast_match(fast_matcher_, map_, fast_options_, vector_cloud, guess_pose, predict_pose, estimate_pose); result_pose = score >= 0 ? estimate_pose : predict_pose; // -1 means failed return ceres_match(ceres_matcher_, map_, vector_cloud, result_pose, estimate_pose); default: return -1.f; } } }