int Scene::parseScene(char* file) { char trash[256]; std::ifstream scn; scn.open(file); if(scn.fail()) { printf("File could not be opened: %s\n", file); return -1; } Material curMat(Vector::zero(), Vector::one(), Vector::zero(), Vector::zero(), 5, 1); std::string str; while(scn >> str) { if(str == "" || str[0] == '#') { scn.getline(trash, 256); continue; } else if(str == "camera") { float x, y, z, dx, dy, dz, ux, uy, uz, ha; scn >> x >> y >> z >> dx >> dy >> dz >> ux >> uy >> uz >> ha; camera.setPosition(Vector(x, y, z)); camera.setDirection(Vector(dx, dy, dz)); camera.setUpDirection(Vector(ux, uy, uz)); camera.setHalfFOV(ha*DEGTORAD); } else if(str == "film_resolution") {
void AbsoluteOrientation::compute( std::vector<Eigen::Vector3d> &left, std::vector<Eigen::Vector3d> &right, Eigen::Quaterniond &result ) { int i, pairNum = left.size(); Eigen::MatrixXd muLmuR = Eigen::MatrixXd::Zero(3,3), M = Eigen::MatrixXd::Zero(3,3), curMat = Eigen::MatrixXd::Zero(3,3), N = Eigen::MatrixXd::Zero(4,4); Eigen::Vector3d meanFirst(0,0,0), meanSecond(0,0,0); //assume points set to zero by constructor //compute the mean of both point sets for (i=0; i<pairNum; i++) { meanFirst[0] += left[i][0]; meanFirst[1] += left[i][1]; meanFirst[2] += left[i][2]; meanSecond[0] += right[i][0]; meanSecond[1] += right[i][1]; meanSecond[2] += right[i][2]; } meanFirst[0]/=pairNum; meanFirst[1]/=pairNum; meanFirst[2]/=pairNum; meanSecond[0]/=pairNum; meanSecond[1]/=pairNum; meanSecond[2]/=pairNum; //compute the matrix muLmuR muLmuR(0,0) = meanFirst[0]*meanSecond[0]; muLmuR(0,1) = meanFirst[0]*meanSecond[1]; muLmuR(0,2) = meanFirst[0]*meanSecond[2]; muLmuR(1,0) = meanFirst[1]*meanSecond[0]; muLmuR(1,1) = meanFirst[1]*meanSecond[1]; muLmuR(1,2) = meanFirst[1]*meanSecond[2]; muLmuR(2,0) = meanFirst[2]*meanSecond[0]; muLmuR(2,1) = meanFirst[2]*meanSecond[1]; muLmuR(2,2) = meanFirst[2]*meanSecond[2]; //compute the matrix M for (i=0; i<pairNum; i++) { Eigen::Vector3d &leftPoint = left[i]; Eigen::Vector3d &rightPoint = right[i]; curMat(0,0) = leftPoint[0]*rightPoint[0]; curMat(0,1) = leftPoint[0]*rightPoint[1]; curMat(0,2) = leftPoint[0]*rightPoint[2]; curMat(1,0) = leftPoint[1]*rightPoint[0]; curMat(1,1) = leftPoint[1]*rightPoint[1]; curMat(1,2) = leftPoint[1]*rightPoint[2]; curMat(2,0) = leftPoint[2]*rightPoint[0]; curMat(2,1) = leftPoint[2]*rightPoint[1]; curMat(2,2) = leftPoint[2]*rightPoint[2]; M+=curMat; } M+= (muLmuR *(-pairNum)); //compute the matrix N Eigen::MatrixXd tmpMat = Eigen::MatrixXd::Zero(3,3); double A12, A20, A01; double traceM = 0.0; for(i=0; i<3; i++) traceM += M(i,i); tmpMat.diagonal() = Eigen::VectorXd::Constant(3, -traceM); //tmpMat.fill_diagonal(-traceM); tmpMat += (M + M.transpose()); A12 = M(1,2) - M(2,1); A20 = M(2,0) - M(0,2); A01 = M(0,1) - M(1,0); N(0,0)=traceM; N(0,1)=A12; N(0,2)=A20; N(0,3)=A01; N(1,0)=A12; N(2,0)=A20; N(3,0)=A01; N.bottomRightCorner(3,3) = tmpMat; //N.update(tmpMat,1,1); ////find the eigenvector that belongs to the maximal ////eigenvalue of N, eigenvalues are sorted from smallest to largest //vnl_symmetric_eigensystem<double> eigenSystem(N); Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es; es.compute(N); Eigen::MatrixXd V = es.eigenvectors(); //std::stringstream ss;ss << V; //qDebug() << qPrintable(ss.str().c_str()); //setRotationQuaternion(eigenSystem.V(0,3),eigenSystem.V(1,3),eigenSystem.V(2,3),eigenSystem.V(3,3), true); result = Eigen::Quaterniond( V(0,3),V(1,3),V(2,3),V(3,3) ).normalized(); }