test

libs/locate_system/adjust_ppk_by_locate/adjust_ppk.cpp

@@ -102,9 +102,9 @@ void AdjustPPK::OnReceivedCloud(const PPointCloud &cloud)
     if(0 == cloud.size()){
         return;
     }
-    if(cloud.back().timestamp < 1683806270){
-        return;
-    }
+//    if(cloud.back().timestamp < 1683806270){
+//        return;
+//    }
     double backPTime = cloud.back().timestamp;
     double frontPTime = cloud.front().timestamp;
     vector<IsometryData> periodPose = HandleLocalPPK(backPTime, frontPTime);
@@ -283,21 +283,23 @@ PointCloudInter::Ptr AdjustPPK::GetBaseFrame(
 vector<IsometryData> AdjustPPK::HandleLocalPPK(
         double backPTime, double frontPTime)
 {
+    static bool isFst = true;
     vector<IsometryData> periodPose;
     periodPose.reserve(150);
     for(; ppkIndex_ < localPoseVec_.size(); ppkIndex_++){
         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;
         }
         if(pose.timestamp <= frontPTime){
             continue;
         }
+        if(isFst){
+            mapPose_ = localPoseVec_.front().pose;
+            isFst = false;
+        }else{
+            mapPose_ = mapPose_ * (localPoseVec_.at(ppkIndex_ - 1).pose.inverse() * pose.pose);
+        }
         periodPose.emplace_back(pose);
     }
     return periodPose;