22 : pt_z_std(0.0), update_map_after_insertion(true)
35 for (
size_t x = 0;
x < size_x;
x++)
36 for (
size_t y = 0;
y < size_y;
y++)
71 const TPlane horz_plane_above(
74 const TPlane horz_plane_below(
90 const double totalDist = Apt.
norm();
91 if (totalDist == 0)
return false;
93 Apt *= resolution * 0.99 / totalDist;
98 const size_t N = ceil(totalDist / resolution);
100 for (
size_t i = 0; i < N; i++)
103 const TPoint3D testPt = pt + Apt_half;
110 pt_z < std::max(pt.
z, pt.
z + Apt.
z))
138 if (robotPose) robotPose3D = (*robotPose);
172 if (!thePointsMoved.
empty())
178 const size_t N = thePointsMoved.
size();
179 for (
size_t i = 0; i < N; i++)
virtual bool dem_get_z(const double x, const double y, double &z_out) const =0
Get cell 'z' (x,y) by metric coordinates.
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...
virtual bool insertIndividualPoint(const double x, const double y, const double z, const TPointInsertParams ¶ms=TPointInsertParams())=0
Update the DEM with one new point.
void changeCoordinatesReference(const mrpt::poses::CPose2D &b)
Replace each point by (pose compounding operator).
const POINTSMAP * buildAuxPointsMap(const void *options=nullptr) const
Returns a cached points map representing this laser scan, building it upon the first call...
double norm() const
Point norm.
virtual bool dem_get_z_by_cell(const size_t cx, const size_t cy, double &z_out) const =0
Get cell 'z' by (cx,cy) cell indices.
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
virtual void dem_update_map()=0
Ensure that all observations are reflected in the map estimate.
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...
bool dem_internal_insertObservation(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D *robotPose=nullptr)
Internal method called by internal_insertObservation()
This namespace contains representation of robot actions and observations.
3D Plane, represented by its equation
double x
X,Y,Z coordinates.
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...
virtual size_t dem_get_size_x() const =0
bool getPoint(TPoint3D &p) const
Gets the content as a point, returning false if the type is not adequate.
unsigned long getPoint(size_t index, float &x, float &y, float &z) const
Access to a given point from map, as a 2D point.
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.
#define IS_CLASS(ptrObj, class_name)
Evaluates to true if the given pointer to an object (derived from mrpt::rtti::CObject) is of the give...
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.
virtual double dem_get_resolution() const =0
bool intersect(const TSegment3D &s1, const TSegment3D &s2, TObject3D &obj)
Gets the intersection between two 3D segments.
virtual ~CHeightGridMap2D_Base()
virtual size_t dem_get_size_y() const =0
3D line, represented by a base point and a director vector.