move PointXYZT from common to oh_my_loam

.gitignore

@@ -2,3 +2,7 @@
 /.log
 *.pcd
 /build
+/.clangd
+compile_commands.json
+.clang-tidy
+.clang-format

CMakeLists.txt

@@ -18,9 +18,6 @@ find_package(catkin REQUIRED COMPONENTS
   rospy
   rosbag
   std_msgs
-  image_transport
-  cv_bridge
-  tf
 )
 
 include_directories(SYSTEM
@@ -29,9 +26,6 @@ include_directories(SYSTEM
   ${G3LOG_INCLUDE_DIRS}
 )
 
-link_directories(${PCL_LIBRARY_DIRS})
-add_definitions(${PCL_DEFINITIONS})
-
 catkin_package(
   CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs
   DEPENDS EIGEN3 PCL 

common/pcl/pcl_types.h

@@ -17,60 +17,12 @@ using Point = pcl::PointXYZ;
 using PointCloud = pcl::PointCloud<Point>;
 using PointCloudPtr = PointCloud::Ptr;
 using PointCloudConstPtr = PointCloud::ConstPtr;
+
 using Indices = std::vector<int>;
 
-struct PointXYZT;
+using Point2D = pcl::PointXY;
-using TPoint = PointXYZT;
+using PointCloud2D = pcl::PointCloud<Point2D>;
-using TPointCloud = pcl::PointCloud<TPoint>;
+using PointCloud2DPtr = PointCloud2D::Ptr;
-using TPointCloudPtr = TPointCloud::Ptr;
+using PointCloud2DConstPtr = PointCloud2D::ConstPtr;
-using TPointCloudConstPtr = TPointCloud::ConstPtr;
-
-struct EIGEN_ALIGN16 PointXYZT {
-  PCL_ADD_POINT4D;
-  union {
-    float time;
-    float intensity;
-    float data_c[4];
-  };
-
-  PointXYZT() {
-    x = y = z = 0.0f;
-    data[3] = 1.0f;
-    time = 0.0f;
-  }
-
-  PointXYZT(float x, float y, float z, float time = 0.0f)
-      : x(x), y(y), z(z), time(time) {
-    data[3] = 1.0f;
-  }
-
-  PointXYZT(const Point& p) {
-    x = p.x;
-    y = p.y;
-    z = p.z;
-    data[3] = 1.0f;
-    time = 0.0f;
-  }
-
-  PointXYZT(const PointXYZT& p) {
-    x = p.x;
-    y = p.y;
-    z = p.z;
-    data[3] = 1.0f;
-    time = p.time;
-  }
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-};
 
 }  // namespace common
-
-// clang-format off
-POINT_CLOUD_REGISTER_POINT_STRUCT(
-  ::common::PointXYZT,
-  (float, x, x)
-  (float, y, y)
-  (float, z, z)
-  (float, time, time)
-  (float, intensity, intensity)
-)
-// clang-format on

oh_my_loam/base/feature.h

@@ -1,20 +1,20 @@
 #pragma once
 
-#include "common/pcl/pcl_types.h"
+#include "oh_my_loam/base/types.h"
 
 namespace oh_my_loam {
 
 struct Feature {
-  common::TPointCloudPtr cloud_corner;
+  TPointCloudPtr cloud_corner;
-  common::TPointCloudPtr cloud_sharp_corner;
+  TPointCloudPtr cloud_sharp_corner;
-  common::TPointCloudPtr cloud_surf;
+  TPointCloudPtr cloud_surf;
-  common::TPointCloudPtr cloud_flat_surf;
+  TPointCloudPtr cloud_flat_surf;
 
   Feature() {
-    cloud_corner.reset(new common::TPointCloud);
+    cloud_corner.reset(new TPointCloud);
-    cloud_sharp_corner.reset(new common::TPointCloud);
+    cloud_sharp_corner.reset(new TPointCloud);
-    cloud_surf.reset(new common::TPointCloud);
+    cloud_surf.reset(new TPointCloud);
-    cloud_flat_surf.reset(new common::TPointCloud);
+    cloud_flat_surf.reset(new TPointCloud);
   }
 };
 

oh_my_loam/base/helper.cc

@@ -4,7 +4,7 @@ namespace oh_my_loam {
 
 void TransformToStart(const Pose3d &pose, const TPoint &pt_in,
                       TPoint *const pt_out) {
-  Pose3d pose_interp = Pose3d().Interpolate(pose, GetTime(pt_in));
+  Pose3d pose_interp = Pose3d().Interpolate(pose, pt_in.time);
   TransformPoint<TPoint>(pose_interp, pt_in, pt_out);
 }
 

oh_my_loam/base/helper.h

@@ -1,17 +1,12 @@
 #pragma once
 
 #include "common/geometry/pose3d.h"
-#include "common/pcl/pcl_types.h"
+#include "oh_my_loam/base/types.h"
 
 namespace oh_my_loam {
 
-using common::TPoint;
 using common::Pose3d;
 
-inline float GetTime(const TPoint& pt) { return pt.time - std::floor(pt.time); }
-
-inline int GetScanId(const TPoint& pt) { return static_cast<int>(pt.time); }
-
 template <typename PointType>
 void TransformPoint(const Pose3d &pose, const PointType &pt_in,
                     PointType *const pt_out) {

oh_my_loam/base/types.h

@@ -12,12 +12,55 @@ enum class PType {
   SHARP_CORNER = 2
 };
 
+struct PointXYZT;
+using TPoint = PointXYZT;
+using TPointCloud = pcl::PointCloud<TPoint>;
+using TPointCloudPtr = TPointCloud::Ptr;
+using TPointCloudConstPtr = TPointCloud::ConstPtr;
+
 struct PointXYZTCT;
 using TCTPoint = PointXYZTCT;
 using TCTPointCloud = pcl::PointCloud<TCTPoint>;
 using TCTPointCloudPtr = TCTPointCloud::Ptr;
 using TCTPointCloudConstPtr = TCTPointCloud::ConstPtr;
 
+struct EIGEN_ALIGN16 PointXYZT {
+  PCL_ADD_POINT4D;
+  union {
+    float time;
+    float intensity;
+    float data_c[4];
+  };
+
+  PointXYZT() {
+    x = y = z = 0.0f;
+    data[3] = 1.0f;
+    time = 0.0f;
+  }
+
+  PointXYZT(float x, float y, float z, float time = 0.0f)
+      : x(x), y(y), z(z), time(time) {
+    data[3] = 1.0f;
+  }
+
+  PointXYZT(const common::Point &p) {
+    x = p.x;
+    y = p.y;
+    z = p.z;
+    data[3] = 1.0f;
+    time = 0.0f;
+  }
+
+  PointXYZT(const PointXYZT &p) {
+    x = p.x;
+    y = p.y;
+    z = p.z;
+    data[3] = 1.0f;
+    time = p.time;
+  }
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
 struct PointXYZTCT {
   PCL_ADD_POINT4D;
   union EIGEN_ALIGN16 {
@@ -52,7 +95,7 @@ struct PointXYZTCT {
     type = PType::NORNAL;
   }
 
-  PointXYZTCT(const common::TPoint& p) {
+  PointXYZTCT(const TPoint &p) {
     x = p.x;
     y = p.y;
     z = p.z;
@@ -78,6 +121,15 @@ struct PointXYZTCT {
 }  // namespace oh_my_loam
 
 // clang-format off
+POINT_CLOUD_REGISTER_POINT_STRUCT(
+  oh_my_loam::PointXYZT,
+  (float, x, x)
+  (float, y, y)
+  (float, z, z)
+  (float, time, time)
+  (float, intensity, intensity)
+)
+
 POINT_CLOUD_REGISTER_POINT_STRUCT(
   oh_my_loam::PointXYZTCT,
   (float, x, x)

oh_my_loam/mapper/mapper.h

@@ -2,7 +2,7 @@
 
 #include "common/common.h"
 #include "common/geometry/pose.h"
-#include "common/pcl/pcl_types.h"
+#include "oh_my_loam/base/types.h"
 
 namespace oh_my_loam {
 
@@ -17,7 +17,7 @@ class Mapper {
  private:
   void Visualize();
 
-  common::TPointCloudPtr map_pts_;
+  TPointCloudPtr map_pts_;
   common::Pose3d pose_curr2world_;
 
   bool is_initialized = false;

oh_my_loam/odometer/odometer.cc

@@ -27,17 +27,19 @@ void Odometer::Process(double timestamp, const std::vector<Feature>& features,
   if (!is_initialized_) {
     UpdatePre(features);
     is_initialized_ = true;
+    pose_curr2last_ = Pose3d::Identity();
+    pose_curr2world_ = Pose3d::Identity();
     *pose = Pose3d::Identity();
     AWARN << "Odometer initialized....";
     return;
   }
-  AINFO << "Pose before: " << pose_curr2world_.ToString();
+  AINFO_IF(verbose_) << "Pose before: " << pose_curr2world_.ToString();
   std::vector<PointLinePair> pl_pairs;
   std::vector<PointPlanePair> pp_pairs;
   for (int i = 0; i < config_["icp_iter_num"].as<int>(); ++i) {
     for (const auto &feature : features) {
       MatchCorn(*feature.cloud_sharp_corner, &pl_pairs);
-      // MatchSurf(*feature.cloud_flat_surf, &pp_pairs);
+      MatchSurf(*feature.cloud_flat_surf, &pp_pairs);
     }
     AINFO_IF(verbose_) << "Iter " << i
                        << ": matched num: point2line = " << pl_pairs.size()
@@ -45,6 +47,7 @@ void Odometer::Process(double timestamp, const std::vector<Feature>& features,
     if (pl_pairs.size() + pp_pairs.size() < kMinMatchNum) {
       AWARN << "Too less correspondence: " << pl_pairs.size() << " + "
             << pp_pairs.size();
+      continue;
     }
     PoseSolver solver(pose_curr2last_);
     for (const auto &pair : pl_pairs) {
@@ -57,8 +60,8 @@ void Odometer::Process(double timestamp, const std::vector<Feature>& features,
   }
   pose_curr2world_ = pose_curr2world_ * pose_curr2last_;
   *pose = pose_curr2world_;
-  AWARN_IF(verbose_) << "Pose increase: " << pose_curr2last_.ToString();
+  AINFO_IF(verbose_) << "Pose increase: " << pose_curr2last_.ToString();
-  AWARN_IF(verbose_) << "Pose after: " << pose_curr2world_.ToString();
+  AINFO_IF(verbose_) << "Pose after: " << pose_curr2world_.ToString();
   UpdatePre(features);
   if (is_vis_) Visualize(features, pl_pairs, pp_pairs);
   AINFO << "Odometer::Porcess: " << BLOCK_TIMER_STOP_FMT;
@@ -90,7 +93,7 @@ void Odometer::MatchCorn(const TPointCloud& src,
         min_dist_pt2_squre = dists[i][0];
       }
     }
-    if (pt2_scan_id > 0) {
+    if (pt2_scan_id >= 0) {
       TPoint pt2 = clouds_corn_pre_[pt2_scan_id]->at(indices[pt2_scan_id][0]);
       pairs->emplace_back(pt, pt1, pt2);
     }
@@ -125,7 +128,7 @@ void Odometer::MatchSurf(const TPointCloud& src,
         min_dist_pt3_squre = dists[i][0];
       }
     }
-    if (pt3_scan_id > 0) {
+    if (pt3_scan_id >= 0) {
       TPoint pt3 = clouds_surf_pre_[pt3_scan_id]->at(indices[pt3_scan_id][0]);
       pairs->emplace_back(pt, pt1, pt2, pt3);
     }
@@ -133,47 +136,52 @@ void Odometer::MatchSurf(const TPointCloud& src,
 }
 
 void Odometer::UpdatePre(const std::vector<Feature> &features) {
-  clouds_corn_pre_.resize(features.size(), common::TPointCloud().makeShared());
+  BLOCK_TIMER_START;
-  clouds_surf_pre_.resize(features.size(), common::TPointCloud().makeShared());
+  clouds_corn_pre_.resize(features.size());
+  clouds_surf_pre_.resize(features.size());
   kdtrees_corn_.resize(features.size());
   kdtrees_surf_.resize(features.size());
 
   for (size_t i = 0; i < features.size(); ++i) {
     const auto &feature = features[i];
     auto &cloud_pre = clouds_corn_pre_[i];
+    if (!cloud_pre) cloud_pre.reset(new TPointCloud);
     cloud_pre->resize(feature.cloud_corner->size());
     for (size_t j = 0; j < feature.cloud_corner->size(); ++j) {
       TransformToEnd(pose_curr2last_, feature.cloud_corner->at(j),
                      &cloud_pre->at(j));
     }
-    kdtrees_corn_[i].setInputCloud(clouds_corn_pre_[i]);
+    kdtrees_corn_[i].setInputCloud(cloud_pre);
   }
 
   for (size_t i = 0; i < features.size(); ++i) {
     const auto &feature = features[i];
     auto &cloud_pre = clouds_surf_pre_[i];
+    if (!cloud_pre) cloud_pre.reset(new TPointCloud);
     cloud_pre->resize(feature.cloud_surf->size());
     for (size_t j = 0; j < feature.cloud_surf->size(); ++j) {
       TransformToEnd(pose_curr2last_, feature.cloud_surf->at(j),
                      &cloud_pre->at(j));
     }
-    kdtrees_surf_[i].setInputCloud(clouds_surf_pre_[i]);
+    kdtrees_surf_[i].setInputCloud(cloud_pre);
   }
+  AINFO << "Odometer::UpdatePre: " << BLOCK_TIMER_STOP_FMT;
 }
 
 void Odometer::NearestKSearch(
-    const std::vector<pcl::KdTreeFLANN<common::TPoint>>& kdtrees,
+    const std::vector<pcl::KdTreeFLANN<TPoint>> &kdtrees,
-    const TPoint& query_pt, size_t k,
+    const TPoint &query_pt, int k, std::vector<std::vector<int>> *const indices,
-    std::vector<std::vector<int>>* const indices,
     std::vector<std::vector<float>> *const dists) const {
   for (const auto &kdtree : kdtrees) {
     std::vector<int> index;
     std::vector<float> dist;
-    if (kdtree.getInputCloud()->size() >= k) {
+    int n_found = 0;
-      kdtree.nearestKSearch(query_pt, k, index, dist);
+    if (kdtree.getInputCloud()->size() >= static_cast<size_t>(k)) {
-    } else {
+      n_found = kdtree.nearestKSearch(query_pt, k, index, dist);
-      index.resize(k, -1);
+    }
-      dist.resize(k, std::numeric_limits<float>::max());
+    if (n_found < k) {
+      std::vector<int>(k, -1).swap(index);
+      std::vector<float>(k, std::numeric_limits<float>::max()).swap(dist);
     }
     indices->push_back(std::move(index));
     dists->push_back(std::move(dist));

oh_my_loam/odometer/odometer.h

@@ -21,31 +21,29 @@ class Odometer {
  protected:
   void UpdatePre(const std::vector<Feature> &features);
 
-  void MatchCorn(const common::TPointCloud& src,
+  void MatchCorn(const TPointCloud &src,
                  std::vector<PointLinePair> *const pairs) const;
 
-  void MatchSurf(const common::TPointCloud& src,
+  void MatchSurf(const TPointCloud &src,
                  std::vector<PointPlanePair> *const pairs) const;
 
   void Visualize(const std::vector<Feature> &features,
                  const std::vector<PointLinePair> &pl_pairs,
                  const std::vector<PointPlanePair> &pp_pairs,
                  double timestamp = 0.0) const;
-
+  void NearestKSearch(const std::vector<pcl::KdTreeFLANN<TPoint>> &kdtrees,
-  void NearestKSearch(
+                      const TPoint &query_pt, int k,
-      const std::vector<pcl::KdTreeFLANN<common::TPoint>>& kdtrees,
-      const TPoint& query_pt, size_t k,
                       std::vector<std::vector<int>> *const indices,
                       std::vector<std::vector<float>> *const dists) const;
 
   common::Pose3d pose_curr2world_;
   common::Pose3d pose_curr2last_;
 
-  std::vector<common::TPointCloudPtr> clouds_corn_pre_;
+  std::vector<TPointCloudPtr> clouds_corn_pre_;
-  std::vector<common::TPointCloudPtr> clouds_surf_pre_;
+  std::vector<TPointCloudPtr> clouds_surf_pre_;
 
-  std::vector<pcl::KdTreeFLANN<common::TPoint>> kdtrees_surf_;
+  std::vector<pcl::KdTreeFLANN<TPoint>> kdtrees_surf_;
-  std::vector<pcl::KdTreeFLANN<common::TPoint>> kdtrees_corn_;
+  std::vector<pcl::KdTreeFLANN<TPoint>> kdtrees_corn_;
 
   YAML::Node config_;
 

oh_my_loam/solver/solver.h

@@ -29,4 +29,4 @@ class PoseSolver {
   DISALLOW_COPY_AND_ASSIGN(PoseSolver)
 };
 
-}  // oh_my_loam
+} // namespace oh_my_loam

oh_my_loam/visualizer/odometer_visualizer.h

@@ -6,8 +6,8 @@
 namespace oh_my_loam {
 
 struct OdometerVisFrame : public common::LidarVisFrame {
-  common::TPointCloudPtr cloud_surf;
+  TPointCloudPtr cloud_surf;
-  common::TPointCloudPtr cloud_corner;
+  TPointCloudPtr cloud_corner;
   std::vector<PointLinePair> pl_pairs;
   std::vector<PointPlanePair> pp_pairs;
   common::Pose3d pose_curr2last;