opencv双目通过特征提取恢复深度

VS2017+OpenCV通过双目视觉原理，通过SIFT特征点匹配恢复物体深度信息，输出三维坐标。

// 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 filedebug;
	file.open（“PointClouds.txt“）;
	debug.open（“debug.txt“）;

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

	double m1[3][3] = { { fx10Lcx }{ 0fy1Lcy }{ 001 } };
	Mat m1_matrix（Size（3 3） CV_64F 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;//左相机投影矩阵

	double m2[3][3] = { { fx20Rcx }{ 0fy2Rcy }{ 001 } };
	Mat m2_matrix（Size（3 3） CV_64F 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_64F）; //右相机旋转向量
	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;//右相机投影矩阵
	 
	float M1[3][3] = { { fx10Lcx }{ 0fy1Lcy }{ 001 } };
	Mat M1_matrix（Size（3 3） CV_32F M1）;  //左相机的内参矩阵

	float M2[3][3] = { { fx20Rcx }{ 0fy2Rcy }{ 001 } };
	Mat M2_matrix（Size（3 3） CV_32F M2）;  //右相机的内参矩阵

	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）;
	initUndis

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2018-06-30 09:37  NewPointClouds\
     目录           0  2018-05-04 16:56  NewPointClouds\.vs\
     目录           0  2018-05-04 16:56  NewPointClouds\.vs\NewPointClouds\
     目录           0  2018-06-28 20:26  NewPointClouds\.vs\NewPointClouds\v15\
     文件       34816  2018-06-28 20:26  NewPointClouds\.vs\NewPointClouds\v15\.suo
     文件    21553152  2018-06-28 20:26  NewPointClouds\.vs\NewPointClouds\v15\Browse.VC.db
     目录           0  2018-06-28 10:50  NewPointClouds\.vs\NewPointClouds\v15\ipch\
     文件     3538944  2018-06-28 10:50  NewPointClouds\.vs\NewPointClouds\v15\ipch\1b0d489489dd94da.ipch
     文件     3604480  2018-05-04 16:56  NewPointClouds\.vs\NewPointClouds\v15\ipch\98b0bdafa38a6eaf.ipch
     目录           0  2018-06-28 10:30  NewPointClouds\NewPointClouds\
     文件        1456  2018-05-04 16:56  NewPointClouds\NewPointClouds.sln
     文件         340  2018-06-28 10:30  NewPointClouds\NewPointClouds\debug.txt
     文件       21246  2018-06-28 10:30  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
     文件        6560  2018-06-28 10:30  NewPointClouds\NewPointClouds\PointClouds.txt
     文件         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-06-28 10:30  NewPointClouds\NewPointClouds\x64\Debug\
     文件        2406  2018-06-28 10:30  NewPointClouds\NewPointClouds\x64\Debug\NewPointClouds.log
     文件     1416235  2018-06-28 10:30  NewPointClouds\NewPointClouds\x64\Debug\NewPointClouds.obj
     文件     2818048  2018-06-28 09:48  NewPointClouds\NewPointClouds\x64\Debug\NewPointClouds.pch
     目录           0  2018-06-28 10:30  NewPointClouds\NewPointClouds\x64\Debug\NewPointClouds.tlog\
     文件        1810  2018-06-28 10:30  NewPointClouds\NewPointClouds\x64\Debug\NewPointClouds.tlog\CL.command.1.tlog
     文件       46204  2018-06-28 10:30  NewPointClouds\NewPointClouds\x64\Debug\NewPointClouds.tlog\CL.read.1.tlog
     文件        1338  2018-06-28 10:30  NewPointClouds\NewPointClouds\x64\Debug\NewPointClouds.tlog\CL.write.1.tlog
............此处省略12个文件信息