odometer almost done

CMakeLists.txt

@@ -54,9 +54,9 @@ target_link_libraries(main
   common
   oh_my_loam
   extractor
-  # odometer
+  odometer
   # mapper
-  # solver
+  solver
   ${CERES_LIBRARIES}
   visualizer
   base

common/geometry/pose.h

@@ -1,85 +0,0 @@
-#pragma once
-
-#include <eigen3/Eigen/Dense>
-
-namespace common {
-
-template <typename T>
-class Pose3 {
- public:
-  Pose3() {
-    r_quat_.setIdentity();
-    t_vec_.setZero();
-  };
-
-  Pose3(const Eigen::Quaterniond& r_quat, const Eigen::Matrix<T, 3, 1>& t_vec)
-      : r_quat_(q), t_vec_(p) {}
-
-  Pose3(const Eigen::Matrix3d& r_mat, const Eigen::Matrix<T, 3, 1>& t_vec)
-      : r_quat_(r_mat), t_vec_(t_vec) {}
-
-  Pose3(const double* const r_quat, const double* const t_vec)
-      : r_quat_(r_quat), t_vec_(t_vec) {}
-
-  Pose3 Inv() const {
-    Eigen::Quaternion<T> r_inv = r_quat_.inverse();
-    Eigen::Matrix<T, 3, 1> t_inv = -r_inv * t_vec_;
-    return {r_inv, t_inv};
-  }
-
-  Eigen::Matrix<T, 3, 1> Transform(const Eigen::Matrix<T, 3, 1>& point) const {
-    return r_quat_ * point + t_vec_;
-  }
-
-  Eigen::Matrix<T, 3, 1> operator*(const Eigen::Matrix<T, 3, 1>& point) const {
-    return Transform(point);
-  }
-
-  Eigen::Matrix<T, 3, 1> Translate(const Eigen::Matrix<T, 3, 1>& vec) const {
-    return vec + t_vec_;
-  }
-
-  Eigen::Matrix<T, 3, 1> Rotate(const Eigen::Matrix<T, 3, 1>& vec) const {
-    return r_quat_ * vec;
-  }
-
-  // Spherical linear interpolation to `pose_to`
-  Pose3 Interpolate(const Pose3& pose_to, double t) const {
-    Pose3 pose_dst;
-    Eigen::Quaternion<T> q_interp = r_quat_.slerp(t, pose_to.r_quat_);
-    Eigen::Matrix<T, 3, 1> p_interp = (pose_to.t_vec_ - t_vec_) * t + t_vec_;
-    return {q_interp, p_interp};
-  }
-
-  std::string ToString() const {
-    std::ostringstream oss;
-    oss << "[Pose3D] q = (" << q_.coeffs().transpose() << "), p = ("
-        << p_.transpose() << ")";
-    return oss.str();
-  }
-
-  Eigen::Quaternion<T> r_quat() const { return r_quat_; }
-
-  Eigen::Matrix<T, 3, 1> t_vec() const { return t_vec_; }
-
- protected:
-  Eigen::Quaternion<T> r_quat_;   // orientation/rotation
-  Eigen::Matrix<T, 3, 1> t_vec_;  // positon/translation
-};
-
-template <typename T>
-Pose3<T> Interpolate(const Pose3<T>& pose_from, const Pose3<T>& pose_to,
-                     double t) {
-  return pose_from.Interpolate(pose_to, t);
-}
-
-template <typename T>
-Pose3<T> operator*(const Pose3<T>& lhs, const Pose3<T>& rhs) {
-  return Pose3<T>(lhs.q() * rhs.q(), lhs.q() * rhs.p() + lhs.p());
-}
-
-using Pose3d = Pose3<double>;
-
-using Pose3f = Pose3<float>;
-
-}  // namespace common

common/geometry/pose3d.h

@@ -0,0 +1,98 @@
+#pragma once
+
+#include <eigen3/Eigen/Dense>
+#include <iomanip>
+
+namespace common {
+
+class Pose3d {
+ public:
+  Pose3d() {
+    r_quat_.setIdentity();
+    t_vec_.setZero();
+  };
+
+  Pose3d(const Eigen::Quaterniond& r_quat, const Eigen::Vector3d& t_vec)
+      : r_quat_(r_quat), t_vec_(t_vec) {}
+
+  Pose3d(const Eigen::Matrix3d& r_mat, const Eigen::Vector3d& t_vec)
+      : r_quat_(r_mat), t_vec_(t_vec) {}
+
+  Pose3d(const double* const r_quat, const double* const t_vec)
+      : r_quat_(r_quat), t_vec_(t_vec) {}
+
+  static Pose3d Identity() { return {}; }
+
+  void setIdentity() {
+    r_quat_.setIdentity();
+    t_vec_.setZero();
+  }
+
+  Pose3d Inv() const {
+    Eigen::Quaterniond r_inv = r_quat_.inverse();
+    Eigen::Vector3d t_inv = r_inv * t_vec_;
+    return {r_inv, -t_inv};
+  }
+
+  Pose3d operator*(const Pose3d& rhs) const {
+    return Pose3d(r_quat_ * rhs.r_quat_, r_quat_ * rhs.t_vec_ + t_vec_);
+  }
+
+  Pose3d& operator*=(const Pose3d& rhs) {
+    t_vec_ += r_quat_ * rhs.t_vec_;
+    r_quat_ *= rhs.r_quat_;
+    return *this;
+  }
+
+  Eigen::Vector3d Transform(const Eigen::Vector3d& point) const {
+    return r_quat_ * point + t_vec_;
+  }
+
+  Eigen::Vector3d operator*(const Eigen::Vector3d& point) const {
+    return Transform(point);
+  }
+
+  Eigen::Vector3d Rotate(const Eigen::Vector3d& vec) const {
+    return r_quat_ * vec;
+  }
+
+  // Spherical linear interpolation to `pose_to`
+  Pose3d Interpolate(const Pose3d& pose_to, double t) const {
+    Pose3d pose_dst = t > 0 ? pose_to : (*this) * pose_to.Inv() * (*this);
+    t = t > 0 ? t : -t;
+    int whole = std::floor(t);
+    double frac = t - whole;
+    Eigen::Quaterniond r_interp = r_quat_.slerp(frac, pose_dst.r_quat_);
+    while (whole--) r_interp *= pose_dst.r_quat_;
+    Eigen::Vector3d t_interp = (pose_dst.t_vec_ - t_vec_) * t + t_vec_;
+    return {r_interp, t_interp};
+  }
+
+  std::string ToString() const {
+    std::ostringstream oss;
+    double w = r_quat_.w(), a = r_quat_.x(), b = r_quat_.y(), c = r_quat_.z();
+    double x = t_vec_.x(), y = t_vec_.y(), z = t_vec_.z();
+    oss << std::setprecision(3) << "[Pose3d] r_quat = (" << w << " " << a << " "
+        << b << " " << c << "), p = (" << x << " " << y << " " << z << ")";
+    return oss.str();
+  }
+
+  // const Eigen::Quaterniond& r_quat() const { return r_quat_; }
+
+  Eigen::Quaterniond r_quat() const { return r_quat_; }
+
+  // const Eigen::Vector3d& t_vec() const { return t_vec_; }
+
+  Eigen::Vector3d t_vec() const { return t_vec_; }
+
+ protected:
+  Eigen::Quaterniond r_quat_;  // orientation/rotation
+  Eigen::Vector3d t_vec_;      // positon/translation
+};
+
+inline Pose3d Interpolate(const Pose3d& pose_from, const Pose3d& pose_to,
+                          double t) {
+  return pose_from.Interpolate(pose_to, t);
+}
+
+}  // namespace common

oh_my_loam/CMakeLists.txt

@@ -1,9 +1,9 @@
 add_subdirectory(base)
 add_subdirectory(extractor)
 add_subdirectory(visualizer)
-# add_subdirectory(odometer)
+add_subdirectory(odometer)
 # add_subdirectory(mapper)
-# add_subdirectory(solver)
+add_subdirectory(solver)
 
 aux_source_directory(. SRC_FILES)
 

oh_my_loam/base/helper.cc

@@ -2,16 +2,28 @@
 
 namespace oh_my_loam {
 
-void TransformToStart(const Pose3D& pose, const TPoint& pt_in,
+void TransformToStart(const Pose3d& pose, const TPoint& pt_in,
                       TPoint* const pt_out) {
-  Pose3D pose_interp = Pose3D().Interpolate(pose, GetTime(pt_in));
+  Pose3d pose_interp = Pose3d().Interpolate(pose, GetTime(pt_in));
   TransformPoint<TPoint>(pose_interp, pt_in, pt_out);
 }
 
-void TransformToEnd(const Pose3D& pose, const TPoint& pt_in,
+TPoint TransformToStart(const Pose3d& pose, const TPoint& pt_in) {
+  TPoint pt_out;
+  TransformToStart(pose, pt_in, &pt_out);
+  return pt_out;
+}
+
+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);
 }
 
+TPoint TransformToEnd(const Pose3d& pose, const TPoint& pt_in) {
+  TPoint pt_out;
+  TransformToEnd(pose, pt_in, &pt_out);
+  return pt_out;
+}
+
 }  // namespace oh_my_loam

oh_my_loam/base/helper.h

@@ -1,20 +1,19 @@
 #pragma once
 
-#include "common/geometry/pose.h"
+#include "common/geometry/pose3d.h"
 #include "common/pcl/pcl_types.h"
 
 namespace oh_my_loam {
 
 using common::TPoint;
-using common::Pose3D;
+using common::Pose3d;
-using common::Trans3d;
 
 inline float GetTime(const TPoint& pt) { return pt.time - std::floor(pt.time); }
 
 inline int GetScanId(const TPoint& pt) { return static_cast<int>(pt.time); }
 
 template <typename PointType>
-void TransformPoint(const Pose3D& pose, const PointType& pt_in,
+void TransformPoint(const Pose3d& pose, const PointType& pt_in,
                     PointType* const pt_out) {
   *pt_out = pt_in;
   Eigen::Vector3d pnt = pose * Eigen::Vector3d(pt_in.x, pt_in.y, pt_in.z);
@@ -23,21 +22,33 @@ void TransformPoint(const Pose3D& pose, const PointType& pt_in,
   pt_out->z = pnt.z();
 }
 
+template <typename PointType>
+PointType TransformPoint(const Pose3d& pose, const PointType& pt_in) {
+  PointType pt_out;
+  TransformPoint<PointType>(pose, pt_in, &pt_out);
+  return pt_out;
+}
+
 /**
  * @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,
+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,
+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 {

oh_my_loam/configs/config.yaml

@@ -6,25 +6,25 @@ vis: true
 
 # configs for extractor
 extractor_config:
-  vis: true
+  vis: false
   verbose: true
   min_point_num: 66
   sharp_corner_point_num: 2
   corner_point_num: 20
   flat_surf_point_num: 4
-  surf_point_num: 20 
+  surf_point_num: 20
   corner_point_curvature_th: 0.5
   surf_point_curvature_th: 0.5
   neighbor_point_dist_th: 0.1
   downsample_voxel_size: 0.3
 
 # configs for odometer
-odometer_config: 
+odometer_config:
-  vis: false
+  vis: true
   verbose: false
-  icp_iter_num : 2
+  icp_iter_num: 2
   match_dist_sq_th: 1
 
 # configs for mapper
-mapper_config: 
+mapper_config:
-  vis: false
+  vis: false

oh_my_loam/mapper/mapper.h

@@ -18,7 +18,7 @@ class Mapper {
   void Visualize();
 
   common::TPointCloudPtr map_pts_;
-  common::Pose3D pose_curr2world_;
+  common::Pose3d pose_curr2world_;
 
   bool is_initialized = false;
 

oh_my_loam/odometer/odometer.cc

@@ -6,7 +6,7 @@
 namespace oh_my_loam {
 
 namespace {
-int kNearbyScanNum = 2;
+int kNearScanN = 2;
 size_t kMinMatchNum = 10;
 using namespace common;
 }  // namespace
@@ -16,20 +16,18 @@ bool Odometer::Init() {
   config_ = config["odometer_config"];
   is_vis_ = config["vis"].as<bool>() && config_["vis"].as<bool>();
   verbose_ = config_["verbose"].as<bool>();
-  kdtree_surf_.reset(new pcl::KdTreeFLANN<TPoint>);
-  kdtree_corn_.reset(new pcl::KdTreeFLANN<TPoint>);
   AINFO << "Odometry visualizer: " << (is_vis_ ? "ON" : "OFF");
   if (is_vis_) visualizer_.reset(new OdometerVisualizer);
   return true;
 }
 
-void Odometer::Process(double timestamp, const Feature& feature,
+void Odometer::Process(double timestamp, const std::vector<Feature>& features,
-                       Pose3D* const pose) {
+                       Pose3d* const pose) {
   BLOCK_TIMER_START;
   if (!is_initialized_) {
+    UpdatePre(features);
     is_initialized_ = true;
-    UpdatePre(feature);
+    *pose = Pose3d::Identity();
-    *pose = Pose3D();
     AWARN << "Odometer initialized....";
     return;
   }
@@ -37,162 +35,152 @@ void Odometer::Process(double timestamp, const Feature& feature,
   std::vector<PointLinePair> pl_pairs;
   std::vector<PointPlanePair> pp_pairs;
   for (int i = 0; i < config_["icp_iter_num"].as<int>(); ++i) {
-    MatchCornFeature(*feature.cloud_sharp_corner, *corn_pre_, &pl_pairs);
+    for (const auto& feature : features) {
-    AINFO_IF(i == 0) << "PL mathch: src size = "
+      MatchCorn(*feature.cloud_sharp_corner, &pl_pairs);
-                     << feature.cloud_sharp_corner->size()
+      // MatchSurf(*feature.cloud_flat_surf, &pp_pairs);
-                     << ", tgt size = " << corn_pre_->size();
-    MatchSurfFeature(*feature.cloud_flat_surf, *surf_pre_, &pp_pairs);
-    AINFO_IF(i == 0) << "PP mathch: src size = "
-                     << feature.cloud_flat_surf->size()
-                     << ", tgt size = " << surf_pre_->size();
-    AINFO << "Matched num, pl = " << pl_pairs.size()
-          << ", pp = " << pp_pairs.size();
-    if (pl_pairs.size() + pp_pairs.size() < kMinMatchNum) {
-      AWARN << "Too less correspondence: num = "
-            << pl_pairs.size() + pp_pairs.size();
     }
-    Eigen::Vector4d q_vec = pose_curr2last_.q().coeffs();
+    AINFO_IF(verbose_) << "Iter " << i
-    Eigen::Vector3d p_vec = pose_curr2last_.p();
+                       << ": matched num: point2line = " << pl_pairs.size()
-    double* q = q_vec.data();
+                       << ", point2plane = " << pp_pairs.size();
-    double* p = p_vec.data();
+    if (pl_pairs.size() + pp_pairs.size() < kMinMatchNum) {
-    PoseSolver solver(q, p);
+      AWARN << "Too less correspondence: " << pl_pairs.size() << " + "
+            << pp_pairs.size();
+    }
+    PoseSolver solver(pose_curr2last_);
     for (const auto& pair : pl_pairs) {
-      solver.AddPointLinePair(pair, GetTime(pair.pt));
+      solver.AddPointLinePair(pair, pair.pt.time);
     }
     for (const auto& pair : pp_pairs) {
-      solver.AddPointPlanePair(pair, GetTime(pair.pt));
+      solver.AddPointPlanePair(pair, pair.pt.time);
     }
-    solver.Solve();
+    solver.Solve(5, verbose_, &pose_curr2last_);
-    pose_curr2last_ = Pose3D(q, p);
   }
   pose_curr2world_ = pose_curr2world_ * pose_curr2last_;
   *pose = pose_curr2world_;
-  AWARN << "Pose increase: " << pose_curr2last_.ToString();
+  AWARN_IF(verbose_) << "Pose increase: " << pose_curr2last_.ToString();
-  AWARN << "Pose after: " << pose_curr2world_.ToString();
+  AWARN_IF(verbose_) << "Pose after: " << pose_curr2world_.ToString();
-  UpdatePre(feature);
+  UpdatePre(features);
+  if (is_vis_) Visualize(features, pl_pairs, pp_pairs);
   AINFO << "Odometer::Porcess: " << BLOCK_TIMER_STOP_FMT;
-  if (is_vis_) Visualize(feature, pl_pairs, pp_pairs);
 }
 
-void Odometer::MatchCornFeature(const TPointCloud& src, const TPointCloud& tgt,
+void Odometer::MatchCorn(const TPointCloud& src,
-                                std::vector<PointLinePair>* const pairs) const {
+                         std::vector<PointLinePair>* const pairs) const {
   double dist_sq_thresh = config_["match_dist_sq_th"].as<double>();
-  for (const auto& q_pt : src) {
+  auto comp = [](const std::vector<float>& v1, const std::vector<float>& v2) {
-    TPoint query_pt;
+    return v1[0] < v2[0];
-    TransformToStart(pose_curr2last_, q_pt, &query_pt);
+  };
-    std::vector<int> indices;
+  for (const auto& pt : src) {
-    std::vector<float> dists;
+    TPoint query_pt = TransformToStart(pose_curr2last_, pt);
-    kdtree_corn_->nearestKSearch(query_pt, 1, indices, dists);
+    std::vector<std::vector<int>> indices;
-    if (dists[0] >= dist_sq_thresh) continue;
+    std::vector<std::vector<float>> dists;
-    TPoint pt1 = tgt.points[indices[0]];
+    NearestKSearch(kdtrees_corn_, query_pt, 1, &indices, &dists);
-    int pt2_idx = -1;
+    int pt1_scan_id =
+        std::min_element(dists.begin(), dists.end(), comp) - dists.begin();
+    if (dists[pt1_scan_id][0] >= dist_sq_thresh) continue;
+    TPoint pt1 = clouds_corn_pre_[pt1_scan_id]->at(indices[pt1_scan_id][0]);
+
     double min_dist_pt2_squre = dist_sq_thresh;
-    int query_pt_scan_id = GetScanId(query_pt);
+    int pt2_scan_id = -1;
-    for (int i = indices[0] + 1; i < static_cast<int>(tgt.size()); ++i) {
+    int i_begin = std::max<int>(0, pt1_scan_id - kNearScanN);
-      const auto pt = tgt.points[i];
+    int i_end = std::min<int>(indices.size(), pt1_scan_id + kNearScanN + 1);
-      int scan_id = GetScanId(pt);
+    for (int i = i_begin; i < i_end && i != pt1_scan_id; ++i) {
-      if (scan_id <= query_pt_scan_id) continue;
+      if (dists[i][0] < min_dist_pt2_squre) {
-      if (scan_id > query_pt_scan_id + kNearbyScanNum) break;
+        pt2_scan_id = i;
-      double dist_squre = DistanceSqure(query_pt, pt);
+        min_dist_pt2_squre = dists[i][0];
-      if (dist_squre < min_dist_pt2_squre) {
-        pt2_idx = i;
-        min_dist_pt2_squre = dist_squre;
       }
     }
-    for (int i = indices[0] - 1; i >= 0; --i) {
+    if (pt2_scan_id > 0) {
-      const auto pt = tgt.points[i];
+      TPoint pt2 = clouds_corn_pre_[pt2_scan_id]->at(indices[pt2_scan_id][0]);
-      int scan_id = GetScanId(pt);
+      pairs->emplace_back(pt, pt1, pt2);
-      if (scan_id >= query_pt_scan_id) continue;
-      if (scan_id < query_pt_scan_id - kNearbyScanNum) break;
-      double dist_squre = DistanceSqure(query_pt, pt);
-      if (dist_squre < min_dist_pt2_squre) {
-        pt2_idx = i;
-        min_dist_pt2_squre = dist_squre;
-      }
-    }
-    if (pt2_idx >= 0) {
-      TPoint pt2 = tgt.points[pt2_idx];
-      pairs->emplace_back(q_pt, pt1, pt2);
     }
   }
 }
 
-void Odometer::MatchSurfFeature(
+void Odometer::MatchSurf(const TPointCloud& src,
-    const TPointCloud& src, const TPointCloud& tgt,
+                         std::vector<PointPlanePair>* const pairs) const {
-    std::vector<PointPlanePair>* const pairs) const {
   double dist_sq_thresh = config_["match_dist_sq_th"].as<double>();
-  for (const auto& q_pt : src) {
+  auto comp = [](const std::vector<float>& v1, const std::vector<float>& v2) {
-    TPoint query_pt;
+    return v1[0] < v2[0];
-    TransformToStart(pose_curr2last_, q_pt, &query_pt);
+  };
-    std::vector<int> indices;
+  for (const auto& pt : src) {
-    std::vector<float> dists;
+    TPoint query_pt = TransformToStart(pose_curr2last_, pt);
-    kdtree_surf_->nearestKSearch(query_pt, 1, indices, dists);
+    std::vector<std::vector<int>> indices;
-    if (dists[0] >= dist_sq_thresh) continue;
+    std::vector<std::vector<float>> dists;
-    TPoint pt1 = tgt.points[indices[0]];
+    NearestKSearch(kdtrees_surf_, query_pt, 2, &indices, &dists);
-    int pt2_idx = -1, pt3_idx = -1;
+    int pt1_scan_id =
-    double min_dist_pt2_squre = dist_sq_thresh;
+        std::min_element(dists.begin(), dists.end(), comp) - dists.begin();
+    if (dists[pt1_scan_id][0] >= dist_sq_thresh) continue;
+    if (dists[pt1_scan_id][1] >= dist_sq_thresh) continue;
+    TPoint pt1 = clouds_surf_pre_[pt1_scan_id]->at(indices[pt1_scan_id][0]);
+    TPoint pt2 = clouds_surf_pre_[pt1_scan_id]->at(indices[pt1_scan_id][1]);
+
     double min_dist_pt3_squre = dist_sq_thresh;
-    int query_pt_scan_id = GetScanId(query_pt);
+    int pt3_scan_id = -1;
-    for (int i = indices[0] + 1; i < static_cast<int>(tgt.size()); ++i) {
+    int i_begin = std::max<int>(0, pt1_scan_id - kNearScanN);
-      const auto pt = tgt.points[i];
+    int i_end = std::min<int>(indices.size(), pt1_scan_id + kNearScanN + 1);
-      int scan_id = GetScanId(pt);
+    for (int i = i_begin; i < i_end && i != pt1_scan_id; ++i) {
-      if (scan_id > query_pt_scan_id + kNearbyScanNum) break;
+      if (dists[i][0] < min_dist_pt3_squre) {
-      double dist_squre = DistanceSqure(query_pt, pt);
+        pt3_scan_id = i;
-      if (scan_id == query_pt_scan_id && dist_squre < min_dist_pt2_squre) {
+        min_dist_pt3_squre = dists[i][0];
-        pt2_idx = i;
-        min_dist_pt2_squre = dist_squre;
       }
-      if (scan_id > query_pt_scan_id && dist_squre < min_dist_pt3_squre) {
-        pt3_idx = i;
-        min_dist_pt3_squre = dist_squre;
-      }
-      if (pt2_idx == -1 && scan_id > query_pt_scan_id) break;
     }
-    for (int i = indices[0] - 1; i >= 0; --i) {
+    if (pt3_scan_id > 0) {
-      const auto pt = tgt.points[i];
+      TPoint pt3 = clouds_surf_pre_[pt3_scan_id]->at(indices[pt3_scan_id][0]);
-      int scan_id = GetScanId(pt);
+      pairs->emplace_back(pt, pt1, pt2, pt3);
-      if (scan_id < query_pt_scan_id - kNearbyScanNum) break;
-      double dist_squre = DistanceSqure(query_pt, pt);
-      if (scan_id == query_pt_scan_id && dist_squre < min_dist_pt2_squre) {
-        pt2_idx = i;
-        min_dist_pt2_squre = dist_squre;
-      }
-      if (scan_id < query_pt_scan_id && dist_squre < min_dist_pt3_squre) {
-        pt3_idx = i;
-        min_dist_pt3_squre = dist_squre;
-      }
-      if (pt2_idx == -1 && scan_id < query_pt_scan_id) break;
-    }
-    if (pt2_idx >= 0 && pt3_idx >= 0) {
-      TPoint pt2 = tgt.points[pt2_idx], pt3 = tgt.points[pt3_idx];
-      pairs->emplace_back(q_pt, pt1, pt2, pt3);
     }
   }
 }
 
-void Odometer::UpdatePre(const Feature& feature) {
+void Odometer::UpdatePre(const std::vector<Feature>& features) {
-  if (!surf_pre_) {
+  clouds_corn_pre_.resize(features.size(), common::TPointCloud().makeShared());
-    surf_pre_.reset(new TPointCloud);
+  clouds_surf_pre_.resize(features.size(), common::TPointCloud().makeShared());
-  }
+  kdtrees_corn_.resize(features.size());
-  if (!corn_pre_) {
+  kdtrees_surf_.resize(features.size());
-    corn_pre_.reset(new TPointCloud);
+
+  for (size_t i = 0; i < features.size(); ++i) {
+    const auto& feature = features[i];
+    auto& cloud_pre = clouds_corn_pre_[i];
+    cloud_pre->resize(feature.cloud_corner->size());
+    for (size_t j = 0; j < feature.cloud_corner->size(); ++j) {
+      TransformToEnd(pose_curr2last_, feature.cloud_corner->at(j),
+                     &cloud_pre->at(j));
+    }
+    kdtrees_corn_[i].setInputCloud(clouds_corn_pre_[i]);
   }
 
-  surf_pre_->resize(feature.cloud_less_flat_surf->size());
+  for (size_t i = 0; i < features.size(); ++i) {
-  for (size_t i = 0; i < feature.cloud_less_flat_surf->size(); ++i) {
+    const auto& feature = features[i];
-    TransformToEnd(pose_curr2last_, feature.cloud_less_flat_surf->points[i],
+    auto& cloud_pre = clouds_surf_pre_[i];
-                   &surf_pre_->points[i]);
+    cloud_pre->resize(feature.cloud_surf->size());
+    for (size_t j = 0; j < feature.cloud_surf->size(); ++j) {
+      TransformToEnd(pose_curr2last_, feature.cloud_surf->at(j),
+                     &cloud_pre->at(j));
+    }
+    kdtrees_surf_[i].setInputCloud(clouds_surf_pre_[i]);
   }
-  corn_pre_->resize(feature.cloud_less_sharp_corner->size());
-  for (size_t i = 0; i < feature.cloud_less_sharp_corner->size(); ++i) {
-    TransformToEnd(pose_curr2last_, feature.cloud_less_sharp_corner->points[i],
-                   &corn_pre_->points[i]);
-  }
-  kdtree_surf_->setInputCloud(surf_pre_);
-  kdtree_corn_->setInputCloud(corn_pre_);
 }
 
-void Odometer::Visualize(const Feature& feature,
+void Odometer::NearestKSearch(
+    const std::vector<pcl::KdTreeFLANN<common::TPoint>>& kdtrees,
+    const TPoint& query_pt, size_t k,
+    std::vector<std::vector<int>>* const indices,
+    std::vector<std::vector<float>>* const dists) const {
+  for (const auto& kdtree : kdtrees) {
+    std::vector<int> index;
+    std::vector<float> dist;
+    if (kdtree.getInputCloud()->size() >= k) {
+      kdtree.nearestKSearch(query_pt, k, index, dist);
+    } else {
+      index.resize(k, -1);
+      dist.resize(k, std::numeric_limits<float>::max());
+    }
+    indices->push_back(std::move(index));
+    dists->push_back(std::move(dist));
+  }
+}
+
+void Odometer::Visualize(const std::vector<Feature>& features,
                          const std::vector<PointLinePair>& pl_pairs,
                          const std::vector<PointPlanePair>& pp_pairs,
                          double timestamp) const {
@@ -204,5 +192,4 @@ void Odometer::Visualize(const Feature& feature,
   frame->pose_curr2world = pose_curr2world_;
   visualizer_->Render(frame);
 }
-
 }  // namespace oh_my_loam

oh_my_loam/odometer/odometer.h

@@ -1,7 +1,7 @@
 #pragma once
 
 #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/helper.h"
 #include "oh_my_loam/visualizer/odometer_visualizer.h"
@@ -16,32 +16,36 @@ class Odometer {
   bool Init();
 
   void Process(double timestamp, const std::vector<Feature>& features,
-               common::Pose3D* const pose);
+               common::Pose3d* const pose);
 
  protected:
-  void UpdatePre(const std::vector<Feature>& feature) s;
+  void UpdatePre(const std::vector<Feature>& features);
 
-  void MatchCornFeature(const common::TPointCloud& src,
+  void MatchCorn(const common::TPointCloud& src,
-                        const common::TPointCloud& tgt,
+                 std::vector<PointLinePair>* const pairs) const;
-                        std::vector<PointLinePair>* const pairs) const;
 
-  void MatchSurfFeature(const common::TPointCloud& src,
+  void MatchSurf(const common::TPointCloud& src,
-                        const common::TPointCloud& tgt,
+                 std::vector<PointPlanePair>* const pairs) const;
-                        std::vector<PointPlanePair>* const pairs) const;
 
   void Visualize(const std::vector<Feature>& features,
                  const std::vector<PointLinePair>& pl_pairs,
                  const std::vector<PointPlanePair>& pp_pairs,
                  double timestamp = 0.0) const;
 
-  common::Pose3D pose_curr2world_;
+  void NearestKSearch(
-  common::Pose3D pose_curr2last_;
+      const std::vector<pcl::KdTreeFLANN<common::TPoint>>& kdtrees,
+      const TPoint& query_pt, size_t k,
+      std::vector<std::vector<int>>* const indices,
+      std::vector<std::vector<float>>* const dists) const;
 
-  common::TPointCloudPtr cloud_corn_pre_{nullptr};
+  common::Pose3d pose_curr2world_;
-  common::TPointCloudPtr cloud_surf_pre_{nullptr};
+  common::Pose3d pose_curr2last_;
 
-  pcl::KdTreeFLANN<common::TPoint>::Ptr kdtree_surf_{nullptr};
+  std::vector<common::TPointCloudPtr> clouds_corn_pre_;
-  pcl::KdTreeFLANN<common::TPoint>::Ptr kdtree_corn_{nullptr};
+  std::vector<common::TPointCloudPtr> clouds_surf_pre_;
+
+  std::vector<pcl::KdTreeFLANN<common::TPoint>> kdtrees_surf_;
+  std::vector<pcl::KdTreeFLANN<common::TPoint>> kdtrees_corn_;
 
   YAML::Node config_;
 

oh_my_loam/oh_my_loam.cc

@@ -17,11 +17,11 @@ bool OhMyLoam::Init() {
     AERROR << "Failed to initialize extractor";
     return false;
   }
-  // odometer_.reset(new Odometer);
+  odometer_.reset(new Odometer);
-  // if (!odometer_->Init()) {
+  if (!odometer_->Init()) {
-  //   AERROR << "Failed to initialize odometer";
+    AERROR << "Failed to initialize odometer";
-  //   return false;
+    return false;
-  // }
+  }
   // mapper_.reset(new Mapper);
   // if (!mapper_->Init()) {
   //   AERROR << "Failed to initialize mapper";
@@ -35,9 +35,9 @@ void OhMyLoam::Run(double timestamp, const PointCloudConstPtr& cloud_in) {
   RemoveOutliers(*cloud_in, cloud.get());
   std::vector<Feature> features;
   extractor_->Process(timestamp, cloud, &features);
-  // Pose3D pose;
+  Pose3d pose;
-  // odometer_->Process(timestamp, feature, &pose);
+  odometer_->Process(timestamp, features, &pose);
-  // poses_.emplace_back(pose);
+  poses_.emplace_back(pose);
 }
 
 void OhMyLoam::RemoveOutliers(const PointCloud& cloud_in,

oh_my_loam/oh_my_loam.h

@@ -3,7 +3,7 @@
 #include "common/common.h"
 #include "oh_my_loam/extractor/extractor.h"
 // #include "oh_my_loam/mapper/mapper.h"
-// #include "oh_my_loam/odometer/odometer.h"
+#include "oh_my_loam/odometer/odometer.h"
 
 namespace oh_my_loam {
 
@@ -18,7 +18,7 @@ class OhMyLoam {
  private:
   std::unique_ptr<Extractor> extractor_{nullptr};
 
-  // std::unique_ptr<Odometer> odometer_{nullptr};
+  std::unique_ptr<Odometer> odometer_{nullptr};
 
   // std::unique_ptr<Mapper> mapper_{nullptr};
 
@@ -26,7 +26,7 @@ class OhMyLoam {
   void RemoveOutliers(const common::PointCloud& cloud_in,
                       common::PointCloud* const cloud_out) const;
 
-  std::vector<common::Pose3D> poses_;
+  std::vector<common::Pose3d> poses_;
 
   DISALLOW_COPY_AND_ASSIGN(OhMyLoam)
 };

oh_my_loam/solver/solver.cc

@@ -6,26 +6,16 @@ namespace {
 double kHuberLossScale = 0.1;
 }
 
-PoseSolver::PoseSolver(double *q, double *p) : q_(q), p_(p) {
+PoseSolver::PoseSolver(const common::Pose3d &pose)
+    : r_quat_(pose.r_quat().coeffs().data()), t_vec_(pose.t_vec().data()) {
   loss_function_ = new ceres::HuberLoss(kHuberLossScale);
-  problem_.AddParameterBlock(q_, 4,
+  problem_.AddParameterBlock(r_quat_, 4,
                              new ceres::EigenQuaternionParameterization());
-  problem_.AddParameterBlock(p_, 3);
+  problem_.AddParameterBlock(t_vec_, 3);
 }
 
-void PoseSolver::AddPointLinePair(const PointLinePair &pair, double time) {
+double PoseSolver::Solve(int max_iter_num, bool verbose,
-  ceres::CostFunction *cost_function =
+                         common::Pose3d *const pose) {
-      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;
@@ -33,6 +23,24 @@ void PoseSolver::Solve(int max_iter_num, bool verbose) {
   ceres::Solver::Summary summary;
   ceres::Solve(options, &problem_, &summary);
   AINFO_IF(verbose) << summary.BriefReport();
+  if (pose) *pose = common::Pose3d(r_quat_, t_vec_);
+  return summary.final_cost;
+}
+
+void PoseSolver::AddPointLinePair(const PointLinePair &pair, double time) {
+  ceres::CostFunction *cost_function =
+      PointLineCostFunction::Create(pair, time);
+  problem_.AddResidualBlock(cost_function, loss_function_, r_quat_, t_vec_);
+}
+
+void PoseSolver::AddPointPlanePair(const PointPlanePair &pair, double time) {
+  ceres::CostFunction *cost_function =
+      PointPlaneCostFunction::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

oh_my_loam/solver/solver.h

@@ -1,25 +1,30 @@
 #pragma once
 
+#include "common/geometry/pose3d.h"
 #include "cost_function.h"
 
 namespace oh_my_loam {
 
 class PoseSolver {
  public:
-  PoseSolver(double* q, double* p);
+  PoseSolver(const common::Pose3d& pose);
 
   void AddPointLinePair(const PointLinePair& pair, double time);
 
   void AddPointPlanePair(const PointPlanePair& pair, double time);
 
-  void Solve(int max_iter_num = 5, bool verbose = false);
+  double Solve(int max_iter_num = 5, bool verbose = false,
+               common::Pose3d* const pose = nullptr);
+
+  common::Pose3d GetPose() const;
 
  private:
   ceres::Problem problem_;
 
   ceres::LossFunction* loss_function_;
 
-  double *q_, *p_;
+  // r_quat_: [x, y, z, w], t_vec_: [x, y, z]
+  double *r_quat_, *t_vec_;
 
   DISALLOW_COPY_AND_ASSIGN(PoseSolver)
 };

oh_my_loam/visualizer/odometer_visualizer.cc

@@ -33,16 +33,16 @@ void OdometerVisualizer::Draw() {
   DrawPointCloud<TPoint>(tgt_corn_pts, BLUE, "tgt_corn_pts", 4);
   DrawPointCloud<TPoint>(src_surf_pts, PURPLE, "src_surf_pts", 7);
   DrawPointCloud<TPoint>(tgt_surf_pts, RED, "tgt_surf_pts", 4);
-  std::vector<Pose3D> poses_n;
+  std::vector<Pose3d> poses_n;
   poses_n.reserve((poses_.size()));
-  Pose3D pose_inv = frame.pose_curr2world.Inv();
+  Pose3d pose_inv = frame.pose_curr2world.Inv();
   for (const auto& pose : poses_) {
     poses_n.emplace_back(pose_inv * pose);
   };
   for (size_t i = 0; i < poses_n.size(); ++i) {
-    Eigen::Vector3f p1 = poses_n[i].p().cast<float>();
+    Eigen::Vector3f p1 = poses_n[i].t_vec().cast<float>();
     if (i < poses_n.size() - 1) {
-      Eigen::Vector3f p2 = poses_n[i + 1].p().cast<float>();
+      Eigen::Vector3f p2 = poses_n[i + 1].t_vec().cast<float>();
       AddLine(Point(p1.x(), p1.y(), p1.z()), Point(p2.x(), p2.y(), p2.z()),
               WHITE, "line" + std::to_string(i), viewer_.get());
     } else {

oh_my_loam/visualizer/odometer_visualizer.h

@@ -10,8 +10,8 @@ struct OdometerVisFrame : public common::LidarVisFrame {
   common::TPointCloudPtr cloud_corner;
   std::vector<PointLinePair> pl_pairs;
   std::vector<PointPlanePair> pp_pairs;
-  common::Pose3D pose_curr2last;
+  common::Pose3d pose_curr2last;
-  common::Pose3D pose_curr2world;
+  common::Pose3d pose_curr2world;
 };
 
 class OdometerVisualizer : public common::LidarVisualizer {
@@ -23,7 +23,7 @@ class OdometerVisualizer : public common::LidarVisualizer {
  private:
   void Draw() override;
 
-  std::deque<common::Pose3D> poses_;
+  std::deque<common::Pose3d> poses_;
 };
 
 }  // namespace oh_my_loam