test

libs/localize/voxel_map/fast_voxel_matcher.cc

@@ -382,7 +382,9 @@ void FastVoxelMatcher::ScoreCandidates(const VoxelMap &voxel_map,
                 no_map_cnt++;
             }
         }
-        LOG(INFO) << "grid_indexes: " << candidates->front().scan->grid_indexes.size() << ", no_map_cnt: " << no_map_cnt;
+        LOG(INFO) << "search_depth: " << candidates->front().search_depth
+                  << " grid_indexes: " << candidates->front().scan->grid_indexes.size()
+                  << ", no_map_cnt: " << no_map_cnt;
     }
     if (0 != no_map_cnt) {
         float multi = (float)candidates->front().scan->grid_indexes.size() /

libs/localize/voxel_map_matcher.cc

@@ -46,7 +46,6 @@ inline float fast_match(FastVoxelMatcher *matcher, const VoxelMap *map, const Fa
 
     if (result != NULL) {
         estimate_pose = result->pose_estimate.cast<double>();
-        LOG(INFO) << "estimate_pose.translation(): " << estimate_pose.translation().transpose();
 #ifdef VOXEL_MAP_DEBUG
         char buf[200];
         // add by weiji: Trans from estimate to predict

libs/locate_system/adjust_ppk_by_locate/adjust_ppk.cpp

@@ -382,10 +382,12 @@ bool AdjustPPK::LocateCloud(const CloudInfoForMatch &cloudInfo)
     GnssPoint finalGnssPose;
     GuessPose guessPose;
     guessPose.map_point = mapPose_;
-    auto increaseRpy = RotationQuaternionToEulerVector(
-                Quaterniond(cloudInfo.filterPoseInfo.increasePose.linear()));
-    guessPose.movement = cloudInfo.filterPoseInfo.increasePose.translation().norm();
-    guessPose.rotation = fabs(increaseRpy.z());
+//    auto increaseRpy = RotationQuaternionToEulerVector(
+//                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.use_gnss = false;
     if(!currMatcher_->IsMapLoaded(mapPose_.translation())){
         currMatcher_->loadArea(mapPose_.translation());
@@ -484,10 +486,10 @@ bool AdjustPPK::LoadMesh(const string &streamPath, boost::shared_ptr<VoxelMapMat
 
     LOG(INFO) << "mesh to load: " << streamPath;
     VoxelMapMatcherOption option;
-    option.option.filter_resolution = 1.0;
+    option.option.filter_resolution = 1.2;
     option.option.cloud_range = 80;
-    option.fast_option.accepted_score = 0.35;
-    option.fast_option.accepted_low_score = 0.45;
+    option.fast_option.accepted_score = 0.4;
+    option.fast_option.accepted_low_score = 0.5;
     matcher.reset(new VoxelMapMatcher(option));
     matcher->SetMapPath(streamPath);
     return true;

libs/voxel_map/libs/area/mesharea.cpp

@@ -270,7 +270,6 @@ void MeshArea::loadBlockAtArea(const Vector3d &position)
     uint16_t blockCntLoaded = 0;
     Vector2i centerIndex =
             calShiftedIndex(position.x(), position.y());
-    LOG(INFO) << "shiftedIndexToLoad: " << centerIndex.transpose();
     vector<Vector2i> neighbors;
     if(fstLoadMap){
         neighbors = firstLoadNeighbors_;
@@ -324,7 +323,8 @@ void MeshArea::loadBlockAtArea(const Vector3d &position)
             blockContent.blockState = Empty;
         }
     }
-    LOG(INFO) << "blockCntLoaded: " <<blockCntLoaded;
+//    LOG(INFO) << "shiftedIndexToLoad: " << centerIndex.transpose()
+//              << " blockCntLoaded: " <<blockCntLoaded;
 }
 
 }