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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ee555d257 Fri Aug 16 10:05:39 2019 +0200 at vie ago 16 10:10:14 CEST 2019