Skip to content

Commit

Permalink
Mesher works with submap fusion.
Browse files Browse the repository at this point in the history
  • Loading branch information
@xuhao1
xuhao1 committed Jan 11, 2023
1 parent 2c9fb74 commit f2061e3
Show file tree
Hide file tree
Showing 5 changed files with 33 additions and 29 deletions.
2 changes: 1 addition & 1 deletion launch/taichislam-d435.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,7 @@
<arg name="show" default="false" />
<arg name="mapping_type" default="tsdf" />
<arg name="texture_enabled" default="false" />
<arg name="enable_mesher" default="false" />
<arg name="enable_mesher" default="true" />
<arg name="output_map" default="false" />
<arg name="max_disp_particles" default="1048576" />
<arg name="max_mesh" default="3000000" />
Expand Down
4 changes: 2 additions & 2 deletions scripts/taichislam_node.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -204,7 +204,7 @@ def initial_mapping(self):
self.mapping = SubmapMapping(DenseTSDF, global_opts=gopts, sub_opts=subopts, keyframe_step=self.keyframe_step)
self.mapping.post_local_to_global_callback = self.post_submapfusion_callback
if self.enable_mesher:
self.mesher = MarchingCubeMesher(self.mapping.submap_collection, self.max_mesh, tsdf_surface_thres=self.voxel_size*5)
self.mesher = MarchingCubeMesher(self.mapping.global_map, self.max_mesh, tsdf_surface_thres=self.voxel_size*5)
self.mapping.map_send_handle = self.send_submap_handle
self.mapping.traj_send_handle = self.traj_send_handle
else:
Expand Down Expand Up @@ -334,7 +334,7 @@ def output(self, R, T):
mesher.generate_mesh(1)
t_mesh = (time.time() - start_time)*1000
if self.enable_rendering:
self.render.set_mesh(mesher.mesh_vertices, mesher.mesh_colors, mesher.mesh_normals, mesher.num_vetices[None])
self.render.set_mesh(mesher.mesh_vertices, mesher.mesh_colors, mesher.mesh_normals, mesh_num=mesher.num_vertices[None])
else:
if self.output_map:
start_time = time.time()
Expand Down
4 changes: 3 additions & 1 deletion taichi_slam/mapping/dense_tsdf.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -441,7 +441,9 @@ def load_numpy(self, submap_id:ti.i32, data_indices: ti.types.ndarray(element_di
self.W_TSDF[sijk] = data_wtsdf[i]
self.occupy[sijk] = data_occ[i]
if ti.static(self.enable_texture):
self.color[sijk] = data_color[i]
self.color[sijk][0] = data_color[i, 0]
self.color[sijk][1] = data_color[i, 1]
self.color[sijk][2] = data_color[i, 2]
self.TSDF_observed[sijk] = 1

def export_submap(self):
Expand Down
51 changes: 26 additions & 25 deletions taichi_slam/mapping/marching_cube_mesher.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,7 @@ def __init__(self, mapping, max_triangles=1000000, tsdf_surface_thres=0.1):
self.mesh_indices = None #ti.field(int, max_triangles*3)

self.num_facelets = ti.field(dtype=ti.i32, shape=())
self.num_vetices = ti.field(dtype=ti.i32, shape=())
self.num_vertices = ti.field(dtype=ti.i32, shape=())
self.mapping = mapping
self.enable_texture = mapping.enable_texture
self.tsdf_surface_thres = tsdf_surface_thres
Expand All @@ -29,11 +29,8 @@ def __init__(self, mapping, max_triangles=1000000, tsdf_surface_thres=0.1):
def init_tables(self):
self.edgeTable = ti.field(dtype=ti.i32, shape=edgeTable.shape[0])
self.triTable = ti.field(dtype=ti.i32, shape=triTable.shape)
for i in range(edgeTable.shape[0]):
self.edgeTable[i] = edgeTable[i]
for i in range(triTable.shape[0]):
for j in range(triTable.shape[1]):
self.triTable[i, j] = triTable[i, j]
self.edgeTable.from_numpy(edgeTable)
self.triTable.from_numpy(triTable)

@ti.kernel
def init_vertices(self):
Expand All @@ -42,7 +39,7 @@ def init_vertices(self):

@ti.func
def ijk_to_xyz(self, ijk):
return (ijk.transpose() - self.mapping.NC_)*self.mapping.voxel_size_
return ijk.transpose()*self.mapping.voxel_size_

@ti.func
def vertexInterp(self, _p1, _p2, valp1, valp2, isolevel):
Expand Down Expand Up @@ -85,23 +82,24 @@ def vertexInterp_color(self, _p1, _p2, valp1, valp2, c1, c2, isolevel):
return p_ret, p_color

@ti.func
def generate_normal(self, p, tsdf):
def generate_normal(self, s, _p, tsdf):
p = ti.cast(ti.round(_p), ti.i32)
#This may not efficient because duplicate compuation, but it's OK
normal_x = tsdf[p[0] + 1, p[1], p[2]] - tsdf[p[0] - 1, p[1], p[2]]
normal_y = tsdf[p[0], p[1] + 1, p[2]] - tsdf[p[0], p[1] - 1, p[2]]
normal_z = tsdf[p[0], p[1], p[2] + 1] - tsdf[p[0], p[1], p[2] - 1]
normal_x = tsdf[s, p[0, 0] + 1, p[0, 1], p[0, 2]] - tsdf[s, p[0, 0] - 1, p[0, 1], p[0, 2]]
normal_y = tsdf[s, p[0, 0], p[0, 1] + 1, p[0, 2]] - tsdf[s, p[0, 0], p[0, 1] - 1, p[0, 2]]
normal_z = tsdf[s, p[0, 0], p[0, 1], p[0, 2] + 1] - tsdf[s, p[0, 0], p[0, 1], p[0, 2] - 1]
normal = ti.Vector([normal_x, normal_y, normal_z])
normal = normal.normalized()
return normal

@ti.func
def add_triangle(self, p0, p1, p2, tsdf, triangle_idx):
def add_triangle(self, s, p0, p1, p2, tsdf, triangle_idx):
self.mesh_vertices[triangle_idx*3] = self.ijk_to_xyz(p0)
self.mesh_vertices[triangle_idx*3 + 1] = self.ijk_to_xyz(p1)
self.mesh_vertices[triangle_idx*3 + 2] = self.ijk_to_xyz(p2)
self.mesh_normals[triangle_idx*3] = self.generate_normal(p0, tsdf)
self.mesh_normals[triangle_idx*3 + 1] = self.generate_normal(p1, tsdf)
self.mesh_normals[triangle_idx*3 + 2] = self.generate_normal(p2, tsdf)
self.mesh_normals[triangle_idx*3] = self.generate_normal(s, p0, tsdf)
self.mesh_normals[triangle_idx*3 + 1] = self.generate_normal(s, p1, tsdf)
self.mesh_normals[triangle_idx*3 + 2] = self.generate_normal(s, p2, tsdf)

@ti.func
def add_triangle_color(self, c0, c1, c2, triangle_idx):
Expand All @@ -110,11 +108,11 @@ def add_triangle_color(self, c0, c1, c2, triangle_idx):
self.mesh_colors[triangle_idx*3+2] = c2.transpose()

@ti.func
def add_triangles_by_cubeindex(self, __k, tsdf, vertlist, vertcolor, cubeindex):
def add_triangles_by_cubeindex(self, s, __k, tsdf, vertlist, vertcolor, cubeindex):
_k = __k*3
if not self.triTable[cubeindex, _k] == -1:
index = ti.atomic_add(self.num_facelets[None], 1)
self.add_triangle(
self.add_triangle(s,
vertlist[self.triTable[cubeindex, _k], :],
vertlist[self.triTable[cubeindex, _k + 1], :],
vertlist[self.triTable[cubeindex, _k + 2], :],
Expand All @@ -127,14 +125,14 @@ def add_triangles_by_cubeindex(self, __k, tsdf, vertlist, vertcolor, cubeindex):
index)

@ti.func
def marching_on_a_cube(self, i, j, k, obs, tsdf, color, step):
def marching_on_a_cube(self, s, i, j, k, obs, tsdf, color, step):
isolevel = 0.0
#Marching...
loop_should_end = 0
cubevalue = ti.Vector([0. for i in range(8)])
for _i in ti.static(range(8)):
dx, dy, dz = ti.static(grid_xyz[_i][0], grid_xyz[_i][1], grid_xyz[_i][2])
indices = [i + dx*step, j + dy*step, k + dz*step]
indices = [s, i + dx*step, j + dy*step, k + dz*step]
cubevalue[_i] = tsdf[indices]
if obs[indices] == 0:
loop_should_end = 1
Expand All @@ -154,23 +152,26 @@ def marching_on_a_cube(self, i, j, k, obs, tsdf, color, step):
if iEdgeFlags & (1<<_j) > 0:
_p0 = ti.static(edges_grid_xyz)[_j][0]
_p1 = ti.static(edges_grid_xyz)[_j][1]
idx_p0 = ti.Vector([s, i + _p0[0]*step, j + _p0[1]*step, k + _p0[2]*step])
idx_p1 = ti.Vector([s, i + _p1[0]*step, j + _p1[1]*step, k + _p1[2]*step])
p0 = ti.Vector([i + _p0[0]*step, j + _p0[1]*step, k + _p0[2]*step])
p1 = ti.Vector([i + _p1[0]*step, j + _p1[1]*step, k + _p1[2]*step])
if self.enable_texture:
p, c = self.vertexInterp_color(p0, p1, tsdf[p0], tsdf[p1], color[p0], color[p1], isolevel)
p, c = self.vertexInterp_color(p0, p1, tsdf[idx_p0], tsdf[idx_p1],
color[idx_p0], color[idx_p1], isolevel)
vertlist[_j, 0] = p[0]
vertlist[_j, 1] = p[1]
vertlist[_j, 2] = p[2]
vertcolor[_j, 0] = c[0]
vertcolor[_j, 1] = c[1]
vertcolor[_j, 2] = c[2]
else:
p = self.vertexInterp(p0, p1, tsdf[p0], tsdf[p1], isolevel)
p = self.vertexInterp(p0, p1, tsdf[idx_p0], tsdf[idx_p1], isolevel)
vertlist[_j, 0] = p[0]
vertlist[_j, 1] = p[1]
vertlist[_j, 2] = p[2]
for __k in range(5):
self.add_triangles_by_cubeindex(__k, tsdf, vertlist, vertcolor, cubeindex)
self.add_triangles_by_cubeindex(s, __k, tsdf, vertlist, vertcolor, cubeindex)
if self.num_facelets[None] > self.max_triangles:
ret = -1
break
Expand All @@ -179,9 +180,9 @@ def marching_on_a_cube(self, i, j, k, obs, tsdf, color, step):
@ti.kernel
def generate_mesh_kernel(self, obs:ti.template(), tsdf: ti.template(), w_tsdf: ti.template(), color: ti.template(), step:ti.i32):
self.num_facelets[None] = 0
for i, j, k in tsdf:
if obs[i, j, k] > 0 and tsdf[i, j, k] < self.tsdf_surface_thres:
ret = self.marching_on_a_cube(i, j, k, obs, tsdf, color, step)
for s, i, j, k in tsdf:
if obs[s, i, j, k] > 0 and tsdf[s, i, j, k] < self.tsdf_surface_thres:
ret = self.marching_on_a_cube(s, i, j, k, obs, tsdf, color, step)

print("Total triangles", self.num_facelets[None])

Expand Down
1 change: 1 addition & 0 deletions taichi_slam/utils/communication.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,7 @@ def handle_traj(self, channel, data):
msg = Buffer.decode(data)
if msg.msg_id in self.sent_msgs:
return
self.sent_msgs.add(msg.msg_id)
# print(f"Received message on channel {channel} msg_id {msg.msg_id}")
self.on_traj(msg.buffer)

Expand Down

0 comments on commit f2061e3

Please sign in to comment.