第二个实战项目处理 3D 点云做旋转和平移。这是计算机图形学、机器人、SLAM 的基础。本项目综合运用vector、几何模块、刚体变换。37.1 什么是点云点云point cloud是一堆三维点的集合。激光雷达扫描、3D 扫描仪输出的就是点云。机器人、自动驾驶靠处理点云来看世界。在 Eigen 里一个点就是一个Vector3d点云就是std::vectorVector3d。37.2 项目目标完成两个任务构造点云生成一个立方体的 8 个顶点变换点云绕 Z 轴旋转 90°再平移观察点云的变化统计点云计算质心平均点37.3 完整代码#includeEigen/Dense#includeEigen/Geometry#includeiostream#includevectorintmain(){constdoublePI3.14159265358979;// ---- 任务 1构造立方体的 8 个顶点 ----std::vectorEigen::Vector3dcloud;for(intx0;x1;x)for(inty0;y1;y)for(intz0;z1;z)cloud.push_back(Eigen::Vector3d(x,y,z));std::cout点云有 cloud.size() 个点std::endl;// ---- 任务 2变换绕 Z 转 90° 再平移----Eigen::Isometry3d TEigen::Isometry3d::Identity();T.rotate(Eigen::AngleAxisd(PI/2,Eigen::Vector3d::UnitZ()));T.pretranslate(Eigen::Vector3d(10,0,0));std::cout变换前 - 变换后std::endl;for(constautop:cloud){Eigen::Vector3d p2T*p;std::cout p.transpose() - p2.transpose()std::endl;}// ---- 任务 3计算质心 ----Eigen::Vector3d centroidEigen::Vector3d::Zero();for(constautop:cloud){centroidp;}centroid/cloud.size();std::cout质心 centroid.transpose()std::endl;// (0.5, 0.5, 0.5)return0;}编译运行存成ch37.cpp按附录 C.3 的命令g-stdc17-O2-I你的Eigen目录ch37.cpp-och37.exe ./ch37.exe实际输出点云有 8 个点 变换前 - 变换后 0 0 0 - 10 0 0 0 0 1 - 10 0 1 0 1 0 - 9 1.61554e-15 0 0 1 1 - 9 1.61554e-15 1 1 0 0 - 10 1 0 1 0 1 - 10 1 1 1 1 0 - 9 1 0 1 1 1 - 9 1 1 质心 0.5 0.5 0.51.61554e-15是 0 的浮点残渣第 21 章末尾讲过不用管它。37.4 运行结果解读立方体 8 个顶点是 (0/1, 0/1, 0/1) 的所有组合绕 Z 转 90°X 轴方向转到 Y 轴(1,0,0) → (0,1,0)平移 (10,0,0)所有点 X 坐标 10质心8 个顶点的平均 (0.5, 0.5, 0.5)37.5 拓展练习拓展 1计算点云的包围盒包围盒bounding box 各方向上的最小值和最大值用来快速判断点云占了多大空间。接着第 37.3 节的程序做把下面几行粘贴到ch37.cpp里打印质心那一行之后、return 0;之前此处的cloud就是前面构造好的 8 个顶点// 包围盒 所有坐标的最小值和最大值Eigen::Vector3d minPtcloud[0],maxPtcloud[0];for(constautop:cloud){minPtminPt.cwiseMin(p);// 逐元素取最小maxPtmaxPt.cwiseMax(p);// 逐元素取最大}std::cout包围盒minPt.transpose() 到 maxPt.transpose()std::endl;重新编译运行最后一行新增包围盒0 0 0 到 1 1 1立方体的 8 个顶点正好落在 0 和 1 上所以包围盒是 (0,0,0) 到 (1,1,1)。注意这段用的是原始点云未变换如果换成T * p的结果包围盒会跟着平移走。拓展 2两点云配准ICP 的思想机器人里常要做点云配准找变换 T 使两个点云对齐。经典算法 ICP 的核心就是对每个点找最近对应点用最小二乘求最优刚体变换迭代直到收敛这综合了本项目的刚体变换和第 36 章的最小二乘是值得深入的方向。37.6 学完这个项目你掌握了什么用vectorEigen::Vector3d表示点云用Isometry3d做刚体变换用cwiseMin/cwiseMax做逐元素运算用循环 归约做统计质心这些正是机器人、SLAM、3D 图形领域天天在用的操作。37.7 小结点云 vectorVector3d。刚体变换 旋转 平移用Isometry3d。质心、包围盒等统计都可用 Eigen 简洁实现。点云配准ICP是更深入的实战方向。练习题生成一个 2×2×2 的网格点云步长 0.5打印点数。对点云做一次刚体变换观察变化。计算点云的质心和包围盒。用cwiseMin/cwiseMax实现包围盒计算。思考怎么判断两个点云是否对齐
