mapper: coding....

oh_my_loam/mapper/mapper.cc

@@ -1,5 +1,7 @@
 #include "mapper.h"
 
+#include <mutex>
+
 namespace oh_my_loam {
 
 namespace {
@@ -20,13 +22,24 @@ void Mapper::Reset() {}
 void Mapper::Process(double timestamp, const TPointCloudConstPtr &cloud_corn,
                      const TPointCloudConstPtr &cloud_surf,
                      common::Pose3d *const pose_out) {
-  if (!is_initialized_) {
+  if (GetState() == UN_INIT) {
     cloud_corn_map_ = cloud_corn;
     cloud_surf_map_ = cloud_surf;
     pose_out->SetIdentity();
-    is_initialized_ = true;
+    SetState(DONE);
     return;
   }
+  if (GetState() == DONE) {
+    Update();
+  } else {  // RUNNING
+  }
+}
+
+void Mapper::Run(double timestamp, const TPointCloudConstPtr &cloud_corn,
+                 const TPointCloudConstPtr &cloud_surf) {
+  TimePose pose;
+  pose.timestamp = timestamp;
+  std::lock_guard<std::mutex> lock(mutex_);
 }
 
 void Mapper::Visualize() {}

oh_my_loam/mapper/mapper.h

@@ -1,5 +1,8 @@
 #pragma once
 
+#include <sys/stat.h>
+
+#include <mutex>
 #include <vector>
 
 #include "common/common.h"
@@ -30,14 +33,45 @@ class Mapper {
   void Reset();
 
  private:
+  enum State { DONE, RUNNING, UN_INIT };
+
+  void Run(double timestamp, const TPointCloudConstPtr &cloud_corn,
+           const TPointCloudConstPtr &cloud_surf);
+
+  void TransformUpdate();
+
+  void MapUpdate();
+
+  State GetState() {
+    std::lock_guard<std::mutex> lock(mutex_);
+    return state_;
+  }
+
+  void SetState(State state) {
+    std::lock_guard<std::mutex> lock(mutex_);
+    state_ = state;
+  }
+
   void Visualize();
 
   TPointCloudPtr cloud_corn_map_;
   TPointCloudPtr cloud_surf_map_;
 
+  std::vector<std::vector<TPointCloudPtr>> cloud_sub_map_;
+
   YAML::Node config_;
 
-  bool is_initialized_ = false;
+  struct TimePose {
+    double timestamp;
+    common::Pose3d pose;
+  };
+
+  std::mutex mutex_;
+  std::vector<TimePose> poses_;
+
+  State state_ = UN_INIT;
+
+  std::unique_ptr<std::thread> thread_{nullptr};
 
   bool is_vis_ = false;
 

oh_my_loam/oh_my_loam.cc

@@ -61,17 +61,6 @@ void OhMyLoam::Run(double timestamp,
   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_;

oh_my_loam/oh_my_loam.h

@@ -22,9 +22,6 @@ class OhMyLoam {
 
   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};
@@ -37,17 +34,7 @@ class OhMyLoam {
   void RemoveOutliers(const common::PointCloud &cloud_in,
                       common::PointCloud *const cloud_out) const;
 
-  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};
+  std::vector<common::Pose3d> poses_curr2world_;
 
   YAML::Node config_;