From b860edf344d24536845570f23aba9b387b119c92 Mon Sep 17 00:00:00 2001 From: Vipin Sharma Date: Sun, 13 Jul 2025 15:46:09 +0400 Subject: [PATCH 01/12] Adds intensity-based Octree Adds a new Octree implementation that stores intensity values in addition to occupancy probability. This new tree type allows for representing intensity information alongside spatial occupancy, enabling applications that require both geometric and radiometric data. --- octomap/include/octomap/IntensityOcTree.h | 203 ++++++++++++++++++++++ octomap/src/IntensityOcTree.cpp | 81 +++++++++ 2 files changed, 284 insertions(+) create mode 100644 octomap/include/octomap/IntensityOcTree.h create mode 100644 octomap/src/IntensityOcTree.cpp diff --git a/octomap/include/octomap/IntensityOcTree.h b/octomap/include/octomap/IntensityOcTree.h new file mode 100644 index 00000000..05a449f6 --- /dev/null +++ b/octomap/include/octomap/IntensityOcTree.h @@ -0,0 +1,203 @@ +/* + * 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 + +namespace octomap +{ + +// node definition +class IntensityOcTreeNode : public OcTreeNode +{ +public: + // 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 && rhs.intensity == intensity); + } + + + // 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 + inline void updateIntensityChildren() + { + if (isLeaf()) + return; + + double sum = 0.0; + unsigned cnt = 0; + for (unsigned i = 0; i < 8; ++i) + if (childExists(i)) + { + sum += getChild(i)->intensity; + ++cnt; + } + + intensity = cnt ? sum / cnt : intensity; + } + + // Reverse pruning expand + void expandNode() + { + if (hasChildren()) + return; + + allocChildren(); + for (unsigned i = 0; i < 8; ++i) + { + getChild(i)->intensity = + intensity;// propagate intensity to children + } + } + + + // Serialisation and deserialisation + std::ostream &writeData(std::ostream &s) const override + { + OcTreeNode::writeData(s);// write log-odds + s.write(reinterpret_cast(&intensity), sizeof(intensity)); + return s; + } + + std::istream &readData(std::istream &s) override + { + OcTreeNode::readData(s); + s.read(reinterpret_cast(&intensity), sizeof(intensity)); + return s; + } + +protected: + double intensity; +}; + + +// tree definition +class IntensityOcTree : public OccupancyOcTreeBase +{ +public: + // Default constructor, sets resolution of leafs + explicit 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"; + } + + void updateInnerOccupancyRecurs(IntensityOcTreeNode *node, + unsigned depth) override; + + void updateNodeLogOdds(IntensityOcTreeNode *node, + const float &log_odds_update) const override; + + void integrateNodeIntensity(IntensityOcTreeNode *node, + double intensity_sample, + unsigned weight = 1) const; + + +protected: + /** + * 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 \ No newline at end of file diff --git a/octomap/src/IntensityOcTree.cpp b/octomap/src/IntensityOcTree.cpp new file mode 100644 index 00000000..ce3e5b69 --- /dev/null +++ b/octomap/src/IntensityOcTree.cpp @@ -0,0 +1,81 @@ +/* + * 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 +{ +explicit IntensityOcTree::IntensityOcTree(double resolution) + : OccupancyOcTreeBase(resolution) +{ + intensityOcTreeMemberInit.ensureLinking(); +} + + +void IntensityOcTree::updateInnerOccupancyRecurs(IntensityOcTreeNode *node, + unsigned int depth) +{ + if (!node) + return; + + if (depth < this->tree_depth) + { + for (unsigned i = 0; i < 8; ++i) + if (nodeChildExists(node, i)) + updateInnerOccupancyRecurs(node->getChild(i), depth + 1); + } + node->updateOccupancyChildren(); + node->updateIntensityChildren(); +} + + +void IntensityOcTree::updateNodeLogOdds(IntensityOcTreeNode *node, + const float &log_odds_update) const +{ + OccupancyOcTreeBase::updateNodeLogOdds( + node, log_odds_update); +} + + +void IntensityOcTree::integrateNodeIntensity(IntensityOcTreeNode *node, + double intensity_sample, + unsigned weight) const +{ + double old = node->getIntensity(); + node->setIntensity( + (old * static_cast(weight - 1) + intensity_sample) / + weight); +} + + +}// namespace octomap \ No newline at end of file From 33883059cfb0b87b70c5d8db9e00857fc128b62a Mon Sep 17 00:00:00 2001 From: Vipin Sharma Date: Sun, 13 Jul 2025 16:31:54 +0400 Subject: [PATCH 02/12] Improves intensity propagation in IntensityOcTree Ensures that intensity values are correctly propagated to child nodes when expanding an IntensityOcTreeNode. This change addresses a potential issue where newly created child nodes in an expanded node would not inherit the intensity of their parent, leading to inaccurate intensity representation in the octree. --- octomap/include/octomap/IntensityOcTree.h | 238 +++++++++------------- octomap/src/IntensityOcTree.cpp | 55 ++--- 2 files changed, 122 insertions(+), 171 deletions(-) diff --git a/octomap/include/octomap/IntensityOcTree.h b/octomap/include/octomap/IntensityOcTree.h index 05a449f6..af536b4d 100644 --- a/octomap/include/octomap/IntensityOcTree.h +++ b/octomap/include/octomap/IntensityOcTree.h @@ -31,7 +31,6 @@ * POSSIBILITY OF SUCH DAMAGE. */ - #ifndef OCTOMAP_INTENSITY_OCTREE_H #define OCTOMAP_INTENSITY_OCTREE_H @@ -40,164 +39,127 @@ #include #include -namespace octomap -{ +namespace octomap { // node definition -class IntensityOcTreeNode : public OcTreeNode -{ +class IntensityOcTreeNode : public OcTreeNode { public: - // 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 && rhs.intensity == intensity); - } - - - // 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 - inline void updateIntensityChildren() - { - if (isLeaf()) - return; - - double sum = 0.0; - unsigned cnt = 0; - for (unsigned i = 0; i < 8; ++i) - if (childExists(i)) - { - sum += getChild(i)->intensity; - ++cnt; - } - - intensity = cnt ? sum / cnt : intensity; + // 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 && rhs.intensity == intensity); + } + + // 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 + inline void updateIntensityChildren() { + if (this->isLeaf()) + return; + + double sum = 0.0; + unsigned cnt = 0; + for (unsigned i = 0; i < 8; ++i) + if (this->childExists(i)) { + sum += this->getChild(i)->intensity; + ++cnt; + } + + intensity = cnt ? sum / cnt : intensity; + } + + // Reverse pruning expand + void expandNode() { + if (hasChildren()) + return; + + allocChildren(); + for (unsigned i = 0; i < 8; ++i) { + this->getChild(i)->intensity = + intensity; // propagate intensity to children } + } - // Reverse pruning expand - void expandNode() - { - if (hasChildren()) - return; - - allocChildren(); - for (unsigned i = 0; i < 8; ++i) - { - getChild(i)->intensity = - intensity;// propagate intensity to children - } - } - - - // Serialisation and deserialisation - std::ostream &writeData(std::ostream &s) const override - { - OcTreeNode::writeData(s);// write log-odds - s.write(reinterpret_cast(&intensity), sizeof(intensity)); - return s; - } + // Serialisation and deserialisation + std::ostream &writeData(std::ostream &s) const { + OcTreeNode::writeData(s); // write log-odds + s.write(reinterpret_cast(&intensity), sizeof(intensity)); + return s; + } - std::istream &readData(std::istream &s) override - { - OcTreeNode::readData(s); - s.read(reinterpret_cast(&intensity), sizeof(intensity)); - return s; - } + std::istream &readData(std::istream &s) { + OcTreeNode::readData(s); + s.read(reinterpret_cast(&intensity), sizeof(intensity)); + return s; + } protected: - double intensity; + double intensity; }; - // tree definition -class IntensityOcTree : public OccupancyOcTreeBase -{ +class IntensityOcTree : public OccupancyOcTreeBase { public: - // Default constructor, sets resolution of leafs - explicit 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); - } + // Default constructor, sets resolution of leafs + IntensityOcTree(double resolution); - std::string getTreeType() const - { - return "IntensityOcTree"; - } + /// 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); + } - void updateInnerOccupancyRecurs(IntensityOcTreeNode *node, - unsigned depth) override; + std::string getTreeType() const { return "IntensityOcTree"; } - void updateNodeLogOdds(IntensityOcTreeNode *node, - const float &log_odds_update) const override; + void updateInnerOccupancyRecurs(IntensityOcTreeNode *node, unsigned depth); - void integrateNodeIntensity(IntensityOcTreeNode *node, - double intensity_sample, - unsigned weight = 1) const; + void updateNodeLogOdds(IntensityOcTreeNode *node, + const float &log_odds_update) const override; + void integrateNodeIntensity(IntensityOcTreeNode *node, + double intensity_sample, + unsigned weight = 1) const; protected: + /** + * 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); + } /** - * 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. + * 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. */ - 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; + void ensureLinking() {} + }; + + /// to ensure static initialization (only once) + static StaticMemberInitializer intensityOcTreeMemberInit; }; -}// namespace octomap +} // namespace octomap -#endif// OCTOMAP_INTENSITY_OCTREE_H \ No newline at end of file +#endif // OCTOMAP_INTENSITY_OCTREE_H \ No newline at end of file diff --git a/octomap/src/IntensityOcTree.cpp b/octomap/src/IntensityOcTree.cpp index ce3e5b69..14a94230 100644 --- a/octomap/src/IntensityOcTree.cpp +++ b/octomap/src/IntensityOcTree.cpp @@ -33,49 +33,38 @@ #include "octomap/IntensityOcTree.h" -namespace octomap -{ -explicit IntensityOcTree::IntensityOcTree(double resolution) - : OccupancyOcTreeBase(resolution) -{ - intensityOcTreeMemberInit.ensureLinking(); +namespace octomap { +IntensityOcTree::IntensityOcTree(double resolution) + : OccupancyOcTreeBase(resolution) { + intensityOcTreeMemberInit.ensureLinking(); } - void IntensityOcTree::updateInnerOccupancyRecurs(IntensityOcTreeNode *node, - unsigned int depth) -{ - if (!node) - return; + unsigned int depth) { + if (!node) + return; - if (depth < this->tree_depth) - { - for (unsigned i = 0; i < 8; ++i) - if (nodeChildExists(node, i)) - updateInnerOccupancyRecurs(node->getChild(i), depth + 1); - } - node->updateOccupancyChildren(); - node->updateIntensityChildren(); + if (depth < this->tree_depth) { + for (unsigned i = 0; i < 8; ++i) + if (nodeChildExists(node, i)) + updateInnerOccupancyRecurs(node->getChild(i), depth + 1); + } + node->updateOccupancyChildren(); + node->updateIntensityChildren(); } - void IntensityOcTree::updateNodeLogOdds(IntensityOcTreeNode *node, - const float &log_odds_update) const -{ - OccupancyOcTreeBase::updateNodeLogOdds( - node, log_odds_update); + const float &log_odds_update) const { + OccupancyOcTreeBase::updateNodeLogOdds(node, + log_odds_update); } - void IntensityOcTree::integrateNodeIntensity(IntensityOcTreeNode *node, double intensity_sample, - unsigned weight) const -{ - double old = node->getIntensity(); - node->setIntensity( - (old * static_cast(weight - 1) + intensity_sample) / - weight); + unsigned weight) const { + double old = node->getIntensity(); + node->setIntensity( + (old * static_cast(weight - 1) + intensity_sample) / weight); } - -}// namespace octomap \ No newline at end of file +} // namespace octomap \ No newline at end of file From c0aac24658327017542e4523da53062a4cfe9d36 Mon Sep 17 00:00:00 2001 From: Vipin Sharma Date: Sun, 13 Jul 2025 19:34:42 +0400 Subject: [PATCH 03/12] Improves IntensityOcTree node handling Adds methods for intensity integration, pruning, and averaging in IntensityOcTree, enhancing node management and data consistency. Also introduces a small epsilon value to avoid division by zero. Removes unused intensity integration method Removes the unused `integrateNodeIntensity` method that takes an `IntensityOcTreeNode` and weight as input. This simplifies the API and removes potentially confusing or redundant methods. --- octomap/include/octomap/IntensityOcTree.h | 189 +++++++++++----------- octomap/src/IntensityOcTree.cpp | 153 +++++++++++++++--- 2 files changed, 226 insertions(+), 116 deletions(-) diff --git a/octomap/include/octomap/IntensityOcTree.h b/octomap/include/octomap/IntensityOcTree.h index af536b4d..fbec0601 100644 --- a/octomap/include/octomap/IntensityOcTree.h +++ b/octomap/include/octomap/IntensityOcTree.h @@ -39,127 +39,132 @@ #include #include -namespace octomap { +namespace octomap +{ // node definition -class IntensityOcTreeNode : public OcTreeNode { +class IntensityOcTreeNode : public OcTreeNode +{ public: - // 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 && rhs.intensity == intensity); - } - - // 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 - inline void updateIntensityChildren() { - if (this->isLeaf()) - return; - - double sum = 0.0; - unsigned cnt = 0; - for (unsigned i = 0; i < 8; ++i) - if (this->childExists(i)) { - sum += this->getChild(i)->intensity; - ++cnt; - } - - intensity = cnt ? sum / cnt : intensity; - } - - // Reverse pruning expand - void expandNode() { - if (hasChildren()) - return; - - allocChildren(); - for (unsigned i = 0; i < 8; ++i) { - this->getChild(i)->intensity = - intensity; // propagate intensity to children + // Constructors + IntensityOcTreeNode() : OcTreeNode(), intensity(0.0) + { } - } - // Serialisation and deserialisation - std::ostream &writeData(std::ostream &s) const { - OcTreeNode::writeData(s); // write log-odds - s.write(reinterpret_cast(&intensity), sizeof(intensity)); - return s; - } + IntensityOcTreeNode(const IntensityOcTreeNode &rhs) + : OcTreeNode(rhs), intensity(rhs.intensity) + { + } + + // Comparator + bool operator==(const IntensityOcTreeNode &rhs) const + { + return (rhs.value == value && rhs.intensity == intensity); + } + + // 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 + inline void updateIntensityChildren(); + double getAverageChildIntensity() const; + inline bool isIntensitySet() const + { + return (ZERO_EPSILON < intensity); + } - std::istream &readData(std::istream &s) { - OcTreeNode::readData(s); - s.read(reinterpret_cast(&intensity), sizeof(intensity)); - return s; - } + // Serialisation and deserialisation + std::istream &readData(std::istream &s); + std::ostream &writeData(std::ostream &s) const; protected: - double intensity; + double intensity; + static constexpr double ZERO_EPSILON = 1e-6;// to avoid division by zero }; // tree definition -class IntensityOcTree : public OccupancyOcTreeBase { +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); - } + // 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"; } + std::string getTreeType() const + { + return "IntensityOcTree"; + } - void updateInnerOccupancyRecurs(IntensityOcTreeNode *node, unsigned depth); + /** + * 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); - void updateNodeLogOdds(IntensityOcTreeNode *node, - const float &log_odds_update) const override; + void updateNodeLogOdds(IntensityOcTreeNode *node, + const float &log_odds_update) const override; + void updateInnerOccupancy(); - void integrateNodeIntensity(IntensityOcTreeNode *node, - double intensity_sample, - unsigned weight = 1) const; + 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); - } - /** + 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() {} - }; + void ensureLinking() + { + } + }; - /// to ensure static initialization (only once) - static StaticMemberInitializer intensityOcTreeMemberInit; + /// to ensure static initialization (only once) + static StaticMemberInitializer intensityOcTreeMemberInit; }; -} // namespace octomap +}// namespace octomap -#endif // OCTOMAP_INTENSITY_OCTREE_H \ No newline at end of file +#endif// OCTOMAP_INTENSITY_OCTREE_H \ No newline at end of file diff --git a/octomap/src/IntensityOcTree.cpp b/octomap/src/IntensityOcTree.cpp index 14a94230..992fcd54 100644 --- a/octomap/src/IntensityOcTree.cpp +++ b/octomap/src/IntensityOcTree.cpp @@ -33,38 +33,143 @@ #include "octomap/IntensityOcTree.h" -namespace octomap { +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; + } + } + } + + double average_intensity = cnt ? sum / cnt : intensity; + return average_intensity; +} + +inline void IntensityOcTreeNode::updateIntensityChildren() +{ + intensity = getAverageChildIntensity(); +} + +// Tree implementation IntensityOcTree::IntensityOcTree(double resolution) - : OccupancyOcTreeBase(resolution) { - intensityOcTreeMemberInit.ensureLinking(); + : OccupancyOcTreeBase(resolution) +{ + intensityOcTreeMemberInit.ensureLinking(); } void IntensityOcTree::updateInnerOccupancyRecurs(IntensityOcTreeNode *node, - unsigned int depth) { - if (!node) - return; - - if (depth < this->tree_depth) { - for (unsigned i = 0; i < 8; ++i) - if (nodeChildExists(node, i)) - updateInnerOccupancyRecurs(node->getChild(i), depth + 1); - } - node->updateOccupancyChildren(); - node->updateIntensityChildren(); + unsigned depth) +{ + if (!node) + return; + + 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(); } + +// 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 + if (node->isIntensitySet()) + 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; +} + + void IntensityOcTree::updateNodeLogOdds(IntensityOcTreeNode *node, - const float &log_odds_update) const { - OccupancyOcTreeBase::updateNodeLogOdds(node, - log_odds_update); + const float &log_odds_update) const +{ + OccupancyOcTreeBase::updateNodeLogOdds( + node, log_odds_update); +} + + +// 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; } -void IntensityOcTree::integrateNodeIntensity(IntensityOcTreeNode *node, - double intensity_sample, - unsigned weight) const { - double old = node->getIntensity(); - node->setIntensity( - (old * static_cast(weight - 1) + intensity_sample) / weight); +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); } -} // namespace octomap \ No newline at end of file +}// namespace octomap \ No newline at end of file From 4a2f28c8583d7c470034c7dc3070bd6090ad526c Mon Sep 17 00:00:00 2001 From: Vipin Sharma Date: Sun, 13 Jul 2025 19:59:44 +0400 Subject: [PATCH 04/12] Adds IntensityOcTree source file to CMakeLists --- octomap/src/CMakeLists.txt | 92 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 92 insertions(+) create mode 100644 octomap/src/CMakeLists.txt diff --git a/octomap/src/CMakeLists.txt b/octomap/src/CMakeLists.txt new file mode 100644 index 00000000..3d7cd95f --- /dev/null +++ b/octomap/src/CMakeLists.txt @@ -0,0 +1,92 @@ +SET (octomap_SRCS + AbstractOcTree.cpp + AbstractOccupancyOcTree.cpp + Pointcloud.cpp + ScanGraph.cpp + CountingOcTree.cpp + OcTree.cpp + OcTreeNode.cpp + OcTreeStamped.cpp + ColorOcTree.cpp + IntensityOcTree.cpp + ) + +# dynamic and static libs, see CMake FAQ: +ADD_LIBRARY( octomap SHARED ${octomap_SRCS}) +set_target_properties( octomap PROPERTIES + VERSION ${OCTOMAP_VERSION} + SOVERSION ${OCTOMAP_SOVERSION} +) +ADD_LIBRARY( octomap-static STATIC ${octomap_SRCS}) +SET_TARGET_PROPERTIES(octomap-static PROPERTIES OUTPUT_NAME "octomap") +add_dependencies(octomap-static octomath-static) + +TARGET_LINK_LIBRARIES(octomap octomath) + +if(NOT EXISTS "${CMAKE_LIBRARY_OUTPUT_DIRECTORY}/cmake/octomap") + file(MAKE_DIRECTORY "${CMAKE_LIBRARY_OUTPUT_DIRECTORY}/cmake/octomap") +endif() + +export(TARGETS octomap octomap-static + APPEND FILE "${CMAKE_LIBRARY_OUTPUT_DIRECTORY}/cmake/octomap/octomap-targets.cmake") + +ADD_SUBDIRECTORY( testing ) + +ADD_EXECUTABLE(graph2tree graph2tree.cpp) +TARGET_LINK_LIBRARIES(graph2tree octomap) + +ADD_EXECUTABLE(log2graph log2graph.cpp) +TARGET_LINK_LIBRARIES(log2graph octomap) + +ADD_EXECUTABLE(binvox2bt binvox2bt.cpp) +TARGET_LINK_LIBRARIES(binvox2bt octomap) + +ADD_EXECUTABLE(bt2vrml bt2vrml.cpp) +TARGET_LINK_LIBRARIES(bt2vrml octomap) + +ADD_EXECUTABLE(edit_octree edit_octree.cpp) +TARGET_LINK_LIBRARIES(edit_octree octomap) + +ADD_EXECUTABLE(convert_octree convert_octree.cpp) +TARGET_LINK_LIBRARIES(convert_octree octomap) + +ADD_EXECUTABLE(eval_octree_accuracy eval_octree_accuracy.cpp) +TARGET_LINK_LIBRARIES(eval_octree_accuracy octomap) + +ADD_EXECUTABLE(compare_octrees compare_octrees.cpp) +TARGET_LINK_LIBRARIES(compare_octrees octomap) + +ADD_EXECUTABLE(simple_example simple_example.cpp) +TARGET_LINK_LIBRARIES(simple_example octomap) + +ADD_EXECUTABLE(pcd_to_bt_example pcd_to_bt_example.cpp) +TARGET_LINK_LIBRARIES(pcd_to_bt_example octomap) + +ADD_EXECUTABLE(normals_example normals_example.cpp) +TARGET_LINK_LIBRARIES(normals_example octomap) + +ADD_EXECUTABLE(intersection_example intersection_example.cpp) +TARGET_LINK_LIBRARIES(intersection_example octomap) + +ADD_EXECUTABLE(octree2pointcloud octree2pointcloud.cpp) +TARGET_LINK_LIBRARIES(octree2pointcloud octomap) + +install(TARGETS octomap octomap-static + EXPORT octomap-targets + INCLUDES DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}" + ${INSTALL_TARGETS_DEFAULT_ARGS} +) +install(EXPORT octomap-targets DESTINATION "${CMAKE_INSTALL_DATADIR}/octomap") + +install(TARGETS + graph2tree + log2graph + binvox2bt + bt2vrml + edit_octree + convert_octree + eval_octree_accuracy + compare_octrees + ${INSTALL_TARGETS_DEFAULT_ARGS} +) + From 9cddbbb20ea18e8f0a6c043d63fe507feec1c3a1 Mon Sep 17 00:00:00 2001 From: Vipin Sharma Date: Sun, 13 Jul 2025 20:17:58 +0400 Subject: [PATCH 05/12] Improves intensity occupancy update Updates inner node occupancy recursively for intensity octrees. This change ensures that the intensity values of inner nodes are correctly updated based on the children's values, leading to a more accurate representation of the intensity distribution in the octree. --- octomap/include/octomap/IntensityOcTree.h | 7 +++++-- octomap/src/IntensityOcTree.cpp | 14 ++++++-------- 2 files changed, 11 insertions(+), 10 deletions(-) diff --git a/octomap/include/octomap/IntensityOcTree.h b/octomap/include/octomap/IntensityOcTree.h index fbec0601..a455025c 100644 --- a/octomap/include/octomap/IntensityOcTree.h +++ b/octomap/include/octomap/IntensityOcTree.h @@ -42,10 +42,15 @@ 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) { @@ -123,8 +128,6 @@ class IntensityOcTree : public OccupancyOcTreeBase */ virtual bool pruneNode(IntensityOcTreeNode *node); - void updateNodeLogOdds(IntensityOcTreeNode *node, - const float &log_odds_update) const override; void updateInnerOccupancy(); IntensityOcTreeNode *integrateNodeIntensity(const OcTreeKey &key, diff --git a/octomap/src/IntensityOcTree.cpp b/octomap/src/IntensityOcTree.cpp index 992fcd54..cd8363a3 100644 --- a/octomap/src/IntensityOcTree.cpp +++ b/octomap/src/IntensityOcTree.cpp @@ -88,6 +88,12 @@ IntensityOcTree::IntensityOcTree(double resolution) intensityOcTreeMemberInit.ensureLinking(); } + +void IntensityOcTree::updateInnerOccupancy() +{ + this->updateInnerOccupancyRecurs(this->root, 0); +} + void IntensityOcTree::updateInnerOccupancyRecurs(IntensityOcTreeNode *node, unsigned depth) { @@ -130,14 +136,6 @@ bool IntensityOcTree::pruneNode(IntensityOcTreeNode *node) } -void IntensityOcTree::updateNodeLogOdds(IntensityOcTreeNode *node, - const float &log_odds_update) const -{ - OccupancyOcTreeBase::updateNodeLogOdds( - node, log_odds_update); -} - - // Integration IntensityOcTreeNode * IntensityOcTree::integrateNodeIntensity(const OcTreeKey &key, double intensity) From b80e78c7d239add69d4ac89f8f794048db94ada6 Mon Sep 17 00:00:00 2001 From: Vipin Sharma Date: Mon, 14 Jul 2025 06:12:22 +0400 Subject: [PATCH 06/12] Adds computeUpdateKeys to IntensityOcTree Adds a `computeUpdateKeys` method to `IntensityOcTree`. This method allows to compute the set of free and occupied cells corresponding to a pointcloud, using an alternative interface based on octomath vectors. --- octomap/include/octomap/IntensityOcTree.h | 6 ++++++ octomap/src/IntensityOcTree.cpp | 11 +++++++++++ 2 files changed, 17 insertions(+) diff --git a/octomap/include/octomap/IntensityOcTree.h b/octomap/include/octomap/IntensityOcTree.h index a455025c..3421fdb9 100644 --- a/octomap/include/octomap/IntensityOcTree.h +++ b/octomap/include/octomap/IntensityOcTree.h @@ -130,6 +130,12 @@ class IntensityOcTree : public OccupancyOcTreeBase void updateInnerOccupancy(); + 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, diff --git a/octomap/src/IntensityOcTree.cpp b/octomap/src/IntensityOcTree.cpp index cd8363a3..1c4a509a 100644 --- a/octomap/src/IntensityOcTree.cpp +++ b/octomap/src/IntensityOcTree.cpp @@ -111,6 +111,17 @@ void IntensityOcTree::updateInnerOccupancyRecurs(IntensityOcTreeNode *node, } +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) { From 540f48a4c7ae686c586cd91ebbd0639cb97e9c51 Mon Sep 17 00:00:00 2001 From: Vipin Sharma Date: Mon, 14 Jul 2025 07:19:36 +0400 Subject: [PATCH 07/12] Refactors inner node occupancy update Improves the efficiency of the inner node occupancy update process by adding a condition to check for the presence of children before recursively updating occupancy. This avoids unnecessary processing when a node has no children, leading to performance gains, especially in sparse octrees. --- octomap/src/IntensityOcTree.cpp | 21 ++++++++++++--------- 1 file changed, 12 insertions(+), 9 deletions(-) diff --git a/octomap/src/IntensityOcTree.cpp b/octomap/src/IntensityOcTree.cpp index 1c4a509a..9846b94b 100644 --- a/octomap/src/IntensityOcTree.cpp +++ b/octomap/src/IntensityOcTree.cpp @@ -97,17 +97,20 @@ void IntensityOcTree::updateInnerOccupancy() void IntensityOcTree::updateInnerOccupancyRecurs(IntensityOcTreeNode *node, unsigned depth) { - if (!node) - return; - - if (depth < this->tree_depth) + if (nodeHasChildren(node)) { - for (unsigned i = 0; i < 8; ++i) - if (nodeChildExists(node, i)) - updateInnerOccupancyRecurs(getNodeChild(node, i), depth + 1); + 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(); } - node->updateOccupancyChildren(); - node->updateIntensityChildren(); } From d7c994683c415704d51ce15f5c5f57b8f6323c92 Mon Sep 17 00:00:00 2001 From: Vipin Sharma Date: Sat, 20 Dec 2025 20:20:26 +0400 Subject: [PATCH 08/12] Adds IntensityOcTree and corresponding tests to CMakeLists --- octomap/CMakeLists.txt | 1 + octomap/src/CMakeLists.txt | 92 --------------------- octomap/src/testing/CMakeLists.txt | 4 + octomap/src/testing/test_intensity_tree.cpp | 81 ++++++++++++++++++ 4 files changed, 86 insertions(+), 92 deletions(-) delete mode 100644 octomap/src/CMakeLists.txt create mode 100644 octomap/src/testing/test_intensity_tree.cpp 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/src/CMakeLists.txt b/octomap/src/CMakeLists.txt deleted file mode 100644 index 3d7cd95f..00000000 --- a/octomap/src/CMakeLists.txt +++ /dev/null @@ -1,92 +0,0 @@ -SET (octomap_SRCS - AbstractOcTree.cpp - AbstractOccupancyOcTree.cpp - Pointcloud.cpp - ScanGraph.cpp - CountingOcTree.cpp - OcTree.cpp - OcTreeNode.cpp - OcTreeStamped.cpp - ColorOcTree.cpp - IntensityOcTree.cpp - ) - -# dynamic and static libs, see CMake FAQ: -ADD_LIBRARY( octomap SHARED ${octomap_SRCS}) -set_target_properties( octomap PROPERTIES - VERSION ${OCTOMAP_VERSION} - SOVERSION ${OCTOMAP_SOVERSION} -) -ADD_LIBRARY( octomap-static STATIC ${octomap_SRCS}) -SET_TARGET_PROPERTIES(octomap-static PROPERTIES OUTPUT_NAME "octomap") -add_dependencies(octomap-static octomath-static) - -TARGET_LINK_LIBRARIES(octomap octomath) - -if(NOT EXISTS "${CMAKE_LIBRARY_OUTPUT_DIRECTORY}/cmake/octomap") - file(MAKE_DIRECTORY "${CMAKE_LIBRARY_OUTPUT_DIRECTORY}/cmake/octomap") -endif() - -export(TARGETS octomap octomap-static - APPEND FILE "${CMAKE_LIBRARY_OUTPUT_DIRECTORY}/cmake/octomap/octomap-targets.cmake") - -ADD_SUBDIRECTORY( testing ) - -ADD_EXECUTABLE(graph2tree graph2tree.cpp) -TARGET_LINK_LIBRARIES(graph2tree octomap) - -ADD_EXECUTABLE(log2graph log2graph.cpp) -TARGET_LINK_LIBRARIES(log2graph octomap) - -ADD_EXECUTABLE(binvox2bt binvox2bt.cpp) -TARGET_LINK_LIBRARIES(binvox2bt octomap) - -ADD_EXECUTABLE(bt2vrml bt2vrml.cpp) -TARGET_LINK_LIBRARIES(bt2vrml octomap) - -ADD_EXECUTABLE(edit_octree edit_octree.cpp) -TARGET_LINK_LIBRARIES(edit_octree octomap) - -ADD_EXECUTABLE(convert_octree convert_octree.cpp) -TARGET_LINK_LIBRARIES(convert_octree octomap) - -ADD_EXECUTABLE(eval_octree_accuracy eval_octree_accuracy.cpp) -TARGET_LINK_LIBRARIES(eval_octree_accuracy octomap) - -ADD_EXECUTABLE(compare_octrees compare_octrees.cpp) -TARGET_LINK_LIBRARIES(compare_octrees octomap) - -ADD_EXECUTABLE(simple_example simple_example.cpp) -TARGET_LINK_LIBRARIES(simple_example octomap) - -ADD_EXECUTABLE(pcd_to_bt_example pcd_to_bt_example.cpp) -TARGET_LINK_LIBRARIES(pcd_to_bt_example octomap) - -ADD_EXECUTABLE(normals_example normals_example.cpp) -TARGET_LINK_LIBRARIES(normals_example octomap) - -ADD_EXECUTABLE(intersection_example intersection_example.cpp) -TARGET_LINK_LIBRARIES(intersection_example octomap) - -ADD_EXECUTABLE(octree2pointcloud octree2pointcloud.cpp) -TARGET_LINK_LIBRARIES(octree2pointcloud octomap) - -install(TARGETS octomap octomap-static - EXPORT octomap-targets - INCLUDES DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}" - ${INSTALL_TARGETS_DEFAULT_ARGS} -) -install(EXPORT octomap-targets DESTINATION "${CMAKE_INSTALL_DATADIR}/octomap") - -install(TARGETS - graph2tree - log2graph - binvox2bt - bt2vrml - edit_octree - convert_octree - eval_octree_accuracy - compare_octrees - ${INSTALL_TARGETS_DEFAULT_ARGS} -) - 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..99b3a77c --- /dev/null +++ b/octomap/src/testing/test_intensity_tree.cpp @@ -0,0 +1,81 @@ +#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); + EXPECT_NEAR(node->getOccupancy(), 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); + EXPECT_NEAR(node->getIntensity(), 10.0 * 0.8 + 20.0 * 0.2, 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; +} From 42353243d6212fd47d395a1efe233d9fdd955934 Mon Sep 17 00:00:00 2001 From: Vipin Sharma Date: Sat, 20 Dec 2025 20:24:05 +0400 Subject: [PATCH 09/12] Apply suggestions from code review Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> --- octomap/include/octomap/IntensityOcTree.h | 4 ++-- octomap/src/IntensityOcTree.cpp | 20 ++++++++++++++------ 2 files changed, 16 insertions(+), 8 deletions(-) diff --git a/octomap/include/octomap/IntensityOcTree.h b/octomap/include/octomap/IntensityOcTree.h index 3421fdb9..b151c2c1 100644 --- a/octomap/include/octomap/IntensityOcTree.h +++ b/octomap/include/octomap/IntensityOcTree.h @@ -92,13 +92,13 @@ class IntensityOcTreeNode : public OcTreeNode return (ZERO_EPSILON < intensity); } - // Serialisation and deserialisation + // 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;// to avoid division by zero + static constexpr double ZERO_EPSILON = 1e-6;// threshold for checking if intensity has been set }; // tree definition diff --git a/octomap/src/IntensityOcTree.cpp b/octomap/src/IntensityOcTree.cpp index 9846b94b..a4d98c2c 100644 --- a/octomap/src/IntensityOcTree.cpp +++ b/octomap/src/IntensityOcTree.cpp @@ -72,11 +72,19 @@ double IntensityOcTreeNode::getAverageChildIntensity() const } } - double average_intensity = cnt ? sum / cnt : intensity; - return average_intensity; + // 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; + } } -inline void IntensityOcTreeNode::updateIntensityChildren() +void IntensityOcTreeNode::updateIntensityChildren() { intensity = getAverageChildIntensity(); } @@ -134,9 +142,8 @@ bool IntensityOcTree::pruneNode(IntensityOcTreeNode *node) // set value to children's values (all assumed equal) node->copyData(*(getNodeChild(node, 0))); - // update intensity - if (node->isIntensitySet()) - node->setIntensity(node->getAverageChildIntensity()); + // update intensity to represent the average of all children + node->setIntensity(node->getAverageChildIntensity()); // delete children for (unsigned int i = 0; i < 8; i++) @@ -184,4 +191,5 @@ IntensityOcTreeNode *IntensityOcTree::integrateNodeIntensity(float x, float y, return integrateNodeIntensity(key, intensity); } +IntensityOcTree::StaticMemberInitializer IntensityOcTree::intensityOcTreeMemberInit; }// namespace octomap \ No newline at end of file From 2a5ca333dac849ea6cb488a49208caa84e2fc7f7 Mon Sep 17 00:00:00 2001 From: Vipin Sharma Date: Sat, 20 Dec 2025 20:25:55 +0400 Subject: [PATCH 10/12] Refactor intensity tree tests for clarity and accuracy --- octomap/src/testing/test_intensity_tree.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/octomap/src/testing/test_intensity_tree.cpp b/octomap/src/testing/test_intensity_tree.cpp index 99b3a77c..a0ad1d34 100644 --- a/octomap/src/testing/test_intensity_tree.cpp +++ b/octomap/src/testing/test_intensity_tree.cpp @@ -16,7 +16,8 @@ int main(int /*argc*/, char** /*argv*/) { point3d p(0.0f, 0.0f, 0.0f); IntensityOcTreeNode* node = tree.updateNode(p, true); EXPECT_TRUE(node); - EXPECT_NEAR(node->getOccupancy(), 0.8, 1e-5); + 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)); @@ -27,7 +28,8 @@ int main(int /*argc*/, char** /*argv*/) { EXPECT_TRUE(tree.integrateNodeIntensity(p.x(), p.y(), p.z(), 20.0)); node = tree.search(p); EXPECT_TRUE(node); - EXPECT_NEAR(node->getIntensity(), 10.0 * 0.8 + 20.0 * 0.2, 1e-5); + 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)); From 229dd9150751fc07a7779a94bdbe7cd79069afe9 Mon Sep 17 00:00:00 2001 From: Vipin Sharma Date: Sat, 20 Dec 2025 20:51:30 +0400 Subject: [PATCH 11/12] Add isNodeCollapsible method and improve intensity comparison in IntensityOcTree --- octomap/include/octomap/IntensityOcTree.h | 11 ++++++++-- octomap/src/IntensityOcTree.cpp | 25 ++++++++++++++++++++++- 2 files changed, 33 insertions(+), 3 deletions(-) diff --git a/octomap/include/octomap/IntensityOcTree.h b/octomap/include/octomap/IntensityOcTree.h index b151c2c1..9966145d 100644 --- a/octomap/include/octomap/IntensityOcTree.h +++ b/octomap/include/octomap/IntensityOcTree.h @@ -34,6 +34,7 @@ #ifndef OCTOMAP_INTENSITY_OCTREE_H #define OCTOMAP_INTENSITY_OCTREE_H +#include #include #include @@ -64,7 +65,8 @@ class IntensityOcTreeNode : public OcTreeNode // Comparator bool operator==(const IntensityOcTreeNode &rhs) const { - return (rhs.value == value && rhs.intensity == intensity); + return (rhs.value == value && + std::fabs(rhs.intensity - intensity) < ZERO_EPSILON); } // Payload helpers @@ -89,6 +91,7 @@ class IntensityOcTreeNode : public OcTreeNode double getAverageChildIntensity() const; inline bool isIntensitySet() const { + // Intensity is treated as non-negative; near-zero values are considered "unset". return (ZERO_EPSILON < intensity); } @@ -128,8 +131,12 @@ class IntensityOcTree : public OccupancyOcTreeBase */ 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, @@ -176,4 +183,4 @@ class IntensityOcTree : public OccupancyOcTreeBase }// namespace octomap -#endif// OCTOMAP_INTENSITY_OCTREE_H \ No newline at end of file +#endif// OCTOMAP_INTENSITY_OCTREE_H diff --git a/octomap/src/IntensityOcTree.cpp b/octomap/src/IntensityOcTree.cpp index a4d98c2c..27c7ad80 100644 --- a/octomap/src/IntensityOcTree.cpp +++ b/octomap/src/IntensityOcTree.cpp @@ -156,6 +156,29 @@ bool IntensityOcTree::pruneNode(IntensityOcTreeNode *node) 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 * @@ -192,4 +215,4 @@ IntensityOcTreeNode *IntensityOcTree::integrateNodeIntensity(float x, float y, } IntensityOcTree::StaticMemberInitializer IntensityOcTree::intensityOcTreeMemberInit; -}// namespace octomap \ No newline at end of file +}// namespace octomap From 96eefdc4630b753d07d276415b3255dac7de8184 Mon Sep 17 00:00:00 2001 From: Vipin Sharma Date: Sat, 20 Dec 2025 20:59:12 +0400 Subject: [PATCH 12/12] Update octomap/include/octomap/IntensityOcTree.h Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> --- octomap/include/octomap/IntensityOcTree.h | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/octomap/include/octomap/IntensityOcTree.h b/octomap/include/octomap/IntensityOcTree.h index 9966145d..92aefc89 100644 --- a/octomap/include/octomap/IntensityOcTree.h +++ b/octomap/include/octomap/IntensityOcTree.h @@ -87,7 +87,23 @@ class IntensityOcTreeNode : public OcTreeNode } // 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 {