// The main mex-function void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] ) { // Check if right number of arguments if( nrhs < 3 || nrhs > 5 ) { mexPrintf("opengv: Not an acceptable number of arguments\n"); mexPrintf("Usage: X = opengv( method, data1, data2 )\n"); mexPrintf("Or: X = opengv( method, indices, data1, data2 )\n"); mexPrintf("Or: X = opengv( method, indices, data1, data2, prior )\n"); return; } // Get the method if( mxGetM(prhs[0]) != 1 ) { mexPrintf("opengv: Bad input to mex function opengv\n"); mexPrintf("Usage: X = opengv( method, data1, data2 )\n"); mexPrintf("Or: X = opengv( method, indices, data1, data2 )\n"); mexPrintf("Or: X = opengv( method, indices, data1, data2, prior )\n"); mexPrintf("Hint: Method must be a string\n"); return; } // Now get the string and find the caseNumber mwSize strlen = (mwSize) mxGetN(prhs[0]) + 1; char * method = (char *) malloc(strlen); mxGetString(prhs[0], method, strlen); int caseNumber = findCase(method, (int) mxGetN(prhs[0])); // Return if method not found if( caseNumber < 0 ) { mexPrintf("opengv: Unknown method\n"); printCases(); return; } // Characterize the type of the call int callCharacter = -1; const mxArray *data1; const mxArray *data2; const mwSize *data1dim; const mwSize *data2dim; if( nrhs == 3 ) // X = opengv( method, data1, data2 ) { // Check the input data1 = prhs[1]; data2 = prhs[2]; // Check the dimensions of the arguments int ndimensions1 = mxGetNumberOfDimensions(data1); int ndimensions2 = mxGetNumberOfDimensions(data2); data1dim = mxGetDimensions(data1); data2dim = mxGetDimensions(data2); // Now check them if( ndimensions1 != 2 || ndimensions2 != 2 || (data1dim[0] != 3 && data1dim[0] != 6) || (data2dim[0] != 3 && data2dim[0] != 6) || data1dim[1] != data2dim[1] || data1dim[1] < 1 || data2dim[1] < 1 ) { mexPrintf("opengv: Bad input to mex function\n"); mexPrintf("Assuming signature: X = opengv( method, data1, data2 )\n"); mexPrintf("Inputs data1 and data2 must have size (3,n) or (6,n),\n"); mexPrintf("with an equal number of columns\n"); return; } callCharacter = 0; } if( nrhs == 4 ) { // X = opengv( method, indices, data1, data2 ) // Check the input data1 = prhs[2]; data2 = prhs[3]; // Check the dimensions of the arguments int ndimensions1 = mxGetNumberOfDimensions(data1); int ndimensions2 = mxGetNumberOfDimensions(data2); int ndimensions3 = mxGetNumberOfDimensions(prhs[1]); data1dim = mxGetDimensions(data1); data2dim = mxGetDimensions(data2); const mwSize *indicesDim = mxGetDimensions(prhs[1]); // Now check them if( ndimensions1 != 2 || ndimensions2 != 2 || ndimensions3 != 2 || (data1dim[0] != 3 && data1dim[0] != 6) || (data2dim[0] != 3 && data2dim[0] != 6) || indicesDim[0] != 1 || data1dim[1] != data2dim[1] || data1dim[1] < 1 || data2dim[1] < 1 || data2dim[1] < indicesDim[1] ) { mexPrintf("opengv: Bad input to mex function opengv\n"); mexPrintf("Assuming signature: X = opengv( method, indices, data1, "); mexPrintf("data2 )\n"); mexPrintf("Inputs data1 and data2 must have size (3,n) or (6,n),\n"); mexPrintf("with an equal number of columns\n"); mexPrintf("indices must be a 1xm vector, with m smaller or equal than n\n"); return; } callCharacter = 1; } if(nrhs == 5) { // X = opengv( method, indices, data1, data2, prior ) // Check the input data1 = prhs[2]; data2 = prhs[3]; // Check the dimensions of the arguments int ndimensions1 = mxGetNumberOfDimensions(data1); int ndimensions2 = mxGetNumberOfDimensions(data2); int ndimensions3 = mxGetNumberOfDimensions(prhs[1]); int ndimensions4 = mxGetNumberOfDimensions(prhs[4]); data1dim = mxGetDimensions(data1); data2dim = mxGetDimensions(data2); const mwSize *indicesDim = mxGetDimensions(prhs[1]); const mwSize *priorDim = mxGetDimensions(prhs[4]); // Now check them if( ndimensions1 != 2 || ndimensions2 != 2 || ndimensions3 != 2 || ndimensions4 != 2 || (data1dim[0] != 3 && data1dim[0] != 6) || (data2dim[0] != 3 && data2dim[0] != 6) || indicesDim[0] != 1 || priorDim[0] != 3 || (priorDim[1] != 1 && priorDim[1] != 3 && priorDim[1] != 4) || data1dim[1] != data2dim[1] || data1dim[1] < 1 || data2dim[1] < 1 || data2dim[1] < indicesDim[1] ) { mexPrintf("opengv: Bad input to mex function opengv\n"); mexPrintf("Assuming signature: X = opengv( method, indices, data1, "); mexPrintf("data2, prior )\n"); mexPrintf("Inputs data1 and data2 must have size (3,n) or (6,n),\n"); mexPrintf("with an equal number of columns\n"); mexPrintf("indices must be a 1xm vector, with m smaller or equal than n\n"); mexPrintf("prior must be a 3x1, 3x3, or 3x4 matrix\n"); return; } callCharacter = 2; } //create three pointers to absolute, relative, and point_cloud adapters here opengv::absolute_pose::AbsoluteAdapterBase* absoluteAdapter; opengv::relative_pose::RelativeAdapterBase* relativeAdapter; opengv::point_cloud::PointCloudAdapterBase* pointCloudAdapter; int translationPrior = 0; int rotationPrior = 0; opengv::translation_t translation; opengv::rotation_t rotation; //set the prior if needed if( callCharacter == 2 ) { const mxArray *prior; const mwSize *priorDim; prior = prhs[4]; priorDim = mxGetDimensions(prhs[4]); if( priorDim[1] == 1 ) { //set translation translationPrior = 1; double * ptr = (double*) mxGetData(prior); translation[0] = ptr[0]; translation[1] = ptr[1]; translation[2] = ptr[2]; } if( priorDim[1] == 3 ) { //set rotation rotationPrior = 1; double * ptr = (double*) mxGetData(prior); rotation(0,0) = ptr[0]; rotation(1,0) = ptr[1]; rotation(2,0) = ptr[2]; rotation(0,1) = ptr[3]; rotation(1,1) = ptr[4]; rotation(2,1) = ptr[5]; rotation(0,2) = ptr[6]; rotation(1,2) = ptr[7]; rotation(2,2) = ptr[8]; } if( priorDim[1] == 4 ) { translationPrior = 1; rotationPrior = 1; double * ptr = (double*) mxGetData(prior); rotation(0,0) = ptr[0]; rotation(1,0) = ptr[1]; rotation(2,0) = ptr[2]; rotation(0,1) = ptr[3]; rotation(1,1) = ptr[4]; rotation(2,1) = ptr[5]; rotation(0,2) = ptr[6]; rotation(1,2) = ptr[7]; rotation(2,2) = ptr[8]; translation[0] = ptr[9]; translation[1] = ptr[10]; translation[2] = ptr[11]; } } if( caseNumber >= absCentralFirst && caseNumber <= absCentralLast ) { //central absolute case if( data1dim[0] != 3 || data2dim[0] != 3 ) { mexPrintf("opengv: Bad input to mex function opengv\n"); mexPrintf("Assuming method: "); mexPrintf(methods[caseNumber]); mexPrintf("\n"); mexPrintf("Inputs data1 and data2 must have size (3,n) for a central "); mexPrintf("absolute method\n"); return; } absoluteAdapter = new opengv::absolute_pose::MACentralAbsolute( (double*) mxGetData(data1), (double*) mxGetData(data2), data1dim[1], data2dim[1] ); if( translationPrior == 1 ) absoluteAdapter->sett(translation); if( rotationPrior == 1 ) absoluteAdapter->setR(rotation); } if(caseNumber >= absNoncentralFirst && caseNumber <= absNoncentralLast ) { //non-central absolute case if( data1dim[0] != 3 || data2dim[0] != 6 ) { mexPrintf("opengv: Bad input to mex function opengv\n"); mexPrintf("Assuming method: "); mexPrintf(methods[caseNumber]); mexPrintf("\n"); mexPrintf("Inputs data1 and data2 must have sizes (3,n) and (6,n) for "); mexPrintf("a noncentral absolute method\n"); return; } absoluteAdapter = new opengv::absolute_pose::MANoncentralAbsolute( (double*) mxGetData(data1), (double*) mxGetData(data2), data1dim[1], data2dim[1] ); if( translationPrior == 1 ) absoluteAdapter->sett(translation); if( rotationPrior == 1 ) absoluteAdapter->setR(rotation); } if(caseNumber >= relCentralFirst && caseNumber <= relCentralLast ) { //central relative case if( data1dim[0] != 3 || data2dim[0] != 3 ) { mexPrintf("opengv: Bad input to mex function opengv\n"); mexPrintf("Assuming method: "); mexPrintf(methods[caseNumber]); mexPrintf("\n"); mexPrintf("Inputs data1 and data2 must have size (3,n) for a central "); mexPrintf("relative method\n"); return; } relativeAdapter = new opengv::relative_pose::MACentralRelative( (double*) mxGetData(data1), (double*) mxGetData(data2), data1dim[1], data2dim[1] ); if( translationPrior == 1 ) relativeAdapter->sett12(translation); if( rotationPrior == 1 ) relativeAdapter->setR12(rotation); } if(caseNumber >= relNoncentralFirst && caseNumber <= relNoncentralLast ) { //noncentral relative case if( data1dim[0] != 6 || data2dim[0] != 6 ) { mexPrintf("opengv: Bad input to mex function opengv\n"); mexPrintf("Assuming method: "); mexPrintf(methods[caseNumber]); mexPrintf("\n"); mexPrintf("Inputs data1 and data2 must have size (6,n) for a "); mexPrintf("noncentral relative method\n"); return; } relativeAdapter = new opengv::relative_pose::MANoncentralRelative( (double*) mxGetData(data1), (double*) mxGetData(data2), data1dim[1], data2dim[1] ); if( translationPrior == 1 ) relativeAdapter->sett12(translation); if( rotationPrior == 1 ) relativeAdapter->setR12(rotation); } if(caseNumber >= pointCloudFirst && caseNumber <= pointCloudLast ) { //point-cloud case if( data1dim[0] != 3 || data2dim[0] != 3 ) { mexPrintf("opengv: Bad input to mex function opengv\n"); mexPrintf("Assuming method: "); mexPrintf(methods[caseNumber]); mexPrintf("\n"); mexPrintf("Inputs data1 and data2 must have size (3,n) for a "); mexPrintf("point-cloud method\n"); return; } pointCloudAdapter = new opengv::point_cloud::MAPointCloud( (double*) mxGetData(data1), (double*) mxGetData(data2), data1dim[1], data2dim[1] ); if( translationPrior == 1 ) pointCloudAdapter->sett12(translation); if( rotationPrior == 1 ) pointCloudAdapter->setR12(rotation); } //check if a return argument is needed, otherwise we won't start computing if( nlhs != 1 ) { if( nlhs > 1 ) mexPrintf("opengv: Returns one parameter only\n"); return; } //create the indices array (todo: check if there is a smarter way for doing this) std::vector<int> indices; int useIndices = 0; if( callCharacter > 0 ) { useIndices = 1; const mwSize *indicesDim = mxGetDimensions(prhs[1]); int numberOfIndices = indicesDim[1]; indices.reserve(numberOfIndices); double * mxIndices = (double*) mxGetData(prhs[1]); for( int i = 0; i < numberOfIndices; i++ ) indices.push_back(floor(mxIndices[i]+0.01)-1); } Method methodEnum = static_cast<Method>(caseNumber); if( caseNumber != (int) methodEnum ) { mexPrintf("opengv: This method is not yet implemented!\n"); return; } // Finally, call the respective algorithm switch (methodEnum) { case P2P: { opengv::translation_t temp; if(useIndices) temp = opengv::absolute_pose::p2p(*absoluteAdapter,indices); else temp = opengv::absolute_pose::p2p(*absoluteAdapter); int dims[2]; dims[0] = 3; dims[1] = 1; plhs[0] = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL); memcpy(mxGetData(plhs[0]), temp.data(), 3*sizeof(double)); break; } case P3P_KNEIP: { opengv::transformations_t temp; if(useIndices) temp = opengv::absolute_pose::p3p_kneip(*absoluteAdapter,indices); else temp = opengv::absolute_pose::p3p_kneip(*absoluteAdapter); int dims[3]; dims[0] = 3; dims[1] = 4; dims[2] = temp.size(); plhs[0] = mxCreateNumericArray(3, dims, mxDOUBLE_CLASS, mxREAL); for( int i = 0; i < temp.size(); i++ ) { void * targetAddress = ((char*) mxGetData(plhs[0])) + i*12*sizeof(double); memcpy(targetAddress, temp[i].data(), 12*sizeof(double)); } break; } case P3P_GAO: { opengv::transformations_t temp; if(useIndices) temp = opengv::absolute_pose::p3p_gao(*absoluteAdapter,indices); else temp = opengv::absolute_pose::p3p_gao(*absoluteAdapter); int dims[3]; dims[0] = 3; dims[1] = 4; dims[2] = temp.size(); plhs[0] = mxCreateNumericArray(3, dims, mxDOUBLE_CLASS, mxREAL); for( int i = 0; i < temp.size(); i++ ) { void * targetAddress = ((char*) mxGetData(plhs[0])) + i*12*sizeof(double); memcpy(targetAddress, temp[i].data(), 12*sizeof(double)); } break; } case EPNP: { opengv::transformation_t temp; if(useIndices) temp = opengv::absolute_pose::epnp(*absoluteAdapter,indices); else temp = opengv::absolute_pose::epnp(*absoluteAdapter); int dims[2]; dims[0] = 3; dims[1] = 4; plhs[0] = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL); memcpy(mxGetData(plhs[0]), temp.data(), 12*sizeof(double)); break; } case P3P_KNEIP_RANSAC: { absRansacPtr problem; if(useIndices) problem = absRansacPtr( new absRansac( *absoluteAdapter, absRansac::KNEIP, indices ) ); else problem = absRansacPtr( new absRansac( *absoluteAdapter, absRansac::KNEIP ) ); opengv::sac::Ransac<absRansac> ransac; ransac.sac_model_ = problem; ransac.threshold_ = 1.0 - cos(atan(sqrt(2.0)*0.5/800.0)); ransac.max_iterations_ = 50; ransac.computeModel(); int dims[2]; dims[0] = 3; dims[1] = 4; plhs[0] = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL); memcpy(mxGetData(plhs[0]), ransac.model_coefficients_.data(), 12*sizeof(double)); break; } case P3P_GAO_RANSAC: { absRansacPtr problem; if(useIndices) problem = absRansacPtr( new absRansac( *absoluteAdapter, absRansac::GAO, indices ) ); else problem = absRansacPtr( new absRansac( *absoluteAdapter, absRansac::GAO ) ); opengv::sac::Ransac<absRansac> ransac; ransac.sac_model_ = problem; ransac.threshold_ = 1.0 - cos(atan(sqrt(2.0)*0.5/800.0)); ransac.max_iterations_ = 50; ransac.computeModel(); int dims[2]; dims[0] = 3; dims[1] = 4; plhs[0] = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL); memcpy(mxGetData(plhs[0]), ransac.model_coefficients_.data(), 12*sizeof(double)); break; } case EPNP_RANSAC: { absRansacPtr problem; if(useIndices) problem = absRansacPtr( new absRansac( *absoluteAdapter, absRansac::EPNP, indices ) ); else problem = absRansacPtr( new absRansac( *absoluteAdapter, absRansac::EPNP ) ); opengv::sac::Ransac<absRansac> ransac; ransac.sac_model_ = problem; ransac.threshold_ = 1.0 - cos(atan(sqrt(2.0)*0.5/800.0)); ransac.max_iterations_ = 50; ransac.computeModel(); int dims[2]; dims[0] = 3; dims[1] = 4; plhs[0] = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL); memcpy(mxGetData(plhs[0]), ransac.model_coefficients_.data(), 12*sizeof(double)); break; } case ABS_NONLIN_CENTRAL: { opengv::transformation_t temp; if(useIndices) temp = opengv::absolute_pose::optimize_nonlinear(*absoluteAdapter,indices); else temp = opengv::absolute_pose::optimize_nonlinear(*absoluteAdapter); int dims[2]; dims[0] = 3; dims[1] = 4; plhs[0] = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL); memcpy(mxGetData(plhs[0]), temp.data(), 12*sizeof(double)); break; } case GP3P: { opengv::transformations_t temp; if(useIndices) temp = opengv::absolute_pose::gp3p(*absoluteAdapter,indices); else temp = opengv::absolute_pose::gp3p(*absoluteAdapter); int dims[3]; dims[0] = 3; dims[1] = 4; dims[2] = temp.size(); plhs[0] = mxCreateNumericArray(3, dims, mxDOUBLE_CLASS, mxREAL); for( int i = 0; i < temp.size(); i++ ) { void * targetAddress = ((char*) mxGetData(plhs[0])) + i*12*sizeof(double); memcpy(targetAddress, temp[i].data(), 12*sizeof(double)); } break; } case GP3P_RANSAC: { absRansacPtr problem; if(useIndices) problem = absRansacPtr( new absRansac( *absoluteAdapter, absRansac::GP3P, indices ) ); else problem = absRansacPtr( new absRansac( *absoluteAdapter, absRansac::GP3P ) ); opengv::sac::Ransac<absRansac> ransac; ransac.sac_model_ = problem; ransac.threshold_ = 1.0 - cos(atan(sqrt(2.0)*0.5/800.0)); ransac.max_iterations_ = 50; ransac.computeModel(); int dims[2]; dims[0] = 3; dims[1] = 4; plhs[0] = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL); memcpy(mxGetData(plhs[0]), ransac.model_coefficients_.data(), 12*sizeof(double)); break; } case GPNP: { opengv::transformation_t temp; if(useIndices) temp = opengv::absolute_pose::gpnp(*absoluteAdapter,indices); else temp = opengv::absolute_pose::gpnp(*absoluteAdapter); int dims[2]; dims[0] = 3; dims[1] = 4; plhs[0] = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL); memcpy(mxGetData(plhs[0]), temp.data(), 12*sizeof(double)); break; } case ABS_NONLIN_NONCENTRAL: { opengv::transformation_t temp; if(useIndices) temp = opengv::absolute_pose::optimize_nonlinear(*absoluteAdapter,indices); else temp = opengv::absolute_pose::optimize_nonlinear(*absoluteAdapter); int dims[2]; dims[0] = 3; dims[1] = 4; plhs[0] = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL); memcpy(mxGetData(plhs[0]), temp.data(), 12*sizeof(double)); break; } case TWOPT: { opengv::translation_t temp; if(useIndices) temp = opengv::relative_pose::twopt(*relativeAdapter,false,indices); else temp = opengv::relative_pose::twopt(*relativeAdapter,false); int dims[2]; dims[0] = 3; dims[1] = 1; plhs[0] = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL); memcpy(mxGetData(plhs[0]), temp.data(), 3*sizeof(double)); break; } case TWOPT_ROTATIONONLY: { opengv::rotation_t temp; if(useIndices) temp = opengv::relative_pose::twopt_rotationOnly(*relativeAdapter,indices); else temp = opengv::relative_pose::twopt_rotationOnly(*relativeAdapter); int dims[2]; dims[0] = 3; dims[1] = 3; plhs[0] = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL); memcpy(mxGetData(plhs[0]), temp.data(), 9*sizeof(double)); break; } case ROTATIONONLY: { opengv::rotation_t temp; if(useIndices) temp = opengv::relative_pose::rotationOnly(*relativeAdapter,indices); else temp = opengv::relative_pose::rotationOnly(*relativeAdapter); int dims[2]; dims[0] = 3; dims[1] = 3; plhs[0] = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL); memcpy(mxGetData(plhs[0]), temp.data(), 9*sizeof(double)); break; } case FIVEPT_STEWENIUS: { opengv::complexEssentials_t temp2; if(useIndices) temp2 = opengv::relative_pose::fivept_stewenius(*relativeAdapter,indices); else temp2 = opengv::relative_pose::fivept_stewenius(*relativeAdapter); opengv::essentials_t temp; for(size_t i = 0; i < temp2.size(); i++) { opengv::essential_t essentialMatrix; for(size_t r = 0; r < 3; r++) { for(size_t c = 0; c < 3; c++) essentialMatrix(r,c) = temp2[i](r,c).real(); } temp.push_back(essentialMatrix); } int dims[3]; dims[0] = 3; dims[1] = 3; dims[2] = temp.size(); plhs[0] = mxCreateNumericArray(3, dims, mxDOUBLE_CLASS, mxREAL); for( int i = 0; i < temp.size(); i++ ) { void * targetAddress = ((char*) mxGetData(plhs[0])) + i*9*sizeof(double); memcpy(targetAddress, temp[i].data(), 9*sizeof(double)); } break; } case FIVEPT_NISTER: { opengv::essentials_t temp; if(useIndices) temp = opengv::relative_pose::fivept_nister(*relativeAdapter,indices); else temp = opengv::relative_pose::fivept_nister(*relativeAdapter); int dims[3]; dims[0] = 3; dims[1] = 3; dims[2] = temp.size(); plhs[0] = mxCreateNumericArray(3, dims, mxDOUBLE_CLASS, mxREAL); for( int i = 0; i < temp.size(); i++ ) { void * targetAddress = ((char*) mxGetData(plhs[0])) + i*9*sizeof(double); memcpy(targetAddress, temp[i].data(), 9*sizeof(double)); } break; } case FIVEPT_KNEIP: { opengv::rotations_t temp; if(useIndices) temp = opengv::relative_pose::fivept_kneip(*relativeAdapter,indices); else { mexPrintf("opengv: Bad input to mex function opengv\n"); mexPrintf("Assuming method: "); mexPrintf(methods[caseNumber]); mexPrintf("\n"); mexPrintf("You must provide an indices vector\n"); break; } int dims[3]; dims[0] = 3; dims[1] = 3; dims[2] = temp.size(); plhs[0] = mxCreateNumericArray(3, dims, mxDOUBLE_CLASS, mxREAL); for( int i = 0; i < temp.size(); i++ ) { void * targetAddress = ((char*) mxGetData(plhs[0])) + i*9*sizeof(double); memcpy(targetAddress, temp[i].data(), 9*sizeof(double)); } break; } case SEVENPT: { opengv::essentials_t temp; if(useIndices) temp = opengv::relative_pose::sevenpt(*relativeAdapter,indices); else temp = opengv::relative_pose::sevenpt(*relativeAdapter); int dims[3]; dims[0] = 3; dims[1] = 3; dims[2] = temp.size(); plhs[0] = mxCreateNumericArray(3, dims, mxDOUBLE_CLASS, mxREAL); for( int i = 0; i < temp.size(); i++ ) { void * targetAddress = ((char*) mxGetData(plhs[0])) + i*9*sizeof(double); memcpy(targetAddress, temp[i].data(), 9*sizeof(double)); } break; } case EIGHTPT: { opengv::essential_t temp; if(useIndices) temp = opengv::relative_pose::eightpt(*relativeAdapter,indices); else temp = opengv::relative_pose::eightpt(*relativeAdapter); int dims[2]; dims[0] = 3; dims[1] = 3; plhs[0] = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL); memcpy(mxGetData(plhs[0]), temp.data(), 9*sizeof(double)); break; } case EIGENSOLVER: { opengv::rotation_t temp; if(useIndices) temp = opengv::relative_pose::eigensolver(*relativeAdapter,indices); else temp = opengv::relative_pose::eigensolver(*relativeAdapter); int dims[2]; dims[0] = 3; dims[1] = 3; plhs[0] = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL); memcpy(mxGetData(plhs[0]), temp.data(), 9*sizeof(double)); break; } case ROTATIONONLY_RANSAC: { rotRansacPtr problem; if(useIndices) problem = rotRansacPtr( new rotRansac( *relativeAdapter, indices ) ); else problem = rotRansacPtr( new rotRansac( *relativeAdapter ) ); opengv::sac::Ransac<rotRansac> ransac; ransac.sac_model_ = problem; ransac.threshold_ = 2.0*(1.0 - cos(atan(sqrt(2.0)*0.5/800.0))); ransac.max_iterations_ = 50; ransac.computeModel(); int dims[2]; dims[0] = 3; dims[1] = 3; plhs[0] = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL); memcpy(mxGetData(plhs[0]), ransac.model_coefficients_.data(), 9*sizeof(double)); break; } case FIVEPT_STEWENIUS_RANSAC: { relRansacPtr problem; if(useIndices) problem = relRansacPtr( new relRansac( *relativeAdapter, relRansac::STEWENIUS, indices ) ); else problem = relRansacPtr( new relRansac( *relativeAdapter, relRansac::STEWENIUS ) ); opengv::sac::Ransac<relRansac> ransac; ransac.sac_model_ = problem; ransac.threshold_ = 2.0*(1.0 - cos(atan(sqrt(2.0)*0.5/800.0))); ransac.max_iterations_ = 50; ransac.computeModel(); opengv::transformation_t optimizedModel; problem->optimizeModelCoefficients(ransac.inliers_,ransac.model_coefficients_,optimizedModel); int dims[2]; dims[0] = 3; dims[1] = 4; plhs[0] = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL); //memcpy(mxGetData(plhs[0]), ransac.model_coefficients_.data(), 12*sizeof(double)); memcpy(mxGetData(plhs[0]), optimizedModel.data(), 12*sizeof(double)); break; } case FIVEPT_NISTER_RANSAC: { relRansacPtr problem; if(useIndices) problem = relRansacPtr( new relRansac( *relativeAdapter, relRansac::NISTER, indices ) ); else problem = relRansacPtr( new relRansac( *relativeAdapter, relRansac::NISTER ) ); opengv::sac::Ransac<relRansac> ransac; ransac.sac_model_ = problem; ransac.threshold_ = 2.0*(1.0 - cos(atan(sqrt(2.0)*0.5/800.0))); ransac.max_iterations_ = 50; ransac.computeModel(); int dims[2]; dims[0] = 3; dims[1] = 4; plhs[0] = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL); memcpy(mxGetData(plhs[0]), ransac.model_coefficients_.data(), 12*sizeof(double)); break; } case SEVENPT_RANSAC: { relRansacPtr problem; if(useIndices) problem = relRansacPtr( new relRansac( *relativeAdapter, relRansac::SEVENPT, indices ) ); else problem = relRansacPtr( new relRansac( *relativeAdapter, relRansac::SEVENPT ) ); opengv::sac::Ransac<relRansac> ransac; ransac.sac_model_ = problem; ransac.threshold_ = 2.0*(1.0 - cos(atan(sqrt(2.0)*0.5/800.0))); ransac.max_iterations_ = 50; ransac.computeModel(); int dims[2]; dims[0] = 3; dims[1] = 4; plhs[0] = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL); memcpy(mxGetData(plhs[0]), ransac.model_coefficients_.data(), 12*sizeof(double)); break; } case EIGHTPT_RANSAC: { relRansacPtr problem; if(useIndices) problem = relRansacPtr( new relRansac( *relativeAdapter, relRansac::EIGHTPT, indices ) ); else problem = relRansacPtr( new relRansac( *relativeAdapter, relRansac::EIGHTPT ) ); opengv::sac::Ransac<relRansac> ransac; ransac.sac_model_ = problem; ransac.threshold_ = 2.0*(1.0 - cos(atan(sqrt(2.0)*0.5/800.0))); ransac.max_iterations_ = 50; ransac.computeModel(); int dims[2]; dims[0] = 3; dims[1] = 4; plhs[0] = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL); memcpy(mxGetData(plhs[0]), ransac.model_coefficients_.data(), 12*sizeof(double)); break; } case EIGENSOLVER_RANSAC: { eigRansacPtr problem; if(useIndices) problem = eigRansacPtr( new eigRansac( *relativeAdapter, 10, indices ) ); else problem = eigRansacPtr( new eigRansac( *relativeAdapter, 10 ) ); opengv::sac::Ransac<eigRansac> ransac; ransac.sac_model_ = problem; ransac.threshold_ = 1.0; ransac.max_iterations_ = 50; ransac.computeModel(); opengv::transformation_t temp; temp.block<3,3>(0,0) = ransac.model_coefficients_.rotation; temp.block<3,1>(0,3) = ransac.model_coefficients_.translation; int dims[2]; dims[0] = 3; dims[1] = 4; plhs[0] = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL); memcpy(mxGetData(plhs[0]), temp.data(), 12*sizeof(double)); break; } case REL_NONLIN_CENTRAL: { opengv::transformation_t temp; if(useIndices) temp = opengv::relative_pose::optimize_nonlinear(*relativeAdapter,indices); else temp = opengv::relative_pose::optimize_nonlinear(*relativeAdapter); int dims[2]; dims[0] = 3; dims[1] = 4; plhs[0] = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL); memcpy(mxGetData(plhs[0]), temp.data(), 12*sizeof(double)); break; } case SIXPT: { opengv::rotations_t temp; if(useIndices) temp = opengv::relative_pose::sixpt(*relativeAdapter,indices); else temp = opengv::relative_pose::sixpt(*relativeAdapter); int dims[3]; dims[0] = 3; dims[1] = 3; dims[2] = temp.size(); plhs[0] = mxCreateNumericArray(3, dims, mxDOUBLE_CLASS, mxREAL); for( int i = 0; i < temp.size(); i++ ) { void * targetAddress = ((char*) mxGetData(plhs[0])) + i*9*sizeof(double); memcpy(targetAddress, temp[i].data(), 9*sizeof(double)); } break; } case SEVENTEENPT: { opengv::transformation_t temp; if(useIndices) temp = opengv::relative_pose::seventeenpt(*relativeAdapter,indices); else temp = opengv::relative_pose::seventeenpt(*relativeAdapter); int dims[2]; dims[0] = 3; dims[1] = 4; plhs[0] = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL); memcpy(mxGetData(plhs[0]), temp.data(), 12*sizeof(double)); break; } case GE: { opengv::rotation_t temp; opengv::geOutput_t output; output.rotation = relativeAdapter->getR12(); if(useIndices) temp = opengv::relative_pose::ge(*relativeAdapter,indices,output); else temp = opengv::relative_pose::ge(*relativeAdapter,output); int dims[2]; dims[0] = 3; dims[1] = 3; plhs[0] = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL); memcpy(mxGetData(plhs[0]),temp.data(), 9*sizeof(double)); break; } case GE2: { Eigen::Matrix<double,4,5> temp; opengv::geOutput_t output; output.rotation = relativeAdapter->getR12(); opengv::rotation_t solution = opengv::relative_pose::ge2(*relativeAdapter,indices,output); temp.block<4,4>(0,0) = output.eigenvectors; temp.block<3,3>(0,0) = solution; temp.col(4) = output.eigenvalues; int dims[2]; dims[0] = 4; dims[1] = 5; plhs[0] = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL); memcpy(mxGetData(plhs[0]),temp.data(), 20*sizeof(double)); break; } case SIXPT_RANSAC: { nrelRansacPtr problem; if(useIndices) problem = nrelRansacPtr( new nrelRansac( *relativeAdapter, nrelRansac::SIXPT, indices ) ); else problem = nrelRansacPtr( new nrelRansac( *relativeAdapter, nrelRansac::SIXPT ) ); opengv::sac::Ransac<nrelRansac> ransac; ransac.sac_model_ = problem; ransac.threshold_ = 2.0*(1.0 - cos(atan(sqrt(2.0)*0.5/800.0))); ransac.max_iterations_ = 50; ransac.computeModel(); int dims[2]; dims[0] = 3; dims[1] = 4; plhs[0] = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL); memcpy(mxGetData(plhs[0]), ransac.model_coefficients_.data(), 12*sizeof(double)); break; } case SEVENTEENPT_RANSAC: { nrelRansacPtr problem; if(useIndices) problem = nrelRansacPtr( new nrelRansac( *relativeAdapter, nrelRansac::SEVENTEENPT, indices ) ); else problem = nrelRansacPtr( new nrelRansac( *relativeAdapter, nrelRansac::SEVENTEENPT ) ); opengv::sac::Ransac<nrelRansac> ransac; ransac.sac_model_ = problem; ransac.threshold_ = 2.0*(1.0 - cos(atan(sqrt(2.0)*0.5/800.0))); ransac.max_iterations_ = 50; ransac.computeModel(); int dims[2]; dims[0] = 3; dims[1] = 4; plhs[0] = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL); memcpy(mxGetData(plhs[0]), ransac.model_coefficients_.data(), 12*sizeof(double)); break; } case GE_RANSAC: { nrelRansacPtr problem; if(useIndices) problem = nrelRansacPtr( new nrelRansac( *relativeAdapter, nrelRansac::GE, indices ) ); else problem = nrelRansacPtr( new nrelRansac( *relativeAdapter, nrelRansac::GE ) ); opengv::sac::Ransac<nrelRansac> ransac; ransac.sac_model_ = problem; ransac.threshold_ = 2.0*(1.0 - cos(atan(sqrt(2.0)*0.5/800.0))); ransac.max_iterations_ = 50; ransac.computeModel(); int dims[2]; dims[0] = 3; dims[1] = 4; plhs[0] = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL); memcpy(mxGetData(plhs[0]), ransac.model_coefficients_.data(), 12*sizeof(double)); break; } case REL_NONLIN_NONCENTRAL: { opengv::transformation_t temp; if(useIndices) temp = opengv::relative_pose::optimize_nonlinear(*relativeAdapter,indices); else temp = opengv::relative_pose::optimize_nonlinear(*relativeAdapter); int dims[2]; dims[0] = 3; dims[1] = 4; plhs[0] = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL); memcpy(mxGetData(plhs[0]), temp.data(), 12*sizeof(double)); break; } case THREEPT_ARUN: { opengv::transformation_t temp; if(useIndices) temp = opengv::point_cloud::threept_arun(*pointCloudAdapter,indices); else temp = opengv::point_cloud::threept_arun(*pointCloudAdapter); int dims[2]; dims[0] = 3; dims[1] = 4; plhs[0] = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL); memcpy(mxGetData(plhs[0]), temp.data(), 12*sizeof(double)); break; } case THREEPT_ARUN_RANSAC: { ptRansacPtr problem; if(useIndices) problem = ptRansacPtr( new ptRansac( *pointCloudAdapter, indices ) ); else problem = ptRansacPtr( new ptRansac( *pointCloudAdapter ) ); opengv::sac::Ransac<ptRansac> ransac; ransac.sac_model_ = problem; ransac.threshold_ = 0.1; ransac.max_iterations_ = 50; ransac.computeModel(); int dims[2]; dims[0] = 3; dims[1] = 4; plhs[0] = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL); memcpy(mxGetData(plhs[0]), ransac.model_coefficients_.data(), 12*sizeof(double)); break; } default: //-1 { // impossible break; } } }
bool opengv::sac_problems:: relative_pose::CentralRelativePoseSacProblem::computeModelCoefficients( const std::vector<int> &indices, model_t & outModel) const { essentials_t essentialMatrices; switch(_algorithm) { case NISTER: { std::vector<int> subIndices1; for(size_t i = 0; i < 5; i++) subIndices1.push_back(indices[i]); essentialMatrices = opengv::relative_pose::fivept_nister(_adapter,subIndices1); break; } case STEWENIUS: { std::vector<int> subIndices2; for(size_t i = 0; i < 5; i++) subIndices2.push_back(indices[i]); complexEssentials_t complexEssentialMatrices = opengv::relative_pose::fivept_stewenius(_adapter,subIndices2); // convert from complexEssential to essential for(size_t i = 0; i < complexEssentialMatrices.size(); i++) { essential_t essentialMatrix; for(size_t r = 0; r < 3; r++) { for(size_t c = 0; c < 3; c++) essentialMatrix(r,c) = complexEssentialMatrices.at(i)(r,c).real(); } essentialMatrices.push_back(essentialMatrix); } break; } case SEVENPT: { std::vector<int> subIndices3; for(size_t i = 0; i < 7; i++) subIndices3.push_back(indices[i]); essentialMatrices = opengv::relative_pose::sevenpt(_adapter,subIndices3); break; } case EIGHTPT: { std::vector<int> subIndices4; for(size_t i = 0; i < 8; i++) subIndices4.push_back(indices[i]); essential_t essentialMatrix = opengv::relative_pose::eightpt(_adapter,subIndices4); essentialMatrices.push_back(essentialMatrix); break; } } //now decompose each essential matrix into transformations and find the //right one Eigen::Matrix3d W = Eigen::Matrix3d::Zero(); W(0,1) = -1; W(1,0) = 1; W(2,2) = 1; double bestQuality = 1000000.0; int bestQualityIndex = -1; int bestQualitySubindex = -1; for(size_t i = 0; i < essentialMatrices.size(); i++) { // decompose Eigen::MatrixXd tempEssential = essentialMatrices[i]; Eigen::JacobiSVD< Eigen::MatrixXd > SVD( tempEssential, Eigen::ComputeFullV | Eigen::ComputeFullU ); Eigen::VectorXd singularValues = SVD.singularValues(); // check for bad essential matrix if( singularValues[2] > 0.001 ) {}; // continue; //singularity constraints not applied -> removed because too harsh if( singularValues[1] < 0.75 * singularValues[0] ) {}; // continue; //bad essential matrix -> removed because too harsh // maintain scale double scale = singularValues[0]; // get possible rotation and translation vectors rotation_t Ra = SVD.matrixU() * W * SVD.matrixV().transpose(); rotation_t Rb = SVD.matrixU() * W.transpose() * SVD.matrixV().transpose(); translation_t ta = scale*SVD.matrixU().col(2); translation_t tb = -ta; // change sign if det = -1 if( Ra.determinant() < 0 ) Ra = -Ra; if( Rb.determinant() < 0 ) Rb = -Rb; //derive transformations transformation_t transformation; transformations_t transformations; transformation.col(3) = ta; transformation.block<3,3>(0,0) = Ra; transformations.push_back(transformation); transformation.col(3) = ta; transformation.block<3,3>(0,0) = Rb; transformations.push_back(transformation); transformation.col(3) = tb; transformation.block<3,3>(0,0) = Ra; transformations.push_back(transformation); transformation.col(3) = tb; transformation.block<3,3>(0,0) = Rb; transformations.push_back(transformation); // derive inverse transformations transformations_t inverseTransformations; for(size_t j = 0; j < 4; j++) { transformation_t inverseTransformation; inverseTransformation.block<3,3>(0,0) = transformations[j].block<3,3>(0,0).transpose(); inverseTransformation.col(3) = -inverseTransformation.block<3,3>(0,0)*transformations[j].col(3); inverseTransformations.push_back(inverseTransformation); } // collect qualities for each of the four solutions solution Eigen::Matrix<double,4,1> p_hom; p_hom[3] = 1.0; for(size_t j = 0; j<4; j++) { // prepare variables for triangulation and reprojection _adapter.sett12(transformations[j].col(3)); _adapter.setR12(transformations[j].block<3,3>(0,0)); // go through all features and compute quality of reprojection double quality = 0.0; for( int k = 0; k < getSampleSize(); k++ ) { p_hom.block<3,1>(0,0) = opengv::triangulation::triangulate2(_adapter,indices[k]); bearingVector_t reprojection1 = p_hom.block<3,1>(0,0); bearingVector_t reprojection2 = inverseTransformations[j] * p_hom; reprojection1 = reprojection1 / reprojection1.norm(); reprojection2 = reprojection2 / reprojection2.norm(); bearingVector_t f1 = _adapter.getBearingVector1(indices[k]); bearingVector_t f2 = _adapter.getBearingVector2(indices[k]); // bearing-vector based outlier criterium (select threshold accordingly): // 1-(f1'*f2) = 1-cos(alpha) \in [0:2] double reprojError1 = 1.0 - (f1.transpose() * reprojection1); double reprojError2 = 1.0 - (f2.transpose() * reprojection2); quality += reprojError1 + reprojError2; } // is quality better? (lower) if( quality < bestQuality ) { bestQuality = quality; bestQualityIndex = i; bestQualitySubindex = j; } } } if( bestQualityIndex == -1 ) return false; // no solution found else { // rederive the best solution // decompose Eigen::MatrixXd tempEssential = essentialMatrices[bestQualityIndex]; Eigen::JacobiSVD< Eigen::MatrixXd > SVD( tempEssential, Eigen::ComputeFullV | Eigen::ComputeFullU ); const Eigen::VectorXd singularValues = SVD.singularValues(); // maintain scale const double scale = singularValues[0]; // get possible rotation and translation vectors translation_t translation; rotation_t rotation; switch(bestQualitySubindex) { case 0: translation = scale*SVD.matrixU().col(2); rotation = SVD.matrixU() * W * SVD.matrixV().transpose(); break; case 1: translation = scale*SVD.matrixU().col(2); rotation = SVD.matrixU() * W.transpose() * SVD.matrixV().transpose(); break; case 2: translation = -scale*SVD.matrixU().col(2); rotation = SVD.matrixU() * W * SVD.matrixV().transpose(); break; case 3: translation = -scale*SVD.matrixU().col(2); rotation = SVD.matrixU() * W.transpose() * SVD.matrixV().transpose(); break; default: return false; } // change sign if det = -1 if( rotation.determinant() < 0 ) rotation = -rotation; // output final selection outModel.block<3,3>(0,0) = rotation; outModel.col(3) = translation; } return true; }