#include "slamBase.hpp"int main( int argc, char** argv )
{// 相机内参包括焦距（fx,fy）、主点位置（cx, cy）,以及深度比例因子scaleCAMERA_INTRINSIC_PARAMETERS cameraParams{517.0,516.0,318.6,255.3,5000.0};int frameNum = 3;// 由于 Eigen 使用 SIMD（单指令多数据）优化，存储该类型时需要使用对齐分配器（Eigen::aligned_allocator）。vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>> poses;PointCloud::Ptr fusedCloud(new PointCloud()); string path = "./data/";string cameraPosePath = path + "cameraTrajectory.txt";readCameraTrajectory(cameraPosePath, poses);for (int idx = 0; idx < frameNum; idx++){string rgbPath = path + "rgb/rgb" + to_string(idx) + ".png";string depthPath = path + "depth/depth" + to_string(idx) + ".png";FRAME frm;frm.rgb = cv::imread(rgbPath);if (frm.rgb.empty()) {cerr << "Fail to load rgb image!" << endl;}frm.depth = cv::imread(depthPath, -1);if (frm.depth.empty()) {cerr << "Fail to load depth image!" << endl;}if (idx == 0) // 如果是第一帧，把第一帧转为点云{fusedCloud = image2PointCloud(frm.rgb, frm.depth, cameraParams);}else // 如果非首帧，则把当前帧加入点云，即点云融合{fusedCloud = pointCloudFusion( fusedCloud, frm, poses[idx], cameraParams );}}pcl::io::savePCDFile( "./fusedCloud.pcd", *fusedCloud ); // 保存点云return 0;
}#include "slamBase.hpp"PointCloud::Ptr image2PointCloud(Mat rgb, Mat depth, CAMERA_INTRINSIC_PARAMETERS camera)
{// cloud 是指向 PointCloud 对象的智能指针（Ptr 表示 shared_ptr 或 boost::shared_ptr 类型）。通过 -> 操作符，可以访问智能指针所指向的对象成员。PointCloud::Ptr cloud ( new PointCloud );for (int m = 0; m < depth.rows; m++)for (int n = 0; n < depth.cols; n++){// 获取深度图中(m,n)处的值ushort d = depth.ptr<ushort>(m)[n];// d 可能没有值，若如此，跳过此点if (d == 0)continue;// d 存在值，则向点云增加一个点PointT p;// 计算这个点的空间坐标p.z = double(d) / camera.scale;p.x = (n - camera.cx) * p.z / camera.fx;p.y = (m - camera.cy) * p.z / camera.fy;// 从rgb图像中获取它的颜色p.b = rgb.ptr<uchar>(m)[n * 3];p.g = rgb.ptr<uchar>(m)[n * 3 + 1];p.r = rgb.ptr<uchar>(m)[n * 3 + 2];// 把p加入到点云中cloud->points.push_back(p);}// 设置并保存点云cloud->height = 1;cloud->width = cloud->points.size();cloud->is_dense = false; // 是否为稠密（没有无效点）点云，设置为false表示可能包含无效点return cloud;
}PointCloud::Ptr pointCloudFusion( PointCloud::Ptr &original, FRAME& newFrame, Eigen::Isometry3d T, CAMERA_INTRINSIC_PARAMETERS camera )
{// ---------- 开始你的代码 ------------- -//// 简单的点云叠加融合PointCloud::Ptr newCloud(new PointCloud()), transCloud(new PointCloud());newCloud = image2PointCloud(newFrame.rgb, newFrame.depth,camera); // 此时的点云有像素信息，位置xyz为相机坐标系下的坐标pcl::transformPointCloud(*newCloud,*transCloud,T.matrix()); // 将新点云从相机坐标系转为世界坐标系*original += *transCloud; // 更新点云return original;// ---------- 结束你的代码 ------------- -//
}// camTransFile：存储相机轨迹的文件路径，文件的每一行包含一帧相机的位姿信息。
// poses：一个 vector，用来存储解析后的相机位姿。每个 Eigen::Isometry3d 对象包含相机的旋转和位移信息。
void readCameraTrajectory(string camTransFile, vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>> &poses) // 注意内存对齐
{ ifstream fcamTrans(camTransFile);if(!fcamTrans.is_open()){cerr << "trajectory is empty!" << endl;return;}// ---------- 开始你的代码 ------------- -//// 参考作业8 绘制轨迹string lineData;while((getline(fcamTrans,lineData)) && !lineData.empty()){// quad：Eigen::Quaterniond 类型，表示旋转部分（四元数）。// t：Eigen::Vector3d 类型，表示平移部分（位置向量）。// T：Eigen::Isometry3d 类型，表示最终的位姿，它是一个 3D 同质变换矩阵（旋转+平移）。初始化为单位矩阵。Eigen::Quaterniond quad;Eigen::Vector3d t;Eigen::Isometry3d T = Eigen::Isometry3d::Identity();if(lineData[0] == '#'){continue;}// 创建一个 istringstream 对象来解析当前行的内容。从这一行中解析出相机的 位置向量 t 和 旋转四元数 quad，这两个值会被依次存储到相应的变量中。解析的顺序是：位置的三个分量 t[0], t[1], t[2] 和四元数的四个分量 quad.x(), quad.y(), quad.z(), quad.w()。istringstream strData(lineData);strData>>t[0]>>t[1]>>t[2]>>quad.x()>>quad.y()>>quad.z()>>quad.w();T.rotate(quad);T.pretranslate(t);//cout<<"test "<<endl;poses.push_back(T);}// ---------- 结束你的代码 ------------- -//
}通过上述代码，我们先做一个简单的内容回顾：
相机内参
一般由焦距，主点坐标，缩放因子，与附加内参（径向畸变，切向畸变）等构成
内参与成像模型
相机内参用于从空间点（X, Y, Z）投影到图像平面上的像素坐标（u, v）
通过成像几何关系可以得到图像坐标系中的像素位置转换到 相机坐标系 下。
其中 归一化坐标系为：
这里设定了缩放参数scale为1。
scale
深度图是深度传感器或深度相机生成的二维图像，其中每个像素的值表示相机到场景中对应点的距离。
这些深度值通常以整数形式存储，例如 16 位无符号整数（uint16），以节省存储空间。
深度图中的像素值（如d）可能需要乘以一个比例因子（即camera.scale）才能得到真实的深度值。
例如，如果深度图的单位是毫米，而你的计算需要以米为单位表示深度值，那么 camera.scale 的值可能是 1000。
例子
上述代码中的image2PointCloud函数中有下面的代码，作用是由图像坐标映射到空间坐标
// d 存在值，则向点云增加一个点PointT p;// 计算这个点的空间坐标p.z = double(d) / camera.scale;p.x = (n - camera.cx) * p.z / camera.fx;p.y = (m - camera.cy) * p.z / camera.fy;
内参获取
内参参数通常通过相机标定（Camera Calibration）获得，过程如下：
拍摄带有已知几何结构的标定板（如棋盘格）的多张照片。
使用标定算法计算内参矩阵 K 和畸变参数，标定方法有很多，这里暂时不做介绍。
PCL 中 PointCloud 类的常见成员变量及其功能
| 成员变量 | 类型 | 描述 |
|---|---|---|
| points | std::vector | 存储点云中的所有点, PointT 可以是各种点类型（如 pcl::PointXYZ、pcl::PointXYZRGB 等） |
| width | uint32_t | 点云的宽度（列数或点总数） |
| height | uint32_t | 点云的高度（行数） |
| is_dense | bool | 是否为稠密点云 |
| sensor_origin_ | Eigen::Vector4f | 点云的传感器原点 |
| sensor_orientation_ | Eigen::Quaternionf | 点云的传感器姿态, 以四元数形式记录传感器的旋转信息 |
| fields | std::vector < pcl::PCLPointField > | 点的字段描述（如 x、y、z） |
| data | std::vector< uint8_t > | 点云的原始字节数据 |
| header | pcl::PCLHeader | 点云头部信息 |
pcl::transformPointCloud(*newCloud, *transCloud, T.matrix())
template <typename PointT> void pcl::transformPointCloud(const pcl::PointCloud<PointT>& cloud_in, pcl::PointCloud<PointT>& cloud_out, const Eigen::Matrix4f& transform);
这个函数是 Point Cloud Library (PCL) 中的一个重要函数，用于对点云数据进行变换。变换操作通常是旋转和平移，常用于点云的配准、对齐等操作。
- cloud_in：输入的点云数据。
- cloud_out：输出的点云数据，变换后的点云结果。
- transform：一个 4x4 齐次变换矩阵（通常是一个 Eigen::Matrix4f 类型），包含了点云变换的旋转和平移信息。