mapper: coding...

CMakeLists.txt

@@ -49,7 +49,7 @@ target_link_libraries(main
   oh_my_loam
   extractor
   odometer
-  # mapper
+  mapper
   solver
   ${CERES_LIBRARIES}
   visualizer

oh_my_loam/CMakeLists.txt

@@ -2,7 +2,7 @@ add_subdirectory(base)
 add_subdirectory(extractor)
 add_subdirectory(visualizer)
 add_subdirectory(odometer)
-# add_subdirectory(mapper)
+add_subdirectory(mapper)
 add_subdirectory(solver)
 
 aux_source_directory(. SRC_FILES)

oh_my_loam/extractor/extractor_VLP16.h

@@ -1,6 +1,6 @@
 #pragma once
 
-#include "extractor.h"
+#include "oh_my_loam/extractor/extractor.h"
 
 namespace oh_my_loam {
 

oh_my_loam/mapper/mapper.cc

@@ -15,8 +15,19 @@ bool Mapper::Init() {
   return true;
 }
 
-void Mapper::Process(double timestamp, const std::vector<Feature> &features,
-                     Pose3d *const pose_out) {}
+void Mapper::Reset() {}
+
+void Mapper::Process(double timestamp, const TPointCloudConstPtr &cloud_corn,
+                     const TPointCloudConstPtr &cloud_surf,
+                     common::Pose3d *const pose_out) {
+  if (!is_initialized_) {
+    cloud_corn_map_ = cloud_corn;
+    cloud_surf_map_ = cloud_surf;
+    pose_out->SetIdentity();
+    is_initialized_ = true;
+    return;
+  }
+}
 
 void Mapper::Visualize() {}
 

oh_my_loam/mapper/mapper.h

@@ -15,11 +15,16 @@ class Mapper {
 
   bool Init();
 
-  void Process(double timestamp, const std::vector<Feature> &features,
+  void Process(double timestamp, const TPointCloudConstPtr &cloud_corn,
+               const TPointCloudConstPtr &cloud_surf,
                common::Pose3d *const pose_out);
 
-  common::PointCloudConstPtr map() const {
-    return cloud_map_;
+  TPointCloudConstPtr cloud_corn_map() const {
+    return cloud_corn_map_;
+  }
+
+  TPointCloudConstPtr cloud_surf_map() const {
+    return cloud_corn_map_;
   }
 
   void Reset();
@@ -27,13 +32,12 @@ class Mapper {
  private:
   void Visualize();
 
-  common::PointCloudPtr cloud_map_;
-
-  common::Pose3d pose_curr2world_;
+  TPointCloudPtr cloud_corn_map_;
+  TPointCloudPtr cloud_surf_map_;
 
   YAML::Node config_;
 
-  bool is_initialized = false;
+  bool is_initialized_ = false;
 
   bool is_vis_ = false;
 

oh_my_loam/odometer/odometer.cc

@@ -36,10 +36,6 @@ void Odometer::Process(double timestamp, const std::vector<Feature> &features,
     return;
   }
   BLOCK_TIMER_START;
-  const auto &cloud_corn = kdtree_corn_.getInputCloud();
-  const auto &cloud_surf = kdtree_surf_.getInputCloud();
-  AINFO_IF(verbose_) << "Points to be matched: " << cloud_corn->size() << " + "
-                     << cloud_surf->size();
   std::vector<PointPlanePair> pp_pairs;
   std::vector<PointLinePair> pl_pairs;
   for (int i = 0; i < config_["icp_iter_num"].as<int>(); ++i) {
@@ -71,13 +67,13 @@ void Odometer::Process(double timestamp, const std::vector<Feature> &features,
     AINFO_IF(verbose_) << "Odometer::ICP: iter_" << i << ": "
                        << BLOCK_TIMER_STOP_FMT;
   }
-  pose_curr2world_ = pose_curr2world_ * pose_curr2last_;
-  *pose_out = pose_curr2world_;
+  *pose_out = *pose_out * pose_curr2last_;
+  pose_curr2world_ = *pose_out;
   AINFO_IF(verbose_) << "Pose increase: " << pose_curr2last_.ToString();
-  AINFO_IF(verbose_) << "Pose after: " << pose_curr2world_.ToString();
+  // mush called before calling UpdatePre
+  if (is_vis_) Visualize(pl_pairs, pp_pairs);
   UpdatePre(features);
   AINFO << "Odometer::Porcess: " << BLOCK_TIMER_STOP_FMT;
-  if (is_vis_) Visualize(cloud_corn, cloud_surf, pl_pairs, pp_pairs);
 }
 
 void Odometer::MatchCorn(const TPointCloud &src,
@@ -191,15 +187,13 @@ void Odometer::UpdatePre(const std::vector<Feature> &features) {
   kdtree_surf_.setInputCloud(surf_pre);
 }
 
-void Odometer::Visualize(const TPointCloudConstPtr &cloud_corn,
-                         const TPointCloudConstPtr &cloud_surf,
-                         const std::vector<PointLinePair> &pl_pairs,
+void Odometer::Visualize(const std::vector<PointLinePair> &pl_pairs,
                          const std::vector<PointPlanePair> &pp_pairs,
                          double timestamp) const {
   std::shared_ptr<OdometerVisFrame> frame(new OdometerVisFrame);
   frame->timestamp = timestamp;
-  frame->cloud_corn = cloud_corn;
-  frame->cloud_surf = cloud_surf;
+  frame->cloud_corn = kdtree_corn_.getInputCloud();
+  frame->cloud_surf = kdtree_surf_.getInputCloud();
   frame->pl_pairs = pl_pairs;
   frame->pp_pairs = pp_pairs;
   frame->pose_curr2last = pose_curr2last_;

oh_my_loam/odometer/odometer.h

@@ -18,6 +18,14 @@ class Odometer {
   void Process(double timestamp, const std::vector<Feature> &features,
                common::Pose3d *const pose_out);
 
+  TPointCloudConstPtr cloud_corn() const {
+    return kdtree_corn_.getInputCloud();
+  }
+
+  TPointCloudConstPtr cloud_surf() const {
+    return kdtree_surf_.getInputCloud();
+  }
+
   void Reset();
 
  protected:
@@ -29,9 +37,7 @@ class Odometer {
   void MatchSurf(const TPointCloud &src,
                  std::vector<PointPlanePair> *const pairs) const;
 
-  void Visualize(const TPointCloudConstPtr &cloud_corn,
-                 const TPointCloudConstPtr &cloud_surf,
-                 const std::vector<PointLinePair> &pl_pairs,
+  void Visualize(const std::vector<PointLinePair> &pl_pairs,
                  const std::vector<PointPlanePair> &pp_pairs,
                  double timestamp = 0.0) const;
 

oh_my_loam/oh_my_loam.cc

@@ -33,11 +33,12 @@ bool OhMyLoam::Init() {
 }
 
 void OhMyLoam::Reset() {
-  timestamp_last_ = timestamp_last_mapping_ = 0.0;
   extractor_->Reset();
   odometer_->Reset();
   mapper_->Reset();
-  std::vector<common::Pose3d>().swap(poses_);
+  std::vector<TimePose>().swap(poses_curr2world_);
+  std::lock_guard<std::mutex> lock(mutex_);
+  pose_mapping_updated_ = true;
 }
 
 void OhMyLoam::Run(double timestamp,
@@ -46,13 +47,41 @@ void OhMyLoam::Run(double timestamp,
   RemoveOutliers(*cloud_in, cloud.get());
   std::vector<Feature> features;
   extractor_->Process(timestamp, cloud, &features);
-  Pose3d pose;
-  odometer_->Process(timestamp, features, &pose);
-  poses_.emplace_back(pose);
-  if (!IsMapping(timestamp)) return;
-  mapper_->Process();
+  FusionOdometryMapping();
+  auto pose_odom =
+      poses_curr2world_.empty() ? TimePose() : poses_curr2world_.back();
+  odometer_->Process(timestamp, features, &pose_odom.pose);
+  poses_curr2world_.push_back(pose_odom);
+  const auto &cloud_corn = odometer_->cloud_corn();
+  const auto &cloud_surf = odometer_->cloud_surf();
+
+  if (!pose_mapping_updated_) return;
+  mapping_thread_.reset(new std::thread(&OhMyLoam::MappingProcess, this,
+                                        timestamp, cloud_corn, cloud_surf));
+  if (mapping_thread_->joinable()) mapping_thread_->detach();
 }
 
+void OhMyLoam::MappingProcess(double timestamp,
+                              const TPointCloudConstPtr &cloud_corn,
+                              const TPointCloudConstPtr &cloud_surf) {
+  TimePose pose;
+  pose.timestamp = timestamp;
+  mapper_->Process(timestamp, cloud_corn, cloud_corn, &pose.pose);
+  std::lock_guard<std::mutex> lock(mutex_);
+  pose_mapping_ = pose;
+  pose_mapping_updated_ = true;
+}
+
+void OhMyLoam::FusionOdometryMapping() {
+  std::lock_guard<std::mutex> lock(mutex_);
+  TimePose pose_m = pose_mapping_;
+  pose_mapping_updated_ = false;
+  for (;;) {
+  }
+}
+
+void OhMyLoam::Visualize(double timestamp) {}
+
 void OhMyLoam::RemoveOutliers(const common::PointCloud &cloud_in,
                               common::PointCloud *const cloud_out) const {
   common::RemovePoints<common::Point>(cloud_in, cloud_out, [&](const auto &pt) {
@@ -62,9 +91,4 @@ void OhMyLoam::RemoveOutliers(const common::PointCloud &cloud_in,
   });
 }
 
-bool OhMyLoam::IsMapping(double timestamp) const {
-  return std::abs(timestamp - timestamp_last_mapping_) >=
-         config_["mapper_config"]["process_period"].as<double>();
-}
-
 }  // namespace oh_my_loam

oh_my_loam/oh_my_loam.h

@@ -20,7 +20,12 @@ class OhMyLoam {
  private:
   void Reset();
 
-  bool IsMapping(double timestamp) const;
+  void FusionOdometryMapping();
+
+  void MappingProcess(double timestamp, const TPointCloudConstPtr &cloud_corn,
+                      const TPointCloudConstPtr &cloud_surf);
+
+  void Visualize(double timestamp = 0.0);
 
   std::unique_ptr<Extractor> extractor_{nullptr};
 
@@ -32,12 +37,20 @@ class OhMyLoam {
   void RemoveOutliers(const common::PointCloud &cloud_in,
                       common::PointCloud *const cloud_out) const;
 
-  std::vector<common::Pose3d> poses_;
+  struct TimePose {
+    double timestamp;
+    common::Pose3d pose;
+  };
+
+  std::vector<TimePose> poses_curr2world_;
+
+  std::mutex mutex_;
+  TimePose pose_mapping_;
+  bool pose_mapping_updated_ = true;
+  std::unique_ptr<std::thread> mapping_thread_{nullptr};
 
   YAML::Node config_;
 
-  double timestamp_last_ = 0.0, timestamp_last_mapping_ = 0.0;
-
   DISALLOW_COPY_AND_ASSIGN(OhMyLoam)
 };
 

oh_my_loam/visualizer/ohmyloam_visualizer.cc

@@ -0,0 +1,39 @@
+#include "ohmyloam_visualizer.h"
+
+#include "common/color/color.h"
+
+namespace oh_my_loam {
+
+namespace {
+using namespace common;
+#define LIGHT_BLUE common::Color(0, 0, 80)
+#define LIGHT_YELLOW common::Color(80, 80, 0)
+}  // namespace
+
+void OhmyloamVisualizer::Draw() {
+  auto frame = GetCurrentFrame<OhmyloamVisFrame>();
+  DrawPointCloud<TPoint>(frame.cloud_map_corn, LIGHT_YELLOW, "cloud_map_corn");
+  DrawPointCloud<TPoint>(frame.cloud_map_surf, LIGHT_BLUE, "cloud_map_surf");
+
+  DrawPointCloud<TPoint>(frame.cloud_corn, RED, "cloud_corn");
+  DrawPointCloud<TPoint>(frame.cloud_surf, CYAN, "cloud_surf");
+
+  DrawTrajectory(frame.poses);
+}
+
+void OhmyloamVisualizer::DrawTrajectory(std::vector<common::Pose3d> &poses) {
+  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());
+  }
+}
+
+}  // namespace oh_my_loam

oh_my_loam/visualizer/ohmyloam_visualizer.h

@@ -0,0 +1,29 @@
+#pragma once
+
+#include "common/geometry/pose3d.h"
+#include "common/visualizer/lidar_visualizer.h"
+#include "oh_my_loam/base/types.h"
+
+namespace oh_my_loam {
+
+struct OhmyloamVisFrame : public common::LidarVisFrame {
+  TPointCloudConstPtr cloud_map_corn;
+  TPointCloudConstPtr cloud_map_surf;
+  TPointCloudConstPtr cloud_corn;  // current
+  TPointCloudConstPtr cloud_surf;  // current
+  std::vector<common::Pose3d> poses;
+};
+
+class OhmyloamVisualizer : public common::LidarVisualizer {
+ public:
+  explicit OhmyloamVisualizer(const std::string &name = "OhmyloamVisualizer",
+                              size_t max_history_size = 10)
+      : common::LidarVisualizer(name, max_history_size) {}
+
+ private:
+  void Draw() override;
+
+  void DrawTrajectory(std::vector<common::Pose3d> &poses);
+};
+
+}  // namespace oh_my_loam