static void func_jacob_LnT_T(
const CArrayDouble<12> &x,
const double &dummy, CArrayDouble<6> &Y)
{
MRPT_UNUSED_PARAM(dummy);
CPose3D p;
p.setFrom12Vector(x);
Y = p.ln();
}
// ------------------------------------------------------
// 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
}
