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++)
56 scanPts->getPointFast(i,pt.
x,pt.
y,pt.
z);
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>
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;
bool getPointOccupancy(const float x, const float y, const float z, double &prob_occupancy) const
Get the occupancy probability [0,1] of a point.
double x() const
Common members of all points & poses classes.
EIGEN_STRONG_INLINE iterator end()
void writeToStream(mrpt::utils::CStream &out) const
Binary dump to stream.
#define LOADABLEOPTS_DUMP_VAR(variableName, variableType)
Macro for dumping a variable to a stream, within the method "dumpToTextStream(out)" (Variable types a...
Declares a class derived from "CObservation" that encapsules a 3D range scan measurement, as from a time-of-flight range camera or any other RGBD sensor.
virtual bool isEmpty() const
Returns true if the map is empty/no observation has been inserted.
double z
X,Y,Z coordinates.
void dumpToTextStream(mrpt::utils::CStream &out) const
See utils::CLoadableOptions.
virtual double internal_computeObservationLikelihood(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D &takenFrom) MRPT_OVERRIDE
Internal method called by computeObservationLikelihood()
octomap::OcTreeNode 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...
std::vector< float > points3D_z
If hasPoints3D=true, the Z coordinates of the 3D point cloud detected by the camera.
A three-dimensional probabilistic occupancy grid, implemented as an octo-tree with the "octomap" C++ ...
TLikelihoodOptions()
Initilization of default parameters.
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...
void writeToStream(mrpt::utils::CStream &out) const
Binary dump to stream.
A numeric matrix of compile-time fixed size.
std::vector< float > points3D_y
If hasPoints3D=true, the Y coordinates of the 3D point cloud detected by the camera.
Lightweight 3D point (float version).
This CStream derived class allow using a file as a write-only, binary stream.
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.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string §ion)
See utils::CLoadableOptions.
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.
This namespace contains algorithms for SLAM, localization, map building, representation of robot's ac...
TInsertionOptions()
Especial constructor, not attached to a real COctoMap object: used only in limited situations...
void readFromStream(mrpt::utils::CStream &in)
Binary dump to stream.
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...
bool hasPoints3D
true means the field points3D contains valid data.
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().
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
bool pruning
whether the tree is (losslessly) pruned after insertion (default: true)
#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...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot.
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)...
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.
#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 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 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...
std::vector< float > points3D_x
If hasPoints3D=true, the X coordinates of the 3D point cloud detected by the camera.
This class allows the user to create, load, save, and render 3D scenes using OpenGL primitives...
void readFromStream(mrpt::utils::CStream &in)
Binary dump to stream.
uint32_t decimation
Speed up the likelihood computation by considering only one out of N rays (default=1) ...
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string §ion)
See utils::CLoadableOptions.
void dumpToTextStream(mrpt::utils::CStream &out) const
See utils::CLoadableOptions.
mrpt::utils::ignored_copy_ptr< myself_t > m_parent
virtual void load() const
Makes sure all images and other fields which may be externally stored are loaded in memory...
size_t size() const
Returns the number of stored points in the map.
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot at the moment of starting the scan.
void getHomogeneousMatrix(mrpt::math::CMatrixDouble44 &out_HM) const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
virtual int printf(const char *fmt,...) MRPT_printf_format_check(2
Writes a string to the stream in a textual form.
virtual void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const
This virtual method saves the map to a file "filNamePrefix"+< some_file_extension >...
static CSetOfObjectsPtr Create()
const POINTSMAP * buildAuxPointsMap(const void *options=NULL) const
Returns a cached points map representing this laser scan, building it upon the first call...