高斯分布的深度滤波器

全能搬运大师

于 2023-06-04 10:39:37 发布

阅读量156

点赞数

文章标签：算法自动驾驶

本文链接：https://blog.csdn.net/wt15172486270/article/details/131014108

版权

高斯分布的深度滤波器用来衡量你计算的深度值准确性怎么样，下面观测的深度值在真实情况下是你通过三角化测量计算出来的深度值。由于你的参考图和很多图像进行三角化得到很多相同的观测点不同的世界坐标，那么现在哪个深度值最可靠呢？高斯分布的深度滤波器就是解决这个问题。

沿着极线搜索下来，得到一个匹配得分沿着距离的分布，会有很多峰值，真实的对应点只有一个：

对解思路：滤波器与非线性优化。

虽然非线性优化效果较好，但是在SLAM这种实时性要求较强的场合，考虑到前端已经占据了不少的计算量，建图方面则通常采用计算量较少的滤波器。下面结合代码演示高斯分布假设的深度滤波器。

设某个像素点的深度值d服从：

每当新的数据到来，我们都会观测到它的深度(这个深度值是你通过三角化计算出来的)。同样，假设这次观测也是一个高斯分布

于是，我们的问题是，如何使用观测的信息更新原先d的分布。这正是一个信息融合

由于我们仅有观测方程没有运动方程。所以这里深度仅用到了信息融合部分，而无需像完整的卡尔曼那样进行预测和更新。如何确定我们观测到深度的分布呢？那如何计算

几何不确定性分析，现在假设我们通过极线搜索和块匹配确定了参考帧某个像素在当前帧的投影位置。那么，这个位置对深度的不确定性有多大内。现在，考虑极线l2上存在一个像素大小的误差，考虑极线搜索，这个像素的误差会导致p'与p产生多大的差距呢？

||p|| - ||p'||一个像素产生的误差。即可认为是当前观测的深度值的方差。

相应代码：

bool updateDepthFilter(
    const Vector2d &pt_ref,
    const Vector2d &pt_curr,
    const SE3d &T_C_R,
    const Vector2d &epipolar_direction,
    Mat &depth,
    Mat &depth_cov2) {
    // 用三角化计算深度
    SE3d T_R_C = T_C_R.inverse();
    Vector3d f_ref = px2cam(pt_ref);
    f_ref.normalize();
    Vector3d f_curr = px2cam(pt_curr);
    f_curr.normalize();

    // 方程
    // d_ref * f_ref = d_cur * ( R_RC * f_cur ) + t_RC
    // f2 = R_RC * f_cur
    // 转化成下面这个矩阵方程组
    // => [ f_ref^T f_ref, -f_ref^T f2 ] [d_ref] =  [f_ref^T t]
    //    [ f2^T f_ref, -f2^T f2      ] [d_cur] = [f2^T t ]
    Vector3d t = T_R_C.translation();
    Vector3d f2 = T_R_C.so3() * f_curr;
    Vector2d b = Vector2d(t.dot(f_ref), t.dot(f2));
    Matrix2d A;
    A(0, 0) = f_ref.dot(f_ref);
    A(0, 1) = -f_ref.dot(f2);
    A(1, 0) = -A(0, 1);
    A(1, 1) = -f2.dot(f2);
    Vector2d ans = A.inverse() * b;
    Vector3d xm = ans[0] * f_ref;           // ref 侧的结果
    Vector3d xn = t + ans[1] * f2;          // cur 结果
    Vector3d p_esti = (xm + xn) / 2.0;      // P的位置，取两者的平均    P为参照相机下的相机坐标
    double depth_estimation = p_esti.norm();   // 深度值

    // 计算不确定性（以一个像素为误差）认为由单个像素的不确定性引起的深度不确定性
    Vector3d p = f_ref * depth_estimation;   //p为参考相机坐标系下的坐标
    Vector3d a = p - t;
    double t_norm = t.norm();
    double a_norm = a.norm();
    double alpha = acos(f_ref.dot(t) / t_norm);
    double beta = acos(-a.dot(t) / (a_norm * t_norm));
    Vector3d f_curr_prime = px2cam(pt_curr + epipolar_direction);
    f_curr_prime.normalize();
    double beta_prime = acos(f_curr_prime.dot(-t) / t_norm);
    double gamma = M_PI - alpha - beta_prime;
    double p_prime = t_norm * sin(beta_prime) / sin(gamma);
    double d_cov = p_prime - depth_estimation;
    double d_cov2 = d_cov * d_cov;

    // 高斯融合
    double mu = depth.ptr<double>(int(pt_ref(1, 0)))[int(pt_ref(0, 0))];
    double sigma2 = depth_cov2.ptr<double>(int(pt_ref(1, 0)))[int(pt_ref(0, 0))];

    double mu_fuse = (d_cov2 * mu + sigma2 * depth_estimation) / (sigma2 + d_cov2);
    double sigma_fuse2 = (sigma2 * d_cov2) / (sigma2 + d_cov2);

    depth.ptr<double>(int(pt_ref(1, 0)))[int(pt_ref(0, 0))] = mu_fuse;
    depth_cov2.ptr<double>(int(pt_ref(1, 0)))[int(pt_ref(0, 0))] = sigma_fuse2;

    return true;
}