代码解读：DP-SLAM（6）

代码解读：DP-SLAM（6）

分析完了localize()这个函数，可以对重采样有一个比较直观的理解，尽管代码还是有一些复杂

而且分析这个函数代码的时候，我对ftemp和particle的数据结构仍存疑虑

所以这次不急着继续分析low.c，而是查看一下particle的数据结构，争取跟论文的说法对应起来

一路追踪下来，发现particle的数据结构出现在map.h这个文件中

那就开始吧，

首先particle是从属于TParticle_struct这个类，很自然就要看一下TParticle_struct

// Each particle stores certain information, such as its hypothesized postion (x, y, theta) in the global
// sense. Also, the motion taken to get to that point from the last iteration (C, D, T) and the current
// evaluated probability of this particle. For purposes of sorting the correct map for this particle,
// we also keep a pointer tothe particle's immediate parent (from which it was resampled).
struct TParticle_struct {
  float x, y, theta; // The current position of the particle, in terms of grid squares and radians
  float C, D, T;  // Minor and major axis of motion, and change of facing, respectively
  double probability; // The proability of the particle
  // Which ancestor node this particle corresponds to. 
  TAncestor_struct *ancestryNode; 
};
typedef struct TParticle_struct TParticle;

从这个代码，可以了解到：

1，TParticle_struct 的内部元素不用介绍，非常熟悉

2，重新给这个类取了一个名字，叫TParticle

3，变量ancestryNode从属于TAncestor_struct

于是我得分析一下TAncestor_struct，

// Holds the information needed for the ancestry tree. 
// For purposes of indexing in to the map efficiently, we keep which generation (iteration 
// of the particle filter) this particle was created in. 
//
// -Each ancestor node keeps a list of all of the observations made by this ancestor node (or inherited by it)
// mapEntries is a dynamic array maintaining this list.
// -size is the total available size of this dynamic array
// -total indicates how many of those entries are currently being used.
//
// -ID is the unique ID given to this specific particle, so it can be referred to by a "name"
// ID numbers will be reused if the particle is ever removed from the ancestry tree.
// -Each iteration of the particle filter involves resampling from the previous generation of particles.
// The new particle is the child, and old particle is the parent. 
// -numChildren keeps track of the number of particles in the tree that claim this particle as their parent
// (we don't need to know explicitly who they are). 
// -parent points to the particle in the tree that is supposedly the parent of this particle. 
// However, due to the collapsing of non-branching paths in the tree (see UpdateAncestry in low.c),
// the actual particle pointed to is the most recent ancestor of this particle that is shared by at least
// one other surviving particle.
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;

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

1，从注释上可以了解，size，total，ID，numChildren 的意义

2，new particle是children; old particle是parent，回忆一下论文曾经讨论的ancestry tree

3，UpdateAncestry 应该是对ancestry tree的剪切和维护

4， Actual particle指针指向the most recent ancestor of this particle

5，根据我浅薄的知识，这是一个单向链表

简单的讲，粒子有孩子，孩子也有孩子，当然，也有孩子孩子孩子的孩子

这个粒子每一个孩子都表示对应时刻下小车的运动状态，

那么，粒子的所有子孙组成的一个“族”，共同显示了小车的整个运动轨迹