第2个回答 2023-06-26
Eigen::Quaterniond vector_quaterniond(Eigen::Vector3d input_vector) {
double roll, pitch, yaw;
if (fabs(input_vector[1]) > 0.000001) {
roll = atan(input_vector[2] / input_vector[1]);
} else {
roll = M_PI / 2.0;
}
if (fabs(input_vector[2]) > 0.000001) {
pitch = atan(input_vector[0] / input_vector[2]);
} else {
pitch = M_PI / 2.0;
}
if (fabs(input_vector[0]) > 0.000001) {
yaw = atan(input_vector[1] / input_vector[0]);
} else {
yaw = M_PI / 2.0;
}
Eigen::Vector3d eulerAngle(roll, pitch, yaw);
Eigen::AngleAxisd rollAngle(Eigen::AngleAxisd(eulerAngle(0), Eigen::Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(Eigen::AngleAxisd(eulerAngle(1), Eigen::Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(Eigen::AngleAxisd(eulerAngle(2), Eigen::Vector3d::UnitZ()));
Eigen::Quaterniond quaternion;
quaternion = yawAngle * pitchAngle * rollAngle;
return quaternion;
}