Directory: | ./ |
---|---|
File: | include/coal/internal/tools.h |
Date: | 2025-04-01 09:23:31 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 76 | 97 | 78.4% |
Branches: | 75 | 118 | 63.6% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | /* | ||
2 | * Software License Agreement (BSD License) | ||
3 | * | ||
4 | * Copyright (c) 2011-2014, Willow Garage, Inc. | ||
5 | * Copyright (c) 2014-2015, Open Source Robotics Foundation | ||
6 | * All rights reserved. | ||
7 | * | ||
8 | * Redistribution and use in source and binary forms, with or without | ||
9 | * modification, are permitted provided that the following conditions | ||
10 | * are met: | ||
11 | * | ||
12 | * * Redistributions of source code must retain the above copyright | ||
13 | * notice, this list of conditions and the following disclaimer. | ||
14 | * * Redistributions in binary form must reproduce the above | ||
15 | * copyright notice, this list of conditions and the following | ||
16 | * disclaimer in the documentation and/or other materials provided | ||
17 | * with the distribution. | ||
18 | * * Neither the name of Open Source Robotics Foundation nor the names of its | ||
19 | * contributors may be used to endorse or promote products derived | ||
20 | * from this software without specific prior written permission. | ||
21 | * | ||
22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS | ||
23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT | ||
24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS | ||
25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE | ||
26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, | ||
27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, | ||
28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; | ||
29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER | ||
30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT | ||
31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN | ||
32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE | ||
33 | * POSSIBILITY OF SUCH DAMAGE. | ||
34 | */ | ||
35 | |||
36 | /** \author Joseph Mirabel */ | ||
37 | |||
38 | #ifndef COAL_INTERNAL_TOOLS_H | ||
39 | #define COAL_INTERNAL_TOOLS_H | ||
40 | |||
41 | #include "coal/fwd.hh" | ||
42 | |||
43 | #include <cmath> | ||
44 | #include <iostream> | ||
45 | #include <limits> | ||
46 | |||
47 | #include "coal/data_types.h" | ||
48 | |||
49 | namespace coal { | ||
50 | |||
51 | template <typename Derived> | ||
52 | 88901 | static inline typename Derived::Scalar triple( | |
53 | const Eigen::MatrixBase<Derived>& x, const Eigen::MatrixBase<Derived>& y, | ||
54 | const Eigen::MatrixBase<Derived>& z) { | ||
55 | 88901 | return x.derived().dot(y.derived().cross(z.derived())); | |
56 | } | ||
57 | |||
58 | template <typename Derived1, typename Derived2, typename Derived3> | ||
59 | ✗ | void generateCoordinateSystem(const Eigen::MatrixBase<Derived1>& _w, | |
60 | const Eigen::MatrixBase<Derived2>& _u, | ||
61 | const Eigen::MatrixBase<Derived3>& _v) { | ||
62 | typedef typename Derived1::Scalar T; | ||
63 | |||
64 | ✗ | Eigen::MatrixBase<Derived1>& w = const_cast<Eigen::MatrixBase<Derived1>&>(_w); | |
65 | ✗ | Eigen::MatrixBase<Derived2>& u = const_cast<Eigen::MatrixBase<Derived2>&>(_u); | |
66 | ✗ | Eigen::MatrixBase<Derived3>& v = const_cast<Eigen::MatrixBase<Derived3>&>(_v); | |
67 | |||
68 | T inv_length; | ||
69 | ✗ | if (std::abs(w[0]) >= std::abs(w[1])) { | |
70 | ✗ | inv_length = (T)1.0 / sqrt(w[0] * w[0] + w[2] * w[2]); | |
71 | ✗ | u[0] = -w[2] * inv_length; | |
72 | ✗ | u[1] = (T)0; | |
73 | ✗ | u[2] = w[0] * inv_length; | |
74 | ✗ | v[0] = w[1] * u[2]; | |
75 | ✗ | v[1] = w[2] * u[0] - w[0] * u[2]; | |
76 | ✗ | v[2] = -w[1] * u[0]; | |
77 | } else { | ||
78 | ✗ | inv_length = (T)1.0 / sqrt(w[1] * w[1] + w[2] * w[2]); | |
79 | ✗ | u[0] = (T)0; | |
80 | ✗ | u[1] = w[2] * inv_length; | |
81 | ✗ | u[2] = -w[1] * inv_length; | |
82 | ✗ | v[0] = w[1] * u[2] - w[2] * u[1]; | |
83 | ✗ | v[1] = -w[0] * u[2]; | |
84 | ✗ | v[2] = w[0] * u[1]; | |
85 | } | ||
86 | } | ||
87 | |||
88 | /* ----- Start Matrices ------ */ | ||
89 | template <typename Derived, typename OtherDerived> | ||
90 | 7163 | void relativeTransform(const Eigen::MatrixBase<Derived>& R1, | |
91 | const Eigen::MatrixBase<OtherDerived>& t1, | ||
92 | const Eigen::MatrixBase<Derived>& R2, | ||
93 | const Eigen::MatrixBase<OtherDerived>& t2, | ||
94 | const Eigen::MatrixBase<Derived>& R, | ||
95 | const Eigen::MatrixBase<OtherDerived>& t) { | ||
96 |
2/4✓ Branch 2 taken 7163 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7163 times.
✗ Branch 6 not taken.
|
7163 | const_cast<Eigen::MatrixBase<Derived>&>(R) = R1.transpose() * R2; |
97 |
3/6✓ Branch 2 taken 7163 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7163 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7163 times.
✗ Branch 9 not taken.
|
7163 | const_cast<Eigen::MatrixBase<OtherDerived>&>(t) = R1.transpose() * (t2 - t1); |
98 | 7163 | } | |
99 | |||
100 | /// @brief compute the eigen vector and eigen vector of a matrix. dout is the | ||
101 | /// eigen values, vout is the eigen vectors | ||
102 | template <typename Derived, typename Vector> | ||
103 | 1789350 | void eigen(const Eigen::MatrixBase<Derived>& m, | |
104 | typename Derived::Scalar dout[3], Vector* vout) { | ||
105 | typedef typename Derived::Scalar S; | ||
106 |
1/2✓ Branch 2 taken 1789350 times.
✗ Branch 3 not taken.
|
1789350 | Derived R(m.derived()); |
107 | 1789350 | int n = 3; | |
108 | int j, iq, ip, i; | ||
109 | S tresh, theta, tau, t, sm, s, h, g, c; | ||
110 | |||
111 | S b[3]; | ||
112 | S z[3]; | ||
113 | 1789350 | S v[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; | |
114 | S d[3]; | ||
115 | |||
116 |
2/2✓ Branch 0 taken 5368050 times.
✓ Branch 1 taken 1789350 times.
|
7157400 | for (ip = 0; ip < n; ++ip) { |
117 |
1/2✓ Branch 1 taken 5368050 times.
✗ Branch 2 not taken.
|
5368050 | b[ip] = d[ip] = R(ip, ip); |
118 | 5368050 | z[ip] = 0; | |
119 | } | ||
120 | |||
121 |
1/2✓ Branch 0 taken 10490342 times.
✗ Branch 1 not taken.
|
10490342 | for (i = 0; i < 50; ++i) { |
122 | 10490342 | sm = 0; | |
123 |
2/2✓ Branch 0 taken 31471026 times.
✓ Branch 1 taken 10490342 times.
|
41961368 | for (ip = 0; ip < n; ++ip) |
124 |
3/4✓ Branch 1 taken 31471026 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 31471026 times.
✓ Branch 5 taken 31471026 times.
|
62942052 | for (iq = ip + 1; iq < n; ++iq) sm += std::abs(R(ip, iq)); |
125 |
2/2✓ Branch 0 taken 1789350 times.
✓ Branch 1 taken 8700992 times.
|
10490342 | if (sm == 0.0) { |
126 |
3/6✓ Branch 1 taken 1789350 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1789350 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1789350 times.
✗ Branch 8 not taken.
|
1789350 | vout[0] << v[0][0], v[0][1], v[0][2]; |
127 |
3/6✓ Branch 1 taken 1789350 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1789350 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1789350 times.
✗ Branch 8 not taken.
|
1789350 | vout[1] << v[1][0], v[1][1], v[1][2]; |
128 |
3/6✓ Branch 1 taken 1789350 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1789350 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1789350 times.
✗ Branch 8 not taken.
|
1789350 | vout[2] << v[2][0], v[2][1], v[2][2]; |
129 | 1789350 | dout[0] = d[0]; | |
130 | 1789350 | dout[1] = d[1]; | |
131 | 1789350 | dout[2] = d[2]; | |
132 | 1789350 | return; | |
133 | } | ||
134 | |||
135 |
2/2✓ Branch 0 taken 5199615 times.
✓ Branch 1 taken 3501377 times.
|
8700992 | if (i < 3) |
136 | 5199615 | tresh = Scalar(0.2 * sm / (n * n)); | |
137 | else | ||
138 | 3501377 | tresh = 0.0; | |
139 | |||
140 |
2/2✓ Branch 0 taken 26102976 times.
✓ Branch 1 taken 8700992 times.
|
34803968 | for (ip = 0; ip < n; ++ip) { |
141 |
2/2✓ Branch 0 taken 26102976 times.
✓ Branch 1 taken 26102976 times.
|
52205952 | for (iq = ip + 1; iq < n; ++iq) { |
142 |
1/2✓ Branch 1 taken 26102976 times.
✗ Branch 2 not taken.
|
26102976 | g = Scalar(100) * std::abs(R(ip, iq)); |
143 |
8/8✓ Branch 0 taken 5381508 times.
✓ Branch 1 taken 20721468 times.
✓ Branch 4 taken 5158926 times.
✓ Branch 5 taken 222582 times.
✓ Branch 6 taken 5040765 times.
✓ Branch 7 taken 118161 times.
✓ Branch 8 taken 5040765 times.
✓ Branch 9 taken 21062211 times.
|
31261902 | if (i > 3 && std::abs(d[ip]) + g == std::abs(d[ip]) && |
144 | 5158926 | std::abs(d[iq]) + g == std::abs(d[iq])) | |
145 |
1/2✓ Branch 1 taken 5040765 times.
✗ Branch 2 not taken.
|
5040765 | R(ip, iq) = 0.0; |
146 |
3/4✓ Branch 1 taken 21062211 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14746163 times.
✓ Branch 5 taken 6316048 times.
|
21062211 | else if (std::abs(R(ip, iq)) > tresh) { |
147 | 14746163 | h = d[iq] - d[ip]; | |
148 |
2/2✓ Branch 2 taken 3572590 times.
✓ Branch 3 taken 11173573 times.
|
14746163 | if (std::abs(h) + g == std::abs(h)) |
149 |
1/2✓ Branch 1 taken 3572590 times.
✗ Branch 2 not taken.
|
3572590 | t = (R(ip, iq)) / h; |
150 | else { | ||
151 |
1/2✓ Branch 1 taken 11173573 times.
✗ Branch 2 not taken.
|
11173573 | theta = Scalar(0.5) * h / (R(ip, iq)); |
152 | 11173573 | t = 1 / (std::abs(theta) + std::sqrt(1 + theta * theta)); | |
153 |
2/2✓ Branch 0 taken 5316587 times.
✓ Branch 1 taken 5856986 times.
|
11173573 | if (theta < 0.0) t = -t; |
154 | } | ||
155 | 14746163 | c = 1 / std::sqrt(1 + t * t); | |
156 | 14746163 | s = t * c; | |
157 | 14746163 | tau = s / (1 + c); | |
158 |
1/2✓ Branch 1 taken 14746163 times.
✗ Branch 2 not taken.
|
14746163 | h = t * R(ip, iq); |
159 | 14746163 | z[ip] -= h; | |
160 | 14746163 | z[iq] += h; | |
161 | 14746163 | d[ip] -= h; | |
162 | 14746163 | d[iq] += h; | |
163 |
1/2✓ Branch 1 taken 14746163 times.
✗ Branch 2 not taken.
|
14746163 | R(ip, iq) = 0.0; |
164 |
2/2✓ Branch 0 taken 4686448 times.
✓ Branch 1 taken 14746163 times.
|
19432611 | for (j = 0; j < ip; ++j) { |
165 |
1/2✓ Branch 1 taken 4686448 times.
✗ Branch 2 not taken.
|
4686448 | g = R(j, ip); |
166 |
1/2✓ Branch 1 taken 4686448 times.
✗ Branch 2 not taken.
|
4686448 | h = R(j, iq); |
167 |
1/2✓ Branch 1 taken 4686448 times.
✗ Branch 2 not taken.
|
4686448 | R(j, ip) = g - s * (h + g * tau); |
168 |
1/2✓ Branch 1 taken 4686448 times.
✗ Branch 2 not taken.
|
4686448 | R(j, iq) = h + s * (g - h * tau); |
169 | } | ||
170 |
2/2✓ Branch 0 taken 4780366 times.
✓ Branch 1 taken 14746163 times.
|
19526529 | for (j = ip + 1; j < iq; ++j) { |
171 |
1/2✓ Branch 1 taken 4780366 times.
✗ Branch 2 not taken.
|
4780366 | g = R(ip, j); |
172 |
1/2✓ Branch 1 taken 4780366 times.
✗ Branch 2 not taken.
|
4780366 | h = R(j, iq); |
173 |
1/2✓ Branch 1 taken 4780366 times.
✗ Branch 2 not taken.
|
4780366 | R(ip, j) = g - s * (h + g * tau); |
174 |
1/2✓ Branch 1 taken 4780366 times.
✗ Branch 2 not taken.
|
4780366 | R(j, iq) = h + s * (g - h * tau); |
175 | } | ||
176 |
2/2✓ Branch 0 taken 5279349 times.
✓ Branch 1 taken 14746163 times.
|
20025512 | for (j = iq + 1; j < n; ++j) { |
177 |
1/2✓ Branch 1 taken 5279349 times.
✗ Branch 2 not taken.
|
5279349 | g = R(ip, j); |
178 |
1/2✓ Branch 1 taken 5279349 times.
✗ Branch 2 not taken.
|
5279349 | h = R(iq, j); |
179 |
1/2✓ Branch 1 taken 5279349 times.
✗ Branch 2 not taken.
|
5279349 | R(ip, j) = g - s * (h + g * tau); |
180 |
1/2✓ Branch 1 taken 5279349 times.
✗ Branch 2 not taken.
|
5279349 | R(iq, j) = h + s * (g - h * tau); |
181 | } | ||
182 |
2/2✓ Branch 0 taken 44238489 times.
✓ Branch 1 taken 14746163 times.
|
58984652 | for (j = 0; j < n; ++j) { |
183 | 44238489 | g = v[j][ip]; | |
184 | 44238489 | h = v[j][iq]; | |
185 | 44238489 | v[j][ip] = g - s * (h + g * tau); | |
186 | 44238489 | v[j][iq] = h + s * (g - h * tau); | |
187 | } | ||
188 | } | ||
189 | } | ||
190 | } | ||
191 |
2/2✓ Branch 0 taken 26102976 times.
✓ Branch 1 taken 8700992 times.
|
34803968 | for (ip = 0; ip < n; ++ip) { |
192 | 26102976 | b[ip] += z[ip]; | |
193 | 26102976 | d[ip] = b[ip]; | |
194 | 26102976 | z[ip] = 0.0; | |
195 | } | ||
196 | } | ||
197 | |||
198 | ✗ | std::cerr << "eigen: too many iterations in Jacobi transform." << std::endl; | |
199 | |||
200 | ✗ | return; | |
201 | } | ||
202 | |||
203 | template <typename Derived, typename OtherDerived> | ||
204 | 437 | bool isEqual(const Eigen::MatrixBase<Derived>& lhs, | |
205 | const Eigen::MatrixBase<OtherDerived>& rhs, | ||
206 | const Scalar tol = std::numeric_limits<Scalar>::epsilon() * 100) { | ||
207 | 437 | return ((lhs - rhs).array().abs() < tol).all(); | |
208 | } | ||
209 | |||
210 | } // namespace coal | ||
211 | |||
212 | #endif | ||
213 |