CPoseInterpolatorBase.hpp

Go to the documentation of this file.

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

#include <mrpt/poses/CPoseInterpolatorBase.h>

#include <mrpt/utils/CFileOutputStream.h>
#include <mrpt/utils/stl_serialization.h>
#include <mrpt/math/slerp.h>
#include <mrpt/math/wrap2pi.h>
#include <mrpt/math/interp_fit.hpp>
#include <mrpt/math/CMatrixD.h>
#include <mrpt/poses/CPose3DPDFParticles.h>

namespace mrpt {
namespace poses {

template <int DIM>
CPoseInterpolatorBase<DIM>::CPoseInterpolatorBase() : m_method( mrpt::poses::imLinearSlerp )
{
        maxTimeInterpolation = -1.0;
}

template <int DIM>
void CPoseInterpolatorBase<DIM>::clear()
{
        m_path.clear();
}

template <int DIM>
void CPoseInterpolatorBase<DIM>::insert( mrpt::system::TTimeStamp t, const cpose_t &p)
{
        m_path[t] = pose_t(p);
}
template <int DIM>
void CPoseInterpolatorBase<DIM>::insert(mrpt::system::TTimeStamp t, const pose_t &p)
{
        m_path[t] = p;
}

/*---------------------------------------------------------------
                                                interpolate
  ---------------------------------------------------------------*/
template <int DIM>
typename CPoseInterpolatorBase<DIM>::cpose_t & CPoseInterpolatorBase<DIM>::interpolate(mrpt::system::TTimeStamp t, cpose_t &out_interp, bool &out_valid_interp) const
{
        pose_t p;
        this->interpolate(t, p, out_valid_interp);
        out_interp = cpose_t(p);
        return out_interp;
}

template <int DIM>
typename CPoseInterpolatorBase<DIM>::pose_t & CPoseInterpolatorBase<DIM>::interpolate( mrpt::system::TTimeStamp t, pose_t &out_interp, bool &out_valid_interp ) const
{
         // Default value in case of invalid interp
        for (size_t k=0;k<pose_t::static_size;k++) {
                out_interp[k]=0;
        }
        TTimePosePair p1, p2, p3, p4;
        p1.second = p2.second = p3.second = p4.second = out_interp;

        // Invalid?
        if (t==INVALID_TIMESTAMP)
        {
                out_valid_interp = false;
                return out_interp;
        }

        // We'll look for 4 consecutive time points.
        // Check if the selected method needs all 4 points or just the central 2 of them:
        bool interp_method_requires_4pts;
        switch (m_method)
        {
        case imLinear2Neig:
        case imSplineSlerp:
        case imLinearSlerp:
                interp_method_requires_4pts = false;
                break;
        default:
                interp_method_requires_4pts = true;
                break;
        };


        // Out of range?
        const_iterator it_ge1 = m_path.lower_bound( t );

        // Exact match?
        if( it_ge1 != m_path.end() && it_ge1->first == t )
        {
                out_interp = it_ge1->second;
                out_valid_interp = true;
                return out_interp;
        }

        // Are we in the beginning or the end of the path?
        if( it_ge1 == m_path.end() || it_ge1 == m_path.begin() )
        {
                out_valid_interp = false;
                return out_interp;
        } // end

        p3 = *it_ge1;           // Third pair
        const_iterator it_ge2 = it_ge1; ++it_ge2;
        if(it_ge2 == m_path.end() )
        {
                if (interp_method_requires_4pts) {
                        out_valid_interp = false;
                        return out_interp;
                }
        }
        else {
                p4 = *(it_ge2);         // Fourth pair
        }

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

        if( it_ge1 == m_path.begin() )
        {
                if (interp_method_requires_4pts) {
                        out_valid_interp = false;
                        return out_interp;
                }
        }
        else {
                p1 = *(--it_ge1);       // First pair
        }

        // Test if the difference between the desired timestamp and the next timestamp is lower than a certain (configurable) value
        const double dt12 = interp_method_requires_4pts ? (p2.first - p1.first) / 1e7 : .0;
        const double dt23 = (p3.first - p2.first) / 1e7;
        const double dt34 = interp_method_requires_4pts ? (p4.first - p3.first) / 1e7 : .0;

        if( maxTimeInterpolation > 0 &&
          (dt12 > maxTimeInterpolation ||
           dt23 > maxTimeInterpolation ||
           dt34 > maxTimeInterpolation ))
        {
                out_valid_interp = false;
                return out_interp;
        }

        // Do interpolation:
        // ------------------------------------------
        // First Previous point:  p1
        // Second Previous point: p2
        // First Next point:      p3
        // Second Next point:     p4
        // Time where to interpolate:  t
        double td     = mrpt::system::timestampTotime_t(t);

        mrpt::math::CArrayDouble<4> ts;
        ts[0] = mrpt::system::timestampTotime_t(p1.first);
        ts[1] = mrpt::system::timestampTotime_t(p2.first);
        ts[2] = mrpt::system::timestampTotime_t(p3.first);
        ts[3] = mrpt::system::timestampTotime_t(p4.first);

        impl_interpolation(ts,p1,p2,p3,p4, m_method,td,out_interp);

        out_valid_interp = true;
        return out_interp;

} // end interpolate

template <int DIM>
bool CPoseInterpolatorBase<DIM>::getPreviousPoseWithMinDistance(const mrpt::system::TTimeStamp &t, double distance, cpose_t &out_pose)
{
        pose_t p;
        bool ret = getPreviousPoseWithMinDistance(t, distance, p);
        out_pose = cpose_t(p);
        return ret;
}

template <int DIM>
bool CPoseInterpolatorBase<DIM>::getPreviousPoseWithMinDistance(const mrpt::system::TTimeStamp &t, double distance, pose_t &out_pose)
{
        if( m_path.size() == 0 || distance <=0 )
                return false;

        pose_t myPose;

        // Search for the desired timestamp
        iterator  it = m_path.find(t);
        if( it != m_path.end() && it != m_path.begin() )
                myPose = it->second;
        else
                return false;

        double d = 0.0;
        do
        {
                --it;
                d = (point_t(myPose) - point_t(it->second)).norm();
        } while( d < distance && it != m_path.begin() );

        if( d >= distance )
        {
                out_pose = it->second;
                return true;
        }
        else
                return false;
} // end getPreviousPose

template <int DIM>
void CPoseInterpolatorBase<DIM>::setMaxTimeInterpolation( double time )
{
        ASSERT_( time > 0 );
        maxTimeInterpolation = time;
}

template <int DIM>
double CPoseInterpolatorBase<DIM>::getMaxTimeInterpolation( )
{
        return maxTimeInterpolation;
}

template <int DIM>
bool CPoseInterpolatorBase<DIM>::saveToTextFile(const std::string &s) const
{
        try
        {
                mrpt::utils::CFileOutputStream f(s);
                std::string str;
                for (const_iterator i=m_path.begin();i!=m_path.end();++i)
                {
                        const double  t  = mrpt::system::timestampTotime_t(i->first);
                        const auto    &p = i->second;

                        str = mrpt::format("%.06f ",t);
                        for (unsigned int k=0;k<p.size();k++)
                                str+= mrpt::format("%.06f ",p[k]);
                        str+= std::string("\n");

                        f.printf("%s",str.c_str());
                }
                return true;
        }
        catch(...)
        {
                return false;
        }
}

template <int DIM>
bool CPoseInterpolatorBase<DIM>::saveInterpolatedToTextFile(const std::string &s, double period) const
{
        using mrpt::system::TTimeStamp;
        try
        {
                mrpt::utils::CFileOutputStream f(s);
                if (m_path.empty()) return true;

                std::string str;

                const TTimeStamp t_ini = m_path.begin()->first;
                const TTimeStamp t_end = m_path.rbegin()->first;

                TTimeStamp At = mrpt::system::secondsToTimestamp(period);

                pose_t p;
                bool   valid;
                for (TTimeStamp t=t_ini;t<=t_end;t+=At)
                {
                        this->interpolate( t, p, valid );
                        if (!valid) continue;

                        str = mrpt::format("%.06f ",mrpt::system::timestampTotime_t(t));
                        for (unsigned int k=0;k<p.size();k++)
                                str+= mrpt::format("%.06f ",p[k]);
                        str+= std::string("\n");
                        f.printf("%s",str.c_str());
                }
                return true;
        }
        catch(...)
        {
                return false;
        }
}

template <int DIM>
bool CPoseInterpolatorBase<DIM>::loadFromTextFile(const std::string &s)
{
        MRPT_START

        clear();
        mrpt::math::CMatrixD M;

        try
        {
                M.loadFromTextFile(s);
        }
        catch(std::exception &)
        {
                return false;   // error loading file
        }

        // Check valid format:
        if (M.getRowCount()==0) return false;
        ASSERT_(M.getColCount()== pose_t::static_size+1 );

        // load into the path:
        const size_t N = M.getColCount();
        pose_t p;
        for (size_t i=0;i<N;i++) {
                for (unsigned int k=0;k<pose_t::static_size;k++)
                        p[k] = M(i,k+1);
                insert(mrpt::system::time_tToTimestamp( M(i,0) ), p );
        }
        return true;
        MRPT_END
}


template <int DIM>
void CPoseInterpolatorBase<DIM>::getBoundingBox(point_t &Min, point_t &Max) const
{
        MRPT_START
        ASSERT_( !m_path.empty() );

        for (unsigned int k=0;k<point_t::static_size;k++) {
                Min[k] = std::numeric_limits<double>::max();
                Max[k] =-std::numeric_limits<double>::max();
        }

        for (const_iterator p=m_path.begin();p!=m_path.end();++p)
        {
                for (unsigned int k=0;k<point_t::static_size;k++) {
                        mrpt::utils::keep_min( Min[k], p->second[k]);
                        mrpt::utils::keep_max( Max[k], p->second[k]);
                }
        }
        MRPT_END
}

template <int DIM>
void CPoseInterpolatorBase<DIM>::setInterpolationMethod( mrpt::poses::TInterpolatorMethod method)
{
        m_method = method;
}

template <int DIM>
TInterpolatorMethod CPoseInterpolatorBase<DIM>::getInterpolationMethod() const
{
        return m_method;
}

template <int DIM>
void CPoseInterpolatorBase<DIM>::filter( unsigned int component, unsigned int samples )
{
        if (m_path.empty())
                return;

        TPath aux;

        int             ant, post;
        size_t  nitems = size();

        post    = (samples%2) ? (unsigned int)(samples/2) : samples/2;
        ant             = (unsigned int)(samples/2);

        int             k = 0;
        iterator it1, it2, it3;

        //int asamples;
        for( it1 = m_path.begin(); it1 != m_path.end(); ++it1, ++k )
        {
                it2 = m_path.begin();
                if( k-ant > 0 )
                        advance( it2, k-ant );

                if( k+post < (int)nitems )
                {
                        it3 = m_path.begin();
                        advance( it3, k+post+1 );
                }
                else
                {
                        it3 = m_path.end();
                }

                unsigned int nsamples = distance(it2,it3);
                CPose3DPDFParticles particles(nsamples);
                for( unsigned int i = 0; it2 != it3; ++it2, ++i )
                {
                        particles.m_particles[i].log_w = 0;
                        *particles.m_particles[i].d = mrpt::poses::CPose3D(it1->second);
                        switch( component )
                        {
                                case 0: particles.m_particles[i].d->x(it2->second[0]);          break;
                                case 1: particles.m_particles[i].d->y(it2->second[1]);          break;
                                case 2: particles.m_particles[i].d->z(it2->second[2]);          break;
                                case 3: particles.m_particles[i].d->setYawPitchRoll(it2->second[3],it1->second[4],it1->second[5]); break;
                                case 4: particles.m_particles[i].d->setYawPitchRoll(it1->second[3],it2->second[4],it1->second[5]);      break;
                                case 5: particles.m_particles[i].d->setYawPitchRoll(it1->second[3],it1->second[4],it2->second[5]);      break;
                        } // end switch
                } // end for it2

                mrpt::poses::CPose3D auxPose;
                particles.getMean( auxPose );
                aux[it1->first] = pose_t(auxPose);
        } // end for it1
        m_path = aux;
} // end filter

} // end ns
} // end ns