Skip to content

Commit

Permalink
Merge pull request ethz-asl#51 from ethz-asl/feature/unit_test_simila…
Browse files Browse the repository at this point in the history 
…rity_transform

Feature/unit test similarity transform
  • Loading branch information
Markus Achtelik committed Mar 25, 2014
2 parents 1018ec3 + 99346c6 commit 2bdcd19
Show file tree
Hide file tree
Showing 3 changed files with 121 additions and 58 deletions.
4 changes: 2 additions & 2 deletions msf_core/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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})
Expand Down
39 changes: 39 additions & 0 deletions msf_core/include/msf_core/testing_predicates.h
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,39 @@
/*
* Copyright (C) 2014 Simon Lynen, ASL, ETH Zurich, Switzerland
* You can contact the author at <slynen at ethz dot ch>
*
* 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 <gtest/gtest.h>
#include <glog/logging.h>

#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_
136 changes: 80 additions & 56 deletions msf_core/src/test/test_similaritytransform.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,14 +15,87 @@
* limitations under the License.
*/
#include <msf_core/similaritytransform.h>
#include <msf_core/testing_entrypoint.h>
#include <msf_core/testing_predicates.h>

using namespace msf_core;
TEST(MSF_Core, XiMatrix) {
using namespace msf_core;
Eigen::Quaterniond q(Eigen::Matrix<double, 4, 1>::Random());
q.normalize();

// Xi matrix test.
EXPECT_NEAR_EIGEN(XiMat(q.coeffs()).transpose() * q.coeffs(),
(Eigen::Matrix<double, 3, 1>::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++)
for (int c = 0; c < 6; c++)
cov[r + c * 6] = r * c;
Eigen::Map<Matrix6> covm(cov.data());

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);
}

TEST(MSF_Core, EigenCovBlockToGeometryMsgs) {
using namespace msf_core;
Matrix3 covp;
covp << 31, 32, 33, 32, 34, 35, 33, 35, 36;
Matrix3 covq;
covq << 21, 22, 23, 22, 24, 25, 23, 25, 26;
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<Matrix6> covm(cov.data());

EigenCovBlockToGeometryMsgs(cov, covq, geometry_msgs::cov::q,
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);

int main(int /*argc*/, char** /*argv*/) {
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);
}

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.
Expand All @@ -31,9 +104,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;
Expand Down Expand Up @@ -68,54 +138,8 @@ 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;

// Xi matrix test.
std::cout << "\n#####\ntest Xi: \n"
<< XiMat(q.coeffs()).transpose() * q.coeffs() << "\nshould be all 0"
<< std::endl;

// Block writing test.
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<Matrix6> 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;
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<double, 4, 4> 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);
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;

return 0;
EXPECT_NEAR_EIGEN(pr, p, s_p);
EXPECT_NEAR_EIGEN(qr.coeffs(), q.coeffs(), s_q);
}

MSF_UNITTEST_ENTRYPOINT

0 comments on commit 2bdcd19

Please sign in to comment.