对于SE3,SO3本程序提供如下计算类
具体可参考如下代码：
/******************************
Public Sub test()
    Dim so3 As New ClassSO3
    Dim se3 As New ClassSE3

    '1 使用轴角设置SO3
    Dim v(2) As Double
    so3.SetByAxis(v)

    '2 使用四元数设置SO3
    Dim q(3) As Double
    so3.SetByQuaternion(v)

    '3 使用3*3旋转矩阵设置SO3
    Dim vR(2, 2) As Double
    so3.SetByR(vR)

    '4 从SO3获取轴角
    v = so3.ToAxis()

    '5 从SO3获取四元数
    q = so3.ToQuaternion()

    '6 从SO3获取旋转矩阵
    vR = so3.ToR()

    '7 常规雅克比计算
    Dim vJl(2, 2), vInvJl(2, 2), vJr(2, 2), vInvJr(2, 2) As Double
    vJl = so3.LeftJacobi()
    vJr = so3.RightJacobi()
    vInvJl = so3.InvLeftJacobi()
    vInvJr = so3.InvRightJacobi()

    '8 求逆 与 转置(它们结果是一样的)
    Dim invSo3 As ClassSO3 = so3.Inverse()
    Dim tSo3 As ClassSO3 = so3.Transpose()

    '9 左扰动更新
    Dim dv(2) As Double
    so3.UpdateLeftDx(dv)

    '10 对某个3D点旋转 z = R * p
    Dim p(2) As Double
    Dim z() As Double = so3 * p

    '11 两个旋转相乘
    Dim so32 As ClassSO3 = so3 * so3

    '12 获取或设置旋转
    so3 = se3.GetR()
    se3.SetR(so3)

    '13 获取或设置平移
    Dim t(2) As Double
    t = se3.GetTranslation()
    se3.SetTranslation(t)

    '14 获取4*4变换矩阵
    Dim vT(,) As Double = se3.Exp()

    '15 获取李代数向量,前三维为旋转,后三维为平移部分(这不是Translation)
    Dim rho(5) As Double
    rho = se3.Log()

    '16 求逆
    Dim invSE3 As ClassSE3 = se3.Inverse()

    '17 两个SE3相乘
    Dim se32 As ClassSE3 = se3 * se3

    '18 点映射 z = R * p + t
    z = se3 * p
End Sub
/******************************