视觉slam十四讲第3讲的Eigen实践模块代码

平台与工具

• win11
• visual studio 2022 community

代码

#include<Eigen/Dense>
#include<iostream>
#include<cmath>
#include<Eigen/Core>
#include<Eigen/Geometry>
#include<math.h>

using namespace std;
using namespace Eigen;

#define PI acos(-1)

//typedef acos(-1) PI

int main()
{
	Matrix3d rotation_matrix = Matrix3d::Identity();
	// 绕z轴旋转45度
	AngleAxisd rotation_vector(PI / 4, Vector3d(0, 0, 1));
	cout.precision(3);
	cout << "rotation matrix = \n" <<
		rotation_vector.matrix() << endl;
	// 也可以直接赋值
	rotation_matrix = rotation_vector.toRotationMatrix();
	// 用AngleAxis可以进行坐标变换
	Vector3d v(1, 0, 0);
	Vector3d v_rotated = rotation_vector * v;
	cout << "(1,0,0) after rotation (by angle axis) = " << v_rotated.transpose() << endl;
	// 或者用旋转矩阵
	v_rotated = rotation_matrix * v;
	cout << "(1,0,0) after rotation (by matrix) = " << v_rotated.transpose() << endl;
	// 也可以将旋转矩阵直接转换成欧拉角
	Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0); // zyx
	cout << "yaw pitch roll = " << euler_angles.transpose() << endl;

	// 欧式变换矩阵使用 Eigen::Isometry
	Isometry3d T = Isometry3d::Identity();
	// 按照rotation_vector进行旋转
	T.rotate(rotation_vector);
	// 把平移向量设成(1, 3, 4)
	T.pretranslate(Vector3d(1, 3, 4));
	cout << "Transform matrix = \n" << T.matrix() << endl;

	// 用变换矩阵进行坐标变换
	Vector3d v_transformed = T * v;
	cout << "v transformed = \n" << v_transformed.transpose() << endl;

	// 可以直接把AngleAxis赋值给四元数，反之亦然
	Quaterniond q = Quaterniond(rotation_vector);
	cout << "quaternion from rotation vector = " << q.coeffs().transpose() << endl;
	// 使用4元数旋转一个向量,使用重载的乘法即可
	v_rotated = q * v;// 数学上是qvq逆
	cout << "(1,0,0) after rotation = " << v_rotated.transpose() << endl;
	// 用常规向量乘法表示
	cout << "should be equal to " <<
		(q * Quaterniond(0, 1, 0, 0) * q.inverse()).coeffs().transpose() << endl;

	return 0;
}

结果

rotation matrix =
 0.707 -0.707      0
 0.707  0.707      0
     0      0      1
(1,0,0) after rotation (by angle axis) = 0.707 0.707     0
(1,0,0) after rotation (by matrix) = 0.707 0.707     0
yaw pitch roll = 0.785    -0     0
Transform matrix =
 0.707 -0.707      0      1
 0.707  0.707      0      3
     0      0      1      4
     0      0      0      1
v transformed =
1.71 3.71    4
quaternion from rotation vector =     0     0 0.383 0.924
(1,0,0) after rotation = 0.707 0.707     0
should be equal to 0.707 0.707     0     0

E:\projectFiles\CPPcodes\slam\chap3\eigen002\x64\Release\eigen002.exe (进程 35728)已退出，代码为 0。
按任意键关闭此窗口. . .