EigenRotation
引言
ch3.task2.
四元数：四元数的一个最主要的应用就是表示旋转，它既是紧凑的，也没有奇异性。而旋转的其他表示方法各有优劣：
旋转矩阵：用九个数来表示三个自由度，矩阵中的每一列表示旋转后的单位向量方向，缺点是有冗余性，不紧凑[1]。
旋转向量：用一个旋转轴和一个旋转角来表示旋转，但是因为周期性，任何2nπ的旋转等价于没有旋转，具有奇异性。
欧拉角：将旋转分解为三个分离的转角，常用在飞行器上，但因为万向锁问题（Gimbal Lock) 而同样具有奇异性。
0.常用
注意：Eigen的容器定义：
std::vector<Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d> >
//等价于
std::vector<Eigen::Vector4d>
//Eigen需要自己分配内存aligned_allocator<Eigen::Vector4d>，其实上面的写法是错误的
0）.SVD分解
J = U Σ V T J=U \Sigma V^{T} J=UΣVT
JacobiSVD<MatrixXd> svd(J,Eigen::ComputeThinU | Eigen::ComputeThinV);//Eigen::ComputeFullV/U
U = svd.matrixU();
V = svd.matrixV();
A = svd.singularValues();
1）.定义
- 头文件包含：#include <Eigen/Dense>
- Eigen::Matrix<float,行,列> //数据类型，行，列
- Eigen::Vector3d v_3d; //Eigen::Matrix<double, 3, 1>，即三维向量，一般用作平移向量
- Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Zero(); //零矩阵 // Matrix3d 实质上是 Eigen::Matrix<double, 3, 3>,一般用作旋转矩阵
- Eigen::MatrixXd matrix_x;//类比
- Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > matrix_dynamic;//动态矩阵
- Eigen::Quaterniond q1(0.35,0.2,0.3,0.1);//定义并初始化四元数
- vector<Sophus::SE3, Eigen::aligned_allocator< Sophus::SE3>> poses;//这句实际上表达的是vector< Sophus::SE3> poses的意思。但是在Eigen管理内存和C++11中的方法是不一样的，所以需要单独强调元素的内存分配和管理。(特别注意)
2）.初始化
- matrix_23 << 2,3,4,5,6; //注意常量矩阵的赋值
- matrix_33 = Eigen::Matrix3d::Random(); // 随机数矩阵
3）.类型转换
- matrix_23.cast //float类型转换成 double类型,cast关键字转换
4）.常用计算
- matrix_33.inverse()；//求逆
- matrix_33.determinant()；//求行列式
- x = A.colPivHouseholderQr().solve(b);//QR分解
- x = A.lalt().solve(b);//乔氏分解
- x = A.inverse()*b;//直接分解求解
- A.transpose();//转置
5）.SLAM数据
-
旋转矩阵R（3X3）: Eigen::Matrix3d
-
旋转向量v（3X1）: Eigen::AngleAxisd
-
四元数Q（4X1）: Eigen::Quaterniond
-
平移向量t（3X1）: Eigen::Vector3d
-
变换矩阵T（4X4）: Eigen::Isometry3d
-
四元数转变换矩阵：T.rotate(Q);
1.Eigen
#include <iostream>
using namespace std;
#include <ctime>
// Eigen 部分
#include <Eigen/Core>
// 稠密矩阵的代数运算（逆，特征值等）
#include <Eigen/Dense>
#define MATRIX_SIZE 50
/****************************
* 本程序演示了 Eigen 基本类型的使用
****************************/
int main( int argc, char** argv )
{
// 矩阵 Eigen::Matrix<float,行,列>
// Eigen 中所有向量和矩阵都是Eigen::Matrix，它是一个模板类。它的前三个参数为：数据类型，行，列
// 声明一个2*3的float矩阵
Eigen::Matrix<float, 2, 3> matrix_23;//float类型
//向量 Eigen::Vector3d
// 同时，Eigen 通过 typedef 提供了许多内置类型，不过底层仍是Eigen::Matrix
// 例如 Vector3d 实质上是 Eigen::Matrix<double, 3, 1>，即三维向量
Eigen::Vector3d v_3d;//double类型
// 这是一样的
Eigen::Matrix<float,3,1> vd_3d;//float类型
// Matrix3d 实质上是 Eigen::Matrix<double, 3, 3>
Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Zero(); //零矩阵
// MatrixXd::Identity() 单位矩阵 Eigen::Matrix3d::Random(); 随机数矩阵 MatrixXd::Ones(rows,cols)
// 均可以 用C.setXXX 设置 C.setIdentity(rows,cols) 设置单位矩阵
// 向量初始化 VectorXd::LinSpaced(size,low,high) // 线性分布
// 如果不确定矩阵大小，可以使用动态大小的矩阵 建议大矩阵使用
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > matrix_dynamic;
// 更简单的
Eigen::MatrixXd matrix_x;
// 这种类型还有很多，我们不一一列举
// 下面是对Eigen阵的操作
// 输入数据（初始化）
// 在Eigen中重载了”<<”操作符
// 通过该操作符即可以一个一个元素的进行赋值，
// 也可以一块一块的赋值。
// 另外也可以使用下标进行赋值。
//matrix_23 << 1, 2, 3, 4, 5, 6;
matrix_23 << 2,3,4,5,6; //注意常量矩阵的赋值
// 正常矩阵形式输出
cout << matrix_23 << endl;
// 用()访问矩阵中的元素
// 针对向量还提供”[]”操作符，注意矩阵则不可如此使用
for (int i=0; i<2; i++) {
for (int j=0; j<3; j++)
cout<<matrix_23(i,j)<<"\t";//每行元素的分隔符
cout<<endl;//换行
}
// 矩阵和向量相乘（实际上仍是矩阵和矩阵）
v_3d << 3, 2, 1;//double 类型
vd_3d << 4,5,6;//float 类型
// 但是在Eigen里你不能混合两种不同类型的矩阵，像这样是错的
// Eigen::Matrix<double, 2, 1> result_wrong_type = matrix_23 * v_3d;
// 应该显式转换 matrix_23.cast<double> float类型转换成 double类型
Eigen::Matrix<double, 2, 1> result = matrix_23.cast<double>() * v_3d;
cout << result << endl;
// float类型 * float 类型
Eigen::Matrix<float, 2, 1> result2 = matrix_23 * vd_3d;
cout << result2 << endl;
// 同样你不能搞错矩阵的维度
// 试着取消下面的注释，看看Eigen会报什么错
// Eigen::Matrix<double, 2, 3> result_wrong_dimension = matrix_23.cast<double>() * v_3d;
// 一些矩阵运算
// 四则运算就不演示了，直接用+-*/即可。
matrix_33 = Eigen::Matrix3d::Random(); // 随机数矩阵
cout << "Random :Matrix3d matrix_33 =\n" << matrix_33 << endl << endl;
cout << "matrix_33.transpose =\n" << matrix_33.transpose() << endl; // 转置
cout << "matrix_33.sum=\n" << matrix_33.sum() << endl; // 各元素和
cout << "matrix_33.trace=\n" << matrix_33.trace() << endl; // 迹
cout << 10*matrix_33 << endl; // 数乘
cout << matrix_33.inverse() << endl; // 逆
cout << matrix_33.determinant() << endl; // 行列式
// 特征值
// 实对称矩阵可以保证对角化成功
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigen_solver ( matrix_33.transpose()*matrix_33 );
cout << "Eigen values = \n" << eigen_solver.eigenvalues() << endl;//特征值
cout << "Eigen vectors = \n" << eigen_solver.eigenvectors() << endl;//特征向量
// 解方程
// 我们求解 matrix_NN * x = v_Nd 这个方程
// N的大小在前边的宏里定义，它由随机数生成
// 直接求逆自然是最直接的，但是求逆运算量大
Eigen::Matrix< double, MATRIX_SIZE, MATRIX_SIZE > matrix_NN;
matrix_NN = Eigen::MatrixXd::Random( MATRIX_SIZE, MATRIX_SIZE );//随机变量初始化
Eigen::Matrix< double, MATRIX_SIZE, 1> v_Nd; //列向量
v_Nd = Eigen::MatrixXd::Random( MATRIX_SIZE,1 ); //随机变量初始化
clock_t time_stt = clock(); // 计时
// 直接求逆
Eigen::Matrix<double,MATRIX_SIZE,1> x = matrix_NN.inverse()*v_Nd;
//cout << "x = \n" << x << endl;
cout <<"time use in normal inverse is " << 1000* (clock() - time_stt)/(double)CLOCKS_PER_SEC << "ms"<< endl;
// 通常用矩阵分解来求，例如QR分解，速度会快很多
time_stt = clock();
x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
//cout << "x = \n" << x << endl;
cout <<"time use in Qr decomposition is " <<1000* (clock() - time_stt)/(double)CLOCKS_PER_SEC <<"ms" << endl;
//矩阵分块
Eigen::Matrix<double,5,5> Matrix_55;
Matrix_55 = Eigen::MatrixXd::Random(5,5);
cout<<"Random Matrix_55 :\n"<<Matrix_55<<endl;
Eigen::Matrix3d matrixI33 = Eigen::Matrix3d::Identity();
cout<<"Eye matrixI33 :\n"<<matrixI33<<endl;
Matrix_55.topLeftCorner(3,3)=matrixI33;
cout<<"Random Matrix_55 topLeft block replace by Eye matrixI33 :\n"<<Matrix_55<<endl;
return 0;
}
结果
2 3 4
5 6 0
2 3 4
5 6 0
16
27
47
50
Random :Matrix3d matrix_33 =
0.680375 0.59688 -0.329554
-0.211234 0.823295 0.536459
0.566198 -0.604897 -0.444451
matrix_33.transpose =
0.680375 -0.211234 0.566198
0.59688 0.823295 -0.604897
-0.329554 0.536459 -0.444451
matrix_33.sum=
1.61307
matrix_33.trace=
1.05922
6.80375 5.9688 -3.29554
-2.11234 8.23295 5.36459
5.66198 -6.04897 -4.44451
-0.198521 2.22739 2.8357
1.00605 -0.555135 -1.41603
-1.62213 3.59308 3.28973
0.208598
Eigen values =
0.0242899
0.992154
1.80558
Eigen vectors =
-0.549013 -0.735943 0.396198
0.253452 -0.598296 -0.760134
-0.796459 0.316906 -0.514998
time use in normal inverse is 5.727ms
time use in Qr decomposition is 0.141ms
Random Matrix_55 :
0.733986 -0.809318 -0.565308 0.328632 -0.33954
-0.418401 0.637993 0.773498 -0.210317 -0.568724
-0.410198 0.0891278 -0.437198 -0.0349491 0.985537
-0.406587 -0.244224 0.498914 0.901787 -0.796094
-0.597866 0.788795 0.373976 0.580917 0.896412
Eye matrixI33 :
1 0 0
0 1 0
0 0 1
Random Matrix_55 topLeft block replace by Eye matrixI33 :
1 0 0 0.328632 -0.33954
0 1 0 -0.210317 -0.568724
0 0 1 -0.0349491 0.985537
-0.406587 -0.244224 0.498914 0.901787 -0.796094
-0.597866 0.788795 0.373976 0.580917 0.896412
Process finished with exit code 0
CakeLists.txt
cmake_minimum_required( VERSION 2.8 )
project( useEigen )
set( CMAKE_BUILD_TYPE "Release" )
set( CMAKE_CXX_FLAGS "-O3" )
# 添加Eigen头文件
include_directories( "/usr/include/eigen3" )
# in osx and brew install
# include_directories( /usr/local/Cellar/eigen/3.3.3/include/eigen3 )
# eigen 是现编译的 没有调用 动态链接库
add_executable( eigenMatrix eigenMatrix.cpp )
2.EIgen简介
linux下安装
sudo apt-get install libeigen3-dev
定位安装位置
locate eigen3
sudo updatedb
- 当调用 Eigen库 成员 时，一下情况需要注意。 Eigen库中的数据结构作为自定义的结构体或者类中的成员; STL容器含有Eigen的数据结构，Eigen数据结构作为函数的参数 。
1:数据结构使用 Eigen库 成员
class Foo
{
...
Eigen::Vector2d v;//
...
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW //不加 会提示 对其错误
}
2.STL Containers 标准容器vector<> 中使用 Eigen库 成员
vector<Eigen::Matrix4d>;//会提示出错
vector<Eigen::Matrix4d,Eigen::aligned_allocator<Eigen::Matrix4d>>;//aligned_allocator管理C++中的各种数据类型的内存方法是一样的,但是在Eigen中不一样
-
阵类型：
Eigen中的矩阵类型一般都是用类似MatrixXXX来表示，可以根据该名字来判断其数据类型，比如:- ”d”表示double类型
- ”f”表示float类型
- ”i”表示整数
- ”c”表示复数
- Matrix2f，表示的是一个2*2维的，其每个元素都是float类型。
-
数据存储：
- Matrix<double, 3, 3, RowMajor> E; // 行排列(存储)的矩阵
- Matrix<int,3, 4, ColMajor> Acolmajor; // 默认为列排列(存储)的矩阵
-
动态矩阵和静态矩阵：
- 动态矩阵是指其大小在运行时确定，静态矩阵是指其大小在编译时确定。
- MatrixXd：表示任意大小的元素类型为double的矩阵变量，其大小只有在运行时被赋值之后才能知道。
- Matrix3d：表示元素类型为double大小为3*3的矩阵变量，其大小在编译时就知道。
- 在Eigen中行优先的矩阵会在其名字中包含有row，否则就是列优先。
- Eigen中的向量只是一个特殊的矩阵，其维度为1而已。
-
矩阵元素的访问：
- 在矩阵的访问中，行索引总是作为第一个参数，Eigen中矩阵、数组、向量的下标都是从0开始。
- 矩阵元素的访问可以通过”()”操作符完成。
- 例如m(2, 3)既是获取矩阵m的第2行第3列元素。
- 针对向量还提供”[]”操作符，注意矩阵则不可如此使用。
-
设置矩阵的元素：
在Eigen中重载了”<<”操作符， 通过该操作符即可以一个一个元素的进行赋值， 也可以一块一块的赋值。 另外也可以使用下标进行赋值。
A << 1, 2, 3, // 按列填充A
4, 5, 6, //
7, 8, 9; //
B << A, A, A; // 块赋值
A.fill(10); // 函数赋值 -
重置矩阵大小：
- 当前矩阵的行数、列数、大小可以通过rows()、cols()和size()来获取， 对于动态矩阵可以通过resize()函数来动态修改矩阵的大小。 注意：
- (1)、固定大小的矩阵是不能使用resize()来修改矩阵的大小；
- (2)、resize()函数会析构掉原来的数据，因此调用resize()函数之后将不能保证元素的值不改变；
- (3)、使用”=”操作符操作动态矩阵时，如果左右两边的矩阵大小不等，
则左边的动态矩阵的大小会被修改为右边的大小。
- 当前矩阵的行数、列数、大小可以通过rows()、cols()和size()来获取， 对于动态矩阵可以通过resize()函数来动态修改矩阵的大小。 注意：
如何选择动态矩阵和静态矩阵：对于小矩阵(一般大小小于16)使用固定大小的静态矩阵，它可以带来比较高的效率； 对于大矩阵(一般大小大于32)建议使用动态矩阵。
注意：如果特别大的矩阵使用了固定大小的静态矩阵则可能会造成栈溢出的问题。
- Matrix<double, 3, 3> A; // 固定行和列的矩阵 Fixed Matrix3d.
- Matrix<double, 3, Dynamic> B; // 固定行，列数不定的矩阵 Matrix<double, Dynamic，3> BB;
- Matrix<double, Dynamic, Dynamic> C; // 行列数不定的矩阵 MatrixXd.
- Matrix<double, 3, 3, RowMajor> E; // 行排列(存储)的矩阵，默认为列排列(存储)的矩阵 - - Matrix<int,3, 4, ColMajor> Acolmajor;
- Matrix3f P, Q, R; // 3x3 float matrix. 浮点型 3x3矩阵
- Vector3f x, y, z; // 3x1 float matrix. 浮点型 3x1向量
- RowVector3f a, b, c; // 行向量 1x3 float matrix.
- VectorXd v; // 动态 doubles 类型向量
#include <iostream>
using namespace std;
#include <ctime>
// Eigen 部分
#include <Eigen/Core>
// 稠密矩阵的代数运算（逆，特征值等）
#include <Eigen/Dense>
#define MATRIX_SIZE 50
/****************************
旋转矩阵（3X3）: Eigen::Matrix3d
旋转向量（3X1）: Eigen::AngleAxisd
四元数（4X1）: Eigen::Quaterniond
平移向量（3X1）: Eigen::Vector3d
变换矩阵（4X4）: Eigen::Isometry3d
***************************/
int main( int argc, char** argv )
{
// Eigen 中所有向量和矩阵都是Eigen::Matrix，它是一个模板类。它的前三个参数为：数据类型，行，列
// 声明一个2*3的float矩阵
Eigen::Matrix<float, 2, 3> matrix_23;
// 同时，Eigen 通过 typedef 提供了许多内置类型，不过底层仍是Eigen::Matrix
// 例如 Vector3d 实质上是 Eigen::Matrix<double, 3, 1>，即三维向量,三行一列。
Eigen::Vector3d v_3d;
//v_3d<<7, 8 ,9 ;
//cout<<v_3d<<endl;
// 这是一样的
Eigen::Matrix<float,3,1> vd_3d;
// Matrix3d 实质上是 Eigen::Matrix<double, 3, 3>
Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Zero(); //初始化为零
// cout<<matrix_33<<endl;
// 如果不确定矩阵大小，可以使用动态大小的矩阵
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > matrix_dynamic;
// 更简单的
Eigen::MatrixXd matrix_x;
// 这种类型还有很多，我们不一一列举
// 下面是对Eigen阵的操作
// 输入数据（初始化）
matrix_23 << 1, 2, 3, 4, 5, 6;
// 输出
cout << matrix_23 << endl;
// 用()访问矩阵中的元素
for (int i=0; i<2; i++) {
for (int j=0; j<3; j++)
cout<<matrix_23(i,j)<<"\t";
cout<<endl;
}
// 矩阵和向量相乘（实际上仍是矩阵和矩阵）
v_3d << 3, 2, 1;
vd_3d << 4,5,6;
// 但是在Eigen里你不能混合两种不同类型的矩阵，像这样是错的
// Eigen::Matrix<double, 2, 1> result_wrong_type = matrix_23 * v_3d;
// 应该显式转换
Eigen::Matrix<double, 2, 1> result = matrix_23.cast<double>() * v_3d;
cout << result << endl;
Eigen::Matrix<float, 2, 1> result2 = matrix_23 * vd_3d;
cout << result2 << endl;
// 同样你不能搞错矩阵的维度
// 试着取消下面的注释，看看Eigen会报什么错
// Eigen::Matrix<double, 2, 3> result_wrong_dimension = matrix_23.cast<double>() * v_3d;
// 一些矩阵运算
// 四则运算就不演示了，直接用+-*/即可。
matrix_33 = Eigen::Matrix3d::Random(); // 随机数矩阵
cout << matrix_33 << endl << endl;
cout << matrix_33.transpose() << endl; // 转置
cout << matrix_33.sum() << endl; // 各元素和
cout << matrix_33.trace() << endl; // 迹
cout << 10*matrix_33 << endl; // 数乘
cout << matrix_33.inverse() << endl; // 逆
cout << matrix_33.determinant() << endl; // 行列式
// 特征值
// 实对称矩阵可以保证对角化成功
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigen_solver ( matrix_33.transpose()*matrix_33 );
cout << "Eigen values = \n" << eigen_solver.eigenvalues() << endl;
cout << "Eigen vectors = \n" << eigen_solver.eigenvectors() << endl;
// 解方程
// 我们求解 matrix_NN * x = v_Nd 这个方程
// N的大小在前边的宏里定义，它由随机数生成
// 直接求逆自然是最直接的，但是求逆运算量大
Eigen::Matrix< double, MATRIX_SIZE, MATRIX_SIZE > matrix_NN;
matrix_NN = Eigen::MatrixXd::Random( MATRIX_SIZE, MATRIX_SIZE );
Eigen::Matrix< double, MATRIX_SIZE, 1> v_Nd;
v_Nd = Eigen::MatrixXd::Random( MATRIX_SIZE,1 );
clock_t time_stt = clock(); // 计时
// 直接求逆,高斯消元法的算法复杂度是O(n3)；
Eigen::Matrix<double,MATRIX_SIZE,1> x = matrix_NN.inverse()*v_Nd;
cout <<"time use in normal inverse is " << 1000* (clock() - time_stt)/(double)CLOCKS_PER_SEC << "ms"<< endl;
// 通常用矩阵分解来求，例如QR分解，速度会快很多
time_stt = clock();
x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
cout <<"time use in Qr decomposition is " <<1000* (clock() - time_stt)/(double)CLOCKS_PER_SEC <<"ms" << endl;
return 0;
}
// ===============Eigen 矩阵定义===============
#include <Eigen/Dense>
Matrix<double, 3, 3> A; // Fixed rows and cols. Same as Matrix3d.
Matrix<double, 3, Dynamic> B; // Fixed rows, dynamic cols.
Matrix<double, Dynamic, Dynamic> C; // Full dynamic. Same as MatrixXd.
Matrix<double, 3, 3, RowMajor> E; // Row major; default is column-major.
Matrix3f P, Q, R; // 3x3 float matrix.
Vector3f x, y, z; // 3x1 float matrix.
RowVector3f a, b, c; // 1x3 float matrix.
VectorXd v; // Dynamic column vector of doubles
// Eigen // Matlab // comments
x.size() // length(x) // vector size
C.rows() // size(C,1) // number of rows
C.cols() // size(C,2) // number of columns
x(i) // x(i+1) // Matlab is 1-based
C(i,j) // C(i+1,j+1) //
===============Eigen 基础使用===============
// Basic usage
// Eigen // Matlab // comments
x.size() // length(x) // vector size
C.rows() // size(C,1) // number of rows
C.cols() // size(C,2) // number of columns
x(i) // x(i+1) // Matlab is 1-based
C(i, j) // C(i+1,j+1) //
A.resize(4, 4); // Runtime error if assertions are on.
B.resize(4, 9); // Runtime error if assertions are on.
A.resize(3, 3); // Ok; size didn't change.
B.resize(3, 9); // Ok; only dynamic cols changed.
A << 1, 2, 3, // Initialize A. The elements can also be
4, 5, 6, // matrices, which are stacked along cols
7, 8, 9; // and then the rows are stacked.
B << A, A, A; // B is three horizontally stacked A's.
A.fill(10); // Fill A with all 10's.
//===============Eigen 特殊矩阵生成===============
// Eigen // Matlab
MatrixXd::Identity(rows,cols) // eye(rows,cols)
C.setIdentity(rows,cols) // C = eye(rows,cols)
MatrixXd::Zero(rows,cols) // zeros(rows,cols)
C.setZero(rows,cols) // C = ones(rows,cols)
MatrixXd::Ones(rows,cols) // ones(rows,cols)
C.setOnes(rows,cols) // C = ones(rows,cols)
MatrixXd::Random(rows,cols) // rand(rows,cols)*2-1 // MatrixXd::Random returns uniform random numbers in (-1, 1).
C.setRandom(rows,cols) // C = rand(rows,cols)*2-1
VectorXd::LinSpaced(size,low,high) // linspace(low,high,size)'
v.setLinSpaced(size,low,high) // v = linspace(low,high,size)'
//===============Eigen 矩阵分块===============
// Matrix slicing and blocks. All expressions listed here are read/write.
// Templated size versions are faster. Note that Matlab is 1-based (a size N
// vector is x(1)...x(N)).
// Eigen // Matlab
x.head(n) // x(1:n)
x.head<n>() // x(1:n)
x.tail(n) // x(end - n + 1: end)
x.tail<n>() // x(end - n + 1: end)
x.segment(i, n) // x(i+1 : i+n)
x.segment<n>(i) // x(i+1 : i+n)
P.block(i, j, rows, cols) // P(i+1 : i+rows, j+1 : j+cols)
P.block<rows, cols>(i, j) // P(i+1 : i+rows, j+1 : j+cols)
P.row(i) // P(i+1, :)
P.col(j) // P(:, j+1)
P.leftCols<cols>() // P(:, 1:cols)
P.leftCols(cols) // P(:, 1:cols)
P.middleCols<cols>(j) // P(:, j+1:j+cols)
P.middleCols(j, cols) // P(:, j+1:j+cols)
P.rightCols<cols>() // P(:, end-cols+1:end)
P.rightCols(cols) // P(:, end-cols+1:end)
P.topRows<rows>() // P(1:rows, :)
P.topRows(rows) // P(1:rows, :)
P.middleRows<rows>(i) // P(i+1:i+rows, :)
P.middleRows(i, rows) // P(i+1:i+rows, :)
P.bottomRows<rows>() // P(end-rows+1:end, :)
P.bottomRows(rows) // P(end-rows+1:end, :)
P.topLeftCorner(rows, cols) // P(1:rows, 1:cols)
P.topRightCorner(rows, cols) // P(1:rows, end-cols+1:end)
P.bottomLeftCorner(rows, cols) // P(end-rows+1:end, 1:cols)
P.bottomRightCorner(rows, cols) // P(end-rows+1:end, end-cols+1:end)
P.topLeftCorner<rows,cols>() // P(1:rows, 1:cols)
P.topRightCorner<rows,cols>() // P(1:rows, end-cols+1:end)
P.bottomLeftCorner<rows,cols>() // P(end-rows+1:end, 1:cols)
P.bottomRightCorner<rows,cols>() // P(end-rows+1:end, end-cols+1:end)
//===============Eigen 矩阵元素交换===============
// Of particular note is Eigen's swap function which is highly optimized.
// Eigen // Matlab
R.row(i) = P.col(j); // R(i, :) = P(:, i)
R.col(j1).swap(mat1.col(j2)); // R(:, [j1 j2]) = R(:, [j2, j1])
//===============Eigen 矩阵转置===============
// Views, transpose, etc; all read-write except for .adjoint().
// Eigen // Matlab
R.adjoint() // R'
R.transpose() // R.' or conj(R')
R.diagonal() // diag(R)
x.asDiagonal() // diag(x)
R.transpose().colwise().reverse(); // rot90(R)
R.conjugate() // conj(R)
//===============Eigen 矩阵乘积===============
// All the same as Matlab, but matlab doesn't have *= style operators.
// Matrix-vector. Matrix-matrix. Matrix-scalar.
y = M*x; R = P*Q; R = P*s;
a = b*M; R = P - Q; R = s*P;
a *= M; R = P + Q; R = P/s;
R *= Q; R = s*P;
R += Q; R *= s;
R -= Q; R /= s;
//===============Eigen 矩阵单个元素操作===============
// Vectorized operations on each element independently
// Eigen // Matlab
R = P.cwiseProduct(Q); // R = P .* Q
R = P.array() * s.array();// R = P .* s
R = P.cwiseQuotient(Q); // R = P ./ Q
R = P.array() / Q.array();// R = P ./ Q
R = P.array() + s.array();// R = P + s
R = P.array() - s.array();// R = P - s
R.array() += s; // R = R + s
R.array() -= s; // R = R - s
R.array() < Q.array(); // R < Q
R.array() <= Q.array(); // R <= Q
R.cwiseInverse(); // 1 ./ P
R.array().inverse(); // 1 ./ P
R.array().sin() // sin(P)
R.array().cos() // cos(P)
R.array().pow(s) // P .^ s
R.array().square() // P .^ 2
R.array().cube() // P .^ 3
R.cwiseSqrt() // sqrt(P)
R.array().sqrt() // sqrt(P)
R.array().exp() // exp(P)
R.array().log() // log(P)
R.cwiseMax(P) // max(R, P)
R.array().max(P.array()) // max(R, P)
R.cwiseMin(P) // min(R, P)
R.array().min(P.array()) // min(R, P)
R.cwiseAbs() // abs(P)
R.array().abs() // abs(P)
R.cwiseAbs2() // abs(P.^2)
R.array().abs2() // abs(P.^2)
(R.array() < s).select(P,Q); // (R < s ? P : Q)
//===============Eigen 矩阵化简===============
// Reductions.
int r, c;
// Eigen // Matlab
R.minCoeff() // min(R(:))
R.maxCoeff() // max(R(:))
s = R.minCoeff(&r, &c) // [s, i] = min(R(:)); [r, c] = ind2sub(size(R), i);
s = R.maxCoeff(&r, &c) // [s, i] = max(R(:)); [r, c] = ind2sub(size(R), i);
R.sum() // sum(R(:))
R.colwise().sum() // sum(R)
R.rowwise().sum() // sum(R, 2) or sum(R')'
R.prod() // prod(R(:))
R.colwise().prod() // prod(R)
R.rowwise().prod() // prod(R, 2) or prod(R')'
R.trace() // trace(R)
R.all() // all(R(:))
R.colwise().all() // all(R)
R.rowwise().all() // all(R, 2)
R.any() // any(R(:))
R.colwise().any() // any(R)
R.rowwise().any() // any(R, 2)
//===============Eigen 矩阵点乘===============
// Dot products, norms, etc.
// Eigen // Matlab
x.norm() // norm(x). Note that norm(R) doesn't work in Eigen.
x.squaredNorm() // dot(x, x) Note the equivalence is not true for complex
x.dot(y) // dot(x, y)
x.cross(y) // cross(x, y) Requires #include <Eigen/Geometry>
//===============Eigen 矩阵类型转换===============
Type conversion
// Eigen // Matlab
A.cast<double>(); // double(A)
A.cast<float>(); // single(A)
A.cast<int>(); // int32(A)
A.real(); // real(A)
A.imag(); // imag(A)
// if the original type equals destination type, no work is done
//===============Eigen 求解线性方程组 Ax = b===============
// Solve Ax = b. Result stored in x. Matlab: x = A \ b.
x = A.ldlt().solve(b)); // A sym. p.s.d. #include <Eigen/Cholesky>
x = A.llt() .solve(b)); // A sym. p.d. #include <Eigen/Cholesky>
x = A.lu() .solve(b)); // Stable and fast. #include <Eigen/LU>
x = A.qr() .solve(b)); // No pivoting. #include <Eigen/QR>
x = A.svd() .solve(b)); // Stable, slowest. #include <Eigen/SVD>
// .ldlt() -> .matrixL() and .matrixD()
// .llt() -> .matrixL()
// .lu() -> .matrixL() and .matrixU()
// .qr() -> .matrixQ() and .matrixR()
// .svd() -> .matrixU(), .singularValues(), and .matrixV()
//===========Eigen 矩阵特征值===============
// Eigenvalue problems
// Eigen // Matlab
A.eigenvalues(); // eig(A);
EigenSolver<Matrix3d> eig(A); // [vec val] = eig(A)
eig.eigenvalues(); // diag(val)
eig.eigenvectors(); // vec
// For self-adjoint matrices use SelfAdjointEigenSolver<>
3.旋转矩阵、旋转向量、旋转角、四元数
#include <iostream>
#include <Eigen/Dense>
using namespace std;
using namespace Eigen;
int main(int argc, char **argv) {
//下面三个变量作为下面演示的中间变量
AngleAxisd t_V(M_PI / 4, Vector3d(0, 0, 1));
Matrix3d t_R = t_V.matrix();
Quaterniond t_Q(t_V);
//对旋转向量（轴角）赋值的三大种方法
//1.使用旋转的角度和旋转轴向量（此向量为单位向量）来初始化角轴
AngleAxisd V1(M_PI / 4, Vector3d(0, 0, 1));//以（0,0,1）为旋转轴，旋转45度
cout << "Rotation_vector1" << endl << V1.matrix() << endl;
//2.使用旋转矩阵转旋转向量的方式
//2.1 使用旋转向量的fromRotationMatrix()函数来对旋转向量赋值（注意此方法为旋转向量独有,四元数没有）
AngleAxisd V2;
V2.fromRotationMatrix(t_R);
cout << "Rotation_vector2" << endl << V2.matrix() << endl;
//2.2 直接使用旋转矩阵来对旋转向量赋值
AngleAxisd V3;
V3 = t_R;
cout << "Rotation_vector3" << endl << V3.matrix() << endl;
//2.3 使用旋转矩阵来对旋转向量进行初始化
AngleAxisd V4(t_R);
cout << "Rotation_vector4" << endl << V4.matrix() << endl;
//3. 使用四元数来对旋转向量进行赋值
//3.1 直接使用四元数来对旋转向量赋值
AngleAxisd V5;
V5 = t_Q;
cout << "Rotation_vector5" << endl << V5.matrix() << endl;
//3.2 使用四元数来对旋转向量进行初始化
AngleAxisd V6(t_Q);
cout << "Rotation_vector6" << endl << V6.matrix() << endl;
//------------------------------------------------------
//对四元数赋值的三大种方法（注意Eigen库中的四元数前三维是虚部,最后一维是实部）
//1.使用旋转的角度和旋转轴向量（此向量为单位向量）来初始化四元数,即使用q=[cos(A/2),n_x*sin(A/2),n_y*sin(A/2),n_z*sin(A/2)]
Quaterniond Q1(cos((M_PI / 4) / 2), 0 * sin((M_PI / 4) / 2), 0 * sin((M_PI / 4) / 2),
1 * sin((M_PI / 4) / 2));//以（0,0,1）为旋转轴，旋转45度
//第一种输出四元数的方式
cout << "Quaternion1" << endl << Q1.coeffs() << endl;
//第二种输出四元数的方式
cout << Q1.x() << endl << endl;
cout << Q1.y() << endl << endl;
cout << Q1.z() << endl << endl;
cout << Q1.w() << endl << endl;
//2. 使用旋转矩阵转四元數的方式
//2.1 直接使用旋转矩阵来对旋转向量赋值
Quaterniond Q2;
Q2 = t_R;
cout << "Quaternion2" << endl << Q2.coeffs() << endl;
//2.2 使用旋转矩阵来对四元數进行初始化
Quaterniond Q3(t_R);
cout << "Quaternion3" << endl << Q3.coeffs() << endl;
//3. 使用旋转向量对四元数来进行赋值
//3.1 直接使用旋转向量对四元数来赋值
Quaterniond Q4;
Q4 = t_V;
cout << "Quaternion4" << endl << Q4.coeffs() << endl;
//3.2 使用旋转向量来对四元数进行初始化
Quaterniond Q5(t_V);
cout << "Quaternion5" << endl << Q5.coeffs() << endl;
//----------------------------------------------------
//对旋转矩阵赋值的三大种方法
//1.使用旋转矩阵的函数来初始化旋转矩阵
Matrix3d R1 = Matrix3d::Identity();
cout << "Rotation_matrix1" << endl << R1 << endl;
//2. 使用旋转向量转旋转矩阵来对旋转矩阵赋值
//2.1 使用旋转向量的成员函数matrix()来对旋转矩阵赋值
Matrix3d R2;
R2 = t_V.matrix();
cout << "Rotation_matrix2" << endl << R2 << endl;
//2.2 使用旋转向量的成员函数toRotationMatrix()来对旋转矩阵赋值
Matrix3d R3;
R3 = t_V.toRotationMatrix();
cout << "Rotation_matrix3" << endl << R3 << endl;
//3. 使用四元数转旋转矩阵来对旋转矩阵赋值
//3.1 使用四元数的成员函数matrix()来对旋转矩阵赋值
Matrix3d R4;
R4 = t_Q.matrix();
cout << "Rotation_matrix4" << endl << R4 << endl;
//3.2 使用四元数的成员函数toRotationMatrix()来对旋转矩阵赋值
Matrix3d R5;
R5 = t_Q.toRotationMatrix();
cout << "Rotation_matrix5" << endl << R5 << endl;
return 0;
}
4.eigenGeometry
#include <iostream>
#include <cmath>
using namespace std;
// Eigen 部分
#include <Eigen/Core>
// Eigen 几何模块
#include <Eigen/Geometry>
/****************************
* 本程序演示了 Eigen 几何模块的使用方法
* 旋转向量 Eigen::AngleAxisd 角度 轴 Eigen::AngleAxisd rotation_vector ( M_PI/4, Eigen::Vector3d ( 0,0,1 ) ); //沿 Z 轴旋转 45 度
* 旋转矩阵 Eigen::Matrix3d rotation_vector.toRotationMatrix(); //旋转向量转换到旋转矩阵
* Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d(0,0,1)).toRotationMatrix();// 直接转
*
* 欧拉角 Eigen::Vector3d rotation_matrix.eulerAngles ( 2,1,0 );// ( 2,1,0 )表示ZYX顺序，即roll pitch yaw顺序 旋转矩阵到 欧拉角转换到欧拉角
* 四元素 Eigen::Quaterniond q = Eigen::Quaterniond ( rotation_vector );// 旋转向量 定义四元素
* q = Eigen::Quaterniond ( rotation_matrix );//旋转矩阵定义四元素
*
* 欧式变换矩阵 Eigen::Isometry3d T=Eigen::Isometry3d::Identity();// 虽然称为3d，实质上是4＊4的矩阵 旋转 R+ 平移T
* T.rotate ( rotation_vector ); // 按照rotation_vector进行旋转
* 也可 Eigen::Isometry3d T(q) // 一步 按四元素表示的旋转 旋转 转换矩阵
* T.pretranslate ( Eigen::Vector3d ( 1,3,4 ) ); // 把平移向量设成(1,3,4)
* cout<< T.matrix() <<endl;
*
****************************/
int main ( int argc, char** argv )
{
//注意一下类型名的最后一个字符为d表示双精度类型,换成f表示单精度类型，两种类型不能混用，必须显示转换
// Eigen/Geometry 模块提供了各种旋转和平移的表示
// 3D 旋转矩阵直接使用 Matrix3d 或 Matrix3f
/****旋转向量****/
// 旋转向量使用 AngleAxis, 它底层不直接是Matrix，但运算可以当作矩阵（因为重载了运算符）
// 乘以该向量，表示进行一个坐标变换
//任意旋转可用一个旋转轴和一个旋转角度来表示。
//旋转向量，旋转向量的方向与旋转轴一致，长度为旋转角度。
/*********************************/
/*旋转向量 沿 Z 轴旋转 45 度 角度 轴 */
Eigen::AngleAxisd rotation_vector ( M_PI/4, Eigen::Vector3d ( 0,0,1 ) ); //沿 Z 轴旋转 45 度
cout .precision(3);//输出精度
cout<<"rotation matrix =\n"<<rotation_vector.matrix() <<endl; //用matrix()转换成矩阵
// 也可以直接赋值
/*********************************/
/*旋转矩阵*/
Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();//单位阵 3x3
rotation_matrix = rotation_vector.toRotationMatrix(); // 旋转向量转成旋转矩阵 由罗德里格公式进行转换
// 用 AngleAxis 可以进行坐标变换
Eigen::Vector3d v ( 1,0,0 );
/*************旋转向量进行坐标变换********************/
Eigen::Vector3d v_rotated = rotation_vector * v;
cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;// transpose纯粹是为了输出美观一些
// 或者用旋转矩阵
/*****************旋转矩阵进行坐标变换****************/
v_rotated = rotation_matrix * v;
cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;
/**欧拉角表示的旋转**/
// 欧拉角: 可以将 旋转矩阵直接转换成欧拉角
Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles ( 2,1,0 ); // (2,1,0)表示ZYX顺序，即roll pitch yaw顺序
cout<<"yaw pitch roll = "<<euler_angles.transpose()<<endl;
/***欧式变换矩阵表示旋转+平移**/
// R t;0 0 0 1
// 欧氏变换矩阵使用 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)
cout << "转换矩阵 Transform matrix = \n" << T.matrix() <<endl;
// 用变换矩阵进行坐标变换
Eigen::Vector3d v_transformed = T*v; // 相当于R*v+t
cout<<"(1,0,0) after Isometry3d tranformed = "<<v_transformed.transpose()<<endl;
// 对于仿射和射影变换，使用 Eigen::Affine3d 和 Eigen::Projective3d 即可，略
/*******四元数表示的旋转***********/
// 可以直接把AngleAxis赋值给四元数，反之亦然 Quaterniond 表示双精度 四元素 Quaternionf 表示单精度四元素
Eigen::Quaterniond q = Eigen::Quaterniond ( rotation_vector );// 表示沿Z 轴旋转 45 度 的四元素变换
cout<<"quaternion from AngleAxis rotation_vector = \n"<<q.coeffs() <<endl; // 请注意coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部
// 也可以把 旋转矩阵 赋给它
q = Eigen::Quaterniond ( rotation_matrix );
cout<<"quaternion from rotation_matrix = \n"<<q.coeffs() <<endl;
// 使用四元数旋转一个向量，使用重载的乘法即可
/*注意程序表达形式和实际运算的不一样*/
v_rotated = q*v; // 注意数学上是q*v*q^{-1} 而程序为了简化表示 直接使用 q*v代替
cout<<"(1,0,0) after Quaterniond rotation = "<<v_rotated.transpose()<<endl;
/*编程题目
小萝卜１号位姿q1=[0.35,0.2,0.3,0.1],t1=[0.3,0.1,0.1]' 世界坐标系到相机变换
小萝卜２号位姿q2=[-0.5,0.4,-0.1,0.2],t2=[-0.1,0.5,0.3]'
小萝卜１号看到位于自身坐标系下p=[0.5,0,0.2]'
求该向量在小萝卜２号下的坐标
*/
Eigen::Quaterniond q1(0.35,0.2,0.3,0.1);//wxyz q1.coeffs() xyzw q1.vec() xyz
//q1 << 0.35,0.2,0.3,0.1;
Eigen::Matrix<double, 3, 1> t1;//double类型
t1 << 0.3,0.1,0.1;
Eigen::Quaterniond q2(-0.5,0.4,-0.1,0.2);
//q2 << -0.5,0.4,-0.1,0.2;
Eigen::Matrix<double, 3, 1> t2;//double类型
t2 << -0.1,0.5,0.3;
Eigen::Matrix<double, 3, 1> p1;//double类型 坐标
p1 << 0.5,0,0.2;
cout<<"q1= \n"<< q1.coeffs() <<endl;
cout<<"t1= \n"<< t1 <<endl;
cout<<"q2= \n"<< q2.coeffs() <<endl;
cout<<"t2= \n"<< t2 <<endl;
/*
q1.setIdentity(); //单位四元素
cout<<"q1 after setIdentity \n"<<q1.coeffs() <<endl;
q2.setIdentity();
cout<<"q2 after setIdentity \n"<<q2.coeffs() <<endl;
*/
//规范化 归一化 除以模长
q1=q1.normalized();
cout<<"q1 after normalized\n"<<q1.coeffs() <<endl;
q2=q2.normalized();
cout<<"q2 after normalized \n"<<q2.coeffs() <<endl;
Eigen::Matrix3d q1rotation_matrix = Eigen::Matrix3d::Identity();//初始化为单位阵
q1rotation_matrix=q1.toRotationMatrix();//四元素到旋转矩阵
Eigen::Isometry3d Tc1w=Eigen::Isometry3d::Identity();// 虽然称为3d，实质上是4＊4的矩阵
//以上也可 Eigen::Isometry3d Tc1w(q1) 一步 按四元素表示的旋转 旋转 转换矩阵
Tc1w.rotate (q1rotation_matrix ); // 按照q1rotation_matrix进行旋转
Tc1w.pretranslate ( t1); // 把平移向量设成t1
//Eigen::Isometry3d Twc1=Tc1w.inverse();//由world 到c1的逆变换 成 c1到world
Eigen::Matrix<double, 3, 1> pw=Tc1w.inverse()*p1; //将c1坐标系下的点p1变换到world坐标系下
Eigen::Matrix3d q2rotation_matrix = Eigen::Matrix3d::Identity();//初始化为单位阵
q2rotation_matrix=q2.toRotationMatrix();//四元素到旋转矩阵
Eigen::Isometry3d Tc2w=Eigen::Isometry3d::Identity();// 虽然称为3d，实质上是4＊4的矩阵 齐次坐标
Tc2w.rotate (q2rotation_matrix ); // 按照q1rotation_matrix进行旋转 // Eigen::Isometry3d Tc2w(q2)
//以上也可 Eigen::Isometry3d Tc2w(q2) 一步
Tc2w.pretranslate ( t2); // 把平移向量设成t1
Eigen::Matrix<double, 3, 1> p2=Tc2w*pw; //将world坐标系下的点pw变换到c2坐标系下
cout<<"the loc of p1 in c1 = \n"<< p1<<endl;
cout<<"the loc of p1 in world = \n"<< pw<<endl;
cout<<"the loc of p1 in c2 = \n"<< p2<<endl;
return 0;
}
结果
rotation matrix =
0.707 -0.707 0
0.707 0.707 0
0 0 1
(1,0,0) after rotation = 0.707 0.707 0
(1,0,0) after rotation = 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
(1,0,0) after Isometry3d tranformed = 1.71 3.71 4
quaternion from AngleAxis rotation_vector =
0
0
0.383
0.924
quaternion from rotation_matrix =
0
0
0.383
0.924
(1,0,0) after Quaterniond rotation = 0.707 0.707 0
q1=
0.2
0.3
0.1
0.35
t1=
0.3
0.1
0.1
q2=
0.4
-0.1
0.2
-0.5
t2=
-0.1
0.5
0.3
q1 after normalized
0.39
0.586
0.195
0.683
q2 after normalized
0.59
-0.147
0.295
-0.737
the loc of p1 in c1 =
0.5
0
0.2
the loc of p1 in world =
-0.0895
0.0524
0.222
the loc of p1 in c2 =
-0.031
0.735
0.296
Process finished with exit code 0
CmakeLists.txt
cmake_minimum_required( VERSION 2.8 )
project( geometry )
# 添加Eigen头文件
include_directories( "/usr/include/eigen3" )
add_executable( eigenGeometry eigenGeometry.cpp )
5.visualizeGeometry
#include <iostream>
#include <cmath>
using namespace std;
#include <Eigen/Core>
// Eigen 几何模块
#include <Eigen/Geometry>
/****************************
* 本程序演示了 Eigen 几何模块的使用方法
****************************/
void gaoXiangDemo( int argc, char** argv )
{
// Eigen/Geometry 模块提供了各种旋转和平移的表示
// (1) 3D 旋转矩阵R直接使用 Matrix3d 或 Matrix3f
Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();
// (2) 旋转向量(轴角表示法）使用 AngleAxis, 它底层不直接是Matrix，但运算可以当作矩阵（因为重载了运算符）
Eigen::AngleAxisd rotation_vector ( M_PI/4, Eigen::Vector3d ( 0,0,1 ) ); //沿 Z 轴旋转 45 度
cout .precision(3); //输出结果精确到小数点候三位？precision精确
cout<<"rotation matrix =\n"<<rotation_vector.matrix() <<endl; //用matrix()转换成矩阵，旋转向量转换为旋转矩阵
// 也可以直接赋值
rotation_matrix = rotation_vector.toRotationMatrix(); //用toRotationMatrix()转换成矩阵，旋转向量转换为旋转矩阵
// 用 AngleAxis 可以进行坐标变换
Eigen::Vector3d v ( 1,0,0 );
Eigen::Vector3d v_rotated = rotation_vector * v;
//矩阵的转置、共轭矩阵、伴随矩阵transpose(), conjugate(), adjoint()
cout<<"(1,0,0) rotation_vector after rotation = "<<v_rotated.transpose()<<endl; //transpose向量转置
// 或者用旋转矩阵
v_rotated = rotation_matrix * v;
cout<<"(1,0,0) rotation_matrix after rotation = "<<v_rotated.transpose()<<endl;
// (3) 欧拉角: 可以将旋转矩阵直接转换成欧拉角
Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles ( 2,1,0 ); // ZYX顺序，即roll pitch yaw顺序
cout<<"yaw pitch roll = "<<euler_angles.transpose()<<endl;
// (4) 欧氏变换矩阵T 使用 Eigen::Isometry ,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)
cout << "Transform matrix = \n" << T.matrix() <<endl;
// (4） 用变换矩阵进行坐标变换
Eigen::Vector3d v_transformed = T*v; // 相当于R*v+t
cout<<"v tranformed = "<<v_transformed.transpose()<<endl;
// 对于仿射和射影变换，使用 Eigen::Affine3d 和 Eigen::Projective3d 即可，略
// (5） 四元数
// 可以直接把AngleAxis赋值给四元数，反之亦然。类型转换 Eigen::Quaterniond
Eigen::Quaterniond q = Eigen::Quaterniond ( rotation_vector );
cout<<"quaternion = \n"<<q.coeffs() <<endl; // 请注意coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部
// 也可以把旋转矩阵赋给它
q = Eigen::Quaterniond ( rotation_matrix );
cout<<"quaternion = \n"<<q.coeffs() <<endl;
// 使用四元数旋转一个向量，使用重载的乘法即可
v_rotated = q*v; // 注意数学上是qvq^{-1}，此为重载的乘法
cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;
}
//=====================================================================================
/****************************
* 本程序演示了 Eigen 几何模块的使用方法
* 旋转向量 Eigen::AngleAxisd角度 轴Eigen::AngleAxisd rotation_vector ( M_PI/4, Eigen::Vector3d(0,0,1)); //沿Z轴旋转45度
* 旋转矩阵 Eigen::Matrix3d rotation_vector.toRotationMatrix(); //旋转向量转换到旋转矩阵
* Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d(0,0,1)).toRotationMatrix();// 直接转
*
* 欧拉角 Eigen::Vector3d rotation_matrix.eulerAngles ( 2,1,0 );//( 2,1,0 )表示ZYX顺序，即roll pitch yaw顺序旋转矩阵到 欧拉角转换到欧拉角
* 四元素 Eigen::Quaterniond q = Eigen::Quaterniond ( rotation_vector );// 旋转向量 定义四元素
* q = Eigen::Quaterniond ( rotation_matrix );//旋转矩阵定义四元素
*
* 欧式变换矩阵 Eigen::Isometry3d T=Eigen::Isometry3d::Identity();// 虽然称为3d，实质上是4＊4的矩阵 旋转 R+ 平移T
* T.rotate ( rotation_vector ); // 按照rotation_vector进行旋转
* 也可 Eigen::Isometry3d T(q) // 一步 按四元素表示的旋转 旋转 转换矩阵
* T.pretranslate ( Eigen::Vector3d ( 1,3,4 ) ); // 把平移向量设成(1,3,4)
* cout<< T.matrix() <<endl;
*
****************************/
int main ( int argc, char** argv )
{
//注意一下类型名的最后一个字符为d表示双精度类型,换成f表示单精度类型，两种类型不能混用，必须显示转换
// Eigen/Geometry 模块提供了各种旋转和平移的表示
// 3D 旋转矩阵直接使用 Matrix3d 或 Matrix3f
/****旋转向量****/
// 旋转向量使用 AngleAxis, 它底层不直接是Matrix，但运算可以当作矩阵（因为重载了运算符）
// 乘以该向量，表示进行一个坐标变换
//任意旋转可用一个旋转轴和一个旋转角度来表示。
//旋转向量，旋转向量的方向与旋转轴一致，长度为旋转角度。
/*********************************/
/*旋转向量 沿 Z 轴旋转 45 度 角度 轴 */
Eigen::AngleAxisd rotation_vector ( M_PI/4, Eigen::Vector3d ( 0,0,1 ) ); //沿 Z 轴旋转 45 度
cout .precision(3);//输出精度
cout<<"rotation matrix =\n"<<rotation_vector.matrix() <<endl; //用matrix()转换成矩阵
// 也可以直接赋值
/*********************************/
/*旋转矩阵*/
Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();//单位阵 3x3
rotation_matrix = rotation_vector.toRotationMatrix(); // 旋转向量转成旋转矩阵 由罗德里格公式进行转换
// 用 AngleAxis 可以进行坐标变换
Eigen::Vector3d v ( 1,0,0 );
/*************旋转向量进行坐标变换********************/
Eigen::Vector3d v_rotated = rotation_vector * v;
cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;// transpose纯粹是为了输出美观一些
// 或者用旋转矩阵
/*****************旋转矩阵进行坐标变换****************/
v_rotated = rotation_matrix * v;
cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;
/**欧拉角表示的旋转**/
// 欧拉角: 可以将 旋转矩阵直接转换成欧拉角
Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles ( 2,1,0 ); // (2,1,0)表示ZYX顺序，即roll pitch yaw顺序
cout<<"yaw pitch roll = "<<euler_angles.transpose()<<endl;
/***欧式变换矩阵表示旋转+平移**/
// R t;0 0 0 1
// 欧氏变换矩阵使用 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)
cout << "转换矩阵 Transform matrix = \n" << T.matrix() <<endl;
// 用变换矩阵进行坐标变换
Eigen::Vector3d v_transformed = T*v; // 相当于R*v+t
cout<<"(1,0,0) after Isometry3d tranformed = "<<v_transformed.transpose()<<endl;
// 对于仿射和射影变换，使用 Eigen::Affine3d 和 Eigen::Projective3d 即可，略
/*******四元数表示的旋转***********/
// 可以直接把AngleAxis赋值给四元数，反之亦然 Quaterniond 表示双精度 四元素 Quaternionf 表示单精度四元素
Eigen::Quaterniond q = Eigen::Quaterniond ( rotation_vector );// 表示沿Z 轴旋转 45 度 的四元素变换
cout<<"quaternion from AngleAxis rotation_vector = \n"<<q.coeffs() <<endl; // 请注意coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部
// 也可以把 旋转矩阵 赋给它
q = Eigen::Quaterniond ( rotation_matrix );
cout<<"quaternion from rotation_matrix = \n"<<q.coeffs() <<endl;
// 使用四元数旋转一个向量，使用重载的乘法即可
/*注意程序表达形式和实际运算的不一样*/
v_rotated = q*v; // 注意数学上是q*v*q^{-1} 而程序为了简化表示 直接使用 q*v代替
cout<<"(1,0,0) after Quaterniond rotation = "<<v_rotated.transpose()<<endl;
/*编程题目
小萝卜１号位姿q1=[0.35,0.2,0.3,0.1],t1=[0.3,0.1,0.1]' 世界坐标系到相机变换
小萝卜２号位姿q2=[-0.5,0.4,-0.1,0.2],t2=[-0.1,0.5,0.3]'
小萝卜１号看到位于自身坐标系下p=[0.5,0,0.2]'
求该向量在小萝卜２号下的坐标
*/
Eigen::Quaterniond q1(0.35,0.2,0.3,0.1);//wxyz q1.coeffs() xyzw q1.vec() xyz
//q1 << 0.35,0.2,0.3,0.1;
Eigen::Matrix<double, 3, 1> t1;//double类型
t1 << 0.3,0.1,0.1;
Eigen::Quaterniond q2(-0.5,0.4,-0.1,0.2);
//q2 << -0.5,0.4,-0.1,0.2;
Eigen::Matrix<double, 3, 1> t2;//double类型
t2 << -0.1,0.5,0.3;
Eigen::Matrix<double, 3, 1> p1;//double类型 坐标
p1 << 0.5,0,0.2;
cout<<"q1= \n"<< q1.coeffs() <<endl;
cout<<"t1= \n"<< t1 <<endl;
cout<<"q2= \n"<< q2.coeffs() <<endl;
cout<<"t2= \n"<< t2 <<endl;
/*
q1.setIdentity(); //单位四元素
cout<<"q1 after setIdentity \n"<<q1.coeffs() <<endl;
q2.setIdentity();
cout<<"q2 after setIdentity \n"<<q2.coeffs() <<endl;
*/
//规范化 归一化 除以模长
q1=q1.normalized();
cout<<"q1 after normalized\n"<<q1.coeffs() <<endl;
q2=q2.normalized();
cout<<"q2 after normalized \n"<<q2.coeffs() <<endl;
Eigen::Matrix3d q1rotation_matrix = Eigen::Matrix3d::Identity();//初始化为单位阵
q1rotation_matrix=q1.toRotationMatrix();//四元素到旋转矩阵
Eigen::Isometry3d Tc1w=Eigen::Isometry3d::Identity();// 虽然称为3d，实质上是4＊4的矩阵
//以上也可 Eigen::Isometry3d Tc1w(q1) 一步 按四元素表示的旋转 旋转 转换矩阵
Tc1w.rotate (q1rotation_matrix ); // 按照q1rotation_matrix进行旋转
Tc1w.pretranslate ( t1); // 把平移向量设成t1
//Eigen::Isometry3d Twc1=Tc1w.inverse();//由world 到c1的逆变换 成 c1到world
Eigen::Matrix<double, 3, 1> pw=Tc1w.inverse()*p1; //将c1坐标系下的点p1变换到world坐标系下
Eigen::Matrix3d q2rotation_matrix = Eigen::Matrix3d::Identity();//初始化为单位阵
q2rotation_matrix=q2.toRotationMatrix();//四元素到旋转矩阵
Eigen::Isometry3d Tc2w=Eigen::Isometry3d::Identity();// 虽然称为3d，实质上是4＊4的矩阵 齐次坐标
Tc2w.rotate (q2rotation_matrix ); // 按照q1rotation_matrix进行旋转 // Eigen::Isometry3d Tc2w(q2)
//以上也可 Eigen::Isometry3d Tc2w(q2) 一步
Tc2w.pretranslate ( t2); // 把平移向量设成t1
Eigen::Matrix<double, 3, 1> p2=Tc2w*pw; //将world坐标系下的点pw变换到c2坐标系下
cout<<"the loc of p1 in c1 = \n"<< p1<<endl;
cout<<"the loc of p1 in world = \n"<< pw<<endl;
cout<<"the loc of p1 in c2 = \n"<< p2<<endl;
return 0;
}
CmakeLists.txt
cmake_minimum_required( VERSION 2.8 )
project( visualizeGeometry )
set(CMAKE_CXX_FLAGS "-std=c++11")
# 添加Eigen头文件
include_directories( "/usr/include/eigen3" )
# 添加Pangolin依赖
find_package( Pangolin )
include_directories( ${Pangolin_INCLUDE_DIRS} )
add_executable( visualizeGeometry visualizeGeometry.cpp )
target_link_libraries( visualizeGeometry ${Pangolin_LIBRARIES} )
readme.txt
# 库介绍
* Pangolin是一个用于OpenGL显示/交互以及视频输入的一个轻量级、快速开发库
* Pangolin是对OpenGL进行封装的轻量级的OpenGL输入/输出和视频显示的库。
* 可以用于3D视觉和3D导航的视觉图，可以输入各种类型的视频、并且可以保留视频和输入数据用于debug。
* 下载工具Pangolin github: https://github.com/stevenlovegrove/Pangolin
# 安装 pangolin 需要的依赖库
OpenGL (Desktop / ES / ES2)依赖
Glew 依赖
sudo apt-get install libglew-dev
CMake 依赖 编译需要
sudo apt-get install cmake
Boost 依赖 多线程 文件系统 是C++标准化进程的开发引擎之一
sudo apt-get install libboost-dev libboost-thread-dev libboost-filesystem-dev
python 依赖
sudo apt-get install libpython2.7-dev
# 编译 安装l pangolin
cd [path-to-pangolin]
mkdir build
cd build
cmake ..
make
sudo make install
# 编译此项目
* compile this program:
mkdir build
cd build
cmake ..
make
* run the build/visualizeGeometry
2. How to use this program:
The UI in the left panel displays different representations of T_w_c ( camera to world ).
显示 旋转矩阵 平移向量 欧拉角 四元素
Drag your left mouse button to move the camera, 左键 移动相机
right button to rotate it around the box, 右键 以箱子为中心旋转相机
center button to rotate the camera itself, 中键 旋转相机本身
and press both left and right button to roll the view.
Note that in this program the original X axis is right (red line), Y is up (green line) and Z in back axis (blue line). You (camera) are looking at (0,0,0) standing on (3,3,3) at first.
3. Problems may happen:
* I found that in virtual machines there may be an error in pangolin, which was solved in its issue: https://github.com/stevenlovegrove/Pangolin/issues/74 . You need to comment the two lines mentioned by paulinus, and the recompile and reinstall Pangolin, if you happen to find this problem.
If you still have problems using this program, please contact: gaoxiang12@mails.tsinghua.edu.cn
6.练习
#include <iostream>
#include <ctime>
#include <cmath>
// Eigen 部分
#include <Eigen/Core>
// 稠密矩阵的代数运算（逆，特征值等）
#include <Eigen/Dense>
#include <Eigen/Cholesky>
#define MATRIX_SIZE 50
using namespace std;
/****************************
编程实现 A 为 100 × 100 随机矩阵时，⽤ QR 和 Cholesky 分解求 x 的程序。
****************************/
void solve()
{
// 解方程
// 我们求解 matrix_NN * x = v_Nd 这个方程
// N的大小在前边的宏里定义，它由随机数生成
// 直接求逆自然是最直接的，但是求逆运算量大
Eigen::Matrix< double, 100, 100 > matrix_NN;
matrix_NN = Eigen::MatrixXd::Random( 100, 100 );
// cout<<matrix_NN<<endl;
Eigen::Matrix< double, 100, 1> v_Nd;
v_Nd = Eigen::MatrixXd::Random( 100,1 );
// cout<<v_Nd<<endl;
clock_t time_stt = clock(); // 计时
// 直接求逆,高斯消元法的算法复杂度是O(n3)；
Eigen::Matrix<double,100,1> x = matrix_NN.inverse()*v_Nd;
cout <<"time use in normal inverse is " << 1000* (clock() - time_stt)/(double)CLOCKS_PER_SEC << "ms"<< endl;
cout << "The Result is:"<<endl << x << endl << endl;
// 通常用矩阵分解来求，例如QR分解，速度会快很多
time_stt = clock();
x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
cout <<"time use in Qr decomposition is " <<1000* (clock() - time_stt)/(double)CLOCKS_PER_SEC <<"ms" << endl;
cout << "The Result is:"<<endl << x << endl << endl;
time_stt = clock();
x = matrix_NN.ldlt().solve(v_Nd);
cout <<"time use in Cholesky decomposition is " <<1000* (clock() - time_stt)/(double)CLOCKS_PER_SEC <<"ms" << endl;
cout << "The Result is:"<< endl <<x << endl << endl;
}
/****************************
题目：设有⼩萝⼘ 1⼀号和⼩萝⼘⼆号位于世界坐标系中。⼩萝⼘⼀号的位姿为： q1 = [0:55; 0:3; 0:2; 0:2]; t1 =
[0:7; 1:1; 0:2]T（ q 的第⼀项为实部）。这⾥的 q 和 t 表达的是 Tcw，也就是世界到相机的变换关系。⼩萝⼘
⼆号的位姿为 q2 = [−0:1; 0:3; −0:7; 0:2]; t2 = [−0:1; 0:4; 0:8]T。现在，⼩萝⼘⼀号看到某个点在⾃⾝的坐
标系下，坐标为 p1 = [0:5; −0:1; 0:2]T，求该向量在⼩萝⼘⼆号坐标系下的坐标。请编程实现此事，并提交
你的程序。
***************************/
/******************************************************
* 法一
* 整体思路 C1->W1=W2->C2
先把四元数归一化，题目给出的表达是Tcw,是世界到相机的变换关系，所以要先把q1转化为相机到世界的变换，也就是q1的逆可以表达相反的变换
对于单位四元数，其逆和共轭就是同一个量，而四元数的共轭是把虚部取成相反数，所以在归一化的时候把q1的虚部取为相反数就行了。
pw = q1wc * (p1 - t1)
然后计算pw在萝卜二号坐标系的坐标
p2 = pw * q2 + t2
******************************************************/
void rotation1()
{
Eigen::Vector3d p1,t1,t2;
p1 << 0.5,-0.1,0.2;
t1 << 0.7,1.1,0.2;
t2 << -0.1,0.4,0.8;
// 四元数Eigen::Quaterniond 的正确初始化顺序为Eigen::Quaterniond(w,x,y,z)
// 而 coeffs的顺序是（x,y,z,w）,w 为实部，前三者为虚部
// 因为要表示相反的旋转，故输入为q1的共轭，即实部不变，虚部变为相反数
Eigen::Quaterniond q1 = Eigen::Quaterniond(0.55,-0.3,-0.2,-0.2).normalized();//normalized()归一化
cout << q1.coeffs().transpose() << endl;
Eigen::Quaterniond q2 = Eigen::Quaterniond(-0.1,0.3,-0.7,0.2).normalized();
cout << q2.coeffs().transpose() << endl;
Eigen::Vector3d pw = q1*(p1-t1); //数学上是qpq-1,先把平移转换了再进行旋转变换
Eigen::Vector3d p2 = q2*pw + t2;
cout << p2 << endl;
}
/*************************************
* 法二
* 用变换矩阵的方法同样的思路，先求出两个变换矩阵T1和T2 ，p1和T1 的逆矩阵相乘得到 pw,
* 然后再和T2相乘得到 p2.p2 = T2 * T1.inverse() * p1
*************************************/
void rotation2()
{
Eigen::Vector3d p1,t1,t2;
p1 << 0.5,-0.1,0.2;
t1 << 0.7,1.1,0.2;
t2 << -0.1,0.4,0.8;
Eigen::Quaterniond q1 = Eigen::Quaterniond(0.55,0.3,0.2,0.2).normalized();
Eigen::Isometry3d T1 = Eigen::Isometry3d::Identity(); // 平移矩阵,虽然称为3d,实质上是4＊4的矩阵
T1.rotate ( q1 ); //按照q1进行旋转
T1.pretranslate ( t1 ); //按照t1进行平移
cout << "Transform matrix = \n" << T1.matrix() << endl;
Eigen::Quaterniond q2 = Eigen::Quaterniond(-0.1,0.3,-0.7,0.2).normalized();
Eigen::Isometry3d T2 = Eigen::Isometry3d::Identity();
T2.rotate ( q2 );
T2.pretranslate ( t2 );
cout << "Transform matrix = \n" << T2.matrix() << endl;
Eigen::Vector3d p2 = T2*T1.inverse()*p1; //先求逆，再转换到C2.
cout << p2 << endl;
}
int main( int argc, char** argv )
{
solve();
//rotation1();
//rotation2();
return 0;
}
CmakeLists.txt
cmake_minimum_required( VERSION 2.8 )
project( useEigen )
set( CMAKE_BUILD_TYPE "Release" )
set( CMAKE_CXX_FLAGS "-O3" )
# 添加Eigen头文件
include_directories( "/usr/include/eigen3" )
# in osx and brew install
# include_directories( /usr/local/Cellar/eigen/3.3.3/include/eigen3 )
add_executable( eigenMatrix eigenMatrix.cpp )
7.SVD分解求伪逆
MatrixXd 是double类型，如果是float类型请改为 MatrixXf
计算结果 可与matlab的 pinv() 函数 对比，计算结果是一致的。
Eigen::MatrixXd MainWindow::pinv(Eigen::MatrixXd A)
{
Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV);//M=USV*
double pinvtoler = 1.e-8; //tolerance
int row = A.rows();
int col = A.cols();
int k = min(row,col);
Eigen::MatrixXd X = Eigen::MatrixXd::Zero(col,row);
Eigen::MatrixXd singularValues_inv = svd.singularValues();//奇异值
Eigen::MatrixXd singularValues_inv_mat = Eigen::MatrixXd::Zero(col, row);
for (long i = 0; i<k; ++i) {
if (singularValues_inv(i) > pinvtoler)
singularValues_inv(i) = 1.0 / singularValues_inv(i);
else singularValues_inv(i) = 0;
}
for (long i = 0; i < k; ++i)
{
singularValues_inv_mat(i, i) = singularValues_inv(i);
}
X=(svd.matrixV())*(singularValues_inv_mat)*(svd.matrixU().transpose());//X=VS+U*
return X;
}
1772
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
