Skip to content

Commit

Permalink
Octomap works with submap fusion.
Browse files Browse the repository at this point in the history
  • Loading branch information
@xuhao1
xuhao1 committed Jan 12, 2023
1 parent f2061e3 commit 8ad6ba5
Show file tree
Hide file tree
Showing 13 changed files with 264 additions and 163 deletions.
6 changes: 3 additions & 3 deletions TaichiSLAM_demo.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -115,7 +115,7 @@ def ros_subscribe_pcl():
RES_Y = args.resolution[1]
disp_in_rviz = args.rviz

print(f"Res [{RES_X}x{RES_Y}] GPU {args.cuda} RVIZ {disp_in_rviz} size of map {args.map_size} grid {args.voxel_size} ")
print(f"Res [{RES_X}x{RES_Y}] GPU {args.cuda} RVIZ {disp_in_rviz} size of map {args.map_size} grid {args.voxel_scale} ")

if args.record:
ti.core.start_recording('./export/TaichiSLAM.yml')
Expand All @@ -141,14 +141,14 @@ def ros_subscribe_pcl():
max_disp_particles=args.max_disp_particles,
min_occupy_thres = args.occupy_thres,
map_scale=args.map_size,
voxel_size=args.voxel_size,
voxel_scale=args.voxel_scale,
K=args.K)
elif args.method == "esdf":
mapping = DenseTSDF(texture_enabled=args.texture_enabled,
max_disp_particles=args.max_disp_particles,
min_occupy_thres = args.occupy_thres,
map_scale=args.map_size,
voxel_size=args.voxel_size,
voxel_scale=args.voxel_scale,
num_voxel_per_blk_axis=args.blk)

scene.init_control(gui, radius=6, theta=-math.pi/4,center=(0, 0, 0), is_ortho=True)
Expand Down
8 changes: 4 additions & 4 deletions launch/taichislam-d435.launch
View file
Original file line number Diff line number Diff line change
@@ -1,11 +1,11 @@
<launch>
<arg name="use_cuda" default="true" />
<arg name="show" default="false" />
<arg name="mapping_type" default="tsdf" />
<arg name="mapping_type" default="octo" />
<arg name="texture_enabled" default="false" />
<arg name="enable_mesher" default="true" />
<arg name="enable_mesher" default="false" />
<arg name="output_map" default="false" />
<arg name="max_disp_particles" default="1048576" />
<arg name="max_disp_particles" default="10485760" />
<arg name="max_mesh" default="3000000" />
<arg name="max_ray_length" default="3.1" />
<arg name="min_ray_length" default="0.3" />
Expand Down Expand Up @@ -45,7 +45,7 @@
disp_ceiling: 1.8
disp_floor: -0.5
texture_compressed: true
voxel_size: 0.05
voxel_scale: 0.025
disp:
res_x: 1920
res_y: 1080
Expand Down
2 changes: 1 addition & 1 deletion launch/taichislam-quadcam.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,7 +36,7 @@
disp_ceiling: 1.0
disp_floor: -1.5
texture_compressed: true
voxel_size: 0.1
voxel_scale: 0.1
disp:
res_x: 1920
res_y: 1080
Expand Down
29 changes: 16 additions & 13 deletions scripts/taichislam_node.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,7 @@ def __init__(self):
RES_Y = rospy.get_param('~disp/res_y', 1080)
self.render = TaichiSLAMRender(RES_X, RES_Y)
self.render.enable_mesher = self.enable_mesher
self.render.pcl_radius = rospy.get_param('~voxel_size', 0.05)/2
self.render.pcl_radius = rospy.get_param('~voxel_scale', 0.05)/2

self.pub_occ = rospy.Publisher('/dense_mapping', PointCloud2, queue_size=10)
# self.pub_tsdf_surface = rospy.Publisher('/pub_tsdf_surface', PointCloud2, queue_size=10)
Expand Down Expand Up @@ -149,7 +149,7 @@ def get_general_mapping_opts(self):
occupy_thres = rospy.get_param('~occupy_thres', 0)
map_size_xy = rospy.get_param('~map_size_xy', 100)
map_size_z = rospy.get_param('~map_size_z', 10)
self.voxel_size = voxel_size = rospy.get_param('~voxel_size', 0.05)
self.voxel_scale = voxel_scale = rospy.get_param('~voxel_scale', 0.05)
max_ray_length = rospy.get_param('~max_ray_length', 5.1)
min_ray_length = rospy.get_param('~min_ray_length', 0.3)
disp_ceiling = rospy.get_param('~disp_ceiling', 1.8)
Expand All @@ -158,7 +158,7 @@ def get_general_mapping_opts(self):
'texture_enabled': self.texture_enabled,
'max_disp_particles': max_disp_particles,
'map_scale':[map_size_xy, map_size_z],
'voxel_size':voxel_size,
'voxel_scale':voxel_scale,
'max_ray_length':max_ray_length,
'min_ray_length':min_ray_length,
'disp_ceiling':disp_ceiling,
Expand Down 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.global_map, 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_scale*5)
self.mapping.map_send_handle = self.send_submap_handle
self.mapping.traj_send_handle = self.traj_send_handle
else:
Expand All @@ -215,14 +215,15 @@ def initial_mapping(self):
opts = self.get_sdf_opts()
self.mapping = DenseTSDF(**opts)
if self.enable_mesher:
self.mesher = MarchingCubeMesher(self.mapping, self.max_mesh, tsdf_surface_thres=self.voxel_size*5)
self.mesher = MarchingCubeMesher(self.mapping, self.max_mesh, tsdf_surface_thres=self.voxel_scale*5)
self.mapping.set_color_camera_intrinsic(self.Kcolor)
self.mapping.set_dep_camera_intrinsic(self.Kdep)


def init_topology_generator(self):
if not self.skeleton_graph_gen:
self.topo = None
return
print("Initializing skeleton graph generator thread...")
from multiprocessing import Process, Manager
from topo_gen_thread import TopoGenThread
Expand Down Expand Up @@ -295,12 +296,16 @@ def process_pcl_pose(self, msg, pose):
def rendering(self):
start_time = time.time()
mapping = self.mapping
if self.mapping_type == "tsdf" and self.enable_rendering:
if self.render.enable_slice_z:
mapping.cvt_TSDF_to_voxels_slice(self.render.slice_z)
else:
mapping.cvt_TSDF_surface_to_voxels()
self.render.set_particles(mapping.export_TSDF_xyz, mapping.export_color, mapping.num_TSDF_particles[None])
if self.enable_rendering:
if self.mapping_type == "tsdf":
if self.render.enable_slice_z:
mapping.cvt_TSDF_to_voxels_slice(self.render.slice_z)
else:
mapping.cvt_TSDF_surface_to_voxels()
self.render.set_particles(mapping.export_TSDF_xyz, mapping.export_color, mapping.num_TSDF_particles[None])
if self.mapping_type == "octo":
mapping.cvt_occupy_to_voxels(self.disp_level)
self.render.set_particles(mapping.export_x, mapping.export_color, mapping.num_export_particles[None])
self.render.rendering()
return (time.time() - start_time)*1000

Expand All @@ -325,8 +330,6 @@ def output(self, R, T):
if self.output_map:
self.pub_to_ros(mapping.export_x.to_numpy()[:par_count],
mapping.export_color.to_numpy()[:par_count], mapping.enable_texture)
if self.enable_rendering:
self.render.set_particles(mapping.export_x, mapping.export_color)
else:
if self.enable_rendering and self.render.enable_mesher:
mesher = self.mesher
Expand Down
46 changes: 23 additions & 23 deletions taichi_slam/mapping/dense_esdf.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,7 @@
var = [1, 2, 3, 4, 5]
@ti.data_oriented
class DenseSDF(BaseMap):
def __init__(self, map_scale=[10, 10], voxel_size=0.05, min_occupy_thres=0, texture_enabled=False, \
def __init__(self, map_scale=[10, 10], voxel_scale=0.05, min_occupy_thres=0, texture_enabled=False, \
max_disp_particles=1000000, num_voxel_per_blk_axis=16, max_ray_length=10, min_ray_length=0.3,
enable_esdf=False, internal_voxels = 10, max_submap_num=1000, is_global_map=False,
disp_ceiling=1.8, disp_floor=-0.3):
Expand All @@ -18,16 +18,16 @@ def __init__(self, map_scale=[10, 10], voxel_size=0.05, min_occupy_thres=0, text
self.map_size_z = map_scale[1]

self.num_voxel_per_blk_axis = num_voxel_per_blk_axis
self.voxel_size = voxel_size
self.voxel_scale = voxel_scale

self.N = math.ceil(map_scale[0] / voxel_size/num_voxel_per_blk_axis)*num_voxel_per_blk_axis
self.Nz = math.ceil(map_scale[1] / voxel_size/num_voxel_per_blk_axis)*num_voxel_per_blk_axis
self.N = math.ceil(map_scale[0] / voxel_scale/num_voxel_per_blk_axis)*num_voxel_per_blk_axis
self.Nz = math.ceil(map_scale[1] / voxel_scale/num_voxel_per_blk_axis)*num_voxel_per_blk_axis

self.block_num_xy = math.ceil(map_scale[0] / voxel_size/num_voxel_per_blk_axis)
self.block_num_z = math.ceil(map_scale[1] / voxel_size/num_voxel_per_blk_axis)
self.block_num_xy = math.ceil(map_scale[0] / voxel_scale/num_voxel_per_blk_axis)
self.block_num_z = math.ceil(map_scale[1] / voxel_scale/num_voxel_per_blk_axis)

self.map_size_xy = voxel_size * self.N
self.map_size_z = voxel_size * self.Nz
self.map_size_xy = voxel_scale * self.N
self.map_size_z = voxel_scale * self.Nz

self.max_disp_particles = max_disp_particles
self.min_occupy_thres = min_occupy_thres
Expand All @@ -36,8 +36,8 @@ def __init__(self, map_scale=[10, 10], voxel_size=0.05, min_occupy_thres=0, text

self.max_ray_length = max_ray_length
self.min_ray_length = min_ray_length
self.tsdf_surface_thres = self.voxel_size
self.gamma = self.voxel_size
self.tsdf_surface_thres = self.voxel_scale
self.gamma = self.voxel_scale
self.enable_esdf = enable_esdf
self.internal_voxels = internal_voxels
self.max_submap_num = max_submap_num
Expand Down Expand Up @@ -113,7 +113,7 @@ 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], ti.f32)
self.voxel_scale_ = ti.Vector([self.voxel_scale, self.voxel_scale, self.voxel_scale], 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)
Expand Down Expand Up @@ -160,7 +160,7 @@ def init_fields(self):
@ti.kernel
def init_sphere(self):
voxels = 30
radius = self.voxel_size*3
radius = self.voxel_scale*3
print(radius)
for i in range(self.N/2-voxels/2, self.N/2+voxels/2):
for j in range(self.N/2-voxels/2, self.N/2+voxels/2):
Expand All @@ -186,7 +186,7 @@ def assert_coor(self, _i):

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

@ti.func
def i_j_k_to_xyz(self, i, j, k):
Expand All @@ -199,25 +199,25 @@ def submap_i_j_k_to_xyz(self, s, i, j, k):

@ti.func
def xyz_to_ijk(self, xyz):
ijk = xyz / self.voxel_size_ + self.NC_
ijk = xyz / self.voxel_scale_ + self.NC_
return self.constrain_coor(ijk)

@ti.func
def xyz_to_0ijk(self, xyz):
ijk = xyz / self.voxel_size_ + self.NC_
ijk = xyz / self.voxel_scale_ + self.NC_
_ijk = self.constrain_coor(ijk)
return ti.Vector([0, _ijk[0], _ijk[1], _ijk[2]], ti.i32)

@ti.func
def sxyz_to_ijk(self, s, xyz):
ijk = xyz / self.voxel_size_ + self.NC_
ijk = xyz / self.voxel_scale_ + self.NC_
ijk_ = self.constrain_coor(ijk)
return [s, ijk_[0], ijk_[1], ijk_[2]]

@ti.func
def w_x_p(self, d, z):
epi = ti.static(self.voxel_size)
theta = ti.static(self.voxel_size*4)
epi = ti.static(self.voxel_scale)
theta = ti.static(self.voxel_scale*4)
ret = 0.0
if d > ti.static(-epi):
ret = 1.0/(z*z)
Expand Down Expand Up @@ -283,7 +283,7 @@ def process_lower_queue(self):
n_voxel_ijk = dir + _index_head
self.assert_coor(n_voxel_ijk) #L219
if ti.is_active(self.Broot, n_voxel_ijk):
dis = dir.norm()*self.voxel_size
dis = dir.norm()*self.voxel_scale
n_esdf = self.ESDF[n_voxel_ijk]
_n_esdf = self.ESDF[_index_head] + dis
if n_esdf > 0 and _n_esdf < n_esdf:
Expand Down Expand Up @@ -424,10 +424,10 @@ def process_new_pcl(self):
z = self.new_pcl_z[i, j, k]/c

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)
ray_cast_voxels = ti.min(len_pos_s2p/self.voxel_scale+ti.static(self.internal_voxels), self.max_ray_length/self.voxel_scale)
for _j in range(ray_cast_voxels):
j_f += 1.0
x_ = d_s2p*j_f*self.voxel_size + self.input_T[None]
x_ = d_s2p*j_f*self.voxel_scale + self.input_T[None]
xi = self.sxyz_to_ijk(submap_id, x_)

#v2p: vector from current voxel to point, e.g. p-x
Expand Down Expand Up @@ -501,7 +501,7 @@ def cvt_ESDF_to_voxels_slice(self, z: ti.template()):
self.num_export_ESDF_particles[None] = 0
for s, i, j, k in self.ESDF:
if s == self.active_submap_id[None]:
_index = (z+self.map_size_[2]/2)/self.voxel_size
_index = (z+self.map_size_[2]/2)/self.voxel_scale
if _index - 0.5 < k < _index + 0.5:
index = ti.atomic_add(self.num_export_ESDF_particles[None], 1)
if self.num_export_ESDF_particles[None] < self.max_disp_particles:
Expand All @@ -510,7 +510,7 @@ def cvt_ESDF_to_voxels_slice(self, z: ti.template()):

@ti.kernel
def cvt_TSDF_to_voxels_slice_kernel(self, z: ti.template(), dz:ti.template()):
_index = int((z+self.map_size_[2]/2.0)/self.voxel_size)
_index = int((z+self.map_size_[2]/2.0)/self.voxel_scale)
# Number for ESDF
self.num_TSDF_particles[None] = 0
for s, i, j, k in self.TSDF:
Expand Down
Loading

0 comments on commit 8ad6ba5

Please sign in to comment.