icp.h

Go to the documentation of this file.

/*
 * @author Ethan Uppal
 * @copyright Copyright (C) 2024 Ethan Uppal. All rights reserved.
 */
 
#pragma once
 
#include <cmath>
#include <vector>
#include <memory>
#include <string>
#include <functional>
#include <unordered_map>
#include "geo.h"
 
namespace icp {
    class ICP {
    protected:
        // TODO: make this more general to allow for point-to-line
        struct Match {
            size_t point;
            size_t pair;
            double sq_dist;
        };
 
        RBTransform transform;
 
        Vector a_cm;
 
        Vector b_cm;
 
        std::vector<Vector> a;
 
        std::vector<Vector> b;
 
        double previous_cost;
 
        double current_cost;
 
        std::vector<Match> matches;
 
        ICP();
 
        virtual void setup();
 
    public:
        struct ConvergenceReport {
            double final_cost;
 
            size_t iteration_count;
        };
 
        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;
 
        ConvergenceReport converge(size_t burn_in,
            double convergence_threshold);
 
        const RBTransform& current_transform() const;
 
        static bool register_method(std::string name,
            std::function<std::unique_ptr<ICP>(const Config&)> constructor);
 
        static const std::vector<std::string>& registered_methods();
 
        static std::unique_ptr<ICP> from_method(std::string name,
            const Config& params = Config());
 
        static bool is_registered_method(std::string name);
    };
 
    struct Methods {
        std::vector<std::string> registered_method_names;
        std::vector<std::function<std::unique_ptr<ICP>(const ICP::Config&)>>
            registered_method_constructors;
    };
}