代码解读：DP-SLAM（4）

代码解读：DP-SLAM（4）

上次分析的地方有误，应该注意，不好意思，来重新分析一下这一段，

分析之前，把上次的知识回锅一下：里程计发布这一时刻的数据，然后粒子根据里程计数据更析自己状态，但是每

一个粒子对应的probability都设置为0

好，那就开始分析吧，

  threshold = WORST_POSSIBLE-1;  // ensures that we accept anything in 1st round
  for (p = 0; p < PASSES; p++){
    best = 0;
    for (i = 0; i < SAMPLE_NUMBER; i++) {
      if (newSample[i].probability >= threshold) {
    for (k = p; k < SENSE_NUMBER; k += PASSES)
      newSample[i].probability = newSample[i].probability + log(QuickScore(sense, k, i));
    if (newSample[i].probability > newSample[best].probability)
      best = i;
      }
      else
    newSample[i].probability = WORST_POSSIBLE;
    }
    threshold = newSample[best].probability - THRESH;
  }

从这段代码中可以了解：

1，PASSES是筛选粒子的轮数，一共筛选粒子(PASSES - 1)轮

2，在第一轮中(PASSES = 0)，threshold设为负值，保留所有粒子

3，SAMPLE_NUMBER指粒子个数，SENSE_NUMBER指雷达传感器数据

4，凡是合格的粒子，即粒子的概率大于threshold，都会进行下一轮评估，即把这一时刻的雷达数据拿来，做

     “适应性”评价，用评价的分数计算该合格粒子的probability

       评价函数就是之前介绍的QuickScore()

5，当所有合格粒子的probability都被更新一遍之后，自然要更新threshold

       threshold的更新原则：用合格粒子最优的probability减去一个常数THRESH

用激光雷达数据更新过粒子的probability之后，有如下的操作，

  keepers = 0;
  for (i = 0; i < SAMPLE_NUMBER; i++) {
    if (newSample[i].probability >= threshold) {
      keepers++;
      // Don't let this heuristic evaluation be included in the final eval.
      newSample[i].probability = 0.0;
    }
    else
      newSample[i].probability = WORST_POSSIBLE;
  }

  // Letting the user know how many samples survived this first cut.
  fprintf(stderr, "Better %d ", keepers);
  threshold = -1;

从这段代码中，可以了解到：1，keepers用来计数合格粒子，并且把合格粒子的probability置为0;

2,   把not pass的粒子的probability置为WORST_POSSIBLE，而它等于-10000 3，然后threshold置为 -1

接下来有，

  // Now reevaluate all of the surviving samples, using the full laser scan to look for possible
  // obstructions, in order to get the most accurate weights. While doing this evaluation, we can
  // still keep our eye out for unlikely samples before we are finished.
  keepers = 0;
  for (p = 0; p < PASSES; p++){
    best = 0;
    for (i = 0; i < SAMPLE_NUMBER; i++) {
      if (newSample[i].probability >= threshold) {
    if (p == PASSES -1)
      keepers++;
    for (k = p; k < SENSE_NUMBER; k += PASSES)
      newSample[i].probability = newSample[i].probability + log(CheckScore(sense, k, i));
    if (newSample[i].probability > newSample[best].probability)
      best = i;
      }
      else
    newSample[i].probability = WORST_POSSIBLE;
    }
    threshold = newSample[best].probability - THRESH;
  }

从这段代码可以了解到：

1，PASSES指粒子筛选的轮数（跟之前的筛选有些相似）

2，筛选的机制跟之前讨论的类似，同样，threshold更新机制还是一样

然后有，

  // Report how many samples survived the second cut. These numbers help the user have confidence that
  // the threshhold values used for culling are reasonable.
  fprintf(stderr, "Best of %d ", keepers);

第二次筛选 称为 the second cut

筛选之后呢，

  // All probabilities are currently in log form. Exponentiate them, but weight them by the prob of the
  // the most likely sample, to ensure that we don't run into issues of machine precision at really small
  // numbers.
  total = 0.0;
  threshold = newSample[best].probability;
  for (i = 0; i < SAMPLE_NUMBER; i++) {
    // If the sample was culled, it has a weight of 0
    if (newSample[i].probability == WORST_POSSIBLE)
      newSample[i].probability = 0.0;
    else {
      newSample[i].probability = exp(newSample[i].probability-threshold);
      total = total + newSample[i].probability;
    }
  }

从这段代码可以了解到，

1，不合格的粒子的probability记为0

2，合格粒子的probability用指数表示

3，total的计算，是为了归一化，这也是重采样的过程之一


总而言之，这一段粒子筛选的代码就是这样的，

下次再说！