Visual Servoing Platform version 3.5.0
vpFeatureEllipse.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 * 2D ellipse visual feature.
33 *
34 * Authors:
35 * Eric Marchand
36 *
37 *****************************************************************************/
38
44#include <visp3/visual_features/vpBasicFeature.h>
45#include <visp3/visual_features/vpFeatureEllipse.h>
46
47// Exception
48#include <visp3/core/vpException.h>
49#include <visp3/visual_features/vpFeatureException.h>
50
51// Debug trace
52#include <visp3/core/vpDebug.h>
53
54// math
55#include <visp3/core/vpMath.h>
56
57#include <visp3/core/vpFeatureDisplay.h>
58
59/*
60
61attributes and members directly related to the vpBasicFeature needs
62other functionalities ar useful but not mandatory
63
64*/
65
67{
68 // feature dimension
69 dim_s = 5;
70 nbParameters = 8;
71
72 // memory allocation
73 s.resize(dim_s);
74 if (flags == NULL)
75 flags = new bool[nbParameters];
76 for (unsigned int i = 0; i < nbParameters; i++)
77 flags[i] = false;
78
79 // default depth values
80 A = B = 0;
81 C = 1;
82}
83
84vpFeatureEllipse::vpFeatureEllipse() : A(0), B(0), C(0) { init(); }
85
88{
89 vpMatrix L;
90
91 L.resize(0, 6);
92
94 for (unsigned int i = 0; i < nbParameters; i++) {
95 if (flags[i] == false) {
96 switch (i) {
97 case 0:
98 vpTRACE("Warning !!! The interaction matrix is computed but x was "
99 "not set yet");
100 break;
101 case 1:
102 vpTRACE("Warning !!! The interaction matrix is computed but y was "
103 "not set yet");
104 break;
105 case 2:
106 vpTRACE("Warning !!! The interaction matrix is computed but n20 "
107 "was not set yet");
108 break;
109 case 3:
110 vpTRACE("Warning !!! The interaction matrix is computed but n11 "
111 "was not set yet");
112 break;
113 case 4:
114 vpTRACE("Warning !!! The interaction matrix is computed but n02 "
115 "was not set yet");
116 break;
117 case 5:
118 vpTRACE("Warning !!! The interaction matrix is computed but A was "
119 "not set yet");
120 break;
121 case 6:
122 vpTRACE("Warning !!! The interaction matrix is computed but B was "
123 "not set yet");
124 break;
125 case 7:
126 vpTRACE("Warning !!! The interaction matrix is computed but C was "
127 "not set yet");
128 break;
129 default:
130 vpTRACE("Problem during the reading of the variable flags");
131 }
132 }
133 }
134 resetFlags();
135 }
136
137 double xc = s[0];
138 double yc = s[1];
139 double n20 = s[2];
140 double n11 = s[3];
141 double n02 = s[4];
142
143 double Zinv = A * xc + B * yc + C;
144
145 if (vpFeatureEllipse::selectX() & select) {
146 vpMatrix H(1, 6);
147 H = 0;
148 // Eq (14) of Chaumette 2004 TRO paper on moments
149 H[0][0] = -Zinv;
150 H[0][1] = 0;
151 H[0][2] = xc * Zinv + 4.0 * (A * n20 + B * n11);
152 H[0][3] = xc * yc + 4.0 * n11;
153 H[0][4] = -1 - vpMath::sqr(xc) - 4.0 * n20;
154 H[0][5] = yc;
155
156 L = vpMatrix::stack(L, H);
157 }
158
159 if (vpFeatureEllipse::selectY() & select) {
160 vpMatrix H(1, 6);
161 H = 0;
162 // Eq (14) of Chaumette 2004 TRO paper on moments
163 H[0][0] = 0;
164 H[0][1] = -Zinv;
165 H[0][2] = yc * Zinv + 4.0 * (A * n11 + B * n02);
166 H[0][3] = 1 + vpMath::sqr(yc) + 4.0 * n02;
167 H[0][4] = -xc * yc - 4.0 * n11;
168 H[0][5] = -xc;
169
170 L = vpMatrix::stack(L, H);
171 }
172
173 if (vpFeatureEllipse::select_n20() & select) {
174 vpMatrix H(1, 6);
175 H = 0;
176 // Eq (26) of Chaumette 2004 TRO paper on moments
177 H[0][0] = -2.0 * (A * n20 + B * n11);
178 H[0][1] = 0;
179 H[0][2] = 2 * ((Zinv + A * xc) * n20 + B * xc * n11);
180 H[0][3] = 2 * (yc * n20 + xc * n11);
181 H[0][4] = -4 * n20 * xc;
182 H[0][5] = 2 * n11;
183
184 L = vpMatrix::stack(L, H);
185 }
186
187 if (vpFeatureEllipse::select_n11() & select) {
188 vpMatrix H(1, 6);
189 H = 0;
190 // Eq (26) of Chaumette 2004 TRO paper on moments
191 H[0][0] = -A * n11 - B * n02;
192 H[0][1] = -A * n20 - B * n11;
193 H[0][2] = A * yc * n20 + (3 * Zinv - C) * n11 + B * xc * n02;
194 H[0][3] = 3 * yc * n11 + xc * n02;
195 H[0][4] = -yc * n20 - 3 * xc * n11;
196 H[0][5] = n02 - n20;
197
198 L = vpMatrix::stack(L, H);
199 }
200
201 if (vpFeatureEllipse::select_n02() & select) {
202 vpMatrix H(1, 6);
203 H = 0;
204 // Eq (26) of Chaumette 2004 TRO paper on moments
205 H[0][0] = 0;
206 H[0][1] = -2 * (A * n11 + B * n02);
207 H[0][2] = 2 * ((Zinv + B * yc) * n02 + A * yc * n11);
208 H[0][3] = 4 * yc * n02;
209 H[0][4] = -2 * (yc * n11 + xc * n02);
210 H[0][5] = -2 * n11;
211 L = vpMatrix::stack(L, H);
212 }
213
214 return L;
215}
216
219vpColVector vpFeatureEllipse::error(const vpBasicFeature &s_star, unsigned int select)
220{
221 vpColVector e(0);
222
223 try {
224 if (vpFeatureEllipse::selectX() & select) {
225 vpColVector ex(1);
226 ex[0] = s[0] - s_star[0];
227
228 e = vpColVector::stack(e, ex);
229 }
230
231 if (vpFeatureEllipse::selectY() & select) {
232 vpColVector ey(1);
233 ey[0] = s[1] - s_star[1];
234 e = vpColVector::stack(e, ey);
235 }
236
237 if (vpFeatureEllipse::select_n20() & select) {
238 vpColVector ex(1);
239 ex[0] = s[2] - s_star[2];
240
241 e = vpColVector::stack(e, ex);
242 }
243
244 if (vpFeatureEllipse::select_n11() & select) {
245 vpColVector ey(1);
246 ey[0] = s[3] - s_star[3];
247 e = vpColVector::stack(e, ey);
248 }
249
250 if (vpFeatureEllipse::select_n02() & select) {
251 vpColVector ey(1);
252 ey[0] = s[4] - s_star[4];
253 e = vpColVector::stack(e, ey);
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,
294 double n02, double a, double b, double c)
295{
296 s[0] = x;
297 s[1] = y;
298 s[2] = n20;
299 s[3] = n11;
300 s[4] = n02;
301
302 this->A = a;
303 this->B = b;
304 this->C = c;
305
306 for (unsigned int i = 0; i < nbParameters; i++)
307 flags[i] = true;
308}
309
311{
312 s[0] = x;
313 flags[0] = true;
314}
315
317{
318 s[1] = y;
319 flags[1] = true;
320}
321
322void vpFeatureEllipse::set_xy(double x, double y)
323{
324 s[0] = x;
325 s[1] = y;
326 for (int i = 0; i < 2; i++)
327 flags[i] = true;
328}
329
330void vpFeatureEllipse::setABC(double a, double b, double c)
331{
332 this->A = a;
333 this->B = b;
334 this->C = c;
335 for (unsigned int i = 5; i < nbParameters; i++)
336 flags[i] = true;
337}
338
345void vpFeatureEllipse::setMoments(double n20, double n11, double n02)
346{
347 s[2] = n20;
348 s[3] = n11;
349 s[4] = n02;
350 for (int i = 2; i < 5; i++)
351 flags[i] = true;
352}
353
354#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
361void vpFeatureEllipse::setMu(double mu20, double mu11, double mu02)
362{
363 setMoments(mu20, mu11, mu02);
364}
365#endif
366
377 unsigned int thickness) const
378{
379 double x = s[0];
380 double y = s[1];
381
382 double n20 = s[2];
383 double n11 = s[3];
384 double n02 = s[4];
385
386 vpFeatureDisplay::displayEllipse(x, y, n20, n11, n02, cam, I, color, thickness);
387}
388
399 unsigned int thickness) const
400{
401 double x = s[0];
402 double y = s[1];
403
404 double n20 = s[2];
405 double n11 = s[3];
406 double n02 = s[4];
407
408 vpFeatureDisplay::displayEllipse(x, y, n20, n11, n02, cam, I, color, thickness);
409}
410
413{
414 vpFeatureEllipse *feature = new vpFeatureEllipse;
415 return feature;
416}
417
421unsigned int vpFeatureEllipse::selectX() { return FEATURE_LINE[0]; }
425unsigned int vpFeatureEllipse::selectY() { return FEATURE_LINE[1]; }
426
431unsigned int vpFeatureEllipse::select_n20() { return FEATURE_LINE[2]; }
436unsigned int vpFeatureEllipse::select_n11() { return FEATURE_LINE[3]; }
441unsigned int vpFeatureEllipse::select_n02() { return FEATURE_LINE[4]; }
442
443#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
450vp_deprecated unsigned int vpFeatureEllipse::selectMu20() { return FEATURE_LINE[2]; }
457vp_deprecated unsigned int vpFeatureEllipse::selectMu11() { return FEATURE_LINE[3]; }
464vp_deprecated unsigned int vpFeatureEllipse::selectMu02() { return FEATURE_LINE[4]; }
465#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.
Definition: vpColVector.h:131
void stack(double d)
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:158
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
void set_x(double x)
vpFeatureEllipse * duplicate() const
Feature duplication.
static unsigned int selectX()
void init()
Default initialization.
void set_y(double y)
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)
static double sqr(double x)
Definition: vpMath.h:116
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
void stack(const vpMatrix &A)
Definition: vpMatrix.cpp:5879
#define vpTRACE
Definition: vpDebug.h:416