Added Rodrigues and SolvePnP