10 #ifndef MRPT_COctoMapBase_H
11 #define MRPT_COctoMapBase_H
16 #include <octomap/octomap.h>
42 template <
class OCTREE,
class OCTREE_NODE>
80 const bool o_has_parent = o.
m_parent.get()!=NULL;
179 generateGridLines (false),
180 generateOccupiedVoxels (true),
181 visibleOccupiedVoxels (true),
182 generateFreeVoxels (true),
183 visibleFreeVoxels (true)
199 outObj->insert(gl_obj);
210 octomap::OcTreeKey key;
211 return m_octomap.coordToKeyChecked(octomap::point3d(x,y,z), key);
216 bool getPointOccupancy(
const float x,
const float y,
const float z,
double &prob_occupancy)
const;
219 void updateVoxel(
const double x,
const double y,
const double z,
bool occupied)
231 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)
253 bool ignoreUnknownCells=
false,
254 double maxRange=-1.0)
const;
Declares a virtual base class for all metric maps storage classes.
A three-dimensional probabilistic occupancy grid, implemented as an octo-tree with the "octomap" C++ ...
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 double internal_computeObservationLikelihood(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D &takenFrom) MRPT_OVERRIDE
Internal method called by computeObservationLikelihood()
unsigned int getTreeDepth() const
virtual void internal_clear() MRPT_OVERRIDE
Internal method called by clear()
OCTREE octree_t
The type of the octree class in the "octomap" library.
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),...
virtual void getAsOctoMapVoxels(mrpt::opengl::COctoMapVoxels &gl_obj) const =0
Builds a renderizable representation of the octomap as a mrpt::opengl::COctoMapVoxels object.
OCTREE & getOctomap()
Get a reference to the internal octomap object.
void getMetricMin(double &x, double &y, double &z) const
minimum value of the bounding box of all known space in x, y, z
OCTREE_NODE octree_node_t
The type of nodes in the octree, in the "octomap" library.
double getResolution() const
void getMetricMax(double &x, double &y, double &z) const
maximum value of the bounding box of all known space in x, y, z
virtual void getAs3DObject(mrpt::opengl::CSetOfObjectsPtr &outObj) const MRPT_OVERRIDE
Returns a 3D object representing the map.
virtual bool isEmpty() const MRPT_OVERRIDE
Returns true if the map is empty/no observation has been inserted.
COctoMapBase(const double resolution=0.10)
Constructor, defines the resolution of the octomap (length of each voxel side)
size_t getNumLeafNodes() const
Traverses the tree to calculate the total number of leaf nodes.
bool getPointOccupancy(const float x, const float y, const float z, double &prob_occupancy) const
Get the occupancy probability [0,1] of a point.
size_t memoryUsageNode() const
void getMetricSize(double &x, double &y, double &z) const
Size of OcTree (all known space) in meters for x, y and z dimension.
size_t memoryUsage() const
size_t memoryFullGrid() const
TInsertionOptions insertionOptions
The options used when inserting observations in the map.
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...
void getMetricMin(double &x, double &y, double &z)
minimum value of the bounding box of all known space in x, y, z
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.
virtual void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const MRPT_OVERRIDE
This virtual method saves the map to a file "filNamePrefix"+< some_file_extension >,...
size_t calcNumNodes() const
Traverses the tree to calculate the total number of nodes.
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 getMetricSize(double &x, double &y, double &z)
Size of OcTree (all known space) in meters for x, y and z dimension.
OCTREE m_octomap
The actual octo-map object.
COctoMapBase< OCTREE, OCTREE_NODE > myself_t
The type of this MRPT class.
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().
void getMetricMax(double &x, double &y, double &z)
maximum value of the bounding box of all known space in x, y, z
TRenderingOptions renderingOptions
TLikelihoodOptions likelihoodOptions
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors.
Declares a class that represents any robot's observation.
A flexible renderer of voxels, typically from a 3D octo map (see mrpt::maps::COctoMap).
static COctoMapVoxelsPtr Create()
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
This class allows loading and storing values and vectors of different types from a configuration text...
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
virtual void writeToStream(mrpt::utils::CStream &out, int *getVersion) const =0
Introduces a pure virtual method responsible for writing to a CStream.
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...
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
EIGEN_STRONG_INLINE iterator end()
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
With this struct options are provided to the observation insertion process.
bool pruning
whether the tree is (losslessly) pruned after insertion (default: true)
double getClampingThresMin() const
float getClampingThresMinLog() const
TInsertionOptions()
Especial constructor, not attached to a real COctoMap object: used only in limited situations,...
float getProbHitLog() const
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string §ion) MRPT_OVERRIDE
This method load the options from a ".ini"-like file or memory-stored string list.
void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
This method should clearly display all the contents of the structure in textual form,...
mrpt::utils::ignored_copy_ptr< myself_t > m_parent
void setProbHit(double prob)
(key name in .ini files: "probHit")sets the probablility for a "hit" (will be converted to logodds) -...
void setOccupancyThres(double prob)
(key name in .ini files: "occupancyThres") sets the threshold for occupancy (sensor model) (Default=0...
double maxrange
maximum range for how long individual beams are inserted (default -1: complete beam)
float getOccupancyThresLog() const
double getClampingThresMax() const
float getProbMissLog() const
double getProbHit() const
float getClampingThresMaxLog() const
double getProbMiss() const
TInsertionOptions & operator=(const TInsertionOptions &o)
void setClampingThresMax(double thresProb)
(key name in .ini files: "clampingThresMax")sets the maximum threshold for occupancy clamping (sensor...
double getOccupancyThres() const
void setClampingThresMin(double thresProb)
(key name in .ini files: "clampingThresMin")sets the minimum threshold for occupancy clamping (sensor...
void setProbMiss(double prob)
(key name in .ini files: "probMiss")sets the probablility for a "miss" (will be converted to logodds)...
Options used when evaluating "computeObservationLikelihood".
uint32_t decimation
Speed up the likelihood computation by considering only one out of N rays (default=1)
TLikelihoodOptions()
Initilization of default parameters.
void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
This method should clearly display all the contents of the structure in textual form,...
void writeToStream(mrpt::utils::CStream &out) const
Binary dump to stream.
virtual ~TLikelihoodOptions()
void readFromStream(mrpt::utils::CStream &in)
Binary dump to stream.
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string §ion) MRPT_OVERRIDE
This method load the options from a ".ini"-like file or memory-stored string list.
Options for the conversion of a mrpt::maps::COctoMap into a mrpt::opengl::COctoMapVoxels.
bool generateFreeVoxels
Generate voxels for the freespace (Default=true)
bool generateOccupiedVoxels
Generate voxels for the occupied volumes (Default=true)
bool visibleFreeVoxels
Set free voxels visible (requires generateFreeVoxels=true) (Default=true)
bool visibleOccupiedVoxels
Set occupied voxels visible (requires generateOccupiedVoxels=true) (Default=true)
bool generateGridLines
Generate grid lines for all octree nodes, useful to draw the "structure" of the octree,...
A wrapper class for pointers whose copy operations from other objects of the same type are ignored,...