void test_jacobs_P1DP2inv( double x1,double y1,double z1, double yaw1,double pitch1,double roll1, double xd,double yd,double zd, double yawd,double pitchd,double rolld, double x2,double y2,double z2, double yaw2,double pitch2,double roll2) { const CPose3D P1_(x1,y1,z1,yaw1,pitch1,roll1); const CPose3D Pd_(xd,yd,zd,yawd,pitchd,rolld); const CPose3D P2_(x2,y2,z2,yaw2,pitch2,roll2); const POSE_TYPE P1(P1_); const POSE_TYPE Pd(Pd_); const POSE_TYPE P2(P2_); const CPose3D P1DP2inv_( CMatrixDouble44(P1.getHomogeneousMatrixVal() * Pd.getHomogeneousMatrixVal() * P2.getInverseHomogeneousMatrix())); const POSE_TYPE P1DP2inv(P1DP2inv_); // Convert to 2D if needed static const int DIMS = SE_TYPE::VECTOR_SIZE; // Theoretical results: CMatrixFixedNumeric<double,DIMS,DIMS> J1,J2; SE_TYPE::jacobian_dP1DP2inv_depsilon(P1DP2inv, &J1, &J2); // Numerical approx: CMatrixFixedNumeric<double,DIMS,DIMS> num_J1,num_J2; { CArrayDouble<2*DIMS> x_mean; for (int i=0;i<DIMS+DIMS;i++) x_mean[i]=0; TParams params; params.P1 = P1; params.D = Pd; params.P2 = P2; CArrayDouble<DIMS+DIMS> x_incrs; x_incrs.assign(1e-4); CMatrixDouble numJacobs; mrpt::math::jacobians::jacob_numeric_estimate(x_mean,func_numeric,x_incrs, params, numJacobs ); numJacobs.extractMatrix(0,0, num_J1); numJacobs.extractMatrix(0,DIMS, num_J2); } const double max_eror = 1e-3; EXPECT_NEAR(0, (num_J1-J1).array().abs().sum(), max_eror ) << "p1: " << P1 << endl << "d: " << Pd << endl << "p2: " << P2 << endl << "Numeric J1:\n" << num_J1 << endl << "Implemented J1:\n" << J1 << endl << "Error:\n" << J1-num_J1 << endl; EXPECT_NEAR(0, (num_J2-J2).array().abs().sum(), max_eror ) << "p1: " << P1 << endl << "d: " << Pd << endl << "p2: " << P2 << endl << "Numeric J2:\n" << num_J2 << endl << "Implemented J2:\n" << J2 << endl << "Error:\n" << J2-num_J2 << endl; }
void test_composePointJacob(double x1,double y1,double z1, double yaw1,double pitch1,double roll1, double x,double y,double z, bool use_aprox = false) { const CPose3D p1(x1,y1,z1,yaw1,pitch1,roll1); const CPoint3D p(x,y,z); CMatrixFixedNumeric<double,3,3> df_dpoint; CMatrixFixedNumeric<double,3,6> df_dpose; TPoint3D pp; p1.composePoint(x,y,z, pp.x,pp.y,pp.z, &df_dpoint, &df_dpose, NULL, use_aprox ); // Numerical approx: CMatrixFixedNumeric<double,3,3> num_df_dpoint(UNINITIALIZED_MATRIX); CMatrixFixedNumeric<double,3,6> num_df_dpose(UNINITIALIZED_MATRIX); { CArrayDouble<6+3> x_mean; for (int i=0;i<6;i++) x_mean[i]=p1[i]; x_mean[6+0]=x; x_mean[6+1]=y; x_mean[6+2]=z; double DUMMY=0; CArrayDouble<6+3> x_incrs; x_incrs.assign(1e-7); CMatrixDouble numJacobs; mrpt::math::jacobians::jacob_numeric_estimate(x_mean,func_compose_point,x_incrs, DUMMY, numJacobs ); numJacobs.extractMatrix(0,0, num_df_dpose); numJacobs.extractMatrix(0,6, num_df_dpoint); } const double max_eror = use_aprox ? 0.1 : 3e-3; EXPECT_NEAR(0, (df_dpoint-num_df_dpoint).array().abs().sum(), max_eror ) << "p1: " << p1 << endl << "p: " << p << endl << "Numeric approximation of df_dpoint: " << endl << num_df_dpoint << endl << "Implemented method: " << endl << df_dpoint << endl << "Error: " << endl << df_dpoint-num_df_dpoint << endl; EXPECT_NEAR(0, (df_dpose-num_df_dpose).array().abs().sum(), max_eror ) << "p1: " << p1 << endl << "p: " << p << endl << "Numeric approximation of df_dpose: " << endl << num_df_dpose << endl << "Implemented method: " << endl << df_dpose << endl << "Error: " << endl << df_dpose-num_df_dpose << endl; }
void testCompositionJacobian( double x,double y, double z, double yaw, double pitch, double roll, double x2,double y2, double z2, double yaw2, double pitch2, double roll2) { const CPose3D q1(x,y,z,yaw,pitch,roll); const CPose3D q2(x2,y2,z2,yaw2,pitch2,roll2); // Theoretical Jacobians: CMatrixDouble66 df_dx(UNINITIALIZED_MATRIX), df_du(UNINITIALIZED_MATRIX); CPose3DPDF::jacobiansPoseComposition( q1, // x q2, // u df_dx, df_du ); // Numerical approximation: CMatrixDouble66 num_df_dx(UNINITIALIZED_MATRIX), num_df_du(UNINITIALIZED_MATRIX); { CArrayDouble<2*6> x_mean; for (int i=0;i<6;i++) x_mean[i]=q1[i]; for (int i=0;i<6;i++) x_mean[6+i]=q2[i]; double DUMMY=0; CArrayDouble<2*6> x_incrs; x_incrs.assign(1e-7); CMatrixDouble numJacobs; mrpt::math::jacobians::jacob_numeric_estimate(x_mean,func_compose,x_incrs, DUMMY, numJacobs ); numJacobs.extractMatrix(0,0, num_df_dx); numJacobs.extractMatrix(0,6, num_df_du); } // Compare: EXPECT_NEAR(0, (df_dx-num_df_dx).Abs().sumAll(), 3e-3 ) << "q1: " << q1 << endl << "q2: " << q2 << endl << "Numeric approximation of df_dx: " << endl << num_df_dx << endl << "Implemented method: " << endl << df_dx << endl << "Error: " << endl << df_dx-num_df_dx << endl; EXPECT_NEAR(0, (df_du-num_df_du).Abs().sumAll(), 3e-3 ) << "q1: " << q1 << endl << "q2: " << q2 << endl << "Numeric approximation of df_du: " << endl << num_df_du << endl << "Implemented method: " << endl << df_du << endl << "Error: " << endl << df_du-num_df_du << endl; }