Add edge for nodes

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

Hide file tree

Showing 2 changed files with 21 additions and 8 deletions.
diff --git a/taichi_slam/mapping/topo_graph.py b/taichi_slam/mapping/topo_graph.py
@@ -153,7 +153,7 @@ def init_colormap(self, max_facelets, transparent):
         self.colormap.from_numpy(colors)
         self.debug_frontier_color = ti.Vector([1.0, 1.0, 1.0, 0.6])
 
-    def init_fields(self, max_facelets, coll_det_num, facelet_nh_search_queue_size=1024, max_map_node=16*1024):
+    def init_fields(self, max_facelets, coll_det_num, facelet_nh_search_queue_size=1024, max_map_node=4*1024):
         self.tri_vertices = ti.Vector.field(3, ti.f32, shape=max_facelets*3)
         self.tri_colors = ti.Vector.field(4, ti.f32, shape=max_facelets*3)
         self.facelets = Facelet.field(shape=(max_facelets))
@@ -185,10 +185,13 @@ def init_fields(self, max_facelets, coll_det_num, facelet_nh_search_queue_size=1
         self.white_num[None] = 0
         self.neighbor_node_num[None] = 0
 
-        self.edges = ti.Vector.field(3, ti.f32, shape=max_facelets*3)
-        self.edge_color = ti.Vector.field(3, ti.f32, shape=max_facelets*3)
+        self.edges = ti.Vector.field(3, ti.f32, shape=max_facelets*64)
+        self.edge_color = ti.Vector.field(3, ti.f32, shape=max_facelets*64)
         self.edge_num = ti.field(dtype=ti.i32, shape=())
         self.edge_num[None] = 0
+        self.connected_nodes_root = ti.root.dense(ti.i, (max_map_node)).pointer(ti.j, (max_map_node))
+        self.connected_nodes = ti.field(dtype=ti.i32)
+        self.connected_nodes_root.place(self.connected_nodes)
 
     def reset(self):
         self.num_facelets[None] = 0
@@ -200,6 +203,7 @@ def reset(self):
         self.edge_num[None] = 0
         self.black_num[None] = 0
         self.white_num[None] = 0
+        self.connected_nodes_root.deactivate_all()
 
     def generate_uniform_sample_points(self, npoints):
         phi = np.pi * (3 - np.sqrt(5))
@@ -256,9 +260,12 @@ def verify_frontier(self, frontier_idx: ti.i32) -> ti.i32:
 
     def generate_topo_graph(self, start_pt, max_nodes=100, show=False):
         self.node_expansion(start_pt, show)
+        print("Current node num", self.num_polyhedron[None])
         while self.search_frontiers_idx[None] < self.num_frontiers[None] and self.search_frontiers_idx[None] < max_nodes:
             if self.verify_frontier(self.search_frontiers_idx[None]):
+                print("Current node num", self.num_polyhedron[None])
                 frontier = self.frontiers[self.search_frontiers_idx[None]]
+                print("frontier", self.search_frontiers_idx[None], "is valid, expand to", frontier.next_node_initial)
                 self.node_expansion(frontier.next_node_initial, show, last_node_idx=frontier.master_idx)
             self.search_frontiers_idx[None] += 1
         return self.num_polyhedron[None]
@@ -373,8 +380,14 @@ def add_mesh(self, mesh: ti.types.ndarray(), neighbors:ti.types.ndarray(), last_
         self.nodes[self.num_polyhedron[None]].init(self.num_polyhedron[None], last_node_idx, facelet_start_idx, facelet_start_idx + num_facelets, new_node_center)
         if last_node_idx >= 0:
             self.add_edge(self.nodes[last_node_idx].center, new_node_center, ti.Vector([0.0, 0.0, 0.0]), ti.Vector([0.0, 0.0, 0.0]))
+            self.connected_nodes[self.num_polyhedron[None], last_node_idx] = 1
+            self.connected_nodes[last_node_idx, self.num_polyhedron[None]] = 1
         for i in range(self.neighbor_node_num[None]):
-            self.add_edge(self.nodes[self.neighbor_node_ids[i]].center, new_node_center, ti.Vector([0.0, 0.0, 0.0]), ti.Vector([0.0, 0.0, 0.0]))
+            neigh_idx = self.neighbor_node_ids[i]
+            if self.connected_nodes[self.num_polyhedron[None], neigh_idx] == 0:
+                self.connected_nodes[self.num_polyhedron[None], neigh_idx] = 1
+                self.connected_nodes[neigh_idx, self.num_polyhedron[None]] = 1
+                self.add_edge(self.nodes[neigh_idx].center, new_node_center, ti.Vector([0.0, 0.0, 0.0]), ti.Vector([0.0, 0.0, 0.0]))
         #Construct facelet from neighbors relationship
         ti.loop_config(serialize=True)
         for i in range(facelet_start_idx, facelet_start_idx+num_facelets):

diff --git a/tests/gen_topo_graph.py b/tests/gen_topo_graph.py
@@ -24,10 +24,10 @@ def test(mapping, start_pt, render: TaichiSLAMRender, args):
     topo = TopoGraphGen(mapping, max_raycast_dist=args.ray, coll_det_num=args.coll_det_num, 
         frontier_combine_angle_threshold=20)
     num_nodes = topo.generate_topo_graph(start_pt, max_nodes=100000)
-    topo.reset()
-    s = time.time()
-    num_nodes = topo.generate_topo_graph(start_pt, max_nodes=100000)
-    print("Topo graph generated nodes", num_nodes, "time cost", (time.time() - s)*1000, "ms")
+    # topo.reset()
+    # s = time.time()
+    # num_nodes = topo.generate_topo_graph(start_pt, max_nodes=100000)
+    # print("Topo graph generated nodes", num_nodes, "time cost", (time.time() - s)*1000, "ms")
     render.set_mesh(topo.tri_vertices, topo.tri_colors, mesh_num=topo.num_facelets[None])
     render.set_skeleton_graph_edges(topo.edges.to_numpy()[0:topo.edge_num[None]])