Structure From Motion: Implementing Triangulation

Question

Use-case:

---

Given a known ( simulated ) 3D scene, and:

1. 2 vectors of corresponding 2D points V, V’ where V’ present the projection of the 3D points including relative transformation ( extracted using the 5 points algorithm ).
2. Camera Intrinsic matrix ‘K’
3. Relative Camera rotation matrix ‘R’
4. Relative Camera translation ‘t’ ( Up to a scale )

Compute the triangulated 3D Points positions ( up to a scale ) Implementation:

1. For each camera extract the ray projected out of the camera and through the 3D point using
   [RayV] = [Vx, Vy, 0] * K.inv()
   [RayV’] = [V’x, V’y, 0] * K.inv()
   
2. Add relative Rotation to [RayV’] ( affects the Ray direction )
   [RayV’] = [RayV’] * [R]
3. Form the 3D line equations for the two Rays
   L = [0, 0, 0] + [RayV] ( first ray intersects the origin 0, 0, 0 )
   L’=[t] + [RayV’] ( adding relative camera translation )
   
4. Find the point in space minimizing the distance between the two rays in accordance to: https://www.youtube.com/watch?v=HC5YikQxwZA
   
   
   

The Problem:

---

The derived camera rays deviate much in the Z coordinate while X,Y deviation is minimal, the resulting triangulated points doesn’t correspond with the scene it was generated from.

What am I doing wrong here?
Is the above Math correct?
Is the Ray computation and Ray formula derivation correct?

Answer 1

To create a homogeneous coordinate from the 2D point, you have to replace the 0 in the ray calculation by a 1.