Vins-Fusion初始化——三角化triangulate

继上一篇博文Vins-Fusion初始化位姿——3D-2D：PNP求解当前帧位姿，本文继续介绍Vins-Fusion双目初始化时，三角化triangulate。

一、三角化triangulate

三角化triangulate是根据特征点在多个相机下的投影恢复出特征点的3D位置。

二、三角化求解流程

1.遍历当前帧特征点，若该特征点深度为正，说明已经三角化了。

// 已经三角化过
if (it_per_id.estimated_depth > 0)
	continue;

2.双目首帧三角化

三角化是根据多个相机下的投影恢复出特征点的3D位置，需要已知相机位姿和投影像素位置（归一化相机平面像素坐标）

（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);

注：it_per_id.feature_per_frame[0].point.head(2)，表示左目归一化相机平面的前两个数据，即x, y。

（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);
}

以上部分解析如下：

疑问点：根据推导design_matrix的1-2行对应代码中的2-1，暂时还不知道为啥？
ps:对于代码中调用Eigen:svd库求解最小二乘Ax = 0问题的理论基础可以参考文章奇异值分解（SVD）方法求解最小二乘问题 ，有问题留言交流。

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-Fusion双目初始化时，三角化triangulate求解特征点的3D位置（路标点）

参考：

（1）https://blog.csdn.net/u010196944/article/details/127569002?spm=1001.2014.3001.5501
（2）https://blog.csdn.net/u010196944/article/details/127514369?spm=1001.2014.3001.5501