MRPT  2.0.1
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-2020, 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 
12 #include <mrpt/core/round.h>
15 #include <mrpt/math/geometry.h>
18 
19 using namespace mrpt::maps;
20 using namespace std;
21 
23 
24  = default;
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 = mrpt::d2f(z);
46  }
47  else
48  {
49  mrpt::keep_max<float>(z_max, z);
50  mrpt::keep_min<float>(z_min, z);
51  }
52  }
53  }
54  return any;
55 }
56 
58  const mrpt::math::TLine3D& ray, mrpt::math::TObject3D& obj) const
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 = mrpt::round(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 auto& o = static_cast<const CObservation2DRangeScan&>(obs);
149 
150  // Create points map, if not created yet:
152  const auto* thePoints =
153  o.buildAuxPointsMap<mrpt::maps::CPointsMap>(&opts);
154 
155  // And rotate to the robot pose:
156  thePointsMoved.changeCoordinatesReference(*thePoints, robotPose3D);
157  }
158  else if (IS_CLASS(obs, CObservationVelodyneScan))
159  {
160  /********************************************************************
161  OBSERVATION TYPE: CObservationVelodyneScan
162  ********************************************************************/
163  const auto& o = static_cast<const CObservationVelodyneScan&>(obs);
164 
165  // Create points map, if not created yet:
166  thePointsMoved.loadFromVelodyneScan(o, &robotPose3D);
167  }
168 
169  // Factorized insertion of points, for different observation classes:
170  if (!thePointsMoved.empty())
171  {
172  TPointInsertParams pt_params;
173  pt_params.update_map_after_insertion =
174  false; // update only once at end
175 
176  const size_t N = thePointsMoved.size();
177  for (size_t i = 0; i < N; i++)
178  {
179  float x, y, z;
180  thePointsMoved.getPoint(i, x, y, z);
181  insertIndividualPoint(x, y, z, pt_params);
182  } // end for i
183  this->dem_update_map();
184  return true; // Done, new points inserted
185  }
186  return false; // No insertion done
187  MRPT_END
188 }
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:192
bool empty() const
STL-like method to check whether the map is empty:
Definition: CPointsMap.h:887
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...
void changeCoordinatesReference(const mrpt::poses::CPose2D &b)
Replace each point by (pose compounding operator).
Definition: CPointsMap.cpp:545
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.
With this struct options are provided to the observation insertion process.
Definition: CPointsMap.h:220
float d2f(const double d)
shortcut for static_cast<float>(double)
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
TPoint3D_< double > TPoint3D
Lightweight 3D point.
Definition: TPoint3D.h:268
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
T x
X,Y,Z coordinates.
Definition: TPoint3D.h:29
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()
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...
T norm() const
Point norm: |v| = sqrt(x^2+y^2+z^2)
Definition: TPoint3D.h:150
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:440
bool intersect(const TSegment3D &s1, const TSegment3D &s2, TObject3D &obj)
Gets the intersection between two 3D segments.
Definition: geometry.cpp:617
3D line, represented by a base point and a director vector.
Definition: TLine3D.h:19
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:24



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020