PointFittingRansacKernel.hpp

// This file is part of the AliceVision project.
// Copyright (c) 2019 AliceVision 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/config.hpp>
#include <aliceVision/numeric/numeric.hpp>
#include <aliceVision/robustEstimation/conditioning.hpp>
#include <aliceVision/robustEstimation/ISolver.hpp>
#include <aliceVision/robustEstimation/IRansacKernel.hpp>
#include <aliceVision/robustEstimation/PointFittingKernel.hpp>
 
namespace aliceVision {
namespace robustEstimation {
 
template<typename SolverT_, typename ErrorT_, typename ModelT_, typename SolverLsT_ = robustEstimation::UndefinedSolver<ModelT_>>
class PointFittingRansacKernel : public robustEstimation::IRansacKernel<ModelT_>,
                                 public robustEstimation::PointFittingKernel<SolverT_, ErrorT_, ModelT_>
{
  public:
    using PFKernel = robustEstimation::PointFittingKernel<SolverT_, ErrorT_, ModelT_>;
 
    PointFittingRansacKernel(const Mat& x1, const Mat& x2)
      : PFKernel(x1, x2)
    {}
 
    std::size_t getMinimumNbRequiredSamples() const override { return PFKernel::getMinimumNbRequiredSamples(); }
 
    std::size_t getMinimumNbRequiredSamplesLS() const override { return _solverLs.getMinimumNbRequiredSamples(); }
 
    std::size_t getMaximumNbModels() const override { return PFKernel::getMaximumNbModels(); }
 
    void fit(const std::vector<std::size_t>& samples, std::vector<ModelT_>& models) const override { PFKernel::fit(samples, models); }
 
    void fitLS(const std::vector<std::size_t>& inliers, std::vector<ModelT_>& models, const std::vector<double>* weights = nullptr) const override
    {
        const Mat x1 = buildSubsetMatrix(PFKernel::_x1, inliers);
        const Mat x2 = buildSubsetMatrix(PFKernel::_x2, inliers);
 
        if (weights == nullptr)
            _solverLs.solve(x1, x2, models);
        else
            _solverLs.solve(x1, x2, models, *weights);
    }
 
    void computeWeights(const ModelT_& model,
                        const std::vector<std::size_t>& inliers,
                        std::vector<double>& weights,
                        const double eps = 0.001) const override
    {
        const auto numInliers = inliers.size();
        weights.resize(numInliers);
        for (std::size_t sample = 0; sample < numInliers; ++sample)
        {
            const auto idx = inliers[sample];
            weights[sample] = PFKernel::_errorEstimator.error(model, PFKernel::_x1.col(idx), PFKernel::_x2.col(idx));
            // avoid division by zero
            weights[sample] = 1.0 / std::pow(std::max(eps, weights[sample]), 2);
        }
    }
 
    double error(std::size_t sample, const ModelT_& model) const override { return PFKernel::error(sample, model); }
 
    void errors(const ModelT_& model, std::vector<double>& errors) const override { PFKernel::errors(model, errors); }
 
    virtual void unnormalize(ModelT_& model) const = 0;
 
    std::size_t nbSamples() const override { return PFKernel::nbSamples(); }
 
    virtual double logalpha0() const = 0;
 
    virtual double errorVectorDimension() const = 0;
    virtual double unormalizeError(double val) const = 0;
    virtual Mat3 normalizer1() const = 0;
    virtual double thresholdNormalizer() const = 0;
 
  private:
    const SolverLsT_ _solverLs{};
};
 
}  // namespace robustEstimation
}  // namespace aliceVision