Cacher is the code snippet organizer for pro developers

这里只截取其中的processScan函数
 
  * 在scanmatcherprocessor里面也有一个这样的函数 但是那个函数是没有使用的
  @desc 处理一帧激光数据 这里是gmapping算法的主要函数。
  在这个函数里面调用其他的函数，包括里程计预测、激光测量更新、粒子采样等等步骤。
  
  主要步骤如下：
  利用运动模型更新里程计分布
  利用最近的一次观测来提高proposal分布。(scan-match)
  利用proposal分布+激光雷达数据来确定各个粒子的权重
  对粒子进行重采样
 
  bool GridSlamProcessor::processScan(const RangeReading & reading, int adaptParticles)
  {
    /**retireve the position from the reading, and compute the odometry*/
    // 得到当前的里程计的位置 
	OrientedPoint relPose=reading.getPose();
    // relPose.y = m_odoPose.y;
    
	// m_count表示这个函数被调用的次数 如果是第0次调用,则所有的位姿都是一样的*/
	if (!m_count)
	{
      m_lastPartPose=m_odoPose=relPose;
    }
    
    // write the state of the reading and update all the particles using the motion model
	  // 对于每一个粒子，都从里程计运动模型中采样，得到车子的初步估计位置  这一步对应于   里程计的更新 
    int tmp_size = m_particles.size();

    // 这个for循环显然可以用OpenMP进行并行化
    // pragma omp parallel for
    for(int i = 0; i < tmp_size;i++)
    {
        OrientedPoint& pose(m_particles[i].pose);
        pose = m_motionModel.drawFromMotion(m_particles[i],relPose,m_odoPose);
    }

    // invoke the callback
    // 回调函数  实际上什么都没做 
    onOdometryUpdate();
    
    // accumulate the robot translation and rotation
    // 根据两次里程计的数据 计算出来机器人的线性位移和角度位移的累积值 m_odoPose表示上一次的里程计位姿  relPose表示新的里程计的位姿 
    OrientedPoint move=relPose-m_odoPose;
    move.theta=atan2(sin(move.theta), cos(move.theta));

    // 统计机器人在进行激光雷达更新之前 走了多远的距离 以及　平移了多少的角度
    m_linearDistance+=sqrt(move*move);
    m_angularDistance+=fabs(move.theta);

//    cerr <<"linear Distance:"<<m_linearDistance<<endl;
//    cerr <<"angular Distance:"<<m_angularDistance<<endl;
    
    // if the robot jumps throw a warning
    /*
     * 如果机器人在走了m_distanceThresholdCheck这么远的距离都没有进行激光雷达的更新
     * 则需要进行报警。这个误差很可能是里程计或者激光雷达的BUG造成的。
     * 例如里程计数据出错 或者 激光雷达很久没有数据等等
     * 每次进行激光雷达的更新之后 m_linearDistance这个参数就会清零
     */
    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;

    // process a scan only if the robot has traveled a given distance or a certain amount of time has elapsed
	  // 只有当机器人走过一定的距离  或者 旋转过一定的角度  或者过一段指定的时间才处理激光数据
    if (! m_count 
	|| m_linearDistance>=m_linearThresholdDistance 
	|| m_angularDistance>=m_angularThresholdDistance
    || (period_ >= 0.0 && (reading.getTime() - last_update_time_) > period_))
	{
          last_update_time_ = reading.getTime();

          std::cout <<std::endl<<"Start to Process Scan##################"<<std::endl;

          if (m_outputStream.is_open())
          {
              m_outputStream << setiosflags(ios::fixed) << setprecision(6);     // setiosflags 意思就是设置输入输出的标志
                                                                                // iso::fixed 是操作符setiosflags 的参数之一，该参数指定的动作是以带小数点的形式表示浮点数，并且在允许的精度范围内尽可能的把数字移向小数点右侧；
                                                                                // ios::right是 右对齐
                                                                                // setprecision(6)是设置数字的精度为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;

//              m_infoStream << "FRAME " <<  m_readingCount<<endl;
//              m_infoStream <<"Scan-Match Number: "<<m_count<<endl;
//          }
//          cerr << "Laser Pose= " << reading.getPose().x << " " << reading.getPose().y
//                 << " " << reading.getPose().theta << endl;

          //this is for converting the reading in a scan-matcher feedable form
          /*复制一帧数据 把激光数据转换为scan-match需要的格式*/
//          if(reading.getSize() != m_beams)
//          {
//              cerr<<"********************************************"<<endl;
//              cerr<<"********************************************"<<endl;
//              cerr<<"reading_size:"<<reading.getSize()<<"  m_beams:"<<m_beams<<endl;
//              cerr<<"********************************************"<<endl;
//              cerr<<"********************************************"<<endl;
//          }
          int beam_number = reading.getSize();
          double * plainReading = new double[beam_number];
          for(unsigned int i=0; i<beam_number; i++)
          {
            plainReading[i]=reading.m_dists[i];
          }
          //这个备份主要是用来储存的。
          RangeReading* reading_copy;

          //champion_nav_msgs激光数据
          if(reading.m_angles.size() == reading.m_dists.size())
          {
              reading_copy = new RangeReading(beam_number,
                                   &(reading.m_dists[0]),
                                   &(reading.m_angles[0]),
                                   static_cast<const RangeSensor*>(reading.getSensor()),
                                   reading.getTime());

          }
          //ros的激光数据
          else
          {
              reading_copy = new RangeReading(beam_number,
                                   &(reading.m_dists[0]),
                                   static_cast<const RangeSensor*>(reading.getSensor()),
                                   reading.getTime());
          }
          /*如果不是第一帧数据*/
          if (m_count>0)
          {
            /*
            为每个粒子进行scanMatch，计算出来每个粒子的最优位姿，同时计算改最优位姿的得分和似然  对应于gmapping论文中的用最近的一次测量计算proposal的算法
            这里面除了进行scanMatch之外，还对粒子进行了权重的计算，并计算了粒子的有效区域 但不进行内存分配 内存分配在resample()函数中
            这个函数在gridslamprocessor.hxx里面。
            */
            scanMatch(plainReading);

            //至此 关于proposal的更新完毕了，接下来是计算权重
            onScanmatchUpdate();

            /*
            由于scanMatch中对粒子的权重进行了更新，那么这个时候各个粒子的轨迹上的累计权重都需要重新计算
            这个函数即更新各个粒子的轨迹上的累计权重是更新
            GridSlamProcessor::updateTreeWeights(bool weightsAlreadyNormalized) 函数在gridslamprocessor_tree.cpp里面实现
            */
            updateTreeWeights(false);

            /*
             * 粒子重采样  根据neff的大小来进行重采样  不但进行了重采样，也对地图进行更新
             * GridSlamProcessor::resample 函数在gridslamprocessor.hxx里面实现
             */
            std::cerr<<"plainReading:"<<m_beams<<std::endl;
            resample(plainReading, adaptParticles, reading_copy);
          }
          /*如果是第一帧激光数据*/
          else
          {
            //如果是第一帧数据，则可以直接计算activeArea。因为这个时候，对机器人的位置是非常确定的，就是(0,0,0)
            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);
                //m_matcher.registerScan(it->lowResolutionMap,it->pose,plainReading);

                //为每个粒子创建路径的第一个节点。该节点的权重为0,父节点为it->node(这个时候为NULL)。
                //因为第一个节点就是轨迹的根，所以没有父节点
                TNode* node=new	TNode(it->pose, 0., it->node,  0);
                node->reading = reading_copy;
                it->node=node;
            }
         }
          //		cerr  << "Tree: normalizing, resetting and propagating weights at the end..." ;
         //进行重采样之后，粒子的权重又会发生变化，因此需要再次更新粒子轨迹的累计权重
         //GridSlamProcessor::updateTreeWeights(bool weightsAlreadyNormalized) 函数在gridslamprocessor_tree.cpp里面实现
         updateTreeWeights(false);
          //		cerr  << ".done!" <<endl;

        delete [] plainReading;
        m_lastPartPose=m_odoPose; //update the past pose for the next iteration

        //机器人累计行走的多远的路程没有进行里程计的更新 每次更新完毕之后都要把这个数值清零
        m_linearDistance=0;
        m_angularDistance=0;

        m_count++;
        processed=true;

          //keep ready for the next step
        for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++)
        {
            it->previousPose=it->pose;
        }
    }
    m_readingCount++;
    return processed;
 }
  
  
  std::ofstream& GridSlamProcessor::outputStream(){
    return m_outputStream;
  }
  
  std::ostream& GridSlamProcessor::infoStream(){
    return m_infoStream;
  }