test

libs/locate_system/adjust_ppk_by_locate/adjust_ppk.cpp

@@ -108,7 +108,9 @@ void AdjustPPK::OnReceivedCloud(const PPointCloud &cloud)
     }
 
     CloudInfoForMatch cloudInfo;
-    Undisort(cloudPacket, cloudInfo);
+    if(!Undisort(cloudPacket, cloudInfo)){
+        return;
+    }
 
     if(!ConfigMap(cloudInfo.timestamp)){
         return;
@@ -239,7 +241,7 @@ PointCloudInter::Ptr AdjustPPK::GetBaseFrame(
     return frame;
 }
 
-void AdjustPPK::Undisort(
+bool AdjustPPK::Undisort(
         CloudPacket &rawFrame, CloudInfoForMatch &cloudInfo)
 {
     vector<IsometryData> periodPose;
@@ -256,6 +258,9 @@ void AdjustPPK::Undisort(
         }
         periodPose.emplace_back(pose);
     }
+    if(periodPose.size() < 2){
+        return false;
+    }
     PointCloudInter::Ptr frame = GetBaseFrame(rawFrame);
     Isometry3d framePose, increasePose;
     PointCloudJ::Ptr undisortedFrame(new PointCloudJ);
@@ -271,6 +276,7 @@ void AdjustPPK::Undisort(
     LOG(INFO) << "periodPose.size() " << periodPose.size()
               << " framePose: " << framePose.translation().transpose()
               << " rpy: " << rpy.transpose();
+    return true;
 }
 
 bool AdjustPPK::ConfigMap(double timestamp)

libs/locate_system/adjust_ppk_by_locate/adjust_ppk.h

@@ -65,7 +65,7 @@ private:
 
     PointCloudInter::Ptr GetBaseFrame(const CloudPacket &cloudData);
 
-    void Undisort(CloudPacket &baseFrame, CloudInfoForMatch &cloudInfo);
+    bool Undisort(CloudPacket &baseFrame, CloudInfoForMatch &cloudInfo);
 
     bool ConfigMap(double timestamp);