0. 前言
在了解SLAM的原理、流程后，个人经常实时困惑该如何去从零开始去设计编写一套能够符合我们需求的SLAM框架。作者认为Ceres、Eigen、Sophus、G2O这几个函数库无法避免，尤其是Ceres函数库在激光SLAM和V-SLAM的优化中均有着大量的应用。所以作者已从Ceres作为开端，这一篇文章主要对Eigen函数库进行详细的阐述，来方便各位后续的开发。
1. Eigen示例
相较于Ceres而言，Eigen函数库相对较为简单，我们上一篇文章详细描述了Ceres的使用以及注意事项，由于Ceres能够使用ceres::AutoDiffCostFunction这一类的自动求导函数，相对而言更加轻松，所以Eigen更多的是做矩阵运算。这里我们给出上一篇文章最后的Ceres求解的Eigen版本。我们可以看到本质上差别不大，只是Eigen需要自己求解雅克比矩阵J，并在用GN构建增量方程后，使用ldlt求解线性方程HX=g。
#include <iostream>
#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <ctime>
using namespace std;
using namespace Eigen;
int main(int argc, char **argv) {
double ar = 1.0, br = 2.0, cr = 1.0; // 真实参数值
double ae = 2.0, be = -1.0, ce = 5.0; // 估计参数值
int N = 100; // 数据点
double w_sigma = 1.0; // 噪声Sigma值
cv::RNG rng; // OpenCV随机数产生器
vector<double> x_data, y_data; // 数据
for (int i = 0; i < N; i++) {
double x = i / 100.0;
x_data.push_back(x);
y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma));
}
// 开始Gauss-Newton迭代
int iterations = 100; // 迭代次数
double cost = 0, lastCost = 0; // 本次迭代的cost和上一次迭代的cost
for (int iter = 0; iter < iterations; iter++) {
Matrix3d H = Matrix3d::Zero(); // Hessian = J^T J in Gauss-Newton
Vector3d b = Vector3d::Zero(); // bias
cost = 0;
//-----------------用GN构建增量方程，HX=g---------------------------------//
for (int i = 0; i < N; i++) {
double xi = x_data[i], yi = y_data[i]; // 第i个数据点
// start your code here
// double error = 0; // 填写计算error的表达式
double error = yi-exp(ae * xi * xi + be * xi + ce); // 第i个数据点的计算误差
Vector3d J; // 雅可比矩阵,3x1
J[0] = -xi*xi*exp(ae * xi * xi + be * xi + ce); // de/da,函数求倒数，-df/da
J[1] = -xi*exp(ae * xi * xi + be * xi + ce);; // de/db
J[2] = -exp(ae * xi * xi + be * xi + ce);; // de/dc
H += J * J.transpose(); // GN近似的H
b += -error * J;
// end your code here
cost += error * error;
}
// 求解线性方程 Hx=b，建议用ldlt
// start your code here
Vector3d dx;
//LDL^T Cholesky求解
// clock_t time_stt2 = clock();
dx = H.ldlt().solve(b);//Hx=b,,,H.ldlt().solve(b)
// cout<<"LDL^T分解，耗时:\n"<<(clock()-time_stt2)/(double)
// CLOCKS_PER_SEC<<"ms"<<endl;
cout<<"\n dx:"<<dx.transpose()<<endl;
// return 0;//一写就死
// end your code here
if (isnan(dx[0])) {
cout << "result is nan!" << endl;
break;
}
if (iter > 0 && cost > lastCost) {
// 误差增长了，说明近似的不够好
cout << "cost: " << cost << ", last cost: " << lastCost << endl;
break;
}
// 更新abc估计值
ae += dx[0];
be += dx[1];
ce += dx[2];
lastCost = cost;
cout << "total cost: " << cost << endl;
}
cout << "estimated abc = " << ae << ", " << be << ", " << ce << endl;
return 0;
}
TIP：值得注意的是Eigen函数库是不存在动态链接库的，我们在CMakeList.txt编译时候只需要引入include即可。
include_directories(BEFORE /home/xxx/eigen3_3/complex_eigen/install/include/eigen3/)
2. Eigen模块和头文件归纳
Core： #include<Eigen/Core>，包含Matrix和Array类，基础的线性代数运算和数组操作。
Geometry： #include<Eigen/Geometry>，包含旋转，平移，缩放，2维和3维的各种变换。
LU ：#include<Eigen/LU>，包含求逆，行列式，LU分解。
Cholesky： #include<Eigen/Cholesky>，包含LLT和LDLT Cholesky分解。
SVD： #include<Eigen/SVD>，包含SVD分解。
QR： #include<Eigen/QR>，包含QR分解。
Eigenvalues ：#include<Eigen/Eigenvalues>，包含特征值，特征向量分解。
Sparse： #include<Eigen/Sparse>，包含稀疏矩阵的存储和运算。
Dense ：#include<Eigen/Dense>，包含了 Core/Geometry/LU/Cholesky/SVD/QR/Eigenvalues模块。
Eigen： #include<Eigen/Eigen>，包含Dense和Sparse。
上面的代码我们即包含了上述的两个函数库。
#include <Eigen/Core>
#include <Eigen/Dense>
3.Eigen基础函数总结
$\color{blue}{矩阵(Matrix)类的函数介绍}$
在Eigen中，所有矩阵和向量均为Matrix模板类的对象，向量是矩阵的行（或列）为1是的特殊情况。
$\color{red}{Matrix类有6个模板参数，主要使用前三个，剩下的使用默认值。}$
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表示在编译阶段矩阵的上限。
1、矩阵的三参数模板
/* 强制性的三参数模板的原型 (三个参数分别表示：标量的类型，编译时的行，编译时的列) */
/* tips:用typedef定义了很多模板，例如：Matrix4f 表示 4×4 的floats 矩阵 */
typedef Matrix<float, 4, 4> Matrix4f;
2、向量（Vectors）
向量是矩阵的特殊情况，也是用矩阵定义的。
# 列向量
typedef Matrix<double, 3, 1> Vector3d;
# 行向量
typedef Matrix<float, 1, 3> RowVector3f;
3、特殊动态值（special value Dynamic）
Eigen的矩阵不仅能够在编译是确定大小（fixed size），也可以在运行时确定大小，就是所说的动态矩阵（dynamic size）。
typedef Matrix<double, Dynamic, Dynamic> MatrixXd;
typedef Matrix<int, Dynamic, 1> VectorXi;
/* 也可使用‘行’固定‘列’动态的矩阵 */
Matrix<float, 3, Dynamic>
4、构造函数（Constructors）
可以使用默认的构造函数，不执行动态分配内存，也没有初始化矩阵参数：
Matrix3f a; // a是3-by-3矩阵，包含未初始化的 float[9] 数组
Eigen::Matrix3d //旋转矩阵（3*3）
Eigen::AngleAxisd //旋转向量（3*1）
Eigen::Vector3d //欧拉角（3*1）
Eigen::Quaterniond //四元数（4*1）
Eigen::Isometry3d //欧式变换矩阵（4*4）
Eigen::Affine3d //放射变换矩阵（4*4）
Eigen::Projective3d //射影变换矩阵（4*4）
MatrixXf b; // b是动态矩阵，当前大小为 0-by-0, 没有为数组的系数分配内存
/* 矩阵的第一个参数表示“行”，数组只有一个参数。根据跟定的大小分配内存，但不初始化 */
MatrixXf a(10,15); // a 是10-by-15阵，分配了内存，没有初始化
VectorXf b(30); // b是动态矩阵，当前大小为 30, 分配了内存，没有初始化
/* 对于给定的矩阵，传递的参数无效 */
Matrix3f a(3,3);
/* 对于维数最大为4的向量，可以直接初始化 */
Vector2d a(5.0, 6.0);
Vector3d b(5.0, 6.0, 7.0);
Vector4d c(5.0, 6.0, 7.0, 8.0);
5、系数访问
系数都是从0开始，矩阵默认按列存储
#include <iostream>
#include <Eigen/Dense>
using namespace std;
using namespace Eigen;
int main()
{
MatrixXd m(2, 2);
m(0, 0) = 3;
m(1, 0) = 2.5;
m(0, 1) = -1;
m(1, 1) = m(1, 0) + m(0, 1);
cout << "Here is the matrix m:" << endl;
cout << m << endl;
VectorXd v(2);
v(0) = 4;
v[1] = v[0] - 1; //operator[] 在 vectors 中重载，意义和()相同
cout << "Here is the vector v:" << endl;
cout << v << endl;
getchar();
getchar();
}
6、逗号分隔的初始化
Matrix3f m;
m << 1, 2, 3, 4, 5, 6, 7, 8, 9;
cout << m;
7、获取vector&matrix数据大小
默认构造时，指定大小的矩阵，只分配相应大小的空间，不进行初始化。动态大小的矩阵，则未分配空间。[]操作符可以用于向量元素的获取，但不能用于matrix。Eigen支持以下的读/写元素语法。
matrix(i,j);
vector(i)
vector[i]
vector.x() // 第一个系数
vector.y() // 第二个系数
vector.z() // 第三个系数
vector.w() // 第四个系数
matrix的大小可以通过rows(), cols(), size()获取，resize()可以重新调整矩阵大小。
Matrix<double, 3, 3> A; // 固定行列。Matrix3d一样。
Matrix<double, 3, Dynamic> B; // 固定行，动态列。
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.
上述的元素访问方法都通过断言检查范围，代价比较大。
- 通过定义EIGEN_NO_DEBUG 或 NDEBUG，取消断言。
- 通过使用coeff()和coeffRef()，来取消检查。比如，MatrixBase::coeff(int,int) const, MatrixBase::coeffRef(int,int)等。
$\color{blue}{矩阵(Matrix)类的运算}$
- Eigen不支持类型自动转化，因此矩阵元素类型必须相同。
- 支持+, -, +=, -=, _, /, _=, /=基础四则运算。
- 转置和共轭
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);
$\color{blue}{Array类}$
Array是个类模板，前三个参数必须指定，后三个参数可选。和Matrix对比，Matrix的运算遵守矩阵运算规则，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相应元素的最小值
当执行array_array时，执行的是相应元素的乘积，所以两个array必须具有相同的尺寸。
Matrix对象——>Array对象：.array() 函数
Array对象——>Matrix对象：_*.matrix()__ 函数
$\color{blue}{块操作}$
块是matrix或array中的矩形子块。
// 方法1
.block(i, j, p, q) //起点(i, j)，块大小(p, q)，构建一个动态尺寸的block
.block<p, q>(i, j) // 构建一个固定尺寸的block
matrix.row(i): 矩阵第i行matrix.col(j): 矩阵第j列
角相关操作
Vector的块操作
Matrix3f P; // 3x3 float matrix.
Vector3f x; // 3x1 float matrix.
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)
$\color{blue}{特殊矩阵}$
特殊矩阵
- 零阵：类静态成员函数
Zero() - 常量矩阵：
Constant(rows, cols, value) - 随机矩阵：
Random() - 单位矩阵：
Identity() - 构建从low到high等间距的size长度的序列，适用于vector和一维数组:
LinSpaced(size, low, high)
功能函数
- setZero()
- setIdentity()
```cpp
Matrix<double, dynamic,="" dynamic=""> C; // Full dynamic. Same as MatrixXd.</double,>
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 = zeros(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)’
VectorXi::LinSpaced(((hi-low)/step)+1, // low:step:hi
low,low+step*(size-1)) //
#### $\color{blue}{归约，迭代器，广播}$
- 范数计算
- squaredNorm()：L2范数，等价于计算vector自身点积
- norm()：返回`squareNorm的开方根
- .lpNorm<p>()：p范数，p可以取Infinity，表无穷范数
```cpp
Vector3f x; // 3x1 float matrix.
x.norm() // norm(x). 注意norm(R)在Eigen中不起作用。
x.squaredNorm() // dot(x, x) 注意，对于复数并不成立
x.dot(y) // dot(x, y)
x.cross(y) // cross(x, y) 需要 #include <Eigen/Geometry>
- 布尔归约
- 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();
// 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)
部分归约
// 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,用于利用数据的内在，并将其转为Eigen类型。
// Eigen can map existing memory into Eigen matrices.
float array[3];
Vector3f::Map(array).fill(10); // 数组上创建临时Map，并将vector大小设置为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)
4.Eigen分解函数总结
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>
上面是一些常用的分解函数，下面则是对这些函数的具体使用。
#include <iostream>
#include <Eigen/Dense>
//g++ Linear_algebra_and_decompositions.cpp -o la -I/download/eigen
using namespace std;
using namespace Eigen;
//QR方法解线性方程组
int main()
{
Matrix3f A;
Vector3f b;
A << 1,2,3, 4,5,6, 7,8,10;
b << 3, 3, 4;
cout << "Here is the matrix A:\n" << A << endl;
cout << "Here is the vector b:\n" << b << endl;
Vector3f x = A.colPivHouseholderQr().solve(b);
cout << "The solution is:\n" << x << endl;
}
//矩阵求逆
int main()
{
Matrix2f A, b;
A << 2, -1, -1, 3;
b << 1, 2, 3, 1;
cout << "Here is the matrix A:\n" << A << endl;
cout << "Here is the right hand side b:\n" << b << endl;
Matrix2f x = A.ldlt().solve(b);
cout << "The solution is:\n" << x << endl;
}
//计算数值法求解和真实值的残差
int main()
{
MatrixXd A = MatrixXd::Random(100,100);
MatrixXd b = MatrixXd::Random(100,50);
MatrixXd x = A.fullPivLu().solve(b);
double relative_error = (A*x - b).norm() / b.norm(); // norm() is L2 norm
cout << "The relative error is:\n" << relative_error << endl;
}
//计算特征值和特征向量
int main()
{
Matrix2f A;
A << 1, 2, 2, 3;
cout << "Here is the matrix A:\n" << A << endl;
SelfAdjointEigenSolver<Matrix2f> eigensolver(A);
if (eigensolver.info() != Success) abort();
cout << "The eigenvalues of A are:\n" << eigensolver.eigenvalues() << endl;
cout << "Here's a matrix whose columns are eigenvectors of A \n"
<< "corresponding to these eigenvalues:\n"
<< eigensolver.eigenvectors() << endl;
}
//计算逆行列式
int main()
{
Matrix3f A;
A << 1, 2, 1,
2, 1, 0,
-1, 1, 2;
cout << "Here is the matrix A:\n" << A << endl;
cout << "The determinant of A is " << A.determinant() << endl;
cout << "The inverse of A is:\n" << A.inverse() << endl;
}
//最小二乘解
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.jacobiSvd(ComputeThinU | ComputeThinV).solve(b) << endl;
}
//分离矩阵计算与构造(解耦合)
int main()
{
Matrix2f A, b;
LLT<Matrix2f> llt;
A << 2, -1, -1, 3;
b << 1, 2, 3, 1;
cout << "Here is the matrix A:\n" << A << endl;
cout << "Here is the right hand side b:\n" << b << endl;
cout << "Computing LLT decomposition..." << endl;
llt.compute(A);
cout << "The solution is:\n" << llt.solve(b) << endl;
A(1,1)++;
cout << "The matrix A is now:\n" << A << endl;
cout << "Computing LLT decomposition..." << endl;
llt.compute(A);
cout << "The solution is now:\n" << llt.solve(b) << endl;
}
// Rank-revealing分解
int main()
{
Matrix3f A;
A << 1, 2, 5,
2, 1, 4,
3, 0, 3;
cout << "Here is the matrix A:\n" << A << endl;
FullPivLU<Matrix3f> lu_decomp(A);
cout << "The rank of A is " << lu_decomp.rank() << endl;
cout << "Here is a matrix whose columns form a basis of the null-space of A:\n"
<< lu_decomp.kernel() << endl;
cout << "Here is a matrix whose columns form a basis of the column-space of A:\n"
<< lu_decomp.image(A) << endl; // yes, have to pass the original A
}
//LU分解，设定阈值求秩
int main()
{
Matrix2d A;
A << 2, 1,
2, 0.9999999999;
FullPivLU<Matrix2d> lu(A);
cout << "By default, the rank of A is found to be " << lu.rank() << endl;
lu.setThreshold(1e-5);
cout << "With threshold 1e-5, the rank of A is found to be " << lu.rank() << endl;
5.参考链接
https://blog.csdn.net/qq_38210354/article/details/106910865
https://blog.csdn.net/qq_37394634/article/details/105494547
https://blog.csdn.net/michaelhan3/article/details/82150808
https://blog.csdn.net/hanss2/article/details/78827057
评论（0）
您还未登录，请登录后发表或查看评论