refactoring...

CMakeLists.txt

@@ -10,21 +10,21 @@ find_package(PCL QUIET)
 find_package(g3log REQUIRED)
 find_package(yaml-cpp REQUIRED)
 
-find_package(catkin REQUIRED COMPONENTS
+# find_package(catkin REQUIRED COMPONENTS
-  geometry_msgs
+#   geometry_msgs
-  nav_msgs
+#   nav_msgs
-  sensor_msgs
+#   sensor_msgs
-  roscpp
+#   roscpp
-  rospy
+#   rospy
-  rosbag
+#   rosbag
-  std_msgs
+#   std_msgs
-  image_transport
+#   image_transport
-  cv_bridge
+#   cv_bridge
-  tf
+#   tf
-)
+# )
 
 include_directories(SYSTEM
-	${catkin_INCLUDE_DIRS} 
+	# ${catkin_INCLUDE_DIRS} 
   ${PCL_INCLUDE_DIRS}
   ${G3LOG_INCLUDE_DIRS}
 )
@@ -32,33 +32,32 @@ include_directories(SYSTEM
 link_directories(${PCL_LIBRARY_DIRS})
 add_definitions(${PCL_DEFINITIONS})
 
-include_directories(
+# catkin_package(
-  src
+#   CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs
-  common
+#   DEPENDS EIGEN3 PCL 
-)
+#   INCLUDE_DIRS src common
+# )
 
-catkin_package(
+include_directories(
-  CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs
+  ${CMAKE_SOURCE_DIR}
-  DEPENDS EIGEN3 PCL 
-  INCLUDE_DIRS src common
 )
 
 add_subdirectory(common)
-add_subdirectory(src)
+add_subdirectory(oh_my_loam)
 
-add_executable(main main.cc)
+# add_executable(main main.cc)
-target_link_libraries(main
+# target_link_libraries(main
-  ${catkin_LIBRARIES} 
+#   ${catkin_LIBRARIES} 
-  ${PCL_LIBRARIES}
+#   ${PCL_LIBRARIES}
-  ${G3LOG_LIBRARIES}
+#   ${G3LOG_LIBRARIES}
-  ${YAML_CPP_LIBRARIES}
+#   ${YAML_CPP_LIBRARIES}
-  common
+#   common
-  oh_my_loam
+#   oh_my_loam
-  extractor
+#   extractor
-  odometry
+#   odometry
-  mapper
+#   mapper
-  solver
+#   solver
-  ${CERES_LIBRARIES}
+#   ${CERES_LIBRARIES}
-  helper
+#   helper
-  visualizer
+#   visualizer
-)
+# )

common/CMakeLists.txt

@@ -1,3 +1,7 @@
-aux_source_directory(. SRC_FILES)
+file(GLOB SRC_FILES **/*.cc)
+
+message(STATUS "common dir: " ${CMAKE_CURRENT_SOURCE_DIR})
+
+# include_directories(${CMAKE_CURRENT_SOURCE_DIR})
 
 add_library(common SHARED ${SRC_FILES})

common/color/color.h

@@ -1,6 +1,6 @@
 #pragma once
 
-namespace oh_my_loam {
+namespace common {
 
 struct Color {
   unsigned char r, g, b;
@@ -20,4 +20,4 @@ struct Color {
 #define PINK Color(255, 182, 193)
 #define YELLOW Color(255, 255, 0)
 
-}  // namespace oh_my_loam
+}  // namespace common

common/common.h

@@ -1,11 +1,8 @@
 #pragma once
 
-#include "color.h"
+#include "color/color.h"
-#include "config.h"
+#include "config/yaml_config.h"
-#include "log.h"
+#include "log/log.h"
-#include "macros.h"
+#include "macro/macros.h"
-#include "pose.h"
+#include "math/math_utils.h"
-#include "tic_toc.h"
+#include "time/timer.h"
-#include "types.h"
-#include "utils.h"
-#include "yaml-cpp/yaml.h"

common/config.h

@@ -1,36 +0,0 @@
-#pragma once
-
-#include <yaml-cpp/yaml.h>
-
-#include <string>
-
-#include "log.h"
-#include "macros.h"
-
-namespace oh_my_loam {
-
-class Config {
- public:
-  void SetConfigFile(const std::string& file) {
-    config_.reset(new YAML::Node);
-    *config_ = YAML::LoadFile(file);
-  }
-
-  template <typename T>
-  const T Get(const std::string& key) const {
-    AFATAL_IF(!config_) << "No config exists: please call SetConfigFile.";
-    return (*config_)[key].as<T>();
-  }
-
-  const YAML::Node& config() const {
-    AFATAL_IF(!config_) << "No config exists: please call SetConfigFile.";
-    return *config_;
-  }
-
- private:
-  std::shared_ptr<YAML::Node> config_{nullptr};
-
-  DECLARE_SINGLETON(Config)
-};
-
-}  // namespace oh_my_loam

common/config/yaml_config.h

@@ -0,0 +1,34 @@
+#pragma once
+
+#include <yaml-cpp/yaml.h>
+#include <string>
+
+#include "common/log/log.h"
+#include "common/macro/macros.h"
+
+namespace common {
+
+class YAMLConfig {
+ public:
+  void Init(const std::string& file) {
+    config_.reset(new YAML::Node);
+    *config_ = YAML::LoadFile(file);
+  }
+
+  template <typename T>
+  const T Get(const std::string& key) const {
+    AFATAL_IF(!config_) << "Not initialized, please call Init first.";
+    return (*config_)[key].as<T>();
+  }
+
+  const YAML::Node& config() const {
+    AFATAL_IF(!config_) << "Not initialized, please call Init first.";
+    return *config_;
+  }
+
+ private:
+  std::unique_ptr<YAML::Node> config_{nullptr};
+  DECLARE_SINGLETON(YAMLConfig);
+};
+
+}  // namespace common

common/geometry/pose.cc

@@ -0,0 +1,20 @@
+#include "pose.h"
+
+namespace common {
+
+Pose3d Interpolate(const Pose3d& pose_from, const Pose3d& pose_to, double t) {
+  return pose_from.Interpolate(pose_to, t);
+}
+
+Pose3d operator*(const Pose3d& lhs, const Pose3d& rhs) {
+  return Pose3d(lhs.q() * rhs.q(), lhs.q() * rhs.p() + lhs.p());
+}
+
+std::string Pose3d::ToString() const {
+  std::ostringstream oss;
+  oss << "[Pose3d] q = (" << q_.coeffs().transpose() << "), p = ("
+      << p_.transpose() << ")";
+  return oss.str();
+}
+
+}  // namespace common

common/geometry/pose.h

@@ -2,24 +2,24 @@
 
 #include <eigen3/Eigen/Dense>
 
-namespace oh_my_loam {
+namespace common {
 
-class Pose3D {
+class Pose3d {
  public:
-  Pose3D() {
+  Pose3d() {
     q_.setIdentity();
     p_.setZero();
   };
 
-  Pose3D(const Eigen::Quaterniond& q, const Eigen::Vector3d& p)
+  Pose3d(const Eigen::Quaterniond& q, const Eigen::Vector3d& p)
       : q_(q), p_(p) {}
 
-  Pose3D(const Eigen::Matrix3d& r_mat, const Eigen::Vector3d& p)
+  Pose3d(const Eigen::Matrix3d& r_mat, const Eigen::Vector3d& p)
       : q_(r_mat), p_(p) {}
 
-  Pose3D(const double* const q, const double* const p) : q_(q), p_(p) {}
+  Pose3d(const double* const q, const double* const p) : q_(q), p_(p) {}
 
-  Pose3D Inv() const {
+  Pose3d Inv() const {
     Eigen::Quaterniond q_inv = q_.inverse();
     Eigen::Vector3d p_inv = q_inv * p_;
     return {q_inv, -p_inv};
@@ -33,21 +33,6 @@ class Pose3D {
     return Transform(vec);
   }
 
-  template <typename PointT>
-  PointT Transform(const PointT& pt) const {
-    PointT pt_n = pt;
-    Eigen::Vector3d vec = Transform(Eigen::Vector3d(pt.x, pt.y, pt.z));
-    pt_n.x = static_cast<float>(vec.x());
-    pt_n.y = static_cast<float>(vec.y());
-    pt_n.z = static_cast<float>(vec.z());
-    return pt_n;
-  }
-
-  template <typename PointT>
-  PointT operator*(const PointT& vec) const {
-    return Transform<PointT>(vec);
-  }
-
   Eigen::Vector3d Translate(const Eigen::Vector3d& vec) const {
     return vec + p_;
   }
@@ -55,7 +40,7 @@ class Pose3D {
   Eigen::Vector3d Rotate(const Eigen::Vector3d& vec) const { return q_ * vec; }
 
   // Spherical linear interpolation to `pose_to`, `t` belongs [0, 1]
-  Pose3D Interpolate(const Pose3D& pose_to, double t) const {
+  Pose3d Interpolate(const Pose3d& pose_to, double t) const {
     Eigen::Quaterniond q_interp = q_.slerp(t, pose_to.q_);
     Eigen::Vector3d p_interp = (pose_to.p_ - p_) * t + p_;
     return {q_interp, p_interp};
@@ -64,6 +49,7 @@ class Pose3D {
   std::string ToString() const;
 
   Eigen::Quaterniond q() const { return q_; }
+
   Eigen::Vector3d p() const { return p_; }
 
  protected:
@@ -71,10 +57,10 @@ class Pose3D {
   Eigen::Vector3d p_;     // position
 };
 
-Pose3D Interpolate(const Pose3D& pose_from, const Pose3D& pose_to, double t);
+Pose3d Interpolate(const Pose3d& pose_from, const Pose3d& pose_to, double t);
 
-Pose3D operator*(const Pose3D& lhs, const Pose3D& rhs);
+Pose3d operator*(const Pose3d& lhs, const Pose3d& rhs);
 
-using Trans3D = Pose3D;
+using Trans3d = Pose3d;
 
-}  // namespace oh_my_loam
+}  // namespace common

common/log/log.cc

@@ -1,6 +1,8 @@
 #include "log.h"
+#include <filesystem>
+
+namespace common {
 
-namespace g3 {
 void InitG3Logging(bool log_to_file, const std::string& prefix,
                    const std::string& path) {
   static std::unique_ptr<g3::LogWorker> worker;
@@ -23,4 +25,5 @@ void InitG3Logging(bool log_to_file, const std::string& prefix,
   }
   g3::initializeLogging(worker.get());
 }
-}
+
+}  // namespace common

common/log/log.h

@@ -43,6 +43,11 @@ const LEVELS USER(ERROR.value + 100, "USER");
 #define AFATAL_IF(cond) G3LOG_IF(FATAL, cond)
 #define ACHECK(cond) G3CHECK(cond)
 
+namespace common {
+void InitG3Logging(bool log_to_file = false, const std::string& prefix = "",
+                   const std::string& path = "./");
+}  // namespace common
+
 namespace g3 {
 class CustomSink {
  public:
@@ -114,7 +119,4 @@ class CustomSink {
   }
 };
 
-void InitG3Logging(bool log_to_file = false, const std::string& prefix = "",
-                   const std::string& path = "./");
-
 }  // namespace g3

common/macro/macros.h

@@ -4,14 +4,15 @@
 #include <memory>
 #include <mutex>
 
-#define LOG_TIMESTAMP(timestamp) \
+// format timestamp
+#define FMT_TIMESTAMP(timestamp) \
   std::fixed << std::setprecision(3) << timestamp;
 
 #define DISALLOW_COPY_AND_ASSIGN(classname) \
   classname(const classname &) = delete;    \
   classname &operator=(const classname &) = delete;
 
-// adapted form baidu apollo cyber/common/macros.h, it is thread safe
+// adapted form baidu apollo cyber/common/macros.h
 #define DECLARE_SINGLETON(classname)                                           \
  public:                                                                       \
   static classname *Instance() {                                               \

common/math/math_utils.cc

@@ -1,6 +1,8 @@
-#include "utils.h"
+#include "math_utils.h"
 
-namespace oh_my_loam {
+#include "common/log/log.h"
+
+namespace common {
 
 double NormalizeAngle(double ang) {
   const double& two_pi = 2 * M_PI;
@@ -19,4 +21,4 @@ const std::vector<int> Range(int begin, int end, int step) {
 
 const std::vector<int> Range(int end) { return Range(0, end, 1); }
 
-}  // namespace oh_my_loam
+}  // namespace common

common/math/math_utils.h

@@ -0,0 +1,23 @@
+#pragma once
+
+#include <cmath>
+#include <vector>
+
+namespace common {
+
+// normalize an angle to [-pi, pi)
+double NormalizeAngle(double ang);
+
+// Convert an angle from degree to rad
+double Degree2Rad(double degree);
+
+// Convert an angle from rad to degree
+double Rad2Degree(double rad);
+
+// like Python built-in range, [begin, end)
+const std::vector<int> Range(int begin, int end, int step = 1);
+
+// like Python built-in range, [0, end)
+const std::vector<int> Range(int end);
+
+}  // namespace common

common/pcl/pcl_types.h

@@ -0,0 +1,76 @@
+#pragma once
+
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/kdtree/kdtree_flann.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+// Following hpp file should be included if user-defined point type is added,
+// see "How are the point types exposed?" section in
+// https://pointclouds.org/documentation/tutorials/adding_custom_ptype.html
+#include <pcl/filters/impl/voxel_grid.hpp>
+#include <pcl/impl/pcl_base.hpp>
+#include <pcl/kdtree/impl/kdtree_flann.hpp>
+
+namespace common {
+
+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 TPoint = PointXYZT;
+using TPointCloud = pcl::PointCloud<TPoint>;
+using TPointCloudPtr = TPointCloud::Ptr;
+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

common/pcl/pcl_utils.h

@@ -0,0 +1,71 @@
+#pragma once
+
+#include "log/log.h"
+#include "pcl_types.h"
+
+namespace common {
+
+// Distance squred of a point to origin
+template <typename PointType>
+inline double DistanceSqure(const PointType& pt) {
+  return pt.x * pt.x + pt.y * pt.y + pt.z * pt.z;
+}
+
+// Distance squred of two points
+template <typename PointType>
+inline double DistanceSqure(const PointType& pt1, const PointType& pt2) {
+  return (pt1.x - pt2.x) * (pt1.x - pt2.x) + (pt1.y - pt2.y) * (pt1.y - pt2.y) +
+         (pt1.z - pt2.z) * (pt1.z - pt2.z);
+}
+
+// Distance of a point to origin
+template <typename PointType>
+inline double Distance(const PointType& pt) {
+  return std::sqrt(DistanceSqure(pt));
+}
+
+// Distance squred of two points
+template <typename PointType>
+inline double Distance(const PointType& pt1, const PointType& pt2) {
+  return std::sqrt(DistanceSqure(pt1, pt2));
+}
+
+// Check whether is a finite point: neither infinite nor nan
+template <typename PointType>
+inline double IsFinite(const PointType& pt) {
+  return std::isfinite(pt.x) && std::isfinite(pt.y) && std::isfinite(pt.z);
+}
+
+// Remove point if the condition evaluated to true on it
+template <typename PointType>
+void RemovePoints(const typename pcl::PointCloud<PointType>::ConstPtr cloud_in,
+                  std::function<bool(const PointType&)> check,
+                  pcl::PointCloud<PointType>* const cloud_out) {
+  if (cloud_in.get() != cloud_out) {
+    cloud_out->header = cloud_in.header;
+    cloud_out->resize(cloud_in->size());
+  }
+  size_t j = 0;
+  for (size_t i = 0; i < cloud_in->size(); ++i) {
+    const auto pt = cloud_in->points[i];
+    if (check(pt)) continue;
+    cloud_out->points[j++] = pt;
+  }
+
+  cloud_out->points.resize(j);
+  cloud_out->height = 1;
+  cloud_out->width = static_cast<uint32_t>(j);
+  cloud_out->is_dense = true;
+}
+
+template <typename PointType>
+void VoxelDownSample(
+    const typename pcl::PointCloud<PointType>::ConstPtr& cloud_in,
+    pcl::PointCloud<PointType>* const cloud_out, double voxel_size) {
+  pcl::VoxelGrid<PointType> filter;
+  filter.setInputCloud(cloud_in);
+  filter.setLeafSize(voxel_size, voxel_size, voxel_size);
+  filter.filter(*cloud_out);
+}
+
+}  // namespace common

common/pose.cc

@@ -1,20 +0,0 @@
-#include <pose.h>
-
-namespace oh_my_loam {
-
-Pose3D Interpolate(const Pose3D& pose_from, const Pose3D& pose_to, double t) {
-  return pose_from.Interpolate(pose_to, t);
-}
-
-Pose3D operator*(const Pose3D& lhs, const Pose3D& rhs) {
-  return Pose3D(lhs.q() * rhs.q(), lhs.q() * rhs.p() + lhs.p());
-}
-
-std::string Pose3D::ToString() const {
-  std::ostringstream oss;
-  oss << "[Pose3D] q = (" << q_.coeffs().transpose() << "), p = ("
-      << p_.transpose() << ")";
-  return oss.str();
-}
-
-}  // namespace oh_my_loam

common/tic_toc.h

@@ -1,25 +0,0 @@
-#pragma once
-
-#include <chrono>
-#include <cstdlib>
-#include <ctime>
-
-namespace oh_my_loam {
-
-class TicToc {
- public:
-  TicToc() { tic(); }
-
-  void tic() { start_ = std::chrono::system_clock::now(); }
-
-  double toc() {
-    end_ = std::chrono::system_clock::now();
-    std::chrono::duration<double> elapsed_seconds = end_ - start_;
-    return elapsed_seconds.count() * 1000;  // ms
-  }
-
- private:
-  std::chrono::time_point<std::chrono::system_clock> start_, end_;
-};
-
-}  // namespace oh_my_loam

common/time/timer.cc

@@ -0,0 +1,28 @@
+#include "timer.h"
+#include "common/log/log.h"
+
+namespace common {
+
+double Timer::Toc(char unit) {
+  ACHECK(unit == 's' || unit == 'm' || unit == 'u')
+      << "Only 's'(second), 'm'(millisecond) and 'u'(microsecond) are "
+         "supported";
+  double factor = 1.0;
+  if (unit == 'm') factor = 1.0e3;
+  if (unit == 'u') factor = 1.0e6;
+  end_ = std::chrono::system_clock::now();
+  std::chrono::duration<double> elapsed_seconds = end_ - start_;
+  return elapsed_seconds.count() * factor;
+}
+
+TimerWrapper::~TimerWrapper() {
+  double duration = timer_.Toc();
+  if (duration_ms_ < 0) {
+    AINFO << msg_ << ": time elapsed (ms): " << FMT_TIMESTAMP(duration);
+  }
+  if (duration_ms_ > 0 && duration > duration_ms_) {
+    AWARN << msg_ << ": time elapsed (ms): " << FMT_TIMESTAMP(duration);
+  }
+}
+
+}  // namespace common

common/time/timer.h

@@ -0,0 +1,43 @@
+#pragma once
+
+#include <chrono>
+#include <string>
+#include "common/macro/macros.h"
+
+namespace common {
+
+class Timer {
+ public:
+  Timer() { Tic(); }
+
+  void Tic() { start_ = std::chrono::system_clock::now(); }
+
+  // unit: 's' = second, 'm' = millisecond, 'u' = microsecond
+  double Toc(char unit = 'm');
+
+ private:
+  std::chrono::time_point<std::chrono::system_clock> start_, end_;
+  DISALLOW_COPY_AND_ASSIGN(Timer);
+};
+
+class TimerWrapper {
+ public:
+  explicit TimerWrapper(const std::string& msg, double duration_ms = -1.0)
+      : msg_(msg), duration_ms_(duration_ms) {
+    timer_.Tic();
+  };
+
+  ~TimerWrapper();
+
+ private:
+  std::string msg_;
+  double duration_ms_;
+  Timer timer_;
+  DISALLOW_COPY_AND_ASSIGN(TimerWrapper);
+};
+
+#define TIME_ELAPSED(msg) TimerWrapper(msg)
+#define TIME_ELAPSED_EXPECT(msg, max_time_elapsed) \
+  TimerWrapper(msg, max_time_elapsed)
+
+}  // namespace common

common/types.h

@@ -1,144 +0,0 @@
-#pragma once
-
-#include <pcl/filters/voxel_grid.h>
-#include <pcl/kdtree/kdtree_flann.h>
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
-#include <pcl/visualization/pcl_visualizer.h>
-#include <pcl/visualization/point_cloud_handlers.h>
-
-#include <cmath>
-// Thses hpp file should be included if user-defined point type is added, see
-// "How are the point types exposed?" section in
-// https://pointclouds.org/documentation/tutorials/adding_custom_ptype.html
-#include <pcl/filters/impl/voxel_grid.hpp>
-#include <pcl/impl/pcl_base.hpp>
-#include <pcl/kdtree/impl/kdtree_flann.hpp>
-#include <pcl/visualization/impl/pcl_visualizer.hpp>
-#include <pcl/visualization/impl/point_cloud_geometry_handlers.hpp>
-
-namespace oh_my_loam {
-
-using Point = pcl::PointXYZ;
-using PointCloud = pcl::PointCloud<Point>;
-using PointCloudPtr = PointCloud::Ptr;
-using PointCloudConstPtr = PointCloud::ConstPtr;
-
-enum class PointType {
-  FLAT = -2,
-  LESS_FLAT = -1,
-  NORNAL = 0,
-  LESS_SHARP = 1,
-  SHARP = 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 PointXYZT {
-  PCL_ADD_POINT4D;
-  union EIGEN_ALIGN16 {
-    float time;
-    // make sure VoxelGrid can work with this custom point type:
-    // https://github.com/PointCloudLibrary/pcl/issues/2331
-    float intensity;
-  };
-
-  PointXYZT() {
-    x = y = z = 0.0f;
-    time = 0.0f;
-  }
-
-  PointXYZT(float x, float y, float z, float time = 0.0f)
-      : x(x), y(y), z(z), time(time) {}
-
-  PointXYZT(const Point& p) {
-    x = p.x;
-    y = p.y;
-    z = p.z;
-    time = 0.0f;
-  }
-
-  PointXYZT(const PointXYZT& p) {
-    x = p.x;
-    y = p.y;
-    z = p.z;
-    time = p.time;
-  }
-
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-} EIGEN_ALIGN16;
-
-struct PointXYZTCT {
-  PCL_ADD_POINT4D;
-  union EIGEN_ALIGN16 {
-    float data_c[4];
-    struct {
-      float time;
-      float curvature;
-      PointType type;
-    };
-  };
-
-  PointXYZTCT() {
-    x = y = z = 0.0f;
-    time = 0.0f;
-    curvature = std::nanf("");
-    type = PointType::NORNAL;
-  }
-
-  PointXYZTCT(float x, float y, float z, float time = 0.0f,
-              float curvature = std::nanf(""),
-              PointType type = PointType::NORNAL)
-      : x(x), y(y), z(z), time(time), curvature(curvature), type(type) {}
-
-  PointXYZTCT(const Point& p) {
-    x = p.x;
-    y = p.y;
-    z = p.z;
-    time = 0.0f;
-    curvature = std::nanf("");
-    type = PointType::NORNAL;
-  }
-
-  PointXYZTCT(const PointXYZTCT& p) {
-    x = p.x;
-    y = p.y;
-    z = p.z;
-    time = p.time;
-    curvature = p.curvature;
-    type = p.type;
-  }
-
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-} EIGEN_ALIGN16;
-
-}  // 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)
-)
-
-POINT_CLOUD_REGISTER_POINT_STRUCT(
-  oh_my_loam::PointXYZTCT,
-  (float, x, x)
-  (float, y, y)
-  (float, z, z)
-  (float, time, time)
-  (float, curvature, curvature)
-)
-// clang-format on

common/utils.h

@@ -1,89 +0,0 @@
-#pragma once
-
-#include "color.h"
-#include "log.h"
-#include "types.h"
-
-namespace oh_my_loam {
-
-template <typename PointT>
-inline double DistanceSqure(const PointT& pt) {
-  return pt.x * pt.x + pt.y * pt.y + pt.z * pt.z;
-}
-
-template <typename PointT>
-inline double DistanceSqure(const PointT& pt1, const PointT& pt2) {
-  return (pt1.x - pt2.x) * (pt1.x - pt2.x) + (pt1.y - pt2.y) * (pt1.y - pt2.y) +
-         (pt1.z - pt2.z) * (pt1.z - pt2.z);
-}
-
-template <typename PointT>
-inline double Distance(const PointT& pt) {
-  return std::sqrt(DistanceSqure(pt));
-}
-
-template <typename PointT>
-inline double Distance(const PointT& pt1, const PointT& pt2) {
-  return std::sqrt(DistanceSqure(pt1, pt2));
-}
-
-template <typename PointT>
-inline double IsFinite(const PointT& pt) {
-  return std::isfinite(pt.x) && std::isfinite(pt.y) && std::isfinite(pt.z);
-}
-
-// normalize an angle to [-pi, pi)
-double NormalizeAngle(double ang);
-
-// like Python built-in range, [begin, end)
-const std::vector<int> Range(int begin, int end, int step = 1);
-const std::vector<int> Range(int end);  // [0, end)
-
-template <typename PointT>
-void RemovePointsIf(const pcl::PointCloud<PointT>& cloud_in,
-                    pcl::PointCloud<PointT>* const cloud_out,
-                    std::function<bool(const PointT&)> cond) {
-  if (&cloud_in != cloud_out) {
-    cloud_out->header = cloud_in.header;
-    cloud_out->points.resize(cloud_in.size());
-  }
-  size_t j = 0;
-  for (size_t i = 0; i < cloud_in.size(); ++i) {
-    const auto pt = cloud_in.points[i];
-    if (cond(pt)) continue;
-    cloud_out->points[j++] = pt;
-  }
-
-  cloud_out->points.resize(j);
-  cloud_out->height = 1;
-  cloud_out->width = static_cast<uint32_t>(j);
-  cloud_out->is_dense = true;
-}
-
-template <typename PointT>
-void RemoveNaNPoint(const pcl::PointCloud<PointT>& cloud_in,
-                    pcl::PointCloud<PointT>* const cloud_out) {
-  RemovePointsIf<PointT>(cloud_in, cloud_out,
-                         [](const PointT& pt) { return !IsFinite(pt); });
-}
-
-template <typename PointT>
-void RemoveClosedPoints(const pcl::PointCloud<PointT>& cloud_in,
-                        pcl::PointCloud<PointT>* const cloud_out,
-                        double min_dist = 0.1) {
-  RemovePointsIf<PointT>(cloud_in, cloud_out, [&](const PointT& pt) {
-    return DistanceSqure(pt) < min_dist * min_dist;
-  });
-}
-
-template <typename PointT>
-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);
-  filter.setLeafSize(voxel_size, voxel_size, voxel_size);
-  filter.filter(*cloud_out);
-}
-
-}  // namespace oh_my_loam

common/visualizer/lidar_visualizer.h

@@ -1,39 +1,33 @@
 #pragma once
 
-#include <pcl/visualization/pcl_visualizer.h>
-#include <pcl/visualization/point_cloud_handlers.h>
-
 #include <atomic>
 #include <deque>
 #include <mutex>
-#include <string>
 #include <thread>
 
-#include "common.h"
+#include "lidar_visualizer_utils.h"
-#include "utils.h"
 
-namespace oh_my_loam {
+namespace common {
 
-struct VisFrame {
+struct LidarVisFrame {
-  double timestamp = std::nanf("");
+  double timestamp = 0.0;
   PointCloudPtr cloud = nullptr;
 };
 
-class Visualizer {
+class LidarVisualizer {
  public:
-  Visualizer(const std::string &name, size_t max_history_size)
+  explicit LidarVisualizer(const std::string &name,
+                           size_t max_history_size = 10)
       : name_(name), max_history_size_(max_history_size) {
-    // Start the thread to draw
     thread_.reset(new std::thread([&]() {
       viewer_.reset(new PCLVisualizer(name_));
-      // Set background color
+      // Set background color: black
-      const Color &bg_color = {0, 0, 0};
+      viewer_->setBackgroundColor(0, 0, 0);
-      viewer_->setBackgroundColor(bg_color.r, bg_color.g, bg_color.b);
+      // Set camera position
-      // Set camera position.
       viewer_->setCameraPosition(0, 0, 200, 0, 0, 0, 1, 0, 0, 0);
       viewer_->setSize(2500, 1500);
       viewer_->addCoordinateSystem(1.0);
-      // Add mouse and keyboard callback.
+      // Register keyboard callback
       viewer_->registerKeyboardCallback(
           [&](const pcl::visualization::KeyboardEvent &event) -> void {
             KeyboardEventCallback(event);
@@ -42,7 +36,7 @@ class Visualizer {
     }));
   }
 
-  virtual ~Visualizer() {
+  virtual ~LidarVisualizer() {
     is_stopped_ = true;
     viewer_->close();
     if (thread_->joinable()) {
@@ -50,7 +44,7 @@ class Visualizer {
     }
   }
 
-  void Render(const std::shared_ptr<VisFrame> &frame) {
+  void Render(const std::shared_ptr<LidarVisFrame> &frame) {
     std::lock_guard<std::mutex> lock(mutex_);
     while (history_frames_.size() > max_history_size_) {
       history_frames_.pop_back();
@@ -75,13 +69,12 @@ class Visualizer {
   }
 
   /**
-   * @brief Draw objects, pure virtual function. Example code:
+   * @brief Draw objects. This method should be overrided for customization
-   * virtual void Draw() {
-   *   auto frame = GetCurrentFrame<VisFrame>();
-   *   AddPointCloud(frame.cloud, {255, 255, 255}, "point cloud");
-   * }
    */
-  virtual void Draw() = 0;
+  virtual void Draw() {
+    auto frame = GetCurrentFrame<LidarVisFrame>();
+    DrawPointCloud(frame.cloud, {255, 255, 255}, "point cloud");
+  }
 
   /**
    * @brief Keyboard event callback function, This method should be overrided
@@ -111,10 +104,6 @@ class Visualizer {
     }
   }
 
-  /**
-   * @brief Remove all old rendered point clouds
-   *
-   */
   void RemoveRenderedObjects() {
     for (const auto &id : rendered_cloud_ids_) {
       viewer_->removePointCloud(id);
@@ -159,10 +148,11 @@ class Visualizer {
   std::atomic_bool is_updated_{false};
 
   // The visualizer frame list
-  std::deque<std::shared_ptr<VisFrame>> history_frames_;
+  std::deque<std::shared_ptr<LidarVisFrame>> history_frames_;
 
   // The current displayed frame iter.
-  typename std::deque<std::shared_ptr<VisFrame>>::iterator curr_frame_iter_;
+  typename std::deque<std::shared_ptr<LidarVisFrame>>::iterator
+      curr_frame_iter_;
 
   // The rendered cloud ids.
   std::vector<std::string> rendered_cloud_ids_;
@@ -174,4 +164,4 @@ class Visualizer {
   std::unique_ptr<PCLVisualizer> viewer_ = nullptr;
 };
 
-}  // namespace oh_my_loam
+}  // namespace common

common/visualizer/lidar_visualizer_utils.cc

@@ -0,0 +1,16 @@
+#include "lidar_visualizer_utils.h"
+
+namespace common {
+
+void AddLine(const pcl::PointXYZ& pt1, const pcl::PointXYZ& pt2,
+             const Color& color, const std::string& id,
+             PCLVisualizer* const viewer) {
+  viewer->addLine(pt1, pt2, color.r, color.g, color.b, id);
+}
+
+void DrawSphere(const pcl::PointXYZ& center, double radius, const Color& color,
+                const std::string& id, PCLVisualizer* const viewer) {
+  viewer->addSphere(center, radius, color.r, color.g, color.b, id);
+}
+
+}  // namespace common

common/visualizer/lidar_visualizer_utils.h

@@ -1,6 +1,15 @@
 #pragma once
 
-namespace oh_my_loam {
+#include <pcl/visualization/pcl_visualizer.h>
+#include <pcl/visualization/point_cloud_handlers.h>
+#include <pcl/visualization/impl/pcl_visualizer.hpp>
+#include <pcl/visualization/impl/point_cloud_geometry_handlers.hpp>
+#include <string>
+
+#include "common/color/color.h"
+#include "common/pcl/pcl_types.h"
+
+namespace common {
 
 using PCLVisualizer = pcl::visualization::PCLVisualizer;
 #define PCLColorHandlerCustom pcl::visualization::PointCloudColorHandlerCustom
@@ -28,10 +37,11 @@ void AddPointCloud(const typename pcl::PointCloud<PointType>::ConstPtr& cloud,
       pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, id);
 }
 
-template <typename PointType>
+void AddLine(const pcl::PointXYZ& pt1, const pcl::PointXYZ& pt2,
-void AddLine(const PointType& pt1, const PointType& pt2, const Color& color,
+             const Color& color, const std::string& id,
-             const std::string& id, PCLVisualizer* const viewer) {
+             PCLVisualizer* const viewer);
-  viewer->addLine(pt1, pt2, color.r, color.g, color.b, id);
-}
 
-}  // namespace oh_my_loam
+void DrawSphere(const pcl::PointXYZ& center, double radius, const Color& color,
+                const std::string& id, PCLVisualizer* const viewer);
+
+}  // namespace common

oh_my_loam/CMakeLists.txt

@@ -0,0 +1,17 @@
+message(STATUS "oh_my_loam dir: " ${CMAKE_CURRENT_SOURCE_DIR})
+
+include_directories(
+  ${CMAKE_CURRENT_SOURCE_DIR}
+)
+
+add_subdirectory(base)
+add_subdirectory(extractor)
+# add_subdirectory(visualizer)
+# add_subdirectory(odometry)
+# add_subdirectory(mapper)
+# add_subdirectory(solver)
+
+# aux_source_directory(. SRC_FILES)
+
+# add_library(oh_my_loam SHARED ${SRC_FILES})
+

oh_my_loam/base/CMakeLists.txt

@@ -0,0 +1,3 @@
+aux_source_directory(. SRC_FILES)
+
+add_library(base SHARED ${SRC_FILES})

oh_my_loam/base/feature.h

@@ -0,0 +1,21 @@
+#pragma once
+
+#include "common/pcl/pcl_types.h"
+
+namespace oh_my_loam {
+
+struct Feature {
+  common::TPointCloudPtr cloud_sharp_corner;
+  common::TPointCloudPtr cloud_less_sharp_corner;
+  common::TPointCloudPtr cloud_flat_surf;
+  common::TPointCloudPtr cloud_less_flat_surf;
+
+  FeaturePoints() {
+    cloud_sharp_corner.reset(new common::TPointCloud);
+    cloud_less_sharp_corner.reset(new common::TPointCloud);
+    cloud_flat_surf.reset(new common::TPointCloud);
+    cloud_less_flat_surf.reset(new common::TPointCloud);
+  }
+};
+
+}  // namespace oh_my_loam

oh_my_loam/base/helper.h

@@ -1,25 +1,38 @@
 #pragma once
 
-#include "common.h"
+#include "common/geometry/pose.h"
+#include "common/pcl/pcl_types.h"
 
 namespace oh_my_loam {
 
-inline float GetTime(const TPoint& pt) {
+using common::TPoint;
-  return pt.time - static_cast<int>(pt.time);
+using common::Pose3d;
-}
+using common::Trans3d;
+
+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) {
+  *pt_out = pt_in;
+  Eigen::Vector3d pnt = pose * Eigen::Vector3d(pt_in.x, pt_in.y, pt_in.z);
+  pt_out->x = pnt.x();
+  pt_out->y = pnt.y();
+  pt_out->z = pnt.z();
+}
+
 /**
  * @brief Transform a lidar point to the start of the scan
  *
  * @param pose Relative pose, end scan time w.r.t. start scan time
  * @param time Point time relative to the start time of the scan, \in [0, 1]
  */
-void TransformToStart(const Pose3D& pose, const TPoint& pt_in,
+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, GetTime(pt_in));
-  *pt_out = pose_interp * pt_in;
+  TransformPoint<TPoint>(pose_interp, pt_in, pt_out);
 }
 
 /**
@@ -28,10 +41,10 @@ void TransformToStart(const Pose3D& pose, const TPoint& pt_in,
  * @param pose Relative pose, end scan time w.r.t. start scan time
  * @param time Point time relative to the start time of the scan, \in [0, 1]
  */
-void TransformToEnd(const Pose3D& pose, const TPoint& pt_in,
+void TransformToEnd(const Pose3d& pose, const TPoint& pt_in,
                     TPoint* const pt_out) {
   TransformToStart(pose, pt_in, pt_out);
-  *pt_out = pose.Inv() * (*pt_out);
+  TransformPoint<TPoint>(pose.Inv(), *pt_out, pt_out);
 }
 
 struct PointLinePair {

oh_my_loam/base/types.h

@@ -0,0 +1,89 @@
+#pragma once
+
+#include "common/pcl/pcl_types.h"
+
+namespace oh_my_loam {
+
+enum class Type {
+  FLAT = -2,
+  LESS_FLAT = -1,
+  NORNAL = 0,
+  LESS_SHARP = 1,
+  SHARP = 2,
+};
+
+struct PointXYZTCT;
+using TCTPoint = PointXYZTCT;
+using TCTPointCloud = pcl::PointCloud<TCTPoint>;
+using TCTPointCloudPtr = TCTPointCloud::Ptr;
+using TCTPointCloudConstPtr = TCTPointCloud::ConstPtr;
+
+struct PointXYZTCT {
+  PCL_ADD_POINT4D;
+  union EIGEN_ALIGN16 {
+    struct {
+      float time;
+      float curvature;
+      Type type;
+    };
+    float data_c[4];
+  };
+
+  PointXYZTCT() {
+    x = y = z = 0.0f;
+    data[3] = 1.0f;
+    time = curvature = 0.0f;
+    type = Type::NORNAL;
+  }
+
+  PointXYZTCT(float x, float y, float z, float time = 0.0f,
+              float curvature = 0.0f, Type type = Type::NORNAL)
+      : x(x), y(y), z(z), time(time), curvature(curvature), type(type) {
+    data[3] = 1.0f;
+  }
+
+  PointXYZTCT(const common::Point& p) {
+    x = p.x;
+    y = p.y;
+    z = p.z;
+    data[3] = 1.0f;
+    time = 0.0f;
+    curvature = 0.0f;
+    type = Type::NORNAL;
+  }
+
+  PointXYZTCT(const common::TPoint& p) {
+    x = p.x;
+    y = p.y;
+    z = p.z;
+    data[3] = 1.0f;
+    time = p.time;
+    curvature = 0.0f;
+    type = Type::NORNAL;
+  }
+
+  PointXYZTCT(const PointXYZTCT& p) {
+    x = p.x;
+    y = p.y;
+    z = p.z;
+    data[3] = 1.0f;
+    time = p.time;
+    curvature = p.curvature;
+    type = p.type;
+  }
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+} EIGEN_ALIGN16;
+
+}  // namespace oh_my_loam
+
+// clang-format off
+POINT_CLOUD_REGISTER_POINT_STRUCT(
+  oh_my_loam::PointXYZTCT,
+  (float, x, x)
+  (float, y, y)
+  (float, z, z)
+  (float, time, time)
+  (float, curvature, curvature)
+)
+// clang-format on

oh_my_loam/configs/config.yaml

@@ -8,6 +8,7 @@ vis: true
 extractor_config:
   min_point_num: 66
   vis: false
+  verbose: false
   sharp_corner_point_num: 2
   corner_point_num: 20
   flat_surf_point_num: 4
@@ -24,5 +25,5 @@ odometry_config:
   vis: true
 
 # configs for mapper
-odometry_config: 
+mapper_config: 
   vis: false

oh_my_loam/extractor/CMakeLists.txt

@@ -1,3 +1,5 @@
 aux_source_directory(. SRC_FILES)
 
+message(STATUS ${SRC_FILES})
+
 add_library(extractor SHARED ${SRC_FILES})

oh_my_loam/extractor/extractor.cc

@@ -1,4 +1,6 @@
-#include "base_extractor.h"
+#include "extractor.h"
+
+#include "base/helper.h"
 
 #include <cmath>
 
@@ -7,53 +9,49 @@ namespace oh_my_loam {
 namespace {
 const int kScanSegNum = 6;
 const double kTwoPi = 2 * M_PI;
+using namespace common;
 }  // namespace
 
-bool Extractor::Init(const YAML::Node& config) {
+bool Extractor::Init() {
-  config_ = config;
+  const auto& config = YAMLConfig::Instance()->config();
-  is_vis_ = Config::Instance()->Get<bool>("vis") && config_["vis"].as<bool>();
+  config_ = config["extractor_config"];
+  is_vis_ = config["vis"].as<bool>() && config_["vis"].as<bool>();
+  verbose_ = config_["verbose"].as<bool>();
   AINFO << "Extraction visualizer: " << (is_vis_ ? "ON" : "OFF");
   if (is_vis_) visualizer_.reset(new ExtractorVisualizer);
   return true;
 }
 
-void Extractor::Process(const PointCloud& cloud, FeaturePoints* const feature) {
+void Extractor::Process(const PointCloudConstPtr& cloud,
-  if (cloud.size() < config_["min_point_num"].as<size_t>()) {
+                        Feature* const feature) {
-    AWARN << "Too few points ( < " << config_["min_point_num"].as<int>()
+  TIME_ELAPSED(__PRETTY_FUNCTION__);
-          << " ) after remove: " << cloud.size();
+  if (cloud->size() < config_["min_point_num"].as<size_t>()) {
+    AWARN << "Too few input points: num = " << cloud->size() << " (< "
+          << config_["min_point_num"].as<int>() << ")";
     return;
   }
-  TicToc timer;
   // split point cloud int scans
   std::vector<TCTPointCloud> scans;
-  SplitScan(cloud, &scans);
+  SplitScan(*cloud, &scans);
-  double time_split = timer.toc();
   // compute curvature for each point in each scan
   for (auto& scan : scans) {
     ComputePointCurvature(&scan);
   }
-  double time_curv = timer.toc();
+  // assign type to each point in each scan: FLAT, LESS_FLAT,
-  // assign type for each point in each scan, five types: FLAT, LESS_FLAT,
+  // NORMAL, LESS_SHARP or SHARP
-  // NORMAL, LESS_SHARP, SHARP
   for (auto& scan : scans) {
     AssignPointType(&scan);
   }
-  double time_assign = timer.toc();
   // store points into feature point clouds according to their type
   for (const auto& scan : scans) {
-    GenerateFeaturePoints(scan, feature);
+    GenerateFeature(scan, feature);
   }
-  double time_store = timer.toc();
-  AINFO << "Time elapsed (ms): scan_split = " << std::setprecision(3)
-        << time_split << ", curvature_compute = " << time_curv - time_split
-        << ", type_assign = " << time_assign - time_curv
-        << ", store = " << time_store - time_assign;
-  AUSER << "Time elapsed (ms): whole extraction = " << time_store;
   if (is_vis_) Visualize(cloud, *feature);
 }
 
 void Extractor::SplitScan(const PointCloud& cloud,
                           std::vector<TCTPointCloud>* const scans) const {
+  Timer timer;
   scans->resize(num_scans_);
   double yaw_start = -atan2(cloud.points[0].y, cloud.points[0].x);
   bool half_passed = false;
@@ -68,14 +66,15 @@ void Extractor::SplitScan(const PointCloud& cloud,
       half_passed = true;
       yaw_start += kTwoPi;
     }
-    (*scans)[scan_id].points.emplace_back(pt.x, pt.y, pt.z,
+    (*scans)[scan_id].points.emplace_back(
-                                          yaw_diff / kTwoPi + scan_id);
+        pt.x, pt.y, pt.z, yaw_diff / kTwoPi + scan_id, std::nanf(""));
   }
+  AINFO_IF(verbose_) << "Extractor::SplitScan: " << FMT_TIMESTAMP(timer.Toc())
+                     << " ms";
 }
 
-//
+void Extractor::ComputeCurvature(TCTPointCloud* const scan) const {
-void Extractor::ComputePointCurvature(TCTPointCloud* const scan,
+  Timer timer;
-                                      bool remove_nan) const {
   if (scan->size() < 20) return;
   auto& pts = scan->points;
   for (size_t i = 5; i < pts.size() - 5; ++i) {
@@ -90,12 +89,15 @@ void Extractor::ComputePointCurvature(TCTPointCloud* const scan,
                pts[i + 4].z + pts[i + 5].z - 10 * pts[i].z;
     pts[i].curvature = std::sqrt(dx * dx + dy * dy + dz * dz);
   }
-  RemovePointsIf<TCTPoint>(*scan, scan, [](const TCTPoint& pt) {
+  RemovePoints<TCTPoint>(*scan, scan, [](const TCTPoint& pt) {
     return !std::isfinite(pt.curvature);
   });
+  AINFO_IF(verbose_) << "Extractor::ComputeCurvature: "
+                     << FMT_TIMESTAMP(timer.Toc()) << " ms";
 }
 
-void Extractor::AssignPointType(TCTPointCloud* const scan) const {
+void Extractor::AssignType(TCTPointCloud* const scan) const {
+  Timer timer;
   int pt_num = scan->size();
   ACHECK(pt_num >= kScanSegNum);
   int seg_pt_num = (pt_num - 1) / kScanSegNum + 1;
@@ -153,6 +155,50 @@ void Extractor::AssignPointType(TCTPointCloud* const scan) const {
       }
     }
   }
+  AINFO_IF(verbose_) << "Extractor::AssignType: " << FMT_TIMESTAMP(timer.Toc())
+                     << " ms";
+}
+
+void Extractor::GenerateFeature(const TCTPointCloud& scan,
+                                Feature* const feature) const {
+  Timer timer;
+  TPointCloudPtr cloud_less_flat_surf(new TPointCloud);
+  for (const auto& pt : scan.points) {
+    TPoint point(pt.x, pt, y, pt.z, pt.time);
+    switch (pt.type) {
+      case PointType::FLAT:
+        feature->cloud_flat_surf->points.emplace_back(point);
+      // no break: FLAT points are also LESS_FLAT
+      case PointType::LESS_FLAT:
+        cloud_less_flat_surf->points.emplace_back(point);
+        break;
+      case PointType::SHARP:
+        feature->cloud_sharp_corner->points.emplace_back(point);
+      // no break: SHARP points are also LESS_SHARP
+      case PointType::LESS_SHARP:
+        feature->cloud_less_sharp_corner->points.emplace_back(point);
+        break;
+      default:
+        // all the rest are also LESS_FLAT
+        cloud_less_flat_surf->points.emplace_back(pt.x, pt.y, pt.z, pt.time);
+        break;
+    }
+  }
+  TPointCloudPtr dowm_sampled(new TPointCloud);
+  VoxelDownSample<TPoint>(cloud_less_flat_surf, dowm_sampled.get(),
+                          config_["downsample_voxel_size"].as<double>());
+  feature->cloud_less_flat_surf = dowm_sampled;
+  AINFO_IF(verbose_) << "Extractor::GenerateFeature: "
+                     << FMT_TIMESTAMP(timer.Toc()) << " ms";
+}
+
+void Extractor::Visualize(const PointCloudConstPtr& cloud,
+                          const Feature& feature, double timestamp) {
+  std::shared_ptr<ExtractorVisFrame> frame(new ExtractorVisFrame);
+  frame->timestamp = timestamp;
+  frame->cloud = cloud;
+  frame->feature = feature;
+  visualizer_->Render(frame);
 }
 
 void Extractor::SetNeighborsPicked(const TCTPointCloud& scan, size_t ix,
@@ -179,44 +225,4 @@ 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:
-        feature->flat_surf_pts->points.emplace_back(pt.x, pt.y, pt.z, pt.time);
-      // no break: FLAT points are also LESS_FLAT
-      case PointType::LESS_FLAT:
-        less_flat_surf_pts->points.emplace_back(pt.x, pt.y, pt.z, pt.time);
-        break;
-      case PointType::SHARP:
-        feature->sharp_corner_pts->points.emplace_back(pt.x, pt.y, pt.z,
-                                                       pt.time);
-      // no break: SHARP points are also LESS_SHARP
-      case PointType::LESS_SHARP:
-        feature->less_sharp_corner_pts->points.emplace_back(pt.x, pt.y, pt.z,
-                                                            pt.time);
-        break;
-      default:
-        // all the rest are also LESS_FLAT
-        less_flat_surf_pts->points.emplace_back(pt.x, pt.y, pt.z, pt.time);
-        break;
-    }
-  }
-  TPointCloudPtr filtered_less_flat_surf_pts(new TPointCloud);
-  VoxelDownSample<TPoint>(less_flat_surf_pts, filtered_less_flat_surf_pts.get(),
-                          config_["downsample_voxel_size"].as<double>());
-  *feature->less_flat_surf_pts += *filtered_less_flat_surf_pts;
-}
-
-void Extractor::Visualize(const PointCloud& cloud,
-                          const FeaturePoints& feature_pts, double timestamp) {
-  std::shared_ptr<ExtractorVisFrame> frame(new ExtractorVisFrame);
-  frame->timestamp = timestamp;
-  frame->cloud = cloud.makeShared();
-  frame->feature_pts = feature_pts;
-  visualizer_->Render(frame);
-}
-
 }  // namespace oh_my_loam

oh_my_loam/extractor/extractor.h

@@ -0,0 +1,54 @@
+#pragma once
+
+#include "base/feature.h"
+#include "base/types.h"
+#include "common/common.h"
+#include "visualizer/extractor_visualizer.h"
+
+namespace oh_my_loam {
+
+class Extractor {
+ public:
+  Extractor() = default;
+  virtual ~Extractor() = default;
+
+  bool Init();
+
+  void Process(const common::PointCloudConstPtr& cloud, Feature* const feature);
+
+  int num_scans() const { return num_scans_; }
+
+ protected:
+  virtual int GetScanID(const Point& pt) const = 0;
+
+  YAML::Node config_;
+
+  virtual void SplitScan(const common::PointCloud& cloud,
+                         std::vector<TCTPointCloud>* const scans) const;
+
+  virtual void ComputeCurvature(TCTPointCloud* const scan) const;
+
+  virtual void AssignType(TCTPointCloud* const scan) const;
+
+  virtual void GenerateFeature(const TCTPointCloud& scan,
+                               Feature* const feature) const;
+
+  int num_scans_ = 0;
+
+  std::unique_ptr<ExtractorVisualizer> visualizer_{nullptr};
+
+  bool verbose_ = false;
+
+ private:
+  void Visualize(const common::PointCloudConstPtr& cloud,
+                 const Feature& feature, double timestamp = 0.0);
+
+  void SetNeighborsPicked(const TCTPointCloud& scan, size_t ix,
+                          std::vector<bool>* const picked) const;
+
+  bool is_vis_ = false;
+
+  DISALLOW_COPY_AND_ASSIGN(Extractor);
+};
+
+}  // namespace oh_my_loam

oh_my_loam/extractor/extractor_VLP16.h

@@ -1,6 +1,6 @@
 #pragma once
 
-#include "base_extractor.h"
+#include "extractor.h"
 
 namespace oh_my_loam {
 

oh_my_loam/mapper/mapper.h

@@ -16,7 +16,7 @@ class Mapper {
   void Visualize();
 
   TPointCloudPtr map_pts_;
-  Pose3D pose_curr2world_;
+  Pose3d pose_curr2world_;
 
   bool is_initialized = false;
   bool is_vis_ = false;

oh_my_loam/odometer/CMakeLists.txt

@@ -0,0 +1,3 @@
+aux_source_directory(. SRC_FILES)
+
+add_library(odometer SHARED ${SRC_FILES})

oh_my_loam/odometer/odometer.cc

@@ -19,7 +19,7 @@ bool Odometry::Init(const YAML::Node& config) {
   return true;
 }
 
-void Odometry::Process(const FeaturePoints& feature, Pose3D* const pose) {
+void Odometry::Process(const FeaturePoints& feature, Pose3d* const pose) {
   if (!is_initialized_) {
     is_initialized_ = true;
     UpdatePre(feature);
@@ -64,7 +64,7 @@ void Odometry::Process(const FeaturePoints& feature, Pose3D* const pose) {
       solver.AddPointPlanePair(pair, GetTime(pair.pt));
     }
     solver.Solve();
-    pose_curr2last_ = Pose3D(q, p);
+    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

oh_my_loam/odometer/odometer.h

@@ -3,7 +3,6 @@
 #include "extractor/feature_points.h"
 #include "visualizer/odometry_visualizer.h"
 
-#include "common.h"
 #include "helper/helper.h"
 
 namespace oh_my_loam {
@@ -15,7 +14,7 @@ class Odometry {
 
   bool Init(const YAML::Node& config);
 
-  void Process(const FeaturePoints& feature, Pose3D* const pose);
+  void Process(const FeaturePoints& feature, Pose3d* const pose);
 
  protected:
   void UpdatePre(const FeaturePoints& feature);
@@ -32,8 +31,8 @@ class Odometry {
                  const std::vector<PointLinePair>& pl_pairs,
                  const std::vector<PointPlanePair>& pp_pairs) const;
 
-  Pose3D pose_curr2world_;
+  Pose3d pose_curr2world_;
-  Pose3D pose_curr2last_;
+  Pose3d pose_curr2last_;
 
   TPointCloudPtr surf_pts_pre_{nullptr};
   TPointCloudPtr corn_pts_pre_{nullptr};

oh_my_loam/oh_my_loam.cc

@@ -28,7 +28,7 @@ void OhMyLoam::Run(const PointCloud& cloud_in, double timestamp) {
   RemoveOutliers(cloud_in, cloud.get());
   FeaturePoints feature_points;
   extractor_->Process(*cloud, &feature_points);
-  Pose3D pose;
+  Pose3d pose;
   odometry_->Process(feature_points, &pose);
   poses_.emplace_back(pose);
 }

oh_my_loam/oh_my_loam.h

@@ -21,7 +21,7 @@ class OhMyLoam {
   // remove outliers: nan and very close points
   void RemoveOutliers(const PointCloud& cloud_in,
                       PointCloud* const cloud_out) const;
-  std::vector<Pose3D> poses_;
+  std::vector<Pose3d> poses_;
 
   DISALLOW_COPY_AND_ASSIGN(OhMyLoam)
 };

oh_my_loam/visualizer/extractor_visualizer.cc

@@ -0,0 +1,21 @@
+#include "extractor_visualizer.h"
+
+namespace oh_my_loam {
+namespace {
+using namespace common;
+}  // namespace
+
+void ExtractorVisualizer::Draw() {
+  auto frame = GetCurrentFrame<ExtractorVisFrame>();
+  DrawPointCloud<Point>(frame.cloud, WHITE, "cloud_raw");
+  DrawPointCloud<TPoint>(frame.feature.cloud_less_flat_surf, CYAN,
+                         "cloud_less_flat_surf");
+  DrawPointCloud<TPoint>(frame.feature.cloud_flat_surf, BLUE,
+                         "cloud_flat_surf");
+  DrawPointCloud<TPoint>(frame.feature.cloud_less_sharp_corner, PURPLE,
+                         "cloud_less_sharp_corner");
+  DrawPointCloud<TPoint>(frame.feature.cloud_sharp_corner, RED,
+                         "cloud_sharp_corner");
+};
+
+}  // namespace oh_my_loam

oh_my_loam/visualizer/extractor_visualizer.h

@@ -0,0 +1,22 @@
+#pragma once
+
+#include "base/feature.h"
+#include "common/visualizer/lidar_visualizer.h"
+
+namespace oh_my_loam {
+
+struct ExtractorVisFrame : public common::LidarVisFrame {
+  Feature feature;
+};
+
+class ExtractorVisualizer : public common::LidarVisualizer {
+ public:
+  explicit ExtractorVisualizer(const std::string &name = "ExtractorVisualizer",
+                               size_t max_history_size = 10)
+      : common::LidarVisualizer(name, max_history_size) {}
+
+ private:
+  void Draw() override;
+};
+
+}  // namespace oh_my_loam

oh_my_loam/visualizer/odometer_visualizer.cc

@@ -29,9 +29,9 @@ void OdometryVisualizer::Draw() {
   DrawPointCloud<TPoint>(tgt_corn_pts, BLUE, "tgt_corn_pts", 4);
   DrawPointCloud<TPoint>(src_surf_pts, PURPLE, "src_surf_pts", 7);
   DrawPointCloud<TPoint>(tgt_surf_pts, RED, "tgt_surf_pts", 4);
-  std::vector<Pose3D> poses_n;
+  std::vector<Pose3d> poses_n;
   poses_n.reserve((poses_.size()));
-  Pose3D pose_inv = frame.pose_curr2world.Inv();
+  Pose3d pose_inv = frame.pose_curr2world.Inv();
   for (const auto& pose : poses_) {
     poses_n.emplace_back(pose_inv * pose);
   };

oh_my_loam/visualizer/odometer_visualizer.h

@@ -1,17 +1,17 @@
 #pragma once
 
-#include "base_visualizer.h"
+#include "common/visualizer/lidar_visualizer.h"
 #include "helper/helper.h"
 
 namespace oh_my_loam {
 
-struct OdometryVisFrame : public VisFrame {
+struct OdometerVisFrame : public VisFrame {
   TPointCloudPtr surf_pts;
   TPointCloudPtr corn_pts;
   std::vector<PointLinePair> pl_pairs;
   std::vector<PointPlanePair> pp_pairs;
-  Pose3D pose_curr2last;
+  Pose3d pose_curr2last;
-  Pose3D pose_curr2world;
+  Pose3d pose_curr2world;
 };
 
 class OdometryVisualizer : public Visualizer {
@@ -22,7 +22,7 @@ class OdometryVisualizer : public Visualizer {
 
  private:
   void Draw() override;
-  std::deque<Pose3D> poses_;
+  std::deque<Pose3d> poses_;
 };
 
 }  // namespace oh_my_loam

src/CMakeLists.txt

@@ -1,11 +0,0 @@
-add_subdirectory(extractor)
-add_subdirectory(visualizer)
-add_subdirectory(odometry)
-add_subdirectory(mapper)
-add_subdirectory(helper)
-add_subdirectory(solver)
-
-aux_source_directory(. SRC_FILES)
-
-add_library(oh_my_loam SHARED ${SRC_FILES})
-

src/extractor/base_extractor.h

@@ -1,50 +0,0 @@
-#pragma once
-
-#include "feature_points.h"
-#include "visualizer/extractor_visualizer.h"
-
-namespace oh_my_loam {
-
-class Extractor {
- public:
-  Extractor() = default;
-  virtual ~Extractor() = default;
-
-  bool Init(const YAML::Node& config);
-  void Process(const PointCloud& cloud, FeaturePoints* const feature);
-
-  int num_scans() const { return num_scans_; }
-
- protected:
-  virtual int GetScanID(const Point& pt) const = 0;
-
-  int num_scans_ = 0;
-
-  YAML::Node config_;
-
- private:
-  void Visualize(const PointCloud& cloud, const FeaturePoints& feature_pts,
-                 double timestamp = std::nan(""));
-
-  void SplitScan(const PointCloud& cloud,
-                 std::vector<TCTPointCloud>* const scans) const;
-
-  void ComputePointCurvature(TCTPointCloud* const scan,
-                             bool remove_nan = true) const;
-
-  void AssignPointType(TCTPointCloud* const scan) const;
-
-  void SetNeighborsPicked(const TCTPointCloud& scan, size_t ix,
-                          std::vector<bool>* const picked) const;
-
-  void GenerateFeaturePoints(const TCTPointCloud& scan,
-                             FeaturePoints* const feature) const;
-
-  bool is_vis_ = false;
-
-  std::unique_ptr<ExtractorVisualizer> visualizer_{nullptr};
-
-  DISALLOW_COPY_AND_ASSIGN(Extractor);
-};
-
-}  // namespace oh_my_loam

src/extractor/feature_points.h

@@ -1,21 +0,0 @@
-#pragma once
-
-#include "types.h"
-
-namespace oh_my_loam {
-
-struct FeaturePoints {
-  TPointCloudPtr sharp_corner_pts;
-  TPointCloudPtr less_sharp_corner_pts;
-  TPointCloudPtr flat_surf_pts;
-  TPointCloudPtr less_flat_surf_pts;
-
-  FeaturePoints() {
-    sharp_corner_pts.reset(new TPointCloud);
-    less_sharp_corner_pts.reset(new TPointCloud);
-    flat_surf_pts.reset(new TPointCloud);
-    less_flat_surf_pts.reset(new TPointCloud);
-  }
-};
-
-}  // namespace oh_my_loam

src/helper/CMakeLists.txt

@@ -1,3 +0,0 @@
-aux_source_directory(. SRC_FILES)
-
-add_library(helper SHARED ${SRC_FILES})

src/odometry/CMakeLists.txt

@@ -1,3 +0,0 @@
-aux_source_directory(. SRC_FILES)
-
-add_library(odometry SHARED ${SRC_FILES})

src/visualizer/extractor_visualizer.cc

@@ -1,18 +0,0 @@
-#include "extractor_visualizer.h"
-
-namespace oh_my_loam {
-
-void ExtractorVisualizer::Draw() {
-  auto frame = GetCurrentFrame<ExtractorVisFrame>();
-  DrawPointCloud<Point>(frame.cloud, WHITE, "raw_point_cloud");
-  DrawPointCloud<TPoint>(frame.feature_pts.less_flat_surf_pts, CYAN,
-                         "less_flat_surf_pts");
-  DrawPointCloud<TPoint>(frame.feature_pts.flat_surf_pts, BLUE,
-                         "flat_surf_pts");
-  DrawPointCloud<TPoint>(frame.feature_pts.less_sharp_corner_pts, PURPLE,
-                         "less_sharp_corner_pts");
-  DrawPointCloud<TPoint>(frame.feature_pts.sharp_corner_pts, RED,
-                         "sharp_corner_pts");
-};
-
-}  // namespace oh_my_loam

src/visualizer/extractor_visualizer.h

@@ -1,22 +0,0 @@
-#pragma once
-
-#include "base_visualizer.h"
-#include "extractor/feature_points.h"
-
-namespace oh_my_loam {
-
-struct ExtractorVisFrame : public VisFrame {
-  FeaturePoints feature_pts;
-};
-
-class ExtractorVisualizer : public Visualizer {
- public:
-  explicit ExtractorVisualizer(const std::string &name = "ExtractorVisualizer",
-                               size_t max_history_size = 10)
-      : Visualizer(name, max_history_size) {}
-
- private:
-  void Draw() override;
-};
-
-}  // namespace oh_my_loam