ch3-坐标系1转换至坐标系2及其点的变化

//定义x,y,z旋转角度
double roll,pitch,yaw;

//定义旋转矩阵
Matrix3d rotation_matrix;
//总的旋转分为三轴旋转
AngleAxisd rv_roll(roll,Vector3d::UnitX());
AngleAxisd rv_pitch(pitch,rv_roll*Vector3d::UnitY());
AngleAxisd rv_yaw(yaw,rv_pitch*rv_roll*Vector3d::UnitZ());

//根据逻辑旋转x,y,z轴（顺时针为负，逆时针为正）得到旋转矩阵
rotation_matrix = (rv*rv*rv).toRotationMatrix();

//坐标系1到坐标系2的点变化左乘旋转矩阵的转置
Vector3d point1=...;
Vector3d point2= rotation_matrix.transpose()*point1;

-----------------------------------------------------------------------------------------------------------------------------

对第三讲刚体运动进行更深刻描述

上述是对角轴（AngleAxisd）运用，其运用方式为动态轴旋转

下面我们对固定轴旋转方式作出解释：

//RPY方法
Eigen::Matrix3d RPY2rotation(double,double,double);

Eigen::Matrix3d RPY2rotation(double roll,double pitch,double yaw){
    Eigen::Matrix3d R, Rx, Ry, Rz;

	Rx << 1.0, 0.0, 0.0,
		0.0, cos(roll), -sin(roll),
		0.0, sin(roll), cos(roll);

	Ry << cos(pitch), 0.0, sin(pitch),
		0.0, 1.0, 0.0,
		-sin(pitch), 0.0, cos(pitch);

	Rz << cos(yaw), -sin(yaw), 0.0,
		sin(yaw), cos(yaw), 0.0,
		0.0, 0.0, 1.0;

	R = Rz * Ry * Rx;

	return R;
}

//RPY方法使用:(point1->point2坐标变换)
rotation_matrix = RPY2rotation(double,double ,double);
point2 = rotation_matrix.transpose()*point1;


//四元数方法
rotation_matrix = (x,y,z).toRotationMatrix();//旋转矩阵求法和角轴方法有区别，这里是固定轴！
Quaterniond q = Quaterniond(rotation_matrix.transpose());//这里一定要将旋转矩阵的转置赋给四元数！！
point2 = q*point;