Implemented iterative solvePnP using; adjusted some parameters