map ok
feixyz10
feixyz
parent
a3427c50f3
commit
d0fc4f2322
|
|
@ -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};
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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) {
|
||||
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<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
|
||||
|
|
@ -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,
|
||||
double x_step);
|
||||
Map(const std::vector<int> &shape, const std::vector<double> &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<int> &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<Index> 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<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;
|
||||
|
||||
|
|
|
|||
|
|
@ -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_surf_map_ = cloud_surf;
|
||||
cloud_corn_map_->AddPoints(cloud_corn);
|
||||
cloud_surf_map_->AddPoints(cloud_surf);
|
||||
pose_out->SetIdentity();
|
||||
SetState(DONE);
|
||||
return;
|
||||
|
|
|
|||
|
|
@ -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/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<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;
|
||||
|
|
|
|||
|
|
@ -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,
|
||||
|
|
|
|||
Loading…
Reference in New Issue