void generatePolygon(CPolyhedronPtr &poly,const vector<TPoint3D> &profile,const CPose3D &pose) {
math::TPolygon3D p(profile.size());
for (size_t i=0;i<profile.size();i++) pose.composePoint(profile[i].x,profile[i].y,profile[i].z,p[i].x,p[i].y,p[i].z);
vector<math::TPolygon3D> convexPolys;
if (!math::splitInConvexComponents(p,convexPolys)) convexPolys.push_back(p);
poly=CPolyhedron::Create(convexPolys);
}
void test_composePointJacob_se3( const CPose3D &p, const TPoint3D x_l )
{
CMatrixFixedNumeric<double,3,6> df_dse3;
TPoint3D pp;
p.composePoint(x_l.x,x_l.y,x_l.z, pp.x,pp.y,pp.z, NULL, NULL, &df_dse3);
// Numerical approx:
CMatrixFixedNumeric<double,3,6> num_df_dse3(UNINITIALIZED_MATRIX);
{
CArrayDouble<6> x_mean;
for (int i=0;i<6;i++) x_mean[i]=0;
CArrayDouble<3> P;
for (int i=0;i<3;i++) P[i]=pp[i];
CArrayDouble<6> x_incrs;
x_incrs.assign(1e-9);
mrpt::math::jacobians::jacob_numeric_estimate(x_mean,func_compose_point_se3,x_incrs, P, num_df_dse3 );
}
EXPECT_NEAR(0, (df_dse3-num_df_dse3).array().abs().sum(), 3e-3 )
<< "p: " << p << endl
<< "x_l: " << x_l << endl
<< "Numeric approximation of df_dse3: " << endl <<num_df_dse3 << endl
<< "Implemented method: " << endl << df_dse3 << endl
<< "Error: " << endl << df_dse3-num_df_dse3 << endl;
}
