odometry nearly ok, link error

CMakeLists.txt

@@ -3,7 +3,7 @@ project(oh_my_loam)
 
 set(CMAKE_BUILD_TYPE "Release")
 set(CMAKE_CXX_FLAGS "-std=c++17  -lpthread")
-set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g")
+set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall")
 
 find_package(Ceres REQUIRED)
 find_package(PCL QUIET)
@@ -30,6 +30,9 @@ include_directories(SYSTEM
   ${G3LOG_INCLUDE_DIRS}
 )
 
+# link_directories(${PCL_LIBRARY_DIRS})
+# add_definitions(${PCL_DEFINITIONS})
+
 include_directories(
   src
   common
@@ -53,10 +56,10 @@ target_link_libraries(main
   ${YAML_CPP_LIBRARIES}
   g3log
   common
-  oh_my_loam
-  # helper
-  # solver
   extractor
   visualizer
-  # odometry
+  helper
+  solver
+  odometry
+  oh_my_loam
 )

common/common.h

@@ -4,6 +4,7 @@
 #include "config.h"
 #include "log.h"
 #include "macros.h"
+#include "pose.h"
 #include "tic_toc.h"
 #include "types.h"
 #include "utils.h"

common/pose.cc

@@ -6,4 +6,15 @@ Pose3D Interpolate(const Pose3D& pose_from, const Pose3D& pose_to, double t) {
   return pose_from.InterPolate(pose_to, t);
 }
 
+Pose3D operator*(const Pose3D& lhs, const Pose3D& rhs) {
+  return Pose3D(lhs.q() * rhs.q(), lhs.q() * rhs.p() + lhs.p());
+}
+
+std::string Pose3D::ToString() const {
+  std::ostringstream oss;
+  oss << "[Pose3D] q = (" << q_.coeffs().transpose() << "), p = ("
+      << p_.transpose() << ")";
+  return oss.str();
+}
+
 }  // namespace oh_my_loam

common/pose.h

@@ -14,10 +14,10 @@ class Pose3D {
   Pose3D(const Eigen::Quaterniond& q, const Eigen::Vector3d& p)
       : q_(q), p_(p) {}
 
-  Pose3D(const Eigen::Matrix3d& r_mat, const Eigen::Vector3d& p) {
+  Pose3D(const Eigen::Matrix3d& r_mat, const Eigen::Vector3d& p)
-    q_ = Eigen::Quaterniond(r_mat);
+      : q_(r_mat), p_(p) {}
-    p_ = p;
+
-  }
+  Pose3D(const double* const q, const double* const p) : q_(q), p_(p) {}
 
   Pose3D Inv() const {
     auto q_inv = q_.inverse();
@@ -29,6 +29,10 @@ class Pose3D {
     return q_ * vec + p_;
   }
 
+  Eigen::Vector3d operator*(const Eigen::Vector3d& vec) const {
+    return Transform(vec);
+  }
+
   template <typename PointT>
   PointT Transform(const PointT& pt) const {
     PointT pt_n = pt;
@@ -39,6 +43,11 @@ class Pose3D {
     return pt_n;
   }
 
+  template <typename PointT>
+  PointT operator*(const PointT& vec) const {
+    return Transform<PointT>(vec);
+  }
+
   Eigen::Vector3d Translate(const Eigen::Vector3d& vec) const {
     return vec + p_;
   }
@@ -52,6 +61,11 @@ class Pose3D {
     return {q_interp, p_interp};
   }
 
+  std::string ToString() const;
+
+  Eigen::Quaterniond q() const { return q_; }
+  Eigen::Vector3d p() const { return p_; }
+
  protected:
   Eigen::Quaterniond q_;  // orientation
   Eigen::Vector3d p_;     // position
@@ -59,4 +73,8 @@ class Pose3D {
 
 Pose3D Interpolate(const Pose3D& pose_from, const Pose3D& pose_to, double t);
 
+Pose3D operator*(const Pose3D& lhs, const Pose3D& rhs);
+
+using Trans3D = Pose3D;
+
 }  // namespace oh_my_loam

common/types.h

@@ -8,9 +8,13 @@
 #include <pcl/visualization/point_cloud_handlers.h>
 
 #include <cmath>
-// This hpp file should be included if user-defined point type is added, see
+// Thses hpp file should be included if user-defined point type is added, see
 // "How are the point types exposed?" section in
 // https://pointclouds.org/documentation/tutorials/adding_custom_ptype.html
+#include <pcl/filters/impl/voxel_grid.hpp>
+#include <pcl/impl/pcl_base.hpp>
+#include <pcl/kdtree/impl/kdtree_flann.hpp>
+#include <pcl/visualization/impl/pcl_visualizer.hpp>
 #include <pcl/visualization/impl/point_cloud_geometry_handlers.hpp>
 
 namespace oh_my_loam {

configs/config.yaml

@@ -11,8 +11,11 @@ extractor_config:
   sharp_corner_point_num: 2
   corner_point_num: 20
   flat_surf_point_num: 4
-  surf_point_num: 1000
+  surf_point_num: 20 ## useless currently
   corner_point_curvature_thres: 0.5
   surf_point_curvature_thres: 0.5
   neighbor_point_dist_thres: 0.05
   downsample_voxel_size: 0.2
+
+odometry_config: 
+  icp_iter_num : 2

main.cc

@@ -4,7 +4,7 @@
 
 #include <functional>
 
-#include "common/common.h"
+#include "common.h"
 #include "src/oh_my_loam.h"
 
 using namespace oh_my_loam;

src/CMakeLists.txt

@@ -1,6 +1,6 @@
 add_subdirectory(extractor)
 add_subdirectory(visualizer)
-# add_subdirectory(odometry)
+add_subdirectory(odometry)
 add_subdirectory(helper)
 add_subdirectory(solver)
 

src/extractor/base_extractor.cc

@@ -44,7 +44,7 @@ void Extractor::Process(const PointCloud& cloud, FeaturePoints* const feature) {
   oss << "Feature point num: ";
   for (const auto& scan : scans) {
     FeaturePoints scan_feature;
-    StoreToFeaturePoints(scan, &scan_feature);
+    GenerateFeaturePoints(scan, &scan_feature);
     feature->Add(scan_feature);
     oss << scan.size() << ":" << scan_feature.sharp_corner_pts->size() << ":"
         << scan_feature.less_sharp_corner_pts->size() << ":"
@@ -187,29 +187,33 @@ void Extractor::SetNeighborsPicked(const TCTPointCloud& scan, size_t ix,
   }
 }
 
-void Extractor::StoreToFeaturePoints(const TCTPointCloud& scan,
+void Extractor::GenerateFeaturePoints(const TCTPointCloud& scan,
                                       FeaturePoints* const feature) const {
   for (const auto& pt : scan.points) {
     switch (pt.type) {
       case PointType::FLAT:
-        feature->flat_surf_pts->points.emplace_back(pt.x, pt.y, pt.z);
+        feature->flat_surf_pts->points.emplace_back(pt.x, pt.y, pt.z, pt.time);
       // no break: FLAT points are also LESS_FLAT
       case PointType::LESS_FLAT:
-        feature->less_flat_surf_pts->points.emplace_back(pt.x, pt.y, pt.z);
+        feature->less_flat_surf_pts->points.emplace_back(pt.x, pt.y, pt.z,
+                                                         pt.time);
         break;
       case PointType::SHARP:
-        feature->sharp_corner_pts->points.emplace_back(pt.x, pt.y, pt.z);
+        feature->sharp_corner_pts->points.emplace_back(pt.x, pt.y, pt.z,
+                                                       pt.time);
       // no break: SHARP points are also LESS_SHARP
       case PointType::LESS_SHARP:
-        feature->less_sharp_corner_pts->points.emplace_back(pt.x, pt.y, pt.z);
+        feature->less_sharp_corner_pts->points.emplace_back(pt.x, pt.y, pt.z,
+                                                            pt.time);
         break;
       default:
         // all the rest are also LESS_FLAT
-        feature->less_flat_surf_pts->points.emplace_back(pt.x, pt.y, pt.z);
+        feature->less_flat_surf_pts->points.emplace_back(pt.x, pt.y, pt.z,
+                                                         pt.time);
         break;
     }
   }
-  PointCloudPtr filtered_less_flat_surf_pts(new PointCloud);
+  TPointCloudPtr filtered_less_flat_surf_pts(new TPointCloud);
   VoxelDownSample(*feature->less_flat_surf_pts,
                   filtered_less_flat_surf_pts.get(),
                   config_["downsample_voxel_size"].as<double>());

src/extractor/base_extractor.h

@@ -37,7 +37,7 @@ class Extractor {
   void SetNeighborsPicked(const TCTPointCloud& scan, size_t ix,
                           std::vector<bool>* const picked) const;
 
-  void StoreToFeaturePoints(const TCTPointCloud& scan,
+  void GenerateFeaturePoints(const TCTPointCloud& scan,
                              FeaturePoints* const feature) const;
 
   bool is_vis_ = false;

src/extractor/feature_points.h

@@ -1,20 +1,20 @@
 #pragma once
 
-#include "common.h"
+#include "types.h"
 
 namespace oh_my_loam {
 
 struct FeaturePoints {
-  PointCloudPtr sharp_corner_pts;
+  TPointCloudPtr sharp_corner_pts;
-  PointCloudPtr less_sharp_corner_pts;
+  TPointCloudPtr less_sharp_corner_pts;
-  PointCloudPtr flat_surf_pts;
+  TPointCloudPtr flat_surf_pts;
-  PointCloudPtr less_flat_surf_pts;
+  TPointCloudPtr less_flat_surf_pts;
 
   FeaturePoints() {
-    sharp_corner_pts.reset(new PointCloud);
+    sharp_corner_pts.reset(new TPointCloud);
-    less_sharp_corner_pts.reset(new PointCloud);
+    less_sharp_corner_pts.reset(new TPointCloud);
-    flat_surf_pts.reset(new PointCloud);
+    flat_surf_pts.reset(new TPointCloud);
-    less_flat_surf_pts.reset(new PointCloud);
+    less_flat_surf_pts.reset(new TPointCloud);
   }
 
   void Add(const FeaturePoints& rhs) {

src/helper/helper.cc

@@ -6,8 +6,4 @@ float GetTime(const TPoint& pt) { return pt.time - GetScanId(pt); }
 
 int GetScanId(const TPoint& pt) { return static_cast<int>(pt.time); }
 
-double PointLinePair::DistPointToLine() const { return 0.0; }
-
-double PointPlanePair::DistPointToPlane() const { return 0.0; }
-
 }  // oh_my_loam

src/helper/helper.h

@@ -17,7 +17,8 @@ struct PointLinePair {
   };
   Line line;
   PointLinePair(const TPoint& pt, const Line& line) : pt(pt), line(line) {}
-  double DistPointToLine() const;
+  PointLinePair(const TPoint& pt, const TPoint& pt1, const TPoint& pt2)
+      : pt(pt), line(pt1, pt2) {}
 };
 
 struct PointPlanePair {
@@ -30,7 +31,9 @@ struct PointPlanePair {
   };
   Plane plane;
   PointPlanePair(const TPoint& pt, const Plane& plane) : pt(pt), plane(plane) {}
-  double DistPointToPlane() const;
+  PointPlanePair(const TPoint& pt, const TPoint& pt1, const TPoint& pt2,
+                 const TPoint& pt3)
+      : pt(pt), plane(pt1, pt2, pt3) {}
 };
 
 }  // oh_my_loam

src/odometry/odometry.cc

@@ -1,32 +1,52 @@
 #include "odometry.h"
+#include "solver/solver.h"
 
 namespace oh_my_loam {
 
 namespace {
 int kNearbyScanNum = 2;
 double kDistSquareThresh = 25;
-}
+size_t kMinMatchNum = 10;
+}  // namespace
 
 bool Odometry::Init(const YAML::Node& config) {
   config_ = config;
-  kdtree_surf_pts.reset(new pcl::KdTreeFLANN<TPoint>);
+  kdtree_surf_pts_.reset(new pcl::KdTreeFLANN<TPoint>);
-  kdtree_corn_pts.reset(new pcl::KdTreeFLANN<TPoint>);
+  kdtree_corn_pts_.reset(new pcl::KdTreeFLANN<TPoint>);
   return true;
 }
 
-void Odometry::Process(const FeaturePoints& feature) {
+void Odometry::Process(const FeaturePoints& feature, Pose3D* const pose) {
   if (!is_initialized) {
     is_initialized = true;
     UpdatePre(feature);
+    *pose = pose_curr2world_;
     return;
   }
+  for (int i = 0; i < config_["icp_iter_num"].as<int>(); ++i) {
     std::vector<PointLinePair> pl_pairs;
     std::vector<PointPlanePair> pp_pairs;
-  AssociateCornPoints(*feature.sharp_corner_pts, corn_pts_pre_, pl_pairs,
+    AssociateCornPoints(*feature.sharp_corner_pts, *corn_pts_pre_, &pl_pairs,
                         kDistSquareThresh);
-  AssociateSurfPoints(*feature.flat_surf_pts, surf_pts_pre_, pp_pairs,
+    AssociateSurfPoints(*feature.flat_surf_pts, *surf_pts_pre_, &pp_pairs,
                         kDistSquareThresh);
-
+    if (pl_pairs.size() + pp_pairs.size() < kMinMatchNum) {
+      AWARN << "Too less correspondence";
+    }
+    double* q = pose_curr2last_.q().coeffs().data();
+    double* p = pose_curr2last_.p().data();
+    PoseSolver solver(q, p);
+    for (const auto& pair : pl_pairs) {
+      solver.AddPointLinePair(pair, GetTime(pair.pt));
+    }
+    for (const auto& pair : pp_pairs) {
+      solver.AddPointPlanePair(pair, GetTime(pair.pt));
+    }
+    solver.Solve();
+    pose_curr2last_ = Pose3D(q, p);
+  }
+  pose_curr2world_ = pose_curr2world_ * pose_curr2last_;
+  *pose = pose_curr2world_;
   UpdatePre(feature);
 }
 
@@ -34,17 +54,17 @@ void Odometry::AssociateCornPoints(const TPointCloud& src,
                                    const TPointCloud& tgt,
                                    std::vector<PointLinePair>* const pairs,
                                    double dist_thresh) const {
-  kdtree_corn_pts_->setInputCloud(tgt);
+  kdtree_corn_pts_->setInputCloud(tgt.makeShared());
-  for (const query_pt : src) {
+  for (const auto& query_pt : src) {
     std::vector<int> indices;
     std::vector<float> dists;
-    kdtree_corn_pts->nearestKSearch(query_pt, 1, indices, dists);
+    kdtree_corn_pts_->nearestKSearch(query_pt, 1, indices, dists);
     if (dists[0] >= dist_thresh) continue;
-    Point pt1 = tgt.points[indices[0]];
+    TPoint pt1 = tgt.points[indices[0]];
     int pt2_idx = -1;
     double min_dist_pt2_squre = dist_thresh * dist_thresh;
     int query_pt_scan_id = GetScanId(query_pt);
-    for (int i = indices[0] + 1; i < tgt.size(); ++i) {
+    for (int i = indices[0] + 1; i < static_cast<int>(tgt.size()); ++i) {
       const auto pt = tgt.points[i];
       int scan_id = GetScanId(pt);
       if (scan_id <= query_pt_scan_id) continue;
@@ -68,7 +88,7 @@ void Odometry::AssociateCornPoints(const TPointCloud& src,
     }
     if (pt2_idx >= 0) {
       TPoint pt2 = tgt.points[pt2_idx];
-      pairs->emplace_back(query_pt, {pt1, pt2});
+      pairs->emplace_back(query_pt, pt1, pt2);
     }
   }
 }
@@ -77,18 +97,18 @@ void Odometry::AssociateSurfPoints(const TPointCloud& src,
                                    const TPointCloud& tgt,
                                    std::vector<PointPlanePair>* const pairs,
                                    double dist_thresh) const {
-  kdtree_surf_pts_->setInputCloud(tgt);
+  kdtree_surf_pts_->setInputCloud(tgt.makeShared());
-  for (const query_pt : src) {
+  for (const auto& query_pt : src) {
     std::vector<int> indices;
     std::vector<float> dists;
-    kdtree_corn_pts->nearestKSearch(query_pt, 1, indices, dists);
+    kdtree_surf_pts_->nearestKSearch(query_pt, 1, indices, dists);
     if (dists[0] >= dist_thresh) continue;
-    Point pt1 = tgt.points[indices[0]];
+    TPoint pt1 = tgt.points[indices[0]];
     int pt2_idx = -1, pt3_idx = -1;
     double min_dist_pt2_squre = dist_thresh * dist_thresh;
     double min_dist_pt3_squre = dist_thresh * dist_thresh;
     int query_pt_scan_id = GetScanId(query_pt);
-    for (int i = indices[0] + 1; i < tgt.size(); ++i) {
+    for (int i = indices[0] + 1; i < static_cast<int>(tgt.size()); ++i) {
       const auto pt = tgt.points[i];
       int scan_id = GetScanId(pt);
       if (scan_id > query_pt_scan_id + kNearbyScanNum) break;
@@ -97,7 +117,7 @@ void Odometry::AssociateSurfPoints(const TPointCloud& src,
         pt2_idx = i;
         min_dist_pt2_squre = dist_squre;
       }
-      if (scan_id > query_pt_scan_id && dist_squre < min_dist_pt2_squre) {
+      if (scan_id > query_pt_scan_id && dist_squre < min_dist_pt3_squre) {
         pt3_idx = i;
         min_dist_pt3_squre = dist_squre;
       }
@@ -113,7 +133,7 @@ void Odometry::AssociateSurfPoints(const TPointCloud& src,
         pt2_idx = i;
         min_dist_pt2_squre = dist_squre;
       }
-      if (scan_id < query_pt_scan_id && dist_squre < min_dist_pt2_squre) {
+      if (scan_id < query_pt_scan_id && dist_squre < min_dist_pt3_squre) {
         pt3_idx = i;
         min_dist_pt3_squre = dist_squre;
       }
@@ -121,7 +141,7 @@ void Odometry::AssociateSurfPoints(const TPointCloud& src,
     }
     if (pt2_idx >= 0 && pt3_idx >= 0) {
       TPoint pt2 = tgt.points[pt2_idx], pt3 = tgt.points[pt3_idx];
-      pairs->emplace_back(query_pt, {pt1, pt2, pt3});
+      pairs->emplace_back(query_pt, pt1, pt2, pt3);
     }
   }
 }

src/odometry/odometry.h

@@ -1,8 +1,6 @@
 #pragma once
 
-#include <pcl/kdtree/kdtree_flann.h>
+#include <extractor/feature_points.h>
-
-#include <eigen3/Eigen/Dense>
 
 #include "common.h"
 #include "helper/helper.h"
@@ -16,7 +14,7 @@ class Odometry {
 
   bool Init(const YAML::Node& config);
 
-  void Process(const FeaturePoints& feature);
+  void Process(const FeaturePoints& feature, Pose3D* const pose);
 
  protected:
   void UpdatePre(const FeaturePoints& feature);
@@ -28,15 +26,19 @@ class Odometry {
                            std::vector<PointPlanePair>* const pairs,
                            double dist_thresh) const;
 
+  Pose3D pose_curr2world_;
+  Pose3D pose_curr2last_;
+
   TPointCloudPtr surf_pts_pre_;
   TPointCloudPtr corn_pts_pre_;
 
-  pcl::KdTreeFLANN<TPoint>::Ptr kdtree_surf_pts;
+  pcl::KdTreeFLANN<TPoint>::Ptr kdtree_surf_pts_;
-  pcl::KdTreeFLANN<TPoint>::Ptr kdtree_corn_pts;
+  pcl::KdTreeFLANN<TPoint>::Ptr kdtree_corn_pts_;
 
   bool is_initialized = false;
-
   YAML::Node config_;
+
+  DISALLOW_COPY_AND_ASSIGN(Odometry)
 };
 
 }  // namespace oh_my_loam

src/oh_my_loam.cc

@@ -15,6 +15,11 @@ bool OhMyLoam::Init() {
     AERROR << "Failed to initialize extractor";
     return false;
   }
+  odometry_.reset(new Odometry);
+  if (!odometry_->Init(config["odometry_config"])) {
+    AERROR << "Failed to initialize odometry";
+    return false;
+  }
   return true;
 }
 
@@ -25,6 +30,10 @@ void OhMyLoam::Run(const PointCloud& cloud_in, double timestamp) {
          << cloud->size();
   FeaturePoints feature_points;
   extractor_->Process(*cloud, &feature_points);
+  Pose3D pose;
+  odometry_->Process(feature_points, &pose);
+  poses_.emplace_back(pose);
+  AINFO << pose.ToString();
 }
 
 void OhMyLoam::RemoveOutliers(const PointCloud& cloud_in,

src/oh_my_loam.h

@@ -1,9 +1,8 @@
 #pragma once
 
-#include <yaml-cpp/yaml.h>
-
 #include "common.h"
 #include "extractor/base_extractor.h"
+#include "odometry/odometry.h"
 
 namespace oh_my_loam {
 
@@ -17,10 +16,12 @@ class OhMyLoam {
 
  private:
   std::unique_ptr<Extractor> extractor_{nullptr};
+  std::unique_ptr<Odometry> odometry_{nullptr};
 
   // remove outliers: nan and very close points
   void RemoveOutliers(const PointCloud& cloud_in,
                       PointCloud* const cloud_out) const;
+  std::vector<Pose3D> poses_;
 
   DISALLOW_COPY_AND_ASSIGN(OhMyLoam)
 };

src/solver/cost_function.h

@@ -0,0 +1,90 @@
+#pragma once
+
+#include "ceres/ceres.h"
+#include "common.h"
+#include "helper/helper.h"
+
+namespace oh_my_loam {
+
+class PointLineCostFunction {
+ public:
+  PointLineCostFunction(const PointLinePair& pair, double time)
+      : pair_(pair), time_(time){};
+
+  template <typename T>
+  bool operator()(const T* const q, const T* const p, T* residual) const;
+
+  static ceres::CostFunction* Create(const PointLinePair& pair, double time) {
+    return new ceres::AutoDiffCostFunction<PointLineCostFunction, 1, 4, 3>(
+        new PointLineCostFunction(pair, time));
+  }
+
+ private:
+  PointLinePair pair_;
+  double time_;
+
+  DISALLOW_COPY_AND_ASSIGN(PointLineCostFunction)
+};
+
+class PointPlaneCostFunction {
+ public:
+  PointPlaneCostFunction(const PointPlanePair& pair, double time)
+      : pair_(pair), time_(time){};
+
+  template <typename T>
+  bool operator()(const T* const q, const T* const p, T* residual) const;
+
+  static ceres::CostFunction* Create(const PointPlanePair& pair, double time) {
+    return new ceres::AutoDiffCostFunction<PointPlaneCostFunction, 1, 4, 3>(
+        new PointPlaneCostFunction(pair, time));
+  }
+
+ private:
+  PointPlanePair pair_;
+  double time_;
+  DISALLOW_COPY_AND_ASSIGN(PointPlaneCostFunction)
+};
+
+template <typename T>
+bool PointLineCostFunction::operator()(const T* const q, const T* const p,
+                                       T* residual) const {
+  const auto pt = pair_.pt, pt1 = pair_.line.pt1, pt2 = pair_.line.pt2;
+  Eigen::Matrix<T, 3, 1> pnt(T(pt.x), T(pt.y), T(pt.z));
+  Eigen::Matrix<T, 3, 1> pnt1(T(pt1.x), T(pt1.y), T(pt1.z));
+  Eigen::Matrix<T, 3, 1> pnt2(T(pt2.x), T(pt2.y), T(pt2.z));
+
+  Eigen::Quaternion<T> r(q[3], q[0], q[1], q[2]);
+  Eigen::Quaternion<T> q_identity{T(1), T(0), T(0), T(0)};
+  r = q_identity.slerp(T(time_), r);
+  Eigen::Matrix<T, 3, 1> t(T(time_) * p[0], T(time_) * p[1], T(time_) * p[2]);
+  Eigen::Matrix<T, 3, 1> pnt_n = r * pnt + t;
+
+  // norm of cross product: triangle area x 2
+  T area = (pnt_n - pnt1).cross(pnt2 - pnt1).norm() * 0.5;
+  T base_length = (pnt2 - pnt1).norm();
+  residual[0] = area / base_length;
+  return true;
+}
+
+template <typename T>
+bool PointPlaneCostFunction::operator()(const T* const q, const T* const p,
+                                        T* residual) const {
+  const auto &pt = pair_.pt, &pt1 = pair_.plane.pt1, &pt2 = pair_.plane.pt2,
+             &pt3 = pair_.plane.pt3;
+  Eigen::Matrix<T, 3, 1> pnt(T(pt.x), T(pt.y), T(pt.z));
+  Eigen::Matrix<T, 3, 1> pnt1(T(pt1.x), T(pt1.y), T(pt1.z));
+  Eigen::Matrix<T, 3, 1> pnt2(T(pt2.x), T(pt2.y), T(pt2.z));
+  Eigen::Matrix<T, 3, 1> pnt3(T(pt3.x), T(pt3.y), T(pt3.z));
+
+  Eigen::Quaternion<T> r(q[3], q[0], q[1], q[2]);
+  Eigen::Quaternion<T> q_identity{T(1), T(0), T(0), T(0)};
+  r = q_identity.slerp(T(time_), r);
+  Eigen::Matrix<T, 3, 1> t(T(time_) * p[0], T(time_) * p[1], T(time_) * p[2]);
+  Eigen::Matrix<T, 3, 1> pnt_n = r * pnt + t;
+
+  Eigen::Matrix<T, 3, 1> normal = (pnt2 - pnt1).cross(pnt3 - pnt1).normalized();
+  residual[0] = (pnt_n - pnt1).dot(normal);
+  return true;
+}
+
+}  // oh_my_loam

src/solver/solver.cc

@@ -2,6 +2,37 @@
 
 namespace oh_my_loam {
 
-//
+namespace {
+double kHuberLossScale = 0.1;
+}
+
+PoseSolver::PoseSolver(double *q, double *p) : q_(q), p_(p) {
+  loss_function_ = new ceres::HuberLoss(kHuberLossScale);
+  problem_.AddParameterBlock(q_, 4,
+                             new ceres::EigenQuaternionParameterization());
+  problem_.AddParameterBlock(p_, 3);
+}
+
+void PoseSolver::AddPointLinePair(const PointLinePair &pair, double time) {
+  ceres::CostFunction *cost_function =
+      PointLineCostFunction::Create(pair, time);
+  problem_.AddResidualBlock(cost_function, loss_function_, q_, p_);
+}
+
+void PoseSolver::AddPointPlanePair(const PointPlanePair &pair, double time) {
+  ceres::CostFunction *cost_function =
+      PointPlaneCostFunction::Create(pair, time);
+  problem_.AddResidualBlock(cost_function, loss_function_, q_, p_);
+}
+
+void PoseSolver::Solve(int max_iter_num, bool verbose) {
+  ceres::Solver::Options options;
+  options.linear_solver_type = ceres::DENSE_QR;
+  options.max_num_iterations = max_iter_num;
+  options.minimizer_progress_to_stdout = verbose;
+  ceres::Solver::Summary summary;
+  ceres::Solve(options, &problem_, &summary);
+  AINFO_IF(verbose) << summary.BriefReport();
+}
 
 }  // oh_my_loam

src/solver/solver.h

@@ -1,19 +1,27 @@
 #pragma once
 
-#include "ceres/ceres.h"
+#include "cost_function.h"
 
 namespace oh_my_loam {
 
-class Solver {
+class PoseSolver {
  public:
-  Solver() = default;
+  PoseSolver(double* q, double* p);
 
-  void SetInitialGuess(double* param_q, double* param_p);
+  void AddPointLinePair(const PointLinePair& pair, double time);
+
+  void AddPointPlanePair(const PointPlanePair& pair, double time);
+
+  void Solve(int max_iter_num = 5, bool verbose = false);
+
+ private:
+  ceres::Problem problem_;
 
- protected:
-  std::unique_ptr<ceres::Problem> problem_;
-  ceres::LocalParameterization* parameterization_;
   ceres::LossFunction* loss_function_;
+
+  double *q_, *p_;
+
+  DISALLOW_COPY_AND_ASSIGN(PoseSolver)
 };
 
 }  // oh_my_loam