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 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...
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:194
bool empty() const
STL-like method to check whether the map is empty:
Definition: CPointsMap.h:894
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
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:547
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.
GLsizei GLsizei GLuint * obj
Definition: glext.h:4085
With this struct options are provided to the observation insertion process.
Definition: CPointsMap.h:221
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.
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:146
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
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...
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:85
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
Save the point cloud as a PCL PCD file, in either ASCII or binary format.
Definition: CPointsMap.h:441
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: c1796881b Sat Nov 16 19:04:34 2019 +0100 at sáb nov 16 19:15:10 CET 2019