Merge pull request PJLab-ADG#43 from dezhi-l/feature/AmazingFeature

fix issue lidar2imu/manual_calib core dumped PJLab-ADG#12
mz7790 · Nov 21, 2022 · e52115b · e52115b
2 parents 6df6e87 + 4e169e0
commit e52115b
Showing 1 changed file with 5 additions and 1 deletion.
diff --git a/lidar2imu/manual_calib/src/run_lidar2imu.cpp b/lidar2imu/manual_calib/src/run_lidar2imu.cpp
@@ -326,7 +326,11 @@ int ProcessLidarFrame(const std::vector<pcl::PointCloud<pcl::PointXYZI>> &pcds,
       dst_pt.z = p_res(2);
       dst_pt.intensity = src_pt.intensity;
 
-      if (!all_octree->isVoxelOccupiedAtPoint(dst_pt)) {
+      double min_x, min_y, min_z, max_x, max_y, max_z;
+      all_octree->getBoundingBox(min_x, min_y, min_z, max_x, max_y, max_z);
+      bool isInBox = (dst_pt.x >= min_x && dst_pt.x <= max_x) && (dst_pt.y >= min_y && dst_pt.y <= max_y) && (dst_pt.z >= min_z && dst_pt.z <= max_z);
+      if (!isInBox || !all_octree->isVoxelOccupiedAtPoint(dst_pt))
+      {
         all_octree->addPointToCloud(dst_pt, cloudLidar);
       }
     }