feature points extractor: writing...

.gitignore

@@ -0,0 +1 @@
+/.vscode

common/filter.h

@@ -0,0 +1,43 @@
+#pragma once
+
+#include "common/utils.h"
+
+namespace oh_loam {
+
+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.points.size());
+  }
+  size_t j = 0;
+  for (size_t i = 0; i < cloud_in.points.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->widht = 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(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(cloud_in, cloud_out, [](const PointT& pt) {
+    return DistanceSqure(pt) < min_dist * min_dist;
+  });
+}
+
+}  // oh_loam

common/tic_toc.h

@@ -0,0 +1,25 @@
+#pragma once
+
+#include <chrono>
+#include <cstdlib>
+#include <ctime>
+
+namespace oh_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;
+  }
+
+ private:
+  std::chrono::time_point<std::chrono::system_clock> start_, end_;
+};
+
+}  // oh_loam

common/types.h

@@ -0,0 +1,63 @@
+#pragma once
+
+#include <pcl/point_types.h>
+#include <cmath>
+
+namespace oh_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 EIGEN_ALIGN16 PointXYZTCT {
+  PCL_ADD_POINT4D;
+  float time = 0.0f;
+  float curvature = NAN;
+  char type = PointType::NORNAL;
+
+  PointXYZTCT() {
+    x = y = z = 0.0f;
+    data[3] = 1.0f;
+  }
+
+  PointXYZTCT(float x, float y, float z, float time = 0.0f,
+              float curvature = NAN, PointType type = PointType::NORNAL)
+      : x(x), y(y), z(z), time(time), curvature(curvature), type(type) {
+    data[3] = 1.0f;
+  }
+
+  PointXYZTCT(const Point& p) {
+    x = p.x;
+    y = p.y;
+    z = p.z;
+    data[3] = 1.0f;
+  }
+
+  PointXYZTCT(const PointXYZTCT& p) {
+    x = p.x;
+    y = p.y;
+    z = p.z;
+    time = p.time;
+    curvature = p.curvature;
+    type = p.type;
+    data[3] = 1.0f;
+  }
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+using IPoint = pcl::PointXYZTCT;
+using IPointCloud = pcl::PointCloud<IPoint>;
+using IPointCloudPtr = IPointCloud::Ptr;
+using IPointCloudConstPtr = IPointCloud::ConstPtr;
+
+}  // oh_loam

common/utils.h

@@ -0,0 +1,37 @@
+#pragma once
+
+#include "common/types.h"
+
+namespace oh_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 IsFinite(const PointT& pt) {
+  return std::isfinite(pt.x) && std::isfinite(pt.y) && std::isfinite(pt.z);
+}
+
+// normalize an angle to [-pi, pi)
+inline double NormalizeAngle(double ang) {
+  const double& two_pi = 2 * M_PI;
+  return ang - two_pi * std::floor((ang + M_PI) / two_pi);
+}
+
+std::pair<double, double> GetYawRange(const PointCloud& cloud) {
+  const auto& pts = cloud.points;
+  int pt_num = pts.size();
+  double yaw_start = -atan2(pts[0].y, pt[0].x);
+  double yaw_end = -atan2(pts[pt_num - 1].y, pts[pt_num - 1].x) + 2 * M_PI;
+  double yaw_diff = NormalizeAngle(yaw_end - yaw_start);
+  return {yaw_start, yaw_start + yaw_diff + 2 * M_PI};
+}
+
+}  // oh_loam

src/base_feature_extractor.cc

@@ -0,0 +1,93 @@
+#include "src/base_feature_extractor.h"
+
+#include <cmath>
+#include "common/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;
+  ScanSplit(*cloud, &scans);
+  for (auto& scan : sccans) {
+    ComputeCurvature(&scan);
+    AssignType(&scan);
+  }
+  for (const auto& scan : sccans) {
+    *(feature->laser_cloud) += scan;
+    for (const auto& pt : scan.points) {
+      switch (pt.type) {
+        case PointType::FLAT:
+          feature->flat_surf_points.emplace_back(pt);
+          break;
+        case PointType::LESS_FLAT:
+          feature->less_flat_surf_points.emplace_back(pt);
+          break;
+        case PointType::LESS_SHARP:
+          feature->less_sharp_corner_points.emplace_back(pt);
+          break;
+        case PointType::SHARP:
+          feature->sharp_corner_points.emplace_back(pt);
+          break;
+        default:
+          break;
+      }
+    }
+  }
+}
+
+void FeaturePointsExtractor::SplitScan(
+    const PointCloud& cloud, std::vector<IPointCloud>* const scans) const {
+  scans.resize(num_scans_);
+  const auto & [ 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].emplace_back(pt.x, pt.y, pt.z, yaw_diff / yaw_range);
+  }
+}
+
+void FeaturePointsExtractor::ComputePointCurvature(
+    IPointCloud* const scan) const {
+  auto& pts = scan->points;
+  for (int 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

@@ -0,0 +1,47 @@
+#pragma once
+
+#include "common/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 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

@@ -0,0 +1,20 @@
+#pragma once
+
+#include "common/utils.h"
+#include "src/base_feature_extractor.h"
+
+namespace oh_loam {
+
+// for VLP-16
+class FeatureExtractorVLP16 : public FeatureExtractor {
+ public:
+  FeatureExtractorVLP16() { num_scans_ = 16; }
+
+ private:
+  int GetScanID(const Point& pt) const override final {
+    double omega = std::atan2(pt.z, Distance(pt)) * 180 * M_1_PI + 15.0;
+    return static_cast<int>(std::round(omega) + 0.01);
+  }
+}
+
+}  // oh_loam