infinite values for RotationMatrixToAngleAxis (Op and Gradient)

[sikelerd]

There is already a special handling for rotations about 0° but for a rotation about 180° there is no.
If trace of the rotation matrix is -1, theta becomes pi and any division by sin(theta) gives infinite values.

T tmp = R.trace()-1;
T theta = std::acos(std::min(T(1),std::max(T(-1),T(0.5)*tmp)));

Example

rot = np.array([[
        [-0.994789, 0.0789573, -0.0644982],
        [0.0790982, 0.198569, -0.97689],
        [-0.0643253, -0.976901, -0.20378]
]])

with tf.Session() as sess:
    r = tf.Variable(rot, dtype=tf.float32)
    sess.run(tf.global_variables_initializer())
    aa = sops.rotation_matrix_to_angle_axis(r)
    loss = tf.reduce_mean(sops.angle_axis_to_rotation_matrix(aa) - r)
    var_grad = tf.gradients(loss, [r])

    print(aa.eval())
    # [[  197.05762  3106.6025  -2531.7622 ]] norm: 4012.4346

    print(var_grad[0].eval())
    # [[[ inf  inf  inf]
    #  [-inf  inf -inf]
    #  [-inf  inf  inf]]]