radio

libs/locate_system/adjust_ppk_by_locate/adjust_ppk.cpp

@@ -100,7 +100,7 @@ void AdjustPPK::ReadBag(const string &bagPath)
 Vector3d AdjustPPK::ReadCenter(const string& taskName)
 {
     Vector3d center;
-    string centerPath = mapDir_ + taskName + "/" + taskName + "meshes.center.txt";
+    string centerPath = mapDir_ + "/" + taskName + "/" + taskName + "meshes.center.txt";
     std::ifstream ifs(centerPath);
     string line;
     vector<string> line_vec;
@@ -530,7 +530,7 @@ bool AdjustPPK::LocateCloud(const CloudInfoForMatch &cloudInfo)
     double avgGridCnt = currMatcher_->AreaAvgGridCnt(guess.translation());
     if(avgGridCnt < 20000){
         LOG(INFO) << "avgGridCnt < 20000! " << "avgGridCnt: " << avgGridCnt;
-        CloseToPPK();
+        CloseToPPK(0.5);
         return false;
     }
     ofs << setprecision(16) << cloudInfo.timestamp << ", " << guess.translation().x()
@@ -542,7 +542,7 @@ bool AdjustPPK::LocateCloud(const CloudInfoForMatch &cloudInfo)
         guessPose.precision_type = MATCH_PRECISION_INIT;
         currMatcher_->MatchPointCloud(cloudInfo.frame, guessPose, finalGnssPose, finalPose);
         mapPose_ = finalPose.cast<double>();
-        CloseToPPK();
+        CloseToPPK(0.1);
         skipCnt_ = 5;
         return true;
     }
@@ -677,10 +677,9 @@ bool AdjustPPK::LoadMesh(const string &streamPath, boost::shared_ptr<VoxelMapMat
     return true;
 }
 
-void AdjustPPK::CloseToPPK()
+void AdjustPPK::CloseToPPK(double radio)
 {
     if(ppkIndex_ > 0){
-        double radio = 0.9;
         const auto &localPPK = localPoseVec_.at(ppkIndex_ - 1);
         mapPose_.translation() =
                 localPPK.pose.translation() * radio +

libs/locate_system/adjust_ppk_by_locate/adjust_ppk.h

@@ -90,7 +90,7 @@ private:
     bool LoadMesh(const string &streamPath,
                   boost::shared_ptr<VoxelMapMatcher> &matcher);
 
-    void CloseToPPK();
+    void CloseToPPK(double radio);
 private:
     string ieBaseDir_;