Visual Servoing Platform  version 3.6.1 under development (2024-03-28)
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 
48 vpMomentCInvariant::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 
59 void 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 
248 void 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 
259 void 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 
269 void vpMomentCInvariant::printI(unsigned int index) { std::cout << "I(" << index << ")=" << I[index] << std::endl; }
270 
276 void 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 
288 VISP_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 }
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< Type > &A)
Definition: vpArray2D.h:547
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
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 std::string &moment_name, bool &found) const
const std::vector< double > & get() const
vpObjectType getType() const
std::vector< double > values
Definition: vpMoment.h:112
const vpMomentObject & getObject() const
Definition: vpMoment.h:144
vpMomentDatabase & getMoments() const
Definition: vpMoment.h:117