参考:https://blog.csdn.net/u013517182/article/details/52151960
https://blog.csdn.net/kokerf/article/details/72844455
感谢前辈们的分享,对我理解VINS代码有很大的帮助。
三角化(Triangulate)是计算机视觉中的重要的一部分,通过特征匹配的二维坐标点和图像的位姿(R,t)得到三维空间点的位置。
放一张示意图:
根据几何关系,假设两帧图像上匹配特征点的齐次坐标为x, x'。三维空间点齐次坐标记为X。两帧图像对应的投影矩阵为P,P’。可以得到:
x = P X x' = P' X
使用叉乘得到AX = 0的形式(可以使用SVD的方法计算):
x×(P X) = 0 x'×(P' X) = 0
其中对x = (x, y, z)分析,将P和P’按行拆开,得到:
由此可以得到三个方程,由于第三个方程可以由前两个方程得到,因此只需要考虑前两个方程。每对匹配的特征(x和x')都会得到四个方程,表示为AX =0 的形式,
通常由于匹配的特征点对较多,这是一个超定方程,一般无法求得精确解,我们其实需要求一个使AX最小的解。通过对A的SVD分解,最后得到的是A的最小奇异值对应的单位奇异矢量,若X = (x, y, z, w),深度则为z/w。
在VINS-mono中,对滑动窗口中所有帧中depth未知的特征点进行三角化。对应于代码如下:
void FeatureManager::triangulate(Vector3d Ps[], Vector3d tic[], Matrix3d ric[])
{
for (auto &it_per_id : feature)
{
it_per_id.used_num = it_per_id.feature_per_frame.size();
if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))
continue;
if (it_per_id.estimated_depth > 0)
continue;
int imu_i = it_per_id.start_frame, imu_j = imu_i - 1;
ROS_ASSERT(NUM_OF_CAM == 1);
Eigen::MatrixXd svd_A(2 * it_per_id.feature_per_frame.size(), 4);
int svd_idx = 0;
Eigen::Matrix<double, 3, 4> P0;
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();
for (auto &it_per_frame : it_per_id.feature_per_frame)
{
imu_j++;
Eigen::Vector3d t1 = Ps[imu_j] + Rs[imu_j] * tic[0];
Eigen::Matrix3d R1 = Rs[imu_j] * ric[0];
Eigen::Vector3d t = R0.transpose() * (t1 - t0);
Eigen::Matrix3d R = R0.transpose() * R1;
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;
}
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;
//ROS_INFO("before <> feature id %d , start_frame %d, used num %d, depth %f ", it_per_id.feature_id, it_per_id.start_frame, it_per_id.used_num,it_per_id.estimated_depth); //zss
if (it_per_id.estimated_depth < 0.1)
{
it_per_id.estimated_depth = INIT_DEPTH;
}
}
}