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)
233 m_octomap.insertRay( octomap::point3d(sensor_x,sensor_y,sensor_z), octomap::point3d(end_x,end_y,end_z), insertionOptions.maxrange,insertionOptions.pruning);
253 bool ignoreUnknownCells=
false,
254 double maxRange=-1.0)
const;
bool getPointOccupancy(const float x, const float y, const float z, double &prob_occupancy) const
Get the occupancy probability [0,1] of a point.
virtual void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const MRPT_OVERRIDE
This virtual method saves the map to a file "filNamePrefix"+< some_file_extension >...
Options for the conversion of a mrpt::maps::COctoMap into a mrpt::opengl::COctoMapVoxels.
size_t getNumLeafNodes() const
Traverses the tree to calculate the total number of leaf nodes.
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:
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 writeToStream(mrpt::utils::CStream &out, int *getVersion) const =0
Introduces a pure virtual method responsible for writing to a CStream.
float getProbHitLog() const
bool visibleOccupiedVoxels
Set occupied voxels visible (requires generateOccupiedVoxels=true) (Default=true) ...
TLikelihoodOptions likelihoodOptions
float getClampingThresMinLog() const
void getMetricSize(double &x, double &y, double &z) const
Size of OcTree (all known space) in meters for x, y and z dimension.
virtual void internal_clear() MRPT_OVERRIDE
Internal method called by clear()
unsigned int getTreeDepth() const
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 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.
A three-dimensional probabilistic occupancy grid, implemented as an octo-tree with the "octomap" C++ ...
A flexible renderer of voxels, typically from a 3D octo map (see mrpt::maps::COctoMap).
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...
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)
float getProbMissLog() 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.
size_t memoryUsageNode() const
double getClampingThresMin() const
size_t memoryUsage() const
float getOccupancyThresLog() const
bool generateGridLines
Generate grid lines for all octree nodes, useful to draw the "structure" of the octree, but computationally costly (Default: false)
size_t memoryFullGrid() const
void getMetricMin(double &x, double &y, double &z) const
minimum value of the bounding box of all known space in x, y, z
TInsertionOptions()
Especial constructor, not attached to a real COctoMap object: used only in limited situations...
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().
virtual ~TLikelihoodOptions()
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)
float getClampingThresMaxLog() const
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...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
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.
double getProbHit() const
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)
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).
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.
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...
size_t calcNumNodes() const
Traverses the tree to calculate the total number of nodes.
x y t t *t x y t t t x y t t t x *y t *t t x *y t *t t x y t t t x y t t t x(y+z)
virtual void getAs3DObject(mrpt::opengl::CSetOfObjectsPtr &outObj) const MRPT_OVERRIDE
Returns a 3D object representing the map.
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
double getClampingThresMax() const
double getOccupancyThres() const
With this struct options are provided to the observation insertion process.
void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
This method should clearly display all the contents of the structure in textual form, sending it to a CStream.
bool visibleFreeVoxels
Set free voxels visible (requires generateFreeVoxels=true) (Default=true)
double getResolution() 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
double getProbMiss() const
COctoMapBase(const double resolution=0.10)
Constructor, defines the resolution of the octomap (length of each voxel side)
Options used when evaluating "computeObservationLikelihood".
virtual bool isEmpty() const MRPT_OVERRIDE
Returns true if the map is empty/no observation has been inserted.
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...
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
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
void getMetricMax(double &x, double &y, double &z)
maximum value of the bounding box of all known space in x, y, z