基于OpenCV的四元数、旋转矩阵和欧拉角互相转换(一)

发布于:2021-11-30 16:20:08

这部分代码我摘自于google中搜索出来的博客,当然,在格式或者风格上不太统一。如果你想看到更纯粹的OpenCV版本,请戳基于OpenCV的四元数、旋转矩阵和欧拉角互相转换(二)。


四元数转旋转矩阵


void getRotation(double *Quaternion, double *rt_mat)
{
rt_mat[0] = 1 - 2 * (Quaternion[2] * Quaternion[2]) - 2 * (Quaternion[3] * Quaternion[3]);
rt_mat[1] = 2 * Quaternion[1] * Quaternion[2] - 2 * Quaternion[0] * Quaternion[3];
rt_mat[2] = 2 * Quaternion[1] * Quaternion[3] + 2 * Quaternion[0] * Quaternion[2];
rt_mat[3] = 2 * Quaternion[1] * Quaternion[2] + 2 * Quaternion[0] * Quaternion[3];
rt_mat[4] = 1 - 2 * (Quaternion[1] * Quaternion[1]) - 2 * (Quaternion[3] * Quaternion[3]);
rt_mat[5] = 2 * Quaternion[2] * Quaternion[3] - 2 * Quaternion[0] * Quaternion[1];
rt_mat[6] = 2 * Quaternion[1] * Quaternion[3] - 2 * Quaternion[0] * Quaternion[2];
rt_mat[7] = 2 * Quaternion[2] * Quaternion[3] + 2 * Quaternion[0] * Quaternion[1];
rt_mat[8] = 1 - 2 * (Quaternion[1] * Quaternion[1]) - 2 * (Quaternion[2] * Quaternion[2]);
}

旋转矩阵转四元数


void getQuaternion(Mat R, double Q[])
{
double trace = R.at(0,0) + R.at(1,1) + R.at(2,2);

if (trace > 0.0)
{
double s = sqrt(trace + 1.0);
Q[3] = (s * 0.5);
s = 0.5 / s;
Q[0] = ((R.at(2,1) - R.at(1,2)) * s);
Q[1] = ((R.at(0,2) - R.at(2,0)) * s);
Q[2] = ((R.at(1,0) - R.at(0,1)) * s);
}

else
{
int i = R.at(0,0) < R.at(1,1) ? (R.at(1,1) < R.at(2,2) ? 2 : 1) : (R.at(0,0) < R.at(2,2) ? 2 : 0);
int j = (i + 1) % 3;
int k = (i + 2) % 3;

double s = sqrt(R.at(i, i) - R.at(j,j) - R.at(k,k) + 1.0);
Q[i] = s * 0.5;
s = 0.5 / s;

Q[3] = (R.at(k,j) - R.at(j,k)) * s;
Q[j] = (R.at(j,i) + R.at(i,j)) * s;
Q[k] = (R.at(k,i) + R.at(i,k)) * s;
}
}

欧拉角转旋转矩阵


// Calculates rotation matrix given euler angles.
Mat eulerAnglesToRotationMatrix(Vec3f &theta)
{
// Calculate rotation about x axis
Mat R_x = (Mat_(3,3) <<
1, 0, 0,
0, cos(theta[0]), -sin(theta[0]),
0, sin(theta[0]), cos(theta[0])
);

// Calculate rotation about y axis
Mat R_y = (Mat_(3,3) <<
cos(theta[1]), 0, sin(theta[1]),
0, 1, 0,
-sin(theta[1]), 0, cos(theta[1])
);

// Calculate rotation about z axis
Mat R_z = (Mat_(3,3) <<
cos(theta[2]), -sin(theta[2]), 0,
sin(theta[2]), cos(theta[2]), 0,
0, 0, 1);


// Combined rotation matrix
Mat R = R_z * R_y * R_x;

return R;

}

旋转矩阵转欧拉角


// Checks if a matrix is a valid rotation matrix.
bool isRotationMatrix(Mat &R)
{
Mat Rt;
transpose(R, Rt);
Mat shouldBeIdentity = Rt * R;
Mat I = Mat::eye(3,3, shouldBeIdentity.type());
return norm(I, shouldBeIdentity) < 1e-6;

}

// Calculates rotation matrix to euler angles
// The result is the same as MATLAB except the order
// of the euler angles ( x and z are swapped ).
Vec3f rotationMatrixToEulerAngles(Mat &R)
{

assert(isRotationMatrix(R));

float sy = sqrt(R.at(0,0) * R.at(0,0) + R.at(1,0) * R.at(1,0) );

bool singular = sy < 1e-6; // If

float x, y, z;
if (!singular)
{
x = atan2(R.at(2,1) , R.at(2,2));
y = atan2(-R.at(2,0), sy);
z = atan2(R.at(1,0), R.at(0,0));
}
else
{
x = atan2(-R.at(1,2), R.at(1,1));
y = atan2(-R.at(2,0), sy);
z = 0;
}
return Vec3f(x, y, z);
}

四元数转欧拉角


#define _USE_MATH_DEFINES
#include

struct Quaternion {
double w, x, y, z;
};

struct EulerAngles {
double roll, pitch, yaw;
};

EulerAngles ToEulerAngles(Quaternion q) {
EulerAngles angles;

// roll (x-axis rotation)
double sinr_cosp = 2 * (q.w * q.x + q.y * q.z);
double cosr_cosp = 1 - 2 * (q.x * q.x + q.y * q.y);
angles.roll = std::atan2(sinr_cosp, cosr_cosp);

// pitch (y-axis rotation)
double sinp = 2 * (q.w * q.y - q.z * q.x);
if (std::abs(sinp) >= 1)
angles.pitch = std::copysign(M_PI / 2, sinp); // use 90 degrees if out of range
else
angles.pitch = std::asin(sinp);

// yaw (z-axis rotation)
double siny_cosp = 2 * (q.w * q.z + q.x * q.y);
double cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z);
angles.yaw = std::atan2(siny_cosp, cosy_cosp);

return angles;
}

相关推荐

最新更新

猜你喜欢