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-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 
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).unprojectInto(pts, pp);
106 
107  const auto sensorPose3D = robotPose + o.sensorPose;
108  // Shift everything to its proper pose in the global frame:
109  pts.changeCoordinatesReference(sensorPose3D);
110 
111  const auto sensorPt = mrpt::math::TPoint3D(sensorPose3D.asTPose());
112  insertPointCloud(sensorPt, pts, o.maxRange);
113 
114  MRPT_END
115 }
116 
118  const mrpt::math::TPoint3D& sensor, const mrpt::math::TPoint3D& end,
119  bool endIsOccupied)
120 {
121  MRPT_START
122 
123  // m_likelihoodCacheOutDated = true;
124 
125  // the occupied and free probabilities:
126  const float maxCertainty = insertionOptions.maxOccupancyUpdateCertainty;
127  float maxFreeCertainty = insertionOptions.maxFreenessUpdateCertainty;
128  if (maxFreeCertainty == .0f) maxFreeCertainty = maxCertainty;
129 
130  const voxelType logodd_observation_free =
131  std::max<voxelType>(1, p2l(maxFreeCertainty));
132  const voxelType logodd_observation_occupied =
133  3 * std::max<voxelType>(1, p2l(maxCertainty));
134 
135  // saturation limits:
136  const voxelType logodd_thres_occupied =
138  logodd_observation_occupied;
139  const voxelType logodd_thres_free =
140  CLogOddsGridMap3D<voxelType>::CELLTYPE_MAX - logodd_observation_free;
141 
142  // Start: (in cell index units)
143  int cx = m_grid.x2idx(sensor.x);
144  int cy = m_grid.y2idx(sensor.y);
145  int cz = m_grid.z2idx(sensor.z);
146 
147  // End: (in cell index units)
148  int trg_cx = m_grid.x2idx(end.x);
149  int trg_cy = m_grid.y2idx(end.y);
150  int trg_cz = m_grid.z2idx(end.z);
151 
152  // Skip if totally out of bounds:
153  if (m_grid.isOutOfBounds(cx, cy, cz)) return;
154 
155  // Use "fractional integers" to approximate float operations
156  // during the ray tracing:
157  const int Acx = trg_cx - cx;
158  const int Acy = trg_cy - cy;
159  const int Acz = trg_cz - cz;
160 
161  const int Acx_ = std::abs(Acx);
162  const int Acy_ = std::abs(Acy);
163  const int Acz_ = std::abs(Acz);
164 
165  const int nStepsRay = mrpt::max3(Acx_, Acy_, Acz_);
166  if (!nStepsRay) return; // May be...
167 
168  const float N_1 = 1.0f / nStepsRay;
169 
170  // Increments at each raytracing step:
171  const int frAcx = (Acx < 0 ? -1 : +1) * round((Acx_ << FRBITS) * N_1);
172  const int frAcy = (Acy < 0 ? -1 : +1) * round((Acy_ << FRBITS) * N_1);
173  const int frAcz = (Acz < 0 ? -1 : +1) * round((Acz_ << FRBITS) * N_1);
174 
175  // fractional integers for the running raytracing point:
176  int frCX = cx << FRBITS;
177  int frCY = cy << FRBITS;
178  int frCZ = cz << FRBITS;
179 
180  for (int nStep = 0; nStep < nStepsRay; nStep++)
181  {
183  cx, cy, cz, logodd_observation_free, logodd_thres_free);
184 
185  frCX += frAcx;
186  frCY += frAcy;
187  frCZ += frAcz;
188 
189  cx = frCX >> FRBITS;
190  cy = frCY >> FRBITS;
191  cz = frCZ >> FRBITS;
192 
193  // Already out of bounds?
194  if (m_grid.isOutOfBounds(cx, cy, cz)) break;
195  }
196 
197  // And finally, the occupied cell at the end:
199  trg_cx, trg_cy, trg_cz, logodd_observation_occupied,
200  logodd_thres_occupied);
201 
202  MRPT_END
203 }
204 
207 {
208  m_is_empty = false;
209 }
210 
212  const mrpt::config::CConfigFileBase& iniFile, const std::string& section)
213 {
215  iniFile.read_enum(section, "likelihoodMethod", likelihoodMethod);
216 
217  MRPT_LOAD_CONFIG_VAR(LF_stdHit, float, iniFile, section);
218  MRPT_LOAD_CONFIG_VAR(LF_zHit, float, iniFile, section);
219  MRPT_LOAD_CONFIG_VAR(LF_zRandom, float, iniFile, section);
220  MRPT_LOAD_CONFIG_VAR(LF_maxRange, int, iniFile, section);
224 
227 }
228 
230  mrpt::config::CConfigFileBase& c, const std::string& s) const
231 {
232  MRPT_SAVE_CONFIG_VAR_COMMENT(LF_stdHit, "");
233  MRPT_SAVE_CONFIG_VAR_COMMENT(LF_zHit, "");
234  MRPT_SAVE_CONFIG_VAR_COMMENT(LF_zRandom, "");
235  MRPT_SAVE_CONFIG_VAR_COMMENT(LF_maxRange, "");
236  MRPT_SAVE_CONFIG_VAR_COMMENT(LF_decimation, "");
237  MRPT_SAVE_CONFIG_VAR_COMMENT(LF_maxCorrsDistance, "");
238  MRPT_SAVE_CONFIG_VAR_COMMENT(LF_useSquareDist, "");
239 
240  MRPT_SAVE_CONFIG_VAR_COMMENT(rayTracing_stdHit, "");
241  MRPT_SAVE_CONFIG_VAR_COMMENT(rayTracing_decimation, "");
242 }
#define MRPT_START
Definition: exceptions.h:241
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 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:551
uint16_t decimation
Decimation for insertPointCloud() or 2D range scans (Default: 1)
A range or depth 3D scan measurement, as from a time-of-flight range camera or a structured-light dep...
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.
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:557
bool isOutOfBounds(const int cx, const int cy, const int cz) const
int x2idx(coord_t x) const
Transform a coordinate values into voxel indexes.
int32_t rayTracing_decimation
[rayTracing] One out of "rayTracing_decimation" rays will be simulated and compared only: set to 1 to...
void unprojectInto(mrpt::obs::CObservation3DRangeScan &src_obs, POINTMAP &dest_pointcloud, const mrpt::obs::T3DPointsProjectionParams &projectParams, const mrpt::obs::TRangeImageFilterParams &filterParams)
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:423
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()
TPoint3D_< double > TPoint3D
Lightweight 3D point.
Definition: TPoint3D.h:268
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)
T x
X,Y,Z coordinates.
Definition: TPoint3D.h:29
const_iterator end() const
Definition: ts_hash_map.h:246
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:85
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::unprojectInto()
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:272
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.
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:545
float rayTracing_stdHit
[rayTracing] The laser range sigma.
const T max3(const T &A, const T &B, const T &C)
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:24



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020