Homography Matrix

get_inverse_pespective

import cv2
import numpy as np

def get_inverse_pespective(perspective_matrix: np.array)-> np.array:
  """
  This method calculates the inverse of prespective matrix by homography. 
  - Takes 4 random points on the floor plane(destination_plane) and calculates the corresponding points 
  on the camera image plane(src_plane) using perspective matrix.
  - Calculates the Homography matrix to map any point in image plane to floor plane.
  Parameters
  ----------
  perspective_matrix: 3 x 4 camera prespective matrix to convert 3d homogeneous world coordinates to 
  2d homogeneous camera coordinates.
  Returns
  ----------
  3x3 homography matrix for moving from 2d homogeneous image plane to world floor plane(at z=0)
  
  """
  #Take 5 homogenous points on the floor(Unit is in Meters)
  pts_dst = np.array([[0,0,0,1],
                      [0,1,0,1],
                      [1,0,0,1],
                      [1,1,0,1],
                      [0,0,0,1]
                    ])
  #Obtain respective homogenous points on the image plane
  pts_src = (perspective_matrix @ pts_dst.T).T
  
  #convert homogenous coordinates to cartesian coorndinates
  pts_src_cart = np.array([[x/w, y/w] for x,y,w in pts_src])
  pts_dst_cart = np.array([[x/w, y/w] for x,y,z,w in pts_dst])
  
  #find the 3x3 Homography Matrix for transforming image plane to floor plane
  H, status = cv2.findHomography(pts_src_cart, pts_dst_cart)
  return H
  
def project_to_floor(image_coordinates: List[int], H: np.array) -> List[int]: 
  """
  This method takes the Homography matrix and the 2d image cartesian coordinates. It returns the (x, y)
  cartesian coordinates in 3d cartesian world coordinates on floor plane(at z=0). Notice that z coordinate is omitted
  here and added inside the tracking funtion. 
  
  Parameters
  ----------
  image_coordinates: 2d pixel coordinates (x,y)
  h: 3x3 Homography matrix np.array[3x3]
  Returns
  ----------
  floor_coordinates: List of x, y coordinates in 3d world of same pixel on floor plane i.e. (x,y,z) Considering z=0 and 
  ommitted here.
  """
  #adding 1 for homogenous coordinate system
  x, y, w = H @ np.array([[*image_coordinates, 1]]).T
  return [x/w, y/w]

p = np.random.rand(3,4)
H = get_inverse_pespective(p)
src_point = (5,10)
dst_point = project_to_floor(src_point, H)