代码解读：DP-SLAM（1）

代码解读：DP-SLAM（1）

把DP-SLAM的论文看完了，可以对particle filter，ancestry tree以及map representation有一定了解

但是不看代码，很难有更深的了解

网上目前没有DP-SLAM的分析教程，没有办法，只能自己分析了

在具体分析代码之前，先了解一下数据集，打开loop5.log，可以看到

Odometry -2.424026 4.800517 2.590948 
Laser 181 0.642000 0.645000 0.645000 0.644000 0.644000 0.643000 0.642000 0.649000 0.648000 0.646000 0.645000 0.652000 0.651000 0.660000 0.659000 0.659000 0.667000 0.668000 0.667000 0.674000 0.675000 0.675000 0.683000 0.691000 0.692000 0.702000 0.702000 0.710000 0.710000 0.718000 0.726000 0.735000 0.743000 0.743000 0.760000 0.760000 0.771000 0.780000 0.797000 0.797000 0.815000 0.824000 0.833000 0.841000 0.858000 0.872000 0.880000 0.910000 0.917000 0.933000 0.950000 0.967000 0.992000 1.018000 1.024000 1.052000 1.077000 1.104000 1.130000 1.156000 1.191000 1.226000 1.252000 1.284000 1.336000 1.380000 1.421000 1.467000 1.476000 1.469000 1.455000 1.447000 1.437000 1.430000 1.442000 1.513000 1.600000 1.681000 1.796000 1.922000 2.068000 3.150000 3.134000 3.385000 3.757000 4.210000 4.799000 5.584000 6.775000 7.435000 8.183001 8.183001 8.183001 8.183001 8.183001 8.183001 8.183001 8.183000 8.183000 8.183001 8.183001 6.860000 6.397001 6.403001 6.420000 6.112000 6.090000 5.285000 4.937000 4.947000 4.667000 4.243000 3.900000 3.712000 3.544000 3.392000 3.250000 3.129000 3.007000 2.903000 2.808000 2.712000 2.631000 2.549000 2.472000 2.402000 2.335000 2.276000 2.216000 2.163000 2.110000 2.065000 2.019000 1.973000 1.937000 1.903000 1.859000 1.824000 1.798000 1.764000 1.741000 1.714000 1.688000 1.661000 1.634000 1.618000 1.591000 1.574000 1.550000 1.532000 1.515000 1.499000 1.482000 1.465000 1.448000 1.440000 1.423000 1.415000 1.406000 1.389000 1.381000 1.372000 1.363000 1.347000 1.346000 1.339000 1.330000 1.322000 1.314000 1.305000 1.305000 1.298000 1.298000 1.289000 1.282000 1.282000 1.275000 1.276000 1.278000 1.272000 1.264000 

loop5指装有激光雷达的小车围绕“回”字型办公楼通道的圈数

Odometry指小车的x轴和y轴上的速度以及小车朝向与x轴的夹角（也许是这个，得看后面的代码）

lasel 181指小车在这个位置的181个激光雷达观测数据，大概每隔2度就会有一个观测数据

先从代码量比较小的文件看起，

mt-rand.c存放随机数生成的函数，这个不做了解

basic.h存放一些基本的数学函数，例如max，min，sign等等

ThisRobot.h存放一些基本数据，来看一下，

// ThisRobot is the interface between the code and the robot. 
// This contains information specific to the robot currently being used, as well as the functions which
// give commands or information requests to the robot.
//

#include "basic.h"

// The number of sensor readings for this robot (typically 181 for a laser range finder)
#define SENSE_NUMBER 180
// Turn radius of the robot in map squares.Since the "robot" is actually the sensor origin for the
// purposes of this program, the turn radius is the displacement of the sensor from the robot's center
// of rotation (assuming holonomic turns)
#define TURN_RADIUS (0.40 * MAP_SCALE)

// Each sensor reading has direction it is looking (theta) and a distance at which it senses the object.
// Direction of the sensor is relative to the facing of the robot, with "forward" being 0, and is
// measured in radians. 

从注释中可以了解一些信息：

1，ThisRobot是robot和code之间交互的接口

2，观测数据是180，即每隔2度采集一次数据

3，MAP_SCALE定义栅格的尺寸，而TURN_RADIUS指激光雷达转的半径，术语意思可见wiki百科，查一下～

// This defines the number of grid squares per meter. 
// All sensors are assumed to be measuring in meters, whereas all code uses a basic unit of grid squares. 
// We use this constant for conversion.
#define MAP_SCALE 35

定义了Tsense_struct

struct TSense_struct{
  double theta, distance;
};

指激光雷达的转角，以及在该转角下测得的距离

定义了一个TsenseSample，并以这个结构体构建了一个长度为181的数组Tsense

typedef struct TSense_struct TSenseSample;
typedef TSenseSample TSense[SENSE_NUMBER+1];

定义了里程计odometry，同时定义了Todo

// This is the structure for storing odometry data from the robot. The same conditions apply as above.
struct odo_struct{
  double x, y, theta;
};
typedef struct odo_struct TOdo;

extern TOdo odometry;

定义了小车的位置以及转向，并且声明Todo为一个全局的结构体

project是用C语言写的，没有class