/**
* @brief Test if half-planes defines a non empty volume
* @param hplanes A list of half planes
* @retval true If half-planes define a non empty volume
* @retval false If half-planes define an empty volume
* @note Searched volume is defined by intersection of positive sides of half-planes
* @ref :
[1] Paper: Finding the intersection of n half-spaces in time O(n log n).
Author: F.P. Preparata, D.E. Muller
Published in: Theoretical Computer Science, Volume 8, Issue 1, Pages 45-55
Year: 1979
More: ISSN 0304-3975, http://dx.doi.org/10.1016/0304-3975(79)90055-0.
*/
static bool isNotEmpty( const Half_planes & hplanes )
{
// Check if it exists a point on all positive side of the half plane thanks to a Linear Program formulation [1].
// => If a point exists: there is a common subspace defined and so intersections.
// The LP formulation consists in set the Half_plane as constraint and check if a point can fit the equations.
using namespace openMVG;
using namespace openMVG::linearProgramming;
LP_Constraints cstraint;
{
cstraint.nbParams_ = 3; // {X,Y,Z}
cstraint.vec_bounds_.resize( cstraint.nbParams_ );
std::fill( cstraint.vec_bounds_.begin(), cstraint.vec_bounds_.end(),
std::make_pair( ( double ) - 1e+30, ( double )1e+30 ) ); // [X,Y,Z] => -inf, +inf
cstraint.bminimize_ = true;
// Configure constraints
const size_t nbConstraints = hplanes.size();
cstraint.constraint_mat_.resize( nbConstraints, 3 );
cstraint.vec_sign_.resize( nbConstraints );
cstraint.constraint_objective_ = Vec( nbConstraints );
// Fill the constrains (half-space equations)
for ( unsigned char i = 0; i < hplanes.size(); ++i )
{
const Vec & half_plane_coeff = hplanes[i].coeffs();
// add the half plane equation to the system
cstraint.constraint_mat_.row( i ) =
Vec3( half_plane_coeff( 0 ),
half_plane_coeff( 1 ),
half_plane_coeff( 2 ) );
cstraint.vec_sign_[i] = LP_Constraints::LP_GREATER_OR_EQUAL;
cstraint.constraint_objective_( i ) = - half_plane_coeff( 3 );
}
}
// Solve in order to see if a point exists within the half spaces positive side?
OSI_CLP_SolverWrapper solver( cstraint.nbParams_ );
solver.setup( cstraint );
const bool bIntersect = solver.solve(); // Status of the solver tell if there is an intersection or not
return bIntersect;
}
bool OSI_X_SolverWrapper<SOLVERINTERFACE>::setup(const LP_Constraints & cstraints) //cstraints <-> constraints
{
bool bOk = true;
if ( si == NULL )
{
return false;
}
assert(nbParams_ == cstraints.nbParams_);
const unsigned int NUMVAR = cstraints.constraint_mat_.cols();
std::vector<double> col_lb(NUMVAR);//the column lower bounds
std::vector<double> col_ub(NUMVAR);//the column upper bounds
this->nbParams_ = NUMVAR;
si->setObjSense( ((cstraints.bminimize_) ? 1 : -1) );
const Mat & A = cstraints.constraint_mat_;
//Equality constraint will be done by two constraints due to the API limitation ( >= & <=).
const size_t nbLine = A.rows() +
std::count(cstraints.vec_sign_.begin(), cstraints.vec_sign_.end(), LP_Constraints::LP_EQUAL);
std::vector<double> row_lb(nbLine);//the row lower bounds
std::vector<double> row_ub(nbLine);//the row upper bounds
CoinPackedMatrix * matrix = new CoinPackedMatrix(false,0,0);
matrix->setDimensions(0, NUMVAR);
//-- Add row-wise constraint
size_t indexRow = 0;
for (int i=0; i < A.rows(); ++i)
{
Vec temp = A.row(i);
CoinPackedVector row;
if ( cstraints.vec_sign_[i] == LP_Constraints::LP_EQUAL || cstraints.vec_sign_[i] == LP_Constraints::LP_LESS_OR_EQUAL )
{
int coef = 1;
for ( int j = 0; j < A.cols() ; j++ )
{
row.insert(j, coef * temp.data()[j]);
}
row_lb[indexRow] = -1.0 * si->getInfinity();
row_ub[indexRow] = coef * cstraints.constraint_objective_(i);
matrix->appendRow(row);
indexRow++;
}
if ( cstraints.vec_sign_[i] == LP_Constraints::LP_EQUAL || cstraints.vec_sign_[i] == LP_Constraints::LP_GREATER_OR_EQUAL )
{
int coef = -1;
for ( int j = 0; j < A.cols() ; j++ )
{
row.insert(j, coef * temp.data()[j]);
}
row_lb[indexRow] = -1.0 * si->getInfinity();
row_ub[indexRow] = coef * cstraints.constraint_objective_(i);
matrix->appendRow(row);
indexRow++;
}
}
//-- Setup bounds for all the parameters
if (cstraints.vec_bounds_.size() == 1)
{
// Setup the same bound for all the parameters
for (int i=0; i < this->nbParams_; ++i)
{
col_lb[i] = cstraints.vec_bounds_[0].first;
col_ub[i] = cstraints.vec_bounds_[0].second;
}
}
else // each parameter have it's own bounds
{
for (int i=0; i < this->nbParams_; ++i)
{
col_lb[i] = cstraints.vec_bounds_[i].first;
col_ub[i] = cstraints.vec_bounds_[i].second;
}
}
si->loadProblem(*matrix, &col_lb[0], &col_ub[0], cstraints.vec_cost_.empty() ? NULL : &cstraints.vec_cost_[0], &row_lb[0], &row_ub[0] );
delete matrix;
return bOk;
}
bool MOSEK_SolveWrapper::setup(const LP_Constraints & cstraints) //cstraints <-> constraints
{
assert(nbParams_ == cstraints.nbParams_);
MSK_deletetask(&task);
int NUMVAR = cstraints.constraint_mat_.cols();
int NUMCON = cstraints.constraint_mat_.rows();
int NUMANZ = cstraints.constraint_mat_.cols() * cstraints.constraint_mat_.rows(); //DENSE MATRIX
MSKrescodee r = MSK_RES_OK;
if ( r==MSK_RES_OK )
{
// Create the optimization task.
r = MSK_maketask(env,NUMCON,NUMVAR,&task);
// Directs the log task stream to the 'printstr' function.
if ( r==MSK_RES_OK )
MSK_linkfunctotaskstream(task,MSK_STREAM_LOG,NULL,printstr);
// Give MOSEK an estimate of the size of the input data.
//This is done to increase the speed of inputting data.
// However, it is optional.
if (r == MSK_RES_OK)
r = MSK_putmaxnumvar(task,NUMVAR);
if (r == MSK_RES_OK)
r = MSK_putmaxnumcon(task,NUMCON);
if (r == MSK_RES_OK)
r = MSK_putmaxnumanz(task,NUMANZ);
// Append 'NUMCON' empty constraints. The constraints will initially have no bounds.
if ( r == MSK_RES_OK )
r = MSK_append(task,MSK_ACC_CON,NUMCON);
// Append 'NUMVAR' variables. The variables will initially be fixed at zero (x=0).
if ( r == MSK_RES_OK )
r = MSK_append(task,MSK_ACC_VAR,NUMVAR);
}
this->nbParams_ = NUMVAR;
if (cstraints.bminimize_) {
r = MSK_putobjsense(task, MSK_OBJECTIVE_SENSE_MINIMIZE);
}
else {
r = MSK_putobjsense(task, MSK_OBJECTIVE_SENSE_MAXIMIZE);
}
// Optionally add a constant term to the objective.
if ( r ==MSK_RES_OK )
r = MSK_putcfix(task,0.0);
// Add the objective function if any
if (!cstraints.vec_cost_.empty()) {
// Set objective
for (size_t i = 0; i < cstraints.vec_cost_.size(); ++i)
MSK_putcj(task, i, cstraints.vec_cost_[i]);
}
//Add constraint row by row
const Mat & A = cstraints.constraint_mat_;
for (int i=0; i<A.rows() && r == MSK_RES_OK; ++i)
{
std::vector<int> vec_colno(A.cols(), 0);
for (int ii=0; ii<A.cols(); ++ii){
vec_colno[ii] = ii;
}
// Insert a row
Vec temp = A.row(i);
r = MSK_putavec(task,
MSK_ACC_CON, // Input row of A.
MSKidxt (i), // Variable row index.
vec_colno.size(), // Number of non-zeros in row(i) A.
&vec_colno[0], // Pointer to row indexes of row i.
(double*)temp.data()); // Pointer to Values of row i.
}
//Add bound on variables:
if (cstraints.vec_bounds_.size() == 1) {
for (size_t i = 0; i < NUMVAR; ++i) {
if (r == MSK_RES_OK)
r = MSK_putbound(task,
MSK_ACC_VAR, // Put bounds on variables.
i, // Index of variable.
convertSign(cstraints.vec_sign_[0]), // Bound key.
cstraints.vec_bounds_[0].first, // Numerical value of lower bound.
cstraints.vec_bounds_[0].second); // Numerical value of upper bound.
}
}
else{
for (size_t i = 0; i < NUMVAR; ++i) {
// Set the bounds on variable j.
// lowerbound <= x_j <= upper bound
if (r == MSK_RES_OK)
r = MSK_putbound(task,
MSK_ACC_VAR, // Put bounds on variables.
i, // Index of variable.
convertSign(cstraints.vec_sign_[i]), // Bound key.
cstraints.vec_bounds_[i].first, // Numerical value of lower bound.
cstraints.vec_bounds_[i].second); // Numerical value of upper bound.
// in order to add sparse bounds use: MSK_putboundlist
}
}
// Add bounds on constraint
for (size_t i=0; i<NUMCON && r==MSK_RES_OK; ++i)
{
r = MSK_putbound(task,
MSK_ACC_CON, // Put bounds on constraints.
i, // Index of constraint.
convertSign(cstraints.vec_sign_[i]), // Bound key.
-MSK_INFINITY, // Numerical value of lower bound.
cstraints.constraint_objective_(i)); // Numerical value of upper bound.
}
return r == MSK_RES_OK;
}
bool OSI_X_SolverWrapper::setup(const LP_Constraints & cstraints) //cstraints <-> constraints
{
if ( si == nullptr )
{
return false;
}
assert(nbParams_ == cstraints.nbParams_);
const unsigned int NUMVAR = cstraints.constraint_mat_.cols();
std::vector<double>
col_lb(NUMVAR), // the column lower bounds
col_ub(NUMVAR); // the column upper bounds
this->nbParams_ = NUMVAR;
si->setObjSense( ((cstraints.bminimize_) ? 1 : -1) );
const Mat & A = cstraints.constraint_mat_;
//Equality constraint will be done by two constraints due to the API limitation ( >= & <=).
const size_t nbLine = A.rows() +
std::count(cstraints.vec_sign_.begin(), cstraints.vec_sign_.end(), LP_Constraints::LP_EQUAL);
// Define default lower and upper bound to [-inf, inf]
std::vector<double>
row_lb(nbLine, -si->getInfinity()), // the row lower bounds
row_ub(nbLine, si->getInfinity()); // the row upper bounds
std::unique_ptr<CoinPackedMatrix> matrix(new CoinPackedMatrix(false,0,0));
matrix->setDimensions(0, NUMVAR);
//-- Add row-wise constraint
size_t indexRow = 0;
for (int i=0; i < A.rows(); ++i)
{
const Vec temp = A.row(i);
CoinPackedVector row;
if (cstraints.vec_sign_[i] == LP_Constraints::LP_EQUAL ||
cstraints.vec_sign_[i] == LP_Constraints::LP_LESS_OR_EQUAL)
{
const int coef = 1;
for ( int j = 0; j < A.cols(); ++j )
{
row.insert(j, coef * temp.data()[j]);
}
row_ub[indexRow] = coef * cstraints.constraint_objective_(i);
matrix->appendRow(row);
++indexRow;
}
if (cstraints.vec_sign_[i] == LP_Constraints::LP_EQUAL ||
cstraints.vec_sign_[i] == LP_Constraints::LP_GREATER_OR_EQUAL)
{
const int coef = -1;
for ( int j = 0; j < A.cols(); ++j )
{
row.insert(j, coef * temp.data()[j]);
}
row_ub[indexRow] = coef * cstraints.constraint_objective_(i);
matrix->appendRow(row);
++indexRow;
}
}
//-- Setup bounds for all the parameters
if (cstraints.vec_bounds_.size() == 1)
{
// Setup the same bound for all the parameters
std::fill(col_lb.begin(), col_lb.end(), cstraints.vec_bounds_[0].first);
std::fill(col_ub.begin(), col_ub.end(), cstraints.vec_bounds_[0].second);
}
else // each parameter have its own bounds
{
for (int i=0; i < this->nbParams_; ++i)
{
col_lb[i] = cstraints.vec_bounds_[i].first;
col_ub[i] = cstraints.vec_bounds_[i].second;
}
}
si->loadProblem(*matrix, &col_lb[0], &col_ub[0], cstraints.vec_cost_.empty() ? nullptr : &cstraints.vec_cost_[0], &row_lb[0], &row_ub[0] );
return true;
}
bool OSI_X_SolverWrapper<SOLVERINTERFACE>::setup(const LP_Constraints & cstraints) //cstraints <-> constraints
{
bool bOk = true;
if ( si == NULL )
{
return false;
}
assert(_nbParams == cstraints._nbParams);
int NUMVAR = cstraints._constraintMat.cols();
int NUMCON = cstraints._constraintMat.rows();
int NUMANZ = cstraints._constraintMat.cols() * cstraints._constraintMat.rows(); //DENSE MATRIX
std::vector<double> col_lb(NUMVAR);//the column lower bounds
std::vector<double> col_ub(NUMVAR);//the column upper bounds
this->_nbParams = NUMVAR;
if (cstraints._bminimize)
{
si->setObjSense( 1 );
}
else
{
si->setObjSense( -1 );
}
const Mat & A = cstraints._constraintMat;
//Equality constraint will be handked by two constraintsdue to the API limitation.
size_t nbLine = A.rows() + std::count(cstraints._vec_sign.begin(), cstraints._vec_sign.end(), EQ);
std::vector<double> row_lb(nbLine);//the row lower bounds
std::vector<double> row_ub(nbLine);//the row upper bounds
CoinPackedMatrix * matrix = new CoinPackedMatrix(false,0,0);
matrix->setDimensions(0, NUMVAR);
//-- Add row-wise constraint
size_t indexRow = 0;
for (int i=0; i < A.rows(); ++i)
{
Vec temp = A.row(i);
CoinPackedVector row;
if ( cstraints._vec_sign[i] == EQ || cstraints._vec_sign[i] == LE )
{
int coef = 1;
for ( int j = 0; j < A.cols() ; j++ )
{
row.insert(j, coef * temp.data()[j]);
}
row_lb[indexRow] = -1.0 * si->getInfinity();
row_ub[indexRow] = coef * cstraints._Cst_objective(i);
matrix->appendRow(row);
indexRow++;
}
if ( cstraints._vec_sign[i] == EQ || cstraints._vec_sign[i] == GE )
{
int coef = -1;
for ( int j = 0; j < A.cols() ; j++ )
{
row.insert(j, coef * temp.data()[j]);
}
row_lb[indexRow] = -1.0 * si->getInfinity();
row_ub[indexRow] = coef * cstraints._Cst_objective(i);
matrix->appendRow(row);
indexRow++;
}
}
//-- Setup bounds
if (cstraints._vec_bounds.size() == 1)
{
// Setup the same bound for all the parameter
for (int i=0; i < this->_nbParams; ++i)
{
col_lb[i] = cstraints._vec_bounds[0].first;
col_ub[i] = cstraints._vec_bounds[0].second;
}
}
else
{
for (int i=0; i < this->_nbParams; ++i)
{
col_lb[i] = cstraints._vec_bounds[i].first;
col_ub[i] = cstraints._vec_bounds[i].second;
}
}
si->loadProblem(*matrix, &col_lb[0], &col_ub[0], cstraints._vec_cost.empty() ? NULL : &cstraints._vec_cost[0], &row_lb[0], &row_ub[0] );
delete matrix;
return bOk;
}
