From f96de72e77289fce7f0b2178b8298628ff43dae1 Mon Sep 17 00:00:00 2001 From: Simon Lynen Date: Tue, 25 Mar 2014 12:49:17 +0100 Subject: [PATCH 1/2] Adding unit test similarity transform --- msf_core/CMakeLists.txt | 4 +- .../include/msf_core/testing_predicates.h | 39 +++++++++ msf_core/src/test/test_similaritytransform.cc | 87 ++++++++++--------- 3 files changed, 87 insertions(+), 43 deletions(-) create mode 100644 msf_core/include/msf_core/testing_predicates.h diff --git a/msf_core/CMakeLists.txt b/msf_core/CMakeLists.txt index f748408c..1f8e2b3c 100644 --- a/msf_core/CMakeLists.txt +++ b/msf_core/CMakeLists.txt @@ -45,8 +45,8 @@ add_dependencies(msf_core ${PROJECT_NAME}_gencfg) add_library(similaritytransform src/similaritytransform.cc) target_link_libraries(similaritytransform ${catkin_LIBRARIES}) -add_executable(test_similaritytransform src/test/test_similaritytransform.cc) -target_link_libraries(test_similaritytransform similaritytransform) +catkin_add_gtest(test_similaritytransform src/test/test_similaritytransform.cc) +target_link_libraries(test_similaritytransform similaritytransform ${GLOG_LIBRARY}) catkin_add_gtest(test_static_statelist src/test/test_staticstatelist.cc) target_link_libraries(test_static_statelist pthread ${PROJECT_NAME} ${GLOG_LIBRARY}) diff --git a/msf_core/include/msf_core/testing_predicates.h b/msf_core/include/msf_core/testing_predicates.h new file mode 100644 index 00000000..798c45f8 --- /dev/null +++ b/msf_core/include/msf_core/testing_predicates.h @@ -0,0 +1,39 @@ +/* + * Copyright (C) 2014 Simon Lynen, ASL, ETH Zurich, Switzerland + * You can contact the author at + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef TESTING_PREDICATES_H_ +#define TESTING_PREDICATES_H_ + +#include +#include + +#define __INTERNAL_GTEST_NEAR_EIGEN(PREDICATE, matrix_A, matrix_B, precision) \ + PREDICATE##_TRUE((matrix_A).isApprox((matrix_B), precision)) \ + << "For matrices '" << #matrix_A << "' and '" << #matrix_B << "'." \ + << std::endl << "Where '" << #matrix_A << "' equals: " << std::endl \ + << (matrix_A) << std::endl \ + << "and '" << #matrix_B << "' equals: " \ + << std::endl << (matrix_B) << std::endl \ + << "and precision equals: " << precision; + +#define EXPECT_NEAR_EIGEN(matrix_A, matrix_B, precision) \ + __INTERNAL_GTEST_NEAR_EIGEN(EXPECT, matrix_A, matrix_B, precision) + +#define ASSERT_NEAR_EIGEN(matrix_A, matrix_B, precision) \ + __INTERNAL_GTEST_NEAR_EIGEN(ASSERT, matrix_A, matrix_B, precision) + +#endif // TESTING_PREDICATES_H_ diff --git a/msf_core/src/test/test_similaritytransform.cc b/msf_core/src/test/test_similaritytransform.cc index ad639f96..9b3f4935 100644 --- a/msf_core/src/test/test_similaritytransform.cc +++ b/msf_core/src/test/test_similaritytransform.cc @@ -15,14 +15,15 @@ * limitations under the License. */ #include +#include +#include -using namespace msf_core; - -int main(int /*argc*/, char** /*argv*/) { +TEST(MSF_Core, SimilarityTransform) { + using namespace msf_core; similarity_transform::From6DoF T; const int N = 100; - const double s_p = 0.1; + const double s_p = 1e-2; const double s_q = 1e-2; // The pose we want to estimate. @@ -31,9 +32,6 @@ int main(int /*argc*/, char** /*argv*/) { q.normalize(); double scale = 2; - std::cout << "Real pose: \n\tp: " << p.transpose() << "\n\tq(x y z w): " - << q.coeffs().transpose() << std::endl; - // Generate a set of measurements. for (int i = 0; i < N; i++) { Vector3 p1; @@ -68,19 +66,12 @@ int main(int /*argc*/, char** /*argv*/) { Vector3 pr = GeometryMsgsToEigen(Pd.pose.position); Eigen::Quaterniond qr = GeometryMsgsToEigen(Pd.pose.orientation); - std::cout << "\n#####\nResult:\tp:" << pr.transpose() << "\tq: " - << qr.coeffs().transpose() << "\tscale: " << _scale << std::endl; - double p_err = (pr - p).norm(); - - std::cout << "error\tp: " << p_err << "\tq: " - << q.angularDistance(qr) * 180 / M_PI << "\tscale: " - << std::abs(1.0 - scale / _scale) * 100 << "%\tcond: " << cond - << std::endl; + EXPECT_NEAR_EIGEN(pr, p, 1e-2); + EXPECT_NEAR_EIGEN(qr.coeffs(), q.coeffs(), 1e-3); // Xi matrix test. - std::cout << "\n#####\ntest Xi: \n" - << XiMat(q.coeffs()).transpose() * q.coeffs() << "\nshould be all 0" - << std::endl; + EXPECT_NEAR_EIGEN(XiMat(q.coeffs()).transpose() * q.coeffs(), + (Eigen::Matrix::Zero()), 1e-5); // Block writing test. similarity_transform::Pose::_covariance_type cov; @@ -89,33 +80,47 @@ int main(int /*argc*/, char** /*argv*/) { cov[r + c * 6] = r * c; Eigen::Map covm(cov.data()); - std::cout << "\n#####\ntest accessing blocks of cov: \n" << covm << "\np:\n" - << GeometryMsgsCovBlockToEigen(cov, geometry_msgs::cov::p, - geometry_msgs::cov::p) << "\nq:\n" - << GeometryMsgsCovBlockToEigen(cov, geometry_msgs::cov::q, - geometry_msgs::cov::q) << "\npq:\n" - << GeometryMsgsCovBlockToEigen(cov, geometry_msgs::cov::p, - geometry_msgs::cov::q) << "\nqp:\n" - << GeometryMsgsCovBlockToEigen(cov, geometry_msgs::cov::q, - geometry_msgs::cov::p) << std::endl; - - std::cout << "\n#####\ntest writing blocks of cov: " << std::endl; + EXPECT_NEAR_EIGEN(GeometryMsgsCovBlockToEigen(cov, geometry_msgs::cov::p, + geometry_msgs::cov::p), + (covm.block<3, 3>(0, 0)), 1e-5); + + EXPECT_NEAR_EIGEN(GeometryMsgsCovBlockToEigen(cov, geometry_msgs::cov::q, + geometry_msgs::cov::q), + (covm.block<3, 3>(3, 3)), 1e-5); + + EXPECT_NEAR_EIGEN(GeometryMsgsCovBlockToEigen(cov, geometry_msgs::cov::p, + geometry_msgs::cov::q), + (covm.block<3, 3>(0, 3)), 1e-5); + + EXPECT_NEAR_EIGEN(GeometryMsgsCovBlockToEigen(cov, geometry_msgs::cov::q, + geometry_msgs::cov::p), + (covm.block<3, 3>(3, 0)), 1e-5); + Matrix3 covp; covp << 31, 32, 33, 32, 34, 35, 33, 35, 36; Matrix3 covq; covq << 21, 22, 23, 22, 24, 25, 23, 25, 26; - Eigen::Matrix covpq; - covpq << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16; - std::cout << "\ncovp:\n" << covp << "\ncovq:\n" << covq - << "\ncovpq (we will only take the block starting at 1,1):\n" << covpq - << std::endl; - EigenCovBlockToGeometryMsgs(cov, covp, geometry_msgs::cov::p, - geometry_msgs::cov::p); + Matrix3 covpq; + covpq << 1, 2, 3, 4, 5, 6, 7, 8, 9; + EigenCovBlockToGeometryMsgs(cov, covq, geometry_msgs::cov::q, - geometry_msgs::cov::q); - EigenCovBlockToGeometryMsgs(cov, covpq.block<3, 3>(1, 1), - geometry_msgs::cov::p, geometry_msgs::cov::q); - std::cout << "Result: \n" << covm << std::endl; + geometry_msgs::cov::q); + EigenCovBlockToGeometryMsgs(cov, covp, geometry_msgs::cov::p, + geometry_msgs::cov::p); + + EigenCovBlockToGeometryMsgs(cov, covpq, geometry_msgs::cov::p, + geometry_msgs::cov::q); + + EigenCovBlockToGeometryMsgs(cov, covpq.transpose(), geometry_msgs::cov::q, + geometry_msgs::cov::p); - return 0; + EXPECT_NEAR_EIGEN((covm.block<3, 3>(0, 0)), covp, 1e-5); + + EXPECT_NEAR_EIGEN((covm.block<3, 3>(3, 3)), covq, 1e-5); + + EXPECT_NEAR_EIGEN((covm.block<3, 3>(0, 3)), covpq, 1e-5); + + EXPECT_NEAR_EIGEN((covm.block<3, 3>(3, 0)), covpq.transpose(), 1e-5); } + +MSF_UNITTEST_ENTRYPOINT From 99346c6fc0d33c7d4b7bd45ff217076c732565a9 Mon Sep 17 00:00:00 2001 From: Simon Lynen Date: Tue, 25 Mar 2014 12:53:11 +0100 Subject: [PATCH 2/2] Split similarity transform tests into separate test cases --- msf_core/src/test/test_similaritytransform.cc | 123 ++++++++++-------- 1 file changed, 71 insertions(+), 52 deletions(-) diff --git a/msf_core/src/test/test_similaritytransform.cc b/msf_core/src/test/test_similaritytransform.cc index 9b3f4935..1525ace1 100644 --- a/msf_core/src/test/test_similaritytransform.cc +++ b/msf_core/src/test/test_similaritytransform.cc @@ -18,61 +18,19 @@ #include #include -TEST(MSF_Core, SimilarityTransform) { +TEST(MSF_Core, XiMatrix) { using namespace msf_core; - - similarity_transform::From6DoF T; - const int N = 100; - const double s_p = 1e-2; - const double s_q = 1e-2; - - // The pose we want to estimate. - Vector3 p(Vector3::Random()); Eigen::Quaterniond q(Eigen::Matrix::Random()); q.normalize(); - double scale = 2; - - // Generate a set of measurements. - for (int i = 0; i < N; i++) { - Vector3 p1; - p1 = p + Vector3::Random() * 0.1 + Vector3(3, 4, 5); - Eigen::Quaterniond q1(Eigen::Matrix::Random()); - q1.normalize(); - - Vector3 p2 = (q1 * p + p1) / scale; - Eigen::Quaterniond q2 = q1 * q; - p2 += (Vector3::Random() * s_p); - Eigen::Quaterniond q_noise(Eigen::Quaterniond::Identity()); - q_noise.coeffs() += Eigen::Matrix::Random() * s_q; - q_noise.normalize(); - q2 = q2 * q_noise; - - similarity_transform::Pose P1; - similarity_transform::Pose P2; - P1.pose.position = EigenToGeometryMsgs(p1); - P1.pose.orientation = EigenToGeometryMsgs(q1); - P2.pose.position = EigenToGeometryMsgs(p2); - P2.pose.orientation = EigenToGeometryMsgs(q2); - - T.AddMeasurement(P1, P2); - } - - // Estimate pose. - similarity_transform::Pose Pd; - double cond; - double _scale; - T.Compute(Pd, &_scale, &cond); - - Vector3 pr = GeometryMsgsToEigen(Pd.pose.position); - Eigen::Quaterniond qr = GeometryMsgsToEigen(Pd.pose.orientation); - - EXPECT_NEAR_EIGEN(pr, p, 1e-2); - EXPECT_NEAR_EIGEN(qr.coeffs(), q.coeffs(), 1e-3); // Xi matrix test. EXPECT_NEAR_EIGEN(XiMat(q.coeffs()).transpose() * q.coeffs(), - (Eigen::Matrix::Zero()), 1e-5); + (Eigen::Matrix::Zero()), + 1e-5); +} +TEST(MSF_Core, GeometryMsgsCovBlockToEigen) { + using namespace msf_core; // Block writing test. similarity_transform::Pose::_covariance_type cov; for (int r = 0; r < 6; r++) @@ -81,21 +39,24 @@ TEST(MSF_Core, SimilarityTransform) { Eigen::Map covm(cov.data()); EXPECT_NEAR_EIGEN(GeometryMsgsCovBlockToEigen(cov, geometry_msgs::cov::p, - geometry_msgs::cov::p), + geometry_msgs::cov::p), (covm.block<3, 3>(0, 0)), 1e-5); EXPECT_NEAR_EIGEN(GeometryMsgsCovBlockToEigen(cov, geometry_msgs::cov::q, - geometry_msgs::cov::q), + geometry_msgs::cov::q), (covm.block<3, 3>(3, 3)), 1e-5); EXPECT_NEAR_EIGEN(GeometryMsgsCovBlockToEigen(cov, geometry_msgs::cov::p, - geometry_msgs::cov::q), + geometry_msgs::cov::q), (covm.block<3, 3>(0, 3)), 1e-5); EXPECT_NEAR_EIGEN(GeometryMsgsCovBlockToEigen(cov, geometry_msgs::cov::q, - geometry_msgs::cov::p), + geometry_msgs::cov::p), (covm.block<3, 3>(3, 0)), 1e-5); +} +TEST(MSF_Core, EigenCovBlockToGeometryMsgs) { + using namespace msf_core; Matrix3 covp; covp << 31, 32, 33, 32, 34, 35, 33, 35, 36; Matrix3 covq; @@ -103,6 +64,12 @@ TEST(MSF_Core, SimilarityTransform) { Matrix3 covpq; covpq << 1, 2, 3, 4, 5, 6, 7, 8, 9; + similarity_transform::Pose::_covariance_type cov; + for (int r = 0; r < 6; r++) + for (int c = 0; c < 6; c++) + cov[r + c * 6] = r * c; + Eigen::Map covm(cov.data()); + EigenCovBlockToGeometryMsgs(cov, covq, geometry_msgs::cov::q, geometry_msgs::cov::q); EigenCovBlockToGeometryMsgs(cov, covp, geometry_msgs::cov::p, @@ -123,4 +90,56 @@ TEST(MSF_Core, SimilarityTransform) { EXPECT_NEAR_EIGEN((covm.block<3, 3>(3, 0)), covpq.transpose(), 1e-5); } +TEST(MSF_Core, SimilarityTransform) { + using namespace msf_core; + + similarity_transform::From6DoF T; + const int N = 100; + const double s_p = 1e-2; + const double s_q = 1e-2; + + // The pose we want to estimate. + Vector3 p(Vector3::Random()); + Eigen::Quaterniond q(Eigen::Matrix::Random()); + q.normalize(); + double scale = 2; + + // Generate a set of measurements. + for (int i = 0; i < N; i++) { + Vector3 p1; + p1 = p + Vector3::Random() * 0.1 + Vector3(3, 4, 5); + Eigen::Quaterniond q1(Eigen::Matrix::Random()); + q1.normalize(); + + Vector3 p2 = (q1 * p + p1) / scale; + Eigen::Quaterniond q2 = q1 * q; + p2 += (Vector3::Random() * s_p); + Eigen::Quaterniond q_noise(Eigen::Quaterniond::Identity()); + q_noise.coeffs() += Eigen::Matrix::Random() * s_q; + q_noise.normalize(); + q2 = q2 * q_noise; + + similarity_transform::Pose P1; + similarity_transform::Pose P2; + P1.pose.position = EigenToGeometryMsgs(p1); + P1.pose.orientation = EigenToGeometryMsgs(q1); + P2.pose.position = EigenToGeometryMsgs(p2); + P2.pose.orientation = EigenToGeometryMsgs(q2); + + T.AddMeasurement(P1, P2); + } + + // Estimate pose. + similarity_transform::Pose Pd; + double cond; + double _scale; + T.Compute(Pd, &_scale, &cond); + + Vector3 pr = GeometryMsgsToEigen(Pd.pose.position); + Eigen::Quaterniond qr = GeometryMsgsToEigen(Pd.pose.orientation); + + EXPECT_NEAR_EIGEN(pr, p, s_p); + EXPECT_NEAR_EIGEN(qr.coeffs(), q.coeffs(), s_q); +} + MSF_UNITTEST_ENTRYPOINT