opt fitting, cost_function

common/math/fitting.h

@@ -1,5 +1,6 @@
 #pragma once
 
+#include <eigen3/Eigen/Dense>
 #include "common/pcl/pcl_types.h"
 
 namespace common {
@@ -15,11 +16,15 @@ Eigen::Vector3d FitLine2D(const pcl::PointCloud<PT> &cloud,
   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::ComputeFullV);
+  Eigen::Matrix2f cov_mat = data_centered.transpose() * data_centered;
-  Eigen::Vector2f normal = svd.matrixV().col(1);
+  Eigen::SelfAdjointEigenSolver<Eigen::Matrix2f> solver(cov_mat);
-  float c = -centroid * normal;
+  Eigen::Vector2f normal = solver.eigenvectors().col(0);
-  if (score) *score = svd.singularValues()[0] / svd.singularValues()[1];
+  double c = -centroid * normal;
-  return {normal.x(), normal.y(), c};
+  Eigen::Vector3d coeffs(normal(0), normal(1), c);
+  if (score) {
+    *score = solver.eigenvalues()[1] / (solver.eigenvalues()[0] + 1e-7);
+  }
+  return coeffs;
 }
 
 /**
@@ -34,13 +39,16 @@ Eigen::Matrix<double, 6, 1> FitLine3D(const pcl::PointCloud<PT> &cloud,
   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::ComputeFullV);
+  Eigen::Matrix3f cov_mat = data_centered.transpose() * data_centered;
-  Eigen::Vector3f direction = svd.matrixV().col(0);
+  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> solver(cov_mat);
-  Eigen::Matrix<double, 6, 1> line_coeffs;
+  Eigen::Vector3f direction = solver.eigenvectors().col(2);
-  line_coeffs.topRows(3) = centroid.transpose().cast<double>();
+  Eigen::Matrix<double, 6, 1> coeffs;
-  line_coeffs.bottomRows(3) = direction.cast<double>();
+  coeffs.topRows(3) = centroid.transpose().cast<double>();
-  if (score) *score = svd.singularValues()[0] / svd.singularValues()[1];
+  coeffs.bottomRows(3) = direction.cast<double>();
-  return line_coeffs;
+  if (score) {
+    *score = solver.eigenvalues()[2] / (solver.eigenvalues()[1] + 1e-7);
+  }
+  return coeffs;
 }
 
 /**
@@ -55,14 +63,29 @@ Eigen::Vector4d FitPlane(const pcl::PointCloud<PT> &cloud,
   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::ComputeFullV);
+  Eigen::Matrix3f cov_mat = data_centered.transpose() * data_centered;
-  Eigen::Vector3f normal = svd.matrixV().col(2);
+  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> solver(cov_mat);
-  float d = -centroid * normal;
+  Eigen::Vector3f normal = solver.eigenvectors().col(0);
-  if (score) {
+  double d = -centroid * normal;
-    *score = svd.singularValues()[1] * svd.singularValues()[1] /
+  Eigen::Vector4d coeffs(normal(0), normal(1), normal(2), d);
-             (svd.singularValues()[0] * svd.singularValues()[2]);
+  if (score)
-  }
+    *score = solver.eigenvalues()[1] * solver.eigenvalues()[1] /
-  return {normal.x(), normal.y(), normal.z(), d};
+             (solver.eigenvalues()[2] * solver.eigenvalues()[0] + 1e-7);
+  return coeffs;
 }
 
+// template <typename PT>
+// Eigen::Vector4d FitPlane(const pcl::PointCloud<PT> &cloud) {
+//   Eigen::MatrixX3f A(cloud.size(), 3);  // NOLINT
+//   Eigen::VectorXf b(cloud.size());
+//   b.setConstant(-1.0);
+//   size_t i = 0;
+//   for (const auto &p : cloud) {
+//     A.row(i++) << p.x, p.y, p.z;
+//   }
+//   Eigen::Vector3f sol = A.colPivHouseholderQr().solve(b);
+//   Eigen::Vector4d coeff(sol(0), sol(1), sol(2), 1.0);
+//   return coeff / sol.norm();
+// }
+
 }  // namespace common

oh_my_loam/configs/config.yaml

@@ -21,7 +21,7 @@ extractor_config:
 # configs for odometer
 odometer_config:
   vis: false
-  verbose: true
+  verbose: false
   nearby_scan_num: 1
   min_correspondence_num: 10
   icp_iter_num: 4
@@ -32,7 +32,7 @@ odometer_config:
 # configs for mapper
 mapper_config:
   vis: false
-  verbose: false
+  verbose: true
   map_shape: [3, 21, 21]
   map_step: 50
   submap_shape: [1, 5, 5]

oh_my_loam/mapper/mapper.cc

@@ -16,8 +16,8 @@ using LineCoeff = Eigen::Matrix<double, 6, 1>;
 bool Mapper::Init() {
   const auto &config = YAMLConfig::Instance()->config();
   config_ = config["mapper_config"];
-  is_vis_ = config["vis"].as<bool>() && config_["vis"].as<bool>();
+  is_vis_ = config_["vis"].as<bool>();
-  verbose_ = config_["vis"].as<bool>();
+  verbose_ = config_["verbose"].as<bool>();
   AINFO << "Mapping visualizer: " << (is_vis_ ? "ON" : "OFF");
   map_shape_ = YAMLConfig::GetSeq<int>(config_["map_shape"]);
   submap_shape_ = YAMLConfig::GetSeq<int>(config_["submap_shape"]);
@@ -68,7 +68,7 @@ void Mapper::Run(const TPointCloudConstPtr &cloud_corn,
     kdtree_corn.setInputCloud(cloud_corn_map);
     pcl::KdTreeFLANN<TPoint> kdtree_surf;
     kdtree_surf.setInputCloud(cloud_surf_map);
-    std::vector<PointLinePair> pl_pairs;
+    std::vector<PointLineCoeffPair> pl_pairs;
     MatchCorn(kdtree_corn, cloud_corn_map, pose_curr2map, &pl_pairs);
     std::vector<PointPlaneCoeffPair> pp_pairs;
     MatchSurf(kdtree_surf, cloud_surf_map, pose_curr2map, &pp_pairs);
@@ -83,7 +83,7 @@ void Mapper::Run(const TPointCloudConstPtr &cloud_corn,
     }
     PoseSolver solver(pose_curr2map);
     for (const auto &pair : pl_pairs) {
-      solver.AddPointLinePair(pair, 1.0);
+      solver.AddPointLineCoeffPair(pair, 1.0);
     }
     for (const auto &pair : pp_pairs) {
       solver.AddPointPlaneCoeffPair(pair, 1.0);
@@ -105,7 +105,7 @@ void Mapper::Run(const TPointCloudConstPtr &cloud_corn,
 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<PointLineCoeffPair> *const pairs) const {
   std::vector<int> indices;
   std::vector<float> dists;
   int nearest_neighbor_k = config_["nearest_neighbor_k"].as<int>();
@@ -123,10 +123,7 @@ void Mapper::MatchCorn(const pcl::KdTreeFLANN<TPoint> &kdtree,
     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);
+    pairs->emplace_back(pt, coeff);
-    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);
   }
 }
 

oh_my_loam/mapper/mapper.h

@@ -43,7 +43,7 @@ class Mapper {
   void MatchCorn(const pcl::KdTreeFLANN<TPoint> &kdtree,
                  const TPointCloudConstPtr &cloud_curr,
                  const common::Pose3d &pose_curr2map,
-                 std::vector<PointLinePair> *const pairs) const;
+                 std::vector<PointLineCoeffPair> *const pairs) const;
 
   void MatchSurf(const pcl::KdTreeFLANN<TPoint> &kdtree,
                  const TPointCloudConstPtr &cloud_curr,
@@ -60,13 +60,6 @@ class Mapper {
                  const TPointCloudConstPtr &cloud_corn = nullptr,
                  const TPointCloudConstPtr &cloud_surf = nullptr);
 
-  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;
   // map
   mutable std::mutex map_mutex_;
   std::unique_ptr<Map> corn_map_;

oh_my_loam/solver/cost_function.h

@@ -39,6 +39,14 @@ struct PointPlanePair {
       : pt(pt), plane(pt1, pt2, pt3) {}
 };
 
+struct PointLineCoeffPair {
+  TPoint pt;
+  Eigen::Matrix<double, 6, 1> line_coeff;
+  PointLineCoeffPair(const TPoint &pt,
+                     const Eigen::Matrix<double, 6, 1> &line_coeff)
+      : pt(pt), line_coeff(line_coeff) {}
+};
+
 struct PointPlaneCoeffPair {
   TPoint pt;
   Eigen::Vector4d plane_coeff;
@@ -87,6 +95,27 @@ class PointPlaneCostFunction {
   DISALLOW_COPY_AND_ASSIGN(PointPlaneCostFunction)
 };
 
+class PointLineCoeffCostFunction {
+ public:
+  PointLineCoeffCostFunction(const PointLineCoeffPair &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 PointLineCoeffPair &pair,
+                                     double time) {
+    return new ceres::AutoDiffCostFunction<PointLineCoeffCostFunction, 1, 4, 3>(
+        new PointLineCoeffCostFunction(pair, time));
+  }
+
+ private:
+  PointLineCoeffPair pair_;
+  double time_;
+  DISALLOW_COPY_AND_ASSIGN(PointLineCoeffCostFunction)
+};
+
 class PointPlaneCoeffCostFunction {
  public:
   PointPlaneCoeffCostFunction(const PointPlaneCoeffPair &pair, double time)
@@ -159,6 +188,30 @@ bool PointPlaneCostFunction::operator()(const T *const r_quat,
   return true;
 }
 
+template <typename T>
+bool PointLineCoeffCostFunction::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, 6, 1> coeff = pair_.line_coeff.template cast<T>();
+  Eigen::Matrix<T, 3, 1> p1 = coeff.topRows(3);
+  Eigen::Matrix<T, 3, 1> dir = coeff.bottomRows(3);
+
+  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;
+
+  Eigen::Matrix<T, 3, 1> cross = (p0 - p1).cross(dir);
+  residual[0] = cross[0];
+  residual[1] = cross[1];
+  residual[2] = cross[2];
+  return true;
+}
+
 template <typename T>
 bool PointPlaneCoeffCostFunction::operator()(const T *const r_quat,
                                              const T *const t_vec,

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::AddPointLineCoeffPair(const PointLineCoeffPair &pair,
+                                       double time) {
+  ceres::CostFunction *cost_function =
+      PointLineCoeffCostFunction::Create(pair, time);
+  problem_.AddResidualBlock(cost_function, loss_function_, r_quat_, t_vec_);
+}
+
 void PoseSolver::AddPointPlaneCoeffPair(const PointPlaneCoeffPair &pair,
                                         double time) {
   ceres::CostFunction *cost_function =

oh_my_loam/solver/solver.h

@@ -13,6 +13,8 @@ class PoseSolver {
 
   void AddPointPlanePair(const PointPlanePair &pair, double time);
 
+  void AddPointLineCoeffPair(const PointLineCoeffPair &pair, double time);
+
   void AddPointPlaneCoeffPair(const PointPlaneCoeffPair &pair, double time);
 
   bool Solve(int max_iter_num = 5, bool verbose = false,