1.karto-slam涉及的类-雷达以及雷达数据相关

首先是最简单的

1. sensor_msgs::LaserScan, 主要包括header、还有激光参数（扫射范围距离，步长，时间等，不包含位姿信息，header里面含有frame_id）。

typedef ::sensor_msgs::LaserScan_<std::allocator<void> > LaserScan;

struct LaserScan_
{
  typedef LaserScan_<ContainerAllocator> Type;

  LaserScan_()
    : header()
    , angle_min(0.0)
    , angle_max(0.0)
    , angle_increment(0.0)
    , time_increment(0.0)
    , scan_time(0.0)
    , range_min(0.0)
    , range_max(0.0)
    , ranges()
    , intensities()  {
    }
  LaserScan_(const ContainerAllocator& _alloc)
    : header(_alloc)
    , angle_min(0.0)
    , angle_max(0.0)
    , angle_increment(0.0)
    , time_increment(0.0)
    , scan_time(0.0)
    , range_min(0.0)
    , range_max(0.0)
    , ranges(_alloc)
    , intensities(_alloc)  {
  (void)_alloc;
    }



   typedef  ::std_msgs::Header_<ContainerAllocator>  _header_type;
  _header_type header;

   typedef float _angle_min_type;
  _angle_min_type angle_min;

   typedef float _angle_max_type;
  _angle_max_type angle_max;

   typedef float _angle_increment_type;
  _angle_increment_type angle_increment;

   typedef float _time_increment_type;
  _time_increment_type time_increment;

   typedef float _scan_time_type;
  _scan_time_type scan_time;

   typedef float _range_min_type;
  _range_min_type range_min;

   typedef float _range_max_type;
  _range_max_type range_max;

   typedef std::vector<float, typename ContainerAllocator::template rebind<float>::other >  _ranges_type;
  _ranges_type ranges;

   typedef std::vector<float, typename ContainerAllocator::template rebind<float>::other >  _intensities_type;
  _intensities_type intensities;





  typedef boost::shared_ptr< ::sensor_msgs::LaserScan_<ContainerAllocator> > Ptr;
  typedef boost::shared_ptr< ::sensor_msgs::LaserScan_<ContainerAllocator> const> ConstPtr;

}; // struct LaserScan_

2. 算法根据scan的ranges得到了range_scan,这个是

  class LocalizedRangeScan : public LaserRangeScan

先看 LaserRangeScan

主要有两个参数：储存range readings的 成员参数，记录上个参数的size的 成员参数。

其他就是一些得到参数，设置参数的成员函数。

  /**
   * LaserRangeScan representing the range readings from a laser range finder sensor.
   */
  class LaserRangeScan : public SensorData
  {
  public:
    // @cond EXCLUDE
    KARTO_Object(LaserRangeScan)
    // @endcond

  public:
    /**
     * Constructs a scan from the given sensor with the given readings
     * @param rSensorName
     */
    LaserRangeScan(const Name& rSensorName)
      : SensorData(rSensorName)
      , m_pRangeReadings(NULL)
      , m_NumberOfRangeReadings(0)
    {
    }

    /**
     * Constructs a scan from the given sensor with the given readings
     * @param rSensorName
     * @param rRangeReadings
     */
    LaserRangeScan(const Name& rSensorName, const RangeReadingsVector& rRangeReadings)
      : SensorData(rSensorName)
      , m_pRangeReadings(NULL)
      , m_NumberOfRangeReadings(0)
    {
      assert(rSensorName.ToString() != "");

      SetRangeReadings(rRangeReadings);
    }

    /**
     * Destructor
     */
    virtual ~LaserRangeScan()
    {
      delete [] m_pRangeReadings;
    }

  public:
    /**
     * Gets the range readings of this scan
     * @return range readings of this scan
     */
    inline kt_double* GetRangeReadings() const
    {
      return m_pRangeReadings;
    }

    inline RangeReadingsVector GetRangeReadingsVector() const
    {
      return RangeReadingsVector(m_pRangeReadings, m_pRangeReadings + m_NumberOfRangeReadings);
    }

    /**
     * Sets the range readings for this scan
     * @param rRangeReadings
     */
    inline void SetRangeReadings(const RangeReadingsVector& rRangeReadings)
    {
      // ignore for now! XXXMAE BUGUBUG 05/21/2010 << TODO(lucbettaieb): What the heck is this??
      // if (rRangeReadings.size() != GetNumberOfRangeReadings())
      // {
      //   std::stringstream error;
      //   error << "Given number of readings (" << rRangeReadings.size()
      //         << ") does not match expected number of range finder (" << GetNumberOfRangeReadings() << ")";
      //   throw Exception(error.str());
      // }

      if (!rRangeReadings.empty())
      {
        if (rRangeReadings.size() != m_NumberOfRangeReadings)
        {
          // delete old readings
          delete [] m_pRangeReadings;

          // store size of array!
          m_NumberOfRangeReadings = static_cast<kt_int32u>(rRangeReadings.size());

          // allocate range readings
          m_pRangeReadings = new kt_double[m_NumberOfRangeReadings];
        }

        // copy readings
        kt_int32u index = 0;
        const_forEach(RangeReadingsVector, &rRangeReadings)
        {
          m_pRangeReadings[index++] = *iter;
        }
      }
      else
      {
        delete [] m_pRangeReadings;
        m_pRangeReadings = NULL;
      }
    }

    /**
     * Gets the laser range finder sensor that generated this scan
     * @return laser range finder sensor of this scan
     */
    inline LaserRangeFinder* GetLaserRangeFinder() const
    {
      return SensorManager::GetInstance()->GetSensorByName<LaserRangeFinder>(GetSensorName());
    }

    /**
     * Gets the number of range readings
     * @return number of range readings
     */
    inline kt_int32u GetNumberOfRangeReadings() const
    {
      return m_NumberOfRangeReadings;
    }

  private:
    LaserRangeScan(const LaserRangeScan&);
    const LaserRangeScan& operator=(const LaserRangeScan&);

  private:
    kt_double* m_pRangeReadings;
    kt_int32u m_NumberOfRangeReadings;
  };  // LaserRangeScan

上面两个参数：

一帧scan的所有距离值，指向数组的指针
一帧scan的点数，也就是数组的个数

3. 然后是LocalizedRangeScan

  /**
   * The LocalizedRangeScan contains range data from a single sweep of a laser range finder sensor
   * in a two-dimensional space and position information. The odometer position is the position
   * reported by the robot when the range data was recorded. The corrected position is the position
   * calculated by the mapper (or localizer)
   */
  class LocalizedRangeScan : public LaserRangeScan

储存的是激光数据、里程计表储存的是记录激光数据时机器人的位姿、矫正后的正确位姿记录的由mapper计算出的对应的位置。

主要参数包括，odom坐标下的机器人位置，mapper得到的机器人pose，readings中心pose，世界坐标系下的readings，过滤之前的readings，帧雷达数据的边框。

    /**
     * Odometric pose of robot
     */
    Pose2 m_OdometricPose;

    /**
     * Corrected pose of robot calculated by mapper (or localizer)
     */
    Pose2 m_CorrectedPose;

  protected:
    /**
     * Average of all the point readings
     * 所有point的readings的平均值，中心位置的点
     */
    Pose2 m_BarycenterPose;

    /**
     * Vector of point readings
     * 这里存储了将激光数据转换为在世界坐标下的二维坐标结果，在update函数实现。
     */
    PointVectorDouble m_PointReadings;

    /**
     * Vector of unfiltered point readings
     * 过滤之前的集合
     */
    PointVectorDouble m_UnfilteredPointReadings;

    /**
     * Bounding box of localized range scan
     * Bounding box：障碍物的边框？？？
     * 这帧雷达数据的边框
     */
    BoundingBox2 m_BoundingBox;

    /**
     * Internal flag used to update point readings, barycenter and bounding box
     */
    kt_bool m_IsDirty;

主要成员函数，可以实现设置这一帧对应的里程计位姿，更正后的里程计姿态（后面运行程序看看在哪个坐标系下）,计算point readings' center, 计算传感器位置(相对于机器人的位置偏置已知)相关。比较重要的包括：

1. 得到这一帧的边框：

   /**
     * Gets the bounding box of this scan
     * @return bounding box of this scan
     */
    inline const BoundingBox2& GetBoundingBox() const

2. 得到在地图坐标下的point readings 这里其实就是返回的m_pointreadings 参数，

别的函数里看的觉得point readings是

在odom

或者mapper计算下的坐标系，

或者是在世界坐标系下的二维坐标结果，

还没有一个统一的认识。（后续更新，应该是map或世界坐标系，由mapper或者定位模块计算的）

    /**
     * Get point readings in local coordinates
     */
    inline const PointVectorDouble& GetPointReadings(kt_bool wantFiltered = false) const

3. 更新函数主要是计算point readings，以及计算这一帧的boundingBox

    /**
     * Compute point readings based on range readings
     * Only range readings within [minimum range; range threshold] are returned
     */
    virtual void Update()
    {
      LaserRangeFinder* pLaserRangeFinder = GetLaserRangeFinder(); //得到生成这一帧的激光雷达装置

      if (pLaserRangeFinder != NULL)
      {
        m_PointReadings.clear(); //激光数据转化为在世界坐标下的二维坐标结果
        m_UnfilteredPointReadings.clear();

        kt_double rangeThreshold = pLaserRangeFinder->GetRangeThreshold();
        kt_double minimumAngle = pLaserRangeFinder->GetMinimumAngle();
        kt_double angularResolution = pLaserRangeFinder->GetAngularResolution();
        Pose2 scanPose = GetSensorPose(); //得到传感器的位置 , 目前认为是在odom坐标系下表示

        // compute point readings
        Vector2<kt_double> rangePointsSum; // 点readings的和， 在real space 下的 x y
        kt_int32u beamNum = 0;
        for (kt_int32u i = 0; i < pLaserRangeFinder->GetNumberOfRangeReadings(); i++, beamNum++)
        {
          kt_double rangeReading = GetRangeReadings()[i]; // 得到一帧的第i个光束
          if (!math::InRange(rangeReading, pLaserRangeFinder->GetMinimumRange(), rangeThreshold))
          {
            kt_double angle = scanPose.GetHeading() + minimumAngle + beamNum * angularResolution;

            Vector2<kt_double> point;
            point.SetX(scanPose.GetX() + (rangeReading * cos(angle))); //目前认为是在odom坐标系下表示 计算的到point readings 由rang readings --> point readings 
            point.SetY(scanPose.GetY() + (rangeReading * sin(angle)));

            m_UnfilteredPointReadings.push_back(point);
            continue;
          }

          kt_double angle = scanPose.GetHeading() + minimumAngle + beamNum * angularResolution;

          Vector2<kt_double> point;
          point.SetX(scanPose.GetX() + (rangeReading * cos(angle)));
          point.SetY(scanPose.GetY() + (rangeReading * sin(angle)));

          m_PointReadings.push_back(point);
          m_UnfilteredPointReadings.push_back(point);

          rangePointsSum += point;
        }

        // compute barycenter
        kt_double nPoints = static_cast<kt_double>(m_PointReadings.size());
        if (nPoints != 0.0)
        {
          Vector2<kt_double> averagePosition = Vector2<kt_double>(rangePointsSum / nPoints);
          m_BarycenterPose = Pose2(averagePosition, 0.0);
        }
        else
        {
          m_BarycenterPose = scanPose;
        }

        // calculate bounding box of scan
        m_BoundingBox = BoundingBox2(); // 默认无参构造函数
        m_BoundingBox.Add(scanPose.GetPosition());
        //得到这一帧的左下角坐标，右上角坐标，也就边框找到了
        forEach(PointVectorDouble, &m_PointReadings)
        {
          m_BoundingBox.Add(*iter);
        }
      }

      m_IsDirty = false;
    }

参考：https://github.com/kadn/open_karto

得出的结论，

  class LaserRangeScan : public SensorData   //这里面存储了最原始的扫描深度数据，然后在LocalizedRangeScan中存储了扫描点在世界坐标系中的位置

    inline Pose2 GetSensorPose() const
    {
      return GetSensorAt(m_CorrectedPose);   //基于修正的robot位置以及之前设定的laser scan相对于 robot的位置来求出laser scan在地图中的位置
    }

m_CorrectedPose 是地图（map、世界）坐标系中的位置， m_Odo.... 是odom坐标系下的

        m_PointReadings.clear(); //激光数据转化为在世界坐标下（map）的二维坐标结果

            point.SetX(scanPose.GetX() + (rangeReading * cos(angle))); //目前认为是在map坐标系下表示 计算的到point readings 由rang readings --> point readings 

代码中的注释：local coordinates指的也是map frame？？？

rPose1.GetPosition()得到的是pose1的坐标系相对世界坐标系平移，rPose1.GetHeading() 是旋转

附录：调试信息（没有用，不用看）

//---------------------------
Vector2<kt_double> pointmin = m_PointReadings[0];
Vector2<kt_double> pointmax = m_PointReadings[0];
for(int i = 0; i < m_PointReadings.size(); i++)
{
  if(pointmin.GetX() > m_PointReadings[i].GetX() && pointmin.GetY() > m_PointReadings[i].GetY() )
  {
    pointmin.SetX(m_PointReadings[i].GetX());
    pointmin.SetY(m_PointReadings[i].GetY());
  }
  if(pointmax.GetX() < m_PointReadings[i].GetX() && pointmax.GetY() < m_PointReadings[i].GetY() )
  {
    pointmin.SetX(m_PointReadings[i].GetX());
    pointmin.SetY(m_PointReadings[i].GetY());
  }    
}
pointmin.SetX(pointmin.GetX() / 3);
pointmin.SetY(pointmin.GetY() / 3);
m_PointReadings.push_back(pointmin);
pointmax.SetX(pointmax.GetX() * 2);
pointmax.SetY(pointmax.GetY() * 2);
m_PointReadings.push_back(pointmax);
//---------------------------