diff --git a/octomap/CMakeLists.txt b/octomap/CMakeLists.txt index 7ed758ab..7c8a8367 100644 --- a/octomap/CMakeLists.txt +++ b/octomap/CMakeLists.txt @@ -59,6 +59,7 @@ set(octomap_SOURCES src/OcTreeNode.cpp src/OcTreeStamped.cpp src/ColorOcTree.cpp + src/IntensityOcTree.cpp ) add_library(octomap ${octomap_SOURCES}) target_compile_features(octomap PUBLIC cxx_std_11) diff --git a/octomap/include/octomap/IntensityOcTree.h b/octomap/include/octomap/IntensityOcTree.h new file mode 100644 index 00000000..92aefc89 --- /dev/null +++ b/octomap/include/octomap/IntensityOcTree.h @@ -0,0 +1,202 @@ +/* + * OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees + * https://octomap.github.io/ + * + * Copyright (c) 2009-2013, K.M. Wurm and A. Hornung, University of Freiburg + * All rights reserved. + * License: New BSD + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the University of Freiburg nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef OCTOMAP_INTENSITY_OCTREE_H +#define OCTOMAP_INTENSITY_OCTREE_H + +#include +#include + +#include +#include + +namespace octomap +{ + +// forward declaration for "friend" +class IntensityOcTree; + +// node definition +class IntensityOcTreeNode : public OcTreeNode +{ +public: + friend class IntensityOcTree;// needs access to node children (inherited) + + // Constructors + IntensityOcTreeNode() : OcTreeNode(), intensity(0.0) + { + } + + IntensityOcTreeNode(const IntensityOcTreeNode &rhs) + : OcTreeNode(rhs), intensity(rhs.intensity) + { + } + + // Comparator + bool operator==(const IntensityOcTreeNode &rhs) const + { + return (rhs.value == value && + std::fabs(rhs.intensity - intensity) < ZERO_EPSILON); + } + + // Payload helpers + inline double getIntensity() const + { + return intensity; + } + inline void setIntensity(double i) + { + intensity = i; + } + + // Copy when replacing a node + void copyData(const IntensityOcTreeNode &from) + { + OcTreeNode::copyData(from); + this->intensity = from.getIntensity(); + } + + // Update occupancy and intensity of inner nodes + /** + * @brief Update this node's intensity from its children. + * + * The intensity of this node is set to the average intensity of its + * children whose intensity is considered set (see isIntensitySet()). + */ + inline void updateIntensityChildren(); + /** + * @brief Compute the mean intensity of all children with intensity set. + * + * Only children for which isIntensitySet() returns true are taken into + * account. If none of the children have intensity set, this function + * returns 0.0. + * + * @return The average intensity of all children with intensity set, + * or 0.0 if no such children exist. + */ + double getAverageChildIntensity() const; + inline bool isIntensitySet() const + { + // Intensity is treated as non-negative; near-zero values are considered "unset". + return (ZERO_EPSILON < intensity); + } + + // Serialization and deserialization + std::istream &readData(std::istream &s); + std::ostream &writeData(std::ostream &s) const; + +protected: + double intensity; + static constexpr double ZERO_EPSILON = 1e-6;// threshold for checking if intensity has been set +}; + +// tree definition +class IntensityOcTree : public OccupancyOcTreeBase +{ +public: + // Default constructor, sets resolution of leafs + IntensityOcTree(double resolution); + + /// virtual constructor: creates a new object of same type + /// (Covariant return type requires an up-to-date compiler) + IntensityOcTree *create() const override + { + return new IntensityOcTree(resolution); + } + + std::string getTreeType() const + { + return "IntensityOcTree"; + } + + /** + * Prunes a node when it is collapsible. This overloaded + * version only considers the node occupancy for pruning, + * different intensities of child nodes are ignored. + * @return true if pruning was successful + */ + virtual bool pruneNode(IntensityOcTreeNode *node); + + // Compare only occupancy for pruning, ignoring intensity. + virtual bool isNodeCollapsible(const IntensityOcTreeNode *node) const; + + void updateInnerOccupancy(); + + // Compatibility wrapper that forwards to computeUpdate with a point3d origin. + void computeUpdateKeys(const octomap::Pointcloud &scan, + const octomath::Vector3 &origin, + octomap::KeySet &free_cells, + octomap::KeySet &occupied_cells, + double maxrange = -1.0); + + IntensityOcTreeNode *integrateNodeIntensity(const OcTreeKey &key, + double intensity); + IntensityOcTreeNode *integrateNodeIntensity(float x, float y, float z, + double intensity); + +protected: + void updateInnerOccupancyRecurs(IntensityOcTreeNode *node, unsigned depth); + + /** + * Static member object which ensures that this OcTree's prototype + * ends up in the classIDMapping only once. You need this as a + * static member in any derived octree class in order to read .ot + * files through the AbstractOcTree factory. You should also call + * ensureLinking() once from the constructor. + */ + class StaticMemberInitializer + { + public: + StaticMemberInitializer() + { + IntensityOcTree *tree = new IntensityOcTree(0.1); + tree->clearKeyRays(); + AbstractOcTree::registerTreeType(tree); + } + /** + * Dummy function to ensure that MSVC does not drop the + * StaticMemberInitializer, causing this tree failing to register. + * Needs to be called from the constructor of this octree. + */ + void ensureLinking() + { + } + }; + + /// to ensure static initialization (only once) + static StaticMemberInitializer intensityOcTreeMemberInit; +}; + +}// namespace octomap + +#endif// OCTOMAP_INTENSITY_OCTREE_H diff --git a/octomap/src/IntensityOcTree.cpp b/octomap/src/IntensityOcTree.cpp new file mode 100644 index 00000000..27c7ad80 --- /dev/null +++ b/octomap/src/IntensityOcTree.cpp @@ -0,0 +1,218 @@ +/* + * OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees + * https://octomap.github.io/ + * + * Copyright (c) 2009-2013, K.M. Wurm and A. Hornung, University of Freiburg + * All rights reserved. + * License: New BSD + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the University of Freiburg nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "octomap/IntensityOcTree.h" + +namespace octomap +{ +// Node implementation +std::istream &IntensityOcTreeNode::readData(std::istream &s) +{ + s.read((char *) &value, sizeof(value));// occupancy + s.read(reinterpret_cast(&intensity), sizeof(intensity)); + + return s; +} + +std::ostream &IntensityOcTreeNode::writeData(std::ostream &s) const +{ + s.write((const char *) &value, sizeof(value));// occupancy + s.write(reinterpret_cast(&intensity), sizeof(intensity)); + + return s; +} + +double IntensityOcTreeNode::getAverageChildIntensity() const +{ + double sum = 0.0; + unsigned cnt = 0; + + if (children != NULL) + { + for (unsigned i = 0; i < 8; ++i) + { + IntensityOcTreeNode *child = + static_cast(children[i]); + + if (child != NULL && child->isIntensitySet()) + { + sum += child->getIntensity(); + ++cnt; + } + } + } + + // If no children have intensity set, return a neutral default (0.0) + // instead of reusing the parent node's current intensity. + if (cnt > 0) + { + return sum / static_cast(cnt); + } + else + { + return 0.0; + } +} + +void IntensityOcTreeNode::updateIntensityChildren() +{ + intensity = getAverageChildIntensity(); +} + +// Tree implementation +IntensityOcTree::IntensityOcTree(double resolution) + : OccupancyOcTreeBase(resolution) +{ + intensityOcTreeMemberInit.ensureLinking(); +} + + +void IntensityOcTree::updateInnerOccupancy() +{ + this->updateInnerOccupancyRecurs(this->root, 0); +} + +void IntensityOcTree::updateInnerOccupancyRecurs(IntensityOcTreeNode *node, + unsigned depth) +{ + if (nodeHasChildren(node)) + { + if (depth < this->tree_depth) + { + for (unsigned i = 0; i < 8; ++i) + { + if (nodeChildExists(node, i)) + updateInnerOccupancyRecurs(getNodeChild(node, i), + depth + 1); + } + } + node->updateOccupancyChildren(); + node->updateIntensityChildren(); + } +} + + +void IntensityOcTree::computeUpdateKeys(const octomap::Pointcloud &scan, + const octomath::Vector3 &origin, + octomap::KeySet &free_cells, + octomap::KeySet &occupied_cells, + double maxrange) +{ + // Delegates to base class + computeUpdate(scan, octomap::point3d(origin.x(), origin.y(), origin.z()), + free_cells, occupied_cells, maxrange); +} + +// Node Pruning +bool IntensityOcTree::pruneNode(IntensityOcTreeNode *node) +{ + if (!isNodeCollapsible(node)) + return false; + + // set value to children's values (all assumed equal) + node->copyData(*(getNodeChild(node, 0))); + + // update intensity to represent the average of all children + node->setIntensity(node->getAverageChildIntensity()); + + // delete children + for (unsigned int i = 0; i < 8; i++) + { + deleteNodeChild(node, i); + } + delete[] node->children; + node->children = NULL; + + return true; +} + +bool IntensityOcTree::isNodeCollapsible(const IntensityOcTreeNode *node) const +{ + // All children must exist, be leafs, and match in occupancy (ignore intensity). + if (!nodeChildExists(node, 0)) + return false; + + const IntensityOcTreeNode *firstChild = getNodeChild(node, 0); + if (nodeHasChildren(firstChild)) + return false; + + for (unsigned int i = 1; i < 8; ++i) + { + if (!nodeChildExists(node, i) || + nodeHasChildren(getNodeChild(node, i)) || + !(getNodeChild(node, i)->getValue() == firstChild->getValue())) + { + return false; + } + } + + return true; +} + + +// Integration +IntensityOcTreeNode * +IntensityOcTree::integrateNodeIntensity(const OcTreeKey &key, double intensity) +{ + IntensityOcTreeNode *n = search(key); + if (n != 0) + { + if (n->isIntensitySet()) + { + double prev_intensity = n->getIntensity(); + double node_prob = n->getOccupancy(); + + double new_intensity = (prev_intensity * node_prob + + intensity * (1.0 - node_prob)); + n->setIntensity(new_intensity); + } + else + { + n->setIntensity(intensity); + } + } + return n; +} + +IntensityOcTreeNode *IntensityOcTree::integrateNodeIntensity(float x, float y, + float z, + double intensity) +{ + OcTreeKey key; + if (!this->coordToKeyChecked(point3d(x, y, z), key)) + return NULL; + return integrateNodeIntensity(key, intensity); +} + +IntensityOcTree::StaticMemberInitializer IntensityOcTree::intensityOcTreeMemberInit; +}// namespace octomap diff --git a/octomap/src/testing/CMakeLists.txt b/octomap/src/testing/CMakeLists.txt index 4ca72f8b..832c248f 100644 --- a/octomap/src/testing/CMakeLists.txt +++ b/octomap/src/testing/CMakeLists.txt @@ -16,6 +16,9 @@ target_link_libraries(test_scans octomap) add_executable(test_color_tree test_color_tree.cpp) target_link_libraries(test_color_tree octomap) +add_executable(test_intensity_tree test_intensity_tree.cpp) +target_link_libraries(test_intensity_tree octomap) + add_executable(color_tree_histogram color_tree_histogram.cpp) target_link_libraries(color_tree_histogram octomap) @@ -57,6 +60,7 @@ add_test(NAME test_mapcollection ${PROJECT_SOURCE_DIR}/share/data/mapcoll.txt ) add_test(NAME test_color_tree COMMAND test_color_tree) +add_test(NAME test_intensity_tree COMMAND test_intensity_tree) add_test(NAME test_bbx COMMAND test_bbx) set_tests_properties(ReadGraph PROPERTIES DEPENDS InsertScan) diff --git a/octomap/src/testing/test_intensity_tree.cpp b/octomap/src/testing/test_intensity_tree.cpp new file mode 100644 index 00000000..a0ad1d34 --- /dev/null +++ b/octomap/src/testing/test_intensity_tree.cpp @@ -0,0 +1,83 @@ +#include +#include + +#include +#include "testing.h" + +using namespace std; +using namespace octomap; + +int main(int /*argc*/, char** /*argv*/) { + double res = 0.1; + IntensityOcTree tree(res); + EXPECT_EQ(tree.getTreeType(), "IntensityOcTree"); + + tree.setProbHit(0.8); + point3d p(0.0f, 0.0f, 0.0f); + IntensityOcTreeNode* node = tree.updateNode(p, true); + EXPECT_TRUE(node); + double node_prob = node->getOccupancy(); + EXPECT_NEAR(node_prob, 0.8, 1e-5); + EXPECT_FALSE(node->isIntensitySet()); + + EXPECT_TRUE(tree.integrateNodeIntensity(p.x(), p.y(), p.z(), 10.0)); + node = tree.search(p); + EXPECT_TRUE(node); + EXPECT_FLOAT_EQ(node->getIntensity(), 10.0); + + EXPECT_TRUE(tree.integrateNodeIntensity(p.x(), p.y(), p.z(), 20.0)); + node = tree.search(p); + EXPECT_TRUE(node); + double expected_intensity = 10.0 * node_prob + 20.0 * (1.0 - node_prob); + EXPECT_NEAR(node->getIntensity(), expected_intensity, 1e-5); + + EXPECT_FALSE(tree.integrateNodeIntensity(10.0f, 10.0f, 10.0f, 5.0)); + + IntensityOcTreeNode* parent = tree.search(p, tree.getTreeDepth() - 1); + EXPECT_TRUE(parent); + + double intensity_sum = 0.0; + for (unsigned int i = 0; i < 8; ++i) { + if (!tree.nodeChildExists(parent, i)) { + tree.createNodeChild(parent, i); + } + IntensityOcTreeNode* child = tree.getNodeChild(parent, i); + child->setLogOdds(0.0f); + double intensity = static_cast(i + 1); + child->setIntensity(intensity); + intensity_sum += intensity; + } + + tree.updateInnerOccupancy(); + EXPECT_NEAR(parent->getIntensity(), intensity_sum / 8.0, 1e-5); + + // Pruning compares full child data, so align intensities first. + const double uniform_intensity = 2.0; + for (unsigned int i = 0; i < 8; ++i) { + IntensityOcTreeNode* child = tree.getNodeChild(parent, i); + child->setIntensity(uniform_intensity); + } + + EXPECT_TRUE(tree.pruneNode(parent)); + EXPECT_FALSE(tree.nodeHasChildren(parent)); + EXPECT_NEAR(parent->getIntensity(), uniform_intensity, 1e-5); + + std::string filename("simple_intensity_tree.ot"); + EXPECT_TRUE(tree.write(filename)); + + AbstractOcTree* read_tree = AbstractOcTree::read(filename); + EXPECT_TRUE(read_tree); + EXPECT_EQ(read_tree->getTreeType().compare(tree.getTreeType()), 0); + EXPECT_FLOAT_EQ(read_tree->getResolution(), tree.getResolution()); + EXPECT_EQ(read_tree->size(), tree.size()); + + IntensityOcTree* read_intensity_tree = dynamic_cast(read_tree); + EXPECT_TRUE(read_intensity_tree); + EXPECT_TRUE(tree == *read_intensity_tree); + + delete read_tree; + read_tree = NULL; + + std::cerr << "Test successful.\n"; + return 0; +}