implemented the camera pose algorithm