C++ Eigen点云距离计算如何写出更优雅高效的非并行代码

原代码核心问题 #

• 显式遍历center列的for循环无法最大化利用Eigen内置的向量化优化，循环本身存在跳转开销
• 每次循环都会生成base.colwise() - center.col(i)的临时大矩阵，1e5规模下内存拷贝、临时对象销毁的开销极高
• 末尾的std::move属于冗余写法，反而会阻碍Eigen的返回值优化(RVO)，降低返回效率

核心优化思路 #

利用平方欧氏距离的代数展开公式完全消除显式循环：
$$(a-b)^2 = a^2 + b^2 - 2ab$$
将逐点距离计算转换为纯矩阵运算，完全由Eigen内部调度向量化指令执行，避免手动循环的额外开销。

假设输入约定：

• base为D行N列矩阵：D是点维度（通常为3），N为基准点云的点数
• center为D行M列矩阵：M是待计算中心点的数量
• 输出为N行M列矩阵，第(i,j)个元素为base第i个点到center第j个点的平方距离

优化后代码 #

#include <Eigen/Dense>

Eigen::MatrixXd Distance(const Eigen::MatrixXd& base, const Eigen::MatrixXd& center) {
    const int N = base.cols();
    const int M = center.cols();
    
    // 分别计算所有base点、center点的平方范数
    Eigen::RowVectorXd base_sq = base.colwise().squaredNorm();
    Eigen::RowVectorXd center_sq = center.colwise().squaredNorm();
    
    // 完全向量化的矩阵运算实现距离计算，无显式循环
    return base_sq.transpose().replicate(1, M) 
           + center_sq.replicate(N, 1) 
           - 2 * (base.transpose() * center);
}

额外优化建议 #

• 如果点云维度固定（比如3维点云），将MatrixXd替换为固定行的Matrix<double, 3, Eigen::Dynamic>（即Matrix3Xd），固定维度的矩阵可以让Eigen做更激进的编译期优化，内存布局也更紧凑
• 编译时开启-O3优化，同时添加-march=native让编译器适配当前CPU的最高指令集（AVX2/AVX512等），可提升向量化效率30%以上
• 若工程已链接MKL/OpenBLAS等高性能线性代数库，定义EIGEN_USE_BLAS宏，可进一步提升大矩阵乘法的运算速度，1e5规模下性能提升可达数倍
• 无需显式使用std::move返回矩阵，Eigen默认开启返回值优化，会直接在返回地址上构造结果对象，避免拷贝

内容的提问来源于stack exchange，提问作者乔德明