How to efficiently apply a homogeneous transformation to a point cloud using numpy

user_6159217 0
Viewed 300

Supposing I have a point cloud in the form of a numpy array of shape (H, W, 4) where each H, W is the height and width of the depth image from which the point cloud was created, and at each spatial location (h,w) there is a homogeneous vector of shape (4,). I wish to apply a transformation from an extrinsic matrix, a numpy array of shape (4,4), at each spatial location efficiently using numpy.

Right now I am doing this very inefficiently with loops as in:

import numpy as np
def apply_transformation(pc, extr_matr):
   for h in range(pc.shape[0]):
       for w in range(pc.shape[1]):
          pc[h, w, :] = np.matmul(extr_matr, np.array(pc[h, w, :]))

I am not sure how to use the einsum notation or batch matrix multiplication functionalities of numpy to solve this problem, any insights appreciated.

numpy python
1 Answers

I think np.apply_along_axis might be what you are looking for :

import numpy as np
pc = np.ones((3,5,4))
extr_matr = 2*np.ones((4,4))
pc = np.apply_along_axis(lambda x: np.matmul(x,extr_matr), axis = 2, arr = pc)
user_159482400
Related