vins-mono的边缘化分析

##marg 基础   摘自贺一家的博客

在我们这个工科领域，它来源于概率论中的边际分布（marginal distribution）。如从联合分布p(x,y)去掉y得到p(x)，也就是说从一系列随机变量的分布中获得这些变量子集的概率分布。回忆了这个概率论中的概念以后，让我们转到SLAM的Bundle Adjustment上，随着时间的推移，路标特征点(landmark)和相机的位姿pose越来越多，BA的计算量随着变量的增加而增加，即使BA的H矩阵是稀疏的，也吃不消。因此，我们要限制优化变量的多少，不能只一味的增加待优化的变量到BA里，而应该去掉一些变量。那么如何丢变量就成了一个很重要的问题！比如有frame1,frame2,frame3 以及这些frame上的特征点pt1…ptn。新来了一个frame4，为了不再增加BA时的变量，出现在脑海里的直接做法是把frame1以及相关特征点pt直接丢弃，只优化frame2,frame3,frame4及相应特征点。然而，这种做法好吗？

Gabe Sibley [2]在他们的论文中就明确的说明了这个问题。直接丢掉变量，就导致损失了信息，frame1可能能更多的约束相邻的frame，直接丢掉的方式就破坏了这些约束。在SLAM中，一般概率模型都是建模成高斯分布，如相机的位姿都是一个高斯分布，轨迹和特征点形成了一个多元高斯分布p(x1,x2,x3,pt1…)，然后图优化或者BA就从一个概率问题变成一个最小二乘问题。因此，从这个多元高斯分布中去掉一个变量的正确做法是把他从这个多元高斯分布中marginalize out.

这marginalize out具体该如何操作呢？Sliding widow Filter [2]中只是简单的一句应用Schur complement（舍尔补）. 我们知道SLAM中的图优化和BA都是最小二乘问题，如下图所示(ref.[1])

据前面讨论的基于高斯牛顿的非线性优化理论可知，??? = ?可写成如下形式:

（1）

 

值得说明的是，上式中???、???并非一定为相机位姿部分和路标部分，而是希望 marg 的部分和希望保留的部分。另外，VINS 中的边缘化与 G2O 计算过程中的边缘化意义不大相同(虽然处理方法一致):G2O 中对路标点设置边缘化(pPoint->setMarginalized(true))是为了 在计算求解过程中，先消去路标点变量，实现先求解相机位姿，然后再利用求解出来的相机 位姿反过来计算路标点的过程，目的是为了加速求解，并非真的将路标点给边缘化掉;而 VINS 中则真正需要边缘化掉滑动窗口中的最老帧或者次新帧，目的是希望不再计算这一帧 的位姿或者与其相关的路标点，但是希望保留该帧对窗口内其他帧的约束关系。

假设上式中的??是我们要 marg 的变量，比如一个相机的 pose，因此我们更关心如何只 去求解我们希望保留的变量???，而不再求解???，同时我们又不能直接将???和与其相关的 路标点等信息直接删除掉，因为这样会减少约束，丢失信息。因此，采用如下 Schur 进行消 元:

（2）

其中，就称为在中的Schur项，那么有了上面式子，我们就可以直接计算xb了：

（3）

注意上式是从(1)式转化而来，我们并未丢失任何约束，因此不会丢失信息。值得说明的，上面的公式即为要保留变量???的先验信息。 

 

VINS 两种边缘化策略

marginalization_factor.cpp 代码中有几个变量需要提前说明: 

struct ResidualBlockInfo{
CostFunction *cost_function; (其中parameter_block_sizes每个优化变量块的变量大
小，以IMU残差为例，为[7,9,7,9]) LossFunction *loss_function; //优化变量数据
<double *> parameter_blocks; //待marg的优化变量id
<int> drop_set;
//Jacobian
double **raw_jacobians; <MatrixXd> jacobians; //残差，IMU:15×1,视觉:2×1 VectorXd residuals;
}

class MarginalizationInfo{
//所有观测项
<ResidualBlockInfo *> factors; //m为要marg掉的变量个数，也就是parameter_block_idx的总
localSize，以double为单位，VBias为9，PQ为6， //n为要保留下的优化变量的变量个数，
n=localSize(parameter_block_size) – m int m,n;
//<优化变量内存地址，localSize>
<long, int> parameter_block_size;
int sum_block_size;
//<待marg的优化变量内存地址，在 //parameter_block_size中的id,以double为单位>
<long, int> parameter_block_idx; //<优化变量内存地址，数据>
<long, double*> parameter_block_data;
<int> keep_block_size;
<int> keep_block_idx;
<double*> keep_block_data;
MatrixXd linearied_jacabians;
VectorXd lineared_residuals; //加残差块相关信息(优化变量、待marg的变量)
void addResidualBlockInfo(ResidualBlockInfo *); //计算每个残差对应的Jacobian，并更新parameter_block_data void preMarginalize(); //pos为所有变量维度，m为需要marg掉的变量，n为需要保留的变量 void marginalize();
}

VINS 根据次新帧是否为关键帧，分为两种边缘化策略:通过对比次新帧和次次新帧的

视差量，来决定 marg 掉次新帧或者最老帧，对应到 Estimator::optimization 代码中详细分析: 1. 当次新帧为关键帧时，MARGIN_OLD，将 marg 掉最老帧，及其看到的路标点和相关联 的 IMU 数据，将其转化为先验信息加到整体的目标函数中: 

 

1. 当次新帧为关键帧时，MARGIN_OLD，将 marg 掉最老帧，及其看到的路标点和相关联 的 IMU 数据，将其转化为先验信息加到整体的目标函数中:

1) 把上一次先验项中的残差项(尺寸为 n)传递给当前先验项，并从中去除需要丢弃 的状态量;

2) 将滑窗内第 0 帧和第 1 帧间的 IMU 预积分因子(pre_integrations[1])放到 marginalization_info 中，即图 中上半部分中 x0 和 x1 之间的表示 IMU 约束的黄色块;

3) 挑选出第一次观测帧为第 0 帧的路标点，将对应的多组视觉观测放到 marginalization_info 中，即图 中上半部分中 x0 所看到的红色五角星的路标点;

4) marginalization_info->preMarginalize():得到每次 IMU 和视觉观测(cost_function)对 应的参数块(parameter_blocks)，雅可比矩阵(jacobians)，残差值(residuals);

5) marginalization_info->marginalize():多线程计整个先验项的参数块，雅可比矩阵和 残差值，即计算公式(3)，其中与代码对应关系为: 

 

void MarginalizationInfo::marginalize()
{
    int pos = 0;
    for (auto &it : parameter_block_idx)
    {
        it.second = pos;
        pos += localSize(parameter_block_size[it.first]);
    }

    m = pos;

    for (const auto &it : parameter_block_size)
    {
        if (parameter_block_idx.find(it.first) == parameter_block_idx.end())
        {
            parameter_block_idx[it.first] = pos;
            pos += localSize(it.second);
        }
    }

    n = pos - m;

    //ROS_DEBUG("marginalization, pos: %d, m: %d, n: %d, size: %d", pos, m, n, (int)parameter_block_idx.size());

    TicToc t_summing;
    Eigen::MatrixXd A(pos, pos);
    Eigen::VectorXd b(pos);
    A.setZero();
    b.setZero();
    /*
    for (auto it : factors)
    {
        for (int i = 0; i < static_cast<int>(it->parameter_blocks.size()); i++)
        {
            int idx_i = parameter_block_idx[reinterpret_cast<long>(it->parameter_blocks[i])];
            int size_i = localSize(parameter_block_size[reinterpret_cast<long>(it->parameter_blocks[i])]);
            Eigen::MatrixXd jacobian_i = it->jacobians[i].leftCols(size_i);
            for (int j = i; j < static_cast<int>(it->parameter_blocks.size()); j++)
            {
                int idx_j = parameter_block_idx[reinterpret_cast<long>(it->parameter_blocks[j])];
                int size_j = localSize(parameter_block_size[reinterpret_cast<long>(it->parameter_blocks[j])]);
                Eigen::MatrixXd jacobian_j = it->jacobians[j].leftCols(size_j);
                if (i == j)
                    A.block(idx_i, idx_j, size_i, size_j) += jacobian_i.transpose() * jacobian_j;
                else
                {
                    A.block(idx_i, idx_j, size_i, size_j) += jacobian_i.transpose() * jacobian_j;
                    A.block(idx_j, idx_i, size_j, size_i) = A.block(idx_i, idx_j, size_i, size_j).transpose();
                }
            }
            b.segment(idx_i, size_i) += jacobian_i.transpose() * it->residuals;
        }
    }
    ROS_INFO("summing up costs %f ms", t_summing.toc());
    */
    //multi thread


    TicToc t_thread_summing;
    pthread_t tids[NUM_THREADS];
    ThreadsStruct threadsstruct[NUM_THREADS];
    int i = 0;
    for (auto it : factors)
    {
        threadsstruct[i].sub_factors.push_back(it);
        i++;
        i = i % NUM_THREADS;
    }
    for (int i = 0; i < NUM_THREADS; i++)
    {
        TicToc zero_matrix;
        threadsstruct[i].A = Eigen::MatrixXd::Zero(pos,pos);
        threadsstruct[i].b = Eigen::VectorXd::Zero(pos);
        threadsstruct[i].parameter_block_size = parameter_block_size;
        threadsstruct[i].parameter_block_idx = parameter_block_idx;
        int ret = pthread_create( &tids[i], NULL, ThreadsConstructA ,(void*)&(threadsstruct[i]));
        if (ret != 0)
        {
            ROS_WARN("pthread_create error");
            ROS_BREAK();
        }
    }
    for( int i = NUM_THREADS - 1; i >= 0; i--)  
    {
        pthread_join( tids[i], NULL ); 
        A += threadsstruct[i].A;
        b += threadsstruct[i].b;
    }
    //ROS_DEBUG("thread summing up costs %f ms", t_thread_summing.toc());
    //ROS_INFO("A diff %f , b diff %f ", (A - tmp_A).sum(), (b - tmp_b).sum());


    //TODO
    Eigen::MatrixXd Amm = 0.5 * (A.block(0, 0, m, m) + A.block(0, 0, m, m).transpose());
    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> saes(Amm);

    //ROS_ASSERT_MSG(saes.eigenvalues().minCoeff() >= -1e-4, "min eigenvalue %f", saes.eigenvalues().minCoeff());

    Eigen::MatrixXd Amm_inv = saes.eigenvectors() * Eigen::VectorXd((saes.eigenvalues().array() > eps).select(saes.eigenvalues().array().inverse(), 0)).asDiagonal() * saes.eigenvectors().transpose();
    //printf("error1: %f\n", (Amm * Amm_inv - Eigen::MatrixXd::Identity(m, m)).sum());

    Eigen::VectorXd bmm = b.segment(0, m);
    Eigen::MatrixXd Amr = A.block(0, m, m, n);
    Eigen::MatrixXd Arm = A.block(m, 0, n, m);
    Eigen::MatrixXd Arr = A.block(m, m, n, n);
    Eigen::VectorXd brr = b.segment(m, n);
    A = Arr - Arm * Amm_inv * Amr;
    b = brr - Arm * Amm_inv * bmm;

    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> saes2(A);
    Eigen::VectorXd S = Eigen::VectorXd((saes2.eigenvalues().array() > eps).select(saes2.eigenvalues().array(), 0));
    Eigen::VectorXd S_inv = Eigen::VectorXd((saes2.eigenvalues().array() > eps).select(saes2.eigenvalues().array().inverse(), 0));

    Eigen::VectorXd S_sqrt = S.cwiseSqrt();
    Eigen::VectorXd S_inv_sqrt = S_inv.cwiseSqrt();

    linearized_jacobians = S_sqrt.asDiagonal() * saes2.eigenvectors().transpose();
    linearized_residuals = S_inv_sqrt.asDiagonal() * saes2.eigenvectors().transpose() * b;
    //std::cout << A << std::endl
    //          << std::endl;
    //std::cout << linearized_jacobians << std::endl;
    //printf("error2: %f %f\n", (linearized_jacobians.transpose() * linearized_jacobians - A).sum(),
    //      (linearized_jacobians.transpose() * linearized_residuals - b).sum());
}

2. 当次新帧不是关键帧时，MARGIN_SECOND_NEW，我们将直接扔掉次新帧及它的视 觉观测边，而不对次新帧进行 marg，因为我们认为当前帧和次新帧很相似，也就是说当前 帧跟路标点之间的约束和次新帧与路标点的约束很接近，直接丢弃并不会造成整个约束关系 丢失过多信息。但是值得注意的是，我们要保留次新帧的 IMU 数据，从而保证 IMU 预积分的连贯性。 通过以上过程先验项就构造完成了，在对滑动窗口内的状态量进行优化时，把它与 IMU残差项和视觉残差项放在一起优化，从而得到不丢失历史信息的最新状态估计的结果。 

代码分析

首先我们安照正常的对ceres-solver优化添加边缘化残差来分析

首先第一步声明边缘化误差的cost function

    if (last_marginalization_info)
    {
        // construct new marginlization_factor
        MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info);
        problem.AddResidualBlock(marginalization_factor, NULL,
                                 last_marginalization_parameter_blocks);
    }

就是marginalization_factor.cpp/.h

class MarginalizationFactor : public ceres::CostFunction
{
  public:
    MarginalizationFactor(MarginalizationInfo* _marginalization_info);
    virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const;

    MarginalizationInfo* marginalization_info;
};

MarginalizationFactor继承自模版类ceres::CostFunction 

MarginalizationFactor构造函数

MarginalizationFactor::MarginalizationFactor(MarginalizationInfo* _marginalization_info):marginalization_info(_marginalization_info)
{
    int cnt = 0;
    for (auto it : marginalization_info->keep_block_size)
    {
        mutable_parameter_block_sizes()->push_back(it);
        cnt += it;
    }
    //printf("residual size: %d, %d\n", cnt, n);
    set_num_residuals(marginalization_info->n);
};

 

bool MarginalizationFactor::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
{
    //printf("internal addr,%d, %d\n", (int)parameter_block_sizes().size(), num_residuals());
    //for (int i = 0; i < static_cast<int>(keep_block_size.size()); i++)
    //{
    //    //printf("unsigned %x\n", reinterpret_cast<unsigned long>(parameters[i]));
    //    //printf("signed %x\n", reinterpret_cast<long>(parameters[i]));
    //printf("jacobian %x\n", reinterpret_cast<long>(jacobians));
    //printf("residual %x\n", reinterpret_cast<long>(residuals));
    //}
    int n = marginalization_info->n;
    int m = marginalization_info->m;
    Eigen::VectorXd dx(n);
    for (int i = 0; i < static_cast<int>(marginalization_info->keep_block_size.size()); i++)
    {
        int size = marginalization_info->keep_block_size[i];
        int idx = marginalization_info->keep_block_idx[i] - m;
        Eigen::VectorXd x = Eigen::Map<const Eigen::VectorXd>(parameters[i], size);
        Eigen::VectorXd x0 = Eigen::Map<const Eigen::VectorXd>(marginalization_info->keep_block_data[i], size);
        if (size != 7)
            dx.segment(idx, size) = x - x0;
        else
        {
            dx.segment<3>(idx + 0) = x.head<3>() - x0.head<3>();
            dx.segment<3>(idx + 3) = 2.0 * Utility::positify(Eigen::Quaterniond(x0(6), x0(3), x0(4), x0(5)).inverse() * Eigen::Quaterniond(x(6), x(3), x(4), x(5))).vec();
            if (!((Eigen::Quaterniond(x0(6), x0(3), x0(4), x0(5)).inverse() * Eigen::Quaterniond(x(6), x(3), x(4), x(5))).w() >= 0))
            {
                dx.segment<3>(idx + 3) = 2.0 * -Utility::positify(Eigen::Quaterniond(x0(6), x0(3), x0(4), x0(5)).inverse() * Eigen::Quaterniond(x(6), x(3), x(4), x(5))).vec();
            }
        }
    }
    Eigen::Map<Eigen::VectorXd>(residuals, n) = marginalization_info->linearized_residuals + marginalization_info->linearized_jacobians * dx;
    if (jacobians)
    {

        for (int i = 0; i < static_cast<int>(marginalization_info->keep_block_size.size()); i++)
        {
            if (jacobians[i])
            {
                int size = marginalization_info->keep_block_size[i], local_size = marginalization_info->localSize(size);
                int idx = marginalization_info->keep_block_idx[i] - m;
                Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> jacobian(jacobians[i], n, size);
                jacobian.setZero();
                jacobian.leftCols(local_size) = marginalization_info->linearized_jacobians.middleCols(idx, local_size);
            }
        }
    }
    return true;
}

bool MarginalizationFactor::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
是cost function 的标准成员函数，用来输入优化参数，计算残差项和雅可比矩阵。

m为要marg掉的变量个数

n为要保留下的优化变量的变量个数

 

转载于:https://www.cnblogs.com/feifanrensheng/p/10532918.html