refactoring: ok

CMakeLists.txt

@@ -10,21 +10,21 @@ find_package(PCL QUIET)
 find_package(g3log REQUIRED)
 find_package(yaml-cpp REQUIRED)
 
-# find_package(catkin REQUIRED COMPONENTS
+find_package(catkin REQUIRED COMPONENTS
-#   geometry_msgs
+  geometry_msgs
-#   nav_msgs
+  nav_msgs
-#   sensor_msgs
+  sensor_msgs
-#   roscpp
+  roscpp
-#   rospy
+  rospy
-#   rosbag
+  rosbag
-#   std_msgs
+  std_msgs
-#   image_transport
+  image_transport
-#   cv_bridge
+  cv_bridge
-#   tf
+  tf
-# )
+)
 
 include_directories(SYSTEM
-	# ${catkin_INCLUDE_DIRS} 
+	${catkin_INCLUDE_DIRS} 
   ${PCL_INCLUDE_DIRS}
   ${G3LOG_INCLUDE_DIRS}
 )
@@ -32,32 +32,32 @@ include_directories(SYSTEM
 link_directories(${PCL_LIBRARY_DIRS})
 add_definitions(${PCL_DEFINITIONS})
 
-# catkin_package(
+catkin_package(
-#   CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs
+  CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs
-#   DEPENDS EIGEN3 PCL 
+  DEPENDS EIGEN3 PCL 
-#   INCLUDE_DIRS src common
+  INCLUDE_DIRS oh_my_loam common
-# )
+)
 
 include_directories(
-  ${CMAKE_SOURCE_DIR}
+  ${CMAKE_CURRENT_SOURCE_DIR}
 )
 
 add_subdirectory(common)
 add_subdirectory(oh_my_loam)
 
-# add_executable(main main.cc)
+add_executable(main main.cc)
-# target_link_libraries(main
+target_link_libraries(main
-#   ${catkin_LIBRARIES} 
+  ${catkin_LIBRARIES} 
-#   ${PCL_LIBRARIES}
+  ${PCL_LIBRARIES}
-#   ${G3LOG_LIBRARIES}
+  ${G3LOG_LIBRARIES}
-#   ${YAML_CPP_LIBRARIES}
+  ${YAML_CPP_LIBRARIES}
-#   common
+  common
-#   oh_my_loam
+  oh_my_loam
-#   extractor
+  extractor
-#   odometry
+  odometer
-#   mapper
+  mapper
-#   solver
+  solver
-#   ${CERES_LIBRARIES}
+  ${CERES_LIBRARIES}
-#   helper
+  visualizer
-#   visualizer
+  base
-# )
+)

common/CMakeLists.txt

@@ -1,5 +1,3 @@
 file(GLOB SRC_FILES **/*.cc)
 
-message(STATUS "common dir: " ${CMAKE_CURRENT_SOURCE_DIR})
-
 add_library(common SHARED ${SRC_FILES})

common/pcl/pcl_utils.h

@@ -39,8 +39,8 @@ inline double IsFinite(const PointType& pt) {
 // Remove point if the condition evaluated to true on it
 template <typename PointType>
 void RemovePoints(const pcl::PointCloud<PointType>& cloud_in,
-                  std::function<bool(const PointType&)> check,
+                  pcl::PointCloud<PointType>* const cloud_out,
-                  pcl::PointCloud<PointType>* const cloud_out) {
+                  std::function<bool(const PointType&)> check) {
   if (&cloud_in != cloud_out) {
     cloud_out->header = cloud_in.header;
     cloud_out->resize(cloud_in.size());

common/time/timer.h

@@ -37,8 +37,15 @@ class TimerWrapper {
   DISALLOW_COPY_AND_ASSIGN(TimerWrapper);
 };
 
-#define TIME_ELAPSED(msg) TimerWrapper(msg)
+#define BLOCK_TIMER_START ::common::Timer __timer__
-#define TIME_ELAPSED_EXPECT(msg, max_time_elapsed) \
+
-  TimerWrapper(msg, max_time_elapsed)
+#define BLOCK_TIMER_STOP __timer__.Toc()
+
+#define BLOCK_TIMER_STOP_FMT FMT_TIMESTAMP(__timer__.Toc()) << " ms"
+
+#define BLOCK_TIME_ELAPSED(msg) ::common::TimerWrapper __timer_wrapper__(msg)
+
+#define BLOCK_TIME_ELAPSED_EXPECT(msg, max_time) \
+  ::common::TimerWrapper __timer_wrapper__(msg, max_time)
 
 }  // namespace common

common/visualizer/lidar_visualizer.h

@@ -5,6 +5,7 @@
 #include <mutex>
 #include <thread>
 
+#include "common/macro/macros.h"
 #include "lidar_visualizer_utils.h"
 
 namespace common {
@@ -54,7 +55,7 @@ class LidarVisualizer {
     is_updated_ = true;
   }
 
-  std::string Name() const { return name_; }
+  std::string name() const { return name_; }
 
  protected:
   void Run() {
@@ -162,6 +163,8 @@ class LidarVisualizer {
 
   // viewer
   std::unique_ptr<PCLVisualizer> viewer_ = nullptr;
+
+  DISALLOW_COPY_AND_ASSIGN(LidarVisualizer);
 };
 
 }  // namespace common

main.cc

@@ -3,25 +3,29 @@
 #include <ros/ros.h>
 #include <sensor_msgs/PointCloud2.h>
 
+#include <filesystem>
 #include <functional>
 
-#include "common.h"
+#include "common/common.h"
-#include "src/oh_my_loam.h"
+#include "oh_my_loam/oh_my_loam.h"
 
+using namespace common;
 using namespace oh_my_loam;
 
 void PointCloudHandler(const sensor_msgs::PointCloud2ConstPtr& msg,
                        OhMyLoam* const slam);
 
 int main(int argc, char* argv[]) {
+  auto curr_path = std::filesystem::current_path();
   // config
-  Config::Instance()->SetConfigFile("configs/config.yaml");
+  YAMLConfig::Instance()->Init(
-  bool log_to_file = Config::Instance()->Get<bool>("log_to_file");
+      (curr_path / "oh_my_loam/configs/config.yaml").string());
-  std::string log_path = Config::Instance()->Get<std::string>("log_path");
+  bool log_to_file = YAMLConfig::Instance()->Get<bool>("log_to_file");
-  std::string lidar = Config::Instance()->Get<std::string>("lidar");
+  std::string log_path = YAMLConfig::Instance()->Get<std::string>("log_path");
+  std::string lidar = YAMLConfig::Instance()->Get<std::string>("lidar");
 
   // logging
-  g3::InitG3Logging(log_to_file, "oh_my_loam_" + lidar, log_path);
+  InitG3Logging(log_to_file, "oh_my_loam_" + lidar, log_path);
   AUSER << "LOAM start..., lidar = " << lidar;
 
   // SLAM system
@@ -43,9 +47,9 @@ int main(int argc, char* argv[]) {
 
 void PointCloudHandler(const sensor_msgs::PointCloud2ConstPtr& msg,
                        OhMyLoam* const slam) {
-  PointCloud cloud;
+  PointCloudPtr cloud(new PointCloud);
-  pcl::fromROSMsg(*msg, cloud);
+  pcl::fromROSMsg(*msg, *cloud);
   double timestamp = msg->header.stamp.toSec();
-  AINFO << "Timestamp = " << LOG_TIMESTAMP(timestamp);
+  AUSER << "Timestamp: " << FMT_TIMESTAMP(timestamp);
-  slam->Run(cloud, timestamp);
+  slam->Run(timestamp, cloud);
 }

oh_my_loam/CMakeLists.txt

@@ -1,17 +1,11 @@
-message(STATUS "oh_my_loam dir: " ${CMAKE_CURRENT_SOURCE_DIR})
-
-include_directories(
-  ${CMAKE_CURRENT_SOURCE_DIR}
-)
-
 add_subdirectory(base)
 add_subdirectory(extractor)
-# add_subdirectory(visualizer)
+add_subdirectory(visualizer)
-# add_subdirectory(odometry)
+add_subdirectory(odometer)
-# add_subdirectory(mapper)
+add_subdirectory(mapper)
-# add_subdirectory(solver)
+add_subdirectory(solver)
 
-# aux_source_directory(. SRC_FILES)
+aux_source_directory(. SRC_FILES)
 
-# add_library(oh_my_loam SHARED ${SRC_FILES})
+add_library(oh_my_loam SHARED ${SRC_FILES})
 

oh_my_loam/base/helper.cc

@@ -2,6 +2,16 @@
 
 namespace oh_my_loam {
 
-//
+void TransformToStart(const Pose3d& pose, const TPoint& pt_in,
+                      TPoint* const pt_out) {
+  Pose3d pose_interp = Pose3d().Interpolate(pose, GetTime(pt_in));
+  TransformPoint<TPoint>(pose_interp, pt_in, pt_out);
+}
+
+void TransformToEnd(const Pose3d& pose, const TPoint& pt_in,
+                    TPoint* const pt_out) {
+  TransformToStart(pose, pt_in, pt_out);
+  TransformPoint<TPoint>(pose.Inv(), *pt_out, pt_out);
+}
 
 }  // namespace oh_my_loam

oh_my_loam/base/helper.h

@@ -27,25 +27,16 @@ void TransformPoint(const Pose3d& pose, const PointType& pt_in,
  * @brief Transform a lidar point to the start of the scan
  *
  * @param pose Relative pose, end scan time w.r.t. start scan time
- * @param time Point time relative to the start time of the scan, \in [0, 1]
  */
 void TransformToStart(const Pose3d& pose, const TPoint& pt_in,
-                      TPoint* const pt_out) {
+                      TPoint* const pt_out);
-  Pose3d pose_interp = Pose3d().Interpolate(pose, GetTime(pt_in));
-  TransformPoint<TPoint>(pose_interp, pt_in, pt_out);
-}
-
 /**
  * @brief Transform a lidar point to the end of the scan
  *
  * @param pose Relative pose, end scan time w.r.t. start scan time
- * @param time Point time relative to the start time of the scan, \in [0, 1]
  */
 void TransformToEnd(const Pose3d& pose, const TPoint& pt_in,
-                    TPoint* const pt_out) {
+                    TPoint* const pt_out);
-  TransformToStart(pose, pt_in, pt_out);
-  TransformPoint<TPoint>(pose.Inv(), *pt_out, pt_out);
-}
 
 struct PointLinePair {
   TPoint pt;

oh_my_loam/configs/config.yaml

@@ -6,9 +6,9 @@ vis: true
 
 # configs for extractor
 extractor_config:
-  min_point_num: 66
   vis: false
-  verbose: false
+  verbose: true
+  min_point_num: 66
   sharp_corner_point_num: 2
   corner_point_num: 20
   flat_surf_point_num: 4
@@ -18,11 +18,12 @@ extractor_config:
   neighbor_point_dist_thres: 0.05
   downsample_voxel_size: 0.3
 
-# configs for odometry
+# configs for odometer
-odometry_config: 
+odometer_config: 
+  vis: true
+  verbose: false
   icp_iter_num : 2
   match_dist_sq_thresh: 1
-  vis: true
 
 # configs for mapper
 mapper_config: 

oh_my_loam/extractor/extractor.cc

@@ -2,7 +2,6 @@
 
 #include <cmath>
 
-#include "base/helper.h"
 #include "common/pcl/pcl_utils.h"
 
 namespace oh_my_loam {
@@ -25,7 +24,7 @@ bool Extractor::Init() {
 
 void Extractor::Process(const PointCloudConstPtr& cloud,
                         Feature* const feature) {
-  TIME_ELAPSED(__PRETTY_FUNCTION__);
+  BLOCK_TIMER_START;
   if (cloud->size() < config_["min_point_num"].as<size_t>()) {
     AWARN << "Too few input points: num = " << cloud->size() << " (< "
           << config_["min_point_num"].as<int>() << ")";
@@ -34,25 +33,28 @@ void Extractor::Process(const PointCloudConstPtr& cloud,
   // split point cloud int scans
   std::vector<TCTPointCloud> scans;
   SplitScan(*cloud, &scans);
+  AINFO_IF(verbose_) << "Extractor::SplitScan: " << BLOCK_TIMER_STOP_FMT;
   // compute curvature for each point in each scan
   for (auto& scan : scans) {
     ComputeCurvature(&scan);
   }
+  AINFO_IF(verbose_) << "Extractor::ComputeCurvature: " << BLOCK_TIMER_STOP_FMT;
   // assign type to each point in each scan: FLAT, LESS_FLAT,
   // NORMAL, LESS_SHARP or SHARP
   for (auto& scan : scans) {
     AssignType(&scan);
   }
+  AINFO_IF(verbose_) << "Extractor::AssignType: " << BLOCK_TIMER_STOP_FMT;
   // store points into feature point clouds according to their type
   for (const auto& scan : scans) {
     GenerateFeature(scan, feature);
   }
-  if (is_vis_) Visualize(cloud, *feature);
+  AINFO << "Extractor::Process: " << BLOCK_TIMER_STOP_FMT;
+  if (is_vis_) Visualize(*cloud, *feature);
 }
 
 void Extractor::SplitScan(const PointCloud& cloud,
                           std::vector<TCTPointCloud>* const scans) const {
-  Timer timer;
   scans->resize(num_scans_);
   double yaw_start = -atan2(cloud.points[0].y, cloud.points[0].x);
   bool half_passed = false;
@@ -70,12 +72,9 @@ void Extractor::SplitScan(const PointCloud& cloud,
     (*scans)[scan_id].points.emplace_back(
         pt.x, pt.y, pt.z, yaw_diff / kTwoPi + scan_id, std::nanf(""));
   }
-  AINFO_IF(verbose_) << "Extractor::SplitScan: " << FMT_TIMESTAMP(timer.Toc())
-                     << " ms";
 }
 
 void Extractor::ComputeCurvature(TCTPointCloud* const scan) const {
-  Timer timer;
   if (scan->size() < 20) return;
   auto& pts = scan->points;
   for (size_t i = 5; i < pts.size() - 5; ++i) {
@@ -90,15 +89,12 @@ 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>(
+  RemovePoints<TCTPoint>(*scan, scan, [](const TCTPoint& pt) {
-      *scan, [](const TCTPoint& pt) { return !std::isfinite(pt.curvature); },
+    return !std::isfinite(pt.curvature);
-      scan);
+  });
-  AINFO_IF(verbose_) << "Extractor::ComputeCurvature: "
-                     << FMT_TIMESTAMP(timer.Toc()) << " ms";
 }
 
 void Extractor::AssignType(TCTPointCloud* const scan) const {
-  Timer timer;
   int pt_num = scan->size();
   ACHECK(pt_num >= kScanSegNum);
   int seg_pt_num = (pt_num - 1) / kScanSegNum + 1;
@@ -156,13 +152,10 @@ void Extractor::AssignType(TCTPointCloud* const scan) const {
       }
     }
   }
-  AINFO_IF(verbose_) << "Extractor::AssignType: " << FMT_TIMESTAMP(timer.Toc())
-                     << " ms";
 }
 
 void Extractor::GenerateFeature(const TCTPointCloud& scan,
                                 Feature* const feature) const {
-  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);
@@ -189,15 +182,13 @@ void Extractor::GenerateFeature(const TCTPointCloud& scan,
   VoxelDownSample<TPoint>(cloud_less_flat_surf, dowm_sampled.get(),
                           config_["downsample_voxel_size"].as<double>());
   feature->cloud_less_flat_surf = dowm_sampled;
-  AINFO_IF(verbose_) << "Extractor::GenerateFeature: "
-                     << FMT_TIMESTAMP(timer.Toc()) << " ms";
 }
 
-void Extractor::Visualize(const PointCloudConstPtr& cloud,
+void Extractor::Visualize(const PointCloud& cloud, const Feature& feature,
-                          const Feature& feature, double timestamp) {
+                          double timestamp) {
   std::shared_ptr<ExtractorVisFrame> frame(new ExtractorVisFrame);
   frame->timestamp = timestamp;
-  frame->cloud = cloud->makeShared();
+  frame->cloud = cloud.makeShared();
   frame->feature = feature;
   visualizer_->Render(frame);
 }

oh_my_loam/extractor/extractor.h

@@ -1,9 +1,10 @@
 #pragma once
 
-#include "base/feature.h"
-#include "base/types.h"
 #include "common/common.h"
-#include "visualizer/extractor_visualizer.h"
+#include "oh_my_loam/base/feature.h"
+#include "oh_my_loam/base/helper.h"
+#include "oh_my_loam/base/types.h"
+#include "oh_my_loam/visualizer/extractor_visualizer.h"
 
 namespace oh_my_loam {
 
@@ -40,8 +41,8 @@ class Extractor {
   bool verbose_ = false;
 
  private:
-  void Visualize(const common::PointCloudConstPtr& cloud,
+  void Visualize(const common::PointCloud& cloud, const Feature& feature,
-                 const Feature& feature, double timestamp = 0.0);
+                 double timestamp = 0.0);
 
   void SetNeighborsPicked(const TCTPointCloud& scan, size_t ix,
                           std::vector<bool>* const picked) const;

oh_my_loam/mapper/mapper.cc

@@ -2,9 +2,15 @@
 
 namespace oh_my_loam {
 
-bool Mapper::Init(const YAML::Node& config) {
+namespace {
-  config_ = config;
+using namespace common;
-  is_vis_ = Config::Instance()->Get<bool>("vis") && config_["vis"].as<bool>();
+}  // namespace
+
+bool Mapper::Init() {
+  const auto& config = YAMLConfig::Instance()->config();
+  config_ = config["mapper_config"];
+  is_vis_ = config["vis"].as<bool>() && config_["vis"].as<bool>();
+  verbose_ = config_["vis"].as<bool>();
   AINFO << "Mapping visualizer: " << (is_vis_ ? "ON" : "OFF");
   return true;
 }

oh_my_loam/mapper/mapper.h

@@ -1,6 +1,8 @@
 #pragma once
 
-#include "common.h"
+#include "common/common.h"
+#include "common/geometry/pose.h"
+#include "common/pcl/pcl_types.h"
 
 namespace oh_my_loam {
 
@@ -8,19 +10,22 @@ class Mapper {
  public:
   Mapper() = default;
 
-  bool Init(const YAML::Node& config);
+  bool Init();
 
   void Process();
 
  private:
   void Visualize();
 
-  TPointCloudPtr map_pts_;
+  common::TPointCloudPtr map_pts_;
-  Pose3d pose_curr2world_;
+  common::Pose3d pose_curr2world_;
 
   bool is_initialized = false;
+
   bool is_vis_ = false;
 
+  bool verbose_ = false;
+
   YAML::Node config_;
 
   DISALLOW_COPY_AND_ASSIGN(Mapper)

oh_my_loam/odometer/odometer.cc

@@ -1,51 +1,49 @@
-#include "odometry.h"
+#include "odometer.h"
 
-#include "solver/solver.h"
+#include "common/pcl/pcl_utils.h"
+#include "oh_my_loam/solver/solver.h"
 
 namespace oh_my_loam {
 
 namespace {
 int kNearbyScanNum = 2;
 size_t kMinMatchNum = 10;
+using namespace common;
 }  // namespace
 
-bool Odometry::Init(const YAML::Node& config) {
+bool Odometer::Init() {
-  config_ = config;
+  const auto& config = YAMLConfig::Instance()->config();
-  kdtree_surf_pts_.reset(new pcl::KdTreeFLANN<TPoint>);
+  config_ = config["odometer_config"];
-  kdtree_corn_pts_.reset(new pcl::KdTreeFLANN<TPoint>);
+  is_vis_ = config["vis"].as<bool>() && config_["vis"].as<bool>();
-  is_vis_ = Config::Instance()->Get<bool>("vis") && config_["vis"].as<bool>();
+  verbose_ = config_["verbose"].as<bool>();
-  AINFO << "Odometry visualizer: " << (is_vis_ ? "ON" : "OFF");
+  kdtree_surf_.reset(new pcl::KdTreeFLANN<TPoint>);
-  if (is_vis_) visualizer_.reset(new OdometryVisualizer);
+  kdtree_corn_.reset(new pcl::KdTreeFLANN<TPoint>);
+  AINFO << "Odometer visualizer: " << (is_vis_ ? "ON" : "OFF");
+  if (is_vis_) visualizer_.reset(new OdometerVisualizer);
   return true;
 }
 
-void Odometry::Process(const FeaturePoints& feature, Pose3d* const pose) {
+void Odometer::Process(const Feature& feature, Pose3d* const pose) {
+  BLOCK_TIMER_START;
   if (!is_initialized_) {
     is_initialized_ = true;
     UpdatePre(feature);
-    *pose = pose_curr2world_;
+    *pose = Pose3d();
-    AWARN << "Odometry initialized....";
+    AWARN << "Odometer initialized....";
     return;
   }
-  TicToc timer_whole;
-  double match_dist_sq_thresh = config_["match_dist_sq_thresh"].as<double>();
   AINFO << "Pose before: " << pose_curr2world_.ToString();
   std::vector<PointLinePair> pl_pairs;
   std::vector<PointPlanePair> pp_pairs;
   for (int i = 0; i < config_["icp_iter_num"].as<int>(); ++i) {
-    TicToc timer;
+    MatchCornFeature(*feature.cloud_sharp_corner, *corn_pre_, &pl_pairs);
-    MatchCornPoints(*feature.sharp_corner_pts, *corn_pts_pre_, &pl_pairs,
-                    match_dist_sq_thresh);
-    double time_pl_match = timer.toc();
     AINFO_IF(i == 0) << "PL mathch: src size = "
-                     << feature.sharp_corner_pts->size()
+                     << feature.cloud_sharp_corner->size()
-                     << ", tgt size = " << corn_pts_pre_->size();
+                     << ", tgt size = " << corn_pre_->size();
-    MatchSurfPoints(*feature.flat_surf_pts, *surf_pts_pre_, &pp_pairs,
+    MatchSurfFeature(*feature.cloud_flat_surf, *surf_pre_, &pp_pairs);
-                    match_dist_sq_thresh);
-    double timer_pp_match = timer.toc();
     AINFO_IF(i == 0) << "PP mathch: src size = "
-                     << feature.flat_surf_pts->size()
+                     << feature.cloud_flat_surf->size()
-                     << ", tgt size = " << surf_pts_pre_->size();
+                     << ", tgt size = " << surf_pre_->size();
     AINFO << "Matched num, pl = " << pl_pairs.size()
           << ", pp = " << pp_pairs.size();
     if (pl_pairs.size() + pp_pairs.size() < kMinMatchNum) {
@@ -65,29 +63,25 @@ void Odometry::Process(const FeaturePoints& feature, Pose3d* const pose) {
     }
     solver.Solve();
     pose_curr2last_ = Pose3d(q, p);
-    double time_solve = timer.toc();
-    AINFO << "Time elapsed (ms), pl_match = " << time_pl_match
-          << ", pp_match = " << timer_pp_match - time_pl_match
-          << ", solve = " << time_solve - timer_pp_match;
   }
   pose_curr2world_ = pose_curr2world_ * pose_curr2last_;
   *pose = pose_curr2world_;
   AWARN << "Pose increase: " << pose_curr2last_.ToString();
   AWARN << "Pose after: " << pose_curr2world_.ToString();
   UpdatePre(feature);
-  AUSER << "Time elapsed (ms): whole odometry = " << timer_whole.toc();
+  AINFO << "Odometer::Porcess: " << BLOCK_TIMER_STOP_FMT;
   if (is_vis_) Visualize(feature, pl_pairs, pp_pairs);
 }
 
-void Odometry::MatchCornPoints(const TPointCloud& src, const TPointCloud& tgt,
+void Odometer::MatchCornFeature(const TPointCloud& src, const TPointCloud& tgt,
-                               std::vector<PointLinePair>* const pairs,
+                                std::vector<PointLinePair>* const pairs) const {
-                               double dist_sq_thresh) const {
+  double dist_sq_thresh = config_["match_dist_sq_thresh"].as<double>();
   for (const auto& q_pt : src) {
     TPoint query_pt;
     TransformToStart(pose_curr2last_, q_pt, &query_pt);
     std::vector<int> indices;
     std::vector<float> dists;
-    kdtree_corn_pts_->nearestKSearch(query_pt, 1, indices, dists);
+    kdtree_corn_->nearestKSearch(query_pt, 1, indices, dists);
     if (dists[0] >= dist_sq_thresh) continue;
     TPoint pt1 = tgt.points[indices[0]];
     int pt2_idx = -1;
@@ -122,15 +116,16 @@ void Odometry::MatchCornPoints(const TPointCloud& src, const TPointCloud& tgt,
   }
 }
 
-void Odometry::MatchSurfPoints(const TPointCloud& src, const TPointCloud& tgt,
+void Odometer::MatchSurfFeature(
-                               std::vector<PointPlanePair>* const pairs,
+    const TPointCloud& src, const TPointCloud& tgt,
-                               double dist_sq_thresh) const {
+    std::vector<PointPlanePair>* const pairs) const {
+  double dist_sq_thresh = config_["match_dist_sq_thresh"].as<double>();
   for (const auto& q_pt : src) {
     TPoint query_pt;
     TransformToStart(pose_curr2last_, q_pt, &query_pt);
     std::vector<int> indices;
     std::vector<float> dists;
-    kdtree_surf_pts_->nearestKSearch(query_pt, 1, indices, dists);
+    kdtree_surf_->nearestKSearch(query_pt, 1, indices, dists);
     if (dists[0] >= dist_sq_thresh) continue;
     TPoint pt1 = tgt.points[indices[0]];
     int pt2_idx = -1, pt3_idx = -1;
@@ -174,32 +169,32 @@ void Odometry::MatchSurfPoints(const TPointCloud& src, const TPointCloud& tgt,
   }
 }
 
-void Odometry::UpdatePre(const FeaturePoints& feature) {
+void Odometer::UpdatePre(const Feature& feature) {
-  if (surf_pts_pre_ == nullptr) {
+  if (!surf_pre_) {
-    surf_pts_pre_.reset(new TPointCloud);
+    surf_pre_.reset(new TPointCloud);
   }
-  if (corn_pts_pre_ == nullptr) {
+  if (!corn_pre_) {
-    corn_pts_pre_.reset(new TPointCloud);
+    corn_pre_.reset(new TPointCloud);
   }
 
-  surf_pts_pre_->resize(feature.less_flat_surf_pts->size());
+  surf_pre_->resize(feature.cloud_less_flat_surf->size());
-  for (size_t i = 0; i < feature.less_flat_surf_pts->size(); ++i) {
+  for (size_t i = 0; i < feature.cloud_less_flat_surf->size(); ++i) {
-    TransformToEnd(pose_curr2last_, feature.less_flat_surf_pts->points[i],
+    TransformToEnd(pose_curr2last_, feature.cloud_less_flat_surf->points[i],
-                   &surf_pts_pre_->points[i]);
+                   &surf_pre_->points[i]);
   }
-  corn_pts_pre_->resize(feature.less_sharp_corner_pts->size());
+  corn_pre_->resize(feature.cloud_less_sharp_corner->size());
-  for (size_t i = 0; i < feature.less_sharp_corner_pts->size(); ++i) {
+  for (size_t i = 0; i < feature.cloud_less_sharp_corner->size(); ++i) {
-    TransformToEnd(pose_curr2last_, feature.less_sharp_corner_pts->points[i],
+    TransformToEnd(pose_curr2last_, feature.cloud_less_sharp_corner->points[i],
-                   &corn_pts_pre_->points[i]);
+                   &corn_pre_->points[i]);
   }
-  kdtree_surf_pts_->setInputCloud(surf_pts_pre_);
+  kdtree_surf_->setInputCloud(surf_pre_);
-  kdtree_corn_pts_->setInputCloud(corn_pts_pre_);
+  kdtree_corn_->setInputCloud(corn_pre_);
 }
 
-void Odometry::Visualize(const FeaturePoints& feature,
+void Odometer::Visualize(const Feature& feature,
                          const std::vector<PointLinePair>& pl_pairs,
                          const std::vector<PointPlanePair>& pp_pairs) const {
-  std::shared_ptr<OdometryVisFrame> frame(new OdometryVisFrame);
+  std::shared_ptr<OdometerVisFrame> frame(new OdometerVisFrame);
   frame->pl_pairs = pl_pairs;
   frame->pp_pairs = pp_pairs;
   frame->pose_curr2last = pose_curr2last_;

oh_my_loam/odometer/odometer.h

@@ -1,52 +1,57 @@
 #pragma once
 
-#include "extractor/feature_points.h"
+#include "common/common.h"
-#include "visualizer/odometry_visualizer.h"
+#include "common/geometry/pose.h"
-
+#include "oh_my_loam/base/feature.h"
-#include "helper/helper.h"
+#include "oh_my_loam/base/helper.h"
+#include "oh_my_loam/visualizer/odometer_visualizer.h"
 
 namespace oh_my_loam {
 
-// frame to frame lidar odometry
+// frame to frame lidar Odometer
-class Odometry {
+class Odometer {
  public:
-  Odometry() = default;
+  Odometer() = default;
 
-  bool Init(const YAML::Node& config);
+  bool Init();
 
-  void Process(const FeaturePoints& feature, Pose3d* const pose);
+  void Process(const Feature& feature, common::Pose3d* const pose);
 
  protected:
-  void UpdatePre(const FeaturePoints& feature);
+  void UpdatePre(const Feature& feature);
 
-  void MatchCornPoints(const TPointCloud& src, const TPointCloud& tgt,
+  void MatchCornFeature(const common::TPointCloud& src,
-                       std::vector<PointLinePair>* const pairs,
+                        const common::TPointCloud& tgt,
-                       double dist_sq_thresh) const;
+                        std::vector<PointLinePair>* const pairs) const;
 
-  void MatchSurfPoints(const TPointCloud& src, const TPointCloud& tgt,
+  void MatchSurfFeature(const common::TPointCloud& src,
-                       std::vector<PointPlanePair>* const pairs,
+                        const common::TPointCloud& tgt,
-                       double dist_sq_thresh) const;
+                        std::vector<PointPlanePair>* const pairs) const;
 
-  void Visualize(const FeaturePoints& feature,
+  void Visualize(const Feature& feature,
                  const std::vector<PointLinePair>& pl_pairs,
                  const std::vector<PointPlanePair>& pp_pairs) const;
 
-  Pose3d pose_curr2world_;
+  common::Pose3d pose_curr2world_;
-  Pose3d pose_curr2last_;
+  common::Pose3d pose_curr2last_;
 
-  TPointCloudPtr surf_pts_pre_{nullptr};
+  common::TPointCloudPtr corn_pre_{nullptr};
-  TPointCloudPtr corn_pts_pre_{nullptr};
+  common::TPointCloudPtr surf_pre_{nullptr};
 
-  pcl::KdTreeFLANN<TPoint>::Ptr kdtree_surf_pts_{nullptr};
+  pcl::KdTreeFLANN<common::TPoint>::Ptr kdtree_surf_{nullptr};
-  pcl::KdTreeFLANN<TPoint>::Ptr kdtree_corn_pts_{nullptr};
+  pcl::KdTreeFLANN<common::TPoint>::Ptr kdtree_corn_{nullptr};
 
-  bool is_initialized_ = false;
-  bool is_vis_ = false;
   YAML::Node config_;
 
-  std::unique_ptr<OdometryVisualizer> visualizer_{nullptr};
+  bool is_initialized_ = false;
 
-  DISALLOW_COPY_AND_ASSIGN(Odometry)
+  bool is_vis_ = false;
+
+  bool verbose_ = false;
+
+  std::unique_ptr<OdometerVisualizer> visualizer_{nullptr};
+
+  DISALLOW_COPY_AND_ASSIGN(Odometer)
 };
 
 }  // namespace oh_my_loam

oh_my_loam/oh_my_loam.cc

@@ -1,42 +1,51 @@
 #include "oh_my_loam.h"
 
-#include "extractor/extractor_VLP16.h"
+#include "common/pcl/pcl_utils.h"
+#include "oh_my_loam/extractor/extractor_VLP16.h"
 
 namespace oh_my_loam {
 
 namespace {
 const double kPointMinDist = 0.1;
+using namespace common;
 }
 
 bool OhMyLoam::Init() {
-  YAML::Node config = Config::Instance()->config();
+  YAML::Node config = YAMLConfig::Instance()->config();
   extractor_.reset(new ExtractorVLP16);
-  if (!extractor_->Init(config["extractor_config"])) {
+  if (!extractor_->Init()) {
     AERROR << "Failed to initialize extractor";
     return false;
   }
-  odometry_.reset(new Odometry);
+  odometer_.reset(new Odometer);
-  if (!odometry_->Init(config["odometry_config"])) {
+  if (!odometer_->Init()) {
-    AERROR << "Failed to initialize odometry";
+    AERROR << "Failed to initialize odometer";
+    return false;
+  }
+  mapper_.reset(new Mapper);
+  if (!mapper_->Init()) {
+    AERROR << "Failed to initialize mapper";
     return false;
   }
   return true;
 }
 
-void OhMyLoam::Run(const PointCloud& cloud_in, double timestamp) {
+void OhMyLoam::Run(double timestamp, const PointCloudConstPtr& cloud_in) {
   PointCloudPtr cloud(new PointCloud);
-  RemoveOutliers(cloud_in, cloud.get());
+  RemoveOutliers(*cloud_in, cloud.get());
-  FeaturePoints feature_points;
+  Feature feature;
-  extractor_->Process(*cloud, &feature_points);
+  extractor_->Process(cloud, &feature);
   Pose3d pose;
-  odometry_->Process(feature_points, &pose);
+  odometer_->Process(feature, &pose);
   poses_.emplace_back(pose);
 }
 
 void OhMyLoam::RemoveOutliers(const PointCloud& cloud_in,
                               PointCloud* const cloud_out) const {
-  RemoveNaNPoint<Point>(cloud_in, cloud_out);
+  RemovePoints<Point>(cloud_in, cloud_out, [&](const Point& pt) {
-  RemoveClosedPoints<Point>(*cloud_out, cloud_out, kPointMinDist);
+    return !IsFinite<Point>(pt) ||
+           DistanceSqure<Point>(pt) < kPointMinDist * kPointMinDist;
+  });
 }
 
 }  // namespace oh_my_loam

oh_my_loam/oh_my_loam.h

@@ -1,8 +1,9 @@
 #pragma once
 
-#include "common.h"
+#include "common/common.h"
-#include "extractor/base_extractor.h"
+#include "oh_my_loam/extractor/extractor.h"
-#include "odometry/odometry.h"
+#include "oh_my_loam/mapper/mapper.h"
+#include "oh_my_loam/odometer/odometer.h"
 
 namespace oh_my_loam {
 
@@ -12,16 +13,19 @@ class OhMyLoam {
 
   bool Init();
 
-  void Run(const PointCloud& cloud_in, double timestamp);
+  void Run(double timestamp, const common::PointCloudConstPtr& cloud_in);
 
  private:
   std::unique_ptr<Extractor> extractor_{nullptr};
-  std::unique_ptr<Odometry> odometry_{nullptr};
+
+  std::unique_ptr<Odometer> odometer_{nullptr};
+
+  std::unique_ptr<Mapper> mapper_{nullptr};
 
   // remove outliers: nan and very close points
-  void RemoveOutliers(const PointCloud& cloud_in,
+  void RemoveOutliers(const common::PointCloud& cloud_in,
-                      PointCloud* const cloud_out) const;
+                      common::PointCloud* const cloud_out) const;
-  std::vector<Pose3d> poses_;
+  std::vector<common::Pose3d> poses_;
 
   DISALLOW_COPY_AND_ASSIGN(OhMyLoam)
 };

oh_my_loam/solver/cost_function.h

@@ -1,8 +1,9 @@
 #pragma once
 
-#include "ceres/ceres.h"
+#include <ceres/ceres.h>
-#include "common.h"
+
-#include "helper/helper.h"
+#include "common/common.h"
+#include "oh_my_loam/base/helper.h"
 
 namespace oh_my_loam {
 

oh_my_loam/visualizer/extractor_visualizer.h

@@ -1,7 +1,7 @@
 #pragma once
 
-#include "base/feature.h"
 #include "common/visualizer/lidar_visualizer.h"
+#include "oh_my_loam/base/feature.h"
 
 namespace oh_my_loam {
 

oh_my_loam/visualizer/odometer_visualizer.cc

@@ -1,9 +1,13 @@
-#include "odometry_visualizer.h"
+#include "odometer_visualizer.h"
 
 namespace oh_my_loam {
 
-void OdometryVisualizer::Draw() {
+namespace {
-  auto frame = GetCurrentFrame<OdometryVisFrame>();
+using namespace common;
+}  // namespace
+
+void OdometerVisualizer::Draw() {
+  auto frame = GetCurrentFrame<OdometerVisFrame>();
   TPointCloudPtr src_corn_pts(new TPointCloud);
   TPointCloudPtr tgt_corn_pts(new TPointCloud);
   src_corn_pts->resize(frame.pl_pairs.size());

oh_my_loam/visualizer/odometer_visualizer.h

@@ -1,28 +1,29 @@
 #pragma once
 
 #include "common/visualizer/lidar_visualizer.h"
-#include "helper/helper.h"
+#include "oh_my_loam/base/helper.h"
 
 namespace oh_my_loam {
 
-struct OdometerVisFrame : public VisFrame {
+struct OdometerVisFrame : public common::LidarVisFrame {
-  TPointCloudPtr surf_pts;
+  common::TPointCloudPtr surf_pts;
-  TPointCloudPtr corn_pts;
+  common::TPointCloudPtr corn_pts;
   std::vector<PointLinePair> pl_pairs;
   std::vector<PointPlanePair> pp_pairs;
-  Pose3d pose_curr2last;
+  common::Pose3d pose_curr2last;
-  Pose3d pose_curr2world;
+  common::Pose3d pose_curr2world;
 };
 
-class OdometryVisualizer : public Visualizer {
+class OdometerVisualizer : public common::LidarVisualizer {
  public:
-  explicit OdometryVisualizer(const std::string &name = "OdometryVisualizer",
+  explicit OdometerVisualizer(const std::string &name = "OdometerVisualizer",
                               size_t max_history_size = 10)
-      : Visualizer(name, max_history_size) {}
+      : common::LidarVisualizer(name, max_history_size) {}
 
  private:
   void Draw() override;
-  std::deque<Pose3d> poses_;
+
+  std::deque<common::Pose3d> poses_;
 };
 
 }  // namespace oh_my_loam