PCL学习（1）——点云融合

题目：将给定3帧（不连续）RGB-D相机拍摄的 RGB + depth 图像，以及他们之间的变换矩阵RGB-D图像转换为点云，并融合出最终的点云输出

pointCloudFusion.cpp

#include "slamBase.hpp"

int main( int argc, char** argv )
{
    CAMERA_INTRINSIC_PARAMETERS cameraParams{517.0,516.0,318.6,255.3,5000.0};
    int frameNum = 3;
    vector<Eigen::Isometry3d> poses;
    PointCloud::Ptr fusedCloud(new PointCloud());
    string path = "/home/xiaohu/learn_SLAM/zuoye13/practice_pointCloudFusion/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;
}

slamBase.cpp

#include "slamBase.hpp"
#include <string>
#include <iostream>


PointCloud::Ptr image2PointCloud(Mat rgb, Mat depth, CAMERA_INTRINSIC_PARAMETERS camera)
{
    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;
    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);
	pcl::transformPointCloud(*newCloud,*transCloud,T.matrix());
	*original+=*transCloud;
	return original;

	// ---------- 结束你的代码  ------------- -//
}


void readCameraTrajectory(string camTransFile, vector<Eigen::Isometry3d> &poses)
{
    ifstream fcamTrans(camTransFile);
    if (!fcamTrans.is_open())
    {
        cerr << "trajectory is empty!" << endl;
        return;
    }



    else
        {
        string str;
        while ((getline(fcamTrans, str)))
        {
            Eigen::Quaterniond q;
            Eigen::Vector3d t;
            Eigen::Isometry3d T = Eigen::Isometry3d::Identity();

            if (str.at(0) == '#') {
                cout << "str" << str << endl;
                continue;
            }
            istringstream strdata(str);

            strdata >> t[0] >> t[1] >> t[2] >> q.x() >> q.y() >> q.z() >> q.w();
            T.rotate(q);
            T.pretranslate(t);
            poses.push_back(T);
        }
        }
}

slamBase.hpp

# pragma once //保证头文件只被编译一次

#include <iostream>

// opencv
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

// pcl
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/common/transforms.h>

using namespace std;
using namespace cv;

typedef pcl::PointXYZRGB PointT;
typedef pcl::PointCloud<PointT> PointCloud;

// camera instrinsic parameters
struct CAMERA_INTRINSIC_PARAMETERS
{
    double fx, fy, cx, cy, scale;
};

struct FRAME
{
    cv::Mat rgb, depth;
};

PointCloud::Ptr image2PointCloud(Mat rgb, Mat depth, CAMERA_INTRINSIC_PARAMETERS camera);
PointCloud::Ptr pointCloudFusion( PointCloud::Ptr &original, FRAME& newFrame, Eigen::Isometry3d T, CAMERA_INTRINSIC_PARAMETERS camera );
void readCameraTrajectory(string camTransFile, vector<Eigen::Isometry3d> &poses);