test

libs/localize/voxel_map/fast_voxel_matcher.cc

@@ -39,8 +39,6 @@ struct Candidate {
     bool operator>(const Candidate& other) const { return score > other.score; }
 
     Rigid3f GetPose(float resolution) const {
-        LOG(INFO) << "scan->pose.translation(): " << scan->pose.translation().transpose()
-                  << " +offset: " << (scan->pose.translation() + resolution * offset.matrix().cast<float>()).transpose();
         return Rigid3f(scan->pose.translation() + resolution * offset.matrix().cast<float>(),
                        scan->pose.rotation());
     }

libs/localize/voxel_map_matcher.cc

@@ -25,6 +25,8 @@ inline float ceres_match(CeresVoxelMatcher *matcher, const VoxelMap *map,
     ceres::Solver::Summary summary;
     Eigen::Vector4d loss = matcher->Match(
         predict_pose, high_cloud, low_cloud, *map, &estimate_pose, &summary, ceres_type);
+    LOG(INFO) << "ceres_match predict_pose.translation(): " << predict_pose.translation().transpose()
+              << " estimate_pose.translation(): " << estimate_pose.translation().transpose();
     return loss[0];
 }
 
@@ -46,6 +48,7 @@ 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