transform.hpp

Go to the documentation of this file.

// cpd - Coherent Point Drift
// Copyright (C) 2017 Pete Gadomski <pete.gadomski@gmail.com>
//
// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation; either version 2 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License along
// with this program; if not, write to the Free Software Foundation, Inc.,
// 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.


#pragma once

#include <chrono>
#include <cpd/gauss_transform.hpp>
#include <cpd/matrix.hpp>
#include <cpd/normalization.hpp>
#include <cpd/utils.hpp>
#include <memory>

namespace cpd {

const size_t DEFAULT_MAX_ITERATIONS = 150;
const bool DEFAULT_NORMALIZE = true;
const double DEFAULT_OUTLIERS = 0.1;
const double DEFAULT_TOLERANCE = 1e-5;
const double DEFAULT_SIGMA2 = 0.0;
const bool DEFAULT_CORRESPONDENCE = false;
const bool DEFAULT_LINKED = true;

struct Result {
    Matrix points;
    double sigma2;
    IndexVector correspondence;
    std::chrono::microseconds runtime;
    size_t iterations;

    virtual void denormalize(const Normalization& normalization);
};

template <typename Result>
class Transform {
public:
    Transform()
      : m_correspondence(DEFAULT_CORRESPONDENCE)
      , m_gauss_transform(GaussTransform::make_default())
      , m_max_iterations(DEFAULT_MAX_ITERATIONS)
      , m_normalize(DEFAULT_NORMALIZE)
      , m_outliers(DEFAULT_OUTLIERS)
      , m_sigma2(DEFAULT_SIGMA2)
      , m_tolerance(DEFAULT_TOLERANCE) {}

    Transform& correspondence(bool correspondence) {
        m_correspondence = correspondence;
        return *this;
    }

    Transform& gauss_transform(
        std::unique_ptr<GaussTransform> gauss_transform) {
        m_gauss_transform = std::move(gauss_transform);
        return *this;
    }

    Transform& max_iterations(double max_iterations) {
        m_max_iterations = max_iterations;
        return *this;
    }

    Transform& normalize(bool normalize) {
        m_normalize = normalize;
        return *this;
    }

    Transform& outliers(double outliers) {
        m_outliers = outliers;
        return *this;
    }

    Transform& sigma2(double sigma2) {
        m_sigma2 = sigma2;
        return *this;
    }

    Transform& tolerance(double tolerance) {
        m_tolerance = tolerance;
        return *this;
    }

    Result run(Matrix fixed, Matrix moving) {
        auto tic = std::chrono::high_resolution_clock::now();
        Normalization normalization(fixed, moving, linked());
        if (m_normalize) {
            fixed = normalization.fixed;
            moving = normalization.moving;
        }

        this->init(fixed, moving);

        Result result;
        result.points = moving;
        if (m_sigma2 == 0.0) {
            result.sigma2 = cpd::default_sigma2(fixed, moving);
        } else if (m_normalize) {
            result.sigma2 = m_sigma2 / normalization.fixed_scale;
        } else {
            result.sigma2 = m_sigma2;
        }

        size_t iter = 0;
        double ntol = m_tolerance + 10.0;
        double l = 0.;
        while (iter < m_max_iterations && ntol > m_tolerance &&
               result.sigma2 > 10 * std::numeric_limits<double>::epsilon()) {
            Probabilities probabilities = m_gauss_transform->compute(
                fixed, result.points, result.sigma2, m_outliers);
            this->modify_probabilities(probabilities);

            ntol = std::abs((probabilities.l - l) / probabilities.l);
            l = probabilities.l;

            result =
                this->compute_one(fixed, moving, probabilities, result.sigma2);
            ++iter;
        }

        if (m_normalize) {
            result.denormalize(normalization);
        }
        if (m_correspondence) {
            Probabilities probabilities = m_gauss_transform->compute(
                fixed, result.points, m_sigma2, m_outliers);
            result.correspondence = probabilities.correspondence;
            assert(result.correspondence.rows() > 0);
        }
        auto toc = std::chrono::high_resolution_clock::now();
        result.runtime =
            std::chrono::duration_cast<std::chrono::microseconds>(toc - tic);
        result.iterations = iter;
        return result;
    }

    virtual void init(const Matrix& fixed, const Matrix& moving) {}

    virtual void modify_probabilities(Probabilities& probabilities) const {}

    virtual Result compute_one(const Matrix& fixed, const Matrix& moving,
                               const Probabilities& probabilities,
                               double sigma2) const = 0;

    virtual bool linked() const = 0;

private:
    bool m_correspondence;
    std::unique_ptr<GaussTransform> m_gauss_transform;
    size_t m_max_iterations;
    bool m_normalize;
    double m_outliers;
    double m_sigma2;
    double m_tolerance;
};
} // namespace cpd