Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpFeatureEllipse.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 * 2D ellipse visual feature.
33 *
34*****************************************************************************/
35
41#include <visp3/visual_features/vpBasicFeature.h>
42#include <visp3/visual_features/vpFeatureEllipse.h>
43
44// Exception
45#include <visp3/core/vpException.h>
46#include <visp3/visual_features/vpFeatureException.h>
47
48// Debug trace
49#include <visp3/core/vpDebug.h>
50
51// math
52#include <visp3/core/vpMath.h>
53
54#include <visp3/core/vpFeatureDisplay.h>
55
56/*
57
58attributes and members directly related to the vpBasicFeature needs
59other functionalities ar useful but not mandatory
60
61*/
62
64{
65 // feature dimension
66 dim_s = 5;
67 nbParameters = 8;
68
69 // memory allocation
70 s.resize(dim_s);
71 if (flags == NULL)
72 flags = new bool[nbParameters];
73 for (unsigned int i = 0; i < nbParameters; i++)
74 flags[i] = false;
75
76 // default depth values
77 A = B = 0;
78 C = 1;
79}
80
81vpFeatureEllipse::vpFeatureEllipse() : A(0), B(0), C(0) { init(); }
82vpFeatureEllipse::vpFeatureEllipse(double x, double y, double n20, double n11, double n02) { this->buildFrom(x, y, n20, n11, n02); }
83
84
87{
88 vpMatrix L;
89
90 L.resize(0, 6);
91
93 for (unsigned int i = 0; i < nbParameters; i++) {
94 if (flags[i] == false) {
95 switch (i) {
96 case 0:
97 vpTRACE("Warning !!! The interaction matrix is computed but x was "
98 "not set yet");
99 break;
100 case 1:
101 vpTRACE("Warning !!! The interaction matrix is computed but y was "
102 "not set yet");
103 break;
104 case 2:
105 vpTRACE("Warning !!! The interaction matrix is computed but n20 "
106 "was not set yet");
107 break;
108 case 3:
109 vpTRACE("Warning !!! The interaction matrix is computed but n11 "
110 "was not set yet");
111 break;
112 case 4:
113 vpTRACE("Warning !!! The interaction matrix is computed but n02 "
114 "was not set yet");
115 break;
116 case 5:
117 vpTRACE("Warning !!! The interaction matrix is computed but A was "
118 "not set yet");
119 break;
120 case 6:
121 vpTRACE("Warning !!! The interaction matrix is computed but B was "
122 "not set yet");
123 break;
124 case 7:
125 vpTRACE("Warning !!! The interaction matrix is computed but C was "
126 "not set yet");
127 break;
128 default:
129 vpTRACE("Problem during the reading of the variable flags");
130 }
131 }
132 }
133 resetFlags();
134 }
135
136 double xc = s[0];
137 double yc = s[1];
138 double n20 = s[2];
139 double n11 = s[3];
140 double n02 = s[4];
141
142 double Zinv = A * xc + B * yc + C;
143
144 if (vpFeatureEllipse::selectX() & select) {
145 vpMatrix H(1, 6);
146 H = 0;
147 // Eq (14) of Chaumette 2004 TRO paper on moments
148 H[0][0] = -Zinv;
149 H[0][1] = 0;
150 H[0][2] = xc * Zinv + 4.0 * (A * n20 + B * n11);
151 H[0][3] = xc * yc + 4.0 * n11;
152 H[0][4] = -1 - vpMath::sqr(xc) - 4.0 * n20;
153 H[0][5] = yc;
154
155 L = vpMatrix::stack(L, H);
156 }
157
158 if (vpFeatureEllipse::selectY() & select) {
159 vpMatrix H(1, 6);
160 H = 0;
161 // Eq (14) of Chaumette 2004 TRO paper on moments
162 H[0][0] = 0;
163 H[0][1] = -Zinv;
164 H[0][2] = yc * Zinv + 4.0 * (A * n11 + B * n02);
165 H[0][3] = 1 + vpMath::sqr(yc) + 4.0 * n02;
166 H[0][4] = -xc * yc - 4.0 * n11;
167 H[0][5] = -xc;
168
169 L = vpMatrix::stack(L, H);
170 }
171
172 if (vpFeatureEllipse::select_n20() & select) {
173 vpMatrix H(1, 6);
174 H = 0;
175 // Eq (26) of Chaumette 2004 TRO paper on moments
176 H[0][0] = -2.0 * (A * n20 + B * n11);
177 H[0][1] = 0;
178 H[0][2] = 2 * ((Zinv + A * xc) * n20 + B * xc * n11);
179 H[0][3] = 2 * (yc * n20 + xc * n11);
180 H[0][4] = -4 * n20 * xc;
181 H[0][5] = 2 * n11;
182
183 L = vpMatrix::stack(L, H);
184 }
185
186 if (vpFeatureEllipse::select_n11() & select) {
187 vpMatrix H(1, 6);
188 H = 0;
189 // Eq (26) of Chaumette 2004 TRO paper on moments
190 H[0][0] = -A * n11 - B * n02;
191 H[0][1] = -A * n20 - B * n11;
192 H[0][2] = A * yc * n20 + (3 * Zinv - C) * n11 + B * xc * n02;
193 H[0][3] = 3 * yc * n11 + xc * n02;
194 H[0][4] = -yc * n20 - 3 * xc * n11;
195 H[0][5] = n02 - n20;
196
197 L = vpMatrix::stack(L, H);
198 }
199
200 if (vpFeatureEllipse::select_n02() & select) {
201 vpMatrix H(1, 6);
202 H = 0;
203 // Eq (26) of Chaumette 2004 TRO paper on moments
204 H[0][0] = 0;
205 H[0][1] = -2 * (A * n11 + B * n02);
206 H[0][2] = 2 * ((Zinv + B * yc) * n02 + A * yc * n11);
207 H[0][3] = 4 * yc * n02;
208 H[0][4] = -2 * (yc * n11 + xc * n02);
209 H[0][5] = -2 * n11;
210 L = vpMatrix::stack(L, H);
211 }
212
213 return L;
214}
215
218vpColVector vpFeatureEllipse::error(const vpBasicFeature &s_star, unsigned int select)
219{
220 vpColVector e(0);
221
222 try {
223 if (vpFeatureEllipse::selectX() & select) {
224 vpColVector ex(1);
225 ex[0] = s[0] - s_star[0];
226
227 e = vpColVector::stack(e, ex);
228 }
229
230 if (vpFeatureEllipse::selectY() & select) {
231 vpColVector ey(1);
232 ey[0] = s[1] - s_star[1];
233 e = vpColVector::stack(e, ey);
234 }
235
236 if (vpFeatureEllipse::select_n20() & select) {
237 vpColVector ex(1);
238 ex[0] = s[2] - s_star[2];
239
240 e = vpColVector::stack(e, ex);
241 }
242
243 if (vpFeatureEllipse::select_n11() & select) {
244 vpColVector ey(1);
245 ey[0] = s[3] - s_star[3];
246 e = vpColVector::stack(e, ey);
247 }
248
249 if (vpFeatureEllipse::select_n02() & select) {
250 vpColVector ey(1);
251 ey[0] = s[4] - s_star[4];
252 e = vpColVector::stack(e, ey);
253 }
254
255 }
256 catch (...) {
257 throw;
258 }
259
260 return e;
261}
262
263void vpFeatureEllipse::print(unsigned int select) const
264{
265
266 std::cout << "Ellipse: " << std::endl;
267 if (vpFeatureEllipse::selectX() & select)
268 std::cout << " x=" << s[0] << std::endl;
269 ;
270 if (vpFeatureEllipse::selectY() & select)
271 std::cout << " y=" << s[1] << std::endl;
272 if (vpFeatureEllipse::select_n20() & select)
273 std::cout << " n20=" << s[2] << std::endl;
274 if (vpFeatureEllipse::select_n11() & select)
275 std::cout << " n11=" << s[3] << std::endl;
276 if (vpFeatureEllipse::select_n02() & select)
277 std::cout << " n02=" << s[4] << std::endl;
278 std::cout << "A = " << A << " B = " << B << " C = " << C << std::endl;
279}
280
281void vpFeatureEllipse::buildFrom(double x, double y, double n20, double n11, double n02)
282{
283 s[0] = x;
284 s[1] = y;
285 s[2] = n20;
286 s[3] = n11;
287 s[4] = n02;
288
289 for (int i = 0; i < 5; i++)
290 flags[i] = true;
291}
292
293void vpFeatureEllipse::buildFrom(double x, double y, double n20, double n11, double n02, double a, double b, double c)
294{
295 s[0] = x;
296 s[1] = y;
297 s[2] = n20;
298 s[3] = n11;
299 s[4] = n02;
300
301 this->A = a;
302 this->B = b;
303 this->C = c;
304
305 for (unsigned int i = 0; i < nbParameters; i++)
306 flags[i] = true;
307}
308
310{
311 s[0] = x;
312 flags[0] = true;
313}
314
316{
317 s[1] = y;
318 flags[1] = true;
319}
320
321void vpFeatureEllipse::set_xy(double x, double y)
322{
323 s[0] = x;
324 s[1] = y;
325 for (int i = 0; i < 2; i++)
326 flags[i] = true;
327}
328
329void vpFeatureEllipse::setABC(double a, double b, double c)
330{
331 this->A = a;
332 this->B = b;
333 this->C = c;
334 for (unsigned int i = 5; i < nbParameters; i++)
335 flags[i] = true;
336}
337
344void vpFeatureEllipse::setMoments(double n20, double n11, double n02)
345{
346 s[2] = n20;
347 s[3] = n11;
348 s[4] = n02;
349 for (int i = 2; i < 5; i++)
350 flags[i] = true;
351}
352
353#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
360void vpFeatureEllipse::setMu(double mu20, double mu11, double mu02) { setMoments(mu20, mu11, mu02); }
361#endif
362
373 unsigned int thickness) const
374{
375 double x = s[0];
376 double y = s[1];
377
378 double n20 = s[2];
379 double n11 = s[3];
380 double n02 = s[4];
381
382 vpFeatureDisplay::displayEllipse(x, y, n20, n11, n02, cam, I, color, thickness);
383}
384
395 unsigned int thickness) const
396{
397 double x = s[0];
398 double y = s[1];
399
400 double n20 = s[2];
401 double n11 = s[3];
402 double n02 = s[4];
403
404 vpFeatureDisplay::displayEllipse(x, y, n20, n11, n02, cam, I, color, thickness);
405}
406
409{
410 vpFeatureEllipse *feature = new vpFeatureEllipse;
411 return feature;
412}
413
417unsigned int vpFeatureEllipse::selectX() { return FEATURE_LINE[0]; }
421unsigned int vpFeatureEllipse::selectY() { return FEATURE_LINE[1]; }
422
427unsigned int vpFeatureEllipse::select_n20() { return FEATURE_LINE[2]; }
432unsigned int vpFeatureEllipse::select_n11() { return FEATURE_LINE[3]; }
437unsigned int vpFeatureEllipse::select_n02() { return FEATURE_LINE[4]; }
438
439#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
446vp_deprecated unsigned int vpFeatureEllipse::selectMu20() { return FEATURE_LINE[2]; }
453vp_deprecated unsigned int vpFeatureEllipse::selectMu11() { return FEATURE_LINE[3]; }
460vp_deprecated unsigned int vpFeatureEllipse::selectMu02() { return FEATURE_LINE[4]; }
461#endif
class that defines what is a visual feature
vpColVector s
State of the visual feature.
static const unsigned int FEATURE_LINE[32]
unsigned int nbParameters
Number of parameters needed to compute the interaction matrix.
unsigned int dim_s
Dimension of the visual feature.
vpBasicFeatureDeallocatorType deallocate
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
void stack(double d)
void resize(unsigned int i, bool flagNullify=true)
Class to define RGB colors available for display functionalities.
Definition vpColor.h:152
static void displayEllipse(double x, double y, double n20, double n11, double n02, const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1)
Class that defines 2D ellipse visual feature.
void print(unsigned int select=FEATURE_ALL) const
print the name of the feature
vpFeatureEllipse * duplicate() const
Feature duplication.
static unsigned int selectX()
void init()
Default initialization.
vpFeatureEllipse()
Default constructor.
static unsigned int selectY()
void setABC(double A, double B, double C)
vp_deprecated void setMu(double mu20, double mu11, double mu02)
vpMatrix interaction(unsigned int select=FEATURE_ALL)
compute the interaction matrix from a subset a the possible features
static unsigned int select_n20()
void setMoments(double n20, double n11, double n02)
void display(const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1) const
vpColVector error(const vpBasicFeature &s_star, unsigned int select=FEATURE_ALL)
static unsigned int select_n02()
static vp_deprecated unsigned int selectMu20()
static unsigned int select_n11()
void set_xy(double x, double y)
static vp_deprecated unsigned int selectMu11()
static vp_deprecated unsigned int selectMu02()
void buildFrom(double x, double y, double n20, double n11, double n02)
Definition of the vpImage class member functions.
Definition vpImage.h:135
static double sqr(double x)
Definition vpMath.h:124
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
void stack(const vpMatrix &A)
#define vpTRACE
Definition vpDebug.h:411