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

时间: 2024-10-09 09:43:07

vins-mono的边缘化分析的相关文章

Unity Mono运行机制分析及内存优化

一.Mono原理 此处简单介绍下Mono这个奇葩,主要通过Mono可以轻松实现跨平台,目前使用Unity开发手游Android平台大多使用Mono实现,但是诛仙手游使用il2cpp技术.具体il2cpp知识大家可以度娘. 二.内存分配机制 上图为内存分配策略,但是在此结合实践经验有bug存在,当分配大块内存特别明显,来回分配与切换应用会崩溃,主要是GC不够及时即使是每次分配完手动GC也不会立即释放. 三.内存泄漏原因 Mono是如何判断已用内存中哪些是不再需要使用的呢?是通过引用关系的方式来进行

内存是手游的硬伤——Unity游戏Mono内存管理与泄漏

WeTest导读 内存是游戏的硬伤,如果没有做好内存的管理问题,游戏极有可能会出现卡顿,闪退等影响用户体验的现象.本文介绍了在腾讯游戏在Unity游戏开发过程中常见的Mono内存管理问题,并介绍了一系列解决的策略和方法. 内存是手游的硬伤 无论是游戏还是VR应用,内存管理都是其研发阶段的重中之重.然而,90%以上的项目都存在不同程度的内存使用问题.就目前基于Unity引擎开发的移动游戏和移动VR游戏而言,内存的开销无外乎以下三大部分: 1.资源内存占用: 2.引擎模块自身内存占用: 3.托管堆内

Mono生命周期函数调用机制分析

1.生命周期函数的调用机制是反射,而并非继承. 我们打开Mono后发现其的确也没有生命周期函数的实现,向上翻父类中也没有,好,可以确定其就不是继承了. 下面验证下是否是反射,拿void start()函数来说,其原本是private,但如果你硬要改成public,其执行起来也什么影响(但你肯定不想外界去手动调用你的Start吧),甚至于你把它设置成private IEnumerator Start()都可以,因为是发射调用嘛,名字对了就可以.如下图             (针对Start使用I

ImageMagick 命令执行分析

ImageMagick是一款使用量很广的图片处理程序,很多厂商都调用了这个程序进行图片处理,包括图片的伸缩.切割.水印.格式转换等等.但近来有研究者发现,当用户传入一个包含『畸形内容』的图片的时候,就有可能触发命令注入漏洞. 国外的安全人员为此新建了一个网站: https://imagetragick.com/ ,不得不说,有些外国人蛮会玩的. 相对于之前的数个拥有『主页』的漏洞,这个洞确实不一般,确实是一个可以被利用的好洞,乌云主站上也爆出了数个被该漏洞影响的大厂商.我们先来分析一下它出现的原

从游戏脚本语言说起,剖析Mono所搭建的脚本基础

0x00 前言 在日常的工作中,我偶尔能遇到这样的问题:“为何游戏脚本在现在的游戏开发中变得不可或缺?”.那么这周我就写篇文章从游戏脚本聊起,分析一下游戏脚本因何出现,而mono又能提供怎样的脚本基础.最后会通过模拟Unity3D游戏引擎中的脚本功能,将Mono运行时嵌入到一个非托管(C/C++)程序中,实现脚本语言和“引擎”之间的分离. 回到目录 0x01 Why?从为何需要游戏脚本开始 首先聊聊为何现在的游戏开发需要使用游戏脚本这个话题. 为何需要有脚本系统呢?脚本系统又是因何而出现的呢?其

tolua#代码简要分析

简介 tolua#是Unity静态绑定lua的一个解决方案,它通过C#提供的反射信息分析代码并生成包装的类.它是一个用来简化在C#中集成lua的插件,可以自动生成用于在lua中访问Unity的绑定代码,并把C#中的常量.变量.函数.属性.类以及枚举暴露给lua.它是从cstolua衍变而来.从它的名字可以看出,它是集成了原来的tolua代码通过二次封装写了一个C#与tolua(c)的一个中间层. All problems in computer science can be solved by

Mono为何能跨平台?聊聊CIL(MSIL)

前言: 其实小匹夫在U3D的开发中一直对U3D的跨平台能力很好奇.到底是什么原理使得U3D可以跨平台呢?后来发现了Mono的作用,并进一步了解到了CIL的存在.所以,作为一个对Unity3D跨平台能力感兴趣的U3D程序猿,小匹夫如何能不关注CIL这个话题呢?那么下面各位看官就拾起语文老师教导我们的作文口诀(Why,What,How),和小匹夫一起走进CIL的世界吧~ Why? 回到本文的题目,U3D或者说Mono的跨平台是如何做到的? 如果换做小匹夫或者看官你来做,应该怎么实现一套代码对应多种平

猎豹傅盛:老大老二打仗老三就没了 不要边缘化

新浪科技讯 4月28日下午消息,2015全球移动互联网大会(GMIC)今日在北京召开.猎豹移动CEO傅盛做了主旨演讲.他讲述了猎豹的发展故事,从与Facebook合作:到完善全球移动广告网络:再到帮助更多中国公司出海. 傅盛指出,在五年前创业的时候,我在这个行业内有两个老师,第一叫周鸿祎,一个是雷军,第一个老师我学到的互联网的方法论,把一个点做好就可以成功,超强的竞争力和竞争意识.我秉承这个信念从上网助手成为中国第一代的互联网产品经理,我那个时候还是坚信打败对手一定能赢.但是我创业的时候,中间有

谁偷了我的热更新?Mono,JIT,IOS

前言 由于匹夫本人是做游戏开发工作的,所以平时也会加一些玩家的群.而一些困扰玩家的问题,同样也困扰着我们这些手机游戏开发者.这不最近匹夫看自己加的一些群,常常会有人问为啥这个游戏一更新就要重新下载,而不能游戏内更新呢?作为游戏开发者,或者说Unity3D程序猿,我们都清楚Unity3D不支持热更新,甚至于在IOS平台上生成新的代码都会导致游戏报错崩溃(匹夫之所以在此处强调生成新的代码这几个字,就是提醒各位不要混淆Reflection.Emit和反射).但我们是否和普通的玩家一样,看到的仅仅是“不