| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| Line | Branch | Exec | Source |
1 |
// |
||
2 |
// Copyright (c) 2016-2019 CNRS INRIA |
||
3 |
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France. |
||
4 |
// |
||
5 |
|||
6 |
#include "pinocchio/spatial/explog.hpp" |
||
7 |
|||
8 |
#include <boost/test/unit_test.hpp> |
||
9 |
#include <boost/utility/binary.hpp> |
||
10 |
|||
11 |
#include "utils/macros.hpp" |
||
12 |
|||
13 |
using namespace pinocchio; |
||
14 |
|||
15 |
BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE ) |
||
16 |
|||
17 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(exp) |
18 |
{ |
||
19 |
✓✗ | 2 |
SE3 M(SE3::Random()); |
20 |
✓✗✓✗ ✓✗ |
2 |
Motion v(Motion::Random()); v.linear().setZero(); |
21 |
|||
22 |
✓✗✓✗ |
2 |
SE3::Matrix3 R = exp3(v.angular()); |
23 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(R.isApprox(Eigen::AngleAxis<double>(v.angular().norm(), v.angular().normalized()).matrix())); |
24 |
|||
25 |
✓✗✓✗ |
2 |
SE3::Matrix3 R0 = exp3(SE3::Vector3::Zero()); |
26 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(R0.isIdentity()); |
27 |
|||
28 |
// check the NAN case |
||
29 |
#ifdef NDEBUG |
||
30 |
Motion::Vector3 vec3_nan(Motion::Vector3::Constant(NAN)); |
||
31 |
SE3::Matrix3 R_nan = exp3(vec3_nan); |
||
32 |
BOOST_CHECK(R_nan != R_nan); |
||
33 |
#endif |
||
34 |
|||
35 |
✓✗ | 2 |
M = exp6(v); |
36 |
|||
37 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(R.isApprox(M.rotation())); |
38 |
|||
39 |
// check the NAN case |
||
40 |
#ifdef NDEBUG |
||
41 |
Motion::Vector6 vec6_nan(Motion::Vector6::Constant(NAN)); |
||
42 |
SE3 M_nan = exp6(vec6_nan); |
||
43 |
BOOST_CHECK(M_nan != M_nan); |
||
44 |
#endif |
||
45 |
|||
46 |
✓✗✓✗ |
2 |
R = exp3(SE3::Vector3::Zero()); |
47 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(R.isIdentity()); |
48 |
|||
49 |
// Quaternion |
||
50 |
✓✗ | 2 |
Eigen::Quaterniond quat; |
51 |
✓✗✓✗ |
2 |
quaternion::exp3(v.angular(),quat); |
52 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(quat.toRotationMatrix().isApprox(M.rotation())); |
53 |
|||
54 |
✓✗✓✗ |
2 |
quaternion::exp3(SE3::Vector3::Zero(),quat); |
55 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(quat.toRotationMatrix().isIdentity()); |
56 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(quat.vec().isZero() && quat.coeffs().tail<1>().isOnes()); |
57 |
|||
58 |
// Check QuaternionMap |
||
59 |
✓✗ | 2 |
Eigen::Vector4d vec4; |
60 |
✓✗✓✗ |
2 |
Eigen::QuaternionMapd quat_map(vec4.data()); |
61 |
✓✗✓✗ |
2 |
quaternion::exp3(v.angular(),quat_map); |
62 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(quat_map.toRotationMatrix().isApprox(M.rotation())); |
63 |
2 |
} |
|
64 |
|||
65 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(log) |
66 |
{ |
||
67 |
✓✗ | 2 |
SE3 M(SE3::Identity()); |
68 |
✓✗✓✗ ✓✗ |
2 |
Motion v(Motion::Random()); v.linear().setZero(); |
69 |
|||
70 |
✓✗✓✗ |
2 |
SE3::Vector3 omega = log3(M.rotation()); |
71 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(omega.isZero()); |
72 |
|||
73 |
// check the NAN case |
||
74 |
#ifdef NDEBUG |
||
75 |
SE3::Matrix3 mat3_nan(SE3::Matrix3::Constant(NAN)); |
||
76 |
SE3::Vector3 omega_nan = log3(mat3_nan); |
||
77 |
BOOST_CHECK(omega_nan != omega_nan); |
||
78 |
#endif |
||
79 |
|||
80 |
✓✗ | 2 |
M.setRandom(); |
81 |
✓✗✓✗ |
2 |
M.translation().setZero(); |
82 |
|||
83 |
✓✗ | 2 |
v = log6(M); |
84 |
✓✗✓✗ |
2 |
omega = log3(M.rotation()); |
85 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(omega.isApprox(v.angular())); |
86 |
|||
87 |
// check the NAN case |
||
88 |
#ifdef NDEBUG |
||
89 |
SE3::Matrix4 mat4_nan(SE3::Matrix4::Constant(NAN)); |
||
90 |
Motion::Vector6 v_nan = log6(mat4_nan); |
||
91 |
BOOST_CHECK(v_nan != v_nan); |
||
92 |
#endif |
||
93 |
|||
94 |
// Quaternion |
||
95 |
✓✗✓✗ |
2 |
Eigen::Quaterniond quat(SE3::Matrix3::Identity()); |
96 |
✓✗ | 2 |
omega = quaternion::log3(quat); |
97 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(omega.isZero()); |
98 |
|||
99 |
✓✓ | 2002 |
for(int k = 0; k < 1e3; ++k) |
100 |
{ |
||
101 |
✓✗✓✗ |
2000 |
SE3::Vector3 w; w.setRandom(); |
102 |
✓✗ | 2000 |
quaternion::exp3(w,quat); |
103 |
✓✗ | 2000 |
SE3::Matrix3 rot = exp3(w); |
104 |
|||
105 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2000 |
BOOST_CHECK(quat.toRotationMatrix().isApprox(rot)); |
106 |
double theta; |
||
107 |
✓✗ | 2000 |
omega = quaternion::log3(quat,theta); |
108 |
2000 |
const double PI_value = PI<double>(); |
|
109 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2000 |
BOOST_CHECK(omega.norm() <= PI_value); |
110 |
double theta_ref; |
||
111 |
✓✗✓✗ |
2000 |
SE3::Vector3 omega_ref = log3(quat.toRotationMatrix(),theta_ref); |
112 |
|||
113 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2000 |
BOOST_CHECK(omega.isApprox(omega_ref)); |
114 |
} |
||
115 |
|||
116 |
|||
117 |
// Check QuaternionMap |
||
118 |
✓✗ | 2 |
Eigen::Vector4d vec4; |
119 |
✓✗✓✗ |
2 |
Eigen::QuaternionMapd quat_map(vec4.data()); |
120 |
✓✗✓✗ ✓✗ |
2 |
quat_map = SE3::Random().rotation(); |
121 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(quaternion::log3(quat_map).isApprox(log3(quat_map.toRotationMatrix()))); |
122 |
2 |
} |
|
123 |
|||
124 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(explog3) |
125 |
{ |
||
126 |
✓✗ | 2 |
SE3 M(SE3::Random()); |
127 |
✓✗✓✗ ✓✗ |
2 |
SE3::Matrix3 M_res = exp3(log3(M.rotation())); |
128 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(M_res.isApprox(M.rotation())); |
129 |
|||
130 |
✓✗✓✗ |
2 |
Motion::Vector3 v; v.setRandom(); |
131 |
✓✗✓✗ |
2 |
Motion::Vector3 v_res = log3(exp3(v)); |
132 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(v_res.isApprox(v)); |
133 |
2 |
} |
|
134 |
|||
135 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(explog3_quaternion) |
136 |
{ |
||
137 |
✓✗ | 2 |
SE3 M(SE3::Random()); |
138 |
✓✗ | 2 |
Eigen::Quaterniond quat; |
139 |
✓✗✓✗ |
2 |
quat = M.rotation(); |
140 |
✓✗ | 2 |
Eigen::Quaterniond quat_res; |
141 |
✓✗✓✗ |
2 |
quaternion::exp3(quaternion::log3(quat),quat_res); |
142 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(quat_res.isApprox(quat) || quat_res.coeffs().isApprox(-quat.coeffs())); |
143 |
|||
144 |
✓✗✓✗ |
2 |
Motion::Vector3 v; v.setRandom(); |
145 |
✓✗ | 2 |
quaternion::exp3(v,quat); |
146 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(quaternion::log3(quat).isApprox(v)); |
147 |
|||
148 |
✓✗✓✗ ✓✗✓✗ |
2 |
SE3::Matrix3 R_next = M.rotation() * exp3(v); |
149 |
✓✗✓✗ ✓✗✓✗ |
2 |
Motion::Vector3 v_est = log3(M.rotation().transpose() * R_next); |
150 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(v_est.isApprox(v)); |
151 |
|||
152 |
✓✗ | 2 |
SE3::Quaternion quat_v; |
153 |
✓✗ | 2 |
quaternion::exp3(v,quat_v); |
154 |
✓✗ | 2 |
SE3::Quaternion quat_next = quat * quat_v; |
155 |
✓✗✓✗ ✓✗ |
2 |
v_est = quaternion::log3(quat.conjugate() * quat_next); |
156 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(v_est.isApprox(v)); |
157 |
2 |
} |
|
158 |
|||
159 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(Jlog3_fd) |
160 |
{ |
||
161 |
✓✗ | 2 |
SE3 M(SE3::Random()); |
162 |
✓✗✓✗ |
2 |
SE3::Matrix3 R (M.rotation()); |
163 |
|||
164 |
✓✗✓✗ |
2 |
SE3::Matrix3 Jfd, Jlog; |
165 |
✓✗ | 2 |
Jlog3 (R, Jlog); |
166 |
✓✗ | 2 |
Jfd.setZero(); |
167 |
|||
168 |
✓✗✓✗ |
2 |
Motion::Vector3 dR; dR.setZero(); |
169 |
2 |
const double eps = 1e-8; |
|
170 |
✓✓ | 8 |
for (int i = 0; i < 3; ++i) |
171 |
{ |
||
172 |
✓✗ | 6 |
dR[i] = eps; |
173 |
✓✗✓✗ ✓✗ |
6 |
SE3::Matrix3 R_dR = R * exp3(dR); |
174 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
6 |
Jfd.col(i) = (log3(R_dR) - log3(R)) / eps; |
175 |
✓✗ | 6 |
dR[i] = 0; |
176 |
} |
||
177 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(Jfd.isApprox(Jlog, std::sqrt(eps))); |
178 |
2 |
} |
|
179 |
|||
180 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(Jexp3_fd) |
181 |
{ |
||
182 |
✓✗ | 2 |
SE3 M(SE3::Random()); |
183 |
✓✗✓✗ |
2 |
SE3::Matrix3 R (M.rotation()); |
184 |
|||
185 |
✓✗ | 2 |
Motion::Vector3 v = log3(R); |
186 |
|||
187 |
✓✗✓✗ |
2 |
SE3::Matrix3 Jexp_fd, Jexp; |
188 |
|||
189 |
✓✗✓✗ |
2 |
Jexp3(Motion::Vector3::Zero(), Jexp); |
190 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(Jexp.isIdentity()); |
191 |
|||
192 |
✓✗ | 2 |
Jexp3(v, Jexp); |
193 |
|||
194 |
✓✗✓✗ |
2 |
Motion::Vector3 dv; dv.setZero(); |
195 |
2 |
const double eps = 1e-8; |
|
196 |
✓✓ | 8 |
for (int i = 0; i < 3; ++i) |
197 |
{ |
||
198 |
✓✗ | 6 |
dv[i] = eps; |
199 |
✓✗✓✗ |
6 |
SE3::Matrix3 R_next = exp3(v+dv); |
200 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
6 |
Jexp_fd.col(i) = log3(R.transpose()*R_next) / eps; |
201 |
✓✗ | 6 |
dv[i] = 0; |
202 |
} |
||
203 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(Jexp_fd.isApprox(Jexp, std::sqrt(eps))); |
204 |
2 |
} |
|
205 |
|||
206 |
template<typename QuaternionLike, typename Matrix43Like> |
||
207 |
2 |
void Jexp3QuatLocal(const Eigen::QuaternionBase<QuaternionLike> & quat, |
|
208 |
const Eigen::MatrixBase<Matrix43Like> & Jexp) |
||
209 |
{ |
||
210 |
2 |
Matrix43Like & Jout = PINOCCHIO_EIGEN_CONST_CAST(Matrix43Like,Jexp); |
|
211 |
|||
212 |
✓✗✓✗ ✓✗ |
2 |
skew(0.5 * quat.vec(),Jout.template topRows<3>()); |
213 |
✓✗✓✗ ✓✗✓✗ |
2 |
Jout.template topRows<3>().diagonal().array() += 0.5 * quat.w(); |
214 |
✓✗✓✗ ✓✗✓✗ |
2 |
Jout.template bottomRows<1>() = -0.5 * quat.vec().transpose(); |
215 |
2 |
} |
|
216 |
|||
217 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(Jexp3_quat_fd) |
218 |
{ |
||
219 |
typedef double Scalar; |
||
220 |
✓✗✓✗ |
2 |
SE3::Vector3 w; w.setRandom(); |
221 |
✓✗✓✗ |
2 |
SE3::Quaternion quat; quaternion::exp3(w,quat); |
222 |
|||
223 |
typedef Eigen::Matrix<Scalar,4,3> Matrix43; |
||
224 |
✓✗✓✗ |
2 |
Matrix43 Jexp3, Jexp3_fd; |
225 |
✓✗ | 2 |
quaternion::Jexp3CoeffWise(w,Jexp3); |
226 |
✓✗✓✗ |
2 |
SE3::Vector3 dw; dw.setZero(); |
227 |
2 |
const double eps = 1e-8; |
|
228 |
|||
229 |
✓✓ | 8 |
for(int i = 0; i < 3; ++i) |
230 |
{ |
||
231 |
✓✗ | 6 |
dw[i] = eps; |
232 |
✓✗✓✗ ✓✗ |
6 |
SE3::Quaternion quat_plus; quaternion::exp3(w + dw,quat_plus); |
233 |
✓✗✓✗ ✓✗✓✗ |
6 |
Jexp3_fd.col(i) = (quat_plus.coeffs() - quat.coeffs()) / eps; |
234 |
✓✗ | 6 |
dw[i] = 0; |
235 |
} |
||
236 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(Jexp3.isApprox(Jexp3_fd,sqrt(eps))); |
237 |
|||
238 |
✓✗ | 2 |
SE3::Matrix3 Jlog; |
239 |
✓✗✓✗ |
2 |
pinocchio::Jlog3(quat.toRotationMatrix(),Jlog); |
240 |
|||
241 |
✓✗ | 2 |
Matrix43 Jexp_quat_local; |
242 |
✓✗ | 2 |
Jexp3QuatLocal(quat,Jexp_quat_local); |
243 |
|||
244 |
|||
245 |
✓✗✓✗ |
2 |
Matrix43 Jcompositon = Jexp3 * Jlog; |
246 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(Jcompositon.isApprox(Jexp_quat_local)); |
247 |
// std::cout << "Jcompositon\n" << Jcompositon << std::endl; |
||
248 |
// std::cout << "Jexp_quat_local\n" << Jexp_quat_local << std::endl; |
||
249 |
|||
250 |
// Arount zero |
||
251 |
✓✗ | 2 |
w.setZero(); |
252 |
✓✗ | 2 |
w.fill(1e-6); |
253 |
✓✗ | 2 |
quaternion::exp3(w,quat); |
254 |
✓✗ | 2 |
quaternion::Jexp3CoeffWise(w,Jexp3); |
255 |
✓✓ | 8 |
for(int i = 0; i < 3; ++i) |
256 |
{ |
||
257 |
✓✗ | 6 |
dw[i] = eps; |
258 |
✓✗✓✗ ✓✗ |
6 |
SE3::Quaternion quat_plus; quaternion::exp3(w + dw,quat_plus); |
259 |
✓✗✓✗ ✓✗✓✗ |
6 |
Jexp3_fd.col(i) = (quat_plus.coeffs() - quat.coeffs()) / eps; |
260 |
✓✗ | 6 |
dw[i] = 0; |
261 |
} |
||
262 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(Jexp3.isApprox(Jexp3_fd,sqrt(eps))); |
263 |
|||
264 |
2 |
} |
|
265 |
|||
266 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(Jexp3_quat) |
267 |
{ |
||
268 |
✓✗ | 2 |
SE3 M(SE3::Random()); |
269 |
✓✗✓✗ |
2 |
SE3::Quaternion quat(M.rotation()); |
270 |
|||
271 |
✓✗ | 2 |
Motion dv(Motion::Zero()); |
272 |
2 |
const double eps = 1e-8; |
|
273 |
|||
274 |
typedef Eigen::Matrix<double,7,6> Matrix76; |
||
275 |
✓✗✓✗ ✓✗ |
2 |
Matrix76 Jexp6_fd, Jexp6_quat; Jexp6_quat.setZero(); |
276 |
typedef Eigen::Matrix<double,4,3> Matrix43; |
||
277 |
✓✗✓✗ |
2 |
Matrix43 Jexp3_quat; Jexp3QuatLocal(quat,Jexp3_quat); |
278 |
✓✗ | 2 |
SE3 M_next; |
279 |
|||
280 |
✓✗✓✗ ✓✗✓✗ |
2 |
Jexp6_quat.middleRows<3>(Motion::LINEAR).middleCols<3>(Motion::LINEAR) = M.rotation(); |
281 |
✓✗✓✗ ✓✗ |
2 |
Jexp6_quat.middleRows<4>(Motion::ANGULAR).middleCols<3>(Motion::ANGULAR) = Jexp3_quat;// * Jlog6_SE3.middleRows<3>(Motion::ANGULAR); |
282 |
✓✓ | 14 |
for(int i = 0; i < 6; ++i) |
283 |
{ |
||
284 |
✓✗✓✗ |
12 |
dv.toVector()[i] = eps; |
285 |
✓✗✓✗ |
12 |
M_next = M * exp6(dv); |
286 |
✓✗✓✗ |
12 |
const SE3::Quaternion quat_next(M_next.rotation()); |
287 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
12 |
Jexp6_fd.middleRows<3>(Motion::LINEAR).col(i) = (M_next.translation() - M.translation())/eps; |
288 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
12 |
Jexp6_fd.middleRows<4>(Motion::ANGULAR).col(i) = (quat_next.coeffs() - quat.coeffs())/eps; |
289 |
✓✗✓✗ |
12 |
dv.toVector()[i] = 0.; |
290 |
} |
||
291 |
|||
292 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(Jexp6_quat.isApprox(Jexp6_fd,sqrt(eps))); |
293 |
2 |
} |
|
294 |
|||
295 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(Jexplog3) |
296 |
{ |
||
297 |
✓✗ | 2 |
Motion v(Motion::Random()); |
298 |
|||
299 |
✓✗✓✗ |
2 |
Eigen::Matrix3d R (exp3(v.angular())), |
300 |
✓✗✓✗ |
2 |
Jexp, Jlog; |
301 |
✓✗✓✗ |
2 |
Jexp3 (v.angular(), Jexp); |
302 |
✓✗ | 2 |
Jlog3 (R , Jlog); |
303 |
|||
304 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK((Jlog * Jexp).isIdentity()); |
305 |
|||
306 |
✓✗ | 2 |
SE3 M(SE3::Random()); |
307 |
✓✗✓✗ |
2 |
R = M.rotation(); |
308 |
✓✗✓✗ ✓✗ |
2 |
v.angular() = log3(R); |
309 |
✓✗ | 2 |
Jlog3 (R , Jlog); |
310 |
✓✗✓✗ |
2 |
Jexp3 (v.angular(), Jexp); |
311 |
|||
312 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK((Jexp * Jlog).isIdentity()); |
313 |
2 |
} |
|
314 |
|||
315 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(Jlog3_quat) |
316 |
{ |
||
317 |
✓✗✓✗ |
2 |
SE3::Vector3 w; w.setRandom(); |
318 |
✓✗✓✗ |
2 |
SE3::Quaternion quat; quaternion::exp3(w,quat); |
319 |
|||
320 |
✓✗ | 2 |
SE3::Matrix3 R(quat.toRotationMatrix()); |
321 |
|||
322 |
✓✗✓✗ |
2 |
SE3::Matrix3 res, res_ref; |
323 |
✓✗ | 2 |
quaternion::Jlog3(quat,res); |
324 |
✓✗ | 2 |
Jlog3(R,res_ref); |
325 |
|||
326 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(res.isApprox(res_ref)); |
327 |
2 |
} |
|
328 |
|||
329 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(explog6) |
330 |
{ |
||
331 |
✓✗ | 2 |
SE3 M(SE3::Random()); |
332 |
✓✗✓✗ |
2 |
SE3 M_res = exp6(log6(M)); |
333 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(M_res.isApprox(M)); |
334 |
|||
335 |
✓✗ | 2 |
Motion v(Motion::Random()); |
336 |
✓✗✓✗ |
2 |
Motion v_res = log6(exp6(v)); |
337 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(v_res.toVector().isApprox(v.toVector())); |
338 |
2 |
} |
|
339 |
|||
340 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(Jlog6_fd) |
341 |
{ |
||
342 |
✓✗ | 2 |
SE3 M(SE3::Random()); |
343 |
|||
344 |
✓✗✓✗ |
2 |
SE3::Matrix6 Jfd, Jlog; |
345 |
✓✗ | 2 |
Jlog6 (M, Jlog); |
346 |
✓✗ | 2 |
Jfd.setZero(); |
347 |
|||
348 |
✓✗✓✗ |
2 |
Motion dM; dM.setZero(); |
349 |
2 |
double step = 1e-8; |
|
350 |
✓✓ | 14 |
for (int i = 0; i < 6; ++i) |
351 |
{ |
||
352 |
✓✗✓✗ |
12 |
dM.toVector()[i] = step; |
353 |
✓✗✓✗ |
12 |
SE3 M_dM = M * exp6(dM); |
354 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
12 |
Jfd.col(i) = (log6(M_dM).toVector() - log6(M).toVector()) / step; |
355 |
✓✗✓✗ |
12 |
dM.toVector()[i] = 0; |
356 |
} |
||
357 |
|||
358 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(Jfd.isApprox(Jlog, sqrt(step))); |
359 |
2 |
} |
|
360 |
|||
361 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(Jlog6_of_product_fd) |
362 |
{ |
||
363 |
✓✗ | 2 |
SE3 Ma(SE3::Random()); |
364 |
✓✗ | 2 |
SE3 Mb(SE3::Random()); |
365 |
|||
366 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
SE3::Matrix6 Jlog, Ja, Jb, Jfda, Jfdb; |
367 |
✓✗✓✗ ✓✗ |
2 |
Jlog6 (Ma.inverse() * Mb, Jlog); |
368 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
Ja = - Jlog * (Ma.inverse() * Mb).toActionMatrixInverse(); |
369 |
✓✗ | 2 |
Jb = Jlog; |
370 |
✓✗ | 2 |
Jfda.setZero(); |
371 |
✓✗ | 2 |
Jfdb.setZero(); |
372 |
|||
373 |
✓✗✓✗ |
2 |
Motion dM; dM.setZero(); |
374 |
2 |
double step = 1e-8; |
|
375 |
✓✓ | 14 |
for (int i = 0; i < 6; ++i) |
376 |
{ |
||
377 |
✓✗✓✗ |
12 |
dM.toVector()[i] = step; |
378 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
12 |
Jfda.col(i) = (log6((Ma*exp6(dM)).inverse()*Mb).toVector() - log6(Ma.inverse()*Mb).toVector()) / step; |
379 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
12 |
Jfdb.col(i) = (log6(Ma.inverse()*Mb*exp6(dM)).toVector() - log6(Ma.inverse()*Mb).toVector()) / step; |
380 |
✓✗✓✗ |
12 |
dM.toVector()[i] = 0; |
381 |
} |
||
382 |
|||
383 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(Jfda.isApprox(Ja, sqrt(step))); |
384 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(Jfdb.isApprox(Jb, sqrt(step))); |
385 |
2 |
} |
|
386 |
|||
387 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(Jexplog6) |
388 |
{ |
||
389 |
✓✗ | 2 |
Motion v(Motion::Random()); |
390 |
|||
391 |
✓✗ | 2 |
SE3 M (exp6(v)); |
392 |
{ |
||
393 |
✓✗✓✗ |
2 |
Eigen::Matrix<double, 6, 6, Eigen::RowMajor> Jexp, Jlog; |
394 |
✓✗ | 2 |
Jexp6 (v, Jexp); |
395 |
✓✗ | 2 |
Jlog6 (M, Jlog); |
396 |
|||
397 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK((Jlog * Jexp).isIdentity()); |
398 |
} |
||
399 |
|||
400 |
✓✗ | 2 |
M.setRandom(); |
401 |
|||
402 |
✓✗ | 2 |
v = log6(M); |
403 |
{ |
||
404 |
✓✗✓✗ |
2 |
Eigen::Matrix<double, 6, 6> Jexp, Jlog; |
405 |
✓✗ | 2 |
Jlog6 (M, Jlog); |
406 |
✓✗ | 2 |
Jexp6 (v, Jexp); |
407 |
|||
408 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK((Jexp * Jlog).isIdentity()); |
409 |
} |
||
410 |
2 |
} |
|
411 |
|||
412 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(Hlog3_fd) |
413 |
{ |
||
414 |
typedef SE3::Vector3 Vector3; |
||
415 |
typedef SE3::Matrix3 Matrix3; |
||
416 |
✓✗✓✗ |
2 |
SE3::Quaternion q; quaternion::uniformRandom(q); |
417 |
✓✗ | 2 |
Matrix3 R (q.matrix()); |
418 |
|||
419 |
// Hlog3(R, v) returns the matrix H * v |
||
420 |
// We check that H * e_k matches the finite difference of Hlog3(R, e_k) |
||
421 |
✓✗✓✗ |
2 |
Vector3 dR; dR.setZero(); |
422 |
2 |
double step = 1e-8; |
|
423 |
✓✓ | 8 |
for (int k = 0; k < 3; ++k) |
424 |
{ |
||
425 |
✓✗✓✗ |
6 |
Vector3 e_k (Vector3::Zero()); |
426 |
✓✗ | 6 |
e_k[k] = 1.; |
427 |
|||
428 |
✓✗ | 6 |
Matrix3 Hlog_e_k; |
429 |
✓✗ | 6 |
Hlog3(R, e_k, Hlog_e_k); |
430 |
|||
431 |
✓✗✓✗ ✓✗✓✗ |
6 |
Matrix3 R_dR = R * exp3(step * e_k); |
432 |
✓✗✓✗ |
6 |
Matrix3 Jlog_R, Jlog_R_dR; |
433 |
✓✗ | 6 |
Jlog3(R, Jlog_R); |
434 |
✓✗ | 6 |
Jlog3(R_dR, Jlog_R_dR); |
435 |
|||
436 |
✓✗✓✗ ✓✗ |
6 |
Matrix3 Hlog_e_k_fd = (Jlog_R_dR - Jlog_R ) / step; |
437 |
|||
438 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
6 |
BOOST_CHECK(Hlog_e_k.isApprox(Hlog_e_k_fd, sqrt(step))); |
439 |
} |
||
440 |
2 |
} |
|
441 |
|||
442 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE (test_basic) |
443 |
{ |
||
444 |
typedef pinocchio::SE3::Vector3 Vector3; |
||
445 |
typedef pinocchio::SE3::Matrix3 Matrix3; |
||
446 |
typedef Eigen::Matrix4d Matrix4; |
||
447 |
typedef pinocchio::Motion::Vector6 Vector6; |
||
448 |
|||
449 |
2 |
const double EPSILON = 1e-12; |
|
450 |
|||
451 |
// exp3 and log3. |
||
452 |
✓✗✓✗ |
2 |
Vector3 v3(Vector3::Random()); |
453 |
✓✗ | 2 |
Matrix3 R(pinocchio::exp3(v3)); |
454 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(R.transpose().isApprox(R.inverse(), EPSILON)); |
455 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_SMALL(R.determinant() - 1.0, EPSILON); |
456 |
✓✗ | 2 |
Vector3 v3FromLog(pinocchio::log3(R)); |
457 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(v3.isApprox(v3FromLog, EPSILON)); |
458 |
|||
459 |
// exp6 and log6. |
||
460 |
✓✗ | 2 |
pinocchio::Motion nu = pinocchio::Motion::Random(); |
461 |
✓✗ | 2 |
pinocchio::SE3 m = pinocchio::exp6(nu); |
462 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(m.rotation().transpose().isApprox(m.rotation().inverse(), |
463 |
EPSILON)); |
||
464 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_SMALL(m.rotation().determinant() - 1.0, EPSILON); |
465 |
✓✗ | 2 |
pinocchio::Motion nuFromLog(pinocchio::log6(m)); |
466 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(nu.linear().isApprox(nuFromLog.linear(), EPSILON)); |
467 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(nu.angular().isApprox(nuFromLog.angular(), EPSILON)); |
468 |
|||
469 |
✓✗✓✗ |
2 |
Vector6 v6(Vector6::Random()); |
470 |
✓✗ | 2 |
pinocchio::SE3 m2(pinocchio::exp6(v6)); |
471 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(m2.rotation().transpose().isApprox(m2.rotation().inverse(), |
472 |
EPSILON)); |
||
473 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_SMALL(m2.rotation().determinant() - 1.0, EPSILON); |
474 |
✓✗ | 2 |
Matrix4 M = m2.toHomogeneousMatrix(); |
475 |
✓✗ | 2 |
pinocchio::Motion nu2FromLog(pinocchio::log6(M)); |
476 |
✓✗✓✗ |
2 |
Vector6 v6FromLog(nu2FromLog.toVector()); |
477 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(v6.isApprox(v6FromLog, EPSILON)); |
478 |
2 |
} |
|
479 |
|||
480 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_SE3_interpolate) |
481 |
{ |
||
482 |
✓✗ | 2 |
SE3 A = SE3::Random(); |
483 |
✓✗ | 2 |
SE3 B = SE3::Random(); |
484 |
|||
485 |
✓✗ | 2 |
SE3 A_bis = SE3::Interpolate(A,B,0.); |
486 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(A_bis.isApprox(A)); |
487 |
✓✗ | 2 |
SE3 B_bis = SE3::Interpolate(A,B,1.); |
488 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(B_bis.isApprox(B)); |
489 |
|||
490 |
✓✗ | 2 |
A_bis = SE3::Interpolate(A,A,1.); |
491 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(A_bis.isApprox(A)); |
492 |
|||
493 |
✓✗ | 2 |
B_bis = SE3::Interpolate(B,B,1.); |
494 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(B_bis.isApprox(B)); |
495 |
|||
496 |
✓✗ | 2 |
SE3 C = SE3::Interpolate(A,B,0.5); |
497 |
✓✗ | 2 |
SE3 D = SE3::Interpolate(B,A,0.5); |
498 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(D.isApprox(C)); |
499 |
2 |
} |
|
500 |
|||
501 |
BOOST_AUTO_TEST_SUITE_END() |
| Generated by: GCOVR (Version 4.2) |