IGL_INLINE void igl::slice_into(
const Mat& X,
const Eigen::Matrix<int,Eigen::Dynamic,1> & R,
const int dim,
Mat& Y)
{
Eigen::VectorXi C;
switch(dim)
{
case 1:
assert(R.size() == X.rows());
// boring base case
if(X.cols() == 0)
{
return;
}
igl::colon(0,X.cols()-1,C);
return slice_into(X,R,C,Y);
case 2:
assert(R.size() == X.cols());
// boring base case
if(X.rows() == 0)
{
return;
}
igl::colon(0,X.rows()-1,C);
return slice_into(X,C,R,Y);
default:
assert(false && "Unsupported dimension");
return;
}
}
ACKernelAdaptor(
const Mat &x1, int w1, int h1,
const Mat &x2, int w2, int h2, bool bPointToLine = true)
: x1_(x1.rows(), x1.cols()), x2_(x2.rows(), x2.cols()),
N1_(3,3), N2_(3,3), logalpha0_(0.0), bPointToLine_(bPointToLine)
{
assert(2 == x1_.rows());
assert(x1_.rows() == x2_.rows());
assert(x1_.cols() == x2_.cols());
NormalizePoints(x1, &x1_, &N1_, w1, h1);
NormalizePoints(x2, &x2_, &N2_, w2, h2);
// LogAlpha0 is used to make error data scale invariant
if(bPointToLine) {
// Ratio of containing diagonal image rectangle over image area
double D = sqrt(w2*(double)w2 + h2*(double)h2); // diameter
double A = w2*(double)h2; // area
logalpha0_ = log10(2.0*D/A /N2_(0,0));
}
else {
// ratio of area : unit circle over image area
logalpha0_ = log10(M_PI/(w2*(double)h2) /(N2_(0,0)*N2_(0,0)));
}
}
inline
void
op_trapz::apply_noalias(Mat<eT>& out, const Mat<eT>& Y, const uword dim)
{
arma_extra_debug_sigprint();
arma_debug_check( (dim > 1), "trapz(): argument 'dim' must be 0 or 1" );
uword N = 0;
if(dim == 0) { N = Y.n_rows; }
else if(dim == 1) { N = Y.n_cols; }
if(N <= 1)
{
if(dim == 0) { out.zeros(1, Y.n_cols); }
else if(dim == 1) { out.zeros(Y.n_rows, 1); }
return;
}
if(dim == 0)
{
out = sum( (0.5 * (Y.rows(0, N-2) + Y.rows(1, N-1))), 0 );
}
else
if(dim == 1)
{
out = sum( (0.5 * (Y.cols(0, N-2) + Y.cols(1, N-1))), 1 );
}
}
static void Solve(const Mat &x1, const Mat &x2, std::vector<Mat3> *pvec_E)
{
assert(3 == x1.rows());
assert(8 <= x1.cols());
assert(x1.rows() == x2.rows());
assert(x1.cols() == x2.cols());
MatX9 A(x1.cols(), 9);
EncodeEpipolarEquation(x1, x2, &A);
Vec9 e;
Nullspace(&A, &e);
Mat3 E = Map<RMat3>(e.data());
// Find the closest essential matrix to E in frobenius norm
// E = UD'VT
if (x1.cols() > 8) {
Eigen::JacobiSVD<Mat3> USV(E, Eigen::ComputeFullU | Eigen::ComputeFullV);
Vec3 d = USV.singularValues();
double a = d[0];
double b = d[1];
d << (a+b)/2., (a+b)/2., 0.0;
E = USV.matrixU() * d.asDiagonal() * USV.matrixV().transpose();
}
pvec_E->push_back(E);
}
void FourPointSolver::Solve(const Mat &x, const Mat &y, vector<Mat3> *Hs) {
assert(2 == x.rows());
assert(4 <= x.cols());
assert(x.rows() == y.rows());
assert(x.cols() == y.cols());
Mat::Index n = x.cols();
Vec9 h;
if (n == 4) {
// In the case of minimal configuration we use fixed sized matrix to let
// Eigen and the compiler doing the maximum of optimization.
typedef Eigen::Matrix<double, 16, 9> Mat16_9;
Mat16_9 L = Mat::Zero(16, 9);
BuildActionMatrix(L, x, y);
Nullspace(&L, &h);
}
else {
MatX9 L = Mat::Zero(n * 2, 9);
BuildActionMatrix(L, x, y);
Nullspace(&L, &h);
}
Mat3 H = Map<RMat3>(h.data()); // map the linear vector as the H matrix
Hs->push_back(H);
}
void FivePointSolver::Solve(const Mat &x1, const Mat &x2, vector<Mat3> *E) {
assert(2 == x1.rows());
assert(5 <= x1.cols());
assert(x1.rows() == x2.rows());
assert(x1.cols() == x2.cols());
FivePointsRelativePose(x1, x2, E);
}
/** 3D rigid transformation refinement using LM
* Refine the Scale, Rotation and translation rigid transformation
* using a Levenberg-Marquardt opimization.
*
* \param[in] x1 The first 3xN matrix of euclidean points
* \param[in] x2 The second 3xN matrix of euclidean points
* \param[out] S The initial scale factor
* \param[out] t The initial 3x1 translation
* \param[out] R The initial 3x3 rotation
*
* \return none
*/
static void Refine_RTS( const Mat &x1,
const Mat &x2,
double * S,
Vec3 * t,
Mat3 * R )
{
{
lm_SRTRefine_functor functor( 7, 3 * x1.cols(), x1, x2, *S, *R, *t );
Eigen::NumericalDiff<lm_SRTRefine_functor> numDiff( functor );
Eigen::LevenbergMarquardt<Eigen::NumericalDiff<lm_SRTRefine_functor> > lm( numDiff );
lm.parameters.maxfev = 1000;
Vec xlm = Vec::Zero( 7 ); // The deviation vector {tx,ty,tz,rotX,rotY,rotZ,S}
lm.minimize( xlm );
Vec3 transAdd = xlm.block<3, 1>( 0, 0 );
Vec3 rot = xlm.block<3, 1>( 3, 0 );
double SAdd = xlm( 6 );
//Build the rotation matrix
Mat3 Rcor =
( Eigen::AngleAxis<double>( rot( 0 ), Vec3::UnitX() )
* Eigen::AngleAxis<double>( rot( 1 ), Vec3::UnitY() )
* Eigen::AngleAxis<double>( rot( 2 ), Vec3::UnitZ() ) ).toRotationMatrix();
*R = ( *R ) * Rcor;
*t = ( *t ) + transAdd;
*S = ( *S ) + SAdd;
}
// Refine rotation
{
lm_RRefine_functor functor( 3, 3 * x1.cols(), x1, x2, *S, *R, *t );
Eigen::NumericalDiff<lm_RRefine_functor> numDiff( functor );
Eigen::LevenbergMarquardt<Eigen::NumericalDiff<lm_RRefine_functor> > lm( numDiff );
lm.parameters.maxfev = 1000;
Vec xlm = Vec::Zero( 3 ); // The deviation vector {rotX,rotY,rotZ}
lm.minimize( xlm );
Vec3 rot = xlm.block<3, 1>( 0, 0 );
//Build the rotation matrix
Mat3 Rcor =
( Eigen::AngleAxis<double>( rot( 0 ), Vec3::UnitX() )
* Eigen::AngleAxis<double>( rot( 1 ), Vec3::UnitY() )
* Eigen::AngleAxis<double>( rot( 2 ), Vec3::UnitZ() ) ).toRotationMatrix();
*R = ( *R ) * Rcor;
}
}
void Fit(const vector<size_t> &samples, std::vector<Model> *models) const {
const Mat x1 = ExtractColumns(x1_, samples);
const Mat x2 = ExtractColumns(x2_, samples);
assert(3 == x1.rows());
assert(MINIMUM_SAMPLES <= x1.cols());
assert(x1.rows() == x2.rows());
assert(x1.cols() == x2.cols());
EightPointRelativePoseSolver::Solve(x1, x2, models);
}
List DataProps(const Mat<T>& dat, IntegerVector subsetIndices) {
Mat<T> U, V;
Col<T> S;
uvec subsetCols = sort(as<uvec>(subsetIndices) - 1);
// Get the summary profile for the network subset from the SVD.
bool success = svd_econ(U, S, V, dat.cols(subsetCols), "left", "dc");
if (!success) {
Function warning("warning");
warning("SVD failed to converge, does your data contain missing or"
" infinite values?");
return List::create(
Named("moduleSummary") = NA_REAL,
Named("nodeContribution") = NA_REAL,
Named("moduleCoherence") = NA_REAL
);
}
Mat<T> summary(U.col(0));
// Flip the sign of the summary profile so that the eigenvector is
// positively correlated with the average scaled value of the underlying
// data for the network module.
Mat<T> ap = cor(mean(dat.cols(subsetCols), 1), summary);
if (ap(0,0) < 0) {
summary *= -1;
}
// We want the correlation between each variable (node) in the underlying
// data and the summary profile for that network subset.
Mat<T> p = cor(summary, dat.cols(subsetCols));
Mat<T> MM(p);
// To make sure the resulting KIM vector is in the correct order,
// order the results to match the original ordering of subsetIndices.
Function rank("rank"); // Rank only works on R objects like IntegerVector.
uvec idxRank = as<uvec>(rank(subsetIndices)) - 1;
Col<T> oMM = MM(idxRank);
// The proportion of variance explained is the sum of the squared
// correlation between the network module summary profile, and each of the
// variables in the data that correspond to nodes in the network subset.
Mat<T> pve(mean(square(p), 1));
return List::create(
Named("moduleSummary") = summary,
Named("nodeContribution") = oMM,
Named("moduleCoherence") = pve
);
}
// Check the properties of a fundamental matrix:
//
// 1. The determinant is 0 (rank deficient)
// 2. The condition x'T*F*x = 0 is satisfied to precision.
//
bool ExpectFundamentalProperties(const Mat3 &F,
const Mat &ptsA,
const Mat &ptsB,
double precision) {
bool bOk = true;
bOk &= F.determinant() < precision;
assert(ptsA.cols() == ptsB.cols());
const Mat hptsA = ptsA.colwise().homogeneous();
const Mat hptsB = ptsB.colwise().homogeneous();
for (int i = 0; i < ptsA.cols(); ++i) {
const double residual = hptsB.col(i).dot(F * hptsA.col(i));
bOk &= residual < precision;
}
return bOk;
}
TEST(Resection_Kernel, Multiview) {
const int views_num = 3;
const int points_num = 10;
const NViewDataSet d = NRealisticCamerasRing(views_num, points_num,
NViewDatasetConfigurator(1,1,0,0,5,0)); // Suppose a camera with Unit matrix as K
const int nResectionCameraIndex = 2;
// Solve the problem and check that fitted value are good enough
{
Mat x = d.projected_points_[nResectionCameraIndex];
Mat X = d.point_3d_;
mvg::multiview::resection::PoseResectionKernel kernel(x, X);
size_t samples_[6]={0,1,2,3,4,5};
std::vector<size_t> samples(samples_, samples_ + 6);
std::vector<Mat34> Ps;
kernel.Fit(samples, &Ps);
for (size_t i = 0; i < x.cols(); ++i) {
EXPECT_NEAR(0.0, kernel.Error(i, Ps[0]), 1e-8);
}
EXPECT_EQ(1, Ps.size());
// Check that Projection matrix is near to the GT :
Mat34 GT_ProjectionMatrix = d.P(nResectionCameraIndex).array()
/ d.P(nResectionCameraIndex).norm();
Mat34 COMPUTED_ProjectionMatrix = Ps[0].array() / Ps[0].norm();
EXPECT_MATRIX_NEAR(GT_ProjectionMatrix, COMPUTED_ProjectionMatrix, 1e-8);
}
}
/**
* sampleGoodBasis - Creates and returns a good basis for input argument data.
* Uses the left eigenvectors of a random subset of nSample columns from data.
*/
static fmat sampleGoodBasis(const Mat<u16> &data, const int nSample) {
const int L = data.n_rows;
// random subset of columns:
fmat sampleData = conv_to<fmat>::from( data.cols( linspace<uvec>(0, data.n_cols-1, nSample) ) );
// make a transformation matrix that represents taking adjacent differences
// between each row (except the first row):
mat P = eye(L, L);
P.diag(-1) = -ones(L-1);
// same as sampleData = P * sampleData (adjacent differences between rows)
sampleData.rows(1, L-1) -= sampleData.rows(0, L-2);
// this is the time consuming step
// numerical stable with floats because values are much smaller now,
// convert resulting small matrix to double when done:
mat M = conv_to<mat>::from(sampleData * sampleData.t());
// redo the effect of P before calculating eigenvectors
M = P.i() * M * P.t().i(); // M is now original sampleData * sampleData.t()
vec eigenvalues;
mat eigenvectors;
eig_sym(eigenvalues, eigenvectors, M);
// flip because eigenvectors are returned in ascending order:
return conv_to<fmat>::from( fliplr(eigenvectors) );
}
void EuclideanToHomogeneous(const Mat &X, Mat *H) {
Mat::Index d = X.rows();
Mat::Index n = X.cols();
H->resize(d + 1, n);
H->block(0, 0, d, n) = X;
H->row(d).setOnes();
}
bool ExpectFundamentalProperties(const TMat &F,
const Mat &ptsA,
const Mat &ptsB,
double precision) {
bool bOk = true;
bOk &= F.determinant() < precision;
assert(ptsA.cols() == ptsB.cols());
Mat hptsA, hptsB;
EuclideanToHomogeneous(ptsA, &hptsA);
EuclideanToHomogeneous(ptsB, &hptsB);
for (int i = 0; i < ptsA.cols(); ++i) {
double residual = hptsB.col(i).dot(F * hptsA.col(i));
bOk &= residual < precision;
}
return bOk;
}
inline
void
glue_trapz::apply_noalias(Mat<eT>& out, const Mat<eT>& X, const Mat<eT>& Y, const uword dim)
{
arma_extra_debug_sigprint();
arma_debug_check( (dim > 1), "trapz(): argument 'dim' must be 0 or 1" );
arma_debug_check( ((X.is_vec() == false) && (X.is_empty() == false)), "trapz(): argument 'X' must be a vector" );
const uword N = X.n_elem;
if(dim == 0)
{
arma_debug_check( (N != Y.n_rows), "trapz(): length of X must equal the number of rows in Y when dim=0" );
}
else
if(dim == 1)
{
arma_debug_check( (N != Y.n_cols), "trapz(): length of X must equal the number of columns in Y when dim=1" );
}
if(N <= 1)
{
if(dim == 0) { out.zeros(1, Y.n_cols); }
else if(dim == 1) { out.zeros(Y.n_rows, 1); }
return;
}
const Col<eT> vec_X( const_cast<eT*>(X.memptr()), X.n_elem, false, true );
const Col<eT> diff_X = diff(vec_X);
if(dim == 0)
{
const Row<eT> diff_X_t( const_cast<eT*>(diff_X.memptr()), diff_X.n_elem, false, true );
out = diff_X_t * (0.5 * (Y.rows(0, N-2) + Y.rows(1, N-1)));
}
else
if(dim == 1)
{
out = (0.5 * (Y.cols(0, N-2) + Y.cols(1, N-1))) * diff_X;
}
}
void Fit
(
const std::vector<size_t> &samples,
std::vector<ModelArg> *models
)
const
{
const Mat pt2d = ExtractColumns(this->x1_, samples);
const Mat pt3d = ExtractColumns(this->x2_, samples);
assert(2 == pt2d.rows());
assert(3 == pt3d.rows());
assert(SolverArg::MINIMUM_SAMPLES <= pt2d.cols());
assert(pt2d.cols() == pt3d.cols());
SolverArg::Solve(pt2d, pt3d, models);
}
inline
bool
op_princomp::direct_princomp
(
Mat<typename T1::elem_type>& coeff_out,
Mat<typename T1::elem_type>& score_out,
const Base<typename T1::elem_type, T1>& X,
const typename arma_not_cx<typename T1::elem_type>::result* junk
)
{
arma_extra_debug_sigprint();
arma_ignore(junk);
typedef typename T1::elem_type eT;
const unwrap_check<T1> Y( X.get_ref(), score_out );
const Mat<eT>& in = Y.M;
const uword n_rows = in.n_rows;
const uword n_cols = in.n_cols;
if(n_rows > 1) // more than one sample
{
// subtract the mean - use score_out as temporary matrix
score_out = in; score_out.each_row() -= mean(in);
// singular value decomposition
Mat<eT> U;
Col<eT> s;
const bool svd_ok = svd(U, s, coeff_out, score_out);
if(svd_ok == false) { return false; }
// normalize the eigenvalues
s /= std::sqrt( double(n_rows - 1) );
// project the samples to the principals
score_out *= coeff_out;
if(n_rows <= n_cols) // number of samples is less than their dimensionality
{
score_out.cols(n_rows-1,n_cols-1).zeros();
Col<eT> s_tmp = zeros< Col<eT> >(n_cols);
s_tmp.rows(0,n_rows-2) = s.rows(0,n_rows-2);
s = s_tmp;
}
}
else // 0 or 1 samples
{
coeff_out.eye(n_cols, n_cols);
score_out.copy_size(in);
score_out.zeros();
}
return true;
}
static void Solve(const Mat &x1, const Mat &x2, vector<ModelArg> *models) {
assert(2 == x1.rows());
assert(MINIMUM_SAMPLES <= x1.cols());
assert(x1.rows() == x2.rows());
assert(x1.cols() == x2.cols());
// Normalize the data.
Mat3 T1, T2;
Mat x1_normalized, x2_normalized;
NormalizePoints(x1, &x1_normalized, &T1);
NormalizePoints(x2, &x2_normalized, &T2);
SolverArg::Solve(x1_normalized, x2_normalized, models);
// Unormalize model from the computed conditioning.
for (int i = 0; i < models->size(); ++i) {
UnnormalizerArg::Unnormalize(T1, T2, &(*models)[i]);
}
}
inline
bool
op_princomp::direct_princomp
(
Mat< std::complex<T> >& coeff_out,
Mat< std::complex<T> >& score_out,
const Mat< std::complex<T> >& in
)
{
arma_extra_debug_sigprint();
typedef std::complex<T> eT;
const u32 n_rows = in.n_rows;
const u32 n_cols = in.n_cols;
if(n_rows > 1) // more than one sample
{
// subtract the mean - use score_out as temporary matrix
score_out = in - repmat(mean(in), n_rows, 1);
// singular value decomposition
Mat<eT> U;
Col< T> s;
const bool svd_ok = svd(U,s,coeff_out,score_out);
if(svd_ok == false)
{
return false;
}
// U.reset();
// normalize the eigenvalues
s /= std::sqrt( double(n_rows - 1) );
// project the samples to the principals
score_out *= coeff_out;
if(n_rows <= n_cols) // number of samples is less than their dimensionality
{
score_out.cols(n_rows-1,n_cols-1).zeros();
}
}
else // 0 or 1 samples
{
coeff_out.eye(n_cols, n_cols);
score_out.copy_size(in);
score_out.zeros();
}
return true;
}
/** 3D rigid transformation estimation (7 dof)
* Compute a Scale Rotation and Translation rigid transformation.
* This transformation provide a distortion-free transformation
* using the following formulation Xb = S * R * Xa + t.
* "Least-squares estimation of transformation parameters between two point patterns",
* Shinji Umeyama, PAMI 1991, DOI: 10.1109/34.88573
*
* \param[in] x1 The first 3xN matrix of euclidean points
* \param[in] x2 The second 3xN matrix of euclidean points
* \param[out] S The scale factor
* \param[out] t The 3x1 translation
* \param[out] R The 3x3 rotation
*
* \return true if the transformation estimation has succeeded
*
* \note Need at least 3 points
*/
static bool FindRTS( const Mat &x1,
const Mat &x2,
double * S,
Vec3 * t,
Mat3 * R )
{
if ( x1.cols() < 3 || x2.cols() < 3 )
{
return false;
}
assert( 3 == x1.rows() );
assert( 3 <= x1.cols() );
assert( x1.rows() == x2.rows() );
assert( x1.cols() == x2.cols() );
// Get the transformation via Umeyama's least squares algorithm. This returns
// a matrix of the form:
// [ s * R t]
// [ 0 1]
// from which we can extract the scale, rotation, and translation.
const Eigen::Matrix4d transform = Eigen::umeyama( x1, x2, true );
// Check critical cases
*R = transform.topLeftCorner<3, 3>();
if ( R->determinant() < 0 )
{
return false;
}
*S = pow( R->determinant(), 1.0 / 3.0 );
// Check for degenerate case (if all points have the same value...)
if ( *S < std::numeric_limits<double>::epsilon() )
{
return false;
}
// Extract transformation parameters
*S = pow( R->determinant(), 1.0 / 3.0 );
*R /= *S;
*t = transform.topRightCorner<3, 1>();
return true;
}
ACKernelAdaptorResection(const Mat &x2d, int w, int h, const Mat &x3D)
: x2d_(x2d.rows(), x2d.cols()), x3D_(x3D),
N1_(3,3), logalpha0_(log10(M_PI))
{
assert(2 == x2d_.rows());
assert(3 == x3D_.rows());
assert(x2d_.cols() == x3D_.cols());
NormalizePoints(x2d, &x2d_, &N1_, w, h);
}
IGL_INLINE void igl::polar_svd3x3(const Mat& A, Mat& R)
{
// should be caught at compile time, but just to be 150% sure:
assert(A.rows() == 3 && A.cols() == 3);
Eigen::Matrix<typename Mat::Scalar, 3, 3> U, Vt;
Eigen::Matrix<typename Mat::Scalar, 3, 1> S;
svd3x3(A, U, S, Vt);
R = U * Vt.transpose();
}
// Check whether homography transform M actually transforms
// given vectors x1 to x2. Used to check validness of a reconstructed
// homography matrix.
// TODO(sergey): Consider using this in all tests since possible homography
// matrix is not fixed to a single value and different-looking matrices
// might actually crrespond to the same exact transform.
void CheckHomography2DTransform(const Mat3 &H,
const Mat &x1,
const Mat &x2) {
for (int i = 0; i < x2.cols(); ++i) {
Vec3 x2_expected = x2.col(i);
Vec3 x2_observed = H * x1.col(i);
x2_observed /= x2_observed(2);
EXPECT_MATRIX_NEAR(x2_expected, x2_observed, 1e-8);
}
}
static void Solve(const Mat &x, std::vector<Vec2> *lines)
{
Mat X(x.cols(), 2);
X.col(0).setOnes();
X.col(1) = x.row(0).transpose();
Mat A(X.transpose() * X);
const Vec b(X.transpose() * x.row(1).transpose());
Eigen::JacobiSVD<Mat> svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV);
lines->push_back(svd.solve(b));
}
static void planarToSpherical(
const Mat & planarCoords, // Input (x,y)' coords
size_t width, // Width of the 2D planar surface
size_t height, // Height of the 2D planar surface
Mat & sphericalCoords) // Output spherical coordinates (on the unit sphere)
{
sphericalCoords.resize(3, planarCoords.cols());
for (size_t iCol = 0; iCol < planarCoords.cols(); ++iCol)
{
const Vec2 & xy = planarCoords.col(iCol);
double uval = xy(0) / (double)width;
double vval = xy(1) / (double)height;
sphericalCoords.col(iCol) =
(Vec3 (sin(vval*M_PI)*cos(M_PI*(2.0*uval+0.5)),
cos(vval*M_PI),
sin(vval*M_PI)*sin(M_PI*(2.0*uval+0.5)))).normalized();
}
}
/// Solve the computation of the "tensor".
static void Solve(
const Mat &pt0, const Mat & pt1, const Mat & pt2,
const std::vector<Mat3> & vec_KR, std::vector<TrifocalTensorModel> *P,
const double ThresholdUpperBound)
{
//Build the megaMatMatrix
const int n_obs = pt0.cols();
Mat4X megaMat(4, n_obs*3);
{
size_t cpt = 0;
for (size_t i = 0; i < n_obs; ++i) {
megaMat.col(cpt) << pt0.col(i)(0), pt0.col(i)(1), (double)i, 0.0;
++cpt;
megaMat.col(cpt) << pt1.col(i)(0), pt1.col(i)(1), (double)i, 1.0;
++cpt;
megaMat.col(cpt) << pt2.col(i)(0), pt2.col(i)(1), (double)i, 2.0;
++cpt;
}
}
//-- Solve the LInfinity translation and structure from Rotation and points data.
std::vector<double> vec_solution((3 + MINIMUM_SAMPLES)*3);
using namespace openMVG::lInfinityCV;
#ifdef OPENMVG_HAVE_MOSEK
MOSEK_SolveWrapper LPsolver(static_cast<int>(vec_solution.size()));
#else
OSI_CLP_SolverWrapper LPsolver(static_cast<int>(vec_solution.size()));
#endif
Translation_Structure_L1_ConstraintBuilder cstBuilder(vec_KR, megaMat);
double gamma;
if (BisectionLP<Translation_Structure_L1_ConstraintBuilder, LP_Constraints_Sparse>(
LPsolver,
cstBuilder,
&vec_solution,
ThresholdUpperBound,
0.0, 1e-8, 2, &gamma, false))
{
std::vector<Vec3> vec_tis(3);
vec_tis[0] = Vec3(vec_solution[0], vec_solution[1], vec_solution[2]);
vec_tis[1] = Vec3(vec_solution[3], vec_solution[4], vec_solution[5]);
vec_tis[2] = Vec3(vec_solution[6], vec_solution[7], vec_solution[8]);
TrifocalTensorModel PTemp;
PTemp.P1 = HStack(vec_KR[0], vec_tis[0]);
PTemp.P2 = HStack(vec_KR[1], vec_tis[1]);
PTemp.P3 = HStack(vec_KR[2], vec_tis[2]);
P->push_back(PTemp);
}
}
void HomogeneousToEuclidean(const Mat &H, Mat *X) {
Mat::Index d = H.rows() - 1;
Mat::Index n = H.cols();
X->resize(d, n);
for (Mat::Index i = 0; i < n; ++i) {
double h = H(d, i);
for (int j = 0; j < d; ++j) {
(*X)(j, i) = H(j, i) / h;
}
}
}
void run(Mat& A, const int rank){
if (A.cols() == 0 || A.rows() == 0) return;
int r = (rank < A.cols()) ? rank : A.cols();
r = (r < A.rows()) ? r : A.rows();
// Gaussian Random Matrix
Eigen::MatrixXf O(A.rows(), r);
Util::sampleGaussianMat(O);
// Compute Sample Matrix of A
Eigen::MatrixXf Y = A.transpose() * O;
// Orthonormalize Y
Util::processGramSchmidt(Y);
Eigen::MatrixXf B = Y.transpose() * A * Y;
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> eigenOfB(B);
eigenValues_ = eigenOfB.eigenvalues();
eigenVectors_ = Y * eigenOfB.eigenvectors();
}
void applyTransformationToPoints(const Mat &points, const Mat3 &T, Mat *transformed_points)
{
const Mat::Index n = points.cols();//Mat::Index 索引的类型,点的个数
transformed_points->resize(2, n);//设置变换之后的矩阵
for (Mat::Index i = 0; i < n; ++i) {
Vec3 in, out;
in << points(0, i), points(1, i), 1;
out = T * in;
(*transformed_points)(0, i) = out(0) / out(2);
(*transformed_points)(1, i) = out(1) / out(2);
}
}
ACKernelAdaptorResection_K(const Mat &x2d, const Mat &x3D, const Mat3 & K)
: x2d_(x2d.rows(), x2d.cols()), x3D_(x3D),
N1_(K.inverse()),
logalpha0_(log10(M_PI)), K_(K)
{
assert(2 == x2d_.rows());
assert(3 == x3D_.rows());
assert(x2d_.cols() == x3D_.cols());
// Normalize points by inverse(K)
ApplyTransformationToPoints(x2d, N1_, &x2d_);
}
