Added trifocal tensor calculations, it does something