From 99346c6fc0d33c7d4b7bd45ff217076c732565a9 Mon Sep 17 00:00:00 2001 From: Simon Lynen Date: Tue, 25 Mar 2014 12:53:11 +0100 Subject: [PATCH] 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