refactoring: extractor ok

CMakeLists.txt

@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.15)
+cmake_minimum_required(VERSION 3.10)
 project(oh_my_loam)
 
 set(CMAKE_CXX_FLAGS "-std=c++17 -Wall -lpthread")

common/CMakeLists.txt

@@ -2,6 +2,4 @@ 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/config/yaml_config.h

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <yaml-cpp/yaml.h>
+
 #include <string>
 
 #include "common/log/log.h"

common/macro/macros.h

@@ -5,8 +5,7 @@
 #include <mutex>
 
 // format timestamp
-#define FMT_TIMESTAMP(timestamp) \
+#define FMT_TIMESTAMP(timestamp) std::fixed << std::setprecision(3) << timestamp
-  std::fixed << std::setprecision(3) << timestamp;
 
 #define DISALLOW_COPY_AND_ASSIGN(classname) \
   classname(const classname &) = delete;    \

common/pcl/pcl_types.h

@@ -71,6 +71,6 @@ POINT_CLOUD_REGISTER_POINT_STRUCT(
   (float, y, y)
   (float, z, z)
   (float, time, time)
-  // (float, intensity, intensity)
+  (float, intensity, intensity)
 )
 // clang-format on

common/pcl/pcl_utils.h

@@ -1,6 +1,6 @@
 #pragma once
 
-#include "log/log.h"
+#include "common/log/log.h"
 #include "pcl_types.h"
 
 namespace common {
@@ -38,16 +38,16 @@ inline double IsFinite(const PointType& pt) {
 
 // Remove point if the condition evaluated to true on it
 template <typename PointType>
-void RemovePoints(const typename pcl::PointCloud<PointType>::ConstPtr cloud_in,
+void RemovePoints(const pcl::PointCloud<PointType>& cloud_in,
                   std::function<bool(const PointType&)> check,
                   pcl::PointCloud<PointType>* const cloud_out) {
-  if (cloud_in.get() != cloud_out) {
+  if (&cloud_in != cloud_out) {
     cloud_out->header = cloud_in.header;
-    cloud_out->resize(cloud_in->size());
+    cloud_out->resize(cloud_in.size());
   }
   size_t j = 0;
-  for (size_t i = 0; i < cloud_in->size(); ++i) {
+  for (size_t i = 0; i < cloud_in.size(); ++i) {
-    const auto pt = cloud_in->points[i];
+    const auto pt = cloud_in.points[i];
     if (check(pt)) continue;
     cloud_out->points[j++] = pt;
   }

common/time/timer.cc

@@ -1,4 +1,5 @@
 #include "timer.h"
+
 #include "common/log/log.h"
 
 namespace common {
@@ -18,10 +19,10 @@ double Timer::Toc(char unit) {
 TimerWrapper::~TimerWrapper() {
   double duration = timer_.Toc();
   if (duration_ms_ < 0) {
-    AINFO << msg_ << ": time elapsed (ms): " << FMT_TIMESTAMP(duration);
+    AINFO << msg_ << ": time elapsed: " << FMT_TIMESTAMP(duration) << " ms";
   }
   if (duration_ms_ > 0 && duration > duration_ms_) {
-    AWARN << msg_ << ": time elapsed (ms): " << FMT_TIMESTAMP(duration);
+    AWARN << msg_ << ": time elapsed: " << FMT_TIMESTAMP(duration) << " ms";
   }
 }
 

common/time/timer.h

@@ -2,6 +2,7 @@
 
 #include <chrono>
 #include <string>
+
 #include "common/macro/macros.h"
 
 namespace common {

common/visualizer/lidar_visualizer.h

@@ -73,7 +73,7 @@ class LidarVisualizer {
    */
   virtual void Draw() {
     auto frame = GetCurrentFrame<LidarVisFrame>();
-    DrawPointCloud(frame.cloud, {255, 255, 255}, "point cloud");
+    DrawPointCloud<Point>(frame.cloud, WHITE, "point cloud");
   }
 
   /**

common/visualizer/lidar_visualizer_utils.h

@@ -2,6 +2,7 @@
 
 #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>

oh_my_loam/base/feature.h

@@ -10,7 +10,7 @@ struct Feature {
   common::TPointCloudPtr cloud_flat_surf;
   common::TPointCloudPtr cloud_less_flat_surf;
 
-  FeaturePoints() {
+  Feature() {
     cloud_sharp_corner.reset(new common::TPointCloud);
     cloud_less_sharp_corner.reset(new common::TPointCloud);
     cloud_flat_surf.reset(new common::TPointCloud);

oh_my_loam/extractor/extractor.cc

@@ -1,9 +1,10 @@
 #include "extractor.h"
 
-#include "base/helper.h"
-
 #include <cmath>
 
+#include "base/helper.h"
+#include "common/pcl/pcl_utils.h"
+
 namespace oh_my_loam {
 
 namespace {
@@ -35,12 +36,12 @@ void Extractor::Process(const PointCloudConstPtr& cloud,
   SplitScan(*cloud, &scans);
   // compute curvature for each point in each scan
   for (auto& scan : scans) {
-    ComputePointCurvature(&scan);
+    ComputeCurvature(&scan);
   }
   // assign type to each point in each scan: FLAT, LESS_FLAT,
   // NORMAL, LESS_SHARP or SHARP
   for (auto& scan : scans) {
-    AssignPointType(&scan);
+    AssignType(&scan);
   }
   // store points into feature point clouds according to their type
   for (const auto& scan : scans) {
@@ -89,9 +90,9 @@ void Extractor::ComputeCurvature(TCTPointCloud* const scan) const {
                pts[i + 4].z + pts[i + 5].z - 10 * pts[i].z;
     pts[i].curvature = std::sqrt(dx * dx + dy * dy + dz * dz);
   }
-  RemovePoints<TCTPoint>(*scan, scan, [](const TCTPoint& pt) {
+  RemovePoints<TCTPoint>(
-    return !std::isfinite(pt.curvature);
+      *scan, [](const TCTPoint& pt) { return !std::isfinite(pt.curvature); },
-  });
+      scan);
   AINFO_IF(verbose_) << "Extractor::ComputeCurvature: "
                      << FMT_TIMESTAMP(timer.Toc()) << " ms";
 }
@@ -126,9 +127,9 @@ void Extractor::AssignType(TCTPointCloud* const scan) const {
           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;
+          scan->at(ix).type = Type::SHARP;
         } else if (corner_pt_picked_num <= corner_point_num) {
-          scan->at(ix).type = PointType::LESS_SHARP;
+          scan->at(ix).type = Type::LESS_SHARP;
         } else {
           break;
         }
@@ -144,9 +145,9 @@ void Extractor::AssignType(TCTPointCloud* const scan) const {
           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;
+          scan->at(ix).type = Type::FLAT;
         } else if (surf_pt_picked_num <= surf_point_num) {
-          scan->at(ix).type = PointType::LESS_FLAT;
+          scan->at(ix).type = Type::LESS_FLAT;
         } else {
           break;
         }
@@ -164,18 +165,18 @@ void Extractor::GenerateFeature(const TCTPointCloud& scan,
   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);
+    TPoint point(pt.x, pt.y, pt.z, pt.time);
     switch (pt.type) {
-      case PointType::FLAT:
+      case Type::FLAT:
         feature->cloud_flat_surf->points.emplace_back(point);
       // no break: FLAT points are also LESS_FLAT
-      case PointType::LESS_FLAT:
+      case Type::LESS_FLAT:
         cloud_less_flat_surf->points.emplace_back(point);
         break;
-      case PointType::SHARP:
+      case Type::SHARP:
         feature->cloud_sharp_corner->points.emplace_back(point);
       // no break: SHARP points are also LESS_SHARP
-      case PointType::LESS_SHARP:
+      case Type::LESS_SHARP:
         feature->cloud_less_sharp_corner->points.emplace_back(point);
         break;
       default:
@@ -196,7 +197,7 @@ 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->cloud = cloud->makeShared();
   frame->feature = feature;
   visualizer_->Render(frame);
 }

oh_my_loam/extractor/extractor.h

@@ -19,7 +19,7 @@ class Extractor {
   int num_scans() const { return num_scans_; }
 
  protected:
-  virtual int GetScanID(const Point& pt) const = 0;
+  virtual int GetScanID(const common::Point& pt) const = 0;
 
   YAML::Node config_;
 

oh_my_loam/extractor/extractor_VLP16.cc

@@ -2,7 +2,7 @@
 
 namespace oh_my_loam {
 
-int ExtractorVLP16::GetScanID(const Point& pt) const {
+int ExtractorVLP16::GetScanID(const common::Point& pt) const {
   double omega = std::atan2(pt.z, std::hypot(pt.x, pt.y)) * 180 * M_1_PI + 15.0;
   return static_cast<int>(std::round(omega / 2.0) + 1.e-5);
 };

oh_my_loam/extractor/extractor_VLP16.h

@@ -10,7 +10,7 @@ class ExtractorVLP16 : public Extractor {
   ExtractorVLP16() { num_scans_ = 16; }
 
  private:
-  int GetScanID(const Point& pt) const override;
+  int GetScanID(const common::Point& pt) const override;
 };
 
 }  // namespace oh_my_loam