# Example: poses_geometry_3D_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/poses/CPoint3D.h>
#include <mrpt/poses/CPose2D.h>
#include <mrpt/poses/CPose3D.h>
#include <mrpt/system/CTicTac.h>

#include <iostream>

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

void TestGeometry3D2()
{
CPose3D iniPoseVehicle(
745.327749, 407.959716, 14.851070, -2.985091, 0.009412, 0.051315);

CPoint3D GPSPosOnVehicle(-0.25, 0, 0.10);

CPoint3D iniPoseGPS = iniPoseVehicle + GPSPosOnVehicle;

printf(
"Pose: %.6f,%.6f,%.6f", iniPoseGPS.x(), iniPoseGPS.y(), iniPoseGPS.z());
}
// ------------------------------------------------------
//                  TestGeometry3D
// ------------------------------------------------------
void TestGeometry3D()
{
// The landmark (global) position: 3D (x,y,z)
CPoint3D L(0, 4, 2);

// Robot pose: 2D (x,y,phi)

// Camera pose relative to the robot: 6D (x,y,z,yaw,pitch,roll).

// TEST 1. Relative position L' of the landmark wrt the camera
// --------------------------------------------------------------
cout << "L: " << L << endl;
cout << "R: " << R << endl;
cout << "C: " << C << endl;
cout << "R+C:" << (R + C) << endl;
// cout << (R+C).getHomogeneousMatrix();

CPoint3D L2;
CTicTac tictac;
tictac.Tic();
size_t i, N = 10000;
for (i = 0; i < N; i++)
L2 = L - (R + C);
cout << "Computation in: " << 1e6 * tictac.Tac() / ((double)N) << " us"
<< endl;

cout << "L': " << L2 << endl;

// TEST 2. Reconstruct the landmark position:
// --------------------------------------------------------------
CPoint3D L3 = R + C + L2;
cout << "R(+)C(+)L' = " << L3 << endl;
cout << "Should be equal to L = " << L << endl;

// TEST 3. Distance from the camera to the landmark
// --------------------------------------------------------------
cout << "|(R(+)C)-L|= " << (R + C).distanceTo(L) << endl;
cout << "|L-(R(+)C)|= " << (R + C).distanceTo(L) << endl;
}

// ------------------------------------------------------
//                        MAIN
// ------------------------------------------------------
int main()
{
try
{
TestGeometry3D();
// TestGeometry3D2();
return 0;
}
catch (exception& e)
{
std::cerr << "MRPT error: " << mrpt::exception_to_str(e) << std::endl;
return -1;
}
catch (...)
{
cerr << "Untyped excepcion!!";
return -1;
}
}```