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