test

libs/locate_system/adjust_ppk_by_locate/adjust_ppk.cpp

@@ -177,7 +177,9 @@ void AdjustPPK::CalLocalPose()
 {
     localPoseVec_.reserve(rawData_.size());
     LocalCartesian proj(center_.x(), center_.y(), center_.z());
+    std::ofstream ofs(ieBaseDir_ + "/LocalPPK.txt", ios::app);
     for(const IERawData& rawPPK : rawData_){
+//        LOG_EVERY_N(INFO, 1000) << "rawPPK.unix_time: " << rawPPK.unix_time;
         IsometryData localPose;
         proj.Forward(rawPPK.latitude, rawPPK.longitude, rawPPK.height,
                      localPose.pose.translation().x(),
@@ -186,11 +188,10 @@ void AdjustPPK::CalLocalPose()
         localPose.pose.linear() = rawPPK.getOrientation();
         localPose.timestamp = rawPPK.unix_time;
         localPoseVec_.emplace_back(localPose);
-        std::ofstream ofs(ieBaseDir_ + "/LocalPPK.txt", ios::app);
         ofs << setprecision(16) << localPose.timestamp << ", " << localPose.pose.translation().x()
             << ", " << localPose.pose.translation().y() << ", " << localPose.pose.translation().z() << endl;
-        ofs.close();
     }
+    ofs.close();
     LOG(INFO) << "localPoseVec_.size(): " << localPoseVec_.size();
 }
 
@@ -201,7 +202,6 @@ void AdjustPPK::CalPPKPeriod()
     period.meshId = 0;
     period.period.reserve(10000);
     for(const IERawData& rawPPK : rawData_){
-        LOG_EVERY_N(INFO, 10000) << "rawPPK.unix_time: " << rawPPK.unix_time;
         ulong meshId = GetMeshFromPosition(rawPPK.longitude, rawPPK.latitude);
         if(meshId != period.meshId){
             if(period.period.size()){
@@ -277,6 +277,7 @@ bool AdjustPPK::LocateCloud(const CloudInfoForMatch &cloudInfo)
     static uint32_t cnt = 0;
     bool success = true;
     GuessPose guessPose;
+    static std::ofstream ofs(ieBaseDir_ + "/CloudPredictPose.txt", ios::app);
     if(0 == cnt){
         mapPose = cloudInfo.filterPoseInfo.filterPose.cast<float>();
     }
@@ -296,10 +297,8 @@ bool AdjustPPK::LocateCloud(const CloudInfoForMatch &cloudInfo)
             currMatcher_->loadArea(mapPose.translation().cast<double>());
         }
         success = currMatcher_->MatchPointCloud(cloudInfo.frame, guessPose, finalGnssPose, mapPose);
-        std::ofstream ofs(ieBaseDir_ + "/CloudPredictPose.txt", ios::app);
         ofs << setprecision(16) << cloudInfo.timestamp << ", " << mapPose.translation().x()
             << ", " << mapPose.translation().y() << ", " << mapPose.translation().z() << endl;
-        ofs.close();
     }
 
     cnt++;