标签：机器人 矩阵 像素 相机 智能 实验 坐标 图像 坐标系

视觉3D 点云图构建与深度测量

一、实验目的

　　通过对 RGB-D 相机参数读取、画面帧的拼接验证，了解机器视觉中深度图像处理的一般工作流程，掌握 OpenCV、 Eigen、 PCL 等第三方图像图形库的使用方法。尝试自行搭建双目视觉系统，实现对视图中各物体的深度测量。

二、实验内容

1. 矩阵运算库 Eigen 的使用
2. 彩色图、深度图的读取与参数提取
3. 相机参数加载与使用
4. 点云地图拼接
5. 双目视觉系统标定与深度测量(限实验方案设计)

三、实验设备

　　自带笔记本 PC 机摄像头/USB 摄像头，Ubuntu系统，OpenCV 开发库

四、实验代码分析

　　1.点云地图拼接代码分析

  1 #include <iostream>
  2 #include <fstream>
  3 using namespace std;
  4 #include <opencv2/core/core.hpp>
  5 #include <opencv2/highgui/highgui.hpp>
  6 #include <Eigen/Geometry> 
  7 #include <boost/format.hpp>  // for formating strings
  8 #include <pcl/point_types.h> 
  9 #include <pcl/io/pcd_io.h> 
 10 #include <pcl/visualization/pcl_visualizer.h>
 11 
 12 int main( int argc, char** argv )
 13 {
 14     /*
 15     在计算机内存中，数字图像以矩阵的形式存储。OpenCV中，数据结构Mat是保存图像像素信息的矩阵。主要包含两部分：矩阵头和一个指向像素数据的矩阵指针。
 16     矩阵头主要包含矩阵尺寸、存储方法、存储地址和引用次数，矩阵头大小为常数，不会随着图像的大小而改变，但保存图像像素数据的矩阵会随图像大小的改变而改变
 17     当图像进行复制时，不再复制整个Mat数据，而只复制矩阵头和指向像素矩阵的指针
 18     */
 19     vector<cv::Mat> colorImgs, depthImgs;    // 彩色图和深度图；共5张，所有图像分别存储到动态数组中
 20     vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>> poses;         // 相机位姿
 21     
 22     /*
 23     pose.txt以Twc的形式分别记录五张图像的相机位姿（相机的外参数）。[x,y,z,qx,qy,qz,qw]——平移向量加旋转四元数，其中qw是四元数的实部
 24     */
 25     ifstream fin("./pose.txt"); //读取相机位姿文件
 26     if (!fin)
 27     {
 28         cerr<<"请在有pose.txt的目录下运行此程序"<<endl;
 29         return 1;
 30     }
 31     
 32     for ( int i=0; i<5; i++ )
 33     {
 34         /*
 35         用boost中的format格式类来循环读取图片，否则单张读取图片就会有问题
 36         imread读取绝对路径下的图片，其读取的是参数值，所以要取str()；返回Mat对象，添加到彩色图片数组的末尾；循环读取并添加
 37         */
 38         boost::format fmt( "./%s/%d.%s" ); //图像文件格式
 39         colorImgs.push_back( cv::imread( (fmt%"color"%(i+1)%"png").str() ));
 40         depthImgs.push_back( cv::imread( (fmt%"depth"%(i+1)%"pgm").str(), -1 )); // 使用-1读取原始图像
 41         
 42         double data[7] = {0};
 43         for ( auto& d:data )    //auto表示自动根据后面的元素获取符合要求的类型
 44             fin>>d;    //文件流类型的变量fin依次读取5张图像的相机位姿数据（7个），赋值给d数组
 45         Eigen::Quaterniond q( data[6], data[3], data[4], data[5] );  //后面四个数据保存为Egien四元数对象（实部在前，虚部在后）
 46         Eigen::Isometry3d T(q);  //欧式变换矩阵初始化旋转部分（四元数表示）
 47         T.pretranslate( Eigen::Vector3d( data[0], data[1], data[2] ));  //欧式变换矩阵初始化平移向量
 48         poses.push_back( T );   //存储欧式变换矩阵到位姿数组中
 49     
 50     // 计算点云并拼接
 51     // 点云数据：指在一个三维坐标系统中的一组向量的集合。通常以x,y,z三维坐标的形式表示，既表示几何位置信息，还表示一个点的RGB颜色、灰度值、深度和分割结果
 52     // 相机内参 
 53     double cx = 325.5;
 54     double cy = 253.5;
 55     double fx = 518.0;
 56     double fy = 519.0;
 57     double depthScale = 1000.0;
 58     
 59     cout<<"正在将图像转换为点云..."<<endl;
 60     
 61     // 定义点云使用的格式：这里用的是XYZRGB
 62     //程序中点云中的点携带的信息为坐标x,y,z和颜色b,g,r；Point代表一个点，PongyCloud为全部点的集合
 63     typedef pcl::PointXYZRGB PointT; 
 64     typedef pcl::PointCloud<PointT> PointCloud;
 65     
 66     // 新建一个点云
 67     PointCloud::Ptr pointCloud( new PointCloud ); //Ptr是一个智能指针类，这里通过构造函数初始化指针指向申请的空间
 68     /*
 69     接下来的整个for循环对每个图片依次做核心处理
 70     通过图像得知每个像素的坐标，使用内参将像素坐标转换到相机坐标，再使用外参（相机位姿）将像素在相机坐标位置转换到世界坐标
 71     世界坐标中的值和点云中点数据的值是两个不相容的标准，需要将世界坐标点存储到点云格式的变量中（包含颜色信息）
 72     */
 73     for ( int i=0; i<5; i++ )
 74     {
 75         cout<<"转换图像中: "<<i+1<<endl; 
 76         cv::Mat color = colorImgs[i]; 
 77         cv::Mat depth = depthImgs[i];
 78         Eigen::Isometry3d T = poses[i];  //相机外参定义欧式变换矩阵T
 79         for ( int v=0; v<color.rows; v++ )
 80             for ( int u=0; u<color.cols; u++ )
 81             {
 82                 unsigned int d = depth.ptr<unsigned short> ( v )[u]; // 深度值，相当于按行来遍历
 83                 if ( d==0 ) continue; // 为0表示没有测量到
 84                 Eigen::Vector3d point;  //point为三维向量类型，表示相机坐标系下的点
 85                 point[2] = double(d)/depthScale;  //实际尺度的一个缩放
 86                 point[0] = (u-cx)*point[2]/fx;
 87                 point[1] = (v-cy)*point[2]/fy;   //根据针孔成像模型，计算像素点在相机坐标系中的位置
 88                 Eigen::Vector3d pointWorld = T*point;  //通过变换矩阵T，将坐标点从相机坐标系变换到世界坐标系
 89                 
 90                 PointT p ; //定义一个点云的点对象，将世界坐标系下的坐标用PCL专门的点云格式存储起来
 91                 p.x = pointWorld[0];
 92                 p.y = pointWorld[1];
 93                 p.z = pointWorld[2];
 94                 p.b = color.data[ v*color.step+u*color.channels() ];
 95                 p.g = color.data[ v*color.step+u*color.channels()+1 ];
 96                 p.r = color.data[ v*color.step+u*color.channels()+2 ];
 97                 pointCloud->points.push_back( p );
 98             }
 99     }
100     
101     pointCloud->is_dense = false;//这里有可能深度图中某些像素没有深度信息，那么就是包含无效的像素，所以先置为false
102     cout<<"点云共有"<<pointCloud->size()<<"个点."<<endl;  //获取PointCloud指向的对象
103     pcl::io::savePCDFileBinary("map.pcd", *pointCloud );   //函数参数是要存储的点云的名称和要拼接的点云名称；点云用指针的形式
104     return 0;
105 }

　　2.双目相机实现深度测量

　　首先从物理原理上展示了为什么单目相机不能测量深度值而双目可以的原因。我们看到红色线条上三个不同远近的黑色的点在下方相机上投影在同一个位置，因此单目相机无法分辨成的像到底是远的那个点还是近的那个点，但是它们在上方相机的投影却位于三个不同位置，因此通过两个相机的观察可以确定到底是哪一个点。

双目相机确定深度示意图

　　首先我们从理想的情况开始分析：假设左右两个相机位于同一平面（光轴平行），且相机参数（如焦距f）一致。那么深度值的推导原理和公式如下。

理想情况下双目立体视觉相机深度值计算原理

根据上述推导，空间点P离相机的距离（深度）z=f*b/d，可以发现如果要计算深度z，必须要知道：

1、相机焦距f，左右相机基线b。这些参数可以通过先验信息或者相机标定得到。

2、视差d。需要知道左相机的每个像素点(xl, yl)和右相机中对应点(xr, yr)的对应关系。这是双目视觉的核心问题。

对于左图中的一个像素点，如何确定该点在右图中的位置，可以使用极线约束的方法。C1，C2是两个相机，P是空间中的一个点，P和两个相机中心点C1、C2形成了三维空间中的一个平面PC1C2，称为极平面（Epipolar plane）。极平面和两幅图像相交于两条直线，这两条直线称为极线(Epipolar line)。P在相机C1中的成像点是P1，在相机C2中的成像点是P2，但是P的位置事先是未知的。

对于左图的P1点，寻找它在右图中的对应点P2，这样就能确定P点的空间位置，也就是我们想要的空间物体和相机的距离（深度）。所谓极线约束（Epipolar Constraint）就是指当同一个空间点在两幅图像上分别成像时，已知左图投影点p1，那么对应右图投影点p2一定在相对于p1的极线上，这样可以极大的缩小匹配范围。根据极线约束的定义，我们可以在下图中直观的看到P2一定在对极线上，所以我们只需要沿着极线搜索一定可以找到和P1的对应点P2。

极线约束示意图

　　但是因为有些场景下两个相机需要独立固定，很难保证光心C1，C2完全水平，即使是固定在同一个基板上也会因为装配的原因导致光心不完全水平。如下图所示。我们看到两个相机的极线不仅不平行，还不共面。

非理想情况下的极线

　　这时候就要采取图像矫正技术，图像矫正是通过分别对两张图片用单应（homography）矩阵变换（可以通过标定获得）得到的，的目的就是把两个不同方向的图像平面（下图中灰色平面）重新投影到同一个平面且光轴互相平行（下图中黄色平面），这样就可以用前面理想情况下的模型了，两个相机的极线也变成水平的了

 

图像矫正示意图

　　经过图像矫正后，左图中的像素点只需要沿着水平的极线方向搜索对应点就可以了。从下图中我们可以看到三个点对应的视差（红色双箭头线段）是不同的，越远的物体视差越小，越近的物体视差越大，这和我们的常识是一致的。

图像矫正后的结果，红色双箭头线段是对应点的视差

五、实验结果

　　程序刚运行出来的点云拼接图像，是倒立的图像，且上下左右方向也是反过来的。这个 p.x = pointWorld[0];  p.y = pointWorld[1];  p.z = pointWorld[2]; 这三行代码来控制的，将其改为p.x = -pointWorld[0];  p.y = -pointWorld[1];  p.z = -pointWorld[2]; 生成的点云拼接图像是正常角度的。我们点击鼠标左键拖动图像可以调整图像的角度，能够比较清晰的看到重建的3D点云图，实验结果如下图所示。

倒立的点云图像

 正常角度的点云拼接图像

查看不同角度的点云图像

查看不同角度的点云图像

六、问题解答

　　1． 单目相机内参、外参是指哪些参数？外参如何用矩阵表示？

　　针孔和畸变两个模型能够把外部的三维点投影到相机内部成像平面，构成了相机的内参数。针孔模型如下图所示。

　　像素坐标系通常的定义方式是：远点o ′位于图像的左上角，u轴享有与x轴平行，v轴向下与y轴平行。像素坐标系与成像平面之间，相差了一个缩放和一个原点的平移。我们设像素坐标系在u轴上缩放了α倍，v上缩放了β倍。同时，原点平移了[cx; cy]T。那么P ′的坐标与像素坐标 [u; v]T 的关系为：

 

　　其中，f 的单位为米，α, β的单位为像素每米，所以 fx,fy 的单位为像素。把该式写成矩阵形式:

　　把中间的量组成的矩阵称为相机的内参数矩阵K。通常认为，相机的内参在出厂之后是固定的，不会在使用过程中发生变化。

　　由于针孔可以透过的光线太少，成像会不清楚，所以往往都会加上凸透镜汇聚更多的光线。但是加上凸透镜以后，会导致成像畸变，所以还需要校正透镜畸变。透镜的畸变主要分为两类，一类是径向畸变，一类是切向畸变。

　　径向畸变会产生“鱼眼”现象。成像中心处径向畸变为0，径向畸变随着与成像中心距离增大而增大，在图像边缘处达到最大径向畸变。常常用偶次幂的泰勒公式描述径向畸变。

　　切向畸变由透镜和成像平面不平行引起。常用如下公式描述。

　　P 的相机坐标是它的世界坐标（记为 Pw），根据相机的当前位姿，变换到相机坐标系下的结果。相机的位姿由它的旋转矩阵 R 和平移向量 t来描述。那么有：

　　相机的位姿 R, t 又称为相机的外参数。外参矩阵以刚体变换矩阵的形式可以记为：左边一个3*3旋转矩阵，右边一个3*1的平移列向量：

　　常见的做法是在底部增加一行(0,0,0,1)，这使得矩阵为方形的，允许我们进一步将矩阵分解为旋转和平移矩阵：

　　2． 双目相机内参、外参是哪些参数？什么是双目相机的本征矩阵？

　　任意两个坐标系之间的相对位置关系都可以通过两个矩阵来描述：旋转矩阵R和平移矩阵T我们此处用R和T来描述左右两个摄像机{camera}坐标系的相对关系，具体为将左摄像机{camera}下的坐标转换到右摄像机{camera}下的坐标。

　　假设空间中有一点P，其在{world}坐标系下的坐标为PW，其在左右摄像机{camera}坐标系下的坐标可以表示为：　　 

　　其中Pl和Pr又有如下的关系：

　　双目摄像机分析中往往以左摄像机{camera}为主坐标系，但是R和T却是左{camera}向右{camera}转换，所以Tx为负数。

　　综合（1）（2）两式，可以推得：

　　单目标定中相机外参数就是此处的Rl，Tl，Rr和Tr，代入（3）式就可以求出R和T。

　　本征矩阵

　　对级几何在双目问题中非常的重要，可以简化立体匹配等问题，而要应用对级几何去解决问题，比如求级线，需要知道本征矩阵或者基础矩阵，因此双目标定过程中也会把本征矩阵和基础矩阵算出来。之所以说是算，因为这两个矩阵与R和T并不独立。

　　本征矩阵常用字母E来表示，其物理意义是左右{picture}坐标系相互转换的矩阵，可以描述左右摄像机图像平面上对应点之间的关系。

　　假设空间中有一点P，其在{world}坐标系下的坐标为PW，其在左右摄像机{camera}坐标系下的坐标可以为Pl和Pr，右{camera}坐标系原点在左{camera}坐标系的坐标为Tr=[Tx,Ty,Tz]T，则有：

　　则通过点Tr的所有点的Pl所组成的平面（即极面）可以用下式表示：

　　将Pl×Tr写成矩阵相乘的形式：

　　其中S为：

 　　综合（5）（6）式可得：

　　乘积RS即为本征矩阵E，利用投影方程将（8）式简化：

　　（9）式描述了同一物理点在左右摄像机图像平面上投影在{picture}下的关系。

　　基础矩阵F

　　双目系统中，常常只对{pixel}坐标系下的坐标感兴趣，所以给本征矩阵E加上相机内参数矩阵M的相关信息，就可得到描述同一物理点在左右摄像机图像平面上投影在{pixel}下的关系。

　　将（9）式结合Ppix=MPp可得：

　　由此可将基础矩阵F定义为：

　　最终得到同一物理点在左右摄像机图像平面上投影在{pixel}下的关系：

 

 

 

 

　　单目摄像机需要标定的参数双目都需要标定

双目摄像机比单目摄像机多标定的参数（R和T）主要是描述两个摄像机相对位置关系的参数，这些参数在立体校正和对极几何中用处很大

本征矩阵E描述的是同一点投影在{picture}坐标系下的关系，单位为mm

基础矩阵F描述的是同一点投影在{pixel}坐标系下的关系，单位为pix

备注： 该文章为课程实验报告，有的地方可能并不全面，仅供参考，希望对大家有一些帮助，当然请不要直接复制文章内容。

       

 

 

标签：机器人,矩阵,像素,相机,智能,实验,坐标,图像,坐标系
来源： https://www.cnblogs.com/xm20200429/p/12817511.html

本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享；
2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关；
3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关；
4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除；
5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。