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<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;
}
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;
}
int main(int argc, char **argv)
{
enum
{
ROBUST_RIGID_REGISTRATION = 0,
RIGID_REGISTRATION_ALL_POINTS = 1
};
std::string
sSfM_Data_Filename_In,
sSfM_Data_Filename_Out;
unsigned int rigid_registration_method = RIGID_REGISTRATION_ALL_POINTS;
CmdLine cmd;
cmd.add(make_option('i', sSfM_Data_Filename_In, "input_file"));
cmd.add(make_option('o', sSfM_Data_Filename_Out, "output_file"));
cmd.add(make_option('m', rigid_registration_method, "method"));
try
{
if (argc == 1) throw std::string("Invalid command line parameter.");
cmd.process(argc, argv);
}
catch(const std::string& s)
{
std::cerr
<< "Usage: " << argv[0] << '\n'
<< " GPS registration of a SfM Data scene,\n"
<< "[-i|--input_file] path to the input SfM_Data scene\n"
<< "[-o|--output_file] path to the output SfM_Data scene\n"
<< "[-m|--method] method to use for the rigid registration\n"
<< "\t0 => registration is done using a robust estimation,\n"
<< "\t1 (default)=> registration is done using all points.\n"
<< std::endl;
std::cerr << s << std::endl;
return EXIT_FAILURE;
}
if (sSfM_Data_Filename_In.empty() || sSfM_Data_Filename_Out.empty())
{
std::cerr << "Invalid input or output filename." << std::endl;
return EXIT_FAILURE;
}
//
// Load a SfM scene
// For each valid view (pose & intrinsic defined)
// - iff a GPS position can be parsed
// - store corresponding camera pose & GPS position
// - Compute the registration between the selected camera poses & GPS positions
// Load input SfM_Data scene
SfM_Data sfm_data;
if (!Load(sfm_data, sSfM_Data_Filename_In, ESfM_Data(ALL)))
{
std::cerr
<< "\nThe input SfM_Data file \"" << sSfM_Data_Filename_In
<< "\" cannot be read." << std::endl;
return EXIT_FAILURE;
}
// Init the EXIF reader (will be used for GPS data reading)
std::unique_ptr<Exif_IO> exifReader(new Exif_IO_EasyExif);
if (!exifReader)
{
std::cerr << "Cannot instantiate the EXIF metadata reader." << std::endl;
return EXIT_FAILURE;
}
// List corresponding poses (SfM - GPS)
std::vector<Vec3> vec_sfm_center, vec_gps_center;
for (const auto & view_it : sfm_data.GetViews() )
{
if (!sfm_data.IsPoseAndIntrinsicDefined(view_it.second.get()))
continue;
const std::string view_filename =
stlplus::create_filespec(sfm_data.s_root_path, view_it.second->s_Img_path);
// Try to parse EXIF metada & check existence of EXIF data
if (! (exifReader->open( view_filename ) &&
exifReader->doesHaveExifInfo()) )
continue;
// Check existence of GPS coordinates
double latitude, longitude, altitude;
if ( exifReader->GPSLatitude( &latitude ) &&
exifReader->GPSLongitude( &longitude ) &&
exifReader->GPSAltitude( &altitude ) )
{
// Add ECEF XYZ position to the GPS position array
vec_gps_center.push_back( lla_to_ecef( latitude, longitude, altitude ) );
const openMVG::geometry::Pose3 pose(sfm_data.GetPoseOrDie(view_it.second.get()));
vec_sfm_center.push_back( pose.center() );
}
}
if ( vec_sfm_center.empty() )
{
std::cerr << "No valid corresponding GPS data found for the used views." << std::endl;
return EXIT_FAILURE;
}
std::cout << std::endl
<< "Registration report:\n"
<< " #corresponding SFM - GPS data: " << vec_sfm_center.size() << "\n"
<< std::endl;
// Export the corresponding poses (for debugging & see the transformation)
plyHelper::exportToPly( vec_gps_center,
stlplus::create_filespec(stlplus::folder_part(sSfM_Data_Filename_Out), "GPS_position", "ply"));
plyHelper::exportToPly( vec_sfm_center,
stlplus::create_filespec(stlplus::folder_part(sSfM_Data_Filename_Out), "SFM_position", "ply"));
{
// Convert positions to the appropriate data container
const Mat X_SfM = Eigen::Map<Mat>(vec_sfm_center[0].data(), 3, vec_sfm_center.size());
const Mat X_GPS = Eigen::Map<Mat>(vec_gps_center[0].data(), 3, vec_gps_center.size());
openMVG::geometry::Similarity3 sim;
// Compute the registration:
// - using a rigid scheme (using all points)
// - using a robust scheme (using partial points - robust estimation)
switch (rigid_registration_method)
{
case ROBUST_RIGID_REGISTRATION:
{
using namespace openMVG::robust;
using namespace openMVG::geometry;
geometry::kernel::Similarity3_Kernel kernel(X_SfM, X_GPS);
const double lmeds_median = LeastMedianOfSquares
(
kernel,
&sim
);
std::cout << "LMeds found a model with an upper bound of: " << sqrt(lmeds_median) << " user units."<< std::endl;
// Compute & display fitting errors
{
const Vec vec_fitting_errors_eigen(
geometry::kernel::Similarity3ErrorSquaredMetric::ErrorVec(sim, X_SfM, X_GPS).array().sqrt());
std::cout << "\n3D Similarity fitting error using all points (in target coordinate system units):";
minMaxMeanMedian<float>(
vec_fitting_errors_eigen.data(),
vec_fitting_errors_eigen.data() + vec_fitting_errors_eigen.rows() );
}
// INLIERS only
{
std::vector<float> vec_fitting_errors;
for (Mat::Index i = 0; i < X_SfM.cols(); ++i)
{
if (geometry::kernel::Similarity3ErrorSquaredMetric::Error(sim, X_SfM.col(i), X_GPS.col(i)) < lmeds_median)
vec_fitting_errors.push_back((X_GPS.col(i) - sim(X_SfM.col(i))).norm());
}
std::cout << "\nFound: " << vec_fitting_errors.size() << " inliers"
<< " from " << X_SfM.cols() << " points." << std::endl;
std::cout << "\n3D Similarity fitting error using only the fitted inliers (in target coordinate system units):";
minMaxMeanMedian<float>( vec_fitting_errors.begin(), vec_fitting_errors.end() );
}
}
break;
case RIGID_REGISTRATION_ALL_POINTS:
{
Vec3 t;
Mat3 R;
double S;
if (!openMVG::geometry::FindRTS(X_SfM, X_GPS, &S, &t, &R))
{
std::cerr << "Failed to comute the registration" << std::endl;
return EXIT_FAILURE;
}
std::cout
<< "Found transform:\n"
<< " scale: " << S << "\n"
<< " rotation:\n" << R << "\n"
<< " translation: " << std::fixed << std::setprecision(9)
<< t.transpose() << std::endl;
// Encode the transformation as a 3D Similarity transformation matrix // S * R * X + t
sim = openMVG::geometry::Similarity3(geometry::Pose3(R, -R.transpose()* t/S), S);
// Compute & display fitting errors
{
const Vec vec_fitting_errors_eigen(
geometry::kernel::Similarity3ErrorSquaredMetric::ErrorVec(sim, X_SfM, X_GPS).array().sqrt());
std::cout << "\n3D Similarity fitting error (in target coordinate system units):";
minMaxMeanMedian<float>(
vec_fitting_errors_eigen.data(),
vec_fitting_errors_eigen.data() + vec_fitting_errors_eigen.rows() );
}
}
break;
default:
std::cerr << "Unknow rigid registration method" << std::endl;
return EXIT_FAILURE;
}
//--
// Apply the found transformation to the SfM Data Scene
//--
openMVG::sfm::ApplySimilarity(sim, sfm_data);
}
// Export the SfM_Data scene in the expected format
if (Save(
sfm_data,
sSfM_Data_Filename_Out.c_str(),
ESfM_Data(ALL)))
{
return EXIT_SUCCESS;
}
else
{
std::cerr
<< std::endl
<< "An error occured while trying to save \""
<< sSfM_Data_Filename_Out << "\"." << std::endl;
return EXIT_FAILURE;
}
return EXIT_FAILURE;
}