Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpFeatureMomentAreaNormalized.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2023 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 https://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.getDesiredArea() / 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
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 char *type, 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:152
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 char *type, 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:145
const std::vector< double > & get() const
Definition vpMoment.h:150