test

libs/locate_system/adjust_ppk_by_locate/adjust_ppk.cpp

@@ -270,11 +270,6 @@ bool AdjustPPK::Undisort(
     if(periodPose.size() < 2){
         return false;
     }
-    LOG_EVERY_N(INFO, 10) << 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);
@@ -285,11 +280,15 @@ bool AdjustPPK::Undisort(
     cloudInfo.filterPoseInfo.increasePose = increasePose;
     cloudInfo.timestamp = rawFrame.timestamp;
     cloudInfo.filterPoseInfo.filterPose = framePose;
-    Quaterniond q(framePose.linear());
+    Quaterniond q(increasePose.linear());
     Vector3d rpy= RotationQuaternionToEulerVector(q);
-//    LOG(INFO) << "periodPose.size() " << periodPose.size()
-//              << " framePose: " << framePose.translation().transpose()
-//              << " rpy: " << rpy.transpose();
+    LOG_EVERY_N(INFO, 10) << setprecision(15)
+              << "frontPTime: " << frontPTime
+              << " backPTime: " << backPTime
+              << " periodPose.front(): " << periodPose.front().timestamp
+              << " periodPose.back(): " << periodPose.back().timestamp
+              << " increasePose: " << increasePose.translation().transpose()
+              << " rpy: " << rpy.transpose();
     return true;
 }