PointFittingKernel.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/conditioning.hpp>
#include <aliceVision/robustEstimation/ISolver.hpp>
 
#include <vector>
#include <cassert>
 
namespace aliceVision {
namespace robustEstimation {
 
template<typename SolverT_, typename ErrorT_, typename ModelT_ = Mat3Model>
class PointFittingKernel
{
  public:
    using SolverT = SolverT_;
    using ErrorT = ErrorT_;
    using ModelT = ModelT_;
 
    PointFittingKernel(const Mat& x1, const Mat& x2)
      : _x1(x1),
        _x2(x2)
    {}
 
    inline std::size_t getMinimumNbRequiredSamples() const { return _kernelSolver.getMinimumNbRequiredSamples(); }
 
    inline std::size_t getMaximumNbModels() const { return _kernelSolver.getMaximumNbModels(); }
 
    inline virtual void fit(const std::vector<std::size_t>& samples, std::vector<ModelT>& models) const
    {
        const Mat x1 = buildSubsetMatrix(_x1, samples);
        const Mat x2 = buildSubsetMatrix(_x2, samples);
        _kernelSolver.solve(x1, x2, models);
    }
 
    inline virtual double error(std::size_t sample, const ModelT& model) const
    {
        return _errorEstimator.error(model, _x1.col(sample), _x2.col(sample));
    }
 
    inline virtual void errors(const ModelT& model, std::vector<double>& errors) const
    {
        errors.resize(_x1.cols());
        for (std::size_t sample = 0; sample < _x1.cols(); ++sample)
            errors.at(sample) = error(sample, model);
    }
 
    inline std::size_t nbSamples() const { return _x1.cols(); }
 
  protected:
    const Mat& _x1;
    const Mat& _x2;
    const SolverT _kernelSolver{};
    const ErrorT _errorEstimator{};
};
 
template<typename SolverT_, typename ErrorT_, typename UnnormalizerT_, typename ModelT_ = Mat3Model>
class NormalizedPointFittingKernel : public PointFittingKernel<SolverT_, ErrorT_, ModelT_>
{
  public:
    using KernelBase = PointFittingKernel<SolverT_, ErrorT_, ModelT_>;
 
    NormalizedPointFittingKernel(const Mat& x1, const Mat& x2)
      : KernelBase(x1, x2)
    {}
 
    inline void fit(const std::vector<std::size_t>& samples, std::vector<ModelT_>& models) const override
    {
        const Mat x1 = buildSubsetMatrix(KernelBase::_x1, samples);
        const Mat x2 = buildSubsetMatrix(KernelBase::_x2, samples);
 
        assert(2 == x1.rows());
        assert(KernelBase::getMinimumNbRequiredSamples() <= x1.cols());
        assert(x1.rows() == x2.rows());
        assert(x1.cols() == x2.cols());
 
        // normalize the data.
        Mat3 T1;
        Mat3 T2;
        Mat x1_normalized;
        Mat x2_normalized;
 
        normalizePoints(x1, &x1_normalized, &T1);
        normalizePoints(x2, &x2_normalized, &T2);
 
        KernelBase::_kernelSolver.solve(x1_normalized, x2_normalized, models);
 
        // unnormalize model from the computed conditioning.
        for (int i = 0; i < models.size(); ++i)
            UnnormalizerT_::unnormalize(T1, T2, &(models.at(i).getMatrix()));
    }
};
 
}  // namespace robustEstimation
}  // namespace aliceVision