# 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     |
+------------------------------------------------------------------------+ */

#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;
}
}```