gmapping算法核心部分

news2025/1/4 18:27:56

processScan函数在这里插入图片描述

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

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-> weight << " ";
      }
      m_outputStream << endl;
    }
    onScanmatchUpdate(); // 调用扫描匹配更新回调
    updateTreeWeights(false); // 更新树权重
    if (m_infoStream){
      m_infoStream << "neff= " << m_neff  << endl;
    }
    if (m_outputStream.is_open()){
      m_outputStream << setiosflags(ios::fixed) << setprecision(6);
      m_outputStream << "NEFF " << m_neff << endl;
    }
    resample(plainReading, adaptParticles); // 重采样
  } else {
    m_infoStream << "Registering First Scan" << endl;
    for (ParticleVector::iterator it = m_particles.begin(); it != m_particles.end(); it++){    
      m_matcher.invalidateActiveArea();
      m_matcher.computeActiveArea(it->map, it->pose, plainReading);
      m_matcher.registerScan(it->map, it->pose, plainReading);
      TNode* node = new TNode(it->pose, 0., it->node,  0);
      node->reading = 0;
      it->node = node;
    }
  }
  updateTreeWeights(false); // 更新树权重
  delete [] plainReading; // 释放内存
  m_lastPartPose = m_odoPose; // 更新上一次粒子位姿
  m_linearDistance = 0; // 重置线性距离
  m_angularDistance = 0; // 重置角度距离
  m_count++; // 增加计数
  processed = true; // 设置处理标志
  for (ParticleVector::iterator it = m_particles.begin(); it != m_particles.end(); it++){
    it->previousPose = it->pose;
  }
}
if (m_outputStream.is_open())
  m_outputStream << flush;
m_readingCount++; // 增加读取计数
return processed; // 返回处理标志
  • 只有在机器人行驶了一定距离后才处理扫描。
  • 将读取转换为扫描匹配器可用的形式。
  • 如果是第一次处理扫描，则注册第一帧扫描。
  • 进行扫描匹配，更新树权重，重采样。
  • 更新上一次粒子位姿，重置线性距离和角度距离，增加计数。
  • 返回处理标志。

总结代码

主要功能
  • 初始化位姿：如果是第一次处理扫描，初始化上一次粒子位姿和里程计位姿。
  • 更新粒子位姿：使用运动模型更新所有粒子的位姿。
  • 输出信息：将里程计信息和粒子更新信息写入输出文件。
  • 累积距离：累积机器人的平移和旋转距离。
  • 检查跳跃：如果机器人跳跃则发出警告。
  • 处理扫描：只有在机器人行驶了一定距离后才处理扫描，进行扫描匹配、更新树权重、重采样等操作。
  • 返回处理标志：返回是否处理了当前扫描的标志。

该代码的主要功能是处理激光扫描数据，更新粒子滤波器中的粒子位姿，并根据机器人的运动情况进行扫描匹配和重采样，以实现SLAM（同时定位与地图构建）。

bool GridSlamProcessor::processScan(const RangeReading & reading, int adaptParticles){
    
    /** 从读取中检索位置，并计算里程计 */
    OrientedPoint relPose = reading.getPose(); // 获取当前扫描的相对位姿
    if (!m_count){ // 如果是第一次处理扫描
      m_lastPartPose = m_odoPose = relPose; // 初始化上一次粒子位姿和里程计位姿
    }
    
    // 将读取的状态写入并使用运动模型更新所有粒子
    for (ParticleVector::iterator it = m_particles.begin(); it != m_particles.end(); it++){
      OrientedPoint& pose(it->pose);
       // it->pose：当前粒子位姿
      // relPose：当前扫描的相对位姿
      // m_odoPose：上一次粒子位姿
      pose = m_motionModel.drawFromMotion(it->pose, relPose, m_odoPose); // 根据运动模型更新粒子位姿
    }

    // 更新输出文件
    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;
    }
    
    // 调用回调函数
    onOdometryUpdate();
    

    // 累积机器人的平移和旋转
    OrientedPoint move = relPose - m_odoPose;
    move.theta = atan2(sin(move.theta), cos(move.theta));
    m_linearDistance += sqrt(move * move);
    m_angularDistance += fabs(move.theta);
    
    // 如果机器人跳跃则发出警告
    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_odoPose = relPose; // 更新里程计位姿
    
    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-> weight << " ";
          }
          m_outputStream << endl;
        }
        onScanmatchUpdate(); // 调用扫描匹配更新回调
        
        updateTreeWeights(false); // 更新树权重
                
        if (m_infoStream){
          m_infoStream << "neff= " << m_neff  << endl;
        }
        if (m_outputStream.is_open()){
          m_outputStream << setiosflags(ios::fixed) << setprecision(6);
          m_outputStream << "NEFF " << m_neff << endl;
        }
        resample(plainReading, adaptParticles); // 重采样
        
      } else {
        m_infoStream << "Registering First Scan" << endl;
        for (ParticleVector::iterator it = m_particles.begin(); it != m_particles.end(); it++){    
          m_matcher.invalidateActiveArea();
          m_matcher.computeActiveArea(it->map, it->pose, plainReading);
          m_matcher.registerScan(it->map, it->pose, plainReading);
          
          // cyr: 不再需要，粒子一开始就指向根节点！
          TNode* node = new TNode(it->pose, 0., it->node,  0);
          node->reading = 0;
          it->node = node;
          
        }
      }
      // 更新树权重
      updateTreeWeights(false);
      
      delete [] plainReading; // 释放内存
      m_lastPartPose = m_odoPose; // 更新上一次粒子位姿
      m_linearDistance = 0; // 重置线性距离
      m_angularDistance = 0; // 重置角度距离
      m_count++; // 增加计数
      processed = true; // 设置处理标志
      
      // 为下一次迭代准备
      for (ParticleVector::iterator it = m_particles.begin(); it != m_particles.end(); it++){
        it->previousPose = it->pose;
      }
      
    }
    if (m_outputStream.is_open())
      m_outputStream << flush;
    m_readingCount++; // 增加读取计数
    return processed; // 返回处理标志
}

drawFromMotion函数

  1. 计算位移变化：

    OrientedPoint delta = absoluteDifference(pnew, pold);

    假设 pnew 和 pold 分别为：

    pnew = (x_new, y_new, theta_new)
pold = (x_old, y_old, theta_old)

    那么 delta 计算如下：

    delta.x = x_new - x_old
delta.y = y_new - y_old
delta.theta = theta_new - theta_old

  2. 添加高斯噪声：

    • x坐标：

      noisypoint.x += sampleGaussian(srr * fabs(delta.x) + str * fabs(delta.theta) + sxy * fabs(delta.y));

      假设 srr, str, sxy 分别为 sigma_rr, sigma_tr, sigma_xy，那么噪声计算如下：

      noise_x = sampleGaussian(sigma_rr * |delta.x| + sigma_tr * |delta.theta| + sigma_xy * |delta.y|)
noisypoint.x = delta.x + noise_x

    • y坐标：

      noisypoint.y += sampleGaussian(srr * fabs(delta.y) + str * fabs(delta.theta) + sxy * fabs(delta.x));

      噪声计算如下：

      noise_y = sampleGaussian(sigma_rr * |delta.y| + sigma_tr * |delta.theta| + sigma_xy * |delta.x|)
noisypoint.y = delta.y + noise_y

    • 角度：

      noisypoint.theta += sampleGaussian(stt * fabs(delta.theta) + srt * sqrt(delta.x * delta.x + delta.y * delta.y));

      假设 stt, srt 分别为 sigma_tt, sigma_rt，那么噪声计算如下：

      noise_theta = sampleGaussian(sigma_tt * |delta.theta| + sigma_rt * sqrt(delta.x^2 + delta.y^2))
noisypoint.theta = delta.theta + noise_theta

  3. 确保角度在正确范围内：

    noisypoint.theta = fmod(noisypoint.theta, 2 * M_PI);
if (noisypoint.theta > M_PI)
    noisypoint.theta -= 2 * M_PI;

    这一步确保角度在 -π 到 π 之间。

  4. 计算最终位点：

    return absoluteSum(p, noisypoint);

    假设 p 为 (x_p, y_p, theta_p)，那么最终位点计算如下：

    final_point.x = x_p + noisypoint.x
final_point.y = y_p + noisypoint.y
final_point.theta = theta_p + noisypoint.theta

通过这些步骤，代码实现了在给定位移变化的基础上添加高斯噪声，并确保角度在合理范围内，最终得到一个带有噪声的位点。

OrientedPoint 
MotionModel::drawFromMotion(const OrientedPoint& p, const OrientedPoint& pnew, const OrientedPoint& pold) const{
    // 定义一个与位点变化无关的噪声参数
    double sxy=0.3*srr;
    // 计算新旧位点之间的变化量
    OrientedPoint delta=absoluteDifference(pnew, pold);
    // 创建一个基于变化量的噪声位点
    OrientedPoint noisypoint(delta);
    // 为x坐标添加高斯噪声
    noisypoint.x+=sampleGaussian(srr*fabs(delta.x)+str*fabs(delta.theta)+sxy*fabs(delta.y));
    // 为y坐标添加高斯噪声
    noisypoint.y+=sampleGaussian(srr*fabs(delta.y)+str*fabs(delta.theta)+sxy*fabs(delta.x));
    // 为角度添加高斯噪声
    noisypoint.theta+=sampleGaussian(stt*fabs(delta.theta)+srt*sqrt(delta.x*delta.x+delta.y*delta.y));
    // 确保角度在正确的范围内
    noisypoint.theta=fmod(noisypoint.theta, 2*M_PI);
    if (noisypoint.theta>M_PI)
        noisypoint.theta-=2*M_PI;
    // 将噪声位点与原始位点相加，得到最终结果
    return absoluteSum(p,noisypoint);
}

scanMatch

1. 初始化得分总和
double sumScore = 0;

初始化一个变量 sumScore 用于存储所有粒子的扫描匹配得分总和。

2. 遍历所有粒子
for (ParticleVector::iterator it = m_particles.begin(); it != m_particles.end(); it++) {

遍历 m_particles 容器中的每一个粒子。

3. 存储校正后的位姿
OrientedPoint corrected;

声明一个 OrientedPoint 类型的变量 corrected，用于存储优化后的粒子位姿。

4. 存储匹配得分、似然和分数
double score, l, s;

声明三个 double 类型的变量 score、l 和 s，分别用于存储匹配得分、似然和分数。

5. 优化校正后的位姿，并返回匹配得分
score = m_matcher.optimize(corrected, it->map, it->pose, plainReading);

调用 m_matcher.optimize 方法，优化当前粒子的位姿，并将优化后的位姿存储在 corrected 中，同时返回匹配得分 score。

6. 如果匹配得分高于最小得分阈值，则更新粒子的位姿
if (score > m_minimumScore) {
    it->pose = corrected;
} else {
    // 匹配失败时，使用里程计数据，并记录相关信息
    if (m_infoStream) {
        m_infoStream << "Scan Matching Failed, using odometry. Likelihood=" << l << std::endl;
        m_infoStream << "lp:" << m_lastPartPose.x << " " << m_lastPartPose.y << " " << m_lastPartPose.theta << std::endl;
        m_infoStream << "op:" << m_odoPose.x << " " << m_odoPose.y << " " << m_odoPose.theta << std::endl;
    }
}

如果匹配得分 score 高于预设的最小得分阈值 m_minimumScore，则更新粒子的位姿为优化后的位姿 corrected。否则，记录扫描匹配失败的信息，并使用里程计数据。

7. 计算粒子的似然和分数
m_matcher.likelihoodAndScore(s, l, it->map, it->pose, plainReading);

调用 m_matcher.likelihoodAndScore 方法，计算当前粒子的似然 l 和分数 s。

8. 更新得分总和和粒子的权重
sumScore += score;
it->weight += l;
it->weightSum += l;

将当前粒子的匹配得分 score 累加到 sumScore 中，并更新粒子的权重 weight 和权重总和 weightSum。

9. 为选择性复制活跃区域做准备，通过分离将被更新的区域
m_matcher.invalidateActiveArea();
m_matcher.computeActiveArea(it->map, it->pose, plainReading);

调用 m_matcher.invalidateActiveArea 和 m_matcher.computeActiveArea 方法，为选择性复制活跃区域做准备。

10. 记录平均扫描匹配得分
if (m_infoStream)
    m_infoStream << "Average Scan Matching Score=" << sumScore / m_particles.size() << std::endl;

如果 m_infoStream 存在，记录并输出平均扫描匹配得分。

inline void GridSlamProcessor::scanMatch(const double* plainReading){
  // 初始化得分总和
  double sumScore=0;
  // 遍历所有粒子
  for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
    // 存储校正后的位姿
    OrientedPoint corrected;
    // 存储匹配得分、似然和分数
    double score, l, s;
    // 优化校正后的位姿，并返回匹配得分
    score=m_matcher.optimize(corrected, it->map, it->pose, plainReading);
    // 如果匹配得分高于最小得分阈值，则更新粒子的位姿
    if (score>m_minimumScore){
      it->pose=corrected;
    } else {
      // 匹配失败时，使用里程计数据，并记录相关信息
      if (m_infoStream){
        m_infoStream << "Scan Matching Failed, using odometry. Likelihood=" << l <<std::endl;
        m_infoStream << "lp:" << m_lastPartPose.x << " "  << m_lastPartPose.y << " "<< m_lastPartPose.theta <<std::endl;
        m_infoStream << "op:" << m_odoPose.x << " " << m_odoPose.y << " "<< m_odoPose.theta <<std::endl;
      }
    }

    // 计算粒子的似然和分数
    m_matcher.likelihoodAndScore(s, l, it->map, it->pose, plainReading);
    // 更新得分总和和粒子的权重
    sumScore+=score;
    it->weight+=l;
    it->weightSum+=l;

    // 为选择性复制活跃区域做准备，通过分离将被更新的区域
    m_matcher.invalidateActiveArea();
    m_matcher.computeActiveArea(it->map, it->pose, plainReading);
  }
  // 记录平均扫描匹配得分
  if (m_infoStream)
    m_infoStream << "Average Scan Matching Score=" << sumScore/m_particles.size() << std::endl;	
}

resample函数

1. 初始化
bool hasResampled = false;

标记是否已经进行了重采样，初始值为false。

2. 存储旧一代节点
TNodeVector oldGeneration;
for (unsigned int i=0; i<m_particles.size(); i++){
  oldGeneration.push_back(m_particles[i].node);
}

遍历所有粒子，将每个粒子的节点存储到oldGeneration中，以便后续使用。

3. 判断是否需要重采样
if (m_neff < m_resampleThreshold * m_particles.size()){

如果有效粒子数（m_neff）小于重采样阈值（m_resampleThreshold * m_particles.size()），则进行重采样。

4. 输出重采样信息
if (m_infoStream)
  m_infoStream << "*************RESAMPLE***************" << std::endl;

如果信息流（m_infoStream）存在，则输出重采样信息。

5. 创建并使用均匀重采样器
uniform_resampler<double, double> resampler;
m_indexes = resampler.resampleIndexes(m_weights, adaptSize);

创建均匀重采样器，并使用它对权重进行重采样，得到重采样索引m_indexes。

6. 记录重采样信息
if (m_outputStream.is_open()){
  m_outputStream << "RESAMPLE " << m_indexes.size() << " ";
  for (std::vector<unsigned int>::const_iterator it = m_indexes.begin(); it != m_indexes.end(); it++){
    m_outputStream << *it << " ";
  }
  m_outputStream << std::endl;
}

如果输出流（m_outputStream）打开，则记录重采样信息。

7. 执行重采样更新
onResampleUpdate();

调用重采样更新函数。

8. 开始构建粒子树
ParticleVector temp;
unsigned int j = 0;
std::vector<unsigned int> deletedParticles;

初始化临时粒子向量temp，索引j和被删除粒子索引向量deletedParticles。

9. 遍历重采样索引，构建新的粒子树
for (unsigned int i = 0; i < m_indexes.size(); i++){
  while (j < m_indexes[i]){
    deletedParticles.push_back(j);
    j++;
  }
  if (j == m_indexes[i])
    j++;
  Particle & p = m_particles[m_indexes[i]];
  TNode* node = 0;
  TNode* oldNode = oldGeneration[m_indexes[i]];
  node = new TNode(p.pose, 0, oldNode, 0);
  node->reading = 0;
  
  temp.push_back(p);
  temp.back().node = node;
  temp.back().previousIndex = m_indexes[i];
}
while (j < m_indexes.size()){
  deletedParticles.push_back(j);
  j++;
}

遍历重采样索引，构建新的粒子树，并记录被删除的粒子索引。

10. 删除不再需要的粒子节点
std::cerr << "Deleting Nodes:";
for (unsigned int i = 0; i < deletedParticles.size(); i++){
  std::cerr << " " << deletedParticles[i];
  delete m_particles[deletedParticles[i]].node;
  m_particles[deletedParticles[i]].node = 0;
}
std::cerr << " Done" << std::endl;

删除不再需要的粒子节点。

11. 复制临时粒子，并重新注册扫描
std::cerr << "Deleting old particles...";
m_particles.clear();
std::cerr << "Done" << std::endl;
std::cerr << "Copying Particles and Registering scans...";
for (ParticleVector::iterator it = temp.begin(); it != temp.end(); it++){
  it->setWeight(0);
  m_matcher.invalidateActiveArea();
  m_matcher.registerScan(it->map, it->pose, plainReading);
  m_particles.push_back(*it);
}
std::cerr << " Done" << std::endl;
hasResampled = true;

清空旧粒子，复制临时粒子，并重新注册扫描，标记已重采样。

12. 如果不需要重采样，只需注册扫描
int index = 0;
std::cerr << "Registering Scans:";
TNodeVector::iterator node_it = oldGeneration.begin();
for (ParticleVector::iterator it = m_particles.begin(); it != m_particles.end(); it++){
  TNode* node = 0;
  node = new TNode(it->pose, 0.0, *node_it, 0);
  node->reading = 0;
  it->node = node;
  m_matcher.invalidateActiveArea();
  m_matcher.registerScan(it->map, it->pose, plainReading);
  it->previousIndex = index;
  index++;
  node_it++;
}
std::cerr << "Done" << std::endl;

如果不需要重采样，只需注册扫描。

13. 返回是否进行了重采样
return hasResampled;

返回是否进行了重采样。

总结

该代码的主要功能是实现粒子滤波器中的重采样步骤。具体来说，它首先判断是否需要进行重采样，如果需要，则使用均匀重采样器对粒子进行重采样，构建新的粒子树，并删除不再需要的粒子节点。如果不需要重采样，则只需注册扫描。最终返回是否进行了重采样。

inline bool GridSlamProcessor::resample(const double* plainReading, int adaptSize, const RangeReading* ){
  
  // 标记是否已经重采样
  bool hasResampled = false;
  
  // 存储旧一代节点
  TNodeVector oldGeneration;
  for (unsigned int i=0; i<m_particles.size(); i++){
    oldGeneration.push_back(m_particles[i].node);
  }
  
  // 如果有效粒子数小于重采样阈值
  if (m_neff<m_resampleThreshold*m_particles.size()){		
    
    // 输出重采样信息
    if (m_infoStream)
      m_infoStream  << "*************RESAMPLE***************" << std::endl;
    
    // 创建并使用均匀重采样器
    uniform_resampler<double, double> resampler;
    m_indexes=resampler.resampleIndexes(m_weights, adaptSize);
    
    // 记录重采样信息
    if (m_outputStream.is_open()){
      m_outputStream << "RESAMPLE "<< m_indexes.size() << " ";
      for (std::vector<unsigned int>::const_iterator it=m_indexes.begin(); it!=m_indexes.end(); it++){
	m_outputStream << *it <<  " ";
      }
      m_outputStream << std::endl;
    }
    
    // 执行重采样更新
    onResampleUpdate();
    // 开始构建粒子树
    ParticleVector temp;
    unsigned int j=0;
    std::vector<unsigned int> deletedParticles;  		// 存储被重采样移除的粒子索引，以便删除它们。
    
    // 遍历重采样索引，构建新的粒子树
    for (unsigned int i=0; i<m_indexes.size(); i++){
      while(j<m_indexes[i]){
	deletedParticles.push_back(j);
	j++;
			}
      if (j==m_indexes[i])
	j++;
      Particle & p=m_particles[m_indexes[i]];
      TNode* node=0;
      TNode* oldNode=oldGeneration[m_indexes[i]];
      node=new	TNode(p.pose, 0, oldNode, 0);
      node->reading=0;
      
      temp.push_back(p);
      temp.back().node=node;
      temp.back().previousIndex=m_indexes[i];
    }
    while(j<m_indexes.size()){
      deletedParticles.push_back(j);
      j++;
    }
    // 删除不再需要的粒子节点
    std::cerr <<  "Deleting Nodes:";
    for (unsigned int i=0; i<deletedParticles.size(); i++){
      std::cerr <<" " << deletedParticles[i];
      delete m_particles[deletedParticles[i]].node;
      m_particles[deletedParticles[i]].node=0;
    }
    std::cerr  << " Done" <<std::endl;
    
    // 结束构建粒子树
    std::cerr << "Deleting old particles..." ;
    m_particles.clear();
    std::cerr << "Done" << std::endl;
    std::cerr << "Copying Particles and  Registering  scans...";
    // 复制临时粒子，并重新注册扫描
    for (ParticleVector::iterator it=temp.begin(); it!=temp.end(); it++){
      it->setWeight(0);
      m_matcher.invalidateActiveArea();
      m_matcher.registerScan(it->map, it->pose, plainReading);
      m_particles.push_back(*it);
    }
    std::cerr  << " Done" <<std::endl;
    hasResampled = true;
  } else {
    // 如果不需要重采样，只需注册扫描
    int index=0;
    std::cerr << "Registering Scans:";
    TNodeVector::iterator node_it=oldGeneration.begin();
    for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
      // 在粒子树中创建新节点，并将其添加到旧树中
      TNode* node=0;
      node=new TNode(it->pose, 0.0, *node_it, 0);
      
      node->reading=0;
      it->node=node;

      // 结束构建粒子树
      m_matcher.invalidateActiveArea();
      m_matcher.registerScan(it->map, it->pose, plainReading);
      it->previousIndex=index;
      index++;
      node_it++;
    }
    std::cerr  << "Done" <<std::endl;
    
  }
  // 结束构建粒子树
  
  // 返回是否进行了重采样
  return hasResampled;
}

下一篇：scanMatch函数核心部分

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.coloradmin.cn/o/2051958.html

如若内容造成侵权/违法违规/事实不符，请联系多彩编程网进行投诉反馈，一经查实，立即删除！

相关文章

舜宇光学科技社招校招入职测评：商业推理测验真题汇总、答题要求、高分技巧

舜宇光学科技社招校招入职测评：商业推理测验真题汇总、答题要求、高分技巧

舜宇光学科技&#xff08;集团&#xff09;有限公司&#xff0c;成立于1984年&#xff0c;是全球领先的综合光学零件及产品制造商。2007年在香港联交所主板上市&#xff0c;股票代码2382.HK。公司专注于光学产品的设计、研发、生产及销售&#xff0c;产品广泛应用于手机、汽车、…
阅读更多...
BEM架构

BEM架构

视频 总结&#xff1a; BEM架构&#xff1a;一个命名类的规范而已&#xff0c;说白了就是如何给类起名字使用sass的目的&#xff1a;在<style>中模块化的使用类名&#xff0c;同时减少代码数量 1、 BEM架构 &#xff08;通义灵码查询结果&#xff09; BEM (Block Ele…
阅读更多...
【hot100篇-python刷题记录】【和为 K 的子数组】

【hot100篇-python刷题记录】【和为 K 的子数组】

R5-子串篇 目录 思路&#xff1a; 优化&#xff1a; tip: 代码&#xff1a; 结果&#xff1a; ps: 思路&#xff1a; 滑动&#xff0c;应该可以使用滑动窗口来解题。 貌似前缀和也可以&#xff0c;left&#xff0c;right两个指针&#xff0c;right的前缀和-left的前缀…
阅读更多...
【学习笔记】printf中%m的含义

【学习笔记】printf中%m的含义

【学习笔记】printf中%m的含义 在有些代码中会看到如下的写法&#xff1a; printf("%m\n");printf中使用了%m来打印输出&#xff0c;那么%m又是什么意思呢&#xff1f; 其实%m 并不是在所有的 printf 实现中都通用或标准化的选项&#xff0c;而是在某些特定的编程语…
阅读更多...
vue的markdown编辑器插件比对

vue的markdown编辑器插件比对

vue的markdown编辑器插件比对 文章说明md-editor-v3的使用及效果展示vditor的使用及效果展示 文章说明 文章比对 md-editor-v3、vditor 这两个插件的使用及效果体验 md-editor-v3的使用及效果展示 安装 npm install md-editor-v3使用 <script setup> import {reactive} f…
阅读更多...
图神经网络（Graph Neural Networks）是什么？

图神经网络（Graph Neural Networks）是什么？

图神经网络&#xff08;Graph Neural Networks&#xff09;是什么&#xff1f; 引言 在数据科学和机器学习的广阔领域中&#xff0c;图结构数据以其独特的复杂性和丰富性成为了一个重要的研究方向。从社交网络中的用户关系&#xff0c;到生物信息学中的蛋白质交互网络&#x…
阅读更多...
跨进程通信使用 Zenoh中间件 进行高效数据传输的测试和分析

跨进程通信使用 Zenoh中间件 进行高效数据传输的测试和分析

文章目录 1. 引言2. Zenoh C 使用指南2.1 安装 Zenoh C 库2.2 编写基本的 Zenoh C 程序订阅示例发布示例 2.3 编译和运行程序 3. Zenoh 与 ROS2 集成3.1 安装 Zenoh3.2 安装 ROS2 的 Zenoh RMW 实现3.3 设置 RMW 实现为 Zenoh3.4 验证配置 4. 编写基于 Zenoh 的 ROS2 应用程序4…
阅读更多...
Linux系统编程 --- 多线程

Linux系统编程 --- 多线程

线程&#xff1a;是进程内的一个执行分支&#xff0c;线程的执行粒度&#xff0c;要比进程要细。 一、线程的概念 1、Linux中线程该如何理解 地址空间就是进程的资源窗口。 在一个程序里的一个执行路线就叫做线程&#xff08;thread&#xff09;。更准确的定义是&#xff1…
阅读更多...
浏览器遇到的问题

浏览器遇到的问题

下载的时候遇到&#xff0c;需要授权&#xff0c;无法下下载 将隐私里面的全部关掉
阅读更多...
虚幻5|AI巡逻宠物伴随及定点巡逻—初步篇

虚幻5|AI巡逻宠物伴随及定点巡逻—初步篇

一.建立AI基本三件套 1.建立AI基本三件套 二.使用AI的基本设置 1.打开我们想要用的AI宠物的蓝图&#xff0c;选中自我Actor,右侧细节处找到AI&#xff0c;选中对应的AI控制器 三.打开AI控制器 写如下 四&#xff0c;AI行为树 1.新建一个任务&#xff0c;命名含巡逻二字即可…
阅读更多...
BigInteger与BigDecimal

BigInteger与BigDecimal

BigInteger BigInteger构造方法 public BigInteger(int num, Random rnd) 获取随机大整数&#xff0c;范围&#xff1a;[0 ~ 2的num次方-1] public BigInteger(String val) 获取指定的大整数 public BigInteger(String val, int radix) 获取指定进制的大整数 构造方法小结…
阅读更多...
Power Query抓取多页数据导入到Excel

Power Query抓取多页数据导入到Excel

原文链接 举例网站&#xff1a;http://vip.stock.finance.sina.com.cn/q/go.php/vLHBData/kind/ggtj/index.phtml?last5&p1 操作步骤 &#xff08;版本为&#xff1a;Excel2010&#xff09;&#xff1a; Step-01&#xff1a;单击【Power Query】-【从Web】&#xff0c;…
阅读更多...
Java之文件操作和IO

Java之文件操作和IO

目录 File类 属性 构造方法 方法 文件内容的读写 InputStream OutputStream File类 属性 修饰符及类型属性说明static StringpathSeparator依赖于系统的路径分隔符&#xff0c;String类型的表示static charpathSeparator依赖于系统的路径分隔符&#xff0c;char类型的…
阅读更多...
ps磨皮滤镜插件Imagenomic Portraiture 4.5 Build 4501中文版

ps磨皮滤镜插件Imagenomic Portraiture 4.5 Build 4501中文版

PS磨皮神器更新为Portraiture 中文汉化版&#xff08;支持PS 2024&#xff09; 。Portraiture 4.5 Build 4501中文绿色破解版是一款非常强大的适用于Photoshop&#xff0c;Lightroom&#xff0c;Aperture的人物磨皮&#xff08;人物润色&#xff09;插件。Portraiture插件被经常…
阅读更多...
基于eBPF的procstat软件追踪C++ STL容器扩容

基于eBPF的procstat软件追踪C++ STL容器扩容

在性能敏感的C程序中&#xff0c;标准模板库&#xff08;STL&#xff09;容器的扩容操作往往是导致性能抖动的原因之一。扩容操作可能会引发内存重新分配和数据迁移&#xff0c;从而导致性能不稳定。然而&#xff0c;由于C标准库的扩容函数通常被内联化&#xff0c;传统的方法难…
阅读更多...
Geekbench AI 1.0正式发布：AI性能评估

Geekbench AI 1.0正式发布：AI性能评估

每周跟踪AI热点新闻动向和震撼发展 想要探索生成式人工智能的前沿进展吗&#xff1f;订阅我们的简报&#xff0c;深入解析最新的技术突破、实际应用案例和未来的趋势。与全球数同行一同&#xff0c;从行业内部的深度分析和实用指南中受益。不要错过这个机会&#xff0c;成为AI领…
阅读更多...
STM32标准库学习笔记-4.定时器中断

STM32标准库学习笔记-4.定时器中断

参考教程&#xff1a;【STM32入门教程-2023版 细致讲解 中文字幕】 定时器简介 TIM&#xff08;Timer&#xff09;定时器。定时器可以对输入的时钟进行计数&#xff0c;并在计数值达到设定值时触发中断。16位计数器、预分频器、自动重装寄存器的时基单元&#xff0c;在72MHz计…
阅读更多...
vue3响应式工具 toRefs() 和 toRef()

vue3响应式工具 toRefs() 和 toRef()

前言 直接解构响应式对象的属性进行赋值给新的变量&#xff0c;会导致新变量失去响应式。 当修改新变量的值时&#xff0c;不会触发原始响应式对象的更新&#xff0c;从而在模板中也不会有相应的视图更新。 示例&#xff1a; <template><div><p>姓名: {{ …
阅读更多...
垂直行业数字化表现抢眼 亚信科技全年利润展望乐观

垂直行业数字化表现抢眼 亚信科技全年利润展望乐观

大数据产业创新服务媒体 ——聚焦数据 改变商业 2024年8月14日&#xff0c;亚信科技控股有限公司&#xff08;股票代码&#xff1a;01675.HK&#xff09;公布了公司截至2024年6月30日的中期业绩。 财报数据显示&#xff0c;2024年上半年&#xff0c;亚信科技的营业收入为人民币…
阅读更多...
Java巅峰之路---进阶篇---面向对象（一）

Java巅峰之路---进阶篇---面向对象（一）

static关键字 介绍 static表示静态&#xff0c;是java中的一个修饰符&#xff0c;可以修饰成员方法&#xff0c;成员变量。 其中&#xff0c;被static修饰的成员变量&#xff0c;叫做静态变量&#xff1b;被static修饰的成员方法&#xff0c;叫做静态方法。 静态变量 调用…
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023