[关闭]
@lancelot-vim 2016-07-02T12:40:09.000000Z 字数 5048 阅读 1870

lsd-slam源码解读第六篇:GlobalMapping

lsd-slam

地图优化可以说是整个技术里面最简单的一部分了，核心其实就是g2o，可以看到这个里面代码也不多，总共加起来不到1000行，实际上就是这些工作，都是在为g2o服务

g2oTypeSim3Sophus

这个文件里面定义了图优化需要使用的基本定点class VertexSim3 : public g2o::BaseVertex<7, Sophus::Sim3d>和边class EdgeSim3 : public g2o::BaseBinaryEdge<7, Sophus::Sim3d, VertexSim3, VertexSim3>
在这之前，我们需要知道优化的对象是什么，请看论文中的公式
2016-07-02-161458_1161x147_scrot.png-22kB
实际上可以从代码和论文中明白，优化的是sim3(顶点)，代表的是相机坐标的位置，通过约束sim3((边)，代表的是两个坐标之间的变换关系，他们是g2o的基本元素，请自行研究g2o库的使用方法，即明白这里的函数都有什么作用

TrackableKeyFrameSearch

这个类主要是用于查找可用可keyFrame的，让我们直接进入函数

TrackableKeyFrameSearch::findCandidates

不得不说这是一个返回值及其扭曲的函数，下面这么长一串全部是它的返回值

std::unordered_set<Frame*, std::hash<Frame*>,
                std::equal_to<Frame*>,
                Eigen::aligned_allocator< Frame* > >

看起来比较难受，其实是返回了一个hash_set，调用的是std::hash的方法吧指针hash，应该会调用这个模板（我没仔细看，不过十有八九是这个）

  1. template<typename _Tp>
  2.   static size_t
  3.   hash(const _Tp& __val)
  4.   { return hash(&__val, sizeof(__val)); }

比较方法是std::equal_to，实际上就是调用了下面这个模板

  1. template<typename _Tp>
  2. struct equal_to : public binary_function<_Tp, _Tp, bool>
  3. {
  4.   bool
  5.   operator()(const _Tp& __x, const _Tp& __y) const
  6.   { return __x == __y; }
  7. };

最后一个是空间配置器，选用了eigen的空间配置器
传入值是简单的，即一个Frame的指针，一个Frame指针的引用，判断是否使用了FABMAP(本文默认没有)，最后是个阈值，之后向下调用了函数findEuclideanOverlapFrames，计算符合要求的Frame，实际上返回了个TrackableKFStruct结构体

  1. struct TrackableKFStruct
  2. {
  3.     Frame* ref;
  4.     SE3 refToFrame;
  5.     float dist;
  6.     float angle;
  7. };

findEuclideanOverlapFrames

首先计算了阈值，然后本地化了照相机到世界坐标的参数，计算尺度给后面检测使用(传入参数为true)

  1. float cosAngleTH = cosf(angleTH*0.5f*(fowX + fowY));
  3. Eigen::Vector3d pos = frame->getScaledCamToWorld().translation();
  4. Eigen::Vector3d viewingDir = frame->getScaledCamToWorld().rotationMatrix().rightCols<1>();
  6. float distFacReciprocal = 1;
  7. if(checkBothScales)
  8.     distFacReciprocal = frame->meanIdepth / frame->getScaledCamToWorld().scale();

之后进入循环，首先计算两帧之间的距离和角度，并进行阈值检测

  1. Eigen::Vector3d otherPos = graph->keyframesAll[i]->getScaledCamToWorld().translation();
  3. // get distance between the frames, scaled to fit the potential reference frame.
  4. float distFac = graph->keyframesAll[i]->meanIdepth / graph->keyframesAll[i]->getScaledCamToWorld().scale();
  5. if(checkBothScales && distFacReciprocal < distFac) distFac = distFacReciprocal;
  6. Eigen::Vector3d dist = (pos - otherPos) * distFac;
  7. float dNorm2 = dist.dot(dist);
  8. if(dNorm2 > distanceTH) continue;
  10. Eigen::Vector3d otherViewingDir = graph->keyframesAll[i]->getScaledCamToWorld().rotationMatrix().rightCols<1>();    float dirDotProd = otherViewingDir.dot(viewingDir);
  11. if(dirDotProd < cosAngleTH) continue;

如果都符合了要求(没有continue)，那么压入potentialReferenceFrames

  1. potentialReferenceFrames.push_back(TrackableKFStruct());
  2. potentialReferenceFrames.back().ref = graph->keyframesAll[i];
  3. potentialReferenceFrames.back().refToFrame = se3FromSim3(graph->keyframesAll[i]->getScaledCamToWorld().inverse() * frame->getScaledCamToWorld()).inverse();
  4. potentialReferenceFrames.back().dist = dNorm2;
  5. potentialReferenceFrames.back().angle = dirDotProd;

那么之后，就可以在TrackableKeyFrameSearch中得到潜在的帧，之后循环把参考帧都压入result,最后返回result

  1. for(unsigned int i=0;i<potentialReferenceFrames.size();i++)
  2.     results.insert(potentialReferenceFrames[i].ref);

TrackableKeyFrameSearch::findRePositionCandidate

这个函数也是首先调用findEuclideanOverlapFrames查找到潜在的参考帧，然后循环，进入循环之后先判断，然后调用了checkPermaRefOverlap得到可用点的分数，并做了些记录

  1. if(frame->getTrackingParent() == potentialReferenceFrames[i].ref)
  2.     continue;
  4. if(potentialReferenceFrames[i].ref->idxInKeyframes < INITIALIZATION_PHASE_COUNT)
  5.     continue;
  7. struct timeval tv_start, tv_end;
  8. gettimeofday(&tv_start, NULL);
  9. tracker->checkPermaRefOverlap(potentialReferenceFrames[i].ref, potentialReferenceFrames[i].refToFrame);
  10. gettimeofday(&tv_end, NULL);
  11. msTrackPermaRef = 0.9*msTrackPermaRef + 0.1*((tv_end.tv_sec-tv_start.tv_sec)*1000.0f + (tv_end.tv_usec-tv_start.tv_usec)/1000.0f);
  12. nTrackPermaRef++;

之后计算参考帧分数，调用getRefFrameScore

  1. inline float getRefFrameScore(float distanceSquared, float usage)
  2. {
  3.     return distanceSquared*KFDistWeight*KFDistWeight
  4.             + (1-usage)*(1-usage) * KFUsageWeight * KFUsageWeight;
  5. }

然后判断这个分数够不够高，如果不够就重新算分,方法是快速tracking，然后再次计算dist，再调用getRefFrameScore，这样就可以计算到分数，之后计算差异，然后更新goodVal

  1. SE3 RefToFrame_tracked = tracker->trackFrameOnPermaref(potentialReferenceFrames[i].ref, frame, potentialReferenceFrames[i].refToFrame);
  2. Sophus::Vector3d dist = RefToFrame_tracked.translation() * potentialReferenceFrames[i].ref->meanIdepth;
  4. float newScore = getRefFrameScore(dist.dot(dist), tracker->pointUsage);
  5. float poseDiscrepancy = (potentialReferenceFrames[i].refToFrame * RefToFrame_tracked.inverse()).log().norm();
  6. float goodVal = tracker->pointUsage * tracker->lastGoodCount / (tracker->lastGoodCount+tracker->lastBadCount);
  7. checkedSecondary++;

之后判断这个新的tracking符不符合要求，如果符合，就更新全局的最优数据

  1. bestPoseDiscrepancy = poseDiscrepancy;
  2. bestScore = score;
  3. bestFrame = potentialReferenceFrames[i].ref;
  4. bestRefToFrame = potentialReferenceFrames[i].refToFrame;
  5. bestRefToFrame_tracked = RefToFrame_tracked;
  6. bestDist = dist.dot(dist);
  7. bestUsage = tracker->pointUsage;

循环完成之后，判断找没找到，找到了就返回帧的指针，没找到返回空指针

KeyFrameGraph

这个类用于创建g2o需要使用的图，里面有的函数分别有插入帧，插入关键帧，加入约束等，这些函数都很简单，稍微长一点的就是void KeyFrameGraph::dumpMap(std::string folder),然而这个函数点进去看之后，发现竟然是保存数据用的，这个类中所有函数都巨简单无比，请读者自行研究

时至今日，总算吧lsd-slam的所有核心代码看了个精光，其他部分都有有一个让整个系统跑起来的SlamSystem，它大体上就把这些函数封装成了功能，没别的其他的难点，还有一些文件主要是用于显示的，它依托于ros，实际上这个并不需要这么麻烦的ros系统，可以根据这三个(Tracking DepthEstimation以及GlobalMapping)的现成实现，自己做一个没有ros的lsd-slam，这也是我接下来要进行的工作，整个lsd-slam篇就这么多，希望读者多多阅读代码和论文，其实在之前写的部分有一些小问题，我这两天重新阅读代码的时候发现的，我也没有进行修正，读者可以自行研究

添加新批注
在作者公开此批注前，只有你和作者可见。
回复批注