struct sm_params
{
    /** First scan (参考帧) */
    LDP laser_ref;
    /** Second scan ("目标帧) */
    LDP laser_sens;
    double first_guess[3]; 

    /** Maximum angular displacement between scans (deg)*/
    double max_angular_correction_deg;
    /** Maximum translation between scans (m) */
    double max_linear_correction;

    int max_iterations;
    /** A threshold for stopping. */
    double epsilon_xy;
    /** A threshold for stopping. */
    double epsilon_theta;
    /** Maximum distance for a correspondence to be valid */
    double max_correspondence_dist;

    /** 使用 smart tricks 匹配. 只影响匹配速度，不影响收敛结果 */
    int use_corr_tricks;
    
    /** Restart if error under threshold (0 or 1)*/
    int restart;
    /** Threshold for restarting */
    double restart_threshold_mean_error;
    /** Displacement for restarting */
    double restart_dt;
    /** Displacement for restarting */
    double restart_dtheta;
    
    /* Functions concerning discarding correspondences.
       THESE ARE MAGIC NUMBERS -- and they need to be tuned. */

    /** Percentage of correspondences to consider: if 0.9,
        always discard the top 10% of correspondences with more error */
    double outliers_maxPerc;

    /** Parameters describing a simple adaptive algorithm for discarding.
        1) Order the errors.
        2) Choose the percentile according to outliers_adaptive_order.
           (if it is 0.7, get the 70% percentile)
        3) Define an adaptive threshold multiplying outliers_adaptive_mult
           with the value of the error at the chosen percentile.
        4) Discard correspondences over the threshold.
        
        This is useful to be conservative; yet remove the biggest errors.
    */
    double outliers_adaptive_order; /* 0.7 */
    double outliers_adaptive_mult; /* 2 */

    /** Do not allow two different correspondences to share a point */
    int outliers_remove_doubles; 

    /* Functions that compute and use point orientation for defining matches. */
    /** For now, a very simple max-distance clustering algorithm is used */
    double clustering_threshold;
    /** Number of neighbour rays used to estimate the orientation.*/
    int orientation_neighbourhood;
    /** Discard correspondences based on the angles */
    int do_alpha_test;
    double do_alpha_test_thresholdDeg;
        
    /** I believe this trick is documented in one of the papers by Guttman (but I can't find
        the reference). Or perhaps I was told by him directly. 
        
        If you already have a guess of the solution, you can compute the polar angle
        of the points of one scan in the new position. If the polar angle is not a monotone
        function of the readings index, it means that the surface is not visible in the 
        next position. If it is not visible, then we don't use it for matching.
        
        This is confusing without a picture! To understand what's going on, make a drawing
        in which a surface is not visible in one of the poses.
        Implemented in the function visibilityTest().
    */  
    int do_visibility_test;

    /** If 1, use PlICP; if 0, use vanilla ICP. */
    int use_point_to_line_distance;

    /** If 1, the field "true_alpha" is used to compute the incidence
        beta, and the factor (1/cos^2(beta)) used to weight the impact
        of each correspondence. This works fabolously if doing localization,
        that is the first scan has no noise.
        If "true_alpha" is not available, it uses "alpha".
    */
    int use_ml_weights;
    
    /* If 1, the field "readings_sigma" is used to weight the correspondence by 1/sigma^2 */
    int use_sigma_weights;
    
    /** Use the method in http://purl.org/censi/2006/icpcov to compute
        the matching covariance. */
    int do_compute_covariance;

    /** Checks that find_correspondences_tricks give the right answer */
    int debug_verify_tricks;
    
    /** Pose of sensor with respect to robot: used for computing
        the first estimate given the odometry. */
    double laser[3]; 

    /** Noise in the scan */
    double sigma;

    /** mark as invalid ( = don't use ) rays outside of this interval */
    double min_reading, max_reading;
    
    /* Parameters specific to GPM (unfinished :-/ ) */
    double gpm_theta_bin_size_deg;
    double gpm_extend_range_deg; 
    int gpm_interval;
    /* Parameter specific to HSM (unfinished :-/ ) */
    struct hsm_params hsm;
};

struct sm_result
{
    /** 1 if the result is valid */
    int valid;
    /** Scan matching result (x,y,theta) */
    double x[3];
    
    /** Number of iterations done */
    int iterations;
    /** Number of valid correspondence in the end */
    int nvalid;
    /** Total correspondence error */
    double error;
    
    /** Fields used for covariance computation */
    #ifndef RUBY
        gsl_matrix *cov_x_m;    
        gsl_matrix *dx_dy1_m;
        gsl_matrix *dx_dy2_m;
    #endif
};

struct correspondence
{
    /** 1 if this correspondence is valid  */
    int  valid; 
    /** Closest point in the other scan.  */
    int  j1;
    /** Second closest point in the other scan.  */
    int  j2;
    /** Type of correspondence (point to point, or point to line) */
    enum { corr_pp = 0, corr_pl = 1} type;
    /** Squared distance from p(i) to point j1 */
    double  dist2_j1; 
};

int n=10;
gsl_vector *v=gsl_vector_alloc(n);
int i;
/*  read and write vectors  */
for (i=0;i<n;i++)
    gsl_vector_set(v,i,pow(i+1,2));
for (i=0;i<n;i++)
    printf("The %d element of v is %f\n",i, gsl_vector_get(v,i));

// 粒子重采样
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);
}

// 刚启动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!");
    }
}

// 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;
}

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;
}

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);