gtest_local_parametrization

test/CMakeLists.txt

@@ -3,6 +3,17 @@ add_subdirectory(${PROJECT_SOURCE_DIR}/test/gtest)
 
 include_directories(${GTEST_INCLUDE_DIRS})
 
+#############
+## Testing ##
+#############
+
 # Create a specific test executable for gtest_example
 wolf_add_gtest(gtest_example gtest_example.cpp)
 target_link_libraries(gtest_example ${PROJECT_NAME})
+
+###########################
+## Testing Serialization ##
+###########################
+
+wolf_add_gtest(gtest_local_parametrization gtest_local_parametrization.cpp)
+target_link_libraries(gtest_local_parametrization ${PROJECT_NAME} ${Boost_LIBRARIES})

test/gtest_local_parametrization.cpp

+#include "utils_gtest.h"
+
+#include "../src/local_parametrization_quaternion.h"
+#include "../src/local_parametrization_homogeneous.h"
+
+#include "../src/wolf.h"
+
+class TestLocalParam : public testing::Test
+{
+public:
+  TestLocalParam() { init(); }
+
+  ~TestLocalParam() { }
+
+  // matrix dimensions do not matter, just storage size.
+  Eigen::Matrix<wolf::Scalar, 1, 12> M;
+
+  Eigen::Vector4s q, qo;
+  Eigen::Vector3s da;
+
+  Eigen::Vector4s qo_gt;
+
+  Eigen::Matrix<wolf::Scalar, 4, 3> J_gt;
+
+private:
+
+  void init()
+  {
+    q  << 0.6251779112, -0.1940971348, 0.5202638791, 0.5484563590;
+    da << 0.0823294716, -0.0604897261, -0.0329554489;
+  }
+
+};
+
+TEST_F(TestLocalParam, LocalParametrizationHomogeneous)
+{  
+  Eigen::Vector4s h; h << -0.7255368464, 0.6083535085, -0.6866418215, -0.1981112115;
+  ASSERT_NEAR(h.norm(), 1.1862641990, wolf::Constants::EPS);
+
+  Eigen::Vector3s d; d << .1, .2, .3;
+
+  Eigen::Vector4s x; x << 0, 0, 0, 1;
+
+  Eigen::Map<const Eigen::VectorXs> h_const(h.data(), 4);
+  Eigen::Map<const Eigen::VectorXs> d_const(d.data(), 3);
+
+  Eigen::Map<Eigen::VectorXs> h_out(x.data(), 4);
+
+  Eigen::Vector4s h_out_gt; h_out_gt << -0.8817108094, 0.5039779573, -0.6018324895, -0.1166733543;
+
+  Eigen::Map<Eigen::MatrixXs> J(&M(0, 0), 4, 3);
+
+  J_gt <<  0.3627684232, -0.3041767542,  0.3433209107,
+          -0.0990556058, -0.3433209107, -0.3041767542,
+           0.3433209107, -0.0990556058, -0.3627684232,
+           0.3041767542,  0.3627684232, -0.0990556058;
+
+  wolf::LocalParametrizationHomogeneous Hpar;
+
+  ASSERT_TRUE(Hpar.plus(h_const,d_const,h_out));
+
+  ASSERT_NEAR((h_out - h_out_gt).norm(), 0., wolf::Constants::EPS);
+
+  ASSERT_NEAR(h_out.norm(), h_out_gt.norm(), wolf::Constants::EPS);
+
+  ASSERT_TRUE(Hpar.computeJacobian(h_const, J));
+
+  ASSERT_NEAR((J - J_gt).norm(), 0., wolf::Constants::EPS);
+
+  PRINTF("All good at TestLocalParam::LocalParametrizationHomogeneous !\n");
+}
+
+TEST_F(TestLocalParam, LocalParametrizationQuaternionDQ_GLOBAL)
+{
+  //qo << 0.5364591896, -0.4444505784,  0.1079399116, -0.0452058963;
+
+  ASSERT_NEAR(q.norm(),  1.f, wolf::Constants::EPS);
+  //ASSERT_NEAR(qo.norm(), 0.7064130352, wolf::Constants::EPS);
+
+  Eigen::Map<const Eigen::VectorXs> q_const(q.data(), 4);
+  Eigen::Map<const Eigen::VectorXs> da_const(da.data(), 3);
+  Eigen::Map<Eigen::VectorXs> qo_(qo.data(), 4);
+
+  Eigen::Map<Eigen::MatrixXs> J(&M(0, 0), 4, 3);
+
+  J_gt << -0.3125889556,  0.0970485674, -0.2601319396,
+           0.2742281795,  0.2601319396,  0.0970485674,
+          -0.2601319396,  0.2742281795,  0.3125889556,
+          -0.0970485674, -0.3125889556,  0.2742281795;
+
+  qo_gt << 0.6279193434, -0.2421004561, 0.5213949175, 0.5246446487;
+
+  wolf::LocalParametrizationQuaternion<wolf::DQ_GLOBAL> Qpar;
+
+  ASSERT_TRUE(Qpar.plus(q_const, da_const, qo_));
+
+  ASSERT_NEAR(qo.norm(), 1.f, wolf::Constants::EPS);
+  ASSERT_NEAR((qo - qo_gt).norm(), 0., wolf::Constants::EPS);
+
+  ASSERT_TRUE(Qpar.computeJacobian(q_const, J));
+
+  ASSERT_NEAR((J - J_gt).norm(), 0., wolf::Constants::EPS);
+
+  PRINTF("All good at TestLocalParam::LocalParametrizationQuaternionDQ_GLOBAL !\n");
+}
+
+TEST_F(TestLocalParam, LocalParametrizationQuaternionDQ_LOCAL)
+{
+  //qo << 0.5364591896, -0.4444505784,  0.1079399116, -0.0452058963;
+
+  ASSERT_NEAR(q.norm(),  1.f, wolf::Constants::EPS);
+  //ASSERT_NEAR(qo.norm(), 0.7064130352, wolf::Constants::EPS);
+
+  Eigen::Map<const Eigen::VectorXs> q_const(q.data(), 4);
+  Eigen::Map<const Eigen::VectorXs> da_const(da.data(), 3);
+  Eigen::Map<Eigen::VectorXs> qo_(qo.data(), 4);
+
+  Eigen::Map<Eigen::MatrixXs> J(&M(0, 0), 4, 3);
+
+  J_gt << -0.3125889556,  0.0970485674, -0.2601319396,
+           0.2742281795, -0.2601319396, -0.0970485674,
+           0.2601319396,  0.2742281795, -0.3125889556,
+           0.0970485674,  0.3125889556,  0.2742281795;
+
+  qo_gt << 0.6657683424, -0.1786948406, 0.4995684745, 0.5246446487;
+
+  wolf::LocalParametrizationQuaternion<wolf::DQ_LOCAL> Qpar;
+
+  ASSERT_TRUE(Qpar.plus(q_const, da_const, qo_));
+
+  ASSERT_NEAR(qo.norm(), 1.f, wolf::Constants::EPS);
+  ASSERT_NEAR((qo - qo_gt).norm(), 0., wolf::Constants::EPS);
+
+  ASSERT_TRUE(Qpar.computeJacobian(q_const, J));
+
+  ASSERT_NEAR((J - J_gt).norm(), 0., wolf::Constants::EPS);
+
+  PRINTF("All good at TestLocalParam::LocalParametrizationQuaternionDQ_LOCAL !\n");
+}
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}