driver.h

Go to the documentation of this file.

 
#pragma once
 
#include <optional>
#include <chrono>
#include "icp.h"
 
namespace icp {
 
    class ICPDriver {
    public:
        struct ConvergenceState {
            double cost;
 
            size_t iteration_count;
 
            RBTransform transform;
        };
 
        ICPDriver(std::unique_ptr<ICP> icp);
 
        ConvergenceState converge(const std::vector<Vector>& a, const std::vector<Vector>& b,
            RBTransform t);
 
        void set_min_iterations(uint64_t min_iterations);
 
        void set_max_iterations(uint64_t max_iterations);
 
        void set_stop_cost(double stop_cost);
 
        // TODO: fix docs to use math once doxygen is fixed
        void set_relative_cost_tolerance(double relative_cost_tolerance);
 
        void set_absolute_cost_tolerance(double absolute_cost_tolerance);
 
        void set_transform_tolerance(double angle_tolerance, double translation_tolerance);
 
        void set_time_limit(std::chrono::duration<double> time_limit);
 
    private:
        bool should_terminate(ConvergenceState current_state,
            std::optional<ConvergenceState> last_state);
 
        std::unique_ptr<ICP> icp_;
 
        std::optional<uint64_t> min_iterations_;
        std::optional<uint64_t> max_iterations_;
        std::optional<double> stop_cost_;
        std::optional<double> relative_cost_tolerance_;
        std::optional<double> absolute_cost_tolerance_;
        std::optional<double> angle_tolerance_rad_;
        std::optional<double> translation_tolerance_;
        std::optional<std::chrono::duration<double>> time_limit_;
 
        std::chrono::time_point<std::chrono::steady_clock> start_time_;
    };
}