Visual Servoing Platform version 3.5.0
vpFeatureMomentGravityCenterNormalized.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2019 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
39#include <visp3/core/vpConfig.h>
40
41#ifdef VISP_MOMENTS_COMBINE_MATRICES
42
43#include <limits>
44#include <vector>
45
46#include <visp3/core/vpMomentAreaNormalized.h>
47#include <visp3/core/vpMomentCentered.h>
48#include <visp3/core/vpMomentGravityCenter.h>
49#include <visp3/core/vpMomentObject.h>
50#include <visp3/visual_features/vpFeatureMomentAreaNormalized.h>
51#include <visp3/visual_features/vpFeatureMomentDatabase.h>
52#include <visp3/visual_features/vpFeatureMomentGravityCenter.h>
53#include <visp3/visual_features/vpFeatureMomentGravityCenterNormalized.h>
54
65{
66 bool found_moment_gravity;
67 bool found_moment_surface_normalized;
68
69 bool found_featuremoment_gravity;
70 bool found_featuremoment_surfacenormalized;
71
72 const vpMomentAreaNormalized &momentSurfaceNormalized = static_cast<const vpMomentAreaNormalized &>(
73 moments.get("vpMomentAreaNormalized", found_moment_surface_normalized));
74 const vpMomentGravityCenter &momentGravity =
75 static_cast<const vpMomentGravityCenter &>(moments.get("vpMomentGravityCenter", found_moment_gravity));
76 vpFeatureMomentGravityCenter &featureMomentGravity = (static_cast<vpFeatureMomentGravityCenter &>(
77 featureMomentsDataBase->get("vpFeatureMomentGravityCenter", found_featuremoment_gravity)));
78 vpFeatureMomentAreaNormalized featureMomentAreaNormalized = (static_cast<vpFeatureMomentAreaNormalized &>(
79 featureMomentsDataBase->get("vpFeatureMomentAreaNormalized", found_featuremoment_surfacenormalized)));
80
81 if (!found_moment_surface_normalized)
82 throw vpException(vpException::notInitialized, "vpMomentAreaNormalized not found");
83 if (!found_moment_gravity)
84 throw vpException(vpException::notInitialized, "vpMomentGravityCenter not found");
85
86 if (!found_featuremoment_gravity)
87 throw vpException(vpException::notInitialized, "vpFeatureMomentGravityCenter not found");
88 if (!found_featuremoment_surfacenormalized)
89 throw vpException(vpException::notInitialized, "vpFeatureMomentAreaNormalized not found");
90
91 interaction_matrices[0].resize(1, 6);
92 interaction_matrices[1].resize(1, 6);
93
94 interaction_matrices[0] = momentGravity.get()[0] * featureMomentAreaNormalized.interaction(1) +
95 momentSurfaceNormalized.get()[0] * featureMomentGravity.interaction(1);
96 interaction_matrices[1] = momentGravity.get()[1] * featureMomentAreaNormalized.interaction(1) +
97 momentSurfaceNormalized.get()[0] * featureMomentGravity.interaction(2);
98}
99
100#else
101
102#include <limits>
103#include <vector>
104
105#include <visp3/core/vpMomentAreaNormalized.h>
106#include <visp3/core/vpMomentCentered.h>
107#include <visp3/core/vpMomentGravityCenter.h>
108#include <visp3/core/vpMomentObject.h>
109#include <visp3/visual_features/vpFeatureMomentDatabase.h>
110#include <visp3/visual_features/vpFeatureMomentGravityCenterNormalized.h>
111
121{
122
123 bool found_moment_surface_normalized;
124 bool found_moment_gravity;
125 bool found_moment_centered;
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 vpMomentAreaNormalized &momentSurfaceNormalized = static_cast<const vpMomentAreaNormalized &>(
132 moments.get("vpMomentAreaNormalized", found_moment_surface_normalized));
133
134 if (!found_moment_surface_normalized)
135 throw vpException(vpException::notInitialized, "vpMomentAreaNormalized not found");
136 if (!found_moment_gravity)
137 throw vpException(vpException::notInitialized, "vpMomentGravityCenter not found");
138 if (!found_moment_centered)
139 throw vpException(vpException::notInitialized, "vpMomentCentered not found");
140
141 const vpMomentObject &momentObject = moment->getObject();
142 interaction_matrices[0].resize(1, 6);
143 interaction_matrices[1].resize(1, 6);
144
145 double n11 = momentCentered.get(1, 1) / momentObject.get(0, 0);
146 double n20 = momentCentered.get(2, 0) / momentObject.get(0, 0);
147 double n02 = momentCentered.get(0, 2) / momentObject.get(0, 0);
148 double n10 = momentCentered.get(1, 0) / momentObject.get(0, 0);
149 double n01 = momentCentered.get(0, 1) / momentObject.get(0, 0);
150 double n03 = momentCentered.get(0, 3) / momentObject.get(0, 0);
151
152 double Xg = momentGravity.getXg();
153 double Yg = momentGravity.getYg();
154
155 double An = momentSurfaceNormalized.get()[0];
156
157 double Xn = An * Xg;
158 double Yn = An * Yg;
159
160 double Xnvx, Xnvy, Xnvz, Xnwx, Xnwy;
161 double Ynvx, Ynvy, Ynvz, Ynwx, Ynwy;
162
163 if (momentObject.getType() == vpMomentObject::DISCRETE) {
164 double a = momentCentered.get(2, 0) + momentCentered.get(0, 2);
165
166 double e11 = momentCentered.get(1, 1) / a;
167 double e12 = momentCentered.get(1, 2) / a;
168 double e21 = momentCentered.get(2, 1) / a;
169 double e30 = momentCentered.get(3, 0) / a;
170
171 double NA = n20 + n02;
172
173 Xnvx = B * Xn * e11 - Yn * B - An * C - n02 * A * Xn / NA;
174 Xnvy = A * Xn * e11 + n02 * B * Xn / NA;
175
176 Xnwx = An * e11 * NA + Yn * n10 - Xn * Xg * e11 + Xn * n01 + Xn * n10 * e11 - Xn * e21 +
177 (-Xn * n03 + (Xn * n01 - Yn * Xg) * n02) / NA;
178 Xnwy = -An * NA + Xn * e12 + Xn * Xg - An + e11 * Xg * Yn - Xn * n01 * e11 - 2 * Xn * n10 + Xn * e30 + n02 * An +
179 (-Xn * Xg + Xn * n10) * n02 / NA;
180
181 Ynvx = (Yn - n02 * Yn / NA) * A + Yn * e11 * B;
182 Ynvy = (-Xn + e11 * Yn) * A + (-Yn + n02 * Yn / NA) * B - An * C;
183
184 Ynwx = n02 * An + Yn * n10 * e11 - e11 * Xg * Yn + An - Yn * e21 + Yn * n01 +
185 (-Yn * n03 + (Yn * n01 - Yn * Yg) * n02) / NA;
186 Ynwy = -An * e11 * NA + Yn * e11 * Yg - Yn * n01 * e11 + Yn * Xg + Yn * e12 + Yn * e30 - Xn * n01 - 2 * Yn * n10 +
187 (Yn * n10 - Yn * Xg) * n02 / NA;
188
189 } else {
190 Xnvx = -An * C - A * Xn - Yn * B;
191 Xnvy = (0.5) * B * Xn;
192
193 Xnwx = (0.5 * (8. * n10 - Xg)) * Yn + 4. * An * n11 + 4. * n01 * Xn;
194 Xnwy = (0.5 * (-2. - 8. * n20)) * An + (0.5) * Xn * (-8. * n10 + Xg);
195
196 Ynvx = (0.5) * A * Yn;
197 Ynvy = -(0.5) * B * Yn - C * An - A * Xn;
198
199 Ynwx = (0.5) * Yn * (8. * n01 - Yg) + (.5 * (2. + 8. * n02)) * An;
200 Ynwy = (0.5 * (-8. * n10 + Xg)) * Yn - 4. * An * n11 - 4. * n01 * Xn;
201 }
202
203 Ynvz = -A * Ynwy + (-An + Ynwx) * B;
204 Xnvz = -A * An - A * Xnwy + B * Xnwx;
205
206 int VX = 0;
207 int VY = 1;
208 int VZ = 2;
209 int WX = 3;
210 int WY = 4;
211 int WZ = 5;
212
213 interaction_matrices[0][0][VX] = Xnvx;
214 interaction_matrices[0][0][VY] = Xnvy;
215 interaction_matrices[0][0][VZ] = Xnvz;
216
217 interaction_matrices[0][0][WX] = Xnwx;
218 interaction_matrices[0][0][WY] = Xnwy;
219 interaction_matrices[0][0][WZ] = Yn;
220
221 interaction_matrices[1][0][VX] = Ynvx;
222 interaction_matrices[1][0][VY] = Ynvy;
223 interaction_matrices[1][0][VZ] = Ynvz;
224
225 interaction_matrices[1][0][WX] = Ynwx;
226 interaction_matrices[1][0][WY] = Ynwy;
227 interaction_matrices[1][0][WZ] = -Xn;
228}
229#endif
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ notInitialized
Used to indicate that a parameter is not initialized.
Definition: vpException.h:98
Functionality computation for normalized surface moment feature. Computes the interaction matrix asso...
vpFeatureMoment & get(const char *type, bool &found)
Functionality computation for gravity center moment feature. Computes the interaction matrix associat...
std::vector< vpMatrix > interaction_matrices
vpFeatureMomentDatabase * featureMomentsDataBase
vpMomentDatabase & moments
vpMatrix interaction(unsigned int select=FEATURE_ALL)
const vpMoment * moment
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.
const std::vector< double > & get() const
Class for generic objects.
const std::vector< double > & get() const
vpObjectType getType() const
const vpMomentObject & getObject() const
Definition: vpMoment.h:150
const std::vector< double > & get() const
Definition: vpMoment.h:155