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