理论部分请看 :三维空间刚体运动
一、Eigen的使用
首先安装 Eigen:
sudo apt-get install libeigen3-dev
一般都安装在
/usr/include/eigen3/
中
代码:
#include <iostream>
#include <ctime>
using namespace std;
//Eigen 部分
#include <Eigen/Core>
//稠密矩阵的代数运算
#include <Eigen/Dense>
#define MATRIX_SIZE 50
//本程序演示了 Eigen 基本类型的使用
int main(int argc,char** argv){
//声明一个 2×3 的 float 矩阵
Eigen::Matrix<float,2,3> matrix_23;
//Eigen 通过 typedef 提供了许多内置类型,不过底层仍然是 Eigen::Matrix
//例如 Vector3d 实质上是 Eigen::Matrix<double,3,1>
Eigen::Vector3d v_3d;
//Matrix3d 实质上是 Eigen::Matrix<double,3,3>
Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Zero();
//如果不确定矩阵大小,可以使用动态大小的矩阵
Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic> matrix_dynamic;
//更简单的
Eigen::MatrixXd matrix_x;
//矩阵操作
//输入数据
matrix_23 << 1,2,3,4,5,6;
//输出
cout<<"2*3矩阵 "<<matrix_23<<endl;
//用()访问矩阵中的元素
for(int i = 0;i<