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 P2
	const 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 vectors
	const 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;
	
	// SVD
	Eigen::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