双目测距demo

  1 #include <iostream>
  2 #include <opencv2/core/core.hpp>
  3 #include <opencv2/features2d/features2d.hpp>
  4 #include <opencv2/highgui/highgui.hpp>
  5 
  6 using namespace std;
  7 using namespace cv;
  8 
  9 // the IntrinsicMatrix of the left camera
 10 
 11 const Mat IntrinsicMatrix = (Mat_<double>(3, 3) << 806.253555589359  ,  0,   311.16780116005,
 12 0,  805.667519352931 ,  244.341382241868,
 13 0 ,   0,      1);
 14 
 15 
 16 
 17 
 18 
 19 // baseline
 20 //stereoParams.TranslationOfCamera2 里第一个参数，注意双目标定时候，务必设置准确棋盘实际宽度
 21 double baseline = 88.8804297376557;//绝对值
 22 int main()
 23 {
 24     Mat img_1 = imread("Left.jpg", CV_LOAD_IMAGE_COLOR);
 25     Mat img_2 = imread("Right.jpg", CV_LOAD_IMAGE_COLOR);
 26 
 27     //-- 初始化
 28     std::vector<KeyPoint> keypoints_1, keypoints_2;
 29     Mat descriptors_1, descriptors_2;
 30     Ptr<FeatureDetector> detector = ORB::create(5);
 31     Ptr<DescriptorExtractor> descriptor = ORB::create(5);
 32     
 33     Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
 34 
 35     //-- 第一步:检测 Oriented FAST 角点位置
 36     detector->detect(img_1, keypoints_1);
 37     detector->detect(img_2, keypoints_2);
 38 
 39     //-- 第二步:根据角点位置计算 BRIEF 描述子
 40     descriptor->compute(img_1, keypoints_1, descriptors_1);
 41     descriptor->compute(img_2, keypoints_2, descriptors_2);
 42 
 43     Mat outimg1, outimg2;
 44     drawKeypoints(img_1, keypoints_1, outimg1, Scalar::all(-1), DrawMatchesFlags::DEFAULT);
 45     drawKeypoints(img_2, keypoints_2, outimg2, Scalar::all(-1), DrawMatchesFlags::DEFAULT);
 46     imshow("ORB特征点-left", outimg1);
 47     imshow("ORB特征点-right", outimg2);
 48     //-- 第三步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离
 49     vector<DMatch> matches;
 50     //BFMatcher matcher ( NORM_HAMMING );
 51     matcher->match(descriptors_1, descriptors_2, matches);
 52 
 53     //-- 第四步:匹配点对筛选
 54     double min_dist = 10000, max_dist = 0;
 55 
 56     //找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离
 57     for (int i = 0; i < descriptors_1.rows; i++)
 58     {
 59         double dist = matches[i].distance;
 60         if (dist < min_dist) min_dist = dist;
 61         if (dist > max_dist) max_dist = dist;
 62     }
 63 
 64     //当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.
 65     std::vector< DMatch > good_matches;
 66     for (int i = 0; i < descriptors_1.rows; i++)
 67     {
 68         if (matches[i].distance <= max(2 * min_dist, 30.0))
 69         {
 70             good_matches.push_back(matches[i]);
 71         }
 72     }
 73 
 74     //-- 第五步:绘制匹配结果
 75     Mat img_match;
 76     Mat img_goodmatch;
 77     drawMatches(img_1, keypoints_1, img_2, keypoints_2, matches, img_match);
 78     drawMatches(img_1, keypoints_1, img_2, keypoints_2, good_matches, img_goodmatch);
 79     imshow("所有匹配点对", img_match);
 80     imshow("优化后匹配点对", img_goodmatch);
 81 
 82     
 83     if (1 <= good_matches.size())
 84     {
 85         Point2d pointleft  = keypoints_1[good_matches[0].queryIdx].pt;
 86         cout << "point of the left : " << pointleft.x << " ," << pointleft.y << endl;
 87         Point2d pointright = keypoints_2[good_matches[0].trainIdx].pt;
 88         cout << "point of the Right : " << pointright.x << " ," << pointright.y << endl;
 89 
 90         //视差
 91         double disparty = abs(pointleft.x - pointright.x);
 92         cout << "disparty of two points :" << disparty << endl;
 93 
 94         //参数读取 fx cx fy cy
 95         const double fx = IntrinsicMatrix.at<double>(0, 0);
 96         const double cx = IntrinsicMatrix.at<double>(0, 2);
 97         const double fy = IntrinsicMatrix.at<double>(1, 1);
 98         const double cy = IntrinsicMatrix.at<double>(1, 2);
 99         cout << fx<<"," << cx <<","<< fy<<"," << cy << endl;
100 
101         //焦距： f = (fx + fy)/2
102         double f = (fx + fy) / 2.0;
103         cout << "focal length = " << f << endl;
104 
105         //计算点到左边相机光心的  z  坐标
106         //公式： disparty * z = f * baseline (依次：视差、z坐标、焦距、基线；    单位：像素、mm、像素、mm)
107         double Z = (f * baseline) / disparty;
108         cout << "Z = " << Z << endl;
109 
110         //    Z*u      fx   0   cx  |  X        X = (Z*u - cx*Z)/fx
111         //    Z*v   =  0    fy  cy  |  Y    =>  Y = (Z*v - cy*Z)/fy
112         //    Z        0    0    1  |  Z
113 
114         double X = (pointleft.x - cx)*Z / fx;
115         double Y = (pointleft.y - cy)*Z / fy;
116         cout << "3D = " << "[" << X << "," << Y << "," << Z << "]" << endl;
117 
118 
119 
120         //test
121         
122         Point2d pointLeft_(581.535006749608,          155.058265533219);
123         Point2d pointRight_(505.767095783631,          138.316326128893);
124 
125         double disparty_ = abs(pointRight_.x - pointLeft_.x);
126         double Z_ = (f * baseline) / disparty_;
127         double X_ = (pointLeft_.x - cx)*Z_ / fx;
128         double Y_ = (pointLeft_.y - cy)*Z_ / fy;
129         cout << "test 3D = " << "[" << X_ << "," << Y_ << "," << Z_ << "]" << endl;
130 
131     }
132 
133 
134     waitKey(0);
135     return 0;
136 }