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