Main MRPT website > C++ reference for MRPT 1.5.6
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-2017, 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),
23  update_map_after_insertion(true)
24 {
25 }
26 
28 {
29 }
30 
32 {
33 }
34 
35 bool CHeightGridMap2D_Base::getMinMaxHeight(float &z_min, float &z_max) const
36 {
37  const size_t size_x = dem_get_size_x();
38  const size_t size_y = dem_get_size_y();
39 
40  bool any = false;
41  z_min = z_max = 0;
42  for (size_t x=0;x<size_x;x++)
43  for (size_t y=0;y<size_y;y++)
44  {
45  double z;
46  if (dem_get_z_by_cell(x,y,z))
47  {
48  if (!any)
49  {
50  // First:
51  any = true;
52  z_min = z_max = z;
53  }
54  else
55  {
56  mrpt::utils::keep_max(z_max , z );
57  mrpt::utils::keep_min(z_min , z );
58  }
59  }
60  }
61  return any;
62 }
63 
64 
65 
67 {
68  using namespace mrpt::math;
69 
71 
72  obj = TObject3D();
73 
74  const double resolution = dem_get_resolution();
75  float z_min,z_max;
76  if (!getMinMaxHeight(z_min,z_max))
77  return false;
78 
79 
80  // 1st: intersections with 2 horizontal planes at the grid Z limits:
81  const TPlane horz_plane_above(TPoint3D(0,0,z_max+1),TPoint3D(1,0,z_max+1),TPoint3D(0,1,z_max+1));
82  const TPlane horz_plane_below(TPoint3D(0,0,z_min-1),TPoint3D(1,0,z_min-1),TPoint3D(0,1,z_min-1));
83  TPoint3D pt_ab,pt_be;
84  {
85  TObject3D int_ab,int_be;
86  intersect(ray,horz_plane_above, int_ab);
87  intersect(ray,horz_plane_below, int_be);
88 
89  if (!int_ab.getPoint(pt_ab) || !int_be.getPoint(pt_be))
90  return false;
91  }
92 
93  // Now, go from pt_ab -> pt_be doing "ray-tracing" and find the collision with a cell:
94  TPoint3D pt = pt_ab;
95  TPoint3D Apt = pt_be-pt_ab;
96  const double totalDist = Apt.norm();
97  if (totalDist==0) return false;
98  // The step:
99  Apt*= resolution * 0.99/totalDist;
100 
101  TPoint3D Apt_half=Apt;
102  Apt_half*=0.5;
103 
104  const size_t N = ceil(totalDist/resolution);
105 
106  for (size_t i=0;i<N;i++)
107  {
108  // Mid point between this and next step:
109  const TPoint3D testPt = pt + Apt_half;
110  // get its height in the grid:
111  double pt_z;
112  if (dem_get_z(testPt.x,testPt.y, pt_z) )
113  {
114  // Do we go thru the cell?
115  if ( pt_z >= std::min(pt.z,pt.z+Apt.z) && pt_z < std::max(pt.z,pt.z+Apt.z) )
116  {
117  // yes:
118  TPoint3D colPt(testPt.x,testPt.y,pt_z);
119  obj = TObject3D(colPt);
120  return true;
121  }
122  }
123  pt+=Apt;
124  }
125 
126  // No collision found!
127  return false;
128 
129  // None found:
130  MRPT_END
131 }
132 
134 {
135  using namespace mrpt::poses;
136  using namespace mrpt::obs;
137 
138  MRPT_START
139 
140  CPose3D robotPose3D; // Default: 0,0,0
141 
142  if (robotPose)
143  robotPose3D = (*robotPose);
144 
145  // Points to insert:
146  CSimplePointsMap thePointsMoved;
147 
148  if ( IS_CLASS(obs, CObservation2DRangeScan ))
149  {
150  /********************************************************************
151  OBSERVATION TYPE: CObservation2DRangeScan
152  ********************************************************************/
153  const CObservation2DRangeScan *o = static_cast<const CObservation2DRangeScan*>( obs );
154 
155  // Create points map, if not created yet:
157  const CPointsMap *thePoints = o->buildAuxPointsMap<mrpt::maps::CPointsMap>( &opts );
158 
159  // And rotate to the robot pose:
160  thePointsMoved.changeCoordinatesReference( *thePoints, robotPose3D );
161  }
162  else
164  {
165  /********************************************************************
166  OBSERVATION TYPE: CObservationVelodyneScan
167  ********************************************************************/
168  const CObservationVelodyneScan *o = static_cast<const CObservationVelodyneScan*>( obs );
169 
170  // Create points map, if not created yet:
171  thePointsMoved.loadFromVelodyneScan(*o,&robotPose3D);
172  }
173 
174  // Factorized insertion of points, for different observation classes:
175  if (!thePointsMoved.empty())
176  {
177  TPointInsertParams pt_params;
178  pt_params.update_map_after_insertion = false; // update only once at end
179 
180  const size_t N = thePointsMoved.size();
181  for (size_t i=0;i<N;i++)
182  {
183  float x,y,z;
184  thePointsMoved.getPoint(i, x,y,z);
185  insertIndividualPoint(x,y,z,pt_params);
186  } // end for i
187  this->dem_update_map();
188  return true; // Done, new points inserted
189  }
190  return false; // No insertion done
191  MRPT_END
192 }
virtual bool dem_get_z(const double x, const double y, double &z_out) const =0
Get cell 'z' (x,y) by metric coordinates.
GLint GLint GLint GLint GLint GLint y
Definition: glew.h:1166
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 &params=TPointInsertParams())=0
Update the DEM with one new point.
#define min(a, b)
void changeCoordinatesReference(const mrpt::poses::CPose2D &b)
Replace each point by (pose compounding operator).
Definition: CPointsMap.cpp:559
double norm() const
Point norm.
bool getMinMaxHeight(float &z_min, float &z_max) const
Computes the minimum and maximum height in the grid.
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.
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.
struct BASE_IMPEXP TObject3D
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...
virtual void dem_update_map()=0
Ensure that all observations are reflected in the map estimate.
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...
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...
bool dem_internal_insertObservation(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D *robotPose=NULL)
Internal method called by internal_insertObservation()
#define MRPT_END
bool getPoint(TPoint3D &p) const
Gets the content as a point, returning false if the type is not adequate.
GLint GLint GLint GLint GLint x
Definition: glew.h:1166
3D Plane, represented by its equation
GLhandleARB obj
Definition: glew.h:3276
bool update_map_after_insertion
(default: true) run any required operation to ensure the map reflects the changes caused by this poin...
const POINTSMAP * buildAuxPointsMap(const void *options=NULL) const
Returns a cached points map representing this laser scan, building it upon the first call...
virtual size_t dem_get_size_x() const =0
#define MRPT_START
GLdouble GLdouble z
Definition: glew.h:1464
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:72
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::utils::CSerializable) is of t...
Definition: CObject.h:103
Lightweight 3D point.
virtual double dem_get_resolution() const =0
bool BASE_IMPEXP intersect(const TSegment3D &s1, const TSegment3D &s2, TObject3D &obj)
Gets the intersection between two 3D segments.
Definition: geometry.cpp:568
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...
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.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018