SLAM--三角化求解3D空间点

主要是参考各路博客做一下笔记,以防自己忘记了

单目相机中地图点大多数是通过三角化求解出地图点空间位置

3D点齐次坐标,3D点在图像上的投影

两边同时差乘u(这个在多视图几何经常用到,为了是另一边等于0) ,有

展开得(这个在MVG里用向量叉积解答的时候看得糊里糊涂,哦,原来是这样)

三个方程,其中有两个是线性独立的,因为（1）式×(-u)-（2）式×v=（3）式,其中Pi是矩阵P的行

一个帧可以形成两个方程，那么两个帧可以形成四个方程：

这里可以使用SVD求解（参考MVG），齐次坐标  即为  的最小奇异值的奇异向量。(SVD好重要--直接线性变换DLT都用到它求解)

(1)ORB-SLAM:Triangulate

void Initializer::Triangulate(const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Mat &P1, const cv::Mat &P2, cv::Mat &x3D)
{cv::Mat A(4,4,CV_32F);A.row(0) = kp1.pt.x*P1.row(2)-P1.row(0);A.row(1) = kp1.pt.y*P1.row(2)-P1.row(1);A.row(2) = kp2.pt.x*P2.row(2)-P2.row(0);A.row(3) = kp2.pt.y*P2.row(2)-P2.row(1);cv::Mat u,w,vt;cv::SVD::compute(A,w,u,vt,cv::SVD::MODIFY_A| cv::SVD::FULL_UV);x3D = vt.row(3).t();cout<<"vt.row()"<<vt.rows<<std::endl;x3D = x3D.rowRange(0,3)/x3D.at<float>(3);///表示点的归一化--这个代码不太理解
}

(2)VINS-Mono

void FeatureManager::triangulatePoint(Eigen::Matrix<double, 3, 4> &Pose0, Eigen::Matrix<double, 3, 4> &Pose1,Eigen::Vector2d &point0, Eigen::Vector2d &point1, Eigen::Vector3d &point_3d)
{Eigen::Matrix4d design_matrix = Eigen::Matrix4d::Zero();design_matrix.row(0) = point0[0] * Pose0.row(2) - Pose0.row(0);design_matrix.row(1) = point0[1] * Pose0.row(2) - Pose0.row(1);design_matrix.row(2) = point1[0] * Pose1.row(2) - Pose1.row(0);design_matrix.row(3) = point1[1] * Pose1.row(2) - Pose1.row(1);Eigen::Vector4d triangulated_point;triangulated_point =design_matrix.jacobiSvd(Eigen::ComputeFullV).matrixV().rightCols<1>();point_3d(0) = triangulated_point(0) / triangulated_point(3);point_3d(1) = triangulated_point(1) / triangulated_point(3);point_3d(2) = triangulated_point(2) / triangulated_point(3);

 (3)Triangulate

void triangulate ( const Eigen::Matrix3d& K, const Eigen::Matrix4d T1, const Eigen::Matrix4d& T2, const Eigen::Vector2d& uu1, const Eigen::Vector2d& uu2, Eigen::Vector4d& X )
{// construct P1 P2const Eigen::Matrix<double, 3, 4> P1 = K * T1.block(0,0, 3, 4);const Eigen::Matrix<double, 3, 4> P2 = K * T2.block(0, 0, 3, 4);// get vectorsconst Eigen::Matrix<double, 1, 4>& P11 = P1.block(0, 0, 1, 4);const Eigen::Matrix<double, 1, 4>& P12 = P1.block(1, 0, 1, 4);const Eigen::Matrix<double, 1, 4>& P13 = P1.block(2, 0, 1, 4);const Eigen::Matrix<double, 1, 4>& P21 = P2.block(0, 0, 1, 4);const Eigen::Matrix<double, 1, 4>& P22 = P2.block(1, 0, 1, 4);const Eigen::Matrix<double, 1, 4>& P23 = P2.block(2, 0, 1, 4);const double& u1 = uu1[0];const double& v1 = uu1[1];const double& u2 = uu2[0];const double& v2 = uu2[1];// construct H matrix.Eigen::Matrix4d H;H.block(0, 0, 1, 4) = v1 * P13 - P12;H.block(1, 0, 1, 4) = P11 - u1 * P13;H.block(2, 0, 1, 4) = v2 * P23 - P22;H.block(3, 0, 1, 4) = P21 - u2 * P23;// SVDEigen::JacobiSVD<Eigen::MatrixXd> svd ( H, Eigen::ComputeFullU | Eigen::ComputeFullV );Eigen::Matrix4d V = svd.matrixV();X = V.block(0, 3, 4, 1);X = X / X(3, 0);
} // triangulate

上面这三个的三角化,差不多都一样,只是程序的形式不一样

(4)SVO:Triangulate求解点深度

正规方程:  -->    -->  

bool depthFromTriangulation(const SE3& T_search_ref,  //两个相机相对位置const Vector3d& f_ref,   ///上一帧的图像点const Vector3d& f_cur,   ///当前帧的图像点double& depth)
{Matrix<double,3,2> A; A << T_search_ref.rotation_matrix() * f_ref, f_cur;const Matrix2d AtA = A.transpose()*A;if(AtA.determinant() < 0.000001)return false;const Vector2d depth2 = - AtA.inverse()*A.transpose()*T_search_ref.translation();depth = fabs(depth2[0]);return true;
}

其实还是不太理解SVO为什么会有个b这一块.

参考:https://zhuanlan.zhihu.com/p/55530787

https://zhuanlan.zhihu.com/p/63179478