test

libs/locate_system/adjust_ppk_by_locate/adjust_ppk.cpp

@@ -18,8 +18,8 @@ AdjustPPK::AdjustPPK(const string &ieBaseDir, const string &mapDir)
     CalPPKPeriod();
     std::ofstream ofs(ieBaseDir_ + "/CloudPredictPose.txt", ios::out);
     ofs.close();
-    ofs.open(ieBaseDir_ + "/LocalPPK.txt", ios::out);
-    ofs.close();
+//    ofs.open(ieBaseDir_ + "/LocalPPK.txt", ios::out);
+//    ofs.close();
     Matrix4d m;
     /////////////// cpt ///////////////
     m << 0.9999146, 00.0128011, 00.0026317, 00.3097527,
@@ -186,7 +186,7 @@ void AdjustPPK::CalLocalPose()
 {
     localPoseVec_.reserve(rawData_.size());
     LocalCartesian proj(center_.x(), center_.y(), center_.z());
-    std::ofstream ofs(ieBaseDir_ + "/LocalPPK.txt", ios::app);
+    std::ofstream ofs(ieBaseDir_ + "/LocalPPK.txt", ios::out);
     for(const IERawData& rawPPK : rawData_){
         IsometryData localPose;
         proj.Forward(rawPPK.latitude, rawPPK.longitude, rawPPK.height,
@@ -265,6 +265,11 @@ void AdjustPPK::Undisort(
     cloudInfo.filterPoseInfo.increasePose = increasePose;
     cloudInfo.timestamp = rawFrame.timestamp;
     cloudInfo.filterPoseInfo.filterPose = framePose;
+    Quaterniond q(framePose.linear());
+    Vector3d rpy= RotationQuaternionToEulerVector(q);
+    LOG(INFO) << "periodPose.size() " << periodPose.size()
+              << " framePose: " << framePose.translation().transpose()
+              << " rpy: " << rpy.transpose();
 }
 
 bool AdjustPPK::ConfigMap(double timestamp)