代码解读：DP-SLAM（7）

代码解读：DP-SLAM（7）

 

上一次，我分析了map.h的TParticle_struct 的类结构，进而引出TAncestor的类结构

并且我也分析了“粒子一家人”对应的物理意义，对应着小车的轨迹

并且可以发现TAncestor是一个单链表结构，可以想象这样的链表组成一个树结构

这是TAncestor的类代码，

struct TAncestor_struct;
struct TAncestor_struct {
  struct TAncestor_struct *parent;
  TEntryList *mapEntries;
  int size, total;
  short int generation, ID, numChildren;
  TPath *path;  // An addition for hierarchical- maintains the partial robot path represented by this particle
  char seen;  // Used by various functions for speedy traversal of the tree. 
};
typedef struct TAncestor_struct TAncestor;
typedef struct TAncestor_struct *PAncestor;

看这一片代码，还是充满疑问，就是类TEntryList和类TPath的结构

在具体分析这些类之前，可以了解到，一个好程序需要一个好算法以及一个好数据结构，

算法是程序的灵魂，然而数据结构简化了代码的构成

go on，

先来看一下类TEntryList，

// A dynamic array is stored by each ancestor particle of the map squares it has altered. We note which grid 
// square was altered (x, y) as well as an index into that square's array, corresponding to this alteration 
// (node).
struct TEntryList_struct;
struct TEntryList_struct {
  short int node;
  short int x, y;
};
typedef struct TEntryList_struct TEntryList;

从这段代码可以了解到，

变量x和y是小车在栅格地图中的位置，

再来看一下TPath，

// Used for passing the corrected odometric path from the low level to high level for
// further evaluation. Only used for hierarchical slam.
struct TPath_struct {
  // The incremental motion which this robot moved during a single time step.
  // D is the major axis of lateral motion, which is along the average facing angle during this time step
  // C is the minor axis, which is rotated +pi from D
  // T is the angular change in facing angle.
  float C, D, T;
  struct TPath_struct *next;
};
typedef struct TPath_struct TPath;

我也记不清C，D，T具体含义了，似乎跟小车走过的路径有关

对Particle的数据结构有一点了解后，再回到low.c文件，之前分析完localize函数，这次往下看，

//
// DisposeAncestry
//
// When the SLAM process is complete, this function will clean up the memory being used by the ancestry
// tree, and remove all of the associated entries from the low level map.
//
void DisposeAncestry(TAncestor particleID[]) 
{
  int i, j;
  TPath *tempPath, *trashPath;
  TEntryList *entry;

  for (i = 0; i < ID_NUMBER; i++) {
    if (particleID[i].ID == i) {
      // Free up memory
      entry = particleID[i].mapEntries;
      for (j=0; j < particleID[i].total; j++)
	LowDeleteObservation(entry[j].x, entry[j].y, entry[j].node);
      free(entry);
      particleID[i].mapEntries = NULL;
      
      tempPath = particleID[i].path;
      while (tempPath != NULL) {
	trashPath = tempPath;
	tempPath = tempPath->next;
	free(trashPath);
      }
      particleID[i].path = NULL;

      particleID[i].ID = -123;
    }

    for (cleanID=0; cleanID < ID_NUMBER; cleanID++)
      availableID[cleanID] = cleanID;
    cleanID = ID_NUMBER;
  }
}

从这段代码可以了解到，

1, dispose的意思是释放内存的含义，说白了就是把ancestry tree删除掉

具体细节就不过问啦，delete了事，再接着向下看，
 

UpdateAncestry 函数写的非常长，只好一段一段分析了，

从这段代码可以了解到，

//
// UpdateAncestry
//
// Every iteration, after particles have been resampled and evaluated (ie after Localize has been run),
// we will need to update the ancestry tree. This consists of four main steps.
// a) Remove dead nodes. These are defined as any ancestor node which has no descendents in the current
//    generation. This is caused by certain particles not being resampled, or by a node's children all
//    dying off on their own. These nodes not only need to be removed, but every one of their observations
//    in their observations in the map also need to be removed.
// b) Collapse branches of the tree. We want to restrict each internal node to having a branching factor
//    of at least two. Therefore, if a node has only one child, we merge the information in that node with
//    the one child node. This is especially common at the root of the tree, as the different hypotheses
//    coalesce.
// c) Add the new particles into the tree.
// d) Update the map for each new particle. We needed to wait until they were added into the tree, so that
//    the ancestry tree can keep track of the different observations associated with the particle.
//
void UpdateAncestry(TSense sense, TAncestor particleID[])
{
  int i, j;
  TAncestor *temp, *hold, *parentNode;
  TEntryList *entry, *workArray;
  TMapStarter *node;
  TPath *tempPath, *trashPath;

1, 先从注释读起，更新ancestry tree需要四个步骤
     a，删除dead node
     b，剪枝
     c，添加新的粒子
     d，更新每一个粒子的地图信息（这一步需要关注，因为现在还没分析到任何和栅格地图相关的数据结构）
     值得一提，这跟论文分析的是一致的

2, 初始化一些变量
     其中，类TAncestor，TEntryList和TPath 是老朋友，来看一下TMapStarter吧，

struct MapNodeStarter_struct;
struct MapNodeStarter_struct {
  // Total is the number of entries in the array which are currently being used.
  // Size is the total size of the array.
  // Dead indicates how many of those slots currently in use are taken up by obsolete entries
  short int total, size, dead;
  // The dynamic array which holds all of the observations for this grid square
  PMapNode array;
};
typedef struct MapNodeStarter_struct TMapStarter;
typedef struct MapNodeStarter_struct *PMapStarter;

 

从这段代码可以了解到，

1，从注释不难了解，变量total，Size，dead的含义

2，PMapNode型变量array有来存储激光雷达观测数据中的栅格信息

其中，PMapNode是这样定义的，typedef struct MapNode_struct *PMapNode，

是一个指向MapNode_struct结构体的指针，那我就来看看MapNode_struct的结构，

// The maps are each made up of dynamic arrays of MapNodes. 
// Each entry maintains the total distance observed through this square (distance), and corresponding number
// of scans which were observed to stop here (hits). We also keep track of the ancestor particle which made
// the observation (ID), as well as the generation of the ancestor's observation this is a modification of, 
// if any (parentGen). We also keep an index into the array of modified grid squares maintained by the ancestor 
// particle which made the observation which corresponding to this update (source).
struct MapNode_struct;
struct MapNode_struct {
  // An index into the array of observations kept by the associated ancestor node. This
  // way, the array of updates for a node, and the observation itself can point to each other.
  int source;
  // The total distance that laser traces have been observed to pass through this grid square.
  float distance;
  // The number of times that a laser has been observed to stop in this grid square (implying a possible object)
  // Density of the square is hits/distance
  short int hits;
  // The ID of the ancestor node which made this observation
  short int ID;
  // If this observation is an update of a previous observation in this grid square, this indicates the generation
  // that the previous observation was made.
  short int parentGen;
};

从这段代码中，可以了解到，

1，在注释中发现，地图是由很多个由MapNode组成的dynamic array组成的

       在2003年也许STL库都没有成型，其实这个dynamic array就是现在的vector<MapNode>

 

尾声：

DP-SLAM的代码其实并没有分析完，但是不得不结束了，不分析DP-SLAM的原因有两个，

1，对DP-SLAM的整体结构没有一个清晰的理解，知道最后才有一些认识，导致文章思路混乱

2，DP-SLAM作为激光SLAM最早的算法，可以认可其原创价值，但是比较老，不实用

 

源码分析是一个有趣的过程，就好象看数学的定理证明一样

但是源码分析也是一个有所选择的过程，不建议做全部的分析，而仅仅建议做需要研究需要改进的那部分分析

把精力用在刀刃上

从DP-SLAM分析中，收获了关于粒子滤波的基础知识，对DP-SLAM整体框架有一定了解