From de7b5bdcd3c84f9a618856ffca6705bc8cc33fa5 Mon Sep 17 00:00:00 2001
From: Gernot Riegler <riegler@icg.tugraz.at>
Date: Mon, 24 Apr 2017 10:03:34 +0200
Subject: [PATCH] added visualization to example/00_create_data

---
 .gitignore                            |   3 +
 example/00_create_data/create_data.py |  25 ++++-
 example/00_create_data/vis.py         | 132 ++++++++++++++++++++++++++
 3 files changed, 155 insertions(+), 5 deletions(-)
 create mode 100644 example/00_create_data/vis.py

diff --git a/.gitignore b/.gitignore
index 3df2b0f..f24758c 100644
--- a/.gitignore
+++ b/.gitignore
@@ -8,3 +8,6 @@ py/pyoctnet.so
 th/cpu/build
 th/gpu/build
 doc/generated_doc/
+example/**/*.pyc
+example/**/*.ply
+
diff --git a/example/00_create_data/create_data.py b/example/00_create_data/create_data.py
index 6a3caff..dbc6928 100755
--- a/example/00_create_data/create_data.py
+++ b/example/00_create_data/create_data.py
@@ -10,6 +10,8 @@
 import urllib
 import zipfile
 
+import vis
+
 sys.path.append('../../py/')
 import pyoctnet
 
@@ -31,13 +33,26 @@
 oc_from_dense1 = pyoctnet.Octree.create_from_dense(dense, val_range, n_threads=n_threads)
 oc_from_dense2 = pyoctnet.Octree.create_from_dense2(dense, dense[np.newaxis,...].copy(), n_threads=n_threads)
 
+vis.write_ply_voxels('dense.ply', dense)
+vis.write_ply_voxels('oc_from_dense1.ply', oc_from_dense1.to_cdhw())
+vis.write_ply_voxels('oc_from_dense2.ply', oc_from_dense2.to_cdhw())
+
+
 # create from point cloud
 xyz = np.random.normal(0, 1, (100, 3)).astype(np.float32)
+
+xyz_min, xyz_max = xyz.min(axis=0), xyz.max(axis=0)
+src_width = (xyz_max - xyz_min).max()
+xyz_ctr = (xyz_max + xyz_min) / 2
+xyz_scaled = (xyz - xyz_ctr) / src_width * vx_res + vx_res/2
+
 features = np.ones_like(xyz)
-oc_from_pcl = pyoctnet.Octree.create_from_pc(xyz, features, vx_res,vx_res,vx_res, normalize=True, n_threads=n_threads)
+oc_from_pcl1 = pyoctnet.Octree.create_from_pc(xyz, features, vx_res,vx_res,vx_res, normalize=True, n_threads=n_threads)
+
+oc_from_pcl2 = pyoctnet.Octree.create_from_pc(xyz_scaled, features, vx_res,vx_res,vx_res, normalize=False, n_threads=n_threads)
 
-for idx in range(3):
-  xyz[...,idx] = (xyz[...,idx] - xyz[...,idx].min()) / (xyz[...,idx].max() - xyz[...,idx].min())
-  xyz[...,idx] *= vx_res
-oc_from_pcl = pyoctnet.Octree.create_from_pc(xyz, features, vx_res,vx_res,vx_res, normalize=False, n_threads=n_threads)
+vis.write_ply_pcl('xyz.ply', xyz)
+vis.write_ply_pcl('xyz_scaled.ply', xyz_scaled)
+vis.write_ply_voxels('oc_from_pcl1.ply', oc_from_pcl1.to_cdhw()[0])
+vis.write_ply_voxels('oc_from_pcl2.ply', oc_from_pcl2.to_cdhw()[0])
 
diff --git a/example/00_create_data/vis.py b/example/00_create_data/vis.py
new file mode 100644
index 0000000..e4ca1df
--- /dev/null
+++ b/example/00_create_data/vis.py
@@ -0,0 +1,132 @@
+import numpy as np
+
+def write_ply_pcl(out_path, xyz, color=[128,128,128], color_array=None):
+  if xyz.shape[1] != 3:
+    raise Exception('xyz has to be Nx3')
+
+  f = open(out_path, 'w')
+  f.write('ply\n')
+  f.write('format ascii 1.0\n')
+  f.write('element vertex %d\n' % xyz.shape[0])
+  f.write('property float32 x\n')
+  f.write('property float32 y\n')
+  f.write('property float32 z\n')
+  f.write('property uchar red\n')
+  f.write('property uchar green\n')
+  f.write('property uchar blue\n')
+  f.write('end_header\n')
+
+  for row in xrange(xyz.shape[0]):
+    xyz_row = xyz[row]
+    if color_array is not None:
+      c = color_array[row]
+    else:
+      c = color
+    f.write('%f %f %f %d %d %d\n' % (xyz_row[0],xyz_row[1],xyz_row[2], c[0],c[1],c[2]))
+  f.close()
+
+
+def write_ply_boxes(out_path, bxs, binary=False):
+  f = open(out_path, 'wb')
+  f.write("ply\n");
+  if binary:
+    f.write("format binary_little_endian 1.0\n")
+  else:
+    f.write("format ascii 1.0\n")
+  f.write("element vertex %d\n" % (24 * len(bxs)))
+  f.write("property float32 x\n")
+  f.write("property float32 y\n")
+  f.write("property float32 z\n")
+  f.write("property uchar red\n")
+  f.write("property uchar green\n")
+  f.write("property uchar blue\n")
+  f.write("element face %d\n" % (12 * len(bxs)))
+  f.write("property list uchar int32 vertex_indices\n")
+  f.write("end_header\n")
+
+  if binary:
+    write_fcn = lambda x,y,z,c0,c1,c2: f.write(struct.pack('<fffBBB', x,y,z, c0,c1,c2))
+  else:
+    write_fcn = lambda x,y,z,c0,c1,c2: f.write('%f %f %f %d %d %d\n' % (x,y,z, c0,c1,c2))
+  for box in bxs:
+    x0, x1 = box[0], box[1]
+    y0, y1 = box[2], box[3]
+    z0, z1 = box[4], box[5]
+    pts = [
+      [x1, y1, z1],
+      [x1, y1, z0],
+      [x1, y0, z1],
+      [x1, y0, z0],
+      [x0, y1, z1],
+      [x0, y1, z0],
+      [x0, y0, z1],
+      [x0, y0, z0]
+    ];
+
+    write_fcn(pts[6][0],pts[6][1],pts[6][2], box[6],box[7],box[8])
+    write_fcn(pts[7][0],pts[7][1],pts[7][2], box[6],box[7],box[8])
+    write_fcn(pts[3][0],pts[3][1],pts[3][2], box[6],box[7],box[8])
+    write_fcn(pts[2][0],pts[2][1],pts[2][2], box[6],box[7],box[8])
+    write_fcn(pts[4][0],pts[4][1],pts[4][2], box[6],box[7],box[8])
+    write_fcn(pts[5][0],pts[5][1],pts[5][2], box[6],box[7],box[8])
+    write_fcn(pts[1][0],pts[1][1],pts[1][2], box[6],box[7],box[8])
+    write_fcn(pts[0][0],pts[0][1],pts[0][2], box[6],box[7],box[8])
+    write_fcn(pts[4][0],pts[4][1],pts[4][2], box[6],box[7],box[8])
+    write_fcn(pts[6][0],pts[6][1],pts[6][2], box[6],box[7],box[8])
+    write_fcn(pts[7][0],pts[7][1],pts[7][2], box[6],box[7],box[8])
+    write_fcn(pts[5][0],pts[5][1],pts[5][2], box[6],box[7],box[8])
+    write_fcn(pts[0][0],pts[0][1],pts[0][2], box[6],box[7],box[8])
+    write_fcn(pts[2][0],pts[2][1],pts[2][2], box[6],box[7],box[8])
+    write_fcn(pts[3][0],pts[3][1],pts[3][2], box[6],box[7],box[8])
+    write_fcn(pts[1][0],pts[1][1],pts[1][2], box[6],box[7],box[8])
+    write_fcn(pts[6][0],pts[6][1],pts[6][2], box[6],box[7],box[8])
+    write_fcn(pts[2][0],pts[2][1],pts[2][2], box[6],box[7],box[8])
+    write_fcn(pts[0][0],pts[0][1],pts[0][2], box[6],box[7],box[8])
+    write_fcn(pts[4][0],pts[4][1],pts[4][2], box[6],box[7],box[8])
+    write_fcn(pts[7][0],pts[7][1],pts[7][2], box[6],box[7],box[8])
+    write_fcn(pts[3][0],pts[3][1],pts[3][2], box[6],box[7],box[8])
+    write_fcn(pts[1][0],pts[1][1],pts[1][2], box[6],box[7],box[8])
+    write_fcn(pts[5][0],pts[5][1],pts[5][2], box[6],box[7],box[8])
+
+  if binary:
+    write_fcn = lambda i0,i1,i2: f.write(struct.pack('<Biii', 3,i0,i1,i2))
+  else:
+    write_fcn = lambda i0,i1,i2: f.write('3 %d %d %d\n' % (i0,i1,i2))
+  vidx = 0
+  for box in bxs:
+    write_fcn(vidx+0, vidx+1, vidx+2)
+    write_fcn(vidx+0, vidx+3, vidx+2)
+    vidx = vidx + 4;
+    write_fcn(vidx+0, vidx+1, vidx+2)
+    write_fcn(vidx+0, vidx+3, vidx+2)
+    vidx = vidx + 4;
+    write_fcn(vidx+0, vidx+1, vidx+2)
+    write_fcn(vidx+0, vidx+3, vidx+2)
+    vidx = vidx + 4;
+    write_fcn(vidx+0, vidx+1, vidx+2)
+    write_fcn(vidx+0, vidx+3, vidx+2)
+    vidx = vidx + 4;
+    write_fcn(vidx+0, vidx+1, vidx+2)
+    write_fcn(vidx+0, vidx+3, vidx+2)
+    vidx = vidx + 4;
+    write_fcn(vidx+0, vidx+1, vidx+2)
+    write_fcn(vidx+0, vidx+3, vidx+2)
+    vidx = vidx + 4;
+
+  f.close()
+
+def write_ply_voxels(out_path, grid, color=[128,128,128], color_fcn=None, explode=1, binary=False):
+  bxs = []
+  width = 1
+  for d in xrange(grid.shape[0]):
+    for h in xrange(grid.shape[1]):
+      for w in xrange(grid.shape[2]):
+        if grid[d,h,w] == 0:
+          continue
+        x = w * explode
+        y = h * explode
+        z = d * explode
+        if color_fcn is not None:
+          color = color_fcn(d,h,w)
+        bxs.append((x,x+width, y,y+width, z,z+width, color[0], color[1], color[2]))
+  write_ply_boxes(out_path, bxs, binary)