1 类简介

1.1 OccupancyGrid类

2 static OccupancyGrid* CreateFromScans()

2.1 static void ComputeDimensions()

2.2 OccupancyGrid()

3 void CreateFromScans()

3.1 kt_bool AddScan()

3.2 kt_bool RayTrace()

3.3 void Grid::TraceLine()

3.4 virtual void Update()

3.5 virtual void UpdateCell()

slam_karto中的updateMap()调用了karto::OccupancyGrid::CreateFromScans()生成了栅格地图，这篇文章就讲一下栅格地图如何生成的。

bool
SlamKarto::updateMap()
{boost::mutex::scoped_lock lock(map_mutex_);karto::OccupancyGrid* occ_grid = karto::OccupancyGrid::CreateFromScans(mapper_->GetAllProcessedScans(), resolution_);
// ...
}

1 类简介

1.1 OccupancyGrid类

首先看一下OccupancyGrid类的成员变量

  /*** Occupancy grid definition. See GridStates for possible grid values.*/class OccupancyGrid : public Grid<kt_int8u>{friend class CellUpdater;friend class IncrementalOccupancyGrid;protected:/*** Counters of number of times a beam passed through a cell*/Grid<kt_int32u>* m_pCellPassCnt;/*** Counters of number of times a beam ended at a cell*/Grid<kt_int32u>* m_pCellHitsCnt;private:/*** Restrict the copy constructor*/OccupancyGrid(const OccupancyGrid&);/*** Restrict the assignment operator*/const OccupancyGrid& operator=(const OccupancyGrid&);private:CellUpdater* m_pCellUpdater;// NOTE: These two values are dependent on the resolution.  If the resolution is too small,// then not many beams will hit the cell!// Number of beams that must pass through a cell before it will be considered to be occupied// or unoccupied.  This prevents stray beams from messing up the map.Parameter<kt_int32u>* m_pMinPassThrough;// Minimum ratio of beams hitting cell to beams passing through cell for cell to be marked as occupiedParameter<kt_double>* m_pOccupancyThreshold;};  // OccupancyGrid

1.2 CellUpdater类（未进行使用）

  class KARTO_EXPORT CellUpdater : public Functor{public:CellUpdater(OccupancyGrid* pGrid): m_pOccupancyGrid(pGrid){}/*** Updates the cell at the given index based on the grid's hits and pass counters* @param index*/void CellUpdater::operator() (kt_int32u index){kt_int8u* pDataPtr = m_pOccupancyGrid->GetDataPointer();kt_int32u* pCellPassCntPtr = m_pOccupancyGrid->m_pCellPassCnt->GetDataPointer();kt_int32u* pCellHitCntPtr = m_pOccupancyGrid->m_pCellHitsCnt->GetDataPointer();m_pOccupancyGrid->UpdateCell(&pDataPtr[index], pCellPassCntPtr[index], pCellHitCntPtr[index]);}private:OccupancyGrid* m_pOccupancyGrid;};  // CellUpdater

2 static OccupancyGrid* CreateFromScans()

    /*** Create an occupancy grid from the given scans using the given resolution* @param rScans* @param resolution*/static OccupancyGrid* CreateFromScans(const LocalizedRangeScanVector& rScans, kt_double resolution){if (rScans.empty()){return NULL;}kt_int32s width, height;Vector2<kt_double> offset;ComputeDimensions(rScans, resolution, width, height, offset);OccupancyGrid* pOccupancyGrid = new OccupancyGrid(width, height, offset, resolution);pOccupancyGrid->CreateFromScans(rScans);return pOccupancyGrid;}

2.1 static void ComputeDimensions()

    /*** Calculate grid dimensions from localized range scans* @param rScans* @param resolution* @param rWidth* @param rHeight* @param rOffset*/static void ComputeDimensions(const LocalizedRangeScanVector& rScans,kt_double resolution,kt_int32s& rWidth,kt_int32s& rHeight,Vector2<kt_double>& rOffset){BoundingBox2 boundingBox;// 得到所有scan的总体boundingBoxconst_forEach(LocalizedRangeScanVector, &rScans){boundingBox.Add((*iter)->GetBoundingBox());}kt_double scale = 1.0 / resolution;Size2<kt_double> size = boundingBox.GetSize();// 坐标值除以分辨率等于栅格的个数rWidth = static_cast<kt_int32s>(math::Round(size.GetWidth() * scale));rHeight = static_cast<kt_int32s>(math::Round(size.GetHeight() * scale));rOffset = boundingBox.GetMinimum(); // 左下角的坐标值}

2.1.1 BoundingBox2

BoundingBox2 只是储存了矩形左下角坐标与右上角坐标

  /*** Defines a bounding box in 2-dimensional real space.*/class BoundingBox2{public:/*** Get bounding box minimum*/inline const Vector2<kt_double>& GetMinimum() const{return m_Minimum;}/*** Get the size of the bounding box 获取2坐标的差值*/inline Size2<kt_double> GetSize() const{Vector2<kt_double> size = m_Maximum - m_Minimum;return Size2<kt_double>(size.GetX(), size.GetY());}/*** Add vector to bounding box*/inline void Add(const Vector2<kt_double>& rPoint){m_Minimum.MakeFloor(rPoint);// m_Minimum的x和y坐标 如果比 rPoint 大，则用 rPoint 的x或y赋值m_Maximum.MakeCeil(rPoint); // m_Maximum的x和y坐标 如果比 rPoint 小，则用 rPoint 的x或y赋值}/*** Add other bounding box to bounding box*/inline void Add(const BoundingBox2& rBoundingBox){Add(rBoundingBox.GetMinimum());Add(rBoundingBox.GetMaximum());}private:Vector2<kt_double> m_Minimum;Vector2<kt_double> m_Maximum;};  // BoundingBox2

2.2 OccupancyGrid()

    /*** Constructs an occupancy grid of given size* @param width* @param height* @param rOffset* @param resolution*/OccupancyGrid(kt_int32s width, kt_int32s height, const Vector2<kt_double>& rOffset, kt_double resolution): Grid<kt_int8u>(width, height), m_pCellPassCnt(Grid<kt_int32u>::CreateGrid(0, 0, resolution)), m_pCellHitsCnt(Grid<kt_int32u>::CreateGrid(0, 0, resolution)), m_pCellUpdater(NULL){m_pCellUpdater = new CellUpdater(this);if (karto::math::DoubleEqual(resolution, 0.0)){throw Exception("Resolution cannot be 0");}m_pMinPassThrough = new Parameter<kt_int32u>("MinPassThrough", 2);m_pOccupancyThreshold = new Parameter<kt_double>("OccupancyThreshold", 0.1);GetCoordinateConverter()->SetScale(1.0 / resolution);GetCoordinateConverter()->SetOffset(rOffset);  // 左下角坐标值}

3 void CreateFromScans()

    /*** Create grid using scans* @param rScans*/virtual void CreateFromScans(const LocalizedRangeScanVector& rScans){// 设置 pass 网格的size与左下角坐标m_pCellPassCnt->Resize(GetWidth(), GetHeight());m_pCellPassCnt->GetCoordinateConverter()->SetOffset(GetCoordinateConverter()->GetOffset());// 设置 Hit 网格的size与左下角坐标m_pCellHitsCnt->Resize(GetWidth(), GetHeight());m_pCellHitsCnt->GetCoordinateConverter()->SetOffset(GetCoordinateConverter()->GetOffset());const_forEach(LocalizedRangeScanVector, &rScans){LocalizedRangeScan* pScan = *iter;AddScan(pScan);}Update();}/*** Resizes the grid (deletes all old data)* @param width* @param height*/virtual void Resize(kt_int32s width, kt_int32s height){Grid<kt_int8u>::Resize(width, height);    // 同时也将基类中的 Grid resize 了m_pCellPassCnt->Resize(width, height);m_pCellHitsCnt->Resize(width, height);}

3.1 kt_bool AddScan()

    /*** Adds the scan's information to this grid's counters (optionally* update the grid's cells' occupancy status)* @param pScan* @param doUpdate whether to update the grid's cell's occupancy status* @return returns false if an endpoint fell off the grid, otherwise true*/virtual kt_bool AddScan(LocalizedRangeScan* pScan, kt_bool doUpdate = false){kt_double rangeThreshold = pScan->GetLaserRangeFinder()->GetRangeThreshold();kt_double maxRange = pScan->GetLaserRangeFinder()->GetMaximumRange(); // 0kt_double minRange = pScan->GetLaserRangeFinder()->GetMinimumRange(); // 80Vector2<kt_double> scanPosition = pScan->GetSensorPose().GetPosition();// get scan point readings,  false 代表未经过滤波的scanconst PointVectorDouble& rPointReadings = pScan->GetPointReadings(false);kt_bool isAllInMap = true;// draw lines from scan position to all point readings 从雷达坐标向着scan画线int pointIndex = 0;const_forEachAs(PointVectorDouble, &rPointReadings, pointsIter){Vector2<kt_double> point = *pointsIter; // 雷达数据点的坐标kt_double rangeReading = pScan->GetRangeReadings()[pointIndex];kt_bool isEndPointValid = rangeReading < (rangeThreshold - KT_TOLERANCE); // 是否小于12米if (rangeReading <= minRange || rangeReading >= maxRange || std::isnan(rangeReading)){// ignore these readingspointIndex++;continue;}else if (rangeReading >= rangeThreshold) // 大于12米的点进行裁剪，距离越远有效距离越近{// trace up to range readingkt_double ratio = rangeThreshold / rangeReading;kt_double dx = point.GetX() - scanPosition.GetX();kt_double dy = point.GetY() - scanPosition.GetY();point.SetX(scanPosition.GetX() + ratio * dx);point.SetY(scanPosition.GetY() + ratio * dy);}kt_bool isInMap = RayTrace(scanPosition, point, isEndPointValid, doUpdate);if (!isInMap){isAllInMap = false;}pointIndex++;}return isAllInMap;}

3.2 kt_bool RayTrace()

    /*** Traces a beam from the start position to the end position marking* the bookkeeping arrays accordingly.* @param rWorldFrom start position of beam* @param rWorldTo end position of beam* @param isEndPointValid is the reading within the range threshold?* @param doUpdate whether to update the cells' occupancy status immediately* @return returns false if an endpoint fell off the grid, otherwise true*/virtual kt_bool RayTrace(const Vector2<kt_double>& rWorldFrom,const Vector2<kt_double>& rWorldTo,kt_bool isEndPointValid,kt_bool doUpdate = false){assert(m_pCellPassCnt != NULL && m_pCellHitsCnt != NULL);Vector2<kt_int32s> gridFrom = m_pCellPassCnt->WorldToGrid(rWorldFrom);Vector2<kt_int32s> gridTo = m_pCellPassCnt->WorldToGrid(rWorldTo);CellUpdater* pCellUpdater = doUpdate ? m_pCellUpdater : NULL;m_pCellPassCnt->TraceLine(gridFrom.GetX(), gridFrom.GetY(), gridTo.GetX(), gridTo.GetY(), pCellUpdater);// for the end pointif (isEndPointValid){if (m_pCellPassCnt->IsValidGridIndex(gridTo)){kt_int32s index = m_pCellPassCnt->GridIndex(gridTo, false);kt_int32u* pCellPassCntPtr = m_pCellPassCnt->GetDataPointer();kt_int32u* pCellHitCntPtr = m_pCellHitsCnt->GetDataPointer();// increment cell pass through and hit countpCellPassCntPtr[index]++;pCellHitCntPtr[index]++;if (doUpdate){(*m_pCellUpdater)(index);}}}return m_pCellPassCnt->IsValidGridIndex(gridTo);}

3.3 void Grid::TraceLine()

    /*** Increments all the grid cells from (x0, y0) to (x1, y1);* if applicable, apply f to each cell traced* @param x0* @param y0* @param x1* @param y1* @param f*/void Grid::TraceLine(kt_int32s x0, kt_int32s y0, kt_int32s x1, kt_int32s y1, Functor* f = NULL){kt_bool steep = abs(y1 - y0) > abs(x1 - x0);if (steep)           // 如果steep为true，将坐标关于y=x做对称，保持斜率小于45度{std::swap(x0, y0);std::swap(x1, y1);}if (x0 > x1)         // 坐标调换位置，保持x0在左边{std::swap(x0, x1);std::swap(y0, y1);}kt_int32s deltaX = x1 - x0;kt_int32s deltaY = abs(y1 - y0);kt_int32s error = 0;kt_int32s ystep;     // 向上走还是向下走kt_int32s y = y0;if (y0 < y1){ystep = 1;}else{ystep = -1;}kt_int32s pointX;kt_int32s pointY;for (kt_int32s x = x0; x <= x1; x++){if (steep){pointX = y;pointY = x;}else{pointX = x;pointY = y;}error += deltaY;if (2 * error >= deltaX){y += ystep;error -= deltaX;}Vector2<kt_int32s> gridIndex(pointX, pointY);if (IsValidGridIndex(gridIndex)){kt_int32s index = GridIndex(gridIndex, false);  // 二维坐标变为1维索引T* pGridPointer = GetDataPointer();             pGridPointer[index]++;                          // index处的栅格储存的值+1if (f != NULL){(*f)(index);}}}}

    /*** Checks whether the given coordinates are valid grid indices* @param rGrid*/inline kt_bool Grid::IsValidGridIndex(const Vector2<kt_int32s>& rGrid) const{return (math::IsUpTo(rGrid.GetX(), m_Width) && math::IsUpTo(rGrid.GetY(), m_Height));}/*** Checks whether value is in the range [0;maximum)* @param value* @param maximum*/template<typename T>inline kt_bool math::IIsUpTo(const T& value, const T& maximum){return (value >= 0 && value < maximum);}/*** Gets the index into the data pointer of the given grid coordinate* @param rGrid* @param boundaryCheck default value is true* @return grid index*/virtual kt_int32s Grid::GridIndex(const Vector2<kt_int32s>& rGrid, kt_bool boundaryCheck = true) const{if (boundaryCheck == true){if (IsValidGridIndex(rGrid) == false){std::stringstream error;error << "Index " << rGrid << " out of range.  Index must be between [0; "<< m_Width << ") and [0; " << m_Height << ")";throw Exception(error.str());}}kt_int32s index = rGrid.GetX() + (rGrid.GetY() * m_WidthStep);if (boundaryCheck == true){assert(math::IsUpTo(index, GetDataSize()));}return index;}

3.4 virtual void Update()

调用这个函数将更新整个栅格地图，并设置栅格地图的占用状态。更新时是通过pass栅格地图与hit栅格地图中对应栅格的比值进行判断的，hit中的1个值需要pass中的10个值来进行取消（占用的比例为0.1），Grid中的地图，与pass地图，hit地图的大小都一样，Resize()中实现。

Update()在CreateFromScans()中只调用1次，同时维护3个等大小的地图，2个中间地图，一个最终地图。

    /*** Update the grid based on the values in m_pCellHitsCnt and m_pCellPassCnt*/virtual void Update(){assert(m_pCellPassCnt != NULL && m_pCellHitsCnt != NULL);// clear gridClear();// set occupancy status of cellskt_int8u* pDataPtr = GetDataPointer();kt_int32u* pCellPassCntPtr = m_pCellPassCnt->GetDataPointer();kt_int32u* pCellHitCntPtr = m_pCellHitsCnt->GetDataPointer();kt_int32u nBytes = GetDataSize();for (kt_int32u i = 0; i < nBytes; i++, pDataPtr++, pCellPassCntPtr++, pCellHitCntPtr++){UpdateCell(pDataPtr, *pCellPassCntPtr, *pCellHitCntPtr);}}/*** Clear out the grid data*/void Grid::Clear(){memset(m_pData, 0, GetDataSize() * sizeof(T));}

3.5 virtual void UpdateCell()

    /*** Updates a single cell's value based on the given counters* @param pCell* @param cellPassCnt* @param cellHitCnt*/virtual void UpdateCell(kt_int8u* pCell, kt_int32u cellPassCnt, kt_int32u cellHitCnt){if (cellPassCnt > m_pMinPassThrough->GetValue())    // pass栅格图中的值是否大于2{kt_double hitRatio = static_cast<kt_double>(cellHitCnt) / static_cast<kt_double>(cellPassCnt);if (hitRatio > m_pOccupancyThreshold->GetValue()) // hitRatio 是否大于 0.1{                                                 // hit中的1个值需要pass中的10个值来取消掉*pCell = GridStates_Occupied;     // 100 }else{*pCell = GridStates_Free;         // 255}}}

karto探秘之open_karto 第五章 --- 栅格地图的生成相关推荐

Python实现占用栅格地图的生成（Occupancy Grid Generation）
本文的算法细节及推导可以参考Sebastian Thrun的<概率机器人>中占用栅格地图构建部分. 1. 导入所需要的库 import numpy as np import math im ...
第十五章栅格数据重分类、栅格计算器、插值分析
文章目录第十五章栅格数据分析第一章栅格数据重分类第一节栅格数据重分类第二节栅格重分类的使用第三节重分类的使用中的空值使用第四节重分类的案例:分类统计面积第五节坡度矢量分级图 ...
Motion planning for self-driving cars课程笔记1：应用雷达数据生成占用栅格地图（Occupancy Grid Map）
此文章为Motion planning for self-driving cars上第二课的笔记,主要讲述占据栅格地图的生成.由于课程中算法也是参考<probability robot>这 ...
ORB-SLAM2栅格地图构建
ORB-SLAM2栅格地图构建过程栅格地图的构建是基于稠密点云地图的构建和保存实现的,需要了解可以看我们前面的博客基于ORB-SLAM2实时构建稠密点云在点云地图的基础上构建包含占据信息的八叉 ...
matlab构建栅格地图绘图思路
matlab构建栅格地图绘图思路近来因研究需要,调研并思考了栅格地图的生成方法,暂时总结以备不时之需. 栅格的建立最需要注意栅格粒度的问题,即根据需要调整栅格的边长,目前有两种思路一种是固定栅格边长 ...
《NodeJS开发指南》第五章微博实例开发总结
所有文章搬运自我的个人主页:sheilasun.me <NodeJS开发指南>这本书用来NodeJS入门真是太好了,而且书的附录部分还讲到了闭包.this等JavaScript常用特性.第 ...
【转载】【《Real-Time Rendering 3rd》提炼总结】(四) 第五章 · 图形渲染与视觉外观 The Visual Appearance
本文由@浅墨_毛星云出品,转载请注明出处. 文章链接: http://blog.csdn.net/poem_qianmo/article/details/72857602 这篇文章将总结和提炼& ...
计算机辅助设计应用教程,计算机辅助设计基础教程第五章.ppt
计算机辅助设计基础教程第五章第5章 3ds max 5.1 3ds max基础知识 5.2 对象的创建与编辑 5.3 复合对象的创建 5.4 NURBS建模 5.5 材质和贴图 5.6 摄影机.灯光 ...
【《Real-Time Rendering 3rd》提炼总结】(四) 第五章 · 图形渲染与视觉外观 The Visual Appearance
本文由@浅墨_毛星云出品,转载请注明出处. 文章链接: http://blog.csdn.net/poem_qianmo/article/details/72857602 这篇文章将总结和提炼& ...
ArcGIS for Desktop入门教程_第五章_ArcCatalog使用 - ArcGIS知乎-新一代ArcGIS问答社区
原文:ArcGIS for Desktop入门教程_第五章_ArcCatalog使用 - ArcGIS知乎-新一代ArcGIS问答社区 1 ArcCatalog使用 1.1 GIS数据地理信息系统, ...

karto探秘之open_karto 第五章 --- 栅格地图的生成

1 类简介

1.1 OccupancyGrid类

1.2 CellUpdater类（未进行使用）

2 static OccupancyGrid* CreateFromScans()

2.1 static void ComputeDimensions()

2.1.1 BoundingBox2

2.2 OccupancyGrid()

3 void CreateFromScans()

3.1 kt_bool AddScan()

3.2 kt_bool RayTrace()

3.3 void Grid::TraceLine()

3.4 virtual void Update()

3.5 virtual void UpdateCell()

karto探秘之open_karto 第五章 --- 栅格地图的生成相关推荐

最新文章

热门文章