#include "cloudlocalize.h" #include <pcl/filters/voxel_grid.h> #include <pcl/common/transforms.h> #define ACCEPT_SCORE 0.45 #define MIN_MOVEMENT 0.6 #define MIN_ROTATION 0.003 namespace juefx{ CloudLocalize::Ptr cloudLocalizeInstance; CloudLocalize::CloudLocalize() { VoxelMapMatcherOption option; option.option.filter_resolution = 0.5; option.option.cloud_range = 80; option.fast_option.accepted_score = ACCEPT_SCORE; option.fast_option.accepted_low_score = 0.55; matcher_.reset(new VoxelMapMatcher(option)); matcher_->SetMapPath(Parameter::Instance()->GetMapPath()); } void CloudLocalize::Reset() { cloudLocalizeInstance.reset(); } CloudLocalize::Ptr CloudLocalize::Instance() { if(!cloudLocalizeInstance){ LOG(INFO) << "new cloudLocalizeInstance"; cloudLocalizeInstance.reset(new CloudLocalize()); } return cloudLocalizeInstance; } void CloudLocalize::LoadMap() { matcher_->LoadMap(); } void CloudLocalize::LoadArea(const Vector3d &pose){ auto start = std::chrono::high_resolution_clock::now(); matcher_->loadArea(pose); auto end = std::chrono::high_resolution_clock::now(); auto time_used = std::chrono::duration_cast<std::chrono::duration<double>>(end - start).count() * 1000; LOG(INFO) << "Position " << pose.transpose() << " loaded, time_used " << time_used; } bool CloudLocalize::FindMapPose( const FilterPoseInfo &filterPoseInfo, Isometry3d &mapPose, bool isCurrPos) { if(isCurrPos){ currFilterPose = filterPoseInfo.filterPose; } if(State::Instance()->HasState(INIT_MATCHING)){ if(LastMatchedSuccess){ mapPose = guessFilter2map * filterPoseInfo.filterPose; }else{ if(!FindReferencePose(filterPoseInfo.hasGnss, filterPoseInfo.blh, mapPose)){ return false; } } }else if(State::Instance()->HasState(INIT_MATCHED)){ mapPose = Parameter::Instance()->GetFilter2Map() * filterPoseInfo.filterPose; }else{ return false; } return true; } bool CloudLocalize::IsMapLoaded(const Vector3d &pose) { return matcher_->IsMapLoaded(pose); } bool CloudLocalize::MatchMap( const PointCloudJ::Ptr pcloud, GuessPose &guessPose, MatchResult &result) { auto cloudTime = stod(pcloud->header.frame_id); auto frameRpy = RotationQuaternionToEulerVector( Quaterniond(guessPose.map_point.linear())); DeterminePrecision(guessPose.rotation); guessPose.precision_type = Precision; LOG(INFO) << setprecision(15) << "cloudTime: " << setprecision(15) << cloudTime << " framePose: {" << guessPose.map_point.translation().transpose() << "} frameRpy: {" << frameRpy.transpose() << "} guessPose.movement: {" << guessPose.movement << "} guessPose.rotation: {" << guessPose.rotation << "} guessPose.precision_type: {" << (uint16_t)guessPose.precision_type << "}"; if(guessPose.movement > 5){ LOG(WARNING) << "guessPose.movement too large!"; return false; } if(guessPose.movement < MIN_MOVEMENT){ guessPose.movement = MIN_MOVEMENT; } if(guessPose.rotation < MIN_ROTATION){ guessPose.rotation = MIN_ROTATION; } GnssPoint finalGnssPose; Isometry3f finalPose; float score; string process_name = "4.2 MatchPointCloud by " + to_string((uint16_t)Precision); Timer::Evaluate( [&, this]() { score = matcher_->MatchPointCloud(pcloud, guessPose, finalGnssPose, finalPose); }, process_name); CheckMatchResult(score); if(score < 0){ return false; } if(State::Instance()->HasState(INIT_MATCHING)){ if(InitMatched){ Isometry3d filter2map = finalPose.cast<double>() * currFilterPose.inverse(); Parameter::Instance()->SetFilter2Map(filter2map); State::Instance()->RemoveState(INIT_MATCHING); State::Instance()->AddState(INIT_MATCHED); auto rpy = RotationQuaternionToEulerVector( Quaterniond(filter2map.linear())); std::ofstream ofs(Parameter::Instance()->GetBaseDir() + "/local2map.txt", ios::out); ofs << filter2map.translation().x() << ", " << filter2map.translation().y() << ", " << filter2map.translation().z() << ", " << rpy.x() << ", " << rpy.y() << ", " << rpy.z(); ofs.close(); LOG(INFO) << "-----------------------------------------------------"; LOG(INFO) << "First Match! filter2map: " << filter2map.translation().transpose() << " rotation: " << rpy.transpose(); }else{ guessFilter2map = finalPose.cast<double>() * currFilterPose.inverse(); LOG(INFO) << setprecision(15) << "guessFilter2map update: " << cloudTime << " translation: " << guessFilter2map.translation().transpose() << " rotation: " << RotationQuaternionToEulerVector( Quaterniond(guessFilter2map.linear())).transpose(); } }else{ if(MATCH_PRECISION_HIGH == guessPose.precision_type || MATCH_PRECISION_FULL == guessPose.precision_type){ result.state = HIGH_PRECISION; }else if(MATCH_PRECISION_FAST == guessPose.precision_type){ result.state = FAST_PRECISION; } result.state = MatchPrecisionState(result.state + CheckCloud(pcloud)); } PointCloudJ finalCloud; pcl::transformPointCloud(*pcloud, finalCloud, finalPose.cast<float>()); if(finalCloud.size()){ pcl::io::savePCDFileBinary(Parameter::Instance()->GetBaseDir() + "/result/pcd/" + to_string(cloudTime) + ".pcd", finalCloud); } MatchedPose = finalPose.cast<double>(); result.finalPose = finalPose; return true; } void CloudLocalize::DeterminePrecision(float rotation) { if(State::Instance()->HasState(INIT_MATCHING)){ if(!LastMatchedSuccess){ Precision = MATCH_PRECISION_INIT; }else{ if(InitFastMatched){ Precision = MATCH_PRECISION_HIGH; }else if(Precision < MATCH_PRECISION_FAST){ Precision++; } } }else if(State::Instance()->HasState(INIT_MATCHED)){ if(false == LastMatchedSuccess || 9 == fastPassCnt){ Precision = MATCH_PRECISION_HIGH; if(rotation >= 0.015){ Precision = MATCH_PRECISION_FULL; } }else{ Precision = MATCH_PRECISION_FAST; } } } void CloudLocalize::CheckMatchResult(float score) { if(score < 0){ LastMatchedSuccess = false; fastPassCnt = 0; }else{ LastMatchedSuccess = true; if(MATCH_PRECISION_FAST == Precision){ fastPassCnt++; }else{ fastPassCnt = 0; } if(State::Instance()->HasState(INIT_MATCHING)){ if(InitFastMatched){ InitMatched = true; }else if(fastPassCnt >= 3){ InitFastMatched = true; } } } } bool CloudLocalize::FindReferencePose( bool hasGnss, const Vector3d &blh, Isometry3d &mapPose) { DCHECK(State::Instance()->HasState(INIT_MATCHING)); if(hasGnss){ return PoseGuesser::Instance()->GetMapPoseByBlh(blh, mapPose) < 3; }else{ mapPose = Parameter::Instance()->GetInitPose(); } return true; } int CloudLocalize::CheckCloud(const PointCloudJ::Ptr &cloud) { PointCloudJ::Ptr points_filtered; points_filtered.reset(new PointCloudJ); pcl::VoxelGrid<PointJ> grid; grid.setLeafSize(0.5, 0.5, 0.5); grid.setInputCloud(cloud); grid.filter(*points_filtered); if(points_filtered->size() < 600){ return 1; } return 0; } } // end of namespace