Added calculation of essential matrix and 4 possible Ps