基于OpenCv的摄像机立体标定和立体矫正的源代码
2016-10-15 21:19
609 查看
/**********************************************************************************************************************
程序功能:
摄像机立体标定和立体校正的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;
}
内容来自用户分享和网络整理，不保证内容的准确性，如有侵权内容，可联系管理员处理 
相关文章推荐
- 基于matlab标定数据，使用opencv实现双目立体摄像头的标定（源代码）
- 基于matlab标定数据，使用opencv实现双目立体摄像头的标定（源代码）
- 基于OPENCV摄像机标定的源码
- 机器视觉学习笔记（5）——基于OpenCV的单目摄像机标定
- 基于MATLAB和opencv的单目摄像机标定
- 基于EmguCV的摄像机标定及矫正
- 关于基于OPENCV摄像机标定的一点感受
- OpenCV学习笔记（25）基于MFC和OpenCV的摄像机定标与立体匹配测试程序（20140505更新）
- 基于OpenCV立体视觉标定和校正
- 基于OpenCV立体视觉标定和校正
- 基于opencv的MFC多摄像机视场标定软件
- 基于OpenCV的摄像机标定-翻译
- 摄像机标定原理 源代码及解析（基于VC6.0+OpenC1.0）
- 基于OpenCV的双目摄像机标定
- 基于opencv的鱼眼矫正法-棋盘标定
- linux下基于opencv的摄像机标定与重投影
- 基于opencv的摄像机标定
- 机器视觉学习笔记（7）——基于OpenCV的双目摄像机标定
- 基于EmguCV的摄像机标定及矫正
- 机器视觉学习笔记（5）——基于OpenCV的单目摄像机标定