test

libs/locate_system/adjust_ppk_by_locate/adjust_ppk.cpp

@@ -95,9 +95,9 @@ void AdjustPPK::OnReceivedCloud(const PPointCloud &cloud)
     cloudPacket.timestamp = rosTime.toSec();
     cloudPacket.cloud.reserve(cloud.size());
     LOG_EVERY_N(INFO, 100) << setprecision(15) << "OnReceivedCloud: " << cloudPacket.timestamp
-              << " cloud.size()" << cloud.size()
-              << " frontPTime " << cloud.front().timestamp
-              << " backPTime " << cloud.back().timestamp;
+              << " cloud.size(): " << cloud.size()
+              << " frontPTime: " << cloud.front().timestamp
+              << " backPTime: " << cloud.back().timestamp;
 //    pcl::io::savePCDFileBinary(ieBaseDir_ + "/" + to_string(cloudPacket.timestamp) + ".pcd", cloud);
     for(const auto &p : cloud){
         PointInter point;
@@ -254,9 +254,6 @@ bool AdjustPPK::Undisort(
     for(; ppkIndex_ < localPoseVec_.size(); ppkIndex_++){
         const auto &pose = localPoseVec_.at(ppkIndex_);
         if(pose.timestamp > backPTime){
-            LOG_EVERY_N(INFO, 1) << setprecision(15) << "pose.timestamp " << pose.timestamp
-                      << " frontPTime " << frontPTime
-                      << " backPTime " << backPTime;
             break;
         }
         if(pose.timestamp <= frontPTime){
@@ -267,6 +264,11 @@ bool AdjustPPK::Undisort(
     if(periodPose.size() < 2){
         return false;
     }
+    LOG_EVERY_N(INFO, 100) << setprecision(15)
+              << " frontPTime: " << frontPTime
+              << " backPTime: " << backPTime
+              << " periodPose.front(): " << periodPose.front().timestamp
+              << " periodPose.back(): " << periodPose.back().timestamp;
     PointCloudInter::Ptr frame = GetBaseFrame(rawFrame);
     Isometry3d framePose, increasePose;
     PointCloudJ::Ptr undisortedFrame(new PointCloudJ);
@@ -294,6 +296,8 @@ bool AdjustPPK::ConfigMap(double timestamp)
     if(0 == ret){
         periodIndex_++;
         LOG(INFO) << "next period " << periodIndex_;
+    }else{  //no need reset matcher
+        return true;
     }
     if(periodIndex_ >= ppkPeriodVec_.size()){
         LOG(INFO) << "ppkPeriodVec_ reach end";