test

libs/locate_system/adjust_ppk_by_locate/adjust_ppk.cpp

@@ -280,28 +280,16 @@ PointCloudInter::Ptr AdjustPPK::GetBaseFrame(
 vector<IsometryData> AdjustPPK::HandleLocalPPK(
         double backPTime, double frontPTime)
 {
-    static bool isFst = true;
     vector<IsometryData> periodPose;
     std::ofstream ofs(ieBaseDir_ + "/gnss_result.txt", ios::out);
     periodPose.reserve(150);
     for(; ppkIndex_ < localPoseVec_.size(); ppkIndex_++){
         const auto &pose = localPoseVec_.at(ppkIndex_);
-        if(pose.timestamp > backPTime){
-            break;
-        }
-        if(pose.timestamp <= frontPTime){
-            continue;
-        }
-        if(isFst){
+        if(0 == ppkIndex_){
             mapPose_ = localPoseVec_.front().pose;
-            isFst = false;
         }else{
             mapPose_ = mapPose_ * (localPoseVec_.at(ppkIndex_ - 1).pose.inverse() * pose.pose);
         }
-        IsometryData calibedPose;
-        calibedPose.pose = mapPose_;
-        calibedPose.timestamp = pose.timestamp;
-        periodPose.emplace_back(calibedPose);
         Vector3d calibedBLH;
         proj_.Reverse(mapPose_.translation().x(),
                       mapPose_.translation().y(),
@@ -314,6 +302,16 @@ vector<IsometryData> AdjustPPK::HandleLocalPPK(
             << ", " << calibedBLH.x() << ", " << calibedBLH.y() << ", " << calibedBLH.z()
             << ", " << rawData.n_velocity << ", " << rawData.e_velocity << ", " << rawData.u_velocity
             << ", " << rpy.x() << ", " << rpy.y() << ", " << rpy.z() << ", " << rawData.status << endl;
+        if(pose.timestamp > backPTime){
+            break;
+        }
+        if(pose.timestamp <= frontPTime){
+            continue;
+        }
+        IsometryData calibedPose;
+        calibedPose.pose = mapPose_;
+        calibedPose.timestamp = pose.timestamp;
+        periodPose.emplace_back(calibedPose);
     }
     return periodPose;
 }