void SensorBrowseControl::setCalibrationFilename(const QString& filename) {
mUi->calibrationEdit->setText(filename);
const QFileInfo fileInfo(filename);
if (fileInfo.isFile() && fileInfo.isReadable()) {
std::ifstream calibFile(filename.toStdString());
calibFile >> mCalibration;
}
void calibrate(const std::vector<TiePoint>& tiePoints,const int gridSize[2],const double tileSize[2],const unsigned int depthFrameSize[2],const unsigned int colorFrameSize[2])
{
/* Initialize the depth camera's intrinsic parameter matrix: */
Math::Matrix depthV(6,6,0.0);
/* Process all tie points: */
for(std::vector<TiePoint>::const_iterator tpIt=tiePoints.begin(); tpIt!=tiePoints.end(); ++tpIt)
{
/* Enter the tie point's depth homography into the intrinsic parameter matrix: */
Homography::Matrix hm(1.0);
hm(0,2)=double(depthFrameSize[0]);
hm*=tpIt->depthHom.getMatrix();
Homography::Matrix scale(1.0);
scale(0,0)=1.0/tileSize[0];
scale(1,1)=1.0/tileSize[1];
hm*=scale;
double row[3][6];
static const int is[3]= {0,0,1};
static const int js[3]= {1,0,1};
for(int r=0; r<3; ++r)
{
int i=is[r];
int j=js[r];
row[r][0]=hm(0,i)*hm(0,j);
row[r][1]=hm(0,i)*hm(1,j)+hm(1,i)*hm(0,j);
row[r][2]=hm(0,i)*hm(2,j)+hm(2,i)*hm(0,j);
row[r][3]=hm(1,i)*hm(1,j);
row[r][4]=hm(1,i)*hm(2,j)+hm(2,i)*hm(1,j);
row[r][5]=hm(2,i)*hm(2,j);
}
for(int i=0; i<6; ++i)
row[1][i]-=row[2][i];
for(int r=0; r<2; ++r)
{
for(unsigned int i=0; i<6; ++i)
for(unsigned int j=0; j<6; ++j)
depthV(i,j)+=row[r][i]*row[r][j];
}
}
/* Find the intrinsic parameter linear system's smallest eigenvalue: */
std::pair<Math::Matrix,Math::Matrix> depthQe=depthV.jacobiIteration();
unsigned int minEIndex=0;
double minE=Math::abs(depthQe.second(0));
for(unsigned int i=1; i<6; ++i)
{
if(minE>Math::abs(depthQe.second(i)))
{
minEIndex=i;
minE=Math::abs(depthQe.second(i));
}
}
std::cout<<"Smallest eigenvalue of v = "<<depthQe.second(minEIndex)<<std::endl;
/* Calculate the intrinsic parameters: */
Math::Matrix b=depthQe.first.getColumn(minEIndex);
std::cout<<b(0)<<", "<<b(1)<<", "<<b(2)<<", "<<b(3)<<", "<<b(4)<<", "<<b(5)<<std::endl;
double v0=(b(1)*b(2)-b(0)*b(4))/(b(0)*b(3)-Math::sqr(b(1)));
double lambda=b(5)-(Math::sqr(b(2))+v0*(b(1)*b(2)-b(0)*b(4)))/b(0);
double alpha=Math::sqrt(lambda/b(0));
double beta=Math::sqrt(lambda*b(0)/(b(0)*b(3)-Math::sqr(b(1))));
double gamma=-b(1)*Math::sqr(alpha)*beta/lambda;
double u0=gamma*v0/beta-b(2)*Math::sqr(alpha)/lambda;
std::cout<<"Intrinsic camera parameters:"<<std::endl;
std::cout<<alpha<<" "<<gamma<<" "<<u0<<std::endl;
std::cout<<0.0<<" "<<beta<<" "<<v0<<std::endl;
std::cout<<0.0<<" "<<0.0<<" "<<1.0<<std::endl;
/* Create the intrinsic camera parameter matrix: */
Math::Matrix a(3,3,1.0);
a.set(0,0,alpha);
a.set(0,1,gamma);
a.set(0,2,u0);
a.set(1,1,beta);
a.set(1,2,v0);
Math::Matrix aInv=a.inverse();
/* Calculate extrinsic parameters for each tie point to get measurements for the depth formula regression: */
Math::Matrix depthAta(2,2,0.0);
Math::Matrix depthAtb(2,1,0.0);
for(std::vector<TiePoint>::const_iterator tpIt=tiePoints.begin(); tpIt!=tiePoints.end(); ++tpIt)
{
/* Convert the tie point's depth homography to a matrix: */
Homography::Matrix hm(1.0);
hm(0,2)=double(depthFrameSize[0]);
hm*=tpIt->depthHom.getMatrix();
Homography::Matrix scale(1.0);
scale(0,0)=1.0/tileSize[0];
scale(1,1)=1.0/tileSize[1];
hm*=scale;
Math::Matrix h(3,3);
for(unsigned int i=0; i<3; ++i)
for(unsigned int j=0; j<3; ++j)
h(i,j)=hm(i,j);
/* Calculate the extrinsic parameters: */
double lambda=0.5/(aInv*h.getColumn(0)).mag()+0.5/(aInv*h.getColumn(1)).mag();
Math::Matrix r1=lambda*aInv*h.getColumn(0);
Math::Matrix r2=lambda*aInv*h.getColumn(1);
Math::Matrix r3(3,1);
for(unsigned int i=0; i<3; ++i)
r3.set(i,r1((i+1)%3)*r2((i+2)%3)-r1((i+2)%3)*r2((i+1)%3)); // 'Tis a cross product, in case you're wondering
Math::Matrix t=lambda*aInv*h.getColumn(2);
/* Create the extrinsic parameter matrix: */
Math::Matrix rt(3,4);
rt.setColumn(0,r1);
rt.setColumn(1,r2);
rt.setColumn(2,r3);
rt.setColumn(3,t);
Math::Matrix wgc(4,1);
wgc(0)=tileSize[0]*double(gridSize[0])*0.5;
wgc(1)=tileSize[1]*double(gridSize[1])*0.5;
wgc(2)=0.0;
wgc(3)=1.0;
Math::Matrix cgc=rt*wgc;
if(cgc(2)<0.0)
{
/* Flip the extrinsic matrix to move the grid to positive z: */
Math::Matrix flip(3,3,-1.0);
rt=flip*rt;
cgc=rt*wgc;
}
std::cout<<"Grid center: "<<cgc(0)<<", "<<cgc(1)<<", "<<cgc(2)<<std::endl;
/* Transform all world grid points with the extrinsic matrix to get their camera-space z values: */
for(int y=0; y<gridSize[1]; ++y)
for(int x=0; x<gridSize[0]; ++x)
{
/* Create the world point: */
Math::Matrix wp(4,1);
wp(0)=tileSize[0]*double(x);
wp(1)=tileSize[1]*double(y);
wp(2)=0.0;
wp(3)=1.0;
/* Transform the world point to camera space: */
Math::Matrix cp=rt*wp;
double dist=cp(2);
/* Get the depth frame value from the grid's plane in depth image space: */
Point dip=tpIt->depthHom.transform(Point(x,y));
const Plane::Vector& n=tpIt->gridPlane.getNormal();
double o=tpIt->gridPlane.getOffset();
double depth=(o-dip[0]*n[0]-dip[1]*n[1])/n[2];
/* Enter the depth / z pair into the depth formula accumulator: */
depthAta(0,0)+=1.0;
depthAta(0,1)+=-dist;
depthAta(1,0)+=-dist;
depthAta(1,1)+=dist*dist;
depthAtb(0)+=-dist*depth;
depthAtb(1)+=dist*dist*depth;
}
}
/* Solve the depth formula least-squares system: */
Math::Matrix depthX=depthAtb.divideFullPivot(depthAta);
std::cout<<"Depth conversion formula: dist = "<<depthX(0)<<" / ("<<depthX(1)<<" - depth)"<<std::endl;
/* Calculate the full depth unprojection matrix: */
Math::Matrix depthProj(4,4,0.0);
depthProj(0,0)=1.0/alpha;
depthProj(0,1)=-gamma/(alpha*beta);
depthProj(0,3)=-u0/alpha+v0*gamma/(alpha*beta);
depthProj(1,1)=1.0/beta;
depthProj(1,3)=-v0/beta;
depthProj(2,3)=-1.0;
depthProj(3,2)=-1.0/depthX(0);
depthProj(3,3)=depthX(1)/depthX(0);
Math::Matrix colorAta(12,12,0.0);
for(std::vector<TiePoint>::const_iterator tpIt=tiePoints.begin(); tpIt!=tiePoints.end(); ++tpIt)
{
for(int y=0; y<gridSize[1]; ++y)
for(int x=0; x<gridSize[0]; ++x)
{
/* Enter the tie point into the color calibration matrix linear system: */
Point dip=tpIt->depthHom.transform(Point(x,y));
const Plane::Vector& n=tpIt->gridPlane.getNormal();
double o=tpIt->gridPlane.getOffset();
double depth=(o-dip[0]*n[0]-dip[1]*n[1])/n[2];
Math::Matrix dwp(4,1);
dwp(0)=dip[0]+double(depthFrameSize[0]);
dwp(1)=dip[1];
dwp(2)=depth;
dwp(3)=1.0;
dwp=depthProj*dwp;
for(int i=0; i<3; ++i)
dwp(i)/=dwp(3);
Point cip=tpIt->colorHom.transform(Point(x,y));
cip[0]/=double(colorFrameSize[0]);
cip[1]/=double(colorFrameSize[1]);
double eq[2][12];
eq[0][0]=dwp(0);
eq[0][1]=dwp(1);
eq[0][2]=dwp(2);
eq[0][3]=1.0;
eq[0][4]=0.0;
eq[0][5]=0.0;
eq[0][6]=0.0;
eq[0][7]=0.0;
eq[0][8]=-cip[0]*dwp(0);
eq[0][9]=-cip[0]*dwp(1);
eq[0][10]=-cip[0]*dwp(2);
eq[0][11]=-cip[0];
eq[1][0]=0.0;
eq[1][1]=0.0;
eq[1][2]=0.0;
eq[1][3]=0.0;
eq[1][4]=dwp(0);
eq[1][5]=dwp(1);
eq[1][6]=dwp(2);
eq[1][7]=1.0;
eq[1][8]=-cip[1]*dwp(0);
eq[1][9]=-cip[1]*dwp(1);
eq[1][10]=-cip[1]*dwp(2);
eq[1][11]=-cip[1];
for(int row=0; row<2; ++row)
{
for(unsigned int i=0; i<12; ++i)
for(unsigned int j=0; j<12; ++j)
colorAta(i,j)+=eq[row][i]*eq[row][j];
}
}
}
/* Find the color calibration system's smallest eigenvalue: */
std::pair<Math::Matrix,Math::Matrix> colorQe=colorAta.jacobiIteration();
minEIndex=0;
minE=Math::abs(colorQe.second(0,0));
for(unsigned int i=1; i<12; ++i)
{
if(minE>Math::abs(colorQe.second(i,0)))
{
minEIndex=i;
minE=Math::abs(colorQe.second(i,0));
}
}
/* Create the normalized color homography: */
Math::Matrix colorHom(3,4);
double scale=colorQe.first(11,minEIndex);
for(int i=0; i<3; ++i)
for(int j=0; j<4; ++j)
colorHom(i,j)=colorQe.first(i*4+j,minEIndex)/scale;
/* Create the full color unprojection matrix by extending the homography: */
Math::Matrix colorProj(4,4);
for(unsigned int i=0; i<2; ++i)
for(unsigned int j=0; j<4; ++j)
colorProj(i,j)=colorHom(i,j);
for(unsigned int j=0; j<4; ++j)
colorProj(2,j)=j==2?1.0:0.0;
for(unsigned int j=0; j<4; ++j)
colorProj(3,j)=colorHom(2,j);
/* Modify the color unprojection matrix by the depth projection matrix: */
colorProj*=depthProj;
/* Write the calibration file: */
IO::FilePtr calibFile(IO::openFile("CameraCalibrationMatrices.dat",IO::File::WriteOnly));
calibFile->setEndianness(Misc::LittleEndian);
for(int i=0; i<4; ++i)
for(int j=0; j<4; ++j)
calibFile->write<double>(depthProj(i,j));
for(int i=0; i<4; ++i)
for(int j=0; j<4; ++j)
calibFile->write<double>(colorProj(i,j));
}
