CwmGrid3D Class Reference

#include <wmGrid3D.h>

Collaboration diagram for CwmGrid3D:

Public Member Functions

 CwmGrid3D ()
 
 ~CwmGrid3D ()
 
wmStatus setSensorPosition (boost::numeric::ublas::matrix< float > inSensorPosition)
 Set Sensor position and orientation according to a 4x4 matrix [m]. More...
 
wmStatus getSensorPosition (boost::numeric::ublas::matrix< float > outSensorPosition)
 Get Sensor position and orientation according to a 4x4 matrix. More...
 
wmStatus updateMap ()
 Update map regarding the last rangemap which was set. More...
 
gridxyz2grid (double x, double y, double z)
 Returns the pointer to the data stored at (x,y,z). More...
 
wmStatus grid2xyz (int i, double &x, double &y, double &z)
 Returns the position of the grid [i]. More...
 
gridxyz2grid (int x, int y, int z)
 Returns the pointer to the data stored at (x,y,z). More...
 
wmStatus allocateMap (double xstart, double ystart, double zstart, double xend, double yend, double zend)
 Allocate the memory needed for the map. More...
 
wmStatus setFreeBox (double xstart, double ystart, double zstart, double xend, double yend, double zend)
 Allocate the defult free zone in the environmet (a free zone which is freezed to free in all update). More...
 
wmStatus makeBoxFree (double xstart, double ystart, double zstart, double xend, double yend, double zend)
 Change the status of a valume of the envirenment to free. More...
 
wmStatus makeBoxSolid (double xstart, double ystart, double zstart, double xend, double yend, double zend)
 Change the status of a valume of the envirenment to Solid. More...
 
wmStatus clearMap ()
 Reset the alocated map to Unknown. More...
 
wmStatus setResolution (double inResolution)
 Set the size of each grid [m]. More...
 
wmStatus setSensitivity (unsigned int inSensitivity)
 Set the sensetivity of update engin. More...
 
double getResolution ()
 Returns grid size. More...
 
box_area getMapVolume ()
 Returns the geometry of map in a vector as (xstart, ystart, zstart, xend, yend, zend). More...
 
wmStatus setRangeMap (boost::numeric::ublas::matrix< float > inRangeMap)
 Set the information regarding 3D position of the pixel in image. More...
 
int getMapXN ()
 Returns the number of grid along x. More...
 
int getMapYN ()
 Returns the number of grid along y. More...
 
int getMapZN ()
 Returns the number of grid along z. More...
 
gridgetGridMap ()
 Returns the number of grid. More...
 
wmStatus exportOccupancyMap (string fileName)
 Export the result of calculation (array of grids) in a txt file for graphic illustration. More...
 
wmStatus exportVRML (string modelFileName, string solidFileName, string unknownFileName, int exportMode)
 Export vrml file of the 3D model. More...
 
wmStatus importMap (string fileName)
 Import a 3D model. More...
 
wmStatus reportCurrentStatus ()
 Reports all parametere regarding the last update. More...
 
wmStatus setCameraParameters (double inWidthAngle, double inHeightAngle, double inObservableDistance)
 Sets camera parameter. More...
 
double getObservableDistance ()
 Get observable distance of camera. More...
 
wmStatus refreshMap ()
 Filter the noise in map. More...
 
double getVisibility (double inX, double inY, double inTheta, double goalX, double goalY, double goalTheta)
 Return the maximum number of unknown voxel which can be updated based on the robot config. More...
 
wmStatus setCoveringAngle (double inHeightAngleMin, double inHeightAngleMax, double inWidthAngleMin, double inWidthAngleMax)
 Set the maximum covering angle in a robot bounding box configuration. More...
 
wmStatus setGridState (std::vector< char > inStateVector)
 Set the state of each voxel in map by entering an state vector:0-Free, 1-Occupied, 2-Unknown. More...
 
wmStatus setRobotBoxPos (double inX, double inY, double inTheta)
 Set the robot position. More...
 
wmStatus setRobotBoxFree ()
 Set the robot bounding box as Free. More...
 

Protected Member Functions

double getGridParameter (double inMean, double inVariance, double inDistance, int k)
 Return the odd by getting the distance from a 3D observed point. More...
 

Protected Attributes

boost::numeric::ublas::matrix< float > attSensorPosition
 Sensor's position. More...
 
int attMapXN
 Number of grid along X, Y and Z. More...
 
int attMapYN
 
int attMapZN
 
box_area attMapArea
 Geometric information of Map. More...
 
box_area attFreeBox
 Geometric information of free volume of Map (Initial position of robot). More...
 
gridattGridMap
 The occupancy grid map. More...
 
double attResolution
 Resolution of the map. More...
 
unsigned int attSensitivity
 This parametr is the minimum number of point that should be detected in a pixel Volume to be considered as a solid object. More...
 
boost::numeric::ublas::matrix< float > attRangeMap
 vector that includes the Informatin of 3D position of pixel in image. More...
 
double attWidthAngle
 Camera Parametr:the visible angles of camera . More...
 
double attHeightAngle
 
double attObservableDistance
 the acceptable distance for filtering the valid information of 3D position More...
 
double attHeightAngleMin
 Camera Parameters for calculating the visibility. More...
 
double attHeightAngleMax
 
double attWidthAngleMin
 
double attWidthAngleMax
 
double attRobotPosX
 Robot position in each update. More...
 
double attRobotPosY
 
double attRobotPosTheta
 

Constructor & Destructor Documentation

◆ CwmGrid3D()

CwmGrid3D::CwmGrid3D ( )

◆ ~CwmGrid3D()

CwmGrid3D::~CwmGrid3D ( )

Member Function Documentation

◆ allocateMap()

wmStatus CwmGrid3D::allocateMap ( double  xstart,
double  ystart,
double  zstart,
double  xend,
double  yend,
double  zend 
)

Allocate the memory needed for the map.

◆ clearMap()

wmStatus CwmGrid3D::clearMap ( )

Reset the alocated map to Unknown.

◆ exportOccupancyMap()

wmStatus CwmGrid3D::exportOccupancyMap ( string  fileName)

Export the result of calculation (array of grids) in a txt file for graphic illustration.

◆ exportVRML()

wmStatus CwmGrid3D::exportVRML ( string  modelFileName,
string  solidFileName,
string  unknownFileName,
int  exportMode 
)

Export vrml file of the 3D model.

by choosing exportMode equal 1, VRML of solid and unknown part are generated seperately.

◆ getGridMap()

grid * CwmGrid3D::getGridMap ( )

Returns the number of grid.

◆ getGridParameter()

double CwmGrid3D::getGridParameter ( double  inMean,
double  inVariance,
double  inDistance,
int  k 
)
protected

Return the odd by getting the distance from a 3D observed point.

◆ getMapVolume()

box_area CwmGrid3D::getMapVolume ( )

Returns the geometry of map in a vector as (xstart, ystart, zstart, xend, yend, zend).

◆ getMapXN()

int CwmGrid3D::getMapXN ( )

Returns the number of grid along x.

◆ getMapYN()

int CwmGrid3D::getMapYN ( )

Returns the number of grid along y.

◆ getMapZN()

int CwmGrid3D::getMapZN ( )

Returns the number of grid along z.

◆ getObservableDistance()

double CwmGrid3D::getObservableDistance ( )

Get observable distance of camera.

◆ getResolution()

double CwmGrid3D::getResolution ( )

Returns grid size.

◆ getSensorPosition()

wmStatus CwmGrid3D::getSensorPosition ( boost::numeric::ublas::matrix< float >  outSensorPosition)

Get Sensor position and orientation according to a 4x4 matrix.

◆ getVisibility()

double CwmGrid3D::getVisibility ( double  inX,
double  inY,
double  inTheta,
double  goalX,
double  goalY,
double  goalTheta 
)

Return the maximum number of unknown voxel which can be updated based on the robot config.

◆ grid2xyz()

wmStatus CwmGrid3D::grid2xyz ( int  i,
double &  x,
double &  y,
double &  z 
)

Returns the position of the grid [i].

◆ importMap()

wmStatus CwmGrid3D::importMap ( string  fileName)

Import a 3D model.

◆ makeBoxFree()

wmStatus CwmGrid3D::makeBoxFree ( double  xstart,
double  ystart,
double  zstart,
double  xend,
double  yend,
double  zend 
)

Change the status of a valume of the envirenment to free.

◆ makeBoxSolid()

wmStatus CwmGrid3D::makeBoxSolid ( double  xstart,
double  ystart,
double  zstart,
double  xend,
double  yend,
double  zend 
)

Change the status of a valume of the envirenment to Solid.

◆ refreshMap()

wmStatus CwmGrid3D::refreshMap ( )

Filter the noise in map.

◆ reportCurrentStatus()

wmStatus CwmGrid3D::reportCurrentStatus ( )

Reports all parametere regarding the last update.

◆ setCameraParameters()

wmStatus CwmGrid3D::setCameraParameters ( double  inWidthAngle,
double  inHeightAngle,
double  inObservableDistance 
)

Sets camera parameter.

◆ setCoveringAngle()

wmStatus CwmGrid3D::setCoveringAngle ( double  inHeightAngleMin,
double  inHeightAngleMax,
double  inWidthAngleMin,
double  inWidthAngleMax 
)

Set the maximum covering angle in a robot bounding box configuration.

◆ setFreeBox()

wmStatus CwmGrid3D::setFreeBox ( double  xstart,
double  ystart,
double  zstart,
double  xend,
double  yend,
double  zend 
)

Allocate the defult free zone in the environmet (a free zone which is freezed to free in all update).

◆ setGridState()

wmStatus CwmGrid3D::setGridState ( std::vector< char >  inStateVector)

Set the state of each voxel in map by entering an state vector:0-Free, 1-Occupied, 2-Unknown.

◆ setRangeMap()

wmStatus CwmGrid3D::setRangeMap ( boost::numeric::ublas::matrix< float >  inRangeMap)

Set the information regarding 3D position of the pixel in image.

◆ setResolution()

wmStatus CwmGrid3D::setResolution ( double  inResolution)

Set the size of each grid [m].

◆ setRobotBoxFree()

wmStatus CwmGrid3D::setRobotBoxFree ( )

Set the robot bounding box as Free.

◆ setRobotBoxPos()

wmStatus CwmGrid3D::setRobotBoxPos ( double  inX,
double  inY,
double  inTheta 
)

Set the robot position.

◆ setSensitivity()

wmStatus CwmGrid3D::setSensitivity ( unsigned int  inSensitivity)

Set the sensetivity of update engin.

◆ setSensorPosition()

wmStatus CwmGrid3D::setSensorPosition ( boost::numeric::ublas::matrix< float >  inSensorPosition)

Set Sensor position and orientation according to a 4x4 matrix [m].

◆ updateMap()

wmStatus CwmGrid3D::updateMap ( )

Update map regarding the last rangemap which was set.

◆ xyz2grid() [1/2]

grid * CwmGrid3D::xyz2grid ( double  x,
double  y,
double  z 
)

Returns the pointer to the data stored at (x,y,z).

◆ xyz2grid() [2/2]

grid * CwmGrid3D::xyz2grid ( int  x,
int  y,
int  z 
)

Returns the pointer to the data stored at (x,y,z).

Member Data Documentation

◆ attFreeBox

box_area CwmGrid3D::attFreeBox
protected

Geometric information of free volume of Map (Initial position of robot).

◆ attGridMap

grid* CwmGrid3D::attGridMap
protected

The occupancy grid map.

◆ attHeightAngle

double CwmGrid3D::attHeightAngle
protected

◆ attHeightAngleMax

double CwmGrid3D::attHeightAngleMax
protected

◆ attHeightAngleMin

double CwmGrid3D::attHeightAngleMin
protected

Camera Parameters for calculating the visibility.

◆ attMapArea

box_area CwmGrid3D::attMapArea
protected

Geometric information of Map.

◆ attMapXN

int CwmGrid3D::attMapXN
protected

Number of grid along X, Y and Z.

◆ attMapYN

int CwmGrid3D::attMapYN
protected

◆ attMapZN

int CwmGrid3D::attMapZN
protected

◆ attObservableDistance

double CwmGrid3D::attObservableDistance
protected

the acceptable distance for filtering the valid information of 3D position

◆ attRangeMap

boost::numeric::ublas::matrix<float> CwmGrid3D::attRangeMap
protected

vector that includes the Informatin of 3D position of pixel in image.

◆ attResolution

double CwmGrid3D::attResolution
protected

Resolution of the map.

◆ attRobotPosTheta

double CwmGrid3D::attRobotPosTheta
protected

◆ attRobotPosX

double CwmGrid3D::attRobotPosX
protected

Robot position in each update.

◆ attRobotPosY

double CwmGrid3D::attRobotPosY
protected

◆ attSensitivity

unsigned int CwmGrid3D::attSensitivity
protected

This parametr is the minimum number of point that should be detected in a pixel Volume to be considered as a solid object.

◆ attSensorPosition

boost::numeric::ublas::matrix<float> CwmGrid3D::attSensorPosition
protected

Sensor's position.

◆ attWidthAngle

double CwmGrid3D::attWidthAngle
protected

Camera Parametr:the visible angles of camera .

◆ attWidthAngleMax

double CwmGrid3D::attWidthAngleMax
protected

◆ attWidthAngleMin

double CwmGrid3D::attWidthAngleMin
protected




WM3D library documentation