From bca105b544c7893a1bd92d343823a557ed9f2540 Mon Sep 17 00:00:00 2001
From: feixyz10 <fei-china@139.com>
Date: Wed, 27 Jan 2021 00:48:27 +0800
Subject: [PATCH] mapper: coding....

---
 oh_my_loam/mapper/mapper.cc | 17 +++++++++++++++--
 oh_my_loam/mapper/mapper.h  | 36 +++++++++++++++++++++++++++++++++++-
 oh_my_loam/oh_my_loam.cc    | 11 -----------
 oh_my_loam/oh_my_loam.h     | 15 +--------------
 4 files changed, 51 insertions(+), 28 deletions(-)

diff --git a/oh_my_loam/mapper/mapper.cc b/oh_my_loam/mapper/mapper.cc
index 8baa37f..6effbe1 100644
--- a/oh_my_loam/mapper/mapper.cc
+++ b/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() {}
diff --git a/oh_my_loam/mapper/mapper.h b/oh_my_loam/mapper/mapper.h
index c7e40b7..4492413 100644
--- a/oh_my_loam/mapper/mapper.h
+++ b/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;
 
diff --git a/oh_my_loam/oh_my_loam.cc b/oh_my_loam/oh_my_loam.cc
index be83011..a4464f7 100644
--- a/oh_my_loam/oh_my_loam.cc
+++ b/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_;
diff --git a/oh_my_loam/oh_my_loam.h b/oh_my_loam/oh_my_loam.h
index 38ee88c..40ee88d 100644
--- a/oh_my_loam/oh_my_loam.h
+++ b/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_;