test

libs/locate_system/adjust_ppk_by_locate/adjust_ppk.cpp

@@ -62,9 +62,7 @@ void AdjustPPK::ReadBag(const string &bagPath)
     {
         const rosbag::MessageInstance &m = *viewIterator;
 //        LOG_EVERY_N(INFO, 1) << setprecision(15) << "m.getTime(): " << m.getTime().toSec();
-        if(m.getTime().toSec() < 1683806268.1){
-            continue;
-        }
+
         if(!m.isType<pandar_msgs::PandarScan>()){
             continue;
         }
@@ -72,7 +70,9 @@ void AdjustPPK::ReadBag(const string &bagPath)
         if(convert_->processScan(m.instantiate<pandar_msgs::PandarScan>(), cloud)){
             OnReceivedCloud(cloud);
         }
-        exit(0);
+//        if(m.getTime().toSec() > 1683806268.1){
+//            exit(0);
+//        }
     }
 }
 
@@ -330,8 +330,6 @@ bool AdjustPPK::LocateCloud(const CloudInfoForMatch &cloudInfo)
     }
     static Isometry3f mapPose = Isometry3f::Identity();
     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>();
@@ -343,25 +341,33 @@ bool AdjustPPK::LocateCloud(const CloudInfoForMatch &cloudInfo)
         Vector3f rpy= RotationQuaternionToEulerVector(q);
         LOG(INFO) << "guessPose: " << mapPose.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.cast<double>();
         auto increaseRpy = RotationQuaternionToEulerVector(
                     Quaterniond(cloudInfo.filterPoseInfo.increasePose.linear()));
         guessPose.movement = cloudInfo.filterPoseInfo.increasePose.translation().norm();
         guessPose.rotation = fabs(increaseRpy.z());
         guessPose.use_gnss = false;
-        guessPose.precision_type = MATCH_PRECISION_FULL;
-        pcl::io::savePCDFileBinary(ieBaseDir_ + "/baseCloud/" + to_string(cloudInfo.timestamp) + ".pcd", *cloudInfo.frame);
-        GnssPoint finalGnssPose;
         if(!currMatcher_->IsMapLoaded(mapPose.translation().cast<double>())){
             currMatcher_->loadArea(mapPose.translation().cast<double>());
         }
-        success = currMatcher_->MatchPointCloud(cloudInfo.frame, guessPose, finalGnssPose, mapPose);
+        guessPose.precision_type = MATCH_PRECISION_FAST;
+        float score = currMatcher_->MatchPointCloud(cloudInfo.frame, guessPose, finalGnssPose, mapPose);
+        if(score < 0.45){
+            return false;
+        }
+        guessPose.map_point = mapPose.cast<double>();
+        guessPose.precision_type = MATCH_PRECISION_FULL;
+        currMatcher_->MatchPointCloud(cloudInfo.frame, guessPose, finalGnssPose, mapPose);
         ofs << setprecision(16) << cloudInfo.timestamp << ", " << mapPose.translation().x()
             << ", " << mapPose.translation().y() << ", " << mapPose.translation().z() << endl;
+        exit(0);
     }
 
     cnt++;
-    return success;
+    return true;
 }
 
 void AdjustPPK::LoadMap()