odometry visualizer: ok

CMakeLists.txt

@@ -57,9 +57,9 @@ target_link_libraries(main
   common
   oh_my_loam
   extractor
-  visualizer
   odometry
   solver
   ${CERES_LIBRARIES}
   helper
+  visualizer
 )

common/log.h

@@ -6,8 +6,8 @@
 #include <g3log/logworker.hpp>
 #include <iostream>
 
-// heere we define G3LOG instead of use LOG directly, since LOG macro in g3log
+// here 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
+// conflicts LOG macro in glog. Same for G3LOG_IF and G3CHECK
 #define G3LOG(level)          \
   if (!g3::logLevel(level)) { \
   } else                      \

configs/config.yaml

@@ -7,7 +7,7 @@ vis: true
 # configs for feature points extractor
 extractor_config:
   min_point_num: 66
-  vis: true
+  vis: false
   sharp_corner_point_num: 2
   corner_point_num: 20
   flat_surf_point_num: 4
@@ -20,4 +20,4 @@ extractor_config:
 odometry_config: 
   icp_iter_num : 2
   match_dist_sq_thresh: 1
-  vis: false
+  vis: true

src/odometry/odometry.cc

@@ -13,12 +13,15 @@ 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>);
+  is_vis_ = Config::Instance()->Get<bool>("vis") && config_["vis"].as<bool>();
+  AINFO << "Odometry visualizer: " << (is_vis_ ? "ON" : "OFF");
+  if (is_vis_) visualizer_.reset(new OdometryVisualizer);
   return true;
 }
 
 void Odometry::Process(const FeaturePoints& feature, Pose3D* const pose) {
-  if (!is_initialized) {
+  if (!is_initialized_) {
-    is_initialized = true;
+    is_initialized_ = true;
     UpdatePre(feature);
     *pose = pose_curr2world_;
     AWARN << "Odometry initialized....";
@@ -27,10 +30,10 @@ void Odometry::Process(const FeaturePoints& feature, Pose3D* const pose) {
   TicToc timer_whole;
   double match_dist_sq_thresh = config_["match_dist_sq_thresh"].as<double>();
   AINFO << "Pose before: " << pose_curr2world_.ToString();
-  for (int i = 0; i < config_["icp_iter_num"].as<int>(); ++i) {
-    TicToc timer;
   std::vector<PointLinePair> pl_pairs;
   std::vector<PointPlanePair> pp_pairs;
+  for (int i = 0; i < config_["icp_iter_num"].as<int>(); ++i) {
+    TicToc timer;
     MatchCornPoints(*feature.sharp_corner_pts, *corn_pts_pre_, &pl_pairs,
                     match_dist_sq_thresh);
     double time_pl_match = timer.toc();
@@ -73,6 +76,7 @@ void Odometry::Process(const FeaturePoints& feature, Pose3D* const pose) {
   AWARN << "Pose after: " << pose_curr2world_.ToString();
   UpdatePre(feature);
   AUSER << "Time elapsed (ms): whole odometry = " << timer_whole.toc();
+  if (is_vis_) Visualize(feature, pl_pairs, pp_pairs);
 }
 
 void Odometry::MatchCornPoints(const TPointCloud& src, const TPointCloud& tgt,
@@ -192,4 +196,15 @@ void Odometry::UpdatePre(const FeaturePoints& feature) {
   kdtree_corn_pts_->setInputCloud(corn_pts_pre_);
 }
 
+void Odometry::Visualize(const FeaturePoints& feature,
+                         const std::vector<PointLinePair>& pl_pairs,
+                         const std::vector<PointPlanePair>& pp_pairs) const {
+  std::shared_ptr<OdometryVisFrame> frame(new OdometryVisFrame);
+  frame->pl_pairs = pl_pairs;
+  frame->pp_pairs = pp_pairs;
+  frame->pose_curr2last = pose_curr2last_;
+  frame->pose_curr2world = pose_curr2world_;
+  visualizer_->Render(frame);
+}
+
 }  // namespace oh_my_loam

src/odometry/odometry.h

@@ -1,6 +1,7 @@
 #pragma once
 
-#include <extractor/feature_points.h>
+#include "extractor/feature_points.h"
+#include "visualizer/odometry_visualizer.h"
 
 #include "common.h"
 #include "helper/helper.h"
@@ -18,6 +19,7 @@ class Odometry {
 
  protected:
   void UpdatePre(const FeaturePoints& feature);
+
   void MatchCornPoints(const TPointCloud& src, const TPointCloud& tgt,
                        std::vector<PointLinePair>* const pairs,
                        double dist_sq_thresh) const;
@@ -26,6 +28,10 @@ class Odometry {
                        std::vector<PointPlanePair>* const pairs,
                        double dist_sq_thresh) const;
 
+  void Visualize(const FeaturePoints& feature,
+                 const std::vector<PointLinePair>& pl_pairs,
+                 const std::vector<PointPlanePair>& pp_pairs) const;
+
   Pose3D pose_curr2world_;
   Pose3D pose_curr2last_;
 
@@ -35,9 +41,12 @@ class Odometry {
   pcl::KdTreeFLANN<TPoint>::Ptr kdtree_surf_pts_{nullptr};
   pcl::KdTreeFLANN<TPoint>::Ptr kdtree_corn_pts_{nullptr};
 
-  bool is_initialized = false;
+  bool is_initialized_ = false;
+  bool is_vis_ = false;
   YAML::Node config_;
 
+  std::unique_ptr<OdometryVisualizer> visualizer_{nullptr};
+
   DISALLOW_COPY_AND_ASSIGN(Odometry)
 };
 

src/visualizer/odometry_visualizer.cc

@@ -1,38 +1,66 @@
-#include "Odometry_visualizer.h"
+#include "odometry_visualizer.h"
 
 namespace oh_my_loam {
 
 void OdometryVisualizer::Draw() {
   auto frame = GetCurrentFrame<OdometryVisFrame>();
-  {  // add raw point cloud
+  TPointCloud src_corn_pts, tgt_corn_pts;
-    std::string id = "raw point cloud";
+  src_corn_pts.resize(frame.pl_pairs.size());
-    DrawPointCloud(*frame.cloud, WHITE, id, viewer_.get());
+  tgt_corn_pts.resize(frame.pl_pairs.size() * 2);
+  for (size_t i = 0; i < frame.pl_pairs.size(); ++i) {
+    const auto& pair = frame.pl_pairs[i];
+    TransformToStart(frame.pose_curr2last, pair.pt, &src_corn_pts[i]);
+    tgt_corn_pts[2 * i] = pair.line.pt1;
+    tgt_corn_pts[2 * i + 1] = pair.line.pt2;
+  }
+  TPointCloud src_surf_pts, tgt_surf_pts;
+  src_surf_pts.resize(frame.pp_pairs.size());
+  tgt_surf_pts.resize(frame.pp_pairs.size() * 3);
+  for (size_t i = 0; i < frame.pp_pairs.size(); ++i) {
+    const auto& pair = frame.pp_pairs[i];
+    TransformToStart(frame.pose_curr2last, pair.pt, &src_surf_pts[i]);
+    tgt_surf_pts[3 * i] = pair.plane.pt1;
+    tgt_surf_pts[3 * i + 1] = pair.plane.pt2;
+    tgt_surf_pts[3 * i + 2] = pair.plane.pt3;
+  }
+  {  // add src_corn_pts
+    std::string id = "src_corn_pts";
+    DrawPointCloud(src_corn_pts, CYAN, id, viewer_.get(), 7);
     rendered_cloud_ids_.push_back(id);
   }
-  {  // add less_flat_surf_pts
+  {  // add tgt_corn_pts
-    std::string id = "less_flat_surf_pts";
+    std::string id = "tgt_corn_pts";
-    DrawPointCloud(*frame.feature_pts.less_flat_surf_pts, CYAN, id,
+    DrawPointCloud(tgt_corn_pts, BLUE, id, viewer_.get(), 4);
-                   viewer_.get(), 5);
     rendered_cloud_ids_.push_back(id);
   }
-  {  // add flat_surf_pts
+  {  // add src_surf_pts
-    std::string id = "flat_surf_pts";
+    std::string id = "src_surf_pts";
-    DrawPointCloud(*frame.feature_pts.flat_surf_pts, BLUE, id, viewer_.get(),
+    DrawPointCloud(src_surf_pts, PURPLE, id, viewer_.get(), 7);
-                   7);
     rendered_cloud_ids_.push_back(id);
   }
-  {  // add less_sharp_corner_pts
+  {  // add tgt_surf_pts
-    std::string id = "less_sharp_corner_pts";
+    std::string id = "tgt_surf_pts";
-    DrawPointCloud(*frame.feature_pts.less_sharp_corner_pts, PURPLE, id,
+    DrawPointCloud(tgt_surf_pts, RED, id, viewer_.get(), 4);
-                   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);
   }
+  std::vector<Pose3D> poses_n;
+  poses_n.reserve((poses_.size()));
+  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>();
+    if (i < poses_n.size() - 1) {
+      Eigen::Vector3f p2 = poses_n[i + 1].p().cast<float>();
+      DrawLine(Point(p1.x(), p1.y(), p1.z()), Point(p2.x(), p2.y(), p2.z()),
+               WHITE, "line" + std::to_string(i), viewer_.get());
+    } else {
+      DrawLine(Point(p1.x(), p1.y(), p1.z()), Point(0, 0, 0), WHITE,
+               "line" + std::to_string(i), viewer_.get());
+    }
+  }
+  poses_.push_back(frame.pose_curr2world);
+}
 
 }  // namespace oh_my_loam

src/visualizer/odometry_visualizer.h

@@ -1,12 +1,17 @@
 #pragma once
 
 #include "base_visualizer.h"
-#include "extractor/feature_points.h"
+#include "helper/helper.h"
 
 namespace oh_my_loam {
 
 struct OdometryVisFrame : public VisFrame {
-  FeaturePoints feature_pts;
+  TPointCloudPtr surf_pts;
+  TPointCloudPtr corn_pts;
+  std::vector<PointLinePair> pl_pairs;
+  std::vector<PointPlanePair> pp_pairs;
+  Pose3D pose_curr2last;
+  Pose3D pose_curr2world;
 };
 
 class OdometryVisualizer : public Visualizer {
@@ -17,6 +22,7 @@ class OdometryVisualizer : public Visualizer {
 
  private:
   void Draw() override;
+  std::deque<Pose3D> poses_;
 };
 
 }  // namespace oh_my_loam

src/visualizer/utils.h

@@ -28,4 +28,10 @@ void DrawPointCloud(const pcl::PointCloud<PointT>& cloud,
       pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pt_size, id);
 }
 
+template <typename PointT>
+void DrawLine(const PointT& pt1, const PointT& pt2, const Color& color,
+              const std::string& id, PCLVisualizer* const viewer) {
+  viewer->addLine(pt1, pt2, color.r, color.g, color.b, id);
+}
+
 }  // namespace oh_my_loam