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 }