pcl ptr转换_PCL: RGB-D图像转换成点云数据+点云融合

╰半橙微兮° 2023-01-10 05:09 285阅读 0赞

1. 题目

已经给定3帧(不连续)RGB-D相机拍摄的 RGB + depth 图像，以及他们之间的变换矩阵(以第一帧为参考帧)，请将上述3帧RGB-D图像分别生成点云并融合出最终的点云输出。

数据如下:

![0068944923db][]

rgb0.png

![0068944923db][]

rgb1.png

![0068944923db][]

rgb2.png

![0068944923db][]

depth0.png

![0068944923db][]

depth1.png

![0068944923db][]

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

![0068944923db][]

image.png

2.RGBD图像转点云数据+点云融合代码

// RDGD图像转点云数据

\#include

//点云文件IO(pcd文件和ply文件)

\#include

//kd树

\#include

//特征提取

\#include

//重构

\#include

//可视化

\#include

// 矩阵变换

\#include

//多线程

\#include

using namespace std;

using namespace cv;

typedef pcl::PointXYZRGB PointT;

typedef pcl::PointCloud 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(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(m)\[n \* 3\];

p.g = rgb.ptr(m)\[n \* 3 + 1\];

p.r = rgb.ptr(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<: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<:visualization::pclvisualizer> rgbVis(

pcl::PointCloud<:pointxyzrgb>::ConstPtr cloud)

// --------------------------------------------

// -----Open 3D viewer and add point cloud-----

// --------------------------------------------

boost::shared\_ptr<:visualization::pclvisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));

viewer->setBackgroundColor(0, 0, 0);

pcl::visualization::PointCloudColorHandlerRGBField<:pointxyzrgb> rgb(cloud);

viewer->addPointCloud<: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<: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<:visualization::pclvisualizer> viewer1;

viewer1 = rgbVis(fusedCloud);

else

fusedCloud = pointCloudFusion(fusedCloud, color\_im,depth\_im, poses\[idx\], cameraParams);

boost::shared\_ptr<: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;

第一帧点云

![0068944923db][]

image.png

3帧点云融合

![0068944923db][]

image.png

参考:

[0068944923db]: /images/20221119/61ce1f64d91b415dadb714365e2d0d31.png

pcl ptr转换_PCL: RGB-D图像转换成点云数据+点云融合

发表评论取消回复

还没有评论，来说两句吧...

相关阅读

相关 PCL大规模点云水平面校准

相关 pcl ptr转换_PCL: RGB-D图像转换成点云数据+点云融合

相关点云定义、PCL数据类型和点云处理方式

相关 PCL_三维点云拼接融合/点云粗配准/点云精配准

相关 PCL_点云数据处理方法概述

相关 RGBD 转换成点云 open3d

相关点云学习笔记9——python pcl可视化点云数据

相关点云学习笔记8——python pcl将点云数据转换成正视图

相关 PCL点云数据滤波降噪

相关 PCL点云库配置整理