Visual Servoing Platform  version 3.6.1 under development (2024-04-23)
vpFeatureMomentAreaNormalized.cpp
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
4  *
5  * This software is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
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 https://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 #include <visp3/core/vpConfig.h>
34 
35 #ifdef VISP_MOMENTS_COMBINE_MATRICES
36 #include <limits>
37 #include <vector>
38 
39 #include <visp3/core/vpMomentAreaNormalized.h>
40 #include <visp3/core/vpMomentCentered.h>
41 #include <visp3/core/vpMomentObject.h>
42 #include <visp3/visual_features/vpFeatureMomentAreaNormalized.h>
43 #include <visp3/visual_features/vpFeatureMomentBasic.h>
44 #include <visp3/visual_features/vpFeatureMomentCentered.h>
45 #include <visp3/visual_features/vpFeatureMomentDatabase.h>
46 
57 {
58  bool found_moment_centered;
59  bool found_moment_surface_normalized;
60  bool found_FeatureMoment_centered;
61 
62  bool found_featuremoment_basic;
63  vpFeatureMomentBasic &featureMomentBasic = (static_cast<vpFeatureMomentBasic &>(
64  featureMomentsDataBase->get("vpFeatureMomentBasic", found_featuremoment_basic)));
65 
66  const vpMomentCentered &momentCentered =
67  static_cast<const vpMomentCentered &>(moments.get("vpMomentCentered", found_moment_centered));
68  const vpMomentObject &momentObject = moment->getObject();
69  const vpMomentAreaNormalized &momentSurfaceNormalized = static_cast<const vpMomentAreaNormalized &>(
70  moments.get("vpMomentAreaNormalized", found_moment_surface_normalized));
71  vpFeatureMomentCentered &featureMomentCentered = (static_cast<vpFeatureMomentCentered &>(
72  featureMomentsDataBase->get("vpFeatureMomentCentered", found_FeatureMoment_centered)));
73 
74  if (!found_FeatureMoment_centered)
75  throw vpException(vpException::notInitialized, "vpFeatureMomentCentered not found");
76  if (!found_moment_surface_normalized)
77  throw vpException(vpException::notInitialized, "vpMomentAreaNormalized not found");
78  if (!found_moment_centered)
79  throw vpException(vpException::notInitialized, "vpMomentCentered not found");
80  if (!found_featuremoment_basic)
81  throw vpException(vpException::notInitialized, "vpFeatureMomentBasic not found");
82  interaction_matrices.resize(1);
83  interaction_matrices[0].resize(1, 6);
84  double normalized_multiplier;
85  double a;
86  vpMatrix La;
87  if (momentObject.getType() == vpMomentObject::DISCRETE) {
88  a = momentCentered.get(2, 0) + momentCentered.get(0, 2);
89  La = featureMomentCentered.interaction(2, 0) + featureMomentCentered.interaction(0, 2);
90  }
91  else {
92  a = momentObject.get(0, 0);
93  La = featureMomentBasic.interaction(0, 0);
94  }
95 
96  normalized_multiplier =
97  (-momentSurfaceNormalized.getDesiredDepth() / (2 * a)) * sqrt(momentSurfaceNormalized.getDesiredArea() / a);
98  interaction_matrices[0] = normalized_multiplier * La;
99 }
100 
101 #else
102 
103 #include <limits>
104 #include <vector>
105 
106 #include <visp3/core/vpMomentAreaNormalized.h>
107 #include <visp3/core/vpMomentCentered.h>
108 #include <visp3/core/vpMomentGravityCenter.h>
109 #include <visp3/core/vpMomentObject.h>
110 #include <visp3/visual_features/vpFeatureMomentAreaNormalized.h>
111 #include <visp3/visual_features/vpFeatureMomentDatabase.h>
112 
122 {
123  bool found_moment_centered;
124  bool found_moment_surface_normalized;
125  bool found_moment_gravity;
126 
127  const vpMomentCentered &momentCentered =
128  static_cast<const vpMomentCentered &>(moments.get("vpMomentCentered", found_moment_centered));
129  const vpMomentGravityCenter &momentGravity =
130  static_cast<const vpMomentGravityCenter &>(moments.get("vpMomentGravityCenter", found_moment_gravity));
131  const vpMomentObject &momentObject = moment->getObject();
132  const vpMomentAreaNormalized &momentSurfaceNormalized = static_cast<const vpMomentAreaNormalized &>(
133  moments.get("vpMomentAreaNormalized", found_moment_surface_normalized));
134 
135  if (!found_moment_surface_normalized)
136  throw vpException(vpException::notInitialized, "vpMomentAreaNormalized not found");
137  if (!found_moment_centered)
138  throw vpException(vpException::notInitialized, "vpMomentCentered not found");
139  if (!found_moment_gravity)
140  throw vpException(vpException::notInitialized, "vpMomentGravityCenter not found");
141  interaction_matrices.resize(1);
142  interaction_matrices[0].resize(1, 6);
143  double n11 = momentCentered.get(1, 1) / momentObject.get(0, 0);
144  double n20 = momentCentered.get(2, 0) / momentObject.get(0, 0);
145  double n02 = momentCentered.get(0, 2) / momentObject.get(0, 0);
146  double Xg = momentGravity.getXg();
147  double Yg = momentGravity.getYg();
148 
149  double An = momentSurfaceNormalized.get()[0];
150 
151  double Xn = An * Xg;
152  double Yn = An * Yg;
153 
154  double Anvx, Anvy, Anvz, Anwx, Anwy;
155 
156  if (momentObject.getType() == vpMomentObject::DISCRETE) {
157  double a = momentCentered.get(2, 0) + momentCentered.get(0, 2);
158 
159  double e01 = momentCentered.get(0, 1) / a;
160  double e10 = momentCentered.get(1, 0) / a;
161  double e11 = momentCentered.get(1, 1) / a;
162  double e02 = momentCentered.get(0, 2) / a;
163  double e20 = momentCentered.get(2, 0) / a;
164  double e12 = momentCentered.get(1, 2) / a;
165  double e21 = momentCentered.get(2, 1) / a;
166  double e03 = momentCentered.get(0, 3) / a;
167  double e30 = momentCentered.get(3, 0) / a;
168 
169  Anvx = An * A * e20 + An * B * e11;
170  Anvy = An * A * e11 + An * B * e02;
171 
172  Anwx = (n02 * e01 + n11 * e10 - e03 - e21) * An - Xn * e11 + (-1 - e02) * Yn;
173  Anwy = (e12 + e30 - n11 * e01 - n20 * e10) * An + (2 - e02) * Xn + Yn * e11;
174 
175  Anvz = -An * C + B * Anwx - A * Anwy;
176 
177  }
178  else {
179  Anvx = A * An / 2.;
180  Anvy = B * An / 2.;
181  Anvz = -An * C - (3. / 2.) * A * Xn - (3. / 2.) * B * Yn;
182 
183  Anwx = -(3. / 2.) * Yn;
184  Anwy = (3. / 2.) * Xn;
185  }
186 
187  int VX = 0;
188  int VY = 1;
189  int VZ = 2;
190  int WX = 3;
191  int WY = 4;
192  int WZ = 5;
193 
194  interaction_matrices[0][0][VX] = Anvx;
195  interaction_matrices[0][0][VY] = Anvy;
196  interaction_matrices[0][0][VZ] = Anvz;
197 
198  interaction_matrices[0][0][WX] = Anwx;
199  interaction_matrices[0][0][WY] = Anwy;
200  interaction_matrices[0][0][WZ] = 0.;
201 }
202 
203 #endif
error that can be emitted by ViSP classes.
Definition: vpException.h:59
@ notInitialized
Used to indicate that a parameter is not initialized.
Definition: vpException.h:86
Functionality computation for basic moment feature. Computes the interaction matrix associated with v...
vpMatrix interaction(unsigned int select_one, unsigned int select_two) const
Functionality computation for centered moment feature. Computes the interaction matrix associated wit...
vpMatrix interaction(unsigned int select_one, unsigned int select_two) const
vpFeatureMoment & get(const std::string &feature_name, bool &found)
std::vector< vpMatrix > interaction_matrices
vpFeatureMomentDatabase * featureMomentsDataBase
vpMomentDatabase & moments
const vpMoment * moment
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
Class handling the normalized surface moment that is invariant in scale and used to estimate depth.
This class defines the double-indexed centered moment descriptor .
double get(unsigned int i, unsigned int j) const
const vpMoment & get(const std::string &moment_name, bool &found) const
Class describing 2D gravity center moment.
Class for generic objects.
const std::vector< double > & get() const
vpObjectType getType() const
const vpMomentObject & getObject() const
Definition: vpMoment.h:144
const std::vector< double > & get() const
Definition: vpMoment.h:149