Go to the documentation of this file.
31 using namespace std::string_literals;
34 const auto sSectCreation =
sect +
"_creationOpts"s;
51 std::ostream&
out)
const
72 obj->insertionOptions = def.insertionOpts;
73 obj->likelihoodOptions = def.likelihoodOpts;
100 double res,
float default_value)
111 const auto def_value =
p2l(default_value);
113 cmin.
x, cmax.
x, cmin.
y, cmax.
y, cmin.
z, cmax.
z, res, res, &def_value);
123 float new_cells_default_value)
127 const auto def_value =
p2l(new_cells_default_value);
128 const double additionalMargin = .0;
130 cmin.
x, cmax.
x, cmin.
y, cmax.
y, cmin.
z, cmax.
z, def_value,
224 const TColorf general_color = gl_obj.
getColor();
225 const TColor general_color_u = general_color.
asTColor();
244 const float inv_dz = 1.0f /
d2f(bbmax.
z - bbmin.
z + 0.01f);
259 const bool is_occupied = occ > 0.501f;
260 const bool is_free = occ < 0.499f;
268 case COctoMapVoxels::FIXED:
269 vx_color = general_color_u;
271 case COctoMapVoxels::COLOR_FROM_HEIGHT:
272 coefc = 255 * inv_dz *
d2f(z - bbmin.
z);
274 f2u8(coefc * general_color.R),
275 f2u8(coefc * general_color.G),
276 f2u8(coefc * general_color.B),
277 f2u8(255 * general_color.A));
280 case COctoMapVoxels::COLOR_FROM_OCCUPANCY:
281 coefc = 240 * (1 - occ) + 15;
283 f2u8(coefc * general_color.R),
284 f2u8(coefc * general_color.G),
285 f2u8(coefc * general_color.B),
286 f2u8(255 * general_color.A));
289 case COctoMapVoxels::TRANSPARENCY_FROM_OCCUPANCY:
290 coeft = 255 - 510 * (1 - occ);
295 vx_color = general_color.asTColor();
299 case COctoMapVoxels::TRANS_AND_COLOR_FROM_OCCUPANCY:
300 coefc = 240 * (1 - occ) + 15;
302 f2u8(coefc * general_color.R),
303 f2u8(coefc * general_color.G),
304 f2u8(coefc * general_color.B), 50);
307 case COctoMapVoxels::MIXED:
308 coefc =
d2f(255 * inv_dz * (z - bbmin.
z));
309 coeft =
d2f(255 - 510 * (1 - occ));
315 f2u8(coefc * general_color.R),
316 f2u8(coefc * general_color.G),
317 f2u8(coefc * general_color.B),
318 static_cast<uint8_t
>(coeft));
325 const size_t vx_set =
361 outObj->insert(gl_obj);
368 #ifdef OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS
377 #ifdef OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS
380 out.WriteBufferFixEndianness
416 uint8_t bitsPerCellStream;
417 in >> bitsPerCellStream;
419 #ifdef OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS
420 ASSERT_(bitsPerCellStream == 8);
422 ASSERT_(bitsPerCellStream == 16);
428 #ifdef OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS
469 const int8_t version = 0;
471 out << generateGridLines << generateOccupiedVoxels << visibleOccupiedVoxels
472 << generateFreeVoxels << visibleFreeVoxels;
484 in >> generateGridLines >> generateOccupiedVoxels >>
485 visibleOccupiedVoxels >> generateFreeVoxels >>
495 const std::string& filNamePrefix)
const
497 using namespace std::string_literals;
498 const auto fil = filNamePrefix +
".txt";
void updateCell(int cx_idx, int cy_idx, int cz_idx, float v)
Performs the Bayesian fusion of a new observation of a cell.
bool isCubeTransparencyEnabled() const
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string §ion) override
This method load the options from a ".ini" file.
bool generateFreeVoxels
Generate voxels for the freespace (Default=true)
#define ASSERT_ABOVE_(__A, __B)
void resizeGrid(const mrpt::math::TPoint3D &corner_min, const mrpt::math::TPoint3D &corner_max, float new_voxels_default_value=0.5f)
Change the size of gridmap, maintaining previous contents.
bool m_is_empty
True upon construction; used by isEmpty()
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
T * cellByIndex(unsigned int cx, unsigned int cy, unsigned int cz)
Returns a pointer to the contents of a voxel given by its voxel indexes, or nullptr if it is out of t...
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
void fill(float default_value=0.5f)
Fills all the voxels with a default value.
float LF_maxCorrsDistance
[LikelihoodField] The max.
float rayTracing_stdHit
[rayTracing] The laser range sigma.
TLikelihoodMethod
The type for selecting a likelihood computation method.
std::shared_ptr< mrpt::opengl ::CSetOfObjects > Ptr
grid_t m_grid
The actual 3D voxels container.
#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...
float LF_maxRange
[LikelihoodField] The max.
coord_t getResolutionZ() const
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string §ion) override
This method load the options from a ".ini" file.
bool isOutOfBounds(const int cx, const int cy, const int cz) const
void getAsOctoMapVoxels(mrpt::opengl::COctoMapVoxels &gl_obj) const
The info of each grid block.
#define MAP_DEFINITION_REGISTER(_CLASSNAME_STRINGS, _CLASSNAME_WITH_NS)
Registers one map class into TMetricMapInitializer factory.
mrpt::maps::COccupancyGridMap3D::TInsertionOptions insertionOpts
Observations insertion options.
uint8_t f2u8(const float f)
converts a float [0,1] into an uint8_t [0,255] (without checking for out of bounds)
void resizeVoxelSets(const size_t nVoxelSets)
TColor asTColor() const
Returns the 0-255 integer version of this color: RGBA_u8
Parameters for CMetricMap::compute3DMatchingRatio()
coord_t idx2z(int cz) const
bool isEmpty() const override
Returns true upon map construction or after calling clear(), the return changes to false upon success...
mrpt::vision::TStereoCalibResults out
COccupancyGridMap3D(const mrpt::math::TPoint3D &corner_min={-5.0f, -5.0f, -5.0f}, const mrpt::math::TPoint3D &corner_max={5.0f, 5.0f, 5.0f}, float resolution=0.25f)
Constructor.
static CLogOddsGridMapLUT< COccupancyGridMap3D::voxelType > logodd_lut
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
size_t ReadBuffer(void *Buffer, size_t Count)
Reads a block of bytes from the stream into Buffer.
Virtual base for specifying the kind and parameters of one map (normally, to be inserted into mrpt::m...
uint16_t decimation_3d_range
Specify the decimation of 3D range scans.
void writeToStream(mrpt::serialization::CArchive &out) const
Binary dump to stream.
void readFromStream(mrpt::serialization::CArchive &in)
Binary dump to stream.
mrpt::maps::TMapGenericParams genericMapParams
Common params for all maps: These are automatically set in TMetricMapTypesRegistry::factoryMapObjectF...
#define THROW_EXCEPTION(msg)
#define ASSERT_(f)
Defines an assertion mechanism.
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...
void saveMetricMapRepresentationToFile(const std::string &f) const override
This virtual method saves the map to a file "filNamePrefix"+< some_file_extension >,...
Virtual base class for "archives": classes abstracting I/O streams.
void reserveVoxels(const size_t set_index, const size_t nVoxels)
void loadFromConfigFile_map_specific(const mrpt::config::CConfigFileBase &source, const std::string §ionNamePrefix) override
Load all map-specific params.
A generic provider of log-odds grid-map maintainance functions.
bool LF_useSquareDist
[LikelihoodField] (Default:false) Use exp(dist^2/std^2) instead of exp(dist^2/std^2)
One static instance of this struct should exist in any class implementing CLogOddsGridMap2D to hold t...
void clear()
Clears everything.
void determineMatching2D(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose2D &otherMapPose, mrpt::tfest::TMatchingPairList &correspondences, const TMatchingParams ¶ms, TMatchingExtraResults &extraResults) const override
See docs in base class: in this class this always returns 0.
Declares a virtual base class for all metric maps storage classes.
void push_back_GridCube(const TGridCube &c)
OccGridCellTraits::cellType voxelType
The type of the map voxels:
#define ASSERT_BELOWEQ_(__A, __B)
float LF_zHit
[LikelihoodField]
float maxDistanceInsertion
Largest distance at which voxels are updated (Default: 15 meters)
int round(const T value)
Returns the closer integer (int) to x.
void setBoundingBox(const mrpt::math::TPoint3D &bb_min, const mrpt::math::TPoint3D &bb_max)
Manually changes the bounding box (normally the user doesn't need to call this)
This class allows loading and storing values and vectors of different types from a configuration text...
float getCellFreeness(unsigned int cx, unsigned int cy, unsigned int cz) const
Read the real valued [0,1] (0:occupied, 1:free) contents of a voxel, given its index.
void fill(const T &value)
Fills all the cells with the same value.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
static CLogOddsGridMapLUT< voxelType > & get_logodd_lut()
Lookup tables for log-odds.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
float maxFreenessUpdateCertainty
A value in the range [0.5,1] for updating a free voxel.
An RGBA color - floats in the range [0,1].
mrpt::maps::COccupancyGridMap3D::TLikelihoodOptions likelihoodOpts
Probabilistic observation likelihood options.
TLikelihoodOptions likelihoodOptions
void showVoxels(unsigned int voxel_set, bool show)
Shows/hides the voxels (voxel_set is a 0-based index for the set of voxels to modify,...
bool visibleOccupiedVoxels
Set occupied voxels visible (requires generateOccupiedVoxels=true) (Default=true)
float d2f(const double d)
shortcut for static_cast<float>(double)
TRenderingOptions renderingOptions
bool generateGridLines
Generate grid lines for all octree nodes, useful to draw the "structure" of the octree,...
float min_x
See COccupancyGridMap3D::COccupancyGridMap3D.
float LF_stdHit
[LikelihoodField] The laser range "sigma" used in computations; Default value = 0....
#define LOADABLEOPTS_DUMP_VAR(variableName, variableType)
Macro for dumping a variable to a stream, within the method "dumpToTextStream(out)" (Variable types a...
void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr &outObj) const override
Returns a 3D object representing the map.
bool generateOccupiedVoxels
Generate voxels for the occupied volumes (Default=true)
void dyngridcommon_readFromStream(ARCHIVE &in)
Serialization of all parameters, except the contents of each voxel (responsability of the derived cla...
mrpt::vision::TStereoCalibParams params
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
TLikelihoodMethod likelihoodMethod
The selected method to compute an observation likelihood.
void reserveGridCubes(const size_t nCubes)
Parameters for the determination of matchings between point clouds, etc.
int32_t rayTracing_decimation
[rayTracing] One out of "rayTracing_decimation" rays will be simulated and compared only: set to 1 to...
void setSize(const mrpt::math::TPoint3D &corner_min, const mrpt::math::TPoint3D &corner_max, double resolution, float default_value=0.5f)
Change the size of gridmap, erasing all its previous contents.
coord_t getResolutionXY() const
A flexible renderer of voxels, typically from a 3D octo map (see mrpt::maps::COctoMap).
void ReadAsAndCastTo(CAST_TO_TYPE &read_here)
Read a value from a stream stored in a type different of the target variable, making the conversion v...
#define ASSERT_ABOVEEQ_(__A, __B)
bool visibleFreeVoxels
Set free voxels visible (requires generateFreeVoxels=true) (Default=true)
TInsertionOptions insertionOptions
With this struct options are provided to the observation insertion process.
static Ptr Create(Args &&... args)
size_t ReadBufferFixEndianness(T *ptr, size_t ElementCount)
Reads a sequence of elemental datatypes, taking care of reordering their bytes from the MRPT stream s...
void push_back_Voxel(const size_t set_index, const TVoxel &v)
virtual void resize(coord_t new_x_min, coord_t new_x_max, coord_t new_y_min, coord_t new_y_max, coord_t new_z_min, coord_t new_z_max, const T &defaultValueNewCells, coord_t additionalMarginMeters=2)
Changes the size of the grid, maintaining previous contents.
mrpt::img::TColorf getColor() const
Returns the object color property as a TColorf.
A 3D occupancy grid map with a regular, even distribution of voxels.
uint16_t decimation
Decimation for insertPointCloud() or 2D range scans (Default: 1)
coord_t idx2y(int cy) const
OccGridCellTraits::cellType cell_t
The type of cells.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
coord_t idx2x(int cx) const
Transform a voxel index into a coordinate value of the voxel central point.
The namespace for 3D scene representation and rendering.
virtual void setSize(const coord_t x_min, const coord_t x_max, const coord_t y_min, const coord_t y_max, const coord_t z_min, const coord_t z_max, const coord_t resolution_xy, const coord_t resolution_z_=-1.0, const T *fill_value=nullptr)
Changes the size of the grid, ERASING all previous contents.
float maxOccupancyUpdateCertainty
A value in the range [0.5,1] used for updating voxel with a Bayesian approach (default 0....
float compute3DMatchingRatio(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose3D &otherMapPose, const TMatchingRatioParams ¶ms) const override
See docs in base class: in this class this always returns 0.
virtual void dumpToTextStream(std::ostream &out) const
This method should clearly display all the contents of the structure in textual form,...
void internal_clear() override
Clear the map: It set all voxels to their default occupancy value (0.5), without changing the resolut...
void dumpToTextStream_map_specific(std::ostream &out) const override
The info of each of the voxels.
uint32_t LF_decimation
[LikelihoodField] The decimation of the points in a scan, default=1 == no decimation
static mrpt::maps::CMetricMap * internal_CreateFromMapDefinition(const mrpt::maps::TMetricMapInitializer &def)
void dyngridcommon_writeToStream(ARCHIVE &out) const
Serialization of all parameters, except the contents of each voxel (responsability of the derived cla...
visualization_mode_t getVisualizationMode() const
Page generated by Doxygen 1.8.18 for MRPT 2.0.4 at Thu Sep 24 07:14:18 UTC 2020 | |