fix axis angle conversion when the rotation matrix is close to identity

[yzqin]

Fix rotation matrix to axis-angle conversion for near-identity matrices

Description: This PR fixes the rotation matrix to axis-angle conversion when the matrix is close to identity. The current implementation uses strict equality (==) to check for identity, which fails due to float precision issues. This can lead to division by zero and NaN results.

This fix prevents NaN outputs and ensures accurate conversion for small rotations.