tart writing mapper

common/config/yaml_config.h

@@ -29,6 +29,7 @@ class YAMLConfig {
 
  private:
   std::unique_ptr<YAML::Node> config_{nullptr};
+
   DECLARE_SINGLETON(YAMLConfig);
 };
 

oh_my_loam/configs/config.yaml

@@ -9,10 +9,10 @@ extractor_config:
   vis: false
   verbose: false
   min_point_num: 66
-  scan_seg_num: 36
+  scan_seg_num: 12
   sharp_corner_point_num: 1
-  corner_point_num: 4
+  corner_point_num: 10
-  flat_surf_point_num: 1
+  flat_surf_point_num: 2
   corner_point_curvature_th: 0.5
   surf_point_curvature_th: 0.5
   neighbor_point_dist_sq_th: 0.1
@@ -26,9 +26,11 @@ odometer_config:
   min_correspondence_num: 10
   icp_iter_num: 2
   solve_iter_num: 4
-  corn_match_dist_sq_th: 1.0
+  corn_match_dist_sq_th: 16.0
   surf_match_dist_sq_th: 16.0
 
 # configs for mapper
 mapper_config:
   vis: false
+  verbose: false
+  process_period: 0.5

oh_my_loam/extractor/extractor.cc

@@ -107,8 +107,6 @@ void Extractor::AssignType(TCTPointCloud *const scan) const {
   int seg_num = config_["scan_seg_num"].as<int>();
   if (pt_num < seg_num) return;
   int seg_pt_num = (pt_num - 1) / seg_num + 1;
-  AERROR << pt_num << " " << seg_num << " " << seg_pt_num;
-
   std::vector<bool> picked(pt_num, false);
   std::vector<int> indices = common::Range(pt_num);
   int sharp_corner_point_num = config_["sharp_corner_point_num"].as<int>();
@@ -121,6 +119,7 @@ void Extractor::AssignType(TCTPointCloud *const scan) const {
   for (int seg = 0; seg < seg_num; ++seg) {
     int b = seg * seg_pt_num;
     int e = std::min((seg + 1) * seg_pt_num, pt_num);
+    if (b >= e) break;
     // sort by curvature for each segment: large -> small
     std::sort(indices.begin() + b, indices.begin() + e, [&](int i, int j) {
       return scan->at(i).curvature > scan->at(j).curvature;
@@ -128,7 +127,7 @@ void Extractor::AssignType(TCTPointCloud *const scan) const {
     // pick corner points
     int corner_point_picked_num = 0;
     for (int i = b; i < e; ++i) {
-      size_t ix = indices[i];
+      int ix = indices[i];
       if (!picked.at(ix) &&
           scan->at(ix).curvature > corner_point_curvature_th) {
         ++corner_point_picked_num;
@@ -146,7 +145,7 @@ void Extractor::AssignType(TCTPointCloud *const scan) const {
     // pick surface points
     int surf_point_picked_num = 0;
     for (int i = e - 1; i >= b; --i) {
-      size_t ix = indices[i];
+      int ix = indices[i];
       if (!picked.at(ix) && scan->at(ix).curvature < surf_point_curvature_th) {
         ++surf_point_picked_num;
         if (surf_point_picked_num <= flat_surf_point_num) {
@@ -200,21 +199,21 @@ void Extractor::Visualize(const common::PointCloudConstPtr &cloud,
   visualizer_->Render(frame);
 }
 
-void Extractor::UpdateNeighborsPicked(const TCTPointCloud &scan, size_t ix,
+void Extractor::UpdateNeighborsPicked(const TCTPointCloud &scan, int ix,
                                       std::vector<bool> *const picked) const {
   auto dist_sq = [&](size_t i, size_t j) -> double {
     return common::DistanceSquare<TCTPoint>(scan[i], scan[j]);
   };
   double neighbor_point_dist_sq_th =
       config_["neighbor_point_dist_sq_th"].as<float>();
-  for (size_t i = 1; i <= 5; ++i) {
+  for (int i = 1; i <= 5; ++i) {
     if (ix - i < 0) break;
     if (picked->at(ix - i)) continue;
     if (dist_sq(ix - i, ix - i + 1) > neighbor_point_dist_sq_th) break;
     picked->at(ix - i) = true;
   }
-  for (size_t i = 1; i <= 5; ++i) {
+  for (int i = 1; i <= 5; ++i) {
-    if (ix + i >= scan.size()) break;
+    if (static_cast<size_t>(ix + i) >= scan.size()) break;
     if (picked->at(ix + i)) continue;
     if (dist_sq(ix + i, ix + i - 1) > neighbor_point_dist_sq_th) break;
     picked->at(ix + i) = true;

oh_my_loam/extractor/extractor.h

@@ -22,6 +22,8 @@ class Extractor {
     return num_scans_;
   }
 
+  virtual void Reset() {}
+
  protected:
   virtual int GetScanID(const common::Point &pt) const = 0;
 
@@ -48,7 +50,7 @@ class Extractor {
   bool verbose_ = false;
 
  private:
-  void UpdateNeighborsPicked(const TCTPointCloud &scan, size_t ix,
+  void UpdateNeighborsPicked(const TCTPointCloud &scan, int ix,
                              std::vector<bool> *const picked) const;
 
   bool is_vis_ = false;

oh_my_loam/mapper/mapper.cc

@@ -15,7 +15,8 @@ bool Mapper::Init() {
   return true;
 }
 
-void Mapper::Process() {}
+void Mapper::Process(double timestamp, const std::vector<Feature> &features,
+                     Pose3d *const pose_out) {}
 
 void Mapper::Visualize() {}
 

oh_my_loam/mapper/mapper.h

@@ -1,7 +1,10 @@
 #pragma once
 
+#include <vector>
+
 #include "common/common.h"
-#include "common/geometry/pose.h"
+#include "common/geometry/pose3d.h"
+#include "oh_my_loam/base/feature.h"
 #include "oh_my_loam/base/types.h"
 
 namespace oh_my_loam {
@@ -12,22 +15,30 @@ class Mapper {
 
   bool Init();
 
-  void Process();
+  void Process(double timestamp, const std::vector<Feature> &features,
+               common::Pose3d *const pose_out);
+
+  common::PointCloudConstPtr map() const {
+    return cloud_map_;
+  }
+
+  void Reset();
 
  private:
   void Visualize();
 
-  TPointCloudPtr map_pts_;
+  common::PointCloudPtr cloud_map_;
+
   common::Pose3d pose_curr2world_;
 
+  YAML::Node config_;
+
   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

@@ -20,6 +20,10 @@ bool Odometer::Init() {
   return true;
 }
 
+void Odometer::Reset() {
+  is_initialized_ = false;
+}
+
 void Odometer::Process(double timestamp, const std::vector<Feature> &features,
                        Pose3d *const pose_out) {
   if (!is_initialized_) {

oh_my_loam/odometer/odometer.h

@@ -18,6 +18,8 @@ class Odometer {
   void Process(double timestamp, const std::vector<Feature> &features,
                common::Pose3d *const pose_out);
 
+  void Reset();
+
  protected:
   void UpdatePre(const std::vector<Feature> &features);
 

oh_my_loam/oh_my_loam.cc

@@ -1,6 +1,9 @@
 #include "oh_my_loam.h"
 
+#include <vector>
+
 #include "common/pcl/pcl_utils.h"
+#include "oh_my_loam/base/helper.h"
 #include "oh_my_loam/extractor/extractor_VLP16.h"
 
 namespace oh_my_loam {
@@ -10,7 +13,7 @@ const double kPointMinDist = 0.1;
 }  // namespace
 
 bool OhMyLoam::Init() {
-  YAML::Node config = common::YAMLConfig::Instance()->config();
+  config_ = common::YAMLConfig::Instance()->config();
   extractor_.reset(new ExtractorVLP16);
   if (!extractor_->Init()) {
     AERROR << "Failed to initialize extractor";
@@ -21,14 +24,22 @@ bool OhMyLoam::Init() {
     AERROR << "Failed to initialize odometer";
     return false;
   }
-  // mapper_.reset(new Mapper);
+  mapper_.reset(new Mapper);
-  // if (!mapper_->Init()) {
+  if (!mapper_->Init()) {
-  //   AERROR << "Failed to initialize mapper";
+    AERROR << "Failed to initialize mapper";
-  //   return false;
+    return false;
-  // }
+  }
   return true;
 }
 
+void OhMyLoam::Reset() {
+  timestamp_last_ = timestamp_last_mapping_ = 0.0;
+  extractor_->Reset();
+  odometer_->Reset();
+  mapper_->Reset();
+  std::vector<common::Pose3d>().swap(poses_);
+}
+
 void OhMyLoam::Run(double timestamp,
                    const common::PointCloudConstPtr &cloud_in) {
   common::PointCloudPtr cloud(new common::PointCloud);
@@ -38,6 +49,8 @@ void OhMyLoam::Run(double timestamp,
   Pose3d pose;
   odometer_->Process(timestamp, features, &pose);
   poses_.emplace_back(pose);
+  if (!IsMapping(timestamp)) return;
+  mapper_->Process();
 }
 
 void OhMyLoam::RemoveOutliers(const common::PointCloud &cloud_in,
@@ -49,4 +62,9 @@ void OhMyLoam::RemoveOutliers(const common::PointCloud &cloud_in,
   });
 }
 
+bool OhMyLoam::IsMapping(double timestamp) const {
+  return std::abs(timestamp - timestamp_last_mapping_) >=
+         config_["mapper_config"]["process_period"].as<double>();
+}
+
 }  // namespace oh_my_loam

oh_my_loam/oh_my_loam.h

@@ -1,8 +1,10 @@
 #pragma once
 
+#include <yaml-cpp/node/node.h>
+
 #include "common/common.h"
 #include "oh_my_loam/extractor/extractor.h"
-// #include "oh_my_loam/mapper/mapper.h"
+#include "oh_my_loam/mapper/mapper.h"
 #include "oh_my_loam/odometer/odometer.h"
 
 namespace oh_my_loam {
@@ -16,18 +18,26 @@ class OhMyLoam {
   void Run(double timestamp, const common::PointCloudConstPtr &cloud_in);
 
  private:
+  void Reset();
+
+  bool IsMapping(double timestamp) const;
+
   std::unique_ptr<Extractor> extractor_{nullptr};
 
   std::unique_ptr<Odometer> odometer_{nullptr};
 
-  // std::unique_ptr<Mapper> mapper_{nullptr};
+  std::unique_ptr<Mapper> mapper_{nullptr};
 
-  // remove outliers: nan and very close points
+  // remove outliers: nan or very close points
   void RemoveOutliers(const common::PointCloud &cloud_in,
                       common::PointCloud *const cloud_out) const;
 
   std::vector<common::Pose3d> poses_;
 
+  YAML::Node config_;
+
+  double timestamp_last_ = 0.0, timestamp_last_mapping_ = 0.0;
+
   DISALLOW_COPY_AND_ASSIGN(OhMyLoam)
 };
 

oh_my_loam/solver/cost_function.h

@@ -59,22 +59,10 @@ bool PointLineCostFunction::operator()(const T *const r_quat,
   Eigen::Matrix<T, 3, 1> p(T(pt.x), T(pt.y), T(pt.z));
   Eigen::Matrix<T, 3, 1> p1(T(pt1.x), T(pt1.y), T(pt1.z));
   Eigen::Matrix<T, 3, 1> p2(T(pt2.x), T(pt2.y), T(pt2.z));
-  // if constexpr (!std::is_arithmetic<T>::value) {
-  //   AERROR << p.x().a << ", " << p.y().a << ", " << p.z().a;
-  //   AERROR << p1.x().a << ", " << p1.y().a << ", " << p1.z().a;
-  //   AERROR << p2.x().a << ", " << p2.y().a << ", " << p2.z().a;
-  // }
 
   Eigen::Quaternion<T> r(r_quat[3], r_quat[0], r_quat[1], r_quat[2]);
   Eigen::Quaternion<T> r_interp =
       Eigen::Quaternion<T>::Identity().slerp(T(time_), r);
-  // if constexpr (!std::is_arithmetic<T>::value) {
-  //   AERROR << time_;
-  //   AERROR << r.w().a << " " << r.x().a << ", " << r.y().a << ", " <<
-  //   r.z().a; AERROR << r_interp.w().a << " " << r_interp.x().a << ", " <<
-  //   r_interp.y().a
-  //          << ", " << r_interp.z().a;
-  // }
   Eigen::Matrix<T, 3, 1> t(T(time_) * t_vec[0], T(time_) * t_vec[1],
                            T(time_) * t_vec[2]);
   Eigen::Matrix<T, 3, 1> p0 = r_interp * p + t;
@@ -85,9 +73,6 @@ bool PointLineCostFunction::operator()(const T *const r_quat,
   residual[0] = area[0] / base_length;
   residual[1] = area[1] / base_length;
   residual[2] = area[2] / base_length;
-  // if constexpr (!std::is_arithmetic<T>::value) {
-  //   AERROR << base_length.a;
-  // }
   return true;
 }