Go to the documentation of this file.
20 template <
class OCTREE,
class OCTREE_NODE>
30 robotPose3D = (*robotPose);
44 sensorPt = octomap::point3d(sensorPose.
x(),sensorPose.
y(),sensorPose.z());
47 const size_t nPts = scanPts->
size();
53 for (
size_t i=0;i<nPts;i++)
63 scan.push_back(gx,gy,gz);
81 sensorPt = octomap::point3d(sensorPose.
x(),sensorPose.
y(),sensorPose.z());
84 const size_t sizeRangeScan = o->
points3D_x.size();
87 scan.reserve(sizeRangeScan);
92 const float m00 = H.get_unsafe(0,0);
93 const float m01 = H.get_unsafe(0,1);
94 const float m02 = H.get_unsafe(0,2);
95 const float m03 = H.get_unsafe(0,3);
96 const float m10 = H.get_unsafe(1,0);
97 const float m11 = H.get_unsafe(1,1);
98 const float m12 = H.get_unsafe(1,2);
99 const float m13 = H.get_unsafe(1,3);
100 const float m20 = H.get_unsafe(2,0);
101 const float m21 = H.get_unsafe(2,1);
102 const float m22 = H.get_unsafe(2,2);
103 const float m23 = H.get_unsafe(2,3);
106 for (
size_t i=0;i<sizeRangeScan;i++)
113 if ( pt.
x!=0 || pt.
y!=0 || pt.
z!=0 )
116 const float gx = m00*pt.
x + m01*pt.
y + m02*pt.
z + m03;
117 const float gy = m10*pt.
x + m11*pt.
y + m12*pt.
z + m13;
118 const float gz = m20*pt.
x + m21*pt.
y + m22*pt.
z + m23;
121 scan.push_back(gx,gy,gz);
130 template <
class OCTREE,
class OCTREE_NODE>
133 return m_octomap.size()==1;
136 template <
class OCTREE,
class OCTREE_NODE>
146 this->getAs3DObject(obj3D);
150 const std::string fil = filNamePrefix + std::string(
"_3D.3Dscene");
157 const std::string fil = filNamePrefix + std::string(
"_binary.bt");
158 const_cast<OCTREE*
>(&m_octomap)->writeBinary(fil);
163 template <
class OCTREE,
class OCTREE_NODE>
166 octomap::point3d sensorPt;
167 octomap::Pointcloud scan;
169 if (!internal_build_PointCloud_for_observation(obs,&takenFrom, sensorPt, scan))
172 octomap::OcTreeKey key;
173 const size_t N=scan.size();
176 for (
size_t i=0;i<N;i+=likelihoodOptions.decimation)
178 if (m_octomap.coordToKeyChecked(scan.getPoint(i), key))
182 log_lik += std::log(node->getOccupancy());
189 template <
class OCTREE,
class OCTREE_NODE>
192 octomap::OcTreeKey key;
193 if (m_octomap.coordToKeyChecked(octomap::point3d(x,y,z), key))
196 if (!node)
return false;
198 prob_occupancy = node->getOccupancy();
204 template <
class OCTREE,
class OCTREE_NODE>
208 const octomap::point3d sensorPt(sensor_x,sensor_y,sensor_z);
210 const float *xs,*ys,*zs;
212 for (
size_t i=0;i<N;i++)
213 m_octomap.insertRay(sensorPt, octomap::point3d(xs[i],ys[i],zs[i]), insertionOptions.maxrange,insertionOptions.pruning);
217 template <
class OCTREE,
class OCTREE_NODE>
220 octomap::point3d _end;
222 const bool ret=m_octomap.castRay(
223 octomap::point3d(origin.
x,origin.
y,origin.
z),
224 octomap::point3d(direction.
x,direction.
y,direction.
z),
225 _end,ignoreUnknownCells,maxRange);
238 template <
class OCTREE,
class OCTREE_NODE>
244 occupancyThres (0.5),
247 clampingThresMin(0.1192),
248 clampingThresMax(0.971)
252 template <
class OCTREE,
class OCTREE_NODE>
258 occupancyThres (0.5),
261 clampingThresMin(0.1192),
262 clampingThresMax(0.971)
266 template <
class OCTREE,
class OCTREE_NODE>
272 template <
class OCTREE,
class OCTREE_NODE>
275 const int8_t version = 0;
280 template <
class OCTREE,
class OCTREE_NODE>
300 template <
class OCTREE,
class OCTREE_NODE>
303 out.
printf(
"\n----------- [COctoMapBase<>::TInsertionOptions] ------------ \n\n");
317 template <
class OCTREE,
class OCTREE_NODE>
320 out.
printf(
"\n----------- [COctoMapBase<>::TLikelihoodOptions] ------------ \n\n");
328 template <
class OCTREE,
class OCTREE_NODE>
331 const std::string §ion)
343 this->setOccupancyThres(occupancyThres);
344 this->setProbHit(probHit);
345 this->setProbMiss(probMiss);
346 this->setClampingThresMin(clampingThresMin);
347 this->setClampingThresMax(clampingThresMax);
350 template <
class OCTREE,
class OCTREE_NODE>
353 const std::string §ion)
359 template <
class OCTREE,
class OCTREE_NODE>
362 const int8_t version = 0;
364 out << generateGridLines << generateOccupiedVoxels << visibleOccupiedVoxels
365 << generateFreeVoxels << visibleFreeVoxels;
368 template <
class OCTREE,
class OCTREE_NODE>
377 in >> generateGridLines >> generateOccupiedVoxels >> visibleOccupiedVoxels
378 >> generateFreeVoxels >> visibleFreeVoxels;
Lightweight 3D point (float version).
static CSetOfObjectsPtr Create()
size_t size() const
Returns the number of stored points in the map.
bool hasPoints3D
true means the field points3D contains valid data.
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,...
void getPointFast(size_t index, float &x, float &y, float &z) const
Just like getPoint() but without checking out-of-bound index and without returning the point weight,...
#define LOADABLEOPTS_DUMP_VAR(variableName, variableType)
Macro for dumping a variable to a stream, within the method "dumpToTextStream(out)" (Variable types a...
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
This class allows the user to create, load, save, and render 3D scenes using OpenGL primitives.
std::vector< float > points3D_z
If hasPoints3D=true, the (X,Y,Z) coordinates of the 3D point cloud detected by the camera.
double z
X,Y,Z coordinates.
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.
virtual int printf(const char *fmt,...) MRPT_printf_format_check(2
Writes a string to the stream in a textual form.
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors.
const POINTSMAP * buildAuxPointsMap(const void *options=NULL) const
Returns a cached points map representing this laser scan, building it upon the first call.
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.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
std::vector< float > points3D_y
This namespace contains representation of robot actions and observations.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
A three-dimensional probabilistic occupancy grid, implemented as an octo-tree with the "octomap" C++ ...
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().
bool getPointOccupancy(const float x, const float y, const float z, double &prob_occupancy) const
Get the occupancy probability [0,1] of a point.
void composePoint(double lx, double ly, double lz, double &gx, double &gy, double &gz, mrpt::math::CMatrixFixedNumeric< double, 3, 3 > *out_jacobian_df_dpoint=NULL, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dpose=NULL, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dse3=NULL, bool use_small_rot_approx=false) const
An alternative, slightly more efficient way of doing with G and L being 3D points and P this 6D pose...
TInsertionOptions()
Especial constructor, not attached to a real COctoMap object: used only in limited situations,...
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot.
Declares a class derived from "CObservation" that encapsules a 3D range scan measurement,...
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...
double x() const
Common members of all points & poses classes.
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()
#define IS_CLASS(ptrObj, class_name)
Evaluates to true if the given pointer to an object (derived from mrpt::utils::CSerializable) is of t...
void insert(const CRenderizablePtr &newObject, const std::string &viewportName=std::string("main"))
Insert a new object into the scene, in the given viewport (by default, into the "main" viewport).
std::vector< float > points3D_x
#define MRPT_LOAD_CONFIG_VAR(variableName, variableType, configFileObject, sectionNameStr)
An useful macro for loading variables stored in a INI-like file under a key with the same name that t...
virtual void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const MRPT_OVERRIDE
This virtual method saves the map to a file "filNamePrefix"+< some_file_extension >,...
void getPointsBuffer(size_t &outPointsCount, const float *&xs, const float *&ys, const float *&zs) const
Provides a direct access to points buffer, or NULL if there is no points in the map.
void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
This method should clearly display all the contents of the structure in textual form,...
A numeric matrix of compile-time fixed size.
void readFromStream(mrpt::utils::CStream &in)
Binary dump to stream.
void writeToStream(mrpt::utils::CStream &out) const
Binary dump to stream.
virtual bool isEmpty() const MRPT_OVERRIDE
Returns true if the map is empty/no observation has been inserted.
octomap::OcTreeNode octree_node_t
The type of nodes in the octree, in the "octomap" library.
Declares a class that represents any robot's observation.
void composeFrom(const CPose3D &A, const CPose3D &B)
Makes "this = A (+) B"; this method is slightly more efficient than "this= A + B;" since it avoids th...
void getHomogeneousMatrix(mrpt::math::CMatrixDouble44 &out_HM) const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
virtual void load() const MRPT_OVERRIDE
Makes sure all images and other fields which may be externally stored are loaded in memory.
void writeToStream(mrpt::utils::CStream &out) const
Binary dump to stream.
void readFromStream(mrpt::utils::CStream &in)
Binary dump to stream.
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot at the moment of starting the scan.
TLikelihoodOptions()
Initilization of default parameters.
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.
This CStream derived class allow using a file as a write-only, binary stream.
Page generated by Doxygen 1.8.17 for MRPT 1.4.0 SVN: at Sat Jan 18 22:37:07 UTC 2020 | | |