예제 #1
0
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);
 }
예제 #4
0
파일: symamd.cpp 프로젝트: mgenet/LMTpp
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;
}
예제 #5
0
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);
				}

			}
예제 #7
0
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
}
예제 #9
0
/// 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
}
예제 #12
0
파일: 2592.cpp 프로젝트: arrows-1011/CPro
void init(){
    vw.clear(); vw.resize(N);
    pf.clear(); pf.resize(N);
    vf.clear(); vf.resize(N);
    th.clear(); th.resize(N);
}