odometry: writing...

CMakeLists.txt

@@ -56,4 +56,5 @@ target_link_libraries(main
   oh_my_loam
   extractor
   visualizer
+  # odometry
 )

common/pose.cc

@@ -0,0 +1,9 @@
+#include <pose.h>
+
+namespace oh_my_loam {
+
+Pose3D Interpolate(const Pose3D& pose_from, const Pose3D& pose_to, double t) {
+  return pose_from.InterPolate(pose_to, t);
+}
+
+}  // namespace oh_my_loam

common/pose.h

@@ -0,0 +1,62 @@
+#pragma once
+
+#include <eigen3/Eigen/Dense>
+
+namespace oh_my_loam {
+
+class Pose3D {
+ public:
+  Pose3D() {
+    q_.setIdentity();
+    p_.setZero();
+  };
+
+  Pose3D(const Eigen::Quaterniond& q, const Eigen::Vector3d& p)
+      : q_(q), p_(p) {}
+
+  Pose3D(const Eigen::Matrix3d& r_mat, const Eigen::Vector3d& p) {
+    q_ = Eigen::Quaterniond(r_mat);
+    p_ = p;
+  }
+
+  Pose3D Inv() const {
+    auto q_inv = q_.inverse();
+    auto p_inv = q_inv * p_;
+    return {q_inv, p_inv};
+  }
+
+  Eigen::Vector3d Transform(const Eigen::Vector3d& vec) const {
+    return q_ * vec + p_;
+  }
+
+  template <typename PointT>
+  PointT Transform(const PointT& pt) const {
+    PointT pt_n = pt;
+    Eigen::Vector3d vec = Transform(Eigen::Vector3d(pt.x, pt.y, pt.z));
+    pt_n.x = vec.x;
+    pt_n.y = vec.y;
+    pt_n.z = vec.z;
+    return pt_n;
+  }
+
+  Eigen::Vector3d Translate(const Eigen::Vector3d& vec) const {
+    return vec + p_;
+  }
+
+  Eigen::Vector3d Rotate(const Eigen::Vector3d& vec) const { return q_ * vec; }
+
+  // Spherical linear interpolation to `pose_to`, `t` belongs [0, 1]
+  Pose3D InterPolate(const Pose3D& pose_to, double t) const {
+    auto q_interp = q_.slerp(t, pose_to.q_);
+    auto p_interp = (pose_to.p_ - p_) * t;
+    return {q_interp, p_interp};
+  }
+
+ protected:
+  Eigen::Quaterniond q_;  // orientation
+  Eigen::Vector3d p_;     // position
+};
+
+Pose3D Interpolate(const Pose3D& pose_from, const Pose3D& pose_to, double t);
+
+}  // namespace oh_my_loam

common/types.h

@@ -28,17 +28,48 @@ enum class PointType {
   SHARP = 2,
 };
 
+struct PointXYZT;
+using TPoint = PointXYZT;
+using TPointCloud = pcl::PointCloud<TPoint>;
+using TPointCloudPtr = TPointCloud::Ptr;
+using TPointCloudConstPtr = TPointCloud::ConstPtr;
+
 struct PointXYZTCT;
+using TCTPoint = PointXYZTCT;
+using TCTPointCloud = pcl::PointCloud<TCTPoint>;
+using TCTPointCloudPtr = TCTPointCloud::Ptr;
+using TCTPointCloudConstPtr = TCTPointCloud::ConstPtr;
 
-using IPoint = PointXYZTCT;
-using IPointCloud = pcl::PointCloud<IPoint>;
-using IPointCloudPtr = IPointCloud::Ptr;
-using IPointCloudConstPtr = IPointCloud::ConstPtr;
+struct PointXYZT {
+  PCL_ADD_POINT4D;
+  float time;
 
-using PCLVisualizer = pcl::visualization::PCLVisualizer;
-#define PCLColorHandlerCustom pcl::visualization::PointCloudColorHandlerCustom
-#define PCLColorHandlerGenericField \
-  pcl::visualization::PointCloudColorHandlerGenericField
+  PointXYZT() {
+    x = y = z = 0.0f;
+    time = 0.0f;
+  }
+
+  PointXYZT(float x, float y, float z, float time = 0.0f)
+      : x(x), y(y), z(z), time(time) {}
+
+  PointXYZT(const Point& p) {
+    x = p.x;
+    y = p.y;
+    z = p.z;
+    time = 0.0f;
+  }
+
+  PointXYZT(const PointXYZT& p) {
+    x = p.x;
+    y = p.y;
+    z = p.z;
+    time = p.time;
+    curvature = p.curvature;
+    type = p.type;
+  }
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+} EIGEN_ALIGN16;
 
 struct PointXYZTCT {
   PCL_ADD_POINT4D;
@@ -87,6 +118,14 @@ struct PointXYZTCT {
 }  // namespace oh_my_loam
 
 // clang-format off
+POINT_CLOUD_REGISTER_POINT_STRUCT(
+  oh_my_loam::PointXYZT,
+  (float, x, x)
+  (float, y, y)
+  (float, z, z)
+  (float, time, time)
+)
+
 POINT_CLOUD_REGISTER_POINT_STRUCT(
   oh_my_loam::PointXYZTCT,
   (float, x, x)
@@ -94,6 +133,5 @@ POINT_CLOUD_REGISTER_POINT_STRUCT(
   (float, z, z)
   (float, time, time)
   (float, curvature, curvature)
-  // (int8_t, type, type)
 )
 // clang-format on

common/utils.h

@@ -11,9 +11,20 @@ inline double DistanceSqure(const PointT& pt) {
   return pt.x * pt.x + pt.y * pt.y + pt.z * pt.z;
 }
 
+template <typename PointT>
+inline double DistanceSqure(const PointT& pt1, const PointT& pt2) {
+  return (pt1.x - pt2.x) * (pt1.x - pt2.x) + (pt1.y - pt2.y) * (pt1.y - pt2.y) +
+         (pt1.z - pt2.z) * (pt1.z - pt2.z);
+}
+
 template <typename PointT>
 inline double Distance(const PointT& pt) {
-  return std::sqrt(pt.x * pt.x + pt.y * pt.y + pt.z * pt.z);
+  return std::sqrt(DistanceSqure(pt));
+}
+
+template <typename PointT>
+inline double Distance(const PointT& pt1, const PointT& pt2) {
+  return std::sqrt(DistanceSqure(pt1, pt2));
 }
 
 template <typename PointT>
@@ -28,27 +39,6 @@ double NormalizeAngle(double ang);
 const std::vector<int> Range(int begin, int end, int step = 1);
 const std::vector<int> Range(int end);  // [0, end)
 
-template <typename PointT>
-void DrawPointCloud(const pcl::PointCloud<PointT>& cloud, const Color& color,
-                    const std::string& id, PCLVisualizer* const viewer,
-                    int pt_size = 3) {
-  PCLColorHandlerCustom<PointT> color_handler(cloud.makeShared(), color.r,
-                                              color.g, color.b);
-  viewer->addPointCloud<PointT>(cloud.makeShared(), color_handler, id);
-  viewer->setPointCloudRenderingProperties(
-      pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pt_size, id);
-}
-
-template <typename PointT>
-void DrawPointCloud(const pcl::PointCloud<PointT>& cloud,
-                    const std::string& field, const std::string& id,
-                    PCLVisualizer* const viewer, int pt_size = 3) {
-  PCLColorHandlerGenericField<PointT> color_handler(cloud.makeShared(), field);
-  viewer->addPointCloud<PointT>(cloud.makeShared(), color_handler, id);
-  viewer->setPointCloudRenderingProperties(
-      pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pt_size, id);
-}
-
 template <typename PointT>
 void RemovePointsIf(const pcl::PointCloud<PointT>& cloud_in,
                     pcl::PointCloud<PointT>* const cloud_out,

src/CMakeLists.txt

@@ -1,5 +1,6 @@
 add_subdirectory(extractor)
 add_subdirectory(visualizer)
+# add_subdirectory(odometry)
 
 aux_source_directory(. SRC_FILES)
 

src/extractor/base_extractor.cc

@@ -17,7 +17,7 @@ bool Extractor::Init(const YAML::Node& config) {
   return true;
 }
 
-void Extractor::Extract(const PointCloud& cloud, FeaturePoints* const feature) {
+void Extractor::Process(const PointCloud& cloud, FeaturePoints* const feature) {
   if (cloud.size() < config_["min_point_num"].as<size_t>()) {
     AWARN << "Too few points ( < " << config_["min_point_num"].as<int>()
           << " ) after remove: " << cloud.size();
@@ -25,7 +25,7 @@ void Extractor::Extract(const PointCloud& cloud, FeaturePoints* const feature) {
   }
   TicToc timer;
   // split point cloud int scans
-  std::vector<IPointCloud> scans;
+  std::vector<TCTPointCloud> scans;
   SplitScan(cloud, &scans);
   double time_split = timer.toc();
   // compute curvature for each point in each scan
@@ -62,7 +62,7 @@ void Extractor::Extract(const PointCloud& cloud, FeaturePoints* const feature) {
 }
 
 void Extractor::SplitScan(const PointCloud& cloud,
-                          std::vector<IPointCloud>* const scans) const {
+                          std::vector<TCTPointCloud>* const scans) const {
   scans->resize(num_scans_);
   double yaw_start = -atan2(cloud.points[0].y, cloud.points[0].x);
   bool half_passed = false;
@@ -82,7 +82,7 @@ void Extractor::SplitScan(const PointCloud& cloud,
 }
 
 //
-void Extractor::ComputePointCurvature(IPointCloud* const scan,
+void Extractor::ComputePointCurvature(TCTPointCloud* const scan,
                                       bool remove_nan) const {
   if (scan->size() < 20) return;
   auto& pts = scan->points;
@@ -98,12 +98,12 @@ void Extractor::ComputePointCurvature(IPointCloud* const scan,
                pts[i + 4].z + pts[i + 5].z - 10 * pts[i].z;
     pts[i].curvature = std::sqrt(dx * dx + dy * dy + dz * dz);
   }
-  RemovePointsIf<IPoint>(*scan, scan, [](const IPoint& pt) {
+  RemovePointsIf<TCTPoint>(*scan, scan, [](const TCTPoint& pt) {
     return !std::isfinite(pt.curvature);
   });
 }
 
-void Extractor::AssignPointType(IPointCloud* const scan) const {
+void Extractor::AssignPointType(TCTPointCloud* const scan) const {
   int pt_num = scan->size();
   ACHECK(pt_num >= kScanSegNum);
   int seg_pt_num = (pt_num - 1) / kScanSegNum + 1;
@@ -163,7 +163,7 @@ void Extractor::AssignPointType(IPointCloud* const scan) const {
   }
 }
 
-void Extractor::SetNeighborsPicked(const IPointCloud& scan, size_t ix,
+void Extractor::SetNeighborsPicked(const TCTPointCloud& scan, size_t ix,
                                    std::vector<bool>* const picked) const {
   auto DistSqure = [&](int i, int j) -> float {
     float dx = scan.at(i).x - scan.at(j).x;
@@ -187,7 +187,7 @@ void Extractor::SetNeighborsPicked(const IPointCloud& scan, size_t ix,
   }
 }
 
-void Extractor::StoreToFeaturePoints(const IPointCloud& scan,
+void Extractor::StoreToFeaturePoints(const TCTPointCloud& scan,
                                      FeaturePoints* const feature) const {
   for (const auto& pt : scan.points) {
     switch (pt.type) {

src/extractor/base_extractor.h

@@ -11,7 +11,7 @@ class Extractor {
   virtual ~Extractor() = default;
 
   bool Init(const YAML::Node& config);
-  void Extract(const PointCloud& cloud, FeaturePoints* const feature);
+  void Process(const PointCloud& cloud, FeaturePoints* const feature);
 
   int num_scans() const { return num_scans_; }
 
@@ -27,17 +27,17 @@ class Extractor {
                  double timestamp = std::nan(""));
 
   void SplitScan(const PointCloud& cloud,
-                 std::vector<IPointCloud>* const scans) const;
+                 std::vector<TCTPointCloud>* const scans) const;
 
-  void ComputePointCurvature(IPointCloud* const scan,
+  void ComputePointCurvature(TCTPointCloud* const scan,
                              bool remove_nan = true) const;
 
-  void AssignPointType(IPointCloud* const scan) const;
+  void AssignPointType(TCTPointCloud* const scan) const;
 
-  void SetNeighborsPicked(const IPointCloud& scan, size_t ix,
+  void SetNeighborsPicked(const TCTPointCloud& scan, size_t ix,
                           std::vector<bool>* const picked) const;
 
-  void StoreToFeaturePoints(const IPointCloud& scan,
+  void StoreToFeaturePoints(const TCTPointCloud& scan,
                             FeaturePoints* const feature) const;
 
   bool is_vis_ = false;

src/odometry/CMakeLists.txt

@@ -0,0 +1,3 @@
+aux_source_directory(. SRC_FILES)
+
+add_library(odometry SHARED ${SRC_FILES})

src/odometry/odometry.cc

@@ -0,0 +1,52 @@
+#include "odometry.h"
+
+namespace oh_my_loam {
+
+bool Odometry::Init(const YAML::Node& config) {
+  config_ = config;
+  return true;
+}
+
+void Odometry::Process(const FeaturePoints& feature) {
+  if (!is_initialized) {
+    is_initialized = true;
+    UpdatePre(feature);
+    return;
+  }
+  AssociateCornPoints();
+  AssociateSurfPoints();
+
+  UpdatePre(feature);
+}
+
+void Odometry::AssociateCornPoints(
+    const TPointCloud& sharp_corn_pts_curr,
+    std::vector<PointLinePair>* const pairs) const {
+  for (const pt : sharp_corn_pts_curr) {
+    Point p1, p2;
+
+    pairs->emplace_back(pt, {p1, p2});
+  }
+}
+
+void Odometry::AssociateSurfPoints(
+    const TPointCloud& flat_surf_pts_curr,
+    std::vector<PointPlanePair>* const pairs) const {
+  for (const pt : flat_surf_pts_curr) {
+    Point p1, p2, p3;
+    pairs->emplace_back(pt, {p1, p2, p3});
+  }
+}
+
+void Odometry::UpdatePre(const FeaturePoints& feature) {
+  surf_pts_pre_ = feature.less_flat_surf_pts;
+  corn_pts_pre_ = feature.less_sharp_corner_pts;
+  kdtree_surf_pts_->setInputCloud(surf_pts_pre_);
+  kdtree_corn_pts_->setInputCloud(corn_pts_pre_);
+}
+
+float GetTime(const TPoint& pt) { return pt.time - GetScanId(pt); }
+
+int GetScanId(const TPoint& pt) { return static_cast<int>(pt.time); }
+
+}  // namespace oh_my_loam

src/odometry/odometry.h

@@ -0,0 +1,65 @@
+#pragma once
+
+#include <pcl/kdtree/kdtree_flann.h>
+
+#include <eigen3/Eigen/Dense>
+
+#include "common.h"
+
+namespace oh_my_loam {
+
+float GetTime(const TPoint& pt);
+int GetScanId(const TPoint& pt);
+
+struct PointLinePair {
+  Point pt;
+  struct Line {
+    Point pt1, pt2;
+    Line(const Point& pt1, const Point& pt2) : pt1(pt1), pt2(pt2) {}
+  };
+  Line line;
+  PointLinePair(const Point& pt, const Line& line) : pt(pt), line(line) {}
+  double PointToLineDist() { return 0.0; }
+};
+
+struct PointPlanePair {
+  Point pt;
+  struct Plane {
+    Point pt1, pt2, pt3;
+    Plane(const Point& pt1, const Point& pt2, const Point& pt3)
+        : pt1(pt1), pt2(pt2), pt3(pt3) {}
+  };
+  Plane plane;
+  PointLinePair(const Point& pt, const Plane& plane) : pt(pt), plane(plane) {}
+  double PointToPlaneDist() { return 0.0; }
+};
+
+// frame to frame lidar odometry
+class Odometry {
+ public:
+  Odometry() = default;
+
+  bool Init(const YAML::Node& config);
+
+  void Process(const FeaturePoints& feature);
+
+ protected:
+  void UpdatePre(const FeaturePoints& feature);
+  void AssociateCornPoints(const TPointCloud& sharp_corn_pts_curr,
+                           std::vector<PointLinePair>* const pairs) const;
+
+  void AssociateSurfPoints(const TPointCloud& flat_surf_pts_curr,
+                           std::vector<PointPlanePair>* const pairs) const;
+
+  YAML::Node config_;
+
+  bool is_initialized = false;
+
+  TPointCloudPtr surf_pts_pre_;
+  TPointCloudPtr corn_pts_pre_;
+
+  pcl::KdTreeFLANN<TPoint>::Ptr kdtree_surf_pts(new pcl::KdTreeFLANN<TPoint>);
+  pcl::KdTreeFLANN<TPoint>::Ptr kdtree_corn_pts(new pcl::KdTreeFLANN<TPoint>);
+};
+
+}  // namespace oh_my_loam

src/oh_my_loam.cc

@@ -24,7 +24,7 @@ void OhMyLoam::Run(const PointCloud& cloud_in, double timestamp) {
   ADEBUG << "After remove, point num: " << cloud_in.size() << " -> "
          << cloud->size();
   FeaturePoints feature_points;
-  extractor_->Extract(*cloud, &feature_points);
+  extractor_->Process(*cloud, &feature_points);
 }
 
 void OhMyLoam::RemoveOutliers(const PointCloud& cloud_in,

src/visualizer/base_visualizer.h

@@ -10,6 +10,7 @@
 #include <thread>
 
 #include "common.h"
+#include "utils.h"
 
 namespace oh_my_loam {
 
@@ -77,7 +78,7 @@ class Visualizer {
    * @brief Draw objects. This method should be overrided for customization.
    *
    * virtual void Draw() {
-   *   VisFrame frame* = GetCurrentFrame();
+   *   auto frame = GetCurrentFrame<VisFrame>();
    *   {  // Update cloud
    *     std::string id = "point cloud";
    *     DrawPointCloud(*frame.cloud, {255, 255, 255}, id, viewer_.get());

src/visualizer/utils.h

@@ -0,0 +1,31 @@
+#pragma once
+
+namespace oh_my_loam {
+
+using PCLVisualizer = pcl::visualization::PCLVisualizer;
+#define PCLColorHandlerCustom pcl::visualization::PointCloudColorHandlerCustom
+#define PCLColorHandlerGenericField \
+  pcl::visualization::PointCloudColorHandlerGenericField
+
+template <typename PointT>
+void DrawPointCloud(const pcl::PointCloud<PointT>& cloud, const Color& color,
+                    const std::string& id, PCLVisualizer* const viewer,
+                    int pt_size = 3) {
+  PCLColorHandlerCustom<PointT> color_handler(cloud.makeShared(), color.r,
+                                              color.g, color.b);
+  viewer->addPointCloud<PointT>(cloud.makeShared(), color_handler, id);
+  viewer->setPointCloudRenderingProperties(
+      pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pt_size, id);
+}
+
+template <typename PointT>
+void DrawPointCloud(const pcl::PointCloud<PointT>& cloud,
+                    const std::string& field, const std::string& id,
+                    PCLVisualizer* const viewer, int pt_size = 3) {
+  PCLColorHandlerGenericField<PointT> color_handler(cloud.makeShared(), field);
+  viewer->addPointCloud<PointT>(cloud.makeShared(), color_handler, id);
+  viewer->setPointCloudRenderingProperties(
+      pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pt_size, id);
+}
+
+}  // namespace oh_my_loam