From 08c1a9999b28a1f5b22afe14cfb7b8242066c8a5 Mon Sep 17 00:00:00 2001
From: feixyz <fei-china@139.com>
Date: Wed, 3 Feb 2021 00:16:43 +0800
Subject: [PATCH] fix several errors

---
 oh_my_loam/configs/config.yaml |  4 ++--
 oh_my_loam/mapper/map.cc       | 14 ++++++++++----
 oh_my_loam/mapper/mapper.cc    |  8 ++++----
 3 files changed, 16 insertions(+), 10 deletions(-)

diff --git a/oh_my_loam/configs/config.yaml b/oh_my_loam/configs/config.yaml
index f3322f7..d2f8ab3 100644
--- a/oh_my_loam/configs/config.yaml
+++ b/oh_my_loam/configs/config.yaml
@@ -2,11 +2,11 @@
 lidar: VPL16
 log_to_file: false
 log_path: /data/log/oh_my_loam
-vis: false
+vis: true
 
 # configs for extractor
 extractor_config:
-  vis: true
+  vis: false
   verbose: false
   min_point_num: 66
   scan_seg_num: 12
diff --git a/oh_my_loam/mapper/map.cc b/oh_my_loam/mapper/map.cc
index 529f4d9..b3614e3 100644
--- a/oh_my_loam/mapper/map.cc
+++ b/oh_my_loam/mapper/map.cc
@@ -117,11 +117,11 @@ bool Map::IsIndexValid(const Index &index) const {
 
 TPointCloudPtr Map::GetSurrPoints(const TPoint &point,
                                   const std::vector<int> &surr_shapes) const {
-  TPointCloudPtr cloud_all(new TPointCloud);
+  TPointCloudPtr cloud(new TPointCloud);
   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 {
@@ -180,9 +180,15 @@ std::vector<Index> Map::GetSurrIndices(
   Index index = GetIndex(point);
   int nz = surr_shapes[0] / 2, ny = surr_shapes[1] / 2, nx = surr_shapes[2] / 2;
   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) {
+      int idx_j = index.j + j;
+      if (idx_j < 0 || idx_j >= shape_[1]) continue;
       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);
       }
     }
   }
diff --git a/oh_my_loam/mapper/mapper.cc b/oh_my_loam/mapper/mapper.cc
index f28f465..fdb94bc 100644
--- a/oh_my_loam/mapper/mapper.cc
+++ b/oh_my_loam/mapper/mapper.cc
@@ -60,9 +60,9 @@ void Mapper::Run(const TPointCloudConstPtr &cloud_corn,
   common::Pose3d pose_curr2map = pose_odom2map_ * pose_curr2odom;
   TPoint cnt(pose_curr2map.t_vec().x(), pose_curr2map.t_vec().y(),
              pose_curr2map.t_vec().z());
-  AdjustMap(cnt);
+  // AdjustMap(cnt);
   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) {
     pcl::KdTreeFLANN<TPoint> kdtree_corn;
     kdtree_corn.setInputCloud(cloud_corn_map);
@@ -83,10 +83,10 @@ void Mapper::Run(const TPointCloudConstPtr &cloud_corn,
     }
     PoseSolver solver(pose_curr2map);
     for (const auto &pair : pl_pairs) {
-      solver.AddPointLinePair(pair, 0.0);
+      solver.AddPointLinePair(pair, 1.0);
     }
     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_,
                       &pose_curr2map)) {