add mapper visualizer, not done

oh_my_loam/configs/config.yaml

@@ -32,7 +32,7 @@ odometer_config:
 # configs for mapper
 mapper_config:
   vis: false
-  verbose: true
+  verbose: false
   map_shape: [3, 21, 21]
   map_step: 50
   submap_shape: [1, 5, 5]

oh_my_loam/extractor/extractor.cc

@@ -75,7 +75,8 @@ void Extractor::SplitScan(const common::PointCloud &cloud,
       half_passed = true;
       yaw_diff += kTwoPi;
     }
-    double time = std::min(yaw_diff / kTwoPi, 1 - kEps) + scan_id;
+    // double time = std::min(yaw_diff / kTwoPi, 1 - kEps) + scan_id;
+    double time = 0.999999 + scan_id;
     scans->at(scan_id).push_back(
         {pt.x, pt.y, pt.z, static_cast<float>(time), std::nanf("")});
   }

oh_my_loam/mapper/mapper.cc

@@ -1,10 +1,12 @@
 #include "mapper.h"
 
+#include <memory>
 #include <mutex>
 
 #include "common/log/log.h"
 #include "common/math/fitting.h"
 #include "common/pcl/pcl_utils.h"
+#include "oh_my_loam/visualizer/mapper_visualizer.h"
 
 namespace oh_my_loam {
 
@@ -24,6 +26,7 @@ bool Mapper::Init() {
   map_step_ = config_["map_step"].as<double>();
   corn_map_.reset(new Map(map_shape_, map_step_));
   surf_map_.reset(new Map(map_shape_, map_step_));
+  if (is_vis_) visualizer_.reset(MapperVisualizer);
   return true;
 }
 
@@ -63,15 +66,13 @@ void Mapper::Run(const TPointCloudConstPtr &cloud_corn,
   // AdjustMap(cnt);
   TPointCloudPtr cloud_corn_map = corn_map_->GetSurrPoints(cnt, submap_shape_);
   TPointCloudPtr cloud_surf_map = surf_map_->GetSurrPoints(cnt, submap_shape_);
+  kdtree_corn_.setInputCloud(cloud_corn_map);
+  kdtree_surf_.setInputCloud(cloud_surf_map);
   for (int i = 0; i < config_["icp_iter_num"].as<int>(); ++i) {
-    pcl::KdTreeFLANN<TPoint> kdtree_corn;
-    kdtree_corn.setInputCloud(cloud_corn_map);
-    pcl::KdTreeFLANN<TPoint> kdtree_surf;
-    kdtree_surf.setInputCloud(cloud_surf_map);
     std::vector<PointLineCoeffPair> pl_pairs;
-    MatchCorn(kdtree_corn, cloud_corn_map, pose_curr2map, &pl_pairs);
+    MatchCorn(cloud_corn_map, pose_curr2map, &pl_pairs);
     std::vector<PointPlaneCoeffPair> pp_pairs;
-    MatchSurf(kdtree_surf, cloud_surf_map, pose_curr2map, &pp_pairs);
+    MatchSurf(cloud_surf_map, pose_curr2map, &pp_pairs);
     AINFO_IF(verbose_) << "Iter " << i
                        << ": matched num: point2line = " << pl_pairs.size()
                        << ", point2plane = " << pp_pairs.size();
@@ -102,8 +103,7 @@ void Mapper::Run(const TPointCloudConstPtr &cloud_corn,
   AUSER << "Mapper::Run: " << BLOCK_TIMER_STOP_FMT;
 }
 
-void Mapper::MatchCorn(const pcl::KdTreeFLANN<TPoint> &kdtree,
+void Mapper::MatchCorn(const TPointCloudConstPtr &cloud_curr,
-                       const TPointCloudConstPtr &cloud_curr,
                        const common::Pose3d &pose_curr2map,
                        std::vector<PointLineCoeffPair> *const pairs) const {
   std::vector<int> indices;
@@ -113,8 +113,8 @@ void Mapper::MatchCorn(const pcl::KdTreeFLANN<TPoint> &kdtree,
       config_["neighbor_point_dist_sq_th"].as<float>();
   for (const auto &pt : *cloud_curr) {
     TPoint pt_query = common::TransformPoint(pose_curr2map, pt);
-    if (kdtree.nearestKSearch(pt_query, nearest_neighbor_k, indices, dists) !=
+    if (kdtree_corn_.nearestKSearch(pt_query, nearest_neighbor_k, indices,
-        nearest_neighbor_k) {
+                                    dists) != nearest_neighbor_k) {
       continue;
     }
     if (dists.back() > neighbor_point_dist_sq_th) continue;
@@ -127,8 +127,7 @@ void Mapper::MatchCorn(const pcl::KdTreeFLANN<TPoint> &kdtree,
   }
 }
 
-void Mapper::MatchSurf(const pcl::KdTreeFLANN<TPoint> &kdtree,
+void Mapper::MatchSurf(const TPointCloudConstPtr &cloud_curr,
-                       const TPointCloudConstPtr &cloud_curr,
                        const common::Pose3d &pose_curr2map,
                        std::vector<PointPlaneCoeffPair> *const pairs) const {
   std::vector<int> indices;
@@ -138,8 +137,8 @@ void Mapper::MatchSurf(const pcl::KdTreeFLANN<TPoint> &kdtree,
       config_["neighbor_point_dist_sq_th"].as<float>();
   for (const auto &pt : *cloud_curr) {
     TPoint pt_query = common::TransformPoint(pose_curr2map, pt);
-    if (kdtree.nearestKSearch(pt_query, nearest_neighbor_k, indices, dists) !=
+    if (kdtree_surf_.nearestKSearch(pt_query, nearest_neighbor_k, indices,
-        nearest_neighbor_k) {
+                                    dists) != nearest_neighbor_k) {
       continue;
     }
     if (dists.back() > neighbor_point_dist_sq_th) continue;
@@ -230,4 +229,17 @@ void Mapper::SetState(State state) {
   state_ = state;
 }
 
+void Mapper::Visualize(const std::vector<PointLineCoeffPair> &pl_pairs,
+                       const std::vector<PointPlaneCoeffPair> &pp_pairs,
+                       const common::Pose3d &pose_curr2odom,
+                       const common::Pose3d &pose_curr2map, double timestamp) {
+  std::shared_ptr<MapperVisFrame> frame(new MapperVisFrame);
+  frame->timestamp = timestamp;
+  frame->cloud_corn = kdtree_corn_.getInputCloud()->makeShared();
+  frame->cloud_surf = kdtree_surf_.getInputCloud()->makeShared();
+  frame->pl_pairs = pl_pairs;
+  frame->pp_pairs = pp_pairs;
+  frame->pose_curr2world
+}
+
 }  // namespace oh_my_loam

oh_my_loam/mapper/mapper.h

@@ -10,7 +10,9 @@
 #include "oh_my_loam/base/types.h"
 #include "oh_my_loam/base/utils.h"
 #include "oh_my_loam/mapper/map.h"
+#include "oh_my_loam/solver/cost_function.h"
 #include "oh_my_loam/solver/solver.h"
+#include "oh_my_loam/visualizer/mapper_visualizer.h"
 
 namespace oh_my_loam {
 
@@ -40,13 +42,11 @@ class Mapper {
            const TPointCloudConstPtr &cloud_surf,
            const common::Pose3d &pose_init);
 
-  void MatchCorn(const pcl::KdTreeFLANN<TPoint> &kdtree,
+  void MatchCorn(const TPointCloudConstPtr &cloud_curr,
-                 const TPointCloudConstPtr &cloud_curr,
                  const common::Pose3d &pose_curr2map,
                  std::vector<PointLineCoeffPair> *const pairs) const;
 
-  void MatchSurf(const pcl::KdTreeFLANN<TPoint> &kdtree,
+  void MatchSurf(const TPointCloudConstPtr &cloud_curr,
-                 const TPointCloudConstPtr &cloud_curr,
                  const common::Pose3d &pose_curr2map,
                  std::vector<PointPlaneCoeffPair> *const pairs) const;
 
@@ -60,10 +60,17 @@ class Mapper {
                  const TPointCloudConstPtr &cloud_corn = nullptr,
                  const TPointCloudConstPtr &cloud_surf = nullptr);
 
+  void Visualize(const std::vector<PointLineCoeffPair> &pl_pairs,
+                 const std::vector<PointPlaneCoeffPair> &pp_pairs,
+                 const common::Pose3d &pose_curr2odom,
+                 const common::Pose3d &pose_curr2map, double timestamp = 0.0);
+
   // map
   mutable std::mutex map_mutex_;
   std::unique_ptr<Map> corn_map_;
   std::unique_ptr<Map> surf_map_;
+  pcl::KdTreeFLANN<TPoint> kdtree_corn_;
+  pcl::KdTreeFLANN<TPoint> kdtree_surf_;
   // state
   mutable std::mutex state_mutex_;
   State state_ = UN_INIT;
@@ -77,6 +84,8 @@ class Mapper {
   bool is_vis_ = false;
   bool verbose_ = false;
 
+  std::unique_ptr<MapperVisualizer> visualizer_{nullptr};
+
   DISALLOW_COPY_AND_ASSIGN(Mapper)
 };
 

oh_my_loam/odometer/odometer.cc

@@ -54,10 +54,10 @@ void Odometer::Process(double timestamp, const std::vector<Feature> &features,
     }
     PoseSolver solver(pose_curr2last_);
     for (const auto &pair : pl_pairs) {
-      solver.AddPointLinePair(pair, GetTime(pair.pt));
+      solver.AddPointLinePair(pair, 1.0);
     }
     for (const auto &pair : pp_pairs) {
-      solver.AddPointPlanePair(pair, GetTime(pair.pt));
+      solver.AddPointPlanePair(pair, 1.0);
     }
     bool is_converge = solver.Solve(config_["solve_iter_num"].as<int>(),
                                     verbose_, &pose_curr2last_);
@@ -190,8 +190,8 @@ void Odometer::Visualize(const std::vector<PointLinePair> &pl_pairs,
                          double timestamp) const {
   std::shared_ptr<OdometerVisFrame> frame(new OdometerVisFrame);
   frame->timestamp = timestamp;
-  frame->cloud_corn = kdtree_corn_.getInputCloud();
+  frame->cloud_corn = kdtree_corn_.getInputCloud()->makeShared();
-  frame->cloud_surf = kdtree_surf_.getInputCloud();
+  frame->cloud_surf = kdtree_surf_.getInputCloud()->makeShared();
   frame->pl_pairs = pl_pairs;
   frame->pp_pairs = pp_pairs;
   frame->pose_curr2last = pose_curr2last_;

oh_my_loam/visualizer/mapper_visualizer.cc

@@ -0,0 +1,116 @@
+#include "mapper_visualizer.h"
+
+#include "common/color/color.h"
+
+namespace oh_my_loam {
+
+namespace {
+using namespace common;
+}  // namespace
+
+void MapperVisualizer::Draw() {
+  auto frame = GetCurrentFrame<MapperVisFrame>();
+  DrawPointCloud<TPoint>(frame.cloud_corn, GRAY, "cloud_corn");
+  DrawPointCloud<TPoint>(frame.cloud_surf, GRAY, "cloud_surf");
+
+  DrawCorn(frame.pose_curr2odom, frame.pl_pairs);
+  DrawSurf(frame.pose_curr2odom, frame.pp_pairs);
+
+  poses_.push_back(frame.pose_curr2map);
+  DrawTrajectory();
+}
+
+void MapperVisualizer::DrawCorn(const Pose3d &pose_curr2odom,
+                                const Pose3d &pose_curr2map,
+                                const std::vector<PointLineCoeffPair> &pairs) {
+  TPointCloudPtr src(new TPointCloud);
+  TPointCloudPtr tgt(new TPointCloud);
+  src->resize(pairs.size());
+  tgt->resize(pairs.size() * 2);
+  for (size_t i = 0; i < pairs.size(); ++i) {
+    const auto &pair = pairs[i];
+    src->at(i) = pair.pt;
+    if (trans_) TransformToStart(pose, pair.pt, &src->points[i]);
+  }
+  DrawPointCloud<TPoint>(tgt, YELLOW, "tgt_corn", 4);
+  DrawPointCloud<TPoint>(src, RED, "src_corn", 4);
+  for (size_t i = 0; i < src->size(); ++i) {
+    const auto &p = src->at(i), &p1 = tgt->at(2 * i), &p2 = tgt->at(2 * i + 1);
+    common::AddLine<TPoint>(p, p1, WHITE, "corn_line1_" + std::to_string(i),
+                            viewer_.get());
+    common::AddLine<TPoint>(p, p2, WHITE, "corn_line2_" + std::to_string(i),
+                            viewer_.get());
+  }
+}
+
+void MapperVisualizer::DrawSurf(const Pose3d &pose_curr2odom,
+                                const Pose3d &pose_curr2map,
+                                const std::vector<PointPlaneCoeffPair> &pairs) {
+  TPointCloudPtr src(new TPointCloud);
+  TPointCloudPtr tgt(new TPointCloud);
+  src->resize(pairs.size());
+  tgt->resize(pairs.size() * 3);
+  for (size_t i = 0; i < pairs.size(); ++i) {
+    const auto &pair = pairs[i];
+    src->at(i) = pair.pt;
+    if (trans_) TransformToStart(pose, pair.pt, &src->points[i]);
+    // tgt->at(3 * i) = pair.plane.pt1;
+    // tgt->at(3 * i + 1) = pair.plane.pt2;
+    // tgt->at(3 * i + 2) = pair.plane.pt3;
+  }
+  DrawPointCloud<TPoint>(tgt, BLUE, "tgt_surf", 4);
+  DrawPointCloud<TPoint>(src, CYAN, "src_surf", 4);
+  for (size_t i = 0; i < src->size(); ++i) {
+    const auto &p = src->at(i), &p1 = tgt->at(3 * i), &p2 = tgt->at(3 * i + 1),
+               &p3 = tgt->at(3 * i + 2);
+    AddLine<TPoint>(p, p1, WHITE, "surf_line1_" + std::to_string(i),
+                    viewer_.get());
+    AddLine<TPoint>(p, p2, WHITE, "surf_line2_" + std::to_string(i),
+                    viewer_.get());
+    AddLine<TPoint>(p, p3, WHITE, "surf_line3_" + std::to_string(i),
+                    viewer_.get());
+  }
+}
+
+void MapperVisualizer::DrawTrajectory() {
+  std::vector<Pose3d> poses_n;
+  poses_n.reserve((poses_.size()));
+  Pose3d pose_inv = poses_.back().Inv();
+  for (const auto &pose : poses_) {
+    poses_n.emplace_back(pose_inv * pose);
+  };
+  for (size_t i = 0; i < poses_n.size() - 1; ++i) {
+    Eigen::Vector3f p1 = poses_n[i].t_vec().cast<float>();
+    Eigen::Vector3f p2 = poses_n[i + 1].t_vec().cast<float>();
+    AddLine<Point>({p1.x(), p1.y(), p1.z()}, {p2.x(), p2.y(), p2.z()}, PINK,
+                   "trajectory" + std::to_string(i), viewer_.get());
+  }
+}
+
+void MapperVisualizer::KeyboardEventCallback(
+    const pcl::visualization::KeyboardEvent &event) {
+  if (event.getKeySym() == "p" && event.keyDown()) {
+    std::lock_guard<std::mutex> lock(mutex_);
+    ++curr_frame_iter_;
+    if (curr_frame_iter_ == history_frames_.end()) {
+      --curr_frame_iter_;
+    }
+    is_updated_ = true;
+  } else if (event.getKeySym() == "n" && event.keyDown()) {
+    std::lock_guard<std::mutex> lock(mutex_);
+    if (curr_frame_iter_ != history_frames_.begin()) {
+      --curr_frame_iter_;
+    }
+    is_updated_ = true;
+  } else if (event.getKeySym() == "t" && event.keyDown()) {
+    trans_ = !trans_;
+    is_updated_ = true;
+  } else if (event.getKeySym() == "r" && event.keyDown()) {
+    viewer_->setCameraPosition(0, 0, 200, 0, 0, 0, 1, 0, 0, 0);
+    viewer_->setSize(2500, 1500);
+    trans_ = true;
+    is_updated_ = true;
+  }
+}
+
+}  // namespace oh_my_loam

oh_my_loam/visualizer/mapper_visualizer.h

@@ -0,0 +1,42 @@
+#pragma once
+
+#include "common/visualizer/lidar_visualizer.h"
+#include "oh_my_loam/solver/cost_function.h"
+
+namespace oh_my_loam {
+
+struct MapperVisFrame : public common::LidarVisFrame {
+  TPointCloudConstPtr cloud_surf;
+  TPointCloudConstPtr cloud_corn;
+  std::vector<PointLineCoeffPair> pl_pairs;
+  std::vector<PointPlaneCoeffPair> pp_pairs;
+  common::Pose3d pose_curr2odom;
+  common::Pose3d pose_curr2map;
+};
+
+class MapperVisualizer : public common::LidarVisualizer {
+ public:
+  explicit MapperVisualizer(const std::string &name = "MapperVisualizer",
+                            size_t max_history_size = 10)
+      : common::LidarVisualizer(name, max_history_size) {}
+
+ private:
+  void Draw() override;
+
+  void DrawCorn(const common::Pose3d &pose,
+                const std::vector<PointLineCoeffPair> &pairs);
+
+  void DrawSurf(const common::Pose3d &pose,
+                const std::vector<PointPlaneCoeffPair> &pairs);
+
+  void DrawTrajectory();
+
+  void KeyboardEventCallback(
+      const pcl::visualization::KeyboardEvent &event) override;
+
+  bool trans_ = true;
+
+  std::vector<common::Pose3d> poses_;
+};
+
+}  // namespace oh_my_loam