First pass at GP integration

Author: keevindoherty

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}

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

include/block.h renamed to 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 <unordered_map>
 #include <array>
 #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

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 <unordered_map>
 #include <vector>
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
 #include "rtree.h"
-#include "block.h"
-#include "octree_node.h"
+#include "bgkblock.h"
+#include "bgkoctree_node.h"
 
 namespace la3dm {
 

include/octree.h renamed to 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 <stack>
 #include <vector>
 #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

include/octree_node.h renamed to 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 <iostream>
 #include <fstream>
@@ -94,4 +94,4 @@ namespace la3dm {
     typedef Occupancy OcTreeNode;
 }
 
-#endif // LA3DM_OCCUPANCY_H
+#endif // LA3DM_BGK_OCCUPANCY_H

include/gpblock.h

@@ -0,0 +1,110 @@
+#ifndef LA3DM_GP_BLOCK_H
+#define LA3DM_GP_BLOCK_H
+
+#include <unordered_map>
+#include <array>
+#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<OcTreeHashKey, point3f> init_key_loc_map(float resolution, unsigned short max_depth);
+
+    std::unordered_map<unsigned short, OcTreeHashKey> init_index_map(const std::unordered_map<OcTreeHashKey, point3f> &key_loc_map,
+                                                                     unsigned short max_depth);
+
+    /// Extended Block
+#ifdef PREDICT
+    typedef std::array<BlockHashKey, 27> ExtendedBlock;
+#else
+    typedef std::array<BlockHashKey, 7> 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<OcTreeHashKey, point3f> key_loc_map;
+        static std::unordered_map<unsigned short, OcTreeHashKey> index_map;
+        static float resolution;
+        static float size;
+        static unsigned short cell_num;
+
+        point3f center;
+    };
+
+}
+
+#endif // LA3DM_GP_BLOCK_H