Visual Servoing Platform  version 3.6.1 under development (2024-02-13)
vpImageDraw.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  * 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  }
81  else if (x1 == x2) {
82  y1 = std::min<int>(std::max<int>(y1, 0), h);
83  y2 = std::min<int>(std::max<int>(y2, 0), h);
84  }
85  else {
86  int x0 = (x1 * y2 - y1 * x2) / (y2 - y1);
87  int y0 = (y1 * x2 - x1 * y2) / (x2 - x1);
88  int xh = (x1 * y2 - y1 * x2 + h * (x2 - x1)) / (y2 - y1);
89  int yw = (y1 * x2 - x1 * y2 + w * (y2 - y1)) / (x2 - x1);
90 
91  if (x1 < 0) {
92  x1 = 0;
93  y1 = y0;
94  }
95  if (x2 < 0) {
96  x2 = 0;
97  y2 = y0;
98  }
99  if (x1 > w) {
100  x1 = w;
101  y1 = yw;
102  }
103  if (x2 > w) {
104  x2 = w;
105  y2 = yw;
106  }
107  if ((y1 < 0 && y2 < 0) || (y1 > h && y2 > h)) {
108  return;
109  }
110 
111  if (y1 < 0) {
112  x1 = x0;
113  y1 = 0;
114  }
115  if (y2 < 0) {
116  x2 = x0;
117  y2 = 0;
118  }
119 
120  if (y1 > h) {
121  x1 = xh;
122  y1 = h;
123  }
124  if (y2 > h) {
125  x2 = xh;
126  y2 = h;
127  }
128  }
129  }
130 
131  const bool inverse = ((std::abs(y2 - y1) - std::abs(x2 - x1)) > 0);
132  if (inverse) {
133  std::swap(x1, y1);
134  std::swap(x2, y2);
135  }
136 
137  if (x1 > x2) {
138  std::swap(x1, x2);
139  std::swap(y1, y2);
140  }
141 
142  const double dx = x2 - x1;
143  const double dy = static_cast<double>(std::abs(y2 - y1));
144 
145  double error = dx / 2.0f;
146  const int ystep = (y1 < y2) ? 1 : -1;
147  int y0 = y1 - static_cast<int>(width) / 2;
148 
149  for (int x = x1; x <= x2; x++) {
150  for (int i = 0; i < static_cast<int>(width); i++) {
151  int y = y0 + i;
152  if (y >= 0) {
153  if (inverse) {
154  if (y < w) {
155  canvas[x][y] = color;
156  }
157  }
158  else {
159  if (y < h) {
160  canvas[y][x] = color;
161  }
162  }
163  }
164  }
165 
166  error -= dy;
167  if (error < 0) {
168  y0 += ystep;
169  error += dx;
170  }
171  }
172 }
173 
174 template <class Type>
175 void DrawCircle(vpImage<Type> &canvas, const vpImagePoint &center, int radius, const Type &color,
176  unsigned int width = 1)
177 {
178  const size_t n = 8 * std::max(static_cast<size_t>(1), static_cast<size_t>(::pow(radius, 0.5)));
179  double px = 0, py = 0, da = 2 * M_PI / n;
180  for (size_t i = 0; i <= n; i++) {
181  double a = i * da;
182  double cx = radius * ::cos(a) + center.get_u();
183  double cy = radius * ::sin(a) + center.get_v();
184  if (i > 0) {
185  DrawLine(canvas, static_cast<int>(cx), static_cast<int>(cy), static_cast<int>(px), static_cast<int>(py), color,
186  width);
187  }
188  px = cx;
189  py = cy;
190  }
191 }
192 
193 template <class Type> void DrawFilledRectangle(vpImage<Type> &canvas, vpRect rect, const Type &color)
194 {
195  rect &= vpRect(0, 0, canvas.getWidth(), canvas.getHeight());
196  for (int row = static_cast<int>(rect.getTop()); row < static_cast<int>(rect.getBottom()); row++) {
197  Type *dst = canvas[row];
198  for (int col = static_cast<int>(rect.getLeft()); col < static_cast<int>(rect.getRight()); col++) {
199  dst[col] = color;
200  }
201  }
202 }
203 
204 template <class Type>
205 void DrawPolygon(vpImage<Type> &canvas, const std::vector<vpImagePoint> &polygon, const Type &color,
206  unsigned int width = 1, bool closed = true)
207 {
208  if (closed) {
209  for (size_t i = 0; i < polygon.size(); i++) {
210  const vpImagePoint &p1 = (i ? polygon[i - 1] : polygon.back()), p2 = polygon[i];
211  DrawLine(canvas, static_cast<int>(p1.get_u()), static_cast<int>(p1.get_v()), static_cast<int>(p2.get_u()),
212  static_cast<int>(p2.get_v()), color, width);
213  }
214  }
215  else {
216  for (size_t i = 1; i < polygon.size(); i++) {
217  DrawLine(canvas, static_cast<int>(polygon[i - 1].get_u()), static_cast<int>(polygon[i - 1].get_v()),
218  static_cast<int>(polygon[i].get_u()), static_cast<int>(polygon[i].get_v()), color, width);
219  }
220  }
221 }
222 
223 template <class Type>
224 void DrawRectangle(vpImage<Type> &canvas, const vpRect &rect, const Type &color, unsigned int width = 1)
225 {
226  DrawLine(canvas, static_cast<int>(rect.getLeft()), static_cast<int>(rect.getTop()), static_cast<int>(rect.getRight()),
227  static_cast<int>(rect.getTop()), color, width);
228  DrawLine(canvas, static_cast<int>(rect.getRight()), static_cast<int>(rect.getTop()),
229  static_cast<int>(rect.getRight()), static_cast<int>(rect.getBottom()), color, width);
230  DrawLine(canvas, static_cast<int>(rect.getRight()), static_cast<int>(rect.getBottom()),
231  static_cast<int>(rect.getLeft()), static_cast<int>(rect.getBottom()), color, width);
232  DrawLine(canvas, static_cast<int>(rect.getLeft()), static_cast<int>(rect.getBottom()),
233  static_cast<int>(rect.getLeft()), static_cast<int>(rect.getTop()), color, width);
234 }
235 } // namespace
236 
246  unsigned char color, unsigned int w, unsigned int h, unsigned int thickness)
247 {
248  double a = ip2.get_i() - ip1.get_i();
249  double b = ip2.get_j() - ip1.get_j();
250  double lg = sqrt(vpMath::sqr(a) + vpMath::sqr(b));
251 
252  if ((std::fabs(a) <= std::numeric_limits<double>::epsilon()) &&
253  (std::fabs(b) <= std::numeric_limits<double>::epsilon())) {
254  // DisplayCrossLarge(i1,j1,3,col) ;
255  }
256  else {
257  a /= lg;
258  b /= lg;
259 
260  vpImagePoint ip3;
261  ip3.set_i(ip2.get_i() - w * a);
262  ip3.set_j(ip2.get_j() - w * b);
263 
264  vpImagePoint ip4;
265  ip4.set_i(ip3.get_i() - b * h);
266  ip4.set_j(ip3.get_j() + a * h);
267 
268  if (lg > 2 * vpImagePoint::distance(ip2, ip4)) {
269  drawLine(I, ip2, ip4, color, thickness);
270  }
271 
272  ip4.set_i(ip3.get_i() + b * h);
273  ip4.set_j(ip3.get_j() - a * h);
274 
275  if (lg > 2 * vpImagePoint::distance(ip2, ip4)) {
276  drawLine(I, ip2, ip4, color, thickness);
277  }
278 
279  drawLine(I, ip1, ip2, color, thickness);
280  }
281 }
282 
291 void vpImageDraw::drawArrow(vpImage<vpRGBa> &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color,
292  unsigned int w, unsigned int h, unsigned int thickness)
293 {
294  double a = ip2.get_i() - ip1.get_i();
295  double b = ip2.get_j() - ip1.get_j();
296  double lg = sqrt(vpMath::sqr(a) + vpMath::sqr(b));
297 
298  if ((std::fabs(a) <= std::numeric_limits<double>::epsilon()) &&
299  (std::fabs(b) <= std::numeric_limits<double>::epsilon())) {
300  // DisplayCrossLarge(i1,j1,3,col) ;
301  }
302  else {
303  a /= lg;
304  b /= lg;
305 
306  vpImagePoint ip3;
307  ip3.set_i(ip2.get_i() - w * a);
308  ip3.set_j(ip2.get_j() - w * b);
309 
310  vpImagePoint ip4;
311  ip4.set_i(ip3.get_i() - b * h);
312  ip4.set_j(ip3.get_j() + a * h);
313 
314  if (lg > 2 * vpImagePoint::distance(ip2, ip4)) {
315  drawLine(I, ip2, ip4, color, thickness);
316  }
317 
318  ip4.set_i(ip3.get_i() + b * h);
319  ip4.set_j(ip3.get_j() - a * h);
320 
321  if (lg > 2 * vpImagePoint::distance(ip2, ip4)) {
322  drawLine(I, ip2, ip4, color, thickness);
323  }
324 
325  drawLine(I, ip1, ip2, color, thickness);
326  }
327 }
328 
337  unsigned char color, unsigned int thickness)
338 {
339  DrawCircle(I, circle.getCenter(), static_cast<int>(circle.getRadius()), color, thickness);
340 }
341 
350 void vpImageDraw::drawCircle(vpImage<unsigned char> &I, const vpImagePoint &center, unsigned int radius,
351  unsigned char color, unsigned int thickness)
352 {
353  DrawCircle(I, center, static_cast<int>(radius), color, thickness);
354 }
355 
363 void vpImageDraw::drawCircle(vpImage<vpRGBa> &I, const vpImageCircle &circle, const vpColor &color,
364  unsigned int thickness)
365 {
366  DrawCircle(I, circle.getCenter(), static_cast<int>(circle.getRadius()), vpRGBa(color.R, color.G, color.B), thickness);
367 }
368 
377 void vpImageDraw::drawCircle(vpImage<vpRGBa> &I, const vpImagePoint &center, unsigned int radius, const vpColor &color,
378  unsigned int thickness)
379 {
380  DrawCircle(I, center, static_cast<int>(radius), vpRGBa(color.R, color.G, color.B), thickness);
381 }
382 
391 void vpImageDraw::drawCross(vpImage<unsigned char> &I, const vpImagePoint &ip, unsigned int size, unsigned char color,
392  unsigned int thickness)
393 {
394  vpImagePoint top, bottom, left, right;
395  top.set_i(ip.get_i() - size / 2);
396  top.set_j(ip.get_j());
397  bottom.set_i(ip.get_i() + size / 2);
398  bottom.set_j(ip.get_j());
399  left.set_i(ip.get_i());
400  left.set_j(ip.get_j() - size / 2);
401  right.set_i(ip.get_i());
402  right.set_j(ip.get_j() + size / 2);
403  drawLine(I, top, bottom, color, thickness);
404  drawLine(I, left, right, color, thickness);
405 }
406 
415 void vpImageDraw::drawCross(vpImage<vpRGBa> &I, const vpImagePoint &ip, unsigned int size, const vpColor &color,
416  unsigned int thickness)
417 {
418  vpImagePoint top, bottom, left, right;
419  top.set_i(ip.get_i() - size / 2);
420  top.set_j(ip.get_j());
421  bottom.set_i(ip.get_i() + size / 2);
422  bottom.set_j(ip.get_j());
423  left.set_i(ip.get_i());
424  left.set_j(ip.get_j() - size / 2);
425  right.set_i(ip.get_i());
426  right.set_j(ip.get_j() + size / 2);
427  drawLine(I, top, bottom, color, thickness);
428  drawLine(I, left, right, color, thickness);
429 }
430 
439  unsigned char color, unsigned int thickness)
440 {
441  vpImagePoint ip1_ = ip1;
442  vpImagePoint ip2_ = ip2;
443 
444  double size = 10;
445  double length = sqrt(vpMath::sqr(ip2_.get_i() - ip1_.get_i()) + vpMath::sqr(ip2_.get_j() - ip1_.get_j()));
446  bool vertical_line = static_cast<int>(ip2_.get_j()) == static_cast<int>(ip1_.get_j());
447  if (vertical_line) {
448  if (ip2_.get_i() < ip1_.get_i()) {
449  std::swap(ip1_, ip2_);
450  }
451  }
452  else if (ip2_.get_j() < ip1_.get_j()) {
453  std::swap(ip1_, ip2_);
454  }
455 
456  double diff_j = vertical_line ? 1 : ip2_.get_j() - ip1_.get_j();
457  double deltaj = size / length * diff_j;
458  double deltai = size / length * (ip2_.get_i() - ip1_.get_i());
459  double slope = (ip2_.get_i() - ip1_.get_i()) / diff_j;
460  double orig = ip1_.get_i() - slope * ip1_.get_j();
461 
462  if (vertical_line) {
463  for (unsigned int i = static_cast<unsigned int>(ip1_.get_i()); i < ip2_.get_i();
464  i += static_cast<unsigned int>(2 * deltai)) {
465  double j = ip1_.get_j();
466  drawLine(I, vpImagePoint(i, j), vpImagePoint(i + deltai, j), color, thickness);
467  }
468  }
469  else {
470  for (unsigned int j = static_cast<unsigned int>(ip1_.get_j()); j < ip2_.get_j();
471  j += static_cast<unsigned int>(2 * deltaj)) {
472  double i = slope * j + orig;
473  drawLine(I, vpImagePoint(i, j), vpImagePoint(i + deltai, j + deltaj), color, thickness);
474  }
475  }
476 }
477 
486  const vpColor &color, unsigned int thickness)
487 {
488  vpImagePoint ip1_ = ip1;
489  vpImagePoint ip2_ = ip2;
490 
491  double size = 10;
492  double length = sqrt(vpMath::sqr(ip2_.get_i() - ip1_.get_i()) + vpMath::sqr(ip2_.get_j() - ip1_.get_j()));
493  bool vertical_line = static_cast<int>(ip2_.get_j()) == static_cast<int>(ip1_.get_j());
494  if (vertical_line) {
495  if (ip2_.get_i() < ip1_.get_i()) {
496  std::swap(ip1_, ip2_);
497  }
498  }
499  else if (ip2_.get_j() < ip1_.get_j()) {
500  std::swap(ip1_, ip2_);
501  }
502 
503  double diff_j = vertical_line ? 1 : ip2_.get_j() - ip1_.get_j();
504  double deltaj = size / length * diff_j;
505  double deltai = size / length * (ip2_.get_i() - ip1_.get_i());
506  double slope = (ip2_.get_i() - ip1_.get_i()) / diff_j;
507  double orig = ip1_.get_i() - slope * ip1_.get_j();
508 
509  if (vertical_line) {
510  for (unsigned int i = static_cast<unsigned int>(ip1_.get_i()); i < ip2_.get_i();
511  i += static_cast<unsigned int>(2 * deltai)) {
512  double j = ip1_.get_j();
513  drawLine(I, vpImagePoint(i, j), vpImagePoint(i + deltai, j), color, thickness);
514  }
515  }
516  else {
517  for (unsigned int j = static_cast<unsigned int>(ip1_.get_j()); j < ip2_.get_j();
518  j += static_cast<unsigned int>(2 * deltaj)) {
519  double i = slope * j + orig;
520  drawLine(I, vpImagePoint(i, j), vpImagePoint(i + deltai, j + deltaj), color, thickness);
521  }
522  }
523 }
524 
561 void vpImageDraw::drawEllipse(vpImage<unsigned char> &I, const vpImagePoint &center, double coef1, double coef2,
562  double coef3, bool use_normalized_centered_moments, unsigned char color,
563  double smallalpha, double highalpha, unsigned int thickness)
564 {
565  double a = 0., b = 0., e = 0.;
566 
567  if (use_normalized_centered_moments) {
568  // Chaumette, Image Moments: A General and Useful Set of Features for Visual Servoing, TRO 2004, eq 24
569  // Similar code as in function vpMeEllipse::computeAbeFromNij() in vpMeEllipse.cpp
570  double n20_p = coef1;
571  double n11_p = coef2;
572  double n02_p = coef3;
573  double num = n20_p - n02_p;
574  double d = num * num + 4.0 * n11_p * n11_p; // always >= 0
575 
576  if (d <= std::numeric_limits<double>::epsilon()) { // circle
577  e = 0.0; // case n20 = n02 and n11 = 0 : circle, e undefined
578  a = b = 2.0 * sqrt(n20_p);
579  }
580  else { // real ellipse
581  e = atan2(2.0 * n11_p, num) / 2.0; // e in [-Pi/2 ; Pi/2]
582  d = sqrt(d); // d in sqrt always >= 0
583  num = n20_p + n02_p;
584  a = sqrt(2.0 * (num + d)); // term in sqrt always > 0
585  b = sqrt(2.0 * (num - d)); // term in sqrt always > 0
586  }
587  }
588  else {
589  a = coef1;
590  b = coef2;
591  e = coef3;
592  }
593 
594  // For all what follows similar code as in function vpMeEllipse::display() in vpMeEllipse.cpp
595 
596  // Approximation of the circumference of an ellipse:
597  // [Ramanujan, S., "Modular Equations and Approximations to ,"
598  // Quart. J. Pure. Appl. Math., vol. 45 (1913-1914), pp. 350-372]
599  double angle = highalpha - smallalpha;
600 
601  double t = (a - b) / (a + b);
602  t *= t; // t^2
603  double circumference = (angle / 2.0) * (a + b) * (1.0 + 3.0 * t / (10.0 + sqrt(4.0 - 3.0 * t)));
604  unsigned int nbpoints = (unsigned int)(floor(circumference / 20));
605  if (nbpoints < 10) {
606  nbpoints = 10;
607  }
608  double incr = angle / nbpoints; // angle increment
609 
610  double u0 = center.get_u();
611  double v0 = center.get_v();
612  double cose = cos(e);
613  double sine = sin(e);
614 
615  double u = a * cos(smallalpha); // equation of an ellipse
616  double v = b * sin(smallalpha); // equation of an ellipse
617  angle = smallalpha;
618  // (i1,j1) are the coordinates on the origin centered ellipse ;
619  // a rotation by "e" and a translation by (xci,jc) are done
620  // to get the coordinates of the point on the shifted ellipse
621  vpImagePoint iP11;
622  iP11.set_uv(u0 + cose * u - sine * v, v0 + sine * u + cose * v);
623 
624  // display the arc of the ellipse by successive small segments
625  for (unsigned int i = 0; i < nbpoints; i++) {
626  angle += incr;
627  // Two concentric circles method used
628  u = a * cos(angle);
629  v = b * sin(angle);
630  // to get the coordinates of the point on the shifted ellipse
631  vpImagePoint iP22;
632  iP22.set_uv(u0 + cose * u - sine * v, v0 + sine * u + cose * v);
633 
634  drawLine(I, iP11, iP22, color, thickness);
635 
636  iP11 = iP22;
637  }
638 }
639 
676 void vpImageDraw::drawEllipse(vpImage<vpRGBa> &I, const vpImagePoint &center, double coef1, double coef2, double coef3,
677  bool use_normalized_centered_moments, const vpColor &color, double smallalpha,
678  double highalpha, unsigned int thickness)
679 {
680  double a = 0., b = 0., e = 0.;
681 
682  if (use_normalized_centered_moments) {
683  // Chaumette, Image Moments: A General and Useful Set of Features for Visual Servoing, TRO 2004, eq 24
684  // Similar code as in function vpMeEllipse::computeAbeFromNij() in vpMeEllipse.cpp
685  double n20_p = coef1;
686  double n11_p = coef2;
687  double n02_p = coef3;
688  double num = n20_p - n02_p;
689  double d = num * num + 4.0 * n11_p * n11_p; // always >= 0
690 
691  if (d <= std::numeric_limits<double>::epsilon()) { // circle
692  e = 0.0; // case n20 = n02 and n11 = 0 : circle, e undefined
693  a = b = 2.0 * sqrt(n20_p);
694  }
695  else { // real ellipse
696  e = atan2(2.0 * n11_p, num) / 2.0; // e in [-Pi/2 ; Pi/2]
697  d = sqrt(d); // d in sqrt always >= 0
698  num = n20_p + n02_p;
699  a = sqrt(2.0 * (num + d)); // term in sqrt always > 0
700  b = sqrt(2.0 * (num - d)); // term in sqrt always > 0
701  }
702  }
703  else {
704  a = coef1;
705  b = coef2;
706  e = coef3;
707  }
708 
709  // For all what follows similar code as in function vpMeEllipse::display() in vpMeEllipse.cpp
710 
711  // Approximation of the circumference of an ellipse:
712  // [Ramanujan, S., "Modular Equations and Approximations to ,"
713  // Quart. J. Pure. Appl. Math., vol. 45 (1913-1914), pp. 350-372]
714  double angle = highalpha - smallalpha;
715 
716  double t = (a - b) / (a + b);
717  t *= t; // t^2
718  double circumference = (angle / 2.0) * (a + b) * (1.0 + 3.0 * t / (10.0 + sqrt(4.0 - 3.0 * t)));
719  unsigned int nbpoints = (unsigned int)(floor(circumference / 20));
720  if (nbpoints < 10) {
721  nbpoints = 10;
722  }
723  double incr = angle / nbpoints; // angle increment
724 
725  double u0 = center.get_u();
726  double v0 = center.get_v();
727  double cose = cos(e);
728  double sine = sin(e);
729 
730  double u = a * cos(smallalpha); // equation of an ellipse
731  double v = b * sin(smallalpha); // equation of an ellipse
732  angle = smallalpha;
733  // (i1,j1) are the coordinates on the origin centered ellipse ;
734  // a rotation by "e" and a translation by (xci,jc) are done
735  // to get the coordinates of the point on the shifted ellipse
736  vpImagePoint iP11;
737  iP11.set_uv(u0 + cose * u - sine * v, v0 + sine * u + cose * v);
738 
739  // display the arc of the ellipse by successive small segments
740  for (unsigned int i = 0; i < nbpoints; i++) {
741  angle += incr;
742  // Two concentric circles method used
743  u = a * cos(angle);
744  v = b * sin(angle);
745  // to get the coordinates of the point on the shifted ellipse
746  vpImagePoint iP22;
747  iP22.set_uv(u0 + cose * u - sine * v, v0 + sine * u + cose * v);
748 
749  drawLine(I, iP11, iP22, color, thickness);
750 
751  iP11 = iP22;
752  }
753 }
754 
771  double size, unsigned char color, unsigned int thickness, const vpImagePoint &offset)
772 {
773  vpPoint o(0.0, 0.0, 0.0);
774  vpPoint x(size, 0.0, 0.0);
775  vpPoint y(0.0, size, 0.0);
776  vpPoint z(0.0, 0.0, size);
777 
778  o.track(cMo);
779  x.track(cMo);
780  y.track(cMo);
781  z.track(cMo);
782 
783  vpImagePoint ipo, ip1;
784 
785  vpMeterPixelConversion::convertPoint(cam, o.p[0], o.p[1], ipo);
786 
787  vpMeterPixelConversion::convertPoint(cam, x.p[0], x.p[1], ip1);
788  drawArrow(I, ipo + offset, ip1 + offset, color, 4 * thickness, 2 * thickness, thickness);
789 
790  vpMeterPixelConversion::convertPoint(cam, y.p[0], y.p[1], ip1);
791  drawArrow(I, ipo + offset, ip1 + offset, color, 4 * thickness, 2 * thickness, thickness);
792 
793  vpMeterPixelConversion::convertPoint(cam, z.p[0], z.p[1], ip1);
794  drawArrow(I, ipo + offset, ip1 + offset, color, 4 * thickness, 2 * thickness, thickness);
795 }
796 
813  double size, const vpColor &color, unsigned int thickness, const vpImagePoint &offset)
814 {
815  vpPoint o(0.0, 0.0, 0.0);
816  vpPoint x(size, 0.0, 0.0);
817  vpPoint y(0.0, size, 0.0);
818  vpPoint z(0.0, 0.0, size);
819 
820  o.track(cMo);
821  x.track(cMo);
822  y.track(cMo);
823  z.track(cMo);
824 
825  vpImagePoint ipo, ip1;
826 
827  if (color == vpColor::none) {
828  vpMeterPixelConversion::convertPoint(cam, o.p[0], o.p[1], ipo);
829 
830  vpMeterPixelConversion::convertPoint(cam, x.p[0], x.p[1], ip1);
831  drawArrow(I, ipo + offset, ip1 + offset, vpColor::red, 4 * thickness, 2 * thickness, thickness);
832 
833  vpMeterPixelConversion::convertPoint(cam, y.p[0], y.p[1], ip1);
834  drawArrow(I, ipo + offset, ip1 + offset, vpColor::green, 4 * thickness, 2 * thickness, thickness);
835 
836  vpMeterPixelConversion::convertPoint(cam, z.p[0], z.p[1], ip1);
837  drawArrow(I, ipo + offset, ip1 + offset, vpColor::blue, 4 * thickness, 2 * thickness, thickness);
838  }
839  else {
840  vpMeterPixelConversion::convertPoint(cam, o.p[0], o.p[1], ipo);
841 
842  vpMeterPixelConversion::convertPoint(cam, x.p[0], x.p[1], ip1);
843  drawArrow(I, ipo + offset, ip1 + offset, color, 4 * thickness, 2 * thickness, thickness);
844 
845  vpMeterPixelConversion::convertPoint(cam, y.p[0], y.p[1], ip1);
846  drawArrow(I, ipo + offset, ip1 + offset, color, 4 * thickness, 2 * thickness, thickness);
847 
848  vpMeterPixelConversion::convertPoint(cam, z.p[0], z.p[1], ip1);
849  drawArrow(I, ipo + offset, ip1 + offset, color, 4 * thickness, 2 * thickness, thickness);
850  }
851 }
852 
861  unsigned char color, unsigned int thickness)
862 {
863  DrawLine(I, static_cast<int>(ip1.get_u()), static_cast<int>(ip1.get_v()), static_cast<int>(ip2.get_u()),
864  static_cast<int>(ip2.get_v()), color, thickness);
865 }
866 
874 void vpImageDraw::drawLine(vpImage<vpRGBa> &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color,
875  unsigned int thickness)
876 {
877  DrawLine(I, static_cast<int>(ip1.get_u()), static_cast<int>(ip1.get_v()), static_cast<int>(ip2.get_u()),
878  static_cast<int>(ip2.get_v()), vpRGBa(color.R, color.G, color.B), thickness);
879 }
880 
888 void vpImageDraw::drawPoint(vpImage<unsigned char> &I, const vpImagePoint &ip, unsigned char color,
889  unsigned int thickness)
890 {
891  drawRectangle(I, vpRect(ip, thickness, thickness), color, true);
892 }
893 
901 void vpImageDraw::drawPoint(vpImage<vpRGBa> &I, const vpImagePoint &ip, const vpColor &color, unsigned int thickness)
902 {
903  drawRectangle(I, vpRect(ip, thickness, thickness), color, true);
904 }
905 
914 void vpImageDraw::drawPolygon(vpImage<unsigned char> &I, const std::vector<vpImagePoint> &vip, unsigned char color,
915  unsigned int thickness, bool closed)
916 {
917  DrawPolygon(I, vip, color, thickness, closed);
918 }
919 
928 void vpImageDraw::drawPolygon(vpImage<vpRGBa> &I, const std::vector<vpImagePoint> &vip, const vpColor &color,
929  unsigned int thickness, bool closed)
930 {
931  DrawPolygon(I, vip, vpRGBa(color.R, color.G, color.B), thickness, closed);
932 }
933 
946 void vpImageDraw::drawRectangle(vpImage<unsigned char> &I, const vpRect &rectangle, unsigned char color, bool fill,
947  unsigned int thickness)
948 {
949  if (fill) {
950  DrawFilledRectangle(I, rectangle, color);
951  }
952  else {
953  DrawRectangle(I, rectangle, color, thickness);
954  }
955 }
956 
969 void vpImageDraw::drawRectangle(vpImage<vpRGBa> &I, const vpRect &rectangle, const vpColor &color, bool fill,
970  unsigned int thickness)
971 {
972  if (fill) {
973  DrawFilledRectangle(I, rectangle, vpRGBa(color.R, color.G, color.B));
974  }
975  else {
976  DrawRectangle(I, rectangle, vpRGBa(color.R, color.G, color.B), thickness);
977  }
978 }
Generic class defining intrinsic camera parameters.
Class to define RGB colors available for display functionalities.
Definition: vpColor.h:152
static const vpColor red
Definition: vpColor.h:211
static const vpColor none
Definition: vpColor.h:223
static const vpColor blue
Definition: vpColor.h:217
static const vpColor green
Definition: vpColor.h:214
void track(const vpHomogeneousMatrix &cMo)
Implementation of an homogeneous matrix and operations on such kind of matrices.
Class that defines a 2D circle in an image.
Definition: vpImageCircle.h:56
float getRadius() const
vpImagePoint getCenter() const
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 vpImageCircle &circle, 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:82
void set_j(double jj)
Definition: vpImagePoint.h:304
double get_j() const
Definition: vpImagePoint.h:125
static double distance(const vpImagePoint &iP1, const vpImagePoint &iP2)
void set_i(double ii)
Definition: vpImagePoint.h:293
double get_u() const
Definition: vpImagePoint.h:136
void set_uv(double u, double v)
Definition: vpImagePoint.h:352
double get_i() const
Definition: vpImagePoint.h:114
double get_v() const
Definition: vpImagePoint.h:147
Definition of the vpImage class member functions.
Definition: vpImage.h:69
unsigned int getWidth() const
Definition: vpImage.h:242
unsigned int getHeight() const
Definition: vpImage.h:184
static double sqr(double x)
Definition: vpMath.h:201
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:77
Definition: vpRGBa.h:61
unsigned char B
Blue component.
Definition: vpRGBa.h:139
unsigned char R
Red component.
Definition: vpRGBa.h:137
unsigned char G
Green component.
Definition: vpRGBa.h:138
Defines a rectangle in the plane.
Definition: vpRect.h:76
double getLeft() const
Definition: vpRect.h:170
double getRight() const
Definition: vpRect.h:176
double getBottom() const
Definition: vpRect.h:94
double getTop() const
Definition: vpRect.h:189
vpColVector p
Definition: vpTracker.h:67