Faster-LIO论文与代码笔记（1）

介绍

相比Fast-LIO2方案，精度相当，同时算法效率更高。

高翔："本文要谈的Faster-LIO是基于FastLIO2开发的。FastLIO2是开源LIO中比较优秀的一个，前端用了增量的kdtree（ikd-tree），后端用了迭代ESKF（IEKF），流程短，计算快。Faster-LIO则把ikd-tree替换成了iVox（后文介绍），顺带优化了一些代码逻辑，实现了更快的LIO。我们在典型的32线激光雷达中可以取得100-200Hz左右的计算频率，在固态雷达中甚至可以达到1000-2000Hz，能够达到FastLIO2的1.5-2倍左右的速度。"

论文与代码总结

点云配准（cloud registration）是点云定位建图的关键一环，需要依赖用于查询最近邻的数据结构。

点云最近邻的数据结构

大体分为树（tree）类、体素（voxel）类，笔者看到还有图（graph）类，极大团匹配算法。树结构可视为一种特殊的图。

高博认为LIO的最近临问题是低维的，用体素更合适。

"广义的，高维的最近邻问题是一个比较复杂的问题，但LIO里的最近邻则是低维的、增量式的问题。于是，像R树、B 树等静态的数据结构并不是非常适合LIO。FastLIO2里提出使用增量式的kdtree来处理最近邻，我们则认为增量的体素更适合LIO系统。"

理解iVoxel

思路：

点云配准，即当前扫描点云对齐到已建图点云（local map），需要local map的点云点数较少，且保持稀疏，因此适宜用空间类的数据结构，如常用的八叉树地图（octo-map）；

它把全部空间体素化，很多地方为空，而维护空的体素浪费存储资源；

能否维护一个稀疏的体素结构呢，有点的地方才创建，问题是相比传统体素直接按空间坐标线性地创建，即线性数据结构，如何创建、查找这种非线性数据结构，即如何给他分配内存地址，想到哈希表，C++中unordered_map就是哈希表。对于点云数据需要自定义哈希函数。

哈希函数
论文使用空间哈希函数。

v = 1 s [ p x , p y , p z ] T i d v = h a s h ( v ) = ( v x n x ) x o r ( v y n y ) x o r ( v y n y ) x o r ( v z n z ) \bold v= \frac{1}{s}[p_x,p_y,p_z]^T \\ id_v=hash(v)=(v_xn_x)xor(v_yn_y)xor(v_yn_y)xor(v_zn_z) v=s1[px,py,pz]Tidv=hash(v)=(vxnx)xor(vyny)xor(vyny)xor(vznz)

代码实现，
faster-lio/include/ivox3d/ivox3d.h第101行声明表示点云地图的哈希表，

cpp 复制代码

// ...
class IVOX{
    std::unordered_map<KeyType, typename std::list<std::pair<KeyType, NodeType>>::iterator, hash_vec<dim>>
        grids_map_;                                        // voxel hash map
}

dim默认是3，faster-lio/include/ivox3d/eigen_types.h实现哈希定义，

cpp 复制代码

template <>
inline size_t hash_vec<3>::operator()(const Eigen::Matrix<int, 3, 1>& v) const {
    return size_t(((v[0]) * 73856093) ^ ((v[1]) * 471943) ^ ((v[2]) * 83492791)) % 10000000;
}

体素内点的存储

有两种配置，数组或伪希尔伯特折（曲）线，默认第一种。
include/ivox3d/ivox3d_node.hpp 中类IVoxNode即第一种，私有变量std::vector<PointT> points_;；类IVoxNodePhc即第二种。

类IVoxel则是整个地图，用模板参数控制模板类（VoxelNode）的类型。

cpp 复制代码

enum class IVoxNodeType {
    DEFAULT,  // linear ivox
    PHC,      // phc ivox
};

/// traits for NodeType
template <IVoxNodeType node_type, typename PointT, int dim>
struct IVoxNodeTypeTraits {};

template <typename PointT, int dim>
struct IVoxNodeTypeTraits<IVoxNodeType::DEFAULT, PointT, dim> {
    using NodeType = IVoxNode<PointT, dim>;
};

template <typename PointT, int dim>
struct IVoxNodeTypeTraits<IVoxNodeType::PHC, PointT, dim> {
    using NodeType = IVoxNodePhc<PointT, dim>;
};
template <int dim = 3, IVoxNodeType node_type = IVoxNodeType::DEFAULT, typename PointType = pcl::PointXYZ>
classs IVox{
    public:
    // ...
    // 模板参数node_type控制模板参数IVoxNodeTypeTraits中NodeType模板的实例，实现两种点的二选一。
    using Nodetype = typename IVoxNodeTypeTraits<node_type, PointType, dim>::NodeType;
    // local map
    std::unordered_map<KeyType, typename std::list<std::pair<KeyType, NodeType>>::iterator, hash_vec<dim>>
        grids_map_;                                        // voxel hash map
    // LRU(最近最少使用)缓存
    std::list<std::pair<KeyType, NodeType>> grids_cache_;  // voxel cache
    // 邻域
    std::vector<KeyType> nearby_grids_;                    // nearbys
}

下一篇将重点介绍希尔伯特曲线在Faster-LIO工程上的具体应用。