map ok

common/config/yaml_config.h

@@ -27,6 +27,26 @@ class YAMLConfig {
     return *config_;
   }
 
+  template <typename T>
+  static const std::vector<T> GetSeq(const YAML::Node &node) {
+    ACHECK(node.IsSequence()) << "Not sequence.";
+    std::vector<T> seq;
+    for (auto it = node.begin(); it != node.end(); ++it) {
+      seq.push_back(it->as<T>());
+    }
+    return seq;
+  }
+
+  template <typename TK, typename TV>
+  static const std::map<TK, TV> GetMap(const YAML::Node &node) {
+    ACHECK(node.IsMap()) << "Not sequence.";
+    std::map<TK, TV> map;
+    for (auto it = node.begin(); it != node.end(); ++it) {
+      map.insert({it->first.as<TK>(), it->second.as<TV>()});
+    }
+    return map;
+  }
+
  private:
   std::unique_ptr<YAML::Node> config_{nullptr};
 

oh_my_loam/configs/config.yaml

@@ -33,4 +33,5 @@ odometer_config:
 mapper_config:
   vis: false
   verbose: false
-  process_period: 0.5
+  map_shape: [11, 21, 21]
+  map_step_: 50 # meter

oh_my_loam/mapper/map.cc

@@ -1,20 +1,201 @@
 #include "oh_my_loam/mapper/map.h"
 
-#include "common/log/log.h"
+#include "common/pcl/pcl_utils.h"
 
 namespace oh_my_loam {
 
+Map::Map(const std::vector<int> &shape, const std::vector<double> &step) {
+  ACHECK(shape.size() == 3);
+  std::copy_n(shape.begin(), 3, shape_);
+  ACHECK(shape_[0] % 2 == 1 && shape_[1] % 2 == 1 && shape_[2] % 2 == 1);
+  std::copy_n(step.begin(), 3, step_);
+  ACHECK(step_[0] > 0.0 && step_[1] > 0.0 && step_[2] > 0.0);
+  center_[0] = shape_[0] / 2;
+  center_[1] = shape_[1] / 2;
+  center_[2] = shape_[2] / 2;
+  for (int i = 0; i < shape_[0]; ++i) map_.emplace_back(shape_[1], shape_[2]);
+}
+
+Map::Map(const std::vector<int> &shape, double step)
+    : Map(shape, {step, step, step}) {}
+
+void Map::Clear() {
+  for (auto &grid : map_) grid.Clear();
+}
+
+TPointCloudPtr &Map::at(int z_idx, int y_idx, int x_idx) {
+  return map_.at(z_idx).at(y_idx).at(x_idx);
+}
+
+const TPointCloudPtr &Map::at(int z_idx, int y_idx, int x_idx) const {
+  return map_.at(z_idx).at(y_idx).at(x_idx);
+}
+
+TPointCloudPtr &Map::at(const Index &index) {
+  return map_.at(index.k).at(index.j).at(index.i);
+}
+
+const TPointCloudPtr &Map::at(const Index &index) const {
+  return map_.at(index.k).at(index.j).at(index.i);
+}
+
+void Map::ShiftZ(int n) {
+  if (n == 0) return;
+  if (n > 0) {
+    for (int k = shape_[0] - 1; k >= n; --k) {
+      std::swap(this->at(k), this->at(k - n));
+    }
+    for (int k = 0; k < n; ++k) this->at(k).Clear();
+  } else {
+    for (int k = 0; k < shape_[0] - n; ++k) {
+      std::swap(this->at(k), this->at(k + n));
+    }
+    for (int k = shape_[0] - n - 1; k < shape_[0]; ++k) {
+      this->at(k).Clear();
+    }
+  }
+  center_[0] -= n;
+}
+
+void Map::ShiftY(int n) {
+  if (n == 0) return;
+  for (int k = 0; k < shape_[0]; ++k) {
+    if (n > 0) {
+      for (int j = shape_[1] - 1; j >= n; --j) {
+        std::swap(this->at(k, j), this->at(k, j - n));
+      }
+      for (int j = 0; j < n; ++j) this->at(k, j).Clear();
+    } else {
+      for (int j = 0; j < shape_[1] - n; ++j) {
+        std::swap(this->at(k, j), this->at(k, j + n));
+      }
+      for (int j = shape_[1] - n - 1; j < shape_[1]; ++j) {
+        this->at(k, j).Clear();
+      }
+    }
+  }
+  center_[1] -= n;
+}
+
+void Map::ShiftX(int n) {
+  if (n == 0) return;
+  for (int k = 0; k < shape_[0]; ++k) {
+    for (int j = 0; j < shape_[1]; ++j) {
+      if (n > 0) {
+        for (int i = shape_[2] - 1; i >= n; --i) {
+          std::swap(this->at(k, j, i), this->at(k, j, i - n));
+        }
+        for (int i = 0; i < n; ++i) this->at(k, j, i)->clear();
+
+      } else {
+        for (int i = 0; i < shape_[2] - n; ++i) {
+          std::swap(this->at(k, j, i), this->at(k, j, i + n));
+        }
+        for (int i = shape_[2] - n - 1; i < shape_[2]; ++i) {
+          this->at(k, j, i)->clear();
+        }
+      }
+    }
+  }
+  center_[2] -= n;
+}
+
+Index Map::GetIndex(const TPoint &point) const {
+  Index index;
+  index.k = static_cast<int>(point.z / step_[0]) + center_[0];
+  index.j = static_cast<int>(point.y / step_[1]) + center_[1];
+  index.i = static_cast<int>(point.x / step_[2]) + center_[2];
+  return index;
+}
+
+TPointCloudPtr Map::GetSurrPoints(const TPoint &point, int n) const {
+  TPointCloudPtr cloud_all(new TPointCloud);
+  for (const auto &index : GetSurrIndices(point, n)) {
+    *cloud_all += *this->at(index);
+  }
+  return cloud_all;
+}
+
+TPointCloudPtr Map::GetAllPoints() const {
+  TPointCloudPtr cloud_all(new TPointCloud);
+  for (const auto &grid : map_) *cloud_all += *grid.GetAllPoints();
+  return cloud_all;
+}
+
+void Map::AddPoints(const TPointCloudConstPtr &cloud, IndexSet *const indices) {
+  for (const auto &point : *cloud) {
+    Index index = GetIndex(point);
+    this->at(index)->push_back(point);
+    if (indices) indices->insert(index);
+  }
+}
+
+void Map::Downsample(double voxel_size) {
+  auto indices = GetAllIndices();
+  Downsample(indices, voxel_size);
+}
+
+void Map::Downsample(const std::vector<Index> &indices, double voxel_size) {
+  for (const auto &index : indices) {
+    TPointCloudPtr cloud_down_sampled(new TPointCloud);
+    common::VoxelDownSample<TPoint>(this->at(index), cloud_down_sampled.get(),
+                                    voxel_size);
+    this->at(index) = cloud_down_sampled;
+  }
+}
+
+Grid &Map::at(int z_idx) {
+  return map_.at(z_idx);
+}
+
+const Grid &Map::at(int z_idx) const {
+  return map_.at(z_idx);
+}
+
+Row &Map::at(int z_idx, int y_idx) {
+  return map_.at(z_idx).at(y_idx);
+}
+
+const Row &Map::at(int z_idx, int y_idx) const {
+  return map_.at(z_idx).at(y_idx);
+}
+
+std::vector<Index> Map::GetSurrIndices(const TPoint &point, int n) const {
+  std::vector<Index> indices;
+  Index index = GetIndex(point);
+  for (int k = -n; k <= n; ++k) {
+    for (int j = -n; j <= n; ++j) {
+      for (int i = -n; i <= n; ++i) {
+        indices.emplace_back(index.k + k, index.j + j, index.i + i);
+      }
+    }
+  }
+  return indices;
+}
+
+std::vector<Index> Map::GetAllIndices() const {
+  std::vector<Index> indices;
+  for (int k = 0; k <= shape_[0]; ++k) {
+    for (int j = 0; j <= shape_[1]; ++j) {
+      for (int i = 0; i <= shape_[2]; ++i) {
+        indices.emplace_back(k, j, i);
+      }
+    }
+  }
+  return indices;
+}
+
 Row::Row(int n) {
   row_.resize(n);
   for (auto &cloud : row_) cloud.reset(new TPointCloud);
 }
 
-TPointCloudPtr &Row::at(int ix) {
+TPointCloudPtr &Row::at(int idx) {
-  return row_.at(ix);
+  return row_.at(idx);
 }
 
-const TPointCloudPtr &Row::at(int ix) const {
+const TPointCloudPtr &Row::at(int idx) const {
-  return row_.at(ix);
+  return row_.at(idx);
 }
 
 void Row::Clear() {
@@ -35,12 +216,12 @@ void Grid::Clear() {
   for (auto &row : grid_) row.Clear();
 }
 
-Row &Grid::at(int ix) {
+Row &Grid::at(int idx) {
-  return grid_.at(ix);
+  return grid_.at(idx);
 }
 
-const Row &Grid::at(int ix) const {
+const Row &Grid::at(int idx) const {
-  return grid_.at(ix);
+  return grid_.at(idx);
 }
 
 TPointCloudPtr Grid::GetAllPoints() const {
@@ -49,152 +230,4 @@ TPointCloudPtr Grid::GetAllPoints() const {
   return cloud_all;
 }
 
-Map::Map(int z_size, int y_size, int x_size, double z_step, double y_step,
-         double x_step)
-    : z_size_(z_size),
-      y_size_(y_size),
-      x_size_(x_size),
-      z_step_(z_step),
-      y_step_(y_step),
-      x_step_(x_step),
-      z_center_(x_size / 2),
-      y_center_(y_size / 2),
-      x_center_(x_size / 2) {
-  ACHECK(z_size % 2 == 1 && y_size % 2 == 1 && x_size % 2 == 1);
-  ACHECK(z_step_ > 0.0 && y_step_ > 0.0 && x_step_ > 0.0);
-  for (int i = 0; i < z_size; ++i) map_.emplace_back(y_size, x_size);
-}
-
-Grid &Map::at(int z_ix) {
-  return map_.at(z_ix);
-}
-
-const Grid &Map::at(int z_ix) const {
-  return map_.at(z_ix);
-}
-
-Row &Map::at(int z_ix, int y_ix) {
-  return map_.at(z_ix).at(y_ix);
-}
-
-const Row &Map::at(int z_ix, int y_ix) const {
-  return map_.at(z_ix).at(y_ix);
-}
-
-TPointCloudPtr &Map::at(int z_ix, int y_ix, int x_ix) {
-  return map_.at(z_ix).at(y_ix).at(x_ix);
-}
-
-const TPointCloudPtr &Map::at(int z_ix, int y_ix, int x_ix) const {
-  return map_.at(z_ix).at(y_ix).at(x_ix);
-}
-
-TPointCloudPtr &Map::at(const Index &index) {
-  return map_.at(index.k).at(index.j).at(index.i);
-}
-
-const TPointCloudPtr &Map::at(const Index &index) const {
-  return map_.at(index.k).at(index.j).at(index.i);
-}
-
-void Map::Clear() {
-  for (auto &grid : map_) grid.Clear();
-}
-
-void Map::ShiftZ(int n) {
-  if (n == 0) return;
-  if (n > 0) {
-    for (int k = z_size_ - 1; k >= n; --k) {
-      std::swap(this->at(k), this->at(k - n));
-    }
-    for (int k = 0; k < n; ++k) this->at(k).Clear();
-  } else {
-    for (int k = 0; k < z_size_ - n; ++k) {
-      std::swap(this->at(k), this->at(k + n));
-    }
-    for (int k = z_size_ - n - 1; k < z_size_; ++k) {
-      this->at(k).Clear();
-    }
-  }
-  z_center_ -= n;
-}
-
-void Map::ShiftY(int n) {
-  if (n == 0) return;
-  for (int k = 0; k < z_size_; ++k) {
-    if (n > 0) {
-      for (int j = y_size_ - 1; j >= n; --j) {
-        std::swap(this->at(k, j), this->at(k, j - n));
-      }
-      for (int j = 0; j < n; ++j) this->at(k, j).Clear();
-    } else {
-      for (int j = 0; j < y_size_ - n; ++j) {
-        std::swap(this->at(k, j), this->at(k, j + n));
-      }
-      for (int j = y_size_ - n - 1; j < y_size_; ++j) {
-        this->at(k, j).Clear();
-      }
-    }
-  }
-  y_center_ -= n;
-}
-
-void Map::ShiftX(int n) {
-  if (n == 0) return;
-  for (int k = 0; k < z_size_; ++k) {
-    for (int j = 0; j < y_size_; ++j) {
-      if (n > 0) {
-        for (int i = x_size_ - 1; i >= n; --i) {
-          std::swap(this->at(k, j, i), this->at(k, j, i - n));
-        }
-        for (int i = 0; i < n; ++i) this->at(k, j, i)->clear();
-
-      } else {
-        for (int i = 0; i < x_size_ - n; ++i) {
-          std::swap(this->at(k, j, i), this->at(k, j, i + n));
-        }
-        for (int i = x_size_ - n - 1; i < x_size_; ++i) {
-          this->at(k, j, i)->clear();
-        }
-      }
-    }
-  }
-  x_center_ -= n;
-}
-
-Index Map::GetIndex(const TPoint &point) {
-  Index index;
-  index.k = static_cast<int>(point.x / z_step_) + z_center_;
-  index.j = static_cast<int>(point.x / y_step_) + y_center_;
-  index.i = static_cast<int>(point.x / x_step_) + x_center_;
-  return index;
-}
-
-std::vector<Index> Map::GetSurrIndices(const TPoint &point, int n) {
-  std::vector<Index> indices;
-  Index index = GetIndex(point);
-  for (int k = -n; k <= n; ++k) {
-    for (int j = -n; j <= n; ++j) {
-      for (int i = -n; i <= n; ++i) {
-        indices.emplace_back(index.k + k, index.j + j, index.i + i);
-      }
-    }
-  }
-  return indices;
-}
-
-TPointCloudPtr Map::GetSurrPoints(const TPoint &point, int n) {
-  TPointCloudPtr cloud_all(new TPointCloud);
-  for (const auto &index : GetSurrIndices(point, n)) {
-    *cloud_all += *this->at(index);
-  }
-  return cloud_all;
-}
-
-TPointCloudPtr Map::GetAllPoints() const {
-  TPointCloudPtr cloud_all(new TPointCloud);
-  for (const auto &grid : map_) *cloud_all += *grid.GetAllPoints();
-  return cloud_all;
-}
-
 }  // namespace oh_my_loam

oh_my_loam/mapper/map.h

@@ -2,7 +2,7 @@
 
 #include <vector>
 
-#include "common/macro/macros.h"
+#include "common/common.h"
 #include "oh_my_loam/base/types.h"
 
 namespace oh_my_loam {
@@ -13,24 +13,22 @@ class Grid;
 
 struct Index {
   int k, j, i;
+
+  struct Comp {
+    bool operator()(const Index &idx1, const Index &idx2) const {
+      return (idx1.k < idx2.k) || (idx1.k == idx2.k && idx1.j < idx2.j) ||
+             (idx1.k == idx2.k && idx1.j == idx2.j && idx1.i < idx2.i);
+    }
   };
+};
+
+using IndexSet = std::set<Index, Index::Comp>;
 
 class Map {
  public:
-  Map(int z_size, int y_size, int x_size, double z_step, double y_step,
+  Map(const std::vector<int> &shape, const std::vector<double> &step);
-      double x_step);
 
-  Grid &at(int z_ix);
+  Map(const std::vector<int> &shape, double step);
-
-  const Grid &at(int z_ix) const;
-
-  Row &at(int z_ix, int y_ix);
-
-  const Row &at(int z_ix, int y_ix) const;
-
-  TPointCloudPtr &at(int z_ix, int y_ix, int x_ix);
-
-  const TPointCloudPtr &at(int z_ix, int y_ix, int x_ix) const;
 
   TPointCloudPtr &at(const Index &index);
 
@@ -44,20 +42,39 @@ class Map {
 
   void ShiftX(int n);
 
-  Index GetIndex(const TPoint &point);
+  Index GetIndex(const TPoint &point) const;
 
-  std::vector<Index> GetSurrIndices(const TPoint &point, int n);
+  TPointCloudPtr GetSurrPoints(const TPoint &point, int n) const;
-
-  TPointCloudPtr GetSurrPoints(const TPoint &point, int n);
 
   TPointCloudPtr GetAllPoints() const;
 
+  void AddPoints(const TPointCloudConstPtr &cloud,
+                 IndexSet *const indices = nullptr);
+
+  void Downsample(double voxel_size);
+
+  void Downsample(const std::vector<Index> &indices, double voxel_size);
+
  private:
-  int z_size_, y_size_, x_size_;
+  Grid &at(int z_idx);
 
-  double z_step_, y_step_, x_step_;
+  const Grid &at(int z_idx) const;
 
-  int z_center_, y_center_, x_center_;
+  Row &at(int z_idx, int y_idx);
+
+  const Row &at(int z_idx, int y_idx) const;
+
+  TPointCloudPtr &at(int z_idx, int y_idx, int x_idx);
+
+  const TPointCloudPtr &at(int z_idx, int y_idx, int x_idx) const;
+
+  std::vector<Index> GetSurrIndices(const TPoint &point, int n) const;
+
+  std::vector<Index> GetAllIndices() const;
+
+  int shape_[3], center_[3];  // order: z, y, x
+
+  double step_[3];
 
   std::vector<Grid> map_;
 
@@ -68,9 +85,9 @@ class Row {
  public:
   explicit Row(int n);
 
-  TPointCloudPtr &at(int ix);
+  TPointCloudPtr &at(int idx);
 
-  const TPointCloudPtr &at(int ix) const;
+  const TPointCloudPtr &at(int idx) const;
 
   void Clear();
 
@@ -86,9 +103,9 @@ class Grid {
 
   void Clear();
 
-  Row &at(int ix);
+  Row &at(int idx);
 
-  const Row &at(int ix) const;
+  const Row &at(int idx) const;
 
   TPointCloudPtr GetAllPoints() const;
 

oh_my_loam/mapper/mapper.cc

@@ -14,6 +14,9 @@ bool Mapper::Init() {
   is_vis_ = config["vis"].as<bool>() && config_["vis"].as<bool>();
   verbose_ = config_["vis"].as<bool>();
   AINFO << "Mapping visualizer: " << (is_vis_ ? "ON" : "OFF");
+  std::vector<int> shape = YAMLConfig::GetSeq<int>(config_["map_shape"]);
+  cloud_corn_map_.reset(new Map(shape, config_["map_step"].as<double>()));
+  cloud_surf_map_.reset(new Map(shape, config_["map_step"].as<double>()));
   return true;
 }
 
@@ -23,8 +26,8 @@ void Mapper::Process(double timestamp, const TPointCloudConstPtr &cloud_corn,
                      const TPointCloudConstPtr &cloud_surf,
                      common::Pose3d *const pose_out) {
   if (GetState() == UN_INIT) {
-    cloud_corn_map_ = cloud_corn;
+    cloud_corn_map_->AddPoints(cloud_corn);
-    cloud_surf_map_ = cloud_surf;
+    cloud_surf_map_->AddPoints(cloud_surf);
     pose_out->SetIdentity();
     SetState(DONE);
     return;

oh_my_loam/mapper/mapper.h

@@ -5,10 +5,8 @@
 #include <mutex>
 #include <vector>
 
-#include "common/common.h"
 #include "common/geometry/pose3d.h"
-#include "oh_my_loam/base/feature.h"
+#include "oh_my_loam/mapper/map.h"
-#include "oh_my_loam/base/types.h"
 
 namespace oh_my_loam {
 
@@ -22,14 +20,6 @@ class Mapper {
                const TPointCloudConstPtr &cloud_surf,
                common::Pose3d *const pose_out);
 
-  TPointCloudConstPtr cloud_corn_map() const {
-    return cloud_corn_map_;
-  }
-
-  TPointCloudConstPtr cloud_surf_map() const {
-    return cloud_corn_map_;
-  }
-
   void Reset();
 
  private:
@@ -54,11 +44,6 @@ class Mapper {
 
   void Visualize();
 
-  TPointCloudPtr cloud_corn_map_;
-  TPointCloudPtr cloud_surf_map_;
-
-  std::vector<std::vector<TPointCloudPtr>> cloud_sub_map_;
-
   YAML::Node config_;
 
   struct TimePose {
@@ -66,13 +51,15 @@ class Mapper {
     common::Pose3d pose;
   };
 
+  std::unique_ptr<Map> cloud_corn_map_;
+  std::unique_ptr<Map> cloud_surf_map_;
+
   std::mutex mutex_;
   std::vector<TimePose> poses_;
+  std::unique_ptr<std::thread> thread_{nullptr};
 
   State state_ = UN_INIT;
 
-  std::unique_ptr<std::thread> thread_{nullptr};
-
   bool is_vis_ = false;
 
   bool verbose_ = false;

oh_my_loam/oh_my_loam.cc

@@ -36,9 +36,6 @@ void OhMyLoam::Reset() {
   extractor_->Reset();
   odometer_->Reset();
   mapper_->Reset();
-  std::vector<TimePose>().swap(poses_curr2world_);
-  std::lock_guard<std::mutex> lock(mutex_);
-  pose_mapping_updated_ = true;
 }
 
 void OhMyLoam::Run(double timestamp,