BSC币安链智能合约系统开发搭建技术（参照）

 // 各种头文件 
11 // C++标准库
12 #include <fstream>
13 #include <vector>
14 using namespace std;
15 
16 // OpenCV
17 #include <opencv2/core/core.hpp>
18 #include <opencv2/highgui/highgui.hpp>
19 
20 //PCL
21 #include <pcl/io/pcd_io.h>
22 #include <pcl/point_types.h>
23 
24 // 类型定义
25 typedef pcl::PointXYZRGBA PointT;
26 typedef pcl::PointCloud<PointT> PointCloud;
27 
28 // 相机内参结构
29 struct CAMERA_INTRINSIC_PARAMETERS 
30 { 
31     double cx, cy, fx, fy, scale;
32 };
33 
34 // 函数接口
35 // image2PonitCloud 将rgb图转换为点云
36 PointCloud::Ptr image2PointCloud( cv::Mat& rgb, cv::Mat& depth, CAMERA_INTRINSIC_PARAMETERS& camera );
37 
38 // point2dTo3d 将单个点从图像坐标转换为空间坐标
39 // input: 3维点Point3f (u,v,d)
40 cv::Point3f point2dTo3d( cv::Point3f& point, CAMERA_INTRINSIC_PARAMETERS& camera );

 1 /*************************************************************************
 2     > File Name: src/slamBase.cpp
 3     > Author: xiang gao
 4     > Mail: gaoxiang12@mails.tsinghua.edu.cn
 5     > Implementation of slamBase.h
 6     > Created Time: 2015年07月18日 星期六 15时31分49秒
 7  ************************************************************************/
 8 
 9 #include "slamBase.h"
10 
11 PointCloud::Ptr image2PointCloud( cv::Mat& rgb, cv::Mat& depth, CAMERA_INTRINSIC_PARAMETERS& camera )
12 {
13     PointCloud::Ptr cloud ( new PointCloud );
14 
15     for (int m = 0; m < depth.rows; m++)
16         for (int n=0; n < depth.cols; n++)
17         {
18             // 获取深度图中(m,n)处的值
19             ushort d = depth.ptr<ushort>(m)[n];
20             // d 可能没有值，若如此，跳过此点
21             if (d == 0)
22                 continue;
23             // d 存在值，则向点云增加一个点
24             PointT p;
25 
26             // 计算这个点的空间坐标
27             p.z = double(d) / camera.scale;
28             p.x = (n - camera.cx) * p.z / camera.fx;
29             p.y = (m - camera.cy) * p.z / camera.fy;
30             
31             // 从rgb图像中获取它的颜色
32             // rgb是三通道的BGR格式图，所以按下面的顺序获取颜色
33             p.b = rgb.ptr<uchar>(m)[n*3];
34             p.g = rgb.ptr<uchar>(m)[n*3+1];
35             p.r = rgb.ptr<uchar>(m)[n*3+2];
36 
37             // 把p加入到点云中
38             cloud->points.push_back( p );
39         }
40     // 设置并保存点云
41     cloud->height = 1;
42     cloud->width = cloud->points.size();
43     cloud->is_dense = false;
44 
45     return cloud;
46 }
47 
48 cv::Point3f point2dTo3d( cv::Point3f& point, CAMERA_INTRINSIC_PARAMETERS& camera )
49 {
50     cv::Point3f p; // 3D 点
51     p.z = double( point.z ) / camera.scale;
52     p.x = ( point.x - camera.cx) * p.z / camera.fx;
53     p.y = ( point.y - camera.cy) * p.z / camera.fy;
54     return p;
55 }