Eigen头文件
#include<Eigen/Core>，包含Matrix和Array类，基础的线性代数运算和数组操作。
#include<Eigen/Geometry>，包含旋转，平移，缩放，2维和3维的各种变换。
#include<Eigen/LU>，包含求逆，行列式，LU分解。
#include<Eigen/Cholesky>，包含LLT和LDLT Cholesky分解。
#include<Eigen/SVD>，包含SVD分解。
#include<Eigen/QR>，包含QR分解。
#include<Eigen/Eigenvalues>，包含特征值，特征向量分解。
#include<Eigen/Sparse>，包含稀疏矩阵的存储和运算。
#include<Eigen/Dense>，包含了Core/Geometry/LU/Cholesky/SVD/QR/Eigenvalues模块。
#include<Eigen/Eigen>，包含Dense和Sparse。
坐标系之间的变化
geodetic2enu enu2geodetic
姿态的操作
坐标系从0坐标系变换到1坐标系，变换关系为
R
01
R_{01}
R
01
，这个时候如果1系有个坐标点
p
1
p_1
p
1
,要转到原0系下的坐标就表示为：
p
0
=
R
01
∗
p
1
p_0 = R_{01} * p_1
p
0
=
R
01
∗
p
1
- Eigen使用的角度是弧度制的,坐标都是取得是:x轴向前，y轴向右，z轴向下
-
坐标系变换先变换yaw
R01
R_{01}
R
01
，再变换pitch
R12
R_{12}
R
12
，最后再变换roll
R23
R_{23}
R
23
，用Eigen的轴角表示就是,物理意义就是，变换之后坐标系的一个点
p3
p_3
p
3
通过以下的变换
R03
R_{03}
R
03
就可以变到变换之前的状态
p0
p_0
p
0
Eigen::Matrix3d r03 =Eigen::AngleAxisd(yaw,Eigen::Vector3d::UnitZ())*
Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX());
- 解析欧拉角的操作为旋转的反操作，得到的就是以上的公式得到的yaw,pitch,roll
Eigen::Vector3d ypr = rotation_vector1.matrix().eulerAngles(2,1,0);
表示姿态的操作
- Eigen使用的角度是弧度制的
// 头文件
```cpp
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <iostream>
#include <Eigen/Core>
#include <Eigen/Geometry>
using namespace std;
#define PI (3.1415926535897932346f)
int main(int argc, char **argv)
{
/**** 1. 旋转向量 ****/
cout << endl << "********** AngleAxis **********" << endl;
//1.0 初始化旋转向量,沿Z轴旋转45度的旋转向量
Eigen::AngleAxisd rotation_vector1 (M_PI/4, Eigen::Vector3d(0, 0, 1));
//1.1 旋转向量转换为旋转矩阵
//旋转向量用matrix()转换成旋转矩阵
Eigen::Matrix3d rotation_matrix1 = Eigen::Matrix3d::Identity();
rotation_matrix1 = rotation_vector1.matrix();
cout << "rotation matrix1 =\n" << rotation_matrix1 << endl;
//或者由罗德里格公式进行转换
rotation_matrix1 = rotation_vector1.toRotationMatrix();
cout << "rotation matrix1 =\n" << rotation_matrix1 << endl;
/*1.2 旋转向量转换为欧拉角*/
//将旋转向量转换为旋转矩阵,再由旋转矩阵转换为欧拉角,详见旋转矩阵转换为欧拉角
Eigen::Vector3d eulerAngle1 = rotation_vector1.matrix().eulerAngles(2,1,0);
cout << "eulerAngle1, z y x: " << eulerAngle1 << endl;
/*1.3 旋转向量转四元数*/
Eigen::Quaterniond quaternion1(rotation_vector1);
//或者
Eigen::Quaterniond quaternion1_1;
quaternion1_1 = rotation_vector1;
cout << "quaternion1 x: " << quaternion1.x() << endl;
cout << "quaternion1 y: " << quaternion1.y() << endl;
cout << "quaternion1 z: " << quaternion1.z() << endl;
cout << "quaternion1 w: " << quaternion1.w() << endl;
cout << "quaternion1_1 x: " << quaternion1_1.x() << endl;
cout << "quaternion1_1 y: " << quaternion1_1.y() << endl;
cout << "quaternion1_1 z: " << quaternion1_1.z() << endl;
cout << "quaternion1_1 w: " << quaternion1_1.w() << endl;
/**** 2. 旋转矩阵 *****/
cout << endl << "********** RotationMatrix **********" << endl;
//2.0 旋转矩阵初始化
Eigen::Matrix3d rotation_matrix2;
rotation_matrix2 << 0.707107, -0.707107, 0, 0.707107, 0.707107, 0, 0, 0, 1;
;
//或直接单位矩阵初始化
Eigen::Matrix3d rotation_matrix2_1 = Eigen::Matrix3d::Identity();
//2.1 旋转矩阵转换为欧拉角
//ZYX顺序，即先绕x轴roll,再绕y轴pitch,最后绕z轴yaw,0表示X轴,1表示Y轴,2表示Z轴
Eigen::Vector3d euler_angles = rotation_matrix2.eulerAngles(2, 1, 0);
cout << "yaw(z) pitch(y) roll(x) = " << euler_angles.transpose() << endl;
//2.2 旋转矩阵转换为旋转向量
Eigen::AngleAxisd rotation_vector2;
rotation_vector2.fromRotationMatrix(rotation_matrix2);
//或者
Eigen::AngleAxisd rotation_vector2_1(rotation_matrix2);
cout << "rotation_vector2 " << "angle is: " << rotation_vector2.angle() * (180 / M_PI)
<< " axis is: " << rotation_vector2.axis().transpose() << endl;
cout << "rotation_vector2_1 " << "angle is: " << rotation_vector2_1.angle() * (180 / M_PI)
<< " axis is: " << rotation_vector2_1.axis().transpose() << endl;
//2.3 旋转矩阵转换为四元数
Eigen::Quaterniond quaternion2(rotation_matrix2);
//或者
Eigen::Quaterniond quaternion2_1;
quaternion2_1 = rotation_matrix2;
cout << "quaternion2 x: " << quaternion2.x() << endl;
cout << "quaternion2 y: " << quaternion2.y() << endl;
cout << "quaternion2 z: " << quaternion2.z() << endl;
cout << "quaternion2 w: " << quaternion2.w() << endl;
cout << "quaternion2_1 x: " << quaternion2_1.x() << endl;
cout << "quaternion2_1 y: " << quaternion2_1.y() << endl;
cout << "quaternion2_1 z: " << quaternion2_1.z() << endl;
cout << "quaternion2_1 w: " << quaternion2_1.w() << endl;
/**** 3. 欧拉角 ****/
cout << endl << "********** EulerAngle **********" << endl;
//3.0 初始化欧拉角(Z-Y-X，即RPY, 先绕x轴roll,再绕y轴pitch,最后绕z轴yaw)
Eigen::Vector3d ea(0.785398, -0, 0);
//3.1 欧拉角转换为旋转矩阵
Eigen::Matrix3d rotation_matrix3;
rotation_matrix3 = Eigen::AngleAxisd(ea[0], Eigen::Vector3d::UnitZ()) *
Eigen::AngleAxisd(ea[1], Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(ea[2], Eigen::Vector3d::UnitX());
cout << "rotation matrix3 =\n" << rotation_matrix3 << endl;
//3.2 欧拉角转换为四元数,
Eigen::Quaterniond quaternion3;
quaternion3 = Eigen::AngleAxisd(ea[0], Eigen::Vector3d::UnitZ()) *
Eigen::AngleAxisd(ea[1], Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(ea[2], Eigen::Vector3d::UnitX());
cout << "quaternion3 x: " << quaternion3.x() << endl;
cout << "quaternion3 y: " << quaternion3.y() << endl;
cout << "quaternion3 z: " << quaternion3.z() << endl;
cout << "quaternion3 w: " << quaternion3.w() << endl;
//3.3 欧拉角转换为旋转向量
Eigen::AngleAxisd rotation_vector3;
rotation_vector3 = Eigen::AngleAxisd(ea[0], Eigen::Vector3d::UnitZ()) *
Eigen::AngleAxisd(ea[1], Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(ea[2], Eigen::Vector3d::UnitX());
cout << "rotation_vector3 " << "angle is: " << rotation_vector3.angle() * (180 / M_PI)
<< " axis is: " << rotation_vector3.axis().transpose() << endl;
/**** 4.四元数 ****/
cout << endl << "********** Quaternion **********" << endl;
//4.0 初始化四元素,注意eigen Quaterniond类四元数初始化参数顺序为w,x,y,z
Eigen::Quaterniond quaternion4(0.92388, 0, 0, 0.382683);
//4.1 四元数转换为旋转向量
Eigen::AngleAxisd rotation_vector4(quaternion4);
//或者
Eigen::AngleAxisd rotation_vector4_1;
rotation_vector4_1 = quaternion4;
cout << "rotation_vector4 " << "angle is: " << rotation_vector4.angle() * (180 / M_PI)
<< " axis is: " << rotation_vector4.axis().transpose() << endl;
cout << "rotation_vector4_1 " << "angle is: " << rotation_vector4_1.angle() * (180 / M_PI)
<< " axis is: " << rotation_vector4_1.axis().transpose() << endl;
//4.2 四元数转换为旋转矩阵
Eigen::Matrix3d rotation_matrix4;
rotation_matrix4 = quaternion4.matrix();
Eigen::Matrix3d rotation_matrix4_1;
rotation_matrix4_1 = quaternion4.toRotationMatrix();
cout << "rotation matrix4 =\n" << rotation_matrix4 << endl;
cout << "rotation matrix4_1 =\n" << rotation_matrix4_1 << endl;
//4.4 四元数转欧拉角(Z-Y-X，即RPY)
Eigen::Vector3d eulerAngle4 = quaternion4.matrix().eulerAngles(2,1,0);
cout << "yaw(z) pitch(y) roll(x) = " << eulerAngle4.transpose() << endl;
return 0;
}
四元数的操作
定义两个四元数。四元数和一般的数组或者向量在构造上没有区别，只是理解的方法（运算结果）不一样而已
p=[1 2 3 4];
q=[4,3,2,1];
模（Modulus）：quatmod(p) % 5.4772
范数（Norm）：quatnorm(p) 0
单位化（Normalize）：quatnormalize(p) % 0.1826 0.3651 0.5477 0.7303
求逆（Inverse）：quatinv(p) %0.0333 -0.0667 -0.1000 -0.1333
四元数除法：quatdivide(q,p) %0.6667 0 -0.6667 -0.3333
四元数乘法：quatmultiply(p,q) % -12 6 24 12
共轭四元数：quatconj(p) % 1 -2 -3 -4
另外还有旋转函数quatrotate、四元数和欧拉角互换的函数quat2angle、angle2quat
四元数转矩阵
q1=quatnormalize(q1); %单位化
R1=quat2dcm(q1);%q1的第一位是实部
R2=eul2rotm(eul,'ZYX');%欧拉角转旋转矩阵
欧拉角转四元数
q = angle2quat(yaw/180*pi,pitch/180*pi,roll/180*pi);
欧拉角转旋转矩阵
R = angle2dcm(yaw/180*pi,pitch/180*pi,roll/180*pi);
四元数转旋转矩阵
R=quat2dcm([q0 q1 q2 q3])
旋转矩阵转四元数
q =dcm2quat(R)；
欧拉角转四元数
q=angle2quat(r1,r2,r3，S)；
以下两种输出都是以Eigen中的向量Vector形式输出
q.coeffs(); //[x y z w] 输出系数
q.vec(); //[x y z] 输出虚部
输出选择矩阵，四元数表示旋转矩阵首先要进行单位化，单位矩阵才能表示旋转矩阵
q.normalized(); //important
Matrix3d R=q.toRotationMatrix();
共轭反向矩阵，一般不用inverse，在表示旋转的时候（范数是1），共轭即可表示相反的的旋转。
//q.inverse();
q.conjugate();
遍历元素
cout<<q.w()<<" "<<q.x()<<" "<<q.y()<<" "<<q.z()<<endl;
单元素操作
// 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)
Matrix类
Matrix<typename Scalar,
int RowsAtCompileTime,
int ColsAtCompileTime,
int Options = 0,
int MaxRowsAtCompileTime = RowsAtCompileTime,
int MaxColsAtCompileTime = ColsAtCompileTime>
# Scalar 元素类型
# RowsAtCompileTime 行
# ColsAtCompileTime 列
# 例 typedef Matrix<int, 3, 3> Matrix3i;
# Options 比特标志位
# MaxRowsAtCompileTime和MaxColsAtCompileTime表示在编译阶段矩阵的上限。
# 列向量
typedef Matrix<double, 3, 1> Vector3d;
# 行向量
typedef Matrix<float, 1, 3> RowVector3f;
# 动态大小
typedef Matrix<double, Dynamic, Dynamic> MatrixXd;
typedef Matrix<float, Dynamic, 1> VectorXf;
type
矩阵与向量的运算
MatrixXcf a = MatrixXcf::Random(3,3);
a.transpose(); # 转置
a.conjugate(); # 共轭
a.adjoint(); # 共轭转置（伴随矩阵）
# 对于实数矩阵，conjugate不执行任何操作，adjoint等价于transpose
a.transposeInPlace() #原地转置
Vector3d v(1,2,3);
Vector3d w(4,5,6);
v.dot(w); # 点积
v.cross(w); # 叉积
Matrix2d a;
a << 1, 2, 3, 4;
a.sum(); # 所有元素求和
a.prod(); # 所有元素乘积
a.mean(); # 所有元素求平均
a.minCoeff(); # 所有元素中最小元素
a.maxCoeff(); # 所有元素中最大元素
a.trace(); # 迹，对角元素的和
# minCoeff和maxCoeff还可以返回结果元素的位置信息
int i, j;
a.minCoeff(&i, &j);
Array类
Array<typename Scalar,
int RowsAtCompileTime,
int ColsAtCompileTime>
# 常见类定义
typedef Array<float, Dynamic, 1> ArrayXf
typedef Array<float, 3, 1> Array3f
typedef Array<double, Dynamic, Dynamic> ArrayXXd
typedef Array<double, 3, 3> Array33d
ArrayXf a = ArrayXf::Random(5);
a.abs(); # 绝对值
a.sqrt(); # 平方根
a.min(a.abs().sqrt()); # 两个array相应元素的最小值
归约，迭代器，广播
-
范数计算
- squareNorm()：L2范数，等价于计算vector自身点积
- norm()：返回`squareNorm的开方根
-
.lpNorm
()：p范数，p可以取Infinity，表无穷范数
-
布尔归约
- all()=true: matrix或array中所有元素为true
- any()=true: 到少有一个为true
- count(): 返回true元素个数
// sample
ArrayXXf A(2, 2);
A << 1,2,3,4;
(A > 0).all();
(A > 0).any();
(A > 0).count();
- 迭代器
// sample
Eigen::MatrixXf m(2,2);
m << 1,2,3,4;
MatrixXf::Index maxRow, maxCol;
float max = m.maxCoeff(&minRow, &minCol);
- 部分归约
// sample
Eigen::MatrixXf mat(2,3);
mat << 1,2,3,
4,5,6;
std::cout << mat.colwise().maxCoeff();
// output: 4, 5, 6
// mat.rowWise() the same as before
- 广播，针对vector，沿行或列重复构建一个matrix。
// sample
Eigen::MatrixXf mat(2,3);
Eigen::VectorXf v(2);
mat << 1,2,3,4,5,6;
v << 0,1;
mat.colwise() += v;
// output: 1, 2, 3, 5, 6, 7
Map类
-
Map类用于利用数据的内在，并将其转为Eigen类型。
定义：
Map<Matrix<typename Scalar, int RowAtCompileTime, int ColsAtCompileTime> >
通过Map来reshape矩阵的形状。
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
double s;
Eigen 基础使用
[cpp] view plain copy
// 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 特殊矩阵生成
[cpp] view plain copy
// 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 矩阵分块
[cpp] view plain copy
// 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 矩阵元素交换
[cpp] view plain copy
// 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 矩阵转置
[cpp] view plain copy
// 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 矩阵乘积
[cpp] view plain copy
// 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 矩阵单个元素操作
[cpp] view plain copy
// 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 矩阵化简
[cpp] view plain copy
// 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 矩阵点乘
[cpp] view plain copy
// 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 矩阵类型转换
[cpp] view plain copy
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
[cpp] view plain copy
// Note that for most operations Eigen requires all operands to have the same type:
MatrixXf F = MatrixXf::Zero(3,3);
A += F; // illegal in Eigen. In Matlab A = A+F is allowed
A += F.cast<double>(); // F converted to double and then added (generally, conversion happens on-the-fly)
// Eigen can map existing memory into Eigen matrices.
float array[3];
Vector3f::Map(array).fill(10); // create a temporary Map over array and sets entries to 10
int data[4] = {1, 2, 3, 4};
Matrix2i mat2x2(data); // copies data into mat2x2
Matrix2i::Map(data) = 2*mat2x2; // overwrite elements of data with 2*mat2x2
MatrixXi::Map(data, 2, 2) += mat2x2; // adds mat2x2 to elements of data (alternative syntax if size is not know at compile time)
Eigen 求解线性方程组 Ax = b
[cpp] view plain copy
// 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 矩阵特征值
[cpp] view plain copy
// 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<>
matlab实现队列
classdef myQueue <handle
properties (Access = public)%private
buffer % a cell, to maintain the data
beg % the start position of the queue
rear % the end position of the queue
% the actually data is buffer(beg:rear-1)
end
properties (Access = public)
capacity
end
methods
function obj = myQueue(c)
if nargin >= 1 && iscell(c)
obj.buffer = [c(:); cell(numel(c), 1)];% numel - Number of array elements
obj.beg = 1;
obj.rear = numel(c) + 1;
obj.capacity = 2*numel(c);
elseif nargin >= 1
obj.buffer = cell(100, 1);
obj.buffer{1} = c;
obj.beg = 1;
obj.rear = 2;
obj.capacity = 100;
else
obj.buffer = cell(100, 1);
obj.capacity = 100;
obj.beg = 1;
obj.rear = 1;
end
end
function s = size(obj)
if obj.rear >= obj.beg
s = obj.rear - obj.beg;
else
s = obj.rear - obj.beg + obj.capacity;
end
end
function b = isempty(obj) % return true when the queue is empty
b = ~logical(obj.size());
end
function s = empty(obj) % clear all the data in the queue
s = obj.size();
obj.beg = 1;
obj.rear = 1;
end
function push(obj, el)
if obj.size >= obj.capacity - 1
sz = obj.size();
if obj.rear >= obj.beg
obj.buffer(1:sz) = obj.buffer(obj.beg:obj.rear-1);
else
obj.buffer(1:sz) = obj.buffer([obj.beg:obj.capacity 1:obj.rear-1]);
end
obj.buffer(sz+1:obj.capacity*2) = cell(obj.capacity*2-sz, 1);
obj.capacity = numel(obj.buffer);
obj.beg = 1;
obj.rear = sz+1;
end
obj.buffer{obj.rear} = el;
obj.rear = mod(obj.rear, obj.capacity) + 1;
end
function el = front(obj)
if obj.rear ~= obj.beg
el = obj.buffer{obj.beg};
else
el = [];
warning('CQueue:NO_DATA', 'try to get data from an empty queue');
end
end
function el = back(obj)
if obj.rear == obj.beg
el = [];
warning('CQueue:NO_DATA', 'try to get data from an empty queue');
else
if obj.rear == 1
el = obj.buffer{obj.capacity};
else
el = obj.buffer{obj.rear - 1};
end
end
end
function el = pop(obj)
if obj.rear == obj.beg
error('CQueue:NO_Data', 'Trying to pop an empty queue');
else
el = obj.buffer{obj.beg};
obj.beg = obj.beg + 1;
if obj.beg > obj.capacity, obj.beg = 1; end
end
end
function remove(obj)
obj.beg = 1;
obj.rear = 1;
end
function display(obj)
if obj.size()
if obj.beg <= obj.rear
for i = obj.beg : obj.rear-1
disp([num2str(i - obj.beg + 1) '-th element of the stack:']);
disp(obj.buffer{i});
end
else
for i = obj.beg : obj.capacity
disp([num2str(i - obj.beg + 1) '-th element of the stack:']);
disp(obj.buffer{i});
end
for i = 1 : obj.rear-1
disp([num2str(i + obj.capacity - obj.beg + 1) '-th element of the stack:']);
disp(obj.buffer{i});
end
end
else
disp('The queue is empty');
end
end
function c = content(obj)
if obj.rear >= obj.beg
c = obj.buffer(obj.beg:obj.rear-1);
else
c = obj.buffer([obj.beg:obj.capacity 1:obj.rear-1]);
end
end
end
end
矩阵的赋值和输出
矩阵的赋值和输出
Eigen::Matrix2d m_matrix;
m_matrix<< 2,3,2.2,1;
std::cout << "m_matrix = \n"<< m_matrix<std::endl;
Eigen::MatrixXd xx(5,2);
xx << 2,3,2.2,1,2,3,2,1,2,3;
特殊矩阵之零矩阵
Eigen::MatrixXf m_matrix = Eigen::MatrixXf::Zero(5,5);
Eigen::MatrixXf m_matrix = Eigen::MatrixXf::Zero(5,2);
特殊矩阵之对角矩阵
Eigen::MatrixXf m_matrix = Eigen::MatrixXf::Identity(5,5);
Eigen::MatrixXf m_matrix = Eigen::MatrixXf::Identity(2,5);
矩阵大小
int cols = m_matrix.cols();
int rows = m_matrix.rows();
std::cout<<cols<<std::endl;
std::cout<<rows<<std::endl;
矩阵元素操作
矩阵里的每个元素求绝对值eigen中矩阵是无法直接对矩阵操作，让每个元素都求绝对值的。但是可以通过转化为Array类型来操作。
m_matrix(1,1) = 2;
m_matrix = A * B;
A = B;
矩阵的最大元素和绝对值
matrix转为array： .array()
array转为matrix：.matrix()
求绝对值
Eigen::MatrixXf x;
Eigen::ArrayXXf x_abs = x.array().abs();
求矩阵绝对值后的最大元素
Eigen::MatrixXf x;
Eigen::ArrayXXf x_abs = x.array().abs();
float max_value = x_abs.maxCoeff();
矩阵转置和矩阵求逆
转置
Eigen::MatrixXf m_matrix;
Eigen::MatrixXf m_matrix_T;
m_matrix_T = m_matrix.transpose();
求逆
Eigen::MatrixXf m_matrix;
Eigen::MatrixXf m_matrix_inv;
m_matrix_inv = m_matrix.inverse();
特征值和特征向量
特征值
Eigen::Matrix2d m_matrix;
m_matrix << 2,3,2.2,1;
Eigen::EigenSolver<Eigen::Matrix2d> eigen_solver ( m_matrix );
Eigen::MatrixXd eig_value = eigen_solver.pseudoEigenvalueMatrix();
std::cout << "matrix values = \n" <<eig_value<<std::endl;
特征向量
Eigen::Matrix2d m_matrix;
m_matrix << 2,3,2.2,1;
Eigen::EigenSolver<Eigen::Matrix2d> eigen_solver ( m_matrix );
Eigen::MatrixXd eig_vector = eigen_solver.pseudoEigenvectors();
std::cout << "matrix vectors = \n" <<eig_vector<<std::endl;
std::cout << "matrix vectors(1,0) = \n" <<eig_vector(1,0)<<std::endl;
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
动态大小矩阵的定义
int rows,cols;//从其他地方获取的尺寸行列数
Eigen::MatrixXf tmp_mat;
tmp_mat=Eigen::Matrix<float,Dynamic,Dynamic>();
tmp_mat.resize(rows,cols);
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::MatrixXd test(10, 10);
test.setRandom();
test.diagonal() << 0, 1, 2, 3, 4, 5, 6, 7, 8, 9; // 对角线元素确定，其他值随意的矩阵
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()
//非方阵的函数svd求解
9 MatrixXf A = MatrixXf::Random(3,2);
10 cout<<"Here is the matrix A:\n"<<A<<endl;
11 VectorXf b = VectorXf::Random(3);
12 cout<<"Here is the right hand side b:\n"<<b<<endl;
13 cout<<"The least-squares solution is:\n"
14 <<A.bdcSvd(ComputeThinU|ComputeThinV).solve(b)<<endl;
最小二乘解的三种求解方式
svd求解
#include <iostream>
#include <Eigen/Dense>
using namespace std;
using namespace Eigen;
int main()
{
MatrixXf A = MatrixXf::Random(3, 2);
cout << "Here is the matrix A:\n" << A << endl;
VectorXf b = VectorXf::Random(3);
cout << "Here is the right hand side b:\n" << b << endl;
cout << "The least-squares solution is:\n"
<< A.bdcSvd(ComputeThinU | ComputeThinV).solve(b) << endl;
}
QR分解
有3种QR分解类： There are three QR decomposition classes: HouseholderQR (no pivoting, so fast but unstable), ColPivHouseholderQR (column pivoting, thus a bit slower but more accurate) and FullPivHouseholderQR (full pivoting, so slowest and most stable).
#include <iostream>
#include <Eigen/Dense>
using namespace std;
using namespace Eigen;
int main()
{
MatrixXf A = MatrixXf::Random(3, 2);
VectorXf b = VectorXf::Random(3);
cout << "The solution using the QR decomposition is:\n"
<< A.colPivHouseholderQr().solve(b) << endl;
}
使用normal equations
找到Ax = b的最小二乘解等效于求解法线方程
A
T
A
x
=
A
T
b
A^{^{T}}Ax = A^{T}b
A
T
A
x
=
A
T
b
#include <iostream>
#include <Eigen/Dense>
using namespace std;
using namespace Eigen;
int main()
{
MatrixXf A = MatrixXf::Random(3, 2);
VectorXf b = VectorXf::Random(3);
cout << "The solution using normal equations is:\n"
<< (A.transpose() * A).ldlt().solve(A.transpose() * b) << endl;
}
Eigen 矩阵特征值
// Eigenvalue problems
// Eigen // Matlab
A.eigenvalues(); // eig(A);
EigenSolver<Matrix3d> eig(A); // [vec val] = eig(A)
eig.eigenvalues(); // diag(val)
eig.eigenvectors(); // vec
Eigen求解最大特征值和最大特征值对应的特征向量，同理最小
Matrix<double, 4, 4>K= MatrixXd::Random(4,4);
EigenSolverdouble, 4, 4>> es(K);
MatrixXcd evecs = es.eigenvectors();//獲取矩陣特征向量4*4，這裏定義的MatrixXcd必須有c，表示獲得的是complex複數矩陣
MatrixXcd evals = es.eigenvalues();//獲取矩陣特征值 4*1
MatrixXd evalsReal;//注意這裏定義的MatrixXd裏沒有c
evalsReal=evals.real();//獲取特征值實數部分
MatrixXf::Index evalsMax;
evalsReal.rowwise().sum().maxCoeff(&evalsMax);//得到最大特征值的位置
Vector4d q;
q << evecs.real()(0, evalsMax), evecs.real()(1, evalsMax), evecs.real()(2, evalsMax), evecs.real()(3, evalsMax);//得到對應特征向量
Eigen齐次坐标和非齐次坐标
Eigen::Vector3d vec{1,2,3};
vec_homo = vec.homogeneous; // 转化为齐次坐标
vec_hnorm = vec_homo.hnormalized; // 转回正常坐标
vec.normalize() 自身修改成归一化的坐标
vec.norm() 二范数
vec.normalized() 不修改自身的坐标，就是返回一个新的向量
Eigen求解矩阵近似逆
A
v
i
=
λ
i
v
i
,
A
[
v
1
,
v
2
,
⋯
v
n
]
=
[
v
1
,
v
2
,
⋯
v
n
]
[
λ
1
λ
2
⋯
λ
n
]
Av_i = \lambda_iv_i,A[v_1,v_2,\cdots v_n]=[v_1,v_2,\cdots v_n]\begin{equation} \begin{bmatrix} \lambda_1 & & & \\ & \lambda_2 & & \\ & & \cdots&\\ & & &\lambda_n \end{bmatrix} \end{equation}
A
v
i
=
λ
i
v
i
,
A
[
v
1
,
v
2
,
⋯
v
n
]
=
[
v
1
,
v
2
,
⋯
v
n
]
λ
1
λ
2
⋯
λ
n
A
=
[
v
1
,
v
2
,
⋯
v
n
]
[
λ
1
λ
2
⋯
λ
n
]
[
v
1
T
v
2
T
⋮
v
n
T
]
A
=
L
T
V
L
,
A
−
1
=
L
V
−
1
L
T
A = \left[ \begin{array}{l} v_1,v_2,\cdots v_n\end{array}\right] \begin{bmatrix} \lambda_1 & & & \\ & \lambda_2 & & \\ & & \cdots&\\ & & &\lambda_n \end{bmatrix} \left[ \begin{array}{l} v_1^T \\ v_2^T \\ \vdots \\v_n^T\end{array} \right] \\A = L^TVL,A^{-1}=LV^{-1}L^T
A
=
[
v
1
,
v
2
,
⋯
v
n
]
λ
1
λ
2
⋯
λ
n
v
1
T
v
2
T
⋮
v
n
T
A
=
L
T
V
L
,
A
−
1
=
L
V
−
1
L
T
s
a
e
s
.
e
i
g
e
n
v
e
c
t
o
r
s
(
)
=
L
=
[
v
1
T
v
2
T
⋮
v
n
T
]
s
a
e
s
.
e
i
g
e
n
v
e
c
t
o
r
s
(
)
.
t
r
a
n
s
p
o
s
e
(
)
=
L
T
=
[
v
1
,
v
2
,
⋯
v
n
]
saes.eigenvectors()=L=\left[ \begin{array}{l} v_1^T \\ v_2^T \\ \vdots \\v_n^T\end{array} \right] \\ saes.eigenvectors().transpose()=L^T=\left[ \begin{array}{l} v_1,v_2,\cdots v_n\end{array}\right]
s
a
es
.
e
i
g
e
n
v
ec
t
ors
(
)
=
L
=
v
1
T
v
2
T
⋮
v
n
T
s
a
es
.
e
i
g
e
n
v
ec
t
ors
(
)
.
t
r
an
s
p
ose
(
)
=
L
T
=
[
v
1
,
v
2
,
⋯
v
n
]
对应到Eigen中的代码为：
//1.用于求解特征值和特征向量
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> saes(Amm);
//2.具体求解逆矩阵
Eigen::MatrixXd Amm_inv = saes.eigenvectors() * Eigen::VectorXd((saes.eigenvalues().array() > eps).select(saes.eigenvalues().array().inverse(), 0)).asDiagonal() * saes.eigenvectors().transpose();
// saes.eigenvectors() L
// Eigen::VectorXd((saes.eigenvalues().array() > eps).select(saes.eigenvalues().array().inverse(), 0)).asDiagonal() D^{-1}
// saes.eigenvectors().transpose() LT
Eigen分解信息矩阵边缘化后的信息矩阵A和边缘化后的b
Eigen中的基础的矩阵类模板使用
template <typename Derived>
nlohmann::json CreateEigenJson(Eigen::MatrixBase<Derived> const& matrix){
std::vector<Derived> datas;
...
}
Eigen库求解icp
- 比如说两组轨迹进行icp
1.pts0
Eigen::Matrix<double,3,Eigen::Dynamic> pts0(3,size),pts1(3,size);
2.对齐
Eigen::Matrix4d T10 = Eigen::umeyame(pts0,pts1,false);
umeyama(const MatrixBase<Derived>& src, const MatrixBase<OtherDerived>& dst, bool with_scaling = true)
Eigen::MatrixBase类
- 作用是输入任意的Eigen::Matrix，都可以用该类来表示
template <typename Derived>
json EigenMatrixTojson(Eigen::MatrixBase<Derived> const& matrix){
。。。。
}
比如输入:Eigen::Vector3d 实际上Derived对应的是Vector3d,而不是double