LVI-SAM视觉特征点深度恢复原理解析

说明

对LVI-SAM视觉里程计中特征点深度信息恢复的原理进行简单解析，详细代码见feature_tracker.h/get_depth函数，这里只把相关代码进行粘贴

// 取3个激光点数据：A、B、C, 每个点的三维坐标(归一化坐标*深度)和深度r
                // 归一化坐标*深度 = 原始真实坐标
                float r1 = depth_cloud_unit_sphere->points[pointSearchInd[0]].intensity;
                Eigen::Vector3f A(depth_cloud_unit_sphere->points[pointSearchInd[0]].x * r1,
                                  depth_cloud_unit_sphere->points[pointSearchInd[0]].y * r1,
                                  depth_cloud_unit_sphere->points[pointSearchInd[0]].z * r1);

                float r2 = depth_cloud_unit_sphere->points[pointSearchInd[1]].intensity;
                Eigen::Vector3f B(depth_cloud_unit_sphere->points[pointSearchInd[1]].x * r2,
                                  depth_cloud_unit_sphere->points[pointSearchInd[1]].y * r2,
                                  depth_cloud_unit_sphere->points[pointSearchInd[1]].z * r2);

                float r3 = depth_cloud_unit_sphere->points[pointSearchInd[2]].intensity;
                Eigen::Vector3f C(depth_cloud_unit_sphere->points[pointSearchInd[2]].x * r3,
                                  depth_cloud_unit_sphere->points[pointSearchInd[2]].y * r3,
                                  depth_cloud_unit_sphere->points[pointSearchInd[2]].z * r3);

                // https://math.stackexchange.com/questions/100439/determine-where-a-vector-will-intersect-a-plane
                // 归一化的图像特征点坐标
                Eigen::Vector3f V(features_3d_sphere->points[i].x,
                                  features_3d_sphere->points[i].y,
                                  features_3d_sphere->points[i].z);
                // 计算由ABC构成平面的法向量
                Eigen::Vector3f N = (A - B).cross(B - C);
                float s = (N(0) * A(0) + N(1) * A(1) + N(2) * A(2)) / (N(0) * V(0) + N(1) * V(1) + N(2) * V(2));

深度恢复过程

• 根据代码首先拿到三个激光点ABC的坐标和对应深度r1,r2,r3
• 然后计算由ABC三个激光点组成平面的法向量
  Eigen::Vector3f N = (A - B).cross(B - C);
• 如下图，代码在计算s时，计算了两个距离值，一个是相机光心O到特征点归一化平面的直线距离Op，一个是相机光心到3D点的直线距离OP，所求的深度值s其实是3D点在z轴上的距离OM，则有以下过程：
  OP = (N(0) * A(0) + N(1) * A(1) + N(2) * A(2))
Op = (N(0) * V(0) + N(1) * V(1) + N(2) * V(2))
  由相似三角形原理，s=OM可以通过以下公式求得：
  $$\frac{s}{1}=\frac{OM}{Om}=\frac{OM}{1}=\frac{OP}{Op}$$
  把1约掉即为代码中的形式，s=OP/Op=(N(0) * A(0) + N(1) * A(1) + N(2) * A(2))/ (N(0) * V(0) + N(1) * V(1) + N(2) * V(2))