Class representing an occupancy grid. More...
#include <gz/math/OccupancyGrid.hh>
Public Member Functions | |
OccupancyGrid (double _resolutionMeters, int _widthCells, int _heightCells, double _originX=0.0, double _originY=0.0) | |
Constructor. | |
int | CalculateIGain (int _x0, int _y0, int _x1, int _y1) |
Calculate information gain along a line. This is useful when implementing safe frontier exploration algorithms. | |
OccupancyCellState | CellState (int _gridX, int _gridY) const |
Get the state of a cell. | |
void | CellState (int _gridX, int _gridY, OccupancyCellState _state) |
Set the state of a cell. | |
void | ExportToRGBImage (std::vector< uint8_t > &_pixels) const |
Export the occupancy grid to a RGB image buffer. | |
void | GridToWorld (int _gridX, int _gridY, double &_worldX, double &_worldY) const |
Convert grid coordinates (cells) to world coordinates (meters - center of cell). | |
int | Height () const |
Get the number of cells in height. | |
bool | IsValidGridCoordinate (int _gridX, int _gridY) const |
Check if grid coordinates are within bounds. | |
bool | MarkFree (double _worldX0, double _worldY0, double _worldX1, double _worldY1) |
Mark a path as free (e.g., a clear line of sight). | |
void | MarkLine (int _x0, int _y0, int _x1, int _y1, OccupancyCellState _state) |
Use Bresenham's Line Algorithm to mark cells along a line. This function will not modify cells that are already marked as Occupied. It marks cells from (_x0, _y0) to (_x1, _y1) with the specified state. | |
bool | MarkOccupied (double _worldX, double _worldY) |
Mark a single point as occupied (e.g., an obstacle detection). | |
double | OriginX () const |
Get the origin X position. | |
double | OriginY () const |
Get the origin Y position. | |
void | RawOccupancy (std::vector< int8_t > &_data) const |
Export the occupancy grid to a raw buffer. | |
double | Resolution () const |
Get the resolution of the occupancy grid. | |
int | Width () const |
Get the number of cells in width. | |
bool | WorldToGrid (double _worldX, double _worldY, int &_gridX, int &_gridY) const |
Convert world coordinates (meters) to grid coordinates (cells). | |
Detailed Description
Class representing an occupancy grid.
Constructor & Destructor Documentation
◆ OccupancyGrid()
OccupancyGrid | ( | double | _resolutionMeters, |
int | _widthCells, | ||
int | _heightCells, | ||
double | _originX = 0.0 , |
||
double | _originY = 0.0 |
||
) |
Constructor.
- Parameters
-
[in] _resolutionMeters Resolution of the grid in meters per cell. [in] _widthCells Width of the grid in cells. [in] _heightCells Height of the grid in cells. [in] _originX X-coordinate of the grid's origin in world coordinates. [in] _originY Y-coordinate of the grid's origin in world coordinates.
Member Function Documentation
◆ CalculateIGain()
Calculate information gain along a line. This is useful when implementing safe frontier exploration algorithms.
- Parameters
-
[in] _x0 X coordinate of the start point in cells. [in] _y0 Y coordinate of the start point in cells. [in] _x1 X coordinate of the end point in cells. [in] _y1 Y coordinate of the end point in cells.
- Returns
- The information gain.
◆ CellState() [1/2]
OccupancyCellState CellState | ( | int | _gridX, |
int | _gridY | ||
) | const |
Get the state of a cell.
- Parameters
-
[in] _gridX Grid X coordinate in cells. [in] _gridY Grid Y coordinate in cells.
- Returns
- The state of the cell.
◆ CellState() [2/2]
void CellState | ( | int | _gridX, |
int | _gridY, | ||
OccupancyCellState | _state | ||
) |
Set the state of a cell.
- Parameters
-
[in] _gridX Grid X coordinate in cells. [in] _gridY Grid Y coordinate in cells. [in] _state The new state of the cell.
◆ ExportToRGBImage()
void ExportToRGBImage | ( | std::vector< uint8_t > & | _pixels | ) | const |
Export the occupancy grid to a RGB image buffer.
- Parameters
-
[out] _pixels The output buffer to store the RGB image data.
◆ GridToWorld()
Convert grid coordinates (cells) to world coordinates (meters - center of cell).
- Parameters
-
[in] _gridX Grid X coordinate in cells. [in] _gridY Grid Y coordinate in cells. [out] _worldX World X coordinate in meters. [out] _worldY World Y coordinate in meters.
◆ Height()
int Height | ( | ) | const |
Get the number of cells in height.
- Returns
- The height of the grid in cells.
◆ IsValidGridCoordinate()
Check if grid coordinates are within bounds.
- Parameters
-
[in] _gridX Grid X coordinate in cells. [in] _gridY Grid Y coordinate in cells.
- Returns
- True if the coordinates are valid, false otherwise.
◆ MarkFree()
Mark a path as free (e.g., a clear line of sight).
- Parameters
-
[in] _worldX0 World X coordinate of the start point in meters. [in] _worldY0 World Y coordinate of the start point in meters. [in] _worldX1 World X coordinate of the end point in meters. [in] _worldY1 World Y coordinate of the end point in meters.
- Returns
- true if the grid was inside the coordinates, false otherwise
◆ MarkLine()
Use Bresenham's Line Algorithm to mark cells along a line. This function will not modify cells that are already marked as Occupied. It marks cells from (_x0, _y0) to (_x1, _y1) with the specified state.
- Parameters
-
[in] _x0 X coordinate of the start point in cells. [in] _y0 Y coordinate of the start point in cells. [in] _x1 X coordinate of the end point in cells. [in] _y1 Y coordinate of the end point in cells. [in] _state The state to mark the cells with.
◆ MarkOccupied()
Mark a single point as occupied (e.g., an obstacle detection).
- Parameters
-
[in] _worldX World X coordinate in meters. [in] _worldY World Y coordinate in meters.
- Returns
- true if the grid was inside the coordinates, false otherwise
◆ OriginX()
double OriginX | ( | ) | const |
Get the origin X position.
- Returns
- The X-coordinate of the grid's origin in world coordinates.
◆ OriginY()
double OriginY | ( | ) | const |
Get the origin Y position.
- Returns
- The Y-coordinate of the grid's origin in world coordinates.
◆ RawOccupancy()
void RawOccupancy | ( | std::vector< int8_t > & | _data | ) | const |
Export the occupancy grid to a raw buffer.
- Parameters
-
[out] _data The output buffer to store the raw occupancy data.
◆ Resolution()
double Resolution | ( | ) | const |
Get the resolution of the occupancy grid.
- Returns
- The resolution in meters per cell.
◆ Width()
int Width | ( | ) | const |
Get the number of cells in width.
- Returns
- The width of the grid in cells.
◆ WorldToGrid()
Convert world coordinates (meters) to grid coordinates (cells).
- Parameters
-
[in] _worldX World X coordinate in meters. [in] _worldY World Y coordinate in meters. [out] _gridX Grid X coordinate in cells. [out] _gridY Grid Y coordinate in cells.
- Returns
- True if coordinates are within bounds, false otherwise. May also
The documentation for this class was generated from the following file: