mapper almost done

common/math/fitting.h

@@ -0,0 +1,68 @@
+#pragma once
+
+#include "common/pcl/pcl_types.h"
+
+namespace common {
+/**
+ * @return Coefficients representing line equation ax + by + c = 0 (a^2 +
+ * b^2 = 1)
+ */
+template <typename PT>
+Eigen::Vector3d FitLine2D(const pcl::PointCloud<PT> &cloud,
+                          double *const score = nullptr) {
+  Eigen::MatrixX2f data(cloud.size(), 2);
+  size_t i = 0;
+  for (const auto &p : cloud) data.row(i++) << p.x, p.y;
+  Eigen::RowVector2f centroid = data.colwise().mean();
+  Eigen::MatrixX2f data_centered = data.rowwise() - centroid;
+  Eigen::JacobiSVD<Eigen::MatrixX2f> svd(data_centered, Eigen::ComputeThinV);
+  Eigen::Vector2f normal = svd.matrixV().col(1);
+  float c = -centroid * normal;
+  if (score) *score = svd.singularValues()[0] / svd.singularValues()[1];
+  return {normal.x(), normal.y(), c};
+}
+
+/**
+ * @return Coefficients with its first three components representing point
+ * cloud's centroid and last three -- line direction
+ */
+template <typename PT>
+Eigen::Matrix<double, 6, 1> FitLine3D(const pcl::PointCloud<PT> &cloud,
+                                      double *const score = nullptr) {
+  Eigen::MatrixX3f data(cloud.size(), 3);
+  size_t i = 0;
+  for (const auto &p : cloud) data.row(i++) << p.x, p.y, p.z;
+  Eigen::RowVector3f centroid = data.colwise().mean();
+  Eigen::MatrixX3f data_centered = data.rowwise() - centroid;
+  Eigen::JacobiSVD<Eigen::MatrixX3f> svd(data_centered, Eigen::ComputeThinV);
+  Eigen::Vector3f direction = svd.matrixV().col(0);
+  Eigen::Matrix<double, 6, 1> line_coeffs;
+  line_coeffs.topRows(3) = centroid.transpose().cast<double>();
+  line_coeffs.bottomRows(3) = direction.cast<double>();
+  if (score) *score = svd.singularValues()[0] / svd.singularValues()[1];
+  return line_coeffs;
+}
+
+/**
+ * @return Coefficients representing plane equation ax + by + cz + d = 0 (a^2 +
+ * b^2 + c^2 = 1)
+ */
+template <typename PT>
+Eigen::Vector3d FitPlane(const pcl::PointCloud<PT> &cloud,
+                         double *const score = nullptr) {
+  Eigen::MatrixX3f data(cloud.size(), 3);
+  size_t i = 0;
+  for (const auto &p : cloud) data.row(i++) << p.x, p.y, p.z;
+  Eigen::RowVector3f centroid = data.colwise().mean();
+  Eigen::MatrixX3f data_centered = data.rowwise() - centroid;
+  Eigen::JacobiSVD<Eigen::MatrixX3f> svd(data_centered, Eigen::ComputeThinV);
+  Eigen::Vector2f normal = svd.matrixV().col(2);
+  float d = -centroid * normal;
+  if (score) {
+    *score = svd.singularValues()[1] * svd.singularValues()[1] /
+             (svd.singularValues()[0] * svd.singularValues()[2]);
+  }
+  return {normal.x(), normal.y(), normal.z(), d};
+}
+
+}  // namespace common

oh_my_loam/base/helper.h

@@ -1,66 +0,0 @@
-#pragma once
-
-#include "common/geometry/pose3d.h"
-#include "oh_my_loam/base/types.h"
-
-namespace oh_my_loam {
-
-using common::Pose3d;
-
-inline int GetScanId(const TPoint &pt) {
-  return static_cast<int>(pt.time);
-}
-
-inline float GetTime(const TPoint &pt) {
-  return pt.time - GetScanId(pt);
-}
-
-/**
- * @brief Transform a lidar point to the start of the scan
- *
- * @param pose Relative pose, end scan time w.r.t. start scan time
- */
-void TransformToStart(const Pose3d &pose, const TPoint &pt_in,
-                      TPoint *const pt_out);
-
-TPoint TransformToStart(const Pose3d &pose, const TPoint &pt_in);
-
-/**
- * @brief Transform a lidar point to the end of the scan
- *
- * @param pose Relative pose, end scan time w.r.t. start scan time
- */
-void TransformToEnd(const Pose3d &pose, const TPoint &pt_in,
-                    TPoint *const pt_out);
-
-TPoint TransformToEnd(const Pose3d &pose, const TPoint &pt_in);
-
-struct PointLinePair {
-  TPoint pt;
-  struct Line {
-    TPoint pt1, pt2;
-    Line() = default;
-    Line(const TPoint &pt1, const TPoint &pt2) : pt1(pt1), pt2(pt2) {}
-  };
-  Line line;
-  PointLinePair(const TPoint &pt, const Line &line) : pt(pt), line(line) {}
-  PointLinePair(const TPoint &pt, const TPoint &pt1, const TPoint &pt2)
-      : pt(pt), line(pt1, pt2) {}
-};
-
-struct PointPlanePair {
-  TPoint pt;
-  struct Plane {
-    TPoint pt1, pt2, pt3;
-    Plane() = default;
-    Plane(const TPoint &pt1, const TPoint &pt2, const TPoint &pt3)
-        : pt1(pt1), pt2(pt2), pt3(pt3) {}
-  };
-  Plane plane;
-  PointPlanePair(const TPoint &pt, const Plane &plane) : pt(pt), plane(plane) {}
-  PointPlanePair(const TPoint &pt, const TPoint &pt1, const TPoint &pt2,
-                 const TPoint &pt3)
-      : pt(pt), plane(pt1, pt2, pt3) {}
-};
-
-}  // namespace oh_my_loam

oh_my_loam/base/utils.h

@@ -0,0 +1,36 @@
+#pragma once
+
+#include "common/geometry/pose3d.h"
+#include "oh_my_loam/base/types.h"
+
+namespace oh_my_loam {
+
+inline int GetScanId(const TPoint &pt) {
+  return static_cast<int>(pt.time);
+}
+
+inline float GetTime(const TPoint &pt) {
+  return pt.time - GetScanId(pt);
+}
+
+/**
+ * @brief Transform a lidar point to the start of the scan
+ *
+ * @param pose Relative pose, end scan time w.r.t. start scan time
+ */
+void TransformToStart(const common::Pose3d &pose, const TPoint &pt_in,
+                      TPoint *const pt_out);
+
+TPoint TransformToStart(const common::Pose3d &pose, const TPoint &pt_in);
+
+/**
+ * @brief Transform a lidar point to the end of the scan
+ *
+ * @param pose Relative pose, end scan time w.r.t. start scan time
+ */
+void TransformToEnd(const common::Pose3d &pose, const TPoint &pt_in,
+                    TPoint *const pt_out);
+
+TPoint TransformToEnd(const common::Pose3d &pose, const TPoint &pt_in);
+
+}  // namespace oh_my_loam

oh_my_loam/configs/config.yaml

@@ -34,5 +34,9 @@ mapper_config:
   vis: false
   verbose: false
   map_shape: [3, 21, 21]
-  map_step_: 50 # meter
+  map_step: 50 # meter
   submap_shape: [1, 5, 5]
+  nearest_neighbor_k: 5
+  neighbor_point_dist_sq_th: 1.0
+  min_line_fit_score: 2
+  min_plane_fit_score: 2

oh_my_loam/extractor/extractor.h

@@ -2,7 +2,7 @@
 
 #include "common/common.h"
 #include "oh_my_loam/base/feature.h"
-#include "oh_my_loam/base/helper.h"
+#include "oh_my_loam/base/utils.h"
 #include "oh_my_loam/base/types.h"
 #include "oh_my_loam/visualizer/extractor_visualizer.h"
 

oh_my_loam/mapper/map.cc

@@ -108,9 +108,10 @@ Index Map::GetIndex(const TPoint &point) const {
   return index;
 }
 
-TPointCloudPtr Map::GetSurrPoints(const TPoint &point, int n) const {
+TPointCloudPtr Map::GetSurrPoints(const TPoint &point,
+                                  const std::vector<int> &surr_shapes) const {
   TPointCloudPtr cloud_all(new TPointCloud);
-  for (const auto &index : GetSurrIndices(point, n)) {
+  for (const auto &index : GetSurrIndices(point, surr_shapes)) {
     *cloud_all += *this->at(index);
   }
   return cloud_all;
@@ -165,12 +166,14 @@ const Row &Map::at(int z_idx, int y_idx) const {
   return map_.at(z_idx).at(y_idx);
 }
 
-std::vector<Index> Map::GetSurrIndices(const TPoint &point, int n) const {
+std::vector<Index> Map::GetSurrIndices(
+    const TPoint &point, const std::vector<int> &surr_shapes) const {
   std::vector<Index> indices;
   Index index = GetIndex(point);
-  for (int k = -n; k <= n; ++k) {
+  int nz = surr_shapes[0] / 2, ny = surr_shapes[1] / 2, nx = surr_shapes[2] / 2;
-    for (int j = -n; j <= n; ++j) {
+  for (int k = -nz; k <= nz; ++k) {
-      for (int i = -n; i <= n; ++i) {
+    for (int j = -ny; j <= ny; ++j) {
+      for (int i = -nx; i <= nx; ++i) {
         indices.emplace_back(index.k + k, index.j + j, index.i + i);
       }
     }

oh_my_loam/mapper/map.h

@@ -46,7 +46,8 @@ class Map {
 
   Index GetIndex(const TPoint &point) const;
 
-  TPointCloudPtr GetSurrPoints(const TPoint &point, int n) const;
+  TPointCloudPtr GetSurrPoints(const TPoint &point,
+                               const std::vector<int> &surr_shapes) const;
 
   TPointCloudPtr GetAllPoints() const;
 
@@ -70,7 +71,8 @@ class Map {
 
   const TPointCloudPtr &at(int z_idx, int y_idx, int x_idx) const;
 
-  std::vector<Index> GetSurrIndices(const TPoint &point, int n) const;
+  std::vector<Index> GetSurrIndices(const TPoint &point,
+                                    const std::vector<int> &surr_shapes) const;
 
   std::vector<Index> GetAllIndices() const;
 

oh_my_loam/mapper/mapper.cc

@@ -3,16 +3,14 @@
 #include <mutex>
 
 #include "common/log/log.h"
+#include "common/math/fitting.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 {
-using namespace common;
+using common::YAMLConfig;
+using LineCoeff = Eigen::Matrix<double, 6, 1>;
 }  // namespace
 
 bool Mapper::Init() {
@@ -29,7 +27,9 @@ bool Mapper::Init() {
   return true;
 }
 
-void Mapper::Reset() {}
+void Mapper::Reset() {
+  SetState(UN_INIT);
+}
 
 void Mapper::Process(double timestamp, const TPointCloudConstPtr &cloud_corn,
                      const TPointCloudConstPtr &cloud_surf,
@@ -46,32 +46,37 @@ void Mapper::Process(double timestamp, const TPointCloudConstPtr &cloud_corn,
   if (GetState() == DONE) {
     thread_.reset(new std::thread(&Mapper::Run, this, cloud_corn, cloud_surf,
                                   pose_curr2odom));
-    thread_->detach();
+    if (thread_->joinable()) thread_->detach();
   }
   std::lock_guard<std::mutex> lock(state_mutex_);
   *pose_curr2map = pose_odom2map_ * pose_curr2odom;
 }
 
-void Mapper::Run(const TPointCloudConstPtr &cloud_corn_in,
+void Mapper::Run(const TPointCloudConstPtr &cloud_corn,
-                 const TPointCloudConstPtr &cloud_surf_in,
+                 const TPointCloudConstPtr &cloud_surf,
-                 const Pose3d &pose_curr2odom) {
+                 const common::Pose3d &pose_curr2odom) {
   SetState(RUNNING);
-  Pose3d pose_curr2map = pose_odom2map_ * pose_curr2odom;
+  common::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));
+  AdjustMap(cnt);
-  int submap_shape = config_["submap_shape"].as<int>();
+  TPointCloudPtr cloud_corn_map = corn_map_->GetSurrPoints(cnt, submap_shape_);
-  TPointCloudPtr cloud_corn(new TPointCloud);
+  TPointCloudPtr cloud_surf_map = corn_map_->GetSurrPoints(cnt, submap_shape_);
-  TPointCloudPtr cloud_surf(new TPointCloud);
+  pcl::KdTreeFLANN<TPoint> kdtree_corn;
-  TransformPointCloud(pose_curr2map, *cloud_corn_in, cloud_corn.get());
+  kdtree_corn.setInputCloud(cloud_corn_map);
-  TransformPointCloud(pose_curr2map, *cloud_surf_in, cloud_surf.get());
+  pcl::KdTreeFLANN<TPoint> kdtree_surf;
-  TPointCloudPtr cloud_corn_map =
+  kdtree_surf.setInputCloud(cloud_surf_map);
-      corn_map_->GetSurrPoints(cnt, submap_shape / 2);
+  std::vector<PointLinePair> pl_pairs;
-  TPointCloudPtr cloud_surf_map =
+  std::vector<PointPlaneCoeffPair> pp_pairs;
-      corn_map_->GetSurrPoints(cnt, submap_shape / 2);
+  MatchCorn(kdtree_corn, cloud_corn_map, pose_curr2map, &pl_pairs);
-  // MatchCorn();
+  MatchSurf(kdtree_surf, cloud_surf_map, pose_curr2map, &pp_pairs);
-  // MatchSurf();
   PoseSolver solver(pose_curr2map);
+  for (const auto &pair : pl_pairs) {
+    solver.AddPointLinePair(pair, 0.0);
+  }
+  for (const auto &pair : pp_pairs) {
+    solver.AddPointPlaneCoeffPair(pair, 0.0);
+  }
   if (!solver.Solve(5, false, &pose_curr2map)) {
     AWARN << "Solve: no_convergence";
   }
@@ -80,7 +85,62 @@ void Mapper::Run(const TPointCloudConstPtr &cloud_corn_in,
   SetState(DONE);
 }
 
-void Mapper::AdjustMap(const Index &index) {
+void Mapper::MatchCorn(const pcl::KdTreeFLANN<TPoint> &kdtree,
+                       const TPointCloudConstPtr &cloud_curr,
+                       const common::Pose3d &pose_curr2map,
+                       std::vector<PointLinePair> *const pairs) const {
+  std::vector<int> indices;
+  std::vector<float> dists;
+  int nearest_neighbor_k = config_["nearest_neighbor_k"].as<int>();
+  float neighbor_point_dist_sq_th =
+      config_["neighbor_point_dist_sq_th"].as<float>();
+  for (const auto &pt : *cloud_curr) {
+    TPoint pt_query = common::TransformPoint(pose_curr2map, pt);
+    if (kdtree.nearestKSearch(pt_query, nearest_neighbor_k, indices, dists) !=
+        nearest_neighbor_k) {
+      continue;
+    }
+    if (dists.back() > neighbor_point_dist_sq_th) continue;
+    TPointCloud neighbor_pts;
+    pcl::copyPointCloud(*cloud_curr, indices, neighbor_pts);
+    double fit_score = 0.0;
+    LineCoeff coeff = common::FitLine3D(neighbor_pts, &fit_score);
+    if (fit_score < config_["min_line_fit_score"].as<double>()) continue;
+    TPoint pt1(coeff[0], coeff[1], coeff[2], 0.0);
+    TPoint pt2(coeff[0] + 0.1 * coeff[3], coeff[1] + 0.1 * coeff[4],
+               coeff[2] + 0.1 * coeff[5], 0.0);
+    pairs->emplace_back(pt, pt1, pt2);
+  }
+}
+
+void Mapper::MatchSurf(const pcl::KdTreeFLANN<TPoint> &kdtree,
+                       const TPointCloudConstPtr &cloud_curr,
+                       const common::Pose3d &pose_curr2map,
+                       std::vector<PointPlaneCoeffPair> *const pairs) const {
+  std::vector<int> indices;
+  std::vector<float> dists;
+  int nearest_neighbor_k = config_["nearest_neighbor_k"].as<int>();
+  float neighbor_point_dist_sq_th =
+      config_["neighbor_point_dist_sq_th"].as<float>();
+  for (const auto &pt : *cloud_curr) {
+    TPoint pt_query = common::TransformPoint(pose_curr2map, pt);
+    if (kdtree.nearestKSearch(pt_query, nearest_neighbor_k, indices, dists) !=
+        nearest_neighbor_k) {
+      continue;
+    }
+    if (dists.back() > neighbor_point_dist_sq_th) continue;
+    TPointCloud neighbor_pts;
+    pcl::copyPointCloud(*cloud_curr, indices, neighbor_pts);
+    double fit_score = 0.0;
+    Eigen::Vector4d coeff = common::FitPlane(neighbor_pts, &fit_score);
+    if (fit_score < config_["min_plane_fit_score"].as<double>()) continue;
+    TPoint pt1(coeff[0], coeff[1], coeff[2], 0.0);
+    pairs->emplace_back(pt, coeff);
+  }
+}
+
+void Mapper::AdjustMap(const TPoint &center) {
+  Index index = corn_map_->GetIndex(center);
   int min_idx_z = submap_shape_[0] / 2 + 1;
   int max_idx_z = map_shape_[0] - min_idx_z - 1;
   if (index.k < min_idx_z) {
@@ -113,6 +173,31 @@ void Mapper::AdjustMap(const Index &index) {
   }
 }
 
+TPointCloudPtr Mapper::GetCornMapPoints() const {
+  return corn_map_->GetAllPoints();
+}
+
+TPointCloudPtr Mapper::GetSurfMapPoints() const {
+  return surf_map_->GetAllPoints();
+}
+
+TPointCloudPtr Mapper::GetMapPoints() const {
+  TPointCloudPtr map_points(new TPointCloud);
+  *map_points += *GetCornMapPoints();
+  *map_points += *GetSurfMapPoints();
+  return map_points;
+}
+
+Mapper::State Mapper::GetState() const {
+  std::lock_guard<std::mutex> lock(state_mutex_);
+  return state_;
+}
+
+void Mapper::SetState(State state) {
+  std::lock_guard<std::mutex> lock(state_mutex_);
+  state_ = state;
+}
+
 void Mapper::Visualize() {}
 
 }  // namespace oh_my_loam

oh_my_loam/mapper/mapper.h

@@ -7,9 +7,10 @@
 #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/base/utils.h"
 #include "oh_my_loam/mapper/map.h"
+#include "oh_my_loam/solver/solver.h"
 
 namespace oh_my_loam {
 
@@ -24,22 +25,11 @@ class Mapper {
                const common::Pose3d &pose_curr2odom,
                common::Pose3d *const pose_curr2map);
 
-  TPointCloudPtr GetCornMapPoints() const {
+  TPointCloudPtr GetCornMapPoints() const;
-    std::shared_lock<std::shared_mutex> lock(corn_map_mutex_);
-    return corn_map_->GetAllPoints();
-  }
 
-  TPointCloudPtr GetSurfMapPoints() const {
+  TPointCloudPtr GetSurfMapPoints() const;
-    std::shared_lock<std::shared_mutex> lock(surf_map_mutex_);
-    return surf_map_->GetAllPoints();
-  }
 
-  TPointCloudPtr GetMapPoints() const {
+  TPointCloudPtr GetMapPoints() const;
-    TPointCloudPtr map_points(new TPointCloud);
-    *map_points += *GetCornMapPoints();
-    *map_points += *GetSurfMapPoints();
-    return map_points;
-  }
 
   void Reset();
 
@@ -47,41 +37,44 @@ class Mapper {
   enum State { DONE, RUNNING, UN_INIT };
 
   void Run(const TPointCloudConstPtr &cloud_corn,
-           const TPointCloudConstPtr &cloud_surf, const Pose3d &pose_init);
+           const TPointCloudConstPtr &cloud_surf,
+           const common::Pose3d &pose_init);
 
-  void MatchCorn(const TPointCloudConstPtr &src, const TPointCloudConstPtr &tgt,
+  void MatchCorn(const pcl::KdTreeFLANN<TPoint> &kdtree,
-                 std::vector<PointLinePair> *const pair) const;
+                 const TPointCloudConstPtr &cloud_curr,
+                 const common::Pose3d &pose_curr2map,
+                 std::vector<PointLinePair> *const pairs) const;
 
-  void MatchSurf(const TPointCloudConstPtr &src, const TPointCloudConstPtr &tgt,
+  void MatchSurf(const pcl::KdTreeFLANN<TPoint> &kdtree,
-                 std::vector<PointPlanePair> *const pair) const;
+                 const TPointCloudConstPtr &cloud_curr,
+                 const common::Pose3d &pose_curr2map,
+                 std::vector<PointPlaneCoeffPair> *const pairs) const;
 
-  State GetState() const {
+  State GetState() const;
-    std::lock_guard<std::mutex> lock(state_mutex_);
-    return state_;
-  }
 
-  void SetState(State state) {
+  void SetState(State state);
-    std::lock_guard<std::mutex> lock(state_mutex_);
-    state_ = state;
-  }
 
-  void AdjustMap(const Index &index);
+  void AdjustMap(const TPoint &center);
+
+  void MatchCorn(const TPointCloudConstPtr &src,
+                 const TCTPointCloudConstPtr &tgt,
+                 std::vector<PointLinePair> *const pairs) const;
+
+  void MatchSurf(const TPointCloudConstPtr &src,
+                 const TCTPointCloudConstPtr &tgt,
+                 std::vector<PointLinePair> *const pairs) const;
 
   void Visualize();
 
   YAML::Node config_;
 
-  mutable std::shared_mutex corn_map_mutex_;
-  mutable std::shared_mutex surf_map_mutex_;
   std::vector<int> map_shape_, submap_shape_;
   double map_step_;
   std::unique_ptr<Map> corn_map_;
   std::unique_ptr<Map> surf_map_;
-  pcl::KdTreeFLANN<TPoint> kdtree_corn_map_;
-  pcl::KdTreeFLANN<TPoint> kdtree_surf_map_;
 
   mutable std::mutex state_mutex_;
-  Pose3d pose_odom2map_;
+  common::Pose3d pose_odom2map_;
   State state_ = UN_INIT;
 
   mutable std::unique_ptr<std::thread> thread_{nullptr};

oh_my_loam/odometer/odometer.cc

@@ -5,8 +5,6 @@
 #include "common/log/log.h"
 #include "common/pcl/pcl_utils.h"
 #include "common/time/timer.h"
-#include "oh_my_loam/base/types.h"
-#include "oh_my_loam/solver/solver.h"
 
 namespace oh_my_loam {
 

oh_my_loam/odometer/odometer.h

@@ -1,9 +1,9 @@
 #pragma once
 
 #include "common/common.h"
-#include "common/geometry/pose3d.h"
 #include "oh_my_loam/base/feature.h"
-#include "oh_my_loam/base/helper.h"
+#include "oh_my_loam/base/utils.h"
+#include "oh_my_loam/solver/solver.h"
 #include "oh_my_loam/visualizer/odometer_visualizer.h"
 
 namespace oh_my_loam {

oh_my_loam/solver/cost_function.h

@@ -2,14 +2,50 @@
 
 #include <ceres/ceres.h>
 
-#include <eigen3/Eigen/Dense>
-#include <type_traits>
-
 #include "common/common.h"
-#include "oh_my_loam/base/helper.h"
+#include "oh_my_loam/base/utils.h"
 
 namespace oh_my_loam {
 
+namespace {
+const double kEpsilon = 1.0e-6;
+}
+
+struct PointLinePair {
+  TPoint pt;
+  struct Line {
+    TPoint pt1, pt2;
+    Line() = default;
+    Line(const TPoint &pt1, const TPoint &pt2) : pt1(pt1), pt2(pt2) {}
+  };
+  Line line;
+  PointLinePair(const TPoint &pt, const Line &line) : pt(pt), line(line) {}
+  PointLinePair(const TPoint &pt, const TPoint &pt1, const TPoint &pt2)
+      : pt(pt), line(pt1, pt2) {}
+};
+
+struct PointPlanePair {
+  TPoint pt;
+  struct Plane {
+    TPoint pt1, pt2, pt3;
+    Plane() = default;
+    Plane(const TPoint &pt1, const TPoint &pt2, const TPoint &pt3)
+        : pt1(pt1), pt2(pt2), pt3(pt3) {}
+  };
+  Plane plane;
+  PointPlanePair(const TPoint &pt, const Plane &plane) : pt(pt), plane(plane) {}
+  PointPlanePair(const TPoint &pt, const TPoint &pt1, const TPoint &pt2,
+                 const TPoint &pt3)
+      : pt(pt), plane(pt1, pt2, pt3) {}
+};
+
+struct PointPlaneCoeffPair {
+  TPoint pt;
+  Eigen::Vector4d plane_coeff;
+  PointPlaneCoeffPair(const TPoint &pt, const Eigen::Vector4d &plane_coeff)
+      : pt(pt), plane_coeff(plane_coeff) {}
+};
+
 class PointLineCostFunction {
  public:
   PointLineCostFunction(const PointLinePair &pair, double time)
@@ -51,6 +87,28 @@ class PointPlaneCostFunction {
   DISALLOW_COPY_AND_ASSIGN(PointPlaneCostFunction)
 };
 
+class PointPlaneCoeffCostFunction {
+ public:
+  PointPlaneCoeffCostFunction(const PointPlaneCoeffPair &pair, double time)
+      : pair_(pair), time_(time){};
+
+  template <typename T>
+  bool operator()(const T *const r_quat, const T *const t_vec,
+                  T *residual) const;
+
+  static ceres::CostFunction *Create(const PointPlaneCoeffPair &pair,
+                                     double time) {
+    return new ceres::AutoDiffCostFunction<PointPlaneCoeffCostFunction, 1, 4,
+                                           3>(
+        new PointPlaneCoeffCostFunction(pair, time));
+  }
+
+ private:
+  PointPlaneCoeffPair pair_;
+  double time_;
+  DISALLOW_COPY_AND_ASSIGN(PointPlaneCoeffCostFunction)
+};
+
 template <typename T>
 bool PointLineCostFunction::operator()(const T *const r_quat,
                                        const T *const t_vec,
@@ -61,11 +119,12 @@ bool PointLineCostFunction::operator()(const T *const r_quat,
   Eigen::Matrix<T, 3, 1> p2(T(pt2.x), T(pt2.y), T(pt2.z));
 
   Eigen::Quaternion<T> r(r_quat[3], r_quat[0], r_quat[1], r_quat[2]);
-  Eigen::Quaternion<T> r_interp =
+  Eigen::Matrix<T, 3, 1> t(t_vec[0], t_vec[1], t_vec[2]);
-      Eigen::Quaternion<T>::Identity().slerp(T(time_), r);
+  if (time_ <= 1.0 - kEpsilon) {
-  Eigen::Matrix<T, 3, 1> t(T(time_) * t_vec[0], T(time_) * t_vec[1],
+    r = Eigen::Quaternion<T>::Identity().slerp(T(time_), r);
-                           T(time_) * t_vec[2]);
+    t = T(time_) * t;
-  Eigen::Matrix<T, 3, 1> p0 = r_interp * p + t;
+  }
+  Eigen::Matrix<T, 3, 1> p0 = r * p + t;
 
   // norm of cross product: triangle area x 2
   Eigen::Matrix<T, 3, 1> area = (p0 - p1).cross(p0 - p2) * 0.5;
@@ -88,15 +147,35 @@ bool PointPlaneCostFunction::operator()(const T *const r_quat,
   Eigen::Matrix<T, 3, 1> p3(T(pt3.x), T(pt3.y), T(pt3.z));
 
   Eigen::Quaternion<T> r(r_quat[3], r_quat[0], r_quat[1], r_quat[2]);
-  Eigen::Quaternion<T> r_interp =
+  Eigen::Matrix<T, 3, 1> t(t_vec[0], t_vec[1], t_vec[2]);
-      Eigen::Quaternion<T>::Identity().slerp(T(time_), r);
+  if (time_ <= 1.0 - kEpsilon) {
-  Eigen::Matrix<T, 3, 1> t(T(time_) * t_vec[0], T(time_) * t_vec[1],
+    r = Eigen::Quaternion<T>::Identity().slerp(T(time_), r);
-                           T(time_) * t_vec[2]);
+    t = T(time_) * t;
-  Eigen::Matrix<T, 3, 1> p0 = r_interp * p + t;
+  }
+  Eigen::Matrix<T, 3, 1> p0 = r * p + t;
 
   Eigen::Matrix<T, 3, 1> normal = (p2 - p1).cross(p3 - p1).normalized();
   residual[0] = (p0 - p1).dot(normal);
   return true;
 }
 
+template <typename T>
+bool PointPlaneCoeffCostFunction::operator()(const T *const r_quat,
+                                             const T *const t_vec,
+                                             T *residual) const {
+  Eigen::Matrix<T, 3, 1> p(T(pair_.pt.x), T(pair_.pt.y), T(pair_.pt.z));
+  Eigen::Matrix<T, 4, 1> coeff = pair_.plane_coeff.template cast<T>();
+
+  Eigen::Quaternion<T> r(r_quat[3], r_quat[0], r_quat[1], r_quat[2]);
+  Eigen::Matrix<T, 3, 1> t(t_vec[0], t_vec[1], t_vec[2]);
+  if (time_ <= 1.0 - kEpsilon) {
+    r = Eigen::Quaternion<T>::Identity().slerp(T(time_), r);
+    t = T(time_) * t;
+  }
+  Eigen::Matrix<T, 3, 1> p0 = r * p + t;
+
+  residual[0] = coeff.topRows(3).transpose() * p + coeff[3];
+  return true;
+}
+
 }  // namespace oh_my_loam

oh_my_loam/solver/solver.cc

@@ -46,6 +46,13 @@ void PoseSolver::AddPointPlanePair(const PointPlanePair &pair, double time) {
   problem_.AddResidualBlock(cost_function, loss_function_, r_quat_, t_vec_);
 }
 
+void PoseSolver::AddPointPlaneCoeffPair(const PointPlaneCoeffPair &pair,
+                                        double time) {
+  ceres::CostFunction *cost_function =
+      PointPlaneCoeffCostFunction::Create(pair, time);
+  problem_.AddResidualBlock(cost_function, loss_function_, r_quat_, t_vec_);
+}
+
 common::Pose3d PoseSolver::GetPose() const {
   return common::Pose3d(r_quat_, t_vec_);
 }

oh_my_loam/solver/solver.h

@@ -13,6 +13,8 @@ class PoseSolver {
 
   void AddPointPlanePair(const PointPlanePair &pair, double time);
 
+  void AddPointPlaneCoeffPair(const PointPlaneCoeffPair &pair, double time);
+
   bool Solve(int max_iter_num = 5, bool verbose = false,
              common::Pose3d *const pose = nullptr);