罗德里戈公式的推导和实现C++

Σίσυφος1900

已于 2023-06-29 16:42:32 修改

阅读量454

点赞数

分类专栏：数学之美 C++ PCL 文章标签：人工智能

于 2023-02-01 18:29:56 首次发布

本文链接：https://blog.csdn.net/weixin_39354845/article/details/128838009

版权

PCL 同时被 3 个专栏收录

56 篇文章 16 订阅

订阅专栏

C++

47 篇文章 5 订阅

订阅专栏

数学之美

23 篇文章 3 订阅

订阅专栏

罗德里格斯公式附图推导，理解_霜桥月馆的博客-CSDN博客_罗德里格斯公式

罗德里格斯公式（Rodrigues Formula）_默_存的博客-CSDN博客

一、向量的叉乘

3d 中两个向量的叉乘的意义表示的是平行四边形的面积。

有方向有大小

可以将两个向量的叉乘写出一个反对称矩阵和向量的乘机

向量三重积展开：

叉积分配率：

二、旋转向量的分解

我们可以将v vv分解为平行与旋转轴，以及垂直于旋转轴的两个分量，v ∥ 和 v ⊥。

三、PCL代码

EIGEN_DEVICE_FUNC Quaternion<Scalar,Options> Quaternion<Scalar,Options>::FromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b)
{
    Quaternion quat;
    quat.setFromTwoVectors(a, b);
    return quat;
}

template<class Derived>
template<typename Derived1, typename Derived2>
EIGEN_DEVICE_FUNC inline Derived& QuaternionBase<Derived>::setFromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b)
{
  EIGEN_USING_STD_MATH(sqrt)
  Vector3 v0 = a.normalized();
  Vector3 v1 = b.normalized();
  Scalar c = v1.dot(v0);

  // if dot == -1, vectors are nearly opposites
  // => accurately compute the rotation axis by computing the
  //    intersection of the two planes. This is done by solving:
  //       x^T v0 = 0
  //       x^T v1 = 0
  //    under the constraint:
  //       ||x|| = 1
  //    which yields a singular value problem
  if (c < Scalar(-1)+NumTraits<Scalar>::dummy_precision())
  {
    c = numext::maxi(c,Scalar(-1));
    Matrix<Scalar,2,3> m; m << v0.transpose(), v1.transpose();
    JacobiSVD<Matrix<Scalar,2,3> > svd(m, ComputeFullV);
    Vector3 axis = svd.matrixV().col(2);

    Scalar w2 = (Scalar(1)+c)*Scalar(0.5);
    this->w() = sqrt(w2);
    this->vec() = axis * sqrt(Scalar(1) - w2);
    return derived();
  }
  Vector3 axis = v0.cross(v1);
  Scalar s = sqrt((Scalar(1)+c)*Scalar(2));
  Scalar invs = Scalar(1)/s;
  this->vec() = axis * invs;
  this->w() = s * Scalar(0.5);

  return derived();
}