Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpMomentCInvariant.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
5 *
6 * This software is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
10 * See the file LICENSE.txt at the root directory of this source
11 * distribution for additional information about the GNU GPL.
12 *
13 * For using ViSP with software that can not be combined with the GNU
14 * GPL, please contact Inria about acquiring a ViSP Professional
15 * Edition License.
16 *
17 * See https://visp.inria.fr for more information.
18 *
19 * This software was developed at:
20 * Inria Rennes - Bretagne Atlantique
21 * Campus Universitaire de Beaulieu
22 * 35042 Rennes Cedex
23 * France
24 *
25 * If you have questions regarding the use of this file, please contact
26 * Inria at visp@inria.fr
27 *
28 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30 *
31 * Description:
32 * Descriptor for various invariants used to drive space rotations around X and
33 *Y axis.
34 *
35 * Authors:
36 * Filip Novotny
37 *
38*****************************************************************************/
39
40#include <visp3/core/vpMomentCInvariant.h>
41#include <visp3/core/vpMomentCentered.h>
42#include <visp3/core/vpMomentObject.h>
43
48vpMomentCInvariant::vpMomentCInvariant(bool flg_sxsynormalization)
49 : I(16), II(4), c(4), s(4), K(0.0), cn(4), sn(4), In1(0.0), flg_sxsynormalization_(flg_sxsynormalization)
50{
51 values.resize(14);
52}
53
59void vpMomentCInvariant::computeI(const vpMomentCentered &momentCentered, std::vector<double> &I_val)
60{
61
62 double mu30 = momentCentered.get(3, 0);
63 double mu30_2 = mu30 * mu30;
64 double mu30_3 = mu30_2 * mu30;
65
66 double mu03 = momentCentered.get(0, 3);
67 double mu03_2 = mu03 * mu03;
68 double mu03_3 = mu03 * mu03_2;
69
70 double mu20 = momentCentered.get(2, 0);
71 double mu02 = momentCentered.get(0, 2);
72 double mu50 = momentCentered.get(5, 0);
73 double mu32 = momentCentered.get(3, 2);
74 double mu14 = momentCentered.get(1, 4);
75 double mu05 = momentCentered.get(0, 5);
76 double mu23 = momentCentered.get(2, 3);
77 double mu41 = momentCentered.get(4, 1);
78 double mu40 = momentCentered.get(4, 0);
79 double mu04 = momentCentered.get(0, 4);
80 double mu31 = momentCentered.get(3, 1);
81 double mu13 = momentCentered.get(1, 3);
82 double mu22 = momentCentered.get(2, 2);
83 double mu21 = momentCentered.get(2, 1);
84 double mu12 = momentCentered.get(1, 2);
85 double mu11 = momentCentered.get(1, 1);
86
87 double mu11_2 = mu11 * mu11;
88 double mu12_2 = mu12 * mu12;
89 double mu21_2 = mu21 * mu21;
90 double mu22_2 = mu22 * mu22;
91 double mu13_2 = mu13 * mu13;
92 double mu31_2 = mu31 * mu31;
93 double mu04_2 = mu04 * mu04;
94 double mu40_2 = mu40 * mu40;
95 double mu21_3 = mu21 * mu21_2;
96 double mu12_3 = mu12_2 * mu12;
97 double mu12_4 = mu12_3 * mu12;
98 double mu21_4 = mu21_2 * mu21_2;
99
100 // double kappa = mu30_2+mu03_2-3*mu21_2+6*mu21*mu03; //Used in I8
101 // calculation but simplified with MAPLE and found it to be wrong
102 double zeta = mu20 - mu02;
103 double zeta_2 = zeta * zeta;
104 double omicron = (mu03_2 + 3 * mu03 * mu21 + mu30 * (mu30 + 3 * mu12));
105 double omega = mu50 + 2 * mu32 + mu14;
106 double nu = mu05 + 2 * mu23 + mu41;
107 double ro = mu50 - 2 * mu32 - 3 * mu14;
108 double gamma = mu05 - 2 * mu23 - 3 * mu41;
109
110 double delta = mu50 - 10 * mu32 + 5 * mu14;
111 double phi = mu05 - 10 * mu23 + 5 * mu41;
112 double omega_2 = omega * omega;
113 double nu_2 = nu * nu;
114 double ro_2 = ro * ro;
115 double gamma_2 = gamma * gamma;
116 double delta_2 = delta * delta;
117 double phi_2 = phi * phi;
118
119 I_val[1] = -mu20 * mu02 + mu11_2;
120 I_val[2] = zeta_2 + 4 * mu11_2;
121 I_val[3] = (mu30 - 3 * mu12) * (mu30 - 3 * mu12) + (mu03 - 3 * mu21) * (mu03 - 3 * mu21);
122 I_val[4] = (mu30 + mu12) * (mu30 + mu12) + (mu21 + mu03) * (mu21 + mu03);
123 I_val[5] = -mu30_2 * mu03_2 + (-4 * mu12_3 + 6 * mu21 * mu12 * mu03) * mu30 - 4 * mu21_3 * mu03 + 3 * mu21_2 * mu12_2;
124 I_val[6] = 3 * mu12_4 + 2 * mu30 * mu12_3 + (3 * mu30_2 - 6 * mu03 * mu21) * mu12_2 -
125 6 * mu30 * mu21 * (mu21 + mu03) * mu12 + 2 * mu30_2 * mu03_2 + 2 * mu21_3 * mu03 + 3 * mu21_2 * mu03_2 +
126 3 * mu21_4;
127 I_val[7] = (3 * mu21 + 2 * mu03) * mu12_3 + 3 * mu30 * (mu03 + 2 * mu21) * mu12_2 -
128 3 * mu21 * (mu30 + mu03 + mu21) * (-mu30 + mu03 + mu21) * mu12 +
129 mu30 * (-mu30_2 * mu03 - 2 * mu21_3 - 3 * mu03 * mu21_2 + mu03_3);
130 // I_val[8]=3*mu21_4-3*mu21_3*mu03+(3*mu03_2+kappa-6*mu12_2)*mu21_2-mu03*(-15*mu12_2+kappa)*mu21-(-3*mu12_2*mu30+(2*kappa-3*mu03_2)*mu12+kappa*mu30)*mu12;
131 I_val[8] = 3 * mu03 * mu21_3 - 2 * mu03_2 * mu21_2 + mu21_2 * mu30_2 + 3 * mu12_2 * mu03 * mu21 -
132 mu03 * mu21 * mu30_2 - mu03_3 * mu21 + 3 * mu12_3 * mu30 - 2 * mu12_2 * mu30_2 + mu12_2 * mu03_2 -
133 mu12 * mu30_3 - mu12 * mu30 * mu03_2 + 3 * mu12 * mu30 * mu21_2 - 6 * mu12 * mu30 * mu03 * mu21;
134 I_val[9] = omicron * omicron;
135
136 I_val[10] = mu40 * mu04 - 4 * mu31 * mu13 + 3 * mu22_2;
137 I_val[11] = 3 * mu13_2 + 2 * mu31 * mu13 + (-3 * mu40 - 3 * mu04) * mu22 - 2 * mu40 * mu04 + 3 * mu31_2;
138 I_val[12] = 3 * mu04_2 + (2 * mu40 + 12 * mu22) * mu04 + 3 * mu40_2 + 12 * mu40 * mu22 + 16 * mu31 * mu13;
139 I_val[13] = omega_2 + nu_2;
140 I_val[14] = ro_2 + gamma_2;
141 I_val[15] = delta_2 + phi_2;
142
143 double a;
145 a = momentCentered.get(2, 0) + momentCentered.get(0, 2);
146 else
147 a = getObject().get(0, 0);
148
149 c[1] = momentCentered.get(2, 0) - momentCentered.get(0, 2);
150 s[1] = 2 * momentCentered.get(1, 1);
151 c[2] = momentCentered.get(0, 3) - 3 * momentCentered.get(2, 1);
152 s[2] = momentCentered.get(3, 0) - 3 * momentCentered.get(1, 2);
153 c[3] = c[1] * c[1] - s[1] * s[1];
154 s[3] = 2 * s[1] * c[1];
155
156 II[1] = c[1] * c[1] + s[1] * s[1];
157 II[2] = c[2] * c[2] + s[2] * s[2];
158 II[3] = momentCentered.get(2, 0) + momentCentered.get(0, 2);
159
160 K = (II[1] * (II[3] * sqrt(std::fabs(II[3])))) / sqrt(std::fabs(a));
161
162 /*
163 * Intermediate quantities required for calculation of normalized version of
164 * Sx and Sy The pij doubles below are the respective centered moment values
165 * mu_ij scaled by mu20 + mu02
166 */
167 double p20 = momentCentered.get(2, 0) / II[3]; // II[3] is the normalization factor for the 2nd order moments
168 double p11 = momentCentered.get(1, 1) / II[3];
169 double p02 = momentCentered.get(0, 2) / II[3];
170
171 double d =
172 sqrt(std::fabs(a)) / (II[3] * sqrt(std::fabs(II[3]))); // d is the normalization factor for 3rd order moments
173 double p30 = momentCentered.get(3, 0) * d;
174 double p21 = momentCentered.get(2, 1) * d;
175 double p12 = momentCentered.get(1, 2) * d;
176 double p03 = momentCentered.get(0, 3) * d;
177
178 cn[1] = p20 - p02;
179 sn[1] = 2.0 * p11;
180 sn[2] = p30 - 3.0 * p12;
181 cn[2] = p03 - 3.0 * p21;
182
183 cn[3] = cn[1] * cn[1] - sn[1] * sn[1];
184 sn[3] = 2.0 * sn[1] * cn[1];
185
186 In1 = cn[1] * cn[1] + sn[1] * sn[1];
187}
188
196{
197 if (getObject().getOrder() < 5)
198 throw vpException(vpException::notInitialized, "Order is not high enough for vpMomentCInvariant. "
199 "Specify at least order 5.");
200 bool found_moment_centered;
201 const vpMomentCentered &momentCentered =
202 (static_cast<const vpMomentCentered &>(getMoments().get("vpMomentCentered", found_moment_centered)));
203
204 if (!found_moment_centered)
205 throw vpException(vpException::notInitialized, "vpMomentCentered not found");
206
207 computeI(momentCentered, I);
208 double II3_2 = II[3] * II[3];
209 double II3_3 = II3_2 * II[3];
210
211 double a;
212 if (getObject().getType() == vpMomentObject::DISCRETE)
213 a = momentCentered.get(2, 0) + momentCentered.get(0, 2);
214 else
215 a = getObject().get(0, 0);
216
217 values[0] = I[1] / I[2];
218 values[1] = I[3] / I[4];
219
220 values[2] = I[5] / I[6];
221
222 values[3] = I[7] / I[6];
223
224 values[4] = I[8] / I[6];
225
226 values[5] = I[9] / I[6];
227
228 values[6] = I[11] / I[10];
229
230 values[7] = I[12] / I[10];
231
232 values[8] = I[13] / I[15];
233
234 values[9] = I[14] / I[15];
235
236 if (flg_sxsynormalization_)
237 calcSxSyNormalized(values[10], values[11]);
238 else
239 calcSxSy(values[10], values[11]);
240
241 values[12] = II[1] / (II3_2); // Px
242 values[13] = a * II[2] / (II3_3); // Py
243}
244
248void vpMomentCInvariant::calcSxSy(double &sx, double &sy) const
249{
250 sx = (c[2] * c[3] + s[2] * s[3]) / K;
251 sy = (s[2] * c[3] - c[2] * s[3]) / K;
252}
253
259void vpMomentCInvariant::calcSxSyNormalized(double &sx, double &sy) const
260{
261 sx = (cn[2] * cn[3] + sn[2] * sn[3]) / In1;
262 sy = (sn[2] * cn[3] - cn[2] * sn[3]) / In1;
263}
264
269void vpMomentCInvariant::printI(unsigned int index) { std::cout << "I(" << index << ")=" << I[index] << std::endl; }
270
276void vpMomentCInvariant::printInvariants(std::ostream &os) const
277{
278 for (unsigned int i = 1; i < I.size(); ++i) { // i = 1 since vector I has been indexed from 1 in
279 // vpMomentCinvariant
280 os << "I[" << i << "]=" << I[i] << std::endl;
281 }
282 os << std::endl;
283}
284
288VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpMomentCInvariant &c)
289{
290 for (unsigned int i = 0; i < c.values.size(); i++) {
291 os << c.values[i] << "," << std::endl;
292 }
293 return os;
294}
error that can be emitted by ViSP classes.
Definition vpException.h:59
@ notInitialized
Used to indicate that a parameter is not initialized.
Definition vpException.h:86
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
Definition vpImage.h:795
vpMomentCInvariant(bool flg_sxsynormalization=false)
void printI(unsigned int index)
void printInvariants(std::ostream &os) const
This class defines the double-indexed centered moment descriptor .
double get(unsigned int i, unsigned int j) const
const vpMoment & get(const char *type, bool &found) const
const std::vector< double > & get() const
vpObjectType getType() const
const vpMomentObject & getObject() const
Definition vpMoment.h:145
std::vector< double > values
Definition vpMoment.h:113
vpMomentDatabase & getMoments() const
Definition vpMoment.h:118