目录
1. 前言
1.1 为什么要进行矩阵分解?
1.2 矩阵与矩阵分解的几何意义?
2. LU三角分解
3. Cholesky分解 — LDLT分解
4. Cholesky分解 — LLT分解
5. QR分解
6. 奇异值分解
7. 特征值分解
本文转自大佬博客:https://blog.csdn.net/Hansry/article/details/104174651
本博客主要介绍在SLAM问题中常常出现的一些线性代数相关的知识,很早就想整理一下了,刚好看到Manii 的博客对矩阵分解的方法进行了总结,以方便求解线性方程组AX=B。在基于《计算机视觉—算法与应用》附录A 的内容 ,重点介绍了各种分解的适用情况、分解的特点。
1、矩阵分解可以在一定程度上降低存储空间,可以大大减少问题处理的计算量(如对一个矩阵进行求逆、求解方程组等),从而高效地解决目标问题。
2、矩阵分解可以提高算法的数值稳定性。
在矩阵分解中,我们常常期望将矩阵分解成正交矩阵、对角矩阵以及上三角(下三角)矩阵的乘积。以三维矩阵为例,一个普通矩阵的几何意义是对坐标进行某种线性变换,而正交矩阵的几何意义是坐标的旋转,对角矩阵的几何意义是坐标的缩放,三角矩阵的几何意义是对坐标的切边。因此对矩阵分解的几何意义就是将这种变换分解成缩放、切边和旋转的过程。
三角分解又称为LU分解或LR分解,是将原正方(square)矩阵分解成一个上三角矩阵和一个下三角矩阵。
其中L是单位下三角矩阵,D是对角矩阵,U是单位上三角矩阵。
假设矩阵A为对称矩阵,且任意K阶主子式均不为0时(即正定),A有如下唯一的分解形式:
即L为下三角单位矩阵,D为对角矩阵。LDLT方法实际上是Cholesky分解法的改进(LLT分解需要开平方),具体代码可见Chelesky分解LDLT用于求解线性方程组。
注:一个对称矩阵A是正定的充要条件是对任何非零向量 x 有 ,即对称矩阵A正定,非奇异,也可以说任意K阶主子式均不为0。
这里举个例子,比如VINS的初始化过程中,在做视觉sfm和IMU预积分的PVQ对齐的时候(即在initial_alignment.cpp文件中的LinearAlignment()函数中),会在对齐流程的第3步利用平移约束估计重力、速度和尺度初始值时用到ldlt来求解待优化的变量[v0, v1, ...vn, g, s]。构建H△x=b的形式进行优化参数求解,具体代码如下:
/**
* @brief 求解各帧的速度,枢纽帧的重力方向,以及尺度
*
* @param[in] all_image_frame
* @param[in] g
* @param[in] x
* @return true
* @return false
*/
bool LinearAlignment(map &all_image_frame, Vector3d &g, VectorXd &x)
{
// 这一部分内容对照论文进行理解
// 这里是《VIO 第7讲》 —— 视觉与IMU对齐估计流程第3步:利用平移约束估计重力、速度以及尺度初始值
int all_frame_count = all_image_frame.size();
int n_state = all_frame_count * 3 + 3 + 1; // 速度 + 重力 + 尺度因子
MatrixXd A{n_state, n_state};
A.setZero();
VectorXd b{n_state};
b.setZero();
map::iterator frame_i;
map::iterator frame_j;
int i = 0;
for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end(); frame_i ++, i ++)
{
frame_j = next(frame_i);
MatrixXd tmp_A(6, 10);
tmp_A.setZero();
VectorXd tmp_b(6);
tmp_b.setZero();
double dt = frame_j->second.pre_integration->sum_dt;
// 《VIO第7讲》,公式(17)
tmp_A.block<3, 3>(0, 0) = -dt * Matrix3d::Identity();
tmp_A.block<3, 3>(0, 6) = frame_i->second.R.transpose() * dt * dt / 2 * Matrix3d::Identity();
tmp_A.block<3, 1>(0, 9) = frame_i->second.R.transpose() * (frame_j->second.T - frame_i->second.T) / 100.0;
tmp_b.block<3, 1>(0, 0) = frame_j->second.pre_integration->delta_p + frame_i->second.R.transpose() * frame_j->second.R * TIC[0] - TIC[0];
// cout << "delta_p " << frame_j->second.pre_integration->delta_p.transpose() << endl;
tmp_A.block<3, 3>(3, 0) = -Matrix3d::Identity();
tmp_A.block<3, 3>(3, 3) = frame_i->second.R.transpose() * frame_j->second.R;
tmp_A.block<3, 3>(3, 6) = frame_i->second.R.transpose() * dt * Matrix3d::Identity();
tmp_b.block<3, 1>(3, 0) = frame_j->second.pre_integration->delta_v;
// cout << "delta_v " << frame_j->second.pre_integration->delta_v.transpose() << endl;
Matrix cov_inv = Matrix::Zero();
// cov.block<6, 6>(0, 0) = IMU_cov[i + 1];
// MatrixXd cov_inv = cov.inverse();
cov_inv.setIdentity();
MatrixXd r_A = tmp_A.transpose() * cov_inv * tmp_A;
VectorXd r_b = tmp_A.transpose() * cov_inv * tmp_b;
A.block<6, 6>(i * 3, i * 3) += r_A.topLeftCorner<6, 6>();
b.segment<6>(i * 3) += r_b.head<6>();
A.bottomRightCorner<4, 4>() += r_A.bottomRightCorner<4, 4>();
b.tail<4>() += r_b.tail<4>();
A.block<6, 4>(i * 3, n_state - 4) += r_A.topRightCorner<6, 4>();
A.block<4, 6>(n_state - 4, i * 3) += r_A.bottomLeftCorner<4, 6>();
}
// 增强数值稳定性
A = A * 1000.0;
b = b * 1000.0;
x = A.ldlt().solve(b); // 注意这里的求解方式是ldlt分解 △△△△△
double s = x(n_state - 1) / 100.0; // 取出尺度
ROS_DEBUG("estimated scale: %f", s);
g = x.segment<3>(n_state - 4); // 取出重力
ROS_DEBUG_STREAM(" result g " << g.norm() << " " << g.transpose());
// 做一些检查
if(fabs(g.norm() - G.norm()) > 1.0 || s < 0)
{
return false;
}
// 重力修复:《VIO第7讲》 —— 视觉与IMU对齐流程中第4步:对重力向量g_c0进行优化
RefineGravity(all_image_frame, g, x);
// 得到真实尺度
s = (x.tail<1>())(0) / 100.0;
(x.tail<1>())(0) = s;
ROS_DEBUG_STREAM(" refine " << g.norm() << " " << g.transpose());
if(s < 0.0 )
return false;
else
return true;
}
LLT分解又被称为平方根分解,是LDLT分解的一种特殊形式,即其中的D为单位矩阵。
一个对称正定矩阵A可以唯一地被分解成一个下三角矩阵L和L的转置LT相乘的形式:
其中的L是下三角矩阵,R是上三角矩阵。
(正定要求矩阵的所有特征值必须大于0,因此分解的下三角对角元也是大于0的)
LLT分解常用于求解最小二乘问题中的。
因子经过因子分解后,x 可以通过解下面的方程获得,即只需求解两个三角系统,通过一系列前向和后向迭代运算。
LLT分解的总操作数为O (N2),具体代码可见Chelesky分解LLT,对于系数矩阵来说操作数会大大降低。
如果A是mxn实(复)矩阵,且其n个列线性无关,则A有分解:
QR分解有三种常用方法:Givens 变换、Householder 变换,以及 Gram-Schmidt正交化。
QR分解是一项广泛用于稳定求解病态最小二乘问题的方法,也是一些更复杂算法的矩阵,如计算SVD及特征值分解。在计算机视觉中,QR分解可以用于将相机矩阵转换为一个旋转矩阵和一个上三角的标定矩阵。
这里比如ICP算法中求解R和t。
// 《SLAM14讲》SVD求解 P174
void pose_estimation_3d3d(
const vector& pts1,
const vector& pts2,
Mat& R, Mat& t)
{
Point3f p1, p2; // center of mass 质心
int N = pts1.size();
for(int i=0; i q1(N), q2(N); // remove the center,去中心化之后的点
for(int i=0; i svd(W, Eigen::ComputeFullU | Eigen::ComputeThinV);
Eigen::Matrix3d U = svd.matrixU(); // 得到U矩阵
Eigen::Matrix3d V = svd.matrixV(); // 得到V矩阵
// 第2帧到第1帧的变换,与PnP中第1 determinant行列式
if( U.determinant() * V.determinant() < 0 )
{
for(int x=0; x < 3; x ++)
{
U(x, 2) *= -1;
}
}
cout << "U = " << U << endl;
cout << "V = " << V << endl;
Eigen::Matrix3d R_ = U * (V.transpose());
Eigen::Vector3d t_ = Eigen::Vector3d( p1.x, p1.y, p1.z ) - R_ * Eigen::Vector3d(p2.x, p2.y, p2.z); // P174页 式7.53 t* = p - R p'
// convert to cv::Mat 将矩阵和向量统一转换为转换为Mat矩阵形式
R = (Mat_ (3,3) <<
R_(0,0), R_(0,1), R_(0,2),
R_(1,0), R_(1,1), R_(1,2),
R_(2,0), R_(2,1), R_(2,2)
);
t = (Mat_(3,1) <
设A是一个mxn的矩阵,则存在一个分解的m阶正交矩阵U、非负对角阵Σ和n阶正交矩阵V:
其中Σ = d i a g ( σ 1 , σ 2 , . . . , σ r ) ,σ 为矩阵A的全部非零奇异值,且一般我们会将Σ 的值从大到小排序。奇异值分解的一个重要性质是:在实际大多数情况中,奇异值σ 减小的速度特别快,因此可以使用前r 个奇异值来对矩阵做近似(即丢弃U和V的后几列),将获得原始矩阵A在最小二乘意义下的最佳逼近。
矩阵的奇异值分解通常是不唯一的。
SVD分解在最优化问题、特征值问题、最小二乘问题(尤其是亏秩最小二乘问题)等具有巨大的作用。
SVD分解的几何意义可以通过公式的重写获得:
另一种解释为:当一个矩阵A作用于一个向量时,由于矩阵A可以被分解为正交矩阵、对角矩阵、正交矩阵,因此可以理解为对该向量先旋转、再缩放,然后再旋转。
如果A是一个NxN的方阵,且有N个线性无关的特征向量,则可以被写成特征值分解的形式:
其中U为NxN方阵,且第i 列为 A 的特征向量,Λ 为对角矩阵,其对角线上的元素为对应的特征值。注意只有可对角化矩阵才能作特征值分解。
特征值分解可用于求解矩阵的逆:
在数据统计分析中常常出现A为半正定矩阵,其表示数据点的协方差,此时特征值分解就是通常所说的主分量分析(PCA),因为它完成了对数据点分布在其中心周围变化的主方向和幅度的建模。
在求解最小二乘问题时,常常通过一系列外积之和构造对称矩阵C,此时C也是半正定的:
此时C的特征值和特征向量与A的奇异值和奇异向量:
由此我们可以得到特征值。
对称矩阵:任意的 N×N 实对称矩阵都有 N 个线性无关的特征向量。并且这些特征向量都可以正交单位化而得到一组正交且模为 1 的向量。故实对称矩阵 A 可被分解成 A=UΛU,其中 U 为 正交矩阵, Λ 为实对角矩阵,因此一个实对称矩阵有实特征值,其特征向量两两正交。—《多视图几何》附录A4.2