UFOMap代码Debug

zxw6102026-06-09 9:09

问题描述：

根因不是单纯的点云误分类，而是 UFOMap (https://github.com/UnknownFreeOccupied/ufomap) 内部的 occupancy 聚合、自动剪枝逻辑，以及查询路径的 depth 下钻错误共同导致的。

查询路径 Octree::getNode(code) 和写入路径 Octree::getNodePath(code) 在 depth0 查询时不一致。写入路径已经更新到真实 depth0 leaf，但查询路径可能少下钻一层，读到父级节点的 occupancy。

代码修改

路径 3rdparty/ufomap/ufomap/include/ufo/map/octree.h:975-986

Git DIff

diff 复制代码

 std::pair<LEAF_NODE const*, DepthType> getNode(Code const& code) const
 {
     LEAF_NODE const* node = &getRoot();
-    for (DepthType depth = getTreeDepthLevels() - 1; depth > code.getDepth(); --depth) {
+    for (DepthType depth = getTreeDepthLevels(); depth > code.getDepth(); --depth) {
         INNER_NODE const& inner_node = static_cast<INNER_NODE const&>(*node);
         if (!hasChildren(inner_node)) {
-            return std::make_pair(node, depth + 1);
+            return std::make_pair(node, depth);
         }
-        node = &getChild(inner_node, depth, code.getChildIdx(depth));
+        DepthType child_depth = depth - 1;
+        node = &getChild(inner_node, child_depth, code.getChildIdx(child_depth));
     }
     return std::make_pair(node, code.getDepth());
 }

修复后的完整代码:

cpp 复制代码

std::pair<LEAF_NODE const*, DepthType> getNode(Code const& code) const
{
    LEAF_NODE const* node = &getRoot();
    for (DepthType depth = getTreeDepthLevels(); depth > code.getDepth(); --depth) {
        INNER_NODE const& inner_node = static_cast<INNER_NODE const&>(*node);
        if (!hasChildren(inner_node)) {
            return std::make_pair(node, depth);
        }
        DepthType child_depth = depth - 1;
        node = &getChild(inner_node, child_depth, code.getChildIdx(child_depth));
    }
    return std::make_pair(node, code.getDepth());
}