Triangulation.hpp

// This file is part of the AliceVision project.
// Copyright (c) 2016 AliceVision contributors.
// Copyright (c) 2012 openMVG contributors.
// Copyright (c) 2010 libmv contributors.
// This Source Code Form is subject to the terms of the Mozilla Public License,
// v. 2.0. If a copy of the MPL was not distributed with this file,
// You can obtain one at https://mozilla.org/MPL/2.0/.
 
#pragma once
 
#include <aliceVision/numeric/numeric.hpp>
#include <aliceVision/robustEstimation/ISolver.hpp>
#include <aliceVision/numeric/algebra.hpp>
 
#include <vector>
#include <random>
 
namespace aliceVision {
namespace multiview {
 
void TriangulateNView(const Mat2X& x, const std::vector<Mat34>& Ps, Vec4& X, const std::vector<double>* weights = nullptr);
 
 
template<class ContainerT>
void TriangulateNViewAlgebraic(const ContainerT& x, const std::vector<Mat34>& Ps, Vec4& X, const std::vector<double>* weights = nullptr)
 
{
    Mat2X::Index nviews = CountElements(x);
    assert(static_cast<std::size_t>(nviews) == Ps.size());
 
    Mat design(2 * nviews, 4);
    for (Mat2X::Index i = 0; i < nviews; ++i)
    {
        design.block<2, 4>(2 * i, 0) = SkewMatMinimal(getElement<ContainerT>(x, i)) * Ps[i];
        if (weights != nullptr)
        {
            design.block<2, 4>(2 * i, 0) *= (*weights)[i];
        }
    }
    Nullspace(design, X);
}
 
void TriangulateNViewAlgebraicSpherical(const std::vector<Vec3> &xs, 
                               const std::vector<Eigen::Matrix4d> & Ts,
                               Vec4 & X, 
                               const std::vector<double> *weights = nullptr);
 
void TriangulateNViewLORANSAC(const std::vector<Vec2>& x,
                              const std::vector<Mat34>& Ps,
                              std::mt19937& generator,
                              Vec4& X,
                              std::vector<std::size_t>* inliersIndex = NULL,
                              const double& thresholdError = 4.0);
 
// Iterated linear method
 
class Triangulation
{
  public:
    std::size_t size() const { return views.size(); }
 
    void clear() { views.clear(); }
 
    void add(const Mat34& projMatrix, const Vec2& p) { views.emplace_back(projMatrix, p); }
 
    // Return squared L2 sum of error
    double error(const Vec3& X) const;
 
    Vec3 compute(int iter = 3) const;
 
    // Accessors
 
    // These values are defined after a successful call to compute
    double minDepth() const { return zmin; }
    double maxDepth() const { return zmax; }
    double error() const { return err; }
 
    // Data members
 
  protected:
    mutable double zmin;                        // min depth, mutable since modified in compute(...) const;
    mutable double zmax;                        // max depth, mutable since modified in compute(...) const;
    mutable double err;                         // re-projection error, mutable since modified in compute(...) const;
    std::vector<std::pair<Mat34, Vec2>> views;  // Proj matrix and associated image point
};
 
template<class ContainerT>
struct TriangulateNViewsSolver
{
    inline std::size_t getMinimumNbRequiredSamples() const { return 2; }
 
    inline std::size_t getMaximumNbModels() const { return 1; }
 
    void solve(const ContainerT& x, const std::vector<Mat34>& Ps, std::vector<robustEstimation::MatrixModel<Vec4>>& X) const
    {
        Vec4 pt3d;
        TriangulateNViewAlgebraic(x, Ps, pt3d);
        X.push_back(robustEstimation::MatrixModel<Vec4>(pt3d));
        assert(X.size() == 1);
    }
 
    void solve(const ContainerT& x,
               const std::vector<Mat34>& Ps,
               std::vector<robustEstimation::MatrixModel<Vec4>>& X,
               const std::vector<double>& weights) const
    {
        Vec4 pt3d;
        TriangulateNViewAlgebraic(x, Ps, pt3d, &weights);
        X.push_back(robustEstimation::MatrixModel<Vec4>(pt3d));
        assert(X.size() == 1);
    }
};
 
struct TriangulateNViewsSphericalSolver 
{
    inline std::size_t getMinimumNbRequiredSamples() const
    {
        return 2;
    }
 
    inline std::size_t getMaximumNbModels() const
    {
        return 1;
    }
 
    void solve(const std::vector<Vec3> & x, const std::vector<Eigen::Matrix4d>& Ts, std::vector<robustEstimation::MatrixModel<Vec4>>& X) const
    {
        Vec4 pt3d;
        TriangulateNViewAlgebraicSpherical(x, Ts, pt3d);
        X.push_back(robustEstimation::MatrixModel<Vec4>(pt3d));
        assert(X.size() == 1);
    }
 
 
 
    void solve(const std::vector<Vec3> & x, const std::vector<Eigen::Matrix4d>& Ts, std::vector<robustEstimation::MatrixModel<Vec4>>& X, const std::vector<double>& weights) const
    {
        Vec4 pt3d;
        TriangulateNViewAlgebraicSpherical(x, Ts, pt3d, &weights);
        X.push_back(robustEstimation::MatrixModel<Vec4>(pt3d));
        assert(X.size() == 1);
    }
 
};
 
}  // namespace multiview
}  // namespace aliceVision