12345qiupeng
/
oh_my_loam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

mapper coding ..

main
feixyz10 2021-01-30 17:50:07 +08:00 committed by feixyz
parent 682d8426c1
commit 4143d2f67f
3 changed files with 26 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
oh_my_loam/configs/config.yaml
View File

@ -33,6 +33,6 @@ odometer_config:
mapper_config:
vis: false
verbose: false
map_shape: [11, 21, 21]
map_shape: [3, 21, 21]
map_step_: 50 # meter
submap_shape: 5
submap_shape: [1, 5, 5]

37
oh_my_loam/mapper/mapper.cc
View File

@ -21,9 +21,11 @@ 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"]);
corn_map_.reset(new Map(shape, config_["map_step"].as<double>()));
surf_map_.reset(new Map(shape, config_["map_step"].as<double>()));
map_shape_ = YAMLConfig::GetSeq<int>(config_["map_shape"]);
submap_shape_ = YAMLConfig::GetSeq<int>(config_["submap_shape"]);
map_step_ = config_["map_step"].as<double>();
corn_map_.reset(new Map(map_shape_, map_step_));
surf_map_.reset(new Map(map_shape_, map_step_));
return true;
}
@ -79,30 +81,31 @@ void Mapper::Run(const TPointCloudConstPtr &cloud_corn_in,
}
void Mapper::AdjustMap(const Index &index) {
std::vector<int> map_shape = corn_map_->Shape();
int min_idx = config_["submap_shape"].as<int>() / 2 + 1;
int max_idx_z = map_shape[0] - min_idx - 1;
if (index.k < min_idx) {
corn_map_->ShiftZ(index.k - min_idx);
surf_map_->ShiftZ(index.k - min_idx);
int min_idx_z = submap_shape_[0] / 2 + 1;
int max_idx_z = map_shape_[0] - min_idx_z - 1;
if (index.k < min_idx_z) {
corn_map_->ShiftZ(index.k - min_idx_z);
surf_map_->ShiftZ(index.k - min_idx_z);
}
if (index.k > max_idx_z) {
corn_map_->ShiftZ(index.k - max_idx_z);
surf_map_->ShiftZ(index.k - max_idx_z);
}
int max_idx_y = map_shape[1] - min_idx - 1;
if (index.j < min_idx) {
corn_map_->ShiftY(index.j - min_idx);
surf_map_->ShiftY(index.j - min_idx);
int min_idx_y = submap_shape_[0] / 2 + 1;
int max_idx_y = map_shape_[1] - min_idx_y - 1;
if (index.j < min_idx_y) {
corn_map_->ShiftY(index.j - min_idx_y);
surf_map_->ShiftY(index.j - min_idx_y);
}
if (index.j > max_idx_y) {
corn_map_->ShiftY(index.j - max_idx_y);
surf_map_->ShiftY(index.j - max_idx_y);
}
int max_idx_x = map_shape[2] - min_idx - 1;
if (index.i < min_idx) {
corn_map_->ShiftX(index.i - min_idx);
surf_map_->ShiftX(index.i - min_idx);
int min_idx_x = submap_shape_[0] / 2 + 1;
int max_idx_x = map_shape_[2] - min_idx_x - 1;
if (index.i < min_idx_x) {
corn_map_->ShiftX(index.i - min_idx_x);
surf_map_->ShiftX(index.i - min_idx_x);
}
if (index.i > max_idx_x) {
corn_map_->ShiftX(index.i - max_idx_x);

4
oh_my_loam/mapper/mapper.h
View File

@ -73,8 +73,12 @@ class Mapper {
mutable std::shared_mutex corn_map_mutex_;
mutable std::shared_mutex surf_map_mutex_;
std::vector<int> map_shape_, submap_shape_;
double map_step_;
std::unique_ptr<Map> corn_map_;
std::unique_ptr<Map> surf_map_;
pcl::KdTreeFLANN<TPoint> kdtree_corn_map_;
pcl::KdTreeFLANN<TPoint> kdtree_surf_map_;
mutable std::mutex state_mutex_;
Pose3d pose_odom2map_;