Commit ec46bb00 authored by wangdawei's avatar wangdawei

test

parent 73904574
......@@ -381,13 +381,15 @@ bool AdjustPPK::LocateCloud(const CloudInfoForMatch &cloudInfo)
ofs << setprecision(16) << cloudInfo.timestamp << ", " << mapPose_.translation().x()
<< ", " << mapPose_.translation().y() << ", " << mapPose_.translation().z() << endl;
guessPose.precision_type = MATCH_PRECISION_FAST;
float score = currMatcher_->MatchPointCloud(cloudInfo.frame, guessPose, finalGnssPose, mapPose_.cast<float>());
Isometry3f finalPose;
float score = currMatcher_->MatchPointCloud(cloudInfo.frame, guessPose, finalGnssPose, finalPose);
if(score < 0.45){
return false;
}
guessPose.map_point = mapPose_.cast<double>();
guessPose.precision_type = MATCH_PRECISION_HIGH;
currMatcher_->MatchPointCloud(cloudInfo.frame, guessPose, finalGnssPose, mapPose_.cast<float>());
currMatcher_->MatchPointCloud(cloudInfo.frame, guessPose, finalGnssPose, finalPose);
mapPose_ = finalPose.cast<double>();
return true;
}
......
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