CSplineInterpolator1D.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include "math-precomp.h"  // Precompiled headers

#include <mrpt/math/CSplineInterpolator1D.h>  // for CSplineInterpolator1D
#include <mrpt/math/CVectorDynamic.h>
#include <mrpt/serialization/CSerializable.h>  // for CSerializable, CSeriali...
#include <mrpt/serialization/stl_serialization.h>  // for operator<<, operator>>
#include <map>
#include <mrpt/math/interp_fit.hpp>  // for spline

using namespace mrpt;
using namespace mrpt::math;
using namespace std;

// This must be added to any CSerializable class implementation file.
IMPLEMENTS_SERIALIZABLE(CSplineInterpolator1D, CSerializable, mrpt::math)

/*---------------------------------------------------------------
                        Constructor
  ---------------------------------------------------------------*/
CSplineInterpolator1D::CSplineInterpolator1D(bool wrap2pi) : m_wrap2pi(wrap2pi)
{
}

/*---------------------------------------------------------------
                        appendXY
  ---------------------------------------------------------------*/
void CSplineInterpolator1D::appendXY(double x, double y) { m_x2y[x] = y; }
/*---------------------------------------------------------------
                        query
  ---------------------------------------------------------------*/
double& CSplineInterpolator1D::query(double x, double& y, bool& out_valid) const
{
    out_valid = false;
    y = 0;

    std::pair<double, double> p1, p2, p3, p4;

    auto it_ge1 = m_x2y.lower_bound(x);

    // Exact match?
    if (it_ge1 != m_x2y.end() && it_ge1->first == x)
    {
        y = it_ge1->second;
        out_valid = true;
        return y;
    }

    // Are we in the beginning or the end of the path?
    if (it_ge1 == m_x2y.end() || it_ge1 == m_x2y.begin())
    {
        return y;
    }

    p3 = *it_ge1;  // Third pair
    ++it_ge1;
    if (it_ge1 == m_x2y.end())
    {
        return y;
    }
    p4 = *it_ge1;  // Fourth pair

    --it_ge1;
    --it_ge1;
    p2 = *it_ge1;  // Second pair

    if (it_ge1 == m_x2y.begin())
    {
        return y;
    }

    p1 = *(--it_ge1);  // First pair

    // ---------------------------------------
    //    SPLINE INTERPOLATION
    // ---------------------------------------
    CVectorDouble xs(4);
    xs[0] = p1.first;
    xs[1] = p2.first;
    xs[2] = p3.first;
    xs[3] = p4.first;

    CVectorDouble ys(4);
    ys[0] = p1.second;
    ys[1] = p2.second;
    ys[2] = p3.second;
    ys[3] = p4.second;

    out_valid = true;
    return y = math::spline(x, xs, ys, m_wrap2pi);
}

uint8_t CSplineInterpolator1D::serializeGetVersion() const { return 0; }
void CSplineInterpolator1D::serializeTo(
    mrpt::serialization::CArchive& out) const
{
    out << m_x2y << m_wrap2pi;
}

void CSplineInterpolator1D::serializeFrom(
    mrpt::serialization::CArchive& in, uint8_t version)
{
    switch (version)
    {
        case 0:  // floats
        {
            in >> m_x2y >> m_wrap2pi;
        }
        break;
        default:
            MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
    };
}