CPose3DPDFGaussianInf.cpp

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 "base-precomp.h"  // Precompiled headers

#include <mrpt/random.h>
#include <mrpt/math/wrap2pi.h>
#include <mrpt/utils/CStream.h>
#include <mrpt/poses/CPose3D.h>
#include <mrpt/poses/CPose3DPDFGaussianInf.h>
#include <mrpt/poses/CPose3DQuatPDFGaussian.h>
#include <mrpt/poses/CPose3DPDFGaussian.h>
#include <mrpt/poses/CPosePDFGaussianInf.h>
#include <mrpt/system/os.h>

using namespace mrpt;
using namespace mrpt::poses;
using namespace mrpt::math;
using namespace mrpt::random;
using namespace mrpt::utils;
using namespace mrpt::system;
using namespace std;

IMPLEMENTS_SERIALIZABLE(CPose3DPDFGaussianInf, CPose3DPDF, mrpt::poses)

/*---------------------------------------------------------------
        Constructor
  ---------------------------------------------------------------*/
CPose3DPDFGaussianInf::CPose3DPDFGaussianInf() : mean(0, 0, 0), cov_inv() {}
/*---------------------------------------------------------------
        Constructor
  ---------------------------------------------------------------*/
CPose3DPDFGaussianInf::CPose3DPDFGaussianInf(
        TConstructorFlags_Poses constructor_dummy_param)
        : mean(UNINITIALIZED_POSE), cov_inv(UNINITIALIZED_MATRIX)
{
        MRPT_UNUSED_PARAM(constructor_dummy_param);
}

/*---------------------------------------------------------------
        Constructor
  ---------------------------------------------------------------*/
CPose3DPDFGaussianInf::CPose3DPDFGaussianInf(
        const CPose3D& init_Mean, const CMatrixDouble66& init_Cov)
        : mean(init_Mean), cov_inv(init_Cov)
{
}

/*---------------------------------------------------------------
        Constructor
  ---------------------------------------------------------------*/
CPose3DPDFGaussianInf::CPose3DPDFGaussianInf(const CPose3D& init_Mean)
        : mean(init_Mean), cov_inv()
{
}

/*---------------------------------------------------------------
                                        CPose3DPDFGaussianInf
 ---------------------------------------------------------------*/
CPose3DPDFGaussianInf::CPose3DPDFGaussianInf(const CPose3DQuatPDFGaussian& o)
        : mean(UNINITIALIZED_POSE), cov_inv(UNINITIALIZED_MATRIX)
{
        this->copyFrom(o);
}

/*---------------------------------------------------------------
                                                copyFrom
 ---------------------------------------------------------------*/
void CPose3DPDFGaussianInf::copyFrom(const CPose3DQuatPDFGaussian& o)
{
        const CPose3DPDFGaussian p(o);
        this->copyFrom(p);
}

/*---------------------------------------------------------------
                                                writeToStream
  ---------------------------------------------------------------*/
void CPose3DPDFGaussianInf::writeToStream(
        mrpt::utils::CStream& out, int* version) const
{
        if (version)
                *version = 0;
        else
        {
                out << mean;
                for (size_t r = 0; r < size(cov_inv, 1); r++)
                        out << cov_inv.get_unsafe(r, r);
                for (size_t r = 0; r < size(cov_inv, 1); r++)
                        for (size_t c = r + 1; c < size(cov_inv, 2); c++)
                                out << cov_inv.get_unsafe(r, c);
        }
}

/*---------------------------------------------------------------
                                                readFromStream
  ---------------------------------------------------------------*/
void CPose3DPDFGaussianInf::readFromStream(
        mrpt::utils::CStream& in, int version)
{
        switch (version)
        {
                case 0:
                {
                        in >> mean;

                        for (size_t r = 0; r < size(cov_inv, 1); r++)
                                in >> cov_inv.get_unsafe(r, r);
                        for (size_t r = 0; r < size(cov_inv, 1); r++)
                                for (size_t c = r + 1; c < size(cov_inv, 2); c++)
                                {
                                        double x;
                                        in >> x;
                                        cov_inv.get_unsafe(r, c) = cov_inv.get_unsafe(c, r) = x;
                                }
                }
                break;
                default:
                        MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version)
        };
}

void CPose3DPDFGaussianInf::copyFrom(const CPose3DPDF& o)
{
        if (this == &o) return;  // It may be used sometimes

        if (IS_CLASS(&o, CPose3DPDFGaussianInf))
        {  // It's my same class:
                const CPose3DPDFGaussianInf* ptr =
                        static_cast<const CPose3DPDFGaussianInf*>(&o);
                mean = ptr->mean;
                cov_inv = ptr->cov_inv;
        }
        else
        {
                // Convert to gaussian pdf:
                CMatrixDouble66 cov(UNINITIALIZED_MATRIX);
                o.getCovarianceAndMean(cov, mean);
                cov.inv_fast(this->cov_inv);
        }
}

void CPose3DPDFGaussianInf::copyFrom(const CPosePDF& o)
{
        if (IS_CLASS(&o, CPosePDFGaussianInf))
        {  // cov is already inverted, but it's a 2D pose:
                const CPosePDFGaussianInf* ptr =
                        static_cast<const CPosePDFGaussianInf*>(&o);

                mean = CPose3D(ptr->mean);

                // 3x3 inv_cov -> 6x6 inv_cov
                cov_inv.zeros();
                cov_inv(0, 0) = ptr->cov_inv(0, 0);
                cov_inv(1, 1) = ptr->cov_inv(1, 1);
                cov_inv(3, 3) = ptr->cov_inv(2, 2);

                cov_inv(0, 1) = cov_inv(1, 0) = ptr->cov_inv(0, 1);
                cov_inv(0, 3) = cov_inv(3, 0) = ptr->cov_inv(0, 2);
                cov_inv(1, 3) = cov_inv(3, 1) = ptr->cov_inv(1, 2);
        }
        else
        {
                CPose3DPDFGaussian p(UNINITIALIZED_POSE);
                p.copyFrom(o);
                this->copyFrom(p);
        }
}

/*---------------------------------------------------------------

  ---------------------------------------------------------------*/
void CPose3DPDFGaussianInf::saveToTextFile(const string& file) const
{
        FILE* f = os::fopen(file.c_str(), "wt");
        if (!f) return;

        os::fprintf(
                f, "%e %e %e %e %e %e\n", mean.x(), mean.y(), mean.z(), mean.yaw(),
                mean.pitch(), mean.roll());

        for (unsigned int i = 0; i < 6; i++)
                os::fprintf(
                        f, "%e %e %e %e %e %e\n", cov_inv(i, 0), cov_inv(i, 1),
                        cov_inv(i, 2), cov_inv(i, 3), cov_inv(i, 4), cov_inv(i, 5));

        os::fclose(f);
}

/*---------------------------------------------------------------
                                                changeCoordinatesReference
 ---------------------------------------------------------------*/
void CPose3DPDFGaussianInf::changeCoordinatesReference(
        const CPose3D& newReferenceBase)
{
        MRPT_START

        CPose3DPDFGaussian a;
        a.copyFrom(*this);
        a.changeCoordinatesReference(newReferenceBase);
        this->copyFrom(a);

        MRPT_END
}

/*---------------------------------------------------------------
                                        drawSingleSample
 ---------------------------------------------------------------*/
void CPose3DPDFGaussianInf::drawSingleSample(CPose3D& outPart) const
{
        MRPT_UNUSED_PARAM(outPart);
        MRPT_START

        CMatrixDouble66 cov(UNINITIALIZED_MATRIX);
        this->cov_inv.inv(cov);

        CVectorDouble v;
        getRandomGenerator().drawGaussianMultivariate(v, cov);

        outPart.setFromValues(
                mean.x() + v[0], mean.y() + v[1], mean.z() + v[2], mean.yaw() + v[3],
                mean.pitch() + v[4], mean.roll() + v[5]);

        MRPT_END_WITH_CLEAN_UP(
                cov_inv.saveToTextFile(
                        "__DEBUG_EXC_DUMP_drawSingleSample_COV_INV.txt"););
}

/*---------------------------------------------------------------
                                        drawManySamples
 ---------------------------------------------------------------*/
void CPose3DPDFGaussianInf::drawManySamples(
        size_t N, vector<CVectorDouble>& outSamples) const
{
        MRPT_START

        CMatrixDouble66 cov(UNINITIALIZED_MATRIX);
        this->cov_inv.inv(cov);

        getRandomGenerator().drawGaussianMultivariateMany(outSamples, N, cov);

        for (vector<CVectorDouble>::iterator it = outSamples.begin();
                 it != outSamples.end(); ++it)
        {
                (*it)[0] += mean.x();
                (*it)[1] += mean.y();
                (*it)[2] += mean.z();
                (*it)[3] = math::wrapToPi((*it)[3] + mean.yaw());
                (*it)[4] = math::wrapToPi((*it)[4] + mean.pitch());
                (*it)[5] = math::wrapToPi((*it)[5] + mean.roll());
        }

        MRPT_END
}

/*---------------------------------------------------------------
                                        bayesianFusion
 ---------------------------------------------------------------*/
void CPose3DPDFGaussianInf::bayesianFusion(
        const CPose3DPDF& p1_, const CPose3DPDF& p2_)
{
        MRPT_UNUSED_PARAM(p1_);
        MRPT_UNUSED_PARAM(p2_);

        THROW_EXCEPTION("TO DO!!!");
}

/*---------------------------------------------------------------
                                        inverse
 ---------------------------------------------------------------*/
void CPose3DPDFGaussianInf::inverse(CPose3DPDF& o) const
{
        ASSERT_(o.GetRuntimeClass() == CLASS_ID(CPose3DPDFGaussianInf));
        CPose3DPDFGaussianInf& out = static_cast<CPose3DPDFGaussianInf&>(o);

        // This is like: b=(0,0,0)
        //  OUT = b - THIS
        CPose3DPDFGaussianInf b;  // Init: all zeros.
        out = b - *this;
}

/*---------------------------------------------------------------
                                                        +=
 ---------------------------------------------------------------*/
void CPose3DPDFGaussianInf::operator+=(const CPose3D& Ap)
{
        // COV:
        const CMatrixDouble66 OLD_COV_INV = this->cov_inv;
        CMatrixDouble66 df_dx(UNINITIALIZED_MATRIX), df_du(UNINITIALIZED_MATRIX);

        CPose3DPDF::jacobiansPoseComposition(
                this->mean,  // x
                Ap,  // u
                df_dx, df_du);

        // this->cov = H1*this->cov*~H1 + H2*Ap.cov*~H2;
        // cov_inv   = ... => The same than above!
        df_dx.multiply_HCHt(OLD_COV_INV, cov_inv);
        // df_du: Nothing to do, since COV(Ap) = zeros

        // MEAN:
        this->mean = this->mean + Ap;
}

/*---------------------------------------------------------------
                                                        +=
 ---------------------------------------------------------------*/
void CPose3DPDFGaussianInf::operator+=(const CPose3DPDFGaussianInf& Ap)
{
        CPose3DPDFGaussian a(UNINITIALIZED_POSE);
        CPose3DPDFGaussian b(UNINITIALIZED_POSE);
        a.copyFrom(*this);
        b.copyFrom(Ap);

        a += b;

        this->mean = a.mean;
        a.cov.inv(this->cov_inv);
}

/*---------------------------------------------------------------
                                                        -=
 ---------------------------------------------------------------*/
void CPose3DPDFGaussianInf::operator-=(const CPose3DPDFGaussianInf& Ap)
{
        CPose3DPDFGaussian a(UNINITIALIZED_POSE);
        CPose3DPDFGaussian b(UNINITIALIZED_POSE);
        a.copyFrom(*this);
        b.copyFrom(Ap);

        a -= b;

        this->mean = a.mean;
        a.cov.inv(this->cov_inv);
}

/*---------------------------------------------------------------
                                                evaluatePDF
 ---------------------------------------------------------------*/
double CPose3DPDFGaussianInf::evaluatePDF(const CPose3D& x) const
{
        MRPT_UNUSED_PARAM(x);
        THROW_EXCEPTION("TO DO!!!");
}

/*---------------------------------------------------------------
                                                evaluateNormalizedPDF
 ---------------------------------------------------------------*/
double CPose3DPDFGaussianInf::evaluateNormalizedPDF(const CPose3D& x) const
{
        MRPT_UNUSED_PARAM(x);
        THROW_EXCEPTION("TO DO!!!");
}

/*---------------------------------------------------------------
                                                assureSymmetry
 ---------------------------------------------------------------*/
void CPose3DPDFGaussianInf::assureSymmetry()
{
        // Differences, when they exist, appear in the ~15'th significant
        //  digit, so... just take one of them arbitrarily!
        for (unsigned int i = 0; i < size(cov_inv, 1) - 1; i++)
                for (unsigned int j = i + 1; j < size(cov_inv, 1); j++)
                        cov_inv.get_unsafe(i, j) = cov_inv.get_unsafe(j, i);
}

/*---------------------------------------------------------------
                                                mahalanobisDistanceTo
 ---------------------------------------------------------------*/
double CPose3DPDFGaussianInf::mahalanobisDistanceTo(
        const CPose3DPDFGaussianInf& theOther)
{
        MRPT_START

        const CMatrixDouble66 cov = this->cov_inv.inv();
        const CMatrixDouble66 cov2 = theOther.cov_inv.inv();

        CMatrixDouble66 COV_ = cov + cov2;
        CMatrixDouble61 MU = CMatrixDouble61(theOther.mean) - CMatrixDouble61(mean);

        for (int i = 0; i < 6; i++)
        {
                if (COV_.get_unsafe(i, i) == 0)
                {
                        if (MU.get_unsafe(i, 0) != 0)
                                return std::numeric_limits<double>::infinity();
                        else
                                COV_.get_unsafe(i, i) = 1;  // Any arbitrary value since
                        // MU(i)=0, and this value doesn't
                        // affect the result.
                }
        }

        return std::sqrt(MU.multiply_HtCH_scalar(COV_.inv()));

        MRPT_END
}

/*---------------------------------------------------------------
                                                operator <<
 ---------------------------------------------------------------*/
ostream& mrpt::poses::operator<<(ostream& out, const CPose3DPDFGaussianInf& obj)
{
        out << "Mean: " << obj.mean << "\n";
        out << "Inverse cov:\n" << obj.cov_inv << "\n";

        return out;
}

/*---------------------------------------------------------------
                                                getInvCovSubmatrix2D
 ---------------------------------------------------------------*/
void CPose3DPDFGaussianInf::getInvCovSubmatrix2D(CMatrixDouble& out_cov) const
{
        out_cov.setSize(3, 3);

        for (int i = 0; i < 3; i++)
        {
                int a = i == 2 ? 3 : i;
                for (int j = i; j < 3; j++)
                {
                        int b = j == 2 ? 3 : j;
                        double f = cov_inv.get_unsafe(a, b);
                        out_cov.set_unsafe(i, j, f);
                        out_cov.set_unsafe(j, i, f);
                }
        }
}

bool mrpt::poses::operator==(
        const CPose3DPDFGaussianInf& p1, const CPose3DPDFGaussianInf& p2)
{
        return p1.mean == p2.mean && p1.cov_inv == p2.cov_inv;
}

CPose3DPDFGaussianInf mrpt::poses::operator+(
        const CPose3DPDFGaussianInf& x, const CPose3DPDFGaussianInf& u)
{
        CPose3DPDFGaussianInf res(x);
        res += u;
        return res;
}

CPose3DPDFGaussianInf mrpt::poses::operator-(
        const CPose3DPDFGaussianInf& x, const CPose3DPDFGaussianInf& u)
{
        CPose3DPDFGaussianInf res(x);
        res -= u;
        return res;
}