From be68199a69a1b2e3416c2244ca5d496b8feb036b Mon Sep 17 00:00:00 2001
From: jcarpent <jcarpent@laas.fr>
Date: Wed, 27 Jul 2016 19:24:15 +0200
Subject: [PATCH] [unittest] Add exp/log unit test independent from spatial
tests
---
unittest/CMakeLists.txt | 1 +
unittest/explog.cpp | 93 +++++++++++++++++++++++++++++++++++++++++
unittest/tspatial.cpp | 40 ------------------
3 files changed, 94 insertions(+), 40 deletions(-)
create mode 100644 unittest/explog.cpp
diff --git a/unittest/CMakeLists.txt b/unittest/CMakeLists.txt
index 40c5cd056..b164c786f 100644
--- a/unittest/CMakeLists.txt
+++ b/unittest/CMakeLists.txt
@@ -101,3 +101,4 @@ ADD_UNIT_TEST(energy eigen3)
ADD_UNIT_TEST(frames eigen3)
ADD_UNIT_TEST(joint-configurations eigen3)
ADD_UNIT_TEST(joint eigen3)
+ADD_UNIT_TEST(explog eigen3)
diff --git a/unittest/explog.cpp b/unittest/explog.cpp
new file mode 100644
index 000000000..d82e38061
--- /dev/null
+++ b/unittest/explog.cpp
@@ -0,0 +1,93 @@
+//
+// Copyright (c) 2016 CNRS
+// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
+//
+// This file is part of Pinocchio
+// Pinocchio is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// Pinocchio is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// Pinocchio If not, see
+// <http://www.gnu.org/licenses/>.
+
+#define BOOST_TEST_DYN_LINK
+#define BOOST_TEST_MODULE JointConfigurationsTest
+#include <boost/test/unit_test.hpp>
+#include <boost/utility/binary.hpp>
+
+#include "pinocchio/spatial/explog.hpp"
+
+using namespace se3;
+
+BOOST_AUTO_TEST_SUITE(ExpLog)
+
+BOOST_AUTO_TEST_CASE(exp)
+{
+ SE3 M(SE3::Random());
+ Motion v(Motion::Random()); v.linear().setZero();
+
+ SE3::Matrix3 R = exp3(v.angular());
+ M = exp6(v);
+
+ BOOST_CHECK(R.isApprox(M.rotation()));
+
+}
+
+BOOST_AUTO_TEST_CASE(explog)
+{
+ SE3 M(SE3::Random());
+ Motion v(Motion::Random());
+
+ SE3 M_res = exp6(log6(M));
+
+ BOOST_CHECK(M_res.isApprox(M));
+
+ Motion v_res = log6(exp6(v));
+ BOOST_CHECK(v_res.toVector().isApprox(v.toVector()));
+}
+
+BOOST_AUTO_TEST_CASE (test_basic)
+{
+ typedef se3::SE3::Vector3 Vector3;
+ typedef se3::SE3::Matrix3 Matrix3;
+ typedef Eigen::Matrix4d Matrix4;
+ typedef se3::Motion::Vector6 Vector6;
+
+ const double EPSILON = 1e-12;
+
+ // exp3 and log3.
+ Vector3 v3(Vector3::Random());
+ Matrix3 R(se3::exp3(v3));
+ BOOST_CHECK(R.transpose().isApprox(R.inverse(), EPSILON));
+ BOOST_CHECK_SMALL(R.determinant() - 1.0, EPSILON);
+ Vector3 v3FromLog(se3::log3(R));
+ BOOST_CHECK(v3.isApprox(v3FromLog, EPSILON));
+
+ // exp6 and log6.
+ se3::Motion nu = se3::Motion::Random();
+ se3::SE3 m = se3::exp6(nu);
+ BOOST_CHECK(m.rotation().transpose().isApprox(m.rotation().inverse(),
+ EPSILON));
+ BOOST_CHECK_SMALL(m.rotation().determinant() - 1.0, EPSILON);
+ se3::Motion nuFromLog(se3::log6(m));
+ BOOST_CHECK(nu.linear().isApprox(nuFromLog.linear(), EPSILON));
+ BOOST_CHECK(nu.angular().isApprox(nuFromLog.angular(), EPSILON));
+
+ Vector6 v6(Vector6::Random());
+ se3::SE3 m2(se3::exp6(v6));
+ BOOST_CHECK(m2.rotation().transpose().isApprox(m2.rotation().inverse(),
+ EPSILON));
+ BOOST_CHECK_SMALL(m2.rotation().determinant() - 1.0, EPSILON);
+ Matrix4 M = m2.toHomogeneousMatrix();
+ se3::Motion nu2FromLog(se3::log6(M));
+ Vector6 v6FromLog(nu2FromLog.toVector());
+ BOOST_CHECK(v6.isApprox(v6FromLog, EPSILON));
+}
+
+BOOST_AUTO_TEST_SUITE_END()
diff --git a/unittest/tspatial.cpp b/unittest/tspatial.cpp
index cea5ba06c..f34173c9f 100644
--- a/unittest/tspatial.cpp
+++ b/unittest/tspatial.cpp
@@ -1,6 +1,5 @@
//
// Copyright (c) 2015-2016 CNRS
-// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
@@ -30,7 +29,6 @@
#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>
-
BOOST_AUTO_TEST_SUITE ( tspatialTest)
BOOST_AUTO_TEST_CASE ( test_SE3 )
@@ -312,42 +310,4 @@ BOOST_AUTO_TEST_CASE ( test_ActOnSet )
}
-BOOST_AUTO_TEST_CASE ( test_Explog )
-{
- typedef se3::SE3::Vector3 Vector3;
- typedef se3::SE3::Matrix3 Matrix3;
- typedef Eigen::Matrix4d Matrix4;
- typedef se3::Motion::Vector6 Vector6;
-
- const double EPSILON = 1e-12;
-
- // exp3 and log3.
- Vector3 v3(Vector3::Random());
- Matrix3 R(se3::exp3(v3));
- BOOST_CHECK(R.transpose().isApprox(R.inverse(), EPSILON));
- BOOST_CHECK_SMALL(R.determinant() - 1.0, EPSILON);
- Vector3 v3FromLog(se3::log3(R));
- BOOST_CHECK(v3.isApprox(v3FromLog, EPSILON));
-
- // exp6 and log6.
- se3::Motion nu = se3::Motion::Random();
- se3::SE3 m = se3::exp6(nu);
- BOOST_CHECK(m.rotation().transpose().isApprox(m.rotation().inverse(),
- EPSILON));
- BOOST_CHECK_SMALL(m.rotation().determinant() - 1.0, EPSILON);
- se3::Motion nuFromLog(se3::log6(m));
- BOOST_CHECK(nu.linear().isApprox(nuFromLog.linear(), EPSILON));
- BOOST_CHECK(nu.angular().isApprox(nuFromLog.angular(), EPSILON));
-
- Vector6 v6(Vector6::Random());
- se3::SE3 m2(se3::exp6(v6));
- BOOST_CHECK(m2.rotation().transpose().isApprox(m2.rotation().inverse(),
- EPSILON));
- BOOST_CHECK_SMALL(m2.rotation().determinant() - 1.0, EPSILON);
- Matrix4 M = m2.toHomogeneousMatrix();
- se3::Motion nu2FromLog(se3::log6(M));
- Vector6 v6FromLog(nu2FromLog.toVector());
- BOOST_CHECK(v6.isApprox(v6FromLog, EPSILON));
-}
-
BOOST_AUTO_TEST_SUITE_END ()
--
GitLab