test

libs/locate_system/adjust_ppk_by_locate/adjust_ppk.cpp

@@ -23,8 +23,7 @@ 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();
+
     Matrix4d m;
     /////////////// cpt ///////////////
     m << -0.9999146, 00.0128011, 00.0026317, 00.3097527,
@@ -70,10 +69,14 @@ void AdjustPPK::ReadBag(const string &bagPath)
             continue;
         }
         PPointCloud cloud;
+        if(cloudCnt_ % 10 != 0){
+            cloudCnt_++;
+            continue;
+        }
         if(convert_->processScan(m.instantiate<pandar_msgs::PandarScan>(), cloud)){
             OnReceivedCloud(cloud);
-            cloudCnt_++;
         }
+        cloudCnt_++;
     }
 }
 
@@ -108,9 +111,6 @@ void AdjustPPK::OnReceivedCloud(const PPointCloud &cloud)
     double backPTime = cloud.back().timestamp;
     double frontPTime = cloud.front().timestamp;
     vector<IsometryData> periodPose = HandleLocalPPK(backPTime, frontPTime);
-    if(cloudCnt_ % 10 != 0){
-        return;
-    }
     CloudPacket cloudPacket;
     ros::Time rosTime;
     pcl_conversions::fromPCL(cloud.header.stamp, rosTime);
@@ -221,11 +221,11 @@ void AdjustPPK::LoadMapInfo()
 void AdjustPPK::CalLocalPose()
 {
     localPoseVec_.reserve(rawData_.size());
-    LocalCartesian proj(center_.y(), center_.x(), center_.z());
+    proj_.Reset(center_.y(), center_.x(), center_.z());
     std::ofstream ofs(ieBaseDir_ + "/LocalPPK.txt", ios::out);
     for(const IERawData& rawPPK : rawData_){
         IsometryData localPose;
-        proj.Forward(rawPPK.latitude, rawPPK.longitude, rawPPK.height,
+        proj_.Forward(rawPPK.latitude, rawPPK.longitude, rawPPK.height,
                      localPose.pose.translation().x(),
                      localPose.pose.translation().y(),
                      localPose.pose.translation().z());
@@ -282,6 +282,7 @@ vector<IsometryData> AdjustPPK::HandleLocalPPK(
 {
     static bool isFst = true;
     vector<IsometryData> periodPose;
+    std::ofstream ofs(ieBaseDir_ + "/gnss_result.txt", ios::out);
     periodPose.reserve(150);
     for(; ppkIndex_ < localPoseVec_.size(); ppkIndex_++){
         const auto &pose = localPoseVec_.at(ppkIndex_);
@@ -297,7 +298,19 @@ vector<IsometryData> AdjustPPK::HandleLocalPPK(
         }else{
             mapPose_ = mapPose_ * (localPoseVec_.at(ppkIndex_ - 1).pose.inverse() * pose.pose);
         }
-        periodPose.emplace_back(pose);
+        periodPose.emplace_back(mapPose_);
+        Vector3d calibedBLH;
+        proj_.Reverse(mapPose_.translation().x(),
+                      mapPose_.translation().y(),
+                      mapPose_.translation().z(),
+                      calibedBLH.x(), calibedBLH.y(), calibedBLH.z());
+        Quaterniond q(mapPose_.linear());
+        Vector3d rpy= RotationQuaternionToEulerVector(q);
+        auto rawData = rawData_.at(ppkIndex_);
+        ofs << setprecision(16) << "qx" << ", " << rawData.week << ", " << rawData.seconds
+            << ", " << calibedBLH.x() << ", " << calibedBLH.y() << ", " << calibedBLH.z()
+            << ", " << rawData.n_velocity << ", " << rawData.e_velocity << ", " << rawData.u_velocity
+            << ", " << rpy.x() << ", " << rpy.y() << ", " << rpy.z() << ", " << rawData.status << endl;
     }
     return periodPose;
 }

libs/locate_system/adjust_ppk_by_locate/adjust_ppk.h

@@ -86,6 +86,8 @@ private:
 
     string mapDir_;
 
+    LocalCartesian proj_;
+
     vector<IERawData> rawData_;
 
     vector<PPKPeriod> ppkPeriodVec_;