基于OpenCv的摄像机立体标定和立体矫正的源代码

/********************************************************************************************************************** 
程序功能: 
        摄像机立体标定和立体校正的Demo------此程序现在还不可以运行,但思路是正确的,最近正在调试这个程序
开发环境: 
        OpenCv2.4.8 + VS2012 + Halocn10.0 + Win10
时间地点: 
        陕西师范大学 2016.10.14
作者信息: 
        九月----马卫飞 
***********************************************************************************************************************/  
/*************************************************【函数的头文件和命名空间】*******************************************/ 
#include<stdio.h>
#include<iostream>


#include<opencv2/calib3d/calib3d.hpp>
#include<opencv2/core/core.hpp>
#include<opencv2/highgui/highgui.hpp>
#include<opencv2/imgproc/imgproc.hpp>

#include<opencv2/core/core_c.h>
#include<opencv2/highgui/highgui_c.h>
#include<opencv2/imgproc/imgproc_c.h>

using namespace std;
using namespace cv;

/*****************************************************************************************************
******************************************************************************************************/
static void StereoCalibrate(const char* strImageList,int iNx,int iNy,int iUseUncalibrateed)
{
	int   iDisplayCorners      = 0;
	int   iShowUndistorted     = 1;
	bool  bIsVerticalStereo    = false;        //【1】OpenCv can handle left-right or up-down camera arrangements

	const int   iMaxScale      = 1;
	const float fSquareSize    = 1.f;          //【2】Set this to your actual square size
	                                           //【3】"rt"读写一个文本文件,允许读和写
	std::FILE*  pF             = std::fopen(strImageList,"rt");

	int   i                    = 0;
	int   j                    = 0;
	int   lr                   = 0;
	int   iNFreams             = 0;
	int   n                    = iNx*iNy;
	int   N                    = 0;

	std::vector<string>         vImageNames[2];
	std::vector<CvPoint3D32f>   vObjectPoints;
	std::vector<CvPoint2D32f>   vPoints[2];
	std::vector<int>            vNPoints;
	std::vector<uchar>          vActive[2];
	std::vector<CvPoint2D32f>   vTemp(n);
	CvSize    imageSize        =cvSize(0,0);
	
	double                      M1[3][3];
	double                      M2[3][3];
	double                      D1[5];
	double                      D2[5];
	double                      R[3][3];
	double                      T[3];
	double                      E[3][3];
	double                      F[3][3];

	CvMat*   _M1 = cvCreateMat(3,3,CV_64F);
	CvMat*   _M2 = cvCreateMat(3,3,CV_64F);
	CvMat*   _D1 = cvCreateMat(1,5,CV_64F);
	CvMat*   _D2 = cvCreateMat(1,5,CV_64F);
	CvMat*   _R  = cvCreateMat(3,3,CV_64F);
	CvMat*   _T  = cvCreateMat(3,1,CV_64F);
	CvMat*   _E  = cvCreateMat(3,3,CV_64F);
	CvMat*   _F  = cvCreateMat(3,3,CV_64F);

	if(iDisplayCorners)
		cvNamedWindow("【corners】",1);

	//【1】读棋盘格列表
	if(!pF)
	{
		fprintf(stderr,"can not open file %s\n",strImageList);
		return;
	}
	for(int i=0; ;i++)
	{
		char   cBuf[1024];
		int    iCount              = 0;
		int    iResult             = 0;
		lr                         = i%2;

		std::vector<CvPoint2D32f>& pts = vPoints[lr];
		if(!fgets(cBuf,sizeof(cBuf)-3,pF))
			break;
		size_t len     = strlen(cBuf);
			while(len>0&&isspace(cBuf[len-1]))
				cBuf[--len]='\0';
		if(cBuf[0]=='#')
			continue;
		IplImage* pImg = cvLoadImage(cBuf,0);
		if(!pImg)
			break;
		imageSize      = cvGetSize(pImg);

		vImageNames[lr].push_back(cBuf);

		for(int s=1;s<=iMaxScale;s++)
		{
			IplImage* pTImg = pImg;
			if(s>1)
			{
				pTImg = cvCreateImage(cvSize(pImg->width*s,pImg->height*s),pImg->depth,pImg->nChannels);
				cvResize(pImg,pTImg,CV_INTER_CUBIC);
			}
			iResult = cvFindChessboardCorners(pTImg,                   //【1】提取棋盘格上的角点(ICS::像素)
				                              cvSize(iNx,iNy),
											  &vTemp[0],
											  &iCount,
											  CV_CALIB_CB_ADAPTIVE_THRESH|CV_CALIB_CB_NORMALIZE_IMAGE
				); 
			if(pTImg!=pImg)
			{
				cvReleaseImage(&pTImg);
			}
			if(iResult||s==iMaxScale)
				for(int j=0;j<iCount;j++)
				{
					vTemp[j].x /= s;
					vTemp[j].y /= s;
				}
				if(iResult)
					break;
		}
		if(iDisplayCorners)
		{
			std::printf("%s\n",cBuf);
			IplImage* pCImg = cvCreateImage(imageSize,8,3);
			cvCvtColor(pImg,pCImg,CV_GRAY2BGR);                          //【2】将灰度图像转化为彩色图像
			cvDrawChessboardCorners(pImg,                                //【3】将从棋盘格上检测出来的角点绘制出来
				                    cvSize(iNx,iNy),
									&vTemp[0],
									iCount,
									iResult
				);
			cvShowImage("【corners】",pCImg);
			cvReleaseImage(&pCImg);
			if(cvWaitKey(0)==27)
				std::exit(-1);
		}
		else
		{
			std::putchar('.');
		}
		N = pts.size();

		CvPoint2D32f ptTemp;

		ptTemp.x = 0;
		ptTemp.y = 0;

		pts.resize(N+n,ptTemp);
		vActive[lr].push_back((uchar)iResult);
		if(iResult)
		{
			cvFindCornerSubPix(pImg,                                     //【4】精确提取的角点
				               &vTemp[0],
							   iCount,
							   cvSize(11,11),
							   cvSize(-1,-1),
							   cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,30,0.01));
		}
		copy(vTemp.begin(),vTemp.end(),pts.begin()+N);
	}
	fclose(pF);
	//【5】捕获棋盘格上3D目标对象的列表
	std::cout<<std::endl;
	iNFreams = vActive[0].size();                                        //【1】棋盘格角点的数目
	vObjectPoints.resize(iNFreams*n);                                    //【2】WCS下棋盘格角点的3D坐标(WCS::cm/mm)
	for(int i=0;i<iNy;i++)                                               //【3】计算WCS下棋盘格的3D坐标点
	{
		for(int j=0;j<iNx;j++)
		{
			vObjectPoints[i*iNx+j].x = i*fSquareSize;
			vObjectPoints[i*iNx+j].y = j*fSquareSize;
			vObjectPoints[i*iNx+j].z = 0; 

		}
	}
	vNPoints.resize(iNFreams,n);
	N = iNFreams*n;

	CvMat* _pObjectPoints = cvCreateMat(1,N,CV_32FC3);
	CvMat* _pImagePoints1 = cvCreateMat(1,N,CV_32FC2);
	CvMat* _pImagePoints2 = cvCreateMat(1,N,CV_32FC2);
	CvMat* _pNPoints      = cvCreateMat(1,vNPoints.size(),CV_32S);

	cvSetIdentity(&_M1);
	cvSetIdentity(&_M2);
	cvZero(&_D1);
	cvZero(&_D2);

	//【6】进行摄像机的立体标定
	std::cout<<"【HELP INFO】进行摄像机的立体标定..........."<<std::endl;
	fflush(stdout);                                                            //【1】摄像机立体标定的第一个重要模块
	cvStereoCalibrate(_pObjectPoints,                                      
		              _pImagePoints1,
		              _pImagePoints2,
					  _pNPoints,
		              _M1,
		              _D2,
		              _M2,
		              _D2,
		              imageSize,
					  _R,
					  _T,
					  _E,
					  _F,
					  cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,100,1e-5),
					  CV_CALIB_FIX_ASPECT_RATIO|
					  CV_CALIB_ZERO_TANGENT_DIST|
					  CV_CALIB_SAME_FOCAL_LENGTH);
	printf("Done\n");
	/***********************************************************************************************
	*模块说明:
	*         1---立体标定的评价
	*         2---因为输出的【基础矩阵】已经隐式的包含了所有的输出信息
	*         3---我们可以使用极线约束去检查摄像机标定的质量
	************************************************************************************************/
	std::vector<CvPoint3D32f>          vLines[2];
	vPoints[0].resize(N);
	vPoints[1].resize(N);

	//_pImagePoints1 = 
    //_pImagePoints2 =
	vLines[0].resize(N);
	vLines[1].resize(N);

	CvMat* _L1 = cvCreateMat(1,N,CV_32FC3);
	CvMat* _L2 = cvCreateMat(1,N,CV_32FC3);

	cvUndistortPoints(_pImagePoints1,
		              _pImagePoints1,
					  _M1,
					  _D1,
					  0,
					  _M1
		);
	cvUndistortPoints(_pImagePoints2,
		              _pImagePoints2,
					  _M2,
					  _D2,
					  0,_M2
		);
	cvComputeCorrespondEpilines(_pImagePoints1,
		                        1,
								_F,
								_L1
	    );
	cvComputeCorrespondEpilines(_pImagePoints2,
		                        1,
								_F,
								_L2
		);
	double    dAveErr = 0;
	for(int i=0;i<N;i++)
	{
		double dErr = std::fabs(vPoints[0][i].x*vLines[1][i].x+vPoints[0][i].y*vPoints[0][i].y);
		dAveErr = dAveErr+dErr;
	}
	printf("dAveErr = %g\n",dAveErr/(iNFreams*n));
	/***********************************************************************************************
	*模块说明:
	*         1---计算并且显示矫正
	************************************************************************************************/
	if(iShowUndistorted)
	{
		CvMat* pMx1    = cvCreateMat(imageSize.height,imageSize.width,CV_32F);
		CvMat* pMy1    = cvCreateMat(imageSize.height,imageSize.width,CV_32F);
		CvMat* pMx2    = cvCreateMat(imageSize.height,imageSize.width,CV_32F);
		CvMat* pMy2    = cvCreateMat(imageSize.height,imageSize.width,CV_32F);

		CvMat* pImg1r  = cvCreateMat(imageSize.height,imageSize.width,CV_8U);
		CvMat* pImg2r  = cvCreateMat(imageSize.height,imageSize.width,CV_8U);
		CvMat* pDisp   = cvCreateMat(imageSize.height,imageSize.width,CV_8U);

		CvMat* pPair   = NULL;
		double R1[3][3];
		double R2[3][3];
		double P1[3][4];
		double P2[3][4];

		CvMat _R1     = cvMat(3,4,CV_64F,R1);
		CvMat _R2     = cvMat(3,4,CV_64F,R2);

		if(iUseUncalibrateed == 0)
		{
			CvMat _P1     = cvMat(3,4,CV_64F,P1);
		    CvMat _P2     = cvMat(3,4,CV_64F,P2);
			cvStereoRectify(_M1,
				            _M2,
							_D1,
							_D2,
							imageSize,
							_R,
							_T,
							&_R1,
							&_R2,
							&_P1,
							&_P2,
							0,
							0);
			cvInitUndistortRectifyMap(_M1,_D1,&_R1,&_P1,pMx1,pMy1);
			cvInitUndistortRectifyMap(_M2,_D2,&_R2,&_P2,pMx2,pMy2);
		}
		else if (iUseUncalibrateed==1||iUseUncalibrateed==2)
		{
			double dH1[3][3];
			double dH2[3][3];
			double dIM[3][3];
			CvMat  _H1 = cvMat(3,3,CV_64F,dH1);
			CvMat  _H2 = cvMat(3,3,CV_64F,dH2);
			CvMat  _iM = cvMat(3,3,CV_64F,dIM);
			if(iUseUncalibrateed==2)
				cvFindFundamentalMat(_pImagePoints1,_pImagePoints2,_F);
			cvStereoRectifyUncalibrated(_pImagePoints1,
				                        _pImagePoints2,
										_F,
										imageSize,
										&_H1,
										&_H2,
										3
										);
			cvInvert(&_M1,&_iM);
			cvMatMul(&_H1,&_M1,&_R1);
			cvMatMul(&_iM,&_R1,&_R1);
			cvInvert(&_M2,&_iM);
			cvMatMul(&_H2,&_M2,&_R2);
			cvMatMul(&_iM,&_R2,&_R2);
			cvInitUndistortRectifyMap(_M1,_D1,&_R1,_M1,pMx1,pMy1);
			cvInitUndistortRectifyMap(_M2,_D2,&_R2,_M2,pMx2,pMy2);
		}
		else
		{
			assert(0);
		}
		cvNamedWindow("【*rectified】",1);
		if(!bIsVerticalStereo)
		{
			pPair = cvCreateMat(imageSize.height,imageSize.width*2,CV_8UC3);
		}
		else
		{
			pPair = cvCreateMat(imageSize.height*2,imageSize.width,CV_8UC3);
		}
		CvStereoBMState *BMState = cvCreateStereoBMState();
		assert(BMState!=0);
		BMState->preFilterSize       = 41;
		BMState->preFilterCap        = 31;
		BMState->SADWindowSize       = 41;
		BMState->minDisparity        = -64;
		BMState->numberOfDisparities = 128;
		BMState->textureThreshold    = 10;
		BMState->uniquenessRatio     = 15;
		for(int i=0;i<iNFreams;i++)
		{
			IplImage* pImg1 = cvLoadImage(vImageNames[0][i].c_str(),0);
			IplImage* pImg2 = cvLoadImage(vImageNames[1][i].c_str(),0);
			if(pImg1&&pImg2)
			{
				CvMat part;
				cvRemap(pImg1,pImg1r,pMx1,pMy1);
				cvRemap(pImg2,pImg2r,pMx2,pMy2);
				if(!bIsVerticalStereo||iUseUncalibrateed!=0)
				{
					cvFindStereoCorrespondenceBM(pImg1r,
						                         pImg2r,
												 pDisp,
												 BMState);
					cvNormalize(pDisp,pDisp,0,256,CV_MINMAX);
					cvNamedWindow("【DispParity】",1);
					cvShowImage("【DispParity】",pDisp);
				}
				if(!bIsVerticalStereo)
				{
					cvGetCols(pPair,&part,0,imageSize.width);
					cvCvtColor(pImg1r,&part,CV_GRAY2BGR);
					cvGetCols(pPair,&part,imageSize.width,imageSize.width*2);
					for(int j=0;j<imageSize.width;j+=16)
						cvLine(pPair,cvPoint(0,j),cvPoint(imageSize.width*2,j),CV_RGB(0,255,0));
				}
				else
				{
					cvGetRows(pPair,&part,0,imageSize.height);
					cvCvtColor(pImg1r,&part,CV_GRAY2BGR);
					cvGetRows(pPair,&part,imageSize.height,imageSize.height*2);
					cvCvtColor(pImg2,&part,CV_GRAY2RGB);
				}
				cvShowImage("【DispParity】",pPair);
				if(cvWaitKey()==27)
					break;
			}
			cvReleaseImage(&pImg1);
			cvReleaseImage(&pImg2);
		}
		cvReleaseStereoBMState(&BMState);
		cvReleaseMat(&pMx1);
		cvReleaseMat(&pMy1);
		cvReleaseMat(&pMx2);
		cvReleaseMat(&pMy1);
		cvReleaseMat(&pImg1r);
		cvReleaseMat(&pImg2r);
		cvReleaseMat(&pDisp);
	}
}
int main(int argc,char** argv)
{
	StereoCalibrate("list.txt",9,6,1);
	return 0;
}

  • 0
    点赞
  • 2
    收藏
    觉得还不错? 一键收藏
  • 0
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值