re-organize code structure

.gitignore

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

CMakeLists.txt

@@ -44,13 +44,16 @@ catkin_package(
 add_subdirectory(common)
 add_subdirectory(src)
 
-add_executable(oh_my_loam main.cc)
+add_executable(main main.cc)
-target_link_libraries(oh_my_loam
+target_link_libraries(main
   ${catkin_LIBRARIES} 
   ${PCL_LIBRARIES}
   ${CERES_LIBRARIES}
   ${G3LOG_LIBRARIES}
   ${YAML_CPP_LIBRARIES}
   g3log
-  OhMyLoam
+  common
+  oh_my_loam
+  feature_points_extractor
+  visualizer
 )

common/CMakeLists.txt

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

common/common.h

@@ -1,7 +1,8 @@
 #pragma once
 
-#include "filter.h"
+#include "config.h"
 #include "log.h"
+#include "macros.h"
 #include "tic_toc.h"
 #include "types.h"
 #include "utils.h"

common/config.h

@@ -5,9 +5,9 @@
 #include <string>
 
 #include "log.h"
-#include "micros.h"
+#include "macros.h"
 
-namespace oh_my_slam {
+namespace oh_my_loam {
 
 class Config {
  public:
@@ -17,7 +17,7 @@ class Config {
   }
 
   template <typename T>
-  const T& Get(const std::string& key) const {
+  const T Get(const std::string& key) const {
     AFATAL_IF(!config_) << "No config exists: please call SetConfigFile.";
     return (*config_)[key].as<T>();
   }
@@ -30,7 +30,7 @@ class Config {
  private:
   std::shared_ptr<YAML::Node> config_{nullptr};
 
-  DECLARE_SINGLETON(Config);
+  DECLARE_SINGLETON(Config)
 };
 
-}  // namespace oh_my_slam
+}  // namespace oh_my_loam

common/filter.h

@@ -1,44 +0,0 @@
-#pragma once
-
-#include "utils.h"
-
-namespace oh_my_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.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;
-  });
-}
-
-}  // namespace oh_my_loam

common/log.cc

@@ -0,0 +1,25 @@
+#include "log.h"
+
+namespace g3 {
+void InitG3Logging(bool log_to_file, const std::string& prefix,
+                   const std::string& path) {
+  static std::unique_ptr<g3::LogWorker> worker;
+  if (worker != nullptr) return;
+  worker = std::move(g3::LogWorker::createLogWorker());
+  worker->addSink(std::make_unique<g3::CustomSink>(),
+                  &g3::CustomSink::StdLogMessage);
+  if (log_to_file) {
+    std::ostringstream oss;
+    oss << path;
+    if (*path.rbegin() != '/') oss << '/';
+    if (prefix.empty()) oss << "g3log";
+    oss << prefix << ".";
+    auto now = std::chrono::system_clock::now();
+    oss << g3::localtime_formatted(now, "%Y%m%d-%H%M%S");
+    oss << ".log";
+    worker->addSink(std::make_unique<g3::CustomSink>(oss.str()),
+                    &g3::CustomSink::FileLogMessage);
+  }
+  g3::initializeLogging(worker.get());
+}
+}

common/log.h

@@ -1,17 +1,23 @@
 #pragma once
 
 #include <chrono>
+#include <fstream>
 #include <g3log/g3log.hpp>
 #include <g3log/logworker.hpp>
+#include <iostream>
+
+const LEVELS ERROR{WARNING.value + 100, "ERROR"};
 
 #define ADEBUG LOG(DEBUG) << "[DEBUG] "
 #define AINFO LOG(INFO)
 #define AWARN LOG(WARNING)
+#define AERROR LOG(ERROR)
 #define AFATAL LOG(FATAL)
 
 // LOG_IF
 #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 AFATAL_IF(cond) LOG_IF(FATAL, cond)
 #define ACHECK(cond) CHECK(cond)
 
@@ -19,10 +25,12 @@ namespace g3 {
 class CustomSink {
  public:
   CustomSink() = default;
+
   explicit CustomSink(const std::string& log_file_name)
       : log_file_name_(log_file_name), log_to_file_(true) {
     ofs_.reset(new std::ofstream(log_file_name));
   }
+
   ~CustomSink() {
     std::ostringstream oss;
     oss << "\ng3log " << (log_to_file_ ? "FileSink" : "StdSink")
@@ -71,6 +79,9 @@ class CustomSink {
     if (level.value == DEBUG.value) {
       return 32;  // green
     }
+    if (level.value == ERROR.value) {
+      return 31;  // red
+    }
     if (g3::internal::wasFatal(level)) {
       return 31;  // red
     }
@@ -78,25 +89,7 @@ class CustomSink {
   }
 };
 
-template <bool LogToFile>
+void InitG3Logging(bool log_to_file = false, const std::string& prefix = "",
-void InitG3Logging(const std::string& prefix, const std::string& path) {
+                   const std::string& path = "./");
-  static std::unique_ptr<g3::LogWorker> worker;
-  if (worker != nullptr) return;
-  worker = std::move(g3::LogWorker::createLogWorker());
-  worker->addSink(std::make_unique<g3::CustomSink>(),
-                  &g3::CustomSink::StdLogMessage);
-  if (LogToFile) {
-    std::ostringstream oss;
-    oss << path;
-    if (*path.rbegin() != '/') oss << '/';
-    oss << prefix << ".";
-    auto now = std::chrono::system_clock::now();
-    oss << g3::localtime_formatted(now, "%Y%m%d-%H%M%S");
-    oss << ".log";
-    worker->addSink(std::make_unique<g3::CustomSink>(oss.str()),
-                    &g3::CustomSink::FileLogMessage);
-  }
-  g3::initializeLogging(worker.get());
-}
 
 }  // namespace g3

common/macros.h

@@ -3,10 +3,14 @@
 #include <memory>
 #include <mutex>
 
+#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
 #define DECLARE_SINGLETON(classname)                                           \
  public:                                                                       \
-  static classname* Instance() {                                               \
+  static classname *Instance() {                                               \
     static std::unique_ptr<classname> instance = nullptr;                      \
     if (!instance) {                                                           \
       static std::once_flag flag;                                              \
@@ -18,5 +22,4 @@
                                                                                \
  private:                                                                      \
   classname() = default;                                                       \
-  classname(const classname&) = delete;                                        \
+  DISALLOW_COPY_AND_ASSIGN(classname)
-  classname& operator=(const classname&) = delete;

common/utils.cc

@@ -0,0 +1,10 @@
+#include "utils.h"
+
+namespace oh_my_loam {
+
+double NormalizeAngle(double ang) {
+  const double& two_pi = 2 * M_PI;
+  return ang - two_pi * std::floor((ang + M_PI) / two_pi);
+}
+
+}  // oh_my_loam

common/utils.h

@@ -20,10 +20,7 @@ inline double IsFinite(const PointT& pt) {
 }
 
 // normalize an angle to [-pi, pi)
-inline double NormalizeAngle(double ang) {
+double NormalizeAngle(double ang);
-  const double& two_pi = 2 * M_PI;
-  return ang - two_pi * std::floor((ang + M_PI) / two_pi);
-}
 
 template <typename PointT>
 void DrawPointCloud(const pcl::PointCloud<PointT>& cloud, const Color& color,
@@ -46,4 +43,41 @@ void DrawPointCloud(const pcl::PointCloud<PointT>& cloud,
       pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pt_size, id);
 }
 
+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;
+  });
+}
+
 }  // namespace oh_my_loam

config/config.yaml

@@ -1,4 +0,0 @@
-lidar: VPL16
-
-feature_points_extractor_config:
-  min_scan_point_num: 66

configs/config.yaml

@@ -0,0 +1,10 @@
+# global configs
+lidar: VPL16
+log_to_file: true
+log_path: /home/liufei/catkin_ws/src/oh_my_loam/.log
+vis: false
+
+# configs for feature points extractor
+feature_extractor_config:
+  min_scan_point_num: 66
+  vis: true

main.cc

@@ -1,27 +1,38 @@
 #include <pcl_conversions/pcl_conversions.h>
 #include <ros/ros.h>
 #include <sensor_msgs/PointCloud2.h>
-#include <yaml-cpp/yaml.h>
 
 #include <functional>
 
-#include "log.h"
+#include "common/common.h"
-#include "oh_my_loam.h"
+#include "src/oh_my_loam.h"
+
+using namespace oh_my_loam;
 
 void PointCloudHandler(const sensor_msgs::PointCloud2ConstPtr& msg,
-                       oh_my_loam::OhMyLoam* const slam);
+                       OhMyLoam* const slam);
 
 int main(int argc, char* argv[]) {
-  // configurations
+  // config
-  YAML::Node config = YAML::LoadFile("./config/config.yaml");
+  Config::Instance()->SetConfigFile("configs/config.yaml");
+  bool log_to_file = Config::Instance()->Get<bool>("log_to_file");
+  std::string log_path = Config::Instance()->Get<std::string>("log_path");
+  std::string lidar = Config::Instance()->Get<std::string>("lidar");
 
   // logging
-  g3::InitG3Logging<true>("oh_my_loam", ".log");
+  g3::InitG3Logging(log_to_file, "oh_my_loam_" + lidar, log_path);
-  AWARN << config["lidar"].as<std::string>();
+  AINFO << "Lidar: " << lidar;
 
   // SLAM system
-  oh_my_loam::OhMyLoam slam;
+  OhMyLoam slam;
-  slam.Init(config["feature_points_extractor_config"]);
+  if (!slam.Init()) {
+    AFATAL << "Failed to initilize slam system.";
+  }
+  ADEBUG << "DEBUG";
+  AINFO << "INFO";
+  AWARN << "WARN";
+  AERROR << "ERROR";
+  AFATAL << "FATAL";
 
   // ros
   ros::init(argc, argv, "oh_my_loam");
@@ -35,8 +46,8 @@ int main(int argc, char* argv[]) {
 }
 
 void PointCloudHandler(const sensor_msgs::PointCloud2ConstPtr& msg,
-                       oh_my_loam::OhMyLoam* const slam) {
+                       OhMyLoam* const slam) {
-  oh_my_loam::PointCloud cloud;
+  PointCloud cloud;
   pcl::fromROSMsg(*msg, cloud);
   AINFO << "Point num = " << cloud.size()
         << ", ts = " << msg->header.stamp.toSec();

src/CMakeLists.txt

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

src/feature_extractor_VLP16.h

@@ -1,26 +0,0 @@
-#pragma once
-
-#include "feature_extractor_base.h"
-
-namespace oh_my_loam {
-
-// for VLP-16
-class FeatureExtractorVLP16 : public FeaturePointsExtractor {
- public:
-  FeatureExtractorVLP16() { num_scans_ = 16; }
-
- 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) {
-      AWARN << "OMEGA: " << std::atan2(pt.z, Distance(pt)) * 180 * M_1_PI + 15.0
-            << " id = " << static_cast<int>(std::round(omega / 2.0) + 0.01)
-            << " z = " << pt.z << " "
-            << " d = " << Distance(pt) << std::endl;
-    }
-    return static_cast<int>(std::round(omega / 2.0) + 1.e-5);
-  }
-};
-
-}  // namespace oh_my_loam

src/feature_points_extractor/CMakeLists.txt

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

src/feature_points_extractor/base_feature_points_extractor.cc

@@ -1,9 +1,7 @@
-#include "feature_extractor_base.h"
+#include "base_feature_points_extractor.h"
 
 #include <cmath>
 
-#include "filter.h"
-
 namespace oh_my_loam {
 
 namespace {
@@ -15,11 +13,14 @@ const int kMinPtsNum = 100;
 
 bool FeaturePointsExtractor::Init(const YAML::Node& config) {
   config_ = config;
+  is_vis_ = Config::Instance()->Get<bool>("vis") && config_["vis"].as<bool>();
+  AINFO << "Feature points visualization: " << (is_vis_ ? "ON" : "OFF");
+  if (is_vis_) visualizer_.reset(new FeaturePointsVisualizer);
   return true;
 }
 
 void FeaturePointsExtractor::Extract(const PointCloud& cloud_in,
-                                     FeaturePoints* const feature) const {
+                                     FeaturePoints* const feature) {
   PointCloudPtr cloud(new PointCloud);
   AINFO << "BEFORE REMOVE, num = " << cloud_in.size();
   RemoveNaNPoint<Point>(cloud_in, cloud.get());
@@ -42,26 +43,27 @@ void FeaturePointsExtractor::Extract(const PointCloud& cloud_in,
   }
   AWARN << oss.str();
   for (const auto& scan : scans) {
-    feature->feature_pts += scan;
+    *feature->feature_pts += scan;
     for (const auto& pt : scan.points) {
       switch (pt.Type()) {
         case PointType::FLAT:
-          feature->flat_surf_pts.points.emplace_back(pt);
+          feature->flat_surf_pts->points.emplace_back(pt);
           break;
         case PointType::LESS_FLAT:
-          feature->less_flat_surf_pts.points.emplace_back(pt);
+          feature->less_flat_surf_pts->points.emplace_back(pt);
           break;
         case PointType::LESS_SHARP:
-          feature->less_sharp_corner_pts.points.emplace_back(pt);
+          feature->less_sharp_corner_pts->points.emplace_back(pt);
           break;
         case PointType::SHARP:
-          feature->sharp_corner_pts.points.emplace_back(pt);
+          feature->sharp_corner_pts->points.emplace_back(pt);
           break;
         default:
           break;
       }
     }
   }
+  if (is_vis_) Visualize(cloud_in, *feature);
 }
 
 void FeaturePointsExtractor::SplitScan(
@@ -104,13 +106,23 @@ void FeaturePointsExtractor::ComputePointCurvature(
 }
 
 void FeaturePointsExtractor::AssignPointType(IPointCloud* const scan) const {
-  int pt_num = scan->size();
+  // int pt_num = scan->size();
-  int pt_num_seg = (pt_num - 1) / kScanSegNum + 1;
+  // int pt_num_seg = (pt_num - 1) / kScanSegNum + 1;
-  std::vector<bool> picked(pt_num, false);
+  // std::vector<bool> picked(pt_num, false);
-  for (int i = 0; i < kScanSegNum; ++i) {
+  // for (int i = 0; i < kScanSegNum; ++i) {
-    int begin = i * pt_num_seg;
+  //   int begin = i * pt_num_seg;
-    int end = std::max((i + 1) * pt_num_seg, pt_num);
+  //   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,32 +1,26 @@
 #pragma once
 
-#include "common.h"
+#include "feature_points.h"
+#include "visualizer/feature_points_visualizer.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;
 
   bool Init(const YAML::Node& config);
-  void Extract(const PointCloud& cloud_in, FeaturePoints* const feature) const;
+  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;
 
@@ -35,7 +29,14 @@ class FeaturePointsExtractor {
   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

@@ -0,0 +1,23 @@
+#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.cc

@@ -0,0 +1,17 @@
+#include "feature_points_extractor_VLP16.h"
+
+namespace oh_my_loam {
+
+int FeaturePointsExtractorVLP16::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) {
+    AWARN << "OMEGA: " << std::atan2(pt.z, Distance(pt)) * 180 * M_1_PI + 15.0
+          << " id = " << static_cast<int>(std::round(omega / 2.0) + 0.01)
+          << " z = " << pt.z << " "
+          << " d = " << Distance(pt) << std::endl;
+  }
+  return static_cast<int>(std::round(omega / 2.0) + 1.e-5);
+};
+
+}  // namespace oh_my_loam

src/feature_points_extractor/feature_points_extractor_VLP16.h

@@ -0,0 +1,16 @@
+#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,31 +1,21 @@
 #include "oh_my_loam.h"
+#include "feature_points_extractor/feature_points_extractor_VLP16.h"
 
 namespace oh_my_loam {
 
-bool OhMyLoam::Init(const YAML::Node& config) {
+bool OhMyLoam::Init() {
-  is_vis_ = true;
+  YAML::Node config = Config::Instance()->config();
-  config_ = config;
+  feature_extractor_.reset(new FeaturePointsExtractorVLP16);
-  feature_extractor_.reset(new FeatureExtractorVLP16);
+  if (!feature_extractor_->Init(config["feature_extractor_config"])) {
-  if (is_vis_) {
+    AERROR << "Failed to initialize feature points extractor";
-    visualizer_.reset(new FeaturePointsVisualizer);
+    return false;
   }
   return true;
 }
 
 void OhMyLoam::Run(const PointCloud& cloud, double timestamp) {
-  std::shared_ptr<FeaturePoints> feature_pts(new FeaturePoints);
+  FeaturePoints feature_pts;
-  feature_extractor_->Extract(cloud, feature_pts.get());
+  feature_extractor_->Extract(cloud, &feature_pts);
-  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(new FeaturePointsVisFrame);
-  frame->timestamp = timestamp;
-  frame->cloud = cloud.makeShared();
-  frame->feature_pts = feature_pts;
-  visualizer_->Render(frame);
 }
 
 }  // namespace oh_my_loam

src/oh_my_loam.h

@@ -3,31 +3,22 @@
 #include <yaml-cpp/yaml.h>
 
 #include "common.h"
-#include "feature_extractor_VLP16.h"
+#include "feature_points_extractor/base_feature_points_extractor.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(const YAML::Node& config);
+  bool Init();
 
   void Run(const PointCloud& cloud, double timestamp);
 
  private:
-  void Visualize(const PointCloud& cloud,
+  std::unique_ptr<FeaturePointsExtractor> feature_extractor_{nullptr};
-                 const std::shared_ptr<const FeaturePoints>& feature_pts,
-                 double timestamp = std::nanf(""));
 
-  bool is_vis_ = false;
+  DISALLOW_COPY_AND_ASSIGN(OhMyLoam)
-  std::unique_ptr<FeaturePointsExtractor> feature_extractor_ = nullptr;
-  std::unique_ptr<FeaturePointsVisualizer> visualizer_ = nullptr;
-  YAML::Node config_;
 };
 
 }  // namespace oh_my_loam

src/visualizer/CMakeLists.txt

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

src/visualizer/feature_points_visualizer.cc

@@ -0,0 +1,43 @@
+#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

@@ -0,0 +1,23 @@
+#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

src/visualizer_feature_points.h

@@ -1,59 +0,0 @@
-#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, "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