void loadDataTable( std::vector< std::vector<T> >& dt, const std::string& infile, const char& sep = ',', const char& quote = '"' ) { std::cout << "loading data" << std::endl << std::endl; std::ifstream matrixFile(infile.c_str()); if (!matrixFile) { std::cout << "not exists..." << std::endl; return; } dt.reserve(100000); utility::CSVIterator<T> matrixLine(matrixFile); for( ; matrixLine != utility::CSVIterator<T>(); ++matrixLine ) { std::vector<T> row(matrixLine->size(), 0); for (unsigned i = 0; i < matrixLine->size(); ++i) { row[i] = matrixLine->at(i); } dt.push_back(row); } dt.resize(dt.size()); size_t ncols = dt.empty() ? 0 : dt[0].size(); std::cout << "done loading matrix of (" << dt.size() << "," << ncols << ")" << std::endl << std::endl; }
int main(void) { /* Create the linear system: */ Math::Matrix a(12,12,0.0); { /* Open the calibration data file: */ Misc::FileCharacterSource dataSource("CalibrationData.csv"); Misc::CSVSource data(dataSource); unsigned int numEntries=0; while(!data.eof()) { /* Read a calibration entry from the data file: */ double x=data.readField<double>(); double y=data.readField<double>(); double z=data.readField<double>(); double s=data.readField<double>()/640.0; double t=data.readField<double>()/480.0; /* Insert the entry's two linear equations into the linear system: */ double eq[2][12]; eq[0][0]=x; eq[0][1]=y; eq[0][2]=z; 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]=-s*x; eq[0][9]=-s*y; eq[0][10]=-s*z; eq[0][11]=-s; eq[1][0]=0.0; eq[1][1]=0.0; eq[1][2]=0.0; eq[1][3]=0.0; eq[1][4]=x; eq[1][5]=y; eq[1][6]=z; eq[1][7]=1.0; eq[1][8]=-t*x; eq[1][9]=-t*y; eq[1][10]=-t*z; eq[1][11]=-t; for(int row=0;row<2;++row) { for(unsigned int i=0;i<12;++i) for(unsigned int j=0;j<12;++j) a.set(i,j,a(i,j)+eq[row][i]*eq[row][j]); } ++numEntries; } std::cout<<numEntries<<" calibration data entries read from file"<<std::endl; } /* Find the linear system's smallest eigenvalue: */ std::pair<Math::Matrix,Math::Matrix> qe=a.jacobiIteration(); unsigned int minEIndex=0; double minE=Math::abs(qe.second(0,0)); for(unsigned int i=1;i<12;++i) { if(minE>Math::abs(qe.second(i,0))) { minEIndex=i; minE=Math::abs(qe.second(i,0)); } } /* Create the normalized homography: */ Math::Matrix hom(3,4); double scale=qe.first(11,minEIndex); for(int i=0;i<3;++i) for(int j=0;j<4;++j) hom.set(i,j,qe.first(i*4+j,minEIndex)/scale); { /* Open the calibration data file: */ Misc::FileCharacterSource dataSource("CalibrationData.csv"); Misc::CSVSource data(dataSource); /* Test the homography on all calibration data entries: */ while(!data.eof()) { /* Read a calibration entry from the data file: */ Math::Matrix world(4,1); for(unsigned int i=0;i<3;++i) world.set(i,data.readField<double>()); world.set(3,1.0); double s=data.readField<double>(); double t=data.readField<double>(); /* Apply the homography: */ Math::Matrix str=hom*world; std::cout<<"Result: s = "<<str(0)/str(2)<<", t = "<<str(1)/str(2)<<std::endl; } } /* Open the calibration file: */ Misc::File matrixFile("CameraCalibrationMatrices.dat","wb",Misc::File::LittleEndian); /* Create the depth projection matrix: */ Math::Matrix depthProjection(4,4,0.0); double cameraFov=560.0; depthProjection(0,0)=1.0/cameraFov; depthProjection(0,3)=-320.0/cameraFov; depthProjection(1,1)=1.0/cameraFov; depthProjection(1,3)=-240.0/cameraFov; depthProjection(2,3)=-1.0; depthProjection(3,2)=-1.0/34400.0; depthProjection(3,3)=1090.0/34400.0; /* Save the depth projection matrix: */ for(unsigned int i=0;i<4;++i) for(unsigned int j=0;j<4;++j) matrixFile.write<double>(depthProjection(i,j)); /* Create the color projection matrix by extending the homography: */ Math::Matrix colorProjection(4,4); for(unsigned int i=0;i<2;++i) for(unsigned int j=0;j<4;++j) colorProjection(i,j)=hom(i,j); for(unsigned int j=0;j<4;++j) colorProjection(2,j)=j==2?1.0:0.0; for(unsigned int j=0;j<4;++j) colorProjection(3,j)=hom(2,j); /* Modify the color projection matrix by the depth projection matrix: */ colorProjection*=depthProjection; /* Save the color projection matrix: */ for(unsigned int i=0;i<4;++i) for(unsigned int j=0;j<4;++j) matrixFile.write<double>(colorProjection(i,j)); return 0; }