diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt index e032be51fbabe9b1b30f0c17eb3687654f5ab675..db07a46d5c8d3540c2fc8a5e14cc9109a787e694 100644 --- a/src/examples/CMakeLists.txt +++ b/src/examples/CMakeLists.txt @@ -1,4 +1,5 @@ + # Matrix product test #ADD_EXECUTABLE(test_matrix_prod test_matrix_prod.cpp) diff --git a/src/examples/test_local_param.cpp b/src/examples/test_local_param.cpp index 0e34b0182cfac9b5b7ba2c77a0e8f8f160f8cf62..c005cd83c8f519df1a29ecf3db61836e8a475c38 100644 --- a/src/examples/test_local_param.cpp +++ b/src/examples/test_local_param.cpp @@ -13,18 +13,32 @@ #include <iostream> +#define JAC_NUMERIC(T, _x0, _J, dx) \ +{ Eigen::VectorXs dv(3); \ + Eigen::Map<const Eigen::VectorXs> _dv (dv.data(), 3); \ + Eigen::VectorXs xo(3); \ + Eigen::Map<Eigen::VectorXs> _xo (xo.data(), 4); \ + for (int i = 0; i< 3; i++) {\ + dv.setZero();\ + dv(i) = dx;\ + T.plus(_x0, _dv, _xo);\ + _J.col(i) = (_xo - _x0)/dx; }} + + int main(){ using namespace Eigen; using namespace std; using namespace wolf; + // Storage VectorXs x(22); MatrixXs M(1,12); // matrix dimensions do not matter, just storage size. x.setRandom(); M.setZero(); + // QUATERNION ------------------------------------------ Map<VectorXs> q(&x(0),4); q.normalize(); @@ -41,21 +55,29 @@ int main(){ LocalParametrizationQuaternion<DQ_LOCAL> Qpar_loc; cout << "\nGLOBAL D_QUAT plus()" << endl; - Map<const VectorXs> q_const(q.data(),4); - Map<const VectorXs> da_const(da.data(),3); - Qpar.plus(q_const,da_const,qo); + Map<const VectorXs> q_m(q.data(),4); + Map<const VectorXs> da_m(da.data(),3); + Qpar.plus(q_m,da_m,qo); cout << "qo = " << qo.transpose() << " with norm = " << qo.norm() << endl; - Qpar.computeJacobian(q_const,J); - cout << " J = " << J << endl << endl; + Qpar.computeJacobian(q_m,J); + cout << " J = \n" << J << endl << endl; + + MatrixXs J_num(4,3); + JAC_NUMERIC(Qpar, q_m, J_num, 1e-9) + cout << " J_num = \n" << J_num << endl; cout << "\nLOCAL D_QUAT plus()" << endl; - Qpar_loc.plus(q_const,da_const,qo); + Qpar_loc.plus(q_m,da_m,qo); cout << "qo = " << qo.transpose() << " with norm = " << qo.norm() << endl; - Qpar_loc.computeJacobian(q_const,J); + Qpar_loc.computeJacobian(q_m,J); cout << " J = " << J << endl; + JAC_NUMERIC(Qpar_loc, q_m, J_num, 1e-9) + cout << " J_num = \n" << J_num << endl; + + // HOMOGENEOUS ---------------------------------------- cout << "\nHOMOGENEOUS plus()" << endl; Map<VectorXs> h(&x(11),4); h.setRandom(); @@ -77,6 +99,9 @@ int main(){ Hpar.computeJacobian(h_const,J); cout << " J = " << J << "\n" << endl; + JAC_NUMERIC(Hpar, q_m, J_num, 1e-9) + cout << " J_num = \n" << J_num << endl; + return 0; }