add ros

CMakeLists.txt

@@ -1,23 +1,47 @@
 cmake_minimum_required(VERSION 2.8.3)
-project(oh_loam)
+project(oh_my_loam)
 
 set(CMAKE_BUILD_TYPE "Release")
-set(CMAKE_CXX_FLAGS "-std=c++17")
+set(CMAKE_CXX_FLAGS "-std=c++17  -lpthread")
 set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g")
 
-find_package(PCL QUIET)
+find_package(Ceres REQUIRED)
-if (NOT PCL_FOUND) 
+find_package(PCL REQUIRED)
-  message(FATAL_ERROR "PCL not found.")
+
-endif()
+find_package(catkin REQUIRED COMPONENTS
+  geometry_msgs
+  nav_msgs
+  sensor_msgs
+  roscpp
+  rospy
+  rosbag
+  std_msgs
+  image_transport
+  cv_bridge
+  tf
+)
 
 include_directories(
-  ${PROJECT_SOURCE_DIR}/common
+	${catkin_INCLUDE_DIRS} 
-  ${PROJECT_SOURCE_DIR}/src
   ${PCL_INCLUDE_DIRS}
+  ${CERES_INCLUDE_DIRS}
+  src
+  common
 )
 
-set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib)
+catkin_package(
-
+  CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs
-add_library(${PROJECT_NAME} SHARED
+  DEPENDS EIGEN3 PCL 
-  ${PROJECT_SOURCE_DIR}/src/base_feature_extractor.cc
+  INCLUDE_DIRS src common
+)
+
+add_subdirectory(common)
+add_subdirectory(src)
+
+add_executable(oh_my_loam main.cc)
+target_link_libraries(oh_my_loam
+  ${catkin_LIBRARIES} 
+  ${PCL_LIBRARIES}
+  ${CERES_LIBRARIES}
+  OhMyLoam
 )

common/filter.h

@@ -2,7 +2,7 @@
 
 #include "utils.h"
 
-namespace oh_loam {
+namespace oh_my_loam {
 
 template <typename PointT>
 void RemovePointsIf(const pcl::PointCloud<PointT>& cloud_in,
@@ -10,14 +10,15 @@ void RemovePointsIf(const pcl::PointCloud<PointT>& cloud_in,
                     std::function<bool(const PointT&)> cond) {
   if (&cloud_in != cloud_out) {
     cloud_out->header = cloud_in.header;
-    cloud_out->points.resize(cloud_in.points.size());
+    cloud_out->points.resize(cloud_in.size());
   }
   size_t j = 0;
-  for (size_t i = 0; i < cloud_in.points.size(); ++i) {
+  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);
@@ -40,4 +41,4 @@ void RemoveClosedPoints(const pcl::PointCloud<PointT>& cloud_in,
   });
 }
 
-}  // oh_loam
+}  // oh_my_loam

common/tic_toc.h

@@ -4,7 +4,7 @@
 #include <cstdlib>
 #include <ctime>
 
-namespace oh_loam {
+namespace oh_my_loam {
 
 class TicToc {
  public:
@@ -22,4 +22,4 @@ class TicToc {
   std::chrono::time_point<std::chrono::system_clock> start_, end_;
 };
 
-}  // oh_loam
+}  // oh_my_loam

common/types.h

@@ -2,9 +2,11 @@
 
 #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>
 
-namespace oh_loam {
+namespace oh_my_loam {
 
 using Point = pcl::PointXYZ;
 using PointCloud = pcl::PointCloud<Point>;
@@ -19,11 +21,37 @@ enum class PointType {
   SHARP = 2,
 };
 
+struct Color {
+  uint8_t r, g, b;
+  Color(uint8_t r, uint8_t g, uint8_t b) : r(r), g(g), b(b) {}
+};
+#define BLACK Color(0, 0, 0)
+#define WHITE Color(255, 255, 255)
+#define RED Color(255, 0, 0)
+#define GREEN Color(0, 255, 0)
+#define BLUE Color(0, 0, 255)
+#define GRAY Color(50, 50, 50)
+#define CYAN Color(0, 255, 255)
+#define PURPLE Color(160, 32, 240)
+#define ORANGE Color(255, 97, 0)
+
+struct PointXYZTCT;
+
+using IPoint = PointXYZTCT;
+using IPointCloud = pcl::PointCloud<IPoint>;
+using IPointCloudPtr = IPointCloud::Ptr;
+using IPointCloudConstPtr = IPointCloud::ConstPtr;
+
+using PCLVisualizer = pcl::visualization::PCLVisualizer;
+#define PCLColorHandlerCustom pcl::visualization::PointCloudColorHandlerCustom
+#define PCLColorHandlerGenericField \
+  pcl::visualization::PointCloudColorHandlerGenericField
+
 struct EIGEN_ALIGN16 PointXYZTCT {
   PCL_ADD_POINT4D;
   float time = 0.0f;
-  float curvature = NAN;
+  float curvature = std::nanf("");
-  PointType type = PointType::NORNAL;
+  // PointType label = PointType::NORNAL;
 
   PointXYZTCT() {
     x = y = z = 0.0f;
@@ -31,8 +59,8 @@ struct EIGEN_ALIGN16 PointXYZTCT {
   }
 
   PointXYZTCT(float x, float y, float z, float time = 0.0f,
-              float curvature = NAN, PointType type = PointType::NORNAL)
+              float curvature = NAN)
-      : x(x), y(y), z(z), time(time), curvature(curvature), type(type) {
+      : x(x), y(y), z(z), time(time), curvature(curvature) {
     data[3] = 1.0f;
   }
 
@@ -49,16 +77,19 @@ struct EIGEN_ALIGN16 PointXYZTCT {
     z = p.z;
     time = p.time;
     curvature = p.curvature;
-    type = p.type;
+    // type = p.type;
     data[3] = 1.0f;
   }
 
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 
-using IPoint = PointXYZTCT;
+}  // oh_my_loam
-using IPointCloud = pcl::PointCloud<IPoint>;
-using IPointCloudPtr = IPointCloud::Ptr;
-using IPointCloudConstPtr = IPointCloud::ConstPtr;
 
-}  // oh_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, curvatur)
+)
+// clang-format on

common/utils.h

@@ -2,18 +2,18 @@
 
 #include "types.h"
 
-namespace oh_loam {
+namespace oh_my_loam {
-
-template <typename PointT>
-inline double Distance(const PointT& pt) {
-  return std::hypot(pt.x, pt.y, pt.z);
-}
 
 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 Distance(const PointT& pt) {
+  return std::sqrt(pt.x * pt.x + pt.y * pt.y + pt.z * pt.z);
+}
+
 template <typename PointT>
 inline double IsFinite(const PointT& pt) {
   return std::isfinite(pt.x) && std::isfinite(pt.y) && std::isfinite(pt.z);
@@ -25,13 +25,25 @@ inline double NormalizeAngle(double ang) {
   return ang - two_pi * std::floor((ang + M_PI) / two_pi);
 }
 
-std::pair<double, double> GetYawRange(const PointCloud& cloud) {
+template <typename PointT>
-  const auto& pts = cloud.points;
+void DrawPointCloud(const pcl::PointCloud<PointT>& cloud, const Color& color,
-  int pt_num = pts.size();
+                    const std::string& id, PCLVisualizer* const viewer,
-  double yaw_start = -atan2(pts[0].y, pts[0].x);
+                    int pt_size = 3) {
-  double yaw_end = -atan2(pts[pt_num - 1].y, pts[pt_num - 1].x) + 2 * M_PI;
+  PCLColorHandlerCustom<PointT> color_handler(cloud.makeShared(), color.r,
-  double yaw_diff = NormalizeAngle(yaw_end - yaw_start);
+                                              color.g, color.b);
-  return {yaw_start, yaw_start + yaw_diff + 2 * M_PI};
+  viewer->addPointCloud<PointT>(cloud.makeShared(), color_handler, id);
+  viewer->setPointCloudRenderingProperties(
+      pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pt_size, id);
 }
 
-}  // oh_loam
+template <typename PointT>
+void DrawPointCloud(const pcl::PointCloud<PointT>& cloud,
+                    const std::string& field, const std::string& id,
+                    PCLVisualizer* const viewer, int pt_size = 3) {
+  PCLColorHandlerGenericField<PointT> color_handler(cloud.makeShared(), field);
+  viewer->addPointCloud<PointT>(cloud.makeShared(), color_handler, id);
+  viewer->setPointCloudRenderingProperties(
+      pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pt_size, id);
+}
+
+}  // oh_my_loam

main.cc

@@ -0,0 +1,31 @@
+#include <pcl_conversions/pcl_conversions.h>
+#include <ros/ros.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <functional>
+#include "oh_my_loam.h"
+
+void PointCloudHandler(const sensor_msgs::PointCloud2ConstPtr& msg,
+                       oh_my_loam::OhMyLoam* const slam);
+
+int main(int argc, char* argv[]) {
+  oh_my_loam::OhMyLoam slam;
+  slam.Init();
+
+  ros::init(argc, argv, "oh_my_loam");
+  ros::NodeHandle nh;
+  ros::Subscriber sub_point_cloud = nh.subscribe<sensor_msgs::PointCloud2>(
+      "/velodyne_points", 100,
+      std::bind(PointCloudHandler, std::placeholders::_1, &slam));
+  ros::spin();
+
+  return 0;
+}
+
+void PointCloudHandler(const sensor_msgs::PointCloud2ConstPtr& msg,
+                       oh_my_loam::OhMyLoam* const slam) {
+  oh_my_loam::PointCloud cloud;
+  pcl::fromROSMsg(*msg, cloud);
+  ROS_INFO_STREAM("Point num = " << cloud.size()
+                                 << "ts = " << msg->header.stamp.toSec());
+  slam->Run(cloud, 0.0);
+}

package.xml

@@ -0,0 +1,39 @@
+<?xml version="1.0"?>
+<package>
+  <name>oh_my_loam</name>
+  <version>0.0.0</version>
+
+  <description>
+    Reimplementation of LOAM.
+  </description>
+
+  <maintainer email="fei-china@139.com">qintong</maintainer>
+
+  <license>BSD</license>
+
+  <author email="zhangji@cmu.edu">Ji Zhang</author>
+  
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>nav_msgs</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>  
+  <build_depend>rosbag</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>tf</build_depend>
+  <build_depend>image_transport</build_depend>
+  
+  <run_depend>geometry_msgs</run_depend>
+  <run_depend>nav_msgs</run_depend>
+  <run_depend>sensor_msgs</run_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>rospy</run_depend>
+  <run_depend>std_msgs</run_depend>
+  <run_depend>rosbag</run_depend>
+  <run_depend>tf</run_depend>
+  <run_depend>image_transport</run_depend>
+
+  <export>
+  </export>
+</package>

src/CMakeLists.txt

@@ -0,0 +1,7 @@
+include_directories(
+  ${PROJECT_SOURCE_DIR}/common
+)
+
+aux_source_directory(. SRC_FILES)
+
+add_library(OhMyLoam SHARED ${SRC_FILES})

src/base_feature_extractor.cc

@@ -1,95 +0,0 @@
-#include "base_feature_extractor.h"
-
-#include <cmath>
-#include "filter.h"
-
-namespace oh_loam {
-
-const double kPointMinDist = 0.1;
-const int kScanSegNum = 6;
-
-bool FeaturePointsExtractor::Extract(const PointCloud& cloud_in,
-                                     FeaturePoints* const feature) const {
-  PointCloudPtr cloud(new PointCloud);
-  RemoveNaNPoint<Point>(cloud_in, cloud.get());
-  RemoveClosedPoints<Point>(*cloud, cloud.get(), kPointMinDist);
-  std::vector<IPointCloud> scans;
-  SplitScan(*cloud, &scans);
-  for (auto& scan : scans) {
-    ComputePointCurvature(&scan);
-    AssignPointType(&scan);
-  }
-  for (const auto& scan : scans) {
-    *(feature->laser_cloud) += scan;
-    for (const auto& pt : scan.points) {
-      switch (pt.type) {
-        case PointType::FLAT:
-          feature->flat_surf_points->points.emplace_back(pt);
-          break;
-        case PointType::LESS_FLAT:
-          feature->less_flat_surf_points->points.emplace_back(pt);
-          break;
-        case PointType::LESS_SHARP:
-          feature->less_sharp_corner_points->points.emplace_back(pt);
-          break;
-        case PointType::SHARP:
-          feature->sharp_corner_points->points.emplace_back(pt);
-          break;
-        default:
-          break;
-      }
-    }
-  }
-}
-
-void FeaturePointsExtractor::SplitScan(
-    const PointCloud& cloud, std::vector<IPointCloud>* const scans) const {
-  scans->resize(num_scans_);
-  double yaw_start, yaw_end;
-  std::tie(yaw_start, yaw_end) = GetYawRange(cloud);
-  const double yaw_range = yaw_end - yaw_start;
-  bool half_passed = false;
-  for (const auto& pt : cloud.points) {
-    int scan_id = GetScanID(pt);
-    if (scan_id >= num_scans_ || scan_id < 0) continue;
-    double yaw = -atan2(pt.y, pt.x);
-    double yaw_diff = NormalizeAngle(yaw - yaw_start);
-    if (yaw_diff > 0) {
-      if (half_passed) yaw_start += 2 * M_PI;
-    } else {
-      half_passed = true;
-      yaw_start += 2 * M_PI;
-    }
-    (*scans)[scan_id].points.emplace_back(pt.x, pt.y, pt.z,
-                                          yaw_diff / yaw_range);
-  }
-}
-
-void FeaturePointsExtractor::ComputePointCurvature(
-    IPointCloud* const scan) const {
-  auto& pts = scan->points;
-  for (size_t i = 5; i < pts.size() - 5; ++i) {
-    float diffX = pts[i - 5].x + pts[i - 4].x + pts[i - 3].x + pts[i - 2].x +
-                  pts[i - 1].x + pts[i + 1].x + pts[i + 2].x + pts[i + 3].x +
-                  pts[i + 4].x + pts[i + 5].x - 10 * pts[i].x;
-    float diffY = pts[i - 5].y + pts[i - 4].y + pts[i - 3].y + pts[i - 2].y +
-                  pts[i - 1].y + pts[i + 1].y + pts[i + 2].y + pts[i + 3].y +
-                  pts[i + 4].y + pts[i + 5].y - 10 * pts[i].y;
-    float diffZ = pts[i - 5].z + pts[i - 4].z + pts[i - 3].z + pts[i - 2].z +
-                  pts[i - 1].z + pts[i + 1].z + pts[i + 2].z + pts[i + 3].z +
-                  pts[i + 4].z + pts[i + 5].z - 10 * pts[i].z;
-    pts[i].curvature = diffX * diffX + diffY * diffY + diffZ * diffZ;
-  }
-}
-
-void FeaturePointsExtractor::AssignPointType(IPointCloud* const scan) const {
-  int pt_num = scan->size();
-  int pt_num_seg = pt_num / kScanSegNum;
-  std::vector<bool> picked(pt_num, false);
-  for (int i = 0; i < kScanSegNum; ++i) {
-    int begin = i * pt_num_seg;
-    int end = i * pt_num_seg + pt_num_seg;
-  }
-}
-
-}  // oh_loam

src/base_feature_extractor.h

@@ -1,47 +0,0 @@
-#pragma once
-
-#include "utils.h"
-
-namespace oh_loam {
-
-struct FeaturePoints {
-  IPointCloudPtr laser_cloud;
-  IPointCloudPtr sharp_corner_points;
-  IPointCloudPtr less_sharp_corner_points;
-  IPointCloudPtr flat_surf_points;
-  IPointCloudPtr less_flat_surf_points;
-
-  FeaturePoints() {
-    laser_cloud.reset(new IPointCloud);
-    sharp_corner_points.reset(new IPointCloud);
-    less_sharp_corner_points.reset(new IPointCloud);
-    flat_surf_points.reset(new IPointCloud);
-    less_flat_surf_points.reset(new IPointCloud);
-  }
-};
-
-class FeaturePointsExtractor {
- public:
-  FeaturePointsExtractor() = default;
-  virtual ~FeaturePointsExtractor() = default;
-  FeaturePointsExtractor(const FeaturePointsExtractor&) = delete;
-  FeaturePointsExtractor& operator=(const FeaturePointsExtractor&) = delete;
-
-  bool Extract(const PointCloud& cloud_in, FeaturePoints* const feature) const;
-
-  int num_scans() const { return num_scans_; }
-
- protected:
-  virtual int GetScanID(const Point& pt) const = 0;
-
-  void SplitScan(const PointCloud& cloud,
-                 std::vector<IPointCloud>* const scans) const;
-
-  void ComputePointCurvature(IPointCloud* const scan) const;
-
-  void AssignPointType(IPointCloud* const scan) const;
-
-  int num_scans_ = 0;
-};
-
-}  // oh_loam

src/feature_extractor_VLP16.h

@@ -1,20 +1,27 @@
 #pragma once
 
-#include "base_feature_extractor.h"
+#include "feature_extractor_base.h"
 #include "utils.h"
 
-namespace oh_loam {
+namespace oh_my_loam {
 
 // for VLP-16
-class FeatureExtractorVLP16 : public FeatureExtractor {
+class FeatureExtractorVLP16 : public FeaturePointsExtractor {
  public:
   FeatureExtractorVLP16() { num_scans_ = 16; }
 
- private:
+ protected:
   int GetScanID(const Point& pt) const override final {
+    static int i = 0;
     double omega = std::atan2(pt.z, Distance(pt)) * 180 * M_1_PI + 15.0;
+    if (i++ < 10)
+      std::cout << "OMEGA: "
+                << std::atan2(pt.z, Distance(pt)) * 180 * M_1_PI + 15.0
+                << " id = " << static_cast<int>(std::round(omega) + 0.01)
+                << " z = " << pt.z << " "
+                << " d = " << Distance(pt) << std::endl;
     return static_cast<int>(std::round(omega) + 0.01);
   }
-}
+};
 
-}  // oh_loam
+}  // oh_my_loam

src/feature_extractor_base.cc

@@ -0,0 +1,101 @@
+#include "feature_extractor_base.h"
+
+#include <cmath>
+#include "filter.h"
+
+namespace oh_my_loam {
+
+const double kPointMinDist = 0.1;
+const int kScanSegNum = 6;
+const double kTwoPi = kTwoPi;
+const int kMinPtsNum = 100;
+
+void FeaturePointsExtractor::Extract(const PointCloud& cloud_in,
+                                     FeaturePoints* const feature) const {
+  PointCloudPtr cloud(new PointCloud);
+  std::cout << "BEFORE REMOVE, num = " << cloud->size() << std::endl;
+  RemoveNaNPoint<Point>(cloud_in, cloud.get());
+  RemoveClosedPoints<Point>(*cloud, cloud.get(), kPointMinDist);
+  std::cout << "AFTER REMOVE, num = " << cloud->size() << std::endl;
+  if (cloud->size() < kMinPtsNum) {
+    return;
+  }
+  std::vector<IPointCloud> scans;
+  SplitScan(*cloud, &scans);
+  for (auto& scan : scans) {
+    std::cout << scan.size() << "  ";
+    // ComputePointCurvature(&scan);
+    // AssignPointType(&scan);
+  }
+  std::cout << std::endl;
+  for (const auto& scan : scans) {
+    feature->feature_pts += scan;
+    // for (const auto& pt : scan.points) {
+    //   switch (pt.type) {
+    //     case PointType::FLAT:
+    //       feature->flat_surf_pts->points.emplace_back(pt);
+    //       break;
+    //     case PointType::LESS_FLAT:
+    //       feature->less_flat_surf_pts->points.emplace_back(pt);
+    //       break;
+    //     case PointType::LESS_SHARP:
+    //       feature->less_sharp_corner_pts->points.emplace_back(pt);
+    //       break;
+    //     case PointType::SHARP:
+    //       feature->sharp_corner_pts->points.emplace_back(pt);
+    //       break;
+    //     default:
+    //       break;
+    //   }
+    // }
+  }
+}
+
+void FeaturePointsExtractor::SplitScan(
+    const PointCloud& cloud, std::vector<IPointCloud>* const scans) const {
+  scans->resize(num_scans_);
+  double yaw_start = -atan2(cloud.points[0].y, cloud.points[0].x);
+  bool half_passed = false;
+  for (const auto& pt : cloud.points) {
+    int scan_id = GetScanID(pt);
+    if (scan_id >= num_scans_ || scan_id < 0) continue;
+    double yaw = -atan2(pt.y, pt.x);
+    double yaw_diff = NormalizeAngle(yaw - yaw_start);
+    if (yaw_diff > 0) {
+      if (half_passed) yaw_start += kTwoPi;
+    } else {
+      half_passed = true;
+      yaw_start += kTwoPi;
+    }
+    (*scans)[scan_id].points.emplace_back(pt.x, pt.y, pt.z, yaw_diff / kTwoPi);
+  }
+}
+
+// void FeaturePointsExtractor::ComputePointCurvature(
+//     IPointCloud* const scan) const {
+//   auto& pts = scan->points;
+//   for (size_t i = 5; i < pts.size() - 5; ++i) {
+//     float diffX = pts[i - 5].x + pts[i - 4].x + pts[i - 3].x + pts[i - 2].x +
+//                   pts[i - 1].x + pts[i + 1].x + pts[i + 2].x + pts[i + 3].x +
+//                   pts[i + 4].x + pts[i + 5].x - 10 * pts[i].x;
+//     float diffY = pts[i - 5].y + pts[i - 4].y + pts[i - 3].y + pts[i - 2].y +
+//                   pts[i - 1].y + pts[i + 1].y + pts[i + 2].y + pts[i + 3].y +
+//                   pts[i + 4].y + pts[i + 5].y - 10 * pts[i].y;
+//     float diffZ = pts[i - 5].z + pts[i - 4].z + pts[i - 3].z + pts[i - 2].z +
+//                   pts[i - 1].z + pts[i + 1].z + pts[i + 2].z + pts[i + 3].z +
+//                   pts[i + 4].z + pts[i + 5].z - 10 * pts[i].z;
+//     pts[i].curvature = diffX * diffX + diffY * diffY + diffZ * diffZ;
+//   }
+// }
+
+// void FeaturePointsExtractor::AssignPointType(IPointCloud* const scan) const {
+//   int pt_num = scan->size();
+//   int pt_num_seg = pt_num / kScanSegNum;
+//   std::vector<bool> picked(pt_num, false);
+//   for (int i = 0; i < kScanSegNum; ++i) {
+//     int begin = i * pt_num_seg;
+//     int end = i * pt_num_seg + pt_num_seg;
+//   }
+// }
+
+}  // oh_my_loam

src/feature_extractor_base.h

@@ -0,0 +1,39 @@
+#pragma once
+
+#include "utils.h"
+
+namespace oh_my_loam {
+
+struct FeaturePoints {
+  IPointCloud feature_pts;  // all feature points
+  IPointCloud sharp_corner_pts;
+  IPointCloud less_sharp_corner_pts;
+  IPointCloud flat_surf_pts;
+  IPointCloud less_flat_surf_pts;
+};
+
+class FeaturePointsExtractor {
+ public:
+  FeaturePointsExtractor() = default;
+  virtual ~FeaturePointsExtractor() = default;
+  FeaturePointsExtractor(const FeaturePointsExtractor&) = delete;
+  FeaturePointsExtractor& operator=(const FeaturePointsExtractor&) = delete;
+
+  void Extract(const PointCloud& cloud_in, FeaturePoints* const feature) const;
+
+  int num_scans() const { return num_scans_; }
+
+ protected:
+  virtual int GetScanID(const Point& pt) const = 0;
+
+  void SplitScan(const PointCloud& cloud,
+                 std::vector<IPointCloud>* const scans) const;
+
+  // void ComputePointCurvature(IPointCloud* const scan) const;
+
+  // void AssignPointType(IPointCloud* const scan) const;
+
+  int num_scans_ = 0;
+};
+
+}  // oh_my_loam

src/oh_my_loam.cc

@@ -0,0 +1,31 @@
+#include "oh_my_loam.h"
+
+namespace oh_my_loam {
+
+bool OhMyLoam::Init() {
+  is_vis_ = false;
+  feature_extractor_.reset(new FeatureExtractorVLP16);
+  if (is_vis_) {
+    visualizer_.reset(new FeaturePointsVisualizer);
+  }
+  return true;
+}
+
+void OhMyLoam::Run(const PointCloud& cloud, double timestamp) {
+  pcl::PointCloud<pcl::PointXYZ> cl;
+  std::shared_ptr<FeaturePoints> feature_pts(new FeaturePoints);
+  feature_extractor_->Extract(cloud, feature_pts.get());
+  if (is_vis_) Visualize(cloud, feature_pts, timestamp);
+}
+
+void OhMyLoam::Visualize(
+    const PointCloud& cloud,
+    const std::shared_ptr<const FeaturePoints>& feature_pts, double timestamp) {
+  std::shared_ptr<FeaturePointsVisFrame> frame;
+  frame->timestamp = timestamp;
+  frame->cloud = cloud.makeShared();
+  frame->feature_pts = feature_pts;
+  visualizer_->Render(frame);
+}
+
+}  // oh_my_loam

src/oh_my_loam.h

@@ -0,0 +1,29 @@
+#pragma once
+
+#include "feature_extractor_VLP16.h"
+#include "visualizer_feature_points.h"
+
+namespace oh_my_loam {
+
+class OhMyLoam {
+ public:
+  OhMyLoam() = default;
+  ~OhMyLoam() = default;
+  OhMyLoam(const OhMyLoam&) = delete;
+  OhMyLoam& operator=(const OhMyLoam&) = delete;
+
+  bool Init();
+
+  void Run(const PointCloud& cloud, double timestamp);
+
+ private:
+  void Visualize(const PointCloud& cloud,
+                 const std::shared_ptr<const FeaturePoints>& feature_pts,
+                 double timestamp = std::nanf(""));
+
+  bool is_vis_ = false;
+  std::unique_ptr<FeaturePointsExtractor> feature_extractor_ = nullptr;
+  std::unique_ptr<FeaturePointsVisualizer> visualizer_ = nullptr;
+};
+
+}  // oh_my_loam

src/visualizer_base.h

@@ -0,0 +1,163 @@
+#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 "types.h"
+
+namespace oh_my_loam {
+
+struct VisFrame {
+  double timestamp = std::nanf("");
+  PointCloudPtr cloud = nullptr;
+};
+
+template <typename FrameT>
+class Visualizer {
+ public:
+  Visualizer(const std::string &name, size_t max_history_size)
+      : 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
+      const Color &bg_color = {0, 0, 0};
+      viewer_->setBackgroundColor(bg_color.r, bg_color.g, bg_color.b);
+      // 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.
+      viewer_->registerKeyboardCallback(
+          [&](const pcl::visualization::KeyboardEvent &event) -> void {
+            KeyboardEventCallback(event);
+          });
+      Run();
+    }));
+  }
+
+  virtual ~Visualizer() {
+    is_stopped_ = true;
+    viewer_->close();
+    if (thread_->joinable()) {
+      thread_->join();
+    }
+  }
+
+  void Render(const std::shared_ptr<FrameT> &frame) {
+    std::lock_guard<std::mutex> lock(mutex_);
+    while (history_frames_.size() > max_history_size_) {
+      history_frames_.pop_back();
+    }
+    history_frames_.emplace_front(frame);
+    curr_frame_iter_ = history_frames_.begin();
+    is_updated_ = true;
+  }
+
+  std::string Name() { return name_; }
+
+ protected:
+  void Run() {
+    while (!is_stopped_) {
+      if (is_updated_) {
+        RemoveRenderedObjects();
+        Draw();
+        is_updated_ = false;
+      }
+      viewer_->spinOnce(20);  // ms
+    }
+  }
+
+  /**
+   * @brief Draw objects. This method should be overrided for customization.
+   *
+   * virtual void Draw() {
+   *   FrameT frame = GetCurrentFrame();
+   *   {  // Update cloud
+   *     std::string id = "point cloud";
+   *     DrawPointCloud(*frame.cloud, {255, 255, 255}, id, viewer_.get());
+   *     rendered_cloud_ids_.push_back(id);
+   *   }
+   * }
+   */
+  virtual void Draw() = 0;
+
+  /**
+   * @brief Keyboard event callback function, This method should be overrided
+   * for customization
+   *
+   * @param event Keyboard event
+   */
+  virtual void KeyboardEventCallback(
+      const pcl::visualization::KeyboardEvent &event) {
+    if (event.getKeySym() == "p" && event.keyDown()) {
+      std::lock_guard<std::mutex> lock(mutex_);
+      ++curr_frame_iter_;
+      if (curr_frame_iter_ == history_frames_.end()) {
+        --curr_frame_iter_;
+      }
+      is_updated_ = true;
+    } else if (event.getKeySym() == "n" && event.keyDown()) {
+      std::lock_guard<std::mutex> lock(mutex_);
+      if (curr_frame_iter_ != history_frames_.begin()) {
+        --curr_frame_iter_;
+      }
+      is_updated_ = true;
+    } else if (event.getKeySym() == "r" && event.keyDown()) {
+      viewer_->setCameraPosition(0, 0, 200, 0, 0, 0, 1, 0, 0, 0);
+      viewer_->setSize(2500, 1500);
+      is_updated_ = true;
+    }
+  }
+
+  /**
+   * @brief Remove all old rendered point clouds
+   *
+   */
+  void RemoveRenderedObjects() {
+    for (const auto &id : rendered_cloud_ids_) {
+      viewer_->removePointCloud(id);
+    }
+    rendered_cloud_ids_.clear();
+    viewer_->removeAllShapes();
+  }
+
+  FrameT GetCurrentFrame() const {
+    std::lock_guard<std::mutex> lock(mutex_);
+    return *(*curr_frame_iter_);
+  }
+
+  // visualizer name
+  std::string name_;
+  size_t max_history_size_;
+
+  // atomic bool variable
+  std::atomic_bool is_stopped_{false};
+
+  // mutex for visualizer_frame.
+  mutable std::mutex mutex_;
+
+  // bool flag indicates whether the Visualizer frame is updated.
+  std::atomic_bool is_updated_{false};
+
+  // The visualizer frame list
+  std::deque<std::shared_ptr<FrameT>> history_frames_;
+
+  // The current displayed frame iter.
+  typename std::deque<std::shared_ptr<FrameT>>::const_iterator curr_frame_iter_;
+
+  // The rendered cloud ids.
+  std::vector<std::string> rendered_cloud_ids_;
+
+  // thread for visualization
+  std::unique_ptr<std::thread> thread_ = nullptr;
+
+  // viewer
+  std::unique_ptr<PCLVisualizer> viewer_ = nullptr;
+};
+
+}  // oh_my_loam

src/visualizer_feature_points.h

@@ -0,0 +1,60 @@
+#pragma once
+
+#include "feature_extractor_base.h"
+#include "visualizer_base.h"
+
+namespace oh_my_loam {
+
+struct FeaturePointsVisFrame : public VisFrame {
+  std::shared_ptr<const FeaturePoints> feature_pts;
+};
+
+class FeaturePointsVisualizer : public Visualizer<FeaturePointsVisFrame> {
+ public:
+  explicit FeaturePointsVisualizer(
+      const std::string &name = "FeaturePointsVisFrame",
+      size_t max_history_size = 10)
+      : Visualizer<FeaturePointsVisFrame>(name, max_history_size) {}
+
+ protected:
+  void Draw() override final {
+    auto frame = GetCurrentFrame();
+    {  // add raw point cloud
+      std::string id = "raw point cloud";
+      DrawPointCloud<Point>(*frame.cloud, WHITE, id, viewer_.get());
+      rendered_cloud_ids_.push_back(id);
+    }
+    {  // add all feature_pts
+      std::string id = "feature_pts";
+      DrawPointCloud<IPoint>(frame.feature_pts->feature_pts, GRAY, id,
+                             viewer_.get());
+      rendered_cloud_ids_.push_back(id);
+    }
+    // {  // add flat_surf_pts
+    //   std::string id = "flat_surf_pts";
+    //   DrawPointCloud(*frame.feature_ptsflat_surf_pts, CYAN, id,
+    //   viewer_.get());
+    //   rendered_cloud_ids_.push_back(id);
+    // }
+    // {  // add less_flat_surf_pts
+    //   std::string id = "less_flat_surf_pts";
+    //   DrawPointCloud(*frame.feature_ptsless_flat_surf_pts, GREEN, id,
+    //   viewer_.get());
+    //   rendered_cloud_ids_.push_back(id);
+    // }
+    // {  // add less_sharp_corner_pts
+    //   std::string id = "less_sharp_corner_pts";
+    //   DrawPointCloud(*frame.feature_ptsless_sharp_corner_pts, ORANGE, id,
+    //   viewer_.get());
+    //   rendered_cloud_ids_.push_back(id);
+    // }
+    // {  // add sharp_corner_pts
+    //   std::string id = "sharp_corner_pts";
+    //   DrawPointCloud(*frame.feature_pts.sharp_corner_pts, ORANGE, id,
+    //   viewer_.get());
+    //   rendered_cloud_ids_.push_back(id);
+    // }
+  }
+};
+
+}  // oh_my_loam