Bug fixed on verify_frontier

xuhao1 · Nov 22, 2022 · 0ac376b · 0ac376b
1 parent 36bbec8
commit 0ac376b
Show file tree

Hide file tree

Showing 2 changed files with 4 additions and 4 deletions.
diff --git a/launch/taichislam-d435.launch b/launch/taichislam-d435.launch
@@ -60,8 +60,8 @@
                 cx: 323.4873046875
                 cy: 235.0628204345703
             skeleton_graph_gen:
-                max_raycast_dist: 2.0
-                coll_det_num: 128
+                max_raycast_dist: 3.0
+                coll_det_num: 64
                 frontier_combine_angle_threshold: 20
         </rosparam>
         <!-- <rosparam>

diff --git a/taichi_slam/mapping/topo_graph.py b/taichi_slam/mapping/topo_graph.py
@@ -245,8 +245,8 @@ def node_expansion(self, start_pt, show=False, last_node_idx = -1):
     def verify_frontier(self, frontier_idx: ti.i32) -> ti.i32:
         normal = self.frontiers[frontier_idx].projected_normal
         proj_center = self.frontiers[frontier_idx].projected_center + normal * ti.static(self.check_frontier_small_distance)
-        succ, t, col_pos, _len, node_idx = self.raycast(proj_center, normal, self.frontier_verify_threshold*2)
-        if succ and t < self.frontier_verify_threshold:
+        succ, t, col_pos, _len, node_idx = self.raycast(proj_center, normal, self.max_raycast_dist*2)
+        if succ and _len < self.frontier_verify_threshold:
             self.frontiers[frontier_idx].is_valid = False
         else:
             self.frontiers[frontier_idx].is_valid = True