test

libs/localize/voxel_map_matcher.cc

@@ -60,7 +60,7 @@ inline float fast_match(FastVoxelMatcher *matcher, const VoxelMap *map, const Fa
 #endif
         return result->score;
     }
-    return result->score;
+    return -1.f;
 }
 
 ////////////////////////////////////////////////////////////
@@ -98,7 +98,7 @@ FastVoxelMapMatcher::~FastVoxelMapMatcher() {
 float FastVoxelMapMatcher::InnerMatch(const VectorCloud &vector_cloud, const GuessPose guess_pose,
                                       const Rigid3d &predict_pose, Rigid3d &estimate_pose) {
     float score = fast_match(matcher_, map_, fast_options_, vector_cloud, guess_pose, predict_pose, estimate_pose);
-    return score >= fast_options_.accepted_score? score: -1 * score;
+    return score >= fast_options_.accepted_score? score: -1;
 }
 
 ////////////////////////////////////////////////////////////

libs/locate_system/adjust_ppk_by_locate/adjust_ppk.cpp

@@ -403,8 +403,8 @@ bool AdjustPPK::LocateCloud(const CloudInfoForMatch &cloudInfo)
 //                Quaterniond(cloudInfo.filterPoseInfo.increasePose.linear()));
 //    guessPose.movement = cloudInfo.filterPoseInfo.increasePose.translation().norm();
 //    guessPose.rotation = fabs(increaseRpy.z());
-    guessPose.movement = 0.6;
-    guessPose.rotation = 0.003;
+    guessPose.movement = 2;
+    guessPose.rotation = 0.01;
     guessPose.use_gnss = false;
     if(!currMatcher_->IsMapLoaded(guess.translation())){
         currMatcher_->loadArea(guess.translation());