Could gen topograph

xuhao1 · Sep 7, 2022 · 553e5bc · 553e5bc
1 parent 3769869
commit 553e5bc
Show file tree

Hide file tree

Showing 9 changed files with 149 additions and 52 deletions.
diff --git a/launch/taichislam-d435.launch b/launch/taichislam-d435.launch
@@ -2,19 +2,19 @@
     <arg name="use_cuda" default="true" />
     <arg name="show" default="false" />
     <arg name="mapping_type" default="tsdf" />
-    <arg name="texture_enabled" default="true" />
-    <arg name="enable_mesher" default="true" />
+    <arg name="texture_enabled" default="false" />
+    <arg name="enable_mesher" default="false" />
     <arg name="output_map" default="false" />
     <arg name="max_disp_particles" default="1000000" />
     <arg name="max_mesh" default="3000000" />
     <arg name="max_ray_length" default="3.1" />
     <arg name="min_ray_length" default="0.3" />
     <arg name="vins" default="false" />
     <arg name="viz" default="false" />
-    <arg name="enable_submap" default="false" />
+    <arg name="enable_submap" default="true" />
     <arg name="vins_config" default="/home/xuhao/bags/swarm_raw_parallel2_2021-10-18_17-09/Configs/SwarmConfig1/fisheye_ptgrey_n3/fisheye_cuda.yaml" />
 
-    <node pkg="TaichiSLAM" type="taichislam_node.py" name="taichislam_node" output="screen" >
+    <node pkg="taichislam" type="taichislam_node.py" name="taichislam_node" output="screen" >
         <param name="use_cuda" value="$(arg use_cuda)" type="boolean" />
         <param name="enable_rendering" value="$(arg show)" type="boolean" />
         <param name="mapping_type" value="$(arg mapping_type)" type="string" />

diff --git a/requirements.txt b/requirements.txt
@@ -0,0 +1,5 @@
+numpy
+scipy
+matplotlib
+taichi
+transformations
diff --git a/scripts/taichislam_node.py b/scripts/taichislam_node.py
@@ -39,7 +39,7 @@ def __init__(self):
         self.enable_submap = rospy.get_param('~enable_submap', False)
 
         if cuda:
-            ti.init(arch=ti.cuda, device_memory_fraction=0.2, dynamic_index=True, offline_cache=False, packed=True)
+            ti.init(arch=ti.cuda, device_memory_fraction=0.2, dynamic_index=True, offline_cache=False, packed=True, debug=False)
         else:
             ti.init(arch=ti.cpu, dynamic_index=True, offline_cache=False, packed=True)
         self.disp_level = 0

diff --git a/taichi_slam/mapping/dense_tsdf.py b/taichi_slam/mapping/dense_tsdf.py
@@ -36,7 +36,7 @@ def __init__(self, map_scale=[10, 10], voxel_size=0.05, texture_enabled=False, \
 
         self.max_ray_length = max_ray_length
         self.min_ray_length = min_ray_length
-        self.tsdf_surface_thres = self.voxel_size
+        self.tsdf_surface_thres = self.voxel_size*1.5
         self.internal_voxels = internal_voxels
         self.max_submap_size = max_submap_size
 

diff --git a/taichi_slam/mapping/mapping_common.py b/taichi_slam/mapping/mapping_common.py
@@ -158,16 +158,12 @@ def raycast(self, pos, dir, max_dist):
         x_ = ti.Vector([0., 0., 0.], ti.f32)
         succ = False
         _len = 0.0
+        ti.loop_config(serialize=True, parallelize=False)
         for _j in range(ray_cast_voxels):
             _len = _j*self.voxel_size
             x_ = dir*_len + pos
             submap_id = self.active_submap_id[None]
-            ijk = self.sxyz_to_ijk(submap_id, x_)
-            if self.is_pos_unobserved(x_):
-                succ = True
-                break
             if self.is_pos_occupy(x_):
-                # print("dir", dir, "len", _len, "occupy")
                 succ = True
                 break
         return succ, x_, _len
@@ -210,7 +206,7 @@ def is_occupy(self, ijk):
 
     @ti.func 
     def color_from_colomap(self, z, min_z, max_z):
-        _c = int(max(min(( (z - min_z)/(max_z-min_z) )*1024, 1024), 0))
+        _c = int(max(min(( (z - min_z)/(max_z-min_z) )*1023, 1023), 0))
         return self.colormap[_c]
 
     @ti.func
@@ -235,7 +231,7 @@ def xyz_to_ijk(self, xyz):
     def xyz_to_0ijk(self, xyz):
         ijk =  xyz / self.voxel_size_
         _ijk = self.constrain_coor(ijk)
-        return ti.Vector([0, _ijk[0], _ijk[1], _ijk[2]], ti.i32)
+        return ti.Vector([0, _ijk[0, 0], _ijk[1, 0], _ijk[2, 0]], ti.i32)
 
     @ti.func
     def sxyz_to_ijk(self, s, xyz):
@@ -246,9 +242,4 @@ def sxyz_to_ijk(self, s, xyz):
     @ti.func
     def constrain_coor(self, _i):
         ijk = _i.cast(ti.i32)
-        for d in ti.static(range(3)):
-            if ijk[d] >= self.NC_[d]:
-                ijk[d] = self.NC_[d] - 1
-            if ijk[d] <= - self.NC_[d]:
-                ijk[d] = self.NC_[d] + 1
         return ijk
diff --git a/taichi_slam/mapping/topo_graph.py b/taichi_slam/mapping/topo_graph.py
@@ -33,12 +33,14 @@ class Facelet:
     assigned: ti.i32
 
     @ti.func
-    def init(self, poly_idx, facelet_idx, v0, v1, v2):
+    def init(self, poly_idx, facelet_idx, v0, v1, v2, naive_norm):
         self.poly_idx = poly_idx
         self.edge1 = v1 - v0
         self.edge2 = v2 - v0
         self.center = (v0 + v1 + v2) / 3
         self.normal = self.edge1.cross(self.edge2).normalized()
+        if self.normal.dot(naive_norm) < 0:
+            self.normal = -self.normal
         self.v0 = v0
         self.v1 = v1
         self.v2 = v2
@@ -79,18 +81,52 @@ class Frontier:
     projected_center: vec3f
     projected_normal: vec3f
     next_node_pos: vec3f
-
+    next_node_initial: vec3f
+    is_valid : ti.i32
     @ti.func
     def init(self, frontier_idx, avg_center, outwards_unit_normal):
         self.frontier_idx = frontier_idx
         self.avg_center = avg_center
         self.outwards_unit_normal = outwards_unit_normal
+        self.is_valid = False
+
+@ti.dataclass
+class Gate:
+    gate_idx: ti.i32
+    master_idx: ti.i32
+    frontier_idx: ti.i32
+    avg_center: vec3f
+    outwards_unit_normal: vec3f
+    projected_center: vec3f
+    projected_normal: vec3f
+    next_node_pos: vec3f
+
+    @ti.func
+    def init(self, gate_idx, avg_center, outwards_unit_normal):
+        self.gate_idx = gate_idx
+        self.avg_center = avg_center
+        self.outwards_unit_normal = outwards_unit_normal
+
+@ti.dataclass
+class MapNode:
+    start_facelet_idx: ti.i32
+    end_facelet_idx: ti.i32
+    center: vec3f
+    idx: ti.i32
+
+    @ti.func
+    def init(self, idx, start_facelet_idx, end_facelet_idx, center):
+        self.idx = idx
+        self.start_facelet_idx = start_facelet_idx
+        self.end_facelet_idx = end_facelet_idx
+        self.center = center
 
 @ti.data_oriented
 class TopoGraphGen:
     mapping: BaseMap
     def __init__(self, mapping: BaseMap, coll_det_num = 128, max_raycast_dist=2, max_facelets=64*1024, 
-            thres_size=0.5, transparent=0.9, transparent_frontier=0.4, frontier_creation_threshold=1.0):
+            thres_size=0.5, transparent=0.7, transparent_frontier=0.6, frontier_creation_threshold=0.5, frontier_verify_threshold=0.5, 
+            frontier_combine_angle_threshold=40):
         self.mapping = mapping
         self.coll_det_num = coll_det_num
         self.generate_uniform_sample_points(coll_det_num)
@@ -99,7 +135,10 @@ def __init__(self, mapping: BaseMap, coll_det_num = 128, max_raycast_dist=2, max
         self.thres_size = thres_size
         self.init_colormap(max_facelets, transparent)
         self.frontier_creation_threshold = frontier_creation_threshold
+        self.frontier_verify_threshold = frontier_verify_threshold
         self.transparent_frontier = transparent_frontier
+        self.frontier_normal_dot_threshold = np.cos(np.deg2rad(frontier_combine_angle_threshold))
+        self.check_frontier_small_distance = 0.02
 
     def init_colormap(self, max_facelets, transparent):
         self.colormap = ti.Vector.field(4, float, shape=max_facelets)
@@ -108,21 +147,25 @@ 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):
+    def init_fields(self, max_facelets, coll_det_num, facelet_nh_search_queue_size=1024, max_map_node=16*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))
         self.frontiers = Frontier.field(shape=(max_facelets))
+        self.nodes = MapNode.field(shape=(max_map_node))
+
         self.num_facelets = ti.field(dtype=ti.i32, shape=())
         self.num_polyhedron = ti.field(dtype=ti.i32, shape=())
         self.num_frontiers = ti.field(dtype=ti.i32, shape=())
-        self.start_point = ti.Vector.field(3, dtype=ti.i32, shape=())
+        self.start_point = ti.Vector.field(3, dtype=ti.f32, shape=())
         self.facelet_nh_search_queue = ti.field(ti.i32, shape=facelet_nh_search_queue_size)
         self.facelet_nh_search_idx = ti.field(dtype=ti.i32, shape=())
         self.facelet_nh_search_queue_size = ti.field(dtype=ti.i32, shape=())
+        self.search_frontiers_idx = ti.field(dtype=ti.i32, shape=())
         self.num_facelets[None] = 0
         self.num_polyhedron[None] = 0
         self.num_frontiers[None] = 0
+        self.search_frontiers_idx[None] = 0
 
         self.white_list = ti.Vector.field(3, ti.f32, shape=coll_det_num)
         self.white_num = ti.field(dtype=ti.i32, shape=())
@@ -132,6 +175,12 @@ def init_fields(self, max_facelets, coll_det_num, facelet_nh_search_queue_size=1
         self.black_num = ti.field(dtype=ti.i32, shape=())
         self.black_num[None] = 0
         self.white_num[None] = 0
+
+        self.lines_show = ti.Vector.field(3, ti.f32, shape=max_facelets*3)
+        self.lines_color = ti.Vector.field(3, ti.f32, shape=max_facelets*3)
+        self.lines_num = ti.field(dtype=ti.i32, shape=())
+        self.lines_num[None] = 0
+
 
     def generate_uniform_sample_points(self, npoints):
         phi = np.pi * (3 - np.sqrt(5))
@@ -169,11 +218,36 @@ def node_expansion_benchmark(self, start_pt, show=False, run_num=100):
         print(f"avg gen convex cost time {(time.time()-s)*1000/run_num:.3f}ms")
 
     def node_expansion(self, start_pt, show=False):
-        self.start_point[None] = ti.Vector(start_pt, ti.f32)
+        self.start_point[None] = start_pt
         if self.detect_collisions():
             self.generate_poly_on_blacks(start_pt, show)
 
+    @ti.kernel
+    def verify_frontier(self, frontier_idx: ti.i32) -> ti.i32:
+        # print("Verifing frontier...... center ", self.frontiers[frontier_idx].projected_center, )
+        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 = self.raycast(proj_center, normal, self.frontier_verify_threshold*2)
+        if succ and t < self.frontier_verify_threshold:
+            self.frontiers[frontier_idx].is_valid = False
+        else:
+            self.frontiers[frontier_idx].is_valid = True
+            self.frontiers[frontier_idx].next_node_initial = self.frontiers[frontier_idx].projected_center + \
+                self.frontiers[frontier_idx].projected_normal*_len
+        return self.frontiers[frontier_idx].is_valid 
+
+    def generate_topo_graph(self, start_pt, max_nodes=100, show=False):
+        self.node_expansion(start_pt, show)
+        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]):
+                frontier = self.frontiers[self.search_frontiers_idx[None]]
+                print(f"expansion from frontier {self.search_frontiers_idx[None]}, pos :", frontier.next_node_initial)
+                self.node_expansion(frontier.next_node_initial, show)
+            self.search_frontiers_idx[None] += 1
+
     def generate_mesh_from_hull(self, hull, start_pt):
+        if not isinstance(start_pt, np.ndarray):
+            start_pt = start_pt.to_numpy()
         lens = self.black_len_list.to_numpy()[0:self.black_num[None]]
         vertices = hull.points*lens[:, None]
         vertices = np.apply_along_axis(lambda x:x+start_pt, 1, vertices)
@@ -189,6 +263,14 @@ def generate_poly_on_blacks(self, start_pt, show=False):
             show_convex(hull)
             show_mesh(mesh)
 
+    @ti.func
+    def add_line(self, a, b, color_a, color_b):
+        self.lines_show[self.lines_num[None]] = a
+        self.lines_show[self.lines_num[None]+1] = b
+        self.lines_color[self.lines_num[None]] = color_a
+        self.lines_color[self.lines_num[None]+1] = color_b
+        self.lines_num[None] += 2
+
     @ti.func
     def detect_facelet_frontier(self, facelet):
         #First check if center has obstacle.
@@ -231,13 +313,11 @@ def construct_frontier(self, idx_start_facelet):
             if succ:
                 break
         if succ:
-            self.frontiers[frontier_idx].projected_center = center + normal*t
+            proj_center = center + t*normal
+            self.frontiers[frontier_idx].projected_center = proj_center
             self.frontiers[frontier_idx].projected_normal = projected_normal
-            # for i in range(self.facelet_nh_search_queue_size[None]):
-            #     facelet_idx = self.facelet_nh_search_queue[i] + idx_start_facelet 
-            #     for j in range(ti.static(3)):
-            #         self.tri_colors[facelet_idx*3 + j] = self.colormap[frontier_idx + 100]
-            #         self.tri_colors[facelet_idx*3 + j][3] = 1.0
+            self.add_line(proj_center, proj_center+projected_normal*0.5, ti.Vector([0.0, 0.0, 0.0]), ti.Vector([0.0, 1.0, 0.0]))
+
         else:
             print("Failed to raycast center set facelets to different color", self.colormap[frontier_idx + 10])
             for i in range(self.facelet_nh_search_queue_size[None]):
@@ -247,11 +327,12 @@ def construct_frontier(self, idx_start_facelet):
 
     @ti.kernel
     def add_mesh(self, mesh: ti.types.ndarray(), neighbors:ti.types.ndarray()):
-        index_poly = ti.atomic_add(self.num_polyhedron[None], 1)
+        index_poly = self.num_polyhedron[None]
         idx_start_facelet = self.num_facelets[None]
         num_facelets = mesh.shape[0]
-        print("add_mesh with num_facelets:", num_facelets)
         facelet_start_idx = ti.atomic_add(self.num_facelets[None], num_facelets)
+        center_pos = ti.Vector([0., 0., 0.], ti.f32)
+        center_count = 0.0
         for i in range(num_facelets):
             facelet_idx = i + facelet_start_idx #To avoid parallel make the indice wrong
             for j in range(ti.static(3)):
@@ -260,14 +341,22 @@ def add_mesh(self, mesh: ti.types.ndarray(), neighbors:ti.types.ndarray()):
             v0 = self.tri_vertices[facelet_idx*3]
             v1 = self.tri_vertices[facelet_idx*3 + 1]
             v2 = self.tri_vertices[facelet_idx*3 + 2]
-            self.facelets[facelet_idx].init(index_poly, facelet_idx, v0, v1, v2)
+            center_pos += v0 + v1 + v2
+            center_count += 3.0
+            naive_norm = (v0 + v1 + v2 - 3.0*self.start_point[None]).normalized()
+            self.facelets[facelet_idx].init(index_poly, facelet_idx, v0, v1, v2, naive_norm)
             self.facelets[facelet_idx].is_frontier = self.detect_facelet_frontier(self.facelets[facelet_idx])
             #For visualization
             for j in range(ti.static(3)):
                 self.tri_colors[facelet_idx*3 + j] = self.colormap[index_poly]
                 if self.facelets[facelet_idx].is_frontier:
                     self.tri_colors[facelet_idx*3 + j][3] = ti.static(self.transparent_frontier)
-
+            # if self.facelets[facelet_idx].is_frontier:
+            #     self.tri_vertices[facelet_idx*3] = ti.Vector([0.0, 0.0, 0.0])
+            #     self.tri_vertices[facelet_idx*3+1] = ti.Vector([0.0, 0.0, 0.0])
+            #     self.tri_vertices[facelet_idx*3+2] = ti.Vector([0.0, 0.0, 0.0])
+        self.nodes[index_poly].init(index_poly, facelet_start_idx, facelet_start_idx + num_facelets, center_pos/center_count)
+        print("add map node", index_poly, "with ", num_facelets, " facelets center", self.nodes[index_poly].center)
         #Construct facelet from neighbors relationship
         ti.loop_config(serialize=True)
         for i in range(idx_start_facelet, idx_start_facelet+num_facelets):
@@ -277,6 +366,7 @@ def add_mesh(self, mesh: ti.types.ndarray(), neighbors:ti.types.ndarray()):
                 self.facelet_nh_search_idx[None] = 0
                 self.facelet_nh_search_queue_size[None] = 1
                 self.facelet_nh_search_queue[0] = idx 
+                normal = self.facelets[i].normal
                 while self.facelet_nh_search_idx[None] < self.facelet_nh_search_queue_size[None]:
                     search_idx = ti.atomic_add(self.facelet_nh_search_idx[None], 1)
                     _idx = self.facelet_nh_search_queue[search_idx] #This idx is define in hull
@@ -285,17 +375,14 @@ def add_mesh(self, mesh: ti.types.ndarray(), neighbors:ti.types.ndarray()):
                     for j in range(ti.static(3)):
                         #Check if neighor is assigned and frontier
                         idx_neighbor = neighbors[_idx, j] + idx_start_facelet
-                        if not self.facelets[idx_neighbor].assigned and self.facelets[idx_neighbor].is_frontier:
+                        if self.facelets[idx_neighbor].is_frontier and not self.facelets[idx_neighbor].assigned and \
+                                normal.dot(self.facelets[idx_neighbor].normal) > ti.static(self.frontier_normal_dot_threshold):
                             self.facelet_nh_search_queue[ti.atomic_add(self.facelet_nh_search_queue_size[None], 1)] = neighbors[_idx, j]
                             # print("neighbor", neighbors[_idx, j], "idx", idx_neighbor, "v0", self.facelets[idx_neighbor].v0, "v1", self.facelets[idx_neighbor].v1, "v2", self.facelets[idx_neighbor].v2)
                 # Now we can construct the frontier from these facelets
                 # It uses logs in facelet_nh_search_queue to find the facelets
                 self.construct_frontier(idx_start_facelet)
-
-
-    @ti.kernel
-    def generate_topo_graph(self, start_pt: ti.types.ndarray()):
-        pass
+        self.num_polyhedron[None] = index_poly + 1
 
     @ti.kernel
     def detect_collisions(self) ->ti.i32:
@@ -323,22 +410,23 @@ def detect_collisions(self) ->ti.i32:
         return succ
 
     @ti.func
-    def detect_collision_polys(self, pos, dir, max_dist:ti.f32):
+    def detect_collision_facelets(self, pos, dir, max_dist:ti.f32):
         succ = False
         best_t = max_dist
         best_poly_ind = -1
-        for i in range(self.num_polyhedron[None]):
-            succ, t = self.facelets[i].rayTriangleIntersect(pos, dir)
-            if succ and t < best_t:
+        for i in range(self.num_facelets[None]):
+            _succ, t = self.facelets[i].rayTriangleIntersect(pos, dir)
+            if _succ and -0.01 < t < best_t:
                 best_t = t
                 best_poly_ind = self.facelets[i].poly_idx
+                succ = True
         pos_poly = pos + dir*best_t
         return succ, pos_poly, best_t, best_poly_ind
 
     @ti.func
     def raycast(self, pos, dir, max_dist):
         mapping = self.mapping
-        succ_poly, pos_poly, len_poly, poly_ind = self.detect_collision_polys(pos, dir, max_dist)
+        succ_poly, pos_poly, len_poly, poly_ind = self.detect_collision_facelets(pos, dir, max_dist)
         max_dist_recast = max_dist
         if succ_poly:
             max_dist_recast = len_poly
@@ -350,6 +438,7 @@ def raycast(self, pos, dir, max_dist):
                 pos_col = pos_poly
                 _len = len_poly
                 recast_type = 1
+        succ = succ or succ_poly
         return succ, recast_type, pos_col, _len
 
     def test_detect_collisions(self, start_pt):