opencv双摄像头拼接接缝处的处理

#include "stdafx.h"
#include "highgui/highgui.hpp"    
#include "opencv2/nonfree/nonfree.hpp"    
#include "opencv2/legacy/legacy.hpp"   
#include <fstream>
#include <iostream>
#include <string>
#include "windows.h"

using namespace cv;
using namespace std;

void OptimizeSeam(Mat& img1, Mat& trans, Mat& dst);
Point2f getTransformPoint(const Point2f originalPoint, const Mat &transformMaxtri);


typedef struct
{
	Point2f left_top;
	Point2f left_bottom;
	Point2f right_top;
	Point2f right_bottom;
}four_corners_t;

four_corners_t corners;

void CalcCorners(const Mat& H, const Mat& src)
{
	double v2[] = { 0, 0, 1 };//左上角
	double v1[3];//变换后的坐标值
	Mat V2 = Mat(3, 1, CV_64FC1, v2);  //列向量
	Mat V1 = Mat(3, 1, CV_64FC1, v1);  //列向量

	V1 = H * V2;
	//左上角(0,0,1)
	cout << "V2: " << V2 << endl;
	cout << "V1: " << V1 << endl;
	corners.left_top.x = v1[0] / v1[2];
	corners.left_top.y = v1[1] / v1[2];

	//左下角(0,src.rows,1)
	v2[0] = 0;
	v2[1] = src.rows;
	v2[2] = 1;
	V2 = Mat(3, 1, CV_64FC1, v2);  //列向量
	V1 = Mat(3, 1, CV_64FC1, v1);  //列向量
	V1 = H * V2;
	corners.left_bottom.x = v1[0] / v1[2];
	corners.left_bottom.y = v1[1] / v1[2];

	//右上角(src.cols,0,1)
	v2[0] = src.cols;
	v2[1] = 0;
	v2[2] = 1;
	V2 = Mat(3, 1, CV_64FC1, v2);  //列向量
	V1 = Mat(3, 1, CV_64FC1, v1);  //列向量
	V1 = H * V2;
	corners.right_top.x = v1[0] / v1[2];
	corners.right_top.y = v1[1] / v1[2];

	//右下角(src.cols,src.rows,1)
	v2[0] = src.cols;
	v2[1] = src.rows;
	v2[2] = 1;
	V2 = Mat(3, 1, CV_64FC1, v2);  //列向量
	V1 = Mat(3, 1, CV_64FC1, v1);  //列向量
	V1 = H * V2;
	corners.right_bottom.x = v1[0] / v1[2];
	corners.right_bottom.y = v1[1] / v1[2];

}

//优化两图的连接处，使得拼接自然
void OptimizeSeam(Mat& img1, Mat& trans, Mat& dst)
{
	int start = MIN(corners.left_top.x, corners.left_bottom.x);//开始位置，即重叠区域的左边界  

	double processWidth = img1.cols - start;//重叠区域的宽度  
	int rows = dst.rows;
	int cols = img1.cols; //注意，是列数*通道数
	double alpha = 1;//img1中像素的权重  
	for (int i = 0; i < rows; i++)
	{
		uchar* p = img1.ptr<uchar>(i);  //获取第i行的首地址
		uchar* t = trans.ptr<uchar>(i);
		uchar* d = dst.ptr<uchar>(i);
		for (int j = start; j < cols; j++)
		{
			//如果遇到图像trans中无像素的黑点，则完全拷贝img1中的数据
			if (t[j * 3] == 0 && t[j * 3 + 1] == 0 && t[j * 3 + 2] == 0)
			{
				alpha = 1;
			}
			else
			{
				//img1中像素的权重，与当前处理点距重叠区域左边界的距离成正比，实验证明，这种方法确实好  
				alpha = (processWidth - (j - start)) / processWidth;
			}

			d[j * 3] = p[j * 3] * alpha + t[j * 3] * (1 - alpha);
			d[j * 3 + 1] = p[j * 3 + 1] * alpha + t[j * 3 + 1] * (1 - alpha);
			d[j * 3 + 2] = p[j * 3 + 2] * alpha + t[j * 3 + 2] * (1 - alpha);

		}
	}

}

//计算原始图像点位在经过矩阵变换后在目标图像上对应位置
Point2f getTransformPoint(const Point2f originalPoint, const Mat &transformMaxtri)
{
	Mat originelP, targetP;
	originelP = (Mat_<double>(3, 1) << originalPoint.x, originalPoint.y, 1.0);
	targetP = transformMaxtri*originelP;
	float x = targetP.at<double>(0, 0) / targetP.at<double>(2, 0);
	float y = targetP.at<double>(1, 0) / targetP.at<double>(2, 0);
	return Point2f(x, y);
}




int _tmain(int argc, _TCHAR* argv[])
{
	VideoCapture cap1(0);//左
	VideoCapture cap2(1);//右

	double rate = 60;
	int delay = 1000 / rate;
	bool stop(false);
	Mat frame1;
	Mat frame2;
	Mat frame3;
	Mat frame4;
	int k = 100;
	int n = 50;
	Mat image01;
	Mat image02;
	Mat image03;
	Mat image04;
	/*Mat imageTransform1, imageTransform2;*/
	/*Mat homo;*/

	namedWindow("cam1", CV_WINDOW_AUTOSIZE);
	namedWindow("cam2", CV_WINDOW_AUTOSIZE);

	if (cap1.isOpened() && cap2.isOpened())
	{
		cout << "*** ***" << endl;
		cout << "摄像头已启动！" << endl;
	}
	else
	{
		cout << "*** ***" << endl;
		cout << "警告：请检查摄像头是否安装好!" << endl;
		cout << "程序结束！" << endl << "*** ***" << endl;
		return -1;
	}

	/*cap1.set(CV_CAP_PROP_FRAME_WIDTH, 640);
	cap1.set(CV_CAP_PROP_FRAME_HEIGHT, 480);
	cap2.set(CV_CAP_PROP_FRAME_WIDTH, 640);
	cap2.set(CV_CAP_PROP_FRAME_HEIGHT, 480);
	cap1.set(CV_CAP_PROP_FOCUS, 0);
	cap2.set(CV_CAP_PROP_FOCUS, 0);*/

	//获取两幅图像，通过这两幅图像来估计摄像机参数

	while (n--)
	{
		if (cap1.read(frame1) && cap2.read(frame2))
		{
			imshow("cam1", frame1);
			imshow("cam2", frame2);
			imwrite("frame1.bmp", frame1);
			imwrite("frame2.bmp", frame2);
		}
		if (waitKey(1) == 27)//按ESC键
		{
			stop = true;
			cout << "程序结束！" << endl;
			cout << "*** ***" << endl;
		}
	}

	image01 = imread("frame1.bmp", 1);    //左图
	image02 = imread("frame2.bmp", 1);    //右图

	//灰度图转换  
	Mat image1, image2;
	cvtColor(image01, image1, CV_RGB2GRAY);
	cvtColor(image02, image2, CV_RGB2GRAY);


	//提取特征点    
	SurfFeatureDetector Detector(1000);
	vector<KeyPoint> keyPoint1, keyPoint2;
	Detector.detect(image1, keyPoint1);
	Detector.detect(image2, keyPoint2);

	//特征点描述，为下边的特征点匹配做准备    
	SurfDescriptorExtractor Descriptor;
	Mat imageDesc1, imageDesc2;
	Descriptor.compute(image1, keyPoint1, imageDesc1);
	Descriptor.compute(image2, keyPoint2, imageDesc2);

	FlannBasedMatcher matcher;
	vector<DMatch> matchePoints;
	matcher.match(imageDesc1, imageDesc2, matchePoints, Mat());
	sort(matchePoints.begin(), matchePoints.end()); //特征点排序
	//获取排在前N个的最优匹配特征点
	vector<Point2f> imagePoints1, imagePoints2;
	for (int i = 0; i < 20; i++)
	{
		imagePoints1.push_back(keyPoint1[matchePoints[i].queryIdx].pt);
		imagePoints2.push_back(keyPoint2[matchePoints[i].trainIdx].pt);
	}

	//获取图像1到图像2的投影映射矩阵，尺寸为3*3

	Mat homo = findHomography(imagePoints1, imagePoints2, CV_RANSAC);
	cout << homo << endl;
	ofstream file;
	file.open("D:\\DataFile\\test.txt", ios::app);
	file << homo << endl;
	file.close();
	Mat adjustMat = (Mat_<double>(3, 3) << 1.0, 0, image01.cols, 0, 1.0, 0, 0, 0, 1.0);
	Mat adjustHomo = adjustMat*homo;

	//获取最强配对点在原始图像和矩阵变换后图像上的对应位置，用于图像拼接点的定位
	Point2f originalLinkPoint, targetLinkPoint, basedImagePoint;
	originalLinkPoint = keyPoint1[matchePoints[0].queryIdx].pt;
	targetLinkPoint = getTransformPoint(originalLinkPoint, adjustHomo);
	basedImagePoint = keyPoint2[matchePoints[0].trainIdx].pt;

	//计算配准图的四个顶点坐标
	CalcCorners(adjustHomo, image01);
	cout << "left_top:" << corners.left_top << endl;
	cout << "left_bottom:" << corners.left_bottom << endl;
	cout << "right_top:" << corners.right_top << endl;
	cout << "right_bottom:" << corners.right_bottom << endl;

	//int left_top_x = corners.left_top.x;
	//int left_top_y = corners.left_top.y;
	//int left_bottom_x = corners.left_bottom.x;
	//int left_bottom_y = corners.left_bottom.y;
	//int right_top_x = corners.right_top.x;
	//int right_top_y = corners.right_top.y;
	//int right_bottom_x = corners.right_bottom.x;
	//int right_bottom_y = corners.right_bottom.y;

	分配拼接后图的大小
	//int dst_width = right_bottom_x - left_top_x;
	//int dst_height = left_bottom_y - left_top_y;

	//Mat dst(dst_height, dst_width, CV_8UC3);
	//dst.setTo(0);

	//图像配准
	Mat imageTransform1;

	while (!stop)
	{
		double t1 = getTickCount();

		if (cap1.read(frame3) && cap2.read(frame4))
		{
			imshow("cam1", frame3);
			imshow("cam2", frame4);

			image03 = frame3.clone();
			image04 = frame4.clone();
			
			warpPerspective(image03, imageTransform1, adjustMat*homo, Size(image04.cols + image03.cols +110, image04.rows));
			/*warpPerspective(image03, imageTransform1, adjustMat*homo, Size(MAX(corners.right_top.x, corners.right_bottom.x), image03.rows));*/


			//在最强匹配点左侧的重叠区域进行累加，是衔接稳定过渡，消除突变
			Mat image1Overlap, image2Overlap; //图1和图2的重叠部分
			image1Overlap = imageTransform1(Rect(Point(targetLinkPoint.x - basedImagePoint.x, 0), Point(targetLinkPoint.x, image04.rows)));
			image2Overlap = image04(Rect(0, 0, image1Overlap.cols, image1Overlap.rows));
			Mat image1ROICopy = image1Overlap.clone();  //复制一份图1的重叠部分
			for (int i = 0; i < image1Overlap.rows; i++)
			{
				for (int j = 0; j < image1Overlap.cols; j++)
				{
					double weight;
					weight = (double)j / image1Overlap.cols;//随距离改变而改变的叠加系数
					image1Overlap.at<Vec3b>(i, j)[0] = (1 - weight)*image1ROICopy.at<Vec3b>(i, j)[0] + weight*image2Overlap.at<Vec3b>(i, j)[0];
					image1Overlap.at<Vec3b>(i, j)[1] = (1 - weight)*image1ROICopy.at<Vec3b>(i, j)[1] + weight*image2Overlap.at<Vec3b>(i, j)[1];
					image1Overlap.at<Vec3b>(i, j)[2] = (1 - weight)*image1ROICopy.at<Vec3b>(i, j)[2] + weight*image2Overlap.at<Vec3b>(i, j)[2];
				}
			}
			Mat ROIMat = image04(Rect(Point(image1Overlap.cols, 0), Point(image04.cols, image04.rows)));	 //图2中不重合的部分
			ROIMat.copyTo(Mat(imageTransform1, Rect(targetLinkPoint.x, 0, ROIMat.cols, image04.rows))); //不重合的部分直接衔接上去

			Mat finally = imageTransform1(Rect(Point(MAX(corners.left_top.x, corners.left_top.y), 0), Point(1280, 480)));


			namedWindow("dst", WINDOW_AUTOSIZE);
			imshow("dst", finally);

			
			if (waitKey(1) == 27)//按ESC键
			{
				stop = true;
				cout << "程序结束！" << endl;
				cout << "*** ***" << endl;
			}
		}
		cout << "拼接时间: " << ((getTickCount() - t1) / getTickFrequency()) << " sec" << endl;
	}

	return 0;
}