Fix RotationMatrixToAngleAxis near Pi.

Use Eigen's much more complicated conversion routine
when we encounter cases where the angle of rotation is
close to Pi.

Along the way also fix the way angle_axis vectors are
compared by making the matcher more robust.

Thanks to Tobias Strauss for reporting this.

Change-Id: Ia7e65dafad92c48d29d5f3cd22c4d6534789c183
3 files changed