test

libs/locate_system/adjust_ppk_by_locate/adjust_ppk.cpp

@@ -34,6 +34,10 @@ AdjustPPK::AdjustPPK(const string &ieBaseDir, const string &mapDir)
 
     calib_.translation() = m.block(0, 3, 3, 1);
     calib_.linear() = m.block(0, 0, 3, 3);
+    Quaterniond q(calib_.linear());
+    Vector3d rpy= RotationQuaternionToEulerVector(q);
+    LOG(INFO) << "calib_: " << calib_.translation().transpose()
+              << " rpy: " << rpy.transpose();
     boost::thread trd(boost::bind(&AdjustPPK::LoadMap, this));
 }
 
@@ -109,7 +113,7 @@ void AdjustPPK::OnReceivedCloud(const PPointCloud &cloud)
     if(cloudPacket.timestamp > 1683806268.156870){
         exit(0);
     }
-    pcl::io::savePCDFileBinary(ieBaseDir_ + "/" + to_string(cloudPacket.timestamp) + ".pcd", cloud);
+    pcl::io::savePCDFileBinary(ieBaseDir_ + "/" + to_string(cloudPacket.timestamp) + "_raw.pcd", cloud);
 //    exit(0);
     for(const auto &p : cloud){
         PointInter point;
@@ -258,7 +262,7 @@ PointCloudInter::Ptr AdjustPPK::GetBaseFrame(
 }
 
 bool AdjustPPK::Undisort(
-        CloudPacket &rawFrame, CloudInfoForMatch &cloudInfo)
+        const CloudPacket &rawFrame, CloudInfoForMatch &cloudInfo)
 {
     vector<IsometryData> periodPose;
     periodPose.reserve(150);

libs/locate_system/adjust_ppk_by_locate/adjust_ppk.h

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