sophus库 根据se3的exp函数求解平移向量

在优化过程中,求解出来的se3小量前三列并不是平移向量,因此需要转换一下:
se3.cpp源码:

SE3 SE3::exp(const Vector6d & update)
{
  Vector3d upsilon = update.head<3>();
  Vector3d omega = update.tail<3>();

  double theta;
  SO3 so3 = SO3::expAndTheta(omega, &theta);

  Matrix3d Omega = SO3::hat(omega);
  Matrix3d Omega_sq = Omega*Omega;
  Matrix3d V;

  if(theta<SMALL_EPS)
  {
    V = so3.matrix();
    //Note: That is an accurate expansion!
  }
  else
  {
    double theta_sq = theta*theta;
    V = (Matrix3d::Identity()
         + (1-cos(theta))/(theta_sq)*Omega
         + (theta-sin(theta))/(theta_sq*theta)*Omega_sq);
  }
  return SE3(so3,V*upsilon);
}

自己改写后的代码,只求平移向量:

 Eigen::Matrix3d expAndTheta(const Eigen::Vector3d & omega, double * theta)
    {
        double SMALL_EPS = 1e-10;
        *theta = omega.norm();
        double half_theta = 0.5*(*theta);

        double imag_factor;
      

最后

以上就是儒雅香烟为你收集整理的sophus库 根据se3的exp函数求解平移向量的全部内容，希望文章能够帮你解决sophus库 根据se3的exp函数求解平移向量所遇到的程序开发问题。

如果觉得靠谱客网站的内容还不错，欢迎将靠谱客网站推荐给程序员好友。