29 const size_t size_x = dem_get_size_x();
30 const size_t size_y = dem_get_size_y();
34 for (
size_t x = 0;
x < size_x;
x++)
35 for (
size_t y = 0;
y < size_y;
y++)
38 if (dem_get_z_by_cell(
x,
y,
z))
65 const double resolution = dem_get_resolution();
67 if (!getMinMaxHeight(z_min, z_max))
return false;
70 const TPlane horz_plane_above(
73 const TPlane horz_plane_below(
89 const double totalDist = Apt.
norm();
90 if (totalDist == 0)
return false;
92 Apt *= resolution * 0.99 / totalDist;
97 const size_t N = ceil(totalDist / resolution);
99 for (
size_t i = 0; i < N; i++)
102 const TPoint3D testPt = pt + Apt_half;
105 if (dem_get_z(testPt.
x, testPt.
y, pt_z))
109 pt_z < std::max(pt.
z, pt.
z + Apt.
z))
137 if (robotPose) robotPose3D = (*robotPose);
151 const auto* thePoints =
169 if (!thePointsMoved.
empty())
175 const size_t N = thePointsMoved.
size();
176 for (
size_t i = 0; i < N; i++)
180 insertIndividualPoint(
x,
y,
z, pt_params);
182 this->dem_update_map();
void getPoint(size_t index, float &x, float &y, float &z) const
Access to a given point from map, as a 2D point.
bool empty() const
STL-like method to check whether the map is empty:
Extra params for insertIndividualPoint()
A CObservation-derived class for RAW DATA (and optionally, point cloud) of scans from 3D Velodyne LID...
double x
X,Y,Z coordinates.
void changeCoordinatesReference(const mrpt::poses::CPose2D &b)
Replace each point by (pose compounding operator).
double norm() const
Point norm: |v| = sqrt(x^2+y^2+z^2)
void loadFromVelodyneScan(const mrpt::obs::CObservationVelodyneScan &scan, const mrpt::poses::CPose3D *robotPose=nullptr)
Like loadFromRangeScan() for Velodyne 3D scans.
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans. ...
Standard object for storing any 3D lightweight object.
void keep_min(T &var, const K test_val)
If the second argument is below the first one, set the first argument to this lower value...
GLsizei GLsizei GLuint * obj
With this struct options are provided to the observation insertion process.
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors...
This base provides a set of functions for maths stuff.
void keep_max(T &var, const K test_val)
If the second argument is above the first one, set the first argument to this higher value...
This namespace contains representation of robot actions and observations.
3D Plane, represented by its equation
#define IS_CLASS(obj, class_name)
True if the given reference to object (derived from mrpt::rtti::CObject) is of the given class...
bool update_map_after_insertion
(default: true) run any required operation to ensure the map reflects the changes caused by this poin...
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
bool getPoint(TPoint3D &p) const
Gets the content as a point, returning false if the type is not adequate.
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Declares a class that represents any robot's observation.
bool dem_internal_insertObservation(const mrpt::obs::CObservation &obs, const mrpt::poses::CPose3D *robotPose=nullptr)
Internal method called by internal_insertObservation()
bool intersectLine3D(const mrpt::math::TLine3D &r1, mrpt::math::TObject3D &obj) const
Gets the intersection between a 3D line and a Height Grid map (taking into account the different heig...
bool getMinMaxHeight(float &z_min, float &z_max) const
Computes the minimum and maximum height in the grid.
size_t size() const
Returns the number of stored points in the map.
bool intersect(const TSegment3D &s1, const TSegment3D &s2, TObject3D &obj)
Gets the intersection between two 3D segments.
virtual ~CHeightGridMap2D_Base()
3D line, represented by a base point and a director vector.