Commit c0255e37 authored by wangdawei's avatar wangdawei

init rp search

parent c426bc6d
...@@ -393,12 +393,12 @@ bool AdjustPPK::LocateCloud(const CloudInfoForMatch &cloudInfo) ...@@ -393,12 +393,12 @@ bool AdjustPPK::LocateCloud(const CloudInfoForMatch &cloudInfo)
guessPose.use_gnss = false; guessPose.use_gnss = false;
if(!currMatcher_->IsMapLoaded(mapPose_.translation())){ if(!currMatcher_->IsMapLoaded(mapPose_.translation())){
currMatcher_->loadArea(mapPose_.translation()); currMatcher_->loadArea(mapPose_.translation());
}
double avgGridCnt = currMatcher_->AreaAvgGridCnt(mapPose_.translation()); double avgGridCnt = currMatcher_->AreaAvgGridCnt(mapPose_.translation());
LOG(INFO) << "avgGridCnt: " << avgGridCnt; LOG(INFO) << "avgGridCnt: " << avgGridCnt;
if(avgGridCnt < 5){ if(avgGridCnt < 5){
return false; return false;
} }
}
ofs << setprecision(16) << cloudInfo.timestamp << ", " << mapPose_.translation().x() ofs << setprecision(16) << cloudInfo.timestamp << ", " << mapPose_.translation().x()
<< ", " << mapPose_.translation().y() << ", " << mapPose_.translation().z() << endl; << ", " << mapPose_.translation().y() << ", " << mapPose_.translation().z() << endl;
guessPose.precision_type = MATCH_PRECISION_FAST; guessPose.precision_type = MATCH_PRECISION_FAST;
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment