Skip to content

Commit

Permalink
OccupancyOcTreeBase: add setTreeValues()
Browse files Browse the repository at this point in the history 
Add a method to set many values at once from another tree.

Add a bounds-limited version of this method as well to make updating
portions of a tree from another tree possible.

Provide a method argument, maximum_only, to limit the set values to
only the maximum of the value stored in the tree being update and the
values in the other tree.

Provide a method argument, delete_first, to delete any regions to be
updated from this tree first. This argument is useful when using a
bounds-limiting tree to update a tree from the parts of another tree
that have changed.
  • Loading branch information
@c-andy-martin
C. Andy Martin authored and c-andy-martin committed May 29, 2024
1 parent f594b4e commit 3aeaea3
Show file tree
Hide file tree
Showing 4 changed files with 977 additions and 8 deletions.
74 changes: 74 additions & 0 deletions octomap/include/octomap/OccupancyOcTreeBase.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,7 @@
#include <list>
#include <stdlib.h>
#include <vector>
#include <functional>

#include "octomap_types.h"
#include "octomap_utils.h"
Expand Down Expand Up @@ -145,6 +146,56 @@ namespace octomap {
*/
virtual void insertPointCloudRays(const Pointcloud& scan, const point3d& sensor_origin, double maxrange = -1., bool lazy_eval = false);

/** Used to copy values in setTreeValues */
using CopyValueFunction = std::function<void(const NODE*, NODE*, bool, const OcTreeKey&, unsigned int)>;

/**
* Set this tree's values from another tree.
*
* Note: this operation does not currently work with change detection.
* It would be very inefficient to push potentially millions of
* OcTreeKeys into a KeySet.
*
* @param value_tree OccupancyOcTree containing the values to set.
* @param maximum_only Only set a value from value_tree if it is bigger
* than what is in our tree.
* @param delete_first Deletes this tree first.
*/
virtual void setTreeValues(const OccupancyOcTreeBase<NODE>* value_tree,
bool maximum_only = false,
bool delete_first = false,
CopyValueFunction copy_value_function = CopyValueFunction());

/**
* Set this tree's values from another tree, using a third bounds tree.
* Only modifies the contents of this tree from the value_tree where the
* bounds_tree is defined. An application of this function is to update
* this tree from an accumulated update tree as the values tree and a
* separate tree that represents the bounding boxes of all the regions
* that have changed. Node pruning makes an OcTree an efficent mechanism
* to specify all the regions of interest. It is not possible to represent
* regions that should be erased in an OccupancyOcTreeBase with
* OcTreeDataNode's. Also, while a KeySet may seem a natural way of
* specifying the bounds, a KeySet only represents leaves at the maximum
* depth and has no opportunity for pruning nearby update regions.
*
* Note: this operation does not currently work with change detection.
* It would be very inefficient to push potentially millions of
* OcTreeKeys into a KeySet.
*
* @param value_tree OccupancyOcTree containing the values to set
* @param bounds_tree OccupancyOcTree indicating bounds for setting
* @param maximum_only Only set a value from value_tree if it is bigger
* than what is in our tree.
* @param delete_first Deletes nodes inside the regions defined by the
* bounds_tree first.
*/
virtual void setTreeValues(const OccupancyOcTreeBase<NODE>* value_tree,
const OccupancyOcTreeBase<NODE>* bounds_tree,
bool maximum_only = false,
bool delete_first = false,
CopyValueFunction copy_value_function = CopyValueFunction());

/**
* Set log_odds value of voxel to log_odds_value. This only works if key is at the lowest
* octree level
Expand Down Expand Up @@ -500,6 +551,29 @@ namespace octomap {
unsigned int current_depth, unsigned int target_depth,
float log_odds_value, bool lazy_eval = false);

// recursive update values from other tree with bounds
NODE* setTreeValuesRecurs(NODE* node,
bool node_just_created,
const OcTreeKey& key,
unsigned int depth,
const OccupancyOcTreeBase<NODE>* value_tree,
const OccupancyOcTreeBase<NODE>* bounds_tree,
const NODE* value_node,
const NODE* bounds_node,
bool maximum_only,
bool delete_first,
const CopyValueFunction& copy_value_function);

// recursive update values from other tree
void setTreeValuesRecurs(NODE* node,
bool node_just_created,
const OcTreeKey& key,
unsigned int depth,
const OccupancyOcTreeBase<NODE>* value_tree,
const NODE* value_node,
bool maximum_only,
const CopyValueFunction& copy_value_function);

void updateInnerOccupancyRecurs(NODE* node, unsigned int depth);

void toMaxLikelihoodRecurs(NODE* node, unsigned int depth, unsigned int max_depth);
Expand Down
288 changes: 280 additions & 8 deletions octomap/include/octomap/OccupancyOcTreeBase.hxx
View file
Original file line number Diff line number Diff line change
Expand Up @@ -525,14 +525,7 @@ namespace octomap {
bool created_node = false;
if (current_depth == target_depth || current_depth == this->tree_depth) {
// At the target node (or end of tree). Delete all children and set the log odds
if (this->nodeHasChildren(node)) {
for (unsigned int i=0; i<8; i++) {
if (this->nodeChildExists(node, i)) {
this->deleteNodeRecurs(this->getNodeChild(node, i));
this->setNodeChild(node, i, NULL);
}
}
}
this->deleteNodeChildren(node);
node->setLogOdds(log_odds_value);
return node;
}
Expand Down Expand Up @@ -566,6 +559,285 @@ namespace octomap {
return retval;
}

template <class NODE>
void OccupancyOcTreeBase<NODE>::setTreeValues(const OccupancyOcTreeBase<NODE>* value_tree,
bool maximum_only, bool delete_first,
CopyValueFunction copy_value_function){
setTreeValues(value_tree, NULL, maximum_only, delete_first, copy_value_function);
}

template <class NODE>
void OccupancyOcTreeBase<NODE>::setTreeValues(const OccupancyOcTreeBase<NODE>* value_tree,
const OccupancyOcTreeBase<NODE>* bounds_tree,
bool maximum_only, bool delete_first,
CopyValueFunction copy_value_function){
octomap::OcTreeKey root_key(this->tree_max_val, this->tree_max_val, this->tree_max_val);

// delete_first implies maximum_only = false.
if (delete_first)
maximum_only = false;

bool created_root = false;
if (this->root == NULL)
{
this->root = new NODE();
this->tree_size++;
created_root = true;
}

const NODE* value_node = NULL;
if (value_tree != NULL) {
value_node = value_tree->root;
}

const NODE* bounds_node = NULL;
if (bounds_tree != NULL) {
bounds_node = bounds_tree->root;
}

this->root = setTreeValuesRecurs(this->root,
created_root,
root_key,
0,
value_tree,
bounds_tree,
value_node,
bounds_node,
maximum_only,
delete_first,
copy_value_function);
}

template <class NODE>
NODE* OccupancyOcTreeBase<NODE>::setTreeValuesRecurs(NODE* node,
bool node_just_created,
const OcTreeKey& key,
unsigned int depth,
const OccupancyOcTreeBase<NODE>* value_tree,
const OccupancyOcTreeBase<NODE>* bounds_tree,
const NODE* value_node,
const NODE* bounds_node,
bool maximum_only,
bool delete_first,
const CopyValueFunction& copy_value_function){
if (bounds_tree != NULL && bounds_node == NULL) {
// We are out-of-bounds.
// Must check this first to not attempt referencing bounds_node
// Be sure to delete a newly-created node, as nothing will be added to it.
if (node != NULL && node_just_created) {
this->deleteNodeRecurs(node);
node = NULL;
}
return node;
}
if (bounds_tree != NULL && bounds_tree->nodeHasChildren(bounds_node)) {
// We aren't yet in a leaf of the bounds tree, descend
// First expand if our node is a pruned leaf.
if (!this->nodeHasChildren(node) && !node_just_created) {
// Our node is a leaf. Expand it as we will need
// to update some of the sub-nodes.
this->expandNode(node);
}
for (unsigned int i=0; i<8; ++i) {
if (bounds_tree->nodeChildExists(bounds_node, i)) {
const NODE* value_child = NULL;
if (value_node) {
if (!value_tree->nodeHasChildren(value_node)) {
// The value node is a leaf, just keep it the same as we
// descend, as all we need is its value to set.
value_child = value_node;
} else if (value_tree->nodeChildExists(value_node, i)) {
value_child = value_tree->getNodeChild(value_node, i);
}
}
// If the value child node is NULL, there is nothing left to do,
// unless delete_first is set.
if (delete_first || value_child != NULL) {
bool created_node = false;
if (!this->nodeChildExists(node, i)) {
// The child does not exist. Create a child if appropriate.
if (delete_first && value_child == NULL) {
// There is no point in creating a node that will certainly
// be deleted. Do nothing.
} else {
this->createNodeChild(node, i);
created_node = true;
}
}
// It is possible to still have no child in the case that we are
// set to delete_first and the value tree is empty here. In such a
// case, there is no point in creating nodes just to delete them.
if (this->nodeChildExists(node, i)) {
key_type center_offset_key = this->tree_max_val >> (depth + 1);
OcTreeKey child_key;
computeChildKey(i, center_offset_key, key, child_key);
NODE* rv = setTreeValuesRecurs(this->getNodeChild(node, i),
created_node,
child_key,
depth + 1,
value_tree,
bounds_tree,
value_child,
bounds_tree->getNodeChild(bounds_node, i),
maximum_only,
delete_first,
copy_value_function);
this->setNodeChild(node, i, rv);
}
}
}
}
if (node != NULL && !this->nodeHasChildren(node)) {
// We didn't end up with any children. Since we haven't pruned this
// node yet, this means we lost any children we had to start with.
// Delete.
this->deleteNodeRecurs(node);
node = NULL;
}
} else {
// we are inside the bounds of the update
if (delete_first) {
if (value_node == NULL) {
// There are no values to copy over, delete this node completely.
this->deleteNodeRecurs(node);
node = NULL;
} else {
// There is at least some value to copy over.
// Delete all our children so they can be replaced by the values.
this->deleteNodeChildren(node);
// Be sure to set node_just_created as this indicates that the given
// node is not a pruned leaf, even though it has no children.
node_just_created = true;
}
}
setTreeValuesRecurs(node, node_just_created, key, depth, value_tree, value_node, maximum_only, copy_value_function);
}
if (node != NULL && node_just_created && !this->nodeHasChildren(node) && value_node == NULL) {
// We are not a leaf node and didn't end up with any children after being newly created. Delete.
this->deleteNodeRecurs(node);
node = NULL;
}
if (node != NULL) {
// If our node is still alive, and has children, attempt a prune, and
// then update our occupancy if we didn't prune our kids.
if (this->nodeHasChildren(node)) {
if (!this->pruneNode(node)) {
node->updateOccupancyChildren();
}
}
}
return node;
}

template <class NODE>
void OccupancyOcTreeBase<NODE>::setTreeValuesRecurs(NODE* node,
bool node_just_created,
const OcTreeKey& key,
unsigned int depth,
const OccupancyOcTreeBase<NODE>* value_tree,
const NODE* value_node,
bool maximum_only,
const CopyValueFunction& copy_value_function){
if (node == NULL || value_tree == NULL || value_node == NULL)
return;
key_type center_offset_key = this->tree_max_val >> (depth + 1);
OcTreeKey child_key;
if (!value_tree->nodeHasChildren(value_node)) {
// The value node has no children (its a leaf)
if (!this->nodeHasChildren(node)) {
// both nodes are leafs
if (node_just_created || !maximum_only || value_node->getValue() > node->getValue()) {
// Update our node if it was just created (which means its empty),
// or if we are not setting only maximum values, or if the value
// node is a higher log odds than this node. Otherwise, there is
// nothing to set.
if (copy_value_function) {
copy_value_function(value_node, node, node_just_created, key, depth);
} else {
node->copyData(*value_node);
}
}
} else {
// The value node is a leaf, but we are not.
if (!maximum_only) {
// We are updating all values, not just maximum. Delete our children
// and copy the value node's data into our node.
this->deleteNodeChildren(node);
if (copy_value_function) {
copy_value_function(value_node, node, node_just_created, key, depth);
} else {
node->copyData(*value_node);
}
} else {
// Descend our tree, but keep using the node pointer from the
// value tree's leaf to get its log odds value. It is necessary
// to descend. Consider, we may have log odds -1.0 and 2.0 in
// our sub tree, and the value node may point to a leaf with
// value of 1.0. Since we are using maximum only, we must only
// change our log odds values that are smaller.
for (unsigned int i=0; i<8; ++i) {
if (this->nodeChildExists(node, i)) {
// In this case the value node is already at a leaf.
// This value node leaf is OK to pass down the recursion, as all
// we need is its value.
computeChildKey(i, center_offset_key, key, child_key);
setTreeValuesRecurs(this->getNodeChild(node, i),
false,
child_key,
depth + 1,
value_tree,
value_node,
true,
copy_value_function);
} else {
// The value node has a value for this space, but we don't have
// a child for it. Create a leaf node for our child and assign
// it the value.
this->createNodeChild(node, i);
NODE* child_node = this->getNodeChild(node, i);
if (copy_value_function) {
copy_value_function(value_node, child_node, node_just_created, key, depth);
} else {
child_node->copyData(*value_node);
}
}
}
}
}
} else {
// The value node has children. We must descend.
for (unsigned int i=0; i<8; ++i) {
if (value_tree->nodeChildExists(value_node, i)) {
bool created_node = false;
if (!this->nodeChildExists(node, i)) {
if (!this->nodeHasChildren(node) && !node_just_created) {
// Our node is a leaf. Expand it so we can update indivdual
// leaves as we go.
this->expandNode(node);
} else {
this->createNodeChild(node, i);
created_node = true;
}
}
computeChildKey(i, center_offset_key, key, child_key);
setTreeValuesRecurs(this->getNodeChild(node, i),
created_node,
child_key,
depth + 1,
value_tree,
value_tree->getNodeChild(value_node, i),
maximum_only,
copy_value_function);
}
}
}
if (this->nodeHasChildren(node)) {
if (!this->pruneNode(node)) {
node->updateOccupancyChildren();
}
}
}

template <class NODE>
void OccupancyOcTreeBase<NODE>::updateInnerOccupancy(){
if (this->root)
Expand Down
Loading

0 comments on commit 3aeaea3

Please sign in to comment.