双目相机空间坐标重建.拼接融合（五）

双目相机空间坐标重建.拼接融合（五）

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

#include "stdafx.h"

#pragma warning(disable : 4996)

#include "stdafx.h"
#include <iostream>
#include <vector>
#include <opencv2/opencv.hpp>
#include <opencv2/xfeatures2d.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <string.h>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>

using namespace cv;
using namespace std;
using namespace cv::xfeatures2d;


#define	PI	(3.1415926)

#if 0


cv::Mat g_Im = cv::Mat::zeros(300, 600, CV_8UC1);

#define key_opration "key opration"

#define lenthR "lenthR"

#define AngleV "AngleV"
#define AngleH "AngleH"

#define PictureW "PictureW"
#define PictureH  "PictureH"

int g_R  =2;//2000mm
const int g_RMin=0;
const int g_RMax =60;//60m
const int g_Rcount =60;

int g_angle =90;//angle
const int g_angleMin =0;
const int g_angleMax =120;
const int g_anglecount =120;

int g_Picturepix  =640;//pix
const int g_PicturepixMin =0;
const int g_PicturepixMax =640;
const int g_Picturepixcount =640;

int oldvalueR = 0			,newvalueR = 0;
int oldvalueAngleV = 0		,newvalueAngleV = 0;
int oldvalueAngleH = 0		,newvalueAngleH = 0;
int oldvaluePictureW = 0	,newvaluePictureW = 0;
int oldvaluePictureH = 0	,newvaluePictureH = 0;

void onChange(int,void*)
{
   
	//show picture
	imshow(key_opration,g_Im);	
}

void Creat_win_bar()
{
   
	//create window 
   	namedWindow(key_opration,WINDOW_NORMAL);

	//createTrackbar R
	createTrackbar(lenthR,key_opration,&g_R,g_Rcount,onChange);
	setTrackbarMin(lenthR,key_opration,g_RMin);
	setTrackbarMax(lenthR,key_opration,g_RMax);
	setTrackbarPos(lenthR,key_opration,g_R);
	//call onChange()
	onChange(0,0);
	oldvalueR = getTrackbarPos(lenthR,key_opration);
	
	//createTrackbar anglev
	createTrackbar(AngleV,key_opration,&g_angle,g_anglecount,onChange);
	setTrackbarMin(AngleV,key_opration,g_angleMin);
	setTrackbarMax(AngleV,key_opration,g_angleMax);
	setTrackbarPos(AngleV,key_opration,g_angle);
	//call onChange()
	onChange(0,0);
	oldvalueAngleV = getTrackbarPos(AngleV,key_opration);
	
	//createTrackbar angleh
	createTrackbar(AngleH,key_opration,&g_angle,g_anglecount,onChange);
	setTrackbarMin(AngleH,key_opration,g_angleMin);
	setTrackbarMax(AngleH,key_opration,g_angleMax);
	setTrackbarPos(AngleH,key_opration,g_angle);
	//call onChange()
	onChange(0,0);	
	oldvalueAngleH = getTrackbarPos(AngleH,key_opration);

	//createTrackbar PictureW
	createTrackbar(PictureW,key_opration,&g_Picturepix,g_Picturepixcount,onChange);
	setTrackbarMin(PictureW,key_opration,g_PicturepixMin);
	setTrackbarMax(PictureW,key_opration,g_PicturepixMax);
	setTrackbarPos(PictureW,key_opration,g_Picturepix);
	//call onChange()
	onChange(0,0);
	oldvaluePictureW = getTrackbarPos(PictureW,key_opration);
	
	//createTrackbar PictureH
	createTrackbar(PictureH,key_opration,&g_Picturepix,g_Picturepixcount,onChange);
	setTrackbarMin(PictureH,key_opration,g_PicturepixMin);
	setTrackbarMax(PictureH,key_opration,g_PicturepixMax);
	setTrackbarPos(PictureH,key_opration,g_Picturepix);
	//call onChange()
	onChange(0,0);	
	oldvaluePictureH = getTrackbarPos(PictureH,key_opration);	
}



int main(int argc, char **argv)
//int A()
{
   
	//
	double viewAngleDegV = 0;
	double viewAngleDegH = 0;
	double arfa   = 0;
	double berta = 0;

	//
	double r = 0;  //mm
	int w = 0, h = 0;
	//
	double fx1 = 534.10766364;//mm
	double fy1 = 534.01052742;//mm
	double cx1 = 341.14525437;//mm
	double cy1 = 234.85237461;//mm	
	//
	cv::Mat Im0 = cv::imread("E:\\zcb_work\\2113\\pic\\ch1\\left01.jpg", 0);

	if (Im0.empty())
		return -1;

	imshow("image0", Im0);
	
	printf("++++ open image succesful +++\r\n");

	Creat_win_bar();

	while(1)
	{
   	
		newvalueR 			= getTrackbarPos(lenthR,key_opration);	
		newvalueAngleV 		= getTrackbarPos(AngleV,key_opration);
		newvalueAngleH 		= getTrackbarPos(AngleH,key_opration);
		
		newvaluePictureW 	= getTrackbarPos(PictureW,key_opration);
		newvaluePictureH 	= getTrackbarPos(PictureH,key_opration);

		if((oldvalueR != newvalueR) \
		||(oldvalueAngleV != newvalueAngleV) \
		||(oldvalueAngleH != newvalueAngleH) \
		||(oldvaluePictureW != newvaluePictureW) \
		||(oldvaluePictureH != newvaluePictureH)
		)
		{
   
			//
			 oldvalueR = newvalueR ;
			 oldvalueAngleV = newvalueAngleV ;
			 oldvalueAngleH =newvalueAngleH;
			 oldvaluePictureW = newvaluePictureW;
			 oldvaluePictureH = newvaluePictureH;	 
			//
			viewAngleDegV = newvalueAngleV;   //  94deg
			viewAngleDegH = newvalueAngleH;   //80deg
			arfa   = (viewAngleDegV /2) *PI/180;  //c++ deg
			berta = (viewAngleDegH /2) *PI/180;  //c++ deg
			//	
			r = newvalueR *1000;   //  default 2000mm		
			//
			w = 640;   //640pix
			h = 480;   //480pix
	
			if((w % 2) != 0)
				w = w -1;
			if((h % 2) != 0)
				h = h -1;
	
			cv::Mat Im2 = cv::Mat::zeros(h, w, CV_8UC1);	
			cv::Mat Im3 = cv::Mat::zeros(h, w, CV_8UC1);	
			//
			double *Wxyz = (double *)malloc(sizeof(double)* w * h * 3);
			double *Cxyz1 = (double *)malloc(sizeof(double)* w * h * 3);
			double *Pxy1 = (double *)malloc(sizeof(double)* w * h * 2);
			//	
			double Wxyz0[3], Wxyz1[3], Wxyz2[3], Wxyz3[3], Wxyz4[3];
			Wxyz0[0] = 0;
			Wxyz0[1] = 0;
			Wxyz0[2] =  r;

			Wxyz1[0] = -r*tan(arfa);//mm
			Wxyz1[1] = -r*tan(berta);//mm
			Wxyz1[2] =  r;//mm

			Wxyz2[0] = -r*tan(arfa);//mm
			Wxyz2[1] = r*tan(berta);//mm
			Wxyz2[2] =  r;//mm
			
			Wxyz3[0] = r*tan(arfa);//mm
			Wxyz3[1] = -r*tan(berta);//mm
			Wxyz3[2] =  r;//mm
			
			Wxyz4[0] = r*tan(arfa);//mm
			Wxyz4[1] = r*tan(berta);//mm
			Wxyz4[2] =  r;//mm//mm

			//
			double dertaX = abs(Wxyz3[0] - Wxyz1[0]) / (w -1);
			double dertaY = abs(Wxyz2[1] - Wxyz1[1]) / (h - 1);
			double tempx[1200] = {
    0 };
			double tempy[1200] = {
    0 };

			//one row world position
			{
   
					for (int i = 0; i < w /2;i++)
					{
   
						tempx[i] = Wxyz1[0] + i * dertaX;
						//printf("+ %d x:%f\r\n",i, tempx[i]);
					}

					for (int i = w /2; i < w;i++)
					{
   
						tempx[i] = Wxyz3[0] - (w-1 -i) * dertaX;
						//printf("- %d x:%f\r\n",i, tempx[i]);
					}
			}
			//one col world position	
			{
   
					for (int i = 0; i < h /2 ;i++)
					{
   
						tempy[i] = Wxyz1[1] + i * dertaY;
						//printf("+ %d y:%f\r\n",i, tempy[i]);
					}

					for (int i = h /2; i < h;i++)
					{
   
						tempy[i] = Wxyz2[1] - (h-1 -i) * dertaY;
						//printf("- %d y:%f\r\n",i, tempy[i]);
					}
			}

			//all world position
			{
   
				for(int i = 0; i < h;i++)
				{
   
					for(int j = 0; j < w ;j++)
					{
   
						//get Wxyz
						{
   
							Wxyz[3*(i * w + j) + 0] = tempx[j];
							Wxyz[3*(i * w + j) + 1] = tempy[i];
							Wxyz[3*(i * w + j) + 2] = r;
						}
						
						//gloable Wxyz
						double temp = 0;
						{
   
							temp  = pow(Wxyz[3*(i * w + j) + 0], 2);
							temp += pow(Wxyz[3*(i * w + j) + 1], 2);
							temp += pow(Wxyz[3*(i * w + j) + 2], 2);
							temp = sqrt(temp);				
							//cout << "r:    " << r<< "    R:    " << temp<< endl;
							//cout << "Wxyz[3*(i * w + j) + 0]:    " << Wxyz[3*(i * w + j) + 0]<< "    Wxyz[3*(i * w + j) + 1]:    " << Wxyz[3*(i * w + j) + 1]<< "    Wxyz[3*(i * w + j) + 2]:    " << Wxyz[3*(i * w + j) + 2]<< endl;
							
							temp = Wxyz[3*(i * w + j) + 2]/temp;
							//cout << "r:    " << r<< "    r/R:    " << temp<< endl;
							
							Wxyz[3*(i * w + j) + 0] = Wxyz[3*(i * w + j) + 0] * temp;
							Wxyz[3*(i * w + j) + 1] = Wxyz[3*(i * w + j) + 1] * temp;
							Wxyz[3*(i * w + j) + 2] = Wxyz[3*(i * w + j) + 2] * temp;
							
							//验证半径是不是等于r，
							
							//temp   = pow(Wxyz[3*(i * w + j) + 0], 2);
							//temp += pow(Wxyz[3*(i * w + j) + 1], 2);
							//temp += pow(Wxyz[3*(i * w + j) + 2], 2);
							//temp = sqrt(temp);
							//cout << "r:    " << r<< "     R':    " << temp<< endl;
							//和原来平面上的坐标对比
							//cout << "Wxyz[3*(i * w + j) + 0]:    " << Wxyz[3*(i * w + j) + 0]<< "    Wxyz[3*(i * w + j) + 1]:    " << Wxyz[3*(i * w + j) + 1]<< "    Wxyz[3*(i * w + j) + 2]:    " 		  << Wxyz[3*(i * w + j) + 2]<< endl;
							
						}
					}
				}
			}

			//计算像素坐标
			{
   
				int i = 0;
				int j = 0;
				printf("pic2 camera position\r\n");	
				for( i = 0; i < h;i++)
				{
   			
					for( j = 0; j < w ;j++)
					{
   		
						Cxyz1[3*(i * w + j) + 0] = Wxyz[3*(i * w + j) + 0];
						Cxyz1[3*(i * w + j) + 1] = Wxyz[3*(i * w + j) + 1];
						Cxyz1[3*(i * w + j) + 2] = Wxyz[3*(i * w + j) + 2];

						Pxy1[2*(i * w + j) + 0] = (fx1 * Cxyz1[3*(i * w + j) + 0])/ Cxyz1[3*(i * w + j) + 2] +cx1;
						Pxy1[2*(i * w + j) + 1] = (fy1 * Cxyz1[3*(i * w + j) + 1])/ Cxyz1[3*(i * w + j) + 2] +cy1;

						if((0 < Pxy1[2*(i * w + j) + 0]  &&  Pxy1[2*(i * w + j) + 0]< Im0.cols -1) && (0 < Pxy1[2*(i * w + j) + 1]  &&  Pxy1[2*(i * w + j) + 1]< Im0.rows -1))
						{
   
							Im2.at<uchar>(i, j)=Im0.at<uchar>(round(Pxy1[2*(i * w + j) + 1]),round(Pxy1[2*(i * w + j) + 0]));
							Im3.at<uchar>(round(Pxy1[2*(i * w + j) + 1]),round(Pxy1[2*(i * w + j) + 0])) = Im0.at<uchar>(round(Pxy1[2 * (i * w + j) + 1]), round(Pxy1[2 * (i * w + j) + 0]));

							//printf(" px1:%d,py1:%d", int(Pxy1[i * w + j + 0]), int(Pxy1[i * w + j + 1]));
						}
					}
				}
			}

			

			//显示图片
		    imshow("image2", Im2);
		    imshow("image3", Im3);
		}
		cv::waitKey(0);	
	}
	return 0;
}


#else
cv::Mat g_Im = cv::Mat::zeros(500, 800, CV_8UC1);

#define key_opration "key opration"

#define lenthR "lenthR"

#define AngleV "AngleV"
#define AngleH "AngleH"

#define PictureW "PictureW"
#define PictureH  "PictureH"

#define Rotatex "Rotatex"
#define Rotatey "Rotatey"
#define Rotatez "Rotatez"

int g_R  =2;//2000mm
const int g_RMin=0;
const int g_RMax =60;//60m
const int g_Rcount =60;

int g_angle =90;//angle
const int g_angleMin =0;
const int g_angleMax =180;
const int g_anglecount =180;

int g_Picturepix  =480;//pix
const int g_PicturepixMin =100;
const int g_PicturepixMax =2000;
const int g_Picturepixcount =1900;

int g_Rotation  =0;
const int g_RotationMin =0;
const int g_RotationMax =360;//-45  /45
const int g_Rotationcount =360;

int oldvalueR = 0			,newvalueR = 0;
int oldvalueAngleV = 0		,newvalueAngleV = 0;
int oldvalueAngleH = 0		,newvalueAngleH = 0;
int oldvaluePictureW = 0	,newvaluePictureW = 0;
int oldvaluePictureH = 0	,newvaluePictureH = 0;
int oldvalueRotatex = 0	,newvalueRotatex= 0;
int oldvalueRotatey = 0	,newvalueRotatey = 0;
int oldvalueRotatez = 0	,newvalueRotatez = 0;

void onChange(int,void*)
{
   
	//show picture
	imshow(key_opration,g_Im);	
	cout<<"x"<<getTrackbarPos(Rotatex,key_opration)<<endl;
	cout<<"y"<<getTrackbarPos(Rotatey,key_opration)<<endl;
	cout<<"z"<<getTrackbarPos(Rotatez,key_opration)<<endl;
}

void Creat_win_bar()
{
   
	//create window 
   	namedWindow(key_opration,WINDOW_NORMAL);

	//createTrackbar R
	createTrackbar(lenthR,key_opration,&g_R,g_Rcount,onChange);
	setTrackbarMin(lenthR,key_opration,g_RMin);
	setTrackbarMax(lenthR,key_opration,g_RMax);
	setTrackbarPos(lenthR,key_opration,g_R);
	//call onChange()
	onChange(0,0);
	oldvalueR = getTrackbarPos(lenthR,key_opration);
	
	//createTrackbar anglev
	createTrackbar(AngleV,key_opration,&g_angle,g_anglecount,onChange);
	setTrackbarMin(AngleV,key_opration,g_angleMin);
	setTrackbarMax(AngleV,key_opration,g_angleMax);
	setTrackbarPos(AngleV,key_opration,g_angle