diff --git a/src/examples/test_local_param.cpp b/src/examples/test_local_param.cpp index c005cd83c8f519df1a29ecf3db61836e8a475c38..408ab004112a88b1f7dc0434b4171c9181dea91d 100644 --- a/src/examples/test_local_param.cpp +++ b/src/examples/test_local_param.cpp @@ -25,6 +25,9 @@ _J.col(i) = (_xo - _x0)/dx; }} + + + int main(){ using namespace Eigen; @@ -53,6 +56,7 @@ int main(){ LocalParametrizationQuaternion<DQ_GLOBAL> Qpar; LocalParametrizationQuaternion<DQ_LOCAL> Qpar_loc; + bool pass; cout << "\nGLOBAL D_QUAT plus()" << endl; Map<const VectorXs> q_m(q.data(),4); @@ -65,7 +69,10 @@ int main(){ MatrixXs J_num(4,3); JAC_NUMERIC(Qpar, q_m, J_num, 1e-9) - cout << " J_num = \n" << J_num << endl; + cout << " J_num = \n" << J_num; + + pass = (J-J_num).isMuchSmallerThan(1,1e-6); + std::cout << "Jacobians test " << (pass ? "PASSED" : "FAIL") << std::endl; cout << "\nLOCAL D_QUAT plus()" << endl; Qpar_loc.plus(q_m,da_m,qo); @@ -77,6 +84,10 @@ int main(){ JAC_NUMERIC(Qpar_loc, q_m, J_num, 1e-9) cout << " J_num = \n" << J_num << endl; + pass = (J-J_num).isMuchSmallerThan(1,1e-6); + std::cout << "Jacobians test " << (pass ? "PASSED" : "FAIL") << std::endl; + + // HOMOGENEOUS ---------------------------------------- cout << "\nHOMOGENEOUS plus()" << endl; Map<VectorXs> h(&x(11),4); @@ -102,6 +113,9 @@ int main(){ JAC_NUMERIC(Hpar, q_m, J_num, 1e-9) cout << " J_num = \n" << J_num << endl; + pass = (J-J_num).isMuchSmallerThan(1,1e-6); + std::cout << "Jacobians test " << (pass ? "PASSED" : "FAIL") << std::endl; + return 0; }