对从零手写VIO的第六次作业进行总结
文章目录
三角化问题
证明y=u4为最优解
回顾我们的原始问题如下:
求解
D 2 n × 4 y 4 × 1 = 0 D_{2 n \times 4} y_{4 \times 1}=0 D2n×4y4×1=0
对于齐次线性方程组 D y = 0 Dy=0 Dy=0。当D行数大于列数时，就是一个超定方程,本质上求解得到的是一个最小二乘解
在||y||=1的约束下，其最小二乘解为矩阵 D T D D^TD DTD最小特征值所对应的特征向量，下面给出证明
min y ∥ D y ∥ 2 = ( D y ) T ( D y ) = y T D T D y \min _{\boldsymbol{y}}\|D y\|^{2}=(D y)^{T}(D y)=y^{T} D^{T} D y ymin∥Dy∥2=(Dy)T(Dy)=yTDTDy
其中 ∣ ∣ y ∣ ∣ = 1 ||y||=1 ∣∣y∣∣=1
y y y可以由 D T D D^TD DTD的奇异值向量线性组合得到，也就是可以表示成如下形式
y = ∑ i = 1 4 k i u i = k i u i + v y=\sum_{i=1}^{4} k_{i} u_{i}=k_{i} u_{i}+v y=i=1∑4kiui=kiui+v
其中
v = ∑ j = 1 , j ≠ i 4 k j u j k i , k j ∈ R \begin{array}{c} v=\sum_{j=1, j \neq i}^{4} k_{j} u_{j} \\ k_{i}, k_{j} \in R \end{array} v=∑j=1,j=i4kjujki,kj∈R
容易知道， u i u_i ui和 v v v正交。
将 y y y代入到 y T D T D y y^TD^TDy yTDTDy得到
min y ∥ D y ∥ 2 = ( k i u i + v ) T D T D ( k i u i + v ) = k i 2 u i T D T D u i + v T D T D v + k i u i T D T D v + k i v T D T D u i \min _{\boldsymbol{y}}\left\|D_{\boldsymbol{y}}\right\|^{2}=\left(\boldsymbol{k}_{\boldsymbol{i}} u_{\boldsymbol{i}}+\boldsymbol{v}\right)^{\boldsymbol{T}} \boldsymbol{D}^{\boldsymbol{T}} \boldsymbol{D}\left(\boldsymbol{k}_{\boldsymbol{i}} u_{\boldsymbol{i}}+v\right) \\ =\boldsymbol{k}_{\boldsymbol{i}}^{2} u_{i}^{\boldsymbol{T}} \boldsymbol{D}^{\boldsymbol{T}} \boldsymbol{D}_{\boldsymbol{u}_{\boldsymbol{i}}}+\boldsymbol{v}^{\boldsymbol{T}} \boldsymbol{D}^{\boldsymbol{T}} \boldsymbol{D} \boldsymbol{v}+\boldsymbol{k}_{\boldsymbol{i}} u_{i}^{\boldsymbol{T}} \boldsymbol{D}^{\boldsymbol{T}} \boldsymbol{D} \boldsymbol{v}+\boldsymbol{k}_{\boldsymbol{i}} \boldsymbol{v}^{\boldsymbol{T}} \boldsymbol{D}^{\boldsymbol{T}} \boldsymbol{D} \boldsymbol{u}_{\boldsymbol{i}} ymin∥Dy∥2=(kiui+v)TDTD(kiui+v)=ki2uiTDTDui+vTDTDv+kiuiTDTDv+kivTDTDui
由于 u i u_i ui和 v v v正交，所以后两项为0 ; 并且 D u i = σ i u i , D u_{i}=\sigma_{i} u_{i}, Dui=σiui, 带入得到
min x ∥ D y ∥ 2 = ( k i u i + v ) T D T D ( k i u i + v ) = k i 2 u i T D T D u i + v T D T D v = k i 2 σ i 2 ∥ u i ∥ 2 + v T D T D v > = k i 2 u i T D T D u i + v T D T D v = k i 2 σ i 2 ∥ u i ∥ 2 \min _{x}\|D y\|^{2}=\left(k_{i} u_{i}+v\right)^{T} D^{T} D\left(k_{i} u_{i}+v\right)= \\ k_{i}^{2} u_{i}^{T} D^{T} D u_{i}+v^{T} D^{T} D v=k_{i}^{2} \sigma_{i}^{2}\left\|u_{i}\right\|^{2}+v^{T} D^{T} D v>=k_{i}^{2} u_{i}^{T} D^{T} D u_{i}+v^{T} D^{T} D v=k_{i}^{2} \sigma_{i}^{2}\left\|u_{i}\right\|^{2} xmin∥Dy∥2=(kiui+v)TDTD(kiui+v)=ki2uiTDTDui+vTDTDv=ki2σi2∥ui∥2+vTDTDv>=ki2uiTDTDui+vTDTDv=ki2σi2∥ui∥2
当且仅当 v = 0 v=0 v=0的时候等号成立
如果想取得最小值，则 σ i = σ 4 \sigma _i = \sigma 4 σi=σ4，也就是取得最小奇异值的时候，目标函数取得最小值，此时
y = k 4 u 4 + v = k 4 u 4 y = k_4u_4 + v = k_4u_4 y=k4u4+v=k4u4
由于 ∣ ∣ y ∣ ∣ = 1 ||y||=1 ∣∣y∣∣=1，所以 k 4 = 1 k_4=1 k4=1，所以
y = u 4 y = u_4 y=u4
三角化程序思路
步骤
step1：通过
P
c
w
P_{cw}
Pcw将世界坐标点转化到归一化相机坐标点,然后构造D矩阵
step2：缩放D,scale取D绝对值元素最大的逆,然后
D
T
D
D^TD
DTD
step3：判断第四个奇异值是否远小于第三个(1e-2,1e-4) ,若是,则三角化有效,
y
=
u
4
y=u_4
y=u4,并且恢复scale,并且除以第四个元素(u4/u4[3]).head(3)得到非齐次坐标.
分析
代码
int main()
{
int poseNums = 10;
double radius = 8;
double fx = 1.;
double fy = 1.;
std::vector<Pose> camera_pose;
for(int n = 0; n < poseNums; ++n ) {
double theta = n * 2 * M_PI / ( poseNums * 4); // 1/4 圆弧
// 绕 z轴 旋转
Eigen::Matrix3d R;
R = Eigen::AngleAxisd(theta, Eigen::Vector3d::UnitZ());
Eigen::Vector3d t = Eigen::Vector3d(radius * cos(theta) - radius, radius * sin(theta), 1 * sin(2 * theta));
camera_pose.push_back(Pose(R,t));
}
// 随机数生成 1 个 三维特征点
std::default_random_engine generator;
std::uniform_real_distribution<double> xy_rand(-4, 4.0);
std::uniform_real_distribution<double> z_rand(8., 10.);
double tx = xy_rand(generator);
double ty = xy_rand(generator);
double tz = z_rand(generator);
Eigen::Vector3d Pw(tx, ty, tz);
// 这个特征从第三帧相机开始被观测，i=3
int const start_frame_id = 3;
int end_frame_id = poseNums;
for (int i = start_frame_id; i < end_frame_id; ++i) {
Eigen::Matrix3d Rcw = camera_pose[i].Rwc.transpose();
Eigen::Vector3d Pc = Rcw * (Pw - camera_pose[i].twc); // 将世界坐标下的点转换到相机坐标系下
double x = Pc.x();
double y = Pc.y();
double z = Pc.z();
camera_pose[i].uv = Eigen::Vector2d(x/z,y/z);
}
/// TODO::homework; 请完成三角化估计深度的代码
// 遍历所有的观测数据，并三角化
Eigen::Vector3d P_est; // 结果保存到这个变量
P_est.setZero();
/* your code begin */
//step1:constuct D Dy=0,其中y为世界系下的空间点
Eigen::Matrix<double, 2*(10 - start_frame_id), 4> D; // D的维度为 2n×4
for (int i = start_frame_id; i < end_frame_id; ++i) {
// 得到Tcw
Eigen::Matrix<double,3,4> Tcw;
Eigen::Matrix3d Rcw = camera_pose[i].Rwc.transpose();
Eigen::Vector3d tcw = -Rcw*camera_pose[i].twc;//注意是-:Pc=Pc0-tcw
Tcw<< Rcw, tcw;
//rows = mat.rows(2); // 获取第3行
Eigen::Vector2d uv = camera_pose[i].uv ;
D.block(2*(i - start_frame_id), 0, 1,4) = uv[0]*Tcw.row(2) - Tcw.row(0);
D.block(2*(i - start_frame_id)+1, 0, 1,4) = uv[1]*Tcw.row(2) - Tcw.row(1);
}
std::cout<<"D Matrix is :"<<D<<std::endl;
//step2:recale D,找到D中最大元素
// 对D进行缩放S，其中 S 为一个对角阵,取 D 最大元素之逆即可
Eigen::MatrixXd::Index maxRow, maxCol;
double max = D.maxCoeff(&maxRow,&maxCol);
//注意是D的绝对值中最大元素的D.cwiseAbs().maxCoeff()
std::cout << "Max = \n" << max <<"行:"<<maxRow<<"列"<<maxCol<< std::endl;
Eigen::MatrixXd DtD((D/max).transpose()*(D/max));
clock_t time_stt = clock();
Eigen::JacobiSVD<Eigen::MatrixXd> svd_holder(DtD, Eigen::ComputeThinU | Eigen::ComputeThinV );
clock_t time_stt1 = clock();
std::cout<<"SVD分解，耗时:\n"<<(clock()-time_stt1)/(double) CLOCKS_PER_SEC<<"ms"<<std::endl;
// 构建SVD分解结果 Eigen::MatrixXd U = svd_holder.matrixU(); Eigen::MatrixXd V = svd_holder.matrixV();
Eigen::MatrixXd S(svd_holder.singularValues());
std::cout<<"singularValues :\n"<<S<<std::endl;
//step3:判断分解是否有效,然后求解y
if(std::abs(svd_holder.singularValues()[3]/svd_holder.singularValues()[2]) < 1e-2){
Eigen::Vector4d U4 = (max*svd_holder.matrixU().rightCols(1));//最后一列
P_est = (U4/U4[3]).head(3); // U4是一个4*1的向量，最后以为归一化后，取前三维为y
}
else{
std::cout<<"这次求解无效"<<std::endl;
}
/* your code end */
std::cout <<"ground truth: \n"<< Pw.transpose() <<std::endl;
std::cout <<"your result: \n"<< P_est.transpose() <<std::endl;
return 0;
}
