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; }