MRPT  1.9.9
CHeightGridMap2D_Base.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "maps-precomp.h" // Precomp header
11 
13 #include <mrpt/math/geometry.h>
17 
18 using namespace mrpt::maps;
19 using namespace std;
20 
22  : pt_z_std(0.0), update_map_after_insertion(true)
23 {
24 }
25 
28 bool CHeightGridMap2D_Base::getMinMaxHeight(float& z_min, float& z_max) const
29 {
30  const size_t size_x = dem_get_size_x();
31  const size_t size_y = dem_get_size_y();
32 
33  bool any = false;
34  z_min = z_max = 0;
35  for (size_t x = 0; x < size_x; x++)
36  for (size_t y = 0; y < size_y; y++)
37  {
38  double z;
39  if (dem_get_z_by_cell(x, y, z))
40  {
41  if (!any)
42  {
43  // First:
44  any = true;
45  z_min = z_max = z;
46  }
47  else
48  {
49  mrpt::keep_max(z_max, z);
50  mrpt::keep_min(z_min, z);
51  }
52  }
53  }
54  return any;
55 }
56 
59 {
60  using namespace mrpt::math;
61 
63 
64  obj = TObject3D();
65 
66  const double resolution = dem_get_resolution();
67  float z_min, z_max;
68  if (!getMinMaxHeight(z_min, z_max)) return false;
69 
70  // 1st: intersections with 2 horizontal planes at the grid Z limits:
71  const TPlane horz_plane_above(
72  TPoint3D(0, 0, z_max + 1), TPoint3D(1, 0, z_max + 1),
73  TPoint3D(0, 1, z_max + 1));
74  const TPlane horz_plane_below(
75  TPoint3D(0, 0, z_min - 1), TPoint3D(1, 0, z_min - 1),
76  TPoint3D(0, 1, z_min - 1));
77  TPoint3D pt_ab, pt_be;
78  {
79  TObject3D int_ab, int_be;
80  intersect(ray, horz_plane_above, int_ab);
81  intersect(ray, horz_plane_below, int_be);
82 
83  if (!int_ab.getPoint(pt_ab) || !int_be.getPoint(pt_be)) return false;
84  }
85 
86  // Now, go from pt_ab -> pt_be doing "ray-tracing" and find the collision
87  // with a cell:
88  TPoint3D pt = pt_ab;
89  TPoint3D Apt = pt_be - pt_ab;
90  const double totalDist = Apt.norm();
91  if (totalDist == 0) return false;
92  // The step:
93  Apt *= resolution * 0.99 / totalDist;
94 
95  TPoint3D Apt_half = Apt;
96  Apt_half *= 0.5;
97 
98  const size_t N = ceil(totalDist / resolution);
99 
100  for (size_t i = 0; i < N; i++)
101  {
102  // Mid point between this and next step:
103  const TPoint3D testPt = pt + Apt_half;
104  // get its height in the grid:
105  double pt_z;
106  if (dem_get_z(testPt.x, testPt.y, pt_z))
107  {
108  // Do we go thru the cell?
109  if (pt_z >= std::min(pt.z, pt.z + Apt.z) &&
110  pt_z < std::max(pt.z, pt.z + Apt.z))
111  {
112  // yes:
113  TPoint3D colPt(testPt.x, testPt.y, pt_z);
114  obj = TObject3D(colPt);
115  return true;
116  }
117  }
118  pt += Apt;
119  }
120 
121  // No collision found!
122  return false;
123 
124  // None found:
125  MRPT_END
126 }
127 
129  const mrpt::obs::CObservation* obs, const mrpt::poses::CPose3D* robotPose)
130 {
131  using namespace mrpt::poses;
132  using namespace mrpt::obs;
133 
134  MRPT_START
135 
136  CPose3D robotPose3D; // Default: 0,0,0
137 
138  if (robotPose) robotPose3D = (*robotPose);
139 
140  // Points to insert:
141  CSimplePointsMap thePointsMoved;
142 
144  {
145  /********************************************************************
146  OBSERVATION TYPE: CObservation2DRangeScan
147  ********************************************************************/
148  const CObservation2DRangeScan* o =
149  static_cast<const CObservation2DRangeScan*>(obs);
150 
151  // Create points map, if not created yet:
153  const CPointsMap* thePoints =
155 
156  // And rotate to the robot pose:
157  thePointsMoved.changeCoordinatesReference(*thePoints, robotPose3D);
158  }
159  else if (IS_CLASS(obs, CObservationVelodyneScan))
160  {
161  /********************************************************************
162  OBSERVATION TYPE: CObservationVelodyneScan
163  ********************************************************************/
164  const CObservationVelodyneScan* o =
165  static_cast<const CObservationVelodyneScan*>(obs);
166 
167  // Create points map, if not created yet:
168  thePointsMoved.loadFromVelodyneScan(*o, &robotPose3D);
169  }
170 
171  // Factorized insertion of points, for different observation classes:
172  if (!thePointsMoved.empty())
173  {
174  TPointInsertParams pt_params;
175  pt_params.update_map_after_insertion =
176  false; // update only once at end
177 
178  const size_t N = thePointsMoved.size();
179  for (size_t i = 0; i < N; i++)
180  {
181  float x, y, z;
182  thePointsMoved.getPoint(i, x, y, z);
183  insertIndividualPoint(x, y, z, pt_params);
184  } // end for i
185  this->dem_update_map();
186  return true; // Done, new points inserted
187  }
188  return false; // No insertion done
189  MRPT_END
190 }
virtual bool dem_get_z(const double x, const double y, double &z_out) const =0
Get cell &#39;z&#39; (x,y) by metric coordinates.
bool empty() const
STL-like method to check whether the map is empty:
Definition: CPointsMap.h:854
Extra params for insertIndividualPoint()
#define MRPT_START
Definition: exceptions.h:262
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 &params=TPointInsertParams())=0
Update the DEM with one new point.
GLdouble GLdouble z
Definition: glext.h:3872
#define min(a, b)
void changeCoordinatesReference(const mrpt::poses::CPose2D &b)
Replace each point by (pose compounding operator).
Definition: CPointsMap.cpp:574
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 &#39;z&#39; 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.
STL namespace.
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
Definition: glext.h:4070
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.
Definition: CPointsMap.h:205
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors...
Definition: CPointsMap.h:64
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.
Definition: CPointsMap.cpp:211
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).
Definition: CPose3D.h:86
Declares a class that represents any robot&#39;s observation.
Definition: CObservation.h:43
#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...
Definition: CObject.h:102
#define MRPT_END
Definition: exceptions.h:266
GLenum GLint GLint y
Definition: glext.h:3538
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...
GLenum GLint x
Definition: glext.h:3538
Lightweight 3D point.
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.
Definition: CPointsMap.h:408
virtual double dem_get_resolution() const =0
bool intersect(const TSegment3D &s1, const TSegment3D &s2, TObject3D &obj)
Gets the intersection between two 3D segments.
Definition: geometry.cpp:631
virtual size_t dem_get_size_y() const =0
3D line, represented by a base point and a director vector.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020