MRPT  2.0.4
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 }
mrpt::maps::CPointsMap::getPointsBufferRef_y
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:570
mrpt::maps::CPointsMap::getPointsBufferRef_x
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:564
mrpt::maps::COccupancyGridMap3D::m_is_empty
bool m_is_empty
True upon construction; used by isEmpty()
Definition: COccupancyGridMap3D.h:48
mrpt::containers::CDynamicGrid3D::y2idx
int y2idx(coord_t y) const
Definition: CDynamicGrid3D.h:331
mrpt::math::TPoint3D
TPoint3D_< double > TPoint3D
Lightweight 3D point.
Definition: TPoint3D.h:268
mrpt::maps::COccupancyGridMap3D::TLikelihoodOptions::LF_maxCorrsDistance
float LF_maxCorrsDistance
[LikelihoodField] The max.
Definition: COccupancyGridMap3D.h:290
mrpt::maps::COccupancyGridMap3D::TLikelihoodOptions::rayTracing_stdHit
float rayTracing_stdHit
[rayTracing] The laser range sigma.
Definition: COccupancyGridMap3D.h:299
mrpt::containers::end
const_iterator end() const
Definition: ts_hash_map.h:246
mrpt::obs::T3DPointsProjectionParams
Used in CObservation3DRangeScan::unprojectInto()
Definition: T3DPointsProjectionParams.h:20
MRPT_SAVE_CONFIG_VAR_COMMENT
#define MRPT_SAVE_CONFIG_VAR_COMMENT(variableName, __comment)
Definition: config/CConfigFileBase.h:480
mrpt::math::TPoint3D_< double >
mrpt::max3
const T max3(const T &A, const T &B, const T &C)
Definition: core/include/mrpt/core/bits_math.h:129
mrpt::maps::CLogOddsGridMap3D< OccGridCellTraits::cellType >::m_grid
grid_t m_grid
The actual 3D voxels container.
Definition: CLogOddsGridMap3D.h:35
mrpt::maps::COccupancyGridMap3D::insertRay
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...
Definition: COccupancyGridMap3D_insert.cpp:117
MRPT_LOAD_CONFIG_VAR
#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...
Definition: config/CConfigFileBase.h:306
mrpt::maps::COccupancyGridMap3D::TLikelihoodOptions::LF_maxRange
float LF_maxRange
[LikelihoodField] The max.
Definition: COccupancyGridMap3D.h:284
mrpt::obs::CObservation2DRangeScan
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
Definition: CObservation2DRangeScan.h:54
mrpt::maps::COccupancyGridMap3D::TLikelihoodOptions::loadFromConfigFile
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini" file.
Definition: COccupancyGridMap3D_insert.cpp:211
mrpt::containers::CDynamicGrid3D::isOutOfBounds
bool isOutOfBounds(const int cx, const int cy, const int cz) const
Definition: CDynamicGrid3D.h:230
mrpt::math::TPoint3D_data::z
T z
Definition: TPoint3D.h:29
mrpt::containers::CDynamicGrid3D::x2idx
int x2idx(coord_t x) const
Transform a coordinate values into voxel indexes.
Definition: CDynamicGrid3D.h:327
mrpt::obs::T3DPointsProjectionParams::decimation
uint8_t decimation
(Default:1) If !=1, split the range image in blocks of DxD (D=decimation), and only generates one poi...
Definition: T3DPointsProjectionParams.h:39
mrpt::maps::COccupancyGridMap3D::internal_insertObservationScan3D
void internal_insertObservationScan3D(const mrpt::obs::CObservation3DRangeScan &o, const mrpt::poses::CPose3D &robotPose)
Definition: COccupancyGridMap3D_insert.cpp:93
mrpt::maps::CPointsMap
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors.
Definition: CPointsMap.h:67
CObservation3DRangeScan.h
mrpt::maps::COccupancyGridMap3D::TInsertionOptions::decimation_3d_range
uint16_t decimation_3d_range
Specify the decimation of 3D range scans.
Definition: COccupancyGridMap3D.h:232
mrpt::maps::COccupancyGridMap3D::p2l
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...
Definition: COccupancyGridMap3D.h:122
mrpt::maps::CPointsMap::getPointsBufferRef_z
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:576
mrpt::maps::COccupancyGridMap3D::insertPointCloud
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...
Definition: COccupancyGridMap3D_insert.cpp:73
mrpt::maps::CLogOddsGridMap3D
A generic provider of log-odds grid-map maintainance functions.
Definition: CLogOddsGridMap3D.h:27
mrpt::maps::COccupancyGridMap3D::TLikelihoodOptions::LF_useSquareDist
bool LF_useSquareDist
[LikelihoodField] (Default:false) Use exp(dist^2/std^2) instead of exp(dist^2/std^2)
Definition: COccupancyGridMap3D.h:293
mrpt::containers::CDynamicGrid3D::z2idx
int z2idx(coord_t z) const
Definition: CDynamicGrid3D.h:335
mrpt::maps::COccupancyGridMap3D::voxelType
OccGridCellTraits::cellType voxelType
The type of the map voxels:
Definition: COccupancyGridMap3D.h:40
mrpt::poses::CPose3D::changeCoordinatesReference
void changeCoordinatesReference(const CPose3D &p)
makes: this = p (+) this
Definition: CPose3D.h:423
mrpt::obs::CObservation3DRangeScan::sensorPose
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot.
Definition: CObservation3DRangeScan.h:546
mrpt::maps::COccupancyGridMap3D::TLikelihoodOptions::LF_zHit
float LF_zHit
[LikelihoodField]
Definition: COccupancyGridMap3D.h:282
mrpt::obs::CObservation3DRangeScan
A range or depth 3D scan measurement, as from a time-of-flight range camera or a structured-light dep...
Definition: CObservation3DRangeScan.h:168
mrpt::round
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:24
MRPT_START
#define MRPT_START
Definition: exceptions.h:241
mrpt::config::CConfigFileBase
This class allows loading and storing values and vectors of different types from a configuration text...
Definition: config/CConfigFileBase.h:44
mrpt::maps::COccupancyGridMap3D::TLikelihoodOptions::saveToConfigFile
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.
Definition: COccupancyGridMap3D_insert.cpp:229
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::maps::COccupancyGridMap3D::TInsertionOptions::maxFreenessUpdateCertainty
float maxFreenessUpdateCertainty
A value in the range [0.5,1] for updating a free voxel.
Definition: COccupancyGridMap3D.h:226
iniFile
string iniFile(myDataDir+string("benchmark-options.ini"))
mrpt::maps::CSimplePointsMap
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans.
Definition: CSimplePointsMap.h:30
FRBITS
static constexpr unsigned FRBITS
Definition: COccupancyGridMap3D_insert.cpp:21
mrpt::maps::COccupancyGridMap3D::TLikelihoodOptions::LF_stdHit
float LF_stdHit
[LikelihoodField] The laser range "sigma" used in computations; Default value = 0....
Definition: COccupancyGridMap3D.h:280
mrpt::maps::COccupancyGridMap3D::TLikelihoodOptions::likelihoodMethod
TLikelihoodMethod likelihoodMethod
The selected method to compute an observation likelihood.
Definition: COccupancyGridMap3D.h:277
mrpt::maps::COccupancyGridMap3D::TLikelihoodOptions::rayTracing_decimation
int32_t rayTracing_decimation
[rayTracing] One out of "rayTracing_decimation" rays will be simulated and compared only: set to 1 to...
Definition: COccupancyGridMap3D.h:297
mrpt::math::TPoint3D_data::y
T y
Definition: TPoint3D.h:29
mrpt::obs::detail::unprojectInto
void unprojectInto(mrpt::obs::CObservation3DRangeScan &src_obs, POINTMAP &dest_pointcloud, const mrpt::obs::T3DPointsProjectionParams &projectParams, const mrpt::obs::TRangeImageFilterParams &filterParams)
Definition: CObservation3DRangeScan_project3D_impl.h:111
CPose3D.h
mrpt::obs::CObservation3DRangeScan::maxRange
float maxRange
The maximum range allowed by the device, in meters (e.g.
Definition: CObservation3DRangeScan.h:544
mrpt::maps::COccupancyGridMap3D::insertionOptions
TInsertionOptions insertionOptions
With this struct options are provided to the observation insertion process.
Definition: COccupancyGridMap3D.h:240
mrpt::maps::COccupancyGridMap3D::internal_insertObservation
bool internal_insertObservation(const mrpt::obs::CObservation &obs, const mrpt::poses::CPose3D *robotPose=nullptr) override
Insert the observation information into this map.
Definition: COccupancyGridMap3D_insert.cpp:23
mrpt::maps::CPointsMap::insertionOptions
TInsertionOptions insertionOptions
The options used when inserting observations in the map.
Definition: CPointsMap.h:274
maps-precomp.h
MRPT_END
#define MRPT_END
Definition: exceptions.h:245
mrpt::math::TPoint3D_data::x
T x
X,Y,Z coordinates.
Definition: TPoint3D.h:29
mrpt::obs::CObservation
Declares a class that represents any robot's observation.
Definition: CObservation.h:43
mrpt::maps::COccupancyGridMap3D::TInsertionOptions::decimation
uint16_t decimation
Decimation for insertPointCloud() or 2D range scans (Default: 1)
Definition: COccupancyGridMap3D.h:235
mrpt::maps::COccupancyGridMap3D::TLikelihoodOptions::LF_zRandom
float LF_zRandom
Definition: COccupancyGridMap3D.h:282
mrpt::maps::COccupancyGridMap3D::internal_insertObservationScan2D
void internal_insertObservationScan2D(const mrpt::obs::CObservation2DRangeScan &o, const mrpt::poses::CPose3D &robotPose)
Definition: COccupancyGridMap3D_insert.cpp:49
mrpt::maps
Definition: CBeacon.h:21
CSimplePointsMap.h
mrpt::obs::T3DPointsProjectionParams::takeIntoAccountSensorPoseOnRobot
bool takeIntoAccountSensorPoseOnRobot
(Default: false) If false, local (sensor-centric) coordinates of points are generated.
Definition: T3DPointsProjectionParams.h:26
mrpt::obs::CObservation2DRangeScan::sensorPose
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot at the moment of starting the scan.
Definition: CObservation2DRangeScan.h:122
COccupancyGridMap3D.h
mrpt::maps::CLogOddsGridMap3D< OccGridCellTraits::cellType >::updateCell_fast_free
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.
Definition: CLogOddsGridMap3D.h:83
mrpt::maps::COccupancyGridMap3D::TInsertionOptions::maxOccupancyUpdateCertainty
float maxOccupancyUpdateCertainty
A value in the range [0.5,1] used for updating voxel with a Bayesian approach (default 0....
Definition: COccupancyGridMap3D.h:223
mrpt::maps::COccupancyGridMap3D::OnPostSuccesfulInsertObs
void OnPostSuccesfulInsertObs(const mrpt::obs::CObservation &) override
See base class.
Definition: COccupancyGridMap3D_insert.cpp:205
CObservation2DRangeScan.h
mrpt::maps::CLogOddsGridMap3D< OccGridCellTraits::cellType >::updateCell_fast_occupied
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.
Definition: CLogOddsGridMap3D.h:46
mrpt::maps::COccupancyGridMap3D::TLikelihoodOptions::LF_decimation
uint32_t LF_decimation
[LikelihoodField] The decimation of the points in a scan, default=1 == no decimation
Definition: COccupancyGridMap3D.h:287



Page generated by Doxygen 1.8.17 for MRPT 2.0.4 at Sun Jul 19 17:54:30 UTC 2020