move PointXYZT from common to oh_my_loam

.gitignore

@@ -1,4 +1,8 @@
 /.vscode
 /.log
 *.pcd
-/build
+/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 {
+}  // namespace common
-  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

@@ -2,25 +2,25 @@
 
 namespace oh_my_loam {
 
-void TransformToStart(const Pose3d& pose, const TPoint& pt_in,
+void TransformToStart(const Pose3d &pose, const TPoint &pt_in,
-                      TPoint* const pt_out) {
+                      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);
 }
 
-TPoint TransformToStart(const Pose3d& pose, const TPoint& pt_in) {
+TPoint TransformToStart(const Pose3d &pose, const TPoint &pt_in) {
   TPoint pt_out;
   TransformToStart(pose, pt_in, &pt_out);
   return pt_out;
 }
 
-void TransformToEnd(const Pose3d& pose, const TPoint& pt_in,
+void TransformToEnd(const Pose3d &pose, const TPoint &pt_in,
-                    TPoint* const pt_out) {
+                    TPoint *const pt_out) {
   TransformToStart(pose, pt_in, pt_out);
   TransformPoint<TPoint>(pose.Inv(), *pt_out, pt_out);
 }
 
-TPoint TransformToEnd(const Pose3d& pose, const TPoint& pt_in) {
+TPoint TransformToEnd(const Pose3d &pose, const TPoint &pt_in) {
   TPoint pt_out;
   TransformToEnd(pose, pt_in, &pt_out);
   return pt_out;

oh_my_loam/base/helper.h

@@ -1,20 +1,15 @@
 #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,
+void TransformPoint(const Pose3d &pose, const PointType &pt_in,
-                    PointType* const pt_out) {
+                    PointType *const pt_out) {
   *pt_out = pt_in;
   Eigen::Vector3d pnt = pose * Eigen::Vector3d(pt_in.x, pt_in.y, pt_in.z);
   pt_out->x = pnt.x();
@@ -23,7 +18,7 @@ void TransformPoint(const Pose3d& pose, const PointType& pt_in,
 }
 
 template <typename PointType>
-PointType TransformPoint(const Pose3d& pose, const PointType& pt_in) {
+PointType TransformPoint(const Pose3d &pose, const PointType &pt_in) {
   PointType pt_out;
   TransformPoint<PointType>(pose, pt_in, &pt_out);
   return pt_out;
@@ -34,31 +29,31 @@ PointType TransformPoint(const Pose3d& pose, const PointType& pt_in) {
  *
  * @param pose Relative pose, end scan time w.r.t. start scan time
  */
-void TransformToStart(const Pose3d& pose, const TPoint& pt_in,
+void TransformToStart(const Pose3d &pose, const TPoint &pt_in,
-                      TPoint* const pt_out);
+                      TPoint *const pt_out);
 
-TPoint TransformToStart(const Pose3d& pose, const TPoint& pt_in);
+TPoint TransformToStart(const Pose3d &pose, const TPoint &pt_in);
 
 /**
  * @brief Transform a lidar point to the end of the scan
  *
  * @param pose Relative pose, end scan time w.r.t. start scan time
  */
-void TransformToEnd(const Pose3d& pose, const TPoint& pt_in,
+void TransformToEnd(const Pose3d &pose, const TPoint &pt_in,
-                    TPoint* const pt_out);
+                    TPoint *const pt_out);
 
-TPoint TransformToEnd(const Pose3d& pose, const TPoint& pt_in);
+TPoint TransformToEnd(const Pose3d &pose, const TPoint &pt_in);
 
 struct PointLinePair {
   TPoint pt;
   struct Line {
     TPoint pt1, pt2;
     Line() = default;
-    Line(const TPoint& pt1, const TPoint& pt2) : pt1(pt1), pt2(pt2) {}
+    Line(const TPoint &pt1, const TPoint &pt2) : pt1(pt1), pt2(pt2) {}
   };
   Line line;
-  PointLinePair(const TPoint& pt, const Line& line) : pt(pt), line(line) {}
+  PointLinePair(const TPoint &pt, const Line &line) : pt(pt), line(line) {}
-  PointLinePair(const TPoint& pt, const TPoint& pt1, const TPoint& pt2)
+  PointLinePair(const TPoint &pt, const TPoint &pt1, const TPoint &pt2)
       : pt(pt), line(pt1, pt2) {}
 };
 
@@ -67,13 +62,13 @@ struct PointPlanePair {
   struct Plane {
     TPoint pt1, pt2, pt3;
     Plane() = default;
-    Plane(const TPoint& pt1, const TPoint& pt2, const TPoint& pt3)
+    Plane(const TPoint &pt1, const TPoint &pt2, const TPoint &pt3)
         : pt1(pt1), pt2(pt2), pt3(pt3) {}
   };
   Plane plane;
-  PointPlanePair(const TPoint& pt, const Plane& plane) : pt(pt), plane(plane) {}
+  PointPlanePair(const TPoint &pt, const Plane &plane) : pt(pt), plane(plane) {}
-  PointPlanePair(const TPoint& pt, const TPoint& pt1, const TPoint& pt2,
+  PointPlanePair(const TPoint &pt, const TPoint &pt1, const TPoint &pt2,
-                 const TPoint& pt3)
+                 const TPoint &pt3)
       : pt(pt), plane(pt1, pt2, pt3) {}
 };
 

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 {
@@ -42,7 +85,7 @@ struct PointXYZTCT {
     data[3] = 1.0f;
   }
 
-  PointXYZTCT(const common::Point& p) {
+  PointXYZTCT(const common::Point &p) {
     x = p.x;
     y = p.y;
     z = p.z;
@@ -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;
@@ -62,7 +105,7 @@ struct PointXYZTCT {
     type = PType::NORNAL;
   }
 
-  PointXYZTCT(const PointXYZTCT& p) {
+  PointXYZTCT(const PointXYZTCT &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

@@ -12,7 +12,7 @@ using namespace common;
 }  // namespace
 
 bool Odometer::Init() {
-  const auto& config = YAMLConfig::Instance()->config();
+  const auto &config = YAMLConfig::Instance()->config();
   config_ = config["odometer_config"];
   is_vis_ = config["vis"].as<bool>() && config_["vis"].as<bool>();
   verbose_ = config_["verbose"].as<bool>();
@@ -21,23 +21,25 @@ bool Odometer::Init() {
   return true;
 }
 
-void Odometer::Process(double timestamp, const std::vector<Feature>& features,
+void Odometer::Process(double timestamp, const std::vector<Feature> &features,
-                       Pose3d* const pose) {
+                       Pose3d *const pose) {
   BLOCK_TIMER_START;
   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) {
+    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,32 +47,33 @@ 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) {
+    for (const auto &pair : pl_pairs) {
       solver.AddPointLinePair(pair, pair.pt.time);
     }
-    for (const auto& pair : pp_pairs) {
+    for (const auto &pair : pp_pairs) {
       solver.AddPointPlanePair(pair, pair.pt.time);
     }
     solver.Solve(5, verbose_, &pose_curr2last_);
   }
   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;
 }
 
-void Odometer::MatchCorn(const TPointCloud& src,
+void Odometer::MatchCorn(const TPointCloud &src,
-                         std::vector<PointLinePair>* const pairs) const {
+                         std::vector<PointLinePair> *const pairs) const {
   double dist_sq_thresh = config_["match_dist_sq_th"].as<double>();
-  auto comp = [](const std::vector<float>& v1, const std::vector<float>& v2) {
+  auto comp = [](const std::vector<float> &v1, const std::vector<float> &v2) {
     return v1[0] < v2[0];
   };
-  for (const auto& pt : src) {
+  for (const auto &pt : src) {
     TPoint query_pt = TransformToStart(pose_curr2last_, pt);
     std::vector<std::vector<int>> indices;
     std::vector<std::vector<float>> dists;
@@ -90,20 +93,20 @@ 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);
     }
   }
 }
 
-void Odometer::MatchSurf(const TPointCloud& src,
+void Odometer::MatchSurf(const TPointCloud &src,
-                         std::vector<PointPlanePair>* const pairs) const {
+                         std::vector<PointPlanePair> *const pairs) const {
   double dist_sq_thresh = config_["match_dist_sq_th"].as<double>();
-  auto comp = [](const std::vector<float>& v1, const std::vector<float>& v2) {
+  auto comp = [](const std::vector<float> &v1, const std::vector<float> &v2) {
     return v1[0] < v2[0];
   };
-  for (const auto& pt : src) {
+  for (const auto &pt : src) {
     TPoint query_pt = TransformToStart(pose_curr2last_, pt);
     std::vector<std::vector<int>> indices;
     std::vector<std::vector<float>> dists;
@@ -125,64 +128,69 @@ 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);
     }
   }
 }
 
-void Odometer::UpdatePre(const std::vector<Feature>& features) {
+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];
+    const auto &feature = features[i];
-    auto& cloud_pre = clouds_corn_pre_[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];
+    const auto &feature = features[i];
-    auto& cloud_pre = clouds_surf_pre_[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 {
-    std::vector<std::vector<float>>* const dists) const {
+  for (const auto &kdtree : kdtrees) {
-  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));
   }
 }
 
-void Odometer::Visualize(const std::vector<Feature>& features,
+void Odometer::Visualize(const std::vector<Feature> &features,
-                         const std::vector<PointLinePair>& pl_pairs,
+                         const std::vector<PointLinePair> &pl_pairs,
-                         const std::vector<PointPlanePair>& pp_pairs,
+                         const std::vector<PointPlanePair> &pp_pairs,
                          double timestamp) const {
   std::shared_ptr<OdometerVisFrame> frame(new OdometerVisFrame);
   frame->timestamp = timestamp;

oh_my_loam/odometer/odometer.h

@@ -15,37 +15,35 @@ class Odometer {
 
   bool Init();
 
-  void Process(double timestamp, const std::vector<Feature>& features,
+  void Process(double timestamp, const std::vector<Feature> &features,
-               common::Pose3d* const pose);
+               common::Pose3d *const pose);
 
  protected:
-  void UpdatePre(const std::vector<Feature>& features);
+  void UpdatePre(const std::vector<Feature> &features);
 
-  void MatchCorn(const common::TPointCloud& src,
+  void MatchCorn(const TPointCloud &src,
-                 std::vector<PointLinePair>* const pairs) const;
+                 std::vector<PointLinePair> *const pairs) const;
 
-  void MatchSurf(const common::TPointCloud& src,
+  void MatchSurf(const TPointCloud &src,
-                 std::vector<PointPlanePair>* const pairs) const;
+                 std::vector<PointPlanePair> *const pairs) const;
 
-  void Visualize(const std::vector<Feature>& features,
+  void Visualize(const std::vector<Feature> &features,
-                 const std::vector<PointLinePair>& pl_pairs,
+                 const std::vector<PointLinePair> &pl_pairs,
-                 const std::vector<PointPlanePair>& pp_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,
+                      std::vector<std::vector<int>> *const indices,
-      const TPoint& query_pt, size_t k,
+                      std::vector<std::vector<float>> *const dists) const;
-      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

@@ -6,22 +6,22 @@
 namespace oh_my_loam {
 
 class PoseSolver {
- public:
+public:
-  PoseSolver(const common::Pose3d& pose);
+  PoseSolver(const common::Pose3d &pose);
 
-  void AddPointLinePair(const PointLinePair& pair, double time);
+  void AddPointLinePair(const PointLinePair &pair, double time);
 
-  void AddPointPlanePair(const PointPlanePair& pair, double time);
+  void AddPointPlanePair(const PointPlanePair &pair, double time);
 
   double Solve(int max_iter_num = 5, bool verbose = false,
-               common::Pose3d* const pose = nullptr);
+               common::Pose3d *const pose = nullptr);
 
   common::Pose3d GetPose() const;
 
- private:
+private:
   ceres::Problem problem_;
 
-  ceres::LossFunction* loss_function_;
+  ceres::LossFunction *loss_function_;
 
   // r_quat_: [x, y, z, w], t_vec_: [x, y, z]
   double *r_quat_, *t_vec_;
@@ -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;