UFOMap代码Debug

问题描述:

根因不是单纯的点云误分类,而是 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());
}
相关推荐
我不会插花弄玉7 小时前
13.多路转接epoll【由浅入深-网络】
网络·c++·epoll
郝学胜-神的一滴9 小时前
Effective STL 条款03:容器中的对象复制要廉价且正确
开发语言·数据结构·c++·算法·rpc·stl
西西弗Sisyphus10 小时前
C++ explicit 关键字工程实践
开发语言·c++
AIgorithmGEEK12 小时前
std::unordered_map 与 std::unordered_set:接口、算法技巧与权衡 (速查)
c++·算法·哈希表·负载因子·桶·unordered_map·unordered_set
一拳一个呆瓜14 小时前
【STL】头文件:<cmath>
c++·stl
草莓熊Lotso15 小时前
【C++ 基于微服务的即时通讯系统】ElasticSearch 用户数据管理实现
linux·数据库·c++·elasticsearch·微服务·架构
慧都产品智能化15 小时前
Qt 开发 CAD/CAM 工序表的隐性成本分析
c++·qt·qt5·qt6.3
eggwyw17 小时前
linux下的C\C++多进程多线程编程实例详解
开发语言·c++
青少儿编程课堂18 小时前
第十八届蓝桥杯青少组图形化真题 + 全国青少年信息素养大赛 + 电子学会图形化考级:校园泡泡堂大挑战解析
c++·python·算法·bfs·信息学竞赛
小七在进步19 小时前
string的实现(3)
开发语言·c++·笔记·visual studio