1.karto-slam涉及的类-雷达以及雷达数据相关
首先是最简单的
1. sensor_msgs::LaserScan, 主要包括header、还有激光参数（扫射范围距离，步长，时间等，不包含位姿信息，header里面含有frame_id）。
typedef ::sensor_msgs::LaserScan_ > LaserScan; struct LaserScan_
{
typedef LaserScan_ 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_ _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::other > _ranges_type;
_ranges_type ranges;
typedef std::vector::other > _intensities_type;
_intensities_type intensities;
typedef boost::shared_ptr< ::sensor_msgs::LaserScan_ > Ptr;
typedef boost::shared_ptr< ::sensor_msgs::LaserScan_ 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(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(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() const2. 得到在地图坐标下的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 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 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 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(m_PointReadings.size());
if (nPoints != 0.0)
{
Vector2 averagePosition = Vector2(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 pointmin = m_PointReadings[0];
Vector2 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);
//---------------------------