Linear trinagulation yields poor results

Question

I am working ona project regarding visual odometry and I have trouble extracting the 3D coordinates of matched features using the method of linear triangulation. I a using the KITTI dataset and specifically I am using the synced and rectified data. For simplification purposes i have chosen a feature that is detected only in the first and second camera frame. Since the rotation and translation of the camera poses are not given directly i have chosen to use the GPS/IMU rotation and translation matrices and use the relevant transformations to calculate the camera rotation and translation. acoording to equation (8) of the relevant papaer (https://www.cvlibs.net/publications/Geiger2013IJRR.pdf) a 3D point in GPS IMU coordinates (current coordinate system of the GPS/IMU) is projected to a 2D point in the cameras frame as follows : y = P_rectR_rectTvelo_cam*Timu_velo, where P_rect is the projection matrix of the camera after rectification. R_rect is the rectifying rotation matrix , Tvelo_cam is the transformation from the velodyne's coordinate frame to the cameras frame and Timu_velo is the transformation from the IMU's coordinate frame to the velodyne's coordinate frame. Note that I have chosen the first pose of the IMU as the origin of my world coordinate frame, meaning at point (0,0,0) the IMU has not rotated or moved.

Assuming that X is computed with respect to the real world coordinate frame, the results dont make sense : Specifically X,Y,Z = array([-10.55149986, 4.61478588, 2.40134485]). By taking into account the configuration of the worlds coordinate frame (x axis forward, y axis left and z axis up) this means that the point was detected behind the IMU (X<0), which is impossible because the IMU is behind the camera. I would appreciate any kind of help, since I am stuck a long time in this.

gray1 = images[0,:,:]

gray2 = images[1,:,:]

kp1 , ds1 = sift.detectAndCompute(gray1, None)

kp2 , ds2 = sift.detectAndCompute(gray2, None)


bf = cv2.BFMatcher()
matches12 = bf.knnMatch(ds1, ds2, k=2)

       
        

ratio_thresh = 0.4
good_matches = []
for m, n in matches12:
    if m.distance < ratio_thresh * n.distance:
        good_matches.append(m)

coordinates_kp1 = []
coordinates_kp2 = []

for match in good_matches:
    # Get the coordinates in the first image
    x1, y1 = kp1[match.queryIdx].pt
    coordinates_kp1.append((x1, y1))
    
    # Get the coordinates in the second image
    x2, y2 = kp2[match.trainIdx].pt
    coordinates_kp2.append((x2, y2))

    
XY1 = coordinates_kp1[440]
XY2 = coordinates_kp2[440]

#%% Slides

K2 = P0@R0@T_VelotoCam@T_IMUtoVelo
Rt1 = Rt_IMU[:2]

tracks = np.vstack((XY1,XY2)).T



A = np.zeros((2 * tracks.shape[1], 4))




P1 = K2@Rt1[0]
P2 = K2@Rt1[1]


A[0,:] = P1[2,:]*tracks[0,0] - P1[0,:]
A[1,:] = P1[2,:]*tracks[1,0] - P1[1,:]
A[2,:] = P2[2,:]*tracks[0,1] - P2[0,:]
A[3,:] = P2[2,:]*tracks[1,1] - P2[1,:]

U, S, Vt = np.linalg.svd(A)

V = np.transpose(Vt)

Xtilde = V[:, -1]

X = Xtilde[0:3] / Xtilde[3]