def project_pixel_on_plane(pixel_point: np.ndarray, plane_params: np.ndarray,
camera_pose: Pose, camera_matrix: np.ndarray):
"""
Find pose of specifc image pixel
Assumption is that the image content lies on a plane
Parameters
----------
pixel_point : np.ndarray
pixel point [x, y]
plane_params : np.ndarray
plane in general form ax + by + cz + d = 0
as numpy array [a,b,c,d]
camera_pose : xamla_motion.data_types.Pose
pose of camera in world coordinates
camera_matrix : np.ndarray
3x3 camera matrix see opencv for more information
"""
cam_pose = camera_pose.transformation_matrix()
# create ray in camera space
htp = np.array([pixel_point[0], pixel_point[1], 1])
ray = np.matmul(np.linalg.inv(camera_matrix), htp)
# transform form camera to world axis
ray_world = np.matmul(cam_pose[0:3, 0:3], ray)
eye = cam_pose[0:3, 3]
at = eye + ray_world
# compute intersection ray and plane
t, hit = x3d.intersect_ray_plane(eye, at, plane_params)
return hit
def plane_parameters_from_pose(pose: Pose):
"""
Compute plane parameters in general form ax + by + cz + d = 0
Parameters
----------
pose : xamla_motion.data_types.Pose
pose which plane in extracted z is normal axis
"""
pm = pose.transformation_matrix()
z = normalize(pm[0:3, 2])
d = np.matmul(z, pm[0:3, 3])
return np.asarray([z[0], z[1], z[2], -d])
