#include "map_matcher_interface.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" namespace juefx { MapMatcherInterface::~MapMatcherInterface() { if (map_ != NULL) delete map_; } bool MapMatcherInterface::SetMapPath(std::string path_name) { map_ = new VoxelMap(); return map_->SetMapPath(path_name); } bool MapMatcherInterface::LoadMap() { bool ret = false; if (map_ != NULL) { ret = map_->LoadMap(); LOG(INFO) << "Map matcher map loaded, resolution: " << map_->GetResolution(VOXEL_RESOLUTION_1X); } return ret; } bool MapMatcherInterface::loadArea(const Eigen::Vector3d &pose){ if (map_ != NULL) { map_->loadArea(pose); return true; } return false; } bool MapMatcherInterface::IsMapLoaded(const GnssPoint blh_pose) { return map_ != NULL && map_->IsMapLoaded(blh_pose); } bool MapMatcherInterface::IsMapLoaded(const Eigen::Vector3d &pose) { return map_ != NULL && map_->IsMapLoaded(pose); } double MapMatcherInterface::AreaAvgGridCnt(const Eigen::Vector3d& pose){ if(map_ != NULL){ return map_->AreaAvgGridCnt(pose); } return 0; } float MapMatcherInterface::MatchPointCloud(const PointCloudJ::Ptr pcloud, const GuessPose guess_pose, GnssPoint &final_pose, Eigen::Isometry3f &local_pose) { TIMER_START(start); Eigen::Isometry3d pose_in_map; // if (guess_pose.use_gnss) { // const auto &init_pose = guess_pose.gnss_point; // if (map_ == NULL || !map_->GuessPosition(init_pose, pose_in_map)) { // char buf[1024]; // snprintf(buf, 1023, "{ blh: %.8f %.8f %.3f, euler: %.5f %.5f %.5f }", // init_pose.B, init_pose.L, init_pose.H, // init_pose.rpy.x(), init_pose.rpy.y(), init_pose.rpy.z()); // LOG(INFO) << "Match scan with map, init pose: " << buf << ", guess position failed."; // return -1; // } // pose_in_map.linear() = euler_rpy_2_matrix(init_pose.rpy); //#ifdef VOXEL_MAP_DEBUG // char buf1[1024]; // Eigen::Vector3d trans3 = pose_in_map.translation(); // Eigen::Vector3d euler3 = matrix_2_euler_rpy(pose_in_map.rotation()); // snprintf(buf1, 1023, "{ pos: %.3f %.3f %.3f, euler: %.5f %.5f %.5f }", // trans3.x(), trans3.y(), trans3.z(), euler3.x(), euler3.y(), euler3.z()); // std::cout << "GuessPosition() return: " << buf1 << std::endl; //#endif // } else { // pose_in_map = guess_pose.map_point; // } pose_in_map = guess_pose.map_point; VectorCloud vector_cloud; // fix_num_filter(pcloud, vector_cloud, guess_pose.movement, options_.cloud_range); vector_voxel_filter(pcloud, vector_cloud, options_.filter_resolution, options_.cloud_range); #ifdef VOXEL_MAP_DEBUG { TIMER_END(start, seconds); printf("Prepare finished, cost %.3f sec.\n", seconds); } #endif Rigid3d predict_pose(pose_in_map.translation(), Eigen::Quaterniond(pose_in_map.rotation())); Rigid3d estimate_pose; float score = InnerMatch(vector_cloud, guess_pose, predict_pose, estimate_pose); if (score >= 0) { map_->ConfirmPosition(estimate_pose.translation(), final_pose); rigid3d_to_isometry3f(estimate_pose, local_pose); final_pose.rpy = cartographer::transform::RotationQuaternionToEulerVector(estimate_pose.rotation()); char buf[1024]; #ifdef VOXEL_MAP_DEBUG Rigid3d delta_pose = predict_pose.inverse() * estimate_pose; // Trans from estimate to predict auto delta_rpy = cartographer::transform::RotationQuaternionToEulerVector(delta_pose.rotation()); snprintf(buf, 1023, "{ 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("match scan delta: %s\n", buf); #endif snprintf(buf, 1023, "{ pos: %.3f %.3f %.3f, euler: %.5f %.5f %.5f }", estimate_pose.translation().x(), estimate_pose.translation().y(), estimate_pose.translation().z(), final_pose.rpy.x(), final_pose.rpy.y(), final_pose.rpy.z()); TIMER_END(start, seconds); LOG(INFO) << "Match scan with map, type: " << (uint16_t)guess_pose.precision_type << " score: " << score << ", transform: " << buf << ", cost " << seconds << " sec."; } else { final_pose = guess_pose.gnss_point; local_pose = pose_in_map.cast<float>(); TIMER_END(start, seconds); LOG(INFO) << "Match scan with map FAILED, type: " << (uint16_t)guess_pose.precision_type << " score: " << score << ", cost " << seconds << " sec."; } return score; } } // namespace juefx