ViSP  2.8.0
vpFeatureMomentCentered.cpp
1 /****************************************************************************
2  *
3  * $Id: vpFeatureMomentImpl.cpp 3317 2011-09-06 14:14:47Z fnovotny $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2013 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  *
34  * Description:
35  * Implementation for all supported moment features.
36  *
37  * Authors:
38  * Filip Novotny
39  *
40  *****************************************************************************/
41 
42 #include <visp/vpConfig.h>
43 
44 #ifdef VISP_MOMENTS_COMBINE_MATRICES
45 #include <vector>
46 #include <limits>
47 
48 #include <visp/vpMomentObject.h>
49 #include <visp/vpFeatureMomentCentered.h>
50 #include <visp/vpFeatureMomentBasic.h>
51 #include <visp/vpFeatureMomentGravityCenter.h>
52 #include <visp/vpMomentGravityCenter.h>
53 #include <visp/vpFeatureMomentDatabase.h>
54 
64  double A, double B, double C,
65  vpFeatureMomentDatabase* featureMoments) :
66  vpFeatureMoment(moments,A,B,C,featureMoments)
67 
68 {
69 }
70 
77 vpMatrix vpFeatureMomentCentered::interaction (unsigned int select_one,unsigned int select_two){
78  if(select_one+select_two>moment->getObject().getOrder())
80  "The requested value has not been computed, you should specify a higher order.");
81  return interaction_matrices[select_two*order+select_one];
82 }
83 
84 
94  bool found_featuremoment_basic;
95  bool found_feature_gravity_center;
96  bool found_moment_gravity;
97 
98  vpMomentObject& momentObject = moment->getObject();
99  order = momentObject.getOrder()+1;
100  interaction_matrices.resize(order*order);
101  for(std::vector< vpMatrix >::iterator i=interaction_matrices.begin();i!=interaction_matrices.end(); ++i)
102  i->resize(1,6);
103 
104  vpFeatureMomentBasic& featureMomentBasic= (static_cast<vpFeatureMomentBasic&>(featureMoments->get("vpFeatureMomentBasic",found_featuremoment_basic)));
105  vpFeatureMomentGravityCenter& featureMomentGravityCenter= (static_cast<vpFeatureMomentGravityCenter&>(featureMoments->get("vpFeatureMomentGravityCenter",found_feature_gravity_center)));
106  vpMomentGravityCenter& momentGravity = static_cast<vpMomentGravityCenter&>(moments.get("vpMomentGravityCenter",found_moment_gravity));
107  vpMatrix zeros(1,6);
108  for(int i=0;i<6;i++) zeros[0][i]=0;
109 
110  if(!found_featuremoment_basic) throw vpException(vpException::notInitialized,"vpFeatureMomentBasic not found");
111  if(!found_feature_gravity_center) throw vpException(vpException::notInitialized,"vpFeatureMomentGravityCenter not found");
112  if(!found_moment_gravity) throw vpException(vpException::notInitialized,"vpMomentGravityCenter not found");
113 
114  vpMatrix LXg = featureMomentGravityCenter.interaction(1<<0);
115  vpMatrix LYg = featureMomentGravityCenter.interaction(1<<1);
116 
117  //compute centered moment features as a combination of basic features
118  for(int i=0;i<(int)order;i++){
119  for(int j=0;j<(int)order-i;j++){
120  interaction_matrices[(unsigned int)j*order+(unsigned int)i] = zeros;
121  vpMatrix mat1 = zeros;
122  vpMatrix mat2 = zeros;
123  vpMatrix mat3 = zeros;
124  for(int k=0;k<=i;k++){
125  for(int l=0;l<=j;l++){
126  mat1+= std::abs(momentGravity.get()[0])<std::numeric_limits<double>::epsilon()?zeros:vpMath::comb((unsigned int)i, (unsigned int)k) * vpMath::comb((unsigned int)j, (unsigned int)l) * pow(-momentGravity.get()[0], i - k) * (i - k) * LXg / momentGravity.get()[0] * pow(-momentGravity.get()[1], j - l) * momentObject.get((unsigned int)k, (unsigned int)l);
127  mat2+= std::abs(momentGravity.get()[1])<std::numeric_limits<double>::epsilon()?zeros:vpMath::comb((unsigned int)i, (unsigned int)k) * vpMath::comb((unsigned int)j, (unsigned int)l) * pow(-momentGravity.get()[0], i - k) * pow(-momentGravity.get()[1], j - l) * (j - l) * LYg / momentGravity.get()[1] * momentObject.get((unsigned int)k, (unsigned int)l);
128  mat3+= vpMath::comb((unsigned int)i, (unsigned int)k) * vpMath::comb((unsigned int)j, (unsigned int)l) * pow(-momentGravity.get()[0], i - k) * pow(-momentGravity.get()[1], j - l) * featureMomentBasic.interaction((unsigned int)k, (unsigned int)l);
129  }
130  }
131  interaction_matrices[(unsigned int)j*order+(unsigned int)i]=(mat1+mat2+mat3);
132  }
133  }
134 }
135 
136 #else
137 
138 #include <vector>
139 #include <limits>
140 
141 #include <visp/vpMomentObject.h>
142 #include <visp/vpMomentGravityCenter.h>
143 #include <visp/vpMomentCentered.h>
144 #include <visp/vpFeatureMomentCentered.h>
145 #include <visp/vpFeatureMomentDatabase.h>
146 
147 
156 vpFeatureMomentCentered::vpFeatureMomentCentered(vpMomentDatabase& moments,double A, double B, double C,vpFeatureMomentDatabase* featureMoments) :
157  vpFeatureMoment(moments,A,B,C,featureMoments)
158 
159 {
160 }
161 
168 vpMatrix vpFeatureMomentCentered::interaction (unsigned int select_one,unsigned int select_two){
169  if(select_one+select_two>moment->getObject().getOrder())
170  throw vpException(vpException::badValue,"The requested value has not been computed, you should specify a higher order.");
171  return interaction_matrices[select_two*order+select_one];
172 }
173 
174 
183 
184  bool found_moment_centered;
185  bool found_moment_gravity;
186 
187  vpMomentCentered& momentCentered= (static_cast<vpMomentCentered&>(moments.get("vpMomentCentered",found_moment_centered)));
188  vpMomentGravityCenter& momentGravity = static_cast<vpMomentGravityCenter&>(moments.get("vpMomentGravityCenter",found_moment_gravity));
189 
190  if(!found_moment_centered) throw vpException(vpException::notInitialized,"vpMomentCentered not found");
191  if(!found_moment_gravity) throw vpException(vpException::notInitialized,"vpMomentGravityCenter not found");
192 
193  int delta;
194  int epsilon;
195  vpMomentObject& momentObject = moment->getObject();
196  order = momentObject.getOrder()+1;
197  interaction_matrices.resize(order*order);
198  for (std::vector< vpMatrix >::iterator i=interaction_matrices.begin(); i!=interaction_matrices.end(); ++i)
199  i->resize(1,6);
200  if (momentObject.getType()==vpMomentObject::DISCRETE) {
201  delta=0;
202  epsilon=1;
203  } else {
204  delta=1;
205  epsilon=4;
206  }
207  double n11 = momentCentered.get(1,1)/momentObject.get(0,0);
208  double n20 = momentCentered.get(2,0)/momentObject.get(0,0);
209  double n02 = momentCentered.get(0,2)/momentObject.get(0,0);
210  double Xg = momentGravity.getXg();
211  double Yg = momentGravity.getYg();
212  double mu00 = momentCentered.get(0,0);
213 
214  unsigned int VX = 0;
215  unsigned int VY = 1;
216  unsigned int VZ = 2;
217  unsigned int WX = 3;
218  unsigned int WY = 4;
219  unsigned int WZ = 5;
220 
221  interaction_matrices[0][0][VX] = -(delta)*A*mu00;
222  interaction_matrices[0][0][VY] = -(delta)*B*mu00;
223 
224  // Since mu10=0 and mu01=0
225  // interaction_matrices[0][0][WX] = (3*delta)*MU(0,1)+(3*delta)*Yg*mu00;
226  // interaction_matrices[0][0][WY] = -(3*delta)*MU(1,0)-(3*delta)*Xg*mu00;
227  // we get the simplification:
228  interaction_matrices[0][0][WX] = (3*delta)*Yg*mu00;
229  interaction_matrices[0][0][WY] = -(3*delta)*Xg*mu00;
230  interaction_matrices[0][0][VZ] = -A*interaction_matrices[0][0][WY]+B*interaction_matrices[0][0][WX]+(2*delta)*C*mu00;
231  interaction_matrices[0][0][WZ] = 0.;
232 
233  for (int i=1; i<(int)order-1; i++){
234  unsigned int i_ = (unsigned int) i;
235  unsigned int im1_ = i_ - 1;
236  unsigned int ip1_ = i_ + 1;
237 
238  double mu_im10 = momentCentered.get(im1_,0);
239  double mu_ip10 = momentCentered.get(ip1_,0);
240  double mu_im11 = momentCentered.get(im1_,1);
241  double mu_i0 = momentCentered.get(i_,0);
242  double mu_i1 = momentCentered.get(i_,1);
243 
244  interaction_matrices[i_][0][VX] = -(i+delta)*A*mu_i0-(i*B*mu_im11);
245  interaction_matrices[i_][0][VY] = -(delta)*B*mu_i0;
246 
247  interaction_matrices[i_][0][WX] = (i+3*delta)*mu_i1+(i+3*delta)*Yg*mu_i0+i*Xg*mu_im11-i*epsilon*n11*mu_im10;
248  interaction_matrices[i_][0][WY] = -(i+3*delta)*mu_ip10-(2*i+3*delta)*Xg*mu_i0+i*epsilon*n20*mu_im10;
249  interaction_matrices[i_][0][VZ] = -A*interaction_matrices[i_][0][WY]+B*interaction_matrices[i_][0][WX]+(i+2*delta)*C*mu_i0;
250  interaction_matrices[i_][0][WZ] = i*mu_im11;
251  }
252 
253  for(int j=1;j<(int)order-1;j++){
254  unsigned int j_ = (unsigned int) j;
255  unsigned int jm1_ = j_ - 1;
256  unsigned int jp1_ = j_ + 1;
257 
258  double mu_0jm1 = momentCentered.get(0,jm1_);
259  double mu_0jp1 = momentCentered.get(0,jp1_);
260  double mu_1jm1 = momentCentered.get(1,jm1_);
261  double mu_0j = momentCentered.get(0,j_);
262  double mu_1j = momentCentered.get(1,j_);
263 
264  interaction_matrices[j_*order][0][VX] = -(delta)*A*mu_0j;
265  interaction_matrices[j_*order][0][VY] = -j*A*mu_1jm1-(j+delta)*B*mu_0j;
266 
267  interaction_matrices[j_*order][0][WX] = (j+3*delta)*mu_0jp1+(2*j+3*delta)*Yg*mu_0j-j*epsilon*n02*mu_0jm1;
268  interaction_matrices[j_*order][0][WY] = -(j+3*delta)*mu_1j-(j+3*delta)*Xg*mu_0j-j*Yg*mu_1jm1+j*epsilon*n11*mu_0jm1;
269  interaction_matrices[j_*order][0][VZ] = -A*interaction_matrices[j_*order][0][WY]+B*interaction_matrices[j_*order][0][WX]+(j+2*delta)*C*mu_0j;
270  interaction_matrices[j_*order][0][WZ] = -j*mu_1jm1;
271  }
272 
273  for(int j=1; j<(int)order-1; j++) {
274  unsigned int j_ = (unsigned int) j;
275  unsigned int jm1_ = j_ - 1;
276  unsigned int jp1_ = j_ + 1;
277  for(int i=1; i<(int)order-j-1; i++) {
278  unsigned int i_ = (unsigned int) i;
279  unsigned int im1_ = i_ - 1;
280  unsigned int ip1_ = i_ + 1;
281 
282  double mu_ijm1 = momentCentered.get(i_,jm1_);
283  double mu_ij = momentCentered.get(i_,j_);
284  double mu_ijp1 = momentCentered.get(i_,jp1_);
285  double mu_im1j = momentCentered.get(im1_,j_);
286  double mu_im1jp1 = momentCentered.get(im1_,jp1_);
287  double mu_ip1jm1 = momentCentered.get(ip1_,jm1_);
288  double mu_ip1j = momentCentered.get(ip1_,j_);
289 
290  interaction_matrices[j_*order+i_][0][VX] = -(i+delta)*A*mu_ij-i*B*mu_im1jp1;
291  interaction_matrices[j_*order+i_][0][VY] = -j*A*mu_ip1jm1-(j+delta)*B*mu_ij;
292 
293  interaction_matrices[j_*order+i_][0][WX] = (i+j+3*delta)*mu_ijp1+(i+2*j+3*delta)*Yg*mu_ij
294  +i*Xg*mu_im1jp1-i*epsilon*n11*mu_im1j-j*epsilon*n02*mu_ijm1;
295  interaction_matrices[j_*order+i_][0][WY] = -(i+j+3*delta)*mu_ip1j-(2*i+j+3*delta)*Xg*mu_ij
296  -j*Yg*mu_ip1jm1+i*epsilon*n20*mu_im1j+j*epsilon*n11*mu_ijm1;
297  interaction_matrices[j_*order+i_][0][VZ] = -A*interaction_matrices[j_*order+i_][0][WY]+B*interaction_matrices[j_*order+i_][0][WX]+(i+j+2*delta)*C*mu_ij;
298  interaction_matrices[j_*order+i_][0][WZ] = i*mu_im1jp1-j*mu_ip1jm1;
299  }
300  }
301  }
302 #endif
Functionality computation for basic moment feature. Computes the interaction matrix associated with v...
vpFeatureMomentCentered(vpMomentDatabase &moments, double A, double B, double C, vpFeatureMomentDatabase *featureMoments=NULL)
Definition of the vpMatrix class.
Definition: vpMatrix.h:96
vpMomentObject & getObject() const
Definition: vpMoment.h:119
double get(unsigned int i, unsigned int j)
error that can be emited by ViSP classes.
Definition: vpException.h:75
Class for generic objects.
std::vector< vpMatrix > interaction_matrices
This class allows to register all vpMoments so they can access each other according to their dependen...
vpMatrix interaction(const unsigned int select=FEATURE_ALL)
vpMoment & get(const char *type, bool &found)
This class defines shared system methods/attributes for 2D moment features but no functional code...
std::vector< double > & get()
vpMomentDatabase & moments
This class defines the double-indexed centered moment descriptor .
Class describing 2D gravity center moment.
vpObjectType getType() const
vpMatrix interaction(unsigned int select_one, unsigned int select_two)
static long double comb(unsigned int n, unsigned int p)
Definition: vpMath.h:213
Functionality computation for gravity center moment feature. Computes the interaction matrix associat...
std::vector< double > & get()
vpMatrix interaction(unsigned int select_one, unsigned int select_two)
vpFeatureMoment & get(const char *type, bool &found)
This class allows to register all feature moments (implemented in vpFeatureMoment... classes) so they can access each other according to their dependencies.
unsigned int getOrder() const