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

fix several errors

main
feixyz 2021-02-03 00:16:43 +08:00
parent 79f61e4e7d
commit 08c1a9999b
3 changed files with 16 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
oh_my_loam/configs/config.yaml
View File

@ -2,11 +2,11 @@
lidar: VPL16 lidar: VPL16
log_to_file: false log_to_file: false
log_path: /data/log/oh_my_loam log_path: /data/log/oh_my_loam
vis: false vis: true
# configs for extractor # configs for extractor
extractor_config: extractor_config:
vis: true vis: false
verbose: false verbose: false
min_point_num: 66 min_point_num: 66
scan_seg_num: 12 scan_seg_num: 12

14
oh_my_loam/mapper/map.cc
View File

@ -117,11 +117,11 @@ bool Map::IsIndexValid(const Index &index) const {
TPointCloudPtr Map::GetSurrPoints(const TPoint &point, TPointCloudPtr Map::GetSurrPoints(const TPoint &point,
const std::vector<int> &surr_shapes) const { const std::vector<int> &surr_shapes) const {
TPointCloudPtr cloud_all(new TPointCloud); TPointCloudPtr cloud(new TPointCloud);
for (const auto &index : GetSurrIndices(point, surr_shapes)) { for (const auto &index : GetSurrIndices(point, surr_shapes)) {
*cloud_all += *this->at(index); *cloud += *this->at(index);
} }
return cloud_all; return cloud;
} }
TPointCloudPtr Map::GetAllPoints() const { TPointCloudPtr Map::GetAllPoints() const {
@ -180,9 +180,15 @@ std::vector<Index> Map::GetSurrIndices(
Index index = GetIndex(point); Index index = GetIndex(point);
int nz = surr_shapes[0] / 2, ny = surr_shapes[1] / 2, nx = surr_shapes[2] / 2; int nz = surr_shapes[0] / 2, ny = surr_shapes[1] / 2, nx = surr_shapes[2] / 2;
for (int k = -nz; k <= nz; ++k) { for (int k = -nz; k <= nz; ++k) {
int idx_k = index.k + k;
if (idx_k < 0 || idx_k >= shape_[0]) continue;
for (int j = -ny; j <= ny; ++j) { for (int j = -ny; j <= ny; ++j) {
int idx_j = index.j + j;
if (idx_j < 0 || idx_j >= shape_[1]) continue;
for (int i = -nx; i <= nx; ++i) { for (int i = -nx; i <= nx; ++i) {
indices.emplace_back(index.k + k, index.j + j, index.i + i); int idx_i = index.i + i;
if (idx_i < 0 || idx_i >= shape_[2]) continue;
indices.emplace_back(idx_k, idx_j, idx_i);
} }
} }
} }

8
oh_my_loam/mapper/mapper.cc
View File

@ -60,9 +60,9 @@ void Mapper::Run(const TPointCloudConstPtr &cloud_corn,
common::Pose3d pose_curr2map = pose_odom2map_ * pose_curr2odom; common::Pose3d pose_curr2map = pose_odom2map_ * pose_curr2odom;
TPoint cnt(pose_curr2map.t_vec().x(), pose_curr2map.t_vec().y(), TPoint cnt(pose_curr2map.t_vec().x(), pose_curr2map.t_vec().y(),
pose_curr2map.t_vec().z()); pose_curr2map.t_vec().z());
AdjustMap(cnt); // AdjustMap(cnt);
TPointCloudPtr cloud_corn_map = corn_map_->GetSurrPoints(cnt, submap_shape_); TPointCloudPtr cloud_corn_map = corn_map_->GetSurrPoints(cnt, submap_shape_);
TPointCloudPtr cloud_surf_map = corn_map_->GetSurrPoints(cnt, submap_shape_); TPointCloudPtr cloud_surf_map = surf_map_->GetSurrPoints(cnt, submap_shape_);
for (int i = 0; i < config_["icp_iter_num"].as<int>(); ++i) { for (int i = 0; i < config_["icp_iter_num"].as<int>(); ++i) {
pcl::KdTreeFLANN<TPoint> kdtree_corn; pcl::KdTreeFLANN<TPoint> kdtree_corn;
kdtree_corn.setInputCloud(cloud_corn_map); kdtree_corn.setInputCloud(cloud_corn_map);
@ -83,10 +83,10 @@ void Mapper::Run(const TPointCloudConstPtr &cloud_corn,
} }
PoseSolver solver(pose_curr2map); PoseSolver solver(pose_curr2map);
for (const auto &pair : pl_pairs) { for (const auto &pair : pl_pairs) {
solver.AddPointLinePair(pair, 0.0); solver.AddPointLinePair(pair, 1.0);
} }
for (const auto &pair : pp_pairs) { for (const auto &pair : pp_pairs) {
solver.AddPointPlaneCoeffPair(pair, 0.0); solver.AddPointPlaneCoeffPair(pair, 1.0);
} }
if (!solver.Solve(config_["solve_iter_num"].as<int>(), verbose_, if (!solver.Solve(config_["solve_iter_num"].as<int>(), verbose_,
&pose_curr2map)) { &pose_curr2map)) {