因為前一陣子忙于自己的畢設，是以就沒有及時更新日志，今天正好沒其他事兒，是以，我就把圖像拼接程式寫上來了。。。歡迎大家的閱讀以及批評和指正。
下面的程式是基于opencv2.3.1+vs2010的搭建的環境下程式設計的。。。
首先對兩個usb通用攝像頭進行了标定。然後進行圖像拼接，最後進行測距。
這不是最終版，因為最終版是我的論文内容。
是以，要過一陣子才能寫上來，因為現在寫上來我的論文可能被網際網路的查重率坑爹的。。。
呵呵，請大家見諒。。。不過，很多重點代碼都是完好無損的。。。。
#include <math.h>
#include <ctime>
#include <cv.h>
#include <highgui.h>
#include <features2d/features2d.hpp>
#include <cvaux.h>
#include <string>
#include <iostream>
#include <fstream>
using namespace std;
using namespace cv;
void main()
{
ofstream fout("Result.txt");
float f_left[2],f_right[2];
IplImage * show; //RePlay圖像指針
cvNamedWindow("RePlay",1);
int number_image_copy=7; //複制圖像幀數
CvSize board_size=cvSize(9,6); //标定闆角點數
CvSize2D32f square_size=cvSize2D32f(10,10); //假設我的每個标定方格長寬都是1.82厘米
float square_length=square_size.width; //方格長度
float square_height=square_size.height; //方格高度
int board_width=board_size.width; //每行角點數
int board_height=board_size.height; //每列角點數
int total_per_image=board_width*board_height; //每張圖檔角點總數
int count; //存儲每幀圖像中實際識别的角點數
int found; //識别标定闆角點的标志位
int step; //存儲步長，step=successes*total_per_image;
int successes=0; //存儲成功找到标定闆上所有角點的圖像幀數
int a=1; //臨時變量，表示在操作第a幀圖像
const int NImages = 7;//圖檔總數
CvMat *rotation_vectors;
CvMat *translation_vectors;
CvPoint2D32f * image_points_buf = new CvPoint2D32f[total_per_image]; //存儲角點圖像坐标的數組
CvMat * image_points=cvCreateMat(NImages*total_per_image,2,CV_32FC1);//存儲角點的圖像坐标的矩陣
CvMat * object_points=cvCreateMat(NImages*total_per_image,3,CV_32FC1);//存儲角點的三維坐标的矩陣
CvMat * point_counts=cvCreateMat(NImages,1,CV_32SC1);//存儲每幀圖像的識别的角點數
CvMat * intrinsic_matrix=cvCreateMat(3,3,CV_32FC1);//内參數矩陣
CvMat * distortion_coeffs=cvCreateMat(5,1,CV_32FC1);//畸變系數
rotation_vectors = cvCreateMat(NImages,3,CV_32FC1);//旋轉矩陣
translation_vectors = cvCreateMat(NImages,3,CV_32FC1);//平移矩陣
ifstream fin("calibdata1.txt");
while(a<=number_image_copy)
{
//sprintf_s (filename,"%d.jpg",a);
string filename;
getline(fin,filename);
show=cvLoadImage(filename.c_str(),1);
//尋找棋盤圖的内角點位置
found=cvFindChessboardCorners(show,board_size,image_points_buf,&count,
CV_CALIB_CB_ADAPTIVE_THRESH|CV_CALIB_CB_FILTER_QUADS);
if (found==0)
{ //如果沒找到标定闆角點時
cout<<"第"<<a<<"幀圖檔無法找到棋盤格所有角點!\n\n";
fout<<"第"<<a<<"幀圖檔無法找到棋盤格所有角點!\n\n";
cvNamedWindow("RePlay",1);
cvShowImage("RePlay",show);
cvWaitKey(2000);
}
else
{ //找到标定闆角點時
cout<<"第"<<a<<"幀圖像成功獲得"<<count<<"個角點...\n";
fout<<"第"<<a<<"幀圖像成功獲得"<<count<<"個角點...\n";
cvNamedWindow("RePlay",1);
IplImage * gray_image= cvCreateImage(cvGetSize(show),8,1);
cvCvtColor(show,gray_image,CV_BGR2GRAY);
cout<<"擷取源圖像灰階圖過程完成...\n";
fout<<"擷取源圖像灰階圖過程完成...\n";
cvFindCornerSubPix(gray_image,image_points_buf,count,cvSize(11,11),cvSize(-1,-1),
cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER,30,0.1));
cout<<"灰階圖亞像素化過程完成...\n";
fout<<"灰階圖亞像素化過程完成...\n";
cvDrawChessboardCorners(show,board_size,image_points_buf,count,found);
cout<<"在源圖像上繪制角點過程完成...\n\n";
fout<<"在源圖像上繪制角點過程完成...\n\n";
cvShowImage("RePlay",show);
cvWaitKey(500);
}
if(total_per_image==count)
{
step=successes*total_per_image; //計算存儲相應坐标資料的步長
for(int i=step,j=0;j<total_per_image;++i,++j)
{
CV_MAT_ELEM(*image_points, float,i,0)=image_points_buf[j].x;
CV_MAT_ELEM(*image_points, float,i,1)=image_points_buf[j].y;
CV_MAT_ELEM(*object_points,float,i,0)=(float)((j/board_width)*square_length);
CV_MAT_ELEM(*object_points,float,i,1)=(float)((j%board_width)*square_height);
CV_MAT_ELEM(*object_points,float,i,2)=0.0f;
}
CV_MAT_ELEM(*point_counts,int,successes,0)=total_per_image;
successes++;
}
cout<<"hahahahh======"<<a<<endl;
fout<<"hahahahh======"<<a<<endl;
a++;
}
cvReleaseImage(&show);
cvDestroyWindow("RePlay");
cout<<"*********************************************\n";
fout<<"*********************************************\n";
cout<<NImages<<"幀圖檔中，标定成功的圖檔為"<<successes<<"幀...\n";
fout<<NImages<<"幀圖檔中，标定成功的圖檔為"<<successes<<"幀...\n";
cout<<NImages<<"幀圖檔中，标定失敗的圖檔為"<<NImages-successes<<"幀...\n\n";
fout<<NImages<<"幀圖檔中，标定失敗的圖檔為"<<NImages-successes<<"幀...\n\n";
cout<<"*********************************************\n\n";
fout<<"*********************************************\n\n";
cout<<"按任意鍵開始計算錄影機内參數...\n\n";
fout<<"按任意鍵開始計算錄影機内參數...\n\n";
IplImage * show_colie;
show_colie = cvLoadImage("F:\\graduatelunwen\\opencvprojects\\june\\opecv_ransac_sift_cameraCalibration\\dinggao_wanchengban_sift_ransac\\left_43.jpg",1);
CvMat * object_points2 = cvCreateMat(successes*total_per_image,3,CV_32FC1);
CvMat * image_points2 = cvCreateMat(successes*total_per_image,2,CV_32FC1);
CvMat * point_counts2 = cvCreateMat(successes,1,CV_32SC1);
for(int i=0;i<successes*total_per_image;++i)
{
CV_MAT_ELEM(*image_points2, float,i,0)=CV_MAT_ELEM(*image_points, float,i,0);
CV_MAT_ELEM(*image_points2, float,i,1)=CV_MAT_ELEM(*image_points, float,i,1);
CV_MAT_ELEM(*object_points2,float,i,0)=CV_MAT_ELEM(*object_points,float,i,0);
CV_MAT_ELEM(*object_points2,float,i,1)=CV_MAT_ELEM(*object_points,float,i,1);
CV_MAT_ELEM(*object_points2,float,i,2)=CV_MAT_ELEM(*object_points,float,i,2);
}
for(int i=0;i<successes;++i)
{
CV_MAT_ELEM(*point_counts2,int,i,0) = CV_MAT_ELEM(*point_counts,int,i,0);
}
cvReleaseMat(&object_points);
cvReleaseMat(&image_points);
cvReleaseMat(&point_counts);
CV_MAT_ELEM(*intrinsic_matrix,float,0,0)=1.0f;
CV_MAT_ELEM(*intrinsic_matrix,float,1,1)=1.0f;
cvCalibrateCamera2(object_points2,image_points2,point_counts2,cvGetSize(show_colie),
intrinsic_matrix,distortion_coeffs,rotation_vectors,translation_vectors,0);
cout<<"錄影機内參數矩陣為：\n";
fout<<"錄影機内參數矩陣為：\n";
cout<<CV_MAT_ELEM(*intrinsic_matrix,float,0,0)<<" "<<CV_MAT_ELEM(*intrinsic_matrix,float,0,1)
<<" "<<CV_MAT_ELEM(*intrinsic_matrix,float,0,2)
<<"\n\n";
cout<<CV_MAT_ELEM(*intrinsic_matrix,float,1,0)<<" "<<CV_MAT_ELEM(*intrinsic_matrix,float,1,1)
<<" "<<CV_MAT_ELEM(*intrinsic_matrix,float,1,2)
<<"\n\n";
cout<<CV_MAT_ELEM(*intrinsic_matrix,float,2,0)<<" "<<CV_MAT_ELEM(*intrinsic_matrix,float,2,1)
<<" "<<CV_MAT_ELEM(*intrinsic_matrix,float,2,2)
<<"\n\n";
fout<<CV_MAT_ELEM(*intrinsic_matrix,float,0,0)<<" "<<CV_MAT_ELEM(*intrinsic_matrix,float,0,1)
<<" "<<CV_MAT_ELEM(*intrinsic_matrix,float,0,2)
<<"\n\n";
fout<<CV_MAT_ELEM(*intrinsic_matrix,float,1,0)<<" "<<CV_MAT_ELEM(*intrinsic_matrix,float,1,1)
<<" "<<CV_MAT_ELEM(*intrinsic_matrix,float,1,2)
<<"\n\n";
fout<<CV_MAT_ELEM(*intrinsic_matrix,float,2,0)<<" "<<CV_MAT_ELEM(*intrinsic_matrix,float,2,1)
<<" "<<CV_MAT_ELEM(*intrinsic_matrix,float,2,2)
<<"\n\n";
f_left[0]=CV_MAT_ELEM(*intrinsic_matrix,float,0,0);
f_left[1]=CV_MAT_ELEM(*intrinsic_matrix,float,1,1);
cout<<"畸變系數矩陣為：\n";
cout<<CV_MAT_ELEM(*distortion_coeffs,float,0,0)<<" "<<CV_MAT_ELEM(*distortion_coeffs,float,1,0)
<<" "<<CV_MAT_ELEM(*distortion_coeffs,float,2,0)
<<" "<<CV_MAT_ELEM(*distortion_coeffs,float,3,0)
<<" "<<CV_MAT_ELEM(*distortion_coeffs,float,4,0)
<<"\n\n";
fout<<"畸變系數矩陣為：\n";
fout<<CV_MAT_ELEM(*distortion_coeffs,float,0,0)<<" "<<CV_MAT_ELEM(*distortion_coeffs,float,1,0)
<<" "<<CV_MAT_ELEM(*distortion_coeffs,float,2,0)
<<" "<<CV_MAT_ELEM(*distortion_coeffs,float,3,0)
<<" "<<CV_MAT_ELEM(*distortion_coeffs,float,4,0)
<<"\n\n";
cvSave("Intrinsics.xml",intrinsic_matrix);
cvSave("Distortion.xml",distortion_coeffs);
cout<<"錄影機矩陣、畸變系數向量已經分别存儲在名為Intrinsics.xml、Distortion.xml文檔中\n\n";
fout<<"錄影機矩陣、畸變系數向量已經分别存儲在名為Intrinsics.xml、Distortion.xml文檔中\n\n";
for(int ii = 0; ii < NImages; ++ii)
{ float tranv[3] = {0.0};
float rotv[3] = {0.0};
for ( int i = 0; i < 3; i++)
{
tranv[i] = ((float*)(translation_vectors->data.ptr+ii*translation_vectors->step))[i];
rotv[i] = ((float*)(rotation_vectors->data.ptr+rotation_vectors->step))[i];
}
cout << "第" << ii+1 << "幅圖的外參數" << endl;
fout << "第" << ii+1 << "幅圖的外參數" << endl;
printf("ROTATION VECTOR 旋轉向量 : \n");
printf("[ %6.4f %6.4f %6.4f ] \n", rotv[0], rotv[1], rotv[2]);
printf("TRANSLATION VECTOR 平移向量: \n");
printf("[ %6.4f %6.4f %6.4f ] \n", tranv[0], tranv[1], tranv[2]);
printf("-----------------------------------------\n");
fout<<"ROTATION VECTOR 旋轉向量 : \n"<<endl;
fout<< rotv[0]<<" "<< rotv[1]<<" "<< rotv[2]<<endl;
fout<<"TRANSLATION VECTOR 平移向量: \n"<<endl;
fout<< tranv[0]<<" "<< tranv[1]<< " "<<tranv[2]<<endl;
fout<<"-----------------------------------------\n"<<endl;
}
CvMat * intrinsic=(CvMat *)cvLoad("Intrinsics.xml");
CvMat * distortion=(CvMat *)cvLoad("Distortion.xml");
IplImage * mapx=cvCreateImage(cvGetSize(show_colie),IPL_DEPTH_32F,1);
IplImage * mapy=cvCreateImage(cvGetSize(show_colie),IPL_DEPTH_32F,1);
cvInitUndistortMap(intrinsic,distortion,mapx,mapy);
cvNamedWindow("原始圖像",1);
cvNamedWindow("非畸變圖像",1);
// cout<<"按‘E’鍵退出顯示...\n\n";
IplImage * clone=cvCloneImage(show_colie);
cvShowImage("原始圖像",show_colie);
cvRemap(clone,show_colie,mapx,mapy);
cvReleaseImage(&clone);
cvShowImage("非畸變圖像",show_colie);
cvWaitKey(500);
/
/// //
// 右錄影機标定 //
/
IplImage * showR; //RePlay圖像指針
cvNamedWindow("RePlayR",1);
int number_image_copyR=7; //複制圖像幀數
CvSize board_sizeR=cvSize(9,6); //标定闆角點數
CvSize2D32f square_sizeR=cvSize2D32f(10,10); //假設我的每個标定方格長寬都是1.82厘米
float square_lengthR=square_sizeR.width; //方格長度
float square_heightR=square_sizeR.height; //方格高度
int board_widthR=board_sizeR.width; //每行角點數
int board_heightR=board_sizeR.height; //每列角點數
int total_per_imageR=board_widthR*board_heightR; //每張圖檔角點總數
int countR; //存儲每幀圖像中實際識别的角點數
int foundR; //識别标定闆角點的标志位
int stepR; //存儲步長，stepR=successesR*total_per_imageR;
int successesR=0; //存儲成功找到标定闆上所有角點的圖像幀數
int aR=1; //臨時變量，表示在操作第a幀圖像
const int NImagesR = 7;//圖檔總數
CvMat *rotation_vectorsR;
CvMat *translation_vectorsR;
CvPoint2D32f * image_pointsR_bufR = new CvPoint2D32f[total_per_imageR]; //存儲角點圖像坐标的數組
CvMat * image_pointsR=cvCreateMat(NImagesR*total_per_imageR,2,CV_32FC1);//存儲角點的圖像坐标的矩陣
CvMat * object_pointsR=cvCreateMat(NImagesR*total_per_imageR,3,CV_32FC1);//存儲角點的三維坐标的矩陣
CvMat * point_countRsR=cvCreateMat(NImagesR,1,CV_32SC1);//存儲每幀圖像的識别的角點數
CvMat * intrinsicR_matrixR=cvCreateMat(3,3,CV_32FC1);//内參數矩陣
CvMat * distortionR_coeffsR=cvCreateMat(5,1,CV_32FC1);//畸變系數
rotation_vectorsR = cvCreateMat(NImagesR,3,CV_32FC1);//旋轉矩陣
translation_vectorsR = cvCreateMat(NImagesR,3,CV_32FC1);//平移矩陣
ifstream finR("calibdata2.txt");
while(aR<=number_image_copyR)
{
//sprintf_s (filename,"%d.jpg",a);
string filenameR;
getline(finR,filenameR);
showR=cvLoadImage(filenameR.c_str(),1);
//尋找棋盤圖的内角點位置
foundR=cvFindChessboardCorners(showR,board_sizeR,image_pointsR_bufR,&countR,
CV_CALIB_CB_ADAPTIVE_THRESH|CV_CALIB_CB_FILTER_QUADS);
if (foundR==0)
{ //如果沒找到标定闆角點時
cout<<"第"<<aR<<"幀圖檔無法找到棋盤格所有角點!\n\n";
fout<<"第"<<aR<<"幀圖檔無法找到棋盤格所有角點!\n\n";
cvNamedWindow("RePlayR",1);
cvShowImage("RePlayR",showR);
cvWaitKey(500);
}
else
{ //找到标定闆角點時
cout<<"第"<<aR<<"幀圖像成功獲得"<<countR<<"個角點...\n";
fout<<"第"<<aR<<"幀圖像成功獲得"<<countR<<"個角點...\n";
cvNamedWindow("RePlayR",1);
IplImage * gray_imageR= cvCreateImage(cvGetSize(showR),8,1);
cvCvtColor(showR,gray_imageR,CV_BGR2GRAY);
cout<<"擷取源圖像灰階圖過程完成...\n";
fout<<"擷取源圖像灰階圖過程完成...\n";
cvFindCornerSubPix(gray_imageR,image_pointsR_bufR,countR,cvSize(11,11),cvSize(-1,-1),
cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER,30,0.1));
cout<<"灰階圖亞像素化過程完成...\n";
fout<<"灰階圖亞像素化過程完成...\n";
cvDrawChessboardCorners(showR,board_sizeR,image_pointsR_bufR,countR,foundR);
cout<<"在源圖像上繪制角點過程完成...\n\n";
fout<<"在源圖像上繪制角點過程完成...\n\n";
cvShowImage("RePlayR",showR);
cvWaitKey(500);
}
if(total_per_imageR==countR)
{
stepR=successesR*total_per_imageR; //計算存儲相應坐标資料的步長
for(int i=stepR,j=0;j<total_per_imageR;++i,++j)
{
CV_MAT_ELEM(*image_pointsR, float,i,0)=image_pointsR_bufR[j].x;
CV_MAT_ELEM(*image_pointsR, float,i,1)=image_pointsR_bufR[j].y;
CV_MAT_ELEM(*object_pointsR,float,i,0)=(float)((j/board_widthR)*square_lengthR);
CV_MAT_ELEM(*object_pointsR,float,i,1)=(float)((j%board_widthR)*square_heightR);
CV_MAT_ELEM(*object_pointsR,float,i,2)=0.0f;
}
CV_MAT_ELEM(*point_countRsR,int,successesR,0)=total_per_imageR;
successesR++;
}
cout<<"hahahahh======"<<aR<<endl;
fout<<"hahahahh======"<<aR<<endl;
aR++;
}
cvReleaseImage(&showR);
cvDestroyWindow("RePlayR");
cout<<"*********************************************\n";
cout<<NImagesR<<"幀圖檔中，标定成功的圖檔為"<<successesR<<"幀...\n";
cout<<NImagesR<<"幀圖檔中，标定失敗的圖檔為"<<NImagesR-successesR<<"幀...\n\n";
cout<<"*********************************************\n\n";
cout<<"按任意鍵開始計算錄影機内參數...\n\n";
fout<<"*********************************************\n";
fout<<NImagesR<<"幀圖檔中，标定成功的圖檔為"<<successesR<<"幀...\n";
fout<<NImagesR<<"幀圖檔中，标定失敗的圖檔為"<<NImagesR-successesR<<"幀...\n\n";
fout<<"*********************************************\n\n";
fout<<"按任意鍵開始計算錄影機内參數...\n\n";
IplImage * showR_colieR;
showR_colieR = cvLoadImage("F:\\graduatelunwen\\opencvprojects\\june\\opecv_ransac_sift_cameraCalibration\\dinggao_wanchengban_sift_ransac\\right_43.jpg",1);
CvMat * object_pointsR2R = cvCreateMat(successesR*total_per_imageR,3,CV_32FC1);
CvMat * image_pointsR2R = cvCreateMat(successesR*total_per_imageR,2,CV_32FC1);
CvMat * point_countRsR2R = cvCreateMat(successesR,1,CV_32SC1);
for(int i=0;i<successesR*total_per_imageR;++i)
{
CV_MAT_ELEM(*image_pointsR2R, float,i,0)=CV_MAT_ELEM(*image_pointsR, float,i,0);
CV_MAT_ELEM(*image_pointsR2R, float,i,1)=CV_MAT_ELEM(*image_pointsR, float,i,1);
CV_MAT_ELEM(*object_pointsR2R,float,i,0)=CV_MAT_ELEM(*object_pointsR,float,i,0);
CV_MAT_ELEM(*object_pointsR2R,float,i,1)=CV_MAT_ELEM(*object_pointsR,float,i,1);
CV_MAT_ELEM(*object_pointsR2R,float,i,2)=CV_MAT_ELEM(*object_pointsR,float,i,2);
}
for(int i=0;i<successesR;++i)
{
CV_MAT_ELEM(*point_countRsR2R,int,i,0) = CV_MAT_ELEM(*point_countRsR,int,i,0);
}
cvReleaseMat(&object_pointsR);
cvReleaseMat(&image_pointsR);
cvReleaseMat(&point_countRsR);
CV_MAT_ELEM(*intrinsicR_matrixR,float,0,0)=1.0f;
CV_MAT_ELEM(*intrinsicR_matrixR,float,1,1)=1.0f;
cvCalibrateCamera2(object_pointsR2R,image_pointsR2R,point_countRsR2R,cvGetSize(showR_colieR),
intrinsicR_matrixR,distortionR_coeffsR,rotation_vectorsR,translation_vectorsR,0);
cout<<"錄影機内參數矩陣為：\n";
cout<<CV_MAT_ELEM(*intrinsicR_matrixR,float,0,0)<<" "<<CV_MAT_ELEM(*intrinsicR_matrixR,float,0,1)
<<" "<<CV_MAT_ELEM(*intrinsicR_matrixR,float,0,2)
<<"\n\n";
cout<<CV_MAT_ELEM(*intrinsicR_matrixR,float,1,0)<<" "<<CV_MAT_ELEM(*intrinsicR_matrixR,float,1,1)
<<" "<<CV_MAT_ELEM(*intrinsicR_matrixR,float,1,2)
<<"\n\n";
cout<<CV_MAT_ELEM(*intrinsicR_matrixR,float,2,0)<<" "<<CV_MAT_ELEM(*intrinsicR_matrixR,float,2,1)
<<" "<<CV_MAT_ELEM(*intrinsicR_matrixR,float,2,2)
<<"\n\n";
fout<<"錄影機内參數矩陣為：\n";
fout<<CV_MAT_ELEM(*intrinsicR_matrixR,float,0,0)<<" "<<CV_MAT_ELEM(*intrinsicR_matrixR,float,0,1)
<<" "<<CV_MAT_ELEM(*intrinsicR_matrixR,float,0,2)
<<"\n\n";
fout<<CV_MAT_ELEM(*intrinsicR_matrixR,float,1,0)<<" "<<CV_MAT_ELEM(*intrinsicR_matrixR,float,1,1)
<<" "<<CV_MAT_ELEM(*intrinsicR_matrixR,float,1,2)
<<"\n\n";
fout<<CV_MAT_ELEM(*intrinsicR_matrixR,float,2,0)<<" "<<CV_MAT_ELEM(*intrinsicR_matrixR,float,2,1)
<<" "<<CV_MAT_ELEM(*intrinsicR_matrixR,float,2,2)
<<"\n\n";
f_right[0]=CV_MAT_ELEM(*intrinsicR_matrixR,float,0,0);
f_right[1]=CV_MAT_ELEM(*intrinsicR_matrixR,float,1,1);
cout<<"畸變系數矩陣為：\n";
cout<<CV_MAT_ELEM(*distortionR_coeffsR,float,0,0)<<" "<<CV_MAT_ELEM(*distortionR_coeffsR,float,1,0)
<<" "<<CV_MAT_ELEM(*distortionR_coeffsR,float,2,0)
<<" "<<CV_MAT_ELEM(*distortionR_coeffsR,float,3,0)
<<" "<<CV_MAT_ELEM(*distortionR_coeffsR,float,4,0)
<<"\n\n";
fout<<"畸變系數矩陣為：\n";
fout<<CV_MAT_ELEM(*distortionR_coeffsR,float,0,0)<<" "<<CV_MAT_ELEM(*distortionR_coeffsR,float,1,0)
<<" "<<CV_MAT_ELEM(*distortionR_coeffsR,float,2,0)
<<" "<<CV_MAT_ELEM(*distortionR_coeffsR,float,3,0)
<<" "<<CV_MAT_ELEM(*distortionR_coeffsR,float,4,0)
<<"\n\n";
cvSave("intrinsicRs.xml",intrinsicR_matrixR);
cvSave("distortionR.xml",distortionR_coeffsR);
cout<<"錄影機矩陣、畸變系數向量已經分别存儲在名為intrinsicRs.xml、distortionR.xml文檔中\n\n";
fout<<"錄影機矩陣、畸變系數向量已經分别存儲在名為intrinsicRs.xml、distortionR.xml文檔中\n\n";
for(int ii = 0; ii < NImagesR; ++ii)
{ float tranvR[3] = {0.0};
float rotvR[3] = {0.0};
for ( int i = 0; i < 3; i++)
{
tranvR[i] = ((float*)(translation_vectorsR->data.ptr+ii*translation_vectorsR->step))[i];
rotvR[i] = ((float*)(rotation_vectorsR->data.ptr+rotation_vectorsR->step))[i];
}
cout << "第" << ii+1 << "幅圖的外參數" << endl;
printf("ROTATION VECTOR 旋轉向量 : \n");
printf("[ %6.4f %6.4f %6.4f ] \n", rotvR[0], rotvR[1], rotvR[2]);
printf("TRANSLATION VECTOR 平移向量: \n");
printf("[ %6.4f %6.4f %6.4f ] \n", tranvR[0], tranvR[1], tranvR[2]);
printf("-----------------------------------------\n");
fout << "第" << ii+1 << "幅圖的外參數" << endl;
fout<<"ROTATION VECTOR 旋轉向量 : \n";
fout<<"[ %6.4f %6.4f %6.4f ] \n"<<rotvR[0]<<" "<< rotvR[1]<<" "<< rotvR[2]<<endl;
fout<<"TRANSLATION VECTOR 平移向量: \n";
fout<<"[ %6.4f %6.4f %6.4f ] \n"<< tranvR[0]<<" "<< tranvR[1]<<" "<< tranvR[2]<<endl;
fout<<"-----------------------------------------\n";
}
CvMat * intrinsicR=(CvMat *)cvLoad("intrinsicRs.xml");
CvMat * distortionR=(CvMat *)cvLoad("distortionR.xml");
IplImage * mapxR=cvCreateImage(cvGetSize(showR_colieR),IPL_DEPTH_32F,1);
IplImage * mapyR=cvCreateImage(cvGetSize(showR_colieR),IPL_DEPTH_32F,1);
cvInitUndistortMap(intrinsicR,distortionR,mapxR,mapyR);
cvNamedWindow("原始圖像右",1);
cvNamedWindow("非畸變圖像右",1);
// cout<<"按‘E’鍵退出顯示...\n\n";
IplImage * cloneR=cvCloneImage(showR_colieR);
cvShowImage("原始圖像右",showR_colieR);
cvRemap(cloneR,showR_colieR,mapxR,mapyR);
cvReleaseImage(&cloneR);
cvShowImage("非畸變圖像右",showR_colieR);
cvWaitKey(500);
time_t start=0,end=0;
int i,j,k;
int max=-1;
float hh[8];
start=time(0);
float para[8][9];
float h[8];
//CvCapture* cap1;
//CvCapture* cap2;
//Mat input11,input22;
//cap1=cvCreateCameraCapture(0);
//cap2=cvCreateCameraCapture(1);
Mat input11,input22;
//Mat input11=imread("123.jpg",1);
//Mat input22=imread("124.jpg",1);
//Mat input11(show_colie);
//Mat input22(showR_colieR);
Mat input1,input2;
input11=imread("left_2.jpg",1);
input22=imread("right_2.jpg",1);
cvtColor(input11,input1,CV_RGB2GRAY,1);
cvtColor(input22,input2,CV_RGB2GRAY,1);
imwrite("left_2heibei.jpg",input1);
imwrite("right_2heibai.jpg",input2);
SiftFeatureDetector detector;
vector<KeyPoint> keypoint1,keypoint2;
detector.detect(input1,keypoint1);
Mat output1;
drawKeypoints(input1,keypoint1,output1);
imshow("sift_result1.png",output1);
imwrite("sift_result1.png",output1);
Mat output2;
SiftDescriptorExtractor extractor;
Mat descriptor1,descriptor2;
BruteForceMatcher<L2<float>> matcher;
vector<DMatch> matches;
Mat img_matches;
detector.detect(input2,keypoint2);
drawKeypoints(input2,keypoint2,output2);
imshow("sift_result2.png",output2);
imwrite("sift_result2.png",output2);
extractor.compute(input1,keypoint1,descriptor1);
extractor.compute(input2,keypoint2,descriptor2);
matcher.match(descriptor1,descriptor2,matches);
drawMatches(input1,keypoint1,input2,keypoint2,matches,img_matches);
imshow("matches",img_matches);
imwrite("matches.png",img_matches);
//配置設定空間
int pointcount=(int)matches.size();
Mat point1(pointcount,2,CV_32F);
Mat point2(pointcount,2,CV_32F);
//把Keypoint轉換為Mat
Point2f point;
for (i=0;i<pointcount;i++)
{
point=keypoint1[matches[i].queryIdx].pt;
point1.at<float>(i,0)=point.x;
point1.at<float>(i,1)=point.y;
point=keypoint2[matches[i].trainIdx].pt;
point2.at<float>(i,0)=point.x;
point2.at<float>(i,1)=point.y;
}
//用RANSAC方法計算F
Mat m_fundamental;
vector<uchar> m_ransacstatus;
m_fundamental=findFundamentalMat(point1,point2,m_ransacstatus,FM_RANSAC);
//計算外點個數
int outlinercount=0;
for(i=0;i<pointcount;i++)
{
if(m_ransacstatus[i]==0)//動态為0表示外點
{
outlinercount++;
}
}
//計算内點
vector<Point2f> m_leftinliner;
vector<Point2f> m_rightinliner;
vector<DMatch> m_inlinermatches;
//上面三個變量用于儲存内點和比對關系
int inlinercount=pointcount-outlinercount;
m_inlinermatches.resize(inlinercount);
m_leftinliner.resize(inlinercount);
m_rightinliner.resize(inlinercount);
inlinercount=0;
for(i=0;i<pointcount;i++)
{
if(m_ransacstatus[i]!=0)
{
m_leftinliner[inlinercount].x=point1.at<float>(i,0);
m_leftinliner[inlinercount].y=point1.at<float>(i,1);
m_rightinliner[inlinercount].x=point2.at<float>(i,0);
m_rightinliner[inlinercount].y=point2.at<float>(i,1);
m_inlinermatches[inlinercount].queryIdx=inlinercount;
m_inlinermatches[inlinercount].trainIdx=inlinercount;
inlinercount++;
}
}
//把内點轉換為drawMatches可以使用的格式
vector<KeyPoint> key1(inlinercount);
vector<KeyPoint> key2(inlinercount);
KeyPoint::convert(m_leftinliner,key1);
KeyPoint::convert(m_rightinliner,key2);
//顯示計算F過後的内點比對
Mat outimage;
drawMatches(input11,key1,input22,key2,m_inlinermatches,outimage);
// namedWindow("Match features");
imshow("Match feature",outimage);
imwrite("提純後的.jpg",outimage);
vector<KeyPoint> ransac1,ransac2;
vector<KeyPoint> left,right;
vector<int> tichunyihou;
int counter=0;
int counterdidi=0;
srand(unsigned(time(0)));
int counterwo;
int *countergege;
countergege=new int[inlinercount];
int * zuizhong=new int[inlinercount];
for(i=0;i<inlinercount;i++)
{
zuizhong[i]=0;
}
left.clear();
right.clear();
while(counter<250)
{
counterwo=0;
for(i=0;i<inlinercount;i++)
{
countergege[i]=0;
}
counterdidi++;
ransac1.clear();
ransac2.clear();
int temp[4];
for (i=0;i<4;i++)
temp[i]=rand()%inlinercount;
int gibal=0;
for (i=0;i<3;i++)
{
for (j=i+1;j<4;j++)
{
if (temp[i]==temp[j])
{
gibal=1;
break;
}
}
}
if(gibal==1)
continue;
int a,b;
float tan1=0,tan2=0;
for(i=0;i<3;i++)
{
j=i+1;
a=(j-1+4)%4;
b=(j+1+4)%4;
tan1=(key1[m_inlinermatches[j].queryIdx].pt.y-key1[m_inlinermatches[a].queryIdx].pt.y)
/(key1[m_inlinermatches[j].queryIdx].pt.x-key1[m_inlinermatches[a].queryIdx].pt.x);
tan2=(key1[m_inlinermatches[j].queryIdx].pt.y-key1[m_inlinermatches[b].queryIdx].pt.y)
/(key1[m_inlinermatches[j].queryIdx].pt.x-key1[m_inlinermatches[b].queryIdx].pt.x);
if(abs(tan1/tan2-1)<=0.05)
gibal=2;
}
if (gibal==2)
continue;
for (i=0;i<4;i++)
{
cout<<temp[i]<<" ";
fout<<temp[i]<<" ";
}
cout<<endl;
fout<<endl;
if(gibal==1)
continue;
for(j=0;j<4;j++)
{
ransac1.push_back(key1[m_inlinermatches[temp[j]].queryIdx]);
ransac2.push_back(key2[m_inlinermatches[temp[j]].trainIdx]);
}
if(ransac1.empty() || ransac2.empty())
{
cout<<"随機抽出四個特征點比對對失敗，請重試"<<endl;
fout<<"随機抽出四個特征點比對對失敗，請重試"<<endl;
continue;
}
//求透視矩陣
for(i=0;i<8;i++)
h[i]=0.0;
for (i=0;i<=7;i++)
{
for (j=0;j<=8;j++)
{
para[i][j]=0;
}
}
for (i=0;i<=2;i++)
{
para[i][2]=1;
para[i+3][5]=1;
para[i][0]=ransac1[i].pt.x;
para[i][1]=ransac1[i].pt.y;
para[i+3][3]=ransac1[i].pt.x;
para[i+3][4]=ransac1[i].pt.y;
para[i][6]=-ransac1[i].pt.x*ransac2[i].pt.x;
para[i][7]=-ransac1[i].pt.y*ransac2[i].pt.x;
para[i][8]=ransac2[i].pt.x;
para[i+3][6]=-ransac1[i].pt.x*ransac2[i].pt.y;
para[i+3][7]=-ransac1[i].pt.y*ransac2[i].pt.y;
para[i+3][8]=ransac2[i].pt.y;
}
para[6][2]=1;
para[7][5]=1;
para[6][0]=ransac1[3].pt.x;
para[6][1]=ransac1[3].pt.y;
para[7][3]=ransac1[3].pt.x;
para[7][4]=ransac1[3].pt.y;
para[6][6]=-ransac1[3].pt.x*ransac2[3].pt.y;
para[6][7]=-ransac1[3].pt.y*ransac2[3].pt.x;
para[6][8]=ransac2[3].pt.x;
para[7][6]=-ransac1[3].pt.x*ransac2[3].pt.y;
para[7][7]=-ransac1[3].pt.y*ransac2[3].pt.y;
para[7][8]=ransac2[3].pt.y;
/
//建立完擴充矩陣，求出透視矩陣的參數：透視矩陣參數估值
for (int g=0;g<8;g++)//初始化
h[g]=0.0;
for (i=0;i<8;i++)
{
float temper1=para[i][i];
if (para[i][i]!=0)
{
for (int j=i+1;j<8;j++)
{
float temper2=para[j][i];
if (para[j][i]!=0)
{
for (int k=i;k<9;k++)
{
para[j][k]=para[i][k]-para[j][k]*para[i][i]/temper2;
}
}
else continue;
}
for (int r=i;r<9;r++)
para[i][r]/=temper1;
}
else continue;
}
for (i=0;i<=7;i++)
h[i]=para[i][8];
for (j=7;j>=0;j--)
{
for (int k=0;k<7-j;k++)
{
h[j]-=h[7-k]*para[j][7-k];
}
}
//計算出每個透視矩陣所滿足的内點數
float xx,yy;
float juli;
int ok=0;
for(i=0;i<inlinercount;i++)
{
xx=0;yy=0;
xx=h[0]*key1[m_inlinermatches[i].queryIdx].pt.x+h[1]*key1[m_inlinermatches[i].queryIdx].pt.y+h[2];
yy=h[3]*key1[m_inlinermatches[i].queryIdx].pt.x+h[4]*key1[m_inlinermatches[i].queryIdx].pt.y+h[5];
juli=sqrt(pow((xx-key2[m_inlinermatches[i].trainIdx].pt.x),2)+pow((yy-key2[m_inlinermatches[i].trainIdx].pt.y),2));
if (juli<2)
{
countergege[counterwo]=i;
counterwo++;
ok++;
}
}
if(ok>max)
{
max=ok;
for(k=0;k<8;k++)
hh[k]=h[k];
for(i=0;i<inlinercount;i++)
{
zuizhong[i]=countergege[i];
}
}
counter++;
}
for(i=0;i<8;i++)
{
cout<<hh[i]<<endl;
fout<<hh[i]<<endl;
}
for(i=0;i<max;i++)
{
left.push_back(key1[m_inlinermatches[zuizhong[i]].queryIdx]);
right.push_back(key2[m_inlinermatches[zuizhong[i]].trainIdx]);
}
cout<<"%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%"<<endl;
vector<DMatch> wanquantichun;
int haha=left.size();
wanquantichun.resize(haha);
for(i=0;i<haha;i++)
{
wanquantichun[i].queryIdx=i;
wanquantichun[i].trainIdx=i;
}
Mat outimagehaha;
float f=85;
float T=2000;
drawMatches(input11,left,input22,right,wanquantichun,outimagehaha);
float xl=0,xr=0;
float Z=430;
for(i=0;i<haha;i++)
{
Z=0;
if(key1[i].pt.x<428 && key1[i].pt.x>213 )
{
if(key2[i].pt.x<428 && key2[i].pt.x>213)
{
xl=abs(385-key1[i].pt.x);
xr=abs(282-key2[i].pt.x);
Z=abs(f*T/(xl+xr));
cout<<"距離為=="<<Z<<endl;
fout<<"距離為=="<<Z<<endl;
}
}
}
// namedWindow("Match features");
imshow("完全提純後 Match feature",outimagehaha);
imwrite("完全提純後的.jpg",outimagehaha);
float uxleft,uxright;
//uxleft=
//圖像配準
int xx1=input11.cols;
int yy1=input11.rows;
int xx2=input22.cols;
int yy2=input22.rows;
int garo=0;
int saero=0;
int yiweiX=1000,yiweiYmin=1000,yiweiYmax=-1000;
int xxx,yyy;
for(i=0;i<yy1;i++)
{
for (j=0;j<xx1;j++)
{
garo=int(j*hh[0]+i*hh[1]+hh[2]);
saero=int(j*hh[3]+i*hh[4]+hh[5]);
if(yiweiX>garo)
yiweiX=garo;
if(yiweiYmin>saero)
yiweiYmin=saero;
if(yiweiYmax<saero)
yiweiYmax=saero;
}
}
if(yiweiX>0)
yiweiX=0;
cout<<yiweiYmin<<" "<<yiweiX<<" "<<yiweiYmax<<endl;
fout<<yiweiYmin<<" "<<yiweiX<<" "<<yiweiYmax<<endl;
if(yiweiYmax>=yy2)
{
//^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^1111111111111111111111111
if(yiweiYmin<0)
{
cout<<"第一種情況"<<endl;
fout<<"第一種情況"<<endl;
Mat zuihoupeizhun(yiweiYmax-yiweiYmin,xx2-yiweiX,CV_8UC3,Scalar(0,0,0));
for(i=0;i<yy1;i++)
{
for(j=0;j<xx1;j++)
{
garo=int(j*hh[0]+i*hh[1]+hh[2]);
saero=int(j*hh[3]+i*hh[4]+hh[5]);
xxx=garo-yiweiX;
yyy=saero-yiweiYmin;
if (yyy>=0 && xxx>=0&&xxx<(xx2-yiweiX)&&yyy<(yiweiYmax-yiweiYmin))
{
zuihoupeizhun.at<Vec3b>(yyy,xxx)[0]=input11.at<Vec3b>(i,j)[0];
zuihoupeizhun.at<Vec3b>(yyy,xxx)[1]=input11.at<Vec3b>(i,j)[1];
zuihoupeizhun.at<Vec3b>(yyy,xxx)[2]=input11.at<Vec3b>(i,j)[2];
}
else
continue;
}
}
for (i=0;i<yy2;i++)
{
for (j=0;j<xx2;j++)
{
xxx=j-yiweiX;
yyy=i-yiweiYmin;
if (yyy>=0 && xxx>=0&&xxx<(xx2-yiweiX)&&yyy<(yiweiYmax-yiweiYmin))
{
zuihoupeizhun.at<Vec3b>(yyy,xxx)[0]=input22.at<Vec3b>(i,j)[0];
zuihoupeizhun.at<Vec3b>(yyy,xxx)[1]=input22.at<Vec3b>(i,j)[1];
zuihoupeizhun.at<Vec3b>(yyy,xxx)[2]=input22.at<Vec3b>(i,j)[2];
}
else
continue;
}
}
namedWindow("src");
imshow("src",zuihoupeizhun);
imwrite("src.png",zuihoupeizhun);
}
//^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^222222222222222
else
{
cout<<"第二種情況"<<endl;
fout<<"第二種情況"<<endl;
Mat zuihoupeizhun(yiweiYmax,xx2-yiweiX,CV_8UC3,Scalar(0,0,0));
for(i=0;i<yy1;i++)
{
for(j=0;j<xx1;j++)
{
garo=int(j*hh[0]+i*hh[1]+hh[2]);
saero=int(j*hh[3]+i*hh[4]+hh[5]);
xxx=garo-yiweiX;
yyy=saero;
if (yyy>=0 && xxx>=0 && yyy<yiweiYmax && xxx<(xx2-yiweiX))
{
zuihoupeizhun.at<Vec3b>(yyy,xxx)[0]=input11.at<Vec3b>(i,j)[0];
zuihoupeizhun.at<Vec3b>(yyy,xxx)[1]=input11.at<Vec3b>(i,j)[1];
zuihoupeizhun.at<Vec3b>(yyy,xxx)[2]=input11.at<Vec3b>(i,j)[2];
}
else
continue;
}
}
for (i=0;i<yy2;i++)
{
for (j=0;j<xx2;j++)
{
xxx=j-yiweiX;
yyy=i;
if (yyy>=0 && xxx>=0 && yyy<yiweiYmax && xxx<(xx2-yiweiX))
{
zuihoupeizhun.at<Vec3b>(yyy,xxx)[0]=input22.at<Vec3b>(i,j)[0];
zuihoupeizhun.at<Vec3b>(yyy,xxx)[1]=input22.at<Vec3b>(i,j)[1];
zuihoupeizhun.at<Vec3b>(yyy,xxx)[2]=input22.at<Vec3b>(i,j)[2];
}
else
continue;
}
}
namedWindow("src");
imshow("src",zuihoupeizhun);
imwrite("src.png",zuihoupeizhun);
}
}
else
{
//^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^3333333333333333333333333333
if(yiweiYmin<0)
{
cout<<"第三種情況"<<endl;
fout<<"第三種情況"<<endl;
Mat zuihoupeizhun(yy2-yiweiYmin,xx2-yiweiX,CV_8UC3,Scalar(0,0,0));
for(i=0;i<yy1;i++)
{
for(j=0;j<xx1;j++)
{
garo=int(j*hh[0]+i*hh[1]+hh[2]);
saero=int(j*hh[3]+i*hh[4]+hh[5]);
xxx=garo-yiweiX;
yyy=saero-yiweiYmin;
if (yyy>=0 && xxx>=0 && yyy<(yy2-yiweiYmin) && xxx<(xx2-yiweiX))
{
zuihoupeizhun.at<Vec3b>(yyy,xxx)[0]=input11.at<Vec3b>(i,j)[0];
zuihoupeizhun.at<Vec3b>(yyy,xxx)[1]=input11.at<Vec3b>(i,j)[1];
zuihoupeizhun.at<Vec3b>(yyy,xxx)[2]=input11.at<Vec3b>(i,j)[2];
}
else
continue;
}
}
for (i=0;i<yy2;i++)
{
for (j=0;j<xx2;j++)
{
xxx=j-yiweiX;
yyy=i-yiweiYmin;
if (yyy>=0 && xxx>=0 && yyy<(yy2-yiweiYmin) && xxx<(xx2-yiweiX))
{
zuihoupeizhun.at<Vec3b>(yyy,xxx)[0]=input22.at<Vec3b>(i,j)[0];
zuihoupeizhun.at<Vec3b>(yyy,xxx)[1]=input22.at<Vec3b>(i,j)[1];
zuihoupeizhun.at<Vec3b>(yyy,xxx)[2]=input22.at<Vec3b>(i,j)[2];
}
else
continue;
}
}
namedWindow("src");
imshow("src",zuihoupeizhun);
imwrite("src.png",zuihoupeizhun);
}
else
{
//^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^444444444444444444
cout<<"第四種情況"<<endl;
fout<<"第四種情況"<<endl;
Mat zuihoupeizhun(yy2,xx2-yiweiX,CV_8UC3,Scalar(0,0,0));
for(i=0;i<yy1;i++)
{
for(j=0;j<xx1;j++)
{
garo=int(j*hh[0]+i*hh[1]+hh[2]);
saero=int(j*hh[3]+i*hh[4]+hh[5]);
xxx=garo-yiweiX;
yyy=saero;
if (yyy>=0 && xxx>=0 && yyy<yy2 && xxx<(xx2-yiweiX))
{
zuihoupeizhun.at<Vec3b>(yyy,xxx)[0]=input11.at<Vec3b>(i,j)[0];
zuihoupeizhun.at<Vec3b>(yyy,xxx)[1]=input11.at<Vec3b>(i,j)[1];
zuihoupeizhun.at<Vec3b>(yyy,xxx)[2]=input11.at<Vec3b>(i,j)[2];
}
else
continue;
}
}
for (i=0;i<yy2;i++)
{
for (j=0;j<xx2;j++)
{
xxx=j-yiweiX;
yyy=i;
if (xxx>=0 && yyy>=0)
{
zuihoupeizhun.at<Vec3b>(yyy,xxx)[0]=input22.at<Vec3b>(i,j)[0];
zuihoupeizhun.at<Vec3b>(yyy,xxx)[1]=input22.at<Vec3b>(i,j)[1];
zuihoupeizhun.at<Vec3b>(yyy,xxx)[2]=input22.at<Vec3b>(i,j)[2];
}
else
continue;
}
}
namedWindow("src");
imshow("src",zuihoupeizhun);
imwrite("src.jpg",zuihoupeizhun);
}
}
end=time(0);
cout<<"i love you"<<endl;
fout<<"i love you"<<endl;
cout<<"最後的max===="<<max<<endl;
fout<<"最後的max===="<<max<<endl;
end=time(0);
cout<<endl<<"總跑程式的時間為："<<end-start<<"秒"<<endl;
fout<<endl<<"總跑程式的時間為："<<end-start<<"秒"<<endl;
cout<<endl;fout<<endl;
cout<<"counter=="<<counter<<endl<<"counterdidi=="<<counterdidi<<endl;
fout<<"counter=="<<counter<<endl<<"counterdidi=="<<counterdidi<<endl;
waitKey(30000);
}
![查找算法之二分查找查找算法之二分查找[圖]](data:image/gif;base64,R0lGODlhAQABAIAAAP///wAAACwAAAAAAQABAAACAkQBADs=)