Samplers.h

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    RobotComponents
 * @author     Raphael Grimm ( raphael dot grimm at kit dot edu )
 * @date       2015
 * @copyright  http://www.gnu.org/licenses/gpl.txt
 *             GNU General Public License
 */
#pragma once
 
#include <random>
#include <tuple>
#include <type_traits>
#include <vector>
 
#include <Eigen/Core>
#include <Eigen/LU>
#include <Eigen/SVD>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
#include "Metrics.h"
#include "Volume.h"
 
namespace armarx
{
    /**
     * @brief Stores a distribution and a generator.
     * The call operator passes the generator to the distribution
     */
    template <class Distribution, class Generator>
    class Sampler
    {
    public:
        /**
         * @brief Type of the stored distribution
         */
        using DistributionType = Distribution;
        /**
         * @brief Type of the stored generator
         */
        using GeneratorType = Generator;
 
    protected:
        /**
        * @brief The stored distribution.
        */
        Distribution distribution;
        /**
         * @brief The stored generator
         */
        Generator generator;
 
    public:
        /**
         * @brief Ctor
         * @param dist The distribution to store.
         * @param gen The generator to store.
         */
        Sampler(Distribution&& dist, Generator&& gen) : distribution{dist}, generator{gen}
        {
        }
 
        /**
         * @brief Passes all arguments followed by the generator to the distribution.
         * @param args Additional parameters to pass to the distribution.
         * @return The return value of the distribution.
         */
        template <class... Args>
        auto
        operator()(Args&&... args) -> decltype(distribution(std::forward<Args>(args)..., generator))
        {
            return distribution(std::forward<Args>(args)..., generator);
        }
 
        /**
         * @brief Returns the stored distribution.
         * @return The stored distribution.
         */
        Distribution&
        getDistribution()
        {
            return distribution;
        }
 
        /**
         * @brief Returns the stored generator.
         * @return The stored generator.
         */
        Generator&
        getGenerator()
        {
            return generator;
        }
    };
 
    /**
     * @brief Uniform distribution over the surface of an n dimensional unit sphere.
     *
     * (using the method described in doi>10.1145/377939.377946 (https://dl.acm.org/citation.cfm?doid=377939.377946))
     */
    template <class RealType>
    class UniformUnitSphereSurfaceDistribution
    {
    public:
        /**
         * @brief Ctor
         */
        UniformUnitSphereSurfaceDistribution() : normDist{0.0, 0.1}
        {
        }
 
        /**
         * @brief Fills [first, last) with a uniform sample of n dimensional unit sphere's surface (n =  distance(first, last)).
         * @param first The first value to fill.
         * @param last One past the last value to fill.
         * @param gen The generator to use.
         */
        template <class Iterator, class Generator>
        void
        operator()(const Iterator& first, const Iterator& last, Generator& gen)
        {
            RealType length = 0;
 
            for (auto it = first; it != last; ++it)
            {
                *it = normDist(gen);
                length += std::pow(*it, 2.0);
            }
 
            length = std::sqrt(length);
 
            for (auto it = first; it != last; ++it)
            {
                *it /= length;
            }
        }
 
    protected:
        /**
         * @brief The internally used normal distribution.
         */
        std::normal_distribution<RealType> normDist;
    };
 
    /**
     * @brief Uniform distribution over the volume (closed set) of an n dimensional unit sphere.
     *
     * https://math.stackexchange.com/questions/87230/picking-random-points-in-the-volume-of-sphere-with-uniform-probability/87238#87238
     */
    template <class RealType>
    class UniformUnitSphereDistribution
    {
    public:
        /**
         * @brief Ctor
         */
        UniformUnitSphereDistribution() : normDist{0.0, 0.1}, radDist{0.0, 1.0}
        {
        }
 
        /**
         * @brief Fills [first, last) with a uniform sample of n dimensional unit sphere's volume (n =  distance(first, last)).
         * @param first
         * @param last
         * @param gen
         * @param first The first value to fill.
         * @param last One past the last value to fill.
         * @param gen The generator to use.
         */
        template <class Iterator, class Generator>
        void
        operator()(const Iterator& first, const Iterator& last, Generator& gen)
        {
            const RealType dim = std::distance(first, last);
            RealType length = 0;
 
            for (auto it = first; it != last; ++it)
            {
                *it = normDist(gen);
                length += std::pow(*it, 2.0);
            }
 
            length = std::sqrt(length);
            const auto scalingFactor = std::pow(radDist(gen), 1.0 / dim);
 
            for (auto it = first; it != last; ++it)
            {
                *it = ((*it) / length) * scalingFactor;
            }
        }
 
    protected:
        /**
         * @brief The internal used normal distribution for samples of the sphere surfaces.
         */
        std::normal_distribution<RealType> normDist;
        /**
         * @brief The uniform distribution used to transform sphere surface samples to samples in the sphere.
         */
        std::uniform_real_distribution<RealType> radDist;
    };
 
    /**
     * @brief Uniform distribution of an cuboid space.
     */
    template <class RealType>
    class UniformCuboidDistribution
    {
    public:
        /**
         * @brief Type to pass the bounds of a dimension.
         */
        using BoundsType = std::pair<RealType, RealType>;
 
        /**
         * @brief Constructs a cuboid distribution with the bounds [boundsFirst, boundsLast).
         * bounds.first is used as lower and bounds.second as upper bound.
         * @param boundsFirst The bounds for the first dimension.
         * @param boundsLast One past the bounds for the last dimension.
         */
        template <class Iterator>
        UniformCuboidDistribution(Iterator boundsFirst, Iterator boundsLast)
        {
            volume = 1;
 
            for (; boundsFirst != boundsLast; ++boundsFirst)
            {
                ARMARX_CHECK_EXPRESSION(boundsFirst->first < boundsFirst->second);
                volume *= (boundsFirst->second - boundsFirst->first);
 
                //set distribution
                spaceDist.emplace_back(
                    boundsFirst->first,
                    std::nextafter(boundsFirst->second, std::numeric_limits<RealType>::max()));
            }
        }
 
        /**
         * @brief Fills the range [first, first + dimensionality) with an uniform sample of the cuboid.
         * [first, first + dimensionality) has to be a valid range. If not the behavior is undefined.
         * @param first The first value to fill.
         * @param gen The used generator.
         */
        template <class Iterator, class Generator>
        void
        operator()(Iterator first, Generator& gen)
        {
            for (std::size_t i = 0; i < spaceDist.size(); ++i, ++first)
            {
                *first = spaceDist.at(i)(gen);
            }
        }
 
        /**
         * @brief Returns whether the range [first, first + dimensionality) is a point contained by the cuboid.
         * [first, first + dimensionality) has to be a valid range. If not the behavior is undefined.
         * @param first The first value of the point.
         * @return Whether the point is contained by the cuboid.
         */
        template <class Iterator>
        bool
        isInBounds(Iterator first)
        {
            for (std::size_t i = 0; i < spaceDist.size(); ++i, ++first)
            {
                if ((*first < spaceDist.at(i).a()) || (*first > spaceDist.at(i).b()))
                {
                    return false;
                }
            }
 
            return true;
        }
 
        /**
         * @brief Returns the cuboid's volume.
         * @return The cuboid's volume.
         */
        RealType
        getVolume() const
        {
            return volume;
        }
 
        /**
         * @brief Returns the cuboid's dimensionality.
         * @return The cuboid's dimensionality.
         */
        template <class T = std::size_t>
        T
        getDimensionality() const
        {
            return static_cast<T>(spaceDist.size());
        }
 
    protected:
        /**
         * @brief The cuboid's volume.
         */
        RealType volume;
        /**
         * @brief Distributions for the dimensions of the cuboid.
         */
        std::vector<std::uniform_real_distribution<RealType>> spaceDist;
    };
 
    using CuboidSampler = Sampler<UniformCuboidDistribution<float>, std::mt19937>;
 
    /**
     * @brief Uniform distribution of a prolate hyper spheroid.
     */
    template <class RealType = float>
    class UniformProlateSpheroidDistribution
    {
    protected:
        //internally used. are not required for the external interface => protected
        /**
         * @brief Internal used Eigen vector type.
         */
        using VecType = Eigen::Matrix<RealType, Eigen::Dynamic, 1>;
        /**
         * @brief Internal used Eigen matrix type.
         */
        using MatType = Eigen::Matrix<RealType, Eigen::Dynamic, Eigen::Dynamic>;
 
    public:
        /**
         * @brief Constructs a uiform distribution of a prolate hyper spheroid with the focal points [focalAFirst, focalALast)
         * and [focalBFirst, focalBFirst + distance(focalAFirst, focalALast)).
         * @param focalAFirst The first value of the first focal point.
         * @param focalALast One past the last value of the first focal point.
         * @param focalBFirst The first value of the second focal point.
         */
        template <class ValIter>
        UniformProlateSpheroidDistribution(ValIter focalAFirst,
                                           ValIter focalALast,
                                           ValIter focalBFirst) :
            sphereDistribution{},
            tmpVal(std::distance(focalAFirst, focalALast)),
            //distanceFocalPoints,
            polarDiameter{std::numeric_limits<RealType>::infinity()},
            volumeProlateSpheroid{std::numeric_limits<RealType>::infinity()},
            focalPointA(getDimensionality()),
            focalPointB(getDimensionality()),
            centre(getDimensionality()),
            rotation{getDimensionality(), getDimensionality()},
            scaleThenRotate{getDimensionality(), getDimensionality()}
        {
            setFocalPoints(focalAFirst, focalBFirst);
        }
 
        /**
         * @brief Constructs a uiform distribution of a prolate hyper spheroid with the focal points [focalAFirst, focalALast)
         * and [focalBFirst, focalBFirst + distance(focalAFirst, focalALast)) and the given rotation matrix.
         * @param focalAFirst The first value of the first focal point.
         * @param focalALast One past the last value of the first focal point.
         * @param focalBFirst The first value of the second focal point.
         * @param rotationMatrix The used rotation matrix (the values should be ordered to be conform with eigens mapping of matrices to vectors.
         * (as defined by Eigen::Map<Eigen::Matrix<RealType, dimensionality, dimensionality>>)
         */
        template <class ValIter>
        UniformProlateSpheroidDistribution(ValIter focalAFirst,
                                           ValIter focalALast,
                                           ValIter focalBFirst,
                                           const std::vector<RealType>& rotationMatrix) :
            sphereDistribution{},
            tmpVal(std::distance(focalAFirst, focalALast)),
            //distanceFocalPoints,
            polarDiameter{std::numeric_limits<RealType>::infinity()},
            volumeProlateSpheroid{std::numeric_limits<RealType>::infinity()},
            focalPointA(getDimensionality()),
            focalPointB(getDimensionality()),
            centre(getDimensionality()),
            rotation{getDimensionality(), getDimensionality()},
            scaleThenRotate{getDimensionality(), getDimensionality()}
        {
 
            setFocalPoints(focalAFirst, focalBFirst, false);
            ARMARX_CHECK_EXPRESSION(rotationMatrix.size() ==
                                    getDimensionality() * getDimensionality());
 
            rotation = Eigen::Map<const MatType>(
                rotationMatrix.data(), getDimensionality(), getDimensionality());
        }
 
        /**
         * @brief Fills [first, first + dimensionality) with a uniform sample of the prolate hyper spheroid.
         * @param first The first value to fill.
         * @param gen The used generator.
         */
        template <class Generator>
        void
        operator()(RealType* first, Generator& gen)
        {
            sphereDistribution(tmpVal.begin(), tmpVal.end(), gen);
            Eigen::Map<VecType> tmpVec{tmpVal.data(), getDimensionality<long int>()};
            Eigen::Map<VecType>(first, getDimensionality()) = scaleThenRotate * tmpVec + centre;
 
            ARMARX_CHECK_EXPRESSION(isInBounds(first));
        }
 
        /**
         * @brief Returns the dimensionality of the distribution.
         * @return The dimensionality of the distribution.
         */
        template <class T = std::size_t>
        T
        getDimensionality() const
        {
            return static_cast<T>(tmpVal.size());
        }
 
        /**
         * @brief Returns whether [first, first + dimensionality) is a point contained by the prolate hyper spheroid.
         * @param first The first value of the point to check.
         * @return Whether [first, first + dimensionality) is a point contained by the prolate hyper spheroid.
         */
        bool
        isInBounds(RealType* first) const
        {
            const auto srcVec = Eigen::Map<VecType>(first, getDimensionality());
            //dont use std::numeric_limits<RealType>::epsilon() since its about 1e-7 and all the floating errors add up
            return ((focalPointA - srcVec).norm() + (focalPointB - srcVec).norm()) <=
                   getPolarDiameter() + 0.001;
        }
 
        /**
         * @brief Returns the prolate hyper spheroid's volume.
         * @return The prolate hyper spheroid's volume.
         */
        RealType
        getVolume() const
        {
            return volumeProlateSpheroid;
        }
 
        /**
         * @brief Returns the distance between the prolate hyper spheroid's focal points.
         * @return The distance between the prolate hyper spheroid's focal points.
         */
        RealType
        getDistanceFocalPoints() const
        {
            return distanceFocalPoints;
        }
 
        /**
         * @brief Returns the prolate hyper spheroid's polar diameter.
         * @return The prolate hyper spheroid's polar diameter.
         */
        RealType
        getPolarDiameter() const
        {
            return polarDiameter;
        }
 
        /**
         * @brief Sets the prolate hyper spheroid's polar diameter.
         * @param The new prolate hyper spheroid's polar diameter.
         */
        void
        setPolarDiameter(RealType newPolarDiameter)
        {
            ARMARX_CHECK_EXPRESSION(newPolarDiameter < std::numeric_limits<RealType>::infinity());
            Eigen::Map<VecType> tmpVec{tmpVal.data(), getDimensionality<long int>()};
            tmpVec(0) = newPolarDiameter / 2.0;
 
            const auto polarDiameterQuad = std::pow(newPolarDiameter, 2.0);
            const auto distanceFocalPointsQuad = std::pow(distanceFocalPoints, 2.0);
            const auto equatorialRadius =
                std::sqrt(polarDiameterQuad - distanceFocalPointsQuad) / 2.0;
 
            for (std::size_t i = 1; i < getDimensionality(); ++i) //requires this-> (2 phase lookup)
            {
                tmpVec(i) = equatorialRadius;
            }
 
            volumeProlateSpheroid = volumeOfHyperspheroid<RealType>(
                getDimensionality(), newPolarDiameter / 2.0, equatorialRadius);
            //calculate compound transformation
            scaleThenRotate = rotation * tmpVec.asDiagonal();
            polarDiameter = newPolarDiameter;
        }
 
        /**
         * @brief Returns the rotation matrix stored in a vector.
         * @return The rotation matrix stored in a vector.
         */
        std::vector<RealType>
        getRotationMatrix() const
        {
            std::vector<RealType> copy(getDimensionality() * getDimensionality());
            Eigen::Map<MatType>(copy.data(), getDimensionality(), getDimensionality()) = rotation;
            return copy;
        }
 
    protected:
        /**
         * @brief Sets the focal points.
         * @param focalAFirst The first value of the first focal point.
         * @param focalBFirst The first value of the second focal point.
         * @param updateRotationMatrix Whether the rotation matrix should be calculated.
         */
        template <class IteratorType>
        void
        setFocalPoints(IteratorType focalAFirst,
                       IteratorType focalBFirst,
                       bool updateRotationMatrix = true)
        {
            Eigen::Map<VecType> tmpVec{tmpVal.data(), getDimensionality<long int>()};
            //reset
            distanceFocalPoints = 0.0;
 
            for (std::size_t i = 0; i < getDimensionality(); ++i, ++focalAFirst, ++focalBFirst)
            {
                focalPointA(i) = *focalAFirst;
                focalPointB(i) = *focalBFirst;
                centre(i) = (focalPointA(i) + focalPointB(i)) / 2.0;
 
                const auto focalPointsDelta = focalPointB(i) - focalPointA(i);
 
                distanceFocalPoints += std::pow(focalPointsDelta, 2.0);
                tmpVec(i) = focalPointsDelta;
            }
 
            distanceFocalPoints = std::sqrt(distanceFocalPoints);
            tmpVec /= distanceFocalPoints;
 
            if (updateRotationMatrix)
            {
                refreshRotationMatrix(tmpVec);
            }
        }
 
        /**
         * @brief Calculates the rotation matrix as described in the paper
         *
         * Variable names (mostly) represent the names from the paper
         * @param normalizedFocalPointAToB corresponds to a_1 from the paper
         */
        void
        refreshRotationMatrix(const VecType& normalizedFocalPointAToB)
        {
            const auto n = getDimensionality();
            ARMARX_CHECK_EXPRESSION(normalizedFocalPointAToB.size() >= 0);
            ARMARX_CHECK_EXPRESSION(static_cast<std::size_t>(normalizedFocalPointAToB.size()) == n);
 
            if (distanceFocalPoints < std::numeric_limits<RealType>::epsilon())
            {
                //it is a circle => no rotation
                rotation.setIdentity(n, n);
            }
            else
            {
                //from eigen doc (http://eigen.tuxfamily.org/dox/classEigen_1_1JacobiSVD.html)
                //
                //NoQRPreconditioner allows not to use a QR preconditioner at all.
                //This is useful if you know that you will only be computing JacobiSVD decompositions of square matrices.
                //Non-square matrices require a QR preconditioner. Using this option will result in faster compilation
                //and smaller executable code.
                //It won't significantly speed up computation, since JacobiSVD is always checking if QR
                //preconditioning is needed before applying it anyway.
                //
                //our matrix is square, so we will take this small performance boost
                Eigen::JacobiSVD<MatType, Eigen::NoQRPreconditioner> jacoby(
                    //M = a_1 * <first column of the identity matrix>^T
                    (normalizedFocalPointAToB * (MatType::Identity(n, n).col(0).transpose())),
                    Eigen::ComputeFullV |
                        Eigen::ComputeFullU // we need U and V and we dont want thin matrices
                );
 
                //C = U * diag{1,...,1, det(U)* det(V)} * V^T
                VecType diag = VecType::Ones(n); // fill with 1
                //set last value
                diag(n - 1) = jacoby.matrixU().determinant() * jacoby.matrixV().determinant();
                //calc rotation = C = U * diag{1,...,1, det(U)* det(V)} * V^T
 
                rotation = jacoby.matrixU() * diag.asDiagonal() * jacoby.matrixV().transpose();
            }
        }
 
        //data
        /**
         * @brief The internal used sphere volume distribution. (It's results are transformed to the prolate hyper spheroid)
         */
        UniformUnitSphereDistribution<RealType> sphereDistribution;
 
        /**
         * @brief Buffer for a temporary sample.
         */
        mutable std::vector<RealType> tmpVal;
        /**
         * @brief The distance of the focalpoints.
         */
        RealType distanceFocalPoints;
        /**
         * @brief The prolate hyper spheroid's diameter.
         */
        RealType polarDiameter;
        /**
         * @brief The prolate hyper spheroid's volume.
         */
        RealType volumeProlateSpheroid;
 
        /**
         * @brief The prolate hyper spheroid's first focal point.
         */
        VecType focalPointA;
        /**
         * @brief The prolate hyper spheroid's second focal point.
         */
        VecType focalPointB;
        /**
         * @brief The prolate hyper spheroid's center. (0.5 * (focalPointA + focalPointB))
         */
        VecType centre;
        /**
         * @brief The used rotation matrix.
         */
        MatType rotation;
        /**
         * @brief The used transformation matrix to transform uniform points from the volume of a sphere
         * to a uniform sample of the prolate hyper spheroid's volume.
         */
        MatType scaleThenRotate;
    };
 
    /**
     * @brief Implements a distribution as required by Informed RRT*
     *
     * (as described in DOI>10.1109/IROS.2014.6942976)
     */
    template <class RealType = float>
    class UniformInformedProlateSpheroidDistribution
    {
    public:
        /**
         * @brief Constructs a uniform informed distribution containing a cuboid distribution (with the bounds [boundsFirst, boundsLast))
         * and a prolate hyper spheroid distribution (with the focal points [focalAFirst, focalALast)
         * and [focalBFirst, focalBFirst + distance(focalAFirst, focalALast)) and the given rotation matrix).
         * @param boundsFirst The bounds for the first dimension.
         * @param boundsLast One past the bounds for the last dimension.
         * @param focalAFirst The first value of the first focal point.
         * @param focalALast One past the last value of the first focal point.
         * @param focalBFirst The first value of the second focal point.
         * @param rotationMatrix The used rotation matrix (the values should be ordered to be conform with eigens mapping of matrices to vectors.
         * (as defined by Eigen::Map<Eigen::Matrix<RealType, dimensionality, dimensionality>>)
         */
        template <class ValIter, class BoundsIter>
        UniformInformedProlateSpheroidDistribution(BoundsIter boundsFirst,
                                                   BoundsIter boundsLast,
                                                   ValIter focalAFirst,
                                                   ValIter focalALast,
                                                   ValIter focalBFirst,
                                                   const std::vector<RealType>& rotationMatrix) :
            spheroidDistribution{focalAFirst, focalALast, focalBFirst, rotationMatrix},
            cuboidDistribution{boundsFirst, boundsLast},
            cMax{std::numeric_limits<RealType>::infinity()}
        {
            if (spheroidDistribution.getDimensionality() != cuboidDistribution.getDimensionality())
            {
                throw std::logic_error{"differend bounds for spheroid and cuboid"};
            }
        }
 
        /**
         * @brief Constructs a uniform informed distribution containing a cuboid distribution (with the bounds [boundsFirst, boundsLast))
         * and a prolate hyper spheroid distribution (with the focal points [focalAFirst, focalALast)
         * and [focalBFirst, focalBFirst + distance(focalAFirst, focalALast))).
         * @param boundsFirst The bounds for the first dimension.
         * @param boundsLast One past the bounds for the last dimension.
         * @param focalAFirst The first value of the first focal point.
         * @param focalALast One past the last value of the first focal point.
         * @param focalBFirst The first value of the second focal point.
         */
        template <class ValIter, class BoundsIter>
        UniformInformedProlateSpheroidDistribution(BoundsIter boundsFirst,
                                                   BoundsIter boundsLast,
                                                   ValIter focalAFirst,
                                                   ValIter focalALast,
                                                   ValIter focalBFirst) :
            spheroidDistribution{focalAFirst, focalALast, focalBFirst},
            cuboidDistribution{boundsFirst, boundsLast},
            cMax{std::numeric_limits<RealType>::infinity()}
        {
            if (spheroidDistribution.getDimensionality() != cuboidDistribution.getDimensionality())
            {
                throw std::logic_error{"differend bounds for spheroid and cuboid"};
            }
        }
 
        /**
         * @brief Fills [first, first + dimensionality) with a uniform sample of the intersection of the prolate hyper spheroid and cuboid space.
         * @param first The first value to fill.
         * @param gen The used generator.
         */
        template <class Generator>
        void
        operator()(RealType* first, Generator& gen)
        {
            if (cMax < std::numeric_limits<RealType>::infinity())
            {
                if (cuboidDistribution.getVolume() <= spheroidDistribution.getVolume())
                {
                    //the space has a smaller volume => sample the space
                    do
                    {
                        cuboidDistribution(first, gen);
                    } while (!spheroidDistribution.isInBounds(first));
                }
                else
                {
                    //rejection sample the ellipsoid until the result is valid
                    do
                    {
                        spheroidDistribution(first, gen);
                    } while (!cuboidDistribution.isInBounds(first));
                }
            }
            else
            {
                //sample space
                cuboidDistribution(first, gen);
            }
        }
 
        /**
         * @brief Sets the maximal cost to max if the current maximal cost is higher.
         * @param max The new maximal cost to set.
         * @return Whether the new cost was lower than the old cost.
         */
        bool
        setMaximalCost(RealType max)
        {
            max = std::max(
                max, spheroidDistribution.getDistanceFocalPoints()); //in case of floating error
 
            if (!(max < cMax))
            {
                //don't set to a worse value + if there is no change we don't need to refresh
                return false;
            }
 
            //cMax < max <= std::numeric_limits<RealType>::infinity() => cMax < inf
            cMax = max;
            //if(cMax < std::numeric_limits<RealType>::infinity())
            //{
            //since cMax is init with std::numeric_limits<RealType>::infinity()
            //it should never be std::numeric_limits<RealType>::infinity() here.
            //the assert might catch other nans
            ARMARX_CHECK_EXPRESSION(cMax < std::numeric_limits<RealType>::infinity());
            spheroidDistribution.setPolarDiameter(cMax);
            //}
            return true;
        }
 
        /**
         * @brief Returns the distributions volume. It is the minimum of the prolate hyper spheroid's and the cuboid's volumes.
         * @return The distributions volume.
         */
        RealType
        getVolume() const
        {
            return std::min(cuboidDistribution.getVolume(), spheroidDistribution.getVolume());
        }
 
        /**
         * @brief Returns the distributions dimensionality.
         * @return The distributions dimensionality.
         */
        template <class T = std::size_t>
        T
        getDimensionality() const
        {
            return cuboidDistribution.template getDimensionality<T>();
        }
 
        /**
         * @brief Returns whether [first, first + dimensionality) is a point contained by the prolate hyper spheroid and the cuboid.
         * @param first The first value of the point to check.
         * @return Whether [first, first + dimensionality) is a point contained by the prolate hyper spheroid and the cuboid.
         */
        bool
        isInBounds(const RealType* first) const
        {
            return cuboidDistribution.isInBounds(first) && spheroidDistribution.isInBounds(first);
        }
 
    protected:
        /**
         * @brief The prolate hyper spheroid's distribution.
         */
        UniformProlateSpheroidDistribution<RealType> spheroidDistribution;
        /**
         * @brief The cuboid's distribution.
         */
        UniformCuboidDistribution<RealType> cuboidDistribution;
        /**
         * @brief The current maximal cost.
         */
        RealType cMax;
    };
 
    /**
     * @brief Typedef for a sampler as required by informed rrt*.
     */
    using InformedSampler =
        Sampler<UniformInformedProlateSpheroidDistribution<float>, std::mt19937>;
} // namespace armarx