test.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2018, 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 <mrpt/poses/CPose3D.h>
#include <iostream>
 
using namespace mrpt;
using namespace mrpt::poses;
using namespace std;
 
// ------------------------------------------------------
//                              TestSE3
// ------------------------------------------------------
void TestSE3()
{
        const CPose3D p0;
        const CPose3D p1(1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0));
        const CPose3D p2(1, 2, 3, DEG2RAD(20), DEG2RAD(0), DEG2RAD(0));
 
        cout << "p0: " << p0 << " => ln SE(3)->se(3) => " << p0.ln() << endl;
        cout << "p1: " << p1 << " => ln SE(3)->se(3) => " << p1.ln() << endl;
        cout << "p2: " << p2 << " => ln SE(3)->se(3) => " << p2.ln() << endl;
 
#if 0
        const CPose3D  p3(1,2,3, DEG2RAD(20),DEG2RAD(-30),DEG2RAD(15));
        const CPose3D  p4(4,2.5,1.5, DEG2RAD(-25),DEG2RAD(5),DEG2RAD(10));
        cout << "p3+p4: "<< (p3+p4) << " => ln SE(3)->se(3) => "
                << endl << (p3.ln()+p4.ln())
                << endl << (p3+p4).ln() << endl
                << endl << CPose3D::exp( (p3.ln()+p4.ln()) ) << endl;
#endif
}
 
// ------------------------------------------------------
//                                              MAIN
// ------------------------------------------------------
int main()
{
        try
        {
                TestSE3();
                return 0;
        }
        catch (exception& e)
        {
                cout << "MRPT exception caught: " << e.what() << endl;
                return -1;
        }
        catch (...)
        {
                printf("Untyped exception!!");
                return -1;
        }
}