Eigen::Matrix_QTRtv
EigenRotation
- 引言
- 0、常用
- 1、Eigen
- 2、 EIgen简介
- 3、旋转矩阵、旋转向量、旋转角、四元数
- 4、eigenGeometry
- 5、visualizeGeometry
- 6、练习
- 7.SVD分解求伪逆
引言
ch3.task2.
0、常用
注意：Eigen的容器定义：
std::vector >
//等价于
std::vector
//Eigen需要自己分配内存aligned_allocator，其实上面的写法是错误的
0）.SVD分解
J = U Σ V T J=U \Sigma V^{T} J=UΣVT
JacobiSVD svd(J,Eigen::ComputeThinU | Eigen::ComputeThinV);//Eigen::ComputeFullV/U
U = svd.matrixU();
V = svd.matrixV();
A = svd.singularValues();
1）.定义
- 头文件包含：#include
- Eigen::Matrix
- Eigen::Vector3d v_3d; //Eigen::Matrix
- Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Zero(); //零矩阵 // Matrix3d 实质上是 Eigen::Matrix
- 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
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);
-
参考：EIgen:Matricx和vector类的定义和使用.
-
参考：矩阵块操作.
1、Eigen
#include
using namespace std;
#include
// Eigen 部分
#include
// 稠密矩阵的代数运算（逆，特征值等）
#include
#define MATRIX_SIZE 50
/****************************
* 本程序演示了 Eigen 基本类型的使用
****************************/
int main( int argc, char** argv )
{
// 矩阵 Eigen::Matrix
// Eigen 中所有向量和矩阵都是Eigen::Matrix，它是一个模板类。它的前三个参数为：数据类型，行，列
// 声明一个2*3的float矩阵
Eigen::Matrix matrix_23;//float类型
//向量 Eigen::Vector3d
// 同时，Eigen 通过 typedef 提供了许多内置类型，不过底层仍是Eigen::Matrix
// 例如 Vector3d 实质上是 Eigen::Matrix，即三维向量
Eigen::Vector3d v_3d;//double类型
// 这是一样的
Eigen::Matrix vd_3d;//float类型
// Matrix3d 实质上是 Eigen::Matrix
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< result_wrong_type = matrix_23 * v_3d;
// 应该显式转换 matrix_23.cast float类型转换成 double类型
Eigen::Matrix result = matrix_23.cast() * v_3d;
cout << result << endl;
// float类型 * float 类型
Eigen::Matrix result2 = matrix_23 * vd_3d;
cout << result2 << endl;
// 同样你不能搞错矩阵的维度
// 试着取消下面的注释，看看Eigen会报什么错
// Eigen::Matrix result_wrong_dimension = matrix_23.cast() * 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_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 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 Matrix_55;
Matrix_55 = Eigen::MatrixXd::Random(5,5);
cout<<"Random Matrix_55 :\n"< 结果
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中的矩阵类型一般都是用类似MatrixXXX来表示，可以根据该名字来判断其数据类型，比如:
”d”表示double类型
”f”表示float类型
”i”表示整数
”c”表示复数
Matrix2f，表示的是一个2*2维的，其每个元素都是float类型。 -
数据存储：
Matrix
Matrix -
动态矩阵和静态矩阵：
动态矩阵是指其大小在运行时确定，静态矩阵是指其大小在编译时确定。
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)、使用”=”操作符操作动态矩阵时，如果左右两边的矩阵大小不等，
则左边的动态矩阵的大小会被修改为右边的大小。
如何选择动态矩阵和静态矩阵：对于小矩阵(一般大小小于16)使用固定大小的静态矩阵，它可以带来比较高的效率； 对于大矩阵(一般大小大于32)建议使用动态矩阵。
注意：如果特别大的矩阵使用了固定大小的静态矩阵则可能会造成栈溢出的问题。
- Matrix
- Matrix
- Matrix
- Matrix
- 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
using namespace std;
#include
// Eigen 部分
#include
// 稠密矩阵的代数运算（逆，特征值等）
#include
#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 matrix_23;
// 同时，Eigen 通过 typedef 提供了许多内置类型，不过底层仍是Eigen::Matrix
// 例如 Vector3d 实质上是 Eigen::Matrix，即三维向量,三行一列。
Eigen::Vector3d v_3d;
//v_3d<<7, 8 ,9 ;
//cout< vd_3d;
// Matrix3d 实质上是 Eigen::Matrix
Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Zero(); //初始化为零
// cout< 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< result_wrong_type = matrix_23 * v_3d;
// 应该显式转换
Eigen::Matrix result = matrix_23.cast() * v_3d;
cout << result << endl;
Eigen::Matrix result2 = matrix_23 * vd_3d;
cout << result2 << endl;
// 同样你不能搞错矩阵的维度
// 试着取消下面的注释，看看Eigen会报什么错
// Eigen::Matrix result_wrong_dimension = matrix_23.cast() * 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_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 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
Matrix A; // Fixed rows and cols. Same as Matrix3d.
Matrix B; // Fixed rows, dynamic cols.
Matrix C; // Full dynamic. Same as MatrixXd.
Matrix 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() // x(1:n)
x.tail(n) // x(end - n + 1: end)
x.tail() // x(end - n + 1: end)
x.segment(i, n) // x(i+1 : i+n)
x.segment(i) // x(i+1 : i+n)
P.block(i, j, rows, cols) // P(i+1 : i+rows, j+1 : j+cols)
P.block(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() // P(:, 1:cols)
P.leftCols(cols) // P(:, 1:cols)
P.middleCols(j) // P(:, j+1:j+cols)
P.middleCols(j, cols) // P(:, j+1:j+cols)
P.rightCols() // P(:, end-cols+1:end)
P.rightCols(cols) // P(:, end-cols+1:end)
P.topRows() // P(1:rows, :)
P.topRows(rows) // P(1:rows, :)
P.middleRows(i) // P(i+1:i+rows, :)
P.middleRows(i, rows) // P(i+1:i+rows, :)
P.bottomRows() // 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() // P(1:rows, 1:cols)
P.topRightCorner() // P(1:rows, end-cols+1:end)
P.bottomLeftCorner() // P(end-rows+1:end, 1:cols)
P.bottomRightCorner() // 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 矩阵类型转换===============
//// Type conversion
// Eigen // Matlab
A.cast(); // double(A)
A.cast(); // single(A)
A.cast(); // 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
x = A.llt() .solve(b)); // A sym. p.d. #include
x = A.lu() .solve(b)); // Stable and fast. #include
x = A.qr() .solve(b)); // No pivoting. #include
x = A.svd() .solve(b)); // Stable, slowest. #include
// .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 eig(A); // [vec val] = eig(A)
eig.eigenvalues(); // diag(val)
eig.eigenvectors(); // vec
// For self-adjoint matrices use SelfAdjointEigenSolver<>
3、旋转矩阵、旋转向量、旋转角、四元数
#include
#include
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
#include
using namespace std;
// Eigen 部分
#include
// Eigen 几何模块
#include
/****************************
* 本程序演示了 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() < 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 p2=Tc2w*pw; //将world坐标系下的点pw变换到c2坐标系下
cout<<"the loc of p1 in c1 = \n"<< p1< 结果
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
#include
using namespace std;
#include
// Eigen 几何模块
#include
/****************************
* 本程序演示了 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"< 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 p2=Tc2w*pw; //将world坐标系下的点pw变换到c2坐标系下
cout<<"the loc of p1 in c1 = \n"<< p1< 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: [email protected]
6、练习
#include
#include
#include
// Eigen 部分
#include
// 稠密矩阵的代数运算（逆，特征值等）
#include
#include
#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< v_Nd;
v_Nd = Eigen::MatrixXd::Random( 100,1 );
// cout< 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:"<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 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 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;
}