代码解读: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
时间有限,咱们下回再分析