Lie Group Configuration Space Tangent Space Description
nD vector nD vector translations
2D unit vector（不使用2x2？） scalar 2D rotations
3×3 rotation matrix or unit quaternion 3D vector (angular velocity) 3D rotations
2D position + rotation 2D 3D vector (linear + angular) Planar rigid body motion
3D position + rotation 3D 6D vector (linear + angular) 3D rigid body motion

exp：Tangent Space -> Configuration Space

log：Configuration Space -> Tangent Space

Matrix3 exp3(const Vector3 & w)

void quaternion::exp3(const Vector3 & w, Quaternion & quat)

罗德里格斯公式：

其中，是的斜对称矩阵。

另一种理解方式是以这个角速度旋转单位时间的积分。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
/// \brief Exp: so3 -> SO3.
///
/// Return the integral of the input angular velocity during time 1.
///
/// \param[in] v The angular velocity vector.
///
/// \return The rotational matrix associated to the integration of the angular velocity during
/// time 1.
///
template<typename Vector3Like>
typename Eigen::
  Matrix<typename Vector3Like::Scalar, 3, 3, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
  exp3(const Eigen::MatrixBase<Vector3Like> & v)
{
// 断言输入向量必须是3x1的三维向量
PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector3Like, v, 3, 1);
// 定义标量类型
typedef typename Vector3Like::Scalar Scalar;
// 定义输入向量的Plain类型（保证内存连续的Eigen类型）
typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like) Vector3LikePlain;
// 定义3x3矩阵类型，保持与输入向量相同的存储选项
typedef Eigen::Matrix<Scalar, 3, 3, Vector3LikePlain::Options> Matrix3;

// 机器精度，用于数值稳定性
const static Scalar eps = Eigen::NumTraits<Scalar>::epsilon();
// 计算旋转向量的平方范数，并添加eps^2避免数值问题
const Scalar t2 = v.squaredNorm() + eps * eps;
// 计算旋转角度t（旋转向量的长度）
const Scalar t = math::sqrt(t2);

// 存储t的正弦值和余弦值
Scalar ct, st;
// 计算t的正弦和余弦（高效计算）
SINCOS(t, &st, &ct);
// 计算罗德里格斯公式中的系数alpha_vxvx = (1 - cos(t))/t^2
// 当t较小时（小于三阶泰勒展开精度），使用泰勒展开近似避免除零
const Scalar alpha_vxvx = internal::if_then_else(
  internal::GT, t, TaylorSeriesExpansion<Scalar>::template precision<3>(),
  static_cast<Scalar>((1 - ct) / t2),                     // 常规公式
  static_cast<Scalar>(Scalar(1) / Scalar(2) - t2 / 24));  // 泰勒展开近似：1/2 - t²/24 + ...
// 计算罗德里格斯公式中的系数alpha_vx = sin(t)/t
// 当t较小时（小于三阶泰勒展开精度），使用泰勒展开近似避免除零
const Scalar alpha_vx = internal::if_then_else(
  internal::GT, t, TaylorSeriesExpansion<Scalar>::template precision<3>(),
  static_cast<Scalar>((st) / t),                          // 常规公式
  static_cast<Scalar>(Scalar(1) - t2 / 6));               // 泰勒展开近似：1 - t²/6 + ...
// 初始化旋转矩阵：res = alpha_vxvx * (v * v^T)
Matrix3 res(alpha_vxvx * v * v.transpose());

// 构造反对称矩阵部分：alpha_vx * [v]×
// 反对称矩阵[v]×表示向量v的叉积矩阵
res.coeffRef(0, 1) -= alpha_vx * v[2];  // 反对称矩阵的(0,1)元素：-v[2]
res.coeffRef(1, 0) += alpha_vx * v[2];  // 反对称矩阵的(1,0)元素：v[2]
res.coeffRef(0, 2) += alpha_vx * v[1];  // 反对称矩阵的(0,2)元素：v[1]
res.coeffRef(2, 0) -= alpha_vx * v[1];  // 反对称矩阵的(2,0)元素：-v[1]
res.coeffRef(1, 2) -= alpha_vx * v[0];  // 反对称矩阵的(1,2)元素：-v[0]
res.coeffRef(2, 1) += alpha_vx * v[0];  // 反对称矩阵的(2,1)元素：v[0]
// 当t较小时，对cos(t)使用泰勒展开近似：1 - t²/2 + ...
ct = internal::if_then_else(
  internal::GT, t, TaylorSeriesExpansion<Scalar>::template precision<3>(),
  ct,                                                    // 常规cos(t)
  static_cast<Scalar>(Scalar(1) - t2 / 2));              // 泰勒展开近似
// 完成罗德里格斯公式：R = I + alpha_vx*[v]× + alpha_vxvx*(v*v^T)
// 其中I是单位矩阵，通过将ct加到对角线实现（当t=0时，ct=1，得到单位矩阵）
res.diagonal().array() += ct;
// 返回计算得到的旋转矩阵
return res;
}

SE3 exp6(const Motion & nu)

SE3 exp6(const Vector6 & v)

Vector7 quaternion::exp6(const Vector6 & v)

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
  ///
  /// \brief Exp: se3 -> SE3.
  ///
  /// Return the integral of the input twist during time 1.
  ///
  /// \param[in] nu The input twist.
  ///
  /// \return The rigid transformation associated to the integration of the twist during time 1.
  ///
  template<typename MotionDerived>
SE3Tpl<
  typename MotionDerived::Scalar,
  PINOCCHIO_EIGEN_PLAIN_TYPE(typename MotionDerived::Vector3)::Options>
exp6(const MotionDense<MotionDerived> & nu)
{
  // 定义标量类型
  typedef typename MotionDerived::Scalar Scalar;
  // 定义存储选项（与输入向量保持一致）
  enum
  {
    Options = PINOCCHIO_EIGEN_PLAIN_TYPE(typename MotionDerived::Vector3)::Options
  };
  // 定义SE(3)类型
  typedef SE3Tpl<Scalar, Options> SE3;
  // 创建结果SE(3)变换
  SE3 res;
  // 引用结果的平移部分和旋转部分
  typename SE3::LinearType & trans = res.translation();
  typename SE3::AngularType & rot = res.rotation();
  // 从输入运动向量中提取角速度和线速度
  const typename MotionDerived::ConstAngularType & w = nu.angular();
  const typename MotionDerived::ConstLinearType & v = nu.linear();
  // 机器精度，用于数值稳定性
  const static Scalar eps = Eigen::NumTraits<Scalar>::epsilon();
  // 定义指数映射公式中的系数
  Scalar alpha_wxv, alpha_v, alpha_w, diagonal_term;
  // 计算角速度向量的平方范数，并添加eps^2避免数值问题
  const Scalar t2 = w.squaredNorm() + eps * eps;
  // 计算旋转角度t（角速度向量的长度）
  const Scalar t = math::sqrt(t2);
  // 存储t的正弦值和余弦值
  Scalar ct, st;
  // 计算t的正弦和余弦（高效计算）
  SINCOS(t, &st, &ct);
  // 计算t²的倒数（用于后续计算）
  const Scalar inv_t2 = Scalar(1) / t2;
  // 计算平移部分的系数alpha_wxv = (1 - cos(t))/t^2
  // 当t较小时（小于三阶泰勒展开精度），使用泰勒展开近似避免除零
  alpha_wxv = internal::if_then_else(
    internal::LT, t, TaylorSeriesExpansion<Scalar>::template precision<3>(),
    static_cast<Scalar>(Scalar(1) / Scalar(2) - t2 / 24),    // 泰勒展开近似：1/2 - t²/24 + ...
    static_cast<Scalar>((Scalar(1) - ct) * inv_t2));         // 常规公式
  // 计算平移和旋转部分的系数alpha_v = sin(t)/t
  // 当t较小时，使用泰勒展开近似
  alpha_v = internal::if_then_else(
    internal::LT, t, TaylorSeriesExpansion<Scalar>::template precision<3>(),
    static_cast<Scalar>(Scalar(1) - t2 / 6),                 // 泰勒展开近似：1 - t²/6 + ...
    static_cast<Scalar>((st) / t));                          // 常规公式
  // 计算平移部分的系数alpha_w = (1 - alpha_v)/t^2
  // 当t较小时，使用泰勒展开近似
  alpha_w = internal::if_then_else(
    internal::LT, t, TaylorSeriesExpansion<Scalar>::template precision<3>(),
    static_cast<Scalar>((Scalar(1) / Scalar(6) - t2 / 120)),  // 泰勒展开近似：1/6 - t²/120 + ...
    static_cast<Scalar>((Scalar(1) - alpha_v) * inv_t2));     // 常规公式
  // 计算旋转矩阵对角线元素的系数
  // 当t较小时，使用泰勒展开近似
  diagonal_term = internal::if_then_else(
    internal::LT, t, TaylorSeriesExpansion<Scalar>::template precision<3>(),
    static_cast<Scalar>(Scalar(1) - t2 / 2),                  // 泰勒展开近似：1 - t²/2 + ...
    ct);                                                      // 常规公式：cos(t)
  // 计算平移向量
  // 使用Baker-Campbell-Hausdorff公式的一阶近似：t = v*t + (v×w)*(t²/2) + w*(w·v)*(t²/2 - t^4/24)
  trans.noalias() = (alpha_v * v + (alpha_w * w.dot(v)) * w + alpha_wxv * w.cross(v));
  // 计算旋转矩阵（与exp3函数类似，基于Rodrigues公式）
  rot.noalias() = alpha_wxv * w * w.transpose();  // (1 - cos(t))/t² * w*w^T
  // 构造反对称矩阵部分：sin(t)/t * [w]×
  rot.coeffRef(0, 1) -= alpha_v * w[2];
  rot.coeffRef(1, 0) += alpha_v * w[2];
  rot.coeffRef(0, 2) += alpha_v * w[1];
  rot.coeffRef(2, 0) -= alpha_v * w[1];
  rot.coeffRef(1, 2) -= alpha_v * w[0];
  rot.coeffRef(2, 1) += alpha_v * w[0];
  // 完成旋转矩阵：R = I + sin(t)/t*[w]× + (1 - cos(t))/t²*w*w^T
  rot.diagonal().array() += diagonal_term;
  // 返回计算得到的SE(3)变换
  return res;
}

Vector3 log3(const Matrix3 & R)

Vector3 log3(const Matrix3 & R, Scalar & theta)

Vector3 quaternion::log3(const Quaternion & quat)

Vector3 quaternion::log3(const Quaternion & quat, Scalar & theta)

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
/// \brief 计算旋转矩阵的对数映射，将3x3旋转矩阵转换为旋转向量（angle-axis表示）
/// \param[in] R 输入的3x3旋转矩阵
/// \param[out] theta 输出的旋转角度（弧度），范围[0, π]
/// \param[out] angle_axis 输出的旋转向量，其模长等于旋转角度
template<typename Matrix3Like, typename Vector3Out>
static void run(
  const Eigen::MatrixBase<Matrix3Like> & R,
  typename Matrix3Like::Scalar & theta,
  const Eigen::MatrixBase<Vector3Out> & angle_axis)
{
  // 验证输入矩阵的尺寸
  PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like, R, 3, 3);
  PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector3Out, angle_axis, 3, 1);
  using namespace internal;

  // 定义标量类型和向量类型
  typedef typename Matrix3Like::Scalar Scalar;
  typedef Eigen::Matrix<Scalar, 3, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options> Vector3;
  static const Scalar eps = Eigen::NumTraits<Scalar>::epsilon(); // 机器精度

  const static Scalar PI_value = PI<Scalar>(); // π的值
  Vector3Out & angle_axis_ = angle_axis.const_cast_derived(); // 获取可修改的输出引用

  // 对旋转矩阵进行归一化处理，确保其是正交的
  typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like) Matrix3;
  const Matrix3 Rnormed = renormalize_rotation_matrix(R);

  // 计算旋转矩阵的迹和cos(theta)
  const Scalar tr = Rnormed.trace();
  const Scalar cos_value = (tr - Scalar(1)) / Scalar(2);

  // 获取泰勒级数展开的精度阈值
  const Scalar prec = TaylorSeriesExpansion<Scalar>::template precision<2>();
  
  // 处理theta接近π时的奇异情况（旋转180度）
  Vector3 angle_axis_singular; // 奇异情况下的旋转轴
  Scalar theta_singular;        // 奇异情况下的旋转角度

  {
    // 计算奇异情况下的中间值
    Vector3 val_singular;
    val_singular.array() = Scalar(2) * Rnormed.diagonal().array() - tr + Scalar(1);
    
    // 分别计算三个可能的旋转轴和角度
    Vector3 axis_0, axis_1, axis_2;
    Scalar theta_0, theta_1, theta_2;

    internal::compute_theta_axis<0>(val_singular[0], Rnormed, theta_0, axis_0);
    internal::compute_theta_axis<1>(val_singular[1], Rnormed, theta_1, axis_1);
    internal::compute_theta_axis<2>(val_singular[2], Rnormed, theta_2, axis_2);

    // 选择数值上最稳定的旋转角度（基于对角线元素的值）
    theta_singular = if_then_else(
      GE, val_singular[0], val_singular[1],
      if_then_else(GE, val_singular[0], val_singular[2], theta_0, theta_2),
      if_then_else(GE, val_singular[1], val_singular[2], theta_1, theta_2));

    // 选择对应的旋转轴
    for (int k = 0; k < 3; ++k)
      angle_axis_singular[k] = if_then_else(
        GE, val_singular[0], val_singular[1],
        if_then_else(GE, val_singular[0], val_singular[2], axis_0[k], axis_2[k]),
        if_then_else(GE, val_singular[1], val_singular[2], axis_1[k], axis_2[k]));
  }
  
  // 计算acos的数值稳定替代方案，避免接近-1时的数值问题
  const Scalar acos_expansion = math::sqrt(Scalar(2) * (Scalar(1) - cos_value) + eps * eps);
  
  // 根据旋转矩阵的迹选择不同的theta计算方法
  const Scalar theta_nominal = if_then_else(
    LE, tr, static_cast<Scalar>(Scalar(3) - prec), // 接近单位矩阵的情况
    if_then_else(
      GE, tr, static_cast<Scalar>(Scalar(-1) + prec), // 一般情况
      math::acos(cos_value),                         // 使用acos计算theta
      static_cast<Scalar>(PI_value - acos_expansion) // 接近π的情况，使用稳定的替代方案
      ),
    static_cast<Scalar>(acos_expansion) // 非常接近单位矩阵的情况，使用泰勒展开近似
  );
  
  // 确保theta_nominal不是NaN
  assert(
    check_expression_if_real<Scalar>(theta_nominal == theta_nominal)
    && "theta contains some NaN");

  // 计算旋转矩阵的反对称部分
  Vector3 antisymmetric_R;
  unSkew(Rnormed, antisymmetric_R);
  const Scalar norm_antisymmetric_R_squared = antisymmetric_R.squaredNorm();

  // 计算缩放因子t，根据theta的大小选择不同的计算方法以保证数值稳定性
  const Scalar t = if_then_else(
    GE, theta_nominal, prec,
    static_cast<Scalar>(theta_nominal / sin(theta_nominal)), // theta较大时，直接计算
    static_cast<Scalar>(
      Scalar(1.) + norm_antisymmetric_R_squared / Scalar(6)
      + norm_antisymmetric_R_squared * norm_antisymmetric_R_squared * Scalar(3)
          / Scalar(40)) // theta较小时，使用泰勒级数展开
  );

  // 根据cos(theta)的值选择最终的theta（正常情况或奇异情况）
  theta = if_then_else(
    GE, cos_value, static_cast<Scalar>(Scalar(-1.) + prec), theta_nominal, theta_singular);

  // 根据cos(theta)的值选择最终的旋转向量计算方法
  for (int k = 0; k < 3; ++k)
    angle_axis_[k] = if_then_else(
      GE, cos_value, static_cast<Scalar>(Scalar(-1.) + prec),
      static_cast<Scalar>(t * antisymmetric_R[k]), // 正常情况：使用反对称部分计算
      static_cast<Scalar>(theta_singular * angle_axis_singular[k])); // 奇异情况：使用预先计算的奇异解
}

1. Lie