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