Heco是火币开放平台的公链基础设施,heco链智能合约dapp系统开发,未来将成为承载用户、资产和应用的基础平台。
概述RISC-V service为Huobi Chain提供了一个支持RISC-V指令集的虚拟机服务。用户可以通过该服务自行部署和运行合约,实现强大的自定义功能。
代码语言:javascript复制 /*************************************************************************
2 > File Name: src/jointPointCloud.cpp
3 > Author: Xiang gao
4 > Mail: gaoxiang12@mails.tsinghua.edu.cn
5 > Created Time: 2015年07月22日 星期三 20时46分08秒
6 ************************************************************************/
7
8 #include<iostream>
9 using namespace std;
10
11 #include "slamBase.h"
12
13 #include <opencv2/core/eigen.hpp>
14
15 #include <pcl/common/transforms.h>
16 #include <pcl/visualization/cloud_viewer.h>
17
18 // Eigen !
19 #include <Eigen/Core>
20 #include <Eigen/Geometry>
21
22 int main( int argc, char** argv )
23 {
24 //本节要拼合data中的两对图像
25 ParameterReader pd;
26 // 声明两个帧,FRAME结构请见include/slamBase.h
27 FRAME frame1, frame2;
28
29 //读取图像
30 frame1.rgb = cv::imread( "./data/rgb1.png" );
31 frame1.depth = cv::imread( "./data/depth1.png", -1);
32 frame2.rgb = cv::imread( "./data/rgb2.png" );
33 frame2.depth = cv::imread( "./data/depth2.png", -1 );
34
35 // 提取特征并计算描述子
36 cout<<"extracting features"<<endl;
37 string detecter = pd.getData( "detector" );
38 string descriptor = pd.getData( "descriptor" );
39
40 computeKeyPointsAndDesp( frame1, detecter, descriptor );
41 computeKeyPointsAndDesp( frame2, detecter, descriptor );
42
43 // 相机内参
44 CAMERA_INTRINSIC_PARAMETERS camera;
45 camera.fx = atof( pd.getData( "camera.fx" ).c_str());
46 camera.fy = atof( pd.getData( "camera.fy" ).c_str());
47 camera.cx = atof( pd.getData( "camera.cx" ).c_str());
48 camera.cy = atof( pd.getData( "camera.cy" ).c_str());
49 camera.scale = atof( pd.getData( "camera.scale" ).c_str() );
50
51 cout<<"solving pnp"<<endl;
52 // 求解pnp
53 RESULT_OF_PNP result = estimateMotion( frame1, frame2, camera );
54
55 cout<<result.rvec<<endl<<result.tvec<<endl;
56
57 // 处理result
58 // 将旋转向量转化为旋转矩阵
59 cv::Mat R;
60 cv::Rodrigues( result.rvec, R );
61 Eigen::Matrix3d r;
62 cv::cv2eigen(R, r);
63
64 // 将平移向量和旋转矩阵转换成变换矩阵
65 Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
66
67 Eigen::AngleAxisd angle(r);
68 cout<<"translation"<<endl;
69 Eigen::Translation<double,3> trans(result.tvec.at<double>(0,0), result.tvec.at<double>(0,1), result.tvec.at<double>(0,2));
70 T = angle;
71 T(0,3) = result.tvec.at<double>(0,0);
72 T(1,3) = result.tvec.at<double>(0,1);
73 T(2,3) = result.tvec.at<double>(0,2);
74
75 // 转换点云
76 cout<<"converting image to clouds"<<endl;
77 PointCloud::Ptr cloud1 = image2PointCloud( frame1.rgb, frame1.depth, camera );
78 PointCloud::Ptr cloud2 = image2PointCloud( frame2.rgb, frame2.depth, camera );
79
80 // 合并点云
81 cout<<"combining clouds"<<endl;
82 PointCloud::Ptr output (new PointCloud());
83 pcl::transformPointCloud( *cloud1, *output, T.matrix() );
84 *output = *cloud2;
85 pcl::io::savePCDFile("data/result.pcd", *output);
86 cout<<"Final result saved."<<endl;
87
88 pcl::visualization::CloudViewer viewer( "viewer" );
89 viewer.showCloud( output );
90 while( !viewer.wasStopped() )
91 {
92
93 }
94 return 0;
95 }