0

如何使用 Eigen 库反转在四元数中构造的 3d 旋转序列？

我正在根据偏航，滚动，俯仰构造四元数，如下所示：

Eigen::AngleAxisf yawAngle(thetaZ, Eigen::Vector3f::UnitZ());
Eigen::AngleAxisf rollAngle(thetaY, Eigen::Vector3f::UnitY());
Eigen::AngleAxisf pitchAngle(thetaX, Eigen::Vector3f::UnitX());

Eigen::Quaternionf qauternion = pitchAngle * rollAngle * yawAngle;

Eigen::Vector3f offset = Eigen::Vector3f(0.0f, 0.0f, 0.0f);
pcl::transformPointCloud(*pclCloud, *pclCloud, offset, qauternion);

我怎样才能从最后回来？

编辑：我想要做的是在 XZ 可控转盘上转换回扫描结果。开发代码如下

Eigen::Quaternionf _quaternion; pcl::PointCloud::Ptr pclCloud;

void initTransformation(float yaw, float pitch){
   _transformation = Eigen::Affine3f::Identity();
   _yaw = yaw;
   _pitch = pitch;
}

void updateTransformation(float newYaw, float newPitch){
    // Warning!
    // Don't change both axis at the same time
    // Yaw: Z  // Pitch: X

    float yawDif = _yaw - newYaw;
    float pitchDif = _pitch - newPitch;

    float yawRad = M_PI * yawDif / 180.0f;
    float pitchRad = M_PI * pitchDif / 180.0f;

    Eigen::AngleAxisf yawAngle(yawRad, Eigen::Vector3f::UnitZ());
    Eigen::AngleAxisf rollAngle(0.0f, Eigen::Vector3f::UnitY());
    Eigen::AngleAxisf pitchAngle(pitchRad, Eigen::Vector3f::UnitX());

    Eigen::Quaternionf quater = pitchAngle * rollAngle * yawAngle;
    _quaternion = quater * _quaternion;

    _yaw = newYaw;
    _pitch = newPitch;
}


 void inversetheCloud();
    Eigen::Quaternionf invQuaternion = _quaternion.inverse();
    Eigen::Vector3f offset = Eigen::Vector3f(0.0f, 0.0f, 0.0f);
    pcl::transformPointCloud(*pclCloud, *pclCloud, offset, invQuaternion);
 }

// Sample Usage
void main(){
    initTransformation(45.0f, 0.0f);
    updateTransformation(45.0f, 90.0f);
    updateTransformation(0.0f, 90.0f);
    inversetheCloud();
}

结果，第二个逆变换是正确的，但后面的逆变换是错误的，如图所示。白云：第 1 紫云：第 2 绿云：第 3

转换结果2 转换结果3

4

0 回答 0