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