parse test

apps/localize_ppk.cpp

@@ -61,8 +61,9 @@ int main(int argc, char *argv[])
         boost::filesystem::create_directory(FLAGS_log_dir);
     }
     AdjustPPK adjustPPK(FLAGS_ie_base_dir, FLAGS_map_dir);
-    adjustPPK.ConfigConvert(FLAGS_ie_base_dir + "/Pandar128_Firetimes.csv",
+    adjustPPK.ConfigConvert(FLAGS_ie_base_dir + "/P12838CC539738CE52.csv",
                             FLAGS_ie_base_dir + "/Pandar128_Correction.csv");
+    adjustPPK.ReadBag("/home/wdw/Documents/test/test.bag");
     directory_iterator bag_iter(FLAGS_ie_base_dir);
     directory_iterator bag_end_iter;
     for(; bag_iter != bag_end_iter; bag_iter++) {

libs/locate_system/adjust_ppk_by_locate/adjust_ppk.cpp

@@ -81,10 +81,10 @@ void AdjustPPK::ReadBag(const string &bagPath)
             continue;
         }
         PPointCloud cloud;
-        if(cloudCnt_ % 20 != 0){
-            cloudCnt_++;
-            continue;
-        }
+//        if(cloudCnt_ % 20 != 0){
+//            cloudCnt_++;
+//            continue;
+//        }
         if(convert_->processScan(m.instantiate<pandar_msgs::PandarScan>(), cloud)){
             OnReceivedCloud(cloud);
         }
@@ -110,7 +110,7 @@ Vector3d AdjustPPK::ReadCenter(const string& taskName)
         boost::split(line_vec, line, boost::is_any_of(":"), boost::token_compress_on);
         center.z() = stof(line_vec.at(1));
     }
-    LOG(INFO) << "center_: " << center.transpose();
+//    LOG(INFO) << "center_: " << center.transpose();
     return center;
 }
 
@@ -141,6 +141,8 @@ void AdjustPPK::OnReceivedCloud(const PPointCloud &cloud)
 //              << " frontPTime: " << cloud.front().timestamp
 //              << " backPTime: " << cloud.back().timestamp;
 
+    pcl::io::savePCDFileBinary(ieBaseDir_ + "/baseCloud/" + to_string(cloudPacket.timestamp) + "_ori.pcd", cloud);
+    exit(0);
     for(size_t i = 0; i < cloud.size(); i++){
         if(i % 5 != 0){
             continue;
@@ -412,7 +414,7 @@ bool AdjustPPK::Undisort(
     Quaterniond q(increasePose.linear());
     Vector3d rpy= RotationQuaternionToEulerVector(q);
     LOG_EVERY_N(INFO, 100) << setprecision(15)
-              << " periodPose.front(): " << periodPose.front().timestamp
+              << "periodPose.front(): " << periodPose.front().timestamp
               << " periodPose.back(): " << periodPose.back().timestamp
               << " increasePose: " << increasePose.translation().transpose()
               << " rpy: " << rpy.transpose();