ROS Navigation之amcl源码解析（完全详解）

柔情只为你懂 2023-06-13 11:14 15阅读 0赞

# 0. 写在最前面 #

本文持续更新地址：[https://haoqchen.site/2018/05/06/amcl-code/][https_haoqchen.site_2018_05_06_amcl-code]

这篇文章记录下自己在阅读amcl源码过程中的一些理解，如有不妥，欢迎评论或私信。

本文中所有代码因为篇幅等问题，都只给出主要部分，详细的自己下载下来对照着看。

如果觉得写得还不错，就请收藏一下啦～～～也可以找一下我写的其他包的源码解读来看一下。关注一下我的专栏什么的。

**帮我[github page点个 Star][github page_ Star]呗～～～**

# 1. amcl是干什么的 #

Amcl is a probabilistic localization system for a robot moving in 2D. It implements the adaptive (or KLD-sampling) Monte Carlo localization approach (as described by Dieter Fox), which uses a particle filter to track the pose of a robot against a known map.

以上是官网的介绍，说白了就是2D的概率定位系统，输入激光雷达数据、里程计数据，输出机器人在地图中的位姿。用的是自适应蒙特卡洛定位方法，这个方法是在已知地图中使用粒子滤波方法得到位姿的。

如下图所示，如果里程计没有误差，完美的情况下，我们可以直接使用里程计信息（上半图）推算出机器人（base\_frame）相对里程计坐标系的位置。但现实情况，里程计存在漂移以及无法忽略的累计误差，所以AMCL采用下半图的方法，即先根据里程计信息初步定位base\_frame，然后通过测量模型得到base\_frame相对于map\_frame（全局地图坐标系），也就知道了机器人在地图中的位姿。（注意，这里虽然估计的是base到map的转换，但最后发布的是map到odom的转换，可以理解为里程计的漂移。）  
![frame\_relation][frame_relation]

# 2. 总体情况 #

## 2.1 CMakeLists ##

研究一个ROS包，肯定要先看CMakeLists。我们可以看到，这个包会生成

**三个库：**

*  amcl\_pf
 *  amcl\_map
 *  amcl\_sensors  
    **一个节点：**
 *  amcl

## 2.2 其中amcl的订阅与发布： ##

**发布话题：**

1.  amcl\_pose： geometry\_msgs::PoseWithCovarianceStamped，后验位姿+一个6\*6的协方差矩阵（xyz+三个转角）
2.  particlecloud：geometry\_msgs::PoseArray，粒子位姿的数组
3.  一个15秒的定时器：AmclNode::checkLaserReceived，检查上一次收到激光雷达数据至今是否超过15秒，如超过则报错。

**发布服务：**

1.  global\_localization：&AmclNode::globalLocalizationCallback，这里是没有给定初始位姿的情况下在全局范围内初始化粒子位姿，该Callback调用pf\_init\_model，然后调用AmclNode::uniformPoseGenerator在地图的free点随机生成pf->max\_samples个粒子
2.  request\_nomotion\_update：&AmclNode::nomotionUpdateCallback没运动模型更新的情况下也暂时更新粒子群
3.  set\_map：&AmclNode::setMapCallback
4.  dynamic\_reconfigure::Server动态参数配置器。

**订阅话题：**

1.  scan\_topic\_：sensor\_msgs::LaserScan，AmclNode::laserReceived，这里是tf订阅，转换到odom\_frame\_id\_
2.  initialpose：AmclNode::initialPoseReceived，这个应该就是订阅rviz中给的初始化位姿，调用AmclNode::handleInitialPoseMessage，只接受global\_frame\_id\_（一般为map）的坐标，并重新生成粒子
3.  map：AmclNode::mapReceived这个在use\_map\_topic\_的时候才订阅，否则requestMap();我这里也没有订阅，因为只使用了一个固定的地图。

# 3. amcl\_node.cpp #

这个文件实现了上述的amcl节点功能。

## 3.1 main ##

int main(int argc, char** argv)
    {
        
      ros::init(argc, argv, "amcl");
      ros::NodeHandle nh;
      // Override default sigint handler
      signal(SIGINT, sigintHandler);
      // Make our node available to sigintHandler
      amcl_node_ptr.reset(new AmclNode());
      if (argc == 1)
      {
        
        // run using ROS input
        ros::spin();
      }
     }

没啥好说的，主要就是定义了amcl节点，初始化了一个AmclNode的类对象，最关键的中断函数配置都在该类的构造函数中实现。

## 3.2 AmclNode ##

AmclNode::AmclNode()
    {
        
      boost::recursive_mutex::scoped_lock l(configuration_mutex_);
    //测量模型选择，具体参考《概率机器人》第6章内容，这里默认的likelihood_field是6.4节的似然域模型
      std::string tmp_model_type;
      private_nh_.param("laser_model_type", tmp_model_type, std::string("likelihood_field"));
      
    //机器人模型选择，具体参考《概率机器人》5.4节里程计运动模型内容，这里默认的diff应该是差分型（两轮驱动）的机器人？，另外一个是全向机器人？
      private_nh_.param("odom_model_type", tmp_model_type, std::string("diff"));
    //从参数服务器中获取初始位姿及初始分布
      updatePoseFromServer();
    //定义话题及订阅等，具体在本文第2章有讲
      pose_pub_ = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("amcl_pose", 2, true);
    }

## 3.3 requestMap ##

这里请求服务static\_server提供map，然后调用handleMapMessage处理地图信息。这里的地图类型是nav\_msgs::OccupancyGrid，这里不介绍，有兴趣自己去看数据类型。然后获取初始位姿、初始化粒子、里程计信息等。

AmclNode::requestMap()
    {
        
    //一直请求服务static_map直到成功，该服务在map_server这个包的map_server节点中进行定义
      while(!ros::service::call("static_map", req, resp))
      {
        
        ROS_WARN("Request for map failed; trying again...");
        ros::Duration d(0.5);
        d.sleep();
      }
      handleMapMessage( resp.map );
    }
    
    AmclNode::handleMapMessage(const nav_msgs::OccupancyGrid& msg)
    {
        
    //free相应的指针
      freeMapDependentMemory();
    //转换成标准地图，0->-1(不是障碍);100->+1（障碍）;else->0（不明）
      map_ = convertMap(msg);
    
    //将不是障碍的点的坐标保存下来
    #if NEW_UNIFORM_SAMPLING
      // Index of free space
      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));
    #endif
      // Create the particle filter，定义了一个回调，尚未清除干啥
      pf_ = pf_alloc(min_particles_, max_particles_,
                     alpha_slow_, alpha_fast_,
                     (pf_init_model_fn_t)AmclNode::uniformPoseGenerator,
                     (void *)map_);
      // 从参数服务器获取初始位姿及方差放到pf中
      updatePoseFromServer();
    
      // 定义里程计与激光雷达并初始化数据
      // Odometry
      delete odom_;
      odom_ = new AMCLOdom();
      ROS_ASSERT(odom_);
      odom_->SetModel( odom_model_type_, alpha1_, alpha2_, alpha3_, alpha4_, alpha5_ );
      // Laser
      delete laser_;
      laser_ = new AMCLLaser(max_beams_, map_);
    
      // In case the initial pose message arrived before the first map,
      // try to apply the initial pose now that the map has arrived.
      applyInitialPose();
    }

## 3.4 laserReceived ##

其中变量pose是base相对于odom的位姿；pf\_odom\_pose\_则是上一时刻base相对于odom的位姿，用于后续得到机器人的相对运动程度。

AmclNode::laserReceived(const sensor_msgs::LaserScanConstPtr& laser_scan)
    {
         
      // Do we have the base->base_laser Tx yet?感觉这里是支持多个激光雷达的，找到当前响应的激光雷达之前存储的信息，
    //如相对于base的转换，是否更新等，使用map结构直接通过id来找到对应序号即可
    //如果之前没有备案则在map结构中备案，然后存到frame_to_laser_及lasers_中下次备用
      if(frame_to_laser_.find(laser_scan->header.frame_id) == frame_to_laser_.end())
      {
        
        frame_to_laser_[laser_scan->header.frame_id] = laser_index;
      } else {
        
        // we have the laser pose, retrieve laser index
        laser_index = frame_to_laser_[laser_scan->header.frame_id];
      }
    
      // Where was the robot when this scan was taken?获得base在激光雷达扫描时候相对于odom的相对位姿
      pf_vector_t pose;
      if(!getOdomPose(latest_odom_pose_, pose.v[0], pose.v[1], pose.v[2],
                      laser_scan->header.stamp, base_frame_id_))
      {
        
        ROS_ERROR("Couldn't determine robot's pose associated with laser scan");
        return;
      }
    
      pf_vector_t delta = pf_vector_zero();
    //如果不是第一帧，看运动幅度是否超过设定值需要更新（第一帧是指更新了地图或者更新初始位姿）
      if(pf_init_)
      {
        
        // Compute change in pose
        //delta = pf_vector_coord_sub(pose, pf_odom_pose_);
        delta.v[0] = pose.v[0] - pf_odom_pose_.v[0];
        delta.v[1] = pose.v[1] - pf_odom_pose_.v[1];
        delta.v[2] = angle_diff(pose.v[2], pf_odom_pose_.v[2]);
    
        // See if we should update the filter
        bool update = fabs(delta.v[0]) > d_thresh_ ||
                      fabs(delta.v[1]) > d_thresh_ ||
                      fabs(delta.v[2]) > a_thresh_;
        update = update || m_force_update;
        m_force_update=false;
    
        // Set the laser update flags
        if(update)
          for(unsigned int i=0; i < lasers_update_.size(); i++)
            lasers_update_[i] = true;
      }
    //第一帧则初始化一些值
      bool force_publication = false;
      if(!pf_init_)
      {
        
        // Pose at last filter update
        pf_odom_pose_ = pose;
        // Filter is now initialized
        pf_init_ = true;
      }
      // If the robot has moved, update the filter
      else if(pf_init_ && lasers_update_[laser_index])//如果已经初始化并需要更新则更新运动模型
      {
        
    //这是amcl_odom.cpp中最重要的一个函数，实现了用运动模型来更新现有的每一个粒子的位姿（这里得到的只是当前时刻的先验位姿）
        // Use the action data to update the filter
        odom_->UpdateAction(pf_, (AMCLSensorData*)&odata);
      }
    
      bool resampled = false;
      // If the robot has moved, update the filter
      if(lasers_update_[laser_index])
      {
        //接下来一大片都是对原始激光雷达数据进行处理，转换到AMCLLaserData。包括角度最小值、增量到base_frame的转换、测距距离最大值、最小值。
        //具体看代码，篇幅限制不详细列了
        try
        {
        
          tf_->transformQuaternion(base_frame_id_, min_q, min_q);
          tf_->transformQuaternion(base_frame_id_, inc_q, inc_q);
        }
      
        ldata.range_max = std::min(laser_scan->range_max, (float)laser_max_range_);
        range_min = std::max(laser_scan->range_min, (float)laser_min_range_);
            
        ldata.ranges = new double[ldata.range_count][2];
        for(int i=0;i<ldata.range_count;i++)
        {
        
          // amcl doesn't (yet) have a concept of min range.  So we'll map short
          // readings to max range.//激光雷达传上来的数据只标记了最大值最小值，但是没做处理，直接将原始数据传上来，
          if(laser_scan->ranges[i] <= range_min)//这里将最小值当最大值处理，因为在类似likelihood_field模型中，会直接将最大值丢弃
            ldata.ranges[i][0] = ldata.range_max;
        }
    //注意这里是amcl_laser.cpp的UpdateSensor，不是amcl_sensor.cpp的。通过判断前面设置的测量模型调用pf_update_sensor，
    //该函数需要传入相应模型的处理函数，这里所有的测量模型在《概率机器人》的第六章有详细讲，具体自己看，后面只针对自己使用的likelihood_field模型讲一下
    //pf_update_sensor实现对所有粒子更新权重，并归一化、计算出《概率机器人》8.3.5的失效恢复的长期似然和短期似然
        lasers_[laser_index]->UpdateSensor(pf_, (AMCLSensorData*)&ldata);
    
        lasers_update_[laser_index] = false;
        pf_odom_pose_ = pose;
    //多少次激光雷达回调之后进行重采样呗，我这里resample_interval_=0.5，只有一个激光雷达，每次都更新。
        // Resample the particles
        if(!(++resample_count_ % resample_interval_))
        {
        
    //按照一定的规则重采样粒子，包括前面说的失效恢复、粒子权重等，然后放入到kdtree，暂时先理解成关于位姿的二叉树，
    //然后进行聚类，得到均值和方差等信息,个人理解就是将相近的一堆粒子融合成一个粒子了，没必要维持太多相近的@TODO
          pf_update_resample(pf_);
          resampled = true;
        }
    
        // Publish the resulting cloud
        // TODO: set maximum rate for publishing
        if (!m_force_update) {
        
          //将新粒子发布到全局坐标系下，一般是map
          particlecloud_pub_.publish(cloud_msg);
        }
      }
    
      if(resampled || force_publication)
      {
        
        //遍历所有粒子簇，找出权重均值最大的簇，其平均位姿就是我们要求的机器人后验位姿，到此一次循环已经所有完成
        for(int hyp_count = 0;
            hyp_count < pf_->sets[pf_->current_set].cluster_count; hyp_count++)
        {
        
          if (!pf_get_cluster_stats(pf_, hyp_count, &weight, &pose_mean, &pose_cov))
          {
        
            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)
        {
        
          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++)
            {
        
              p.pose.covariance[6*i+j] = set->cov.m[i][j];
            }
          }
          p.pose.covariance[6*5+5] = set->cov.m[2][2];
    
          pose_pub_.publish(p);
          last_published_pose = p;
    //这里就是所说的，map～base减去odom～base得到map～odom，最后发布的是map～odom。
      // 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!");
        }
      }
      else if(latest_tf_valid_)//if(resampled || force_publication)
      {
        
    
      }
    }

## 3.5 AMCLOdom::UpdateAction ##

首先获取上一帧的位姿，然后根据自己前面选择的机器人模型进行运动模型更新。这里由于我自己的模型是ODOM\_MODEL\_DIFF，所以只针对这个模型进行讲解，剩下的原理应该一样，细节上有区别，自己看。代码中有讲到，利用到的算法是《Probabilistic Robotics 2》136页的内容，如果是Probabilistic Robotics 1或者中文版其实就是5.4节中的采样算法，这里不再细述。

我直白一点的理解就是，已知上一时刻所有粒子的后验位姿、上一时刻到现在两个轮子的编码器值，给所有粒子随机采样当前时刻的先验位姿。为什么是随机，不是确定的直接从编码器计算得到呢？因为编码器等存在误差，所以采样到的每个粒子的先验位姿都是不确定的，但理论上是服从一个分布的（这里是正态，书本上给出三种分布），跟实际运动有关，如果运动越大、则采样到的跟直接里程计计算得到的值就越有可能相差的比较大（算法上表示为方差越大）。

// Apply the action model
    bool AMCLOdom::UpdateAction(pf_t *pf, AMCLSensorData *data)
    {
        
      set = pf->sets + pf->current_set;
      pf_vector_t old_pose = pf_vector_sub(ndata->pose, ndata->delta);
    
      switch( this->model_type )
      {
        
       case ODOM_MODEL_DIFF:
      {
        
        // Implement sample_motion_odometry (Prob Rob p 136)
        double delta_rot1, delta_trans, delta_rot2;
        double delta_rot1_hat, delta_trans_hat, delta_rot2_hat;
        double delta_rot1_noise, delta_rot2_noise;
    //计算里程计得到的ut，即用旋转、直线运动和另一个旋转来表示相对运动
        // Avoid computing a bearing from two poses that are extremely near each
        // other (happens on in-place rotation).
        if(sqrt(ndata->delta.v[1]*ndata->delta.v[1] + //如果原地旋转的情况，定义第一个旋转为0，认为所有旋转都是第二个旋转做的
                ndata->delta.v[0]*ndata->delta.v[0]) < 0.01)
          delta_rot1 = 0.0;
        else
          delta_rot1 = angle_diff(atan2(ndata->delta.v[1], ndata->delta.v[0]),
                                  old_pose.v[2]);
        delta_trans = sqrt(ndata->delta.v[0]*ndata->delta.v[0] +
                           ndata->delta.v[1]*ndata->delta.v[1]);
        delta_rot2 = angle_diff(ndata->delta.v[2], delta_rot1);
    //这里作者比书本多考虑了一种情况，就是机器人旋转小角度，然后往后走的情况，比如旋转-20°，然后往后走，计算出来可能是160°，
    //但实际只转了20°。按书本可能是比较大的方差（160°），不太准确，但这里的话还是按照比较小的方差来采样。
    //但我表示怀疑，万一真的转了160°再前向运动的话怎么办？还是用比较小的方差来采样，可能会使得采样的准确率下降。
    //实际中，编码器采样率很高，且设定了一个很小的变化角度就进行运动模型更新，所以我说的这种情况几乎不会发生
        // We want to treat backward and forward motion symmetrically for the
        // noise model to be applied below.  The standard model seems to assume
        // forward motion.
        delta_rot1_noise = std::min(fabs(angle_diff(delta_rot1,0.0)),
                                    fabs(angle_diff(delta_rot1,M_PI)));
        delta_rot2_noise = std::min(fabs(angle_diff(delta_rot2,0.0)),
                                    fabs(angle_diff(delta_rot2,M_PI)));
    //对每个粒子用书本的方法进行更新
        for (int i = 0; i < set->sample_count; i++)
        {
        
          pf_sample_t* sample = set->samples + i;
    
          // Sample pose differences
          delta_rot1_hat = angle_diff(delta_rot1,
                                      pf_ran_gaussian(this->alpha1*delta_rot1_noise*delta_rot1_noise +
                                                      this->alpha2*delta_trans*delta_trans));
          delta_trans_hat = delta_trans - 
                  pf_ran_gaussian(this->alpha3*delta_trans*delta_trans +
                                  this->alpha4*delta_rot1_noise*delta_rot1_noise +
                                  this->alpha4*delta_rot2_noise*delta_rot2_noise);
          delta_rot2_hat = angle_diff(delta_rot2,
                                      pf_ran_gaussian(this->alpha1*delta_rot2_noise*delta_rot2_noise +
                                                      this->alpha2*delta_trans*delta_trans));
    
          // Apply sampled update to particle pose
          sample->pose.v[0] += delta_trans_hat * 
                  cos(sample->pose.v[2] + delta_rot1_hat);
          sample->pose.v[1] += delta_trans_hat * 
                  sin(sample->pose.v[2] + delta_rot1_hat);
          sample->pose.v[2] += delta_rot1_hat + delta_rot2_hat;
        }
      }
      }
      return true;
    }

## 3.6 LikelihoodFieldModel ##

算法原理请参看《概率机器人》6.4节，这里会给每个粒子（代表一个位姿）计算概率、更新概率，然后返回所有粒子概率的总和。

double AMCLLaser::LikelihoodFieldModel(AMCLLaserData *data, pf_sample_set_t* set)
    {
        
      // Compute the sample weights遍历所有激光雷达数据点，
      for (j = 0; j < set->sample_count; j++)
      {
        
        sample = set->samples + j;
        pose = sample->pose;//遍历每个粒子，这是粒子对应的位姿，是经运动模型更新后先验位姿
    
        // Take account of the laser pose relative to the robot
    //这个应该是通过机器人与全局坐标系的位姿（每个粒子的位姿），计算激光雷达相对于全局坐标系的位姿。方便后续将激光雷达扫描的终点转换到全局坐标系
    //点进去pf_vector_coord_add这个函数，b对应的就是《概率机器人》P128,6.4第一个公式中的机器人在t时刻的位姿，a代表“与机器人固连的传感器局部坐标系位置”
        pose = pf_vector_coord_add(self->laser_pose, pose);//这里的laser_pose其实是laserReceived函数一开始初始化每个激光雷达时得到的激光雷达在base中的位姿
    
        p = 1.0;
    
        // Pre-compute a couple of things
        double z_hit_denom = 2 * self->sigma_hit * self->sigma_hit;//测量噪声的方差
        double z_rand_mult = 1.0/data->range_max;//无法解释的随机测量的分母
    //因为时间问题，并不是全部点都进行似然计算的，这里只是间隔地选点，我这里设置的是一共选择60个点～～～
        step = (data->range_count - 1) / (self->max_beams - 1);
        for (i = 0; i < data->range_count; i += step)
        {
        
          obs_range = data->ranges[i][0];
          obs_bearing = data->ranges[i][1];
    
    //继续上面《概率机器人》P128的后半段公式，得到激光雷达点最远端的世界坐标
          // Compute the endpoint of the beam
          hit.v[0] = pose.v[0] + obs_range * cos(pose.v[2] + obs_bearing);
          hit.v[1] = pose.v[1] + obs_range * sin(pose.v[2] + obs_bearing);
    
    //转换到栅格坐标
          // Convert to map grid coords.
          int mi, mj;
          mi = MAP_GXWX(self->map, hit.v[0]);
          mj = MAP_GYWY(self->map, hit.v[1]);
    
    //这是提前计算好的最近距离，计算函数在map_cspace.cpp的map_update_cspace中实现遍历计算，该函数是被AMCLLaser::SetModelLikelihoodField调用
    //而这个函数则只在AmclNode::handleMapMessage中调用？？？而这个handle在多个地方调用，暂时未清楚@TODO      
          // Part 1: Get distance from the hit to closest obstacle.
          // Off-map penalized as max distance
          if(!MAP_VALID(self->map, mi, mj))
            z = self->map->max_occ_dist;
          else
            z = self->map->cells[MAP_INDEX(self->map,mi,mj)].occ_dist;
    //书本算法流程的公式
          // Gaussian model
          // NOTE: this should have a normalization of 1/(sqrt(2pi)*sigma)
          pz += self->z_hit * exp(-(z * z) / z_hit_denom);
          // Part 2: random measurements
          pz += self->z_rand * z_rand_mult;
    
          // TODO: outlier rejection for short readings
    
          assert(pz <= 1.0);
          assert(pz >= 0.0);
          //      p *= pz;
          // here we have an ad-hoc weighting scheme for combining beam probs
          // works well, though...
          p += pz*pz*pz;//这里有点不太懂。。。理论上应该是上面注释掉的公式。这里应该是为了防止噪声，比如某个出现错误的0，其余都不管用了
        }//每个粒子的所有激光雷达点循环
    
        sample->weight *= p;
        total_weight += sample->weight;
      }//粒子循环
    
      return(total_weight);
    }

# 参考 #

[https://wenku.baidu.com/view/92a45164a9956bec0975f46527d3240c8447a1d5.html][https_wenku.baidu.com_view_92a45164a9956bec0975f46527d3240c8447a1d5.html]  
[https://www.cnblogs.com/lysuns/articles/4710712.html][https_www.cnblogs.com_lysuns_articles_4710712.html]  
[ROS官网参数配置][ROS]

**喜欢我的文章的话Star一下呗[Star][github page_ Star]**

[https_haoqchen.site_2018_05_06_amcl-code]: https://haoqchen.site/2018/05/06/amcl-code/
[github page_ Star]: https://github.com/HaoQChen/HaoQChen.github.io
[frame_relation]: https://imgconvert.csdnimg.cn/aHR0cHM6Ly9oYW9xY2hlbi5zaXRlL2ltZy9pbl9wb3N0L2FtY2xfY29kZS9mcmFtZV9yZWxhdGlvbi5wbmc?x-oss-process=image/format,png
[https_wenku.baidu.com_view_92a45164a9956bec0975f46527d3240c8447a1d5.html]: https://wenku.baidu.com/view/92a45164a9956bec0975f46527d3240c8447a1d5.html
[https_www.cnblogs.com_lysuns_articles_4710712.html]: https://www.cnblogs.com/lysuns/articles/4710712.html
[ROS]: http://wiki.ros.org/amcl?distro=lunar