test

libs/locate_system/adjust_ppk_by_locate/adjust_ppk.cpp

@@ -72,6 +72,7 @@ void AdjustPPK::ReadBag(const string &bagPath)
         PPointCloud cloud;
         if(convert_->processScan(m.instantiate<pandar_msgs::PandarScan>(), cloud)){
             OnReceivedCloud(cloud);
+            cloudCnt_++;
         }
     }
 }
@@ -101,14 +102,13 @@ void AdjustPPK::OnReceivedCloud(const PPointCloud &cloud)
     if(0 == cloud.size()){
         return;
     }
-    if(cloud.back().timestamp < 1683806268){
+    if(cloud.back().timestamp < 1683806270){
         return;
     }
     double backPTime = cloud.back().timestamp;
     double frontPTime = cloud.front().timestamp;
     vector<IsometryData> periodPose = HandleLocalPPK(backPTime, frontPTime);
-    cloudCnt_++;
-    if(cloudCnt_ % 10 != 1){
+    if(cloudCnt_ % 10 != 0){
         return;
     }
     CloudPacket cloudPacket;
@@ -358,6 +358,7 @@ bool AdjustPPK::ConfigMap(double timestamp)
 
 bool AdjustPPK::LocateCloud(const CloudInfoForMatch &cloudInfo)
 {
+    bool fstMatch = true;
     if(!currMatcher_){
         LOG(INFO) << "no currMatcher_!";
         return false;
@@ -383,8 +384,9 @@ bool AdjustPPK::LocateCloud(const CloudInfoForMatch &cloudInfo)
     }
     ofs << setprecision(16) << cloudInfo.timestamp << ", " << mapPose_.translation().x()
         << ", " << mapPose_.translation().y() << ", " << mapPose_.translation().z() << endl;
-    if(0 == cloudCnt_){
+    if(fstMatch){
         guessPose.precision_type = MATCH_PRECISION_INIT;
+        fstMatch = false;
     }else{
         guessPose.precision_type = MATCH_PRECISION_FAST;
     }
@@ -397,7 +399,7 @@ bool AdjustPPK::LocateCloud(const CloudInfoForMatch &cloudInfo)
     if(score == 0){
         return true;
     }
-    guessPose.map_point = mapPose_.cast<double>();
+    guessPose.map_point = finalPose.cast<double>();
     guessPose.precision_type = MATCH_PRECISION_HIGH;
     currMatcher_->MatchPointCloud(cloudInfo.frame, guessPose, finalGnssPose, finalPose);
     mapPose_ = finalPose.cast<double>();