I'm having some issues when projecting a 3D vector in an image with known pose, and triangulating two 2D lines in two images with known poses.
Project 3D vector
Let's suppose we have a vector v_world = (x_v, y_v, z_v) in world coordinates. I want to project it in an image taken from a camera with known intrinsic and extrinsic parameters. The pose of this camera is an homogeneous matrix 4x4 Tcw. For a vector projection, I only need the rotation 3x3 submatrix Rcw. Let's get the vector v in camera coordinates.
v_camera = Rcw * v_world
This would be the same as.
(v_camera, 0) = Tcw * (v_world, 0)
To get now the vector in 2D, I just have to ignore the depth dimension and realize this operation.
v_camera_2D = atan2(v_camera[1], v_camera[0])
Triangulate two 2D lines
Let's suppose I want to triangulate two lines. The first one pass through the pixel (x_1, y_1) in the first image with known optical center (cx_1, cy_2), focal length (fx_1, fy_1) and pose Tcw_1. Second line pass through the pixel (x_2, y_2) in the second image with known optical center (cx_2, cy_2), focal length (fx_2, fy_2) and pose Tcw_2. Both lines' orientation are also known, v_1 and v_2, in given images.
I want to triangulate these lines to get the world orientation of the 3D line.
My first step is to construct the plane that contains the 2D line and the optical center of the camera. The 3D line will be the the intersection of the two constructed planes in both images. This is, the cross products of the planes' normal vector.
I just need then the normal vector of the desired planes. For this, I calculate the vector from the optical center to the line point.
v_plane1 = ( (x_1 - cx_1) / fx_1, (y_1 - cy_1) / fy_1, 1)
And convert it to world coordinates.
v_plane1_world = (Rcw)^T * v_plane
I convert too the 2D line orientation to 3D world coordinates.
v_line1 = ( cos(v_1), sin(v_1), 0 )
v_line1_world = (Rcw)^T * v_line1
The normal vector of the desired plane is the cross product of its two containing vectors.
v_normal_plane1_world = v_plane1_world x v_line1_world
And the orientation of the 3D line will be.
v_world = v_normal_plane1_world x v_normal_plane2_world
I have implemented it and doesn't work. I create an initial 3D line. I project it in two images, and triangulate both projections again. The triangulation and the initial 3D line should be the same (with a float precision threshold). Both lines are the same when I create a basic 3D line (an horizontal line, for example) and project it in two basic images (with any rotation). But the lines don't match when doing the same with rotated and translated images.
Is there any step I am wrongly doing? I don't pay attention to the vectors' magnitude, because I only want the orientation. I can upload my code to a repository if it's needed.