摄像机RAC两步标定

实现了快速相机标定，方法是利用了RAC标定

#include “stdafx.h“
//#include “LaserMeasureSys.h“
#include “math.h“
#include “biaoding.h“
#include “fstream“
using namespace std;

#ifdef _DEBUG
#define new DEBUG_NEW			  
#undef THIS_FILE
static char THIS_FILE[] = __FILE__;
#endif

double flfrklkrk;        //f1为左相机（96）的聚焦，f2为右相机（90）的焦距
double Tl[3]Tr[3];  //T1为左相机（96）的平移矩阵，T2为右相机（90）的平移矩阵
double Rl[9]Rr[9];  //R1为左相机（96）的旋转矩阵，R2为右相机（90）的平移矩阵

int Read_Data96（double *all_data）
{	
	int i=0;
	ifstream in（“左右像面坐标.txt“）;
	while（in>>all_data[i]）
	{
		i++;
	}
	return i;
}
int Read_Data90（double *all_data）
{	
	int i=0;
	ifstream in（“world1.txt“）;
	while（in>>all_data[i]）
	{
		i++;
	}
	return i;
}
/****************************************************************
函数名:cinv_inv（）;矩阵求逆运算
参  数:a[3][3]待求逆矩阵
       n 矩阵维数
返回值:无返回值，求逆后矩阵返回原矩阵
*****************************************************************/
void cinv_inv （double a[3][3]int n）          //执行求逆运算
{ 
	int *is*jsijk;
	double dp;
	is=new int[n];
	js=new int[n];
	for（k=0;k<=n-1;k++）
	{
		d=0.0;
		for（i=k;i<=n-1;i++）
			for（j=k;j<=n-1;j++）
			{
				p=fabs（a[i][j]）;
				if（p>d）
				{
					d=p;
					is[k]=i;
					js[k]=j;
				}
			}
			if（d+1.0==1.0）
			{
				delete[] isjs;
//				cout<<“\nA为奇异矩阵！没有逆矩阵。“<				exit（1）;
			}
			if（is[k]!=k）
				for（j=0;j<=n-1;j++）
				{
					p=a[k][j];
					a[k][j]=a[is[k]][j];
					a[is[k]][j]=p;
				}
				if（js[k]!=k）
					for（i=0;i<=n-1;i++）
					{
						p=a[i][k];
						a[i][k]=a[i][js[k]];
						a[i][js[k]]=p;
					}
					a[k][k]=1.0/a[k][k];
					for（j=0;j<=n-1;j++）
						if（j!=k）
							a[k][j]=a[k][j]*a[k][k];
						for（i=0;i<=n-1;i++）
							if（i!=k）
								for（j=0;j<=n-1;j++）
									if（j!=k）
										a[i][j]=a[i][j]-a[i][k]*a[k][j];
									for（i=0;i<=n-1;i++）
										if（i!=k）
											a[i][k]=-a[i][k]*a[k][k];
	}
	for（k=n-1;k>=0;k--）
	{
		if（js[k]!=k）
			for（j=0;j<=n-1;j++）
			{
				p=a[k][j];
				a[k][j]=a[js[k]][j];
				a[js[k]][j]=p;
			}
			if（is[k]!=k）
				for（i=0;i<=n-1;i++）
				{
					p=a[i][k];
					a[i][k]=a[i][is[k]];
					a[i][is[k]]=p;
				}
	}
	delete[] isjs;	
}
/****************************************************************
函数名:ppp（），sss（），bmuav（），bginv（），agmiv（）标定过程中参与运算的函数
说  明:这5个函数使用过程中无需改动
*****************************************************************/
void ppp（double *a double *e double *s double *v int m int n）
{
	int ijpq;
    double d;
    if （m>=n） i=n;
    else i=m;
    for （j=1; j<=i-1; j++）
      { a[（j-1）*n+j-1]=s[j-1];
        a[（j-1）*n+j]=e[j-1];
      }
    a[（i-1）*n+i-1]=s[i-1];
    if （m    for （i=1; i<=n-1; i++）
    for （j=i+1; j<=n; j++）
      { p=（i-1）*n+j-1; q=（j-1）*n+i-1;
        d=v[p]; v[p]=v[q]; v[q]=d;
      }

	return;
}

void sss（double *fg double *cs）
{
	double rd;
    if （（fabs（fg[0]）+fabs（fg[1]））==0.0）
      { cs[0]=1.0; cs[1]=0.0; d=0.0;}
    else 
      

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件      21643  2011-09-14 19:30  RAC\biaoding.cpp

     文件        559  2011-09-14 19:28  RAC\biaoding.h

     文件      43386  2011-09-14 19:30  RAC\Debug\biaoding.obj

     文件          0  2011-09-14 19:30  RAC\Debug\biaoding.sbr

     文件      19729  2011-09-07 20:24  RAC\Debug\MainFrm.obj

     文件          0  2011-09-07 20:32  RAC\Debug\MainFrm.sbr

     文件    4121600  2011-09-14 20:55  RAC\Debug\RAC.bsc

     文件     139307  2011-09-14 20:55  RAC\Debug\RAC.exe

     文件     432952  2011-09-14 20:55  RAC\Debug\RAC.ilk

     文件      22597  2011-09-07 20:24  RAC\Debug\RAC.obj

     文件    6997696  2011-09-07 20:24  RAC\Debug\RAC.pch

     文件     467968  2011-09-14 20:55  RAC\Debug\RAC.pdb

     文件       8664  2011-09-07 20:24  RAC\Debug\RAC.res

     文件          0  2011-09-07 20:32  RAC\Debug\RAC.sbr

     文件      14381  2011-09-07 20:24  RAC\Debug\RACDoc.obj

     文件          0  2011-09-07 20:32  RAC\Debug\RACDoc.sbr

     文件      35724  2011-09-14 20:55  RAC\Debug\RACView.obj

     文件          0  2011-09-14 20:55  RAC\Debug\RACView.sbr

     文件     105525  2011-09-07 20:24  RAC\Debug\StdAfx.obj

     文件    1374948  2011-09-07 20:24  RAC\Debug\StdAfx.sbr

     文件     254976  2011-09-16 19:56  RAC\Debug\vc60.idb

     文件     446464  2011-09-14 20:55  RAC\Debug\vc60.pdb

     文件       2504  2011-09-06 18:16  RAC\MainFrm.cpp

     文件       1581  2011-09-06 18:16  RAC\MainFrm.h

     文件      45408  2011-09-07 15:13  RAC\RAC.APS

     文件       2399  2011-09-14 20:57  RAC\RAC.clw

     文件       4155  2011-09-06 18:16  RAC\RAC.cpp

     文件       4757  2011-09-08 15:27  RAC\RAC.dsp

     文件        529  2011-09-06 18:16  RAC\RAC.dsw

     文件       1323  2011-09-06 18:16  RAC\RAC.h

............此处省略28个文件信息