// 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;
}
