ViSP  2.8.0
vpFeatureMomentGravityCenterNormalized.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 - Brngne 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 
46 #include <vector>
47 #include <limits>
48 
49 #include <visp/vpMomentObject.h>
50 #include <visp/vpMomentCentered.h>
51 #include <visp/vpMomentAreaNormalized.h>
52 #include <visp/vpMomentGravityCenter.h>
53 #include <visp/vpFeatureMomentGravityCenter.h>
54 #include <visp/vpFeatureMomentAreaNormalized.h>
55 #include <visp/vpFeatureMomentGravityCenterNormalized.h>
56 #include <visp/vpFeatureMomentDatabase.h>
57 
68  bool found_moment_gravity;
69  bool found_moment_surface_normalized;
70 
71  bool found_featuremoment_gravity;
72  bool found_featuremoment_surfacenormalized;
73 
74  vpMomentAreaNormalized& momentSurfaceNormalized = static_cast<vpMomentAreaNormalized&>(moments.get("vpMomentAreaNormalized",found_moment_surface_normalized));
75  vpMomentGravityCenter& momentGravity = static_cast<vpMomentGravityCenter&>(moments.get("vpMomentGravityCenter",found_moment_gravity));
76  vpFeatureMomentGravityCenter& featureMomentGravity = (static_cast<vpFeatureMomentGravityCenter&>(featureMoments->get("vpFeatureMomentGravityCenter",found_featuremoment_gravity)));
77  vpFeatureMomentAreaNormalized featureMomentAreaNormalized = (static_cast<vpFeatureMomentAreaNormalized&>(featureMoments->get("vpFeatureMomentAreaNormalized",found_featuremoment_surfacenormalized)));
78 
79  if(!found_moment_surface_normalized) throw vpException(vpException::notInitialized,"vpMomentAreaNormalized not found");
80  if(!found_moment_gravity) throw vpException(vpException::notInitialized,"vpMomentGravityCenter not found");
81 
82  if(!found_featuremoment_gravity) throw vpException(vpException::notInitialized,"vpFeatureMomentGravityCenter not found");
83  if(!found_featuremoment_surfacenormalized) throw vpException(vpException::notInitialized,"vpFeatureMomentAreaNormalized not found");
84 
85 
86  interaction_matrices[0].resize(1,6);
87  interaction_matrices[1].resize(1,6);
88 
89  interaction_matrices[0] = momentGravity.get()[0]*featureMomentAreaNormalized.interaction(1) + momentSurfaceNormalized.get()[0]*featureMomentGravity.interaction(1);
90  interaction_matrices[1] = momentGravity.get()[1]*featureMomentAreaNormalized.interaction(1) + momentSurfaceNormalized.get()[0]*featureMomentGravity.interaction(2);
91 }
92 
93 #else
94 
95 #include <vector>
96 #include <limits>
97 
98 #include <visp/vpMomentObject.h>
99 #include <visp/vpMomentCentered.h>
100 #include <visp/vpFeatureMomentDatabase.h>
101 #include <visp/vpFeatureMomentGravityCenterNormalized.h>
102 #include <visp/vpMomentAreaNormalized.h>
103 #include <visp/vpMomentGravityCenter.h>
104 
105 
115 
116  bool found_moment_surface_normalized;
117  bool found_moment_gravity;
118  bool found_moment_centered;
119 
120  vpMomentCentered& momentCentered = static_cast<vpMomentCentered&>(moments.get("vpMomentCentered",found_moment_centered));
121  vpMomentGravityCenter& momentGravity = static_cast<vpMomentGravityCenter&>(moments.get("vpMomentGravityCenter",found_moment_gravity));
122  vpMomentAreaNormalized& momentSurfaceNormalized = static_cast<vpMomentAreaNormalized&>(moments.get("vpMomentAreaNormalized",found_moment_surface_normalized));
123 
124  if(!found_moment_surface_normalized) throw vpException(vpException::notInitialized,"vpMomentAreaNormalized not found");
125  if(!found_moment_gravity) throw vpException(vpException::notInitialized,"vpMomentGravityCenter not found");
126  if(!found_moment_centered) throw vpException(vpException::notInitialized,"vpMomentCentered not found");
127 
128  vpMomentObject& momentObject = moment->getObject();
129  interaction_matrices[0].resize(1,6);
130  interaction_matrices[1].resize(1,6);
131 
132 
133  double n11 = momentCentered.get(1,1)/momentObject.get(0,0);
134  double n20 = momentCentered.get(2,0)/momentObject.get(0,0);
135  double n02 = momentCentered.get(0,2)/momentObject.get(0,0);
136  double n10 = momentCentered.get(1,0)/momentObject.get(0,0);
137  double n01 = momentCentered.get(0,1)/momentObject.get(0,0);
138  double n03 = momentCentered.get(0,3)/momentObject.get(0,0);
139 
140 
141  double Xg = momentGravity.getXg();
142  double Yg = momentGravity.getYg();
143 
144  double An=momentSurfaceNormalized.get()[0];
145 
146  double Xn = An*Xg;
147  double Yn = An*Yg;
148 
149 
150  double Xnvx,Xnvy,Xnvz,Xnwx,Xnwy;
151  double Ynvx,Ynvy,Ynvz,Ynwx,Ynwy;
152 
153  if(momentObject.getType()==vpMomentObject::DISCRETE){
154  double a = momentCentered.get(2,0)+momentCentered.get(0,2);
155 
156  double e11 = momentCentered.get(1,1)/a;
157  double e12 = momentCentered.get(1,2)/a;
158  double e21 = momentCentered.get(2,1)/a;
159  double e30 = momentCentered.get(3,0)/a;
160 
161  double NA = n20+n02;
162 
163 
164  Xnvx = B*Xn*e11-Yn*B-An*C-n02*A*Xn/NA;
165  Xnvy = A*Xn*e11+n02*B*Xn/NA;
166 
167  Xnwx = An*e11*NA+Yn*n10-Xn*Xg*e11+Xn*n01+Xn*n10*e11-Xn*e21+(-Xn*n03+(Xn*n01-Yn*Xg)*n02)/NA;
168  Xnwy = -An*NA+Xn*e12+Xn*Xg-An+e11*Xg*Yn-Xn*n01*e11-2*Xn*n10+Xn*e30+n02*An+(-Xn*Xg+Xn*n10)*n02/NA;
169 
170 
171 
172  Ynvx = (Yn-n02*Yn/NA)*A+Yn*e11*B;
173  Ynvy = (-Xn+e11*Yn)*A+(-Yn+n02*Yn/NA)*B-An*C;
174 
175  Ynwx = n02*An+Yn*n10*e11-e11*Xg*Yn+An-Yn*e21+Yn*n01+(-Yn*n03+(Yn*n01-Yn*Yg)*n02)/NA;
176  Ynwy = -An*e11*NA+Yn*e11*Yg-Yn*n01*e11+Yn*Xg+Yn*e12+Yn*e30-Xn*n01-2*Yn*n10+(Yn*n10-Yn*Xg)*n02/NA;
177 
178 
179  }else{
180  Xnvx = -An*C-A*Xn-Yn*B;
181  Xnvy = (0.5)*B*Xn;
182 
183  Xnwx = (0.5*(8.*n10-Xg))*Yn+4.*An*n11+4.*n01*Xn;
184  Xnwy = (0.5*(-2.-8.*n20))*An+(0.5)*Xn*(-8.*n10+Xg);
185 
186  Ynvx = (0.5)*A*Yn;
187  Ynvy = -(0.5)*B*Yn-C*An-A*Xn;
188 
189  Ynwx = (0.5)*Yn*(8.*n01-Yg)+(.5*(2.+8.*n02))*An;
190  Ynwy = (0.5*(-8.*n10+Xg))*Yn-4.*An*n11-4.*n01*Xn;
191 
192  }
193 
194  Ynvz = -A*Ynwy+(-An+Ynwx)*B;
195  Xnvz = -A*An-A*Xnwy+B*Xnwx;
196 
197  int VX = 0;
198  int VY = 1;
199  int VZ = 2;
200  int WX = 3;
201  int WY = 4;
202  int WZ = 5;
203 
204  interaction_matrices[0][0][VX] = Xnvx;
205  interaction_matrices[0][0][VY] = Xnvy;
206  interaction_matrices[0][0][VZ] = Xnvz;
207 
208  interaction_matrices[0][0][WX] = Xnwx;
209  interaction_matrices[0][0][WY] = Xnwy;
210  interaction_matrices[0][0][WZ] = Yn;
211 
212  interaction_matrices[1][0][VX] = Ynvx;
213  interaction_matrices[1][0][VY] = Ynvy;
214  interaction_matrices[1][0][VZ] = Ynvz;
215 
216  interaction_matrices[1][0][WX] = Ynwx;
217  interaction_matrices[1][0][WY] = Ynwy;
218  interaction_matrices[1][0][WZ] = -Xn;
219 
220 }
221 #endif
Class handling the normalized surface moment that is invariant in scale and used to estimate depth...
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
Functionality computation for normalized surface moment feature. Computes the interaction matrix asso...
vpMatrix interaction(const unsigned int select=FEATURE_ALL)
vpMoment & get(const char *type, bool &found)
std::vector< double > & get()
vpMomentDatabase & moments
This class defines the double-indexed centered moment descriptor .
Class describing 2D gravity center moment.
vpObjectType getType() const
std::vector< double > & get()
Definition: vpMoment.h:125
Functionality computation for gravity center moment feature. Computes the interaction matrix associat...
std::vector< double > & get()
vpFeatureMoment & get(const char *type, bool &found)