odometry vis to be done

common/log.h

@@ -6,24 +6,41 @@
 #include <g3log/logworker.hpp>
 #include <iostream>
 
+// heere we define G3LOG instead of use LOG directly, since LOG macro in g3log
+// is conflict with LOG macro in glog. Same for G3LOG_IF and G3CHECK
+#define G3LOG(level)          \
+  if (!g3::logLevel(level)) { \
+  } else                      \
+  INTERNAL_LOG_MESSAGE(level).stream()
+
+#define G3LOG_IF(level, boolean_expression)                    \
+  if (false == (boolean_expression) || !g3::logLevel(level)) { \
+  } else                                                       \
+  INTERNAL_LOG_MESSAGE(level).stream()
+
+#define G3CHECK(boolean_expression)   \
+  if (true == (boolean_expression)) { \
+  } else                              \
+  INTERNAL_CONTRACT_MESSAGE(#boolean_expression).stream()
+
 const LEVELS ERROR{WARNING.value + 100, "ERROR"};
 const LEVELS USER(ERROR.value + 100, "USER");
 
-#define ADEBUG LOG(DEBUG)
+#define ADEBUG G3LOG(DEBUG)
-#define AINFO LOG(INFO)
+#define AINFO G3LOG(INFO)
-#define AWARN LOG(WARNING)
+#define AWARN G3LOG(WARNING)
-#define AERROR LOG(ERROR)
+#define AERROR G3LOG(ERROR)
-#define AUSER LOG(USER)
+#define AUSER G3LOG(USER)
-#define AFATAL LOG(FATAL)
+#define AFATAL G3LOG(FATAL)
 
 // LOG_IF
-#define ADEBUG_IF(cond) LOG_IF(DEBUG, cond)
+#define ADEBUG_IF(cond) G3LOG_IF(DEBUG, cond)
-#define AINFO_IF(cond) LOG_IF(INFO, cond)
+#define AINFO_IF(cond) G3LOG_IF(INFO, cond)
-#define AWARN_IF(cond) LOG_IF(WARNING, cond)
+#define AWARN_IF(cond) G3LOG_IF(WARNING, cond)
-#define AERROR_IF(cond) LOG_IF(ERROR, cond)
+#define AERROR_IF(cond) G3LOG_IF(ERROR, cond)
-#define AUSER_IF(cond) LOG_IF(USER, cond)
+#define AUSER_IF(cond) G3LOG_IF(USER, cond)
-#define AFATAL_IF(cond) LOG_IF(FATAL, cond)
+#define AFATAL_IF(cond) G3LOG_IF(FATAL, cond)
-#define ACHECK(cond) CHECK(cond)
+#define ACHECK(cond) G3CHECK(cond)
 
 namespace g3 {
 class CustomSink {

common/pose.cc

@@ -3,7 +3,7 @@
 namespace oh_my_loam {
 
 Pose3D Interpolate(const Pose3D& pose_from, const Pose3D& pose_to, double t) {
-  return pose_from.InterPolate(pose_to, t);
+  return pose_from.Interpolate(pose_to, t);
 }
 
 Pose3D operator*(const Pose3D& lhs, const Pose3D& rhs) {

common/pose.h

@@ -20,9 +20,9 @@ class Pose3D {
   Pose3D(const double* const q, const double* const p) : q_(q), p_(p) {}
 
   Pose3D Inv() const {
-    auto q_inv = q_.inverse();
+    Eigen::Quaterniond q_inv = q_.inverse();
-    auto p_inv = q_inv * p_;
+    Eigen::Vector3d p_inv = q_inv * p_;
-    return {q_inv, p_inv};
+    return {q_inv, -p_inv};
   }
 
   Eigen::Vector3d Transform(const Eigen::Vector3d& vec) const {
@@ -37,9 +37,9 @@ class Pose3D {
   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.x = static_cast<float>(vec.x());
-    pt_n.y = vec.y;
+    pt_n.y = static_cast<float>(vec.y());
-    pt_n.z = vec.z;
+    pt_n.z = static_cast<float>(vec.z());
     return pt_n;
   }
 
@@ -55,9 +55,9 @@ class Pose3D {
   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 {
+  Pose3D Interpolate(const Pose3D& pose_to, double t) const {
-    auto q_interp = q_.slerp(t, pose_to.q_);
+    Eigen::Quaterniond q_interp = q_.slerp(t, pose_to.q_);
-    auto p_interp = (pose_to.p_ - p_) * t;
+    Eigen::Vector3d p_interp = (pose_to.p_ - p_) * t + p_;
     return {q_interp, p_interp};
   }
 

common/types.h

@@ -46,7 +46,12 @@ using TCTPointCloudConstPtr = TCTPointCloud::ConstPtr;
 
 struct PointXYZT {
   PCL_ADD_POINT4D;
-  float time;
+  union EIGEN_ALIGN16 {
+    float time;
+    // make sure VoxelGrid can work with this custom point type:
+    // https://github.com/PointCloudLibrary/pcl/issues/2331
+    float intensity;
+  };
 
   PointXYZT() {
     x = y = z = 0.0f;

configs/config.yaml

@@ -10,12 +10,12 @@ extractor_config:
   vis: true
   sharp_corner_point_num: 2
   corner_point_num: 20
-  flat_surf_point_num: 3
+  flat_surf_point_num: 4
   surf_point_num: 3 ## useless currently
   corner_point_curvature_thres: 0.5
   surf_point_curvature_thres: 0.5
   neighbor_point_dist_thres: 0.05
-  downsample_voxel_size: 0.5
+  downsample_voxel_size: 0.3
 
 odometry_config: 
   icp_iter_num : 2

src/extractor/base_extractor.cc

@@ -47,8 +47,8 @@ void Extractor::Process(const PointCloud& cloud, FeaturePoints* const feature) {
   AINFO << "Time elapsed (ms): scan_split = " << std::setprecision(3)
         << time_split << ", curvature_compute = " << time_curv - time_split
         << ", type_assign = " << time_assign - time_curv
-        << ", store = " << time_store - time_assign
+        << ", store = " << time_store - time_assign;
-        << ", TOTAL = " << time_store;
+  AUSER << "Time elapsed (ms): whole extraction = " << time_store;
   if (is_vis_) Visualize(cloud, *feature);
 }
 
@@ -181,15 +181,14 @@ void Extractor::SetNeighborsPicked(const TCTPointCloud& scan, size_t ix,
 
 void Extractor::GenerateFeaturePoints(const TCTPointCloud& scan,
                                       FeaturePoints* const feature) const {
-  // TPointCloudPtr less_flat_surf_pts(new TPointCloud);
+  TPointCloudPtr less_flat_surf_pts(new TPointCloud);
   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, 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,
+        less_flat_surf_pts->points.emplace_back(pt.x, pt.y, pt.z, pt.time);
-                                                         pt.time);
         break;
       case PointType::SHARP:
         feature->sharp_corner_pts->points.emplace_back(pt.x, pt.y, pt.z,
@@ -201,16 +200,14 @@ void Extractor::GenerateFeaturePoints(const TCTPointCloud& scan,
         break;
       default:
         // all the rest are also LESS_FLAT
-        feature->less_flat_surf_pts->points.emplace_back(pt.x, pt.y, pt.z,
+        less_flat_surf_pts->points.emplace_back(pt.x, pt.y, pt.z, pt.time);
-                                                         pt.time);
         break;
     }
   }
-  // TPointCloudPtr filtered_less_flat_surf_pts(new TPointCloud);
+  TPointCloudPtr filtered_less_flat_surf_pts(new TPointCloud);
-  // VoxelDownSample<TPoint>(less_flat_surf_pts,
+  VoxelDownSample<TPoint>(less_flat_surf_pts, filtered_less_flat_surf_pts.get(),
-  // filtered_less_flat_surf_pts.get(),
+                          config_["downsample_voxel_size"].as<double>());
-  //                         config_["downsample_voxel_size"].as<double>());
+  *feature->less_flat_surf_pts += *filtered_less_flat_surf_pts;
-  // *feature->less_flat_surf_pts += *filtered_less_flat_surf_pts;
 }
 
 void Extractor::Visualize(const PointCloud& cloud,

src/helper/helper.h

@@ -10,6 +10,30 @@ inline float GetTime(const TPoint& pt) {
 
 inline int GetScanId(const TPoint& pt) { return static_cast<int>(pt.time); }
 
+/**
+ * @brief Transform a lidar point to the start of the scan
+ *
+ * @param pose Relative pose, end scan time w.r.t. start scan time
+ * @param time Point time relative to the start time of the scan, \in [0, 1]
+ */
+void TransformToStart(const Pose3D& pose, const TPoint& pt_in,
+                      TPoint* const pt_out) {
+  Pose3D pose_interp = Pose3D().Interpolate(pose, GetTime(pt_in));
+  *pt_out = pose_interp * 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
+ * @param time Point time relative to the start time of the scan, \in [0, 1]
+ */
+void TransformToEnd(const Pose3D& pose, const TPoint& pt_in,
+                    TPoint* const pt_out) {
+  TransformToStart(pose, pt_in, pt_out);
+  *pt_out = pose.Inv() * (*pt_out);
+}
+
 struct PointLinePair {
   TPoint pt;
   struct Line {

src/odometry/odometry.cc

@@ -72,13 +72,15 @@ void Odometry::Process(const FeaturePoints& feature, Pose3D* const pose) {
   AWARN << "Pose increase: " << pose_curr2last_.ToString();
   AWARN << "Pose after: " << pose_curr2world_.ToString();
   UpdatePre(feature);
-  AINFO << "Time elapsed (ms): whole odometry = " << timer_whole.toc();
+  AUSER << "Time elapsed (ms): whole odometry = " << timer_whole.toc();
 }
 
 void Odometry::MatchCornPoints(const TPointCloud& src, const TPointCloud& tgt,
                                std::vector<PointLinePair>* const pairs,
                                double dist_sq_thresh) const {
-  for (const auto& query_pt : src) {
+  for (const auto& q_pt : src) {
+    TPoint query_pt;
+    TransformToStart(pose_curr2last_, q_pt, &query_pt);
     std::vector<int> indices;
     std::vector<float> dists;
     kdtree_corn_pts_->nearestKSearch(query_pt, 1, indices, dists);
@@ -111,7 +113,7 @@ void Odometry::MatchCornPoints(const TPointCloud& src, const TPointCloud& tgt,
     }
     if (pt2_idx >= 0) {
       TPoint pt2 = tgt.points[pt2_idx];
-      pairs->emplace_back(query_pt, pt1, pt2);
+      pairs->emplace_back(q_pt, pt1, pt2);
     }
   }
 }
@@ -119,7 +121,9 @@ void Odometry::MatchCornPoints(const TPointCloud& src, const TPointCloud& tgt,
 void Odometry::MatchSurfPoints(const TPointCloud& src, const TPointCloud& tgt,
                                std::vector<PointPlanePair>* const pairs,
                                double dist_sq_thresh) const {
-  for (const auto& query_pt : src) {
+  for (const auto& q_pt : src) {
+    TPoint query_pt;
+    TransformToStart(pose_curr2last_, q_pt, &query_pt);
     std::vector<int> indices;
     std::vector<float> dists;
     kdtree_surf_pts_->nearestKSearch(query_pt, 1, indices, dists);
@@ -134,7 +138,6 @@ void Odometry::MatchSurfPoints(const TPointCloud& src, const TPointCloud& tgt,
       int scan_id = GetScanId(pt);
       if (scan_id > query_pt_scan_id + kNearbyScanNum) break;
       double dist_squre = DistanceSqure(query_pt, pt);
-      // AWARN_IF(scan_id != 0) << "SA" << query_pt_scan_id << ", " << scan_id;
       if (scan_id == query_pt_scan_id && dist_squre < min_dist_pt2_squre) {
         pt2_idx = i;
         min_dist_pt2_squre = dist_squre;
@@ -150,7 +153,6 @@ void Odometry::MatchSurfPoints(const TPointCloud& src, const TPointCloud& tgt,
       int scan_id = GetScanId(pt);
       if (scan_id < query_pt_scan_id - kNearbyScanNum) break;
       double dist_squre = DistanceSqure(query_pt, pt);
-      // AWARN_IF(scan_id != 0) << "SD" << query_pt_scan_id << ", " << scan_id;
       if (scan_id == query_pt_scan_id && dist_squre < min_dist_pt2_squre) {
         pt2_idx = i;
         min_dist_pt2_squre = dist_squre;
@@ -163,14 +165,29 @@ void Odometry::MatchSurfPoints(const TPointCloud& src, const TPointCloud& tgt,
     }
     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(q_pt, pt1, pt2, pt3);
     }
   }
 }
 
 void Odometry::UpdatePre(const FeaturePoints& feature) {
-  surf_pts_pre_ = feature.less_flat_surf_pts;
+  if (surf_pts_pre_ == nullptr) {
-  corn_pts_pre_ = feature.less_sharp_corner_pts;
+    surf_pts_pre_.reset(new TPointCloud);
+  }
+  if (corn_pts_pre_ == nullptr) {
+    corn_pts_pre_.reset(new TPointCloud);
+  }
+
+  surf_pts_pre_->resize(feature.less_flat_surf_pts->size());
+  for (size_t i = 0; i < feature.less_flat_surf_pts->size(); ++i) {
+    TransformToEnd(pose_curr2last_, feature.less_flat_surf_pts->points[i],
+                   &surf_pts_pre_->points[i]);
+  }
+  corn_pts_pre_->resize(feature.less_sharp_corner_pts->size());
+  for (size_t i = 0; i < feature.less_sharp_corner_pts->size(); ++i) {
+    TransformToEnd(pose_curr2last_, feature.less_sharp_corner_pts->points[i],
+                   &corn_pts_pre_->points[i]);
+  }
   kdtree_surf_pts_->setInputCloud(surf_pts_pre_);
   kdtree_corn_pts_->setInputCloud(corn_pts_pre_);
 }

src/odometry/odometry.h

@@ -29,11 +29,11 @@ class Odometry {
   Pose3D pose_curr2world_;
   Pose3D pose_curr2last_;
 
-  TPointCloudPtr surf_pts_pre_;
+  TPointCloudPtr surf_pts_pre_{nullptr};
-  TPointCloudPtr corn_pts_pre_;
+  TPointCloudPtr corn_pts_pre_{nullptr};
 
-  pcl::KdTreeFLANN<TPoint>::Ptr kdtree_surf_pts_;
+  pcl::KdTreeFLANN<TPoint>::Ptr kdtree_surf_pts_{nullptr};
-  pcl::KdTreeFLANN<TPoint>::Ptr kdtree_corn_pts_;
+  pcl::KdTreeFLANN<TPoint>::Ptr kdtree_corn_pts_{nullptr};
 
   bool is_initialized = false;
   YAML::Node config_;

src/visualizer/odometry_visualizer.cc

@@ -0,0 +1,38 @@
+#include "Odometry_visualizer.h"
+
+namespace oh_my_loam {
+
+void OdometryVisualizer::Draw() {
+  auto frame = GetCurrentFrame<OdometryVisFrame>();
+  {  // add raw point cloud
+    std::string id = "raw point cloud";
+    DrawPointCloud(*frame.cloud, WHITE, id, viewer_.get());
+    rendered_cloud_ids_.push_back(id);
+  }
+  {  // add less_flat_surf_pts
+    std::string id = "less_flat_surf_pts";
+    DrawPointCloud(*frame.feature_pts.less_flat_surf_pts, CYAN, id,
+                   viewer_.get(), 5);
+    rendered_cloud_ids_.push_back(id);
+  }
+  {  // add flat_surf_pts
+    std::string id = "flat_surf_pts";
+    DrawPointCloud(*frame.feature_pts.flat_surf_pts, BLUE, id, viewer_.get(),
+                   7);
+    rendered_cloud_ids_.push_back(id);
+  }
+  {  // add less_sharp_corner_pts
+    std::string id = "less_sharp_corner_pts";
+    DrawPointCloud(*frame.feature_pts.less_sharp_corner_pts, PURPLE, id,
+                   viewer_.get(), 5);
+    rendered_cloud_ids_.push_back(id);
+  }
+  {  // add sharp_corner_pts
+    std::string id = "sharp_corner_pts";
+    DrawPointCloud(*frame.feature_pts.sharp_corner_pts, RED, id, viewer_.get(),
+                   7);
+    rendered_cloud_ids_.push_back(id);
+  }
+};
+
+}  // namespace oh_my_loam

src/visualizer/odometry_visualizer.h

@@ -0,0 +1,22 @@
+#pragma once
+
+#include "base_visualizer.h"
+#include "extractor/feature_points.h"
+
+namespace oh_my_loam {
+
+struct OdometryVisFrame : public VisFrame {
+  FeaturePoints feature_pts;
+};
+
+class OdometryVisualizer : public Visualizer {
+ public:
+  explicit OdometryVisualizer(const std::string &name = "OdometryVisualizer",
+                              size_t max_history_size = 10)
+      : Visualizer(name, max_history_size) {}
+
+ private:
+  void Draw() override;
+};
+
+}  // namespace oh_my_loam