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