点云融合代码学习

点云融合代码学习

#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 是 C++ 标准库中的类，属于 <fstream> 头文件。全称为 "input file stream"（输入文件流），fcamTrans 是一个 ifstream 对象的实例
    ifstream fcamTrans(camTransFile);
    if(!fcamTrans.is_open())
    {
        cerr << "trajectory is empty!" << endl;
        return;
    }

   	// ---------- 开始你的代码  ------------- -//
	// 参考作业8 绘制轨迹
    string lineData;
    while((getline(fcamTrans,lineData)) && !lineData.empty()){
    
        Eigen::Quaterniond quad;   // 表示旋转部分（四元数）。
        Eigen::Vector3d t;         // 表示平移部分（位置向量）。
        Eigen::Isometry3d T = Eigen::Isometry3d::Identity();  // 表示最终的位姿，它是一个 3D 同质变换矩阵（旋转+平移）。初始化为单位矩阵。

        if(lineData[0] == '#'){
            continue;
        }

// 创建一个 istringstream 对象来解析当前行的内容。从这一行中解析出相机的 位置向量 t 和 旋转四元数 quad，这两个值会被依次存储到相应的变量中。解析的顺序是：位置的三个分量 t[0], t[1], t[2] 和四元数的四个分量 quad.x(), quad.y(), quad.z(), quad.w()。
        istringstream strData(lineData);    // istringstream全称是 "input string stream"（输入字符串流）
        // 流操作符（>>）会从 strData 中依次读取数据，解析为数值类型，并存储到指定的变量中。
        strData>>t[0]>>t[1]>>t[2]>>quad.x()>>quad.y()>>quad.z()>>quad.w();

        T.rotate(quad);           //将四元数 quad 的旋转应用到变换矩阵 T 的旋转部分。
        T.pretranslate(t);        // 将平移向量 t 直接添加到变换矩阵 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 类型），包含了点云变换的旋转和平移信息。

点云融合处理过程

1. 点云预处理
   在融合前对点云数据进行清理和优化：

• 降噪：去除孤立点、噪声点。例如，使用统计滤波或半径滤波方法。
• 下采样：对点云进行体素化降采样，减少点的数量，同时保留结构信息，提高计算效率。
• 裁剪：根据感兴趣的区域裁剪点云，移除不需要的部分。

2. 点云配准
   将不同视角或帧的点云对齐到一个统一的坐标系下。

• 粗配准：
  使用全局特征提取（如 FPFH）计算点云的初始配准。
  方法：RANSAC 或其他全局搜索算法。
• 精配准：
  使用迭代优化算法（如 ICP：Iterative Closest Point（迭代最近点算法））对点云进行精细对齐。
  优化目标是最小化点云之间的最近点距离。

3. 坐标变换

• 通过刚体变换矩阵（旋转和平移）将配准后的点云转换到全局坐标系。
• 使用从点云配准计算得到的旋转和平移，将局部点云数据映射到统一的全局坐标系中。

4. 点云融合
   将多个点云合并为一个统一的点云模型，常见的方法包括：

• 直接合并：将点云数据点直接合并，形成新的点云集合。
• 体素融合：将点云划分为体素网格，对每个体素进行点统计（例如取平均值或加权平均）。
• 概率地图：在动态环境中，采用概率方法更新体素的占据状态（如 OctoMap）。

5. 冲突处理
   处理点云融合中可能出现的冲突或冗余点：

• 重复点去除：通过距离阈值或体素化方法移除重叠的点。
• 密度平衡：调整融合后的点云密度，避免某些区域点过于稀疏或密集。

6. 表面重建（可选）
   对融合后的点云进行表面重建，使其从离散点集变成连续的三维表面模型：

• 三角网格重建：如使用 Delaunay 三角剖分或 Poisson Surface Reconstruction。
• 法线估计和纹理映射：为点云添加法线和纹理，提升模型的质量和真实性。

7. 融合后优化
   对最终融合的点云进行后处理，提升质量：

• 边界修整：去除非平滑或断裂的区域。
• 孔洞填补：使用插值方法填补点云模型中的孔洞。
• 重新采样：根据需求调整点的密度，使点云均匀分布。

8. 应用领域的特定处理
   根据具体应用（如 SLAM、3D建模、AR/VR 等）进行额外处理：

• 语义分割：将点云中的不同部分标注为特定类别（如建筑物、道路等）。
• 颜色融合：将不同视角的颜色信息结合到点云中。
• 动态点云处理：处理动态场景中的点云变化，例如背景建模和动态物体移除。