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