Critical bug of TSDF fixed.

xuhao1 · Jul 7, 2022 · 3e28c31 · 3e28c31
1 parent 7c69f07
commit 3e28c31
Show file tree

Hide file tree

Showing 5 changed files with 30 additions and 21 deletions.
diff --git a/launch/taichislam-d435.launch b/launch/taichislam-d435.launch
@@ -8,7 +8,7 @@
     <arg name="max_disp_particles" default="8000000" />
     <arg name="max_mesh" default="1000000" />
     <arg name="max_ray_length" default="3.1" />
-    <arg name="min_ray_length" default="1.0" />
+    <arg name="min_ray_length" default="0.8" />
     <arg name="vins" default="false" />
     <arg name="viz" default="false" />
     <arg name="vins_config" default="/home/xuhao/bags/swarm_raw_parallel2_2021-10-18_17-09/Configs/SwarmConfig1/fisheye_ptgrey_n3/fisheye_cuda.yaml" />
@@ -29,7 +29,8 @@
 
         <remap from="~depth" to="/camera/depth/image_rect_raw" />
         <remap from="~pose" to="/vins_estimator/camera_pose" />
-        <remap from="~image" to="/camera/infra1/image_rect_raw/compressed" />
+        <!-- <remap from="~image" to="/camera/infra1/image_rect_raw/compressed" /> -->
+        <remap from="~image" to="/camera/color/image_raw/compressed" />
 
         <rosparam>
             texture_compressed: true

diff --git a/scripts/taichislam_node.py b/scripts/taichislam_node.py
@@ -78,6 +78,7 @@ def __init__(self):
             RES_X = rospy.get_param('~disp/res_x', 1920)
             RES_Y = rospy.get_param('~disp/res_y', 1080)
             self.render = TaichiSLAMRender(RES_X, RES_Y)
+            self.render.enable_mesher = self.enable_mesher
 
         self.pub_occ = rospy.Publisher('/occ', PointCloud2, queue_size=10)
         self.pub_tsdf_surface = rospy.Publisher('/pub_tsdf_surface', PointCloud2, queue_size=10)
@@ -198,7 +199,7 @@ def taichimapping_depth_callback(self, pose, depth_msg, rgb_array=None):
             if self.enable_rendering:
                 self.render.set_particles(mapping.export_x, mapping.export_color)
         else:
-            if self.enable_mesher:
+            if self.render.enable_mesher:
                 mesher = self.mesher
                 start_time = time.time()
                 mesher.generate_mesh(1)

diff --git a/taichi_slam/mapping/dense_esdf.py b/taichi_slam/mapping/dense_esdf.py
@@ -89,6 +89,7 @@ def initialize_fields(self):
 
         self.new_pcl_count = ti.field(dtype=ti.i32)
         self.new_pcl_sum_pos = ti.Vector.field(3, dtype=ti.f32) #position in sensor coor
+        self.new_pcl_z = ti.field(dtype=ti.f32) #position in sensor coor
 
         self.updated_TSDF = ti.field(dtype=ti.i32)
 
@@ -117,7 +118,7 @@ def initialize_fields(self):
         self.T.place(self.updated_TSDF)
 
         self.PCL, self.PCLroot = self.data_structures_pointer(block_num_xy, block_num_z, block_size)
-        self.PCL.place(self.new_pcl_count, self.new_pcl_sum_pos)
+        self.PCL.place(self.new_pcl_count, self.new_pcl_sum_pos, self.new_pcl_z)
         if self.TEXTURE_ENABLED:
             self.PCL.place(self.new_pcl_sum_color)
 
@@ -340,11 +341,11 @@ def recast_depth_to_map_kernel(self, depthmap: ti.ext_arr(), texture: ti.ext_arr
                 if depthmap[j, i] > ti.static(self.max_ray_length*1000) or depthmap[j, i] < ti.static(self.min_ray_length*1000):
                     continue
 
-                dep = depthmap[j, i]/1000.0
+                dep = ti.cast(depthmap[j, i], ti.f32)/1000.0
 
                 pt = ti.Vector([
-                    (i-cx)*dep/fx, 
-                    (j-cy)*dep/fy, 
+                    (ti.cast(i, ti.f32)-cx)*dep/fx, 
+                    (ti.cast(j, ti.f32)-cy)*dep/fy, 
                     dep], ti.f32)
 
                 pt_map = self.input_R[None]@pt
@@ -358,9 +359,9 @@ def recast_depth_to_map_kernel(self, depthmap: ti.ext_arr(), texture: ti.ext_arr
                     color_j = ti.cast((j-cy)/fy*fy_c+cy_c, ti.int32)
                     if color_i < 0 or color_i >= 640 or color_j < 0 or color_j >= 480:
                         continue
-                    self.process_point(pt_map, [texture[color_j, color_i, 0], texture[color_j, color_i, 1], texture[color_j, color_i, 2]])
+                    self.process_point(pt_map, dep, [texture[color_j, color_i, 0], texture[color_j, color_i, 1], texture[color_j, color_i, 2]])
                 else:
-                    self.process_point(pt_map)
+                    self.process_point(pt_map, dep)
 
         self.process_new_pcl()
 
@@ -369,12 +370,13 @@ def recast_depth_to_map_kernel(self, depthmap: ti.ext_arr(), texture: ti.ext_arr
             self.propogate_esdf()
 
     @ti.func
-    def process_point(self, pt, rgb=None):
+    def process_point(self, pt, z, rgb=None):
         pti = self.xyz_to_ijk(pt)
-        self.new_pcl_count[pti] += 1
-        self.new_pcl_sum_pos[pti] += pt
+        self.new_pcl_count[pti] = 1
+        self.new_pcl_sum_pos[pti] = pt
+        self.new_pcl_z[pti] = z
         if ti.static(self.TEXTURE_ENABLED):
-            self.new_pcl_sum_color[pti] += rgb
+            self.new_pcl_sum_color[pti] = rgb
 
         pti = self.xyz_to_ijk(pt + self.input_T[None])
 
@@ -387,12 +389,14 @@ def process_point(self, pt, rgb=None):
     @ti.func
     def process_new_pcl(self):
         for i, j, k in self.new_pcl_count:
+            if self.new_pcl_count[i, j, k] == 0:
+                continue
             c = self.new_pcl_count[i, j, k]
-            pos_s2p = self.new_pcl_sum_pos[i, j, k]/c
+            pos_s2p = self.new_pcl_sum_pos[i, j, k]#/c
             len_pos_s2p = pos_s2p.norm()
             d_s2p = pos_s2p /len_pos_s2p
             pos_p = pos_s2p + self.input_T[None]
-            z = pos_s2p[2]
+            z = self.new_pcl_z[i, j, k]
 
             j_f = 0.0
             ray_cast_voxels = ti.min(len_pos_s2p/self.voxel_size+ti.static(self.internal_voxels), self.max_ray_length/self.voxel_size)
@@ -402,10 +406,11 @@ def process_new_pcl(self):
                 xi = self.xyz_to_ijk(x_)
                 xi = self.constrain_coor(xi)
 
-                #vector from current voxel to point, e.g. p-x
+                #v2p: vector from current voxel to point, e.g. p-x
+                #pos_s2p sensor to point
                 v2p = pos_p - x_
                 d_x_p = v2p.norm()
-                d_x_p_s = d_x_p*sign(v2p.dot(pos_p))
+                d_x_p_s = d_x_p*sign(v2p.dot(pos_s2p))
 
                 w_x_p = self.w_x_p(d_x_p, z)
 
@@ -415,8 +420,8 @@ def process_new_pcl(self):
                 self.W_TSDF[xi] = ti.min(self.W_TSDF[xi]+w_x_p, Wmax)
                 self.updated_TSDF[xi] = 1
                 if ti.static(self.TEXTURE_ENABLED):
-                    self.color[xi] = self.new_pcl_sum_color[i, j, k]/ c/255.0
-
+                    self.color[xi] = self.new_pcl_sum_color[i, j, k]/255.0
+            self.new_pcl_count[i, j, k] = 0
 
     @ti.kernel
     def cvt_occupy_to_voxels(self):

diff --git a/taichi_slam/mapping/marching_cube_mesher.py b/taichi_slam/mapping/marching_cube_mesher.py
@@ -177,7 +177,7 @@ def marching_on_a_cube(self, i, j, k, obs, tsdf, color, step):
         return ret
 
     @ti.kernel
-    def generate_mesh_kernel(self, obs:ti.template(), tsdf: ti.template(), color: ti.template(), step:ti.i32):
+    def generate_mesh_kernel(self, obs:ti.template(), tsdf: ti.template(), w_tsdf: ti.template(), color: ti.template(), step:ti.i32):
         self.num_triangles[None] = 0
         for i, j, k in tsdf:
             if obs[i, j, k] > 0 and tsdf[i, j, k] < self.tsdf_surface_thres:
@@ -189,7 +189,7 @@ def vertice_num(self):
         return self.num_triangles[None]*3
 
     def generate_mesh(self, step=1):
-        self.generate_mesh_kernel(self.mapping.TSDF_observed, self.mapping.TSDF, self.mapping.color, step)
+        self.generate_mesh_kernel(self.mapping.TSDF_observed, self.mapping.TSDF, self.mapping.W_TSDF, self.mapping.color, step)
 
 
 grid_xyz = [

diff --git a/taichi_slam/utils/visualization.py b/taichi_slam/utils/visualization.py
@@ -31,6 +31,7 @@ def __init__(self, RES_X, RES_Y):
         self.lock_pos_drone = False
         self.slice_z = 0.5
         self.enable_slice_z = False
+        self.enable_mesher = False
 
         self.disp_particles = True
         self.disp_mesh = True
@@ -57,6 +58,7 @@ def options(self):
         self.pcl_radius = window.GUI.slider_float("particles radius ",
                                             self.pcl_radius, 0.005, 0.03)
         self.lock_pos_drone = window.GUI.checkbox("Look Drone", self.lock_pos_drone)
+        self.enable_mesher = window.GUI.checkbox("Enable Mesher", self.enable_mesher)
         self.slice_z = window.GUI.slider_float("slice z",
                                             self.slice_z, 0, 2)
         self.enable_slice_z = window.GUI.checkbox("Show Z Slice", self.enable_slice_z)