icp.h

Go to the documentation of this file.

 
#pragma once
 
#include <cmath>
#include <vector>
#include <memory>
#include <string>
#include <functional>
#include <unordered_map>
#include <variant>
#include <optional>
 
#include "geo.h"
 
namespace icp {
    class ICP {
    protected:
        struct Match {
            size_t point;
            size_t pair;
            double cost;
        };
 
        RBTransform transform;
 
        std::vector<Vector> a;
 
        std::vector<Vector> b;
 
        std::vector<Match> matches;
 
        ICP();
 
        virtual void setup() = 0;
 
    public:
        class Config {
            using Param = std::variant<int, double, std::string>;
            std::unordered_map<std::string, Param> params;
 
        public:
            Config() {}
 
            template<typename T>
            void set(std::string key, T value) {
                params[key] = value;
            }
 
            template<typename T>
            T get(std::string key, T otherwise) const {
                if (params.find(key) == params.end()) {
                    return otherwise;
                } else {
                    return std::get<T>(params.at(key));
                }
            }
        };
 
        virtual ~ICP() = default;
 
        void begin(const std::vector<Vector>& a, const std::vector<Vector>& b, RBTransform t);
 
        virtual void iterate() = 0;
 
        double calculate_cost() const;
 
        const RBTransform& current_transform() const;
 
        static bool register_method(std::string name,
            std::function<std::unique_ptr<ICP>(const Config&)> constructor);
 
        static bool is_method_registered(std::string name);
 
        static const std::vector<std::string>& registered_methods();
 
        static std::optional<std::unique_ptr<ICP>> from_method(std::string name,
            const Config& params = Config());
 
    private:
        struct Methods {
            std::vector<std::string> registered_method_names;
            std::vector<std::function<std::unique_ptr<ICP>(const ICP::Config&)>>
                registered_method_constructors;
        };
 
        static Methods global;
        static bool builtins_registered;
 
        static void ensure_builtins_registered();
 
        static void register_method_internal(std::string name,
            std::function<std::unique_ptr<ICP>(const Config&)> constructor);
    };
}