ICP系列算法|激光SLAMICP和NDT

SLAM前端匹配，是构建方程的过程。后端优化是求解方程的过程。一个好的SLAM算法重点在前端。

做帧间的匹配是为了得到机器人前后的局部位姿变换关系，可以认为是一种里程计。其实标定了轮子直径跟间距我们就有了一个里程计了，但是因为轮子打滑等各种因素存在误差会比较大。再用雷达的数据做一次配准可以提高定位精度。

  • ICP：迭代最近点
  • PL-ICP：点到线的ICP
  • IMLS-ICP：implicit moving least square，隐式移动最小二乘
  • Generalized ICP (GICP)，综合考虑 point-to-point、point-to-plane 和 plane-to-plane 策略，精度、鲁棒性都有所提高；
    Normal Iterative Closest Point (NICP)
  • NICP：考虑法向量和局部曲率，更进一步利用了点云的局部结构信息，其论文中实验结果比 GICP 的性能更好。

ICP

R,t就是相邻两帧激光对应的机器人位置的欧氏变换，以此类推，根据很多帧的点云，就能求出一系列的机器人位姿变换关系。
ICP有二维和三维。
由于两个点云对应同一个实体，那么理论上配准后的距离应该为0，由于误差的存在，我们要求距离的最小值。

对于已知对应点的情况(已知R)，我们可以求出闭式解，而不必用迭代方法：

实际中不知道对应点匹配，不能直接算出R和t，要进行迭代计算：寻找对应点，根据对应点计算R,t；对点云转换再计算误差；不断迭代直至误差足够小。ICP算法里不能有nan的雷达数据

ICP方法存在以下缺点，所以不在实际SLAM中使用:

  1. 依赖初始值，初始值不好时，迭代次数增加；对于较大的初始误差，可能会出现错误的迭代结果
  2. ICP是一阶收敛，收敛速度慢。（所以会用kd-tree来加快搜索效率）例如我使用A2雷达，频率12.7Hz，两帧间隔0.079s。ICP时间在0.01s~0.02s，而超过0.015s的有一部分，这就了两帧scan间隔的%19。甚至有些雷达，ICP计算时间大于两帧间隔，这样算出来的位置始终是延迟的．
  3. 两帧激光点云中的点不可能表示的是空间中相同的位置，所以用点到点的距离作为误差方程势必会引入随机误差。
  4. 会有离群点及噪声

所以原始的ICP太粗糙，不足以应用实践。

PL-ICP

深蓝学院这一段又讲的不好，对censi论文中的PL-ICP示意图讲解不到位，对红色和蓝色的点，同心圆没有说明，而且数学公式怀疑有问题，看的云里雾里。

Point to Line-ICP修改的是误差尺度，思想和之后的ICP类似。

雷达的激光点是对实际环境中曲面的离散采样。重要的不是激光点，而是曲面。最好的误差尺度为当前激光点到实际
曲面的距离；所以关键的问题在于如何恢复曲面

PL-ICP：用分段线性的方法（折线）来对实际曲面进行近似，从而定义当前帧激光点到曲面的距离

  1. ICP对点对点的距离作为误差，PL-ICP为点到线的距离作为误差；PL-ICP的误差形式更符号实际情况。
  2. 收敛速度不同，ICP为一阶收敛，PL-ICP为二阶收敛。
  3. PL-ICP的求解精度高于ICP，特别是在结构化环境中，但不适合室外
  4. PL-ICP对初始值更敏感。不单独使用，其容易陷入局部循环。与里程计、CSM等一起使用，通常用里程计得到一个初始转换矩阵q0给到PL-ICP算法

经我测试，换成PL-ICP后，配准时间降了一个数量级，耗时比较长的是0.00274035s。但是静止时输出的帧间匹配结果的精度，没有明显提升，可能是雷达问题。


ros的csm包实现了ICP和PL-ICP算法。作者给出了一个该功能包的操作说明文件（csm_manual.pdf）。里面详细描述了各项配置参数的含义。其中sm/app文件夹中的sm0.c sm1.c sm2.c sm3.c 相当于是几个使用示例。 主要的算法实现是在csm/icp文件夹中的几个文件里。论文中的所有算法步骤完整的体现在了icp_loop.c文件中的icp_loop函数里。

AMCL的缺陷和遗留问题|激光SLAMamcl和粒子滤波

缺陷

  1. 如果AMCL算法无法快速解决绑架问题，添加随机位姿的粒子可能会导致粒子集扩散。失效恢复机制有时能重定位成功(可能花十几秒时间)，有时失败

  2. AMCL依赖里程计，所以误差不能太大，否则影响粒子的位姿；依赖扫描匹配，所以环境中缺乏明显的特征或有物体遮挡或者环境有明显变化时，扫描匹配结果有较大偏差，最终影响粒子权重。

  3. AMCL收敛速度不够快，在初值比较好的情况下，如果设置min_particles为300，max_particles为5000，重采样需要14~20次才能收敛到300个粒子。如果设置得更小，收敛速度就更慢。 重采样几次时，有的粒子权重还是另一些粒子权重的三四倍。

问题

  1. AMCL的运动模型是旋转——平移——旋转，但我的里程计实际是按两个时刻机器人走了一小段直线

  2. 似然域模型的高斯模糊图有什么意义，跟激光点到最近障碍的距离有什么关系

  3. AMCL出现定位抖动，很可能是里程计误差

  4. 如果出现机器人行走一段时间后，粒子集稀疏甚至彻底分散的情况。这可能是环境和地图不匹配，雷达扫描也就不匹配，测量模型出问题，结果w_diff比较大，AMCL向地图注入随机粒子

连接的master错误.png

源码分析(七)重采样|激光SLAMamcl和粒子滤波

laserReceived 中的实现

以下内容在 if(lasers_update_[laser_index]) 的括号内

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
// 粒子重采样
if(!(++resample_count_ % resample_interval_))
{
  pf_update_resample(pf_);
  resampled = true;
}

pf_sample_set_t* set = pf_->sets + pf_->current_set;
ROS_DEBUG("Num samples: %d\n", set->sample_count);

// 发布话题particlecloud, 发布的是最终结果cloud，在机器人移动后执行
if (!m_force_update)  //在运动模型那里赋值false
{
  geometry_msgs::PoseArray cloud_msg;
  cloud_msg.header.stamp = ros::Time::now();
  cloud_msg.header.frame_id = global_frame_id_;
  cloud_msg.poses.resize(set->sample_count);
  for(int i=0;i<set->sample_count;i++)  // 粒子个数
  {
    // tf::Pose转为geometry_msgs::Pose，就是用于后面发布消息
    tf::poseTFToMsg(tf::Pose(tf::createQuaternionFromYaw(set->samples[i].pose.v[2]),
                             tf::Vector3(set->samples[i].pose.v[0],
                                         set->samples[i].pose.v[1], 0) ),
                    cloud_msg.poses[i]);
  }
    // 构造函数中注册了话题
    particlecloud_pub_.publish(cloud_msg);
}


以下内容紧接上面内容，不在if内，但还在laserReceived内

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
// 刚启动amcl时不执行,机器人移动后执行
if(resampled || force_publication)
{
    // Read out the current hypotheses
    double max_weight = 0.0;
    int max_weight_hyp = -1;
    std::vector<amcl_hyp_t> hyps;
    hyps.resize(pf_->sets[pf_->current_set].cluster_count);
    for(int hyp_count = 0; hyp_count < pf_->sets[pf_->current_set].cluster_count; hyp_count++)
    {
        double weight;
        pf_vector_t pose_mean;
        pf_matrix_t pose_cov;
        if (!pf_get_cluster_stats(pf_, hyp_count, &weight, &pose_mean, &pose_cov))
        {
            ROS_ERROR("Couldn't get stats on cluster %d", hyp_count);
            break;
        }
        
        hyps[hyp_count].weight = weight;
        hyps[hyp_count].pf_pose_mean = pose_mean;
        hyps[hyp_count].pf_pose_cov = pose_cov;
        
        if(hyps[hyp_count].weight > max_weight)
        {
            max_weight = hyps[hyp_count].weight;
            max_weight_hyp = hyp_count;
        }
    }
    
    if(max_weight > 0.0)
    {
        ROS_DEBUG("Max weight pose: %.3f %.3f %.3f",
                  hyps[max_weight_hyp].pf_pose_mean.v[0],
                hyps[max_weight_hyp].pf_pose_mean.v[1],
                hyps[max_weight_hyp].pf_pose_mean.v[2] );
        geometry_msgs::PoseWithCovarianceStamped p;
        // Fill in the header
        p.header.frame_id = global_frame_id_;
        p.header.stamp = laser_scan->header.stamp;
        // Copy in the pose
        p.pose.pose.position.x = hyps[max_weight_hyp].pf_pose_mean.v[0];
        p.pose.pose.position.y = hyps[max_weight_hyp].pf_pose_mean.v[1];
        tf::quaternionTFToMsg(tf::createQuaternionFromYaw(hyps[max_weight_hyp].pf_pose_mean.v[2]),
                p.pose.pose.orientation);
        // Copy in the covariance, converting from 3-D to 6-D
        pf_sample_set_t* set = pf_->sets + pf_->current_set;
        for(int i=0; i<2; i++)
        {
            for(int j=0; j<2; j++)
            {
                // Report the overall filter covariance, rather than the
                // covariance for the highest-weight cluster
                //p.covariance[6*i+j] = hyps[max_weight_hyp].pf_pose_cov.m[i][j];
                p.pose.covariance[6*i+j] = set->cov.m[i][j];
            }
        }
        // Report the overall filter covariance, rather than the
        // covariance for the highest-weight cluster
        //p.covariance[6*5+5] = hyps[max_weight_hyp].pf_pose_cov.m[2][2];
        p.pose.covariance[6*5+5] = set->cov.m[2][2];
        
        pose_pub_.publish(p);
        last_published_pose = p;
        
        ROS_DEBUG("New pose: %6.3f %6.3f %6.3f",
                  hyps[max_weight_hyp].pf_pose_mean.v[0],
                hyps[max_weight_hyp].pf_pose_mean.v[1],
                hyps[max_weight_hyp].pf_pose_mean.v[2]);
        
        // subtracting base to odom from map to base and send map to odom instead
        tf::Stamped<tf::Pose> odom_to_map;
        try
        {
            tf::Transform tmp_tf(tf::createQuaternionFromYaw(hyps[max_weight_hyp].pf_pose_mean.v[2]),
                    tf::Vector3(hyps[max_weight_hyp].pf_pose_mean.v[0],
                    hyps[max_weight_hyp].pf_pose_mean.v[1],
                    0.0));
            tf::Stamped<tf::Pose> tmp_tf_stamped (tmp_tf.inverse(),
                                                  laser_scan->header.stamp,
                                                  base_frame_id_);
            this->tf_->transformPose(odom_frame_id_,
                                     tmp_tf_stamped,
                                     odom_to_map);
        }
        catch(tf::TransformException)
        {
            ROS_DEBUG("Failed to subtract base to odom transform");
            return;
        }
        latest_tf_ = tf::Transform(tf::Quaternion(odom_to_map.getRotation()),
                                   tf::Point(odom_to_map.getOrigin()));
        latest_tf_valid_ = true;
        if (tf_broadcast_ == true)
        {
            // We want to send a transform that is good up until a
            // tolerance time so that odom can be used
            ros::Time transform_expiration = (laser_scan->header.stamp +
                                              transform_tolerance_);
            tf::StampedTransform tmp_tf_stamped(latest_tf_.inverse(),
                                                transform_expiration,
                                                global_frame_id_, odom_frame_id_);
            this->tfb_->sendTransform(tmp_tf_stamped);
            sent_first_transform_ = true;
        }
    }
    else
    {
        ROS_ERROR("No pose!");
    }
}

laserReceived到这里只剩一个else if(latest_tf_valid_)了

pf_update_resample

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
// Resample 粒子分布
void pf_update_resample(pf_t *pf)
{
  int i;
  double total;
  pf_sample_set_t *set_a, *set_b;
  pf_sample_t *sample_a, *sample_b;

  double* c;
  double w_diff;
  // 用来接收当前粒子集的信息
  set_a = pf->sets + pf->current_set;
  // 保存重采样后的粒子
  set_b = pf->sets + (pf->current_set + 1) % 2;

  // Build up cumulative probability table for resampling.
  // TODO: Replace this with a more efficient procedure  (e.g., GeneralDiscreteDistributions.html)
  // 对粒子的权重进行积分，获得分布函数，后面用于重采样
  c = (double*)malloc(sizeof(double)*(set_a->sample_count+1));
  c[0] = 0.0;
  for(i=0;i<set_a->sample_count;i++)
    c[i+1] = c[i] + set_a->samples[i].weight;
	// c[1] = 第一个粒子权重
	// c[2] = 第一和第二粒子权重之和，以此类推 c[i] 是粒子集的 前i个粒子的权重之和
  // Create the kd tree for adaptive sampling
  pf_kdtree_clear(set_b->kdtree);
  
  // Draw samples from set a to create set b.
  total = 0;
  set_b->sample_count = 0;
  // 表示注入粒子的概率
  w_diff = 1.0 - pf->w_fast / pf->w_slow;
  if(w_diff < 0.0)
    w_diff = 0.0;

  // Can't (easily) combine low-variance sampler with KLD adaptive
  // sampling, so we'll take the more traditional route.
  /*
  // Low-variance resampler, taken from Probabilistic Robotics, p110
  count_inv = 1.0/set_a->sample_count;
  r = drand48() * count_inv;
  c = set_a->samples[0].weight;
  i = 0;
  m = 0;
  */
	// 确保重采样生成的粒子集（set_b）的粒子数不超过规定的最大的粒子数
  while(set_b->sample_count < pf->max_samples)
  {
    sample_b = set_b->samples + set_b->sample_count++;
    // 产生的随机数小于w_diff时，将往set_b中随机注入粒子
    if(drand48() < w_diff)
    	// pf->random_pose_fn 为一个函数指针，其返回一个随机位姿
      sample_b->pose = (pf->random_pose_fn)(pf->random_pose_data);
    else
    {
      // Can't (easily) combine low-variance sampler with KLD adaptive
      // sampling, so we'll take the more traditional route.
      /*
      // Low-variance resampler, taken from Probabilistic Robotics, p110
      U = r + m * count_inv;
      while(U>c)
      {
        i++;
        // Handle wrap-around by resetting counters and picking a new random
        // number
        if(i >= set_a->sample_count)
        {
          r = drand48() * count_inv;
          c = set_a->samples[0].weight;
          i = 0;
          m = 0;
          U = r + m * count_inv;
          continue;
        }
        c += set_a->samples[i].weight;
      }
      m++;
      */

      // Naive discrete event sampler
      double r = drand48();  // drand48 返回服从均匀分布的[0.0, 1.0)之间的 double 随机数
      // c[i]相当于把粒子权重以数轴距离的形式表现，查看r容易落在数轴哪个位置。
      // 当set_a的第i个粒子权重很大时，r落在c[i]与c[i+1]之间的概率就很高，找符合条件的i
      // 虽然这里不能保证每次都能从set_a中提取权重最大的粒子，因为r是一个随机数。但是由于粒子数量较大，
      // 因此提取的大多数粒子权重还是高的，这是一个统计学的方法
      for(i=0; i<set_a->sample_count; i++)
      {
        if((c[i] <= r) && (r < c[i+1]))
          break;
      }
      assert(i<set_a->sample_count);

      sample_a = set_a->samples + i;
      assert(sample_a->weight > 0);
      // 从set_a中挑选粒子赋给一个粒子，之后放进set_b
      sample_b->pose = sample_a->pose;
    }
    sample_b->weight = 1.0;
    // total之前是0
    total += sample_b->weight;
    // Add sample to histogram
    pf_kdtree_insert(set_b->kdtree, sample_b->pose, sample_b->weight);
    // 大的粒子数在定位初期能提高定位精度，但是当粒子集开始聚合后，程序便不再需要这么多的粒子
    //  因此这里需要引入一个新的控制条件来节省计算资源
    // pf_resample_limit根据时间与粒子集测量更新的结果，返回粒子集所需要的最佳粒子数量，
    // 从而实现了粒子数量随着时间的变化而自我调整
    if (set_b->sample_count > pf_resample_limit(pf, set_b->kdtree->leaf_count))
      break;
  }
  // 重置，避免w_diff越来越大，导致大量插入随机位姿的粒子 
  if(w_diff > 0.0)
    pf->w_slow = pf->w_fast = 0.0;

  // 将set_b的新粒子插入Kd-Tree中并且对粒子的权重归一化，为了之后的聚类分析做准备
  for (i = 0; i < set_b->sample_count; i++)
  {
    sample_b = set_b->samples + i;
    sample_b->weight /= total;   // 粒子集b的粒子权重都是 1/total
  }
  // 重新聚类分析
  pf_cluster_stats(pf, set_b);
  // 将set_b设为当前粒子集，参与下一次的更新循环
  // 此时的set_b就对应rviz显示的粒子云
  pf->current_set = (pf->current_set + 1) % 2; 

  pf_update_converged(pf);
  free(c);
  return;
}

如果w_diff在某次计算的比较大，那么就容易注入随机粒子，比如：

1
2
3
4
5
w_diff >0:  0.032770
random pose num: 160

w_diff >0:  0.040729
random pose num: 214

最后收敛后，set_b中的粒子都来自set_a，有些对应set_a中的相同的粒子；有些粒子对应set_a中权重相同的，但不是同一个的粒子

KLD采样|激光SLAMamcl和粒子滤波

根据KLD采样计算定位所需的粒子数量，不需要重采样则对粒子集进行排序，选出权重最大的cluster，其位姿为该时刻机器人的最优估计位姿。

// Compute the required number of samples, given that there are k bins
// with samples in them. This is taken directly from Fox et al.

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
int pf_resample_limit(pf_t *pf, int k)
{
  double a, b, c, x;
  int n;
  if (k <= 1)
    return pf->max_samples;
  a = 1;
  b = 2 / (9 * ((double) k - 1));
  // pop_z 为3
  c = sqrt(2 / (9 * ((double) k - 1))) * pf->pop_z;
  x = a - b + c;
  // pop_err 为0.01
  n = (int) ceil(   (k - 1) / (2 * pf->pop_err) * x * x * x   );

  if (n < pf->min_samples)
    return pf->min_samples;
  if (n > pf->max_samples)
    return pf->max_samples;
  
  return n;
}

粒子收敛，我们在rviz上看到的particlecloud话题，它的元素个数，粒子数其实是重采样之后的粒子集的粒子数set_b：

1
2
3
4
5
6
7
if(  !(++resample_count_ % resample_interval_)  )
{
  pf_update_resample(pf_);
  resampled = true;
}
pf_sample_set_t* set = pf_->sets + pf_->current_set;
ROS_DEBUG("Num samples: %d\n", set->sample_count);

开始为max_particles，开始滤波后，虽然rviz上粒子看上去收敛到一起，但是粒子数不一定只有那么少。

如果min_particles大于pf_resample_limit计算得到的粒子数，那么particlecloud对应得粒子数是min_particles；如果小于，就是后者。也就是取二者的最大值。 但这个函数返回值并不是单调增或单调

Matlab读文件和写文件|Matlab

读文件

txt文件里是这样的：

1
2
3
4
-3.35 2.332 
-3.18 1.876 
-3.10 2.577 
-3.51 2.991

可以这样画散点图，最后设置了坐标轴的范围：

1
2
3
4
5
6
clear all
close all
data=load('E:\data.txt');
scatter(data(:,1),data(:,2))
xlim([-6 4])
ylim([-4 10])

散点图.png

读点云文件画图：

1
2
3
4
5
6
7
8
9
10
11
A = load('cloud3d.dat'); %读入数据
%x,y,z轴坐标
x=A(:,1);
y=A(:,2);
z=A(:,3);
scatter3(x,y,z);    %散点图
[X,Y,Z]=griddata(x,y,z,linspace(min(x),max(x))',linspace(min(y),max(y)),'v4'); %构造坐标点
pcolor(X,Y,Z);
shading interp;        %伪彩色图
fcontourf(X,Y,Z);     %等高线图
figure,surf(X,Y,Z);    %三维曲面

写文件

1
2
3
4
5
6
7
8
fileID = fopen('F:\test.ply', 'w');
fprintf(fileID, 'ply\n');
fprintf(fileID, 'format ascii 1.0\n');
fprintf(fileID, 'property int path_id\n');
fprintf(fileID, 'property int group_id\n');
fprintf(fileID, 'end_header\n');
fprintf(fileID, '%f %f %f %d %d\n', [1,2,3,4,5]);
fclose(fileID);

打开文件是：

1
2
3
4
5
6
ply
format ascii 1.0
property int path_id
property int group_id
end_header
1.000000 2.000000 3.000000 4 5

pf_init 函数|激光SLAMamcl和粒子滤波

原型：void pf_init(pf_t *pf, pf_vector_t mean, pf_matrix_t cov)

pf是之前初始化的粒子滤波器对象，mean是amcl中的初始位姿[x,y,θ], cov是初始协方差

1
2
3
4
5
6
7
8
9
// 清空kd树
void pf_kdtree_clear(pf_kdtree_t *self)
{
  self->root = NULL;
  self->leaf_count = 0;
  self->node_count = 0;

  return;
}

调用 pf_cluster_stats的函数

pf_cluster_stats的调用关系.png
源码里没有用到pf_init_model，它完成的功能与pf_init基本是一样的，只是pf_init使用的是高斯模型，pf_init_model可以由用户提供模型。用户可以定义一个service客户端，发起global_localization服务，AMCL源码已经定义好了回调函数。

pf_init_model有三个参数，其中pf仍然是滤波器对象。init_fn是一个函数指针，用户需要提供一个生成随机样本的函数实现，该参数的作用就相当于pf_init中的高斯分布对象。参数init_data则是生成粒子的样本空间， 我们可以为之赋予地图数据。

pf_alloc函数|激光SLAMamcl和粒子滤波

先看函数流程图：pf_alloc.png

源码分析(二) handleMapMessage 函数|激光SLAMamcl和粒子滤波

调用关系.png
实际用的是requstMap这一支

准备工作

参数是const nav_msgs::OccupancyGrid& msg

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
boost::recursive_mutex::scoped_lock cfl(configuration_mutex_);
// 地图的宽高，分辨率
ROS_INFO("Received a %d X %d map @ %.3f m/pix\n",
         msg.info.width,
         msg.info.height,
         msg.info.resolution);
// 如果目前的全局坐标系不是map
if(msg.header.frame_id != global_frame_id_)
  ROS_WARN("Frame_id of map received:'%s' doesn't match global_frame_id:'%s'. This could cause issues with reading published topics",
           msg.header.frame_id.c_str(),
           global_frame_id_.c_str()  );

//free相应的指针
freeMapDependentMemory();
// Clear queued laser objects because they hold pointers to the existing map
lasers_.clear();
lasers_update_.clear();
frame_to_laser_.clear();

处理获得的地图

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
// AMCL所用的地图类型
typedef struct
{
  // Map origin; the map is a viewport onto a conceptual larger map.
  double origin_x, origin_y;
  
  double scale;  // Map scale (m/cell)

  // Map dimensions (number of cells)
  int size_x, size_y;
  
  // The map data, stored as a grid
  map_cell_t *cells;

  // Max distance at which we care about obstacles, for constructing
  // likelihood field
  double max_occ_dist;
} map_t;

转换为AMCL的地图信息

1
2
3
4
5
6
7
8
9
//转换成AMCL的标准地图，0->-1(不是障碍);100->+1（障碍）;else->0（不明）
map_ = convertMap(msg);  // 类型map_t

//将不是障碍的点的坐标保存下来, 构成地图栅格的空闲区域
free_space_indices.resize(0);
for(int i = 0; i < map_->size_x; i++)
  for(int j = 0; j < map_->size_y; j++)
    if(map_->cells[MAP_INDEX(map_,i,j)].occ_state == -1)
      free_space_indices.push_back(std::make_pair(i,j));

从参数服务器获取初始位姿及方差放到pf中

1
updatePoseFromServer();

其实就是rosparam把6个参数放到init_pose_和init_cov_两个数组中，供下面进一步赋值

创建粒子滤波器并初始化

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
pf_ = pf_alloc(min_particles_, max_particles_,
               alpha_slow_, alpha_fast_,
               (pf_init_model_fn_t)AmclNode::uniformPoseGenerator,
               (void *)map_);
pf_->pop_err = pf_err_;
pf_->pop_z = pf_z_;

pf_vector_t pf_init_pose_mean = pf_vector_zero();
pf_init_pose_mean.v[0] = init_pose_[0];
pf_init_pose_mean.v[1] = init_pose_[1];
pf_init_pose_mean.v[2] = init_pose_[2];
pf_matrix_t pf_init_pose_cov = pf_matrix_zero();
pf_init_pose_cov.m[0][0] = init_cov_[0];
pf_init_pose_cov.m[1][1] = init_cov_[1];
pf_init_pose_cov.m[2][2] = init_cov_[2];

pf_init(pf_, pf_init_pose_mean, pf_init_pose_cov);
pf_init_ = false;

pf_alloc和pf_init是两个重要函数。 pf_alloc只在这里调用了一次，pf_init在这里和applyInitialPose里调用

专门分析pf_alloc

专门分析pf_init

定义里程计与激光雷达并初始化数据

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
delete odom_;
odom_ = new AMCLOdom();
ROS_ASSERT(odom_);
// 设置里程计参数
odom_->SetModel( odom_model_type_, alpha1_, alpha2_, alpha3_, alpha4_, alpha5_ );

// Laser
delete laser_;
// max_beams_就是参数laser_max_beams
// map_ 是AMCL所用的地图格式, 在上面获得
laser_ = new AMCLLaser(max_beams_, map_);
ROS_ASSERT(laser_);
// 4个形参为似然域模型的参数
laser_->SetModelLikelihoodField(z_hit_, z_rand_, sigma_hit_,
                                  laser_likelihood_max_dist_);

SetModelLikelihoodField函数就是使用前三个形参用于赋值, 然后调用 map_update_cspace(this->map, laser_likelihood_max_dist_);， 这个是似然域模型中最复杂的函数
它的专门分析在这里

参数laser_likelihood_max_dist默认为2，是地图上做障碍物膨胀的最大距离，用于似然域模型

这几个参数没有用在似然域模型: laser_z_short, laser_z_max, laser_lambda_short, laser_likelihood_max_dist

applyInitialPose()

// In case the initial pose message arrived before the first map,
// try to apply the initial pose now that the map has arrived.
可能初始位姿的信息比第一个map先到达，map到达后apply初始位姿

1
2
3
4
5
6
7
8
boost::recursive_mutex::scoped_lock cfl(configuration_mutex_);
if( initial_pose_hyp_ != NULL && map_ != NULL ) {
  pf_init(pf_, initial_pose_hyp_->pf_pose_mean, initial_pose_hyp_->pf_pose_cov);
  pf_init_ = false;

  delete initial_pose_hyp_;
  initial_pose_hyp_ = NULL;
}

函数指针|C++C++ 基础

函数名就是个指针,它指向函数代码在内存中的首地址.

1
2
3
4
5
6
7
8
9
typedef double (*FuncType) (int data);
double func(int num)
{
    return num;
}

// 调用
FuncType ptr = func;
cout << ptr(11) <<endl;

第一行定义了一种函数指针,类型为FuncType,它指向的函数返回为 double, 形参为一个int. 然后定义了一个名为func的函数.

FuncType ptr = func;是声明一个指针, 类型为 FuncType, 指向func函数. 接下来就可以拿ptr当函数用了

1
2
3
4
5
6
7
8
void test(int id, FuncType foo)
{
    cout << foo(110) << endl;
    cout << "id: "<<id<<endl;
}

//调用
test(1, static_cast<FuncType>(func) );

定义一个函数test, 它第二个形参是类型为FuncType的函数指针. 调用时, 最好对第二个形参做转换, 这里用static_cast再合适不过.

源码分析(三)laserReceived回调函数|激光SLAMamcl和粒子滤波

这是amcl_node.cpp中最重要的函数，蒙特卡洛算法都在它里面，我们前面分析几个算法时已经涉及到了它的几个部分，下面再串起来看一遍

laserReceived 回调函数

Amcl的业务逻辑总体就是在一个四五百行的巨大函数laserReceived中实现的，它就是刚刚我们提到的消息过滤器laser_scan_filter_的回调函数。

  • 用变量lastlaser_received_ts记录下当前的系统时间， 它用于判定是否长时间未接收到雷达数据。此外如果没有地图对象，将直接退出

  • AmclNode借助一些vector和map的容器，来支持多传感器。它使用lasers记录下当前构建的雷达对象，lasers_update标记雷达的更新状态， 并通过一个string到int的map建立其雷达坐标ID到雷达对象在lasers_中的对应关系。

  • 通过在frameto_laser中查找雷达的坐标ID，如果之前没有收到该雷达的消息，将新建一个对象保存在lasers中，并相应的在lasers_update中添加对应更新状态， 建立映射关系。否则，我们就直接通过frameto_laser获取雷达对象的索引。

  • 利用tf求出雷达坐标系的原点在base_link下的坐标，然后调用SetLaserPose设置雷达的位姿，angle暂时为0

  • getOdomPose获取收到雷达数据时的里程计位姿

  • if(pf_init_)的内容一定执行，如果里程计的数据显示机器人已经发生了明显的位移或者旋转，就标记所有的雷达更新标记为true

  • 根据刚刚更新标识，我们调用里程计对象的UpdateAction接口完成运动更新，也就是机器人移动后执行else if(pf_init_ && lasers_update_[laser_index])的内容

  • 机器人移动，执行if(lasers_update_[laser_index])的内容，根据激光的扫描数据更新滤波器。首先构建AMCLLaserData对象，并指定传感器对象和量程，然后将接收到的传感器数据拷贝到ldata对象中，最后通过激光传感器对象的UpdateSensor接口完成粒子滤波器的测量更新。这里发布了particlecloud话题

  • 机器人移动，则执行if(resampled || force_publication)的内容，发布了amcl_pose话题

AmclNode::getOdomPose维护odom—>base_link的变换

map—>odom的tf变换在1410行左右：

1
2
3
4
5
6
7
ros::Time transform_expiration = (laser_scan->header.stamp + transform_tolerance_);
//  构造一个StampedTransform 需要tf::Transform ,stamp_,father_frame,child_frame 参数
tf::StampedTransform tmp_tf_stamped(latest_tf_.inverse(),// odom 到map的转换矩阵
                                    transform_expiration,
                                    global_frame_id_, odom_frame_id_);

this->tfb_->sendTransform(tmp_tf_stamped);//发布 odom 到map的转换矩阵转换