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