Eigen库学习(2)--------------几何模块

这是在看视觉SLAM十四讲这本书中关于Eigen Geometry模块的代码

主要是在Eigen中演示四元数、欧拉角、旋转矩阵之间的变换方式

Code

int main()
{
	//3D旋转矩阵直接使用Matrix3d或Matrix3f
	Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();//定义一个单位矩阵

	//旋转向量使用AngleAxis，它底层不直接是Matrix，但运算可以当作矩阵（因为重载了运算符）
	Eigen::AngleAxisd rotation_vector(M_PI / 4, Eigen::Vector3d(0, 0, 1));//沿Z轴旋转45°

	std::cout.precision(3);//精确到小数点后3位

	std::cout << "rotation matrix =\n" << rotation_vector.matrix() << std::endl;//用matrix()转换成矩阵也可以直接赋值

	rotation_matrix = rotation_vector.toRotationMatrix();//旋转向量转换成旋转矩阵

	//用AngleAxis可以进行坐标变换
	Eigen::Vector3d v(1, 0, 0);
	Eigen::Vector3d v_rotated = rotation_vector * v;
	std::cout << "(1,0,0) after rotation (by angle axis) = " << v_rotated.transpose() << std::endl;

	//用旋转矩阵
	v_rotated = rotation_matrix * v;
	std::cout << "(1,0,0) after rotation (by matrix) = " << v_rotated.transpose() << std::endl;

	//欧拉角：可以将旋转矩阵直接转换成欧拉角
	Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0);//ZYX顺序，即roll pitch yaw顺序
	std::cout << "yawbpitch roll = " << euler_angles.transpose() << std::endl;

	//欧式变换矩阵使用Eigen::Isometry
	Eigen::Isometry3d T = Eigen::Isometry3d::Identity();//虽然称为3D，实质上是4*4的矩阵
	T.rotate(rotation_vector);                          //按照rotation_vector进行旋转
	T.pretranslate(Eigen::Vector3d(1, 3, 4));           //把平移向量设成（1，3，4）
	std::cout << "Transform matrix = \n" << T.matrix() << std::endl;

	//用变换矩阵进行坐标变换
	Eigen::Vector3d v_transformed = T * v;//相当于R*V+t
	std::cout << "v tranformed = " << v_transformed.transpose() << std::endl;


	//四元数
	//可以直接把AngleAxis赋值给四元数，反之亦然
	Eigen::Quaterniond q = Eigen::Quaterniond(rotation_vector);
	std::cout << "quaternion from rotation vector = " << q.coeffs().transpose() << std::endl;//coeffs的顺序是(x,y,z,w).w是实部

	//把旋转矩阵赋给它
	q = Eigen::Quaterniond(rotation_matrix);
	std::cout << "quaternion from rotation matrix = " << q.coeffs().transpose() << std::endl;

	v_rotated = q * v;//qvq^{-1}
	std::cout << "(1,0,0) after rotation = " << v_rotated.transpose() << std::endl;
	std::cout << "should be equal to " << (q * Eigen::Quaterniond(0, 1, 0, 0) * q.inverse()).coeffs().transpose() << std::endl;

	return 0;
}

• 旋转矩阵$(3\times 3)$：Eigen::Matrix3d
• 旋转向量$(3\times 1)$：Eigen::AngleAxis
• 欧拉角$(3\times 1)$：Eigen::Vector3d
• 四元数$(4\times 1)$：Eigen::Quaterniond
• 欧式变换矩阵$(4\times 4)$：Eigen::Isometry3d
• 放射变换$(4\times 4)$：Eigen::Affine3d
• 射影变换$(4\times 4)$：Eigen::Projective3d

实际坐标变换例子

例子 设有小萝卜一号和小萝卜二号位于世界坐标系中。记世界坐标系为W，小萝卜们的坐标系为$R_1$和$R_2$。小萝卜一号的位姿为$q_1=[0.35,0.2,0.3,0.1{]}^T$，$t_1=[0.3,0.1,0.1{]}^T$。小萝卜二号的位姿为$q_2=[-0.5,0.4,-0.1,0.2{]}^T$，$t_2=[-0.1,0.5,0.3{]}^T$。这里的$q$和$t$表达的是$T_{R_k,W,k}=1,2$也就是世界坐标系到相机坐标系的变换关系。现在小萝卜一号看到某个点在自身的坐标系下坐标为$p_{R_1}=[0.5,0,0.2{]}^T$,求该向量在小萝卜二号坐标系下的坐标。

$$p_{R_2}=T_{R_2,W}{T}_{W,R_1}{p}_{R_1}$$

    Eigen::Quaterniond q1(0.35, 0.2, 0.3, 0.1), q2(-0.5, 0.4, -0.1, 0.2);
	q1.normalize();
	q2.normalize();
	
	Eigen::Vector3d t1(0.3, 0.1, 0.1), t2(-0.1, 0.5, 0.3);
	Eigen::Vector3d p1(0.5, 0, 0.2);

	Eigen::Isometry3d T1w(q1), T2w(q2);
	T1w.pretranslate(t1);
	T2w.pretranslate(t2);

	Eigen::Vector3d p2 = T2w * T1w.inverse() * p1;

	std::cout << std::endl << p2.transpose() << std::endl;

我觉得有问题，假如相机一位于世界坐标系下，世界坐标系到相机坐标系的旋转为0，平移为$[1,1,1{]}^T$，在相机一坐标系下看到点P的坐标为$[1,1,1{]}^T$，显而易见能得到点$P_w[2,2,2{]}^T$,而我们使用视觉SLAM十四讲上面的程序以及公式得到的坐标点为$P_w[0,0,0{]}^T$，这就很郁闷了，想不通咋回事

Eigen::Quaterniond m1(1, 0, 0, 0);
m1.normalize();
Eigen::Vector3d tt(1, 1, 1);
Eigen::Isometry3d T1w1(m1);
T1w1.pretranslate(tt);
Eigen::Vector3d p11(1, 1, 1);
Eigen::Vector3d pp = T1w1.inverse() * p11;

std::cout << std::endl << pp.transpose() << std::endl;