代码解读: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整体框架有一定了解

 

 

评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值