test

libs/locate_system/adjust_ppk_by_locate/adjust_ppk.cpp

@@ -388,16 +388,17 @@ bool AdjustPPK::LocateCloud(const CloudInfoForMatch &cloudInfo)
         return false;
     }
     static std::ofstream ofs(ieBaseDir_ + "/CloudPredictPose.txt", ios::app);
+    auto guess = cloudInfo.filterPoseInfo.filterPose;
 
-    Quaterniond q(mapPose_.linear());
+    Quaterniond q(guess.linear());
     Vector3d rpy= RotationQuaternionToEulerVector(q);
     LOG(INFO) << setprecision(15) << "cloud time: " << cloudInfo.timestamp
-              << " guessPose: " << mapPose_.translation().transpose()
+              << " guessPose: " << guess.translation().transpose()
               << " rpy: " << rpy.transpose();
 //        pcl::io::savePCDFileBinary(ieBaseDir_ + "/baseCloud/" + to_string(cloudInfo.timestamp) + ".pcd", *cloudInfo.frame);
     GnssPoint finalGnssPose;
     GuessPose guessPose;
-    guessPose.map_point = mapPose_;
+    guessPose.map_point = guess;
 //    auto increaseRpy = RotationQuaternionToEulerVector(
 //                Quaterniond(cloudInfo.filterPoseInfo.increasePose.linear()));
 //    guessPose.movement = cloudInfo.filterPoseInfo.increasePose.translation().norm();
@@ -405,30 +406,30 @@ bool AdjustPPK::LocateCloud(const CloudInfoForMatch &cloudInfo)
     guessPose.movement = 0.6;
     guessPose.rotation = 0.003;
     guessPose.use_gnss = false;
-    if(!currMatcher_->IsMapLoaded(mapPose_.translation())){
-        currMatcher_->loadArea(mapPose_.translation());
+    if(!currMatcher_->IsMapLoaded(guess.translation())){
+        currMatcher_->loadArea(guess.translation());
     }
-    double avgGridCnt = currMatcher_->AreaAvgGridCnt(mapPose_.translation());
+    double avgGridCnt = currMatcher_->AreaAvgGridCnt(guess.translation());
     LOG(INFO) << "avgGridCnt: " << avgGridCnt;
     if(avgGridCnt < 20000){
         return false;
     }
-    ofs << setprecision(16) << cloudInfo.timestamp << ", " << mapPose_.translation().x()
-        << ", " << mapPose_.translation().y() << ", " << mapPose_.translation().z() << endl;
+    ofs << setprecision(16) << cloudInfo.timestamp << ", " << guess.translation().x()
+        << ", " << guess.translation().y() << ", " << guess.translation().z() << endl;
     guessPose.precision_type = MATCH_PRECISION_FAST;
     Isometry3f finalPose;
     float score = currMatcher_->MatchPointCloud(cloudInfo.frame, guessPose, finalGnssPose, finalPose);
     if(score < 0){
         guessPose.precision_type = MATCH_PRECISION_INIT;
         currMatcher_->MatchPointCloud(cloudInfo.frame, guessPose, finalGnssPose, finalPose);
-        mapPose_ = finalPose.cast<double>();
+        mapPose_ = mapPose_ * (guess.inverse() * finalPose.cast<double>());
         return true;
     }
 
     guessPose.map_point = finalPose.cast<double>();
     guessPose.precision_type = MATCH_PRECISION_HIGH;
     currMatcher_->MatchPointCloud(cloudInfo.frame, guessPose, finalGnssPose, finalPose);
-    mapPose_ = finalPose.cast<double>();
+    mapPose_ = mapPose_ * (guess.inverse() * finalPose.cast<double>());
     return true;
 }