公司网站域名如何续费,招才猫网站多少钱做的,秦皇岛建设银行网点分布,百度竞价登陆首先理清我们需要实现什么功能#xff0c;怎么实现#xff0c;提供一份整体逻辑#xff1a;包括主函数和功能函数
主函数逻辑#xff1a; 1. 读图,两张rgb#xff08;cv::imread#xff09; 2. 找到两张rgb图中的特征点匹配对 2.1定义所需要的参数#xff1a;keypoints…首先理清我们需要实现什么功能怎么实现提供一份整体逻辑包括主函数和功能函数
主函数逻辑 1. 读图,两张rgbcv::imread 2. 找到两张rgb图中的特征点匹配对 2.1定义所需要的参数keypoints1, keypoints2,matches 2.2 提取每张图像的检测 Oriented FAST 角点位置并匹配筛选调用功能函数1 3. 建立3d点像素坐标到相机坐标 3.1读出深度图cv::imread 3.2取得每个匹配点对的深度 3.2.1 得到第y行,第x个像素的深度值 ushort d d1.ptrunsigned short (row)[column] 3.2.2 去除没有深度的点 3.2.3 转到相机坐标系调用功能函数2
4. 调用epnp求解input3d点2d点对内参是否去畸变求解方式 4.1求解cv::solvePnP 4.2 求解结果为向量需要转成矩阵cv::Rodrigues
int main( int agrc, char** agrv) {
// 1. 读图两张rgbMat image1 imread(agrv[1] , CV_LOAD_IMAGE_COLOR );Mat image2 imread(agrv[2] , CV_LOAD_IMAGE_COLOR );assert(image1.data image2.data Can not load images!);// 2. 找到两张rgb图中的特征点匹配对// 2.1定义keypoints1, keypoints2,matchesstd::vectorKeyPointkeypoints1,keypoints2;std::vectorDMatchmatches;// 2.2 提取每张图像的检测 Oriented FAST 角点位置并匹配筛选Featurematcher(image1,image2, keypoints1,keypoints2,matches);// 3. 建立3d点像素坐标到相机坐标Mat K (Mat_double(3, 3) 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);//内参vectorPoint3f pts_3d;vectorPoint2f pts_2d;//3.1读出深度图Mat d1 imread(agrv[3],CV_LOAD_IMAGE_UNCHANGED);//3.2取得每个匹配点对的深度(ushort d d1.ptrunsigned short (row)[column];就是指向d1的第row行的第column个数据。数据类型为无符号的短整型 )for (DMatch m: matches){//3.2.1 得到第y行,第x个位置的像素的深度值ushort d d1.ptrunsigned short(int (keypoints1[m.queryIdx].pt.y)) [int(keypoints1[m.queryIdx].pt.x)];// 3.2.2 去除没有深度的点if(d0){continue;}float ddd/5000.0 ;//3.2.3 转到相机坐标系Point2d p1 pixtocam(keypoints1[m.queryIdx].pt , K);pts_3d.push_back(Point3f(p1.x*dd,p1.y*dd,dd));pts_2d.push_back(keypoints2[m.trainIdx].pt);}cout 3d-2d pairs: pts_3d.size() endl;// 4. 调用epnp求解input3d点2d点对内参false求解方式// solvePnP( InputArray objectPoints, InputArray imagePoints, InputArray cameraMatrix, InputArray distCoeffs, OutputArray rvec, OutputArray tvec, bool useExtrinsicGuess false, int flags SOLVEPNP_ITERATIVE );Mat r,t;// 4.1求解solvePnP(pts_3d,pts_2d,K,Mat(), r,t,false,SOLVEPNP_EPNP);// 4.2 求解结果为向量需要转成矩阵Mat R;cv::Rodrigues(r,R);coutRRendl;coutTtendl;// 5.可视化匹配Mat img_goodmatch;drawMatches(image1, keypoints1, image2, keypoints2, matches, img_goodmatch);imshow(good matches, img_goodmatch);waitKey(0);return 0;
}
功能函数1 Featurematcher
实现过程在前几篇中已经详细说明视觉slam14讲 逐行解析代码 ch7 / orb_cv.cpp
2.2.1初始化存储特征点数据的变量
2.2.2 提取每张图像的检测 Oriented FAST 角点位置
2.2.3 计算图像角点的BRIEF描述子
2.2.4 根据刚刚计算好的BRIEF描述子对两张图的角点进行匹配
2.2.5 匹配点对筛选计算最小距离和最大距离
2.2.6 当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.
void Featurematcher( const Mat image1, const Mat image2, std::vectorKeyPointkeypoints1, std::vectorKeyPoint keypoints2, std::vectorDMatch matches){// 2.2.1初始化存储特征点数据的变量Mat descr1, descr2;PtrFeatureDetector detector ORB::create();PtrDescriptorExtractor descriptor ORB::create();PtrDescriptorMatcher matcher DescriptorMatcher::create(BruteForce-Hamming);// 2.2.2 提取每张图像的检测 Oriented FAST 角点位置detector-detect(image1, keypoints1);detector-detect(image2, keypoints2);// 2.2.3 计算图像角点的BRIEF描述子descriptor-compute(image1, keypoints1, descr1);descriptor-compute(image2, keypoints2, descr2);// 2.2.4 根据刚刚计算好的BRIEF描述子对两张图的角点进行匹配std::vectorDMatch match;matcher-match(descr1, descr2, match);Mat img_match;drawMatches(image1, keypoints1, image2, keypoints2, match, img_match);imshow(all matches, img_match);waitKey(0);// 2.2.5 匹配点对筛选计算最小距离和最大距离double min_dis 10000, max_dis 0;// 2.2.5.1找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离for (int i 0; i descr1.rows; i){double dist match[i].distance;if (dist min_dis)min_dis dist;if (dist max_dis)max_dis dist;}coutmax_dismax_disendl;coutmin_dismin_disendl;//2.2.6 当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.for (int i 0; i descr1.rows; i){if (match[i].distance max(2*min_dis,30.0)){matches.push_back(match[i]);} }coutmatches.sizematches.size()endl;
}功能函数2
将输入的像素坐标x y转化到归一化相机坐标系下得到XY
我们知道相机的投影模型为, 即 所以 ,
Point2d pixtocam(const Point2d p , const Mat K){return Point2d(// X(u-cx)/fx(p.x - K.atdouble(0,2)) / K.atdouble(0,0) ,// Y(v-cy)/fy(p.y-K.atdouble(1,2)) / K.atdouble(1,1));
}最后匹配效果及位姿结果
allmatch goodmatch 位姿输出RT
