From d0fc4f2322c9717f1de7a5d8d6ad289acf5ca27e Mon Sep 17 00:00:00 2001 From: feixyz10 Date: Fri, 29 Jan 2021 20:45:09 +0800 Subject: [PATCH] map ok --- common/config/yaml_config.h | 20 ++ oh_my_loam/configs/config.yaml | 3 +- oh_my_loam/mapper/map.cc | 347 ++++++++++++++++++--------------- oh_my_loam/mapper/map.h | 67 ++++--- oh_my_loam/mapper/mapper.cc | 7 +- oh_my_loam/mapper/mapper.h | 23 +-- oh_my_loam/oh_my_loam.cc | 3 - 7 files changed, 264 insertions(+), 206 deletions(-) diff --git a/common/config/yaml_config.h b/common/config/yaml_config.h index e2eea25..1b4c770 100644 --- a/common/config/yaml_config.h +++ b/common/config/yaml_config.h @@ -27,6 +27,26 @@ class YAMLConfig { return *config_; } + template + static const std::vector GetSeq(const YAML::Node &node) { + ACHECK(node.IsSequence()) << "Not sequence."; + std::vector seq; + for (auto it = node.begin(); it != node.end(); ++it) { + seq.push_back(it->as()); + } + return seq; + } + + template + static const std::map GetMap(const YAML::Node &node) { + ACHECK(node.IsMap()) << "Not sequence."; + std::map map; + for (auto it = node.begin(); it != node.end(); ++it) { + map.insert({it->first.as(), it->second.as()}); + } + return map; + } + private: std::unique_ptr config_{nullptr}; diff --git a/oh_my_loam/configs/config.yaml b/oh_my_loam/configs/config.yaml index 9e3a6ed..393145f 100644 --- a/oh_my_loam/configs/config.yaml +++ b/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 diff --git a/oh_my_loam/mapper/map.cc b/oh_my_loam/mapper/map.cc index 1af8e64..f73c305 100644 --- a/oh_my_loam/mapper/map.cc +++ b/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 &shape, const std::vector &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 &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(point.z / step_[0]) + center_[0]; + index.j = static_cast(point.y / step_[1]) + center_[1]; + index.i = static_cast(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 &indices, double voxel_size) { + for (const auto &index : indices) { + TPointCloudPtr cloud_down_sampled(new TPointCloud); + common::VoxelDownSample(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 Map::GetSurrIndices(const TPoint &point, int n) const { + std::vector 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 Map::GetAllIndices() const { + std::vector 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) { - return row_.at(ix); +TPointCloudPtr &Row::at(int idx) { + return row_.at(idx); } -const TPointCloudPtr &Row::at(int ix) const { - return row_.at(ix); +const TPointCloudPtr &Row::at(int idx) const { + 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) { - return grid_.at(ix); +Row &Grid::at(int idx) { + return grid_.at(idx); } -const Row &Grid::at(int ix) const { - return grid_.at(ix); +const Row &Grid::at(int idx) const { + 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(point.x / z_step_) + z_center_; - index.j = static_cast(point.x / y_step_) + y_center_; - index.i = static_cast(point.x / x_step_) + x_center_; - return index; -} - -std::vector Map::GetSurrIndices(const TPoint &point, int n) { - std::vector 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 \ No newline at end of file diff --git a/oh_my_loam/mapper/map.h b/oh_my_loam/mapper/map.h index d9df42d..59b40db 100644 --- a/oh_my_loam/mapper/map.h +++ b/oh_my_loam/mapper/map.h @@ -2,7 +2,7 @@ #include -#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; + class Map { public: - Map(int z_size, int y_size, int x_size, double z_step, double y_step, - double x_step); + Map(const std::vector &shape, const std::vector &step); - Grid &at(int z_ix); - - 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; + Map(const std::vector &shape, double step); 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 GetSurrIndices(const TPoint &point, int n); - - TPointCloudPtr GetSurrPoints(const TPoint &point, int n); + TPointCloudPtr GetSurrPoints(const TPoint &point, int n) const; TPointCloudPtr GetAllPoints() const; + void AddPoints(const TPointCloudConstPtr &cloud, + IndexSet *const indices = nullptr); + + void Downsample(double voxel_size); + + void Downsample(const std::vector &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 GetSurrIndices(const TPoint &point, int n) const; + + std::vector GetAllIndices() const; + + int shape_[3], center_[3]; // order: z, y, x + + double step_[3]; std::vector 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; diff --git a/oh_my_loam/mapper/mapper.cc b/oh_my_loam/mapper/mapper.cc index 26ebc72..f083188 100644 --- a/oh_my_loam/mapper/mapper.cc +++ b/oh_my_loam/mapper/mapper.cc @@ -14,6 +14,9 @@ bool Mapper::Init() { is_vis_ = config["vis"].as() && config_["vis"].as(); verbose_ = config_["vis"].as(); AINFO << "Mapping visualizer: " << (is_vis_ ? "ON" : "OFF"); + std::vector shape = YAMLConfig::GetSeq(config_["map_shape"]); + cloud_corn_map_.reset(new Map(shape, config_["map_step"].as())); + cloud_surf_map_.reset(new Map(shape, config_["map_step"].as())); 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_surf_map_ = cloud_surf; + cloud_corn_map_->AddPoints(cloud_corn); + cloud_surf_map_->AddPoints(cloud_surf); pose_out->SetIdentity(); SetState(DONE); return; diff --git a/oh_my_loam/mapper/mapper.h b/oh_my_loam/mapper/mapper.h index 4492413..aaf2b71 100644 --- a/oh_my_loam/mapper/mapper.h +++ b/oh_my_loam/mapper/mapper.h @@ -5,10 +5,8 @@ #include #include -#include "common/common.h" #include "common/geometry/pose3d.h" -#include "oh_my_loam/base/feature.h" -#include "oh_my_loam/base/types.h" +#include "oh_my_loam/mapper/map.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> cloud_sub_map_; - YAML::Node config_; struct TimePose { @@ -66,13 +51,15 @@ class Mapper { common::Pose3d pose; }; + std::unique_ptr cloud_corn_map_; + std::unique_ptr cloud_surf_map_; + std::mutex mutex_; std::vector poses_; + std::unique_ptr thread_{nullptr}; State state_ = UN_INIT; - std::unique_ptr thread_{nullptr}; - bool is_vis_ = false; bool verbose_ = false; diff --git a/oh_my_loam/oh_my_loam.cc b/oh_my_loam/oh_my_loam.cc index a4464f7..e1036bd 100644 --- a/oh_my_loam/oh_my_loam.cc +++ b/oh_my_loam/oh_my_loam.cc @@ -36,9 +36,6 @@ void OhMyLoam::Reset() { extractor_->Reset(); odometer_->Reset(); mapper_->Reset(); - std::vector().swap(poses_curr2world_); - std::lock_guard lock(mutex_); - pose_mapping_updated_ = true; } void OhMyLoam::Run(double timestamp,