From 10b4f2ac3e7800cae1c4d3eaa5224187c4ff74dc Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Sat, 5 Nov 2016 09:39:33 +0100
Subject: [PATCH] test local param checks agains numerical Jacobian
---
src/examples/CMakeLists.txt | 1 +
src/examples/test_local_param.cpp | 39 +++++++++++++++++++++++++------
2 files changed, 33 insertions(+), 7 deletions(-)
diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt
index e032be51f..db07a46d5 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 0e34b0182..c005cd83c 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;
}
--
GitLab