match4pcsBase.h

// Copyright 2017 Nicolas Mellado
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//   http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
// -------------------------------------------------------------------------- //
//
// Authors: Dror Aiger, Yoni Weill, Nicolas Mellado
//
// This file is part of the implementation of the 4-points Congruent Sets (4PCS)
// algorithm presented in:
//
// 4-points Congruent Sets for Robust Surface Registration
// Dror Aiger, Niloy J. Mitra, Daniel Cohen-Or
// ACM SIGGRAPH 2008 and ACM Transaction of Graphics.
//
// Given two sets of points in 3-space, P and Q, the algorithm applies RANSAC
// in roughly O(n^2) time instead of O(n^3) for standard RANSAC, using an
// efficient method based on invariants, to find the set of all 4-points in Q
// that can be matched by rigid transformation to a given set of 4-points in P
// called a base. This avoids the need to examine all sets of 3-points in Q
// against any base of 3-points in P as in standard RANSAC.
// The algorithm can use colors and normals to speed-up the matching
// and to improve the quality. It can be easily extended to affine/similarity
// transformation but then the speed-up is smaller because of the large number
// of congruent sets. The algorithm can also limit the range of transformations
// when the application knows something on the initial pose but this is not
// necessary in general (though can speed the runtime significantly).

// Home page of the 4PCS project (containing the paper, presentations and a
// demo): http://graphics.stanford.edu/~niloy/research/fpcs/fpcs_sig_08.html
// Use google search on "4-points congruent sets" to see many related papers
// and applications.

#ifndef _SUPER4PCS_ALGO_MATCH_4PCS_BASE_
#define _SUPER4PCS_ALGO_MATCH_4PCS_BASE_

#include <vector>

#ifdef SUPER4PCS_USE_OPENMP
#include <omp.h>
#endif

#include "super4pcs/shared4pcs.h"
#include "super4pcs/sampling.h"
#include "super4pcs/accelerators/kdtree.h"
#include "super4pcs/utils/logger.h"

#ifdef TEST_GLOBAL_TIMINGS
#   include "super4pcs/utils/timer.h"
#endif

namespace GlobalRegistration{


class Match4PCSBase {

public:
    using PairsVector =  std::vector< std::pair<int, int> >;
    using Scalar = typename Point3D::Scalar;
    using VectorType = typename Point3D::VectorType;
    using MatrixType = Eigen::Matrix<Scalar, 4, 4>;
    using LogLevel = Utils::LogLevel;
    struct DummyTransformVisitor {
        inline void operator() (float, float, Eigen::Ref<Match4PCSBase::MatrixType>) const {}
        constexpr bool needsGlobalTransformation() const { return false; }
    };
    using DefaultSampler = Sampling::UniformDistSampler;

    static constexpr int kNumberOfDiameterTrials = 1000;
    static constexpr Scalar kLargeNumber = 1e9;
    static constexpr Scalar distance_factor = 2.0;

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    virtual ~Match4PCSBase();

    inline const std::vector<Point3D>& getFirstSampled() const {
        return sampled_P_3D_;
    }

    inline const std::vector<Point3D>& getSecondSampled() const {
        return sampled_Q_3D_;
    }

    template <typename Sampler = DefaultSampler,
              typename Visitor = DummyTransformVisitor>
    Scalar
    ComputeTransformation(const std::vector<Point3D>& P,
                          std::vector<Point3D>* Q,
                          Eigen::Ref<MatrixType> transformation,
                          const Sampler& sampler = Sampler(),
                          const Visitor& v = Visitor());


protected:
    int number_of_trials_;
    Scalar max_base_diameter_;
    Scalar P_diameter_;
    Scalar P_mean_distance_;
    Eigen::Matrix<Scalar, 4, 4> transform_;
    Eigen::Matrix<Scalar, 3, 1> qcentroid1_, qcentroid2_;
    int base_[4];
    int current_congruent_[4];
    std::vector<Point3D> sampled_P_3D_;
    std::vector<Point3D> sampled_Q_3D_;
    std::vector<Point3D> base_3D_;
    std::vector<Point3D> Q_copy_;
    VectorType centroid_P_;
    VectorType centroid_Q_;
    Scalar best_LCP_;
    int current_trial_;
    KdTree<Scalar> kd_tree_;
    const Match4PCSOptions options_;

    std::mt19937 randomGenerator_;

    const Utils::Logger &logger_;

#ifdef SUPER4PCS_USE_OPENMP
    const int omp_nthread_congruent_;
#endif

#ifdef TEST_GLOBAL_TIMINGS

    Scalar totalTime;
    Scalar kdTreeTime;
    Scalar verifyTime;

    using Timer = GlobalRegistration::Utils::Timer;

#endif

protected:

    Match4PCSBase(const Match4PCSOptions& options
                  , const Utils::Logger &logger
#ifdef SUPER4PCS_USE_OPENMP
                  , const int omp_nthread_congruent = omp_get_max_threads()
#endif
        );

    template <Utils::LogLevel level, typename...Args>
    inline void Log(Args...args) const { logger_.Log<level>(args...); }


    Scalar MeanDistance();


    bool SelectRandomTriangle(int& base1, int& base2, int& base3);

    bool TryQuadrilateral(Scalar &invariant1, Scalar &invariant2,
                          int &base1, int &base2, int &base3, int &base4);


    bool ComputeRigidTransformation(const std::array<Point3D, 4>& ref,
                                    const std::array<Point3D, 4>& candidate,
                                    const Eigen::Matrix<Scalar, 3, 1>& centroid1,
                                    Eigen::Matrix<Scalar, 3, 1> centroid2,
                                    Scalar max_angle,
                                    Eigen::Ref<MatrixType> transform,
                                    Scalar& rms_,
                                    bool computeScale ) const;

    Scalar Verify(const Eigen::Ref<const MatrixType> & mat) const;

    template <typename Visitor>
    bool Perform_N_steps(int n,
                         Eigen::Ref<MatrixType> transformation,
                         std::vector<Point3D>* Q,
                         const Visitor& v);

    template <typename Visitor>
    bool TryOneBase(const Visitor &v);

    virtual void
    Initialize(const std::vector<Point3D>& P,
               const std::vector<Point3D>& Q) = 0;

    template <typename Sampler>
    void init(const std::vector<Point3D>& P,
              const std::vector<Point3D>& Q,
              const Sampler& sampler);

    bool SelectQuadrilateral(Scalar &invariant1, Scalar &invariant2,
                             int& base1, int& base2, int& base3, int& base4);

    const std::vector<Point3D>& base3D() const { return base_3D_; }

    virtual void
    ExtractPairs( Scalar pair_distance,
                  Scalar pair_normals_angle,
                  Scalar pair_distance_epsilon, int base_point1,
                  int base_point2,
                  PairsVector* pairs) const = 0;

    virtual bool
    FindCongruentQuadrilaterals(Scalar invariant1, Scalar invariant2,
                                Scalar distance_threshold1,
                                Scalar distance_threshold2,
                                const PairsVector& P_pairs,
                                const PairsVector& Q_pairs,
                                std::vector<Quadrilateral>* quadrilaterals) const = 0;

    template <typename Visitor>
    bool TryCongruentSet(int base_id1,
                         int base_id2,
                         int base_id3,
                         int base_id4,
                         const std::vector<Quadrilateral> &congruent_quads,
                         const Visitor &v,
                         size_t &nbCongruent);
private:
    void initKdTree();

}; 
} 

#include "super4pcs/algorithms/match4pcsBase.hpp"

#endif