From 3381c8b0109a847ac736e8235930aff09e1e36f8 Mon Sep 17 00:00:00 2001 From: Kevin Date: Sun, 26 Feb 2017 20:08:11 -0500 Subject: [PATCH] First pass at GP integration --- CMakeLists.txt | 33 +- config/methods/gpoctomap.yaml | 24 + include/{block.h => bgkblock.h} | 10 +- include/bgkoctomap.h | 8 +- include/{octree.h => bgkoctree.h} | 8 +- include/{octree_node.h => bgkoctree_node.h} | 6 +- include/gpblock.h | 110 ++++ include/gpoctomap.h | 376 ++++++++++++++ include/gpoctree.h | 161 ++++++ include/gpoctree_node.h | 97 ++++ include/gpregressor.h | 169 ++++++ launch/la3dm_static.launch | 1 - src/{block.cpp => bgkblock.cpp} | 2 +- src/bgkoctree.cpp | 156 ++++++ src/{octree_node.cpp => bgkoctree_node.cpp} | 2 +- src/gpblock.cpp | 159 ++++++ src/gpoctomap.cpp | 546 ++++++++++++++++++++ src/gpoctomap_static_node.cpp | 159 ++++++ src/{octree.cpp => gpoctree.cpp} | 3 +- src/gpoctree_node.cpp | 66 +++ 20 files changed, 2061 insertions(+), 35 deletions(-) create mode 100644 config/methods/gpoctomap.yaml rename include/{block.h => bgkblock.h} (96%) rename include/{octree.h => bgkoctree.h} (97%) rename include/{octree_node.h => bgkoctree_node.h} (96%) create mode 100644 include/gpblock.h create mode 100644 include/gpoctomap.h create mode 100644 include/gpoctree.h create mode 100644 include/gpoctree_node.h create mode 100644 include/gpregressor.h rename src/{block.cpp => bgkblock.cpp} (99%) create mode 100755 src/bgkoctree.cpp rename src/{octree_node.cpp => bgkoctree_node.cpp} (98%) create mode 100644 src/gpblock.cpp create mode 100644 src/gpoctomap.cpp create mode 100644 src/gpoctomap_static_node.cpp rename src/{octree.cpp => gpoctree.cpp} (99%) mode change 100755 => 100644 create mode 100644 src/gpoctree_node.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 41ea2e9..c81ea8f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -100,7 +100,6 @@ find_package(catkin REQUIRED COMPONENTS ## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( INCLUDE_DIRS include - LIBRARIES bgkoctomap # CATKIN_DEPENDS geometry_msgs pcl_ros roscpp # DEPENDS system_lib ) @@ -123,19 +122,25 @@ include_directories( # src/${PROJECT_NAME}/bgkoctomap.cpp # ) -## Declare a cpp executable -set(SOURCE_FILES src/block.cpp +# Set relevant BGK source files +set(BGK_SOURCE_FILES src/bgkblock.cpp src/bgkoctomap.cpp - # src/gpoctomap.cpp # Add back when gpoctomap integration is fixed - src/octree_node.cpp - src/octree.cpp - src/point3f.cpp) -add_executable(bgkoctomap_static_node ${SOURCE_FILES} + src/bgkoctree_node.cpp + src/bgkoctree.cpp + src/point3f.cpp +) + +set(GP_SOURCE_FILES src/gpblock.cpp + src/gpoctomap.cpp + src/gpoctree_node.cpp + src/gpoctree.cpp + src/point3f) + +add_executable(bgkoctomap_static_node ${BGK_SOURCE_FILES} src/bgkoctomap_static_node.cpp) -# TODO - fix GPOctoMap integration -# add_executable(gpoctomap_static_node ${SOURCE_FILES} - # src/gpoctomap_static_node.cpp) +add_executable(gpoctomap_static_node ${GP_SOURCE_FILES} + src/gpoctomap_static_node.cpp) add_executable(octomap_static_node src/octomap_static_node.cpp) @@ -156,9 +161,9 @@ target_link_libraries(bgkoctomap_static_node ${catkin_LIBRARIES} ) -# target_link_libraries(gpoctomap_static_node -# ${catkin_LIBRARIES} -# ) +target_link_libraries(gpoctomap_static_node + ${catkin_LIBRARIES} +) target_link_libraries(octomap_static_node ${catkin_LIBRARIES} diff --git a/config/methods/gpoctomap.yaml b/config/methods/gpoctomap.yaml new file mode 100644 index 0000000..91e3c18 --- /dev/null +++ b/config/methods/gpoctomap.yaml @@ -0,0 +1,24 @@ +# GPOctoMap config + +# Map topic, grid cell mininum resolution +topic: /occupied_cells_vis_array +resolution: 0.1 +block_depth: 3 # Test-data octree depth (see Wang & Englot ICRA 2016) + +# GP parameters +sf2: 1.0 +ell: 1.0 +noise: 0.01 +l: 100 +max_var: 1000 +min_var: 0.001 +max_known_var: 0.02 + +# Sampling resolutions +free_resolution: 0.3 # Free space sampling resolution +ds_resolution: 0.1 # Downsampling factor + +# Free/Occupied Thresholds +free_thresh: free_thresh +occupied_thresh: 0.7 +var_thresh: 1000.0 # Threshold on variance to distinguish known/unknown \ No newline at end of file diff --git a/include/block.h b/include/bgkblock.h similarity index 96% rename from include/block.h rename to include/bgkblock.h index eeceef7..7478169 100755 --- a/include/block.h +++ b/include/bgkblock.h @@ -1,11 +1,11 @@ -#ifndef LA3DM_BLOCK_H -#define LA3DM_BLOCK_H +#ifndef LA3DM_BGK_BLOCK_H +#define LA3DM_BGK_BLOCK_H #include #include #include "point3f.h" -#include "octree_node.h" -#include "octree.h" +#include "bgkoctree_node.h" +#include "bgkoctree.h" namespace la3dm { @@ -106,4 +106,4 @@ namespace la3dm { }; } -#endif // LA3DM_BLOCK_H +#endif // LA3DM_BGK_BLOCK_H diff --git a/include/bgkoctomap.h b/include/bgkoctomap.h index c636d97..70154b5 100755 --- a/include/bgkoctomap.h +++ b/include/bgkoctomap.h @@ -1,13 +1,13 @@ -#ifndef LA3DM_BGKOCTOMAP_H -#define LA3DM_BGKOCTOMAP_H +#ifndef LA3DM_BGK_OCTOMAP_H +#define LA3DM_BGK_OCTOMAP_H #include #include #include #include #include "rtree.h" -#include "block.h" -#include "octree_node.h" +#include "bgkblock.h" +#include "bgkoctree_node.h" namespace la3dm { diff --git a/include/octree.h b/include/bgkoctree.h similarity index 97% rename from include/octree.h rename to include/bgkoctree.h index 8553e46..042fa36 100755 --- a/include/octree.h +++ b/include/bgkoctree.h @@ -1,10 +1,10 @@ -#ifndef LA3DM_OCTREE_H -#define LA3DM_OCTREE_H +#ifndef LA3DM_BGK_OCTREE_H +#define LA3DM_BGK_OCTREE_H #include #include #include "point3f.h" -#include "octree_node.h" +#include "bgkoctree_node.h" namespace la3dm { @@ -158,4 +158,4 @@ namespace la3dm { }; } -#endif // LA3DM_OCTREE_H +#endif // LA3DM_BGK_OCTREE_H diff --git a/include/octree_node.h b/include/bgkoctree_node.h similarity index 96% rename from include/octree_node.h rename to include/bgkoctree_node.h index c5841aa..f583058 100755 --- a/include/octree_node.h +++ b/include/bgkoctree_node.h @@ -1,5 +1,5 @@ -#ifndef LA3DM_OCCUPANCY_H -#define LA3DM_OCCUPANCY_H +#ifndef LA3DM_BGK_OCCUPANCY_H +#define LA3DM_BGK_OCCUPANCY_H #include #include @@ -94,4 +94,4 @@ namespace la3dm { typedef Occupancy OcTreeNode; } -#endif // LA3DM_OCCUPANCY_H +#endif // LA3DM_BGK_OCCUPANCY_H diff --git a/include/gpblock.h b/include/gpblock.h new file mode 100644 index 0000000..00fc9fb --- /dev/null +++ b/include/gpblock.h @@ -0,0 +1,110 @@ +#ifndef LA3DM_GP_BLOCK_H +#define LA3DM_GP_BLOCK_H + +#include +#include +#include "point3f.h" +#include "gpoctree_node.h" +#include "gpoctree.h" + +namespace la3dm { + + /// Hask key to index Block given block's center. + typedef int64_t BlockHashKey; + + /// Initialize Look-Up Table + std::unordered_map init_key_loc_map(float resolution, unsigned short max_depth); + + std::unordered_map init_index_map(const std::unordered_map &key_loc_map, + unsigned short max_depth); + + /// Extended Block +#ifdef PREDICT + typedef std::array ExtendedBlock; +#else + typedef std::array ExtendedBlock; +#endif + + /// Convert from block to hash key. + BlockHashKey block_to_hash_key(point3f center); + + /// Convert from block to hash key. + BlockHashKey block_to_hash_key(float x, float y, float z); + + /// Convert from hash key to block. + point3f hash_key_to_block(BlockHashKey key); + + /// Get current block's extended block. + ExtendedBlock get_extended_block(BlockHashKey key); + + /* + * @brief Block is built on top of OcTree, providing the functions to locate nodes. + * + * Block stores the information needed to locate each OcTreeNode's position: + * fixed resolution, fixed block_size, both of which must be initialized. + * The localization is implemented using Loop-Up Table. + */ + class Block : public OcTree { + friend BlockHashKey block_to_hash_key(point3f center); + + friend BlockHashKey block_to_hash_key(float x, float y, float z); + + friend point3f hash_key_to_block(BlockHashKey key); + + friend ExtendedBlock get_extended_block(BlockHashKey key); + + friend class GPOctoMap; + + public: + Block(); + + Block(point3f center); + + /// @return location of the OcTreeNode given OcTree's LeafIterator. + inline point3f get_loc(const LeafIterator &it) const { + return Block::key_loc_map[it.get_hash_key()] + center; + } + + /// @return size of the OcTreeNode given OcTree's LeafIterator. + inline float get_size(const LeafIterator &it) const { + unsigned short depth, index; + hash_key_to_node(it.get_hash_key(), depth, index); + return float(size / pow(2, depth)); + } + + /// @return center of current Block. + inline point3f get_center() const { return center; } + + /// @return min lim of current Block. + inline point3f get_lim_min() const { return center - point3f(size / 2.0f, size / 2.0f, size / 2.0f); } + + /// @return max lim of current Block. + inline point3f get_lim_max() const { return center + point3f(size / 2.0f, size / 2.0f, size / 2.0f); } + + /// @return ExtendedBlock of current Block. + ExtendedBlock get_extended_block() const; + + OcTreeHashKey get_node(unsigned short x, unsigned short y, unsigned short z) const; + + point3f get_point(unsigned short x, unsigned short y, unsigned short z) const; + + void get_index(const point3f &p, unsigned short &x, unsigned short &y, unsigned short &z) const; + + OcTreeNode &search(float x, float y, float z) const; + + OcTreeNode &search(point3f p) const; + + private: + // Loop-Up Table + static std::unordered_map key_loc_map; + static std::unordered_map index_map; + static float resolution; + static float size; + static unsigned short cell_num; + + point3f center; + }; + +} + +#endif // LA3DM_GP_BLOCK_H diff --git a/include/gpoctomap.h b/include/gpoctomap.h new file mode 100644 index 0000000..386161c --- /dev/null +++ b/include/gpoctomap.h @@ -0,0 +1,376 @@ +#ifndef LA3DM_GP_OCTOMAP_H +#define LA3DM_GP_OCTOMAP_H + +#include +#include +#include +#include +#include "rtree.h" +#include "gpblock.h" +#include "gpoctree_node.h" + +namespace la3dm { + /// PCL PointCloud types as input + typedef pcl::PointXYZ PCLPointType; + typedef pcl::PointCloud PCLPointCloud; + + /* + * @brief GPOctoMap + * + * Fast, Accurate Gaussian Process Occupancy Maps via Test-Data Octrees + * and Nested Bayesian Fusion. The space is partitioned by Blocks + * in which OcTrees with fixed depth are rooted. The training part of GP + * is performed Block by Block with training data inside each Block. + * Occupancy values in one Block is predicted by its ExtendedBlock via BCM. + */ + class GPOctoMap { + public: + /// Types used internally + typedef std::vector PointCloud; + typedef std::pair GPPointType; + typedef std::vector GPPointCloud; + typedef RTree MyRTree; + + public: + GPOctoMap(); + + /* + * @param resolution (default 0.1m) + * @param block_depth maximum depth of OcTree (default 4) + * @param sf2 signal variance in GPs (default 1.0) + * @param ell length-scale in GPs (default 1.0) + * @param noise noise variance in GPs (default 0.01) + * @param l length-scale in logistic regression function (default 100) + * @param min_var minimum variance in Occupancy (default 0.001) + * @param max_var maximum variance in Occupancy (default 1000) + * @param max_known_var maximum variance for Occuapncy to be classified as KNOWN State (default 0.02) + * @param free_thresh free threshold for Occupancy probability (default 0.3) + * @param occupied_thresh occupied threshold for Occupancy probability (default 0.7) + */ + GPOctoMap(float resolution, unsigned short block_depth, float sf2, float ell, float noise, float l, + float min_var, + float max_var, float max_known_var, float free_thresh, float occupied_thresh); + + ~GPOctoMap(); + + /// Set resolution. + void set_resolution(float resolution); + + /// Set block max depth. + void set_block_depth(unsigned short max_depth); + + /// Get resolution. + inline float get_resolution() const { return resolution; } + + /// Get block max depth. + inline float get_block_depth() const { return block_depth; } + + /* + * @brief Insert PCL PointCloud into GPOctoMaps. + * @param cloud one scan in PCLPointCloud format + * @param origin sensor origin in the scan + * @param ds_resolution downsampling resolution for PCL VoxelGrid filtering (-1 if no downsampling) + * @param free_res resolution for sampling free training points along sensor beams (default 2.0) + * @param max_range maximum range for beams to be considered as valid measurements (-1 if no limitation) + */ + void insert_pointcloud(const PCLPointCloud &cloud, const point3f &origin, float ds_resolution, + float free_res = 2.0f, + float max_range = -1); + + void insert_training_data(const GPPointCloud &cloud); + + /// Get bounding box of the map. + void get_bbox(point3f &lim_min, point3f &lim_max) const; + + class RayCaster { + public: + RayCaster(const GPOctoMap *map, const point3f &start, const point3f &end) : map(map) { + assert(map != nullptr); + + _block_key = block_to_hash_key(start); + block = map->search(_block_key); + lim = static_cast(pow(2, map->block_depth - 1)); + + if (block != nullptr) { + block->get_index(start, x, y, z); + block_lim = block->get_center(); + block_size = block->size; + current_p = start; + resolution = map->resolution; + + int x0 = static_cast((start.x() / resolution)); + int y0 = static_cast((start.y() / resolution)); + int z0 = static_cast((start.z() / resolution)); + int x1 = static_cast((end.x() / resolution)); + int y1 = static_cast((end.y() / resolution)); + int z1 = static_cast((end.z() / resolution)); + dx = abs(x1 - x0); + dy = abs(y1 - y0); + dz = abs(z1 - z0); + n = 1 + dx + dy + dz; + x_inc = x1 > x0 ? 1 : (x1 == x0 ? 0 : -1); + y_inc = y1 > y0 ? 1 : (y1 == y0 ? 0 : -1); + z_inc = z1 > z0 ? 1 : (z1 == z0 ? 0 : -1); + xy_error = dx - dy; + xz_error = dx - dz; + yz_error = dy - dz; + dx *= 2; + dy *= 2; + dz *= 2; + } else { + n = 0; + } + } + + inline bool end() const { return n <= 0; } + + bool next(point3f &p, OcTreeNode &node, BlockHashKey &block_key, OcTreeHashKey &node_key) { + assert(!end()); + bool valid = false; + unsigned short index = x + y * lim + z * lim * lim; + node_key = Block::index_map[index]; + block_key = _block_key; + if (block != nullptr) { + valid = true; + node = (*block)[node_key]; + current_p = block->get_point(x, y, z); + p = current_p; + } else { + p = current_p; + } + + if (xy_error > 0 && xz_error > 0) { + x += x_inc; + current_p.x() += x_inc * resolution; + xy_error -= dy; + xz_error -= dz; + if (x >= lim || x < 0) { + block_lim.x() += x_inc * block_size; + _block_key = block_to_hash_key(block_lim); + block = map->search(_block_key); + x = x_inc > 0 ? 0 : lim - 1; + } + } else if (xy_error < 0 && yz_error > 0) { + y += y_inc; + current_p.y() += y_inc * resolution; + xy_error += dx; + yz_error -= dz; + if (y >= lim || y < 0) { + block_lim.y() += y_inc * block_size; + _block_key = block_to_hash_key(block_lim); + block = map->search(_block_key); + y = y_inc > 0 ? 0 : lim - 1; + } + } else if (yz_error < 0 && xz_error < 0) { + z += z_inc; + current_p.z() += z_inc * resolution; + xz_error += dx; + yz_error += dy; + if (z >= lim || z < 0) { + block_lim.z() += z_inc * block_size; + _block_key = block_to_hash_key(block_lim); + block = map->search(_block_key); + z = z_inc > 0 ? 0 : lim - 1; + } + } else if (xy_error == 0) { + x += x_inc; + y += y_inc; + n -= 2; + current_p.x() += x_inc * resolution; + current_p.y() += y_inc * resolution; + if (x >= lim || x < 0) { + block_lim.x() += x_inc * block_size; + _block_key = block_to_hash_key(block_lim); + block = map->search(_block_key); + x = x_inc > 0 ? 0 : lim - 1; + } + if (y >= lim || y < 0) { + block_lim.y() += y_inc * block_size; + _block_key = block_to_hash_key(block_lim); + block = map->search(_block_key); + y = y_inc > 0 ? 0 : lim - 1; + } + } + n--; + return valid; + } + + private: + const GPOctoMap *map; + Block *block; + point3f block_lim; + float block_size, resolution; + int dx, dy, dz, error, n; + int x_inc, y_inc, z_inc, xy_error, xz_error, yz_error; + unsigned short index, x, y, z, lim; + BlockHashKey _block_key; + point3f current_p; + }; + + /// LeafIterator for iterating all leaf nodes in blocks + class LeafIterator : public std::iterator { + public: + LeafIterator(const GPOctoMap *map) { + assert(map != nullptr); + + block_it = map->block_arr.cbegin(); + end_block = map->block_arr.cend(); + + if (map->block_arr.size() > 0) { + leaf_it = block_it->second->begin_leaf(); + end_leaf = block_it->second->end_leaf(); + } else { + leaf_it = OcTree::LeafIterator(); + end_leaf = OcTree::LeafIterator(); + } + } + + // just for initializing end iterator + LeafIterator(std::unordered_map::const_iterator block_it, + OcTree::LeafIterator leaf_it) + : block_it(block_it), leaf_it(leaf_it), end_block(block_it), end_leaf(leaf_it) { } + + bool operator==(const LeafIterator &other) { + return (block_it == other.block_it) && (leaf_it == other.leaf_it); + } + + bool operator!=(const LeafIterator &other) { + return !(this->operator==(other)); + } + + LeafIterator operator++(int) { + LeafIterator result(*this); + ++(*this); + return result; + } + + LeafIterator &operator++() { + ++leaf_it; + if (leaf_it == end_leaf) { + ++block_it; + if (block_it != end_block) { + leaf_it = block_it->second->begin_leaf(); + end_leaf = block_it->second->end_leaf(); + } + } + } + + OcTreeNode &operator*() const { + return *leaf_it; + } + + std::vector get_pruned_locs() const { + std::vector pruned_locs; + point3f center = get_loc(); + float size = get_size(); + float x0 = center.x() - size * 0.5 + Block::resolution * 0.5; + float y0 = center.y() - size * 0.5 + Block::resolution * 0.5; + float z0 = center.z() - size * 0.5 + Block::resolution * 0.5; + float x1 = center.x() + size * 0.5; + float y1 = center.y() + size * 0.5; + float z1 = center.z() + size * 0.5; + for (float x = x0; x < x1; x += Block::resolution) { + for (float y = y0; y < y1; y += Block::resolution) { + for (float z = z0; z < z1; z += Block::resolution) { + pruned_locs.emplace_back(x, y, z); + } + } + } + return pruned_locs; + } + + inline OcTreeNode &get_node() const { + return operator*(); + } + + inline point3f get_loc() const { + return block_it->second->get_loc(leaf_it); + } + + inline float get_size() const { + return block_it->second->get_size(leaf_it); + } + + private: + std::unordered_map::const_iterator block_it; + std::unordered_map::const_iterator end_block; + + OcTree::LeafIterator leaf_it; + OcTree::LeafIterator end_leaf; + }; + + /// @return the beginning of leaf iterator + inline LeafIterator begin_leaf() const { return LeafIterator(this); } + + /// @return the end of leaf iterator + inline LeafIterator end_leaf() const { return LeafIterator(block_arr.cend(), OcTree::LeafIterator()); } + + OcTreeNode search(point3f p) const; + + OcTreeNode search(float x, float y, float z) const; + + Block *search(BlockHashKey key) const; + + inline float get_block_size() const { return block_size; } + + private: + /// @return true if point is inside a bounding box given min and max limits. + inline bool gp_point_in_bbox(const GPPointType &p, const point3f &lim_min, const point3f &lim_max) const { + return (p.first.x() > lim_min.x() && p.first.x() < lim_max.x() && + p.first.y() > lim_min.y() && p.first.y() < lim_max.y() && + p.first.z() > lim_min.z() && p.first.z() < lim_max.z()); + } + + /// Get the bounding box of a pointcloud. + void bbox(const GPPointCloud &cloud, point3f &lim_min, point3f &lim_max) const; + + /// Get all block indices inside a bounding box. + void get_blocks_in_bbox(const point3f &lim_min, const point3f &lim_max, + std::vector &blocks) const; + + /// Get all points inside a bounding box assuming pointcloud has been inserted in rtree before. + int get_gp_points_in_bbox(const point3f &lim_min, const point3f &lim_max, + GPPointCloud &out); + + /// @return true if point exists inside a bounding box assuming pointcloud has been inserted in rtree before. + int has_gp_points_in_bbox(const point3f &lim_min, const point3f &lim_max); + + /// Get all points inside a bounding box (block) assuming pointcloud has been inserted in rtree before. + int get_gp_points_in_bbox(const BlockHashKey &key, GPPointCloud &out); + + /// @return true if point exists inside a bounding box (block) assuming pointcloud has been inserted in rtree before. + int has_gp_points_in_bbox(const BlockHashKey &key); + + /// Get all points inside an extended block assuming pointcloud has been inserted in rtree before. + int get_gp_points_in_bbox(const ExtendedBlock &block, GPPointCloud &out); + + /// @return true if point exists inside an extended block assuming pointcloud has been inserted in rtree before. + int has_gp_points_in_bbox(const ExtendedBlock &block); + + /// RTree callback function + static bool count_callback(GPPointType *p, void *arg); + + /// RTree callback function + static bool search_callback(GPPointType *p, void *arg); + + /// Downsample PCLPointCloud using PCL VoxelGrid Filtering. + void downsample(const PCLPointCloud &in, PCLPointCloud &out, float ds_resolution) const; + + /// Sample free training points along sensor beams. + void beam_sample(const point3f &hits, const point3f &origin, PointCloud &frees, + float free_resolution) const; + + /// Get training data from one sensor scan. + void get_training_data(const PCLPointCloud &cloud, const point3f &origin, float ds_resolution, + float free_resolution, float max_range, GPPointCloud &xy) const; + + float resolution; + float block_size; + unsigned short block_depth; + std::unordered_map block_arr; + MyRTree rtree; + }; + +} + +#endif // LA3DM_GP_OCTOMAP_H diff --git a/include/gpoctree.h b/include/gpoctree.h new file mode 100644 index 0000000..eb6c9b9 --- /dev/null +++ b/include/gpoctree.h @@ -0,0 +1,161 @@ +#ifndef LA3DM_GP_OCTREE_H +#define LA3DM_GP_OCTREE_H + +#include +#include +#include "point3f.h" +#include "gpoctree_node.h" + +namespace la3dm { + + /// Hash key to index OcTree nodes given depth and the index in that layer. + typedef int OcTreeHashKey; + + /// Convert from node to hask key. + OcTreeHashKey node_to_hash_key(unsigned short depth, unsigned short index); + + /// Convert from hash key to node. + void hash_key_to_node(OcTreeHashKey key, unsigned short &depth, unsigned short &index); + + /* + * @brief A simple OcTree to organize GP occupancy data in one block. + * + * OcTree doesn't store positions of nodes in order to reduce memory usage. + * The nodes in OcTrees are indexed by OcTreeHashKey which can be used to + * retrieve positions later (See Block). + * For the purpose of mapping, this OcTree has fixed depth which should be + * set before using OcTrees. + */ + class OcTree { + friend class GPOctoMap; + + public: + OcTree(); + + ~OcTree(); + + OcTree(const OcTree &other); + + OcTree &operator=(const OcTree &other); + + /* + * @brief Rursively pruning OcTreeNodes with the same state. + * + * Prune nodes by setting nodes to PRUNED. + * Delete the layer if all nodes are pruned. + */ + bool prune(); + + /// @return true if this node is a leaf node. + bool is_leaf(OcTreeHashKey key) const; + + /// @return true if this node is a leaf node. + bool is_leaf(unsigned short depth, unsigned short index) const; + + /// @return true if this node exists and is not pruned. + bool search(OcTreeHashKey key) const; + + /// @return Occupancy of the node (without checking if it exists!) + OcTreeNode &operator[](OcTreeHashKey key) const; + + /// Leaf iterator for OcTrees: iterate all leaf nodes not pruned. + class LeafIterator : public std::iterator { + public: + LeafIterator() : tree(nullptr) { } + + LeafIterator(const OcTree *tree) + : tree(tree != nullptr && tree->node_arr != nullptr ? tree : nullptr) { + if (tree != nullptr) { + stack.emplace(0, 0); + stack.emplace(0, 0); + ++(*this); + } + } + + LeafIterator(const LeafIterator &other) : tree(other.tree), stack(other.stack) { } + + LeafIterator &operator=(const LeafIterator &other) { + tree = other.tree; + stack = other.stack; + return *this; + } + + bool operator==(const LeafIterator &other) const { + return (tree == other.tree) && + (stack.size() == other.stack.size()) && + (stack.size() == 0 || (stack.size() > 0 && + (stack.top().depth == other.stack.top().depth) && + (stack.top().index == other.stack.top().index))); + } + + bool operator!=(const LeafIterator &other) const { + return !(this->operator==(other)); + } + + LeafIterator operator++(int) { + LeafIterator result(*this); + ++(*this); + return result; + } + + LeafIterator &operator++() { + if (stack.empty()) { + tree = nullptr; + } else { + stack.pop(); + while (!stack.empty() && !tree->is_leaf(stack.top().depth, stack.top().index)) + single_inc(); + if (stack.empty()) + tree = nullptr; + } + return *this; + } + + inline OcTreeNode &operator*() const { + return (*tree)[get_hash_key()]; + } + + inline OcTreeNode &get_node() const { + return operator*(); + } + + inline OcTreeHashKey get_hash_key() const { + OcTreeHashKey key = node_to_hash_key(stack.top().depth, stack.top().index); + return key; + } + + protected: + void single_inc() { + StackElement top(stack.top()); + stack.pop(); + + for (int i = 0; i < 8; ++i) { + stack.emplace(top.depth + 1, top.index * 8 + i); + } + } + + struct StackElement { + unsigned short depth; + unsigned short index; + + StackElement(unsigned short depth, unsigned short index) + : depth(depth), index(index) { } + }; + + const OcTree *tree; + std::stack > stack; + }; + + /// @return the beginning of leaf iterator + inline LeafIterator begin_leaf() const { return LeafIterator(this); }; + + /// @return the end of leaf iterator + inline LeafIterator end_leaf() const { return LeafIterator(nullptr); }; + + private: + OcTreeNode **node_arr; + static unsigned short max_depth; + }; +} + +#endif // LA3DM_GP_OCTREE_H diff --git a/include/gpoctree_node.h b/include/gpoctree_node.h new file mode 100644 index 0000000..cf7317d --- /dev/null +++ b/include/gpoctree_node.h @@ -0,0 +1,97 @@ +#ifndef LA3DM_GP_OCCUPANCY_H +#define LA3DM_GP_OCCUPANCY_H + +#include +#include + +namespace la3dm { + + /// Occupancy state: before pruning: FREE, OCCUPIED, UNKNOWN; after pruning: PRUNED + enum class State : char { + FREE, OCCUPIED, UNKNOWN, PRUNED + }; + + /* + * @brief GP regression ouputs and occupancy state. + * + * Occupancy has member variables: m_ivar (m*ivar), ivar (1/var) and State. + * This representation speeds up the updates via BCM. + * Before using this class, set the static member variables first. + */ + class Occupancy { + friend std::ostream &operator<<(std::ostream &os, const Occupancy &oc); + + friend std::ofstream &operator<<(std::ofstream &os, const Occupancy &oc); + + friend std::ifstream &operator>>(std::ifstream &is, Occupancy &oc); + + friend class GPOctoMap; + + public: + /* + * @brief Constructors and destructor. + */ + Occupancy() : m_ivar(0.0), ivar(Occupancy::min_ivar), state(State::UNKNOWN) { } + + Occupancy(float m, float var); + + Occupancy(const Occupancy &other) : m_ivar(other.m_ivar), ivar(other.ivar), state(other.state) { } + + Occupancy &operator=(const Occupancy &other) { + m_ivar = other.m_ivar; + ivar = other.ivar; + state = other.state; + return *this; + } + + ~Occupancy() { } + + /* + * @brief Bayesian Committee Machine (BCM) update for Gaussian Process regression. + * @param new_m mean resulted from GP regression + * @param new_var variance resulted from GP regression + */ + void update(float new_m, float new_var); + + /// Get probability of occupancy. + float get_prob() const; + + /// Get variance of occupancy (uncertainty) + inline float get_var() const { return 1.0f / ivar; } + + /* + * @brief Get occupancy state of the node. + * @return occupancy state (see State). + */ + inline State get_state() const { return state; } + + /// Prune current node; set state to PRUNED. + inline void prune() { state = State::PRUNED; } + + /// Only FREE and OCCUPIED nodes can be equal. + inline bool operator==(const Occupancy &rhs) const { + return this->state != State::UNKNOWN && this->state == rhs.state; + } + + private: + float m_ivar; // m / var or m * ivar + float ivar; // 1.0 / var + State state; + + static float sf2; // signal variance + static float ell; // length-scale + static float noise; // noise variance + static float l; // gamma in logistic functions + + static float max_ivar; // minimum variance + static float min_ivar; // maximum variance + static float min_known_ivar; // maximum variance for nodes to be considered as FREE or OCCUPIED + + static float free_thresh; // FREE occupancy threshold + static float occupied_thresh; // OCCUPIED occupancy threshold + }; + + typedef Occupancy OcTreeNode; +} + +#endif // LA3DM_GP_OCCUPANCY_H diff --git a/include/gpregressor.h b/include/gpregressor.h new file mode 100644 index 0000000..3638ba9 --- /dev/null +++ b/include/gpregressor.h @@ -0,0 +1,169 @@ +#ifndef LA3DM_GP_REGRESSOR_H +#define LA3DM_GP_REGRESSOR_H + +#include +#include + +namespace la3dm { + + /* + * @brief A Simple Gaussian Process Regressor + * @param dim dimension of data (2, 3, etc.) + * @param T data type (float, double, etc.) + */ + template + class GPRegressor { + public: + /// Eigen matrix type for training and test data and kernel + using MatrixXType = Eigen::Matrix; + using MatrixKType = Eigen::Matrix; + using MatrixDKType = Eigen::Matrix; + using MatrixYType = Eigen::Matrix; + + GPRegressor(T sf2, T ell, T noise) : sf2(sf2), ell(ell), noise(noise), trained(false) { } + + /* + * @brief Train Gaussian Process + * @param x input vector (3N, row major) + * @param y target vector (N) + */ + void train(const std::vector &x, const std::vector &y) { + assert(x.size() % dim == 0 && (int) (x.size() / dim) == y.size()); + MatrixXType _x = Eigen::Map(x.data(), x.size() / dim, dim); + MatrixYType _y = Eigen::Map(y.data(), y.size(), 1); + train(_x, _y); + } + + /* + * @brief Train Gaussian Process + * @param x input matrix (NX3) + * @param y target matrix (NX1) + */ + void train(const MatrixXType &x, const MatrixYType &y) { + this->x = MatrixXType(x); + covMaterniso3(x, x, K); + // covSparse(x, x, K); + K = K + noise * MatrixKType::Identity(K.rows(), K.cols()); + Eigen::LLT llt(K); + alpha = llt.solve(y); + L = llt.matrixL(); + trained = true; + } + + /* + * @brief Predict with Gaussian Process + * @param xs input vector (3M, row major) + * @param m predicted mean vector (M) + * @param var predicted variance vector (M) + */ + void predict(const std::vector &xs, std::vector &m, std::vector &var) const { + assert(xs.size() % dim == 0); + MatrixXType _xs = Eigen::Map(xs.data(), xs.size() / dim, dim); + + MatrixYType _m, _var; + predict(_xs, _m, _var); + + m.resize(_m.rows()); + var.resize(_var.rows()); + for (int r = 0; r < _m.rows(); ++r) { + m[r] = _m(r, 0); + var[r] = _var(r, 0); + } + } + + /* + * @brief Predict with Gaussian Process + * @param xs input vector (MX3) + * @param m predicted mean matrix (MX1) + * @param var predicted variance matrix (MX1) + */ + void predict(const MatrixXType &xs, MatrixYType &m, MatrixYType &var) const { + assert(trained == true); + MatrixKType Ks; + covMaterniso3(x, xs, Ks); + // covSparse(x, xs, Ks); + m = Ks.transpose() * alpha; + + MatrixKType v = L.template triangularView().solve(Ks); + MatrixDKType Kss; + covMaterniso3(xs, xs, Kss); + // covSparse(xs, xs, Kss); + var = Kss - (v.transpose() * v).diagonal(); + } + + private: + /* + * @brief Compute Euclid distances between two vectors. + * @param x input vector + * @param z input vecotr + * @return d distance matrix + */ + void dist(const MatrixXType &x, const MatrixXType &z, MatrixKType &d) const { + d = MatrixKType::Zero(x.rows(), z.rows()); + for (int i = 0; i < x.rows(); ++i) { + d.row(i) = (z.rowwise() - x.row(i)).rowwise().norm(); + } + } + + /* + * @brief Matern3 kernel. + * @param x input vector + * @param z input vector + * @return Kxz covariance matrix + */ + void covMaterniso3(const MatrixXType &x, const MatrixXType &z, MatrixKType &Kxz) const { + dist(1.73205 / ell * x, 1.73205 / ell * z, Kxz); + Kxz = ((1 + Kxz.array()) * exp(-Kxz.array())).matrix() * sf2; + } + + /* + * @brief Diagonal of Matern3 kernel. + * @return Kxz sf2 * I + */ + void covMaterniso3(const MatrixXType &x, const MatrixXType &z, MatrixDKType &Kxz) const { + Kxz = MatrixDKType::Ones(x.rows()) * sf2; + } + + /* + * @brief Sparse kernel. + * @param x input vector + * @param z input vector + * @return Kxz covariance matrix + * @ref A sparse covariance function for exact gaussian process inference in large datasets. + */ + void covSparse(const MatrixXType &x, const MatrixXType &z, MatrixKType &Kxz) const { + dist(x / ell, z / ell, Kxz); + Kxz = ((2 + (Kxz * 2 * 3.1415926).array().cos()) * (1.0 - Kxz.array()) / 3.0 + + (Kxz * 2 * 3.1415926).array().sin() / (2 * 3.1415926)).matrix() * sf2; + for (int i = 0; i < Kxz.rows(); ++i) { + for (int j = 0; j < Kxz.cols(); ++j) { + if (Kxz(i,j) < 0.0) { + Kxz(i,j) = 0.0f; + } + } + } + } + + /* + * @brief Diagonal of sparse kernel. + * @return Kxz sf2 * I + */ + void covSparse(const MatrixXType &x, const MatrixXType &z, MatrixDKType &Kxz) const { + Kxz = MatrixDKType::Ones(x.rows()) * sf2; + } + + T sf2; // signal variance + T ell; // length-scale + T noise; // noise variance + + MatrixXType x; // temporary storage of training data + MatrixKType K; + MatrixYType alpha; + MatrixKType L; + + bool trained; // true if gpregressor has been trained + }; + + typedef GPRegressor<3, float> GPR3f; +} +#endif // LA3DM_GP_REGRESSOR_H diff --git a/launch/la3dm_static.launch b/launch/la3dm_static.launch index 4e575dc..0d1c700 100755 --- a/launch/la3dm_static.launch +++ b/launch/la3dm_static.launch @@ -8,7 +8,6 @@ Choose world: - sim_structured - sim_unstructured - --> diff --git a/src/block.cpp b/src/bgkblock.cpp similarity index 99% rename from src/block.cpp rename to src/bgkblock.cpp index bbdbb0e..08eb399 100755 --- a/src/block.cpp +++ b/src/bgkblock.cpp @@ -1,4 +1,4 @@ -#include "block.h" +#include "bgkblock.h" #include #include diff --git a/src/bgkoctree.cpp b/src/bgkoctree.cpp new file mode 100755 index 0000000..e76aa63 --- /dev/null +++ b/src/bgkoctree.cpp @@ -0,0 +1,156 @@ +#include "bgkoctree.h" + +#include + +namespace la3dm { + + unsigned short OcTree::max_depth = 0; + + OcTreeHashKey node_to_hash_key(unsigned short depth, unsigned short index) { + return (depth << 16) + index; + } + + void hash_key_to_node(OcTreeHashKey key, unsigned short &depth, unsigned short &index) { + depth = (unsigned short) (key >> 16); + index = (unsigned short) (key & 0xFFFF); + } + + OcTree::OcTree() { + if (max_depth <= 0) + node_arr = nullptr; + else { + node_arr = new OcTreeNode *[max_depth](); + for (unsigned short i = 0; i < max_depth; ++i) { + node_arr[i] = new OcTreeNode[(int) pow(8, i)](); + } + } + } + + OcTree::~OcTree() { + if (node_arr != nullptr) { + for (unsigned short i = 0; i < max_depth; ++i) { + if (node_arr[i] != nullptr) { + delete[] node_arr[i]; + } + } + delete[] node_arr; + } + } + + OcTree::OcTree(const OcTree &other) { + if (other.node_arr == nullptr) { + node_arr = nullptr; + return; + } + + node_arr = new OcTreeNode *[max_depth](); + for (unsigned short i = 0; i < max_depth; ++i) { + if (other.node_arr[i] != nullptr) { + int n = (int) pow(8, i); + node_arr[i] = new OcTreeNode[n](); + std::copy(node_arr[i], node_arr[i] + n, other.node_arr[i]); + } else + node_arr[i] = nullptr; + } + } + + OcTree &OcTree::operator=(const OcTree &other) { + OcTreeNode **local_node_arr = new OcTreeNode *[max_depth](); + for (unsigned short i = 0; i < max_depth; ++i) { + if (local_node_arr[i] != nullptr) { + int n = (int) pow(8, i); + local_node_arr[i] = new OcTreeNode[n](); + std::copy(local_node_arr[i], local_node_arr[i] + n, other.node_arr[i]); + } else + local_node_arr[i] = nullptr; + } + + node_arr = local_node_arr; + return *this; + } + + bool OcTree::is_leaf(unsigned short depth, unsigned short index) const { + if (node_arr != nullptr && node_arr[depth] != nullptr && node_arr[depth][index].get_state() != State::PRUNED) { + if (depth + 1 < max_depth) { + if (node_arr[depth + 1] == nullptr || node_arr[depth + 1][index * 8].get_state() == State::PRUNED) + return true; + } else { + return true; + } + } + return false; + } + + bool OcTree::is_leaf(OcTreeHashKey key) const { + unsigned short depth = 0; + unsigned short index = 0; + hash_key_to_node(key, depth, index); + return is_leaf(depth, index); + } + + bool OcTree::search(OcTreeHashKey key) const { + unsigned short depth; + unsigned short index; + hash_key_to_node(key, depth, index); + + return node_arr != nullptr && + node_arr[depth] != nullptr && + node_arr[depth][index].get_state() != State::PRUNED; + } + + bool OcTree::prune() { + if (node_arr == nullptr) + return false; + + bool pruned = false; + for (unsigned short depth = max_depth - 1; depth > 0; --depth) { + OcTreeNode *layer = node_arr[depth]; + OcTreeNode *parent_layer = node_arr[depth - 1]; + if (layer == nullptr) + continue; + + bool empty_layer = true; + unsigned int n = (unsigned int) pow(8, depth); + for (unsigned short index = 0; index < n; index += 8) { + State state = layer[index].get_state(); + if (state == State::UNKNOWN) { + empty_layer = false; + continue; + } + if (state == State::PRUNED) + continue; + + bool collapsible = true; + for (unsigned short i = 1; i < 8; ++i) { + if (layer[index + i].get_state() != state) { + collapsible = false; + continue; + } + } + + if (collapsible) { + parent_layer[(int) floor(index / 8)] = layer[index]; + for (unsigned short i = 0; i < 8; ++i) { + layer[index + i].prune(); + } + pruned = true; + } else { + empty_layer = false; + } + } + + if (empty_layer) { + delete[] layer; + node_arr[depth] = nullptr; + } + } + return pruned; + } + + OcTreeNode &OcTree::operator[](OcTreeHashKey key) const { + unsigned short depth; + unsigned short index; + hash_key_to_node(key, depth, index); + return node_arr[depth][index]; + } +} \ No newline at end of file diff --git a/src/octree_node.cpp b/src/bgkoctree_node.cpp similarity index 98% rename from src/octree_node.cpp rename to src/bgkoctree_node.cpp index 79520b9..ae13437 100755 --- a/src/octree_node.cpp +++ b/src/bgkoctree_node.cpp @@ -1,4 +1,4 @@ -#include "octree_node.h" +#include "bgkoctree_node.h" #include namespace la3dm { diff --git a/src/gpblock.cpp b/src/gpblock.cpp new file mode 100644 index 0000000..19d5812 --- /dev/null +++ b/src/gpblock.cpp @@ -0,0 +1,159 @@ +#include "gpblock.h" +#include +#include + +namespace la3dm { + std::unordered_map init_key_loc_map(float resolution, unsigned short max_depth) { + std::unordered_map key_loc_map; + + std::queue center_q; + center_q.push(point3f(0.0f, 0.0f, 0.0f)); + + for (unsigned short depth = 0; depth < max_depth; ++depth) { + unsigned short q_size = (unsigned short) center_q.size(); + float half_size = (float) (resolution * pow(2, max_depth - depth - 1) * 0.5f); + for (unsigned short index = 0; index < q_size; ++index) { + point3f center = center_q.front(); + center_q.pop(); + key_loc_map.emplace(node_to_hash_key(depth, index), center); + + if (depth == max_depth - 1) + continue; + for (unsigned short i = 0; i < 8; ++i) { + float x = (float) (center.x() + half_size * (i & 4 ? 0.5 : -0.5)); + float y = (float) (center.y() + half_size * (i & 2 ? 0.5 : -0.5)); + float z = (float) (center.z() + half_size * (i & 1 ? 0.5 : -0.5)); + center_q.emplace(x, y, z); + } + } + } + return key_loc_map; + } + + std::unordered_map init_index_map( + const std::unordered_map &key_loc_map, unsigned short max_depth) { + std::vector> temp; + for (auto it = key_loc_map.begin(); it != key_loc_map.end(); ++it) { + unsigned short depth, index; + hash_key_to_node(it->first, depth, index); + if (depth == max_depth - 1) + temp.push_back(std::make_pair(it->first, it->second)); + } + + std::stable_sort(temp.begin(), temp.end(), + [](const std::pair &p1, + const std::pair &p2) { + return p1.second.x() < p2.second.x(); + }); + std::stable_sort(temp.begin(), temp.end(), + [](const std::pair &p1, + const std::pair &p2) { + return p1.second.y() < p2.second.y(); + }); + std::stable_sort(temp.begin(), temp.end(), + [](const std::pair &p1, + const std::pair &p2) { + return p1.second.z() < p2.second.z(); + }); + + std::unordered_map index_map; + int index = 0; + for (auto it = temp.cbegin(); it != temp.cend(); ++it, ++index) { + index_map.insert(std::make_pair(index, it->first)); + } + + return index_map; + }; + + BlockHashKey block_to_hash_key(point3f center) { + return block_to_hash_key(center.x(), center.y(), center.z()); + } + + BlockHashKey block_to_hash_key(float x, float y, float z) { + return (int64_t(x / (double) Block::size + 524288.5) << 40) | + (int64_t(y / (double) Block::size + 524288.5) << 20) | + (int64_t(z / (double) Block::size + 524288.5)); + } + + point3f hash_key_to_block(BlockHashKey key) { + return point3f(((key >> 40) - 524288) * Block::size, + (((key >> 20) & 0xFFFFF) - 524288) * Block::size, + ((key & 0xFFFFF) - 524288) * Block::size); + } + + ExtendedBlock get_extended_block(BlockHashKey key) { + ExtendedBlock blocks; + point3f center = hash_key_to_block(key); + float x = center.x(); + float y = center.y(); + float z = center.z(); + blocks[0] = key; + + float ex, ey, ez; + for (int i = 0; i < 6; ++i) { + ex = (i / 2 == 0) ? (i % 2 == 0 ? Block::size : -Block::size) : 0; + ey = (i / 2 == 1) ? (i % 2 == 0 ? Block::size : -Block::size) : 0; + ez = (i / 2 == 2) ? (i % 2 == 0 ? Block::size : -Block::size) : 0; + blocks[i + 1] = block_to_hash_key(ex + x, ey + y, ez + z); + } + return blocks; + } + + float Block::resolution = 0.1f; + float Block::size = 0.8f; + unsigned short Block::cell_num = static_cast(round(Block::size / Block::resolution)); + + std::unordered_map Block::key_loc_map; + std::unordered_map Block::index_map; + + Block::Block() : OcTree(), center(0.0f, 0.0f, 0.0f) { } + + Block::Block(point3f center) : OcTree(), center(center) { } + + ExtendedBlock Block::get_extended_block() const { + ExtendedBlock blocks; + float x = center.x(); + float y = center.y(); + float z = center.z(); + blocks[0] = block_to_hash_key(x, y, z); + + float ex, ey, ez; + for (int i = 0; i < 6; ++i) { + ex = (i / 2 == 0) ? (i % 2 == 0 ? Block::size : -Block::size) : 0; + ey = (i / 2 == 1) ? (i % 2 == 0 ? Block::size : -Block::size) : 0; + ez = (i / 2 == 2) ? (i % 2 == 0 ? Block::size : -Block::size) : 0; + blocks[i + 1] = block_to_hash_key(ex + x, ey + y, ez + z); + } + + return blocks; + } + + OcTreeHashKey Block::get_node(unsigned short x, unsigned short y, unsigned short z) const { + unsigned short index = x + y * Block::cell_num + z * Block::cell_num * Block::cell_num; + return Block::index_map[index]; + } + + point3f Block::get_point(unsigned short x, unsigned short y, unsigned short z) const { + return Block::key_loc_map[get_node(x, y, z)] + center; + } + + void Block::get_index(const point3f &p, unsigned short &x, unsigned short &y, unsigned short &z) const { + int xx = static_cast((p.x() - center.x()) / resolution + Block::cell_num / 2); + int yy = static_cast((p.y() - center.y()) / resolution + Block::cell_num / 2); + int zz = static_cast((p.z() - center.z()) / resolution + Block::cell_num / 2); + auto clip = [](int a) -> int { return std::max(0, std::min(a, Block::cell_num - 1)); }; + x = static_cast(clip(xx)); + y = static_cast(clip(yy)); + z = static_cast(clip(zz)); + } + + OcTreeNode& Block::search(float x, float y, float z) const { + return search(point3f(x, y, z)); + } + + OcTreeNode& Block::search(point3f p) const { + unsigned short x, y, z; + get_index(p, x, y, z); + return operator[](get_node(x, y, z)); + } +} diff --git a/src/gpoctomap.cpp b/src/gpoctomap.cpp new file mode 100644 index 0000000..19e2362 --- /dev/null +++ b/src/gpoctomap.cpp @@ -0,0 +1,546 @@ +#include +#include +#include "gpoctomap.h" +#include "gpregressor.h" + +using std::vector; + +#ifdef DEBUG + +#include + +#define Debug_Msg(msg) {\ +std::cout << "Debug: " << msg << std::endl; } +#endif + +namespace la3dm { + + GPOctoMap::GPOctoMap() : GPOctoMap(0.1f, 4, 1.0, 1.0, 0.01, 100, 0.001f, 1000.0f, 0.02f, 0.3f, 0.7f) { } + + GPOctoMap::GPOctoMap(float resolution, unsigned short block_depth, float sf2, float ell, float noise, float l, + float min_var, + float max_var, float max_known_var, float free_thresh, float occupied_thresh) + : resolution(resolution), block_depth(block_depth), + block_size((float) pow(2, block_depth - 1) * resolution) { + Block::resolution = resolution; + Block::size = this->block_size; + Block::key_loc_map = init_key_loc_map(resolution, block_depth); + Block::index_map = init_index_map(Block::key_loc_map, block_depth); + + OcTree::max_depth = block_depth; + + OcTreeNode::sf2 = sf2; + OcTreeNode::ell = ell; + OcTreeNode::noise = noise; + OcTreeNode::l = l; + + OcTreeNode::min_ivar = 1.0f / max_var; + OcTreeNode::max_ivar = 1.0f / min_var; + OcTreeNode::min_known_ivar = 1.0f / max_known_var; + OcTreeNode::free_thresh = free_thresh; + OcTreeNode::occupied_thresh = occupied_thresh; + } + + GPOctoMap::~GPOctoMap() { + for (auto it = block_arr.begin(); it != block_arr.end(); ++it) { + if (it->second != nullptr) { + delete it->second; + } + } + } + + void GPOctoMap::set_resolution(float resolution) { + this->resolution = resolution; + Block::resolution = resolution; + this->block_size = (float) pow(2, block_depth - 1) * resolution; + Block::size = this->block_size; + Block::key_loc_map = init_key_loc_map(resolution, block_depth); + } + + void GPOctoMap::set_block_depth(unsigned short max_depth) { + this->block_depth = max_depth; + OcTree::max_depth = max_depth; + this->block_size = (float) pow(2, block_depth - 1) * resolution; + Block::size = this->block_size; + Block::key_loc_map = init_key_loc_map(resolution, block_depth); + } + + void GPOctoMap::insert_training_data(const GPPointCloud &xy) { + point3f lim_min, lim_max; + bbox(xy, lim_min, lim_max); + + vector blocks; + get_blocks_in_bbox(lim_min, lim_max, blocks); + + for (auto it = xy.cbegin(); it != xy.cend(); ++it) { + float p[] = {it->first.x(), it->first.y(), it->first.z()}; + rtree.Insert(p, p, const_cast(&*it)); + } + ///////////////////////////////////////////////// + + ////////// Training ///////////////////////////// + ///////////////////////////////////////////////// + vector test_blocks; + std::unordered_map gpr_arr; +#ifdef OPENMP +#pragma omp parallel for schedule(dynamic) +#endif + for (int i = 0; i < blocks.size(); ++i) { + BlockHashKey key = blocks[i]; + ExtendedBlock eblock = get_extended_block(key); + if (has_gp_points_in_bbox(eblock)) +#ifdef OPENMP +#pragma omp critical +#endif + { + test_blocks.push_back(key); + }; + + GPPointCloud block_xy; + get_gp_points_in_bbox(key, block_xy); + if (block_xy.size() < 1) + continue; + + vector block_x, block_y; + for (auto it = block_xy.cbegin(); it != block_xy.cend(); ++it) { + block_x.push_back(it->first.x()); + block_x.push_back(it->first.y()); + block_x.push_back(it->first.z()); + block_y.push_back(it->second); + } + GPR3f *gpr = new GPR3f(OcTreeNode::sf2, OcTreeNode::ell, OcTreeNode::noise); + gpr->train(block_x, block_y); +#ifdef OPENMP +#pragma omp critical +#endif + { + gpr_arr.emplace(key, gpr); + }; + } +#ifdef DEBUG + Debug_Msg("GP training done"); + Debug_Msg("GP prediction: block number: " << test_blocks.size()); +#endif + ///////////////////////////////////////////////// + + ////////// Prediction /////////////////////////// + ///////////////////////////////////////////////// +#ifdef OPENMP +#pragma omp parallel for schedule(dynamic) +#endif + for (int i = 0; i < test_blocks.size(); ++i) { + BlockHashKey key = test_blocks[i]; + Block *block; +#ifdef OPENMP +#pragma omp critical +#endif + { + block = search(key); + if (block == nullptr) +// if (block_arr.find(key) == block_arr.end()) + block_arr.emplace(key, new Block(hash_key_to_block(key))); + }; + vector xs; + for (auto leaf_it = block->begin_leaf(); leaf_it != block->end_leaf(); ++leaf_it) { + point3f p = block->get_loc(leaf_it); + xs.push_back(p.x()); + xs.push_back(p.y()); + xs.push_back(p.z()); + } + + ExtendedBlock eblock = block->get_extended_block(); + for (auto block_it = eblock.cbegin(); block_it != eblock.cend(); ++block_it) { + auto gpr = gpr_arr.find(*block_it); + if (gpr == gpr_arr.end()) + continue; + + vector m, var; + gpr->second->predict(xs, m, var); + + int j = 0; + for (auto leaf_it = block->begin_leaf(); leaf_it != block->end_leaf(); ++leaf_it, ++j) { + OcTreeNode &node = leaf_it.get_node(); + node.update(m[j], var[j]); + } + } + } +#ifdef DEBUG + Debug_Msg("GP prediction done"); +#endif + ///////////////////////////////////////////////// + + ////////// Pruning ////////////////////////////// + ///////////////////////////////////////////////// +#ifdef OPENMP +#pragma omp parallel for +#endif + for (int i = 0; i < test_blocks.size(); ++i) { + BlockHashKey key = test_blocks[i]; + auto block = block_arr.find(key); + if (block == block_arr.end()) + continue; + block->second->prune(); + } +#ifdef DEBUG + Debug_Msg("Pruning done"); +#endif + ///////////////////////////////////////////////// + + + ////////// Cleaning ///////////////////////////// + ///////////////////////////////////////////////// + for (auto it = gpr_arr.begin(); it != gpr_arr.end(); ++it) + delete it->second; + + rtree.RemoveAll(); + } + + void GPOctoMap::insert_pointcloud(const PCLPointCloud &cloud, const point3f &origin, float ds_resolution, + float free_res, float max_range) { + +#ifdef DEBUG + Debug_Msg("Insert pointcloud: " << "cloud size: " << cloud.size() << " origin: " << origin); +#endif + + ////////// Preparation ////////////////////////// + ///////////////////////////////////////////////// + GPPointCloud xy; + get_training_data(cloud, origin, ds_resolution, free_res, max_range, xy); +#ifdef DEBUG + Debug_Msg("Training data size: " << xy.size()); +#endif + + point3f lim_min, lim_max; + bbox(xy, lim_min, lim_max); + + vector blocks; + get_blocks_in_bbox(lim_min, lim_max, blocks); + + for (auto it = xy.cbegin(); it != xy.cend(); ++it) { + float p[] = {it->first.x(), it->first.y(), it->first.z()}; + rtree.Insert(p, p, const_cast(&*it)); + } + ///////////////////////////////////////////////// + + ////////// Training ///////////////////////////// + ///////////////////////////////////////////////// + vector test_blocks; + std::unordered_map gpr_arr; +#ifdef OPENMP +#pragma omp parallel for schedule(dynamic) +#endif + for (int i = 0; i < blocks.size(); ++i) { + BlockHashKey key = blocks[i]; + ExtendedBlock eblock = get_extended_block(key); + if (has_gp_points_in_bbox(eblock)) +#ifdef OPENMP +#pragma omp critical +#endif + { + test_blocks.push_back(key); + }; + + GPPointCloud block_xy; + get_gp_points_in_bbox(key, block_xy); + if (block_xy.size() < 1) + continue; + + vector block_x, block_y; + for (auto it = block_xy.cbegin(); it != block_xy.cend(); ++it) { + block_x.push_back(it->first.x()); + block_x.push_back(it->first.y()); + block_x.push_back(it->first.z()); + block_y.push_back(it->second); + } + GPR3f *gpr = new GPR3f(OcTreeNode::sf2, OcTreeNode::ell, OcTreeNode::noise); + gpr->train(block_x, block_y); +#ifdef OPENMP +#pragma omp critical +#endif + { + gpr_arr.emplace(key, gpr); + }; + } +#ifdef DEBUG + Debug_Msg("GP training done"); + Debug_Msg("GP prediction: block number: " << test_blocks.size()); +#endif + ///////////////////////////////////////////////// + + ////////// Prediction /////////////////////////// + ///////////////////////////////////////////////// +#ifdef OPENMP +#pragma omp parallel for schedule(dynamic) +#endif + for (int i = 0; i < test_blocks.size(); ++i) { + BlockHashKey key = test_blocks[i]; +#ifdef OPENMP +#pragma omp critical +#endif + { + if (block_arr.find(key) == block_arr.end()) + block_arr.emplace(key, new Block(hash_key_to_block(key))); + }; + Block *block = block_arr[key]; + vector xs; + for (auto leaf_it = block->begin_leaf(); leaf_it != block->end_leaf(); ++leaf_it) { + point3f p = block->get_loc(leaf_it); + xs.push_back(p.x()); + xs.push_back(p.y()); + xs.push_back(p.z()); + } + + ExtendedBlock eblock = block->get_extended_block(); + for (auto block_it = eblock.cbegin(); block_it != eblock.cend(); ++block_it) { + auto gpr = gpr_arr.find(*block_it); + if (gpr == gpr_arr.end()) + continue; + + vector m, var; + gpr->second->predict(xs, m, var); + + int j = 0; + for (auto leaf_it = block->begin_leaf(); leaf_it != block->end_leaf(); ++leaf_it, ++j) { + OcTreeNode &node = leaf_it.get_node(); + node.update(m[j], var[j]); + } + } + } +#ifdef DEBUG + Debug_Msg("GP prediction done"); +#endif + ///////////////////////////////////////////////// + + ////////// Pruning ////////////////////////////// + ///////////////////////////////////////////////// +// #ifdef OPENMP +// #pragma omp parallel for +// #endif +// for (int i = 0; i < test_blocks.size(); ++i) { +// BlockHashKey key = test_blocks[i]; +// auto block = block_arr.find(key); +// if (block == block_arr.end()) +// continue; +// block->second->prune(); +// } +// #ifdef DEBUG +// Debug_Msg("Pruning done"); +// #endif + ///////////////////////////////////////////////// + + + ////////// Cleaning ///////////////////////////// + ///////////////////////////////////////////////// + for (auto it = gpr_arr.begin(); it != gpr_arr.end(); ++it) + delete it->second; + + rtree.RemoveAll(); + } + + void GPOctoMap::get_bbox(point3f &lim_min, point3f &lim_max) const { + lim_min = point3f(0, 0, 0); + lim_max = point3f(0, 0, 0); + + GPPointCloud centers; + for (auto it = block_arr.cbegin(); it != block_arr.cend(); ++it) { + centers.emplace_back(it->second->get_center(), 1); + } + if (centers.size() > 0) { + bbox(centers, lim_min, lim_max); + lim_min -= point3f(block_size, block_size, block_size) * 0.5; + lim_max += point3f(block_size, block_size, block_size) * 0.5; + } + } + + void GPOctoMap::get_training_data(const PCLPointCloud &cloud, const point3f &origin, float ds_resolution, + float free_resolution, float max_range, GPPointCloud &xy) const { + PCLPointCloud sampled_hits; + downsample(cloud, sampled_hits, ds_resolution); + + PCLPointCloud frees; + frees.height = 1; + frees.width = 0; + xy.clear(); + for (auto it = sampled_hits.begin(); it != sampled_hits.end(); ++it) { + point3f p(it->x, it->y, it->z); + if (max_range > 0) { + double l = (p - origin).norm(); + if (l > max_range) + continue; + } + xy.emplace_back(p, 1.0f); + + PointCloud frees_n; + beam_sample(p, origin, frees_n, free_resolution); + + frees.push_back(PCLPointType(origin.x(), origin.y(), origin.z())); + for (auto p = frees_n.begin(); p != frees_n.end(); ++p) { + frees.push_back(PCLPointType(p->x(), p->y(), p->z())); + frees.width++; + } + } + + PCLPointCloud sampled_frees; + downsample(frees, sampled_frees, ds_resolution); + + for (auto it = sampled_frees.begin(); it != sampled_frees.end(); ++it) { + xy.emplace_back(point3f(it->x, it->y, it->z), -1); + } + } + + void GPOctoMap::downsample(const PCLPointCloud &in, PCLPointCloud &out, float ds_resolution) const { + if (ds_resolution < 0) { + out = in; + return; + } + + PCLPointCloud::Ptr pcl_in(new PCLPointCloud(in)); + + pcl::VoxelGrid sor; + sor.setInputCloud(pcl_in); + sor.setLeafSize(ds_resolution, ds_resolution, ds_resolution); + sor.filter(out); + +// vector indices; +// pcl_out.is_dense = false; +// pcl::removeNaNFromPointCloud(out, out, indices); + } + + void GPOctoMap::beam_sample(const point3f &hit, const point3f &origin, PointCloud &frees, + float free_resolution) const { + frees.clear(); + + float x0 = origin.x(); + float y0 = origin.y(); + float z0 = origin.z(); + + float x = hit.x(); + float y = hit.y(); + float z = hit.z(); + + float l = (float) sqrt((x - x0) * (x - x0) + (y - y0) * (y - y0) + (z - z0) * (z - z0)); + + float nx = (x - x0) / l; + float ny = (y - y0) / l; + float nz = (z - z0) / l; + + float d = free_resolution; + while (d < l) { + frees.emplace_back(x0 + nx * d, y0 + ny * d, z0 + nz * d); + d += free_resolution; + } + if (l > free_resolution) + frees.emplace_back(x0 + nx * (l - free_resolution), y0 + ny * (l - free_resolution), z0 + nz * (l - free_resolution)); + } + + void GPOctoMap::bbox(const GPPointCloud &cloud, point3f &lim_min, point3f &lim_max) const { + vector x, y, z; + for (auto it = cloud.cbegin(); it != cloud.cend(); ++it) { + x.push_back(it->first.x()); + y.push_back(it->first.y()); + z.push_back(it->first.z()); + } + + auto xlim = std::minmax_element(x.cbegin(), x.cend()); + auto ylim = std::minmax_element(y.cbegin(), y.cend()); + auto zlim = std::minmax_element(z.cbegin(), z.cend()); + + lim_min.x() = *xlim.first; + lim_min.y() = *ylim.first; + lim_min.z() = *zlim.first; + + lim_max.x() = *xlim.second; + lim_max.y() = *ylim.second; + lim_max.z() = *zlim.second; + } + + void GPOctoMap::get_blocks_in_bbox(const point3f &lim_min, const point3f &lim_max, + vector &blocks) const { + for (float x = lim_min.x() - block_size; x <= lim_max.x() + 2 * block_size; x += block_size) { + for (float y = lim_min.y() - block_size; y <= lim_max.y() + 2 * block_size; y += block_size) { + for (float z = lim_min.z() - block_size; z <= lim_max.z() + 2 * block_size; z += block_size) { + blocks.push_back(block_to_hash_key(x, y, z)); + } + } + } + } + + int GPOctoMap::get_gp_points_in_bbox(const BlockHashKey &key, + GPPointCloud &out) { + point3f half_size(block_size / 2.0f, block_size / 2.0f, block_size / 2.0); + point3f lim_min = hash_key_to_block(key) - half_size; + point3f lim_max = hash_key_to_block(key) + half_size; + return get_gp_points_in_bbox(lim_min, lim_max, out); + } + + int GPOctoMap::has_gp_points_in_bbox(const BlockHashKey &key) { + point3f half_size(block_size / 2.0f, block_size / 2.0f, block_size / 2.0); + point3f lim_min = hash_key_to_block(key) - half_size; + point3f lim_max = hash_key_to_block(key) + half_size; + return has_gp_points_in_bbox(lim_min, lim_max); + } + + int GPOctoMap::get_gp_points_in_bbox(const point3f &lim_min, const point3f &lim_max, + GPPointCloud &out) { + float a_min[] = {lim_min.x(), lim_min.y(), lim_min.z()}; + float a_max[] = {lim_max.x(), lim_max.y(), lim_max.z()}; + return rtree.Search(a_min, a_max, GPOctoMap::search_callback, static_cast(&out)); + } + + int GPOctoMap::has_gp_points_in_bbox(const point3f &lim_min, + const point3f &lim_max) { + float a_min[] = {lim_min.x(), lim_min.y(), lim_min.z()}; + float a_max[] = {lim_max.x(), lim_max.y(), lim_max.z()}; + return rtree.Search(a_min, a_max, GPOctoMap::count_callback, NULL); + } + + bool GPOctoMap::count_callback(GPPointType *p, void *arg) { + return false; + } + + bool GPOctoMap::search_callback(GPPointType *p, void *arg) { + GPPointCloud *out = static_cast(arg); + out->push_back(*p); + return true; + } + + + int GPOctoMap::has_gp_points_in_bbox(const ExtendedBlock &block) { + for (auto it = block.cbegin(); it != block.cend(); ++it) { + if (has_gp_points_in_bbox(*it) > 0) + return 1; + } + return 0; + } + + int GPOctoMap::get_gp_points_in_bbox(const ExtendedBlock &block, + GPPointCloud &out) { + int n = 0; + for (auto it = block.cbegin(); it != block.cend(); ++it) { + n += get_gp_points_in_bbox(*it, out); + } + return n; + } + + Block *GPOctoMap::search(BlockHashKey key) const { + auto block = block_arr.find(key); + if (block == block_arr.end()) { + return nullptr; + } else { + return block->second; + } + } + + OcTreeNode GPOctoMap::search(point3f p) const { + Block *block = search(block_to_hash_key(p)); + if (block == nullptr) { + return OcTreeNode(); + } else { + return OcTreeNode(block->search(p)); + } + } + + OcTreeNode GPOctoMap::search(float x, float y, float z) const { + return search(point3f(x, y, z)); + } +} diff --git a/src/gpoctomap_static_node.cpp b/src/gpoctomap_static_node.cpp new file mode 100644 index 0000000..c3d5579 --- /dev/null +++ b/src/gpoctomap_static_node.cpp @@ -0,0 +1,159 @@ +#include +#include +#include +#include "gpoctomap.h" +#include "markerarray_pub.h" + +void load_pcd(std::string filename, la3dm::point3f &origin, la3dm::PCLPointCloud &cloud) { + pcl::PCLPointCloud2 cloud2; + Eigen::Vector4f _origin; + Eigen::Quaternionf orientaion; + pcl::io::loadPCDFile(filename, cloud2, _origin, orientaion); + pcl::fromPCLPointCloud2(cloud2, cloud); + origin.x() = _origin[0]; + origin.y() = _origin[1]; + origin.z() = _origin[2]; +} + +int main(int argc, char **argv) { + ros::init(argc, argv, "gpoctomap_static_node"); + ros::NodeHandle nh("~"); + + std::string dir; + std::string prefix; + int scan_num = 0; + std::string map_topic("/occupied_cells_vis_array"); + double max_range = -1; + double resolution = 0.1; + int block_depth = 4; + double sf2 = 1.0; + double ell = 1.0; + double noise = 0.01; + double l = 100; + double min_var = 0.001; + double max_var = 1000; + double max_known_var = 0.02; + double free_resolution = 0.5; + double ds_resolution = 0.1; + double free_thresh = 0.3; + double occupied_thresh = 0.7; + double min_z = 0; + double max_z = 0; + bool original_size = false; + + nh.param("dir", dir, dir); + nh.param("prefix", prefix, prefix); + nh.param("topic", map_topic, map_topic); + nh.param("scan_num", scan_num, scan_num); + nh.param("max_range", max_range, max_range); + nh.param("resolution", resolution, resolution); + nh.param("block_depth", block_depth, block_depth); + nh.param("sf2", sf2, sf2); + nh.param("ell", ell, ell); + nh.param("noise", noise, noise); + nh.param("l", l, l); + nh.param("min_var", min_var, min_var); + nh.param("max_var", max_var, max_var); + nh.param("max_known_var", max_known_var, max_known_var); + nh.param("free_resolution", free_resolution, free_resolution); + nh.param("ds_resolution", ds_resolution, ds_resolution); + nh.param("free_thresh", free_thresh, free_thresh); + nh.param("occupied_thresh", occupied_thresh, occupied_thresh); + nh.param("min_z", min_z, min_z); + nh.param("max_z", max_z, max_z); + nh.param("original_size", original_size, original_size); + + ROS_INFO_STREAM("Parameters:" << std::endl << + "dir: " << dir << std::endl << + "prefix: " << prefix << std::endl << + "topic: " << map_topic << std::endl << + "scan_sum: " << scan_num << std::endl << + "max_range: " << max_range << std::endl << + "resolution: " << resolution << std::endl << + "block_depth: " << block_depth << std::endl << + "sf2: " << sf2 << std::endl << + "ell: " << ell << std::endl << + "l: " << l << std::endl << + "min_var: " << min_var << std::endl << + "max_var: " << max_var << std::endl << + "max_known_var: " << max_known_var << std::endl << + "free_resolution: " << free_resolution << std::endl << + "ds_resolution: " << ds_resolution << std::endl << + "free_thresh: " << free_thresh << std::endl << + "occupied_thresh: " << occupied_thresh << std::endl << + "min_z: " << min_z << std::endl << + "max_z: " << max_z << std::endl << + "original_size: " << original_size + ); + + la3dm::GPOctoMap map(resolution, block_depth, sf2, ell, noise, l, min_var, max_var, max_known_var, free_thresh, occupied_thresh); + + ros::Time start = ros::Time::now(); + for (int scan_id = 1; scan_id <= scan_num; ++scan_id) { + la3dm::PCLPointCloud cloud; + la3dm::point3f origin; + std::string filename(dir + "/" + prefix + "_" + std::to_string(scan_id) + ".pcd"); + load_pcd(filename, origin, cloud); + + map.insert_pointcloud(cloud, origin, resolution, free_resolution, max_range); + ROS_INFO_STREAM("Scan " << scan_id << " done"); + } + ros::Time end = ros::Time::now(); + ROS_INFO_STREAM("Mapping finished in " << (end - start).toSec() << "s"); + + ///////// Compute Frontiers ///////////////////// + ROS_INFO_STREAM("Computing frontiers"); + la3dm::MarkerArrayPub f_pub(nh, "frontier_map", resolution); + for (auto it = map.begin_leaf(); it != map.end_leaf(); ++it) { + la3dm::point3f p = it.get_loc(); + if (p.z() > 1.0 || p.z() < 0.3) + continue; + + + if (it.get_node().get_var() > 0.02 && + it.get_node().get_prob() < 0.3) { + f_pub.insert_point3d(p.x(), p.y(), p.z()); + } + } + f_pub.publish(); + + //////// Test Raytracing ////////////////// + la3dm::MarkerArrayPub ray_pub(nh, "/ray", resolution); + la3dm::GPOctoMap::RayCaster ray(&map, la3dm::point3f(1, 1, 0.3), la3dm::point3f(6, 7, 8)); + while (!ray.end()) { + la3dm::point3f p; + la3dm::OcTreeNode node; + la3dm::BlockHashKey block_key; + la3dm::OcTreeHashKey node_key; + if (ray.next(p, node, block_key, node_key)) { + ray_pub.insert_point3d(p.x(), p.y(), p.z()); + } + } + ray_pub.publish(); + + ///////// Publish Map ///////////////////// + la3dm::MarkerArrayPub m_pub(nh, map_topic, 0.1f); + if (min_z == max_z) { + la3dm::point3f lim_min, lim_max; + map.get_bbox(lim_min, lim_max); + min_z = lim_min.z(); + max_z = lim_max.z(); + } + for (auto it = map.begin_leaf(); it != map.end_leaf(); ++it) { + if (it.get_node().get_state() == la3dm::State::OCCUPIED) { + if (original_size) { + la3dm::point3f p = it.get_loc(); + m_pub.insert_point3d(p.x(), p.y(), p.z(), min_z, max_z, it.get_size()); + } else { + auto pruned = it.get_pruned_locs(); + for (auto n = pruned.cbegin(); n < pruned.cend(); ++n) + m_pub.insert_point3d(n->x(), n->y(), n->z(), min_z, max_z, map.get_resolution()); + } + } + } + + m_pub.publish(); + ros::spin(); + + return 0; +} diff --git a/src/octree.cpp b/src/gpoctree.cpp old mode 100755 new mode 100644 similarity index 99% rename from src/octree.cpp rename to src/gpoctree.cpp index 6b27294..0b7b455 --- a/src/octree.cpp +++ b/src/gpoctree.cpp @@ -1,9 +1,8 @@ -#include "octree.h" +#include "gpoctree.h" #include namespace la3dm { - unsigned short OcTree::max_depth = 0; OcTreeHashKey node_to_hash_key(unsigned short depth, unsigned short index) { diff --git a/src/gpoctree_node.cpp b/src/gpoctree_node.cpp new file mode 100644 index 0000000..a58a9e2 --- /dev/null +++ b/src/gpoctree_node.cpp @@ -0,0 +1,66 @@ +#include "gpoctree_node.h" +#include "gpregressor.h" +#include + +namespace la3dm { + /// Default static values + float Occupancy::sf2 = 1.0f; + float Occupancy::ell = 1.0f; + float Occupancy::noise = 0.01f; + float Occupancy::l = 100.f; + + float Occupancy::max_ivar = 1000.0f; + float Occupancy::min_ivar = 0.001f; + + float Occupancy::min_known_ivar = 10.0f; + float Occupancy::free_thresh = 0.3f; + float Occupancy::occupied_thresh = 0.7f; + + Occupancy::Occupancy(float m, float var) : m_ivar(m / var), ivar(1.0f / var) { + if (ivar < Occupancy::min_known_ivar) + state = State::UNKNOWN; + else { + ivar = ivar > Occupancy::max_ivar ? Occupancy::max_ivar : ivar; + float p = get_prob(); + state = p > Occupancy::occupied_thresh ? State::OCCUPIED : (p < Occupancy::free_thresh ? State::FREE + : State::UNKNOWN); + } + } + + float Occupancy::get_prob() const { + // logistic regression function + return 1.0f / (1.0f + (float) exp(-l * m_ivar / Occupancy::max_ivar)); + } + + void Occupancy::update(float new_m, float new_var) { + ivar += 1.0 / new_var - Occupancy::sf2; + m_ivar += new_m / new_var; + if (ivar < Occupancy::min_known_ivar) + state = State::UNKNOWN; + else { + // chop variance + ivar = ivar > Occupancy::max_ivar ? Occupancy::max_ivar : ivar; + float p = get_prob(); + state = p > Occupancy::occupied_thresh ? State::OCCUPIED : (p < Occupancy::free_thresh ? State::FREE + : State::UNKNOWN); + } + } + + std::ofstream &operator<<(std::ofstream &os, const Occupancy &oc) { + os.write((char *) &oc.m_ivar, sizeof(oc.m_ivar)); + os.write((char *) &oc.ivar, sizeof(oc.ivar)); + return os; + } + + std::ifstream &operator>>(std::ifstream &is, Occupancy &oc) { + float m_ivar, ivar; + is.read((char *) &m_ivar, sizeof(m_ivar)); + is.read((char *) &ivar, sizeof(ivar)); + oc = OcTreeNode(m_ivar / ivar, 1.0f / ivar); + return is; + } + + std::ostream &operator<<(std::ostream &os, const Occupancy &oc) { + return os << '(' << (oc.m_ivar / oc.ivar) << ' ' << (1.0 / oc.ivar) << ' ' << oc.get_prob() << ')'; + } +}