import cv2import numpy as np
defget_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 thecorresponding points  onthe camera image plane(src_plane) using perspective matrix.  -Calculates the Homography matrix to map any point in image plane to floorplane. Parameters ---------- perspective_matrix: 3 x 4 camera prespective matrix to convert 3dhomogeneous world coordinates to  2dhomogeneous camera coordinates. Returns ----------  3x3homography matrix for moving from 2d homogeneous image plane to world floorplane(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 floorplane  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 cartesiancoordinates. It returns the (x, y) cartesian coordinates in 3d cartesian world coordinates on floorplane(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 onfloor 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)