Visual Servoing Platform  version 3.1.0
vpFeatureMomentAreaNormalized.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 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 http://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  * Implementation for all supported moment features.
33  *
34  * Authors:
35  * Filip Novotny
36  *
37  *****************************************************************************/
38 #include <visp3/core/vpConfig.h>
39 
40 #ifdef VISP_MOMENTS_COMBINE_MATRICES
41 #include <limits>
42 #include <vector>
43 
44 #include <visp3/core/vpMomentAreaNormalized.h>
45 #include <visp3/core/vpMomentCentered.h>
46 #include <visp3/core/vpMomentObject.h>
47 #include <visp3/visual_features/vpFeatureMomentAreaNormalized.h>
48 #include <visp3/visual_features/vpFeatureMomentBasic.h>
49 #include <visp3/visual_features/vpFeatureMomentCentered.h>
50 #include <visp3/visual_features/vpFeatureMomentDatabase.h>
51 
62 {
63  bool found_moment_centered;
64  bool found_moment_surface_normalized;
65  bool found_FeatureMoment_centered;
66 
67  bool found_featuremoment_basic;
68  vpFeatureMomentBasic &featureMomentBasic = (static_cast<vpFeatureMomentBasic &>(
69  featureMomentsDataBase->get("vpFeatureMomentBasic", found_featuremoment_basic)));
70 
71  const vpMomentCentered &momentCentered =
72  static_cast<const vpMomentCentered &>(moments.get("vpMomentCentered", found_moment_centered));
73  const vpMomentObject &momentObject = moment->getObject();
74  const vpMomentAreaNormalized &momentSurfaceNormalized = static_cast<const vpMomentAreaNormalized &>(
75  moments.get("vpMomentAreaNormalized", found_moment_surface_normalized));
76  vpFeatureMomentCentered &featureMomentCentered = (static_cast<vpFeatureMomentCentered &>(
77  featureMomentsDataBase->get("vpFeatureMomentCentered", found_FeatureMoment_centered)));
78 
79  if (!found_FeatureMoment_centered)
80  throw vpException(vpException::notInitialized, "vpFeatureMomentCentered not found");
81  if (!found_moment_surface_normalized)
82  throw vpException(vpException::notInitialized, "vpMomentAreaNormalized not found");
83  if (!found_moment_centered)
84  throw vpException(vpException::notInitialized, "vpMomentCentered not found");
85  if (!found_featuremoment_basic)
86  throw vpException(vpException::notInitialized, "vpFeatureMomentBasic not found");
87  interaction_matrices.resize(1);
88  interaction_matrices[0].resize(1, 6);
89  double normalized_multiplier;
90  double a;
91  vpMatrix La;
92  if (momentObject.getType() == vpMomentObject::DISCRETE) {
93  a = momentCentered.get(2, 0) + momentCentered.get(0, 2);
94  La = featureMomentCentered.interaction(2, 0) + featureMomentCentered.interaction(0, 2);
95  } else {
96  a = momentObject.get(0, 0);
97  La = featureMomentBasic.interaction(0, 0);
98  }
99 
100  normalized_multiplier =
101  (-momentSurfaceNormalized.getDesiredDepth() / (2 * a)) * sqrt(momentSurfaceNormalized.getDesiredSurface() / a);
102  interaction_matrices[0] = normalized_multiplier * La;
103 }
104 
105 #else
106 
107 #include <limits>
108 #include <vector>
109 
110 #include <visp3/core/vpMomentAreaNormalized.h>
111 #include <visp3/core/vpMomentCentered.h>
112 #include <visp3/core/vpMomentGravityCenter.h>
113 #include <visp3/core/vpMomentObject.h>
114 #include <visp3/visual_features/vpFeatureMomentAreaNormalized.h>
115 #include <visp3/visual_features/vpFeatureMomentDatabase.h>
116 
126 {
127  bool found_moment_centered;
128  bool found_moment_surface_normalized;
129  bool found_moment_gravity;
130 
131  const vpMomentCentered &momentCentered =
132  static_cast<const vpMomentCentered &>(moments.get("vpMomentCentered", found_moment_centered));
133  const vpMomentGravityCenter &momentGravity =
134  static_cast<const vpMomentGravityCenter &>(moments.get("vpMomentGravityCenter", found_moment_gravity));
135  const vpMomentObject &momentObject = moment->getObject();
136  const vpMomentAreaNormalized &momentSurfaceNormalized = static_cast<const vpMomentAreaNormalized &>(
137  moments.get("vpMomentAreaNormalized", found_moment_surface_normalized));
138 
139  if (!found_moment_surface_normalized)
140  throw vpException(vpException::notInitialized, "vpMomentAreaNormalized not found");
141  if (!found_moment_centered)
142  throw vpException(vpException::notInitialized, "vpMomentCentered not found");
143  if (!found_moment_gravity)
144  throw vpException(vpException::notInitialized, "vpMomentGravityCenter not found");
145  interaction_matrices.resize(1);
146  interaction_matrices[0].resize(1, 6);
147  double n11 = momentCentered.get(1, 1) / momentObject.get(0, 0);
148  double n20 = momentCentered.get(2, 0) / momentObject.get(0, 0);
149  double n02 = momentCentered.get(0, 2) / momentObject.get(0, 0);
150  double Xg = momentGravity.getXg();
151  double Yg = momentGravity.getYg();
152 
153  double An = momentSurfaceNormalized.get()[0];
154 
155  double Xn = An * Xg;
156  double Yn = An * Yg;
157 
158  double Anvx, Anvy, Anvz, Anwx, Anwy;
159 
160  if (momentObject.getType() == vpMomentObject::DISCRETE) {
161  double a = momentCentered.get(2, 0) + momentCentered.get(0, 2);
162 
163  double e01 = momentCentered.get(0, 1) / a;
164  double e10 = momentCentered.get(1, 0) / a;
165  double e11 = momentCentered.get(1, 1) / a;
166  double e02 = momentCentered.get(0, 2) / a;
167  double e20 = momentCentered.get(2, 0) / a;
168  double e12 = momentCentered.get(1, 2) / a;
169  double e21 = momentCentered.get(2, 1) / a;
170  double e03 = momentCentered.get(0, 3) / a;
171  double e30 = momentCentered.get(3, 0) / a;
172 
173  Anvx = An * A * e20 + An * B * e11;
174  Anvy = An * A * e11 + An * B * e02;
175 
176  Anwx = (n02 * e01 + n11 * e10 - e03 - e21) * An - Xn * e11 + (-1 - e02) * Yn;
177  Anwy = (e12 + e30 - n11 * e01 - n20 * e10) * An + (2 - e02) * Xn + Yn * e11;
178 
179  Anvz = -An * C + B * Anwx - A * Anwy;
180 
181  } else {
182  Anvx = A * An / 2.;
183  Anvy = B * An / 2.;
184  Anvz = -An * C - (3. / 2.) * A * Xn - (3. / 2.) * B * Yn;
185 
186  Anwx = -(3. / 2.) * Yn;
187  Anwy = (3. / 2.) * Xn;
188  }
189 
190  int VX = 0;
191  int VY = 1;
192  int VZ = 2;
193  int WX = 3;
194  int WY = 4;
195  int WZ = 5;
196 
197  interaction_matrices[0][0][VX] = Anvx;
198  interaction_matrices[0][0][VY] = Anvy;
199  interaction_matrices[0][0][VZ] = Anvz;
200 
201  interaction_matrices[0][0][WX] = Anwx;
202  interaction_matrices[0][0][WY] = Anwy;
203  interaction_matrices[0][0][WZ] = 0.;
204 }
205 
206 #endif
Functionality computation for basic moment feature. Computes the interaction matrix associated with v...
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:104
Class handling the normalized surface moment that is invariant in scale and used to estimate depth...
double get(unsigned int i, unsigned int j) const
const vpMoment * moment
Functionality computation for centered moment feature. Computes the interaction matrix associated wit...
const vpMoment & get(const char *type, bool &found) const
error that can be emited by ViSP classes.
Definition: vpException.h:71
Class for generic objects.
std::vector< vpMatrix > interaction_matrices
const std::vector< double > & get() const
Definition: vpMoment.h:155
const vpMomentObject & getObject() const
Definition: vpMoment.h:150
vpMomentDatabase & moments
const std::vector< double > & get() const
This class defines the double-indexed centered moment descriptor .
vpObjectType getType() const
Class describing 2D gravity center moment.
vpFeatureMomentDatabase * featureMomentsDataBase
vpMatrix interaction(unsigned int select_one, unsigned int select_two) const
vpMatrix interaction(unsigned int select_one, unsigned int select_two) const
vpFeatureMoment & get(const char *type, bool &found)