extractor v1: ok

.gitignore

@@ -1,3 +1,3 @@
 /.vscode
-/.log/*
+/.log
 /build

CMakeLists.txt

@@ -54,6 +54,6 @@ target_link_libraries(main
   g3log
   common
   oh_my_loam
-  feature_points_extractor
+  extractor
   visualizer
 )

common/color.h

@@ -0,0 +1,23 @@
+#pragma once
+
+namespace oh_my_loam {
+
+struct Color {
+  unsigned char r, g, b;
+  Color(unsigned char r, unsigned char g, unsigned char 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 VIOLET Color(238, 130, 238)
+#define ORANGE Color(255, 97, 0)
+#define PINK Color(255, 182, 193)
+#define YELLOW Color(255, 255, 0)
+
+}  // namespace oh_my_loam

common/common.h

@@ -1,5 +1,6 @@
 #pragma once
 
+#include "color.h"
 #include "config.h"
 #include "log.h"
 #include "macros.h"

common/log.h

@@ -7,17 +7,21 @@
 #include <iostream>
 
 const LEVELS ERROR{WARNING.value + 100, "ERROR"};
+const LEVELS USER(ERROR.value + 100, "USER");
 
-#define ADEBUG LOG(DEBUG) << "[DEBUG] "
+#define ADEBUG LOG(DEBUG)
 #define AINFO LOG(INFO)
 #define AWARN LOG(WARNING)
 #define AERROR LOG(ERROR)
+#define AUSER LOG(USER)
 #define AFATAL LOG(FATAL)
 
 // LOG_IF
+#define ADEBUG_IF(cond) LOG_IF(DEBUG, cond)
 #define AINFO_IF(cond) LOG_IF(INFO, cond)
 #define AWARN_IF(cond) LOG_IF(WARNING, cond)
 #define AERROR_IF(cond) LOG_IF(ERROR, cond)
+#define AUSER_IF(cond) LOG_IF(USER, cond)
 #define AFATAL_IF(cond) LOG_IF(FATAL, cond)
 #define ACHECK(cond) CHECK(cond)
 
@@ -37,10 +41,11 @@ class CustomSink {
         << " shutdown at: ";
     auto now = std::chrono::system_clock::now();
     oss << g3::localtime_formatted(now, "%Y%m%d %H:%M:%S.%f3");
-    if (log_to_file_ && ofs_ != nullptr) {
+    if (log_to_file_) {
       (*ofs_) << oss.str() << std::endl;
-    }
+    } else {
       std::clog << oss.str() << std::endl;
+    }
   };
 
   void StdLogMessage(g3::LogMessageMover logEntry) {
@@ -48,7 +53,7 @@ class CustomSink {
   }
 
   void FileLogMessage(g3::LogMessageMover logEntry) {
-    if (log_to_file_ && ofs_ != nullptr) {
+    if (log_to_file_) {
       (*ofs_) << FormatedMessage(logEntry.get()) << std::endl;
     }
   }
@@ -67,25 +72,27 @@ class CustomSink {
 
   std::string ColorFormatedMessage(const g3::LogMessage& msg) const {
     std::ostringstream oss;
-    oss << "\033[" << GetColor(msg._level) << "m" << FormatedMessage(msg)
+    oss << GetColorCode(msg._level) << FormatedMessage(msg) << "\033[m";
-        << "\033[m";
     return oss.str();
   }
 
-  int GetColor(const LEVELS& level) const {
+  std::string GetColorCode(const LEVELS& level) const {
     if (level.value == WARNING.value) {
-      return 33;  // yellow
+      return "\033[33m";  // yellow
     }
     if (level.value == DEBUG.value) {
-      return 32;  // green
+      return "\033[32m";  // green
     }
     if (level.value == ERROR.value) {
-      return 31;  // red
+      return "\033[31m";  // red
+    }
+    if (level.value == USER.value) {
+      return "\033[1m\033[34m";  // bold blue
     }
     if (g3::internal::wasFatal(level)) {
-      return 31;  // red
+      return "\033[1m\033[31m";  // red
     }
-    return 97;  // white
+    return "\033[97m";  // white
   }
 };
 

common/tic_toc.h

@@ -15,7 +15,7 @@ class TicToc {
   double toc() {
     end_ = std::chrono::system_clock::now();
     std::chrono::duration<double> elapsed_seconds = end_ - start_;
-    return elapsed_seconds.count() * 1000;
+    return elapsed_seconds.count() * 1000;  // ms
   }
 
  private:

common/types.h

@@ -1,5 +1,7 @@
 #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>
@@ -26,20 +28,6 @@ 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;
@@ -59,7 +47,7 @@ struct PointXYZTCT {
     struct {
       float time;
       float curvature;
-      int type;  // -2, -1, 0, 1, or 2
+      PointType type;
     };
   };
 
@@ -67,12 +55,12 @@ struct PointXYZTCT {
     x = y = z = 0.0f;
     time = 0.0f;
     curvature = std::nanf("");
-    type = 0;
+    type = PointType::NORNAL;
-    data[3] = 1.0f;
   }
 
   PointXYZTCT(float x, float y, float z, float time = 0.0f,
-              float curvature = std::nanf(""), int type = 0)
+              float curvature = std::nanf(""),
+              PointType type = PointType::NORNAL)
       : x(x), y(y), z(z), time(time), curvature(curvature), type(type) {}
 
   PointXYZTCT(const Point& p) {
@@ -81,7 +69,7 @@ struct PointXYZTCT {
     z = p.z;
     time = 0.0f;
     curvature = std::nanf("");
-    type = 0;
+    type = PointType::NORNAL;
   }
 
   PointXYZTCT(const PointXYZTCT& p) {
@@ -93,8 +81,6 @@ struct PointXYZTCT {
     type = p.type;
   }
 
-  PointType Type() const { return static_cast<PointType>(type); }
-
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 } EIGEN_ALIGN16;
 
@@ -108,6 +94,6 @@ POINT_CLOUD_REGISTER_POINT_STRUCT(
   (float, z, z)
   (float, time, time)
   (float, curvature, curvature)
-  (int8_t, type, type)
+  // (int8_t, type, type)
 )
 // clang-format on

common/utils.cc

@@ -7,4 +7,16 @@ double NormalizeAngle(double ang) {
   return ang - two_pi * std::floor((ang + M_PI) / two_pi);
 }
 
-}  // oh_my_loam
+const std::vector<int> Range(int begin, int end, int step) {
+  ACHECK(step != 0) << "Step must non-zero";
+  int num = (end - begin) / step;
+  if (num <= 0) return {};
+  end = begin + step * num;
+  std::vector<int> seq(num);
+  for (int i = begin; i != end; i += step) seq[i] = i;
+  return seq;
+}
+
+const std::vector<int> Range(int end) { return Range(0, end, 1); }
+
+}  // namespace oh_my_loam

common/utils.h

@@ -1,5 +1,7 @@
 #pragma once
 
+#include "color.h"
+#include "log.h"
 #include "types.h"
 
 namespace oh_my_loam {
@@ -22,6 +24,10 @@ inline double IsFinite(const PointT& pt) {
 // 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 DrawPointCloud(const pcl::PointCloud<PointT>& cloud, const Color& color,
                     const std::string& id, PCLVisualizer* const viewer,
@@ -80,4 +86,14 @@ void RemoveClosedPoints(const pcl::PointCloud<PointT>& cloud_in,
   });
 }
 
+template <typename PointT>
+void VoxelDownSample(const pcl::PointCloud<PointT>& cloud_in,
+                     pcl::PointCloud<PointT>* const cloud_out,
+                     double voxel_size) {
+  pcl::VoxelGrid<PointT> filter;
+  filter.setInputCloud(cloud_in.makeShared());
+  filter.setLeafSize(voxel_size, voxel_size, voxel_size);
+  filter.filter(*cloud_out);
+}
+
 }  // namespace oh_my_loam

configs/config.yaml

@@ -1,10 +1,18 @@
 # global configs
 lidar: VPL16
-log_to_file: true
+log_to_file: false
-log_path: /home/liufei/catkin_ws/src/oh_my_loam/.log
+log_path: /home/lf/catkin_ws/src/oh_my_loam/.log
-vis: false
+vis: true
 
 # configs for feature points extractor
-feature_extractor_config:
+extractor_config:
-  min_scan_point_num: 66
+  min_point_num: 66
   vis: true
+  sharp_corner_point_num: 2
+  corner_point_num: 20
+  flat_surf_point_num: 4
+  surf_point_num: 1000
+  corner_point_curvature_thres: 0.5
+  surf_point_curvature_thres: 0.5
+  neighbor_point_dist_thres: 0.05
+  downsample_voxel_size: 0.2

main.cc

@@ -21,7 +21,7 @@ int main(int argc, char* argv[]) {
 
   // logging
   g3::InitG3Logging(log_to_file, "oh_my_loam_" + lidar, log_path);
-  AINFO << "Lidar: " << lidar;
+  AUSER << "LOAM start..., lidar = " << lidar;
 
   // SLAM system
   OhMyLoam slam;
@@ -44,7 +44,6 @@ void PointCloudHandler(const sensor_msgs::PointCloud2ConstPtr& msg,
                        OhMyLoam* const slam) {
   PointCloud cloud;
   pcl::fromROSMsg(*msg, cloud);
-  ADEBUG << "Point num = " << cloud.size()
+  AINFO << "Timestamp = " << LOG_TIMESTAMP(msg->header.stamp.toSec());
-         << ", ts = " << LOG_TIMESTAMP(msg->header.stamp.toSec());
   slam->Run(cloud, 0.0);
 }

src/CMakeLists.txt

@@ -1,4 +1,4 @@
-add_subdirectory(feature_points_extractor)
+add_subdirectory(extractor)
 add_subdirectory(visualizer)
 
 aux_source_directory(. SRC_FILES)

src/extractor/CMakeLists.txt

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

src/extractor/base_extractor.cc

@@ -0,0 +1,228 @@
+#include "base_extractor.h"
+
+#include <cmath>
+
+namespace oh_my_loam {
+
+namespace {
+const int kScanSegNum = 6;
+const double kTwoPi = 2 * M_PI;
+}  // namespace
+
+bool Extractor::Init(const YAML::Node& config) {
+  config_ = config;
+  is_vis_ = Config::Instance()->Get<bool>("vis") && config_["vis"].as<bool>();
+  AINFO << "Extraction visualizer: " << (is_vis_ ? "ON" : "OFF");
+  if (is_vis_) visualizer_.reset(new ExtractorVisualizer);
+  return true;
+}
+
+void Extractor::Extract(const PointCloud& cloud, FeaturePoints* const feature) {
+  if (cloud.size() < config_["min_point_num"].as<size_t>()) {
+    AWARN << "Too few points ( < " << config_["min_point_num"].as<int>()
+          << " ) after remove: " << cloud.size();
+    return;
+  }
+  TicToc timer;
+  // split point cloud int scans
+  std::vector<IPointCloud> 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 for each point in each scan, five types: FLAT, LESS_FLAT,
+  // 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
+  std::ostringstream oss;
+  oss << "Feature point num: ";
+  for (const auto& scan : scans) {
+    FeaturePoints scan_feature;
+    StoreToFeaturePoints(scan, &scan_feature);
+    feature->Add(scan_feature);
+    oss << scan.size() << ":" << scan_feature.sharp_corner_pts->size() << ":"
+        << scan_feature.less_sharp_corner_pts->size() << ":"
+        << scan_feature.flat_surf_pts->size() << ":"
+        << scan_feature.less_flat_surf_pts->size() << " ";
+  }
+  ADEBUG << oss.str();
+  double time_store = timer.toc();
+  AINFO << "Time elapsed (ms): scan_split = " << std::setprecision(3)
+        << time_split << ", curvature_compute = " << time_curv - time_split
+        << ", type_assign = " << time_assign - time_curv
+        << ", store = " << time_store - time_assign
+        << ", TOTAL = " << time_store;
+  if (is_vis_) Visualize(cloud, *feature);
+}
+
+void Extractor::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 Extractor::ComputePointCurvature(IPointCloud* const scan,
+                                      bool remove_nan) const {
+  if (scan->size() < 20) return;
+  auto& pts = scan->points;
+  for (size_t i = 5; i < pts.size() - 5; ++i) {
+    float dx = 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 dy = 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 dz = 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 = std::sqrt(dx * dx + dy * dy + dz * dz);
+  }
+  RemovePointsIf<IPoint>(*scan, scan, [](const IPoint& pt) {
+    return !std::isfinite(pt.curvature);
+  });
+}
+
+void Extractor::AssignPointType(IPointCloud* const scan) const {
+  int pt_num = scan->size();
+  ACHECK(pt_num >= kScanSegNum);
+  int seg_pt_num = (pt_num - 1) / kScanSegNum + 1;
+  std::vector<bool> picked(pt_num, false);
+  std::vector<int> indices = Range(pt_num);
+  int sharp_corner_point_num = config_["sharp_corner_point_num"].as<int>();
+  int corner_point_num = config_["corner_point_num"].as<int>();
+  int flat_surf_point_num = config_["flat_surf_point_num"].as<int>();
+  int surf_point_num = config_["surf_point_num"].as<int>();
+  float corner_point_curvature_thres =
+      config_["corner_point_curvature_thres"].as<float>();
+  float surf_point_curvature_thres =
+      config_["surf_point_curvature_thres"].as<float>();
+  for (int seg = 0; seg < kScanSegNum; ++seg) {
+    int s = seg * seg_pt_num;
+    int e = std::min((seg + 1) * seg_pt_num, pt_num);
+    // sort by curvature for each segment: large -> small
+    std::sort(indices.begin() + s, indices.begin() + e, [&](int i, int j) {
+      return scan->at(i).curvature > scan->at(j).curvature;
+    });
+    // pick corner points
+    int corner_pt_picked_num = 0;
+    for (int i = s; i < e; ++i) {
+      size_t ix = indices[i];
+      if (!picked.at(ix) &&
+          scan->at(ix).curvature > corner_point_curvature_thres) {
+        ++corner_pt_picked_num;
+        if (corner_pt_picked_num <= sharp_corner_point_num) {
+          scan->at(ix).type = PointType::SHARP;
+        } else if (corner_pt_picked_num <= corner_point_num) {
+          scan->at(ix).type = PointType::LESS_SHARP;
+        } else {
+          break;
+        }
+        picked.at(ix) = true;
+        SetNeighborsPicked(*scan, ix, &picked);
+      }
+    }
+    // pick surface points
+    int surf_pt_picked_num = 0;
+    for (int i = e - 1; i >= s; --i) {
+      size_t ix = indices[i];
+      if (!picked.at(ix) &&
+          scan->at(ix).curvature < surf_point_curvature_thres) {
+        ++surf_pt_picked_num;
+        if (surf_pt_picked_num <= flat_surf_point_num) {
+          scan->at(ix).type = PointType::FLAT;
+        } else if (surf_pt_picked_num <= surf_point_num) {
+          scan->at(ix).type = PointType::LESS_FLAT;
+        } else {
+          break;
+        }
+        picked.at(ix) = true;
+        SetNeighborsPicked(*scan, ix, &picked);
+      }
+    }
+  }
+}
+
+void Extractor::SetNeighborsPicked(const IPointCloud& scan, size_t ix,
+                                   std::vector<bool>* const picked) const {
+  auto DistSqure = [&](int i, int j) -> float {
+    float dx = scan.at(i).x - scan.at(j).x;
+    float dy = scan.at(i).y - scan.at(j).y;
+    float dz = scan.at(i).z - scan.at(j).z;
+    return dx * dx + dy * dy + dz * dz;
+  };
+  float neighbor_point_dist_thres =
+      config_["neighbor_point_dist_thres"].as<float>();
+  for (size_t i = 1; i <= 5; ++i) {
+    if (ix < i) break;
+    if (picked->at(ix - i)) continue;
+    if (DistSqure(ix - i, ix - i + 1) > neighbor_point_dist_thres) break;
+    picked->at(ix - i) = true;
+  }
+  for (size_t i = 1; i <= 5; ++i) {
+    if (ix + i >= scan.size()) break;
+    if (picked->at(ix + i)) continue;
+    if (DistSqure(ix + i, ix + i - 1) > neighbor_point_dist_thres) break;
+    picked->at(ix + i) = true;
+  }
+}
+
+void Extractor::StoreToFeaturePoints(const IPointCloud& scan,
+                                     FeaturePoints* const feature) const {
+  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);
+      // no break: FLAT points are also LESS_FLAT
+      case PointType::LESS_FLAT:
+        feature->less_flat_surf_pts->points.emplace_back(pt.x, pt.y, pt.z);
+        break;
+      case PointType::SHARP:
+        feature->sharp_corner_pts->points.emplace_back(pt.x, pt.y, pt.z);
+      // 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);
+        break;
+      default:
+        // all the rest are also LESS_FLAT
+        feature->less_flat_surf_pts->points.emplace_back(pt.x, pt.y, pt.z);
+        break;
+    }
+  }
+  PointCloudPtr filtered_less_flat_surf_pts(new PointCloud);
+  VoxelDownSample(*feature->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

src/extractor/base_extractor.h

@@ -0,0 +1,50 @@
+#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 Extract(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<IPointCloud>* const scans) const;
+
+  void ComputePointCurvature(IPointCloud* const scan,
+                             bool remove_nan = true) const;
+
+  void AssignPointType(IPointCloud* const scan) const;
+
+  void SetNeighborsPicked(const IPointCloud& scan, size_t ix,
+                          std::vector<bool>* const picked) const;
+
+  void StoreToFeaturePoints(const IPointCloud& 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/extractor_VLP16.cc

@@ -1,8 +1,8 @@
-#include "feature_points_extractor_VLP16.h"
+#include "extractor_VLP16.h"
 
 namespace oh_my_loam {
 
-int FeaturePointsExtractorVLP16::GetScanID(const Point& pt) const {
+int ExtractorVLP16::GetScanID(const Point& pt) const {
   static int i = 0;
   double omega = std::atan2(pt.z, Distance(pt)) * 180 * M_1_PI + 15.0;
   if (i++ < 10) {

src/extractor/extractor_VLP16.h

@@ -0,0 +1,16 @@
+#pragma once
+
+#include "base_extractor.h"
+
+namespace oh_my_loam {
+
+// for VLP-16
+class ExtractorVLP16 : public Extractor {
+ public:
+  ExtractorVLP16() { num_scans_ = 16; }
+
+ private:
+  int GetScanID(const Point& pt) const override;
+};
+
+}  // namespace oh_my_loam

src/extractor/feature_points.h

@@ -0,0 +1,28 @@
+#pragma once
+
+#include "common.h"
+
+namespace oh_my_loam {
+
+struct FeaturePoints {
+  PointCloudPtr sharp_corner_pts;
+  PointCloudPtr less_sharp_corner_pts;
+  PointCloudPtr flat_surf_pts;
+  PointCloudPtr less_flat_surf_pts;
+
+  FeaturePoints() {
+    sharp_corner_pts.reset(new PointCloud);
+    less_sharp_corner_pts.reset(new PointCloud);
+    flat_surf_pts.reset(new PointCloud);
+    less_flat_surf_pts.reset(new PointCloud);
+  }
+
+  void Add(const FeaturePoints& rhs) {
+    *sharp_corner_pts += *rhs.sharp_corner_pts;
+    *less_sharp_corner_pts += *rhs.less_sharp_corner_pts;
+    *flat_surf_pts += *rhs.flat_surf_pts;
+    *less_flat_surf_pts += *rhs.less_flat_surf_pts;
+  }
+};
+
+}  // namespace oh_my_loam

src/feature_points_extractor/CMakeLists.txt

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

src/feature_points_extractor/base_feature_points_extractor.cc

@@ -1,127 +0,0 @@
-#include "base_feature_points_extractor.h"
-
-#include <cmath>
-
-namespace oh_my_loam {
-
-namespace {
-const double kPointMinDist = 0.1;
-const int kScanSegNum = 6;
-const double kTwoPi = 2 * M_PI;
-const int kMinPtsNum = 100;
-}  // namespace
-
-bool FeaturePointsExtractor::Init(const YAML::Node& config) {
-  config_ = config;
-  is_vis_ = Config::Instance()->Get<bool>("vis") && config_["vis"].as<bool>();
-  AINFO << "Feature points extraction visualizer: " << (is_vis_ ? "ON" : "OFF");
-  if (is_vis_) visualizer_.reset(new FeaturePointsVisualizer);
-  return true;
-}
-
-void FeaturePointsExtractor::Extract(const PointCloud& cloud_in,
-                                     FeaturePoints* const feature) {
-  PointCloudPtr cloud(new PointCloud);
-  RemoveNaNPoint<Point>(cloud_in, cloud.get());
-  RemoveClosedPoints<Point>(*cloud, cloud.get(), kPointMinDist);
-  ADEBUG << "AFTER REMOVE, point num = " << cloud->size();
-  if (cloud->size() < kMinPtsNum) {
-    return;
-  }
-  std::vector<IPointCloud> scans;
-  SplitScan(*cloud, &scans);
-  std::ostringstream oss;
-  for (auto& scan : scans) {
-    oss << scan.size() << ":";
-    ComputePointCurvature(&scan);
-    RemovePointsIf<IPoint>(scan, &scan, [](const IPoint& pt) {
-      return !std::isfinite(pt.curvature);
-    });
-    oss << scan.size() << " ";
-    AssignPointType(&scan);
-  }
-  ADEBUG << oss.str();
-  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;
-      }
-    }
-  }
-  if (is_vis_) Visualize(cloud_in, *feature);
-}
-
-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 {
-  if (scan->size() < 20) return;
-  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 = std::sqrt(diffX * diffX + diffY * diffY + diffZ * diffZ);
-  }
-}
-
-void FeaturePointsExtractor::AssignPointType(IPointCloud* const scan) const {
-  // int pt_num = scan->size();
-  // int pt_num_seg = (pt_num - 1) / kScanSegNum + 1;
-  // std::vector<bool> picked(pt_num, false);
-  // for (int i = 0; i < kScanSegNum; ++i) {
-  //   int begin = i * pt_num_seg;
-  //   int end = std::max((i + 1) * pt_num_seg, pt_num);
-  // }
-}
-
-void FeaturePointsExtractor::Visualize(const PointCloud& cloud,
-                                       const FeaturePoints& feature_pts,
-                                       double timestamp) {
-  std::shared_ptr<FeaturePointsVisFrame> frame(new FeaturePointsVisFrame);
-  frame->timestamp = timestamp;
-  frame->cloud = cloud.makeShared();
-  frame->feature_pts = feature_pts;
-  visualizer_->Render(frame);
-}
-
-}  // namespace oh_my_loam

src/feature_points_extractor/base_feature_points_extractor.h

@@ -1,42 +0,0 @@
-#pragma once
-
-#include "feature_points.h"
-#include "visualizer/feature_points_visualizer.h"
-
-namespace oh_my_loam {
-
-class FeaturePointsExtractor {
- public:
-  FeaturePointsExtractor() = default;
-  virtual ~FeaturePointsExtractor() = default;
-
-  bool Init(const YAML::Node& config);
-  void Extract(const PointCloud& cloud_in, FeaturePoints* const feature);
-
-  int num_scans() const { return num_scans_; }
-
- protected:
-  virtual int GetScanID(const Point& pt) const = 0;
-
-  void Visualize(const PointCloud& cloud, const FeaturePoints& feature_pts,
-                 double timestamp = std::nan(""));
-
-  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;
-
-  bool is_vis_ = false;
-
-  std::unique_ptr<FeaturePointsVisualizer> visualizer_{nullptr};
-
-  YAML::Node config_;
-
-  DISALLOW_COPY_AND_ASSIGN(FeaturePointsExtractor);
-};
-
-}  // namespace oh_my_loam

src/feature_points_extractor/feature_points.h

@@ -1,23 +0,0 @@
-#pragma once
-
-#include "common.h"
-
-namespace oh_my_loam {
-
-struct FeaturePoints {
-  IPointCloudPtr feature_pts;  // all feature points
-  IPointCloudPtr sharp_corner_pts;
-  IPointCloudPtr less_sharp_corner_pts;
-  IPointCloudPtr flat_surf_pts;
-  IPointCloudPtr less_flat_surf_pts;
-
-  FeaturePoints() {
-    feature_pts.reset(new IPointCloud);
-    sharp_corner_pts.reset(new IPointCloud);
-    less_sharp_corner_pts.reset(new IPointCloud);
-    flat_surf_pts.reset(new IPointCloud);
-    less_flat_surf_pts.reset(new IPointCloud);
-  }
-};
-
-}  // namespace oh_my_loam

src/feature_points_extractor/feature_points_extractor_VLP16.h

@@ -1,16 +0,0 @@
-#pragma once
-
-#include "base_feature_points_extractor.h"
-
-namespace oh_my_loam {
-
-// for VLP-16
-class FeaturePointsExtractorVLP16 : public FeaturePointsExtractor {
- public:
-  FeaturePointsExtractorVLP16() { num_scans_ = 16; }
-
- private:
-  int GetScanID(const Point& pt) const override;
-};
-
-}  // namespace oh_my_loam

src/oh_my_loam.cc

@@ -1,21 +1,36 @@
 #include "oh_my_loam.h"
-#include "feature_points_extractor/feature_points_extractor_VLP16.h"
+
+#include "extractor/extractor_VLP16.h"
 
 namespace oh_my_loam {
 
+namespace {
+const double kPointMinDist = 0.1;
+}
+
 bool OhMyLoam::Init() {
   YAML::Node config = Config::Instance()->config();
-  feature_extractor_.reset(new FeaturePointsExtractorVLP16);
+  extractor_.reset(new ExtractorVLP16);
-  if (!feature_extractor_->Init(config["feature_extractor_config"])) {
+  if (!extractor_->Init(config["extractor_config"])) {
-    AERROR << "Failed to initialize feature points extractor";
+    AERROR << "Failed to initialize extractor";
     return false;
   }
   return true;
 }
 
-void OhMyLoam::Run(const PointCloud& cloud, double timestamp) {
+void OhMyLoam::Run(const PointCloud& cloud_in, double timestamp) {
-  FeaturePoints feature_pts;
+  PointCloudPtr cloud(new PointCloud);
-  feature_extractor_->Extract(cloud, &feature_pts);
+  RemoveOutliers(cloud_in, cloud.get());
+  ADEBUG << "After remove, point num: " << cloud_in.size() << " -> "
+         << cloud->size();
+  FeaturePoints feature_points;
+  extractor_->Extract(*cloud, &feature_points);
+}
+
+void OhMyLoam::RemoveOutliers(const PointCloud& cloud_in,
+                              PointCloud* const cloud_out) const {
+  RemoveNaNPoint<Point>(cloud_in, cloud_out);
+  RemoveClosedPoints<Point>(*cloud_out, cloud_out, kPointMinDist);
 }
 
 }  // namespace oh_my_loam

src/oh_my_loam.h

@@ -3,7 +3,7 @@
 #include <yaml-cpp/yaml.h>
 
 #include "common.h"
-#include "feature_points_extractor/base_feature_points_extractor.h"
+#include "extractor/base_extractor.h"
 
 namespace oh_my_loam {
 
@@ -13,10 +13,14 @@ class OhMyLoam {
 
   bool Init();
 
-  void Run(const PointCloud& cloud, double timestamp);
+  void Run(const PointCloud& cloud_in, double timestamp);
 
  private:
-  std::unique_ptr<FeaturePointsExtractor> feature_extractor_{nullptr};
+  std::unique_ptr<Extractor> extractor_{nullptr};
+
+  // remove outliers: nan and very close points
+  void RemoveOutliers(const PointCloud& cloud_in,
+                      PointCloud* const cloud_out) const;
 
   DISALLOW_COPY_AND_ASSIGN(OhMyLoam)
 };

src/visualizer/extractor_visualizer.cc

@@ -0,0 +1,38 @@
+#include "extractor_visualizer.h"
+
+namespace oh_my_loam {
+
+void ExtractorVisualizer::Draw() {
+  auto frame = GetCurrentFrame();
+  {  // add raw point cloud
+    std::string id = "raw point cloud";
+    DrawPointCloud(*frame.cloud, WHITE, id, viewer_.get());
+    rendered_cloud_ids_.push_back(id);
+  }
+  {  // add less_flat_surf_pts
+    std::string id = "less_flat_surf_pts";
+    DrawPointCloud(*frame.feature_pts.less_flat_surf_pts, CYAN, id,
+                   viewer_.get(), 5);
+    rendered_cloud_ids_.push_back(id);
+  }
+  {  // add flat_surf_pts
+    std::string id = "flat_surf_pts";
+    DrawPointCloud(*frame.feature_pts.flat_surf_pts, BLUE, id, viewer_.get(),
+                   7);
+    rendered_cloud_ids_.push_back(id);
+  }
+  {  // add less_sharp_corner_pts
+    std::string id = "less_sharp_corner_pts";
+    DrawPointCloud(*frame.feature_pts.less_sharp_corner_pts, PURPLE, id,
+                   viewer_.get(), 5);
+    rendered_cloud_ids_.push_back(id);
+  }
+  {  // add sharp_corner_pts
+    std::string id = "sharp_corner_pts";
+    DrawPointCloud(*frame.feature_pts.sharp_corner_pts, RED, id, viewer_.get(),
+                   7);
+    rendered_cloud_ids_.push_back(id);
+  }
+};
+
+}  // namespace oh_my_loam

src/visualizer/extractor_visualizer.h

@@ -0,0 +1,22 @@
+#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<ExtractorVisFrame> {
+ public:
+  explicit ExtractorVisualizer(const std::string &name = "ExtractorVisualizer",
+                               size_t max_history_size = 10)
+      : Visualizer<ExtractorVisFrame>(name, max_history_size) {}
+
+ private:
+  void Draw() override;
+};
+
+}  // namespace oh_my_loam

src/visualizer/feature_points_visualizer.cc

@@ -1,43 +0,0 @@
-#include "feature_points_visualizer.h"
-
-namespace oh_my_loam {
-
-void FeaturePointsVisualizer::Draw() {
-  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(*frame.feature_pts.feature_pts, "curvature", id,
-                   viewer_.get());
-    rendered_cloud_ids_.push_back(id);
-  }
-  {  // add flat_surf_pts
-    std::string id = "flat_surf_pts";
-    DrawPointCloud(*frame.feature_pts.flat_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_pts.less_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_pts.less_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);
-  }
-};
-
-}  // namespace oh_my_loam

src/visualizer/feature_points_visualizer.h

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