viewer.py

import numpy as np
import pptk
from mayavi import mlab
import cv2

depthmap  = np.array(cv2.imread('depth.png',0));
depthmap = depthmap[300:340, 220:260]
intrinsics = np.array([[525.0, 0, 319.5],
                       [0, 525.0, 239.5],
                       [0, 0, 1]])
img_pose = np.array([[1, 0, 0, 0],
                     [0, 1, 0, 0],
                     [0, 0, 1, 0],
                     [0, 0, 0, 1]], dtype = float)

def display_points(points : np.array, isAxis = True):
     
     axis = [[0, 0, 0]]
     if (isAxis):
          for i in range(1, 101):
               axis = np.vstack((axis, [[i, 0, 0]]))
               axis = np.vstack((axis, [[0, i, 0]]))
               axis = np.vstack((axis, [[0, 0, i]]))

     display = np.vstack((axis, points))
     v = pptk.viewer(display)
     v.set(point_size = 0.01)
     
     return null


def img2world(u : int, v : int, depth : float, pose : np.array, i_inv : np.array):
    
    vertexMap = np.matmul(i_inv, depth * np.array([[u], [v], [1]]))
    vertexMap = np.array([[vertexMap[0]], [vertexMap[1]], [vertexMap[2]], [1.0]], dtype = float)
    #print(pose.shape)
    #print(vertexMap.shape)
    proj3d = np.matmul(pose, vertexMap)
    
    return np.array([proj3d[0][0], proj3d[1][0], proj3d[2][0]])


def points_from_image(depthmap : np.array, intrinsic : np.array):
    
    intrinsics_inv = np.linalg.inv(intrinsic)
    points = np.array([[0, 0, 0]])
    #print(intrinsics_inv.shape)
    #print((2 * np.array([[1], [2], [3]])).shape)
    #print(points.shape)
    
    for i in range(depthmap.shape[0]):
        for j in range(depthmap.shape[1]):
            print(points.shape)
            #print(img2world(i, j, depthmap[i][j], img_pose, intrinsics_inv).shape)
            #points = np.vstack((points, img2world(i, j, depthmap[i][j], img_pose, intrinsics_inv)))
            a = img2world(i, j, depthmap[i][j], img_pose, intrinsics_inv)
            v = mlab.mesh(a[0], a[1], a[2])
            
    #return points

#display_points(points_from_image(depthmap, intrinsics))