题目:将给定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);