test local param checks agains numerical Jacobian

src/examples/CMakeLists.txt

 
+
 # Matrix product test
 #ADD_EXECUTABLE(test_matrix_prod test_matrix_prod.cpp)
 

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