5张彩色图+5张深度图+PCL= 点云拼接

该代码源于《视觉SLAM十四讲》 joinMap.cpp
主要是用已知的图片（5张彩色+5张深度图）+ pose.txt(相机位姿-> 前三位是xyz轴方向上的平移量，后四位是旋转四元数实部+虚部；用以上数据得到点云
思路：先用位姿数据计算得出旋转矩阵T
而后根据图片的像素坐标以及相机内参计算得出实物的相机坐标；最后将相机坐标转换为世界坐标
操作注意事项：
1. 先在build文件夹路径下编译源码得到可执行文件；
2. 将可执行文件复制到 pose.txt路径下（该路径下还有存放彩色图和深度图的文件夹）而后执行./joinMap 得到.pcd 地图
3. 执行pcl_viewer map.pcd 用可视化程序显示拼接的地图

该页代码用于日后查找方便（添加了部分笔记）

#include <iostream>
#include <fstream>
using namespace std;
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <Eigen/Geometry>
#include <boost/format.hpp>  // for formating strings#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>int main( int argc, char** argv )
{vector<cv::Mat> colorImgs, depthImgs;    // 彩色图和深度图 每张图用矩阵存储 多张图以向量形式表示vector<Eigen::Isometry3d> poses;   //欧式变换矩阵  4 *4    // 相机位姿//读操作（输入）的文件类ifstream fin("./pose.txt");//每行前三个数是平移（xyz） 后四个是四元数 指的是相机姿态if (!fin){cerr<<"请在有pose.txt的目录下运行此程序"<<endl;return 1;}for ( int i=0; i<5; i++ ){//boost::format 类似C++中的printfboost::format fmt( "./%s/%d.%s" ); //图像文件格式colorImgs.push_back( cv::imread( (fmt%"color"%(i+1)%"png").str() ));//str() 函数将对象转化为适于人阅读的形式。depthImgs.push_back( cv::imread( (fmt%"depth"%(i+1)%"pgm").str(), -1 )); // 使用-1读取原始图像double data[7] = {0};for ( auto& d:data )fin>>d;//最后一位是四元数实部Eigen::Quaterniond q( data[6], data[3], data[4], data[5] );Eigen::Isometry3d T(q);T.pretranslate( Eigen::Vector3d( data[0], data[1], data[2] ));poses.push_back( T );//T是 相机位姿（装的是5张图的位姿）}// 计算点云并拼接// 相机内参 double cx = 325.5;double cy = 253.5; //平移double fx = 518.0;double fy = 519.0;//缩放×焦距double depthScale = 1000.0;cout<<"正在将图像转换为点云..."<<endl;// 新建一个点云//PointCoud::Ptr是一个智能指针类 通过构造函数初始化指针指向的申请的空间/*Ptr是一个智能指针，返回一个PointCloud<PointT> 其中PointT是pcl::PointXYZRGB类型。它重载了->  返回了指向PointCloud<PointT>的指针*Ptr是下面类型 boost::shared_ptr<PointCloud<PointT> > *//*pointCloud 是一个智能指针类型的对象 具体可以参考http://blog.csdn.net/worldwindjp/article/details/18843087*/// 定义点云使用的格式：这里用的是XYZRGBtypedef pcl::PointXYZRGB PointT; typedef pcl::PointCloud<PointT> PointCloud;// 新建一个点云PointCloud::Ptr pointCloud( new PointCloud ); for ( int i=0; i<5; i++ ){cout<<"转换图像中: "<<i+1<<endl; cv::Mat color = colorImgs[i]; cv::Mat depth = depthImgs[i];Eigen::Isometry3d T = poses[i];for ( int v=0; v<color.rows; v++ )for ( int u=0; u<color.cols; u++ ){unsigned int d = depth.ptr<unsigned short> ( v )[u]; // 深度值（第v行u列）if ( d==0 ) continue; // 为0表示没有测量到Eigen::Vector3d point; //像素坐标point[2] = double(d)/depthScale; //Z 深度point[0] = (u-cx)*point[2]/fx;//x坐标point[1] = (v-cy)*point[2]/fy; Eigen::Vector3d pointWorld = T* point ;//将相机坐标系下的坐标变换到世界坐标系PointT p ;p.x = pointWorld[0];p.y = pointWorld[1];p.z = pointWorld[2];/*  color.step 虽然是一个类，但是它内部有一个转换操作符 operator size_t() const;
131                  * 此时的color.size编译器就会把它当做size_t类型的变量,这个值的大小是1920 这个是随着图像的读入MAT类中会有自动转换然后存储的buf[]中 */p.b = color.data[ v*color.step+u*color.channels() ];p.g = color.data[ v*color.step+u*color.channels()+1 ];p.r = color.data[ v*color.step+u*color.channels()+2 ];pointCloud->points.push_back( p );}}pointCloud->is_dense = false;cout<<"点云共有"<<pointCloud->size()<<"个点."<<endl;pcl::io::savePCDFileBinary("map.pcd", *pointCloud );return 0;
}

5张彩色图+5张深度图+PCL= 点云拼接相关推荐

奥比中光-Astra相机如何用python实现输出RGB彩色图和Depth深度图，并输出三维坐标代码
目前项目组在做这个跟结构光相机相关的课题,我在网上调研了几款相机后,最终确定了使用国产奥比中光的相机,主要原因还是性价比高. 下面介绍如何通过python代码实现调用相机的彩色图像和深度图像: 首先你 ...
RGB彩色图、Depth深度图
python opencv cv.applyColorMap()函数（颜色映射）ColormapTypes【将Intel Realsense D435深度图的黑白图映射为彩色图】
文章目录 API ColormapTypes 完整应用代码[将深度图的黑白图映射为彩色图] map原理能否map CV_24UC3的? API def applyColorMap(src, colo ...
使用OpenCV可视化Intel Realsensen D435 深度图为彩色图
使用OpenCV可视化Intel Realsensen D435 深度图为彩色图使用OpenCV将深度图转化为彩色图像,主要使用下面两个函数: cv2.convertScaleAbs(depth_i ...
c++ opencv 灰度图转彩色图
灰度图转彩色图______主要作用:在图上画彩色线条等等 //灰度图转彩色图 cv::Mat grayToRGB(const cv::Mat input_img) {//创建一个和灰度图一样大小的0值 ...
全民直播CTO张云龙：已全面迁移至阿里云
全民直播CTO张云龙:已全面迁移至阿里云 "建设一家大型的直播平台充满了技术挑战,毫不夸张的说,直播平台囊括了几乎所有的主流互联网技术--Web站点.APP客户端.音视频编解码.即时聊天.充 ...
数据装载指定一张表或者多张表直接装载到目标表_10
数据装载指定一张表或者多张表,直接装载到目标表文章目录 1. 复制脚本 2. 直接加载目标表 1. 复制脚本 # 切换oracle su - oracle# 进入根目录 cd /app/jiazai ...
云图说｜一张图看懂一站式DevOps利器——华为云DevCloud
阅识风云是华为云信息大咖,擅长将复杂信息多元化呈现,其出品的一张图(云图说).深入浅出的博文(云小课)或短视频(云视厅)总有一款能让您快速上手华为云.更多精彩内容请单击此处. 摘要: 华为云DevCl ...
mysql导入库排除某个表_mysql导入数据排除表-mysql导入数据排除某张表或多张表-吾爱编程网...
mysql在导出数据的时候可以使用--ignore-table参数来排除某张或者多张表,在数据库导入同样也可以使用,接下来吾爱编程就为大家介绍一下关于mysql导入数据排除某张表或多张表的方法,有需要 ...

5张彩色图+5张深度图+PCL= 点云拼接

5张彩色图+5张深度图+PCL= 点云拼接相关推荐

最新文章

热门文章