PCL: RGB-D图像转换成点云数据+点云融合
2020-05-18 本文已影响0人
AI秘籍
1. 题目
已经给定3帧（不连续）RGB-D相机拍摄的 RGB + depth 图像，以及他们之间的变换矩阵（以第一帧为参考帧），请将上述3帧RGB-D图像分别生成点云并融合出最终的点云输出。
数据如下:
rgb0.png
rgb1.png
rgb2.png
depth0.png
depth1.png
depth2.png
相机位姿文件:
cameraTrajectory.txt内容如下:
//# tx ty tz qx qy qz qw
0.000000000 0.000000000 0.000000000 0.000000000 0.000000000 0.000000000 1.000000000
-0.288952827 0.222811699 -0.252029210 0.054562528 -0.312418818 -0.288284063 0.903498590
-0.650643229 0.383824050 -0.501303971 -0.016285975 -0.159155473 -0.111743204 0.980774045
image.png
2.RGBD图像转点云数据+点云融合代码
// RDGD图像转点云数据
#include <pcl/point_types.h>
//点云文件IO（pcd文件和ply文件）
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
//kd树
#include <pcl/kdtree/kdtree_flann.h>
//特征提取
#include <pcl/features/normal_3d_omp.h>
#include <pcl/features/normal_3d.h>
//重构
#include <pcl/surface/gp3.h>
#include <pcl/surface/poisson.h>
//可视化
#include <pcl/visualization/pcl_visualizer.h>
// 矩阵变换
#include <pcl/common/transforms.h>
//多线程
#include <boost/thread/thread.hpp>
#include <fstream>
#include <iostream>
#include <stdio.h>
#include <string.h>
#include <string>
#include <opencv2/opencv.hpp>
#include <eigen3/Eigen/Dense>
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;
};
// RGBD图像转点云数据
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;
}
// 读取相机姿态CameraTrajectory
// # tx ty tz qx qy qz qw
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);
}
}
}
// 简单点云叠加融合
PointCloud::Ptr pointCloudFusion(
PointCloud::Ptr &original,
cv::Mat curr_rgb_im,
cv::Mat curr_depth_im,
Eigen::Isometry3d T,
CAMERA_INTRINSIC_PARAMETERS camera )
{
// ---------- 开始你的代码 ------------- -//
PointCloud::Ptr newCloud(new PointCloud()),transCloud(new PointCloud());
newCloud=image2PointCloud(curr_rgb_im,curr_depth_im,camera);
pcl::transformPointCloud(*newCloud,*transCloud,T.matrix());
*original+=*transCloud;
return original;
// ---------- 结束你的代码 ------------- -//
}
// 显示rgb点云
boost::shared_ptr<pcl::visualization::PCLVisualizer> rgbVis(
pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud)
{
// --------------------------------------------
// -----Open 3D viewer and add point cloud-----
// --------------------------------------------
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
viewer->setBackgroundColor(0, 0, 0);
pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(cloud);
viewer->addPointCloud<pcl::PointXYZRGB>(cloud, rgb, "sample cloud");
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud");
viewer->addCoordinateSystem(1.0);
viewer->initCameraParameters();
return (viewer);
}
int main()
{
// 相机内参
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 color_im_path = "xxx/rgb/rgb";
string depth_im_path = "xxx/depth/depth";
string cameraPosePath = "xxx/cameraTrajectory.txt";
readCameraTrajectory(cameraPosePath, poses);
for (int idx = 0; idx < frameNum; idx++)
{
string rgbPath = color_im_path + to_string(idx) + ".png";
string depthPath = depth_im_path + to_string(idx) + ".png";
cv::Mat color_im = cv::imread(rgbPath);
if (color_im.empty())
{
cerr << "Fail to load rgb image!" << endl;
}
cv::Mat depth_im = cv::imread(depthPath, -1);
if (depth_im.empty())
{
cerr << "Fail to load depth image!" << endl;
}
if (idx == 0)
{
fusedCloud = image2PointCloud(color_im, depth_im, cameraParams);
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer1;
viewer1 = rgbVis(fusedCloud);
}
else
{
fusedCloud = pointCloudFusion(fusedCloud, color_im,depth_im, poses[idx], cameraParams);
}
}
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
viewer = rgbVis(fusedCloud);
while (!viewer->wasStopped())
{
viewer->spinOnce(100);
boost::this_thread::sleep(boost::posix_time::microseconds(100000));
}
pcl::io::savePCDFile( "fusedCloud.pcd", *fusedCloud );
return 0;
}
第一帧点云
image.png
3帧点云融合
image.png
