From 5e2a26e1ab1d9b6493efa513bfe013c09a67580f Mon Sep 17 00:00:00 2001
From: feixyz10 <fei-china@139.com>
Date: Thu, 29 Oct 2020 21:12:43 +0800
Subject: [PATCH] build ok

---
 CMakeLists.txt           | 12 ++++++------
 configs/config.yaml      |  3 ++-
 src/odometry/odometry.cc | 27 +++++++++++++--------------
 src/odometry/odometry.h  | 12 ++++++------
 4 files changed, 27 insertions(+), 27 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index ebb5a4d..c160cf8 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -30,8 +30,8 @@ include_directories(SYSTEM
   ${G3LOG_INCLUDE_DIRS}
 )
 
-# link_directories(${PCL_LIBRARY_DIRS})
-# add_definitions(${PCL_DEFINITIONS})
+link_directories(${PCL_LIBRARY_DIRS})
+add_definitions(${PCL_DEFINITIONS})
 
 include_directories(
   src
@@ -51,15 +51,15 @@ add_executable(main main.cc)
 target_link_libraries(main
   ${catkin_LIBRARIES} 
   ${PCL_LIBRARIES}
-  ${CERES_LIBRARIES}
   ${G3LOG_LIBRARIES}
   ${YAML_CPP_LIBRARIES}
   g3log
   common
+  oh_my_loam
   extractor
   visualizer
-  helper
-  solver
   odometry
-  oh_my_loam
+  solver
+  ${CERES_LIBRARIES}
+  helper
 )
diff --git a/configs/config.yaml b/configs/config.yaml
index 566cae3..d333930 100644
--- a/configs/config.yaml
+++ b/configs/config.yaml
@@ -18,4 +18,5 @@ extractor_config:
   downsample_voxel_size: 0.2
 
 odometry_config: 
-  icp_iter_num : 2
\ No newline at end of file
+  icp_iter_num : 2
+  dist_sq_thresh: 100
\ No newline at end of file
diff --git a/src/odometry/odometry.cc b/src/odometry/odometry.cc
index 2672f35..58b8ee2 100644
--- a/src/odometry/odometry.cc
+++ b/src/odometry/odometry.cc
@@ -5,7 +5,6 @@ namespace oh_my_loam {
 
 namespace {
 int kNearbyScanNum = 2;
-double kDistSquareThresh = 25;
 size_t kMinMatchNum = 10;
 }  // namespace
 
@@ -23,15 +22,17 @@ void Odometry::Process(const FeaturePoints& feature, Pose3D* const pose) {
     *pose = pose_curr2world_;
     return;
   }
+  double match_dist_sq_thresh = config_["match_dist_sq_thresh"].as<double>();
   for (int i = 0; i < config_["icp_iter_num"].as<int>(); ++i) {
     std::vector<PointLinePair> pl_pairs;
     std::vector<PointPlanePair> pp_pairs;
-    AssociateCornPoints(*feature.sharp_corner_pts, *corn_pts_pre_, &pl_pairs,
-                        kDistSquareThresh);
-    AssociateSurfPoints(*feature.flat_surf_pts, *surf_pts_pre_, &pp_pairs,
-                        kDistSquareThresh);
+    MatchCornPoints(*feature.sharp_corner_pts, *corn_pts_pre_, &pl_pairs,
+                    match_dist_sq_thresh);
+    MatchSurfPoints(*feature.flat_surf_pts, *surf_pts_pre_, &pp_pairs,
+                    match_dist_sq_thresh);
     if (pl_pairs.size() + pp_pairs.size() < kMinMatchNum) {
-      AWARN << "Too less correspondence";
+      AWARN << "Too less correspondence: num = "
+            << pl_pairs.size() + pp_pairs.size();
     }
     double* q = pose_curr2last_.q().coeffs().data();
     double* p = pose_curr2last_.p().data();
@@ -50,10 +51,9 @@ void Odometry::Process(const FeaturePoints& feature, Pose3D* const pose) {
   UpdatePre(feature);
 }
 
-void Odometry::AssociateCornPoints(const TPointCloud& src,
-                                   const TPointCloud& tgt,
-                                   std::vector<PointLinePair>* const pairs,
-                                   double dist_thresh) const {
+void Odometry::MatchCornPoints(const TPointCloud& src, const TPointCloud& tgt,
+                               std::vector<PointLinePair>* const pairs,
+                               double dist_thresh) const {
   kdtree_corn_pts_->setInputCloud(tgt.makeShared());
   for (const auto& query_pt : src) {
     std::vector<int> indices;
@@ -93,10 +93,9 @@ void Odometry::AssociateCornPoints(const TPointCloud& src,
   }
 }
 
-void Odometry::AssociateSurfPoints(const TPointCloud& src,
-                                   const TPointCloud& tgt,
-                                   std::vector<PointPlanePair>* const pairs,
-                                   double dist_thresh) const {
+void Odometry::MatchSurfPoints(const TPointCloud& src, const TPointCloud& tgt,
+                               std::vector<PointPlanePair>* const pairs,
+                               double dist_thresh) const {
   kdtree_surf_pts_->setInputCloud(tgt.makeShared());
   for (const auto& query_pt : src) {
     std::vector<int> indices;
diff --git a/src/odometry/odometry.h b/src/odometry/odometry.h
index 216e1a5..c196168 100644
--- a/src/odometry/odometry.h
+++ b/src/odometry/odometry.h
@@ -18,13 +18,13 @@ class Odometry {
 
  protected:
   void UpdatePre(const FeaturePoints& feature);
-  void AssociateCornPoints(const TPointCloud& src, const TPointCloud& tgt,
-                           std::vector<PointLinePair>* const pairs,
-                           double dist_thresh) const;
+  void MatchCornPoints(const TPointCloud& src, const TPointCloud& tgt,
+                       std::vector<PointLinePair>* const pairs,
+                       double dist_thresh) const;
 
-  void AssociateSurfPoints(const TPointCloud& src, const TPointCloud& tgt,
-                           std::vector<PointPlanePair>* const pairs,
-                           double dist_thresh) const;
+  void MatchSurfPoints(const TPointCloud& src, const TPointCloud& tgt,
+                       std::vector<PointPlanePair>* const pairs,
+                       double dist_thresh) const;
 
   Pose3D pose_curr2world_;
   Pose3D pose_curr2last_;