Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpFeatureSegment.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 * Segment visual feature.
33 *
34 * Authors:
35 * Filip Novotny
36 *
37*****************************************************************************/
38
39#include <cmath>
40#include <visp3/core/vpDisplay.h>
41#include <visp3/core/vpImagePoint.h>
42#include <visp3/core/vpMath.h>
43#include <visp3/core/vpMeterPixelConversion.h>
44#include <visp3/visual_features/vpBasicFeature.h>
45#include <visp3/visual_features/vpFeatureSegment.h>
46
47// Exception
48#include <visp3/core/vpException.h>
49
50// Debug trace
51#include <visp3/core/vpDebug.h>
52
65{
66 // feature dimension
67 dim_s = 4;
68 nbParameters = 6;
69
70 // memory allocation
71 s.resize(dim_s);
72 if (flags == NULL)
73 flags = new bool[nbParameters];
74 for (unsigned int i = 0; i < nbParameters; i++)
75 flags[i] = false;
76}
77
86 : xc_(0), yc_(0), l_(0), alpha_(0), Z1_(0), Z2_(0), cos_a_(0), sin_a_(0), normalized_(normalized)
87{
88 init();
89}
90
188{
189
190 vpMatrix L;
191 L.resize(0, 6);
192
194 for (unsigned int i = 0; i < nbParameters; i++) {
195 if (flags[i] == false) {
196 switch (i) {
197 case 0:
198 vpTRACE("Warning !!! The interaction matrix is computed but xc "
199 "was not set yet");
200 break;
201 case 1:
202 vpTRACE("Warning !!! The interaction matrix is computed but Yc "
203 "was not set yet");
204 break;
205 case 2:
206 vpTRACE("Warning !!! The interaction matrix is computed but l was "
207 "not set yet");
208 break;
209 case 3:
210 vpTRACE("Warning !!! The interaction matrix is computed but alpha "
211 "was not set yet");
212 break;
213 case 4:
214 vpTRACE("Warning !!! The interaction matrix is computed but Z1 "
215 "was not set yet");
216 break;
217 case 5:
218 vpTRACE("Warning !!! The interaction matrix is computed but Z2 "
219 "was not set yet");
220 break;
221 default:
222 vpTRACE("Problem during the reading of the variable flags");
223 }
224 }
225 }
226 }
227
228 // This version is a simplification
229 double lambda1 = (Z1_ - Z2_) / (Z1_ * Z2_); // -l * lambda
230 double lambda2 = (Z1_ + Z2_) / (2 * Z1_ * Z2_); // 1/Zm
231
232 if (normalized_) {
233 // here var xc_ contains xc/l, yc_ contains yc/l and l_ contains 1/l
234 double xn = xc_;
235 double yn = yc_;
236 double ln = l_;
237 double lambda = -lambda1 * ln;
238 double Zn_inv = lambda2 * ln;
239 double lc = cos_a_ / ln;
240 double ls = sin_a_ / ln;
241 double xnalpha = xn * cos_a_ + yn * sin_a_;
242 double lnc = cos_a_ * ln;
243 double lns = sin_a_ * ln;
244
245 if (vpFeatureSegment::selectXc() & select) {
246 vpMatrix Lxn(1, 6);
247 Lxn[0][0] = -Zn_inv + lambda * xn * cos_a_;
248 Lxn[0][1] = lambda * xn * sin_a_;
249 Lxn[0][2] = lambda1 * (xn * xnalpha - cos_a_ / 4.);
250 Lxn[0][3] = sin_a_ * cos_a_ / 4 / ln - xn * xnalpha * sin_a_ / ln;
251 Lxn[0][4] = -ln * (1. + lc * lc / 4.) + xn * xnalpha * cos_a_ / ln;
252 Lxn[0][5] = yn;
253 L = vpMatrix::stack(L, Lxn);
254 }
255
256 if (vpFeatureSegment::selectYc() & select) {
257 vpMatrix Lyn(1, 6);
258 Lyn[0][0] = lambda * yn * cos_a_;
259 Lyn[0][1] = -Zn_inv + lambda * yn * sin_a_;
260 Lyn[0][2] = lambda1 * (yn * xnalpha - sin_a_ / 4.);
261 Lyn[0][3] = ln * (1 + ls * ls / 4.) - yn * xnalpha * sin_a_ / ln;
262 Lyn[0][4] = -sin_a_ * cos_a_ / 4 / ln + yn * xnalpha * cos_a_ / ln;
263 Lyn[0][5] = -xn;
264 L = vpMatrix::stack(L, Lyn);
265 }
266
267 if (vpFeatureSegment::selectL() & select) {
268 vpMatrix Lln(1, 6);
269 Lln[0][0] = lambda * lnc;
270 Lln[0][1] = lambda * lns;
271 Lln[0][2] = -(Zn_inv + lambda * xnalpha);
272 Lln[0][3] = -yn - xnalpha * sin_a_;
273 Lln[0][4] = xn + xnalpha * cos_a_;
274 Lln[0][5] = 0;
275 L = vpMatrix::stack(L, Lln);
276 }
277 if (vpFeatureSegment::selectAlpha() & select) {
278 // We recall that xc_ contains xc/l, yc_ contains yc/l and l_ contains
279 // 1/l
280 vpMatrix Lalpha(1, 6);
281 Lalpha[0][0] = -lambda1 * sin_a_ * l_;
282 Lalpha[0][1] = lambda1 * cos_a_ * l_;
283 Lalpha[0][2] = lambda1 * (xc_ * sin_a_ - yc_ * cos_a_);
284 Lalpha[0][3] = (-xc_ * sin_a_ * sin_a_ + yc_ * cos_a_ * sin_a_) / l_;
285 Lalpha[0][4] = (xc_ * cos_a_ * sin_a_ - yc_ * cos_a_ * cos_a_) / l_;
286 Lalpha[0][5] = -1;
287 L = vpMatrix::stack(L, Lalpha);
288 }
289 } else {
290 if (vpFeatureSegment::selectXc() & select) {
291 vpMatrix Lxc(1, 6);
292 Lxc[0][0] = -lambda2;
293 Lxc[0][1] = 0.;
294 Lxc[0][2] = lambda2 * xc_ - lambda1 * l_ * cos_a_ / 4.;
295 Lxc[0][3] = xc_ * yc_ + l_ * l_ * cos_a_ * sin_a_ / 4.;
296 Lxc[0][4] = -(1 + xc_ * xc_ + l_ * l_ * cos_a_ * cos_a_ / 4.);
297 Lxc[0][5] = yc_;
298 L = vpMatrix::stack(L, Lxc);
299 }
300
301 if (vpFeatureSegment::selectYc() & select) {
302 vpMatrix Lyc(1, 6);
303 Lyc[0][0] = 0.;
304 Lyc[0][1] = -lambda2;
305 Lyc[0][2] = lambda2 * yc_ - lambda1 * l_ * sin_a_ / 4.;
306 Lyc[0][3] = 1 + yc_ * yc_ + l_ * l_ * sin_a_ * sin_a_ / 4.;
307 Lyc[0][4] = -xc_ * yc_ - l_ * l_ * cos_a_ * sin_a_ / 4.;
308 Lyc[0][5] = -xc_;
309 L = vpMatrix::stack(L, Lyc);
310 }
311
312 if (vpFeatureSegment::selectL() & select) {
313 vpMatrix Ll(1, 6);
314 Ll[0][0] = lambda1 * cos_a_;
315 Ll[0][1] = lambda1 * sin_a_;
316 Ll[0][2] = lambda2 * l_ - lambda1 * (xc_ * cos_a_ + yc_ * sin_a_);
317 Ll[0][3] = l_ * (xc_ * cos_a_ * sin_a_ + yc_ * (1 + sin_a_ * sin_a_));
318 Ll[0][4] = -l_ * (xc_ * (1 + cos_a_ * cos_a_) + yc_ * cos_a_ * sin_a_);
319 Ll[0][5] = 0;
320 L = vpMatrix::stack(L, Ll);
321 }
322 if (vpFeatureSegment::selectAlpha() & select) {
323 vpMatrix Lalpha(1, 6);
324 Lalpha[0][0] = -lambda1 * sin_a_ / l_;
325 Lalpha[0][1] = lambda1 * cos_a_ / l_;
326 Lalpha[0][2] = lambda1 * (xc_ * sin_a_ - yc_ * cos_a_) / l_;
327 Lalpha[0][3] = -xc_ * sin_a_ * sin_a_ + yc_ * cos_a_ * sin_a_;
328 Lalpha[0][4] = xc_ * cos_a_ * sin_a_ - yc_ * cos_a_ * cos_a_;
329 Lalpha[0][5] = -1;
330 L = vpMatrix::stack(L, Lalpha);
331 }
332 }
333
334 return L;
335}
336
362vpColVector vpFeatureSegment::error(const vpBasicFeature &s_star, unsigned int select)
363{
364 vpColVector e(0);
365
366 if (vpFeatureSegment::selectXc() & select) {
367 vpColVector exc(1);
368 exc[0] = xc_ - s_star[0];
369 e = vpColVector::stack(e, exc);
370 }
371
372 if (vpFeatureSegment::selectYc() & select) {
373 vpColVector eyc(1);
374 eyc[0] = yc_ - s_star[1];
375 e = vpColVector::stack(e, eyc);
376 }
377
378 if (vpFeatureSegment::selectL() & select) {
379 vpColVector eL(1);
380 eL[0] = l_ - s_star[2];
381 e = vpColVector::stack(e, eL);
382 }
383
384 if (vpFeatureSegment::selectAlpha() & select) {
385 vpColVector eAlpha(1);
386 eAlpha[0] = alpha_ - s_star[3];
387 while (eAlpha[0] < -M_PI)
388 eAlpha[0] += 2 * M_PI;
389 while (eAlpha[0] > M_PI)
390 eAlpha[0] -= 2 * M_PI;
391 e = vpColVector::stack(e, eAlpha);
392 }
393 return e;
394}
395
423void vpFeatureSegment::print(unsigned int select) const
424{
425 std::cout << "vpFeatureSegment: (";
426 if (vpFeatureSegment::selectXc() & select) {
427 if (normalized_)
428 std::cout << "xn = ";
429 else
430 std::cout << "xc = ";
431 std::cout << s[0] << "; ";
432 }
433 if (vpFeatureSegment::selectYc() & select) {
434 if (normalized_)
435 std::cout << "yn = ";
436 else
437 std::cout << "yc = ";
438 std::cout << s[1] << "; ";
439 }
440 if (vpFeatureSegment::selectL() & select) {
441 if (normalized_)
442 std::cout << "ln = ";
443 else
444 std::cout << "l = ";
445 std::cout << s[2] << "; ";
446 }
447 if (vpFeatureSegment::selectAlpha() & select) {
448 std::cout << "alpha = " << vpMath::deg(s[3]) << " deg";
449 }
450 std::cout << ")" << std::endl;
451}
452
464{
465 vpFeatureSegment *feature;
466
467 feature = new vpFeatureSegment(*this);
468 return feature;
469}
470
483 unsigned int thickness) const
484{
485 double l, x, y;
486 if (normalized_) {
487 l = 1. / l_;
488 x = xc_ * l;
489 y = yc_ * l;
490 } else {
491 l = l_;
492 x = xc_;
493 y = yc_;
494 }
495
496 double x1 = x - (l / 2.) * cos_a_;
497 double x2 = x + (l / 2.) * cos_a_;
498
499 double y1 = y - (l / 2.) * sin_a_;
500 double y2 = y + (l / 2.) * sin_a_;
501 vpImagePoint ip1, ip2;
502
503 vpMeterPixelConversion::convertPoint(cam, x1, y1, ip1);
504 vpMeterPixelConversion::convertPoint(cam, x2, y2, ip2);
505 vpDisplay::displayLine(I, ip1, ip2, color, thickness);
506 vpDisplay::displayCircle(I, ip1, 5, color, true);
508}
509
521 unsigned int thickness) const
522{
523 double l, x, y;
524 if (normalized_) {
525 l = 1. / l_;
526 x = xc_ * l;
527 y = yc_ * l;
528 } else {
529 l = l_;
530 x = xc_;
531 y = yc_;
532 }
533
534 double x1 = x - (l / 2.) * cos_a_;
535 double x2 = x + (l / 2.) * cos_a_;
536
537 double y1 = y - (l / 2.) * sin_a_;
538 double y2 = y + (l / 2.) * sin_a_;
539 vpImagePoint ip1, ip2;
540
541 vpMeterPixelConversion::convertPoint(cam, x1, y1, ip1);
542 vpMeterPixelConversion::convertPoint(cam, x2, y2, ip2);
543 vpDisplay::displayLine(I, ip1, ip2, color, thickness);
544 vpDisplay::displayCircle(I, ip1, 5, vpColor::cyan, true);
546}
547
565void vpFeatureSegment::buildFrom(double x1, double y1, double Z1, double x2, double y2, double Z2)
566{
567 double l = sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2));
568 double x_c = (x1 + x2) / 2.;
569 double y_c = (y1 + y2) / 2.;
570 double alpha = atan2(y1 - y2, x1 - x2);
571
572 if (normalized_) {
573 setXc(x_c / l);
574 setYc(y_c / l);
575 setL(1 / l);
576 setAlpha(alpha);
577
578 setZ1(Z1);
579 setZ2(Z2);
580 } else {
581 setXc(x_c);
582 setYc(y_c);
583 setL(l);
584 setAlpha(alpha);
585
586 setZ1(Z1);
587 setZ2(Z2);
588 }
589}
590
614unsigned int vpFeatureSegment::selectXc() { return FEATURE_LINE[0]; }
615
639unsigned int vpFeatureSegment::selectYc() { return FEATURE_LINE[1]; }
640
664unsigned int vpFeatureSegment::selectL() { return FEATURE_LINE[2]; }
665
689unsigned int vpFeatureSegment::selectAlpha() { return FEATURE_LINE[3]; }
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 const vpColor cyan
Definition vpColor.h:220
static const vpColor yellow
Definition vpColor.h:219
static void displayCircle(const vpImage< unsigned char > &I, const vpImageCircle &circle, const vpColor &color, bool fill=false, unsigned int thickness=1)
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
Class that defines a 2D segment visual features. This class allow to consider two sets of visual feat...
void print(unsigned int select=FEATURE_ALL) const
static unsigned int selectAlpha()
void buildFrom(double x1, double y1, double Z1, double x2, double y2, double Z2)
static unsigned int selectXc()
void setAlpha(double val)
static unsigned int selectYc()
void setL(double val)
void setZ2(double val)
void display(const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1) const
vpFeatureSegment * duplicate() const
Feature duplication.
vpFeatureSegment(bool normalized=false)
void setZ1(double val)
vpMatrix interaction(unsigned int select=FEATURE_ALL)
void setXc(double val)
static unsigned int selectL()
void setYc(double val)
vpColVector error(const vpBasicFeature &s_star, unsigned int select=FEATURE_ALL)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition of the vpImage class member functions.
Definition vpImage.h:135
static double deg(double rad)
Definition vpMath.h:106
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
void stack(const vpMatrix &A)
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
#define vpTRACE
Definition vpDebug.h:411