Go to the documentation of this file.
84 float resolution = 0.25f);
87 void fill(
float default_value = 0.5f);
97 float default_value = 0.5f);
106 float new_voxels_default_value = 0.5f);
129 void updateCell(
int cx_idx,
int cy_idx,
int cz_idx,
float v);
134 unsigned int cx,
unsigned int cy,
unsigned int cz,
float value)
143 unsigned int cx,
unsigned int cy,
unsigned int cz)
const
172 bool endIsOccupied =
true);
184 const float maxValidRange = std::numeric_limits<float>::max());
213 const std::string& section)
override;
217 const std::string& section)
const override;
270 const std::string& section)
override;
274 const std::string& s)
const override;
341 const double x,
const double y,
const double angle_direction,
342 float& out_range,
bool& out_valid,
const double max_range_meters,
343 const float threshold_free = 0.4f,
const double noiseStd = .0,
344 const double angleNoiseStd = .0)
const;
390 float min_x{-5.0f}, max_x{5.0f};
391 float min_y{-5.0f}, max_y{5.0f};
392 float min_z{-5.0f}, max_z{5.0f};
393 float resolution{0.25f};
void updateCell(int cx_idx, int cy_idx, int cz_idx, float v)
Performs the Bayesian fusion of a new observation of a cell.
void setFreenessByPos(float x, float y, float z, float value)
Change the contents [0,1] of a voxel, given its coordinates.
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string §ion) override
This method load the options from a ".ini" file.
bool generateFreeVoxels
Generate voxels for the freespace (Default=true)
static uint8_t l2p_255(const voxelType l)
Scales an integer representation of the log-odd into a linear scale [0,255], using p=exp(l)/(1+exp(l)...
@ lmLikelihoodField_Thrun
void resizeGrid(const mrpt::math::TPoint3D &corner_min, const mrpt::math::TPoint3D &corner_max, float new_voxels_default_value=0.5f)
Change the size of gridmap, maintaining previous contents.
bool m_is_empty
True upon construction; used by isEmpty()
T * cellByIndex(unsigned int cx, unsigned int cy, unsigned int cz)
Returns a pointer to the contents of a voxel given by its voxel indexes, or nullptr if it is out of t...
int y2idx(coord_t y) const
void fill(float default_value=0.5f)
Fills all the voxels with a default value.
float LF_maxCorrsDistance
[LikelihoodField] The max.
float rayTracing_stdHit
[rayTracing] The laser range sigma.
#define MRPT_ENUM_TYPE_END()
const_iterator end() const
TLikelihoodMethod
The type for selecting a likelihood computation method.
std::shared_ptr< mrpt::opengl ::CSetOfObjects > Ptr
#define DEFINE_SERIALIZABLE(class_name, NS)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
grid_t m_grid
The actual 3D voxels container.
TRenderingOptions()=default
void insertRay(const mrpt::math::TPoint3D &sensor, const mrpt::math::TPoint3D &end, bool endIsOccupied=true)
Increases the freeness of a ray segment, and the occupancy of the voxel at its end point (unless endI...
float LF_maxRange
[LikelihoodField] The max.
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string §ion) override
This method load the options from a ".ini" file.
void simulateScanRay(const double x, const double y, const double angle_direction, float &out_range, bool &out_valid, const double max_range_meters, const float threshold_free=0.4f, const double noiseStd=.0, const double angleNoiseStd=.0) const
Simulate just one "ray" in the grid map.
int x2idx(coord_t x) const
Transform a coordinate values into voxel indexes.
void getAsOctoMapVoxels(mrpt::opengl::COctoMapVoxels &gl_obj) const
mrpt::maps::COccupancyGridMap3D::TInsertionOptions insertionOpts
Observations insertion options.
TInsertionOptions()=default
void internal_insertObservationScan3D(const mrpt::obs::CObservation3DRangeScan &o, const mrpt::poses::CPose3D &robotPose)
Parameters for CMetricMap::compute3DMatchingRatio()
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors.
bool isEmpty() const override
Returns true upon map construction or after calling clear(), the return changes to false upon success...
mrpt::vision::TStereoCalibResults out
COccupancyGridMap3D(const mrpt::math::TPoint3D &corner_min={-5.0f, -5.0f, -5.0f}, const mrpt::math::TPoint3D &corner_max={5.0f, 5.0f, 5.0f}, float resolution=0.25f)
Constructor.
uint16_t decimation_3d_range
Specify the decimation of 3D range scans.
void writeToStream(mrpt::serialization::CArchive &out) const
Binary dump to stream.
void readFromStream(mrpt::serialization::CArchive &in)
Binary dump to stream.
#define MRPT_ENUM_TYPE_BEGIN(_ENUM_TYPE_WITH_NS)
void saveToConfigFile(mrpt::config::CConfigFileBase &target, const std::string §ion) const override
This method saves the options to a ".ini"-like file or memory-stored string list.
static voxelType p2l(const float p)
Scales a real valued probability in [0,1] to an integer representation of: log(p)-log(1-p) in the val...
void saveMetricMapRepresentationToFile(const std::string &f) const override
This virtual method saves the map to a file "filNamePrefix"+< some_file_extension >,...
Virtual base class for "archives": classes abstracting I/O streams.
void insertPointCloud(const mrpt::math::TPoint3D &sensorCenter, const mrpt::maps::CPointsMap &pts, const float maxValidRange=std::numeric_limits< float >::max())
Calls insertRay() for each point in the point cloud, using as sensor central point (the origin of all...
Options for the conversion of a mrpt::maps::COccupancyGridMap3D into a mrpt::opengl::COctoMapVoxels.
A generic provider of log-odds grid-map maintainance functions.
bool LF_useSquareDist
[LikelihoodField] (Default:false) Use exp(dist^2/std^2) instead of exp(dist^2/std^2)
One static instance of this struct should exist in any class implementing CLogOddsGridMap2D to hold t...
int z2idx(coord_t z) const
void determineMatching2D(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose2D &otherMapPose, mrpt::tfest::TMatchingPairList &correspondences, const TMatchingParams ¶ms, TMatchingExtraResults &extraResults) const override
See docs in base class: in this class this always returns 0.
Declares a virtual base class for all metric maps storage classes.
OccGridCellTraits::cellType voxelType
The type of the map voxels:
float LF_zHit
[LikelihoodField]
A range or depth 3D scan measurement, as from a time-of-flight range camera or a structured-light dep...
float maxDistanceInsertion
Largest distance at which voxels are updated (Default: 15 meters)
This class allows loading and storing values and vectors of different types from a configuration text...
float getCellFreeness(unsigned int cx, unsigned int cy, unsigned int cz) const
Read the real valued [0,1] (0:occupied, 1:free) contents of a voxel, given its index.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
With this struct options are provided to the observation insertion process.
void saveToConfigFile(mrpt::config::CConfigFileBase &c, const std::string &s) const override
This method saves the options to a ".ini"-like file or memory-stored string list.
OccGridCellTraits::cellTypeUnsigned voxelTypeUnsigned
static CLogOddsGridMapLUT< voxelType > & get_logodd_lut()
Lookup tables for log-odds.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
uint16_t cellTypeUnsigned
float maxFreenessUpdateCertainty
A value in the range [0.5,1] for updating a free voxel.
bool internal_canComputeObservationLikelihood(const mrpt::obs::CObservation &obs) const override
mrpt::maps::COccupancyGridMap3D::TLikelihoodOptions likelihoodOpts
Probabilistic observation likelihood options.
TLikelihoodOptions likelihoodOptions
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
bool visibleOccupiedVoxels
Set occupied voxels visible (requires generateOccupiedVoxels=true) (Default=true)
TRenderingOptions renderingOptions
bool generateGridLines
Generate grid lines for all octree nodes, useful to draw the "structure" of the octree,...
void setCellFreeness(unsigned int cx, unsigned int cy, unsigned int cz, float value)
Change the contents [0,1] (0:occupied, 1:free) of a voxel, given its index.
float LF_stdHit
[LikelihoodField] The laser range "sigma" used in computations; Default value = 0....
MRPT_FILL_ENUM_MEMBER(mrpt::maps::COccupancyGridMap3D, lmRayTracing)
void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr &outObj) const override
Returns a 3D object representing the map.
bool generateOccupiedVoxels
Generate voxels for the occupied volumes (Default=true)
mrpt::vision::TStereoCalibParams params
TLikelihoodMethod likelihoodMethod
The selected method to compute an observation likelihood.
Parameters for the determination of matchings between point clouds, etc.
double computeLikelihoodField_Thrun(const CPointsMap &pm, const mrpt::poses::CPose3D &relativePose=mrpt::poses::CPose3D())
Computes the likelihood [0,1] of a set of points, given the current grid map as reference.
int32_t rayTracing_decimation
[rayTracing] One out of "rayTracing_decimation" rays will be simulated and compared only: set to 1 to...
void setSize(const mrpt::math::TPoint3D &corner_min, const mrpt::math::TPoint3D &corner_max, double resolution, float default_value=0.5f)
Change the size of gridmap, erasing all its previous contents.
A flexible renderer of voxels, typically from a 3D octo map (see mrpt::maps::COctoMap).
#define MAP_DEFINITION_START(_CLASS_NAME_)
Add a MAP_DEFINITION_START() ...
bool visibleFreeVoxels
Set free voxels visible (requires generateFreeVoxels=true) (Default=true)
TInsertionOptions insertionOptions
With this struct options are provided to the observation insertion process.
bool internal_insertObservation(const mrpt::obs::CObservation &obs, const mrpt::poses::CPose3D *robotPose=nullptr) override
Insert the observation information into this map.
#define MAP_DEFINITION_END(_CLASS_NAME_)
A 3D occupancy grid map with a regular, even distribution of voxels.
Declares a class that represents any robot's observation.
uint16_t decimation
Decimation for insertPointCloud() or 2D range scans (Default: 1)
void internal_insertObservationScan2D(const mrpt::obs::CObservation2DRangeScan &o, const mrpt::poses::CPose3D &robotPose)
With this struct options are provided to the observation likelihood computation process.
float getFreenessByPos(float x, float y, float z) const
Read the real valued [0,1] contents of a voxel, given its coordinates.
float maxOccupancyUpdateCertainty
A value in the range [0.5,1] used for updating voxel with a Bayesian approach (default 0....
double internal_computeObservationLikelihood(const mrpt::obs::CObservation &obs, const mrpt::poses::CPose3D &takenFrom) override
Internal method called by computeObservationLikelihood()
static float l2p(const voxelType l)
Scales an integer representation of the log-odd into a real valued probability in [0,...
float compute3DMatchingRatio(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose3D &otherMapPose, const TMatchingRatioParams ¶ms) const override
See docs in base class: in this class this always returns 0.
void OnPostSuccesfulInsertObs(const mrpt::obs::CObservation &) override
See base class.
TLikelihoodOptions()=default
void internal_clear() override
Clear the map: It set all voxels to their default occupancy value (0.5), without changing the resolut...
uint32_t LF_decimation
[LikelihoodField] The decimation of the points in a scan, default=1 == no decimation
Page generated by Doxygen 1.8.17 for MRPT 2.0.4 at Fri Jul 17 08:43:33 UTC 2020 | |