PF_aux_structs.h

Go to the documentation of this file.

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

#include <iostream>
#include <iterator>
#include <vector>

namespace mrpt::slam::detail
{
using namespace mrpt;
using namespace mrpt::math;
using namespace std;

/** Auxiliary structure used in KLD-sampling in particle filters \sa
 * CPosePDFParticles, CMultiMetricMapPDF */
struct TPoseBin2D
{
    TPoseBin2D() = default;
    /** Bin indices */
    int x{0}, y{0}, phi{0};

    /** less-than ordering of bins for usage in STL containers */
    struct lt_operator
    {
        inline bool operator()(const TPoseBin2D& s1, const TPoseBin2D& s2) const
        {
            if (s1.x < s2.x) return true;
            if (s1.x > s2.x) return false;
            if (s1.y < s2.y) return true;
            if (s1.y > s2.y) return false;
            return s1.phi < s2.phi;
        }
    };
};

/** Auxiliary structure   */
struct TPathBin2D
{
    std::vector<TPoseBin2D> bins;

    /** less-than ordering of bins for usage in STL containers */
    struct lt_operator
    {
        bool operator()(const TPathBin2D& s1, const TPathBin2D& s2) const
        {
            ASSERT_(s1.bins.size() == s2.bins.size());
            for (size_t i = 0; i < s1.bins.size(); i++)
            {
                if (s1.bins[i].x < s2.bins[i].x) return true;
                if (s1.bins[i].x > s2.bins[i].x) return false;
                if (s1.bins[i].y < s2.bins[i].y) return true;
                if (s1.bins[i].y > s2.bins[i].y) return false;
                if (s1.bins[i].phi < s2.bins[i].phi) return true;
                if (s1.bins[i].phi > s2.bins[i].phi) return false;
                // else, keep comparing:
            }
            return false;  // If they're exactly equal, s1 is NOT < s2.
        }
    };
};

/** Auxiliary structure used in KLD-sampling in particle filters \sa
 * CPosePDFParticles, CMultiMetricMapPDF */
struct TPoseBin3D
{
    TPoseBin3D() = default;
    /** Bin indices */
    int x{0}, y{0}, z{0}, yaw{0}, pitch{0}, roll{0};

    /** less-than ordering of bins for usage in STL containers */
    struct lt_operator
    {
        bool operator()(const TPoseBin3D& s1, const TPoseBin3D& s2) const
        {
            if (s1.x < s2.x) return true;
            if (s1.x > s2.x) return false;
            if (s1.y < s2.y) return true;
            if (s1.y > s2.y) return false;
            if (s1.z < s2.z) return true;
            if (s1.z > s2.z) return false;
            if (s1.yaw < s2.yaw) return true;
            if (s1.yaw > s2.yaw) return false;
            if (s1.pitch < s2.pitch) return true;
            if (s1.pitch > s2.pitch) return false;
            return s1.roll < s2.roll;
        }
    };
};

}  // namespace mrpt::slam::detail