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為要保留下的優化變量的變量個數