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

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

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

[MVG] ä¸è§åå°å¾ç¹

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求解点深度

正规方程:Ax=b  -->  A^{T}Ax=A^{T}b  -->  x=(AA^{T})^{-1}A^{T}b

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

### 回答1: 三维三角是指根据已知的多个二维图像和相机参数,通过计算和优方法,确定物体在三维空间中的坐标。Matlab提供了一些函数和工具箱,可以方便地进行三维三角的计算和处理。 在Matlab中,可以使用vision.Reconstruction类进行三维云的重建和三角操作。首先,需要将二维图像转换为特征,可以使用vision.CornerDetector类或vision.ForegroundDetector类来检测图像中的角或前景物体。然后,使用相机的内参和外参,可以通过vision.PointTriangulation类将这些特征进行三角,得到三维空间中的云。 具体步骤如下: 1. 导入图像和相机参数:使用imread函数读取图像,并加载相机的内参和外参。 2. 特征检测:使用vision.CornerDetector类或vision.ForegroundDetector类检测图像中的角或前景物体,得到二维图像上的特征。 3. 三角计算:使用vision.PointTriangulation类,将二维图像上的特征与相机参数一起传入,进行三角计算,得到三维空间中的云。 4. 云优:可以使用一些优算法,如bundle adjustment,对得到的云进行优和平滑处理。 5. 可视和分析:使用Matlab的绘图函数,如scatter3和plot3,可以将三维云可视,以便分析和进一步处理。 总之,Matlab提供了丰富的函数和工具箱,可以方便地进行三维三角的计算和处理。根据已知的二维图像和相机参数,可以通过计算和优方法确定物体在三维空间中的坐标。 ### 回答2: 在MATLAB中进行三维三角可以使用多种方法,其中一种常用的方法是使用线性三角算法。以下是一个简单的步骤示例: 1. 收集至少两幅图像中的对应特征坐标。这些特征可以通过图像处理算法或手动标记得到。 2. 将相机的内参数(相机矩阵)和外参数(相机的位置和方向)提供给算法。这些参数可以通过相机标定或其他手段获得。 3. 根据两个图像中的特征坐标和相机参数,构建一个矩阵形式的线性方程组。 4. 使用线性方程组求解方法(如最小二乘法),求解上述方程组,得到三维的齐次坐标。 5. 去除齐次坐标的尺度影响,将其转换为三维的笛卡尔坐标。 6. 迭代重复以上步骤,使用更多的图像和对应特征,以提高三维的准确性和稳定性。 需要注意的是,MATLAB提供了一些内置的函数来支持三维三角操作,比如使用cameraMatrix()函数来构建相机矩阵,使用estimateWorldPoints()函数来进行三维三角等。 此外,还有其他的三维三角算法和工具可供选择,例如通用的非线性最小算法,或者使用视觉SLAM库(如OpenCV)中的功能来进行三维三角等。具体选择哪种方法取决于应用的需求和具体情况。 ### 回答3: matlab中的三维三角是指使用已知的多个二维图像以及与之相关的相机内外参数,来恢复和计算三维空间的位置坐标。 在matlab中,可以使用图像处理和计算机视觉工具箱中提供的函数进行三维三角。首先,需要确定至少两个二维图像中的特征的对应关系,这可以通过特征检测和特征匹配算法来实现。例如,可以使用SIFT、SURF或ORB等算法来提取和匹配特征。 接下来,需要获取相机的内外参数,即相机的内部参数矩阵和外部参数矩阵。相机的内部参数矩阵包括焦距、主坐标和畸变参数等信息,而外部参数矩阵描述了相机在世界坐标系下的位置和姿态。 然后,利用得到的图像对应的特征和相机的内外参数,可以使用三角测量方法计算三维的坐标。三角方法可以根据需要选择不同的算法,例如基于线性方法的DLT(Direct Linear Transform)或非线性方法的最小二乘法。 最后,通过matlab中的图像显示和可视工具,可以将恢复得到的三维在三维空间中进行可视展示。 总结而言,matlab中的三维三角需要进行特征的提取和匹配、获取相机内外参数、选择三角算法和进行三维的可视等步骤。这项技术在计算机视觉、虚拟现实、增强现实等领域有着广泛的应用。
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

Maccy37

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值