点云融合代码学习

#include "slamBase.hpp"

int main( int argc, char** argv )
{
	// 相机内参包括焦距（fx,fy）、主点位置（cx, cy）,以及深度比例因子scale
    CAMERA_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 类的常见成员变量及其功能

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 类型），包含了点云变换的旋转和平移信息。