10 #ifndef MRPT_COctoMapBase_H 11 #define MRPT_COctoMapBase_H 42 template <
class OCTREE,
class OCTREE_NODE>
80 const bool o_has_parent = o.
m_parent.get()!=NULL;
156 const std::string §ion);
189 generateGridLines (false),
190 generateOccupiedVoxels (true),
191 visibleOccupiedVoxels (true),
192 generateFreeVoxels (true),
193 visibleFreeVoxels (true)
209 outObj->insert(gl_obj);
226 bool getPointOccupancy(
const float x,
const float y,
const float z,
double &prob_occupancy)
const;
229 void updateVoxel(
const double x,
const double y,
const double z,
bool occupied)
241 void insertRay(
const float end_x,
const float end_y,
const float end_z,
const float sensor_x,
const float sensor_y,
const float sensor_z)
263 bool ignoreUnknownCells=
false,
264 double maxRange=-1.0)
const;
Options for the conversion of a mrpt::maps::COctoMap into a mrpt::opengl::COctoMapVoxels.
virtual void internal_clear()
Internal method called by clear()
void setOccupancyThres(double prob)
(key name in .ini files: "occupancyThres") sets the threshold for occupancy (sensor model) (Default=0...
void updateVoxel(const double x, const double y, const double z, bool occupied)
Manually updates the occupancy of the voxel at (x,y,z) as being occupied (true) or free (false)...
EIGEN_STRONG_INLINE iterator end()
OCTREE m_octomap
The actual octo-map object.
OCTREE & getOctomap()
Get a reference to the internal octomap object.
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
double getClampingThresMax() const
bool internal_build_PointCloud_for_observation(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D *robotPose, octomap::point3d &point3d_sensorPt, octomap::Pointcloud &ptr_scan) const
Builds the list of 3D points in global coordinates for a generic observation.
virtual void writeToStream(mrpt::utils::CStream &out, int *getVersion) const =0
Introduces a pure virtual method responsible for writing to a CStream.
bool visibleOccupiedVoxels
Set occupied voxels visible (requires generateOccupiedVoxels=true) (Default=true) ...
TLikelihoodOptions likelihoodOptions
virtual void getAs3DObject(mrpt::opengl::CSetOfObjectsPtr &outObj) const
Returns a 3D object representing the map.
void insertRay(const float end_x, const float end_y, const float end_z, const float sensor_x, const float sensor_y, const float sensor_z)
Just like insertPointCloud but with a single ray.
double getResolution() const
virtual double internal_computeObservationLikelihood(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D &takenFrom) MRPT_OVERRIDE
Internal method called by computeObservationLikelihood()
OCTREE_NODE octree_node_t
The type of nodes in the octree, in the "octomap" library.
This class allows loading and storing values and vectors of different types from a configuration text...
TInsertionOptions insertionOptions
The options used when inserting observations in the map.
void getMetricSize(double &x, double &y, double &z) const
Size of OcTree (all known space) in meters for x, y and z dimension.
A three-dimensional probabilistic occupancy grid, implemented as an octo-tree with the "octomap" C++ ...
unsigned int getTreeDepth() const
A flexible renderer of voxels, typically from a 3D octo map (see mrpt::maps::COctoMap).
float getClampingThresMaxLog() const
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors...
bool generateOccupiedVoxels
Generate voxels for the occupied volumes (Default=true)
size_t memoryUsage() const
size_t calcNumNodes() const
Traverses the tree to calculate the total number of nodes.
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string §ion)
See utils::CLoadableOptions.
double getProbMiss() const
double getProbHit() const
float getProbMissLog() const
bool generateGridLines
Generate grid lines for all octree nodes, useful to draw the "structure" of the octree, but computationally costly (Default: false)
TInsertionOptions()
Especial constructor, not attached to a real COctoMap object: used only in limited situations...
double getClampingThresMin() const
A collection of 3D coordinates (point3d), which are regarded as endpoints of a 3D laser scan...
virtual ~TLikelihoodOptions()
float getOccupancyThresLog() const
void getMetricSize(double &x, double &y, double &z)
Size of OcTree (all known space) in meters for x, y and z dimension.
TInsertionOptions & operator=(const TInsertionOptions &o)
virtual bool isEmpty() const
Returns true if the map is empty/no observation has been inserted.
bool pruning
whether the tree is (losslessly) pruned after insertion (default: true)
void setClampingThresMax(double thresProb)
(key name in .ini files: "clampingThresMax")sets the maximum threshold for occupancy clamping (sensor...
bool castRay(const mrpt::math::TPoint3D &origin, const mrpt::math::TPoint3D &direction, mrpt::math::TPoint3D &end, bool ignoreUnknownCells=false, double maxRange=-1.0) const
Performs raycasting in 3d, similar to computeRay().
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
virtual void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const
This virtual method saves the map to a file "filNamePrefix"+< some_file_extension >...
size_t memoryUsageNode() const
bool isPointWithinOctoMap(const float x, const float y, const float z) const
Check whether the given point lies within the volume covered by the octomap (that is...
void setProbHit(double prob)
(key name in .ini files: "probHit")sets the probablility for a "hit" (will be converted to logodds) -...
COctoMapBase< OCTREE, OCTREE_NODE > myself_t
The type of this MRPT class.
void setProbMiss(double prob)
(key name in .ini files: "probMiss")sets the probablility for a "miss" (will be converted to logodds)...
bool generateFreeVoxels
Generate voxels for the freespace (Default=true)
bool getPointOccupancy(const float x, const float y, const float z, double &prob_occupancy) const
Get the occupancy probability [0,1] of a point.
Declares a virtual base class for all metric maps storage classes.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
double maxrange
maximum range for how long individual beams are inserted (default -1: complete beam) ...
Declares a class that represents any robot's observation.
void insertPointCloud(const CPointsMap &ptMap, const float sensor_x, const float sensor_y, const float sensor_z)
Update the octomap with a 2D or 3D scan, given directly as a point cloud and the 3D location of the s...
OcTreeKey is a container class for internal key addressing.
uint32_t decimation
Speed up the likelihood computation by considering only one out of N rays (default=1) ...
void getMetricMin(double &x, double &y, double &z)
minimum value of the bounding box of all known space in x, y, z
With this struct options are provided to the observation insertion process.
size_t memoryFullGrid() const
bool visibleFreeVoxels
Set free voxels visible (requires generateFreeVoxels=true) (Default=true)
float getClampingThresMinLog() const
OCTREE octree_t
The type of the octree class in the "octomap" library.
static COctoMapVoxelsPtr Create()
mrpt::utils::ignored_copy_ptr< myself_t > m_parent
COctoMapBase(const double resolution=0.10)
Constructor, defines the resolution of the octomap (length of each voxel side)
double getOccupancyThres() const
Options used when evaluating "computeObservationLikelihood".
float getProbHitLog() const
void setClampingThresMin(double thresProb)
(key name in .ini files: "clampingThresMin")sets the minimum threshold for occupancy clamping (sensor...
A wrapper class for pointers whose copy operations from other objects of the same type are ignored...
virtual void readFromStream(mrpt::utils::CStream &in, int version)=0
Introduces a pure virtual method responsible for loading from a CStream This can not be used directly...
size_t getNumLeafNodes() const
Traverses the tree to calculate the total number of leaf nodes.
void getMetricMin(double &x, double &y, double &z) const
minimum value of the bounding box of all known space in x, y, z
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
void getMetricMax(double &x, double &y, double &z) const
maximum value of the bounding box of all known space in x, y, z
void dumpToTextStream(mrpt::utils::CStream &out) const
See utils::CLoadableOptions.
virtual void getAsOctoMapVoxels(mrpt::opengl::COctoMapVoxels &gl_obj) const =0
Builds a renderizable representation of the octomap as a mrpt::opengl::COctoMapVoxels object...
TRenderingOptions renderingOptions
This class represents a three-dimensional vector.
void getMetricMax(double &x, double &y, double &z)
maximum value of the bounding box of all known space in x, y, z