Example: poses_unscented_transform_example

C++ example source code:

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2021, 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 <mrpt/gui/CDisplayWindow3D.h>
#include <mrpt/gui/CDisplayWindowPlots.h>
#include <mrpt/math/CVectorFixed.h>
#include <mrpt/math/transform_gaussian.h>
#include <mrpt/math/utils.h>
#include <mrpt/opengl/CEllipsoid3D.h>
#include <mrpt/opengl/CGridPlaneXY.h>
#include <mrpt/poses/CPose3D.h>
#include <mrpt/poses/CPose3DPDFGaussian.h>
#include <mrpt/poses/CPose3DQuat.h>
#include <mrpt/poses/CPose3DQuatPDFGaussian.h>
#include <mrpt/system/CTicTac.h>

#include <Eigen/Dense>
#include <iostream>

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

// Example non-linear function for SUT
//   f: R^5   => R^3
void myFun1(
    const CVectorFixedDouble<3>& x, const double& user_param,
    CVectorFixedDouble<3>& y)
{
    y[0] = cos(x[0]) * exp(x[1]) + x[0];
    y[1] = x[1] / (1 + square(x[0]));
    y[2] = x[2] / (1 + square(x[2])) + sin(x[1] * x[0]);
}

/* ------------------------------------------------------------------------
                    Test_SUT: Scaled Unscented Transform
   ------------------------------------------------------------------------ */
void Test_SUT()
{
    // Inputs:
    const double x0[] = {1.8, 0.7, 0.9};
    // clang-format off
    const double x0cov[] = {
        0.049400,  0.011403,  -0.006389,
        0.011403,  0.026432,  0.005382,
        -0.006389, 0.005382,  0.063268};
    // clang-format on

    const CVectorFixedDouble<3> x_mean(x0);
    const CMatrixFixed<double, 3, 3> x_cov(x0cov);
    const double dumm = 0;

    // Outputs:
    CVectorFixedDouble<3> y_mean;
    CMatrixDouble33 y_cov;

    // Do SUT:
    CTicTac tictac;
    size_t N = 10000;

    tictac.Tic();
    for (size_t i = 0; i < N; i++)
        mrpt::math::transform_gaussian_unscented(
            x_mean, x_cov, myFun1,
            dumm,  // fixed parameter: not used in this example
            y_mean, y_cov);

    cout << "SUT: Time (ms): " << 1e3 * tictac.Tac() / N << endl;

    // Print:
    cout << " ======= Scaled Unscented Transform ======== " << endl;
    cout << "y_mean: " << y_mean << endl;
    cout << "y_cov: " << endl << y_cov << endl << endl;

    // 3D view:
    mrpt::opengl::COpenGLScene::Ptr scene =
        mrpt::opengl::COpenGLScene::Create();
    scene->insert(opengl::CGridPlaneXY::Create(-10, 10, -10, 10, 0, 1));

    {
        opengl::CEllipsoid3D::Ptr el = opengl::CEllipsoid3D::Create();
        el->enableDrawSolid3D(false);
        el->setLocation(y_mean[0], y_mean[1], y_mean[2]);
        el->setCovMatrix(y_cov);
        el->setColor(0, 0, 1);
        scene->insert(el);
    }

    // Do Montecarlo for comparison:
    N = 10;

    std::vector<CVectorFixedDouble<3>> MC_samples;

    tictac.Tic();
    for (size_t i = 0; i < N; i++)
        mrpt::math::transform_gaussian_montecarlo(
            x_mean, x_cov, myFun1,
            dumm,  // fixed parameter: not used in this example
            y_mean, y_cov,
            5e5,  // Samples
            &MC_samples  // we want the samples.
        );

    cout << "MC: Time (ms): " << 1e3 * tictac.Tac() / N << endl;

    CVectorDouble MC_y[3];

    for (int i = 0; i < 3; i++)
        extractColumnFromVectorOfVectors(i, MC_samples, MC_y[i]);

    {
        auto el = opengl::CEllipsoid3D::Create();
        el->enableDrawSolid3D(false);
        el->setLocation(y_mean[0], y_mean[1], y_mean[2]);
        el->setCovMatrix(y_cov);
        el->setColor(0, 1, 0);
        scene->insert(el);
    }

    // Print:
    cout << " ======= Montecarlo Transform ======== " << endl;
    cout << "y_mean: " << y_mean << endl;
    cout << "y_cov: " << endl << y_cov << endl;

    // Do Linear for comparison:
    N = 100;

    CVectorFixedDouble<3> x_incrs;
    x_incrs.fill(1e-6);

    tictac.Tic();
    for (size_t i = 0; i < N; i++)
        mrpt::math::transform_gaussian_linear(
            x_mean, x_cov, myFun1,
            dumm,  // fixed parameter: not used in this example
            y_mean, y_cov, x_incrs);

    cout << "LIN: Time (ms): " << 1e3 * tictac.Tac() / N << endl;

    // Print:
    cout << " ======= Linear Transform ======== " << endl;
    cout << "y_mean: " << y_mean << endl;
    cout << "y_cov: " << endl << y_cov << endl;

    {
        auto el = opengl::CEllipsoid3D::Create();
        el->enableDrawSolid3D(false);
        el->setLocation(y_mean[0], y_mean[1], y_mean[2]);
        el->setCovMatrix(y_cov);
        el->setColor(1, 0, 0);
        scene->insert(el);
    }

    mrpt::gui::CDisplayWindow3D win(
        "Comparison SUT (blue), Linear (red), MC (green)", 400, 300);
    win.get3DSceneAndLock() = scene;
    win.unlockAccess3DScene();

    win.setCameraPointingToPoint(
        d2f(y_mean[0]), d2f(y_mean[1]), d2f(y_mean[2]));
    win.setCameraZoom(5.0);

    // MC-based histograms:
    mrpt::gui::CDisplayWindowPlots::Ptr winHistos[3];

    for (int i = 0; i < 3; i++)
    {
        winHistos[i] = mrpt::gui::CDisplayWindowPlots::Create(
            format("MC-based histogram of the %i dim", i), 300, 150);

        std::vector<double> X;
        std::vector<double> H = mrpt::math::histogram(
            MC_y[i], MC_y[i].minCoeff(), MC_y[i].maxCoeff(), 40, true, &X);

        winHistos[i]->plot(X, H, "b");
        winHistos[i]->axis_fit();
    }

    win.forceRepaint();

    cout << endl << "Press any key to exit" << endl;
    win.waitForKey();
}

// Calibration of SUT parameters for Quat -> 3D pose
// -----------------------------------------------------------

static void aux_posequat2poseypr(
    const CVectorFixedDouble<7>& x, const double& dummy,
    CVectorFixedDouble<6>& y)
{
    const CPose3DQuat p(
        x[0], x[1], x[2],
        mrpt::math::CQuaternionDouble(x[3], x[4], x[5], x[6]));
    const CPose3D p2 = CPose3D(p);
    for (int i = 0; i < 6; i++)
        y[i] = p2[i];
    // cout << "p2: " << y[3] << endl;
}

void TestCalibrate_pose2quat()
{
    // Take a 7x7 representation:
    CPose3DQuatPDFGaussian o;
    o.mean = CPose3DQuat(CPose3D(1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg));
    // o.mean = CPose3D(1.0,2.0,3.0, 00.0_deg,90.0_deg,0.0_deg);

    CMatrixFixed<double, 7, 1> v;
    mrpt::random::getRandomGenerator().drawGaussian1DMatrix(v);
    v *= 1e-3;
    o.cov.matProductOf_AAt(v);  // COV = v*vt
    for (int i = 0; i < 7; i++)
        o.cov(i, i) += 0.01;

    o.cov(0, 1) = o.cov(1, 0) = 0.007;

    cout << "p1quat: " << endl << o << endl;

    // Use UT transformation:
    //   f: R^7 => R^6
    const CVectorFixedDouble<7> x_mean(o.mean);
    CVectorFixedDouble<6> y_mean;
    static const bool elements_do_wrapPI[6] = {
        false, false, false, true, true, true};  // xyz yaw pitch roll

    static const double dummy = 0;
    // MonteCarlo:
    CVectorFixedDouble<6> MC_y_mean;
    CMatrixDouble66 MC_y_cov;
    mrpt::math::transform_gaussian_montecarlo(
        x_mean, o.cov, aux_posequat2poseypr,
        dummy,  // fixed parameter: not used in this example
        MC_y_mean, MC_y_cov, 500);
    cout << "MC: " << endl
         << MC_y_mean << endl
         << endl
         << MC_y_cov << endl
         << endl;

    // SUT:

    CPose3DPDFGaussian p_ypr;

    // double  = 1e-3;
    // alpha = x_mean.size()-3;

    mrpt::math::transform_gaussian_unscented(
        x_mean, o.cov, aux_posequat2poseypr, dummy, y_mean, p_ypr.cov,
        elements_do_wrapPI,
        1e-3,  // alpha
        0,  // K
        2.0  // beta
    );

    cout << "SUT: " << endl
         << y_mean << endl
         << endl
         << p_ypr.cov << endl
         << endl;
}

// ------------------------------------------------------
//                      MAIN
// ------------------------------------------------------
int main(int argc, char** argv)
{
    try
    {
        Test_SUT();

        // TestCalibrate_pose2quat();

        return 0;
    }
    catch (const std::exception& e)
    {
        std::cerr << "MRPT error: " << mrpt::exception_to_str(e) << std::endl;
        return -1;
    }
    catch (...)
    {
        printf("Untyped exception!");
        return -1;
    }
}