Skip to content

Commit

Permalink
First pass at GP integration
Browse files Browse the repository at this point in the history
  • Loading branch information
keevindoherty committed Feb 27, 2017
1 parent ae39579 commit 3381c8b
Show file tree
Hide file tree
Showing 20 changed files with 2,061 additions and 35 deletions.
33 changes: 19 additions & 14 deletions CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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
)
Expand All @@ -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)

Expand All @@ -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}
Expand Down
24 changes: 24 additions & 0 deletions config/methods/gpoctomap.yaml
Original file line number Diff line number Diff line change
@@ -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
10 changes: 5 additions & 5 deletions include/block.h → include/bgkblock.h
Original file line number Diff line number Diff line change
@@ -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 {

Expand Down Expand Up @@ -106,4 +106,4 @@ namespace la3dm {
};
}

#endif // LA3DM_BLOCK_H
#endif // LA3DM_BGK_BLOCK_H
8 changes: 4 additions & 4 deletions include/bgkoctomap.h
Original file line number Diff line number Diff line change
@@ -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 {

Expand Down
8 changes: 4 additions & 4 deletions include/octree.h → include/bgkoctree.h
Original file line number Diff line number Diff line change
@@ -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 {

Expand Down Expand Up @@ -158,4 +158,4 @@ namespace la3dm {
};
}

#endif // LA3DM_OCTREE_H
#endif // LA3DM_BGK_OCTREE_H
6 changes: 3 additions & 3 deletions include/octree_node.h → include/bgkoctree_node.h
Original file line number Diff line number Diff line change
@@ -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>
Expand Down Expand Up @@ -94,4 +94,4 @@ namespace la3dm {
typedef Occupancy OcTreeNode;
}

#endif // LA3DM_OCCUPANCY_H
#endif // LA3DM_BGK_OCCUPANCY_H
110 changes: 110 additions & 0 deletions include/gpblock.h
Original file line number Diff line number Diff line change
@@ -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
Loading

0 comments on commit 3381c8b

Please sign in to comment.