利用双目视觉原理求出匹配点深度信息

在C++下，利用Opencv的库，首先通过Sift找出左右图的匹配点，之后通过Ransac和knn算法剔除错误匹配点后，最后求出匹配点对的深度信息

// NewPointClouds.cpp: 定义控制台应用程序的入口点。
//
// MatchExperiment.cpp: 定义控制台应用程序的入口点。
//

#include “stdafx.h“
#include“iostream“
#include“opencv.hpp“
#include“highgui.hpp“
#include“xfeatures2d.hpp“

#define La1 -0.0765583
#define La2 1.12963
#define La3 -0.000478566
#define La4 -0.00934149   //左相机的四个畸变参数
#define Ra1 -0.0564752
#define Ra2 0.598481
#define Ra3 0.00219212
#define Ra4 -0.000801994  //右相机的四个畸变参数
#define Lcx 563.737           
#define Lcy 449.899    //左相机的光心位置
#define Rcx 638.157
#define Rcy 478.782   //右相机的光心位置
#define fx1 3317.41
#define fy1 3318.89   //左相机的焦距
#define fx2 3346.03
#define fy2 3347.61   //右相机的焦距

using namespace cv;
using namespace std;
int main（）
{
	Mat imageL0 = imread（“Pattenimage_L.bmp“）;
	Mat imageR0 = imread（“Pattenimage_R.bmp“）;
	//Mat imageL1 imageR1;
	//GaussianBlur（imageL0 imageL1 Size（3 3） 0.5）;
	//GaussianBlur（imageR0 imageR1 Size（3 3） 0.5）;
	ofstream file;
	file.open（“debug.txt“）;

	Mat dist1 = （Mat_（5 1） << La1 La2 La3  La4  0）; //左相机畸变参数（k1k2p1p2k3）
	Mat dist2 = （Mat_（5 1） << Ra1 Ra2 Ra3 Ra4 0）; //右相机畸变参数

	float m1[3][3] = { { fx10Lcx }{ 0fy1Lcy }{ 001 } };
	Mat m1_matrix（Size（3 3） CV_32F m1）;  //左相机的内参矩阵
								
	Mat r1_matrix = Mat::eye（3 3 CV_32FC1）;   // 左相机旋转矩阵（对角矩阵）
	Mat t1_matrix = （Mat_（3 1） << 0 0 0）;   //左相机平移矩阵
	Mat p1_matrix（Size（4 3） CV_32F）;
	Mat n1_matrix（Size（4 3） CV_32F）;
	hconcat（r1_matrix t1_matrix n1_matrix）;
	// vconcat（B C，A）;  等同于A=[B ; C] 按列合并   
	// hconcat（B C，A）;  等同于A=[B  C] 按行合并
    p1_matrix = m1_matrix * n1_matrix;//左相机投影矩阵

	float m2[3][3] = { { fx20Rcx }{ 0fy2Rcy }{ 001 } };
	Mat m2_matrix（Size（3 3） CV_32F m2）;  //右相机的内参矩阵
	float r2[3][1] = { { 0.00778951 }{ -0.121633 }{ 0.0150494 } }; 
	Mat r2_src2（Size（1 3） CV_32F r2）;
	Mat r2_matrix（Size（3 3） CV_32F）; //右相机旋转向量
	Rodrigues（r2_src2 r2_matrix）;   //旋转向量转化为旋转矩阵
	Mat t2_matrix = （Mat_（3 1） << 105.017 2.22392 19.288）;   //右相机平移向量
	Mat p2_matrix（Size（4 3） CV_32F）;
	Mat n2_matrix（Size（4 3） CV_32F）;
	hconcat（r2_matrix t2_matrix n2_matrix）;//右相机外参矩阵3X4
	p2_matrix = m2_matrix * n2_matrix;//右相机投影矩阵
	 
	Mat R1 = Mat::eye（3 3 CV_32F）;     //左相机图像畸变矫正
	Mat map1x = Mat（imageL0.size（） CV_32FC1）;
	Mat map1y = Mat（imageL0.size（） CV_32FC1）;
	initUndistortRectifyMap（m1_matrix dist1 R1 m1_matrix imageL0.size（） CV_32FC1 map1x map1y）;
	Mat picture1 = imageL0.clone（）;
	remap（imageL0 picture1 map1x map1y INTER_LINEAR）;

	Mat R2 = Mat::eye（3 3 CV_32F）;     //右相机图像畸变矫正
	Mat map2x = Mat（imageR0.size（） CV_32FC1）;
	Mat map2y = Mat（imageR0.size（） CV_32FC1）;
	initUndistortRectifyMap（m2_matrix dist2 R2 m2_matrix imageR0.size（） CV_32FC1 map2x map2y）;
	Mat picture2 = imageR0.clone（）;
	remap（imageR0 picture2 map2x map2y INTER_LINEAR）;
	imshow（“矫正后左图“ picture1）;
	imshow（“矫正后右图“ picture2）;
	//找出特征点
	Ptrf2d = xfeatures2d::SIFT::create（）

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2018-05-09 20:20  NewPointClouds\
     目录           0  2018-05-04 16:56  NewPointClouds\.vs\
     目录           0  2018-05-04 16:56  NewPointClouds\.vs\NewPointClouds\
     目录           0  2018-05-14 09:12  NewPointClouds\.vs\NewPointClouds\v15\
     文件       32256  2018-05-14 09:12  NewPointClouds\.vs\NewPointClouds\v15\.suo
     文件    21553152  2018-05-14 09:12  NewPointClouds\.vs\NewPointClouds\v15\Browse.VC.db
     目录           0  2018-05-04 16:59  NewPointClouds\.vs\NewPointClouds\v15\ipch\
     文件     3538944  2018-05-14 09:04  NewPointClouds\.vs\NewPointClouds\v15\ipch\1b0d489489dd94da.ipch
     文件     3604480  2018-05-04 16:56  NewPointClouds\.vs\NewPointClouds\v15\ipch\98b0bdafa38a6eaf.ipch
     目录           0  2018-05-09 20:22  NewPointClouds\NewPointClouds\
     文件        1456  2018-05-04 16:56  NewPointClouds\NewPointClouds.sln
     文件        2037  2018-05-09 20:23  NewPointClouds\NewPointClouds\debug.txt
     文件       12554  2018-05-09 20:22  NewPointClouds\NewPointClouds\NewPointClouds.cpp
     文件        8319  2018-05-04 16:59  NewPointClouds\NewPointClouds\NewPointClouds.vcxproj
     文件        1259  2018-05-04 16:56  NewPointClouds\NewPointClouds\NewPointClouds.vcxproj.filters
     文件         165  2018-05-04 16:56  NewPointClouds\NewPointClouds\NewPointClouds.vcxproj.user
     文件      481078  2018-04-23 09:51  NewPointClouds\NewPointClouds\pattenimage_L.bmp
     文件      481078  2018-04-23 09:50  NewPointClouds\NewPointClouds\pattenimage_R.bmp
     文件         330  2018-05-04 16:56  NewPointClouds\NewPointClouds\stdafx.cpp
     文件         366  2018-05-04 16:56  NewPointClouds\NewPointClouds\stdafx.h
     文件         370  2018-05-04 16:56  NewPointClouds\NewPointClouds\targetver.h
     目录           0  2018-05-04 16:57  NewPointClouds\NewPointClouds\x64\
     目录           0  2018-05-09 20:22  NewPointClouds\NewPointClouds\x64\Debug\
     文件        2244  2018-05-09 20:22  NewPointClouds\NewPointClouds\x64\Debug\NewPointClouds.log
     文件     1404495  2018-05-09 20:22  NewPointClouds\NewPointClouds\x64\Debug\NewPointClouds.obj
     文件     2818048  2018-05-04 16:59  NewPointClouds\NewPointClouds\x64\Debug\NewPointClouds.pch
     目录           0  2018-05-09 20:22  NewPointClouds\NewPointClouds\x64\Debug\NewPointClouds.tlog\
     文件        1810  2018-05-09 20:22  NewPointClouds\NewPointClouds\x64\Debug\NewPointClouds.tlog\CL.command.1.tlog
     文件       46204  2018-05-09 20:22  NewPointClouds\NewPointClouds\x64\Debug\NewPointClouds.tlog\CL.read.1.tlog
     文件        1338  2018-05-09 20:22  NewPointClouds\NewPointClouds\x64\Debug\NewPointClouds.tlog\CL.write.1.tlog
     文件        3408  2018-05-09 20:22  NewPointClouds\NewPointClouds\x64\Debug\NewPointClouds.tlog\link.command.1.tlog
............此处省略11个文件信息