代码解读：DP-SLAM（2）

代码解读：DP-SLAM（2）

说实话，dp-slam的代码量挺大的，估计得写很多的篇幅，没办法

从上次的讨论来看，明白了一些基本的数据结构，例如小车的结构，小车的2d坐标以及小车的转角朝向，再比如雷

达的数据结构，即雷达在张角多少度的情况下采集到的数据是怎样的

今天开始分析low.c这个文件

先来看TSample_struct这个结构体

// A sample is a lot like a short-lived particle. More samples are generated than particles,
// since we are certain that most of the generated samples will not get resampled. The basic
// difference is that since samples are not expected to be resampled, they don't need an entry
// in the ancestry tree, nor do they need to update the map.
struct TSample_struct {
   // The position of the sample, with theta being the facing angle of the robot.
  double x, y, theta; 
   // The incremental motion which this sample moved during this iteration.
   // 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.
  double C, D, T;
   // The current probability of this sample, given the observations and this sample's map
  double probability;
   // The index into the array of particles, indicating the parent particle that this sample was resampled 
   // from. This is mostly used to determine the correct map to use when evaluating the sample.
  int parent;
};
typedef struct TSample_struct TSample;

从上面的代码，可以了解一些情况：

1，typedef语句使得TSample等效于TSample_struct

2，每一个TSample成员都表示对小车当前小车位置的一种猜测

3，从上面的代码中，似乎很难理解变量C，D，T的意思，那就暂且放在这里，不做理解

来看下一段代码，

 // Every particle needs a unique ID number. This stack keeps track of the unused IDs.
int cleanID;
int availableID[ID_NUMBER];

这不难理解，给每一个粒子一个id编号

再来看一段赋值语句，

 // We generate a large number of extra samples to evaluate during localization, much larger than the number of true particles.
 // We store the samples that are being localized over in newSample, rather than keep a true particle for each.
TSample newSample[SAMPLE_NUMBER];
 // In order to compute the amount of percieved motion from the odometry, the last odometry readings are recorded 
 // The actual percieved movement is the current odometry readings minus these recorded 'last' readings.
double lastX, lastY, lastTheta;
 // No. of children each particle gets, based on random resampling
int children[PARTICLE_NUMBER];

 // savedParticle is where we store the current set of samples which were resampled, before we have
 // created an ID and an entry in the ancestry tree for each one.
TParticle savedParticle[PARTICLE_NUMBER];
int cur_saved_particles_used;

从这段赋值语句可以发现：

1，要使用大量的粒子来估计小车的位置

2，每一个粒子都有children，用重采样得到的，并且把children保存起来

3，last变量指小车上一次的位置

再看一段赋值语句，

 // Keeps track of what iteration the SLAM process is currently on.
int curGeneration;
 // Stores the most recent set of laser observations.
TSense sense;
 // This array stores the color values for each grid square when printing out the map. For some reason,
 // moving this out as a global variable greatly increases the stability of the code.
unsigned char map[MAP_WIDTH][MAP_HEIGHT];
THold hold[LOW_DURATION];

从中可以了解：

1，sense指传感器激光雷达数据

2，定义了map的大小，一个二维数组

分析函数AddToWorldModel，

//
// AddToWorldModel
//
// sense- the current laser observations
// particleNum - the index into the particle array to the entry that is making additions to the map
//
void AddToWorldModel(TSense sense, int particleNum) {
  int j;

  // Run through each point that the laser found an obstruction at
  for (j=0; j < SENSE_NUMBER; j++) 
    // Normalize readings relative to the pose of current assumed position
    LowAddTrace(l_particle[particleNum].x, l_particle[particleNum].y, sense[j].distance, (sense[j].theta + l_particle[particleNum].theta), 
		l_particle[particleNum].ancestryNode->ID, (sense[j].distance < MAX_SENSE_RANGE));
}

从这段代码可以了解到：

1，从字面理解，就是把激光雷达的数据显现在栅格地图上

2，激光雷达旋转一周，一共采集SENSE_NUMBER个数据（其实是361个数据），所以for语句循环不难理解

3，LowAddTrace()不难理解，但是要注意l_particle的含义：

       每一个particle代表小车的一种位置姿态的猜测，每一种姿态下都能生成一种地图

       简单讲，一个粒子一个地图

再来分析函数CheckScore()，

//
// CheckScore
//
// Determine the fitness of a laser endpoint
//
// sense- the set of all current laser observations
// index- the specific number of the laser observation that we are going to score
// sampleNum- the idnex into the sample array for the sample we are currently concerned with
//
// Returns the unnormalized posterior for current particle
//
inline double CheckScore(TSense sense, int index, int sampleNum) 
{
  double a;

  a = LowLineTrace(newSample[sampleNum].x, newSample[sampleNum].y, (sense[index].theta + newSample[sampleNum].theta), 
		   sense[index].distance, l_particle[ newSample[sampleNum].parent ].ancestryNode->ID, 0);
  return MAX(MAX_TRACE_ERROR, a);
}

从这段代码可以得到：

1，CheckScore函数的主要作用是核查激光雷达数据

       sense指当前激光雷达数据，是一个1×361的数组，

       index指雷达的第几个数据，index取值与1到361之间，

       sampleNum指粒子抽样，比如sampleNum=3时，指抽取第三个粒子，小车姿态的第三个猜测

2，注意：

       l_particle[ newSample[sampleNum].parent ].ancestryNode->ID

时间有限，咱们下回再分析