Skip to content

L
localize_for_ppk
Commit 059d88e9 authored by wangdawei's avatar wangdawei
Browse files
Options

test

parent 3751d706
Hide whitespace changes
Inline Side-by-side
Showing with 9 additions and 4 deletions
libs/locate_system/adjust_ppk_by_locate/adjust_ppk.cpp
View file @ 059d88e9
...@@ -383,12 +383,17 @@ bool AdjustPPK::LocateCloud(const CloudInfoForMatch &cloudInfo) ...@@ -383,12 +383,17 @@ bool AdjustPPK::LocateCloud(const CloudInfoForMatch &cloudInfo)
} }
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_INIT; if(0 == cloudCnt_){
guessPose.precision_type = MATCH_PRECISION_INIT;
}else{
guessPose.precision_type = MATCH_PRECISION_FAST;
}
Isometry3f finalPose; Isometry3f finalPose;
float score = currMatcher_->MatchPointCloud(cloudInfo.frame, guessPose, finalGnssPose, finalPose); float score = currMatcher_->MatchPointCloud(cloudInfo.frame, guessPose, finalGnssPose, finalPose);
if(score < 0.45){ mapPose_ = finalPose.cast<double>();
return false; // if(score < 0.45){
} // return false;
// }
guessPose.map_point = mapPose_.cast<double>(); guessPose.map_point = mapPose_.cast<double>();
guessPose.precision_type = MATCH_PRECISION_HIGH; guessPose.precision_type = MATCH_PRECISION_HIGH;
currMatcher_->MatchPointCloud(cloudInfo.frame, guessPose, finalGnssPose, finalPose); currMatcher_->MatchPointCloud(cloudInfo.frame, guessPose, finalGnssPose, finalPose);
......
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