Go to the documentation of this file.
21 template <
class OCTREE,
class OCTREE_NODE>
27 template <
class OCTREE,
class OCTREE_NODE>
29 : insertionOptions(*this), m_impl(new Impl({resolution}))
33 template <
class OCTREE,
class OCTREE_NODE>
34 template <
class octomap_po
int3d,
class octomap_po
intcloud>
35 bool COctoMapBase<OCTREE, OCTREE_NODE>::
36 internal_build_PointCloud_for_observation(
39 octomap_pointcloud& scan)
const
48 robotPose3D = (*robotPose);
64 octomap::point3d(sensorPose.
x(), sensorPose.
y(), sensorPose.z());
67 const size_t nPts = scanPts->
size();
73 for (
size_t i = 0; i < nPts; i++)
76 scanPts->getPointFast(i, pt.
x, pt.
y, pt.
z);
83 scan.push_back(gx, gy, gz);
93 const auto* o_scan3D =
100 if (o_scan3D && !o_scan3D->hasPoints3D)
return false;
101 if (o_pc && (!o_pc->pointcloud || !o_pc->pointcloud->size()))
103 if (o_velo && !o_velo->point_cloud.size())
return false;
113 octomap::point3d(sensorPose.
x(), sensorPose.
y(), sensorPose.z());
118 std::size_t sizeRangeScan = 0;
119 const float *xs =
nullptr, *ys =
nullptr, *zs =
nullptr;
122 sizeRangeScan = o_scan3D->points3D_x.size();
123 xs = &o_scan3D->points3D_x[0];
124 ys = &o_scan3D->points3D_y[0];
125 zs = &o_scan3D->points3D_z[0];
129 sizeRangeScan = o_pc->pointcloud->size();
130 xs = &o_pc->pointcloud->getPointsBufferRef_x()[0];
131 ys = &o_pc->pointcloud->getPointsBufferRef_y()[0];
132 zs = &o_pc->pointcloud->getPointsBufferRef_z()[0];
136 sizeRangeScan = o_velo->point_cloud.size();
137 xs = &o_velo->point_cloud.x[0];
138 ys = &o_velo->point_cloud.y[0];
139 zs = &o_velo->point_cloud.z[0];
143 scan.reserve(sizeRangeScan);
148 const float m00 = H(0, 0);
149 const float m01 = H(0, 1);
150 const float m02 = H(0, 2);
151 const float m03 = H(0, 3);
152 const float m10 = H(1, 0);
153 const float m11 = H(1, 1);
154 const float m12 = H(1, 2);
155 const float m13 = H(1, 3);
156 const float m20 = H(2, 0);
157 const float m21 = H(2, 1);
158 const float m22 = H(2, 2);
159 const float m23 = H(2, 3);
162 for (
size_t i = 0; i < sizeRangeScan; i++)
169 if (pt.
x != 0 || pt.
y != 0 || pt.
z != 0)
172 const float gx = m00 * pt.
x + m01 * pt.
y + m02 * pt.
z + m03;
173 const float gy = m10 * pt.
x + m11 * pt.
y + m12 * pt.
z + m13;
174 const float gz = m20 * pt.
x + m21 * pt.
y + m22 * pt.
z + m23;
177 scan.push_back(gx, gy, gz);
186 template <
class OCTREE,
class OCTREE_NODE>
188 const std::string& filNamePrefix)
const
198 this->getAs3DObject(obj3D);
202 const std::string fil = filNamePrefix + std::string(
"_3D.3Dscene");
208 const std::string fil = filNamePrefix + std::string(
"_binary.bt");
209 m_impl->m_octomap.writeBinaryConst(fil);
214 template <
class OCTREE,
class OCTREE_NODE>
218 octomap::point3d sensorPt;
219 octomap::Pointcloud scan;
221 if (!internal_build_PointCloud_for_observation(
222 obs, &takenFrom, sensorPt, scan))
225 octomap::OcTreeKey key;
226 const size_t N = scan.size();
229 for (
size_t i = 0; i < N; i += likelihoodOptions.decimation)
231 if (m_impl->m_octomap.coordToKeyChecked(scan.getPoint(i), key))
233 OCTREE_NODE* node = m_impl->m_octomap.search(key, 0 );
234 if (node) log_lik += std::log(node->getOccupancy());
241 template <
class OCTREE,
class OCTREE_NODE>
243 const float x,
const float y,
const float z,
double& prob_occupancy)
const
245 octomap::OcTreeKey key;
246 if (m_impl->m_octomap.coordToKeyChecked(octomap::point3d(x, y, z), key))
248 OCTREE_NODE* node = m_impl->m_octomap.search(key, 0 );
249 if (!node)
return false;
251 prob_occupancy = node->getOccupancy();
258 template <
class OCTREE,
class OCTREE_NODE>
260 const CPointsMap& ptMap,
const float sensor_x,
const float sensor_y,
261 const float sensor_z)
264 const octomap::point3d sensorPt(sensor_x, sensor_y, sensor_z);
266 const float *xs, *ys, *zs;
268 for (
size_t i = 0; i < N; i++)
269 m_impl->m_octomap.insertRay(
270 sensorPt, octomap::point3d(xs[i], ys[i], zs[i]),
271 insertionOptions.maxrange, insertionOptions.pruning);
275 template <
class OCTREE,
class OCTREE_NODE>
280 octomap::point3d _end;
282 const bool ret = m_impl->m_octomap.
castRay(
283 octomap::point3d(origin.
x, origin.
y, origin.
z),
284 octomap::point3d(direction.
x, direction.
y, direction.
z), _end,
285 ignoreUnknownCells, maxRange);
296 template <
class OCTREE,
class OCTREE_NODE>
306 clampingThresMin(0.1192),
307 clampingThresMax(0.971)
311 template <
class OCTREE,
class OCTREE_NODE>
318 template <
class OCTREE,
class OCTREE_NODE>
323 template <
class OCTREE,
class OCTREE_NODE>
327 const int8_t version = 0;
332 template <
class OCTREE,
class OCTREE_NODE>
350 template <
class OCTREE,
class OCTREE_NODE>
352 std::ostream&
out)
const
354 out <<
"\n----------- [COctoMapBase<>::TInsertionOptions] ------------ "
369 template <
class OCTREE,
class OCTREE_NODE>
371 std::ostream&
out)
const
373 out <<
"\n----------- [COctoMapBase<>::TLikelihoodOptions] ------------ "
382 template <
class OCTREE,
class OCTREE_NODE>
403 template <
class OCTREE,
class OCTREE_NODE>
411 template <
class OCTREE,
class OCTREE_NODE>
415 const int8_t version = 0;
417 out << generateGridLines << generateOccupiedVoxels << visibleOccupiedVoxels
418 << generateFreeVoxels << visibleFreeVoxels;
421 template <
class OCTREE,
class OCTREE_NODE>
431 in >> generateGridLines >> generateOccupiedVoxels >>
432 visibleOccupiedVoxels >> generateFreeVoxels >>
size_t size() const
Save the point cloud as a PCL PCD file, in either ASCII or binary format.
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string §ion) override
This method load the options from a ".ini"-like file or memory-stored string list.
const_iterator end() const
virtual void setProbHit(double prob)=0
static Ptr Create(Args &&... args)
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...
std::shared_ptr< mrpt::opengl ::CSetOfObjects > Ptr
void insert(const CRenderizable::Ptr &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).
virtual void load() const
Makes sure all images and other fields which may be externally stored are loaded in memory.
#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...
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
void readFromStream(mrpt::serialization::CArchive &in)
Binary dump to stream.
double x() const
Common members of all points & poses classes.
virtual void setOccupancyThres(double prob)=0
This class allows the user to create, load, save, and render 3D scenes using OpenGL primitives.
void readFromStream(mrpt::serialization::CArchive &in)
Binary dump to stream.
virtual double getProbMiss() const =0
#define IS_CLASS(obj, class_name)
True if the given reference to object (derived from mrpt::rtti::CObject) is of the given class.
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form,...
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors.
mrpt::vision::TStereoCalibResults out
void writeToStream(mrpt::serialization::CArchive &out) const
Binary dump to stream.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
This namespace contains representation of robot actions and observations.
Virtual base class for "archives": classes abstracting I/O streams.
A three-dimensional probabilistic occupancy grid, implemented as an octo-tree with the "octomap" C++ ...
virtual double getClampingThresMin() const =0
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().
A compile-time fixed-size numeric matrix container.
void getHomogeneousMatrix(mrpt::math::CMatrixDouble44 &out_HM) const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
A range or depth 3D scan measurement, as from a time-of-flight range camera or a structured-light dep...
void composePoint(double lx, double ly, double lz, double &gx, double &gy, double &gz, mrpt::optional_ref< mrpt::math::CMatrixDouble33 > out_jacobian_df_dpoint=std::nullopt, mrpt::optional_ref< mrpt::math::CMatrixDouble36 > out_jacobian_df_dpose=std::nullopt, mrpt::optional_ref< mrpt::math::CMatrixDouble36 > out_jacobian_df_dse3=std::nullopt, 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...
This class allows loading and storing values and vectors of different types from a configuration text...
virtual void setProbMiss(double prob)=0
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
string iniFile(myDataDir+string("benchmark-options.ini"))
virtual double getClampingThresMax() const =0
virtual void setClampingThresMax(double thresProb)=0
#define LOADABLEOPTS_DUMP_VAR(variableName, variableType)
Macro for dumping a variable to a stream, within the method "dumpToTextStream(out)" (Variable types a...
void writeToStream(mrpt::serialization::CArchive &out) const
Binary dump to stream.
virtual double getProbHit() const =0
virtual double getOccupancyThres() const =0
bool saveToFile(const std::string &fil) const
Saves the scene to a 3Dscene file, loadable by the application SceneViewer3D.
void getPointsBuffer(size_t &outPointsCount, const float *&xs, const float *&ys, const float *&zs) const
Provides a direct access to points buffer, or nullptr if there is no points in the map.
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string §ion) override
This method load the options from a ".ini"-like file or memory-stored string list.
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form,...
Declares a class that represents any robot's observation.
An observation from any sensor that can be summarized as a pointcloud.
virtual void getSensorPose(mrpt::poses::CPose3D &out_sensorPose) const =0
A general method to retrieve the sensor pose on the robot.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
TLikelihoodOptions()
Initilization of default parameters.
COctoMapBase(double resolution)
Constructor, defines the resolution of the octomap (length of each voxel side)
A CObservation-derived class for RAW DATA (and optionally, point cloud) of scans from 3D Velodyne LID...
virtual void setClampingThresMin(double thresProb)=0
Page generated by Doxygen 1.8.17 for MRPT 2.0.4 at Fri Jul 17 08:43:33 UTC 2020 | |