gmapping算法核心部分

processScan函数

参考：https://blog.csdn.net/CV_Autobot/article/details/131058981

drawFromMotion:根据运动模型更新粒子位姿
scanMatch:进行扫描匹配
resample:重采样

逐步分解并详细解释代码

1. 获取当前扫描的相对位姿

OrientedPoint relPose = reading.getPose(); // 获取当前扫描的相对位姿
if (!m_count){
    // 如果是第一次处理扫描
  m_lastPartPose = m_odoPose = relPose; // 初始化上一次粒子位姿和里程计位姿
}

• 从reading中获取当前扫描的相对位姿relPose。
• 如果是第一次处理扫描（m_count为0），则初始化上一次粒子位姿m_lastPartPose和里程计位姿m_odoPose为relPose。

2. 使用运动模型更新所有粒子

for (ParticleVector::iterator it = m_particles.begin(); it != m_particles.end(); it++){
   
  OrientedPoint& pose(it->pose);
  pose = m_motionModel.drawFromMotion(it->pose, relPose, m_odoPose); // 根据运动模型更新粒子位姿
}

• 遍历所有粒子，使用运动模型m_motionModel更新每个粒子的位姿。

3. 更新输出文件

if (m_outputStream.is_open()){
   
  m_outputStream << setiosflags(ios::fixed) << setprecision(6);
  m_outputStream << "ODOM ";
  m_outputStream << setiosflags(ios::fixed) << setprecision(3) << m_odoPose.x << " " << m_odoPose.y << " ";
  m_outputStream << setiosflags(ios::fixed) << setprecision(6) << m_odoPose.theta << " ";
  m_outputStream << reading.getTime();
  m_outputStream << endl;
}
if (m_outputStream.is_open()){
   
  m_outputStream << setiosflags(ios::fixed) << setprecision(6);
  m_outputStream << "ODO_UPDATE " << m_particles.size() << " ";
  for (ParticleVector::iterator it = m_particles.begin(); it != m_particles.end(); it++){
   
    OrientedPoint& pose(it->pose);
    m_outputStream << setiosflags(ios::fixed) << setprecision(3) << pose.x << " " << pose.y << " ";
    m_outputStream << setiosflags(ios::fixed) << setprecision(6) << pose.theta << " " << it-> weight << " ";
  }
  m_outputStream << reading.getTime();
  m_outputStream << endl;
}

• 如果输出流m_outputStream是打开的，则将里程计信息和粒子更新信息写入输出文件。

4. 调用回调函数

onOdometryUpdate();

• 调用里程计更新回调函数onOdometryUpdate()。

5. 累积机器人的平移和旋转

OrientedPoint move = relPose - m_odoPose;
move.theta = atan2(sin(move.theta), cos(move.theta));
m_linearDistance += sqrt(move * move);
m_angularDistance += fabs(move.theta);

• 计算机器人从上一次里程计位姿到当前位姿的平移和旋转。
• 累积线性距离m_linearDistance和角度距离m_angularDistance。

6. 检查机器人是否跳跃

if (m_linearDistance > m_distanceThresholdCheck){
   
  cerr << "***********************************************************************" << endl;
  cerr << "********** Error: m_distanceThresholdCheck overridden!!!! *************" << endl;
  cerr << "m_distanceThresholdCheck=" << m_distanceThresholdCheck << endl;
  cerr << "Old Odometry Pose= " << m_odoPose.x << " " << m_odoPose.y 
       << " " << m_odoPose.theta << endl;
  cerr << "New Odometry Pose (reported from observation)= " << relPose.x << " " << relPose.y 
       << " " << relPose.theta << endl;
  cerr << "***********************************************************************" << endl;
  cerr << "** The Odometry has a big jump here. This is probably a bug in the   **" << endl;
  cerr << "** odometry/laser input. We continue now, but the result is probably **" << endl;
  cerr << "** crap or can lead to a core dump since the map doesn't fit.... C&G **" << endl;
  cerr << "***********************************************************************" << endl;
}

• 如果线性距离超过阈值m_distanceThresholdCheck，则输出警告信息。

7. 更新里程计位姿

m_odoPose = relPose; // 更新里程计位姿

• 更新里程计位姿m_odoPose为当前相对位姿relPose。

8. 处理扫描

bool processed = false;
if (!m_count 
    || m_linearDistance > m_linearThresholdDistance 
    || m_angularDistance > m_angularThresholdDistance){
   
  if (m_outputStream.is_open()){
   
    m_outputStream << setiosflags(ios::fixed) << setprecision(6);
    m_outputStream << "FRAME " <<  m_readingCount;
    m_outputStream << " " << m_linearDistance;
    m_outputStream << " " << m_angularDistance << endl;
  }
  if (m_infoStream)
    m_infoStream << "update frame " <<  m_readingCount << endl
                 << "update ld=" << m_linearDistance << " ad=" << m_angularDistance << endl;
  cerr << "Laser Pose= " << reading.getPose().x << " " << reading.getPose().y 
       << " " << reading.getPose().theta << endl;
  assert(reading.size() == m_beams);
  double * plainReading = new double[m_beams];
  for(unsigned int i = 0; i < m_beams; i++){
   
    plainReading[i] = reading[i];
  }
  m_infoStream << "m_count " << m_count << endl;
  if (m_count > 0){
   
    scanMatch(plainReading); // 进行扫描匹配
    if (m_outputStream.is_open()){
   
      m_outputStream << "LASER_READING " << reading.size() << " ";
      m_outputStream << setiosflags(ios::fixed) << setprecision(2);
      for (RangeReading::const_iterator b = reading.begin(); b != reading.end(); b++){
   
        m_outputStream << *b << " ";
      }
      OrientedPoint p = reading.getPose();
      m_outputStream << setiosflags(ios::fixed) << setprecision(6);
      m_outputStream << p.x << " " << p.y << " " << p.theta << " " << reading.getTime() << endl;
      m_outputStream << "SM_UPDATE " << m_particles.size() << " ";
      for (ParticleVector::const_iterator it = m_particles.begin(); it != m_particles.end(); it++){
   
        const OrientedPoint& pose = it->pose;
        m_outputStream << setiosflags(ios::fixed) << setprecision(3) <<  pose.x << " " << pose.y << " ";
        m_outputStream << setiosflags(ios::fixed) << setprecision(6) <<  pose.theta << " " << it