feat(min_range): set a default filter out near-ego-points.

* as lots of users report that some SLAM package will include few ego points exactly on the pose location etc. We set a min_range filter out these points.
related issues: #30, #20

---

TODO:
* update video presentation.
* I will try to update the data process to make it easy for both ROS1 and ROS2 users.

Author: Kin-Zhang

assets/config.toml

@@ -12,6 +12,7 @@ inflate_hits_dist = 0.2     # d_s in the paper, default 0.2
 inflate_unknown_compensation = true
 ray_passthrough_hits = false
 down_sampling_method = "center"       # ["none", "center", "centroid", "uniform"]
+min_range = 0.2                       # For filter out the ego points (some SLAM algorithm will include a pose point etc.
 max_range = -1                        # Maximum range to integrate
 only_valid = false                    # Only do ray casting for points within 'max_range'
 hit_depth = 0                         # Level of the octree to integrate hits

main.py

@@ -22,6 +22,10 @@ def inv_pose_matrix(pose):
     inv_pose[:3, :3] = pose[:3, :3].T
     inv_pose[:3, 3] = -pose[:3, :3].T.dot(pose[:3, 3])
     return inv_pose
+
+MIN_AXIS_RANGE = 0.2 # HARD CODED: remove ego vehicle points
+MAX_AXIS_RANGE = 50 # HARD CODED: remove far away points
+
 class DynamicMapData:
     def __init__(self, directory):
         self.scene_id = directory.split("/")[-1]
@@ -44,6 +48,7 @@ def __getitem__(self, index_):
 
 def main_vis(
     data_dir: str = "/home/kin/data/00",
+    voxel_map: bool = True, # output voxel-level map or raw point-level.
 ):
     dataset = DynamicMapData(data_dir)
 
@@ -54,17 +59,20 @@ def main_vis(
         data = dataset[data_id]
         now_scene_id = data['scene_id']
         pbar.set_description(f"id: {data_id}, scene_id: {now_scene_id}, timestamp: {data['timestamp']}")
-
+        norm_pc0 = np.linalg.norm(data['pc'][:, :3] - data['pose'][:3], axis=1)
+        range_mask = (
+                (norm_pc0>MIN_AXIS_RANGE) & 
+                (norm_pc0<MAX_AXIS_RANGE)
+        )
+        # STEP 1: integrate point cloud into dufomap
+        mydufo.run(data['pc'][range_mask], data['pose'], cloud_transform = False)
         # STEP 1: integrate point cloud into dufomap
-        mydufo.run(data['pc'], data['pose'], cloud_transform = False) # since pc already in world frame
         cloud_acc = np.concatenate((cloud_acc, data['pc']), axis=0)
 
     # STEP 2: propagate
     mydufo.oncePropagateCluster(if_propagate=True, if_cluster=False)
-    # STEP 3: Map results
-    mydufo.outputMap(cloud_acc, voxel_map=False)
-    # NOTE(Qingwen): You can also save voxeled map directly based on the resolution we set before:
-    # mydufo.outputMap(cloud_acc, voxel_map=True)
+    # STEP 3: Map results; You can save the voxel map directly based on the resolution we set before:
+    mydufo.outputMap(cloud_acc, voxel_map=voxel_map)
 
     mydufo.printDetailTiming()
 

src/dufomap.cpp

@@ -326,6 +326,15 @@ void cluster(Map& map, Clustering const& clustering)
 	}
 }
 
+template <class PointCloud>
+void filterminDistance(PointCloud& cloud, ufo::Point origin, float min_distance)
+{
+	float sqrt_dist = min_distance * min_distance;
+	std::erase_if(cloud, [origin, sqrt_dist](auto const& point) {
+		return origin.squaredDistance(point) < sqrt_dist;
+	});
+}
+
 int main(int argc, char* argv[])
 {
 	if (1 >= argc) {
@@ -390,6 +399,9 @@ int main(int argc, char* argv[])
 		ufo::readPointCloudPCD(path / "pcd" / filename, cloud, viewpoint);
 		timing[1].stop();
 
+		// NOTE(Qingwen): lots of user facing about ego points inside data, we filterminDistance around ego agent.
+		filterminDistance(cloud, viewpoint.translation, config.integration.min_range);
+
 		cloud_acc.insert(std::end(cloud_acc), std::cbegin(cloud), std::cend(cloud));
 
 		ufo::insertPointCloud(map, cloud, viewpoint.translation, config.integration,