Skip to content

Commit

Permalink
Small fixs.
Browse files Browse the repository at this point in the history
  • Loading branch information
@xuhao1
xuhao1 committed Jul 7, 2022
1 parent 7c7e980 commit f41cc1b
Show file tree
Hide file tree
Showing 4 changed files with 19 additions and 20 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 @@ -31,7 +31,7 @@

<rosparam>
texture_compressed: true
voxel_size: 0.04
voxel_size: 0.1
disp:
res_x: 1024
res_y: 768
Expand Down
3 changes: 2 additions & 1 deletion scripts/taichislam_node.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -154,7 +154,8 @@ def update(self):
self.cur_pose = None
else:
if self.mapping_type == "tsdf" and self.enable_rendering:
self.mapping.cvt_TSDF_to_voxels_slice(self.render.slice_z)
if self.render.enable_slice_z:
self.mapping.cvt_TSDF_to_voxels_slice(self.render.slice_z)
self.render.set_particles(self.mapping.export_TSDF_xyz, self.mapping.export_color)


Expand Down
31 changes: 14 additions & 17 deletions taichi_slam/mapping/dense_esdf.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -73,17 +73,17 @@ def initialize_fields(self):
self.export_ESDF = ti.field(dtype=ti.f32, shape=self.max_disp_particles)
self.export_ESDF_xyz = ti.Vector.field(3, dtype=ti.f32, shape=self.max_disp_particles)

self.voxel_size_ = ti.Vector([self.voxel_size, self.voxel_size, self.voxel_size])
self.map_size_ = ti.Vector([self.map_size_xy, self.map_size_xy, self.map_size_z])
self.NC_ = ti.Vector([self.N/2, self.N/2, self.Nz/2])
self.N_ = ti.Vector([self.N, self.N, self.Nz])
self.voxel_size_ = ti.Vector([self.voxel_size, self.voxel_size, self.voxel_size], ti.f32)
self.map_size_ = ti.Vector([self.map_size_xy, self.map_size_xy, self.map_size_z], ti.f32)
self.NC_ = ti.Vector([self.N/2, self.N/2, self.Nz/2], ti.f32)
self.N_ = ti.Vector([self.N, self.N, self.Nz], ti.f32)

self.occupy = ti.field(dtype=ti.i32)
self.TSDF = ti.field(dtype=ti.f32)
self.W_TSDF = ti.field(dtype=ti.f32)
self.ESDF = ti.field(dtype=ti.f32)
self.observed = ti.field(dtype=ti.i8)
self.TSDF_observed = ti.field(dtype=ti.i8)
self.TSDF_observed = ti.field(dtype=ti.i32)
self.fixed = ti.field(dtype=ti.i8)
self.parent_dir = ti.Vector.field(3, dtype=ti.i32)

Expand Down Expand Up @@ -126,7 +126,7 @@ def initialize_fields(self):
for _dj in range(-1, 2):
for _dk in range(-1, 2):
if _di !=0 or _dj !=0 or _dk != 0:
self.neighbors.append(ti.Vector([_di, _dj, _dk]))
self.neighbors.append(ti.Vector([_di, _dj, _dk], ti.f32))

self.colormap = ti.Vector.field(3, float, shape=1024)
self.color_rate = 2
Expand All @@ -140,9 +140,9 @@ def initialize_fields(self):
@ti.kernel
def init_fields(self):
for i in range(self.max_disp_particles):
self.export_color[i] = ti.Vector([0.5, 0.5, 0.5])
self.export_x[i] = ti.Vector([-100000, -100000, -100000])
self.export_TSDF_xyz[i] = ti.Vector([-100000, -100000, -100000])
self.export_color[i] = ti.Vector([0.5, 0.5, 0.5], ti.f32)
self.export_x[i] = ti.Vector([-100000, -100000, -100000], ti.f32)
self.export_TSDF_xyz[i] = ti.Vector([-100000, -100000, -100000], ti.f32)

@ti.kernel
def init_sphere(self):
Expand All @@ -158,7 +158,7 @@ def init_sphere(self):
self.color[i, j, k] = self.colormap[int((p[2]-0.5)/radius*0.5*1024)]
@ti.func
def constrain_coor(self, _i):
_i = _i.cast(int)
_i = _i.cast(ti.i32)
for d in ti.static(range(3)):
if _i[d] >= self.N_[d]:
_i[d] = self.N_[d] - 1
Expand All @@ -177,7 +177,7 @@ def ijk_to_xyz(self, ijk):

@ti.func
def i_j_k_to_xyz(self, i, j, k):
return self.ijk_to_xyz(ti.Vector([i, j, k]))
return self.ijk_to_xyz(ti.Vector([i, j, k], ti.f32))

@ti.func
def xyz_to_ijk(self, xyz):
Expand Down Expand Up @@ -277,7 +277,7 @@ def propogate_esdf(self):
self.head_lower_queue[None] = 0
count_update_tsdf = 0
for i, j, k in self.updated_TSDF:
_voxel_ijk = ti.Vector([i, j, k])
_voxel_ijk = ti.Vector([i, j, k], ti.f32)
t_d = self.TSDF[_voxel_ijk]
count_update_tsdf += 1
if self.is_fixed(_voxel_ijk):
Expand Down Expand Up @@ -318,7 +318,7 @@ def recast_pcl_to_map_kernel(self, xyz_array: ti.ext_arr(), rgb_array: ti.ext_ar
pt = ti.Vector([
xyz_array[index,0],
xyz_array[index,1],
xyz_array[index,2]])
xyz_array[index,2]], ti.f32)
pt = self.input_R[None]@pt
if ti.static(self.TEXTURE_ENABLED):
self.process_point(pt, rgb_array[index])
Expand All @@ -341,7 +341,7 @@ def recast_depth_to_map_kernel(self, depthmap: ti.ext_arr(), texture: ti.ext_arr
pt = ti.Vector([
(i-cx)*dep/fx,
(j-cy)*dep/fy,
dep])
dep], ti.f32)

if pt.norm() > ti.static(self.max_ray_length) or pt.norm() < ti.static(self.min_ray_length):
continue
Expand Down Expand Up @@ -385,7 +385,6 @@ def process_point(self, pt, rgb=None):

@ti.func
def process_new_pcl(self):
count = 0
for i, j, k in self.new_pcl_count:
c = self.new_pcl_count[i, j, k]
pos_s2p = self.new_pcl_sum_pos[i, j, k]/c
Expand Down Expand Up @@ -414,8 +413,6 @@ def process_new_pcl(self):

self.W_TSDF[xi] = ti.min(self.W_TSDF[xi]+w_x_p, Wmax)
self.updated_TSDF[xi] = 1
count += 1

if ti.static(self.TEXTURE_ENABLED):
self.color[xi] = self.new_pcl_sum_color[i, j, k]/ c/255.0

Expand Down
3 changes: 2 additions & 1 deletion taichi_slam/utils/visualization.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,7 @@ def __init__(self, RES_X, RES_Y):
self.scale_rate = 5
self.lock_pos_drone = False
self.slice_z = 0.5
self.enable_slice_z = False

self.disp_particles = True
self.disp_mesh = True
Expand Down Expand Up @@ -58,7 +59,7 @@ def options(self):
self.lock_pos_drone = window.GUI.checkbox("Look Drone", self.lock_pos_drone)
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)
self.disp_particles = window.GUI.checkbox("Particle", self.disp_particles)
self.disp_mesh = window.GUI.checkbox("Mesh", self.disp_mesh)
self.camera_distance = window.GUI.slider_float("camera_distance",
Expand Down

0 comments on commit f41cc1b

Please sign in to comment.