minor changes

CMakeLists.txt

@@ -1,9 +1,9 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(oh_my_loam)
 
+set(CMAKE_CXX_FLAGS "-std=c++17 -Wall -lpthread")
 set(CMAKE_BUILD_TYPE "Release")
-set(CMAKE_CXX_FLAGS "-std=c++17  -lpthread")
+set(CMAKE_CXX_FLAGS_RELEASE "-O3")
-set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall")
 
 find_package(Ceres REQUIRED)
 find_package(PCL QUIET)

common/macros.h

@@ -4,8 +4,6 @@
 #include <memory>
 #include <mutex>
 
-// #define LOG_TIMESTAMP(timestamp, precision)
-//   std::fixed << std::precision(precision) << timestamp;
 #define LOG_TIMESTAMP(timestamp) \
   std::fixed << std::setprecision(3) << timestamp;
 
@@ -17,7 +15,7 @@
 #define DECLARE_SINGLETON(classname)                                           \
  public:                                                                       \
   static classname *Instance() {                                               \
-    static std::unique_ptr<classname> instance = nullptr;                      \
+    static std::unique_ptr<classname> instance{nullptr};                       \
     if (!instance) {                                                           \
       static std::once_flag flag;                                              \
       std::call_once(flag,                                                     \

common/utils.h

@@ -77,11 +77,11 @@ void RemoveClosedPoints(const pcl::PointCloud<PointT>& cloud_in,
 }
 
 template <typename PointT>
-void VoxelDownSample(const pcl::PointCloud<PointT>& cloud_in,
+void VoxelDownSample(const typename pcl::PointCloud<PointT>::ConstPtr& cloud_in,
                      pcl::PointCloud<PointT>* const cloud_out,
                      double voxel_size) {
   pcl::VoxelGrid<PointT> filter;
-  filter.setInputCloud(cloud_in.makeShared());
+  filter.setInputCloud(cloud_in);
   filter.setLeafSize(voxel_size, voxel_size, voxel_size);
   filter.filter(*cloud_out);
 }

configs/config.yaml

@@ -10,13 +10,14 @@ extractor_config:
   vis: true
   sharp_corner_point_num: 2
   corner_point_num: 20
-  flat_surf_point_num: 4
+  flat_surf_point_num: 3
-  surf_point_num: 20 ## useless currently
+  surf_point_num: 3 ## useless currently
   corner_point_curvature_thres: 0.5
   surf_point_curvature_thres: 0.5
   neighbor_point_dist_thres: 0.05
-  downsample_voxel_size: 0.2
+  downsample_voxel_size: 0.5
 
 odometry_config: 
   icp_iter_num : 2
-  match_dist_thresh: 0.5
+  match_dist_sq_thresh: 1
+  vis: false

src/extractor/base_extractor.cc

@@ -40,18 +40,9 @@ void Extractor::Process(const PointCloud& cloud, FeaturePoints* const feature) {
   }
   double time_assign = timer.toc();
   // store points into feature point clouds according to their type
-  std::ostringstream oss;
-  oss << "Feature point num: ";
   for (const auto& scan : scans) {
-    FeaturePoints scan_feature;
+    GenerateFeaturePoints(scan, feature);
-    GenerateFeaturePoints(scan, &scan_feature);
-    feature->Add(scan_feature);
-    oss << scan.size() << ":" << scan_feature.sharp_corner_pts->size() << ":"
-        << scan_feature.less_sharp_corner_pts->size() << ":"
-        << scan_feature.flat_surf_pts->size() << ":"
-        << scan_feature.less_flat_surf_pts->size() << " ";
   }
-  ADEBUG << oss.str();
   double time_store = timer.toc();
   AINFO << "Time elapsed (ms): scan_split = " << std::setprecision(3)
         << time_split << ", curvature_compute = " << time_curv - time_split
@@ -190,6 +181,7 @@ void Extractor::SetNeighborsPicked(const TCTPointCloud& scan, size_t ix,
 
 void Extractor::GenerateFeaturePoints(const TCTPointCloud& scan,
                                       FeaturePoints* const feature) const {
+  // TPointCloudPtr less_flat_surf_pts(new TPointCloud);
   for (const auto& pt : scan.points) {
     switch (pt.type) {
       case PointType::FLAT:
@@ -214,11 +206,11 @@ void Extractor::GenerateFeaturePoints(const TCTPointCloud& scan,
         break;
     }
   }
-  TPointCloudPtr filtered_less_flat_surf_pts(new TPointCloud);
+  // TPointCloudPtr filtered_less_flat_surf_pts(new TPointCloud);
-  VoxelDownSample(*feature->less_flat_surf_pts,
+  // VoxelDownSample<TPoint>(less_flat_surf_pts,
-                  filtered_less_flat_surf_pts.get(),
+  // filtered_less_flat_surf_pts.get(),
-                  config_["downsample_voxel_size"].as<double>());
+  //                         config_["downsample_voxel_size"].as<double>());
-  feature->less_flat_surf_pts = filtered_less_flat_surf_pts;
+  // *feature->less_flat_surf_pts += *filtered_less_flat_surf_pts;
 }
 
 void Extractor::Visualize(const PointCloud& cloud,

src/extractor/extractor_VLP16.cc

@@ -3,14 +3,15 @@
 namespace oh_my_loam {
 
 int ExtractorVLP16::GetScanID(const Point& pt) const {
-  static int i = 0;
   double omega = std::atan2(pt.z, Distance(pt)) * 180 * M_1_PI + 15.0;
-  if (i++ < 10) {
+  // static int i = 0;
-    ADEBUG << "OMEGA: " << std::atan2(pt.z, Distance(pt)) * 180 * M_1_PI + 15.0
+  // if (i++ < 10) {
-           << " id = " << static_cast<int>(std::round(omega / 2.0) + 0.01)
+  //   ADEBUG << "OMEGA: " << std::atan2(pt.z, Distance(pt)) * 180 * M_1_PI
-           << " z = " << pt.z << " "
+  //   + 15.0
-           << " d = " << Distance(pt) << std::endl;
+  //          << " id = " << static_cast<int>(std::round(omega / 2.0) + 0.01)
-  }
+  //          << " z = " << pt.z << " "
+  //          << " d = " << Distance(pt) << std::endl;
+  // }
   return static_cast<int>(std::round(omega / 2.0) + 1.e-5);
 };
 

src/extractor/feature_points.h

@@ -16,13 +16,6 @@ struct FeaturePoints {
     flat_surf_pts.reset(new TPointCloud);
     less_flat_surf_pts.reset(new TPointCloud);
   }
-
-  void Add(const FeaturePoints& rhs) {
-    *sharp_corner_pts += *rhs.sharp_corner_pts;
-    *less_sharp_corner_pts += *rhs.less_sharp_corner_pts;
-    *flat_surf_pts += *rhs.flat_surf_pts;
-    *less_flat_surf_pts += *rhs.less_flat_surf_pts;
-  }
 };
 
 }  // namespace oh_my_loam

src/helper/helper.cc

@@ -2,8 +2,6 @@
 
 namespace oh_my_loam {
 
-float GetTime(const TPoint& pt) { return pt.time - GetScanId(pt); }
+//
 
-int GetScanId(const TPoint& pt) { return static_cast<int>(pt.time); }
+}  // namespace oh_my_loam
-
-}  // oh_my_loam

src/helper/helper.h

@@ -4,9 +4,11 @@
 
 namespace oh_my_loam {
 
-float GetTime(const TPoint& pt);
+inline float GetTime(const TPoint& pt) {
+  return pt.time - static_cast<int>(pt.time);
+}
 
-int GetScanId(const TPoint& pt);
+inline int GetScanId(const TPoint& pt) { return static_cast<int>(pt.time); }
 
 struct PointLinePair {
   TPoint pt;
@@ -36,4 +38,4 @@ struct PointPlanePair {
       : pt(pt), plane(pt1, pt2, pt3) {}
 };
 
-}  // oh_my_loam
+}  // namespace oh_my_loam

src/odometry/odometry.cc

@@ -1,4 +1,5 @@
 #include "odometry.h"
+
 #include "solver/solver.h"
 
 namespace oh_my_loam {
@@ -20,21 +21,25 @@ void Odometry::Process(const FeaturePoints& feature, Pose3D* const pose) {
     is_initialized = true;
     UpdatePre(feature);
     *pose = pose_curr2world_;
-    AINFO << "Odometry initialized....";
+    AWARN << "Odometry initialized....";
     return;
   }
-  double match_dist_thresh = config_["match_dist_thresh"].as<double>();
+  TicToc timer_whole;
+  double match_dist_sq_thresh = config_["match_dist_sq_thresh"].as<double>();
   AINFO << "Pose before: " << pose_curr2world_.ToString();
   for (int i = 0; i < config_["icp_iter_num"].as<int>(); ++i) {
+    TicToc timer;
     std::vector<PointLinePair> pl_pairs;
     std::vector<PointPlanePair> pp_pairs;
     MatchCornPoints(*feature.sharp_corner_pts, *corn_pts_pre_, &pl_pairs,
-                    match_dist_thresh);
+                    match_dist_sq_thresh);
+    double time_pl_match = timer.toc();
     AINFO_IF(i == 0) << "PL mathch: src size = "
                      << feature.sharp_corner_pts->size()
                      << ", tgt size = " << corn_pts_pre_->size();
     MatchSurfPoints(*feature.flat_surf_pts, *surf_pts_pre_, &pp_pairs,
-                    match_dist_thresh);
+                    match_dist_sq_thresh);
+    double timer_pp_match = timer.toc();
     AINFO_IF(i == 0) << "PP mathch: src size = "
                      << feature.flat_surf_pts->size()
                      << ", tgt size = " << surf_pts_pre_->size();
@@ -44,8 +49,10 @@ void Odometry::Process(const FeaturePoints& feature, Pose3D* const pose) {
       AWARN << "Too less correspondence: num = "
             << pl_pairs.size() + pp_pairs.size();
     }
-    double* q = pose_curr2last_.q().coeffs().data();
+    Eigen::Vector4d q_vec = pose_curr2last_.q().coeffs();
-    double* p = pose_curr2last_.p().data();
+    Eigen::Vector3d p_vec = pose_curr2last_.p();
+    double* q = q_vec.data();
+    double* p = p_vec.data();
     PoseSolver solver(q, p);
     for (const auto& pair : pl_pairs) {
       solver.AddPointLinePair(pair, GetTime(pair.pt));
@@ -55,27 +62,30 @@ void Odometry::Process(const FeaturePoints& feature, Pose3D* const pose) {
     }
     solver.Solve();
     pose_curr2last_ = Pose3D(q, p);
+    double time_solve = timer.toc();
+    AINFO << "Time elapsed (ms), pl_match = " << time_pl_match
+          << ", pp_match = " << timer_pp_match - time_pl_match
+          << ", solve = " << time_solve - timer_pp_match;
   }
   pose_curr2world_ = pose_curr2world_ * pose_curr2last_;
   *pose = pose_curr2world_;
   AWARN << "Pose increase: " << pose_curr2last_.ToString();
   AWARN << "Pose after: " << pose_curr2world_.ToString();
   UpdatePre(feature);
+  AINFO << "Time elapsed (ms): whole odometry = " << timer_whole.toc();
 }
 
 void Odometry::MatchCornPoints(const TPointCloud& src, const TPointCloud& tgt,
                                std::vector<PointLinePair>* const pairs,
-                               double dist_thresh) const {
+                               double dist_sq_thresh) const {
-  kdtree_corn_pts_->setInputCloud(tgt.makeShared());
-
   for (const auto& query_pt : src) {
     std::vector<int> indices;
     std::vector<float> dists;
     kdtree_corn_pts_->nearestKSearch(query_pt, 1, indices, dists);
-    if (dists[0] >= dist_thresh) continue;
+    if (dists[0] >= dist_sq_thresh) continue;
     TPoint pt1 = tgt.points[indices[0]];
     int pt2_idx = -1;
-    double min_dist_pt2_squre = dist_thresh * dist_thresh;
+    double min_dist_pt2_squre = dist_sq_thresh;
     int query_pt_scan_id = GetScanId(query_pt);
     for (int i = indices[0] + 1; i < static_cast<int>(tgt.size()); ++i) {
       const auto pt = tgt.points[i];
@@ -108,25 +118,23 @@ void Odometry::MatchCornPoints(const TPointCloud& src, const TPointCloud& tgt,
 
 void Odometry::MatchSurfPoints(const TPointCloud& src, const TPointCloud& tgt,
                                std::vector<PointPlanePair>* const pairs,
-                               double dist_thresh) const {
+                               double dist_sq_thresh) const {
-  kdtree_surf_pts_->setInputCloud(tgt.makeShared());
-  AINFO << "Point to plane mathch: src size = " << src.size()
-        << ", tgt size = " << tgt.size();
   for (const auto& query_pt : src) {
     std::vector<int> indices;
     std::vector<float> dists;
     kdtree_surf_pts_->nearestKSearch(query_pt, 1, indices, dists);
-    if (dists[0] >= dist_thresh) continue;
+    if (dists[0] >= dist_sq_thresh) continue;
     TPoint pt1 = tgt.points[indices[0]];
     int pt2_idx = -1, pt3_idx = -1;
-    double min_dist_pt2_squre = dist_thresh * dist_thresh;
+    double min_dist_pt2_squre = dist_sq_thresh;
-    double min_dist_pt3_squre = dist_thresh * dist_thresh;
+    double min_dist_pt3_squre = dist_sq_thresh;
     int query_pt_scan_id = GetScanId(query_pt);
     for (int i = indices[0] + 1; i < static_cast<int>(tgt.size()); ++i) {
       const auto pt = tgt.points[i];
       int scan_id = GetScanId(pt);
       if (scan_id > query_pt_scan_id + kNearbyScanNum) break;
       double dist_squre = DistanceSqure(query_pt, pt);
+      // AWARN_IF(scan_id != 0) << "SA" << query_pt_scan_id << ", " << scan_id;
       if (scan_id == query_pt_scan_id && dist_squre < min_dist_pt2_squre) {
         pt2_idx = i;
         min_dist_pt2_squre = dist_squre;
@@ -140,9 +148,9 @@ void Odometry::MatchSurfPoints(const TPointCloud& src, const TPointCloud& tgt,
     for (int i = indices[0] - 1; i >= 0; --i) {
       const auto pt = tgt.points[i];
       int scan_id = GetScanId(pt);
-      if (scan_id >= query_pt_scan_id) continue;
       if (scan_id < query_pt_scan_id - kNearbyScanNum) break;
       double dist_squre = DistanceSqure(query_pt, pt);
+      // AWARN_IF(scan_id != 0) << "SD" << query_pt_scan_id << ", " << scan_id;
       if (scan_id == query_pt_scan_id && dist_squre < min_dist_pt2_squre) {
         pt2_idx = i;
         min_dist_pt2_squre = dist_squre;
@@ -163,6 +171,8 @@ void Odometry::MatchSurfPoints(const TPointCloud& src, const TPointCloud& tgt,
 void Odometry::UpdatePre(const FeaturePoints& feature) {
   surf_pts_pre_ = feature.less_flat_surf_pts;
   corn_pts_pre_ = feature.less_sharp_corner_pts;
+  kdtree_surf_pts_->setInputCloud(surf_pts_pre_);
+  kdtree_corn_pts_->setInputCloud(corn_pts_pre_);
 }
 
 }  // namespace oh_my_loam

src/odometry/odometry.h

@@ -20,11 +20,11 @@ class Odometry {
   void UpdatePre(const FeaturePoints& feature);
   void MatchCornPoints(const TPointCloud& src, const TPointCloud& tgt,
                        std::vector<PointLinePair>* const pairs,
-                       double dist_thresh) const;
+                       double dist_sq_thresh) const;
 
   void MatchSurfPoints(const TPointCloud& src, const TPointCloud& tgt,
                        std::vector<PointPlanePair>* const pairs,
-                       double dist_thresh) const;
+                       double dist_sq_thresh) const;
 
   Pose3D pose_curr2world_;
   Pose3D pose_curr2last_;

src/oh_my_loam.cc

@@ -26,8 +26,6 @@ bool OhMyLoam::Init() {
 void OhMyLoam::Run(const PointCloud& cloud_in, double timestamp) {
   PointCloudPtr cloud(new PointCloud);
   RemoveOutliers(cloud_in, cloud.get());
-  ADEBUG << "After remove, point num: " << cloud_in.size() << " -> "
-         << cloud->size();
   FeaturePoints feature_points;
   extractor_->Process(*cloud, &feature_points);
   Pose3D pose;