void SourceFile::prep_dat( Vec<PI8> &data, ST bin_size, String filename ) {
data.resize( sizeof( int ) + bin_size + sizeof( int ) + filename.size() + 1 );
reinterpret_cast<int &>( data[ 0 ] ) = bin_size;
// -> memcpy( data.ptr() + sizeof( int ), ..., bin_size );
reinterpret_cast<int &>( data[ sizeof( int ) + bin_size ] ) = filename.size();
memcpy( data.ptr() + sizeof( int ) + bin_size + sizeof( int ), filename.c_str(), filename.size() + 1 );
}
void do_moves(Vec& v) const {
assert(v.size() == n_mapped);
unsigned i = 0;
for (; i < n_mapped && keeping(i); ++i)
;
for (; i < n_mapped; ++i)
if (keeping(i)) v[map[i]] = v[i];
v.resize(n_kept);
}
void do_moves_swap(Vec& v) const {
using std::swap;
assert(v.size() == n_mapped);
unsigned i = 0;
for (; i < n_mapped && keeping(i); ++i)
;
for (; i < n_mapped; ++i)
if (keeping(i)) std::swap(v[map[i]], v[i]);
v.resize(n_kept);
}
Vec<unsigned> symamd(const Vec<Vec<unsigned> > &ind) {
Vec<int> lines; lines.resize( ind.size() + 1 );
Vec<int> indices;
unsigned nb_ind = 0;
for(unsigned i=0;i<ind.size();++i)
nb_ind += ind[i].size();
indices.reserve( nb_ind );
lines[0] = 0;
for(unsigned i=0;i<ind.size();++i) {
for(unsigned j=0;j<ind[i].size();++j)
indices.push_back( ind[i][j] );
lines[i+1] = indices.size();
}
Vec<unsigned> P; P.resize( ind.size() );
amd_order( ind.size(), lines.begin(), indices.begin(), (int *)P.begin(), (double *)NULL, (double *)NULL );
return P;
}
static void update_created( Vec<Expr> &created, const Vec<Expr> &out ) {
PI64 coi = ++Inst::cur_op_id;
created.resize( 0 );
for( Expr e : out )
e->mark_children( &created );
for( Expr &e : created )
for( int i = 0; i < e->par.size(); ++i )
if ( e->par[ i ].inst->op_id != coi )
e->par.remove( i );
}
/**
\brief Copy from the internally stored current solution into a final solution.
If preservation of precision is on, this function first returns to the initial precision.
\param[out] solution_at_endtime The solution at the end time
*/
void CopyFinalSolution(Vec<CT> & solution_at_endtime) const override
{
// the current precision is the precision of the output solution point.
unsigned num_vars = this->tracked_system_.NumVariables();
solution_at_endtime.resize(num_vars);
for (unsigned ii=0; ii<num_vars; ii++)
{
solution_at_endtime(ii) = std::get<Vec<CT> >(this->current_space_)(ii);
}
}
void EncodeTriangulation
(
const std::vector<Mat34> & Pi, // Projection matrices
const Mat2X & x_ij, // corresponding observations
double gamma, // Start upper bound
Mat & A,
Vec & C
)
{
// Build A, C matrix.
const size_t nbCamera = Pi.size();
A.resize(5*nbCamera,3);
C.resize(5*nbCamera,1);
int cpt = 0;
for (size_t i = 0; i < nbCamera; ++i)
{
const Mat3 R = Pi[i].block<3,3>(0,0);
const Vec3 t = Pi[i].block<3,1>(0,3);
const Mat2X pt = x_ij.col(i);
// A (Rotational part):
A.block<1,3>(cpt,0) = R.row(0) - pt(0) * R.row(2) - gamma * R.row(2);
A.block<1,3>(cpt+1,0) = R.row(1) - pt(1) * R.row(2) - gamma * R.row(2);
A.block<1,3>(cpt+2,0) = - R.row(2);
A.block<1,3>(cpt+3,0) = - R.row(0) + pt(0) * R.row(2) - gamma * R.row(2);
A.block<1,3>(cpt+4,0) = - R.row(1) + pt(1) * R.row(2) - gamma * R.row(2);
// C (translation part):
C(cpt) = - t(0) + pt(0) * t(2) + gamma * t(2);
C(cpt+1) = - t(1) + pt(1) * t(2) + gamma * t(2);
C(cpt+2) = t(2);
C(cpt+3) = t(0) - pt(0) * t(2) + gamma * t(2);
C(cpt+4) = t(1) - pt(1) * t(2) + gamma * t(2);
//- Next entry
cpt += 5;
}
}
/// Encode translation and structure linear program with slack variables
/// in order to handle noisy measurements.
inline void EncodeTiXi_withNoise(const Mat & M, //Scene representation
const std::vector<Mat3> & Ri,
double sigma, // Start upper bound
sRMat & A, Vec & C,
std::vector<LP_Constraints::eLP_SIGN> & vec_sign,
std::vector<double> & vec_costs,
std::vector< std::pair<double,double> > & vec_bounds)
{
// Build Constraint matrix.
const size_t Ncam = (size_t) M.row(3).maxCoeff()+1;
const size_t N3D = (size_t) M.row(2).maxCoeff()+1;
const size_t Nobs = M.cols();
assert(Ncam == Ri.size());
A.resize(5*Nobs+3, 3 * (Ncam + N3D + Nobs));
C.resize(5 * Nobs + 3, 1);
C.fill(0.0);
vec_sign.resize(5*Nobs+3);
const size_t transStart = 0;
const size_t pointStart = transStart + 3*Ncam;
const size_t offsetStart = pointStart + 3*N3D;
# define TVAR(i, el) (0 + 3*(i) + (el))
# define XVAR(j, el) (pointStart + 3*(j) + (el))
# define OFSVAR(k, el) (offsetStart + 3*(k) + (el))
// Set objective coefficients.
vec_costs = std::vector<double>(3 * (Ncam + N3D + Nobs), 0.0);
for (size_t k = 0; k < Nobs; ++k)
{
vec_costs[OFSVAR(k, 0)] = 1.0;
vec_costs[OFSVAR(k, 1)] = 1.0;
vec_costs[OFSVAR(k, 2)] = 1.0;
}
// By default set free variable:
vec_bounds = std::vector< std::pair<double,double> >(3 * (Ncam + N3D + Nobs));
fill( vec_bounds.begin(), vec_bounds.end(), std::make_pair((double)-1e+30, (double)1e+30));
// Change the offset to be positive
for (size_t k = 0; k < 3*Nobs; ++k)
vec_bounds[offsetStart + k].first = 0;
size_t rowPos = 0;
// Add the cheirality conditions (R_i*X_j + T_i)_3 >= 1
for (size_t k = 0; k < Nobs; ++k)
{
const size_t indexPt3D = M(2,k);
const size_t indexCam = M(3,k);
const Mat3 & R = Ri[indexCam];
A.coeffRef(rowPos, XVAR(indexPt3D, 0)) = R(2,0);
A.coeffRef(rowPos, XVAR(indexPt3D, 1)) = R(2,1);
A.coeffRef(rowPos, XVAR(indexPt3D, 2)) = R(2,2);
A.coeffRef(rowPos, TVAR(indexCam, 2)) = 1.0;
A.coeffRef(rowPos, OFSVAR(k, 2)) = 1.0;
C(rowPos) = 1.0;
vec_sign[rowPos] = LP_Constraints::LP_GREATER_OR_EQUAL;
++rowPos;
} // end for (k)
// Add conic constraint:
for (size_t k = 0; k < Nobs; ++k)
{
const Vec2 pt = M.block<2,1>(0,k);
const double u = pt(0);
const double v = pt(1);
const size_t indexPt3D = M(2,k);
const size_t indexCam = M(3,k);
const Mat3 & R = Ri[indexCam];
// x-residual =>
// (R_i*X_j + T_i)_1 / (R_i*X_j + T_i)_3 - u >= -sigma
// (R_i*X_j + T_i)_1 - u * (R_i*X_j + T_i)_3 + sigma (R_i*X_j + T_i)_3 >= 0.0
// R_i_3 * (sigma-u) + R_i_1 + t_i_1 + t_i_3 * (sigma-u) >= 0
A.coeffRef(rowPos, XVAR(indexPt3D, 0)) = R(0,0) + (sigma-u) * R(2,0);
A.coeffRef(rowPos, XVAR(indexPt3D, 1)) = R(0,1) + (sigma-u) * R(2,1);
A.coeffRef(rowPos, XVAR(indexPt3D, 2)) = R(0,2) + (sigma-u) * R(2,2);
A.coeffRef(rowPos, TVAR(indexCam, 0)) = 1.0;
A.coeffRef(rowPos, TVAR(indexCam, 2)) = sigma-u;
A.coeffRef(rowPos, OFSVAR(k, 0)) = 1.0;
C(rowPos) = 0.0;
vec_sign[rowPos] = LP_Constraints::LP_GREATER_OR_EQUAL;
++rowPos;
A.coeffRef(rowPos, XVAR(indexPt3D, 0)) = R(0,0) - (sigma+u) * R(2,0);
A.coeffRef(rowPos, XVAR(indexPt3D, 1)) = R(0,1) - (sigma+u) * R(2,1);
A.coeffRef(rowPos, XVAR(indexPt3D, 2)) = R(0,2) - (sigma+u) * R(2,2);
A.coeffRef(rowPos, TVAR(indexCam, 0)) = 1.0;
A.coeffRef(rowPos, TVAR(indexCam, 2)) = -(sigma + u);
A.coeffRef(rowPos, OFSVAR(k, 0)) = -1.0;
C(rowPos) = 0.0;
vec_sign[rowPos] = LP_Constraints::LP_LESS_OR_EQUAL;
++rowPos;
// y-residual =>
A.coeffRef(rowPos, XVAR(indexPt3D, 0)) = R(1,0) + (sigma-v) * R(2,0);
A.coeffRef(rowPos, XVAR(indexPt3D, 1)) = R(1,1) + (sigma-v) * R(2,1);
A.coeffRef(rowPos, XVAR(indexPt3D, 2)) = R(1,2) + (sigma-v) * R(2,2);
A.coeffRef(rowPos, OFSVAR(k, 1)) = 1.0;
A.coeffRef(rowPos, TVAR(indexCam, 1)) = 1.0;
A.coeffRef(rowPos, TVAR(indexCam, 2)) = sigma-v;
C(rowPos) = 0.0;
vec_sign[rowPos] = LP_Constraints::LP_GREATER_OR_EQUAL;
++rowPos;
A.coeffRef(rowPos, XVAR(indexPt3D, 0)) = R(1,0) - (sigma+v) * R(2,0);
A.coeffRef(rowPos, XVAR(indexPt3D, 1)) = R(1,1) - (sigma+v) * R(2,1);
A.coeffRef(rowPos, XVAR(indexPt3D, 2)) = R(1,2) - (sigma+v) * R(2,2);
A.coeffRef(rowPos, TVAR(indexCam, 1)) = 1.0;
A.coeffRef(rowPos, TVAR(indexCam, 2)) = -(sigma + v);
A.coeffRef(rowPos, OFSVAR(k, 1)) = -1.0;
C(rowPos) = 0.0;
vec_sign[rowPos] = LP_Constraints::LP_LESS_OR_EQUAL;
++rowPos;
}
// Fix the translation ambiguity. (set first cam at (0,0,0)
//LP_EQUAL
A.coeffRef(rowPos, TVAR(0, 0)) = 1.0; C(rowPos) = 0.0; vec_sign[rowPos] = LP_Constraints::LP_EQUAL; ++rowPos;
A.coeffRef(rowPos, TVAR(0, 1)) = 1.0; C(rowPos) = 0.0; vec_sign[rowPos] = LP_Constraints::LP_EQUAL; ++rowPos;
A.coeffRef(rowPos, TVAR(0, 2)) = 1.0; C(rowPos) = 0.0; vec_sign[rowPos] = LP_Constraints::LP_EQUAL; ++rowPos;
# undef TVAR
# undef XVAR
# undef OFSVAR
}
/// Encode translation and structure linear program
static void EncodeTiXi(const Mat & M, //Scene representation
const std::vector<Mat3> & Ri,
double sigma, // Start upper bound
sRMat & A, Vec & C,
std::vector<LP_Constraints::eLP_SIGN> & vec_sign,
std::vector<double> & vec_costs,
std::vector< std::pair<double,double> > & vec_bounds)
{
// Build Constraint matrix.
const size_t Ncam = (size_t) M.row(3).maxCoeff()+1;
const size_t N3D = (size_t) M.row(2).maxCoeff()+1;
const size_t Nobs = M.cols();
assert(Ncam == Ri.size());
A.resize(5 * Nobs, 3 * (N3D + Ncam));
C.resize(5 * Nobs, 1);
C.fill(0.0);
vec_sign.resize(5 * Nobs + 3);
const size_t transStart = 0;
const size_t pointStart = transStart + 3*Ncam;
# define TVAR(i, el) (0 + 3*(i) + (el))
# define XVAR(j, el) (pointStart + 3*(j) + (el))
// By default set free variable:
vec_bounds = std::vector< std::pair<double,double> >(3 * (N3D + Ncam));
fill( vec_bounds.begin(), vec_bounds.end(), std::make_pair((double)-1e+30, (double)1e+30));
// Fix the translation ambiguity. (set first cam at (0,0,0))
vec_bounds[0] = vec_bounds[1] = vec_bounds[2] = std::make_pair(0,0);
size_t rowPos = 0;
// Add the cheirality conditions (R_i*X_j + T_i)_3 + Z_ij >= 1
for (size_t k = 0; k < Nobs; ++k)
{
const size_t indexPt3D = M(2,k);
const size_t indexCam = M(3,k);
const Mat3 & R = Ri[indexCam];
A.coeffRef(rowPos, XVAR(indexPt3D, 0)) = R(2,0);
A.coeffRef(rowPos, XVAR(indexPt3D, 1)) = R(2,1);
A.coeffRef(rowPos, XVAR(indexPt3D, 2)) = R(2,2);
A.coeffRef(rowPos, TVAR(indexCam, 2)) = 1.0;
C(rowPos) = 1.0;
vec_sign[rowPos] = LP_Constraints::LP_GREATER_OR_EQUAL;
++rowPos;
const Vec2 pt = M.block<2,1>(0,k);
const double u = pt(0);
const double v = pt(1);
// x-residual =>
// (R_i*X_j + T_i)_1 / (R_i*X_j + T_i)_3 - u >= -sigma
// (R_i*X_j + T_i)_1 - u * (R_i*X_j + T_i)_3 + sigma (R_i*X_j + T_i)_3 >= 0.0
// R_i_3 * (sigma-u) + R_i_1 + t_i_1 + t_i_3 * (sigma-u) >= 0
A.coeffRef(rowPos, XVAR(indexPt3D, 0)) = R(0,0) + (sigma-u) * R(2,0);
A.coeffRef(rowPos, XVAR(indexPt3D, 1)) = R(0,1) + (sigma-u) * R(2,1);
A.coeffRef(rowPos, XVAR(indexPt3D, 2)) = R(0,2) + (sigma-u) * R(2,2);
A.coeffRef(rowPos, TVAR(indexCam, 0)) = 1.0;
A.coeffRef(rowPos, TVAR(indexCam, 2)) = sigma-u;
C(rowPos) = 0.0;
vec_sign[rowPos] = LP_Constraints::LP_GREATER_OR_EQUAL;
++rowPos;
A.coeffRef(rowPos, XVAR(indexPt3D, 0)) = R(0,0) - (sigma+u) * R(2,0);
A.coeffRef(rowPos, XVAR(indexPt3D, 1)) = R(0,1) - (sigma+u) * R(2,1);
A.coeffRef(rowPos, XVAR(indexPt3D, 2)) = R(0,2) - (sigma+u) * R(2,2);
A.coeffRef(rowPos, TVAR(indexCam, 0)) = 1.0;
A.coeffRef(rowPos, TVAR(indexCam, 2)) = -(sigma + u);
C(rowPos) = 0.0;
vec_sign[rowPos] = LP_Constraints::LP_LESS_OR_EQUAL;
++rowPos;
// y-residual =>
// (R_i*X_j + T_i)_2 / (R_i*X_j + T_i)_3 - v >= -sigma
// (R_i*X_j + T_i)_2 - v * (R_i*X_j + T_i)_3 + sigma (R_i*X_j + T_i)_3 >= 0.0
// R_i_3 * (sigma-v) + R_i_2 + t_i_2 + t_i_3 * (sigma-v) >= 0
A.coeffRef(rowPos, XVAR(indexPt3D, 0)) = R(1,0) + (sigma-v) * R(2,0);
A.coeffRef(rowPos, XVAR(indexPt3D, 1)) = R(1,1) + (sigma-v) * R(2,1);
A.coeffRef(rowPos, XVAR(indexPt3D, 2)) = R(1,2) + (sigma-v) * R(2,2);
A.coeffRef(rowPos, TVAR(indexCam, 1)) = 1.0;
A.coeffRef(rowPos, TVAR(indexCam, 2)) = sigma-v;
C(rowPos) = 0.0;
vec_sign[rowPos] = LP_Constraints::LP_GREATER_OR_EQUAL;
++rowPos;
A.coeffRef(rowPos, XVAR(indexPt3D, 0)) = R(1,0) - (sigma+v) * R(2,0);
A.coeffRef(rowPos, XVAR(indexPt3D, 1)) = R(1,1) - (sigma+v) * R(2,1);
A.coeffRef(rowPos, XVAR(indexPt3D, 2)) = R(1,2) - (sigma+v) * R(2,2);
A.coeffRef(rowPos, TVAR(indexCam, 1)) = 1.0;
A.coeffRef(rowPos, TVAR(indexCam, 2)) = -(sigma + v);
C(rowPos) = 0.0;
vec_sign[rowPos] = LP_Constraints::LP_LESS_OR_EQUAL;
++rowPos;
}
# undef TVAR
# undef XVAR
}
// Setup the linear program to solve the union of trifocal tensors heading
// directions in a common global coordinate system.
//- Implementation of the LINEAR PROGRAM (9) page 5 of [1]:
//--
static void EncodeTi_from_tij_OneLambdaPerTrif(
const size_t nTranslation,
const std::vector<relativeInfo > & vec_relative,
sRMat & A, Vec & C,
std::vector<LP_Constraints::eLP_SIGN> & vec_sign,
std::vector<double> & vec_costs,
std::vector< std::pair<double,double> > & vec_bounds)
{
// Build Constraint matrix.
const size_t Ncam = (size_t) nTranslation;
const size_t Nrelative = vec_relative.size();
const size_t transStart = 0;
const size_t lambdaStart = 3 * Ncam;
const size_t gammaStart = lambdaStart + Nrelative/3;
#undef TVAR
#undef LAMBDAVAR
#undef GAMMAVAR
# define TVAR(i, el) (transStart + 3*(i) + (el)) // translation (X,Y,Z)
# define LAMBDAVAR(j) (lambdaStart + (int)((j)/3)) // One per relative translation
# define GAMMAVAR gammaStart
const size_t Nconstraint = Nrelative * 6;
const size_t NVar = 3 * Ncam + Nrelative/3 + 1;
A.resize(Nconstraint, NVar);
C.resize(Nconstraint, 1);
C.fill(0.0);
vec_sign.resize(Nconstraint);
// By default set free variable:
vec_bounds = std::vector< std::pair<double,double> >(NVar);
fill( vec_bounds.begin(), vec_bounds.end(),
std::make_pair((double)-1e+30, (double)1e+30));
// Make first camera at origin (translation ambiguity)
vec_bounds[TVAR(0,0)].first = vec_bounds[TVAR(0,0)].second = 0;
vec_bounds[TVAR(0,1)].first = vec_bounds[TVAR(0,1)].second = 0;
vec_bounds[TVAR(0,2)].first = vec_bounds[TVAR(0,2)].second = 0;
// Make lambda variables between 1 and large number => constraint that lambda_ij > 1
for (size_t k = 0; k < Nrelative/3; ++k)
vec_bounds[lambdaStart + k].first = 1;
// Setup gamma >= 0
vec_bounds[vec_bounds.size()-1].first = 0.0;
//-- Minimize gamma
vec_costs.resize(NVar);
std::fill(vec_costs.begin(), vec_costs.end(), 0.0);
vec_costs[GAMMAVAR] = 1.0;
//--
size_t rowPos = 0;
for (size_t k = 0; k < Nrelative; ++k)
{
const size_t i = vec_relative[k].first.first;
const size_t j = vec_relative[k].first.second;
const Mat3 & Rij = vec_relative[k].second.first;
const Vec3 & tij = vec_relative[k].second.second;
// | T_j - R_ij T_i - Lambda_ij t_ij | < Gamma
// Absolute constraint transformed in two sign constraints
// T_j - R_ij T_i - Lambda_ij t_ij < Gamma
// T_j - R_ij T_i - Lambda_ij t_ij > - Gamma
// For X, Y, Z axis:
for (int l = 0; l < 3; ++l)
{
// T_j
A.coeffRef(rowPos, TVAR(j, l)) = 1;
//- R_ij T_i
A.coeffRef(rowPos, TVAR(i, 0)) = - Rij(l, 0);
A.coeffRef(rowPos, TVAR(i, 1)) = - Rij(l, 1);
A.coeffRef(rowPos, TVAR(i, 2)) = - Rij(l, 2);
// - Lambda_ij t_ij
A.coeffRef(rowPos, LAMBDAVAR(k)) = - tij(l);
// - gamma
A.coeffRef(rowPos, GAMMAVAR) = -1;
// < 0
vec_sign[rowPos] = LP_Constraints::LP_LESS_OR_EQUAL;
C(rowPos) = 0;
++rowPos;
// ---------
// Opposite constraint
// T_j - R_ij T_i - Lambda_ij t_ij > - Gamma
// ---------
// T_j
A.coeffRef(rowPos, TVAR(j, l)) = 1;
//- R_ij T_i
A.coeffRef(rowPos, TVAR(i, 0)) = - Rij(l, 0);
A.coeffRef(rowPos, TVAR(i, 1)) = - Rij(l, 1);
A.coeffRef(rowPos, TVAR(i, 2)) = - Rij(l, 2);
// - Lambda_ij t_ij
A.coeffRef(rowPos, LAMBDAVAR(k)) = - tij(l);
// + gamma
A.coeffRef(rowPos, GAMMAVAR) = 1;
// > 0
vec_sign[rowPos] = LP_Constraints::LP_GREATER_OR_EQUAL;
C(rowPos) = 0;
++rowPos;
}
} // end for (k)
#undef TVAR
#undef LAMBDAVAR
#undef GAMMAVAR
}
// Implementation of the formula (1) of [1] with 10 quantiles.
//-- L_infinity alignment of pair of histograms over a graph thanks to a linear program.
static void Encode_histo_relation(
const size_t nImage,
const std::vector<relativeColorHistogramEdge > & vec_relativeHistograms,
const std::vector<size_t> & vec_indexToFix,
sRMat & A, Vec & C,
std::vector<linearProgramming::LP_Constraints::eLP_SIGN> & vec_sign,
std::vector<double> & vec_costs,
std::vector< std::pair<double,double> > & vec_bounds)
{
const size_t Nima = (size_t) nImage;
const size_t Nrelative = vec_relativeHistograms.size();
# define GVAR(i) (2*(i))
# define OFFSETVAR(i) (2*(i)+1)
# define GAMMAVAR (2*Nima)
const size_t nbQuantile = 10;
const size_t Nconstraint = nbQuantile * Nrelative * 2;
const size_t NVar = 2 * Nima+ 1;
A.resize(Nconstraint, NVar);
C.resize(Nconstraint, 1);
C.fill(0.0);
vec_sign.resize(Nconstraint);
// By default set free variable:
vec_bounds = std::vector< std::pair<double,double> >(NVar);
fill( vec_bounds.begin(), vec_bounds.end(),
std::make_pair((double)-1e+30, (double)1e+30));
// Set gain as positive values
for (size_t i = 0; i < Nima; ++i)
{
vec_bounds[GVAR(i)].first = 0.0;
}
//-- Fix the required image to known gain and offset value
for (std::vector<size_t>::const_iterator iter = vec_indexToFix.begin();
iter != vec_indexToFix.end(); ++iter)
{
vec_bounds[GVAR(*iter)] = std::make_pair(1.0, 1.0); // gain = 1.0
vec_bounds[OFFSETVAR(*iter)] = std::make_pair(0.0, 0.0); // offset = 0
}
// Setup gamma >= 0
vec_bounds[GAMMAVAR].first = 0.0;
//-- Minimize gamma
vec_costs.resize(NVar);
std::fill(vec_costs.begin(), vec_costs.end(), 0.0);
vec_costs[GAMMAVAR] = 1.0;
//--
size_t rowPos = 0;
double incrementPourcentile = 1./(double) nbQuantile;
for (size_t i = 0; i < Nrelative; ++i)
{
std::vector<relativeColorHistogramEdge>::const_iterator iter = vec_relativeHistograms.begin();
std::advance(iter, i);
const relativeColorHistogramEdge & edge = *iter;
//-- compute the two cumulated and normalized histogram
const std::vector< size_t > & vec_histoI = edge.histoI;
const std::vector< size_t > & vec_histoJ = edge.histoJ;
const size_t nBuckets = vec_histoI.size();
// Normalize histogram
std::vector<double> ndf_I(nBuckets), ndf_J(nBuckets);
histogram::normalizeHisto(vec_histoI, ndf_I);
histogram::normalizeHisto(vec_histoJ, ndf_J);
// Compute cumulative distribution functions (cdf)
std::vector<double> cdf_I(nBuckets), cdf_J(nBuckets);
histogram::cdf(ndf_I, cdf_I);
histogram::cdf(ndf_J, cdf_J);
double currentPourcentile = 5./100.;
//-- Compute pourcentile and their positions
std::vector<double> vec_pourcentilePositionI, vec_pourcentilePositionJ;
vec_pourcentilePositionI.reserve(1.0/incrementPourcentile);
vec_pourcentilePositionJ.reserve(1.0/incrementPourcentile);
std::vector<double>::const_iterator cdf_I_IterBegin = cdf_I.begin();
std::vector<double>::const_iterator cdf_J_IterBegin = cdf_J.begin();
while( currentPourcentile < 1.0)
{
std::vector<double>::const_iterator iterFI = std::lower_bound(cdf_I.begin(), cdf_I.end(), currentPourcentile);
const size_t positionI = std::distance(cdf_I_IterBegin, iterFI);
std::vector<double>::const_iterator iterFJ = std::lower_bound(cdf_J.begin(), cdf_J.end(), currentPourcentile);
const size_t positionJ = std::distance(cdf_J_IterBegin, iterFJ);
vec_pourcentilePositionI.push_back(positionI);
vec_pourcentilePositionJ.push_back(positionJ);
currentPourcentile += incrementPourcentile;
}
//-- Add the constraints:
// pos * ga + offa - pos * gb - offb <= gamma
// pos * ga + offa - pos * gb - offb >= - gamma
for(size_t k = 0; k < vec_pourcentilePositionI.size(); ++k)
{
A.coeffRef(rowPos, GVAR(edge.I)) = vec_pourcentilePositionI[k];
A.coeffRef(rowPos, OFFSETVAR(edge.I)) = 1.0;
A.coeffRef(rowPos, GVAR(edge.J)) = - vec_pourcentilePositionJ[k];
A.coeffRef(rowPos, OFFSETVAR(edge.J)) = - 1.0;
// - gamma (side change)
A.coeffRef(rowPos, GAMMAVAR) = -1;
// <= gamma
vec_sign[rowPos] = linearProgramming::LP_Constraints::LP_LESS_OR_EQUAL;
C(rowPos) = 0;
++rowPos;
A.coeffRef(rowPos, GVAR(edge.I)) = vec_pourcentilePositionI[k];
A.coeffRef(rowPos, OFFSETVAR(edge.I)) = 1.0;
A.coeffRef(rowPos, GVAR(edge.J)) = - vec_pourcentilePositionJ[k];
A.coeffRef(rowPos, OFFSETVAR(edge.J)) = - 1.0;
// + gamma (side change)
A.coeffRef(rowPos, GAMMAVAR) = 1;
// >= - gamma
vec_sign[rowPos] = linearProgramming::LP_Constraints::LP_GREATER_OR_EQUAL;
C(rowPos) = 0;
++rowPos;
}
}
#undef GVAR
#undef OFFSETVAR
#undef GAMMAVAR
}
void init(){
vw.clear(); vw.resize(N);
pf.clear(); pf.resize(N);
vf.clear(); vf.resize(N);
th.clear(); th.resize(N);
}
