<jupyter_start><jupyter_text>判断一个矩阵是不是SO(3)因为SO(3)空间满足：$R*R^T=I$，通过判断一个矩阵与SO(3)的距离来判断：$$D=||RR^T-I||_F$$<jupyter_code># import ipynb_importer
import import_ipynb

import numpy as np

def DistanceToSO3(mat):
    """Returns the Frobenius norm to describe the distance of mat from the
    SO(3) manifold

    :param mat: A 3x3 matrix
    :return: A quantity describing the distance of mat from the SO(3)
             manifold
    Computes the distance from mat to the SO(3) manifold using the following
    method:
    If det(mat) <= 0, return a large number.
    If det(mat) > 0, return norm(mat^T.mat - I).

    Example Input:
        mat = np.array([[ 1.0,  0.0,   0.0 ],
                        [ 0.0,  0.1,  -0.95],
                        [ 0.0,  1.0,   0.1 ]])
    Output:
        0.08835
    """
    if np.linalg.det(mat) > 0: # SO(3)流形行列式肯定>0
        return np.linalg.norm(np.dot(np.array(mat).T, mat) - np.eye(3)) # 返回RR^T-I的F范数
    else:
        return 1e+9 # 行列式<0则直接返回一个大数


def TestIfSO3(mat):
    """Returns true if mat is close to or on the manifold SO(3)

    :param mat: A 3x3 matrix
    :return: True if mat is very close to or in SO(3), false otherwise
    Computes the distance d from mat to the SO(3) manifold using the
    following method:
    If det(mat) <= 0, d = a large number.
    If det(mat) > 0, d = norm(mat^T.mat - I).
    If d is close to zero, return true. Otherwise, return false.

    Example Input:
        mat = np.array([[1.0, 0.0,  0.0 ],
                        [0.0, 0.1, -0.95],
                        [0.0, 1.0,  0.1 ]])
    Output:
        False
    """
    return abs(DistanceToSO3(mat)) < 1e-3 # 若小于1e-3则认为是SO(3)<jupyter_output><empty_output><jupyter_text>验证一下：<jupyter_code>import chapter_3

mat = np.array([[1.0, 0.0,  0.0 ],
                [0.0, 0.1, -0.95],
                [0.0, 1.0,  0.1 ]])

print(TestIfSO3(mat))

A = chapter_3.Rodrigues(chapter_3.Normalize([1, 1, 0]), 90.0/180.0*np.pi)

print(TestIfSO3(A))<jupyter_output>importing Jupyter notebook from chapter_3.ipynb
[[1. 0. 0.]
 [0. 1. 0.]
 [0. 0. 1.]]
[6.123234e-17 1.000000e+00 0.000000e+00]
[[ 0 -3  2]
 [ 3  0 -1]
 [-2  1  0]]
[1 2 3]
[[ 1.00000000e+00 -6.83580866e-17  0.00000000e+00]
 [-6.83580866e-17  1.00000000e+00  0.00000000e+00]
 [ 0.00000000e+00  0.00000000e+00  1.00000000e+00]]
[[ 1.00000000e+00  4.26642159e-17  1.22133484e-17]
 [ 4.26642159e-17  1.00000000e+00 -3.52068201e-17]
 [ 1.22133484e-17 -3.52068201e-17  1.00000000e+00]]
Axis Angle: 
  (array([0.40824829, 0.81649658, 0.40824829]), 2.449489742783178)
so3mat:
  [[ 0 -1  2]
 [ 1  0 -1]
 [-2  1  0]]
MatrixExp3：
 [[-0.47492144  0.32944181  0.81603782]
 [ 0.85049534  0.41003142  0.32944181]
 [-0.22606924  0.85049534 -0.47492144]]
MatrixLog3:
 [[ 0. -1.  2.]
 [ 1.  0. -1.]
 [-2.  1.  0.]]
so3 vec:  [1. 2. 1.]
Axis Angle: 
  (array([0.40824829, 0.81649658, 0.40824829]), 2.4494897427831814)
[ 0  0  2 -2 -4  0]
[ 0.  0.  2. -2. -4.  0.]
[ 0.   0.  -2.   2.8  4.   0. ]
[ 0.   0.  -2.   2.8  4.[...]
{'fraction_non_alphanumeric': 0.1685656836461126, 'fraction_numerical': 0.19269436997319034, 'mean_word_length': 2.2375271149674623, 'pattern_counts': {'":': 0, '<': 12, '<?xml version=': 0, '>': 12, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 10, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'chain_length': '2', 'dir': 'jupyter-structured-clean-dedup', 'id': '538925', 'license': 'MIT', 'n_tokens_mistral': 1673, 'n_tokens_neox': 1384, 'n_words': 318, 'path': 'appedix_e.ipynb', 'repo_name': 'thinkexist1989/LearnModernRobotics', 'size': '2984'}