GICP与VGICP协方差计算详解

一、概述

GICP和VGICP在协方差计算上既有相同点，也有不同点：

• 相同点：点级别的协方差计算方法完全相同

• 不同点：VGICP额外引入了体素级别的协方差融合

二、点级别协方差计算（GICP和VGICP相同）

2.1 调用时机

GICP：

void FastGICP::computeTransformation(...) {
    // 如果协方差未计算，则计算
    if (source_covs_.size() != input_->size()) {
        calculate_covariances(input_, *search_source_, source_covs_);
    }
    if (target_covs_.size() != target_->size()) {
        calculate_covariances(target_, *search_target_, target_covs_);
    }
    // ... 执行配准
}

VGICP：

// VGICP继承自FastGICP，所以调用链相同
void FastVGICP::computeTransformation(...) {
    FastGICP::computeTransformation(...);  // 调用父类方法
    // 父类方法中会调用calculate_covariances
}

结论：VGICP使用与GICP完全相同的点级别协方差计算方法。

2.2 协方差计算算法

步骤1：查找最近邻

bool FastGICP::calculate_covariances(
    const PointCloud& cloud,
    pcl::search::Search& kdtree,
    std::vector<Eigen::Matrix4d>& covariances
) {
    covariances.resize(cloud->size());
    
    #pragma omp parallel for
    for (int i = 0; i < cloud->size(); i++) {
        // 1. 查找k个最近邻
        std::vector<int> k_indices;
        std::vector<float> k_sq_distances;
        kdtree.nearestKSearch(cloud->at(i), k_correspondences_, k_indices, k_sq_distances);
        
        // k_correspondences_ 默认值为20

步骤2：构建邻域矩阵

  // 2. 构建邻域点矩阵（4×k，齐次坐标）
        Eigen::Matrix<double, 4, -1> neighbors(4, k_correspondences_);
        for (int j = 0; j < k_indices.size(); j++) {
            neighbors.col(j) = cloud->at(k_indices[j]).getVector4fMap().cast<double>();
        }

步骤3：计算协方差矩阵

   // 3. 去中心化：减去均值
        neighbors.colwise() -= neighbors.rowwise().mean().eval();
        
        // 4. 计算协方差矩阵
        // cov = (1/k) * Σ(x_i - mean) * (x_i - mean)^T
        //     = (1/k) * neighbors * neighbors^T
        Eigen::Matrix4d cov = neighbors * neighbors.transpose() / k_correspondences_;

数学公式：

对于点p及其k个最近邻 {p₁, p₂, ..., pₖ}：
​
1. 计算均值：
   mean = (1/k) * Σᵢ pᵢ
​
2. 去中心化：
   p'ᵢ = pᵢ - mean
​
3. 计算协方差：
   cov = (1/k) * Σᵢ p'ᵢ * p'ᵢ^T
       = (1/k) * neighbors * neighbors^T

步骤4：协方差正则化

   // 5. 应用正则化方法
        if (regularization_method_ == RegularizationMethod::NONE) {
            covariances[i] = cov;
        } 
        else if (regularization_method_ == RegularizationMethod::FROBENIUS) {
            // Frobenius正则化
            double lambda = 1e-3;
            Eigen::Matrix3d C = cov.block<3, 3>(0, 0) + lambda * Eigen::Matrix3d::Identity();
            Eigen::Matrix3d C_inv = C.inverse();
            covariances[i].setZero();
            covariances[i].block<3, 3>(0, 0) = (C_inv / C_inv.norm()).inverse();
        } 
        else {
            // SVD分解正则化
            Eigen::JacobiSVD<Eigen::Matrix3d> svd(
                cov.block<3, 3>(0, 0), 
                Eigen::ComputeFullU | Eigen::ComputeFullV
            );
            Eigen::Vector3d values;
            
            switch (regularization_method_) {
                case RegularizationMethod::PLANE:
                    // 平面假设：两个大特征值，一个小特征值
                    values = Eigen::Vector3d(1, 1, 1e-3);
                    break;
                case RegularizationMethod::MIN_EIG:
                    // 最小特征值截断
                    values = svd.singularValues().array().max(1e-3);
                    break;
                case RegularizationMethod::NORMALIZED_MIN_EIG:
                    // 归一化最小特征值截断
                    values = svd.singularValues() / svd.singularValues().maxCoeff();
                    values = values.array().max(1e-3);
                    break;
            }
            
            // 重构协方差矩阵
            covariances[i].setZero();
            covariances[i].block<3, 3>(0, 0) = 
                svd.matrixU() * values.asDiagonal() * svd.matrixV().transpose();
        }
    }
}

2.3 正则化方法详解

方法1：NONE（无正则化）

直接使用计算的协方差矩阵，不进行任何处理。

方法2：FROBENIUS

// 添加小的单位矩阵，然后归一化
C = cov + λ * I
C_inv = C^(-1)
cov_final = (C_inv / ||C_inv||_F)^(-1)

作用：防止协方差矩阵奇异，同时保持其Frobenius范数。

方法3：PLANE（默认）

// 假设点云局部是平面结构
// 特征值设置为 [1, 1, 1e-3]
// 表示：两个主方向（平面内），一个法向方向（平面外，很小）
values = [1, 1, 1e-3]
cov_final = U * diag(values) * V^T

适用场景：大多数点云数据，因为点云通常来自表面扫描。

方法4：MIN_EIG

// 将最小特征值截断到1e-3
values = max(singular_values, 1e-3)
cov_final = U * diag(values) * V^T

作用：防止数值不稳定，同时保持原始特征值比例。

方法5：NORMALIZED_MIN_EIG

// 先归一化，再截断
values = singular_values / max(singular_values)
values = max(values, 1e-3)
cov_final = U * diag(values) * V^T

作用：保持特征值的相对比例，同时防止数值问题。

2.4 完整代码流程

// GICP和VGICP的点级别协方差计算（完全相同）

calculate_covariances(cloud, kdtree, covariances):
    for each point p in cloud:
        1. kdtree.nearestKSearch(p, k=20) → neighbors
        2. neighbors = [p₁, p₂, ..., pₖ]
        3. mean = (1/k) * Σ neighbors
        4. neighbors' = neighbors - mean  // 去中心化
        5. cov = (1/k) * neighbors' * neighbors'^T
        6. cov = regularize(cov)  // 应用正则化
        7. covariances[i] = cov

三、VGICP的体素协方差（VGICP特有）

3.1 体素协方差的作用

VGICP在点级别协方差的基础上，额外引入了体素级别的协方差融合：

1. 点级别协方差：每个点都有自己的协方差（与GICP相同）

2. 体素协方差：将多个点的协方差融合到体素中（VGICP特有）

3.2 体素协方差的计算时机

double FastVGICP::linearize(...) {
    // 首次调用时创建体素地图
    if (voxelmap_ == nullptr) {
        voxelmap_.reset(new GaussianVoxelMap<PointTarget>(voxel_resolution_, voxel_mode_));
        // 创建体素地图时，会融合目标点的协方差
        voxelmap_->create_voxelmap(*target_, target_covs_);
    }
    // ...
}

3.3 体素协方差融合方法

方法1：ADDITIVE（加性融合，默认）

struct AdditiveGaussianVoxel : GaussianVoxel {
    void append(const Eigen::Vector4d& mean_, const Eigen::Matrix4d& cov_) override {
        num_points++;
        mean += mean_;      // 累加均值
        cov += cov_;        // 累加协方差
    }
    
    void finalize() override {
        mean /= num_points;  // 平均均值
        cov /= num_points;   // 平均协方差
    }
};

公式：

对于体素v中的点 {p₁, p₂, ..., pₙ}，每个点有协方差 {cov₁, cov₂, ..., covₙ}：

体素均值：mean_v = (1/n) * Σ mean_i
体素协方差：cov_v = (1/n) * Σ cov_i

特点：

• ✅ 简单直观

• ✅ 计算快速

• ⚠️ 假设所有点权重相同

方法2：MULTIPLICATIVE（乘性融合，信息融合）

struct MultiplicativeGaussianVoxel : GaussianVoxel {
    void append(const Eigen::Vector4d& mean_, const Eigen::Matrix4d& cov_) override {
        num_points++;
        // 信息融合：使用协方差的逆（信息矩阵）
        Eigen::Matrix4d cov_inv = cov_;
        cov_inv(3, 3) = 1;
        cov_inv = cov_inv.inverse().eval();
        
        cov += cov_inv;              // 累加信息矩阵
        mean += cov_inv * mean_;      // 累加加权均值
    }
    
    void finalize() override {
        cov(3, 3) = 1;
        mean[3] = 1;
        
        // 从信息矩阵恢复协方差矩阵
        cov = cov.inverse().eval();
        mean = (cov * mean).eval();
    }
};

公式（信息融合）：

对于体素v中的点 {p₁, p₂, ..., pₙ}，每个点有协方差 {cov₁, cov₂, ..., covₙ}：

信息矩阵：info_v = Σ cov_i^(-1)
加权均值：weighted_mean = Σ cov_i^(-1) * mean_i

体素协方差：cov_v = info_v^(-1)
体素均值：mean_v = cov_v * weighted_mean

特点：

• ✅ 更符合概率论（信息融合）

• ✅ 协方差小的点（更确定）权重更大

• ⚠️ 计算稍慢（需要矩阵求逆）

数学原理：

• 在概率论中，多个高斯分布的融合使用信息矩阵（协方差的逆）

• 信息矩阵越大，表示不确定性越小（更确定）

• 融合时，更确定的点贡献更大

方法3：ADDITIVE_WEIGHTED（加权加性）

与ADDITIVE类似，但可以根据点的其他属性（如强度）进行加权。

3.4 体素协方差的使用

在VGICP的配准过程中，体素协方差用于：

void FastVGICP::update_correspondences(...) {
    // ...
    for (int i = 0; i < voxel_correspondences_.size(); i++) {
        const auto& corr = voxel_correspondences_[i];
        const auto& cov_A = source_covs_[corr.first];      // 源点的协方差（点级别）
        const auto& cov_B = corr.second->cov;             // 体素的协方差（体素级别）
        
        // 组合协方差：RCR = cov_B + R * cov_A * R^T
        Eigen::Matrix4d RCR = cov_B + trans.matrix() * cov_A * trans.matrix().transpose();
        voxel_mahalanobis_[i] = RCR.inverse();
    }
}

关键点：

• cov_A：源点的点级别协方差（与GICP相同）

• cov_B：目标体素的体素级别协方差（VGICP特有）

• 两者组合形成最终的Mahalanobis距离矩阵

四、GICP vs VGICP 协方差对比

4.1 计算流程对比

步骤	GICP	VGICP
1. 点级别协方差	✅ 计算每个点的协方差	✅ 计算每个点的协方差（相同）
2. 体素融合	❌ 无	✅ 将目标点协方差融合到体素
3. 对应关系	点↔点	点↔体素
4. Mahalanobis	cov_B + R*cov_A*R^T	voxel_cov_B + R*point_cov_A*R^T

4.2 代码位置对比

GICP的点级别协方差

// 文件：fast_gicp_impl.hpp
bool FastGICP::calculate_covariances(...) {
    // 1. KDTree搜索k个最近邻
    kdtree.nearestKSearch(cloud->at(i), k, k_indices, ...);
    
    // 2. 计算协方差
    neighbors.colwise() -= neighbors.rowwise().mean();
    cov = neighbors * neighbors.transpose() / k;
    
    // 3. 正则化
    cov = regularize(cov);
}

VGICP的点级别协方差

// VGICP继承自FastGICP，直接使用父类方法
// 文件：fast_vgicp_impl.hpp
void FastVGICP::computeTransformation(...) {
    FastGICP::computeTransformation(...);  // 调用父类，计算点级别协方差
}

VGICP的体素协方差

// 文件：fast_vgicp_voxel.hpp
void GaussianVoxelMap::create_voxelmap(cloud, covs) {
    for each point in cloud:
        coord = voxel_coord(point);
        voxel = lookup_or_create_voxel(coord);
        
        // 融合点协方差到体素
        voxel->append(point_mean, point_cov);
    
    for each voxel:
        voxel->finalize();  // 计算体素协方差
}

4.3 协方差使用对比

GICP中的协方差使用

// 文件：fast_gicp_impl.hpp
void FastGICP::update_correspondences(...) {
    for (int i = 0; i < input_->size(); i++) {
        int target_index = correspondences_[i];
        const auto& cov_A = source_covs_[i];           // 源点协方差
        const auto& cov_B = target_covs_[target_index]; // 目标点协方差
        
        // 组合协方差
        RCR = cov_B + R * cov_A * R^T;
        mahalanobis_[i] = RCR.inverse();
    }
}

VGICP中的协方差使用

// 文件：fast_vgicp_impl.hpp
void FastVGICP::update_correspondences(...) {
    for (int i = 0; i < voxel_correspondences_.size(); i++) {
        const auto& corr = voxel_correspondences_[i];
        const auto& cov_A = source_covs_[corr.first];  // 源点协方差（点级别）
        const auto& cov_B = corr.second->cov;            // 体素协方差（体素级别）
        
        // 组合协方差（与GICP相同公式）
        RCR = cov_B + R * cov_A * R^T;
        voxel_mahalanobis_[i] = RCR.inverse();
    }
}

关键差异：

• GICP：cov_B 是目标点的点级别协方差

• VGICP：cov_B 是目标体素的体素级别协方差（融合了多个点的信息）

五、协方差计算的完整流程

5.1 GICP的协方差流程

输入点云
    ↓
[对每个点]
    ├─ KDTree搜索k个最近邻
    ├─ 计算邻域协方差
    ├─ 正则化
    └─ 存储点级别协方差
    ↓
使用点级别协方差进行配准

5.2 VGICP的协方差流程

输入点云
    ↓
[对每个点] ← 与GICP相同
    ├─ KDTree搜索k个最近邻
    ├─ 计算邻域协方差
    ├─ 正则化
    └─ 存储点级别协方差
    ↓
[体素化目标点云]
    ├─ 将目标点分配到体素
    ├─ 融合每个体素内点的协方差
    └─ 计算体素级别协方差
    ↓
使用点级别协方差（源）+ 体素级别协方差（目标）进行配准

六、关键差异总结

6.1 相同点

1. 点级别协方差计算方法完全相同

   • 都使用KDTree查找k个最近邻

   • 都使用相同的协方差计算公式

   • 都支持相同的正则化方法

2. Mahalanobis距离计算公式相同

   • RCR = cov_B + R * cov_A * R^T

6.2 不同点

特性	GICP	VGICP
点级别协方差	✅ 有	✅ 有（相同）
体素级别协方差	❌ 无	✅ 有（特有）
目标协方差来源	目标点的点级别协方差	目标体素的体素级别协方差
融合方法	无	ADDITIVE / MULTIPLICATIVE
计算复杂度	O(N log N)	O(N log N) + O(N)

6.3 体素协方差的优势

1. 更稳定：融合多个点的信息，减少噪声影响

2. 更高效：体素查找比点查找更快

3. 更鲁棒：体素协方差代表局部区域的统计特性

6.4 体素协方差的劣势

1. 精度损失：体素融合可能丢失细节

2. 参数调优：需要设置合适的体素分辨率

3. 内存占用：需要存储体素地图

七、代码示例

7.1 GICP协方差计算示例

fast_gicp::FastGICP<pcl::PointXYZ, pcl::PointXYZ> gicp;
gicp.setCorrespondenceRandomness(20);  // k=20
gicp.setRegularizationMethod(fast_gicp::RegularizationMethod::PLANE);

gicp.setInputSource(source_cloud);
gicp.setInputTarget(target_cloud);

// 内部会调用：
// calculate_covariances(source_cloud, ...) → source_covs_
// calculate_covariances(target_cloud, ...) → target_covs_
gicp.align(*aligned);

7.2 VGICP协方差计算示例

fast_gicp::FastVGICP<pcl::PointXYZ, pcl::PointXYZ> vgicp;
vgicp.setCorrespondenceRandomness(20);  // k=20（点级别协方差）
vgicp.setRegularizationMethod(fast_gicp::RegularizationMethod::PLANE);
vgicp.setResolution(1.0);  // 体素分辨率
vgicp.setVoxelAccumulationMode(fast_gicp::VoxelAccumulationMode::ADDITIVE);

vgicp.setInputSource(source_cloud);
vgicp.setInputTarget(target_cloud);

// 内部会调用：
// 1. calculate_covariances(source_cloud, ...) → source_covs_（点级别）
// 2. calculate_covariances(target_cloud, ...) → target_covs_（点级别）
// 3. create_voxelmap(target_cloud, target_covs_) → 体素协方差
vgicp.align(*aligned);