3D点云系列———pcl：点云融合

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

3D点云系列———pcl：点云融合相关推荐

点云系列之点云数据格式的认识
点云系列之点云数据格式的认识文章目录点云系列之点云数据格式的认识一.常见的点云数据格式二.格式的具体介绍 2.1 off格式 2.2PLY格式 2.1.1header的特别说明 2.1.2 带 ...
3D点云系列——pcl：点云平滑及法线估计
通过重采样实现点云平滑需要平滑的情况: 用RGB-D激光扫描仪等设备扫描物体,尤其是比较小的物体时,往往会有测量误差.这些误差所造成的不规则数据如果直接拿来曲面重建的话,会使得重建的曲面不光滑或者有 ...
3D点云系列———pcl：点云网格化
参考:https://mp.weixin.qq.com/s/FfHkVY-lmlOSf4jKoZqjEA 什么是网格网格主要用于计算机图形学中,有三角.四角网格等很多种. 计算机图形学中的网格处理绝 ...
阿里云系列——7.阿里云IIS系列详解（过程+通用+最新）
网站部署之~阿里云系列汇总 http://www.cnblogs.com/dunitian/p/4958462.html 先讲IIS系列,Linux部署以后再继续讲先打开主机管理平台,确认域名绑定 ...
pcl点云处理基本步骤
一.功能描述本项目主要为计算机视觉方面的应用,可以实现障碍物或目标物体的检测.提取或识别,文章为项目基础步骤的描述,希望能够帮到一些不知从何下手的同学,也算是为了以后自己的复习吧.(因为项目不是自己 ...
【点云系列】综述： Deep Learning for 3D Point Clouds: A Survey
文章目录起因题目摘要 1 简介 2 背景 2.1 数据集 2.2 衡量指标 3 3D形状分类 3.1基于多视角的方法 3.2基于体素的方法 3.3 基于点的方法 3.3.1逐点MLP网络 3.3 ...
PCL入门系列 —— StatisticalOutlierRemoval 点云统计滤波
PCL入门系列 -- StatisticalOutlierRemoval 点云统计滤波前言程序说明输出结果代码示例总结前言随着工业自动化.智能化的不断推进,机器视觉(2D/3D)在工业领 ...
3D点云系列（一）点云介绍
点云数据简介点云数据(point cloud data)是指在一个三维坐标系统中的一组向量的集合.扫描资料以点的形式记录,每一个点包含有三维坐标,有些可能含有颜色信息(RGB)或反射强度信息(Int ...
激光点云系列之一：详解激光雷达点云数据的处理过程
交流群 | 进"传感器群/滑板底盘群"请加微信号:xsh041388 交流群 | 进"汽车基础软件群"请加微信号:Faye_chloe 备注信息:群名称 + 真 ...

3D点云系列———pcl：点云融合

3D点云系列———pcl：点云融合相关推荐

最新文章

热门文章