姿态转换接口

欧拉角转四元数

	Eigen::Quaterniond euler2Quaternion(const double roll, const double pitch, const double yaw)
	{
		Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitX());
		Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitY());
		Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitZ());

		Eigen::Quaterniond q = yawAngle *  pitchAngle * rollAngle;
		return q;
	}

欧拉角转旋转矩阵

template <typename Derived>
static Eigen::Matrix<typename Derived::Scalar, 3, 3> ypr2R(const Eigen::MatrixBase<Derived>& ypr)
{
	typedef typename Derived::Scalar Scalar_t;

	Scalar_t y = ypr(0) / 180.0 * M_PI;
	Scalar_t p = ypr(1) / 180.0 * M_PI;
	Scalar_t r = ypr(2) / 180.0 * M_PI;

	Eigen::Matrix<Scalar_t, 3, 3> Rz;
	Rz << cos(y), -sin(y), 0,
		sin(y), cos(y), 0,
		0, 0, 1;

	Eigen::Matrix<Scalar_t, 3, 3> Ry;
	Ry << cos(p), 0., sin(p),
		0., 1., 0.,
		-sin(p), 0., cos(p);

	Eigen::Matrix<Scalar_t, 3, 3> Rx;
	Rx << 1., 0., 0.,
		0., cos(r), -sin(r),
		0., sin(r), cos(r);

	return Rz * Ry * Rx;
}

 Q：这个网站和算的结果不一样？

 

旋转矩阵转欧拉角

Eigen::Vector3d R2ypr(const Eigen::Matrix3d& R)
{
	Eigen::Vector3d n = R.col(0);
	Eigen::Vector3d o = R.col(1);
	Eigen::Vector3d a = R.col(2);

	Eigen::Vector3d ypr(3);
	double y = atan2(n(1), n(0));
	double p = atan2(-n(2), n(0) * cos(y) + n(1) * sin(y));
	double r = atan2(a(0) * sin(y) - a(1) * cos(y), -o(0) * sin(y) + o(1) * cos(y));
	ypr(0) = y;
	ypr(1) = p;
	ypr(2) = r;

	return ypr / M_PI * 180.0;
}

void SetMatrix(Eigen::Matrix4d matrix_) {

        Eigen::Affine3d T = Eigen::Affine3d::Identity();
        T.matrix() = std::move(matrix_);
        Eigen::Matrix4d matrix;
        matrix = T.matrix();
        Eigen::Quaterniond quaternion;
        quaternion = Eigen::Quaterniond(T.rotation().matrix());
        Eigen::Vector3d position;
        position.matrix() = T.translation().matrix();
        auto euler = QuaternionToEulerAngles(quaternion);
        double roll = euler[0] * 180. / M_PI;
        double pitch = euler[1]* 180. / M_PI;
        double yaw = euler[2]* 180. / M_PI;
        std::cout << "roll: " << roll << " , pitch: " << pitch << " , yaw: " << yaw << std::endl;
}

四元数转欧拉角

    Eigen::Vector3d QuaternionToEulerAngles(Eigen::Quaterniond quaternion) {
        auto qw = quaternion.w();
        auto qx = quaternion.x();
        auto qy = quaternion.y();
        auto qz = quaternion.z();

        double pitch, roll, yaw; //俯仰角pitch,滚动角roll,航向角yaw
        pitch = atan2f(2.f * (qw * qx + qy * qz), 1 - 2 * (qx * qx + qy * qy));
        roll = asinf(2.f * (qw * qy - qz * qx));
        yaw = atan2f(2.f * (qw * qz + qx * qy), 1 - 2 * (qy * qy + qz * qz));

        return Eigen::Vector3d(roll, pitch, yaw);

    }

四元数转旋转矩阵

Eigen::Quaterniond q = Eigen::Quaterniond::Identity();
Eigen::Matrix3d rotation = q.normalized().toRotationMatrix();