solver: writing...

CMakeLists.txt

@@ -54,6 +54,8 @@ target_link_libraries(main
   g3log
   common
   oh_my_loam
+  # helper
+  # solver
   extractor
   visualizer
   # odometry

common/types.h

@@ -64,8 +64,6 @@ struct PointXYZT {
     y = p.y;
     z = p.z;
     time = p.time;
-    curvature = p.curvature;
-    type = p.type;
   }
 
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW

src/CMakeLists.txt

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

src/helper/CMakeLists.txt

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

src/helper/helper.cc

@@ -0,0 +1,13 @@
+#include "helper.h"
+
+namespace oh_my_loam {
+
+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

@@ -0,0 +1,36 @@
+#pragma once
+
+#include "common.h"
+
+namespace oh_my_loam {
+
+float GetTime(const TPoint& pt);
+
+int GetScanId(const TPoint& pt);
+
+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) {}
+  double DistPointToLine() const;
+};
+
+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) {}
+  double DistPointToPlane() const;
+};
+
+}  // oh_my_loam

src/odometry/odometry.cc

@@ -2,8 +2,15 @@
 
 namespace oh_my_loam {
 
+namespace {
+int kNearbyScanNum = 2;
+double kDistSquareThresh = 25;
+}
+
 bool Odometry::Init(const YAML::Node& config) {
   config_ = config;
+  kdtree_surf_pts.reset(new pcl::KdTreeFLANN<TPoint>);
+  kdtree_corn_pts.reset(new pcl::KdTreeFLANN<TPoint>);
   return true;
 }
 
@@ -13,40 +20,115 @@ void Odometry::Process(const FeaturePoints& feature) {
     UpdatePre(feature);
     return;
   }
-  AssociateCornPoints();
-  AssociateSurfPoints();
+  std::vector<PointLinePair> pl_pairs;
+  std::vector<PointPlanePair> pp_pairs;
+  AssociateCornPoints(*feature.sharp_corner_pts, corn_pts_pre_, pl_pairs,
+                      kDistSquareThresh);
+  AssociateSurfPoints(*feature.flat_surf_pts, surf_pts_pre_, pp_pairs,
+                      kDistSquareThresh);
 
   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::AssociateCornPoints(const TPointCloud& src,
+                                   const TPointCloud& tgt,
+                                   std::vector<PointLinePair>* const pairs,
+                                   double dist_thresh) const {
+  kdtree_corn_pts_->setInputCloud(tgt);
+  for (const query_pt : src) {
+    std::vector<int> indices;
+    std::vector<float> dists;
+    kdtree_corn_pts->nearestKSearch(query_pt, 1, indices, dists);
+    if (dists[0] >= dist_thresh) continue;
+    Point 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) {
+      const auto pt = tgt.points[i];
+      int scan_id = GetScanId(pt);
+      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;
+      }
+    }
+    for (int i = indices[0] - 1; i >= 0; --i) {
+      const auto pt = tgt.points[i];
+      int scan_id = GetScanId(pt);
+      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(query_pt, {pt1, pt2});
+    }
   }
 }
 
-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::AssociateSurfPoints(const TPointCloud& src,
+                                   const TPointCloud& tgt,
+                                   std::vector<PointPlanePair>* const pairs,
+                                   double dist_thresh) const {
+  kdtree_surf_pts_->setInputCloud(tgt);
+  for (const query_pt : src) {
+    std::vector<int> indices;
+    std::vector<float> dists;
+    kdtree_corn_pts->nearestKSearch(query_pt, 1, indices, dists);
+    if (dists[0] >= dist_thresh) continue;
+    Point 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) {
+      const auto pt = tgt.points[i];
+      int scan_id = GetScanId(pt);
+      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_pt2_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) {
+      const auto pt = tgt.points[i];
+      int scan_id = GetScanId(pt);
+      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 (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_pt2_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(query_pt, {pt1, pt2, pt3});
+    }
   }
 }
 
 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

@@ -5,35 +5,10 @@
 #include <eigen3/Eigen/Dense>
 
 #include "common.h"
+#include "helper/helper.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:
@@ -45,21 +20,23 @@ class Odometry {
 
  protected:
   void UpdatePre(const FeaturePoints& feature);
-  void AssociateCornPoints(const TPointCloud& sharp_corn_pts_curr,
-                           std::vector<PointLinePair>* const pairs) const;
+  void AssociateCornPoints(const TPointCloud& src, const TPointCloud& tgt,
+                           std::vector<PointLinePair>* const pairs,
+                           double dist_thresh) const;
 
-  void AssociateSurfPoints(const TPointCloud& flat_surf_pts_curr,
-                           std::vector<PointPlanePair>* const pairs) const;
-
-  YAML::Node config_;
-
-  bool is_initialized = false;
+  void AssociateSurfPoints(const TPointCloud& src, const TPointCloud& tgt,
+                           std::vector<PointPlanePair>* const pairs,
+                           double dist_thresh) const;
 
   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>);
+  pcl::KdTreeFLANN<TPoint>::Ptr kdtree_surf_pts;
+  pcl::KdTreeFLANN<TPoint>::Ptr kdtree_corn_pts;
+
+  bool is_initialized = false;
+
+  YAML::Node config_;
 };
 
 }  // namespace oh_my_loam

src/solver/CMakeLists.txt

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

src/solver/solver.cc

@@ -0,0 +1,7 @@
+#include "solver.h"
+
+namespace oh_my_loam {
+
+//
+
+}  // oh_my_loam

src/solver/solver.h

@@ -0,0 +1,19 @@
+#pragma once
+
+#include "ceres/ceres.h"
+
+namespace oh_my_loam {
+
+class Solver {
+ public:
+  Solver() = default;
+
+  void SetInitialGuess(double* param_q, double* param_p);
+
+ protected:
+  std::unique_ptr<ceres::Problem> problem_;
+  ceres::LocalParameterization* parameterization_;
+  ceres::LossFunction* loss_function_;
+};
+
+}  // oh_my_loam