Skip to content

Commit 3581e98

Browse files
xuhao1xuhao1
committed
Save/Loading TSDF.
1 parent b477388 commit 3581e98

File tree

10 files changed

+231
-12
lines changed

10 files changed

+231
-12
lines changed

‎data/ri_tsdf.npy

33.8 MB
Binary file not shown.

‎launch/taichislam-d435.launch

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -41,7 +41,7 @@
4141
disp_ceiling: 2.0
4242
disp_floor: -0.5
4343
texture_compressed: true
44-
voxel_size: 0.05
44+
voxel_size: 0.1
4545
disp:
4646
res_x: 1920
4747
res_y: 1080

‎scripts/taichislam_node.py

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -114,7 +114,6 @@ def get_general_mapping_opts(self):
114114
octo_opts = {
115115
'texture_enabled': self.texture_enabled,
116116
'max_disp_particles': max_disp_particles,
117-
'min_occupy_thres': occupy_thres,
118117
'map_scale':[map_size_xy, map_size_z],
119118
'voxel_size':voxel_size,
120119
'max_ray_length':max_ray_length,
@@ -127,6 +126,7 @@ def get_general_mapping_opts(self):
127126
def get_octo_opts(self):
128127
opts = self.get_general_mapping_opts()
129128
opts['K'] = rospy.get_param('K', 2)
129+
opts['min_occupy_thres'] = rospy.get_param('min_occupy_thres', 2)
130130
return opts
131131

132132
def get_sdf_opts(self):

‎taichi_slam/mapping/__init__.py

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,5 @@
11
from .taichi_octomap import *
22
from .dense_tsdf import *
33
from .submap_mapping import *
4-
from .marching_cube_mesher import *
4+
from .marching_cube_mesher import *
5+
from .topo_graph import *

‎taichi_slam/mapping/dense_tsdf.py

Lines changed: 74 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -3,13 +3,14 @@
33
import taichi as ti
44
import math
55
from .mapping_common import *
6+
import time
67

78
Wmax = 1000
89

910
var = [1, 2, 3, 4, 5]
1011
@ti.data_oriented
1112
class DenseTSDF(Basemap):
12-
def __init__(self, map_scale=[10, 10], voxel_size=0.05, min_occupy_thres=0, texture_enabled=False, \
13+
def __init__(self, map_scale=[10, 10], voxel_size=0.05, texture_enabled=False, \
1314
max_disp_particles=1000000, block_size=16, max_ray_length=10, min_ray_length=0.3,
1415
internal_voxels = 10, max_submap_size=1000, is_global_map=False,
1516
disp_ceiling=1.8, disp_floor=-0.3):
@@ -30,7 +31,6 @@ def __init__(self, map_scale=[10, 10], voxel_size=0.05, min_occupy_thres=0, text
3031
self.map_size_z = voxel_size * self.Nz
3132

3233
self.max_disp_particles = max_disp_particles
33-
self.min_occupy_thres = min_occupy_thres
3434

3535
self.enable_texture = texture_enabled
3636

@@ -347,3 +347,75 @@ def get_voxels_TSDF_surface(self):
347347
def get_voxels_TSDF_slice(self, z):
348348
self.cvt_TSDF_to_voxels_slice(z)
349349
return self.export_ESDF_xyz.to_numpy(), self.export_TSDF.to_numpy()
350+
351+
@ti.kernel
352+
def count_active(self) -> ti.i32:
353+
count = 0
354+
for s, i, j, k in self.TSDF:
355+
if s == self.active_submap_id[None]:
356+
if self.TSDF_observed[s, i, j, k] > 0:
357+
ti.atomic_add(count, 1)
358+
return count
359+
360+
@ti.kernel
361+
def to_numpy(self, data_indices: ti.any_arr(element_dim=1), data_tsdf: ti.types.ndarray(), data_wtsdf: ti.types.ndarray(), data_color:ti.types.ndarray()):
362+
# Never use it for submap collection! will be extreme large
363+
count = 0
364+
for s, i, j, k in self.TSDF:
365+
if s == self.active_submap_id[None]:
366+
if self.TSDF_observed[s, i, j, k] > 0:
367+
_count = ti.atomic_add(count, 1)
368+
data_indices[_count] = [i, j, k]
369+
data_tsdf[_count] = self.TSDF[s, i, j, k]
370+
data_wtsdf[_count] = self.W_TSDF[s, i, j, k]
371+
if ti.static(self.enable_texture):
372+
data_color[_count] = self.color[s, i, j, k]
373+
374+
@ti.kernel
375+
def load_numpy(self, data_indices: ti.any_arr(element_dim=1), data_tsdf: ti.types.ndarray(), data_wtsdf: ti.types.ndarray(), data_color:ti.types.ndarray()):
376+
for i in range(data_tsdf.shape[0]):
377+
ind = data_indices[i]
378+
sijk = 0, ind[0], ind[1], ind[2]
379+
self.TSDF[sijk] = data_tsdf[i]
380+
self.W_TSDF[sijk] = data_wtsdf[i]
381+
if ti.static(self.enable_texture):
382+
self.color[sijk] = data_color[i]
383+
self.TSDF_observed[sijk] = 1
384+
385+
def saveMap(self, filename):
386+
s = time.time()
387+
num = self.count_active()
388+
indices = np.zeros((num, 3), np.int32)
389+
TSDF = np.zeros((num), np.float32)
390+
W_TSDF = np.zeros((num), np.float32)
391+
if self.enable_texture:
392+
color = np.zeros((num, 3), np.float32)
393+
else:
394+
color = np.array([])
395+
self.to_numpy(indices, TSDF, W_TSDF, color)
396+
obj = {
397+
'indices': indices,
398+
'TSDF': TSDF,
399+
'W_TSDF': W_TSDF,
400+
'color': color,
401+
"map_scale": [self.map_size_xy, self.map_size_z],
402+
"voxel_size": self.voxel_size,
403+
"texture_enabled": self.enable_texture,
404+
"block_size": self.block_size,
405+
}
406+
e = time.time()
407+
print(f"[SubmapMapping] Saving map to {filename} {num} voxels takes {e-s:.1f} seconds")
408+
np.save(filename, obj)
409+
410+
@staticmethod
411+
def loadMap(filename):
412+
obj = np.load(filename, allow_pickle=True).item()
413+
TSDF = obj['TSDF']
414+
W_TSDF = obj['W_TSDF']
415+
color = obj['color']
416+
indices = obj['indices']
417+
mapping = DenseTSDF(map_scale=obj['map_scale'], voxel_size=obj['voxel_size'],
418+
texture_enabled=obj['texture_enabled'], block_size=obj['block_size'], is_global_map=True)
419+
mapping.load_numpy(indices, TSDF, W_TSDF, color)
420+
print(f"[SubmapMapping] Loaded {TSDF.shape[0]} voxels from {filename}")
421+
return mapping

‎taichi_slam/mapping/mapping_common.py

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -146,9 +146,9 @@ def set_pose(self, _R, _T):
146146
def init_colormap(self):
147147
self.colormap = ti.Vector.field(3, float, shape=1024)
148148
for i in range(1024):
149-
self.colormap[i][0] = cm.bwr(i/1024.0)[0]
150-
self.colormap[i][1] = cm.bwr(i/1024.0)[1]
151-
self.colormap[i][2] = cm.bwr(i/1024.0)[2]
149+
self.colormap[i][0] = cm.jet(i/1024.0)[0]
150+
self.colormap[i][1] = cm.jet(i/1024.0)[1]
151+
self.colormap[i][2] = cm.jet(i/1024.0)[2]
152152

153153
@ti.func
154154
def is_occupy(self, i, j, k):

‎taichi_slam/mapping/submap_mapping.py

Lines changed: 7 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -1,16 +1,15 @@
11
from taichi_slam.mapping.mapping_common import Basemap
22
from .dense_tsdf import DenseTSDF
33
import time
4-
import taichi as ti
4+
import numpy as np
55

66
class SubmapMapping:
77
submap_collection: Basemap
88
global_map: Basemap
9-
def __init__(self, submap_type=DenseTSDF, keyframe_step=50, sub_opts={}, global_opts={}):
9+
def __init__(self, submap_type=DenseTSDF, keyframe_step=20, sub_opts={}, global_opts={}):
1010
sdf_default_opts = {
1111
'map_scale': [10, 10],
1212
'voxel_size': 0.05,
13-
'min_occupy_thres': 3,
1413
'texture_enabled': False,
1514
'min_ray_length': 0.3,
1615
'max_ray_length': 3.0,
@@ -38,7 +37,6 @@ def create_globalmap(self, global_opts={}):
3837
sdf_default_opts = {
3938
'map_scale': [100, 100],
4039
'voxel_size': 0.05,
41-
'min_occupy_thres': 3,
4240
'texture_enabled': False,
4341
'min_ray_length': 0.3,
4442
'max_ray_length': 3.0,
@@ -94,6 +92,8 @@ def create_new_submap(self, frame_id, R, T):
9492
self.submaps[frame_id] = submap_id
9593

9694
print(f"[SubmapMapping] Created new submap, now have {submap_id+1} submaps")
95+
if submap_id % 2 == 0:
96+
self.saveMap("/home/xuhao/output/test_map.npy")
9797
return self.submap_collection
9898

9999
def need_create_new_submap(self, is_keyframe, R, T):
@@ -144,3 +144,6 @@ def cvt_TSDF_surface_to_voxels(self):
144144
self.global_map.max_disp_particles, self.export_TSDF_xyz, self.export_color)
145145
else:
146146
self.submap_collection.cvt_TSDF_surface_to_voxels()
147+
148+
def saveMap(self, filename):
149+
self.global_map.saveMap(filename)

‎taichi_slam/mapping/topo_graph.py

Lines changed: 19 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,19 @@
1+
import taichi as ti
2+
from .mapping_common import *
3+
@ti.data_oriented
4+
class TopoGraphGen:
5+
def __init__(self, mapping: Basemap):
6+
self.mapping = mapping
7+
self.generate_uniform_sample_points()
8+
9+
def generate_uniform_sample_points(self, npoints):
10+
self.sample_dirs = ti.Vector.field(3, dtype=ti.f32, shape=())
11+
vec = np.random.randn(3, npoints)
12+
vec /= np.linalg.norm(vec, axis=0)
13+
for i in range(npoints):
14+
self.sample_dirs[i] = ti.Vector(vec[:, i])
15+
16+
@ti.kernel
17+
def generate_topo_graph(self):
18+
pass
19+

‎tests/gen_topo.ipynb

Lines changed: 104 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,104 @@
1+
{
2+
"cells": [
3+
{
4+
"cell_type": "code",
5+
"execution_count": 1,
6+
"metadata": {},
7+
"outputs": [
8+
{
9+
"name": "stdout",
10+
"output_type": "stream",
11+
"text": [
12+
"[Taichi] version 1.0.4, llvm 10.0.0, commit c133fd4f, linux, python 3.8.5\n",
13+
"[I 07/14/22 04:28:58.464 58202] [shell.py:_shell_pop_print@33] Graphical python shell detected, using wrapped sys.stdout\n"
14+
]
15+
},
16+
{
17+
"name": "stderr",
18+
"output_type": "stream",
19+
"text": [
20+
"/home/xuhao/anaconda3/lib/python3.8/site-packages/taichi/__init__.py:71: DeprecationWarning: ti.any_arr is deprecated. Please use ti.types.ndarray instead.\n",
21+
" warnings.warn(\n"
22+
]
23+
},
24+
{
25+
"name": "stdout",
26+
"output_type": "stream",
27+
"text": [
28+
"[Taichi] Starting on arch=cuda\n",
29+
"[I 07/14/22 04:28:59.609 58202] [vulkan_device_creator.cpp:pick_physical_device@364] Found Vulkan Device 0 (NVIDIA GeForce RTX 3080)\n",
30+
"[I 07/14/22 04:28:59.609 58202] [vulkan_device_creator.cpp:pick_physical_device@364] Found Vulkan Device 1 (llvmpipe (LLVM 12.0.0, 256 bits))\n",
31+
"[I 07/14/22 04:28:59.609 58202] [vulkan_device_creator.cpp:find_queue_families@142] Async compute queue 2, graphics queue 0\n",
32+
"[I 07/14/22 04:28:59.609 58202] [vulkan_device_creator.cpp:find_queue_families@142] Async compute queue 2, graphics queue 0\n",
33+
"[I 07/14/22 04:28:59.609 58202] [vulkan_device_creator.cpp:create_logical_device@432] Vulkan Device \"NVIDIA GeForce RTX 3080\" supports Vulkan 0 version 1.3.205\n"
34+
]
35+
}
36+
],
37+
"source": [
38+
"%load_ext autoreload\n",
39+
"%autoreload 2\n",
40+
"\n",
41+
"import sys\n",
42+
"sys.path.append(\"../\")\n",
43+
"from taichi_slam.mapping import *\n",
44+
"from taichi_slam.utils.visualization import *\n",
45+
"\n",
46+
"ti.init(arch=ti.cuda)\n"
47+
]
48+
},
49+
{
50+
"cell_type": "code",
51+
"execution_count": 2,
52+
"metadata": {},
53+
"outputs": [
54+
{
55+
"name": "stdout",
56+
"output_type": "stream",
57+
"text": [
58+
"TSDF map initialized blocks 64x64x7\n",
59+
"[SubmapMapping] Loading 225288 voxels map from /home/xuhao/output/test_map.npy\n",
60+
"[W 07/14/22 04:29:04.521 58202] [vulkan_device.cpp:buffer@615] Overriding last binding\n"
61+
]
62+
}
63+
],
64+
"source": [
65+
"densemap = DenseTSDF.loadMap(\"/home/xuhao/output/test_map.npy\")\n",
66+
"densemap.cvt_TSDF_surface_to_voxels()\n"
67+
]
68+
},
69+
{
70+
"cell_type": "code",
71+
"execution_count": null,
72+
"metadata": {},
73+
"outputs": [],
74+
"source": []
75+
}
76+
],
77+
"metadata": {
78+
"kernelspec": {
79+
"display_name": "Python 3.8.5 ('base')",
80+
"language": "python",
81+
"name": "python3"
82+
},
83+
"language_info": {
84+
"codemirror_mode": {
85+
"name": "ipython",
86+
"version": 3
87+
},
88+
"file_extension": ".py",
89+
"mimetype": "text/x-python",
90+
"name": "python",
91+
"nbconvert_exporter": "python",
92+
"pygments_lexer": "ipython3",
93+
"version": "3.8.5"
94+
},
95+
"orig_nbformat": 4,
96+
"vscode": {
97+
"interpreter": {
98+
"hash": "08ce52785f0fedc81003ce387e097a83d6cc9494681cd746006386992005bb71"
99+
}
100+
}
101+
},
102+
"nbformat": 4,
103+
"nbformat_minor": 2
104+
}

‎tests/gen_topo_graph.py

Lines changed: 20 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,20 @@
1+
#!/usr/bin/env python
2+
import sys, os
3+
sys.path.insert(0,os.path.dirname(__file__) + "/../")
4+
from taichi_slam.mapping import *
5+
from taichi_slam.utils.visualization import *
6+
import time
7+
8+
if __name__ == "__main__":
9+
ti.init(arch=ti.cuda)
10+
densemap = DenseTSDF.loadMap(os.path.dirname(__file__) + "/../data/ri_tsdf.npy")
11+
densemap.cvt_TSDF_surface_to_voxels()
12+
render = TaichiSLAMRender(1920, 1080)
13+
render.pcl_radius = densemap.voxel_size/2
14+
render.set_particles(densemap.export_TSDF_xyz, densemap.export_color)
15+
while True:
16+
try:
17+
render.rendering()
18+
time.sleep(0.01)
19+
except KeyboardInterrupt:
20+
break

0 commit comments

Comments
 (0)