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 × 3 ) (3\times3) (3×3)Eigen::Matrix3d
  • 旋转向量 ( 3 × 1 ) (3\times1) (3×1)Eigen::AngleAxis
  • 欧拉角 ( 3 × 1 ) (3\times1) (3×1)Eigen::Vector3d
  • 四元数 ( 4 × 1 ) (4\times1) (4×1)Eigen::Quaterniond
  • 欧式变换矩阵 ( 4 × 4 ) (4\times4) (4×4)Eigen::Isometry3d
  • 放射变换 ( 4 × 4 ) (4\times4) (4×4)Eigen::Affine3d
  • 射影变换 ( 4 × 4 ) (4\times4) (4×4)Eigen::Projective3d

实际坐标变换例子

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

p R 2 = T R 2 , W T W , R 1 p R 1 p_{R_2}=T_{{R_2},W}T_{W,{R_1}} p_{R_1} pR2=TR2,WTW,R1pR1

    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 [1,1,1]^T [1,1,1]T,在相机一坐标系下看到点P的坐标为 [ 1 , 1 , 1 ] T [1,1,1]^T [1,1,1]T,显而易见能得到点 P w [ 2 , 2 , 2 ] T P_w[2,2,2]^T Pw[2,2,2]T,而我们使用视觉SLAM十四讲上面的程序以及公式得到的坐标点为 P w [ 0 , 0 , 0 ] T P_w[0,0,0]^T Pw[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;

你可能感兴趣的:(SLAM,学习)