ROS-3DSLAM(14):视觉部分visual estimator 第八节 fator2

2021@SDUSC
2021年12月12日星期日——2021年12月16日星期四

一、背景简介：

这一周的工作承接上周的边缘化分析，结束这部分的代码的分析工作。

在查阅资料的过程中我发现了一份讲解，里面有一张这样的图片：

这张图不仅有助于我们对于边缘化的理解，更是有助于对于整个优化流程的理解。实在是太好了。

原文连接放在了文末，感谢大佬的指导。

可以看出来，我们这部分优化中，最重要的库就是ceres。

在之前的介绍中，我们已经知道了ceres是由谷歌公司开发的一款专门用于非线性优化的库，特别是在slam领域有着极为广泛的应用。

在上一部分的initial文件夹的位姿和imu偏差优化中我们给出了介绍，这里就不再重复了。

需要注意的是，在这里的marginalize边缘化操作中，出现了两次优化，一次是利用ceres Solver优化求解的过程，还有一次才是marginalize()函数优化求解的过程。

前者应该是在ceres Solver中出现的，而后者是下文要介绍的代码的部分。

二、代码分析：

这部分的具体的功能必须结合estimator才能完整地看出来。

我们在这里先只是简单分析它的基本功能模块。而这里面的分析必须具有数学基础才行。

我现在的数学水平还不够，只能勉强看懂，等到寒假有时间时要好好补课。

void ResidualBlockInfo::Evaluate()
{
   	
    //获取残差地个数，包括了IMU部分和视觉的部分
    residuals.resize(cost_function->num_residuals());
	//优化了变量参数块的变量大小
    std::vector<int> block_sizes = cost_function->parameter_block_sizes();
    //数组外围的大小，也就是参数快的个数
    raw_jacobians = new double *[block_sizes.size()];eval
    jacobians.resize(block_sizes.size());
	//分配行的大小，包括了残差的位数*每个参数块中参数的个数，block_sizes[i],J矩阵的大小。
    for (int i = 0; i < static_cast<int>(block_sizes.size()); i++)
    {
   
        jacobians[i].resize(cost_function->num_residuals(), block_sizes[i]);
        raw_jacobians[i] = jacobians[i].data();
        //dim += block_sizes[i] == 7 ? 6 : block_sizes[i];
    }
    //利用各自残差的Evaluate函数计算残差和雅克比矩阵
    cost_function->Evaluate(parameter_blocks.data(), residuals.data(), raw_jacobians);

    //std::vector<int> tmp_idx(block_sizes.size());
    //Eigen::MatrixXd tmp(dim, dim);
    //for (int i = 0; i < static_cast<int>(parameter_blocks.size()); i++)
    //{
   
    //    int size_i = localSize(block_sizes[i]);
    //    Eigen::MatrixXd jacobian_i = jacobians[i].leftCols(size_i);
    //    for (int j = 0, sub_idx = 0; j < static_cast<int>(parameter_blocks.size()); sub_idx += block_sizes[j] == 7 ? 6 : block_sizes[j], j++)
    //    {
   
    //        int size_j = localSize(block_sizes[j]);
    //        Eigen::MatrixXd jacobian_j = jacobians[j].leftCols(size_j);
    //        tmp_idx[j] = sub_idx;
    //        tmp.block(tmp_idx[i], tmp_idx[j], size_i, size_j) = jacobian_i.transpose() * jacobian_j;
    //    }
    //}
    //Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> saes(tmp);
    //std::cout << saes.eigenvalues() << std::endl;
    //ROS_ASSERT(saes.eigenvalues().minCoeff() >= -1e-6);
	//视觉里面有Huber核函数，需要重新写雅克比与残差的信息
    if (loss_function)
    {
   
        double residual_scaling_, alpha_sq_norm_;

        double sq_norm, rho[3];

        sq_norm = residuals.squaredNorm();
        loss_function->Evaluate(sq_norm, rho);
        //printf("sq_norm: %f, rho[0]: %f, rho[1]: %f, rho[2]: %f\n", sq_norm, rho[0], rho[1], rho[2]);

        double sqrt_rho1_ = sqrt(rho[1]);

        if ((sq_norm == 0.0) || (rho[2] <= 0.0))
        {
   
            residual_scaling_ = sqrt_rho1_;
            alpha_sq_norm_ = 0.0;
        }
        else
        {
   
            const double D = 1.0 + 2.0 * sq_norm * rho[2] / rho[1];
            const double alpha = 1.0 - sqrt(D);
            residual_scaling_ = sqrt_rho1_ / (1 - alpha);
            alpha_sq_norm_ = alpha / sq_norm;
        }

        for (int i = 0; i < static_cast<int>(parameter_blocks.size()); i++)
        {
   
            jacobians[i] = sqrt_rho1_ * (jacobians[i] - alpha_sq_norm_ * residuals * (residuals.transpose() * jacobians[i]));