Added first steps to use solvePnp