writing mapper ...

common/geometry/pose3d.h

@@ -26,6 +26,13 @@ class Pose3d {
     t_vec_.setZero();
   }
 
+  const Eigen::Matrix4d TransMat() const {
+    Eigen::Matrix4d trans_mat = Eigen::Matrix4d::Identity();
+    trans_mat.topLeftCorner<3, 3>() = r_quat_.toRotationMatrix();
+    trans_mat.topRightCorner<3, 1>() = t_vec_;
+    return trans_mat;
+  }
+
   Pose3d Inv() const {
     Eigen::Quaterniond r_inv = r_quat_.inverse();
     Eigen::Vector3d t_inv = r_inv * t_vec_;

common/pcl/pcl_utils.h

@@ -1,5 +1,10 @@
 #pragma once
 
+#include <pcl/common/eigen.h>
+#include <pcl/common/transforms.h>
+#include <pcl/point_cloud.h>
+
+#include "common/geometry/pose3d.h"
 #include "common/log/log.h"
 #include "pcl_types.h"
 
@@ -36,6 +41,31 @@ inline double IsFinite(const PT &pt) {
   return std::isfinite(pt.x) && std::isfinite(pt.y) && std::isfinite(pt.z);
 }
 
+template <typename PT>
+inline void TransformPoint(const Pose3d &pose, const PT &pt_in,
+                           PT *const pt_out) {
+  *pt_out = pt_in;
+  Eigen::Vector3d p = pose * Eigen::Vector3d(pt_in.x, pt_in.y, pt_in.z);
+  pt_out->x = p.x();
+  pt_out->y = p.y();
+  pt_out->z = p.z();
+}
+
+template <typename PT>
+inline PT TransformPoint(const Pose3d &pose, const PT &pt_in) {
+  PT pt_out;
+  TransformPoint<PT>(pose, pt_in, &pt_out);
+  return pt_out;
+}
+
+template <typename PT>
+inline void TransformPointCloud(const Pose3d &pose,
+                                const pcl::PointCloud<PT> &cloud_in,
+                                pcl::PointCloud<PT> *const cloud_out) {
+  ACHECK(cloud_out);
+  pcl::transformPointCloud(cloud_in, *cloud_out, pose.TransMat().cast<float>());
+}
+
 // Remove point if the condition evaluated to true on it
 template <typename PT>
 void RemovePoints(const pcl::PointCloud<PT> &cloud_in,

oh_my_loam/base/helper.cc

@@ -1,11 +1,13 @@
-#include "helper.h"
+#include "oh_my_loam/base/helper.h"
+
+#include "common/pcl/pcl_utils.h"
 
 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);
+  common::TransformPoint<TPoint>(pose_interp, pt_in, pt_out);
 }
 
 TPoint TransformToStart(const Pose3d &pose, const TPoint &pt_in) {
@@ -17,7 +19,7 @@ TPoint TransformToStart(const Pose3d &pose, const TPoint &pt_in) {
 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);
+  common::TransformPoint<TPoint>(pose.Inv(), *pt_out, pt_out);
 }
 
 TPoint TransformToEnd(const Pose3d &pose, const TPoint &pt_in) {

oh_my_loam/base/helper.h

@@ -15,22 +15,6 @@ inline float GetTime(const TPoint &pt) {
   return pt.time - GetScanId(pt);
 }
 
-template <typename PT>
-void TransformPoint(const Pose3d &pose, const PT &pt_in, PT *const pt_out) {
-  *pt_out = pt_in;
-  Eigen::Vector3d p = pose * Eigen::Vector3d(pt_in.x, pt_in.y, pt_in.z);
-  pt_out->x = p.x();
-  pt_out->y = p.y();
-  pt_out->z = p.z();
-}
-
-template <typename PT>
-PT TransformPoint(const Pose3d &pose, const PT &pt_in) {
-  PT pt_out;
-  TransformPoint<PT>(pose, pt_in, &pt_out);
-  return pt_out;
-}
-
 /**
  * @brief Transform a lidar point to the start of the scan
  *

oh_my_loam/configs/config.yaml

@@ -35,3 +35,4 @@ mapper_config:
   verbose: false
   map_shape: [11, 21, 21]
   map_step_: 50 # meter
+  submap_shape: 5

oh_my_loam/mapper/map.cc

@@ -122,11 +122,16 @@ TPointCloudPtr Map::GetAllPoints() const {
   return cloud_all;
 }
 
-void Map::AddPoints(const TPointCloudConstPtr &cloud, IndexSet *const indices) {
+void Map::AddPoints(const TPointCloudConstPtr &cloud,
+                    std::vector<Index> *const indices) {
+  std::set<Index, Index::Comp> index_set;
   for (const auto &point : *cloud) {
     Index index = GetIndex(point);
     this->at(index)->push_back(point);
-    if (indices) indices->insert(index);
+    if (indices) index_set.insert(index);
+  }
+  if (indices) {
+    for (const auto &index : index_set) indices->push_back(index);
   }
 }
 

oh_my_loam/mapper/map.h

@@ -22,8 +22,6 @@ struct Index {
   };
 };
 
-using IndexSet = std::set<Index, Index::Comp>;
-
 class Map {
  public:
   Map(const std::vector<int> &shape, const std::vector<double> &step);
@@ -42,6 +40,10 @@ class Map {
 
   void ShiftX(int n);
 
+  const std::vector<int> Shape() const {
+    return std::vector<int>(shape_, shape_ + 3);
+  }
+
   Index GetIndex(const TPoint &point) const;
 
   TPointCloudPtr GetSurrPoints(const TPoint &point, int n) const;
@@ -49,7 +51,7 @@ class Map {
   TPointCloudPtr GetAllPoints() const;
 
   void AddPoints(const TPointCloudConstPtr &cloud,
-                 IndexSet *const indices = nullptr);
+                 std::vector<Index> *const indices = nullptr);
 
   void Downsample(double voxel_size);
 

oh_my_loam/mapper/mapper.cc

@@ -2,6 +2,13 @@
 
 #include <mutex>
 
+#include "common/log/log.h"
+#include "common/pcl/pcl_utils.h"
+#include "oh_my_loam/base/helper.h"
+#include "oh_my_loam/base/types.h"
+#include "oh_my_loam/mapper/map.h"
+#include "oh_my_loam/solver/solver.h"
+
 namespace oh_my_loam {
 
 namespace {
@@ -15,8 +22,8 @@ bool Mapper::Init() {
   verbose_ = config_["vis"].as<bool>();
   AINFO << "Mapping visualizer: " << (is_vis_ ? "ON" : "OFF");
   std::vector<int> shape = YAMLConfig::GetSeq<int>(config_["map_shape"]);
-  cloud_corn_map_.reset(new Map(shape, config_["map_step"].as<double>()));
+  corn_map_.reset(new Map(shape, config_["map_step"].as<double>()));
-  cloud_surf_map_.reset(new Map(shape, config_["map_step"].as<double>()));
+  surf_map_.reset(new Map(shape, config_["map_step"].as<double>()));
   return true;
 }
 
@@ -24,24 +31,83 @@ void Mapper::Reset() {}
 
 void Mapper::Process(double timestamp, const TPointCloudConstPtr &cloud_corn,
                      const TPointCloudConstPtr &cloud_surf,
-                     common::Pose3d *const pose_out) {
+                     const common::Pose3d &pose_curr2odom,
+                     common::Pose3d *const pose_curr2map) {
   if (GetState() == UN_INIT) {
-    cloud_corn_map_->AddPoints(cloud_corn);
+    corn_map_->AddPoints(cloud_corn);
-    cloud_surf_map_->AddPoints(cloud_surf);
+    surf_map_->AddPoints(cloud_surf);
-    pose_out->SetIdentity();
+    pose_curr2map->SetIdentity();
+    pose_odom2map_.SetIdentity();
     SetState(DONE);
     return;
   }
   if (GetState() == DONE) {
-  } else {  // RUNNING
+    thread_.reset(new std::thread(&Mapper::Run, this, cloud_corn, cloud_surf,
+                                  pose_curr2odom));
+    thread_->detach();
   }
+  std::lock_guard<std::mutex> lock(state_mutex_);
+  *pose_curr2map = pose_odom2map_ * pose_curr2odom;
 }
 
-void Mapper::Run(double timestamp, const TPointCloudConstPtr &cloud_corn,
+void Mapper::Run(const TPointCloudConstPtr &cloud_corn_in,
-                 const TPointCloudConstPtr &cloud_surf) {
+                 const TPointCloudConstPtr &cloud_surf_in,
-  TimePose pose;
+                 const Pose3d &pose_curr2odom) {
-  pose.timestamp = timestamp;
+  SetState(RUNNING);
-  std::lock_guard<std::mutex> lock(mutex_);
+  Pose3d pose_curr2map = pose_odom2map_ * pose_curr2odom;
+  TPoint cnt(pose_curr2map.t_vec().x(), pose_curr2map.t_vec().y(),
+             pose_curr2map.t_vec().z());
+  AdjustMap(corn_map_->GetIndex(cnt));
+  int submap_shape = config_["submap_shape"].as<int>();
+  TPointCloudPtr cloud_corn(new TPointCloud);
+  TPointCloudPtr cloud_surf(new TPointCloud);
+  TransformPointCloud(pose_curr2map, *cloud_corn_in, cloud_corn.get());
+  TransformPointCloud(pose_curr2map, *cloud_surf_in, cloud_surf.get());
+  TPointCloudPtr cloud_corn_map =
+      corn_map_->GetSurrPoints(cnt, submap_shape / 2);
+  TPointCloudPtr cloud_surf_map =
+      corn_map_->GetSurrPoints(cnt, submap_shape / 2);
+  // MatchCorn();
+  // MatchSurf();
+  PoseSolver solver(pose_curr2map);
+  if (!solver.Solve(5, false, &pose_curr2map)) {
+    AWARN << "Solve: no_convergence";
+  }
+  std::lock_guard<std::mutex> lock(state_mutex_);
+  pose_odom2map_ = pose_curr2map * pose_curr2odom.Inv();
+  SetState(DONE);
+}
+
+void Mapper::AdjustMap(const Index &index) {
+  std::vector<int> map_shape = corn_map_->Shape();
+  int min_idx = config_["submap_shape"].as<int>() / 2 + 1;
+  int max_idx_z = map_shape[0] - min_idx - 1;
+  if (index.k < min_idx) {
+    corn_map_->ShiftZ(index.k - min_idx);
+    surf_map_->ShiftZ(index.k - min_idx);
+  }
+  if (index.k > max_idx_z) {
+    corn_map_->ShiftZ(index.k - max_idx_z);
+    surf_map_->ShiftZ(index.k - max_idx_z);
+  }
+  int max_idx_y = map_shape[1] - min_idx - 1;
+  if (index.j < min_idx) {
+    corn_map_->ShiftY(index.j - min_idx);
+    surf_map_->ShiftY(index.j - min_idx);
+  }
+  if (index.j > max_idx_y) {
+    corn_map_->ShiftY(index.j - max_idx_y);
+    surf_map_->ShiftY(index.j - max_idx_y);
+  }
+  int max_idx_x = map_shape[2] - min_idx - 1;
+  if (index.i < min_idx) {
+    corn_map_->ShiftX(index.i - min_idx);
+    surf_map_->ShiftX(index.i - min_idx);
+  }
+  if (index.i > max_idx_x) {
+    corn_map_->ShiftX(index.i - max_idx_x);
+    surf_map_->ShiftX(index.i - max_idx_x);
+  }
 }
 
 void Mapper::Visualize() {}

oh_my_loam/mapper/mapper.h

@@ -3,9 +3,12 @@
 #include <sys/stat.h>
 
 #include <mutex>
+#include <shared_mutex>
 #include <vector>
 
 #include "common/geometry/pose3d.h"
+#include "oh_my_loam/base/helper.h"
+#include "oh_my_loam/base/types.h"
 #include "oh_my_loam/mapper/map.h"
 
 namespace oh_my_loam {
@@ -18,48 +21,67 @@ class Mapper {
 
   void Process(double timestamp, const TPointCloudConstPtr &cloud_corn,
                const TPointCloudConstPtr &cloud_surf,
-               common::Pose3d *const pose_out);
+               const common::Pose3d &pose_curr2odom,
+               common::Pose3d *const pose_curr2map);
+
+  TPointCloudPtr GetCornMapPoints() const {
+    std::shared_lock<std::shared_mutex> lock(corn_map_mutex_);
+    return corn_map_->GetAllPoints();
+  }
+
+  TPointCloudPtr GetSurfMapPoints() const {
+    std::shared_lock<std::shared_mutex> lock(surf_map_mutex_);
+    return surf_map_->GetAllPoints();
+  }
+
+  TPointCloudPtr GetMapPoints() const {
+    TPointCloudPtr map_points(new TPointCloud);
+    *map_points += *GetCornMapPoints();
+    *map_points += *GetSurfMapPoints();
+    return map_points;
+  }
 
   void Reset();
 
  private:
   enum State { DONE, RUNNING, UN_INIT };
 
-  void Run(double timestamp, const TPointCloudConstPtr &cloud_corn,
+  void Run(const TPointCloudConstPtr &cloud_corn,
-           const TPointCloudConstPtr &cloud_surf);
+           const TPointCloudConstPtr &cloud_surf, const Pose3d &pose_init);
 
-  void TransformUpdate();
+  void MatchCorn(const TPointCloudConstPtr &src, const TPointCloudConstPtr &tgt,
+                 std::vector<PointLinePair> *const pair) const;
 
-  void MapUpdate();
+  void MatchSurf(const TPointCloudConstPtr &src, const TPointCloudConstPtr &tgt,
+                 std::vector<PointPlanePair> *const pair) const;
 
-  State GetState() {
+  State GetState() const {
-    std::lock_guard<std::mutex> lock(mutex_);
+    std::lock_guard<std::mutex> lock(state_mutex_);
     return state_;
   }
 
   void SetState(State state) {
-    std::lock_guard<std::mutex> lock(mutex_);
+    std::lock_guard<std::mutex> lock(state_mutex_);
     state_ = state;
   }
 
+  void AdjustMap(const Index &index);
+
   void Visualize();
 
   YAML::Node config_;
 
-  struct TimePose {
+  mutable std::shared_mutex corn_map_mutex_;
-    double timestamp;
+  mutable std::shared_mutex surf_map_mutex_;
-    common::Pose3d pose;
+  std::unique_ptr<Map> corn_map_;
-  };
+  std::unique_ptr<Map> surf_map_;
-
-  std::unique_ptr<Map> cloud_corn_map_;
-  std::unique_ptr<Map> cloud_surf_map_;
-
-  std::mutex mutex_;
-  std::vector<TimePose> poses_;
-  std::unique_ptr<std::thread> thread_{nullptr};
 
+  mutable std::mutex state_mutex_;
+  Pose3d pose_odom2map_;
   State state_ = UN_INIT;
 
+  mutable std::unique_ptr<std::thread> thread_{nullptr};
+
   bool is_vis_ = false;
 
   bool verbose_ = false;