Visual Servoing Platform  version 3.0.0
vpFeatureMomentGravityCenterNormalized.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2015 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  * Implementation for all supported moment features.
32  *
33  * Authors:
34  * Filip Novotny
35  *
36  *****************************************************************************/
37 
38 #include <visp3/core/vpConfig.h>
39 
40 #ifdef VISP_MOMENTS_COMBINE_MATRICES
41 
42 #include <vector>
43 #include <limits>
44 
45 #include <visp3/core/vpMomentObject.h>
46 #include <visp3/core/vpMomentCentered.h>
47 #include <visp3/core/vpMomentAreaNormalized.h>
48 #include <visp3/core/vpMomentGravityCenter.h>
49 #include <visp3/visual_features/vpFeatureMomentGravityCenter.h>
50 #include <visp3/visual_features/vpFeatureMomentAreaNormalized.h>
51 #include <visp3/visual_features/vpFeatureMomentGravityCenterNormalized.h>
52 #include <visp3/visual_features/vpFeatureMomentDatabase.h>
53 
64  bool found_moment_gravity;
65  bool found_moment_surface_normalized;
66 
67  bool found_featuremoment_gravity;
68  bool found_featuremoment_surfacenormalized;
69 
70  const vpMomentAreaNormalized& momentSurfaceNormalized = static_cast<const vpMomentAreaNormalized&>(moments.get("vpMomentAreaNormalized",found_moment_surface_normalized));
71  const vpMomentGravityCenter& momentGravity = static_cast<const vpMomentGravityCenter&>(moments.get("vpMomentGravityCenter",found_moment_gravity));
72  vpFeatureMomentGravityCenter& featureMomentGravity = (static_cast<vpFeatureMomentGravityCenter&>(featureMomentsDataBase->get("vpFeatureMomentGravityCenter",found_featuremoment_gravity)));
73  vpFeatureMomentAreaNormalized featureMomentAreaNormalized = (static_cast<vpFeatureMomentAreaNormalized&>(featureMomentsDataBase->get("vpFeatureMomentAreaNormalized",found_featuremoment_surfacenormalized)));
74 
75  if(!found_moment_surface_normalized) throw vpException(vpException::notInitialized,"vpMomentAreaNormalized not found");
76  if(!found_moment_gravity) throw vpException(vpException::notInitialized,"vpMomentGravityCenter not found");
77 
78  if(!found_featuremoment_gravity) throw vpException(vpException::notInitialized,"vpFeatureMomentGravityCenter not found");
79  if(!found_featuremoment_surfacenormalized) throw vpException(vpException::notInitialized,"vpFeatureMomentAreaNormalized not found");
80 
81 
82  interaction_matrices[0].resize(1,6);
83  interaction_matrices[1].resize(1,6);
84 
85  interaction_matrices[0] = momentGravity.get()[0]*featureMomentAreaNormalized.interaction(1) + momentSurfaceNormalized.get()[0]*featureMomentGravity.interaction(1);
86  interaction_matrices[1] = momentGravity.get()[1]*featureMomentAreaNormalized.interaction(1) + momentSurfaceNormalized.get()[0]*featureMomentGravity.interaction(2);
87 }
88 
89 #else
90 
91 #include <vector>
92 #include <limits>
93 
94 #include <visp3/core/vpMomentObject.h>
95 #include <visp3/core/vpMomentCentered.h>
96 #include <visp3/visual_features/vpFeatureMomentDatabase.h>
97 #include <visp3/visual_features/vpFeatureMomentGravityCenterNormalized.h>
98 #include <visp3/core/vpMomentAreaNormalized.h>
99 #include <visp3/core/vpMomentGravityCenter.h>
100 
101 
111 
112  bool found_moment_surface_normalized;
113  bool found_moment_gravity;
114  bool found_moment_centered;
115 
116  const vpMomentCentered& momentCentered = static_cast<const vpMomentCentered&>(moments.get("vpMomentCentered",found_moment_centered));
117  const vpMomentGravityCenter& momentGravity = static_cast<const vpMomentGravityCenter&>(moments.get("vpMomentGravityCenter",found_moment_gravity));
118  const vpMomentAreaNormalized& momentSurfaceNormalized = static_cast<const vpMomentAreaNormalized&>(moments.get("vpMomentAreaNormalized",found_moment_surface_normalized));
119 
120  if(!found_moment_surface_normalized) throw vpException(vpException::notInitialized,"vpMomentAreaNormalized not found");
121  if(!found_moment_gravity) throw vpException(vpException::notInitialized,"vpMomentGravityCenter not found");
122  if(!found_moment_centered) throw vpException(vpException::notInitialized,"vpMomentCentered not found");
123 
124  const vpMomentObject& momentObject = moment->getObject();
125  interaction_matrices[0].resize(1,6);
126  interaction_matrices[1].resize(1,6);
127 
128 
129  double n11 = momentCentered.get(1,1)/momentObject.get(0,0);
130  double n20 = momentCentered.get(2,0)/momentObject.get(0,0);
131  double n02 = momentCentered.get(0,2)/momentObject.get(0,0);
132  double n10 = momentCentered.get(1,0)/momentObject.get(0,0);
133  double n01 = momentCentered.get(0,1)/momentObject.get(0,0);
134  double n03 = momentCentered.get(0,3)/momentObject.get(0,0);
135 
136 
137  double Xg = momentGravity.getXg();
138  double Yg = momentGravity.getYg();
139 
140  double An=momentSurfaceNormalized.get()[0];
141 
142  double Xn = An*Xg;
143  double Yn = An*Yg;
144 
145 
146  double Xnvx,Xnvy,Xnvz,Xnwx,Xnwy;
147  double Ynvx,Ynvy,Ynvz,Ynwx,Ynwy;
148 
149  if(momentObject.getType()==vpMomentObject::DISCRETE){
150  double a = momentCentered.get(2,0)+momentCentered.get(0,2);
151 
152  double e11 = momentCentered.get(1,1)/a;
153  double e12 = momentCentered.get(1,2)/a;
154  double e21 = momentCentered.get(2,1)/a;
155  double e30 = momentCentered.get(3,0)/a;
156 
157  double NA = n20+n02;
158 
159 
160  Xnvx = B*Xn*e11-Yn*B-An*C-n02*A*Xn/NA;
161  Xnvy = A*Xn*e11+n02*B*Xn/NA;
162 
163  Xnwx = An*e11*NA+Yn*n10-Xn*Xg*e11+Xn*n01+Xn*n10*e11-Xn*e21+(-Xn*n03+(Xn*n01-Yn*Xg)*n02)/NA;
164  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;
165 
166 
167 
168  Ynvx = (Yn-n02*Yn/NA)*A+Yn*e11*B;
169  Ynvy = (-Xn+e11*Yn)*A+(-Yn+n02*Yn/NA)*B-An*C;
170 
171  Ynwx = n02*An+Yn*n10*e11-e11*Xg*Yn+An-Yn*e21+Yn*n01+(-Yn*n03+(Yn*n01-Yn*Yg)*n02)/NA;
172  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;
173 
174 
175  }else{
176  Xnvx = -An*C-A*Xn-Yn*B;
177  Xnvy = (0.5)*B*Xn;
178 
179  Xnwx = (0.5*(8.*n10-Xg))*Yn+4.*An*n11+4.*n01*Xn;
180  Xnwy = (0.5*(-2.-8.*n20))*An+(0.5)*Xn*(-8.*n10+Xg);
181 
182  Ynvx = (0.5)*A*Yn;
183  Ynvy = -(0.5)*B*Yn-C*An-A*Xn;
184 
185  Ynwx = (0.5)*Yn*(8.*n01-Yg)+(.5*(2.+8.*n02))*An;
186  Ynwy = (0.5*(-8.*n10+Xg))*Yn-4.*An*n11-4.*n01*Xn;
187 
188  }
189 
190  Ynvz = -A*Ynwy+(-An+Ynwx)*B;
191  Xnvz = -A*An-A*Xnwy+B*Xnwx;
192 
193  int VX = 0;
194  int VY = 1;
195  int VZ = 2;
196  int WX = 3;
197  int WY = 4;
198  int WZ = 5;
199 
200  interaction_matrices[0][0][VX] = Xnvx;
201  interaction_matrices[0][0][VY] = Xnvy;
202  interaction_matrices[0][0][VZ] = Xnvz;
203 
204  interaction_matrices[0][0][WX] = Xnwx;
205  interaction_matrices[0][0][WY] = Xnwy;
206  interaction_matrices[0][0][WZ] = Yn;
207 
208  interaction_matrices[1][0][VX] = Ynvx;
209  interaction_matrices[1][0][VY] = Ynvy;
210  interaction_matrices[1][0][VZ] = Ynvz;
211 
212  interaction_matrices[1][0][WX] = Ynwx;
213  interaction_matrices[1][0][WY] = Ynwy;
214  interaction_matrices[1][0][WZ] = -Xn;
215 
216 }
217 #endif
Class handling the normalized surface moment that is invariant in scale and used to estimate depth...
const std::vector< double > & get() const
Definition: vpMoment.h:148
const vpMoment * moment
error that can be emited by ViSP classes.
Definition: vpException.h:73
Class for generic objects.
const std::vector< double > & get() const
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)
const vpMoment & get(const char *type, bool &found) const
vpMomentDatabase & moments
This class defines the double-indexed centered moment descriptor .
double get(unsigned int i, unsigned int j) const
Class describing 2D gravity center moment.
vpObjectType getType() const
const std::vector< double > & get() const
vpFeatureMomentDatabase * featureMomentsDataBase
const vpMomentObject & getObject() const
Definition: vpMoment.h:143
Functionality computation for gravity center moment feature. Computes the interaction matrix associat...
vpFeatureMoment & get(const char *type, bool &found)