fix ppkindex

b80c1fe2 · wangdawei · cfea629c · b80c1fe2
Commit b80c1fe2 authored Jun 02, 2023 by wangdawei
Hide whitespace changes
Inline Side-by-side

Showing with 6 additions and 1 deletion

adjust_ppk.cpp libs/locate_system/adjust_ppk_by_locate/adjust_ppk.cpp +6 -1

No files found.
--- a/libs/locate_system/adjust_ppk_by_locate/adjust_ppk.cpp
+++ b/libs/locate_system/adjust_ppk_by_locate/adjust_ppk.cpp
@@ -313,7 +313,12 @@ vector<IsometryData> AdjustPPK::HandleLocalPPK(
                      mapPose_.translation().z(),
                      calibedBLH.x(), calibedBLH.y(), calibedBLH.z());
        Quaterniond q(mapPose_.linear());
-        Vector3d rpy= RotationQuaternionToEulerVector(q);
+        Vector3d rpy= RotationQuaternionToEulerVector(q) / M_PI * 180;
+        rpy.y() = -rpy.y();
+        rpy.z() = -rpy.z();
+        while(rpy.z() < 0){
+            rpy.z() += 360;
+        }
        auto rawData = rawData_.at(ppkIndex_);
        ofs_ << setprecision(16) << "qx" << ", " << rawData.week << ", " << rawData.seconds
            << ", " << calibedBLH.x() << ", " << calibedBLH.y() << ", " << calibedBLH.z()