Better update PGO speed.

xuhao1 · Oct 22, 2022 · 378abff · 378abff
1 parent 67619d1
commit 378abff
Show file tree

Hide file tree

Showing 8 changed files with 44 additions and 63 deletions.
diff --git a/README.md b/README.md
@@ -14,7 +14,8 @@ The original purpose of this project is to reproduce dense mapping papers, inclu
 Note: This project is only backend of 3d dense mapping. For full SLAM features including real-time state estimation, pose graph optimization, depth generation, please take a look on [VINS](https://github.com/HKUST-Aerial-Robotics/VINS-Fisheye) and my fisheye fork of [VINS](https://github.com/xuhao1/VINS-Fisheye).
 
 ## Demos
-Octomap/Occupy map at different accuacy:
+Octomap/Occupy[1] map at different accuacy:
+
 <img src="./docs/octomap1.png" alt="drawing" style="width:400px;"/>
 <img src="./docs/octomap2.png" alt="drawing" style="width:400px;"/>
 <img src="./docs/octomap3.png" alt="drawing" style="width:400px;"/>
@@ -48,7 +49,7 @@ roslaunch launch/taichislam-d435.launch show:=true
 ```
 
 ## Generation topology skeleton graph [4]
-This demo generate [topological skeleton graph] [4](https://arxiv.org/abs/2208.04248) from TSDF
+This demo generate [topological skeleton graph](https://arxiv.org/abs/2208.04248) from TSDF
 This demo does not require ROS. Nvidia GPU is recommend for better performance.
 
 ```
@@ -61,54 +62,6 @@ This shows the polyhedron
 De-select the mesh in the options to show the skeleton
 ![](./docs/topo_graph_gen_skeleton.png)
 
-## Other demos
-Running TaichiSLAM octomap demo (currently not working...)
-
-```bash
-python examples/TaichiSLAM_demo.py -b ~/pathto/your/bag/cow_and_lady_dataset.bag
-```
-
-TSDF(Voxblox)
-
-```bash
-python examples/TaichiSLAM_demo.py -m esdf -b ~/data/voxblox/cow_and_lady_dataset.bag
-```
-
-Use - and = key to change accuacy. Mouse to rotate the map. -h to get more help.
-
-```bash
-usage: TaichiSLAM_demo.py [-h] [-r RESOLUTION RESOLUTION] [-m METHOD] [-c] [-t] [--rviz] [-p MAX_DISP_PARTICLES] [-b BAGPATH] [-o OCCUPY_THRES] [-s MAP_SIZE MAP_SIZE] [--blk BLK]
-                          [-v VOXEL_SIZE] [-K K] [-f] [--record]
-
-Taichi slam fast demo
-
-optional arguments:
-  -h, --help            show this help message and exit
-  -r RESOLUTION RESOLUTION, --resolution RESOLUTION RESOLUTION
-                        display resolution
-  -m METHOD, --method METHOD
-                        dense mapping method: octo/esdf
-  -c, --cuda            enable cuda acceleration if applicable
-  -t, --texture-enabled
-                        showing the point cloud's texture
-  --rviz                output to rviz
-  -p MAX_DISP_PARTICLES, --max-disp-particles MAX_DISP_PARTICLES
-                        max output voxels
-  -b BAGPATH, --bagpath BAGPATH
-                        path of bag
-  -o OCCUPY_THRES, --occupy-thres OCCUPY_THRES
-                        thresold for occupy
-  -s MAP_SIZE MAP_SIZE, --map-size MAP_SIZE MAP_SIZE
-                        size of map xy,z in meter
-  --blk BLK             block size of esdf, if blk==1; then dense
-  -v VOXEL_SIZE, --voxel-size VOXEL_SIZE
-                        size of voxel
-  -K K                  division each axis of octomap, when K>2, octomap will be K**3-map
-  -f, --rendering-final
-                        only rendering the final state
-  --record              record to C code
-```
-
 ## Bundle Adjustment (In development)
 ![](./docs/gradient_descent_ba.gif)
 

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, debug=False)
+            ti.init(arch=ti.cuda, 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
@@ -90,6 +90,9 @@ def initialize_sdf_fields(self):
         self.B.place(self.W_TSDF,self.TSDF, self.TSDF_observed, self.occupy, offset=offset)
         if self.enable_texture:
             self.B.place(self.color, offset=offset)
+        self.mem_per_voxel = 2 + 2 + 1 + 1
+        if self.enable_texture:
+            self.mem_per_voxel += 6
 
     def initialize_fields(self):
         self.num_export_particles = ti.field(dtype=ti.i32, shape=())
@@ -260,7 +263,15 @@ def process_new_pcl(self):
             self.new_pcl_count[i, j, k] = 0
 
     @ti.kernel
-    def fuse_submaps_kernel(self, TSDF:ti.template(), W_TSDF:ti.template(), TSDF_observed:ti.template(), occ:ti.template(), color:ti.template()):
+    def fuse_submaps_kernel(self, num_submaps: ti.i32, TSDF:ti.template(), W_TSDF:ti.template(), 
+            TSDF_observed:ti.template(), occ:ti.template(), color:ti.template(),
+            submaps_base_R_np: ti.types.ndarray(), submaps_base_T_np: ti.types.ndarray()):
+        for s in range(num_submaps):
+            for i in range(3):
+                self.submaps_base_T[s][i] = submaps_base_T_np[s, i]
+                for j in range(3):
+                    self.submaps_base_R[s][i, j] = submaps_base_R_np[s, i, j]
+
         for s, i, j, k in TSDF:
             if TSDF_observed[s, i, j, k] > 0:
                 tsdf = TSDF[s, i, j, k]
@@ -280,7 +291,8 @@ def fuse_submaps_kernel(self, TSDF:ti.template(), W_TSDF:ti.template(), TSDF_obs
     def fuse_submaps(self, submaps):
         self.B.parent().deactivate_all()
         print("try to fuse all submaps, currently active submap", submaps.active_submap_id[None])
-        self.fuse_submaps_kernel(submaps.TSDF, submaps.W_TSDF, submaps.TSDF_observed, submaps.occupy, submaps.color)
+        self.fuse_submaps_kernel(submaps.active_submap_id[None], submaps.TSDF, submaps.W_TSDF, submaps.TSDF_observed, submaps.occupy, 
+            submaps.color, self.submaps_base_R_np, self.submaps_base_T_np)
 
     def cvt_occupy_to_voxels(self):
         self.cvt_TSDF_surface_to_voxels()
@@ -370,14 +382,24 @@ def get_voxels_TSDF_slice(self, z):
         return self.export_ESDF_xyz.to_numpy(), self.export_TSDF.to_numpy()
 
     @ti.kernel
-    def count_active(self) -> ti.i32:
+    def finalization_current_submap(self):
+        count = self.count_active_func()/1024
+        count_mem = count * ti.static(self.mem_per_voxel)/1024
+        print(f"Will finalize submap {self.active_submap_id[None]} opened voxel: {count}k,{count_mem}MB")
+
+    @ti.func
+    def count_active_func(self):
         count = 0
         for s, i, j, k in self.TSDF:
             if s == self.active_submap_id[None]:
                 if self.TSDF_observed[s, i, j, k] > 0:
                     ti.atomic_add(count, 1)
         return count
 
+    @ti.kernel
+    def count_active(self) -> ti.i32:
+        return self.count_active_func()
+
     @ti.kernel
     def to_numpy(self, data_indices: ti.types.ndarray(element_dim=1), data_tsdf: ti.types.ndarray(), data_wtsdf: ti.types.ndarray(), data_occ: ti.types.ndarray(), data_color:ti.types.ndarray()):
         # Never use it for submap collection! will be extreme large

diff --git a/taichi_slam/mapping/mapping_common.py b/taichi_slam/mapping/mapping_common.py
@@ -112,14 +112,14 @@ def get_active_submap_id(self):
         return self.active_submap_id[None]
 
     def switch_to_next_submap(self):
+        self.finalization_current_submap()
         self.active_submap_id[None] += 1
         return self.active_submap_id[None]
 
     def set_base_pose_submap(self, submap_id, _R, _T):
         self.submaps_base_T_np[submap_id] = _T
         self.submaps_base_R_np[submap_id] = _R
-        self.set_base_pose_submap_kernel(submap_id, _R, _T)
-
+
     @ti.kernel
     def set_base_pose_submap_kernel(self, submap_id:ti.i16, _R:ti.types.ndarray(), _T:ti.types.ndarray()):
         for i in range(3):
@@ -231,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, 0], _ijk[1, 0], _ijk[2, 0]], ti.i32)
+        return ti.Vector([0, _ijk[0], _ijk[1], _ijk[2]], ti.i32)
 
     @ti.func
     def sxyz_to_ijk(self, s, xyz):

diff --git a/taichi_slam/mapping/submap_mapping.py b/taichi_slam/mapping/submap_mapping.py
@@ -78,10 +78,11 @@ def set_frame_poses(self, frame_poses):
         for frame_id in frame_poses:
             R = frame_poses[frame_id][0]
             T = frame_poses[frame_id][1]
-            if self.last_frame_id is None or frame_id > self.last_frame_id:
+            if (self.last_frame_id is None or frame_id > self.last_frame_id) and frame_id in self.ego_motion_poses:
                 self.last_frame_id = frame_id
             if frame_id in self.submaps:
                 self.global_map.set_base_pose_submap(self.submaps[frame_id], R, T)
+        self.pgo_poses = frame_poses
         print(f"[SubmapMapping] Update frame poses from PGO cost {(time.time() - s)*1000:.1f}ms")
 
     def create_new_submap(self, frame_id, R, T):
@@ -94,6 +95,7 @@ def create_new_submap(self, frame_id, R, T):
         submap_id = self.submap_collection.get_active_submap_id()
         self.global_map.set_base_pose_submap(submap_id, R, T)
         self.submap_collection.set_base_pose_submap(submap_id, R, T)
+        self.submap_collection.set_base_pose_submap_kernel(submap_id, R, T)
         self.submaps[frame_id] = submap_id
 
         print(f"[SubmapMapping] Created new submap, now have {submap_id+1} submaps")
@@ -128,7 +130,7 @@ def convert_by_pgo(self, frame_id, R, T):
     def recast_depth_to_map_by_frame(self, frame_id, is_keyframe, pose, ext, depthmap, texture):
         R, T = pose
         R_ext, T_ext = ext
-        R, T = self.convert_by_pgo(frame_id, R, T)
+        # R, T = self.convert_by_pgo(frame_id, R, T)
         if self.need_create_new_submap(is_keyframe, R, T):
             #In early debug we use framecount as frameid
             self.create_new_submap(frame_id, R, T)

diff --git a/taichi_slam/mapping/topo_graph.py b/taichi_slam/mapping/topo_graph.py
@@ -127,8 +127,11 @@ def init(self, idx, master_idx, start_facelet_idx, end_facelet_idx, center):
 @ti.data_oriented
 class TopoGraphGen:
     mapping: BaseMap
-    def __init__(self, mapping: BaseMap, coll_det_num = 128, max_raycast_dist=2, max_facelets=1024*1024, 
-            thres_size=0.5, transparent=0.7, transparent_frontier=0.6, frontier_creation_threshold=0.5, frontier_verify_threshold=0.5, 
+    def __init__(self, mapping: BaseMap, coll_det_num = 128, max_raycast_dist=2, 
+            max_facelets=1024*1024, 
+            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

diff --git a/taichi_slam/utils/visualization.py b/taichi_slam/utils/visualization.py
@@ -128,7 +128,7 @@ def rendering(self):
                index_count=self.mesh_num,
                per_vertex_color=self.mesh_color,
                two_sided=True)
-        # scene.lines(self.grid_lines, self.grid_width, per_vertex_color=self.grid_colors)
+        scene.lines(self.grid_lines, self.grid_width, per_vertex_color=self.grid_colors)
         if self.lines is not None:
             scene.lines(self.lines, self.grid_width*5, per_vertex_color=self.lines_color, vertex_count=self.line_vertex_num)
         scene.point_light(pos=(0.5, 1.5, 0.5), color=(1, 1, 1))

diff --git a/tests/gen_topo_graph.py b/tests/gen_topo_graph.py
@@ -21,7 +21,8 @@ def test(mapping, start_pt, render: TaichiSLAMRender, run_num=100, enable_benchm
     print("Start test topo graph generation")
     if enable_benchmark:
         benchmark(mapping, start_pt, run_num)
-    topo = TopoGraphGen(mapping, max_raycast_dist=1.5)
+    topo = TopoGraphGen(mapping, max_raycast_dist=2.5, coll_det_num=128, 
+        frontier_combine_angle_threshold=20)
     topo.generate_topo_graph(start_pt, max_nodes=100000)
     render.set_mesh(topo.tri_vertices, topo.tri_colors, mesh_num=topo.num_facelets[None])
     render.set_lines(topo.lines_show, topo.lines_color, num=topo.lines_num[None])