⭐ 欢迎来到虫虫下载站！ | 📦 资源下载 📁 资源专辑 ℹ️ 关于我们
⭐ 虫虫下载站
📤 上传资源

📄 alibratecamera.cpp

📁 用于相机标定的程序。工业测量的一个重要手段是近景摄影测量。其中要用到相机标定。
💻 CPP
📖 第 1 页 / 共 2 页
字号:
🔍
12 
// alibrateCamera.cpp: implementation of the CalibrateCamera class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "DLT方法相机标定.h"
#include "alibrateCamera.h"
#include"Adjustment.h"
#include"amerInterPra.h"
#include"ameraOutPra.h"
#include <math.h>
#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW
#endif

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

CalibrateCamera::CalibrateCamera()
{

}

CalibrateCamera::~CalibrateCamera()
{

}
/********************利用DLT方法对单张相片进行相机内外参数的求解****************
*ControlPoint----  控制点坐标（为通过全站仪测得的点坐标或其它方法获得的）
*ImagePointUV------控制点像坐标
*m---------------- 标定装置上控制点的个数

*OutPra----------  相机方位元素,顺序依次为Xs,Ys,Zs,w,k,Fi,u0,v0,fu,fv;
*/
void CalibrateCamera::DLTPra(double ControlPoint[], double  ImagePointUV[], int m, double OutPra[10])
{ int i,j,k; 
//A参数平差时的系数矩阵，L参数平差时的常数矩阵，p参数平差时的权矩阵
//s1存储DLT方法解得的参数平差值,s2存储DLT方法解得在s1上一次的改正数
 //static double s1[11]={0},s2[11]={0};
double *A,*L,*p,*s1,*s2;
//WKFi[3]---存储旋转角
double WKFi[3];
//辅助量：max---循环判断量，SunA---DLT辅助量；；
 double max=1,SunA;
 //辅助量：u0,v0---主点坐标，fu,fv---为焦距fu,fv；平移量 x0,y0,z0;
  double u0,v0,fu,fv,x0,y0,z0;
//r11,r12,r13,r21,r22,r23,r31,r32,r33---存放旋转角对应的旋转矩阵元素,r[9]----旋转矩阵.
double r11,r12,r13,r21,r22,r23,r31,r32,r33,R[9];
//LL3[3]---存放平移量x0,y0,z0(先旋转后平移时的平移量)；XYZs[3]----存放平移量Xs,Ys,Zs(先平移后旋转时的平移量)
double LL3[3],XYZs[3];
Adjustment  adjust; 
//动态开辟数组
A=new double[2*m*11];
L=new double[2*m];
p=new double[2*m*2*m];
s1=new double[11];
s2=new double[11];
//初始化为0
  for(i=0;i<2*m*11;i++)
	  A[i]=0;
//初始化为单位阵
  for(i=0;i<2*m;i++)
	  for(j=0;j<2*m;j++)
	  {
		  if(i==j)
			  p[i*2*m+j]=1;
		  else 
              p[i*2*m+j]=0;
	  }  
//误差方程中A矩阵和L矩阵进行赋值，并进行迭代求解

	  for(j=0;max>0.000000001;j++)
	  {    
		  for(i=0;i<m;i++)
		  {
			  if(0==j)
				  SunA=1;
			  else
			  SunA=s1[8]*ControlPoint[3*i]+s1[9]*ControlPoint[3*i+1]+s1[10]*ControlPoint[3*i+2]+1;
			  A[2*i*11]=ControlPoint[i*3]/SunA;  //a11系数
			  A[2*i*11+1]=ControlPoint[i*3+1]/SunA;//a12系数
			  A[2*i*11+2]=ControlPoint[i*3+2]/SunA; // a13系数       
			  A[2*i*11+3]=1/SunA;       //a14系数
			  A[2*i*11+8]=-ControlPoint[i*3]*ImagePointUV[i*2]/SunA;//a31系数
			  A[2*i*11+9]=-ControlPoint[i*3+1]*ImagePointUV[i*2]/SunA;//a32系数
			  A[2*i*11+10]=-ControlPoint[i*3+2]*ImagePointUV[i*2]/SunA;//a33系数
			  
			  //A矩阵偶数行进行赋值
			  A[(2*i+1)*11+4]=ControlPoint[i*3]/SunA;//a21系数
			  A[(2*i+1)*11+5]=ControlPoint[i*3+1]/SunA;//a22系数
			  A[(2*i+1)*11+6]=ControlPoint[i*3+2]/SunA;//a23系数
			  A[(2*i+1)*11+7]=1/SunA;//a24系数
			  A[(2*i+1)*11+8]=-ControlPoint[i*3]*ImagePointUV[i*2+1]/SunA;//a31系数
			  A[(2*i+1)*11+9]=-ControlPoint[i*3+1]*ImagePointUV[i*2+1]/SunA;//a32系数
		      A[(2*i+1)*11+10]=-ControlPoint[i*3+2]*ImagePointUV[i*2+1]/SunA;//a33系数
			  //L矩阵行进行赋值
			  L[2*i]=ImagePointUV[i*2]/SunA;
			  L[2*i+1]=ImagePointUV[i*2+1]/SunA;
		  }
		  
		  //参数平差，将平差结果放入s1中
		  adjust.ParameterAdjustment(A,p,L,2*m,11,s1);
        //计算邻近两次结果之差的绝对值，当小于0.000001时停止迭代
		  if(j==0)
		  {
			  for(k=0;k<11;k++)
				  s2[k]=s1[k];
			  
		  } 
		  else 
		  {
			  max=0;
			  for(k=0;k<11;k++)
			  max+=pow((s1[k]-s2[k]),2);
				 
			  
		  }
		  
		  for(k=0;k<11;k++)
				  s2[k]=s1[k];

		  
	  } 
	  
	  for(k=0;k<11;k++)
				  s1[k]=-s1[k];
//各参数计算
z0=sqrt(1/(pow(s1[8],2)+pow(s1[9],2)+pow(s1[10],2)));
u0=(s1[0]*s1[8]+s1[1]*s1[9]+s1[2]*s1[10])*pow(z0,2);
v0=(s1[4]*s1[8]+s1[5]*s1[9]+s1[6]*s1[10])*pow(z0,2);
fu=sqrt((pow(s1[0],2)+pow(s1[1],2)+pow(s1[2],2))*pow(z0,2)-pow(u0,2));
//求旋转矩阵中的各元素
r31=-s1[8]*z0;
r32=-s1[9]*z0;
r33=-s1[10]*z0;
r11=(r31*u0+s1[0]*z0)/fu;
r12=(r32*u0+s1[1]*z0)/fu;
r13=(r33*u0+s1[2]*z0)/fu;
r21=r13*r32-r12*r33;
r22=r11*r33-r13*r31;
r23=r12*r31-r11*r32;

fv=((s1[5]*r33-s1[6]*r32)*z0)/(r23*r32-r22*r33);
if(fv<0)
{z0=-z0;
//重新求解旋转矩阵各元素
r31=-s1[8]*z0;
r32=-s1[9]*z0;
r33=-s1[10]*z0;
r11=(r31*u0+s1[0]*z0)/fu;
r12=(r32*u0+s1[1]*z0)/fu;
r13=(r33*u0+s1[2]*z0)/fu;
r21=r13*r32-r12*r33;
r22=r11*r33-r13*r31;
r23=r12*r31-r11*r32;
fv=((s1[5]*r33-s1[6]*r32)*z0)/(r23*r32-r22*r33);


}

x0=(u0-s1[3])*z0/fu;
y0=(u0-s1[7])*z0/fv;
//构造旋转矩阵
R[0]=r11;
R[1]=r12;
R[2]=r13;
R[3]=r21;
R[4]=r22;
R[5]=r23;
R[6]=r31;
R[7]=r32;
R[8]=r33;



//计算旋转角
GetRotAngFromVector(R[1],R[4],R[7],R[2],R[5],R[8],WKFi);

//构造先旋转后平移时的平移量矩阵
LL3[0]=x0;
LL3[1]=y0;
LL3[2]=z0;
//求先平移后旋转时的平移量矩阵
adjust.brmul(R,LL3,3,1,3,XYZs);

//将所计算出的相机参数输出
OutPra[0]=XYZs[0];
OutPra[1]=XYZs[1];
OutPra[2]=XYZs[2];
OutPra[3]=WKFi[0];
OutPra[4]=WKFi[1];
OutPra[5]=WKFi[2];
OutPra[6]=u0;
OutPra[7]=v0;
OutPra[8]=fu;
OutPra[9]=fv;


delete A;
delete L;
delete p;
delete s1;
delete s2;




}
/****************利用正弦值计算角度的弧度***************
a----为所求角度的余弦值
b----为所求角度的正弦值
c----存储所求角度的弧度
**********************************************************/
void CalibrateCamera::Soluteangle(double a, double b, double *c)
{
	//1,4象限
	if(b>1)
		b=1;
	if(b<-1)
		b=-1;
	if(a>=0)
		*c=asin(b);  
	//2,3象限	
	if(a<=0)
	  *c=3.1415926-asin(b);

}

/****************AnalyticalPra在DLT标定结果的基础上对包括镜头畸变参数在内
的所有参数再进行一次解析法标定。*****************************************

*****ControlPoint物方点在世界坐标系中的坐标
*****ImagePoint为对应的象点坐标
*****m为标定装置上控制点的个数
*****DLTResult为用DLT方法标定的参数的结果
*****OutPra输出解析法计算出的相机的参数
*/
void CalibrateCamera::AnalyticalPra(double ControlPoint[], double ImagePoint[], int m, double DLTResult[], double OutPra[])
{
// *A---法方程矩阵;*L---常数矩阵;*p---权矩阵
double *A,*L,*P;
// r11,r12,r13,r21,r22,r23,r31,r32,r33---存放旋转角对应的旋转矩阵元素  
double r11,r12,r13,r21,r22,r23,r31,r32,r33;
// W,K,Fi---旋转角度，用于辅助计算
double W,K,Fi;
//fu,fv----考虑尺度因子的焦距，用于辅助计算
double f;
//u0,v0----像主点偏移量，用于辅助计算
double u0,v0;
//Xs,Ys,Zs----相机外参数中的平移参数，用于辅助计算
double  Xs,Ys,Zs;
//k1,k2,k3,p1,p2,p3,b1,b2相机畸变参数，用于辅助计算 
double k1,k2,k3,p1,p2,p3,b1,b2;
//s1用于存放平差后得的结果
double *s1,*s2;
Adjustment adjust;
int i,j,k;
//辅助量xg,yg,zg;
double xg,yg,zg;
//
double r2;
// 开辟存储区域
A=new double[2*m*10];
L=new double[2*m];
P=new double[2*m*2*m];
s1=new double[10];
s2=new double[10];
//初始化为0
for(i=0;i<2*m*11;i++)
	  A[i]=0; 
//给权赋值
for(i=0;i<2*m;i++)
for(j=0;j<2*m;j++)
{
		  if(i==j)
			  P[i*2*m+j]=1;
		  else 
              P[i*2*m+j]=0;
} 

//循环辅开关量max赋值
double max=1;
//给旋转角赋初值
W=DLTResult[3];
K=DLTResult[4];
Fi=DLTResult[5];
12

⌨️ 快捷键说明

复制代码 Ctrl + C
搜索代码 Ctrl + F
全屏模式 F11
切换主题 Ctrl + Shift + D
显示快捷键 ?
增大字号 Ctrl + =
减小字号 Ctrl + -