Add RotationMatrixToQuaternion.

Use this function to implement RotationMatrixToAngleAxis.
This simplifies the implementation of RotationMatrixToAngleAxis,
just like Eigen does. It is also autodiff compatible, unlike the
Eigen based version.

Also significantly improve the test coverage of
RotationMatrixToAngleAxis.

Change-Id: Ic192a12fb5de952197ee24b0deedc45f195477f1