fix ppkindex

libs/locate_system/adjust_ppk_by_locate/adjust_ppk.cpp

@@ -296,13 +296,15 @@ vector<IsometryData> AdjustPPK::HandleLocalPPK(
     vector<IsometryData> periodPose;
     periodPose.reserve(150);
     LOG(INFO) << "mapPose_ before HandleLocalPPK: " << mapPose_.translation().transpose();
-    for(; ppkIndex_ < localPoseVec_.size(); ppkIndex_++){
+    for(; ppkIndex_ < localPoseVec_.size(); ){
         const auto &pose = localPoseVec_.at(ppkIndex_);
         if(0 == ppkIndex_){
             mapPose_ = localPoseVec_.front().pose;
-        }else{
-            mapPose_ = mapPose_ * (localPoseVec_.at(ppkIndex_ - 1).pose.inverse() * pose.pose);
         }
+        if(pose.timestamp > backPTime){
+            break;
+        }
+        mapPose_ = mapPose_ * (localPoseVec_.at(ppkIndex_ - 1).pose.inverse() * pose.pose);
         Vector3d calibedBLH;
         proj_.Reverse(mapPose_.translation().x(),
                       mapPose_.translation().y(),
@@ -315,16 +317,13 @@ 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;
+        if(pose.timestamp >= frontPTime){
+            IsometryData calibedPose;
+            calibedPose.pose = mapPose_;
+            calibedPose.timestamp = pose.timestamp;
+            periodPose.emplace_back(calibedPose);
         }
-        IsometryData calibedPose;
-        calibedPose.pose = mapPose_;
-        calibedPose.timestamp = pose.timestamp;
-        periodPose.emplace_back(calibedPose);
+        ppkIndex_++;
     }
     LOG(INFO) << "mapPose_ after HandleLocalPPK: " << mapPose_.translation().transpose();
     LOG(INFO) << setprecision(15) << "periodPose.size(): " << periodPose.size()