Commit 92d17c3e authored by Maurice Fallon's avatar Maurice Fallon
Browse files

add conversion

parent dd3fd5c3
......@@ -49,3 +49,4 @@ Eigen::VectorXf get_random_gaussian_variable(float mean, float std_deviation, in
void quat_to_euler(Eigen::Quaterniond q, double& roll, double& pitch, double& yaw);
Eigen::Quaterniond euler_to_quat(double roll, double pitch, double yaw);
......@@ -76,3 +76,30 @@ void quat_to_euler(Eigen::Quaterniond q, double& roll, double& pitch, double& ya
pitch = asin(2*(q0*q2-q3*q1));
yaw = atan2(2*(q0*q3+q1*q2), 1-2*(q2*q2+q3*q3));
}
Eigen::Quaterniond euler_to_quat(double roll, double pitch, double yaw) {
// This conversion function introduces a NaN in Eigen Rotations when:
// roll == pi , pitch,yaw =0 ... or other combinations.
// cos(pi) ~=0 but not exactly 0
if ( ((roll==M_PI) && (pitch ==0)) && (yaw ==0)){
return Eigen::Quaterniond(0,1,0,0);
}else if( ((pitch==M_PI) && (roll ==0)) && (yaw ==0)){
return Eigen::Quaterniond(0,0,1,0);
}else if( ((yaw==M_PI) && (roll ==0)) && (pitch ==0)){
return Eigen::Quaterniond(0,0,0,1);
}
double sy = sin(yaw*0.5);
double cy = cos(yaw*0.5);
double sp = sin(pitch*0.5);
double cp = cos(pitch*0.5);
double sr = sin(roll*0.5);
double cr = cos(roll*0.5);
double w = cr*cp*cy + sr*sp*sy;
double x = sr*cp*cy - cr*sp*sy;
double y = cr*sp*cy + sr*cp*sy;
double z = cr*cp*sy - sr*sp*cy;
return Eigen::Quaterniond(w,x,y,z);
}
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment