马蹄链合约DAPP项目系统开发分析方案（成熟理念）

 1 // 计算图像间的运动关系
 2     // 关键函数：cv::solvePnPRansac()
 3     // 为调用此函数准备必要的参数
 4     
 5     // 第一个帧的三维点
 6     vector<cv::Point3f> pts_obj;
 7     // 第二个帧的图像点
 8     vector< cv::Point2f > pts_img;
 9 
10     // 相机内参
11     CAMERA_INTRINSIC_PARAMETERS C;
12     C.cx = 325.5;
13     C.cy = 253.5;
14     C.fx = 518.0;
15     C.fy = 519.0;
16     C.scale = 1000.0;
17 
18     for (size_t i=0; i<goodMatches.size(); i++)
19     {
20         // query 是第一个, train 是第二个
21         cv::Point2f p = kp1[goodMatches[i].queryIdx].pt;
22         // 获取d是要小心！x是向右的，y是向下的，所以y才是行，x是列！
23         ushort d = depth1.ptr<ushort>( int(p.y) )[ int(p.x) ];
24         if (d == 0)
25             continue;
26         pts_img.push_back( cv::Point2f( kp2[goodMatches[i].trainIdx].pt ) );
27 
28         // 将(u,v,d)转成(x,y,z)
29         cv::Point3f pt ( p.x, p.y, d );
30         cv::Point3f pd = point2dTo3d( pt, C );
31         pts_obj.push_back( pd );
32     }
33 
34     double camera_matrix_data[3][3] = {
35         {C.fx, 0, C.cx},
36         {0, C.fy, C.cy},
37         {0, 0, 1}
38     };
39 
40     // 构建相机矩阵
41     cv::Mat cameraMatrix( 3, 3, CV_64F, camera_matrix_data );
42     cv::Mat rvec, tvec, inliers;
43     // 求解pnp
44     cv::solvePnPRansac( pts_obj, pts_img, cameraMatrix, cv::Mat(), rvec, tvec, false, 100, 1.0, 100, inliers );