Visual Servoing Platform  version 3.5.1 under development (2023-03-14)
vpImageDraw.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  * Drawing functions.
33  *
34  *****************************************************************************/
35 // Contains code from:
36 /*
37  * Simd Library (http://ermig1979.github.io/Simd).
38  *
39  * Copyright (c) 2011-2017 Yermalayeu Ihar.
40  *
41  * Permission is hereby granted, free of charge, to any person obtaining a copy
42  * of this software and associated documentation files (the "Software"), to deal
43  * in the Software without restriction, including without limitation the rights
44  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
45  * copies of the Software, and to permit persons to whom the Software is
46  * furnished to do so, subject to the following conditions:
47  *
48  * The above copyright notice and this permission notice shall be included in
49  * all copies or substantial portions of the Software.
50  *
51  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
52  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
53  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
54  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
55  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
56  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
57  * SOFTWARE.
58  */
59 
60 #include <visp3/core/vpImageDraw.h>
61 #include <visp3/core/vpMeterPixelConversion.h>
62 #include <visp3/core/vpPoint.h>
63 
64 namespace
65 {
66 template <class Type>
67 void DrawLine(vpImage<Type> &canvas, int x1, int y1, int x2, int y2, const Type &color, unsigned int width = 1)
68 {
69  const int w = static_cast<int>(canvas.getWidth()) - 1;
70  const int h = static_cast<int>(canvas.getHeight()) - 1;
71 
72  if (x1 < 0 || y1 < 0 || (x1 - w) > 0 || (y1 - h) > 0 || x2 < 0 || y2 < 0 || (x2 - w) > 0 || (y2 - h) > 0) {
73  if ((x1 < 0 && x2 < 0) || (y1 < 0 && y2 < 0) || ((x1 - w) > 0 && (x2 - w) > 0) || ((y1 - h) > 0 && (y2 - h) > 0)) {
74  return;
75  }
76 
77  if (y1 == y2) {
78  x1 = std::min<int>(std::max<int>(x1, 0), w);
79  x2 = std::min<int>(std::max<int>(x2, 0), w);
80  } else if (x1 == x2) {
81  y1 = std::min<int>(std::max<int>(y1, 0), h);
82  y2 = std::min<int>(std::max<int>(y2, 0), h);
83  } else {
84  int x0 = (x1 * y2 - y1 * x2) / (y2 - y1);
85  int y0 = (y1 * x2 - x1 * y2) / (x2 - x1);
86  int xh = (x1 * y2 - y1 * x2 + h * (x2 - x1)) / (y2 - y1);
87  int yw = (y1 * x2 - x1 * y2 + w * (y2 - y1)) / (x2 - x1);
88 
89  if (x1 < 0) {
90  x1 = 0;
91  y1 = y0;
92  }
93  if (x2 < 0) {
94  x2 = 0;
95  y2 = y0;
96  }
97  if (x1 > w) {
98  x1 = w;
99  y1 = yw;
100  }
101  if (x2 > w) {
102  x2 = w;
103  y2 = yw;
104  }
105  if ((y1 < 0 && y2 < 0) || (y1 > h && y2 > h)) {
106  return;
107  }
108 
109  if (y1 < 0) {
110  x1 = x0;
111  y1 = 0;
112  }
113  if (y2 < 0) {
114  x2 = x0;
115  y2 = 0;
116  }
117 
118  if (y1 > h) {
119  x1 = xh;
120  y1 = h;
121  }
122  if (y2 > h) {
123  x2 = xh;
124  y2 = h;
125  }
126  }
127  }
128 
129  const bool inverse = ((std::abs(y2 - y1) - std::abs(x2 - x1)) > 0);
130  if (inverse) {
131  std::swap(x1, y1);
132  std::swap(x2, y2);
133  }
134 
135  if (x1 > x2) {
136  std::swap(x1, x2);
137  std::swap(y1, y2);
138  }
139 
140  const double dx = x2 - x1;
141  const double dy = static_cast<double>(std::abs(y2 - y1));
142 
143  double error = dx / 2.0f;
144  const int ystep = (y1 < y2) ? 1 : -1;
145  int y0 = y1 - static_cast<int>(width) / 2;
146 
147  for (int x = x1; x <= x2; x++) {
148  for (int i = 0; i < static_cast<int>(width); i++) {
149  int y = y0 + i;
150  if (y >= 0) {
151  if (inverse) {
152  if (y < w) {
153  canvas[x][y] = color;
154  }
155  } else {
156  if (y < h) {
157  canvas[y][x] = color;
158  }
159  }
160  }
161  }
162 
163  error -= dy;
164  if (error < 0) {
165  y0 += ystep;
166  error += dx;
167  }
168  }
169 }
170 
171 template <class Type>
172 void DrawCircle(vpImage<Type> &canvas, const vpImagePoint &center, int radius, const Type &color,
173  unsigned int width = 1)
174 {
175  const size_t n = 8 * std::max(static_cast<size_t>(1), static_cast<size_t>(::pow(radius, 0.5)));
176  double px = 0, py = 0, da = 2 * M_PI / n;
177  for (size_t i = 0; i <= n; i++) {
178  double a = i * da;
179  double cx = radius * ::cos(a) + center.get_u();
180  double cy = radius * ::sin(a) + center.get_v();
181  if (i > 0) {
182  DrawLine(canvas, static_cast<int>(cx), static_cast<int>(cy), static_cast<int>(px), static_cast<int>(py), color,
183  width);
184  }
185  px = cx;
186  py = cy;
187  }
188 }
189 
190 template <class Type> void DrawFilledRectangle(vpImage<Type> &canvas, vpRect rect, const Type &color)
191 {
192  rect &= vpRect(0, 0, canvas.getWidth(), canvas.getHeight());
193  for (int row = static_cast<int>(rect.getTop()); row < static_cast<int>(rect.getBottom()); row++) {
194  Type *dst = canvas[row];
195  for (int col = static_cast<int>(rect.getLeft()); col < static_cast<int>(rect.getRight()); col++) {
196  dst[col] = color;
197  }
198  }
199 }
200 
201 template <class Type>
202 void DrawPolygon(vpImage<Type> &canvas, const std::vector<vpImagePoint> &polygon, const Type &color,
203  unsigned int width = 1, bool closed = true)
204 {
205  if (closed) {
206  for (size_t i = 0; i < polygon.size(); i++) {
207  const vpImagePoint &p1 = (i ? polygon[i - 1] : polygon.back()), p2 = polygon[i];
208  DrawLine(canvas, static_cast<int>(p1.get_u()), static_cast<int>(p1.get_v()), static_cast<int>(p2.get_u()),
209  static_cast<int>(p2.get_v()), color, width);
210  }
211  } else {
212  for (size_t i = 1; i < polygon.size(); i++) {
213  DrawLine(canvas, static_cast<int>(polygon[i - 1].get_u()), static_cast<int>(polygon[i - 1].get_v()),
214  static_cast<int>(polygon[i].get_u()), static_cast<int>(polygon[i].get_v()), color, width);
215  }
216  }
217 }
218 
219 template <class Type>
220 void DrawRectangle(vpImage<Type> &canvas, const vpRect &rect, const Type &color, unsigned int width = 1)
221 {
222  DrawLine(canvas, static_cast<int>(rect.getLeft()), static_cast<int>(rect.getTop()), static_cast<int>(rect.getRight()),
223  static_cast<int>(rect.getTop()), color, width);
224  DrawLine(canvas, static_cast<int>(rect.getRight()), static_cast<int>(rect.getTop()),
225  static_cast<int>(rect.getRight()), static_cast<int>(rect.getBottom()), color, width);
226  DrawLine(canvas, static_cast<int>(rect.getRight()), static_cast<int>(rect.getBottom()),
227  static_cast<int>(rect.getLeft()), static_cast<int>(rect.getBottom()), color, width);
228  DrawLine(canvas, static_cast<int>(rect.getLeft()), static_cast<int>(rect.getBottom()),
229  static_cast<int>(rect.getLeft()), static_cast<int>(rect.getTop()), color, width);
230 }
231 } // namespace
232 
242  unsigned char color, unsigned int w, unsigned int h, unsigned int thickness)
243 {
244  double a = ip2.get_i() - ip1.get_i();
245  double b = ip2.get_j() - ip1.get_j();
246  double lg = sqrt(vpMath::sqr(a) + vpMath::sqr(b));
247 
248  if ((std::fabs(a) <= std::numeric_limits<double>::epsilon()) &&
249  (std::fabs(b) <= std::numeric_limits<double>::epsilon())) {
250  // DisplayCrossLarge(i1,j1,3,col) ;
251  } else {
252  a /= lg;
253  b /= lg;
254 
255  vpImagePoint ip3;
256  ip3.set_i(ip2.get_i() - w * a);
257  ip3.set_j(ip2.get_j() - w * b);
258 
259  vpImagePoint ip4;
260  ip4.set_i(ip3.get_i() - b * h);
261  ip4.set_j(ip3.get_j() + a * h);
262 
263  if (lg > 2 * vpImagePoint::distance(ip2, ip4)) {
264  drawLine(I, ip2, ip4, color, thickness);
265  }
266 
267  ip4.set_i(ip3.get_i() + b * h);
268  ip4.set_j(ip3.get_j() - a * h);
269 
270  if (lg > 2 * vpImagePoint::distance(ip2, ip4)) {
271  drawLine(I, ip2, ip4, color, thickness);
272  }
273 
274  drawLine(I, ip1, ip2, color, thickness);
275  }
276 }
277 
286 void vpImageDraw::drawArrow(vpImage<vpRGBa> &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color,
287  unsigned int w, unsigned int h, unsigned int thickness)
288 {
289  double a = ip2.get_i() - ip1.get_i();
290  double b = ip2.get_j() - ip1.get_j();
291  double lg = sqrt(vpMath::sqr(a) + vpMath::sqr(b));
292 
293  if ((std::fabs(a) <= std::numeric_limits<double>::epsilon()) &&
294  (std::fabs(b) <= std::numeric_limits<double>::epsilon())) {
295  // DisplayCrossLarge(i1,j1,3,col) ;
296  } else {
297  a /= lg;
298  b /= lg;
299 
300  vpImagePoint ip3;
301  ip3.set_i(ip2.get_i() - w * a);
302  ip3.set_j(ip2.get_j() - w * b);
303 
304  vpImagePoint ip4;
305  ip4.set_i(ip3.get_i() - b * h);
306  ip4.set_j(ip3.get_j() + a * h);
307 
308  if (lg > 2 * vpImagePoint::distance(ip2, ip4)) {
309  drawLine(I, ip2, ip4, color, thickness);
310  }
311 
312  ip4.set_i(ip3.get_i() + b * h);
313  ip4.set_j(ip3.get_j() - a * h);
314 
315  if (lg > 2 * vpImagePoint::distance(ip2, ip4)) {
316  drawLine(I, ip2, ip4, color, thickness);
317  }
318 
319  drawLine(I, ip1, ip2, color, thickness);
320  }
321 }
322 
331 void vpImageDraw::drawCircle(vpImage<unsigned char> &I, const vpImagePoint &center, unsigned int radius,
332  unsigned char color, unsigned int thickness)
333 {
334  DrawCircle(I, center, static_cast<int>(radius), color, thickness);
335 }
336 
345 void vpImageDraw::drawCircle(vpImage<vpRGBa> &I, const vpImagePoint &center, unsigned int radius, const vpColor &color,
346  unsigned int thickness)
347 {
348  DrawCircle(I, center, static_cast<int>(radius), vpRGBa(color.R, color.G, color.B), thickness);
349 }
350 
359 void vpImageDraw::drawCross(vpImage<unsigned char> &I, const vpImagePoint &ip, unsigned int size, unsigned char color,
360  unsigned int thickness)
361 {
362  vpImagePoint top, bottom, left, right;
363  top.set_i(ip.get_i() - size / 2);
364  top.set_j(ip.get_j());
365  bottom.set_i(ip.get_i() + size / 2);
366  bottom.set_j(ip.get_j());
367  left.set_i(ip.get_i());
368  left.set_j(ip.get_j() - size / 2);
369  right.set_i(ip.get_i());
370  right.set_j(ip.get_j() + size / 2);
371  drawLine(I, top, bottom, color, thickness);
372  drawLine(I, left, right, color, thickness);
373 }
374 
383 void vpImageDraw::drawCross(vpImage<vpRGBa> &I, const vpImagePoint &ip, unsigned int size, const vpColor &color,
384  unsigned int thickness)
385 {
386  vpImagePoint top, bottom, left, right;
387  top.set_i(ip.get_i() - size / 2);
388  top.set_j(ip.get_j());
389  bottom.set_i(ip.get_i() + size / 2);
390  bottom.set_j(ip.get_j());
391  left.set_i(ip.get_i());
392  left.set_j(ip.get_j() - size / 2);
393  right.set_i(ip.get_i());
394  right.set_j(ip.get_j() + size / 2);
395  drawLine(I, top, bottom, color, thickness);
396  drawLine(I, left, right, color, thickness);
397 }
398 
407  unsigned char color, unsigned int thickness)
408 {
409  vpImagePoint ip1_ = ip1;
410  vpImagePoint ip2_ = ip2;
411 
412  double size = 10;
413  double length = sqrt(vpMath::sqr(ip2_.get_i() - ip1_.get_i()) + vpMath::sqr(ip2_.get_j() - ip1_.get_j()));
414  bool vertical_line = static_cast<int>(ip2_.get_j()) == static_cast<int>(ip1_.get_j());
415  if (vertical_line) {
416  if (ip2_.get_i() < ip1_.get_i()) {
417  std::swap(ip1_, ip2_);
418  }
419  } else if (ip2_.get_j() < ip1_.get_j()) {
420  std::swap(ip1_, ip2_);
421  }
422 
423  double diff_j = vertical_line ? 1 : ip2_.get_j() - ip1_.get_j();
424  double deltaj = size / length * diff_j;
425  double deltai = size / length * (ip2_.get_i() - ip1_.get_i());
426  double slope = (ip2_.get_i() - ip1_.get_i()) / diff_j;
427  double orig = ip1_.get_i() - slope * ip1_.get_j();
428 
429  if (vertical_line) {
430  for (unsigned int i = static_cast<unsigned int>(ip1_.get_i()); i < ip2_.get_i();
431  i += static_cast<unsigned int>(2 * deltai)) {
432  double j = ip1_.get_j();
433  drawLine(I, vpImagePoint(i, j), vpImagePoint(i + deltai, j), color, thickness);
434  }
435  } else {
436  for (unsigned int j = static_cast<unsigned int>(ip1_.get_j()); j < ip2_.get_j();
437  j += static_cast<unsigned int>(2 * deltaj)) {
438  double i = slope * j + orig;
439  drawLine(I, vpImagePoint(i, j), vpImagePoint(i + deltai, j + deltaj), color, thickness);
440  }
441  }
442 }
443 
452  const vpColor &color, unsigned int thickness)
453 {
454  vpImagePoint ip1_ = ip1;
455  vpImagePoint ip2_ = ip2;
456 
457  double size = 10;
458  double length = sqrt(vpMath::sqr(ip2_.get_i() - ip1_.get_i()) + vpMath::sqr(ip2_.get_j() - ip1_.get_j()));
459  bool vertical_line = static_cast<int>(ip2_.get_j()) == static_cast<int>(ip1_.get_j());
460  if (vertical_line) {
461  if (ip2_.get_i() < ip1_.get_i()) {
462  std::swap(ip1_, ip2_);
463  }
464  } else if (ip2_.get_j() < ip1_.get_j()) {
465  std::swap(ip1_, ip2_);
466  }
467 
468  double diff_j = vertical_line ? 1 : ip2_.get_j() - ip1_.get_j();
469  double deltaj = size / length * diff_j;
470  double deltai = size / length * (ip2_.get_i() - ip1_.get_i());
471  double slope = (ip2_.get_i() - ip1_.get_i()) / diff_j;
472  double orig = ip1_.get_i() - slope * ip1_.get_j();
473 
474  if (vertical_line) {
475  for (unsigned int i = static_cast<unsigned int>(ip1_.get_i()); i < ip2_.get_i();
476  i += static_cast<unsigned int>(2 * deltai)) {
477  double j = ip1_.get_j();
478  drawLine(I, vpImagePoint(i, j), vpImagePoint(i + deltai, j), color, thickness);
479  }
480  } else {
481  for (unsigned int j = static_cast<unsigned int>(ip1_.get_j()); j < ip2_.get_j();
482  j += static_cast<unsigned int>(2 * deltaj)) {
483  double i = slope * j + orig;
484  drawLine(I, vpImagePoint(i, j), vpImagePoint(i + deltai, j + deltaj), color, thickness);
485  }
486  }
487 }
488 
525 void vpImageDraw::drawEllipse(vpImage<unsigned char> &I, const vpImagePoint &center, double coef1, double coef2,
526  double coef3, bool use_normalized_centered_moments, unsigned char color,
527  double smallalpha, double highalpha, unsigned int thickness)
528 {
529  double a = 0., b = 0., e = 0.;
530 
531  if (use_normalized_centered_moments) {
532  // Chaumette, Image Moments: A General and Useful Set of Features for Visual Servoing, TRO 2004, eq 24
533  // Similar code as in function vpMeEllipse::computeAbeFromNij() in vpMeEllipse.cpp
534  double n20_p = coef1;
535  double n11_p = coef2;
536  double n02_p = coef3;
537  double num = n20_p - n02_p;
538  double d = num * num + 4.0 * n11_p * n11_p; // always >= 0
539 
540  if (d <= std::numeric_limits<double>::epsilon()) { // circle
541  e = 0.0; // case n20 = n02 and n11 = 0 : circle, e undefined
542  a = b = 2.0 * sqrt(n20_p);
543  } else { // real ellipse
544  e = atan2(2.0 * n11_p, num) / 2.0; // e in [-Pi/2 ; Pi/2]
545  d = sqrt(d); // d in sqrt always >= 0
546  num = n20_p + n02_p;
547  a = sqrt(2.0 * (num + d)); // term in sqrt always > 0
548  b = sqrt(2.0 * (num - d)); // term in sqrt always > 0
549  }
550  } else {
551  a = coef1;
552  b = coef2;
553  e = coef3;
554  }
555 
556  // For all what follows similar code as in function vpMeEllipse::display() in vpMeEllipse.cpp
557 
558  // Approximation of the circumference of an ellipse:
559  // [Ramanujan, S., "Modular Equations and Approximations to ,"
560  // Quart. J. Pure. Appl. Math., vol. 45 (1913-1914), pp. 350-372]
561  double angle = highalpha - smallalpha;
562 
563  double t = (a - b) / (a + b);
564  t *= t; // t^2
565  double circumference = (angle / 2.0) * (a + b) * (1.0 + 3.0 * t / (10.0 + sqrt(4.0 - 3.0 * t)));
566  unsigned int nbpoints = (unsigned int)(floor(circumference / 20));
567  if (nbpoints < 10) {
568  nbpoints = 10;
569  }
570  double incr = angle / nbpoints; // angle increment
571 
572  double u0 = center.get_u();
573  double v0 = center.get_v();
574  double cose = cos(e);
575  double sine = sin(e);
576 
577  double u = a * cos(smallalpha); // equation of an ellipse
578  double v = b * sin(smallalpha); // equation of an ellipse
579  angle = smallalpha;
580  // (i1,j1) are the coordinates on the origin centered ellipse ;
581  // a rotation by "e" and a translation by (xci,jc) are done
582  // to get the coordinates of the point on the shifted ellipse
583  vpImagePoint iP11;
584  iP11.set_uv(u0 + cose * u - sine * v, v0 + sine * u + cose * v);
585 
586  // display the arc of the ellipse by successive small segments
587  for (unsigned int i = 0; i < nbpoints; i++) {
588  angle += incr;
589  // Two concentric circles method used
590  u = a * cos(angle);
591  v = b * sin(angle);
592  // to get the coordinates of the point on the shifted ellipse
593  vpImagePoint iP22;
594  iP22.set_uv(u0 + cose * u - sine * v, v0 + sine * u + cose * v);
595 
596  drawLine(I, iP11, iP22, color, thickness);
597 
598  iP11 = iP22;
599  }
600 }
601 
638 void vpImageDraw::drawEllipse(vpImage<vpRGBa> &I, const vpImagePoint &center, double coef1, double coef2, double coef3,
639  bool use_normalized_centered_moments, const vpColor &color, double smallalpha,
640  double highalpha, unsigned int thickness)
641 {
642  double a = 0., b = 0., e = 0.;
643 
644  if (use_normalized_centered_moments) {
645  // Chaumette, Image Moments: A General and Useful Set of Features for Visual Servoing, TRO 2004, eq 24
646  // Similar code as in function vpMeEllipse::computeAbeFromNij() in vpMeEllipse.cpp
647  double n20_p = coef1;
648  double n11_p = coef2;
649  double n02_p = coef3;
650  double num = n20_p - n02_p;
651  double d = num * num + 4.0 * n11_p * n11_p; // always >= 0
652 
653  if (d <= std::numeric_limits<double>::epsilon()) { // circle
654  e = 0.0; // case n20 = n02 and n11 = 0 : circle, e undefined
655  a = b = 2.0 * sqrt(n20_p);
656  } else { // real ellipse
657  e = atan2(2.0 * n11_p, num) / 2.0; // e in [-Pi/2 ; Pi/2]
658  d = sqrt(d); // d in sqrt always >= 0
659  num = n20_p + n02_p;
660  a = sqrt(2.0 * (num + d)); // term in sqrt always > 0
661  b = sqrt(2.0 * (num - d)); // term in sqrt always > 0
662  }
663  } else {
664  a = coef1;
665  b = coef2;
666  e = coef3;
667  }
668 
669  // For all what follows similar code as in function vpMeEllipse::display() in vpMeEllipse.cpp
670 
671  // Approximation of the circumference of an ellipse:
672  // [Ramanujan, S., "Modular Equations and Approximations to ,"
673  // Quart. J. Pure. Appl. Math., vol. 45 (1913-1914), pp. 350-372]
674  double angle = highalpha - smallalpha;
675 
676  double t = (a - b) / (a + b);
677  t *= t; // t^2
678  double circumference = (angle / 2.0) * (a + b) * (1.0 + 3.0 * t / (10.0 + sqrt(4.0 - 3.0 * t)));
679  unsigned int nbpoints = (unsigned int)(floor(circumference / 20));
680  if (nbpoints < 10) {
681  nbpoints = 10;
682  }
683  double incr = angle / nbpoints; // angle increment
684 
685  double u0 = center.get_u();
686  double v0 = center.get_v();
687  double cose = cos(e);
688  double sine = sin(e);
689 
690  double u = a * cos(smallalpha); // equation of an ellipse
691  double v = b * sin(smallalpha); // equation of an ellipse
692  angle = smallalpha;
693  // (i1,j1) are the coordinates on the origin centered ellipse ;
694  // a rotation by "e" and a translation by (xci,jc) are done
695  // to get the coordinates of the point on the shifted ellipse
696  vpImagePoint iP11;
697  iP11.set_uv(u0 + cose * u - sine * v, v0 + sine * u + cose * v);
698 
699  // display the arc of the ellipse by successive small segments
700  for (unsigned int i = 0; i < nbpoints; i++) {
701  angle += incr;
702  // Two concentric circles method used
703  u = a * cos(angle);
704  v = b * sin(angle);
705  // to get the coordinates of the point on the shifted ellipse
706  vpImagePoint iP22;
707  iP22.set_uv(u0 + cose * u - sine * v, v0 + sine * u + cose * v);
708 
709  drawLine(I, iP11, iP22, color, thickness);
710 
711  iP11 = iP22;
712  }
713 }
714 
731  double size, unsigned char color, unsigned int thickness, const vpImagePoint &offset)
732 {
733  vpPoint o(0.0, 0.0, 0.0);
734  vpPoint x(size, 0.0, 0.0);
735  vpPoint y(0.0, size, 0.0);
736  vpPoint z(0.0, 0.0, size);
737 
738  o.track(cMo);
739  x.track(cMo);
740  y.track(cMo);
741  z.track(cMo);
742 
743  vpImagePoint ipo, ip1;
744 
745  vpMeterPixelConversion::convertPoint(cam, o.p[0], o.p[1], ipo);
746 
747  vpMeterPixelConversion::convertPoint(cam, x.p[0], x.p[1], ip1);
748  drawArrow(I, ipo + offset, ip1 + offset, color, 4 * thickness, 2 * thickness, thickness);
749 
750  vpMeterPixelConversion::convertPoint(cam, y.p[0], y.p[1], ip1);
751  drawArrow(I, ipo + offset, ip1 + offset, color, 4 * thickness, 2 * thickness, thickness);
752 
753  vpMeterPixelConversion::convertPoint(cam, z.p[0], z.p[1], ip1);
754  drawArrow(I, ipo + offset, ip1 + offset, color, 4 * thickness, 2 * thickness, thickness);
755 }
756 
773  double size, const vpColor &color, unsigned int thickness, const vpImagePoint &offset)
774 {
775  vpPoint o(0.0, 0.0, 0.0);
776  vpPoint x(size, 0.0, 0.0);
777  vpPoint y(0.0, size, 0.0);
778  vpPoint z(0.0, 0.0, size);
779 
780  o.track(cMo);
781  x.track(cMo);
782  y.track(cMo);
783  z.track(cMo);
784 
785  vpImagePoint ipo, ip1;
786 
787  if (color == vpColor::none) {
788  vpMeterPixelConversion::convertPoint(cam, o.p[0], o.p[1], ipo);
789 
790  vpMeterPixelConversion::convertPoint(cam, x.p[0], x.p[1], ip1);
791  drawArrow(I, ipo + offset, ip1 + offset, vpColor::red, 4 * thickness, 2 * thickness, thickness);
792 
793  vpMeterPixelConversion::convertPoint(cam, y.p[0], y.p[1], ip1);
794  drawArrow(I, ipo + offset, ip1 + offset, vpColor::green, 4 * thickness, 2 * thickness, thickness);
795 
796  vpMeterPixelConversion::convertPoint(cam, z.p[0], z.p[1], ip1);
797  drawArrow(I, ipo + offset, ip1 + offset, vpColor::blue, 4 * thickness, 2 * thickness, thickness);
798  } else {
799  vpMeterPixelConversion::convertPoint(cam, o.p[0], o.p[1], ipo);
800 
801  vpMeterPixelConversion::convertPoint(cam, x.p[0], x.p[1], ip1);
802  drawArrow(I, ipo + offset, ip1 + offset, color, 4 * thickness, 2 * thickness, thickness);
803 
804  vpMeterPixelConversion::convertPoint(cam, y.p[0], y.p[1], ip1);
805  drawArrow(I, ipo + offset, ip1 + offset, color, 4 * thickness, 2 * thickness, thickness);
806 
807  vpMeterPixelConversion::convertPoint(cam, z.p[0], z.p[1], ip1);
808  drawArrow(I, ipo + offset, ip1 + offset, color, 4 * thickness, 2 * thickness, thickness);
809  }
810 }
811 
820  unsigned char color, unsigned int thickness)
821 {
822  DrawLine(I, static_cast<int>(ip1.get_u()), static_cast<int>(ip1.get_v()), static_cast<int>(ip2.get_u()),
823  static_cast<int>(ip2.get_v()), color, thickness);
824 }
825 
833 void vpImageDraw::drawLine(vpImage<vpRGBa> &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color,
834  unsigned int thickness)
835 {
836  DrawLine(I, static_cast<int>(ip1.get_u()), static_cast<int>(ip1.get_v()), static_cast<int>(ip2.get_u()),
837  static_cast<int>(ip2.get_v()), vpRGBa(color.R, color.G, color.B), thickness);
838 }
839 
847 void vpImageDraw::drawPoint(vpImage<unsigned char> &I, const vpImagePoint &ip, unsigned char color,
848  unsigned int thickness)
849 {
850  drawRectangle(I, vpRect(ip, thickness, thickness), color, true);
851 }
852 
860 void vpImageDraw::drawPoint(vpImage<vpRGBa> &I, const vpImagePoint &ip, const vpColor &color, unsigned int thickness)
861 {
862  drawRectangle(I, vpRect(ip, thickness, thickness), color, true);
863 }
864 
873 void vpImageDraw::drawPolygon(vpImage<unsigned char> &I, const std::vector<vpImagePoint> &vip, unsigned char color,
874  unsigned int thickness, bool closed)
875 {
876  DrawPolygon(I, vip, color, thickness, closed);
877 }
878 
887 void vpImageDraw::drawPolygon(vpImage<vpRGBa> &I, const std::vector<vpImagePoint> &vip, const vpColor &color,
888  unsigned int thickness, bool closed)
889 {
890  DrawPolygon(I, vip, vpRGBa(color.R, color.G, color.B), thickness, closed);
891 }
892 
905 void vpImageDraw::drawRectangle(vpImage<unsigned char> &I, const vpRect &rectangle, unsigned char color, bool fill,
906  unsigned int thickness)
907 {
908  if (fill) {
909  DrawFilledRectangle(I, rectangle, color);
910  } else {
911  DrawRectangle(I, rectangle, color, thickness);
912  }
913 }
914 
927 void vpImageDraw::drawRectangle(vpImage<vpRGBa> &I, const vpRect &rectangle, const vpColor &color, bool fill,
928  unsigned int thickness)
929 {
930  if (fill) {
931  DrawFilledRectangle(I, rectangle, vpRGBa(color.R, color.G, color.B));
932  } else {
933  DrawRectangle(I, rectangle, vpRGBa(color.R, color.G, color.B), thickness);
934  }
935 }
Generic class defining intrinsic camera parameters.
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:158
static const vpColor red
Definition: vpColor.h:217
static const vpColor none
Definition: vpColor.h:229
static const vpColor blue
Definition: vpColor.h:223
static const vpColor green
Definition: vpColor.h:220
void track(const vpHomogeneousMatrix &cMo)
Implementation of an homogeneous matrix and operations on such kind of matrices.
static void drawEllipse(vpImage< unsigned char > &I, const vpImagePoint &center, double coef1, double coef2, double coef3, bool use_normalized_centered_moments, unsigned char color, double smallalpha=0, double highalpha=2 *M_PI, unsigned int thickness=1)
static void drawRectangle(vpImage< unsigned char > &I, const vpRect &rectangle, unsigned char color, bool fill=false, unsigned int thickness=1)
static void drawCircle(vpImage< unsigned char > &I, const vpImagePoint &center, unsigned int radius, unsigned char color, unsigned int thickness=1)
static void drawLine(vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, unsigned char color, unsigned int thickness=1)
static void drawArrow(vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, unsigned char color, unsigned int w=4, unsigned int h=2, unsigned int thickness=1)
static void drawPoint(vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned char color, unsigned int thickness=1)
static void drawFrame(vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, unsigned char color, unsigned int thickness=1, const vpImagePoint &offset=vpImagePoint(0, 0))
static void drawPolygon(vpImage< unsigned char > &I, const std::vector< vpImagePoint > &vip, unsigned char color, unsigned int thickness=1, bool closed=true)
static void drawCross(vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, unsigned char color, unsigned int thickness=1)
static void drawDottedLine(vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, unsigned char color, unsigned int thickness=1)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:89
void set_j(double jj)
Definition: vpImagePoint.h:309
double get_j() const
Definition: vpImagePoint.h:132
static double distance(const vpImagePoint &iP1, const vpImagePoint &iP2)
void set_i(double ii)
Definition: vpImagePoint.h:298
double get_u() const
Definition: vpImagePoint.h:143
void set_uv(double u, double v)
Definition: vpImagePoint.h:357
double get_i() const
Definition: vpImagePoint.h:121
double get_v() const
Definition: vpImagePoint.h:154
Definition of the vpImage class member functions.
Definition: vpImage.h:74
unsigned int getWidth() const
Definition: vpImage.h:247
unsigned int getHeight() const
Definition: vpImage.h:189
static double sqr(double x)
Definition: vpMath.h:127
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:82
Definition: vpRGBa.h:67
unsigned char B
Blue component.
Definition: vpRGBa.h:146
unsigned char R
Red component.
Definition: vpRGBa.h:144
unsigned char G
Green component.
Definition: vpRGBa.h:145
Defines a rectangle in the plane.
Definition: vpRect.h:80
double getLeft() const
Definition: vpRect.h:174
double getRight() const
Definition: vpRect.h:180
double getBottom() const
Definition: vpRect.h:98
double getTop() const
Definition: vpRect.h:193
vpColVector p
Definition: vpTracker.h:73