Invert 3D Quaternion Rotation Sequence in Eigen

656 Views Asked by At

How can I invert the sequence of 3d rotation constructed in quaternions by using Eigen library?

I am constructing quaternions in terms of yaw, roll, pitch as below:

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);

How can I get back from end?

EDIT: What am I trying to do is transform back scanned results on XZ controllable turntable. Developed code is below

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();
}

As a result, second inverse transformation true but the later ones are wrong as seen on image. White cloud: 1st Purple cloud: 2nd Green cloud: 3rd

transformresult2 transformresult3

0

There are 0 best solutions below