test

libs/locate_system/adjust_ppk_by_locate/adjust_ppk.cpp

@@ -58,7 +58,6 @@ void AdjustPPK::ReadBag(const string &bagPath)
         const rosbag::MessageInstance &m = *viewIterator;
 //        LOG_EVERY_N(INFO, 1) << setprecision(15) << "m.getTime(): " << m.getTime().toSec();
         if(!m.isType<pandar_msgs::PandarScan>()){
-            viewIterator++;
             continue;
         }
         PPointCloud cloud;
@@ -307,6 +306,10 @@ bool AdjustPPK::LocateCloud(const CloudInfoForMatch &cloudInfo)
     if(cnt % 10 != 0){
         mapPose = mapPose * cloudInfo.filterPoseInfo.increasePose.cast<float>();
     }else{
+        Quaterniond q(mapPose.linear());
+        Vector3d rpy= RotationQuaternionToEulerVector(q);
+        LOG(INFO) << "guessPose: " << mapPose.translation().transpose()
+                  << " rpy: " << rpy.transpose();
         guessPose.map_point = mapPose.cast<double>();
         auto increaseRpy = RotationQuaternionToEulerVector(
                     Quaterniond(cloudInfo.filterPoseInfo.increasePose.linear()));

libs/locate_system/adjust_ppk_by_locate/adjust_ppk.h

@@ -20,9 +20,9 @@ struct PPKPeriod{
     double endTime = -1;
 
     int InPeriod(double timestamp){
-//        LOG(INFO) << setprecision(15) << "timestamp " << timestamp
-//                  << " startTime " << startTime
-//                  << " endTime " << endTime;
+        LOG_EVERY_N(INFO, 99) << setprecision(15) << "timestamp " << timestamp
+                  << " startTime " << startTime
+                  << " endTime " << endTime;
         if(timestamp < startTime){
             return -1;
         }

libs/locate_system/adjust_ppk_by_locate/convert.cc

@@ -205,7 +205,7 @@ Convert::Convert(const std::string &firetimeFile, const std::string &correctionF
 bool Convert::processScan(
         const pandar_msgs::PandarScan::ConstPtr &scanMsg, PPointCloud &pc)
 {
-    LOG(INFO) << "scanMsg->packets.size(): " << scanMsg->packets.size();
+//    LOG(INFO) << "scanMsg->packets.size(): " << scanMsg->packets.size();
     if(0 == scanMsg->packets.size()){
         return false;
     }