Skip to content

Commit 80846d3

Browse files
SchmlukSchmluk
authored
Feature/rg traversability (#129)
* fix brac initializer * move astro to khronos * WIP: infrastructure for region growing places * WIP: first draft of region growing clustering * more refactoring * add tmp configs * implement max distance region growing * implement region growing traversability clustering * first draft of update functor * implement first version of traversability clustering for new attrs * implement region growing place representation * WIP: implement update functor * draft new update functor * rework region growing update functor * implement proper backend merging * remove debug prints * implement boundary state tracking * undo remnants of old test code * update to new interfaces * fix CMakeLists rebase error * remove tmp configs and update formatting
1 parent dee884e commit 80846d3

16 files changed

+1201
-62
lines changed

‎config/datasets/uhumans2.yaml‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -96,9 +96,11 @@ backend:
9696
places:
9797
type: UpdatePlacesFunctor
9898
traversability:
99-
type: UpdateTraversabilityFunctor
99+
type: UpdateBlockTraversabilityFunctor
100100
min_place_size: *min_place_width
101+
# m
101102
max_place_size: *max_place_width
103+
# m
102104
use_metric_distance: false
103105
tolerance: 0.1 # m
104106
frontiers:

‎include/hydra/backend/dsg_updater.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -56,7 +56,7 @@ class DsgUpdater {
5656
const kimera_pgmo::DeformationGraph&>;
5757
using NodeToRobotMap = std::unordered_map<NodeId, size_t>;
5858

59-
struct Config : VerbosityConfig {
59+
struct Config : public VerbosityConfig {
6060
using FunctorConfig = config::VirtualConfig<UpdateFunctor, true>;
6161

6262
Config();

include/hydra/backend/update_traversability_functor.h renamed to include/hydra/backend/update_block_traversability_functor.h

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -47,7 +47,7 @@ namespace hydra {
4747
* @brief Functor to update traversability places in the DSG. This functor should be
4848
* called with exhaustive merging enabled.
4949
*/
50-
struct UpdateTraversabilityFunctor : public UpdateFunctor {
50+
struct UpdateBlockTraversabilityFunctor : public UpdateFunctor {
5151
struct Config {
5252
//! Layer to update traversability in
5353
std::string layer = DsgLayers::TRAVERSABILITY;
@@ -73,7 +73,7 @@ struct UpdateTraversabilityFunctor : public UpdateFunctor {
7373
using NodeSet = std::set<NodeId>;
7474
using State = spark_dsg::TraversabilityState;
7575

76-
explicit UpdateTraversabilityFunctor(const Config& config);
76+
explicit UpdateBlockTraversabilityFunctor(const Config& config);
7777

7878
Hooks hooks() const override;
7979

@@ -160,6 +160,6 @@ struct UpdateTraversabilityFunctor : public UpdateFunctor {
160160
mutable NearestNodeFinder::Ptr nn_;
161161
};
162162

163-
void declare_config(UpdateTraversabilityFunctor::Config& config);
163+
void declare_config(UpdateBlockTraversabilityFunctor::Config& config);
164164

165165
} // namespace hydra

‎include/hydra/backend/update_region_growing_traversability_functor.h‎

Lines changed: 140 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,140 @@
1+
/* -----------------------------------------------------------------------------
2+
* Copyright 2022 Massachusetts Institute of Technology.
3+
* All Rights Reserved
4+
*
5+
* Redistribution and use in source and binary forms, with or without
6+
* modification, are permitted provided that the following conditions are met:
7+
*
8+
* 1. Redistributions of source code must retain the above copyright notice,
9+
* this list of conditions and the following disclaimer.
10+
*
11+
* 2. Redistributions in binary form must reproduce the above copyright notice,
12+
* this list of conditions and the following disclaimer in the documentation
13+
* and/or other materials provided with the distribution.
14+
*
15+
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
16+
* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
17+
* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
18+
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
19+
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
20+
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
21+
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
22+
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
23+
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
24+
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
25+
*
26+
* Research was sponsored by the United States Air Force Research Laboratory and
27+
* the United States Air Force Artificial Intelligence Accelerator and was
28+
* accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views
29+
* and conclusions contained in this document are those of the authors and should
30+
* not be interpreted as representing the official policies, either expressed or
31+
* implied, of the United States Air Force or the U.S. Government. The U.S.
32+
* Government is authorized to reproduce and distribute reprints for Government
33+
* purposes notwithstanding any copyright notation herein.
34+
* -------------------------------------------------------------------------- */
35+
#pragma once
36+
37+
#include "hydra/backend/deformation_interpolator.h"
38+
#include "hydra/backend/update_functions.h"
39+
#include "hydra/utils/logging.h"
40+
#include "hydra/utils/nearest_neighbor_utilities.h"
41+
42+
namespace hydra {
43+
44+
/**
45+
* @brief Functor to update traversability places in the DSG. This functor should be
46+
* called with exhaustive merging enabled.
47+
*/
48+
struct UpdateRegionGrowingTraversabilityFunctor : public UpdateFunctor {
49+
struct Config : public VerbosityConfig {
50+
//! Layer to update traversability in
51+
std::string layer = DsgLayers::TRAVERSABILITY;
52+
53+
DeformationInterpolator::Config deformation;
54+
} const config;
55+
56+
using EdgeSet = std::set<EdgeKey>;
57+
using NodeSet = std::set<NodeId>;
58+
using State = spark_dsg::TraversabilityState;
59+
60+
explicit UpdateRegionGrowingTraversabilityFunctor(const Config& config);
61+
62+
Hooks hooks() const override;
63+
64+
void call(const DynamicSceneGraph& unmerged,
65+
SharedDsgInfo& dsg,
66+
const UpdateInfo::ConstPtr& info) const override;
67+
68+
protected:
69+
// Hook callbacks.
70+
MergeList findNodeMerges(const DynamicSceneGraph& dsg,
71+
const UpdateInfo::ConstPtr& info) const;
72+
73+
NodeAttributes::Ptr mergeNodes(const DynamicSceneGraph& dsg,
74+
const std::vector<NodeId>& merge_ids) const;
75+
76+
void cleanup(const UpdateInfo::ConstPtr& /* info */, SharedDsgInfo* /* dsg */) const;
77+
78+
// Processing Steps.
79+
/**
80+
* @brief Update the positions of all traversability nodes in the DSG. Propagates to
81+
* the complete DSG in case of new loop closures.
82+
*/
83+
void updateDeformation(const DynamicSceneGraph& unmerged,
84+
SharedDsgInfo& dsg,
85+
const UpdateInfo::ConstPtr& info) const;
86+
87+
/**
88+
* @brief Remove all active window and inactive edges from the graph.
89+
*/
90+
void resetAddedEdges(DynamicSceneGraph& dsg) const;
91+
92+
/**
93+
* @brief Compute edges between overlapping inactive nodes globally.
94+
*/
95+
void findInactiveEdges(DynamicSceneGraph& dsg) const;
96+
97+
/**
98+
* @brief Find and add edges from active to inactive nodes.
99+
*/
100+
void findActiveWindowEdges(DynamicSceneGraph& dsg) const;
101+
102+
/**
103+
* @brief Remove active window edges that no longer have active overlap and move
104+
* designate archived ones as inactive edges.
105+
*/
106+
void pruneActiveWindowEdges(DynamicSceneGraph& dsg) const;
107+
108+
// Helper functions.
109+
110+
/**
111+
* @brief Find places that are inactive and spatially but not temporally overlap with
112+
* the given node.
113+
*/
114+
std::vector<NodeId> findConnections(const DynamicSceneGraph& dsg,
115+
const TravNodeAttributes& from_attrs) const;
116+
117+
/**
118+
* @brief Check if two traversability nodes have active window (temporal) overlap.
119+
*/
120+
static bool hasActiveOverlap(const TravNodeAttributes& attrs1,
121+
const TravNodeAttributes& attrs2);
122+
123+
/**
124+
* @brief View on all active nodes in a layer.
125+
*/
126+
static LayerView activeNodes(const SceneGraphLayer& layer);
127+
128+
protected:
129+
// Members.
130+
const DeformationInterpolator deformation_interpolator_;
131+
132+
// State.
133+
mutable EdgeSet active_edges_; // Active window edges in the current update.
134+
mutable EdgeSet merge_candidates_; // List of nodes that have inactive edges and
135+
// could thus be merged this iteration.
136+
};
137+
138+
void declare_config(UpdateRegionGrowingTraversabilityFunctor::Config& config);
139+
140+
} // namespace hydra

‎include/hydra/places/region_growing_traversability_clustering.h‎

Lines changed: 198 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,198 @@
1+
/* -----------------------------------------------------------------------------
2+
* Copyright 2022 Massachusetts Institute of Technology.
3+
* All Rights Reserved
4+
*
5+
* Redistribution and use in source and binary forms, with or without
6+
* modification, are permitted provided that the following conditions are met:
7+
*
8+
* 1. Redistributions of source code must retain the above copyright notice,
9+
* this list of conditions and the following disclaimer.
10+
*
11+
* 2. Redistributions in binary form must reproduce the above copyright notice,
12+
* this list of conditions and the following disclaimer in the documentation
13+
* and/or other materials provided with the distribution.
14+
*
15+
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
16+
* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
17+
* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
18+
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
19+
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
20+
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
21+
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
22+
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
23+
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
24+
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
25+
*
26+
* Research was sponsored by the United States Air Force Research Laboratory and
27+
* the United States Air Force Artificial Intelligence Accelerator and was
28+
* accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views
29+
* and conclusions contained in this document are those of the authors and should
30+
* not be interpreted as representing the official policies, either expressed or
31+
* implied, of the United States Air Force or the U.S. Government. The U.S.
32+
* Government is authorized to reproduce and distribute reprints for Government
33+
* purposes notwithstanding any copyright notation herein.
34+
* -------------------------------------------------------------------------- */
35+
#pragma once
36+
37+
#include <config_utilities/factory.h>
38+
#include <spark_dsg/traversability_boundary.h>
39+
40+
#include <map>
41+
#include <optional>
42+
#include <utility>
43+
#include <vector>
44+
45+
#include "hydra/places/traversability_clustering.h"
46+
47+
namespace hydra::places {
48+
49+
/**
50+
* @brief Simple clustering that assigns places by block.
51+
*/
52+
class RegionGrowingTraversabilityClustering : public TraversabilityClustering {
53+
public:
54+
struct Config {
55+
//! Maximum radius of a place [m].
56+
float max_radius = 3.0f;
57+
58+
//! Number of rays to consider for boundary computation.
59+
int num_orientation_bins = 16;
60+
} const config;
61+
62+
using Voxels = VoxelIndices;
63+
using VoxelSet = VoxelIndexSet;
64+
using VoxelMap = VoxelIndexMap<spark_dsg::NodeId>;
65+
66+
struct Region {
67+
// ID of the region. This is also the node ID in the DSG.
68+
spark_dsg::NodeId id;
69+
70+
// VoxelSet assigned to this region. These can be in the current layer or outside.
71+
VoxelSet voxels;
72+
73+
// Voxels bordering the voxels of this region. These are the exterior boundary of
74+
// the region and are ordered by traversal.
75+
VoxelSet exterior_boundary;
76+
77+
// Unobserved or intraversable voxels inside the region.
78+
VoxelSet interior_boundary;
79+
80+
// Archived boundary voxels with their last known traversability state.
81+
VoxelIndexMap<spark_dsg::TraversabilityState> archived_boundary;
82+
83+
// True: this region has voxels in the current layer. False: all voxels are outside
84+
// the current layer.
85+
bool is_active = true;
86+
87+
// Centroid of the region.
88+
Eigen::Vector3f centroid;
89+
90+
// Axis-aligned bounding box of the region in voxel coordinates.
91+
Eigen::Vector2i min_coordinates;
92+
Eigen::Vector2i max_coordinates;
93+
94+
// Regions connecting to this one. <region id, num connecting voxels>
95+
std::map<spark_dsg::NodeId, int> neighbors;
96+
97+
// Merge other into this region.
98+
void merge(const Region& other);
99+
100+
/**
101+
* @brief Compute the ordered exterior boundary, min/max extents, and centroid.
102+
*/
103+
void computeBoundary();
104+
105+
/**
106+
* @brief Compute region boundaries, extents, centroids, and compute the neighbors
107+
* from that.
108+
*/
109+
void computeNeighbors(const VoxelMap& assigned_voxels);
110+
};
111+
112+
RegionGrowingTraversabilityClustering(const Config& config);
113+
~RegionGrowingTraversabilityClustering() = default;
114+
115+
void updateGraph(const TraversabilityLayer& layer,
116+
const ActiveWindowOutput& msg,
117+
spark_dsg::DynamicSceneGraph& graph) override;
118+
119+
protected:
120+
size_t current_id_ = 0;
121+
uint64_t current_time_ns_ = 0;
122+
int max_region_size_ = 0; // Cached region size in voxels.
123+
124+
// <id, region>
125+
std::map<spark_dsg::NodeId, Region> regions_;
126+
127+
// Processing steps.
128+
/**
129+
* @brief Prune existing regions by removing all voxels that are no longer
130+
* traversable. Regions without any remaining voxels are marked as inactive.
131+
* @return Map of all inactive assigned voxels to region IDs.
132+
// */
133+
VoxelMap initializeRegions(const TraversabilityLayer& layer,
134+
const VoxelSet& all_voxels);
135+
136+
/**
137+
* @brief Initialize all connected traversable voxels from the starting position of
138+
* the robot.
139+
*/
140+
VoxelSet initializeVoxels(const TraversabilityLayer& layer,
141+
const Eigen::Vector3d& start_position) const;
142+
143+
/**
144+
* @brief Assign all traversable voxels by assigning them to closest existing regions
145+
* and assigning new ones where needed.
146+
*/
147+
void growRegions(VoxelSet& all_voxels, VoxelMap& assigned_voxels);
148+
149+
/**
150+
* @brief Merge all regions that don't exceed the max size.
151+
*/
152+
void mergeRegions(const VoxelMap& assigned_voxels,
153+
spark_dsg::DynamicSceneGraph& graph);
154+
155+
void updatePlaceNodesInDsg(spark_dsg::DynamicSceneGraph& graph,
156+
const TraversabilityLayer& layer);
157+
158+
void updatePlaceEdgesInDsg(spark_dsg::DynamicSceneGraph& graph) const;
159+
160+
void visualizeAssignments(const TraversabilityLayer& layer,
161+
const VoxelMap& assigned_voxels) const;
162+
163+
void pruneRegions();
164+
165+
/**
166+
* @brief Allocate a new region, keeping track of the IDs. ID 0 is reserved.
167+
*/
168+
Region& allocateNewRegion();
169+
170+
/**
171+
* @brief Breadth-first search to grow a region from a seed index.
172+
*/
173+
static VoxelSet growRegion(
174+
const VoxelSet& candidates,
175+
const VoxelIndex& seed_index,
176+
std::function<bool(const VoxelIndex&)> condition = [](const VoxelIndex&) {
177+
return true;
178+
});
179+
180+
void updatePlaceNodeAttributes(spark_dsg::TravNodeAttributes& attrs,
181+
Region& region,
182+
const TraversabilityLayer& layer) const;
183+
184+
inline static const std::array<VoxelIndex, 8> neighbors_ = {
185+
VoxelIndex(0, -1, 0), // bottom
186+
VoxelIndex(-1, 0, 0), // left
187+
VoxelIndex(0, 1, 0), // top
188+
VoxelIndex(1, 0, 0), // right
189+
VoxelIndex(-1, -1, 0), // bottom-left
190+
VoxelIndex(-1, 1, 0), // top-left
191+
VoxelIndex(1, 1, 0), // top-right
192+
VoxelIndex(1, -1, 0) // bottom-right
193+
};
194+
};
195+
196+
void declare_config(RegionGrowingTraversabilityClustering::Config& config);
197+
198+
} // namespace hydra::places

0 commit comments

Comments
 (0)