Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	tests/math/matrix-homogeneous.cpp	Lines:	58	58	100.0 %
Date:	2023-03-13 12:09:37	Branches:	240	450	53.3 %


// Copyright 2011 Florent Lamiraux

#include <sstream>

#define BOOST_TEST_MODULE matrix_homogeneous

#include <boost/math/constants/constants.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/output_test_stream.hpp>
#include <boost/test/unit_test.hpp>
#include <sot/core/matrix-geometry.hh>

using boost::math::constants::pi;
using boost::test_tools::output_test_stream;

namespace dg = dynamicgraph;

#define MATRIX_BOOST_REQUIRE_CLOSE(N, M, LEFT, RIGHT, TOLERANCE) \
  for (unsigned i = 0; i < N; ++i)                               \
    for (unsigned j = 0; j < M; ++j)                             \
  BOOST_REQUIRE_CLOSE(LEFT(i, j), RIGHT(i, j), TOLERANCE)

#define MATRIX_4x4_BOOST_REQUIRE_CLOSE(LEFT, RIGHT, TOLERANCE) \
  MATRIX_BOOST_REQUIRE_CLOSE(4, 4, LEFT, RIGHT, TOLERANCE)

#define MATRIX_IDENTITY_4x4_REQUIRE_CLOSE(M, TOLERANCE) \
  for (unsigned i = 0; i < 4; ++i)                      \
    for (unsigned j = 0; j < 4; ++j)                    \
      if (i == j)                                       \
        BOOST_REQUIRE_CLOSE(M(i, j), 1., TOLERANCE);    \
      else                                              \
        BOOST_CHECK_SMALL(M(i, j), .01 * TOLERANCE)

#define MATRIX_HOMO_INIT(M, tx, ty, tz, roll, pitch, yaw)             \
  M(0, 0) = cos(pitch) * cos(yaw);                                    \
  M(0, 1) = sin(roll) * sin(pitch) * cos(yaw) - cos(roll) * sin(yaw); \
  M(0, 2) = cos(roll) * sin(pitch) * cos(yaw) + sin(roll) * sin(yaw); \
  M(0, 3) = tx;                                                       \
  M(1, 0) = cos(pitch) * sin(yaw);                                    \
  M(1, 1) = sin(roll) * sin(pitch) * sin(yaw) + cos(roll) * cos(yaw); \
  M(1, 2) = cos(roll) * sin(pitch) * sin(yaw) - sin(roll) * cos(yaw); \
  M(1, 3) = ty;                                                       \
  M(2, 0) = -sin(pitch);                                              \
  M(2, 1) = sin(roll) * cos(pitch);                                   \
  M(2, 2) = cos(roll) * cos(pitch);                                   \
  M(2, 3) = tz;                                                       \
  M(3, 0) = 0.;                                                       \
  M(3, 1) = 0.;                                                       \
  M(3, 2) = 0.;                                                       \
  M(3, 3) = 1.

BOOST_AUTO_TEST_CASE(product) {
  for (unsigned int i = 0; i < 1000; i++) {
    double tx, ty, tz;
    double roll, pitch, yaw;
    dynamicgraph::sot::MatrixHomogeneous H1;
    tx = (10. * rand()) / RAND_MAX - 5.;
    ty = (10. * rand()) / RAND_MAX - 5.;
    tz = (10. * rand()) / RAND_MAX - 5.;
    roll = (pi<double>() * rand()) / RAND_MAX - .5 * pi<double>();
    pitch = (pi<double>() * rand()) / RAND_MAX - .5 * pi<double>();
    yaw = (2 * pi<double>() * rand()) / RAND_MAX - pi<double>();
    MATRIX_HOMO_INIT(H1, tx, ty, tz, roll, pitch, yaw);
    dg::Matrix M1(H1.matrix());
    dynamicgraph::sot::MatrixHomogeneous H2;
    tx = (10. * rand()) / RAND_MAX;
    ty = (10. * rand()) / RAND_MAX - 5.;
    tz = (10. * rand()) / RAND_MAX - 5.;
    roll = (pi<double>() * rand()) / RAND_MAX - .5 * pi<double>();
    pitch = (pi<double>() * rand()) / RAND_MAX - .5 * pi<double>();
    yaw = (2 * pi<double>() * rand()) / RAND_MAX - pi<double>();
    MATRIX_HOMO_INIT(H2, tx, ty, tz, roll, pitch, yaw);
    dg::Matrix M2(H2.matrix());
    dynamicgraph::sot::MatrixHomogeneous H3 = H1 * H2;
    dg::Matrix M3;
    M3 = M1 * M2;

    MATRIX_4x4_BOOST_REQUIRE_CLOSE(M3, H3.matrix(), 0.0001);
  }
}

BOOST_AUTO_TEST_CASE(inverse) {
  for (unsigned int i = 0; i < 1000; i++) {
    double tx, ty, tz;
    double roll, pitch, yaw;
    dynamicgraph::sot::MatrixHomogeneous H1;
    tx = (10. * rand()) / RAND_MAX - 5.;
    ty = (10. * rand()) / RAND_MAX - 5.;
    tz = (10. * rand()) / RAND_MAX - 5.;
    roll = (pi<double>() * rand()) / RAND_MAX - .5 * pi<double>();
    pitch = (pi<double>() * rand()) / RAND_MAX - .5 * pi<double>();
    yaw = (2 * pi<double>() * rand()) / RAND_MAX - pi<double>();
    MATRIX_HOMO_INIT(H1, tx, ty, tz, roll, pitch, yaw);
    dynamicgraph::sot::MatrixHomogeneous H2;
    tx = (10. * rand()) / RAND_MAX;
    ty = (10. * rand()) / RAND_MAX - 5.;
    tz = (10. * rand()) / RAND_MAX - 5.;
    roll = (pi<double>() * rand()) / RAND_MAX - .5 * pi<double>();
    pitch = (pi<double>() * rand()) / RAND_MAX - .5 * pi<double>();
    yaw = (2 * pi<double>() * rand()) / RAND_MAX - pi<double>();
    MATRIX_HOMO_INIT(H2, tx, ty, tz, roll, pitch, yaw);
    dynamicgraph::sot::MatrixHomogeneous H3 = H1 * H2;
    dynamicgraph::sot::MatrixHomogeneous invH1, invH2, invH3;
    invH1 = H1.inverse();
    invH2 = H2.inverse();
    invH3 = H3.inverse();

    dynamicgraph::sot::MatrixHomogeneous I4;
    dynamicgraph::sot::MatrixHomogeneous P1 = H1 * invH1;
    dynamicgraph::sot::MatrixHomogeneous P2 = H2 * invH2;
    dynamicgraph::sot::MatrixHomogeneous P3 = H3 * invH3;
    dynamicgraph::sot::MatrixHomogeneous P4 = invH2 * invH1;

    MATRIX_IDENTITY_4x4_REQUIRE_CLOSE(P1, 0.0001);
    MATRIX_IDENTITY_4x4_REQUIRE_CLOSE(P2, 0.0001);
    MATRIX_IDENTITY_4x4_REQUIRE_CLOSE(P3, 0.0001);
    MATRIX_4x4_BOOST_REQUIRE_CLOSE(P4, invH3, 0.0001);
  }
}


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			// Copyright 2011 Florent Lamiraux
2
3			#include <sstream>
4
5			#define BOOST_TEST_MODULE matrix_homogeneous
6
7			#include <boost/math/constants/constants.hpp>
8			#include <boost/test/floating_point_comparison.hpp>
9			#include <boost/test/output_test_stream.hpp>
10			#include <boost/test/unit_test.hpp>
11			#include <sot/core/matrix-geometry.hh>
12
13			using boost::math::constants::pi;
14			using boost::test_tools::output_test_stream;
15
16			namespace dg = dynamicgraph;
17
18			#define MATRIX_BOOST_REQUIRE_CLOSE(N, M, LEFT, RIGHT, TOLERANCE) \
19			for (unsigned i = 0; i < N; ++i) \
20			for (unsigned j = 0; j < M; ++j) \
21			BOOST_REQUIRE_CLOSE(LEFT(i, j), RIGHT(i, j), TOLERANCE)
22
23			#define MATRIX_4x4_BOOST_REQUIRE_CLOSE(LEFT, RIGHT, TOLERANCE) \
24			MATRIX_BOOST_REQUIRE_CLOSE(4, 4, LEFT, RIGHT, TOLERANCE)
25
26			#define MATRIX_IDENTITY_4x4_REQUIRE_CLOSE(M, TOLERANCE) \
27			for (unsigned i = 0; i < 4; ++i) \
28			for (unsigned j = 0; j < 4; ++j) \
29			if (i == j) \
30			BOOST_REQUIRE_CLOSE(M(i, j), 1., TOLERANCE); \
31			else \
32			BOOST_CHECK_SMALL(M(i, j), .01 * TOLERANCE)
33
34			#define MATRIX_HOMO_INIT(M, tx, ty, tz, roll, pitch, yaw) \
35			M(0, 0) = cos(pitch) * cos(yaw); \
36			M(0, 1) = sin(roll) * sin(pitch) * cos(yaw) - cos(roll) * sin(yaw); \
37			M(0, 2) = cos(roll) * sin(pitch) * cos(yaw) + sin(roll) * sin(yaw); \
38			M(0, 3) = tx; \
39			M(1, 0) = cos(pitch) * sin(yaw); \
40			M(1, 1) = sin(roll) * sin(pitch) * sin(yaw) + cos(roll) * cos(yaw); \
41			M(1, 2) = cos(roll) * sin(pitch) * sin(yaw) - sin(roll) * cos(yaw); \
42			M(1, 3) = ty; \
43			M(2, 0) = -sin(pitch); \
44			M(2, 1) = sin(roll) * cos(pitch); \
45			M(2, 2) = cos(roll) * cos(pitch); \
46			M(2, 3) = tz; \
47			M(3, 0) = 0.; \
48			M(3, 1) = 0.; \
49			M(3, 2) = 0.; \
50			M(3, 3) = 1.
51
52	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(product) {
53	✓✓	2002	for (unsigned int i = 0; i < 1000; i++) {
54			double tx, ty, tz;
55			double roll, pitch, yaw;
56	✓✗	2000	dynamicgraph::sot::MatrixHomogeneous H1;
57		2000	tx = (10. * rand()) / RAND_MAX - 5.;
58		2000	ty = (10. * rand()) / RAND_MAX - 5.;
59		2000	tz = (10. * rand()) / RAND_MAX - 5.;
60		2000	roll = (pi<double>() * rand()) / RAND_MAX - .5 * pi<double>();
61		2000	pitch = (pi<double>() * rand()) / RAND_MAX - .5 * pi<double>();
62		2000	yaw = (2 * pi<double>() * rand()) / RAND_MAX - pi<double>();
63	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗	2000	MATRIX_HOMO_INIT(H1, tx, ty, tz, roll, pitch, yaw);
64	✓✗	4000	dg::Matrix M1(H1.matrix());
65	✓✗	2000	dynamicgraph::sot::MatrixHomogeneous H2;
66		2000	tx = (10. * rand()) / RAND_MAX;
67		2000	ty = (10. * rand()) / RAND_MAX - 5.;
68		2000	tz = (10. * rand()) / RAND_MAX - 5.;
69		2000	roll = (pi<double>() * rand()) / RAND_MAX - .5 * pi<double>();
70		2000	pitch = (pi<double>() * rand()) / RAND_MAX - .5 * pi<double>();
71		2000	yaw = (2 * pi<double>() * rand()) / RAND_MAX - pi<double>();
72	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗	2000	MATRIX_HOMO_INIT(H2, tx, ty, tz, roll, pitch, yaw);
73	✓✗	4000	dg::Matrix M2(H2.matrix());
74	✓✗	2000	dynamicgraph::sot::MatrixHomogeneous H3 = H1 * H2;
75	✓✗	4000	dg::Matrix M3;
76	✓✗✓✗	2000	M3 = M1 * M2;
77
78	✓✓✓✓ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	42000	MATRIX_4x4_BOOST_REQUIRE_CLOSE(M3, H3.matrix(), 0.0001);
79			}
80		2	}
81
82	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(inverse) {
83	✓✓	2002	for (unsigned int i = 0; i < 1000; i++) {
84			double tx, ty, tz;
85			double roll, pitch, yaw;
86	✓✗	2000	dynamicgraph::sot::MatrixHomogeneous H1;
87		2000	tx = (10. * rand()) / RAND_MAX - 5.;
88		2000	ty = (10. * rand()) / RAND_MAX - 5.;
89		2000	tz = (10. * rand()) / RAND_MAX - 5.;
90		2000	roll = (pi<double>() * rand()) / RAND_MAX - .5 * pi<double>();
91		2000	pitch = (pi<double>() * rand()) / RAND_MAX - .5 * pi<double>();
92		2000	yaw = (2 * pi<double>() * rand()) / RAND_MAX - pi<double>();
93	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗	2000	MATRIX_HOMO_INIT(H1, tx, ty, tz, roll, pitch, yaw);
94	✓✗	2000	dynamicgraph::sot::MatrixHomogeneous H2;
95		2000	tx = (10. * rand()) / RAND_MAX;
96		2000	ty = (10. * rand()) / RAND_MAX - 5.;
97		2000	tz = (10. * rand()) / RAND_MAX - 5.;
98		2000	roll = (pi<double>() * rand()) / RAND_MAX - .5 * pi<double>();
99		2000	pitch = (pi<double>() * rand()) / RAND_MAX - .5 * pi<double>();
100		2000	yaw = (2 * pi<double>() * rand()) / RAND_MAX - pi<double>();
101	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗	2000	MATRIX_HOMO_INIT(H2, tx, ty, tz, roll, pitch, yaw);
102	✓✗	2000	dynamicgraph::sot::MatrixHomogeneous H3 = H1 * H2;
103	✓✗✓✗ ✓✗	2000	dynamicgraph::sot::MatrixHomogeneous invH1, invH2, invH3;
104	✓✗✓✗	2000	invH1 = H1.inverse();
105	✓✗✓✗	2000	invH2 = H2.inverse();
106	✓✗✓✗	2000	invH3 = H3.inverse();
107
108	✓✗	2000	dynamicgraph::sot::MatrixHomogeneous I4;
109	✓✗	2000	dynamicgraph::sot::MatrixHomogeneous P1 = H1 * invH1;
110	✓✗	2000	dynamicgraph::sot::MatrixHomogeneous P2 = H2 * invH2;
111	✓✗	2000	dynamicgraph::sot::MatrixHomogeneous P3 = H3 * invH3;
112	✓✗	2000	dynamicgraph::sot::MatrixHomogeneous P4 = invH2 * invH1;
113
114	✓✓✓✓ ✓✓✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	42000	MATRIX_IDENTITY_4x4_REQUIRE_CLOSE(P1, 0.0001);
115	✓✓✓✓ ✓✓✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	42000	MATRIX_IDENTITY_4x4_REQUIRE_CLOSE(P2, 0.0001);
116	✓✓✓✓ ✓✓✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	42000	MATRIX_IDENTITY_4x4_REQUIRE_CLOSE(P3, 0.0001);
117	✓✓✓✓ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	42000	MATRIX_4x4_BOOST_REQUIRE_CLOSE(P4, invH3, 0.0001);
118			}
119		2	}