diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 49df9442a8180cf23283e61ca49b328513c01657..298ad2da19fcdbd1daa5f51a2fe10b0036f7bd56 100644 --- a/test/CMakeLists.txt +++ b/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}) diff --git a/test/gtest_local_parametrization.cpp b/test/gtest_local_parametrization.cpp new file mode 100644 index 0000000000000000000000000000000000000000..6899c66a75ed9b7862c6b7b54bfde260f7123693 --- /dev/null +++ b/test/gtest_local_parametrization.cpp @@ -0,0 +1,145 @@ +#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(); +}