test

libs/locate_system/adjust_ppk_by_locate/adjust_ppk.cpp

@@ -329,11 +329,11 @@ bool AdjustPPK::Undisort(
     PointCloudJ::Ptr undisortedFrame(new PointCloudJ);
     PointCloudJ mappedFrame;
     CloudPreprocess::Instance()->Undisort(
-                            frame, undisortedFrame, &mappedFrame, rawFrame.cloud.back().time, &periodPose, framePose, increasePose);
+                            frame, undisortedFrame, &mappedFrame, periodPose.back().timestamp, &periodPose, framePose, increasePose);
     pcl::io::savePCDFileBinary(ieBaseDir_ + "/baseCloud/" + to_string(rawFrame.timestamp) + "_mapped.pcd", mappedFrame);
     cloudInfo.frame = undisortedFrame;
     cloudInfo.filterPoseInfo.increasePose = increasePose;
-    cloudInfo.timestamp = rawFrame.timestamp;
+    cloudInfo.timestamp = periodPose.back().timestamp;
     cloudInfo.filterPoseInfo.filterPose = framePose;
     Quaterniond q(increasePose.linear());
     Vector3d rpy= RotationQuaternionToEulerVector(q);
@@ -411,7 +411,7 @@ bool AdjustPPK::LocateCloud(const CloudInfoForMatch &cloudInfo)
     }
     double avgGridCnt = currMatcher_->AreaAvgGridCnt(guess.translation());
     LOG(INFO) << "avgGridCnt: " << avgGridCnt;
-    if(avgGridCnt < 30000){
+    if(avgGridCnt < 20000){
         return false;
     }
     ofs << setprecision(16) << cloudInfo.timestamp << ", " << guess.translation().x()
@@ -422,14 +422,14 @@ bool AdjustPPK::LocateCloud(const CloudInfoForMatch &cloudInfo)
     if(score < 0){
         guessPose.precision_type = MATCH_PRECISION_INIT;
         currMatcher_->MatchPointCloud(cloudInfo.frame, guessPose, finalGnssPose, finalPose);
-        mapPose_ = mapPose_ * (guess.inverse() * finalPose.cast<double>());
+        mapPose_ = finalPose.cast<double>();
         return true;
     }
 
     guessPose.map_point = finalPose.cast<double>();
     guessPose.precision_type = MATCH_PRECISION_HIGH;
     currMatcher_->MatchPointCloud(cloudInfo.frame, guessPose, finalGnssPose, finalPose);
-    mapPose_ = mapPose_ * (guess.inverse() * finalPose.cast<double>());
+    mapPose_ = finalPose.cast<double>();
     return true;
 }