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