heco生态链 Huobi HECO Chain(HECO)是一个去中心化高效节能公链，也是huo币开放平台推出的产品，在支撑高性能交易得基础上，实现智能合约的兼容，heco链智能合约dapp，案例演示。
我们把关键帧和PnP的结果都封成了结构体，以便将来别的程序调用。这两个函数的实现如下：
src/slamBase.cpp
代码语言：javascript复制 1 // computeKeyPointsAndDesp 同时提取关键点与特征描述子
2 void computeKeyPointsAndDesp( FRAME& frame, string detector, string descriptor )
3 {
4 cv::Ptr<cv::FeatureDetector> _detector;
5 cv::Ptr<cv::DescriptorExtractor> _descriptor;
6
7 cv::initModule_nonfree();
8 _detector = cv::FeatureDetector::create( detector.c_str() );
9 _descriptor = cv::DescriptorExtractor::create( descriptor.c_str() );
10
11 if (!_detector || !_descriptor)
12 {
13 cerr<<"Unknown detector or discriptor type !"<<detector<<","<<descriptor<<endl;
14 return;
15 }
16
17 _detector->detect( frame.rgb, frame.kp );
18 _descriptor->compute( frame.rgb, frame.kp, frame.desp );
19
20 return;
21 }
22
23 // estimateMotion 计算两个帧之间的运动
24 // 输入：帧1和帧2
25 // 输出：rvec 和 tvec
26 RESULT_OF_PNP estimateMotion( FRAME& frame1, FRAME& frame2, CAMERA_INTRINSIC_PARAMETERS& camera )
27 {
28 static ParameterReader pd;
29 vector< cv::DMatch > matches;
30 cv::FlannBasedMatcher matcher;
31 matcher.match( frame1.desp, frame2.desp, matches );
32
33 cout<<"find total "<<matches.size()<<" matches."<<endl;
34 vector< cv::DMatch > goodMatches;
35 double minDis = 9999;
36 double good_match_threshold = atof( pd.getData( "good_match_threshold" ).c_str() );
37 for ( size_t i=0; i<matches.size(); i )
38 {
39 if ( matches[i].distance < minDis )
40 minDis = matches[i].distance;
41 }
42
43 for ( size_t i=0; i<matches.size(); i )
44 {
45 if (matches[i].distance < good_match_threshold*minDis)
46 goodMatches.push_back( matches[i] );
47 }
48
49 cout<<"good matches: "<<goodMatches.size()<<endl;
50 // 第一个帧的三维点
51 vector<cv::Point3f> pts_obj;
52 // 第二个帧的图像点
53 vector< cv::Point2f > pts_img;
54
55 // 相机内参
56 for (size_t i=0; i<goodMatches.size(); i )
57 {
58 // query 是第一个, train 是第二个
59 cv::Point2f p = frame1.kp[goodMatches[i].queryIdx].pt;
60 // 获取d是要小心！x是向右的，y是向下的，所以y才是行，x是列！
61 ushort d = frame1.depth.ptr<ushort>( int(p.y) )[ int(p.x) ];
62 if (d == 0)
63 continue;
64 pts_img.push_back( cv::Point2f( frame2.kp[goodMatches[i].trainIdx].pt ) );
65
66 // 将(u,v,d)转成(x,y,z)
67 cv::Point3f pt ( p.x, p.y, d );
68 cv::Point3f pd = point2dTo3d( pt, camera );
69 pts_obj.push_back( pd );
70 }
71
72 double camera_matrix_data[3][3] = {
73 {camera.fx, 0, camera.cx},
74 {0, camera.fy, camera.cy},
75 {0, 0, 1}
76 };
77
78 cout<<"solving pnp"<<endl;
79 // 构建相机矩阵
80 cv::Mat cameraMatrix( 3, 3, CV_64F, camera_matrix_data );
81 cv::Mat rvec, tvec, inliers;
82 // 求解pnp
83 cv::solvePnPRansac( pts_obj, pts_img, cameraMatrix, cv::Mat(), rvec, tvec, false, 100, 1.0, 100, inliers );
84
85 RESULT_OF_PNP result;
86 result.rvec = rvec;
87 result.tvec = tvec;
88 result.inliers = inliers.rows;
89
90 return result;
91 }此外，我们还实现了一个简单的参数读取类。这个类读取一个参数的文本文件，能够以关键字的形式提供文本文件中的变量：
include/slamBase.h
代码语言：javascript复制 1 // 参数读取类
2 class ParameterReader
3 {
4 public:
5 ParameterReader( string filename="./parameters.txt" )
6 {
7 ifstream fin( filename.c_str() );
8 if (!fin)
9 {
10 cerr<<"parameter file does not exist."<<endl;
11 return;
12 }
13 while(!fin.eof())
14 {
15 string str;
16 getline( fin, str );
17 if (str[0] == '#')
18 {
19 // 以‘＃’开头的是注释
20 continue;
21 }
22
23 int pos = str.find("=");
24 if (pos == -1)
25 continue;
26 string key = str.substr( 0, pos );
27 string value = str.substr( pos 1, str.length() );
28 data[key] = value;
29
30 if ( !fin.good() )
31 break;
32 }
33 }
34 string getData( string key )
35 {
36 map<string, string>::iterator iter = data.find(key);
37 if (iter == data.end())
38 {
39 cerr<<"Parameter name "<<key<<" not found!"<<endl;
40 return string("NOT_FOUND");
41 }
42 return iter->second;
43 }
44 public:
45 map<string, string> data;
46 };
