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 PointWithNegativeDepth::Solve(const Mat &x1, const Mat &x2, vector<Mat3> *Es) {
assert(2 == x1.rows());
assert(8 <= x1.cols());
assert(x1.rows() == x2.rows());
assert(x1.cols() == x2.cols());
MatX9 A(x1.cols(), 9);
fundamental::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();
}
Es->push_back(E);
}
void TriangulateNViewAlgebraic(const Mat2X &x,
const std::vector< Mat34 > &Ps,
Vec4 *X) {
const Mat2X::Index nviews = x.cols();
assert(static_cast<size_t>(nviews) == Ps.size());
Mat design(2*nviews, 4);
for (int i = 0; i < nviews; i++) {
design.block<2, 4>(2*i, 0) = SkewMatMinimal(x.col(i)) * Ps[i];
}
Nullspace(&design, X);
}
void TriangulateNView(const Mat2X &x,
const std::vector< Mat34 > &Ps,
Vec4 *X) {
const Mat2X::Index nviews = x.cols();
assert(static_cast<size_t>(nviews) == Ps.size());
Mat design = Mat::Zero(3*nviews, 4 + nviews);
for (int i = 0; i < nviews; i++) {
design.block<3, 4>(3*i, 0) = -Ps[i];
design(3*i + 0, 4 + i) = x(0, i);
design(3*i + 1, 4 + i) = x(1, i);
design(3*i + 2, 4 + i) = 1.0;
}
Vec X_and_alphas;
Nullspace(&design, &X_and_alphas);
*X = X_and_alphas.head(4);
}
double Nullspace
(
const Eigen::Ref<const Mat> & A,
Eigen::Ref<Vec> nullspace
)
{
if ( A.rows() >= A.cols() )
{
Eigen::JacobiSVD<Mat> svd( A, Eigen::ComputeFullV );
nullspace = svd.matrixV().col( A.cols() - 1 );
return svd.singularValues()( A.cols() - 1 );
}
// Extend A with rows of zeros to make it square. It's a hack, but it is
// necessary until Eigen supports SVD with more columns than rows.
Mat A_extended( A.cols(), A.cols() );
A_extended.block( A.rows(), 0, A.cols() - A.rows(), A.cols() ).setZero();
A_extended.block( 0, 0, A.rows(), A.cols() ) = A;
return Nullspace( A_extended, nullspace );
}
