slam14(1) v1_2旋转向量,旋转矩阵 - MKT-porter

随笔- 1525 文章- 2 评论- 66 阅读- 128万

旋转矩阵

有旋转矩阵计算欧拉角的公式：

欧拉旋转比较好理解，我想实现一个旋转，只要依次绕三个轴转对应的角度就可以实现。但是用欧拉角来表示旋转有一个非常大的缺陷，也就是万向锁也叫万向节死锁的问题，简单的说就是一旦选择±90°作为pitch角，就会导致第一次旋转和第三次旋转等价，整个旋转表示系统被限制在只能绕竖直轴旋转，丢失了一个表示维度。这种角度为±90°的第二次旋转使得第一次和第三次旋转的旋转轴相同的现象，称作万向锁。所以在通常有关旋转的应用场景中基本不使用欧拉角来旋转，而使用四元数Quaternion因为四元数可以实现平滑插值。

旋转矩阵【33】---旋转向量【31】和旋转角度

https://www.cnblogs.com/wtyuan/p/12324495.html

static SE3Quat exp(const Vector6& update) {
  Vector3 omega;
  for (int i = 0; i < 3; i++) omega[i] = update[i];
  Vector3 upsilon;
  for (int i = 0; i < 3; i++) upsilon[i] = update[i + 3];
 
  double theta = omega.norm();
  Matrix3 Omega = skew(omega);
 
  Matrix3 R;
  Matrix3 V;
  if (theta < cst(0.00001)) {
    Matrix3 Omega2 = Omega * Omega;
 
    R = (Matrix3::Identity() + Omega + cst(0.5) * Omega2);
 
    V = (Matrix3::Identity() + cst(0.5) * Omega + cst(1.) / cst(6.) * Omega2);
  } else {
    Matrix3 Omega2 = Omega * Omega;
 
    R = (Matrix3::Identity() + std::sin(theta) / theta * Omega +
         (1 - std::cos(theta)) / (theta * theta) * Omega2);
 
    V = (Matrix3::Identity() +
         (1 - std::cos(theta)) / (theta * theta) * Omega +
         (theta - std::sin(theta)) / (std::pow(theta, 3)) * Omega2);
  }
  return SE3Quat(Quaternion(R), V * upsilon);
}