Optimized version of the code.
Next step: looking for a better way to obtain the fundamental matrix.