338 lines
14 KiB
C++
338 lines
14 KiB
C++
|
/* Copyright 2019-2020 The MathWorks, Inc. */
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @file
|
||
|
|
* External C-API interfaces for Octomap.
|
||
|
|
* To fully support code generation, note that this file needs to be fully
|
||
|
|
* compliant with the C89/C90 (ANSI) standard.
|
||
|
|
*/
|
||
|
|
|
||
|
|
#ifndef OCTOMAPCODEGEN_API_H_
|
||
|
|
#define OCTOMAPCODEGEN_API_H_
|
||
|
|
|
||
|
|
#ifdef BUILDING_LIBMWOCTOMAPCODEGEN
|
||
|
|
#include "octomapcodegen/octomapcodegen_util.hpp"
|
||
|
|
#else
|
||
|
|
/* To deal with the fact that PackNGo has no include file hierarchy during test */
|
||
|
|
#include "octomapcodegen_util.hpp"
|
||
|
|
#endif
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief Intialize algorithm with resolution
|
||
|
|
*
|
||
|
|
* @param resolution Resolution of the octree leaf nodes (in meters)
|
||
|
|
* @return Octomap internal object
|
||
|
|
*/
|
||
|
|
EXTERN_C OCTOMAP_CODEGEN_API void* octomapInitialize_real64(real64_T resolution);
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief Free memory and reset to initial state
|
||
|
|
* @param[in] map Octomap internal object
|
||
|
|
*/
|
||
|
|
EXTERN_C OCTOMAP_CODEGEN_API void octomapCleanup_real64(void* map);
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief Get resolution of octree
|
||
|
|
*
|
||
|
|
* @param[in] map Octomap internal object
|
||
|
|
* @return Resolution of octree leaf nodes (in meters)
|
||
|
|
*/
|
||
|
|
EXTERN_C OCTOMAP_CODEGEN_API real64_T octomapGetResolution_real64(void* map);
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief Get resolution of octree
|
||
|
|
*
|
||
|
|
* @param[in] map Octomap internal object
|
||
|
|
* @return Number of octree leaf nodes (in meters)
|
||
|
|
*/
|
||
|
|
EXTERN_C OCTOMAP_CODEGEN_API uint32_T octomapGetNumLeafNodes_real64(void* map);
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief Set resolution of octree
|
||
|
|
* @param[in] map Octomap internal object
|
||
|
|
* @param[in] resolution Resolution of octree leaf nodes (in meters)
|
||
|
|
*/
|
||
|
|
EXTERN_C OCTOMAP_CODEGEN_API void octomapSetResolution_real64(void* map, real64_T resolution);
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief Get the dimension of the octree based map (with the inserted observations so far)
|
||
|
|
* @param[in] map Octomap internal object
|
||
|
|
* @param[out] dimensions A 3x2 matrix with [xmin xmax; ymin ymax; zmin zmax] (in meters)
|
||
|
|
*/
|
||
|
|
EXTERN_C OCTOMAP_CODEGEN_API void octomapGetMapDimensions_real64(void* map, real64_T* dimensions);
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief Manipulate log_odds value of a voxel directly.
|
||
|
|
* @param[in] map Octomap internal object
|
||
|
|
* @param[in] xyz 3d coordinate of the voxel that is to be updated
|
||
|
|
* @param[in] occupied ProbHit/ProbMiss Value to be added (+) to existing log_odds value of node
|
||
|
|
* after converting to logodds
|
||
|
|
* @param[in] lazyEval Whether update of inner nodes is omitted after the update (default: false).
|
||
|
|
* This speeds up the insertion, but you need to call updateInnerOccupancy() when done.
|
||
|
|
*/
|
||
|
|
EXTERN_C OCTOMAP_CODEGEN_API void octomapUpdateNodeBoolean_real64(void* map, real64_T* xyz, boolean_T occupied, boolean_T lazyEval);
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief Manipulate log_odds value of a voxel directly.
|
||
|
|
* @param[in] map Octomap internal object
|
||
|
|
* @param xyz 3d coordinate of the voxel that is to be updated
|
||
|
|
* @param probUpdate Value to be added (+) to existing log_odds value of node after converting
|
||
|
|
* to logodds
|
||
|
|
* @param lazyEval Whether update of inner nodes is omitted after the update (default: false).
|
||
|
|
* This speeds up the insertion, but you need to call updateInnerOccupancy() when done.
|
||
|
|
*/
|
||
|
|
EXTERN_C OCTOMAP_CODEGEN_API void octomapUpdateNodeDouble_real64(void* map, real64_T* xyz, real64_T probUpdate, boolean_T lazyEval);
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief Directly set the log_odds value of a voxel
|
||
|
|
* @param[in] map Octomap internal object
|
||
|
|
* @param xyz 3d coordinate of the voxel that is to be set
|
||
|
|
* @param prob Probability value to be set for a node after converting to logodds
|
||
|
|
* @param lazyEval Whether update of inner nodes is omitted after the update (default: false).
|
||
|
|
* This speeds up the insertion, but you need to call updateInnerOccupancy() when done.
|
||
|
|
*/
|
||
|
|
EXTERN_C OCTOMAP_CODEGEN_API void octomapSetNodeValue_real64(void* map, real64_T* xyz, real64_T prob, boolean_T lazyEval);
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief Get occupancy value for one or more voxels
|
||
|
|
* @param[in] map Octomap internal object
|
||
|
|
* @param[in] xyz 3d coordinates of the voxels that are to be updated. This is an N-by-3 matrix.
|
||
|
|
* @param[in] nRows number of xyz coordinates.
|
||
|
|
* @param[out] occupancyValues Occupancy value of the voxels. This is a vector of length N.
|
||
|
|
*/
|
||
|
|
EXTERN_C OCTOMAP_CODEGEN_API void octomapGetOccupancy_real64(void* map, real64_T* xyz, uint32_T nRows, real64_T* occupancyValues);
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief Integrate a Pointcloud (in sensor coordinate frame)
|
||
|
|
* @param[in] map Octomap internal object
|
||
|
|
* @param[in] origin origin of sensor in global reference frame, the format being [x y z qw qx qy
|
||
|
|
* qz]
|
||
|
|
* @param[in] points Pointcloud (measurement endpoints), in sensor coordinate frame. Points datatype is "double"
|
||
|
|
* @param[in] nPoints number of points.
|
||
|
|
* @param[in] maxRange maximum range for how long individual beams are inserted (default -1:
|
||
|
|
* complete beam)
|
||
|
|
* @param[in] lazyEval whether update of inner nodes is omitted after the update (default: false).
|
||
|
|
* This speeds up the insertion, but you need to call updateInnerOccupancy() when done.
|
||
|
|
* @param[in] discretize whether the scan is discretized first into octree key cells (default:
|
||
|
|
* false). This reduces the number of raycasts using computeDiscreteUpdate(), resulting in a
|
||
|
|
* potential speedup.*
|
||
|
|
* @return true if pointCloud have inf otherwise false
|
||
|
|
*/
|
||
|
|
EXTERN_C OCTOMAP_CODEGEN_API boolean_T octomapInsertPointCloud_real64(void* map, real64_T* origin,
|
||
|
|
real64_T* points,
|
||
|
|
uint32_T nPoints,
|
||
|
|
real64_T maxRange,
|
||
|
|
boolean_T lazyEval,
|
||
|
|
boolean_T discretize);
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief Integrate a Pointcloud (in sensor coordinate frame)
|
||
|
|
* @param[in] map Octomap internal object
|
||
|
|
* @param[in] origin origin of sensor in global reference frame, the format being [x y z qw qx qy
|
||
|
|
* qz]
|
||
|
|
* @param[in] points Pointcloud (measurement endpoints), in sensor coordinate frame. Points datatype is "single"
|
||
|
|
* @param[in] nPoints number of points.
|
||
|
|
* @param[in] maxRange maximum range for how long individual beams are inserted (default -1:
|
||
|
|
* complete beam)
|
||
|
|
* @param[in] lazyEval whether update of inner nodes is omitted after the update (default: false).
|
||
|
|
* This speeds up the insertion, but you need to call updateInnerOccupancy() when done.
|
||
|
|
* @param[in] discretize whether the scan is discretized first into octree key cells (default:
|
||
|
|
* false). This reduces the number of raycasts using computeDiscreteUpdate(), resulting in a
|
||
|
|
* potential speedup.*
|
||
|
|
* @return true if pointCloud have inf otherwise false
|
||
|
|
*/
|
||
|
|
EXTERN_C OCTOMAP_CODEGEN_API boolean_T octomapInsertPointCloud_real32(void* map, real64_T* origin,
|
||
|
|
real32_T* points,
|
||
|
|
uint32_T nPoints,
|
||
|
|
real64_T maxRange,
|
||
|
|
boolean_T lazyEval,
|
||
|
|
boolean_T discretize);
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief Updates the occupancy of all inner nodes to reflect their children's occupancy.
|
||
|
|
* @param[in] map Octomap internal object
|
||
|
|
* If you performed batch-updates with lazy evaluation enabled, you must call this
|
||
|
|
* before any queries to ensure correct multi-resolution behavior.
|
||
|
|
*/
|
||
|
|
EXTERN_C OCTOMAP_CODEGEN_API void octomapUpdateInternalOccupancy_real64(void* map);
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief Lossless compression of the octree: A node will replace all of its eight
|
||
|
|
* children if they have identical values. You usually don't have to call
|
||
|
|
* prune() after a regular occupancy update, updateNode() incrementally
|
||
|
|
* prunes all affected nodes.
|
||
|
|
* @param[in] map Octomap internal object
|
||
|
|
*/
|
||
|
|
EXTERN_C OCTOMAP_CODEGEN_API void octomapPrune_real64(void* map);
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief Creates the maximum likelihood map by calling toMaxLikelihood on all
|
||
|
|
* tree nodes, setting their occupancy to the corresponding occupancy thresholds.
|
||
|
|
* This enables a very efficient compression if you call prune() afterwards.
|
||
|
|
*/
|
||
|
|
EXTERN_C OCTOMAP_CODEGEN_API void octomapToMaxLikelihood_real64(void* map);
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief Extract the values needed for visualizing the octree data representation as a
|
||
|
|
* occupancy map
|
||
|
|
* @param[in] map Octomap internal object
|
||
|
|
* @param[in] maxDepth The depth at which the visualization data needs to be extracted
|
||
|
|
* @param[out] outData a matrix of size Nx6, where N is the number of nodes and columns: x, y, z,
|
||
|
|
* scale, occupancy value, depth
|
||
|
|
*/
|
||
|
|
EXTERN_C OCTOMAP_CODEGEN_API void octomapExtractVisualizationData_real64(void* map, real64_T maxDepth, real64_T* outData);
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @return memory usage of the complete octree in bytes (may vary between architectures)
|
||
|
|
*/
|
||
|
|
EXTERN_C OCTOMAP_CODEGEN_API uint32_T octomapMmemoryUsage_real64(void* map);
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief Inflate the octree leaf nodes with a radius
|
||
|
|
* @param[in] map Octomap internal object
|
||
|
|
* @param[in] radius This is the inflation radius
|
||
|
|
* @param[in] occThreshold This is the occupancy threshold set by the user
|
||
|
|
*/
|
||
|
|
EXTERN_C OCTOMAP_CODEGEN_API void octomapInflate_real64(void* map, real64_T radius, real64_T occThreshold);
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief Set the minimum and maximum threshold for occupancy clamping
|
||
|
|
* @param[in] map Octomap internal object
|
||
|
|
* @param[in] clampingThresMin The minimum threshold at which the probability saturates
|
||
|
|
* @param[in] clampingThresMax The maximum threshold at which the probability saturates
|
||
|
|
* This should be called before inserting any observations.
|
||
|
|
*/
|
||
|
|
EXTERN_C OCTOMAP_CODEGEN_API void octomapSetClampingThreshold_real64(void* map, real64_T clampingThresMin, real64_T clampingThresMax);
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief Reads OcTree from a binary file.
|
||
|
|
* @param[in] map Octomap internal object
|
||
|
|
* Existing nodes of the tree are deleted before the tree is read.
|
||
|
|
* @param[in] filename path to binary .bt file
|
||
|
|
* @return true if filename path is exist otherwise false.
|
||
|
|
*/
|
||
|
|
EXTERN_C OCTOMAP_CODEGEN_API boolean_T octomapReadBinary_real64(void* map, const char* filename);
|
||
|
|
|
||
|
|
/**
|
||
|
|
*readBinaryImpl
|
||
|
|
*/
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief Writes compressed maximum likelihood OcTree to a binary stream.
|
||
|
|
* @param[in] map Octomap internal object
|
||
|
|
* The OcTree is first converted to the maximum likelihood estimate and pruned
|
||
|
|
* for maximum compression.
|
||
|
|
* @param[in] filename path to binary .ot file
|
||
|
|
*/
|
||
|
|
EXTERN_C OCTOMAP_CODEGEN_API void octomapWriteBinary_real64(void* map, const char* filename);
|
||
|
|
|
||
|
|
/**
|
||
|
|
*writeBinaryImpl
|
||
|
|
*/
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief Read the file header, create the appropriate class and deserialize.
|
||
|
|
* @param[in] map Octomap internal object
|
||
|
|
* This creates a new octree which you need to delete yourself.
|
||
|
|
* @param[in] filename path to binary .ot file
|
||
|
|
* @return true if filename path is valid otherwise false
|
||
|
|
*/
|
||
|
|
EXTERN_C OCTOMAP_CODEGEN_API boolean_T octomapRead_real64(void* map, const char* filename);
|
||
|
|
|
||
|
|
/**
|
||
|
|
*readImpl
|
||
|
|
*/
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief Writes data to a octree stream.
|
||
|
|
* @param[in] map Octomap internal object
|
||
|
|
* Write file header and complete tree to file (serialization)
|
||
|
|
* @param filename path to binary .ot file
|
||
|
|
*/
|
||
|
|
EXTERN_C OCTOMAP_CODEGEN_API void octomapWrite_real64(void* map, const char* filename);
|
||
|
|
|
||
|
|
/**
|
||
|
|
*writeImpl
|
||
|
|
*/
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief Get probability of a HIT (i.e., occupied)
|
||
|
|
* @param[in] map Octomap internal object
|
||
|
|
* @return Hit probability (a global setting on octree)
|
||
|
|
*/
|
||
|
|
EXTERN_C OCTOMAP_CODEGEN_API real64_T octomapGetProbHit_real64(void* map);
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief Get probability of a MISS (i.e., free)
|
||
|
|
* @param[in] map Octomap internal object
|
||
|
|
* @return Miss probability (a global setting on octree)
|
||
|
|
*/
|
||
|
|
EXTERN_C OCTOMAP_CODEGEN_API real64_T octomapGetProbMiss_real64(void* map);
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief data size of _map by converting octomap data to binary char array
|
||
|
|
* @param[in] map Octomap internal object
|
||
|
|
* @return Data size of _map by converting octomap data to binary char array
|
||
|
|
*/
|
||
|
|
EXTERN_C OCTOMAP_CODEGEN_API uint32_T octomapGetSizeSerializationData_real64(void* map);
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief serialize the data in _map by converting octomap data to binary char array
|
||
|
|
* @param[in] map Octomap internal object
|
||
|
|
* @param[out] data A binary char array
|
||
|
|
*/
|
||
|
|
EXTERN_C OCTOMAP_CODEGEN_API void octomapSerialization_real64(void* map, char* data);
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief deserialize the data as binary char array into a octree data
|
||
|
|
* @param[in] map Octomap internal object
|
||
|
|
* @param[in,out] data A binary char array created by serialization()
|
||
|
|
* @param[in] n size of data
|
||
|
|
* @return true if binary char array is right otherwise false
|
||
|
|
*/
|
||
|
|
EXTERN_C OCTOMAP_CODEGEN_API boolean_T octomapDeserialization_real64(void* map, char* data, uint32_T n);
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief Deserialize the full Octomap ROS msg data into a octree data
|
||
|
|
* @param[in] map Octomap internal object
|
||
|
|
* @param[in] resolution a double value in octomap_msgs/Octomap Resolution field
|
||
|
|
* @param[in] data a int8 array in octomap_msgs/Octomap Data field
|
||
|
|
* @param[in] n size of data
|
||
|
|
*/
|
||
|
|
EXTERN_C OCTOMAP_CODEGEN_API void octomapDeserializationFullROSMsgData_real64(void* map, real64_T resolution, char* data, uint32_T n);
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief deserialize the binary Octomap ROS msg data into a octree data
|
||
|
|
* @param[in] map Octomap internal object
|
||
|
|
* @param[in] resolution a double value in octomap_msgs/Octomap Resolution field
|
||
|
|
* @param[in] data a int8 array in octomap_msgs/Octomap Data field
|
||
|
|
* @param[in] n size of data
|
||
|
|
*/
|
||
|
|
EXTERN_C OCTOMAP_CODEGEN_API void octomapDeserializationBinaryROSMsgData_real64(void* map, real64_T resolution, char* data, uint32_T n);
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief Performs ray-casting in 3d and find intersecting point
|
||
|
|
* Rays are cast from 'origin' with given directions, the first non-free
|
||
|
|
* cell is returned. This could also be the origin node if it is occupied or unknown.
|
||
|
|
*
|
||
|
|
* @param[in] map Octomap internal object
|
||
|
|
* @param[in] ptStart starting coordinate of ray
|
||
|
|
* @param[in] ptDirections An Mx3 matrix. Each row is a vector pointing in the direction of the raycast (NOT a point in space). Does not need to be normalized.
|
||
|
|
* @param[in] nPtDirection number of point directions
|
||
|
|
* @param[in] occupiedThreshold Controls whether a cell is determined to be occupied during raycast
|
||
|
|
* @param[in] ignoreUnknownCells whether unknown cells are ignored (= treated as free). If false (default), the raycast aborts when an unknown cell is hit and returns false.
|
||
|
|
* @param[in] maxRange Maximum range after which the raycast is aborted (<= 0: no limit, default)
|
||
|
|
* @param[out] outData an Mx4 matrix, first column is the occupancy probability [0, 1) for known cells and 0.5 for unknown cells, the next 3 columns contain end points of the ray
|
||
|
|
*/
|
||
|
|
EXTERN_C OCTOMAP_CODEGEN_API void octomapGetRayIntersection_real64(
|
||
|
|
void* map,
|
||
|
|
real64_T* ptStart,
|
||
|
|
real64_T* ptDirections,
|
||
|
|
uint32_T nPtDirection,
|
||
|
|
real64_T occupiedThreshold,
|
||
|
|
boolean_T ignoreUnknownCells,
|
||
|
|
real64_T maxRange,
|
||
|
|
real64_T* outData);
|
||
|
|
|
||
|
|
#endif /* OCTOMAPCODEGEN_API_H_ */
|