lsd-slam源码解读第四篇:tracking

lsd-slam源码解读第四篇:tracking

标签（空格分隔）： lsd-slam

进入这一篇之前，我希望你已经很明白了lsd-slam的算法，这很重要，如果不明白算法，阅读后续的代码无异于阅读天书，如果不太明白算法，请先阅读我写的第三篇：lsd-slam源码解读第三篇:算法解析

---

LGSX

于是就进入了Tracking文件夹里的第一个类型LSGX，这里面有3个类，分别是LSG4,LSG6和LSG7，他们定义了4个参数6个参数以及7个参数的最小二乘法
我想值得注意的是这段代码：

     inline void update(const Vector6& J, const float& res, const float& weight)
      {
        A.noalias() += J * J.transpose() * weight;
        b.noalias() -= J * (res * weight);
        error += res * res * weight;
        num_constraints += 1;
      }

这里使用了Eigen库里面的noalias机制，下面是Eigen库的官方文档：

为了讲解这个用法，请看下面这个程序

#include <iostream>
#include <stdio.h>

#include <eigen3/Eigen/Dense>

using namespace std;

template <typename T>
class A {
public:
    A() {printf("A()\n");}
    ~A() {printf("~A()\n");}
    template <typename U>
    A operator*(A<U>&){
        A a;
        return a;
    }

};

int main()
{
    A<int> a1,b1;
    A<int> c1;
    c1 = a1*b1;
    Eigen::Matrix2f a, b, c;

    c.noalias() = a * b;
    a(0, 0) = 0;
    a(0, 1) = 1;
    a(1, 0) = 1;
    a(1, 1) = 0;
    b(0, 0) = 1;
    b(0, 1) = 2;
    b(1, 0) = 3;
    b(1, 1) = 4;
    c.noalias()  = a * b;
    std::cout << c << std::endl;

    return 0;
}

运行结果：

A()
A()
A()
A()
~A()
3 4
1 2
~A()
~A()
~A()

什么意思呢？注意我自己写的模板类A，在main函数里面有一个乘法c1 = a1*b1;，这个会调用重载运算符，然后先构造一个对象，再拷贝，最后析构，这样对于大量使用继承的程序会大大影响效率(构造基类构造子类，析构基类，析构子类，而且如果有内存申请，拷贝，释放，效率会降低更多)，那怎么办呢？Eigen使用了一种机制，让我们减少一次对象的构造，这个机制就是用noalias()调用，这样相当于吧运行a*b的结果直接拷贝到c里面，不再去构造中间对象
如何实现呢，其实由于要考虑到泛化，代码内部实现比较复杂，但原理实际上是写一个算法类，算法类中储存了算法的函数对象(仿函数)或者函数指针和储存算法运算的结果容器的引用或者指针，然后调用算法的时候，不再去构造那个复杂的数据结构，转而构造一个简单的算法类，将运行结果赋值给容器的引用或指针，从而降低内存开销
程序中调用noalias()函数，实际上返回的是一个NoAlias的对象，这个对象维护了真正的乘法(实际上为了泛化，还向下有调用，可能会查看的代码和文档链接:NoAlias.h, MatrixBase.h， NoAlias< Derived, MatrixBase > noalias()， EigenBase< Derived > Struct Template Reference)

---

TrackingReference

讲了一大堆Eigen库和c++模板语法的东西，如果没有理解，忘了它吧，这个和Tracking算法没太大关系，我只是为了表现一下我的c++语法功底，扯了扯题外话而已。
不知你还记不记得之前第三篇里面讲的Tracking，如果记得的话，那么应该很清楚第一个过程是是选择参考帧，程序里面体现的就是这个类了

class TrackingReference
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    /** Creates an empty TrackingReference with optional preallocation per level. */
    TrackingReference();
    ~TrackingReference();
    void importFrame(Frame* source);

    Frame* keyframe;
    boost::shared_lock<boost::shared_mutex> keyframeLock;
    int frameID;

    void makePointCloud(int level);
    void clearAll();
    void invalidate();
    Eigen::Vector3f* posData[PYRAMID_LEVELS];   // (x,y,z)
    Eigen::Vector2f* gradData[PYRAMID_LEVELS];  // (dx, dy)
    Eigen::Vector2f* colorAndVarData[PYRAMID_LEVELS];   // (I, Var)
    int* pointPosInXYGrid[PYRAMID_LEVELS];  // x + y*width
    int numData[PYRAMID_LEVELS];

private:
    int wh_allocated;
    boost::mutex accessMutex;
    void releaseAll();
};

---

void importFrame(Frame* source);

让我们来看第一个函数void importFrame(Frame* source);

void TrackingReference::importFrame(Frame* sourceKF)
{
    boost::unique_lock<boost::mutex> lock(accessMutex);
    keyframeLock = sourceKF->getActiveLock();
    keyframe = sourceKF;
    frameID=keyframe->id();


    // reset allocation if dimensions differ (shouldnt happen usually)
    if(sourceKF->width(0) * sourceKF->height(0) != wh_allocated)
    {
        releaseAll();
        wh_allocated = sourceKF->width(0) * sourceKF->height(0);
    }
    clearAll();
    lock.unlock();
}

前面几行是不难理解的，后面是一个判断，就是判断是否需要重置资源，如果宽度和高度的乘积和先前的高度与宽度的乘积一样大，那么就不用重新配置了，直接用先前的资源即可，如果不同，那么就释放先前左右的资源(请自行查看releaseAll())，然后把资源配置的记录记录成当前的配置数，最后再清空点云的记录数据numData(实际上就是令它等于0，请自行查看clearAll())

---

void makePointCloud(int level);

第二个重要的函数就是这个void makePointCloud(int level)了，代码如下

void TrackingReference::makePointCloud(int level)
{
    assert(keyframe != 0);
    boost::unique_lock<boost::mutex> lock(accessMutex);

    if(numData[level] > 0)
        return; // already exists.

    int w = keyframe->width(level);
    int h = keyframe->height(level);

    float fxInvLevel = keyframe->fxInv(level);
    float fyInvLevel = keyframe->fyInv(level);
    float cxInvLevel = keyframe->cxInv(level);
    float cyInvLevel = keyframe->cyInv(level);

    const float* pyrIdepthSource = keyframe->idepth(level);
    const float* pyrIdepthVarSource = keyframe->idepthVar(level);
    const float* pyrColorSource = keyframe->image(level);
    const Eigen::Vector4f* pyrGradSource = keyframe->gradients(level);

    if(posData[level] == nullptr) posData[level] = new Eigen::Vector3f[w*h];
    if(pointPosInXYGrid[level] == nullptr)
        pointPosInXYGrid[level] = (int*)Eigen::internal::aligned_malloc(w*h*sizeof(int));;
    if(gradData[level] == nullptr) gradData[level] = new Eigen::Vector2f[w*h];
    if(colorAndVarData[level] == nullptr) colorAndVarData[level] = new Eigen::Vector2f[w*h];

    Eigen::Vector3f* posDataPT = posData[level];
    int* idxPT = pointPosInXYGrid[level];
    Eigen::Vector2f* gradDataPT = gradData[level];
    Eigen::Vector2f* colorAndVarDataPT = colorAndVarD