MRPT  1.9.9
COccupancyGridMap3D_insert.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 
16 #include <mrpt/poses/CPose3D.h>
17 
18 using namespace mrpt::maps;
19 
20 // bits to left-shift for fixed-point arithmetic simulation in raytracing.
21 static constexpr unsigned FRBITS = 9;
22 
24  const mrpt::obs::CObservation& obs, const mrpt::poses::CPose3D* robPose)
25 {
27 
28  const mrpt::poses::CPose3D robotPose3D =
29  (robPose != nullptr) ? *robPose : mrpt::poses::CPose3D();
30 
31  if (auto* o = dynamic_cast<const mrpt::obs::CObservation2DRangeScan*>(&obs);
32  o != nullptr)
33  {
34  this->internal_insertObservationScan2D(*o, robotPose3D);
35  return true;
36  }
37  if (auto* o = dynamic_cast<const mrpt::obs::CObservation3DRangeScan*>(&obs);
38  o != nullptr)
39  {
40  this->internal_insertObservationScan3D(*o, robotPose3D);
41  return true;
42  }
43 
44  return false;
45 
46  MRPT_END
47 }
48 
51  const mrpt::poses::CPose3D& robotPose)
52 {
54 
55  // Convert to point cloud:
57  pts.insertionOptions.also_interpolate = false;
58  pts.insertionOptions.addToExistingPointsMap = true;
59  pts.insertionOptions.disableDeletion = true;
60  pts.insertionOptions.fuseWithExisting = false;
61  pts.insertionOptions.insertInvalidPoints = false;
62  pts.insertionOptions.minDistBetweenLaserPoints = .0; // insert all
63 
64  pts.loadFromRangeScan(o, &robotPose);
65 
66  const auto sensorPose3D = robotPose + o.sensorPose;
67  const auto sensorPt = mrpt::math::TPoint3D(sensorPose3D.asTPose());
68  insertPointCloud(sensorPt, pts);
69 
70  MRPT_END
71 }
72 
74  const mrpt::math::TPoint3D& sensorPt, const mrpt::maps::CPointsMap& pts,
75  const float maxValidRange)
76 {
78 
79  const auto& xs = pts.getPointsBufferRef_x();
80  const auto& ys = pts.getPointsBufferRef_y();
81  const auto& zs = pts.getPointsBufferRef_z();
82 
83  // Process points one by one as rays:
84  for (std::size_t idx = 0; idx < xs.size();
86  {
87  insertRay(sensorPt, mrpt::math::TPoint3D(xs[idx], ys[idx], zs[idx]));
88  }
89 
90  MRPT_END
91 }
92 
95  const mrpt::poses::CPose3D& robotPose)
96 {
98 
99  // Depth -> 3D points:
102  pp.takeIntoAccountSensorPoseOnRobot = false; // done below
104 
105  const_cast<mrpt::obs::CObservation3DRangeScan&>(o)
107 
108  const auto sensorPose3D = robotPose + o.sensorPose;
109  // Shift everything to its proper pose in the global frame:
110  pts.changeCoordinatesReference(sensorPose3D);
111 
112  const auto sensorPt = mrpt::math::TPoint3D(sensorPose3D.asTPose());
113  insertPointCloud(sensorPt, pts, o.maxRange);
114 
115  MRPT_END
116 }
117 
119  const mrpt::math::TPoint3D& sensor, const mrpt::math::TPoint3D& end,
120  bool endIsOccupied)
121 {
122  MRPT_START
123 
124  // m_likelihoodCacheOutDated = true;
125 
126  // the occupied and free probabilities:
127  const float maxCertainty = insertionOptions.maxOccupancyUpdateCertainty;
128  float maxFreeCertainty = insertionOptions.maxFreenessUpdateCertainty;
129  if (maxFreeCertainty == .0f) maxFreeCertainty = maxCertainty;
130 
131  const voxelType logodd_observation_free =
132  std::max<voxelType>(1, p2l(maxFreeCertainty));
133  const voxelType logodd_observation_occupied =
134  3 * std::max<voxelType>(1, p2l(maxCertainty));
135 
136  // saturation limits:
137  const voxelType logodd_thres_occupied =
139  logodd_observation_occupied;
140  const voxelType logodd_thres_free =
141  CLogOddsGridMap3D<voxelType>::CELLTYPE_MAX - logodd_observation_free;
142 
143  // Start: (in cell index units)
144  int cx = m_grid.x2idx(sensor.x);
145  int cy = m_grid.y2idx(sensor.y);
146  int cz = m_grid.z2idx(sensor.z);
147 
148  // End: (in cell index units)
149  int trg_cx = m_grid.x2idx(end.x);
150  int trg_cy = m_grid.y2idx(end.y);
151  int trg_cz = m_grid.z2idx(end.z);
152 
153  // Skip if totally out of bounds:
154  if (m_grid.isOutOfBounds(cx, cy, cz)) return;
155 
156  // Use "fractional integers" to approximate float operations
157  // during the ray tracing:
158  const int Acx = trg_cx - cx;
159  const int Acy = trg_cy - cy;
160  const int Acz = trg_cz - cz;
161 
162  const int Acx_ = std::abs(Acx);
163  const int Acy_ = std::abs(Acy);
164  const int Acz_ = std::abs(Acz);
165 
166  const int nStepsRay = mrpt::max3(Acx_, Acy_, Acz_);
167  if (!nStepsRay) return; // May be...
168 
169  const float N_1 = 1.0f / nStepsRay;
170 
171  // Increments at each raytracing step:
172  const int frAcx = (Acx < 0 ? -1 : +1) * round((Acx_ << FRBITS) * N_1);
173  const int frAcy = (Acy < 0 ? -1 : +1) * round((Acy_ << FRBITS) * N_1);
174  const int frAcz = (Acz < 0 ? -1 : +1) * round((Acz_ << FRBITS) * N_1);
175 
176  // fractional integers for the running raytracing point:
177  int frCX = cx << FRBITS;
178  int frCY = cy << FRBITS;
179  int frCZ = cz << FRBITS;
180 
181  for (int nStep = 0; nStep < nStepsRay; nStep++)
182  {
184  cx, cy, cz, logodd_observation_free, logodd_thres_free);
185 
186  frCX += frAcx;
187  frCY += frAcy;
188  frCZ += frAcz;
189 
190  cx = frCX >> FRBITS;
191  cy = frCY >> FRBITS;
192  cz = frCZ >> FRBITS;
193 
194  // Already out of bounds?
195  if (m_grid.isOutOfBounds(cx, cy, cz)) break;
196  }
197 
198  // And finally, the occupied cell at the end:
200  trg_cx, trg_cy, trg_cz, logodd_observation_occupied,
201  logodd_thres_occupied);
202 
203  MRPT_END
204 }
205 
208 {
209  m_is_empty = false;
210 }
211 
213  const mrpt::config::CConfigFileBase& iniFile, const std::string& section)
214 {
216  iniFile.read_enum(section, "likelihoodMethod", likelihoodMethod);
217 
218  MRPT_LOAD_CONFIG_VAR(LF_stdHit, float, iniFile, section);
219  MRPT_LOAD_CONFIG_VAR(LF_zHit, float, iniFile, section);
220  MRPT_LOAD_CONFIG_VAR(LF_zRandom, float, iniFile, section);
221  MRPT_LOAD_CONFIG_VAR(LF_maxRange, int, iniFile, section);
225 
228 }
229 
232 {
233  MRPT_SAVE_CONFIG_VAR_COMMENT(LF_stdHit, "");
234  MRPT_SAVE_CONFIG_VAR_COMMENT(LF_zHit, "");
235  MRPT_SAVE_CONFIG_VAR_COMMENT(LF_zRandom, "");
236  MRPT_SAVE_CONFIG_VAR_COMMENT(LF_maxRange, "");
237  MRPT_SAVE_CONFIG_VAR_COMMENT(LF_decimation, "");
238  MRPT_SAVE_CONFIG_VAR_COMMENT(LF_maxCorrsDistance, "");
239  MRPT_SAVE_CONFIG_VAR_COMMENT(LF_useSquareDist, "");
240 
241  MRPT_SAVE_CONFIG_VAR_COMMENT(rayTracing_stdHit, "");
242  MRPT_SAVE_CONFIG_VAR_COMMENT(rayTracing_decimation, "");
243 }
#define MRPT_START
Definition: exceptions.h:241
double x
X,Y,Z coordinates.
Definition: TPoint3D.h:83
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini" file.
uint8_t decimation
(Default:1) If !=1, split the range image in blocks of DxD (D=decimation), and only generates one poi...
uint32_t LF_decimation
[LikelihoodField] The decimation of the points in a scan, default=1 == no decimation ...
const T max3(const T &A, const T &B, const T &C)
const mrpt::aligned_std_vector< float > & getPointsBufferRef_y() const
Provides a direct access to a read-only reference of the internal point buffer.
Definition: CPointsMap.h:537
uint16_t decimation
Decimation for insertPointCloud() or 2D range scans (Default: 1)
Declares a class derived from "CObservation" that encapsules a 3D range scan measurement, as from a time-of-flight range camera or any other RGBD sensor.
void internal_insertObservationScan2D(const mrpt::obs::CObservation2DRangeScan &o, const mrpt::poses::CPose3D &robotPose)
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans. ...
float LF_stdHit
[LikelihoodField] The laser range "sigma" used in computations; Default value = 0.35
uint16_t decimation_3d_range
Specify the decimation of 3D range scans.
TInsertionOptions insertionOptions
With this struct options are provided to the observation insertion process.
GLdouble s
Definition: glext.h:3682
static void updateCell_fast_occupied(cell_t *theCell, const cell_t logodd_obs, const cell_t thres)
Performs Bayesian fusion of a new observation of a cell.
bool LF_useSquareDist
[LikelihoodField] (Default:false) Use exp(dist^2/std^2) instead of exp(dist^2/std^2) ...
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 class allows loading and storing values and vectors of different types from a configuration text...
bool internal_insertObservation(const mrpt::obs::CObservation &obs, const mrpt::poses::CPose3D *robotPose=nullptr) override
Insert the observation information into this map.
const mrpt::aligned_std_vector< float > & getPointsBufferRef_z() const
Provides a direct access to a read-only reference of the internal point buffer.
Definition: CPointsMap.h:543
bool isOutOfBounds(const int cx, const int cy, const int cz) const
const GLubyte * c
Definition: glext.h:6406
void project3DPointsFromDepthImageInto(mrpt::obs::CObservation3DRangeScan &src_obs, POINTMAP &dest_pointcloud, const mrpt::obs::T3DPointsProjectionParams &projectParams, const mrpt::obs::TRangeImageFilterParams &filterParams)
int x2idx(coord_t x) const
Transform a coordinate values into voxel indexes.
GLuint GLuint end
Definition: glext.h:3532
int32_t rayTracing_decimation
[rayTracing] One out of "rayTracing_decimation" rays will be simulated and compared only: set to 1 to...
void insertRay(const mrpt::math::TPoint3D &sensor, const mrpt::math::TPoint3D &end, bool endIsOccupied=true)
Increases the freeness of a ray segment, and the occupancy of the voxel at its end point (unless endI...
void changeCoordinatesReference(const CPose3D &p)
makes: this = p (+) this
Definition: CPose3D.h:419
string iniFile(myDataDir+string("benchmark-options.ini"))
void OnPostSuccesfulInsertObs(const mrpt::obs::CObservation &) override
See base class.
bool m_is_empty
True upon construction; used by isEmpty()
GLsizei const GLchar ** string
Definition: glext.h:4116
float maxFreenessUpdateCertainty
A value in the range [0.5,1] for updating a free voxel.
#define MRPT_LOAD_CONFIG_VAR( variableName, variableType, configFileObject, sectionNameStr)
An useful macro for loading variables stored in a INI-like file under a key with the same name that t...
void internal_insertObservationScan3D(const mrpt::obs::CObservation3DRangeScan &o, const mrpt::poses::CPose3D &robotPose)
void saveToConfigFile(mrpt::config::CConfigFileBase &c, const std::string &s) const override
This method saves the options to a ".ini"-like file or memory-stored string list. ...
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot.
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
float maxOccupancyUpdateCertainty
A value in the range [0.5,1] used for updating voxel with a Bayesian approach (default 0...
Used in CObservation3DRangeScan::project3DPointsFromDepthImageInto()
static constexpr unsigned FRBITS
#define MRPT_END
Definition: exceptions.h:245
void insertPointCloud(const mrpt::math::TPoint3D &sensorCenter, const mrpt::maps::CPointsMap &pts, const float maxValidRange=std::numeric_limits< float >::max())
Calls insertRay() for each point in the point cloud, using as sensor central point (the origin of all...
TLikelihoodMethod likelihoodMethod
The selected method to compute an observation likelihood.
#define MRPT_SAVE_CONFIG_VAR_COMMENT(variableName, __comment)
A generic provider of log-odds grid-map maintainance functions.
TInsertionOptions insertionOptions
The options used when inserting observations in the map.
Definition: CPointsMap.h:271
static voxelType p2l(const float p)
Scales a real valued probability in [0,1] to an integer representation of: log(p)-log(1-p) in the val...
static void updateCell_fast_free(cell_t *theCell, const cell_t logodd_obs, const cell_t thres)
Performs Bayesian fusion of a new observation of a cell.
Lightweight 3D point.
Definition: TPoint3D.h:90
const mrpt::aligned_std_vector< float > & getPointsBufferRef_x() const
Provides a direct access to a read-only reference of the internal point buffer.
Definition: CPointsMap.h:531
float rayTracing_stdHit
[rayTracing] The laser range sigma.
OccGridCellTraits::cellType voxelType
The type of the map voxels:
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot at the moment of starting the scan.
bool takeIntoAccountSensorPoseOnRobot
(Default: false) If false, local (sensor-centric) coordinates of points are generated.
float maxRange
The maximum range allowed by the device, in meters (e.g.
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:23



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ce1a28c9f Fri Aug 23 08:02:09 2019 +0200 at vie ago 23 08:10:11 CEST 2019