Program Listing for File interpolation.hpp

↰ Return to documentation for file (src/navtk/utils/interpolation.hpp)

#pragma once

#include <memory>
#include <type_traits>
#include <typeinfo>

#include <navtk/aspn.hpp>
#include <navtk/not_null.hpp>
#include <navtk/tensors.hpp>
#include <navtk/utils/Ordered.hpp>

namespace navtk {
namespace utils {

template <typename T>
std::vector<Size> condition_source_data(std::vector<double>& time_source,
                                        std::vector<T>& data_source,
                                        std::vector<double>& time_interp);

std::pair<std::vector<Size>, std::vector<double>> linear_interpolate(
    const std::vector<double>& time_source,
    const std::vector<double>& data_source,
    const std::vector<double>& time_interp);

std::pair<std::vector<Size>, std::vector<double>> quadratic_spline_interpolate(
    const std::vector<double>& time_source,
    const std::vector<double>& data_source,
    const std::vector<double>& time_interp);

std::pair<std::vector<Size>, std::vector<double>> cubic_spline_interpolate(
    const std::vector<double>& orig_time_source,
    const std::vector<double>& data_source,
    const std::vector<double>& orig_time_interp);

template <typename Y>
Y linear_interpolate(double x0, const Y& y0, double x1, const Y& y1, double x) {
    if (x0 == x1) return y1;
    double xpart = (x - x0) / (x1 - x0);
    Y ypart      = (y1 - y0) * xpart;
    return y0 + ypart;
}

template <typename Y>
Y linear_interpolate(const aspn_xtensor::TypeTimestamp& x0,
                     const Y& y0,
                     const aspn_xtensor::TypeTimestamp& x1,
                     const Y& y1,
                     const aspn_xtensor::TypeTimestamp& x) {
    if (x0 == x1) return y1;
    double xpart = (double)(x.get_elapsed_nsec() - x0.get_elapsed_nsec()) /
                   (x1.get_elapsed_nsec() - x0.get_elapsed_nsec());
    Y ypart      = (y1 - y0) * xpart;
    return y0 + ypart;
}

aspn_xtensor::MeasurementPositionVelocityAttitude linear_interp_pva(
    const aspn_xtensor::MeasurementPositionVelocityAttitude& pva1,
    const aspn_xtensor::MeasurementPositionVelocityAttitude& pva2,
    const aspn_xtensor::TypeTimestamp& t);

not_null<std::shared_ptr<aspn_xtensor::MeasurementPositionVelocityAttitude>> linear_interp_pva(
    navtk::not_null<std::shared_ptr<aspn_xtensor::MeasurementPositionVelocityAttitude>> pva1,
    navtk::not_null<std::shared_ptr<aspn_xtensor::MeasurementPositionVelocityAttitude>> pva2,
    const aspn_xtensor::TypeTimestamp& t);

aspn_xtensor::MeasurementPositionVelocityAttitude linear_extrapolate_pva(
    const aspn_xtensor::MeasurementPositionVelocityAttitude& pva1,
    const aspn_xtensor::MeasurementPositionVelocityAttitude& pva2,
    const aspn_xtensor::TypeTimestamp& t);

not_null<std::shared_ptr<aspn_xtensor::MeasurementPositionVelocityAttitude>> linear_extrapolate_pva(
    navtk::not_null<std::shared_ptr<aspn_xtensor::MeasurementPositionVelocityAttitude>> pva1,
    navtk::not_null<std::shared_ptr<aspn_xtensor::MeasurementPositionVelocityAttitude>> pva2,
    const aspn_xtensor::TypeTimestamp& t);

Vector3 linear_interp_rpy(const aspn_xtensor::TypeTimestamp& t1,
                          const Vector3& rpy1,
                          const aspn_xtensor::TypeTimestamp& t2,
                          const Vector3& rpy2,
                          const aspn_xtensor::TypeTimestamp& t);

Vector3 linear_extrapolate_rpy(const aspn_xtensor::TypeTimestamp& t1,
                               const Vector3& rpy1,
                               const aspn_xtensor::TypeTimestamp& t2,
                               const Vector3& rpy2,
                               const aspn_xtensor::TypeTimestamp& t);

}  // namespace utils
}  // namespace navtk