PCL 1.15.1的common/eigen.h详细解析

总体定位

PCL的eigen.h是PCL库中最核心的数学工具箱之一，提供了一套高效、专用的3D几何计算函数，是PCL几乎所有高级算法的数学基础。

---

 五大核心功能

1. 特征值与特征向量计算 (最重要)

// 3D点云处理的数学核心
pcl::eigen33(covariance_matrix, eigenvalue, eigenvector);

用途：

• ✅ 法向量估计（最小特征向量 = 法向量）

• ✅ 主成分分析（PCA降维、主方向提取）

• ✅ 曲率计算（特征值比例）

• ✅ 平面拟合质量评估

服务的PCL模块：

• pcl::NormalEstimation - 法向量估计

• pcl::PCA - 主成分分析

• pcl::SACSegmentation - RANSAC平面拟合

---

2. 高效矩阵求逆

pcl::invert3x3SymMatrix(covariance, inverse);  // 对称矩阵专用
pcl::invert3x3Matrix(transform, inverse);       // 通用矩阵

用途：

• ✅ 协方差矩阵求逆（马氏距离计算）

• ✅ 变换矩阵求逆（坐标系转换）

• ✅ Kalman滤波中的协方差更新

性能优势： 比Eigen通用求逆快2-3倍（利用对称性）

---

3. 刚体变换操作

// 欧拉角 ↔ 变换矩阵 双向转换
pcl::getTransformation(x, y, z, roll, pitch, yaw, transform);
pcl::getTranslationAndEulerAngles(transform, x, y, z, roll, pitch, yaw);

用途：

• ✅ 机器人位姿表示（TF变换）

• ✅ 传感器外参管理

• ✅ 点云配准结果表示

• ✅ SLAM中的位姿图优化

服务的PCL模块：

• pcl::transformPointCloud - 点云变换

• pcl::IterativeClosestPoint - ICP配准

• pcl::Registration - 配准基类

---

4. 几何实体变换

pcl::transformPoint(point_in, point_out, transform);      // 点（考虑平移）
pcl::transformVector(vector_in, vector_out, transform);   // 向量（仅旋转）
pcl::transformPlane(plane_in, plane_out, transform);      // 平面方程

关键区别：

• 点变换 = 旋转 + 平移（用于空间位置）

• 向量变换 = 仅旋转（用于法向量、方向向量）

典型应用：

• 多传感器点云融合

• 法向量坐标系转换

• 平面检测结果对齐

---

5. 点集配准算法

Eigen::Matrix4f transform = pcl::umeyama(source_points, target_points);

Umeyama算法：

• 基于SVD的最优刚体变换估计

• 可选缩放因子

• O(d³)复杂度，适合小规模对应点对

用途：

• ICP初始对齐

• 特征点配准

• 相机-激光雷达标定

---

与PCL其他模块的关系

eigen.h (数学基础)
    ↓
┌───────────────┬───────────────┬───────────────┐
│               │               │               │
features      filters      registration    segmentation
(特征提取)    (滤波)        (配准)          (分割)
    ↓               ↓               ↓               ↓
eigen33         transformPoint   umeyama      eigen33
(法向量)        (点云变换)      (初始对齐)   (平面拟合)

---

性能优化设计

1. 专用算法 - 针对3×3矩阵优化（避免通用算法开销）

2. 对称性利用 - 对称矩阵仅计算上三角

3. 分级接口 - 按需选择（仅最小特征值 vs 完整分解）

4. 原地操作 - 支持transformPoint(p, p, T)避免临时变量

---

典型使用场景统计

根据PCL源码中的调用频率：

函数	调用频率	主要使用者
eigen33	⭐⭐⭐⭐⭐	NormalEstimation, MomentOfInertia, PCA
transformPoint	⭐⭐⭐⭐⭐	transformPointCloud, Registration
getTransformation	⭐⭐⭐⭐	ICP, NDT, Calibration
invert3x3SymMatrix	⭐⭐⭐	Covariance相关算法
umeyama	⭐⭐⭐	Registration初始化

---

核心价值

eigen.h是PCL从"点云数据结构"到"高级几何理解"的桥梁

• 无需直接调用 - PCL高级API已封装

• 必须理解原理 - 调试、优化、扩展时必备

• 性能关键路径 - 法向量估计等热点函数都依赖它

---

学习建议

1. 入门必学： eigen33（理解法向量计算）

2. 进阶必学： getTransformation系列（理解坐标变换）

3. 高级应用： umeyama（理解配准原理）

4. 性能优化： 对比通用Eigen和PCL专用函数的性能差异

总结一句话： eigen.h提供了PCL所需的快速3D几何数学运算，是理解PCL算法内部工作原理的必经之路！

eigen.h核心函数的详细介绍

1. 二次方程求根函数

1.1 computeRoots2 - 二次方程求根

函数签名:

template <typename Scalar, typename Roots> void
computeRoots2 (const Scalar &b, const Scalar &c, Roots &roots);

作用:
求解二次方程 (x^2 + bx + c = 0) 的根。

使用方法:

#include <pcl/common/eigen.h>
#include <Eigen/Core>
#include <iostream>

void example_computeRoots2() {
    // 求解方程 x^2 - 5x + 6 = 0
    // 即 (x-2)(x-3) = 0, 根为 2 和 3
    float b = -5.0f;
    float c = 6.0f;
    Eigen::Vector2f roots;
    
    pcl::computeRoots2(b, c, roots);
    
    std::cout << "方程 x^2 + " << b << "x + " << c << " = 0 的根为:" << std::endl;
    std::cout << "x1 = " << roots(0) << std::endl;
    std::cout << "x2 = " << roots(1) << std::endl;
}

**在拟合、曲率计算等几何特征提取

• 在RANSAC等鲁棒估计算法中求解几何约束

---

2. 特征值与特征向量计算函数组

2.1 computeRoots - 特征多项式求根

函数签名:

template <typename Matrix, typename Roots> void
computeRoots (const Matrix &m, Roots &roots);

作用:
计算矩阵的特征多项式的根,即特征值。

使用示例:

void example_computeRoots() {
    // 创建一个3x3对称矩阵
    Eigen::Matrix3f mat;
    mat << 4, 1, 1,
           1, 3, 1,
           1, 1, 2;
    
    Eigen::Vector3f eigenvalues;
    pcl::computeRoots(mat, eigenvalues);
    
    std::cout << "特征值为:" << std::endl;
    std::cout << eigenvalues.transpose() << std::endl;
}

---

2.2 eigen22 - 2×2矩阵特征值分解

两个重载版本:

版本1: 计算最小特征值及其特征向量

template <typename Matrix, typename Vector> void
eigen22 (const Matrix &mat, typename Matrix::Scalar &eigenvalue, Vector &eigenvector);

版本2: 计算所有特征值和特征向量

template <typename Matrix, typename Vector> void
eigen22 (const Matrix &mat, Matrix &eigenvectors, Vector &eigenvalues);

使用示例:

void example_eigen22() {
    // 创建2×2对称矩阵
    Eigen::Matrix2f mat;
    mat << 3, 1,
           1, 2;
    
    // 方法1: 仅获取最小特征值及其特征向量
    float min_eigenvalue;
    Eigen::Vector2f min_eigenvector;
    pcl::eigen22(mat, min_eigenvalue, min_eigenvector);
    
    std::cout << "最小特征值: " << min_eigenvalue << std::endl;
    std::cout << "对应特征向量: " << min_eigenvector.transpose() << std::endl;
    
    // 方法2: 获取所有特征值和特征向量
    Eigen::Matrix2f eigenvectors;
    Eigen::Vector2f eigenvalues;
    pcl::eigen22(mat, eigenvectors, eigenvalues);
    
    std::cout << "\n所有特征值:" << std::endl;
    std::cout << eigenvalues.transpose() << std::endl;
    std::cout << "特征向量矩阵(按列):" << std::endl;
    std::cout << eigenvectors << std::endl;
}

区别说明:

• 版本1: 快速计算,仅返回最小特征值(用于主成分分析中找主方向)

• 版本2: 完整分解,返回所有特征值和特征向量矩阵

---

2.3 eigen33 - 3×3矩阵特征值分解(核心函数)

三个重载版本:

版本1: 计算最小特征值及其特征向量

template <typename Matrix, typename Vector> void
eigen33 (const Matrix &mat, typename Matrix::Scalar &eigenvalue, Vector &eigenvector);

版本2: 仅计算所有特征值

template <typename Matrix, typename Vector> void
eigen33 (const Matrix &mat, Vector &evals);

版本3: 计算所有特征值和特征向量

template <typename Matrix, typename Vector> void
eigen33 (const Matrix &mat, Matrix &evecs, Vector &evals);

完整使用示例:

#include <pcl/common/eigen.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <Eigen/Dense>

void example_eigen33_complete() {
    // 构造协方差矩阵(常见于法向量估计)
    Eigen::Matrix3f covariance_matrix;
    covariance_matrix << 0.5, 0.1, 0.05,
                         0.1, 0.3, 0.02,
                         0.05, 0.02, 0.1;
    
    std::cout << "协方差矩阵:" << std::endl;
    std::cout << covariance_matrix << std::endl;
    
    // ===== 版本1: 仅获取最小特征值(用于法向量计算) =====
    float min_eigenvalue;
    Eigen::Vector3f normal_vector;
    pcl::eigen33(covariance_matrix, min_eigenvalue, normal_vector);
    
    std::cout << "\n=== 版本1: 最小特征值分析 ===" << std::endl;
    std::cout << "最小特征值: " << min_eigenvalue << std::endl;
    std::cout << "法向量(最小特征向量): " << normal_vector.transpose() << std::endl;
    
    // ===== 版本2: 仅获取所有特征值(用于曲率分析) =====
    Eigen::Vector3f eigenvalues;
    pcl::eigen33(covariance_matrix, eigenvalues);
    
    std::cout << "\n=== 版本2: 特征值分析 ===" << std::endl;
    std::cout << "特征值(升序): " << eigenvalues.transpose() << std::endl;
    
    // 计算曲率(表面弯曲程度)
    float curvature = eigenvalues(0) / (eigenvalues(0) + eigenvalues(1) + eigenvalues(2));
    std::cout << "曲率: " << curvature << std::endl;
    
    // ===== 版本3: 完整特征值分解(用于主方向分析) =====
    Eigen::Matrix3f eigenvectors;
    Eigen::Vector3f all_eigenvalues;
    pcl::eigen33(covariance_matrix, eigenvectors, all_eigenvalues);
    
    std::cout << "\n=== 版本3: 完整特征值分解 ===" << std::endl;
    std::cout << "特征值: " << all_eigenvalues.transpose() << std::endl;
    std::cout << "特征向量矩阵:" << std::endl;
    std::cout << eigenvectors << std::endl;
    
    // 主方向分析
    std::cout << "\n主方向分析:" << std::endl;
    std::cout << "第一主方向(最大变化方向): " << eigenvectors.col(2).transpose() << std::endl;
    std::cout << "第二主方向: " << eigenvectors.col(1).transpose() << std::endl;
    std::cout << "第三主方向(法向量): " << eigenvectors.col(0).transpose() << std::endl;
}

在PCL中的关键应用:

1. 法向量估计 (pcl::NormalEstimation):

void demonstrate_normal_estimation_usage() {
    // 模拟NormalEstimation内部使用eigen33的方式
    Eigen::Matrix3f covariance;
    // ... 计算局部点云的协方差矩阵 ...
    
    // 使用eigen33获取法向量
    float eigenvalue;
    Eigen::Vector3f normal;
    pcl::eigen33(covariance, eigenvalue, normal);
    
    // normal就是该点的法向量
    std::cout << "点云法向量: " << normal.transpose() << std::endl;
}

1. 主成分分析 (pcl::PCA):

void demonstrate_pca_usage() {
    Eigen::Matrix3f covariance;
    // ... 计算点云协方差矩阵 ...
    
    Eigen::Matrix3f eigenvectors;
    Eigen::Vector3f eigenvalues;
    pcl::eigen33(covariance, eigenvectors, eigenvalues);
    
    // eigenvectors的列向量就是主成分方向
    std::cout << "第一主成分: " << eigenvectors.col(2).transpose() << std::endl;
}

---

2.4 computeCorrespondingEigenVector - 计算给定特征值的特征向量

函数签名:

template <typename Matrix, typename Vector> void
computeCorrespondingEigenVector (const Matrix &mat, 
                                 const typename Matrix::Scalar &eigenvalue, 
                                 Vector &eigenvector);

作用:
当已知特征值时,计算对应的特征向量。

使用示例:

void example_computeCorrespondingEigenVector() {
    Eigen::Matrix3f mat;
    mat << 6, 2, 1,
           2, 3, 1,
           1, 1, 1;
    
    // 先计算所有特征值
    Eigen::Vector3f eigenvalues;
    pcl::computeRoots(mat, eigenvalues);
    
    // 为第一个特征值计算特征向量
    Eigen::Vector3f eigenvector;
    pcl::computeCorrespondingEigenVector(mat, eigenvalues(0), eigenvector);
    
    std::cout << "特征值 " << eigenvalues(0) << " 的特征向量:" << std::endl;
    std::cout << eigenvector.transpose() << std::endl;
    
    // 验证: mat * eigenvector ≈ eigenvalue * eigenvector
    Eigen::Vector3f result = mat * eigenvector;
    Eigen::Vector3f expected = eigenvalues(0) * eigenvector;
    std::cout << "验证 mat*v: " << result.transpose() << std::endl;
    std::cout << "预期 λ*v: " << expected.transpose() << std::endl;
}

应用场景:

• 当特征值已通过其他方法计算出来时,避免重复计算

• 在迭代算法中,需要针对特定特征值计算特征向量

---

3. 矩阵求逆函数组

3.1 invert2x2 - 2×2矩阵求逆

函数签名:

template <typename Matrix> typename Matrix::Scalar
invert2x2 (const Matrix &matrix, Matrix &inverse);

作用:
计算2×2矩阵的逆矩阵,返回行列式的值。

使用示例:

void example_invert2x2() {
    Eigen::Matrix2f mat;
    mat << 4, 7,
           2, 6;
    
    Eigen::Matrix2f inv;
    float determinant = pcl::invert2x2(mat, inv);
    
    std::cout << "原矩阵:" << std::endl << mat << std::endl;
    std::cout << "\n逆矩阵:" << std::endl << inv << std::endl;
    std::cout << "\n行列式: " << determinant << std::endl;
    
    // 验证: mat * inv = I
    Eigen::Matrix2f identity = mat * inv;
    std::cout << "\n验证 mat * inv = I:" << std::endl << identity << std::endl;
    
    // 判断矩阵是否可逆
    if (std::abs(determinant) < 1e-6) {
        std::cout << "警告: 矩阵接近奇异,不可逆!" << std::endl;
    }
}

注意事项:

• 函数仅使用上三角部分,假设矩阵是对称的

• 返回值为0时表示矩阵奇异,无逆矩阵

• 非对称矩阵会得到错误结果

---

3.2 invert3x3SymMatrix - 3×3对称矩阵求逆

函数签名:

template <typename Matrix> typename Matrix::Scalar
invert3x3SymMatrix (const Matrix &matrix, Matrix &inverse);

作用:
高效计算3×3对称矩阵的逆矩阵。

使用示例:

void example_invert3x3SymMatrix() {
    // 构造对称矩阵(例如协方差矩阵)
    Eigen::Matrix3f covariance;
    covariance << 2.0, 0.5, 0.3,
                  0.5, 1.5, 0.2,
                  0.3, 0.2, 1.0;
    
    Eigen::Matrix3f inv_cov;
    float det = pcl::invert3x3SymMatrix(covariance, inv_cov);
    
    std::cout << "协方差矩阵:" << std::endl << covariance << std::endl;
    std::cout << "\n逆协方差矩阵(精度矩阵):" << std::endl << inv_cov << std << det << std::endl;
    
    // 验证逆矩阵
    Eigen::Matrix3f should_be_identity = covariance * inv_cov;
    std::cout << "\n验证结果(应接近单位矩阵):" << std::endl;
    std::cout << should_be_identity << std::endl;
}

在PCL中的应用:

• 多元高斯分布: 计算精度矩阵(协方差矩阵的逆)

• 马氏距离计算: 需要协方差矩阵的逆

• Kalman滤波: 更新协方差矩阵

实际应用示例:

// 计算马氏距离(Mahalanobis Distance)
void compute_mahalanobis_distance() {
    Eigen::Vector3f point(1.0, 2.0, 3.0);
    Eigen::Vector3f mean(0.5, 1.5, 2.5);
    Eigen::Matrix3f covariance;
    covariance << 1.0, 0.2, 0.1,
                  0.2, 1.5, 0.15,
                  0.1, 0.15, 2.0;
    
    // 计算逆协方差矩阵
    Eigen::Matrix3f inv_cov;
    pcl::invert3x3SymMatrix(covariance, inv_cov);
    
    // 马氏距离: sqrt((x-μ)^T * Σ^(-1) * (x-μ))
    Eigen::Vector3f diff = point - mean;
    float mahal_dist = std::sqrt(diff.transpose() * inv_cov * diff);
    
    std::cout << "马氏距离: " << mahal_dist << std::endl;
}

---

3.3 invert3x3Matrix - 3×3一般矩阵求逆

函数签名:

template <typename Matrix> typename Matrix::Scalar
invert3x3Matrix (const Matrix &matrix, Matrix &inverse);

作用:
计算3×3一般矩阵(不要求对称)的逆矩阵。

使用示例:

void example_invert3x3Matrix() {
    // 非对称矩阵
    Eigen::Matrix3f mat;
    mat << 1, 2, 3,
           0, 1, 4,
           5, 6, 0;
    
    Eigen::Matrix3f inv;
    float det = pcl::invert3x3Matrix(mat, inv);
    
    std::cout << "原矩阵:" << std::endl << mat << std::endl;
    std::cout << "\n逆矩阵:" << std::endl << inv << std::endl;
    std::cout << "\n行列式: " << det << std::endl;
    
    // 验证
    Eigen::Matrix3f identity = mat * inv;
    std::cout << "\n验证:" << std::endl << identity << std::endl;
}

与 invert3x3SymMatrix 的区别:

特性	invert3x3SymMatrix	invert3x3Matrix
适用矩阵	仅对称矩阵	任意矩阵
计算效率	更快(利用对称性)	较慢
使用数据	仅上三角	完整矩阵
典型应用	协方差矩阵求逆	变换矩阵求逆

---

3.4 determinant3x3Matrix - 3×3矩阵行列式

函数签名:

template <typename Matrix> typename Matrix::Scalar
determinant3x3Matrix (const Matrix &matrix);

使用示例:

void example_determinant3x3() {
    Eigen::Matrix3f mat;
    mat << 6, 1, 1,
           4, -2, 5,
           2, 8, 7;
    
    float det = pcl::determinant3x3Matrix(mat);
    
    std::cout << "矩阵:" << std::endl << mat << std::endl;
    std::cout << "行列式: " << det << std::endl;
    
    // 判断矩阵性质
    if (std::abs(det) < 1e-6) {
        std::cout << "矩阵奇异(不可逆)" << std::endl;
    } else if (det > 0) {
        std::cout << "保向变换(不改变手性)" << std::endl;
    } else {
        std::cout << "反向变换(改变手性)" << std::endl;
    }
}

应用场景:

• 判断矩阵是否可逆

• 计算变换的体积缩放因子

• 判断坐标系手性

---

4. 坐标变换构建函数组

4.1 getTransFromUnitVectorsZY - 从Z轴和Y方向构建变换

两个重载版本:

版本1: 通过引用返回

inline void
getTransFromUnitVectorsZY (const Eigen::Vector3f& z_axis, 
                           const Eigen::Vector3f& y_direction,
                           Eigen::Affine3f& transformation);

版本2: 直接返回

inline Eigen::Affine3f
getTransFromUnitVectorsZY (const Eigen::Vector3f& z_axis, 
                           const Eigen::Vector3f& y_direction);

使用示例:

void example_getTransFromUnitVectorsZY() {
    // 定义一个新坐标系
    Eigen::Vector3f z_axis(0.0, 0.0, 1.0);      // 新Z轴指向世界Z
    Eigen::Vector3f y_direction(0.0, 1.0, 0.0); // Y方向指向世界Y
    
    // 方法1: 引用返回
    Eigen::Affine3f transform1;
    pcl::getTransFromUnitVectorsZY(z_axis, y_direction, transform1);
    
    // 方法2: 直接返回
    Eigen::Affine3f transform2 = pcl::getTransFromUnitVectorsZY(z_axis, y_direction);
    
    std::cout << "变换矩阵:" << std::endl;
    std::cout << transform1.matrix() << std::endl;
    
    // 测试: 变换后Z轴应该指向(0,0,1)
    Eigen::Vector3f world_z(0, 0, 1);
    Eigen::Vector3f transformed_z;
    pcl::transformVector(world_z, transformed_z, transform1);
    std::cout << "\n变换后的Z轴: " << transformed_z.transpose() << std::endl;
}

实际应用 - 建立局部坐标系:

void establish_local_coordinate_system() {
    // 场景: 在点云表面建立局部坐标系
    Eigen::Vector3f surface_normal(0.577, 0.577, 0.577); // 表面法向量
    surface_normal.normalize();
    
    Eigen::Vector3f arbitrary_direction(1, 0, 0); // 任意方向
    
    // 建立局部坐标系: Z轴 = 法向量
    Eigen::Affine3f local_frame = 
        pcl::getTransFromUnitVectorsZY(surface_normal, arbitrary_direction);
    
    std::cout << "局部坐标系变换:" << std::endl;
    std::cout << local_frame.matrix() << std::endl;
    
    // 应用: 将点变换到局部坐标系
    Eigen::Vector3f world_point(1, 2, 3);
    Eigen::Vector3f local_point;
    pcl::transformPoint(world_point, local_point, local_frame.inverse());
    
    std::cout << "世界坐标: " << world_point.transpose() << std::endl;
    std::cout << "局部坐标: " << local_point.transpose() << std::endl;
}

---

4.2 getTransFromUnitVectorsXY - 从X轴和Y方向构建变换

使用示例:

void example_getTransFromUnitVectorsXY() {
    Eigen::Vector3f x_axis(1, 0, 0);
    Eigen::Vector3f y_direction(0, 1, 0);
    
    Eigen::Affine3f transform = 
        pcl::getTransFromUnitVectorsXY(x_axis, y_direction);
    
    std::cout << "从X-Y构建的变换:" << std::endl;
    std::cout << transform.matrix() << std::endl;
}

---

4.3 getTransformationFromTwoUnitVectors - 从两个单位向量构建变换

函数签名:

inline void
getTransformationFromTwoUnitVectors (const Eigen::Vector3f& y_direction, 
                                     const Eigen::Vector3f& z_axis,
                                     Eigen::Affine3f& transformation);

使用示例:

void example_getTransformationFromTwoUnitVectors() {
    // 定义Y方向和Z轴
    Eigen::Vector3f y_dir(0, 1, 0);
    Eigen::Vector3f z_axis(0, 0, 1);
    
    Eigen::Affine3f transform;
    pcl::getTransformationFromTwoUnitVectors(y_dir, z_axis, transform);
    
    std::cout << "变换矩阵:" << std::endl;
    std::cout << transform.matrix() << std::endl;
}

在PCL中的应用 - 对齐点云:

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

void align_cloud_to_normal() {
    // 假设我们有一个点云和它的主法向量
    Eigen::Vector3f main_normal(0.5, 0.5, 0.707);
    main_normal.normalize();
    
    Eigen::Vector3f y_hint(0, 1, 0); // Y方向提示
    
    // 构建将法向量对齐到Z轴的变换
    Eigen::Affine3f align_transform;
    pcl::getTransformationFromTwoUnitVectors(y_hint, main_normal, align_transform);
    
    // 使用逆变换将点云对齐
    Eigen::Affine3f inverse_transform = align_transform.inverse();
    
    // 应用到点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    // ... 填充点云 ...
    
    for (auto& point : cloud->points) {
        Eigen::Vector3f p(point.x, point.y, point.z);
        Eigen::Vector3f p_transformed;
        pcl::transformPoint(p, p_transformed, inverse_transform);
        point.x = p_transformed.x();
        point.y = p_transformed.y();
        point.z = p_transformed.z();
    }
    
    std::cout << "点云已对齐到法向量方向" << std::endl;
}

---

4.4 getTransformationFromTwoUnitVectorsAndOrigin - 包含平移的完整变换

函数签名:

inline void
getTransformationFromTwoUnitVectorsAndOrigin (
    const Eigen::Vector3f& y_direction, 
    const Eigen::Vector3f& z_axis,
    const Eigen::Vector3f& origin, 
    Eigen::Affine3f& transformation);

作用:
构建完整的刚体变换,包括旋转和平移。

使用示例:

void example_getTransformationFromTwoUnitVectorsAndOrigin() {
    // 定义局部坐标系
    Eigen::Vector3f y_direction(0, 1, 0);
    Eigen::Vector3f z_axis(0, 0, 1);
    Eigen::Vector3f origin(1, 2, 3); // 局部原点在世界坐标系中的位置
    
    Eigen::Affine3f transform;
    pcl::getTransformationFromTwoUnitVectorsAndOrigin(
        y_direction, z_axis, origin, transform);
    
    std::cout << "完整变换矩阵(包含平移):" << std::endl;
    std::cout << transform.matrix() << std::endl;
    
    // 验证原点变换
    Eigen::Vector3f local_origin(0, 0, 0);
    Eigen::Vector3f world_origin;
    pcl::transformPoint(local_origin, world_origin, transform);
    
    std::cout << "\n局部原点(0,0,0)变换到世界坐标:" << std::endl;
    std::cout << world_origin.transpose() << std::endl;
    std::cout << "应该等于: " << origin.transpose() << std::endl;
}

实际应用 - 物体姿态估计:

void estimate_object_pose() {
    // 场景: 估计桌面上物体的位姿
    
    // 物体的上表面法向量(世界坐标系)
    Eigen::Vector3f surface_normal(0, 0, 1);
    
    // 物体的前方向
    Eigen::Vector3f front_direction(1, 0, 0);
    
    // 物体中心位置
    Eigen::Vector3f object_center(0.5, 0.3, 0.8);
    
    // 构建物体坐标系到世界坐标系的变换
    Eigen::Affine3f object_pose;
    pcl::getTransformationFromTwoUnitVectorsAndOrigin(
        front_direction,  // Y方向
        surface_normal,   // Z轴(法向量)
        object_center,    // 原点
        object_pose
    );
    
    std::cout << "物体姿态矩阵:" << std::endl;
    std::cout << object_pose.matrix() << std::endl;
    
    // 现在可以将物体模型点云变换到世界坐标系
    Eigen::Vector3f model_point(0.1, 0, 0); // 物体坐标系中的点
    Eigen::Vector3f world_point;
    pcl::transformPoint(model_point, world_point, object_pose);
    
    std::cout << "\n模型点 " << model_point.transpose() 
              << " 在世界坐标系中: " << world_point.transpose() << std::endl;
}

---

5. 欧拉角与变换转换函数组

5.1 getEulerAngles - 从变换矩阵提取欧拉角

函数签名:

template <typename Scalar> void
getEulerAngles (const Eigen::Transform<Scalar, 3, Eigen::Affine> &t, 
                Scalar &roll, Scalar &pitch, Scalar &yaw);

作用:
从仿射变换矩阵中提取欧拉角(ZYX内旋顺序)。

使用示例:

void example_getEulerAngles() {
    // 创建一个旋转变换
    Eigen::Affine3f transform = Eigen::Affine3f::Identity();
    
    // 绕Z轴旋转45度(yaw)
    transform.rotate(Eigen::AngleAxisf(M_PI/4, Eigen::Vector3f::UnitZ()));
    // 绕Y轴旋转30度(pitch)
    transform.rotate(Eigen::AngleAxisf(M_PI/6, Eigen::Vector3f::UnitY()));
    // 绕X轴旋转20度(roll)
    transform.rotate(Eigen::AngleAxisf(M_PI/9, Eigen::Vector3f::UnitX()));
    
    float roll, pitch, yaw;
    pcl::getEulerAngles(transform, roll, pitch, yaw);
    
    std::cout << "欧拉角(弧度):" << std::endl;
    std::cout << "Roll (X轴):  " << roll << " rad = " << roll*180/M_PI << "°" << std::endl;
    std::cout << "Pitch (Y轴): " << pitch << " rad = " << pitch*180/M_PI << "°" << std::endl;
    std::cout << "Yaw (Z轴):   " << yaw << " rad = " << yaw*180/M_PI << "°" << std::endl;
}

注意事项:

• 使用ZYX内旋(intrinsic)顺序

• 适用于航空航天、机器人等领域的姿态表示

• 存在万向节死锁问题(pitch = ±90°时)

---

5.2 getTranslationAndEulerAngles - 提取平移和欧拉角

函数签名:

template <typename Scalar> void
getTranslationAndEulerAngles (const Eigen::Transform<Scalar, 3, Eigen::Affine> &t,
                              Scalar &x, Scalar &y, Scalar &z,
                              Scalar &roll, Scalar &pitch, Scalar &yaw);

使用示例:

void example_getTranslationAndEulerAngles() {
    // 创建完整的刚体变换(平移+旋转)
    Eigen::Affine3f transform = Eigen::Affine3f::Identity();
    
    // 设置平移
    transform.translation() << 1.5, 2.0, 3.5;
    
    // 设置旋转
    transform.rotate(Eigen::AngleAxisf(M_PI/4, Eigen::Vector3f::UnitZ()));
    transform.rotate(Eigen::AngleAxisf(M_PI/6, Eigen::Vector3f::UnitY()));
    transform.rotate(Eigen::AngleAxisf(M_PI/9, Eigen::Vector3f::UnitX()));
    
    float x, y, z, roll, pitch, yaw;
    pcl::getTranslationAndEulerAngles(transform, x, y, z, roll, pitch, yaw);
    
    std::cout << "平移:" << std::endl;
    std::cout << "x = " << x << ", y = " << y << ", z = " << z << std::endl;
    
    std::cout << "\n旋转(欧拉角,度):" << std::endl;
    std::cout << "Roll:  " << roll*180/M_PI << "°" << std::endl;
    std::cout << "Pitch: " << pitch*180/M_PI << "°" << std::endl;
    std::cout << "Yaw:   " << yaw*180/M_PI << "°" << std::endl;
}

实际应用 - 机器人位姿分析:

void analyze_robot_pose() {
    // 假设从TF或SLAM系统获得机器人位姿
    Eigen::Affine3f robot_pose = Eigen::Affine3f::Identity();
    robot_pose.translation() << 2.5, 1.8, 0.0;
    robot_pose.rotate(Eigen::AngleAxisf(M_PI/3, Eigen::Vector3f::UnitZ()));
    
    float x, y, z, roll, pitch, yaw;
    pcl::getTranslationAndEulerAngles(robot_pose, x, y, z, roll, pitch, yaw);
    
    std::cout << "机器人位置: (" << x << ", " << y << ", " << z << ")" << std::endl;
    std::cout << "机器人朝向: " << yaw*180/M_PI << "°" << std::endl;
    
    // 判断机器人是否倾斜
    if (std::abs(roll) > 0.1 || std::abs(pitch) > 0.1) {
        std::cout << "警告: 机器人倾斜!" << std::endl;
        std::cout << "  Roll: " << roll*180/M_PI << "°" << std::endl;
        std::cout << "  Pitch: " << pitch*180/M_PI << "°" << std::endl;
    }
}

---

5.3 getTransformation - 从欧拉角和平移构建变换

函数签名:

template <typename Scalar> void
getTransformation (Scalar x, Scalar y, Scalar z, 
                   Scalar roll, Scalar pitch, Scalar yaw, 
                   Eigen::Transform<Scalar, 3, Eigen::Affine> &t);

使用示例:

void example_getTransformation() {
    // 定义位姿参数
    float x = 1.0, y = 2.0, z = 0.5;
    float roll = 0.0;           // 无滚转
    float pitch = 0.0;          // 无俯仰
    float yaw = M_PI / 4;       // 绕Z轴45度
    
    Eigen::Affine3f transform;
    pcl::getTransformation(x, y, z, roll, pitch, yaw, transform);
    
    std::cout << "从欧拉角构建的变换矩阵:" << std::endl;
    std::cout << transform.matrix() << std::endl;
    
    // 验证: 提取参数应该与输入相同
    float x_out, y_out, z_out, roll_out, pitch_out, yaw_out;
    pcl::getTranslationAndEulerAngles(transform, 
        x_out, y_out, z_out, roll_out, pitch_out, yaw_out);
    
    std::cout << "\n验证提取的参数:" << std::endl;
    std::cout << "输入: (" << x << ", " << y << ", " << z << "), "
              << "(" << roll << ", " << pitch << ", " << yaw << ")" << std::endl;
    std::cout << "输出: (" << x_out << ", " << y_out << ", " << z_out << "), "
              << "(" << roll_out << ", " << pitch_out << ", " << yaw_out << ")" << std::endl;
}

便捷重载版本:

void example_getTransformation_overloads() {
    // 直接返回版本(更简洁)
    Eigen::Affine3f transform = pcl::getTransformation(
        1.0f, 2.0f, 3.0f,      // 平移
        0.1f, 0.2f, 0.3f       // 旋转
    );
    
    std::cout << "使用便捷接口构建的变换:" << std::endl;
    std::cout << transform.matrix() << std::endl;
}

实际应用 - 相机外参设置:

void setup_camera_extrinsics() {
    // 相机相对于机器人底盘的位姿
    // 相机位置: 在机器人前方0.3m, 上方0.5m
    float cam_x = 0.3, cam_y = 0.0, cam_z = 0.5;
    
    // 相机姿态: 向下俯视15度
    float cam_roll = 0.0;
    float cam_pitch = -15.0 * M_PI / 180.0;  // 向下俯视
    float cam_yaw = 0.0;
    
    Eigen::Affine3f camera_to_base;
    pcl::getTransformation(cam_x, cam_y, cam_z, 
                          cam_roll, cam_pitch, cam_yaw, 
                          camera_to_base);
    
    std::cout << "相机外参矩阵:" << std::endl;
    std::cout << camera_to_base.matrix() << std::endl;
    
    // 使用: 将相机坐标系中的点变换到机器人坐标系
    Eigen::Vector3f point_in_camera(0, 0, 1);  // 相机前方1米
    Eigen::Vector3f point_in_base;
    pcl::transformPoint(point_in_camera, point_in_base, camera_to_base);
    
    std::cout << "\n相机前方1m的点在机器人坐标系中: " 
              << point_in_base.transpose() << std::endl;
}

---

6. 点、向量、平面变换函数组

6.1 transformPoint - 点变换

函数签名:

template<typename Scalar> inline void
transformPoint (const Eigen::Matrix<Scalar, 3, 1> &point_in,
                Eigen::Matrix<Scalar, 3, 1> &point_out,
                const Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation);

作用:
使用仿射变换矩阵变换3D点(考虑平移)。

使用示例:

void example_transformPoint() {
    // 创建变换: 平移(1,2,3) + 绕Z轴旋转90度
    Eigen::Affine3f transform = Eigen::Affine3f::Identity();
    transform.translation() << 1, 2, 3;
    transform.rotate(Eigen::AngleAxisf(M_PI/2, Eigen::Vector3f::UnitZ()));
    
    // 原始点
    Eigen::Vector3f point_in(1, 0, 0);
    Eigen::Vector3f point_out;
    
    pcl::transformPoint(point_in, point_out, transform);
    
    std::cout << "原始点: " << point_in.transpose() << std::endl;
    std::cout << "变换后: " << point_out.transpose() << std::endl;
    
    // 可以原地变换
    Eigen::Vector3f point(2, 3, 4);
    pcl::transformPoint(point, point, transform);  // point_in = point_out
    std::cout << "原地变换后: " << point.transpose() << std::endl;
}

批量点云变换:

void transform_point_cloud_example() {
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    
    // 添加示例点
    cloud->points.push_back(pcl::PointXYZ(1, 0, 0));
    cloud->points.push_back(pcl::PointXYZ(0, 1, 0));
    cloud->points.push_back(pcl::PointXYZ(0, 0, 1));
    cloud->width = 3;
    cloud->height = 1;
    
    // 创建变换
    Eigen::Affine3f transform = pcl::getTransformation(
        2.0f, 3.0f, 4.0f,      // 平移
        0.0f, 0.0f, M_PI/4     // 绕Z轴45度
    );
    
    // 逐点变换
    for (auto& point : cloud->points) {
        Eigen::Vector3f p_in(point.x, point.y, point.z);
        Eigen::Vector3f p_out;
        pcl::transformPoint(p_in, p_out, transform);
        
        point.x = p_out.x();
        point.y = p_out.y();
        point.z = p_out.z();
    }
    
    std::cout << "点云变换完成, 共 " << cloud->size() << " 个点" << std::endl;
}

---

6.2 transformVector - 向量变换

函数签名:

template <typename Scalar> inline void
transformVector (const Eigen::Matrix<Scalar, 3, 1> &vector_in,
                 Eigen::Matrix<Scalar, 3, 1> &vector_out,
                 const Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation);

作用:
仅使用旋转部分变换向量(不考虑平移)。

使用示例:

void example_transformVector() {
    // 创建变换
    Eigen::Affine3f transform = Eigen::Affine3f::Identity();
    transform.translation() << 10, 20, 30;  // 大平移
    transform.rotate(Eigen::AngleAxisf(M_PI/2, Eigen::Vector3f::UnitZ()));
    
    Eigen::Vector3f vector_in(1, 0, 0);
    Eigen::Vector3f vector_out;
    
    pcl::transformVector(vector_in, vector_out, transform);
    
    std::cout << "原始向量: " << vector_in.transpose() << std::endl;
    std::cout << "变换后向量: " << vector_out.transpose() << std::endl;
    std::cout << "注意: 向量只受旋转影响,不受平移影响" << std::endl;
}

与 transformPoint 的对比:

void compare_point_vector_transform() {
    Eigen::Affine3f transform = Eigen::Affine3f::Identity();
    transform.translation() << 5, 0, 0;
    transform.rotate(Eigen::AngleAxisf(M_PI/2, Eigen::Vector3f::UnitZ()));
    
    Eigen::Vector3f v(1, 0, 0);
    Eigen::Vector3f v_as_point, v_as_vector;
    
    pcl::transformPoint(v, v_as_point, transform);
    pcl::transformVector(v, v_as_vector, transform);
    
    std::cout << "原始: " << v.transpose() << std::endl;
    std::cout << "作为点变换: " << v_as_point.transpose() 
              << " (受平移影响)" << std::endl;
    std::cout << "作为向量变换: " << v_as_vector.transpose() 
              << " (仅旋转)" << std::endl;
}

实际应用 - 法向量变换:

void transform_normals() {
    // 变换点云的同时变换法向量
    pcl::PointCloud<pcl::PointNormal>::Ptr cloud(
        new pcl::PointCloud<pcl::PointNormal>);
    
    // 添加带法向量的点
    pcl::PointNormal pn;
    pn.x = 1; pn.y = 0; pn.z = 0;
    pn.normal_x = 0; pn.normal_y = 0; pn.normal_z = 1;
    cloud->points.push_back(pn);
    
    Eigen::Affine3f transform = pcl::getTransformation(
        1.0f, 2.0f, 3.0f,
        0.0f, M_PI/4, 0.0f
    );
    
    for (auto& point : cloud->points) {
        // 变换点位置
        Eigen::Vector3f pos(point.x, point.y, point.z);
        Eigen::Vector3f pos_new;
        pcl::transformPoint(pos, pos_new, transform);
        
        // 变换法向量(仅旋转!)
        Eigen::Vector3f normal(point.normal_x, point.normal_y, point.normal_z);
        Eigen::Vector3f normal_new;
        pcl::transformVector(normal, normal_new, transform);
        
        point.x = pos_new.x();
        point.y = pos_new.y();
        point.z = pos_new.z();
        point.normal_x = normal_new.x();
        point.normal_y = normal_new.y();
        point.normal_z = normal_new.z();
    }
    
    std::cout << "点和法向量已正确变换" << std::endl;
}

---

6.3 transformLine - 直线变换

函数签名:

template <typename Scalar> bool
transformLine (const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &line_in,
               Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &line_out,
               const Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation);

作用:
变换3D直线。直线表示为6D向量: [起点(3), 方向(3)]。

使用示例:

void example_transformLine() {
    // 定义直线: 起点(1,2,3), 方向(1,0,0)
    Eigen::Matrix<float, 6, 1> line_in;
    line_in << 1, 2, 3,    // 起点
               1, 0, 0;    // 方向
    
    // 创建变换
    Eigen::Affine3f transform = pcl::getTransformation(
        5.0f, 0.0f, 0.0f,         // 平移
        0.0f, 0.0f, M_PI/2        // 绕Z轴90度
    );
    
    Eigen::Matrix<float, 6, 1> line_out;
    bool success = pcl::transformLine(line_in, line_out, transform);
    
    if (success) {
        std::cout << "原始直线:" << std::endl;
        std::cout << "  起点: " << line_in.head<3>().transpose() << std::endl;
        std::cout << "  方向: " << line_in.tail<3>().transpose() << std::endl;
        
        std::cout << "变换后直线:" << std::endl;
        std::cout << "  起点: " << line_out.head<3>().transpose() << std::endl;
        std::cout << "  方向: " << line_out.tail<3>().transpose() << std::endl;
    }
}

实际应用 - 边缘检测后的直线变换:

void transform_detected_lines() {
    // 假设从边缘检测获得直线
    std::vector<Eigen::Matrix<float, 6, 1>> lines;
    
    // 添加检测到的直线
    Eigen::Matrix<float, 6, 1> line1;
    line1 << 0, 0, 0, 1, 0, 0;  // X轴方向的直线
    lines.push_back(line1);
    
    // 相机到世界坐标系的变换
    Eigen::Affine3f camera_to_world = pcl::getTransformation(
        0.0f, 0.0f, 1.5f,         // 相机高度1.5m
        0.0f, -M_PI/6, 0.0f       // 向下俯视30度
    );
    
    // 变换所有直线到世界坐标系
    std::vector<Eigen::Matrix<float, 6, 1>> world_lines;
    for (const auto& line : lines) {
        Eigen::Matrix<float, 6, 1> world_line;
        if (pcl::transformLine(line, world_line, camera_to_world)) {
            world_lines.push_back(world_line);
        }
    }
    
    std::cout << "变换了 " << world_lines.size() << " 条直线到世界坐标系" << std::endl;
}

---

6.4 transformPlane - 平面变换

两个重载版本:

版本1: 使用Eigen向量

template <typename Scalar> void
transformPlane (const Eigen::Matrix<Scalar, 4, 1> &plane_in,
                Eigen::Matrix<Scalar, 4, 1> &plane_out,
                const Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation);

版本2: 使用PCL ModelCoefficients

template<typename Scalar> void
transformPlane (const pcl::ModelCoefficients::ConstPtr plane_in,
                pcl::ModelCoefficients::Ptr plane_out,
                const Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation);

作用:
变换平面方程 ax + by + cz + d = 0 的系数。

使用示例:

void example_transformPlane() {
    // 定义平面: z = 0 (即 0x + 0y + 1z + 0 = 0)
    Eigen::Vector4f plane_in(0, 0, 1, 0);
    
    // 创建变换: 平移z轴方向5个单位
    Eigen::Affine3f transform = Eigen::Affine3f::Identity();
    transform.translation() << 0, 0, 5;
    
    Eigen::Vector4f plane_out;
    pcl::transformPlane(plane_in, plane_out, transform);
    
    std::cout << "原始平面方程: " 
              << plane_in(0) << "x + " 
              << plane_in(1) << "y + " 
              << plane_in(2) << "z + " 
              << plane_in(3) << " = 0" << std::endl;
    
    std::cout << "变换后平面: " 
              << plane_out(0) << "x + " 
              << plane_out(1) << "y + " 
              << plane_out(2) << "z + " 
              << plane_out(3) << " = 0" << std::endl;
    
    std::cout << "平面从 z=0 变换到 z=-5" << std::endl;
}

使用ModelCoefficients版本:

void example_transformPlane_modelcoeffs() {
    // 使用PCL的ModelCoefficients
    pcl::ModelCoefficients::Ptr plane_in(new pcl::ModelCoefficients);
    plane_in->values.resize(4);
    plane_in->values[0] = 0;  // a
    plane_in->values[1] = 0;  // b
    plane_in->values[2] = 1;  // c
    plane_in->values[3] = -2; // d (平面 z = 2)
    
    Eigen::Affine3f transform = pcl::getTransformation(
        0.0f, 0.0f, 3.0f,     // 向上平移3m
        0.0f, M_PI/6, 0.0f    // 绕Y轴旋转30度
    );
    
    pcl::ModelCoefficients::Ptr plane_out(new pcl::ModelCoefficients);
    pcl::transformPlane(plane_in, plane_out, transform);
    
    std::cout << "变换后平面系数: ";
    for (float v : plane_out->values) {
        std::cout << v << " ";
    }
    std::cout << std::endl;
}

实际应用 - 多传感器平面融合:

void fuse_planes_from_multiple_sensors() {
    // 场景: 两个传感器检测到地面
    
    // 传感器1检测的平面(传感器1坐标系)
    Eigen::Vector4f plane_sensor1(0, 0, 1, -0.1);  // 地面高度0.1m
    
    // 传感器1到世界坐标系的变换
    Eigen::Affine3f sensor1_to_world = pcl::getTransformation(
        1.0f, 0.0f, 0.5f,
        0.0f, 0.0f, 0.0f
    );
    
    // 将平面变换到世界坐标系
    Eigen::Vector4f plane_world;
    pcl::transformPlane(plane_sensor1, plane_world, sensor1_to_world);
    
    std::cout << "传感器1的平面在世界坐标系: ";
    std::cout << plane_world.transpose() << std::endl;
    
    // 现在可以与其他传感器的平面进行比较或融合
}

验证平面变换的正确性:

void verify_plane_transform() {
    Eigen::Vector4f plane(1, 0, 0, -5);  // 平面: x = 5
    
    Eigen::Affine3f transform = Eigen::Affine3f::Identity();
    transform.translation() << 3, 0, 0;  // 向x方向平移3
    
    Eigen::Vector4f plane_transformed;
    pcl::transformPlane(plane, plane_transformed, transform);
    
    // 验证: 原平面上的点变换后应该在新平面上
    Eigen::Vector3f point_on_plane(5, 1, 2);  // 在原平面上
    Eigen::Vector3f point_transformed;
    pcl::transformPoint(point_on_plane, point_transformed, transform);
    
    // 检查变换后的点是否在变换后的平面上
    float dist = plane_transformed.head<3>().dot(point_transformed) 
                 + plane_transformed(3);
    
    std::cout << "点到变换后平面的距离: " << dist << " (应接近0)" << std::endl;
}

---

7. 坐标系验证函数

7.1 checkCoordinateSystem - 检查坐标系有效性

两个重载版本:

版本1: 使用直线表示

template<typename Scalar> bool
checkCoordinateSystem (const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &line_x,
                       const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &line_y,
                       const Scalar norm_limit = 1e-3,
                       const Scalar dot_limit = 1e-3);

版本2: 使用原点和方向向量

template <typename Scalar> inline bool
checkCoordinateSystem (const Eigen::Matrix<Scalar, 3, 1> &origin,
                       const Eigen::Matrix<Scalar, 3, 1> &x_direction,
                       const Eigen::Matrix<Scalar, 3, 1> &y_direction,
                       const Scalar norm_limit = 1e-3,
                       const Scalar dot_limit = 1e-3);

作用:
验证坐标系是否满足正交归一化要求。

使用示例:

void example_checkCoordinateSystem() {
    Eigen::Vector3f origin(0, 0, 0);
    Eigen::Vector3f x_dir(1, 0, 0);
    Eigen::Vector3f y_dir(0, 1, 0);
    
    // 检查有效的坐标系
    bool valid = pcl::checkCoordinateSystem(origin, x_dir, y_dir);
    std::cout << "标准正交坐标系: " << (valid ? "有效" : "无效") << std::endl;
    
    // 检查无效的坐标系(未归一化)
    Eigen::Vector3f x_not_normalized(2, 0, 0);
    bool invalid1 = pcl::checkCoordinateSystem(origin, x_not_normalized, y_dir);
    std::cout << "未归一化的X轴: " << (invalid1 ? "有效" : "无效") << std::endl;
    
    // 检查无效的坐标系(不正交)
    Eigen::Vector3f y_not_orthogonal(0.1, 1, 0);
    bool invalid2 = pcl::checkCoordinateSystem(origin, x_dir, y_not_orthogonal);
    std::cout << "不正交的坐标系: " << (invalid2 ? "有效" : "无效") << std::endl;
}

使用直线表示版本:

void example_checkCoordinateSystem_lines() {
    // 直线格3), direction(3)]
    Eigen::Matrix<float, 6, 1> line_x, line_y;
    
    // X轴: 从原点(0,0,0)出发,方向(1,0,0)
    line_x << 0, 0, 0, 1, 0, 0;
    
    // Y轴: 从原点(0,0,0)出发,方向(0,1,0)
    line_y << 0, 0, 0, 0, 1, 0;
    
    bool valid = pcl::checkCoordinateSystem(line_x, line_y);
    std::cout << "坐标系检查: " << (valid ? "通过" : "失败") << std::endl;
    
    // 错误情况: 原点不同
    Eigen::Matrix<float, 6, 1> line_y_wrong_origin;
    line_y_wrong_origin << 1, 0, 0, 0, 1, 0;  // 原点在(1,0,0)
    
    bool invalid = pcl::checkCoordinateSystem(line_x, line_y_wrong_origin);
    std::cout << "原点不同的坐标系: " << (invalid ? "通过" : "失败") << std::endl;
}

实际应用 - 验证传感器标定结果:

void validate_sensor_calibration() {
    // 从标定算法获得传感器坐标系
    Eigen::Affine3f sensor_frame = Eigen::Affine3f::Identity();
    // ... 标定过程 ...
    
    // 提取坐标轴
    Eigen::Vector3f origin = sensor_frame.translation();
    Eigen::Vector3f x_axis = sensor_frame.linear().col(0);
    Eigen::Vector3f y_axis = sensor_frame.linear().col(1);
    Eigen::Vector3f z_axis = sensor_frame.linear().col(2);
    
    // 验证X-Y轴
    bool xy_valid = pcl::checkCoordinateSystem(origin, x_axis, y_axis, 1e-4, 1e-4);
    // 验证Y-Z轴
    bool yz_valid = pcl::checkCoordinateSystem(origin, y_axis, z_axis, 1e-4, 1e-4);
    // 验证Z-X轴
    bool zx_valid = pcl::checkCoordinateSystem(origin, z_axis, x_axis, 1e-4, 1e-4);
    
    if (xy_valid && yz_valid && zx_valid) {
        std::cout << "传感器标定结果有效,坐标系正交归一化" << std::endl;
    } else {
        std::cout << "警告: 传感器标定结果存在问题!" << std::endl;
        std::cout << "  X-Y检查: " << (xy_valid ? "通过" : "失败") << std::endl;
        std::cout << "  Y-Z检查: " << (yz_valid ? "通过" : "失败") << std::endl;
        std::cout << "  Z-X检查: " << (zx_valid ? "通过" : "失败") << std::endl;
    }
}

---

8. 坐标系间变换计算

8.1 transformBetween2CoordinateSystems - 计算坐标系间变换

函数签名:

template <typename Scalar> bool
transformBetween2CoordinateSystems (
    const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> from_line_x,
    const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> from_line_y,
    const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> to_line_x,
    const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> to_line_y,
    Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation);

作用:
计算从一个坐标系到另一个坐标系的变换矩阵。

使用示例:

void example_transformBetween2CoordinateSystems() {
    // 源坐标系(标准坐标系)
    Eigen::Matrix<float, 6, 1> from_x, from_y;
    from_x << 0, 0, 0,  1, 0, 0;  // X轴
    from_y << 0, 0, 0,  0, 1, 0;  // Y轴
    
    // 目标坐标系(旋转45度,平移(1,2,3))
    float angle = M_PI / 4;
    Eigen::Matrix<float, 6, 1> to_x, to_y;
    to_x << 1, 2, 3,  cos(angle), sin(angle), 0;
    to_y << 1, 2, 3,  -sin(angle), cos(angle), 0;
    
    Eigen::Affine3f transform;
    bool success = pcl::transformBetween2CoordinateSystems(
        from_x, from_y, to_x, to_y, transform);
    
    if (success) {
        std::cout << "坐标系变换矩阵:" << std::endl;
        std::cout << transform.matrix() << std::endl;
        
        // 验证
        Eigen::Matrix<float, 6, 1> transformed_x, transformed_y;
        pcl::transformLine(from_x, transformed_x, transform);
        pcl::transformLine(from_y, transformed_y, transform);
        
        std::cout << "\n验证变换后的X轴:" << std::endl;
        std::cout << "期望: " << to_x.transpose() << std::endl;
        std::cout << "实际: " << transformed_x.transpose() << std::endl;
    } else {
        std::cout << "计算变换失败,坐标系无效" << std::endl;
    }
}

实际应用 - 机器人手眼标定:

void hand_eye_calibration_example() {
    // 场景: 计算相机坐标系到机器人末端坐标系的变换
    
    // 机器人末端坐标系(通过示教或测量获得)
    Eigen::Vector3f robot_origin(0.5, 0.3, 0.8);
    Eigen::Vector3f robot_x(1, 0, 0);
    Eigen::Vector3f robot_y(0, 1, 0);
    
    // 相机坐标系(通过视觉标定获得)
    Eigen::Vector3f camera_origin(0.6, 0.35, 0.75);
    Eigen::Vector3f camera_x(0.866, 0.5, 0);    // 旋转30度
    Eigen::Vector3f camera_y(-0.5, 0.866, 0);
    
    // 构建直线表示
    Eigen::Matrix<float, 6, 1> robot_line_x, robot_line_y;
    Eigen::Matrix<float, 6, 1> camera_line_x, camera_line_y;
    
    robot_line_x << robot_origin, robot_x;
    robot_line_y << robot_origin, robot_y;
    camera_line_x << camera_origin, camera_x;
    camera_line_y << camera_origin, camera_y;
    
    // 计算变换
    Eigen::Affine3f camera_to_robot;
    if (pcl::transformBetween2CoordinateSystems(
            camera_line_x, camera_line_y,
            robot_line_x, robot_line_y,
            camera_to_robot)) {
        
        std::cout << "相机到机器人末端的变换:" << std::endl;
        std::cout << camera_to_robot.matrix() << std::endl;
        
        // 提取变换参数用于存储
        float x, y, z, roll, pitch, yaw;
        pcl::getTranslationAndEulerAngles(camera_to_robot, 
                                         x, y, z, roll, pitch, yaw);
        
        std::cout << "\n手眼标定参数:" << std::endl;
        std::cout << "平移: (" << x << ", " << y << ", " << z << ")" << std::endl;
        std::cout << "旋转: (" << roll*180/M_PI << "°, " 
                  << pitch*180/M_PI << "°, " 
                  << yaw*180/M_PI << "°)" << std::endl;
    }
}

---

9. 高级算法 - Umeyama算法

9.1 umeyama - 点集配准

函数签名:

template <typename Derived, typename OtherDerived> 
typename Eigen::internal::umeyama_transform_matrix_type<Derived, OtherDerived>::type
umeyama (const Eigen::MatrixBase<Derived>& src, 
         const Eigen::MatrixBase<OtherDerived>& dst, 
         bool with_scaling = false);

作用:
使用Umeyama算法计算两个点集之间的最优刚体变换(可选缩放)。

算法原理:
最小化误差: $\frac{1}{n} \sum_{i=1}^n ||y_i - (cRx_i + t)||^2$

其中:

• $c$ 是缩放因子

• $R$ 是旋转矩阵

• $t$ 是平移向量

使用示例:

void example_umeyama() {
    // 源点集(3D点,每列一个点)
    Eigen::Matrix3Xf src(3, 4);
    src << 0, 1, 0, 0,
           0, 0, 1, 0,
           0, 0, 0, 1;
    
    // 目标点集(旋转+平移后的点)
    Eigen::Affine3f ground_truth = pcl::getTransformation(
        2.0f, 3.0f, 4.0f,     // 平移
        0.0f, 0.0f, M_PI/4    // 旋转45度
    );
    
    Eigen::Matrix3Xf dst(3, 4);
    for (int i = 0; i < 4; i++) {
        Eigen::Vector3f p_src = src.col(i);
        Eigen::Vector3f p_dst;
        pcl::transformPoint(p_src, p_dst, ground_truth);
        dst.col(i) = p_dst;
    }
    
    // 使用Umeyama算法恢复变换
    Eigen::Matrix4f estimated_transform = pcl::umeyama(src, dst, false);
    
    std::cout << "真实变换:" << std::endl;
    std::cout << ground_truth.matrix() << std::endl;
    
    std::cout << "\nUmeyama估计的变换:" << std::endl;
    std::cout << estimated_transform << std::endl;
    
    // 计算误差
    Eigen::Matrix4f error = ground_truth.matrix() - estimated_transform;
    std::cout << "\n误差(应接近0):" << std::endl;
    std::cout << error.norm() << std::endl;
}

带缩放的版本:

void example_umeyama_with_scaling() {
    // 源点集
    Eigen::Matrix3Xf src(3, 5);
    src << 1, 2, 3, 4, 5,
           1, 2, 3, 4, 5,
           1, 2, 3, 4, 5;
    
    // 目标点集(缩放2倍+旋转+平移)
    float scale = 2.0f;
    Eigen::Affine3f transform = pcl::getTransformation(
        1.0f, 2.0f, 3.0f,
        0.1f, 0.2f, 0.3f
    );
    
    Eigen::Matrix3Xf dst = scale * (transform.linear() * src);
    dst.colwise() += transform.translation();
    
    // 估计变换(允许缩放)
    Eigen::Matrix4f estimated = pcl::umeyama(src, dst, true);
    
    std::cout << "估计的变换(包含缩放):" << std::endl;
    std::cout << estimated << std::endl;
    
    // 提取缩放因子
    Eigen::Matrix3f rotation = estimated.block<3,3>(0,0);
    float estimated_scale = rotation.col(0).norm();
    std::cout << "\n估计的缩放因子: " << estimated_scale 
              << " (真实值: " << scale << ")" << std::endl;
}

实际应用 - ICP初始对齐:

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/common/centroid.h>

void icp_initial_alignment() {
    // 两个点云的对应点对
    pcl::PointCloud<pcl::PointXYZ>::Ptr src_cloud(
        new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr dst_cloud(
        new pcl::PointCloud<pcl::PointXYZ>);
    
    // 添加对应点
    src_cloud->points.push_back(pcl::PointXYZ(0, 0, 0));
    src_cloud->points.push_back(pcl::PointXYZ(1, 0, 0));
    src_cloud->points.push_back(pcl::PointXYZ(0, 1, 0));
    src_cloud->points.push_back(pcl::PointXYZ(0, 0, 1));
    
    // ... 填充dst_cloud(变换后的点) ...
    
    // 转换为Eigen矩阵
    Eigen::Matrix3Xf src_mat(3, src_cloud->size());
    Eigen::Matrix3Xf dst_mat(3, dst_cloud->size());
    
    for (size_t i = 0; i < src_cloud->size(); i++) {
        src_mat.col(i) << src_cloud->points[i].x,
                         src_cloud->points[i].y,
                         src_cloud->points[i].z;
        dst_mat.col(i) << dst_cloud->points[i].x,
                         dst_cloud->points[i].y,
                         dst_cloud->points[i].z;
    }
    
    // 计算初始变换
    Eigen::Matrix4f initial_guess = pcl::umeyama(src_mat, dst_mat, false);
    
    std::cout << "ICP初始对齐矩阵:" << std::endl;
    std::cout << initial_guess << std::endl;
    
    // 可以将此矩阵作为ICP的初始猜测
    // pcl::IterativeClosestPoint<...> icp;
    // icp.align(*output, initial_guess);
}

---

10. 矩阵二进制存储函数

10.1 saveBinary - 保存矩阵到二进制文件

函数签名:

template <typename Derived> void
saveBinary (const Eigen::MatrixBase<Derived>& matrix, std::ostream& file);

作用:
将Eigen矩阵以二进制格式保存到文件流中,节省空间且精度无损。

使用示例:

#include <fstream>

void example_saveBinary() {
    // 创建一个矩阵
    Eigen::Matrix4f transform_matrix;
    transform_matrix << 1, 0, 0, 5,
                        0, 1, 0, 10,
                        0, 0, 1, 15,
                        0, 0, 0, 1;
    
    // 保存到文件
    std::ofstream file("transform.bin", std::ios::binary);
    if (file.is_open()) {
        pcl::saveBinary(transform_matrix, file);
        file.close();
        std::cout << "矩阵已保存到 transform.bin" << std::endl;
    }
    
    // 显示文件大小
    std::ifstream check("transform.bin", std::ios::binary | std::ios::ate);
    std::cout << "文件大小: " << check.tellg() << " 字节" << std::endl;
    // 4x4 float矩阵 = 16 * 4 = 64字节
}

---

10.2 loadBinary - 从二进制文件加载矩阵

函数签名:

template <typename Derived> void
loadBinary (Eigen::MatrixBase<Derived> const& matrix, std::istream& file);

使用示例:

void example_loadBinary() {
    // 创建相同大小的矩阵来接收数据
    Eigen::Matrix4f loaded_matrix;
    
    // 从文件加载
    std::ifstream file("transform.bin", std::ios::binary);
    if (file.is_open()) {
        pcl::loadBinary(loaded_matrix, file);
        file.close();
        
        std::cout << "加载的矩阵:" << std::endl;
        std::cout << loaded_matrix << std::endl;
    }
}

完整的保存/加载流程:

void example_save_load_workflow() {
    // 1. 创建并保存多个矩阵
    std::ofstream out_file("calibration_data.bin", std::ios::binary);
    
    Eigen::Matrix3f camera_intrinsic;
    camera_intrinsic << 800, 0, 320,
                        0, 800, 240,
                        0, 0, 1;
    
    Eigen::Affine3f camera_extrinsic = pcl::getTransformation(
        0.5f, 0.0f, 1.5f,
        0.0f, -0.1f, 0.0f
    );
    
    pcl::saveBinary(camera_intrinsic, out_file);
    pcl::saveBinary(camera_extrinsic.matrix(), out_file);
    out_file.close();
    
    std::cout << "标定数据已保存" << std::endl;
    
    // 2. 加载数据
    std::ifstream in_file("calibration_data.bin", std::ios::binary);
    
    Eigen::Matrix3f loaded_intrinsic;
    Eigen::Matrix4f loaded_extrinsic_mat;
    
    pcl::loadBinary(loaded_intrinsic, in_file);
    pcl::loadBinary(loaded_extrinsic_mat, in_file);
    in_file.close();
    
    std::cout << "\n加载的内参:" << std::endl;
    std::cout << loaded_intrinsic << std::endl;
    
    std::cout << "\n加载的外参:" << std::endl;
    std::cout << loaded_extrinsic_mat << std::endl;
}

实际应用 - 保存机器人标定结果:

class RobotCalibration {
public:
    Eigen::Affine3f base_to_world;
    Eigen::Affine3f camera_to_base;
    Eigen::Matrix3f camera_intrinsic;
    Eigen::VectorXf distortion_coeffs;
    
    void save(const std::string& filename) {
        std::ofstream file(filename, std::ios::binary);
        if (!file.is_open()) {
            std::cerr << "无法打开文件: " << filename << std::endl;
            return;
        }
        
        // 保存所有标定参数
        pcl::saveBinary(base_to_world.matrix(), file);
        pcl::saveBinary(camera_to_base.matrix(), file);
        pcl::saveBinary(camera_intrinsic, file);
        pcl::saveBinary(distortion_coeffs, file);
        
        file.close();
        std::cout << "标定参数已保存到: " << filename << std::endl;
    }
    
    void load(const std::string& filename) {
        std::ifstream file(filename, std::ios::binary);
        if (!file.is_open()) {
            std::cerr << "无法打开文件: " << filename << std::endl;
            return;
        }
        
        Eigen::Matrix4f base_mat, camera_mat;
        pcl::loadBinary(base_mat, file);
        pcl::loadBinary(camera_mat, file);
        pcl::loadBinary(camera_intrinsic, file);
        pcl::loadBinary(distortion_coeffs, file);
        
        base_to_world.matrix() = base_mat;
        camera_to_base.matrix() = camera_mat;
        
        file.close();
        std::cout << "标定参数已从 " << filename << " 加载" << std::endl;
    }
};

void example_robot_calibration_storage() {
    RobotCalibration calib;
    
    // 设置标定参数
    calib.base_to_world = pcl::getTransformation(
        1.0f, 2.0f, 0.0f, 0.0f, 0.0f, M_PI/4);
    calib.camera_to_base = pcl::getTransformation(
        0.3f, 0.0f, 0.5f, 0.0f, -M_PI/6, 0.0f);
    calib.camera_intrinsic << 800, 0, 320,
                              0, 800, 240,
                              0, 0, 1;
    calib.distortion_coeffs.resize(5);
    calib.distortion_coeffs << 0.1, -0.05, 0.001, 0.002, 0.0;
    
    // 保存
    calib.save("robot_calibration.bin");
    
    // 加载
    RobotCalibration loaded_calib;
    loaded_calib.load("robot_calibration.bin");
}

---

11. 综合应用实例

11.1 完整的点云配准流程

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/common/centroid.h>
#include <pcl/common/transforms.h>

class PointCloudRegistration {
public:
    // 使用特征点对进行初始对齐
    Eigen::Matrix4f initialAlignment(
        const pcl::PointCloud<pcl::PointXYZ>::Ptr& src_keypoints,
        const pcl::PointCloud<pcl::PointXYZ>::Ptr& tgt_keypoints) {
        
        // 转换为Eigen矩阵
        Eigen::Matrix3Xf src_mat(3, src_keypoints->size());
        Eigen::Matrix3Xf tgt_mat(3, tgt_keypoints->size());
        
        for (size_t i = 0; i < src_keypoints->size(); i++) {
            src_mat.col(i) << src_keypoints->points[i].x,
                             src_keypoints->points[i].y,
                             src_keypoints->points[i].z;
            tgt_mat.col(i) << tgt_keypoints->points[i].x,
                             tgt_keypoints->points[i].y,
                             tgt_keypoints->points[i].z;
        }
        
        // 使用Umeyama算法计算初始变换
        Eigen::Matrix4f initial_transform = pcl::umeyama(src_mat, tgt_mat);
        
        std::cout << "初始对齐变换:" << std::endl;
        std::cout << initial_transform << std::endl;
        
        return initial_transform;
    }
    
    // 提取变换参数用于优化
    void extractTransformParameters(const Eigen::Affine3f& transform,
                                   Eigen::VectorXf& params) {
        params.resize(6);
        
        float x, y, z, roll, pitch, yaw;
        pcl::getTranslationAndEulerAngles(transform, x, y, z, roll, pitch, yaw);
        
        params << x, y, z, roll, pitch, yaw;
    }
    
    // 从参数构建变换
    Eigen::Affine3f buildTransformFromParameters(const Eigen::VectorXf& params) {
        Eigen::Affine3f transform;
        pcl::getTransformation(
            params(0), params(1), params(2),  // 平移
            params(3), params(4), params(5),  // 旋转
            transform
        );
        return transform;
    }
    
    // 计算配准误差
    float computeAlignmentError(
        const pcl::PointCloud<pcl::PointXYZ>::Ptr& src,
        const pcl::PointCloud<pcl::PointXYZ>::Ptr& tgt,
        const Eigen::Affine3f& transform) {
        
        float total_error = 0.0f;
        int count = std::min(src->size(), tgt->size());
        
        for (int i = 0; i < count; i++) {
            Eigen::Vector3f p_src(src->points[i].x, 
                                 src->points[i].y, 
                                 src->points[i].z);
            Eigen::Vector3f p_tgt(tgt->points[i].x, 
                                 tgt->points[i].y, 
                                 tgt->points[i].z);
            
            Eigen::Vector3f p_transformed;
            pcl::transformPoint(p_src, p_transformed, transform);
            
            total_error += (p_transformed - p_tgt).squaredNorm();
        }
        
        return std::sqrt(total_error / count);
    }
};

void example_complete_registration() {
    PointCloudRegistration reg;
    
    // 创建测试数据
    pcl::PointCloud<pcl::PointXYZ>::Ptr src(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr tgt(new pcl::PointCloud<pcl::PointXYZ>);
    
    // 添加对应点
    for (int i = 0; i < 10; i++) {
        src->points.push_back(pcl::PointXYZ(i*0.1, i*0.1, 0));
    }
    
    // 创建目标点云(已知变换)
    Eigen::Affine3f ground_truth = pcl::getTransformation(
        1.0f, 2.0f, 3.0f, 0.1f, 0.2f, 0.3f);
    
    for (const auto& p : src->points) {
        Eigen::Vector3f p_in(p.x, p.y, p.z);
        Eigen::Vector3f p_out;
        pcl::transformPoint(p_in, p_out, ground_truth);
        tgt->points.push_back(pcl::PointXYZ(p_out.x(), p_out.y(), p_out.z()));
    }
    
    // 执行配准
    Eigen::Matrix4f estimated = reg.initialAlignment(src, tgt);
    
    // 计算误差
    Eigen::Affine3f est_affine;
    est_affine.matrix() = estimated;
    float error = reg.computeAlignmentError(src, tgt, est_affine);
    
    std::cout << "\n配准误差: " << error << " (应接近0)" << std::endl;
}

---

11.2 多传感器融合系统

class MultiSensorFusion {
private:
    struct SensorFrame {
        std::string name;
        Eigen::Affine3f transform_to_base;
        Eigen::Matrix3f covariance;
    };
    
    std::vector<SensorFrame> sensors_;
    
public:
    void addSensor(const std::string& name,
                   const Eigen::Affine3f& transform,
                   const Eigen::Matrix3f& cov) {
        SensorFrame frame;
        frame.name = name;
        frame.transform_to_base = transform;
        frame.covariance = cov;
        sensors_.push_back(frame);
    }
    
    // 融合多个传感器的平面检测结果
    Eigen::Vector4f fusePlanes(
        const std::vector<Eigen::Vector4f>& local_planes) {
        
        if (local_planes.size() != sensors_.size()) {
            std::cerr << "平面数量与传感器数量不匹配!" << std::endl;
            return Eigen::Vector4f::Zero();
        }
        
        // 将所有平面变换到基坐标系
        std::vector<Eigen::Vector4f> base_planes;
        for (size_t i = 0; i < local_planes.size(); i++) {
            Eigen::Vector4f plane_base;
            pcl::transformPlane(local_planes[i], plane_base, 
                               sensors_[i].transform_to_base);
            base_planes.push_back(plane_base);
        }
        
        // 简单平均融合(实际应用中应使用加权融合)
        Eigen::Vector4f fused_plane = Eigen::Vector4f::Zero();
        for (const auto& plane : base_planes) {
            fused_plane += plane;
        }
        fused_plane /= base_planes.size();
        fused_plane.head<3>().normalize();
        
        std::cout << "融合后的平面: " << fused_plane.transpose() << std::endl;
        return fused_plane;
    }
    
    // 验证传感器标定
    void validateCalibration() {
        std::cout << "\n=== 传感器标定验证 ===" << std::endl;
        
        for (const auto& sensor : sensors_) {
            std::cout << "\n传感器: " << sensor.name << std::endl;
            
            // 提取参数
            float x, y, z, roll, pitch, yaw;
            pcl::getTranslationAndEulerAngles(sensor.transform_to_base,
                                             x, y, z, roll, pitch, yaw);
            
            std::cout << "  位置: (" << x << ", " << y << ", " << z << ")" << std::endl;
            std::cout << "  姿态: (" << roll*180/M_PI << "°, "
                      << pitch*180/M_PI << "°, "
                      << yaw*180/M_PI << "°)" << std::endl;
            
            // 验证旋转矩阵的正交性
            Eigen::Matrix3f R = sensor.transform_to_base.linear();
            float det = pcl::determinant3x3Matrix(R);
            std::cout << "  行列式: " << det << " (应为1或-1)" << std::endl;
            
            // 检查坐标系
            Eigen::Vector3f origin = sensor.transform_to_base.translation();
            Eigen::Vector3f x_axis = R.col(0);
            Eigen::Vector3f y_axis = R.col(1);
            
            bool valid = pcl::checkCoordinateSystem(origin, x_axis, y_axis, 1e-3,效" : "无效") << std::endl;
        }
    }
};

void example_multi_sensor_fusion() {
    MultiSensorFusion fusion;
    
    // 添加传感器1: 前置相机
    Eigen::Affine3f cam_front = pcl::getTransformation(
        0.3f, 0.0f, 0.5f,      // 位置
        0.0f, -0.15f, 0.0f     // 向下俯视约8.6度
    );
    Eigen::Matrix3f cov_ = Eigen::Matrix3f::Identity() * 0.01;
    fusion.addSensor("前置相机", cam_front, cov_cam);
    
    // 添加传感器2: 侧面激光雷达
    Eigen::Affine3f lidar_side = pcl::getTransformation(
        0.0f, 0.2f, 0.3f,
        0.0f, 0.0f, M_PI/2     // 向左旋转90度
    );
    Eigen::Matrix3f cov_lidar = Eigen::Matrix3f::Identity() * 0.005;
    fusion.addSensor("侧面激光雷达", lidar_side, cov_lidar);
    
    // 验证标定
    fusion.validateCalibration();
    
    // 模拟地面检测结果
    std::vector<Eigen::Vector4f> detected_planes;
    detected_planes.push_back(Eigen::Vector4f(0, 0, 1, -0.05));  // 相机检测
    detected_planes.push_back(Eigen::Vector4f(0, 0, 1, -0.03));  // 雷达检测
    
    // 融合结果
    Eigen::Vector4f fused = fusion.fusePlanes(detected_planes);
}

---

12. 最佳实践和性能优化

12.1 性能对比

void performance_comparison() {
    const int N = 1000;
    Eigen::Matrix3f mat = Eigen::Matrix3f::Random();
    
    // 测试eigen33的不同版本
    auto start = std::chrono::high_resolution_clock::now();
    
    for (int i = 0; i < N; i++) {
        float eigenvalue;
        Eigen::Vector3f eigenvector;
        pcl::eigen33(mat, eigenvalue, eigenvector);  // 仅最小特征值
    }
    
    auto end = std::chrono::high_resolution_clock::now();
    auto duration1 = std::chrono::duration = std::chrono::high_resolution_clock::now();
    
    for (int i = 0; i < N; i++) {
        Eigen::Matrix3f eigenvectors;
        Eigen::Vector3f eigenvalues;
        pcl::eigen33(mat, eigenvectors, eigenvalues);  // 完整分解
    }
    
    end = std::chrono::high_resolution_clock::now();
    auto duration2 = std::chrono::duration_cast<std::chrono::microseconds>(end - start);
    
    std::cout << "仅最小特征值: " << duration1.count() / N << " μs/次" << std::endl;
    std::cout << "完整特征分解: " << duration2.count() / N << " μs/次" << std::endl;
    std::cout << "速度提升: " << (float)duration2.count() / duration1.count() << "x" << std::endl;
}

12.2 使用建议

// ✅ 推荐做法
void recommended_practices() {
    // 1. 根据需求选择合适的函数
    Eigen::Matrix3f cov = Eigen::Matrix3f::Random();
    
    // 仅需法向量? 使用最快的版本
    float min_eval;
    Eigen::Vector3f normal;
    pcl::eigen33(cov, min_eval, normal);
    
    // 2. 对称矩阵求逆用专用函数
    Eigen::Matrix3f inv;
    float det = pcl::invert3x3SymMatrix(cov, inv);  // 更快
    // 而不是: inv = cov.inverse();
    
    // 3. 批量变换时复用变换矩阵
    Eigen::Affine3f transform = pcl::getTransformation(1, 2, 3, 0, 0, M_PI/4);
    std::vector<Eigen::Vector3f> points(1000);
    
    for (auto& p : points) {
        pcl::transformPoint(p, p, transform);  // 原地变换,节省内存
    }
    
    // 4. 验证输入数据
    if (!pcl::checkCoordinateSystem(origin, x_axis, y_axis)) {
        std::cerr << "警告: 坐标系无效!" << std::endl;
        return;
    }
}

// ❌ 不推荐做法
void antipatterns() {
    // 1. 不要为完整分解付出代价
    Eigen::Matrix3f mat;
    Eigen::Matrix3f evecs;
    Eigen::Vector3f evals;
    pcl::eigen33(mat, evecs, evals);
    // 然后只使用: evals(0) 和 evecs.col(0)
    // 应该使用: pcl::eigen33(mat, eigenvalue, eigenvector);
    
    // 2. 不要在循环中重复构建相同的变换
    for (int i = 0; i < 1000; i++) {
        Eigen::Affine3f t = pcl::getTransformation(1, 2, 3, 0, 0, 0);  // 浪费!
        // ...
    }
    // 应该在循环外构建一次
    
    // 3. 不要忽略返回值
    Eigen::Matrix3f inv;
    pcl::invert3x3Matrix(mat, inv);  // 没检查行列式!
    // 应该: float det = pcl::invert3x3Matrix(mat, inv);
    //       if (std::abs(det) < 1e-6) { /* 处理奇异情况 */ }
}

---

13. 总结

函数分类速查表

类别	函数	主要用途
特征值计算	eigen22, eigen33	法向量估计、PCA
矩阵求逆	invert2x2, invert3x3SymMatrix	协方差矩阵求逆
坐标变换	getTransformation, getEulerAngles	位姿表示转换
几何变换	transformPoint, transformVector, transformPlane	点云变换
配准	umeyama	点集对齐
验证	checkCoordinateSystem	标定验证

在PCL中的核心应用

1. 法向量估计 (pcl::NormalEstimation) → 使用 eigen33

2. 点云配准 (pcl::ICP) → 使用 umeyama + 变换函数

3. 平面分割 (pcl::SACSegmentation) → 使用平面变换

4. 特征提取 → 使用特征值分解

5. 传感器标定 → 使用坐标系函数

这套函数是PCL几何计算的基础,掌握它们对于深入理解和使用PCL至关重要!