I prepared a toy experiment to project a point defined in the world frame to the image plane. I'm trying to calculate the 3D point (inverse-projection) with the calculated pixel coordinates. I used the same coordinate frames as the figure below [https://www.researchgate.net/figure/World-and-camera-frame-definition_fig1_224318711]. (world x,y,z -> camera z,-x,-y). Then I will project the same point for different image frames. The problem here is that the 3D point defined as (8,-2.1) is calculated at (6.29, -1.60, 0.7). Since there is no loss of information, I think I need to find the same point. I believe there is a problem with depth. I couldn't find where I missed.
import numpy as np
#3d point at 8 -2 1 wrt world frame
P_world = np.array([8, -2, 1, 1]).reshape((4,1))
T_wc = np.array([
[ 0, -1, 0, 0],
[ 0, 0, -1, 0],
[ 1, 0, 0, 0],
[ 0, 0, 0, 1]])
pose0 = np.eye(4)
pose0[:3,-1] = [1, 0, .6]
pose0 = np.matmul(T_wc, pose0)
pose1 = np.eye(4)
pose1[:3,-1] = [3, 0, .6]
pose1 = np.matmul(T_wc, pose1)
depth0 = np.linalg.norm(P_world[:3].flatten() - np.array([1, 0, .6]).flatten())
depth1 = np.linalg.norm(P_world[:3].flatten() - np.array([3, 0, .6]).flatten())
K = np.array([
[173.0, 0 , 173.0],
[ 0 , 173.0, 130.0],
[ 0 , 0 , 1 ]])
uv1 = np.matmul(np.matmul(K, pose0[:3]), P_world)
uv1 = (uv1 / uv1[-1])[:2]
uv2 = np.matmul(np.matmul(K, pose1[:3]), P_world)
uv2 = (uv2 / uv2[-1])[:2]
img0 = np.zeros((260,346))
img0[int(uv1[0]), int(uv1[1])] = 1
img1 = np.zeros((260,346))
img1[int(uv2[0]), int(uv2[1])] = 1
#%% Op
pix_coord = np.array([int(uv1[0]), int(uv1[1]), 1])
pt_infilm = np.matmul(np.linalg.inv(K), pix_coord.reshape(3,1))
pt_incam = depth0*pt_infilm
pt_incam_hom = np.append(pt_incam, 1)
pt_inworld = np.matmul(np.linalg.inv(pose0), pt_incam_hom)
