四元数左乘和右乘

double yaw = 0.6 * M_PI, pitch = 0.25 * M_PI, roll = 0.3 * M_PI;
	Eigen::Matrix3d R;
	R = Eigen::AngleAxisd(yaw, ::Eigen::Vector3d::UnitZ()) *
	    Eigen::AngleAxisd(pitch, ::Eigen::Vector3d::UnitY()) *
	    Eigen::AngleAxisd(roll, ::Eigen::Vector3d::UnitX());
	Eigen::Quaterniond q;
	q = R;

	double yaw_1 = 0.2 * M_PI, pitch_1 = 0.2 * M_PI, roll_1 = 0.45 * M_PI;
	Eigen::Vector3d eulerAngle_1(yaw_1, pitch_1, roll_1);
	Eigen::AngleAxisd rollAngle_1(Eigen::AngleAxisd(eulerAngle_1(2),Eigen::Vector3d::UnitX()));
	Eigen::AngleAxisd pitchAngle_1(Eigen::AngleAxisd(eulerAngle_1(1),Eigen::Vector3d::UnitY()));
	Eigen::AngleAxisd yawAngle_1(Eigen::AngleAxisd(eulerAngle_1(0),Eigen::Vector3d::UnitZ()));
	Eigen::Quaterniond quaternion_1;
	quaternion_1 = yawAngle_1 * pitchAngle_1 * rollAngle_1;

	Eigen::Quaterniond test_1 = q * quaternion_1;
	Eigen::Vector3d test_eulerAngle = test_1.matrix().eulerAngles(2,1,0);
	std::cout << test_eulerAngle[0] << " " << test_eulerAngle[1] << " " << test_eulerAngle[2] << std::endl;

	Eigen::Matrix4d q_l;
	//左乘矩阵
	q_l << q.w(), -q.z(), q.y(), q.x(),
	       q.z(), q.w(), -q.x(), q.y(),
	       -q.y(), q.x(), q.w(), q.z(),
	       -q.x(), -q.y(), -q.z(), q.w();
	Eigen::Vector4d q_1_vector(quaternion_1.x(), quaternion_1.y(), quaternion_1.z(), quaternion_1.w());
	Eigen::Vector4d test_2 = q_l * q_1_vector;
	Eigen::Quaterniond test_2_q(test_2[3], test_2[0], test_2[1], test_2[2]);
	Eigen::Vector3d test_2_eulerAngle = test_2_q.matrix().eulerAngles(2,1,0);
	std::cout << test_2_eulerAngle[0] << " " << test_2_eulerAngle[1] << " " << test_2_eulerAngle[2] << std::endl;

	Eigen::Matrix4d q_r;
	//右乘矩阵
	q_r << quaternion_1.w(), quaternion_1.z(), -quaternion_1.y(), quaternion_1.x(),
		     -quaternion_1.z(), quaternion_1.w(), quaternion_1.x(), quaternion_1.y(),
		     quaternion_1.y(), -quaternion_1.x(), quaternion_1.w(), quaternion_1.z(),
		     -quaternion_1.x(), -quaternion_1.y(), -quaternion_1.z(), quaternion_1.w();
	Eigen::Vector4d q_0_vector(q.x(), q.y(), q.z(), q.w());
	Eigen::Vector4d test_3 = q_r * q_0_vector;
	Eigen::Quaterniond test_3_q(test_2[3], test_2[0], test_2[1], test_2[2]);
	Eigen::Vector3d test_3_eulerAngle = test_3_q.matrix().eulerAngles(2,1,0);
	std::cout << test_3_eulerAngle[0] << " " << test_3_eulerAngle[1] << " " << test_3_eulerAngle[2] << std::endl;