Vins-Fusion——三角化triangulate
发布时间
阅读量：
阅读量
在之前的博文中Vins-Fusion初始化位姿——3D-2D：基于PNP算法实现当前帧的位姿估计，本文进一步阐述了Vins-Fusion双目初始化技术中关于三角定位的关键步骤。
一、三角化triangulate
三角化（triangulation）是一种基于多幅相机图像中特征点投影确定其三维坐标的几何方法
二、三角化求解流程
1.遍历当前帧特征点，若该特征点深度为正，说明已经三角化了。
// 已经三角化过
if (it_per_id.estimated_depth > 0)
continue;
代码解读2.双目首帧三角化
多视图投影重构是指基于多视角的图像信息确定特征点在三维空间中的位置的过程，并且必须具备各摄像机的姿态信息以及各投射单元的实际坐标（通常表示为归一化的平面像素坐标）。
（1）左右目位姿
左目起始帧start_frame位姿
// 起始观测帧位姿 Tcw
int imu_i = it_per_id.start_frame;
Eigen::Matrix<double, 3, 4> leftPose;
Eigen::Vector3d t0 = Ps[imu_i] + Rs[imu_i] * tic[0];
Eigen::Matrix3d R0 = Rs[imu_i] * ric[0];
leftPose.leftCols<3>() = R0.transpose();
leftPose.rightCols<1>() = -R0.transpose() * t0;
代码解读右目起始帧start_frame位姿
// 起始观测帧位姿Tcw
Eigen::Matrix<double, 3, 4> rightPose;
Eigen::Vector3d t1 = Ps[imu_i] + Rs[imu_i] * tic[1];
Eigen::Matrix3d R1 = Rs[imu_i] * ric[1];
rightPose.leftCols<3>() = R1.transpose();
rightPose.rightCols<1>() = -R1.transpose() * t1;
代码解读（2）左右目归一化相机平面像素坐标
// 取左右目对应的归一化相机平面点
Eigen::Vector2d point0, point1;
Eigen::Vector3d point3d;
point0 = it_per_id.feature_per_frame[0].point.head(2);
point1 = it_per_id.feature_per_frame[0].pointRight.head(2);
代码解读注
（3）三角化
有了位姿和对应的投影位置就可以三角化求路标点了（3D坐标）
triangulatePoint(leftPose, rightPose, point0, point1, point3d);
//函数原型：
/** * SVD计算三角化点
* @param Pose0 位姿Tcw
* @param Pose1 位姿Tcw
* @param point0 归一化相机坐标系平面点
* @param point1 归一化相机坐标系平面点
* @param point_3d output 三角化点，世界坐标系点
*/
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.双目非首帧三角化
在完成双目首帧三角化的前提下，在所有当前特征点上逐一进行遍历操作，并将这些观测数据与首帧观测数据建立最小二乘模型；随后通过奇异值分解(SVD)方法进行求解；其求解步骤与首帧相同；具体如下：
int imu_i = it_per_id.start_frame, imu_j = imu_i - 1;
Eigen::MatrixXd svd_A(2 * it_per_id.feature_per_frame.size(), 4);
int svd_idx = 0;
Eigen::Matrix<double, 3, 4> P0;
// 首帧观测帧的位姿 Twc
Eigen::Vector3d t0 = Ps[imu_i] + Rs[imu_i] * tic[0];
Eigen::Matrix3d R0 = Rs[imu_i] * ric[0];
P0.leftCols<3>() = Eigen::Matrix3d::Identity();
P0.rightCols<1>() = Eigen::Vector3d::Zero();
// 遍历当前特征点观测帧，与首帧观测帧构建最小二乘，SVD三角化
for (auto &it_per_frame : it_per_id.feature_per_frame)
{
imu_j++;
// 当前观测帧位姿 Twc
Eigen::Vector3d t1 = Ps[imu_j] + Rs[imu_j] * tic[0];
Eigen::Matrix3d R1 = Rs[imu_j] * ric[0];
// 与首帧观测帧之间的位姿变换 Tij
Eigen::Vector3d t = R0.transpose() * (t1 - t0);
Eigen::Matrix3d R = R0.transpose() * R1;
// Tji
Eigen::Matrix<double, 3, 4> P;
P.leftCols<3>() = R.transpose();
P.rightCols<1>() = -R.transpose() * t;
Eigen::Vector3d f = it_per_frame.point.normalized();
svd_A.row(svd_idx++) = f[0] * P.row(2) - f[2] * P.row(0);
svd_A.row(svd_idx++) = f[1] * P.row(2) - f[2] * P.row(1);
if (imu_i == imu_j)
continue;
}
// todo
ROS_ASSERT(svd_idx == svd_A.rows());
Eigen::Vector4d svd_V = Eigen::JacobiSVD<Eigen::MatrixXd>(svd_A, Eigen::ComputeThinV).matrixV().rightCols<1>();
double svd_method = svd_V[2] / svd_V[3];
//it_per_id->estimated_depth = -b / A;
//it_per_id->estimated_depth = svd_V[2] / svd_V[3];
it_per_id.estimated_depth = svd_method;
//it_per_id->estimated_depth = INIT_DEPTH;
if (it_per_id.estimated_depth < 0.1)
{
it_per_id.estimated_depth = INIT_DEPTH;
}
代码解读在Vins-Fulsion双目初始化的过程中实现了三角化计算以确定特征点的三维坐标（路标基准点）。
由来：近期有同学问vins三角化的代码，相关疑问总结如下。
补充：
一、三角化理论对应代码详细流程
- 在滑窗窗口内的首帧中观测到该特征点后计算其深度；
- 根据相机参数矩阵计算得到该特征点在全局坐标系中的位置与姿态（R(ck2w), t(ck2w)）；
- 为了实现以下三角化过程所需的变换矩阵（R(w2ck), t(w2ck)），需要对相应的矩阵进行转置操作。
最终将归一化的相机平面坐标点(point(0)和point(1))与其对应的位姿(RT参数)进行三维重影图解算
二、代码相关说明
（1）通过滑窗内的所有特征点计算深度，在这一过程中由于同一特征点可能被多帧观测到，在滑窗内仅需计算其首帧的深度；
（2）RS：表示从b系到w系的一种转换关系；其中tic/ric外参表示c（camera）系相对于b系的位置和朝向；
（3）三角化仅依赖于滑窗内的首帧左右目观测帧。为何不用后续的观测茎来求解深度？因为后续会进行位姿和三维点的优化调整。
以上补充，解决了同学的疑问。你呢？有疑问评论区讨论。
参考：
无法对文本进行改写
全部评论 (0)
还没有任何评论哟~