60 #include <visp3/core/vpImageDraw.h>
61 #include <visp3/core/vpMeterPixelConversion.h>
62 #include <visp3/core/vpPoint.h>
67 void DrawLine(
vpImage<Type> &canvas,
int x1,
int y1,
int x2,
int y2,
const Type &color,
unsigned int width = 1)
69 const int w =
static_cast<int>(canvas.
getWidth()) - 1;
70 const int h =
static_cast<int>(canvas.
getHeight()) - 1;
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)) {
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);
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);
105 if ((y1 < 0 && y2 < 0) || (y1 > h && y2 > h)) {
129 const bool inverse = ((std::abs(y2 - y1) - std::abs(x2 - x1)) > 0);
140 const double dx = x2 - x1;
141 const double dy =
static_cast<double>(std::abs(y2 - y1));
143 double error = dx / 2.0f;
144 const int ystep = (y1 < y2) ? 1 : -1;
145 int y0 = y1 -
static_cast<int>(width) / 2;
147 for (
int x = x1; x <= x2; x++) {
148 for (
int i = 0; i < static_cast<int>(width); i++) {
153 canvas[x][y] = color;
157 canvas[y][x] = color;
171 template <
class Type>
173 unsigned int width = 1)
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++) {
179 double cx = radius * ::cos(a) + center.
get_u();
180 double cy = radius * ::sin(a) + center.
get_v();
182 DrawLine(canvas,
static_cast<int>(cx),
static_cast<int>(cy),
static_cast<int>(px),
static_cast<int>(py), color,
190 template <
class Type>
void DrawFilledRectangle(
vpImage<Type> &canvas,
vpRect rect,
const Type &color)
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++) {
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)
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);
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);
219 template <
class Type>
220 void DrawRectangle(
vpImage<Type> &canvas,
const vpRect &rect,
const Type &color,
unsigned int width = 1)
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);
242 unsigned char color,
unsigned int w,
unsigned int h,
unsigned int thickness)
248 if ((std::fabs(a) <= std::numeric_limits<double>::epsilon()) &&
249 (std::fabs(b) <= std::numeric_limits<double>::epsilon())) {
264 drawLine(I, ip2, ip4, color, thickness);
271 drawLine(I, ip2, ip4, color, thickness);
274 drawLine(I, ip1, ip2, color, thickness);
287 unsigned int w,
unsigned int h,
unsigned int thickness)
293 if ((std::fabs(a) <= std::numeric_limits<double>::epsilon()) &&
294 (std::fabs(b) <= std::numeric_limits<double>::epsilon())) {
309 drawLine(I, ip2, ip4, color, thickness);
316 drawLine(I, ip2, ip4, color, thickness);
319 drawLine(I, ip1, ip2, color, thickness);
332 unsigned char color,
unsigned int thickness)
334 DrawCircle(I, center,
static_cast<int>(radius), color, thickness);
346 unsigned int thickness)
348 DrawCircle(I, center,
static_cast<int>(radius),
vpRGBa(color.
R, color.
G, color.
B), thickness);
360 unsigned int thickness)
363 top.set_i(ip.
get_i() - size / 2);
364 top.set_j(ip.
get_j());
371 drawLine(I, top, bottom, color, thickness);
372 drawLine(I, left, right, color, thickness);
384 unsigned int thickness)
387 top.set_i(ip.
get_i() - size / 2);
388 top.set_j(ip.
get_j());
395 drawLine(I, top, bottom, color, thickness);
396 drawLine(I, left, right, color, thickness);
407 unsigned char color,
unsigned int thickness)
414 bool vertical_line =
static_cast<int>(ip2_.
get_j()) ==
static_cast<int>(ip1_.
get_j());
417 std::swap(ip1_, ip2_);
420 std::swap(ip1_, ip2_);
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();
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();
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;
452 const vpColor &color,
unsigned int thickness)
459 bool vertical_line =
static_cast<int>(ip2_.
get_j()) ==
static_cast<int>(ip1_.
get_j());
462 std::swap(ip1_, ip2_);
465 std::swap(ip1_, ip2_);
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();
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();
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;
526 double coef3,
bool use_normalized_centered_moments,
unsigned char color,
527 double smallalpha,
double highalpha,
unsigned int thickness)
529 double a = 0., b = 0., e = 0.;
531 if (use_normalized_centered_moments) {
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;
540 if (d <= std::numeric_limits<double>::epsilon()) {
542 a = b = 2.0 * sqrt(n20_p);
544 e = atan2(2.0 * n11_p, num) / 2.0;
547 a = sqrt(2.0 * (num + d));
548 b = sqrt(2.0 * (num - d));
561 double angle = highalpha - smallalpha;
563 double t = (a - b) / (a + b);
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));
570 double incr = angle / nbpoints;
572 double u0 = center.
get_u();
573 double v0 = center.
get_v();
574 double cose = cos(e);
575 double sine = sin(e);
577 double u = a * cos(smallalpha);
578 double v = b * sin(smallalpha);
584 iP11.
set_uv(u0 + cose * u - sine * v, v0 + sine * u + cose * v);
587 for (
unsigned int i = 0; i < nbpoints; i++) {
594 iP22.
set_uv(u0 + cose * u - sine * v, v0 + sine * u + cose * v);
596 drawLine(I, iP11, iP22, color, thickness);
639 bool use_normalized_centered_moments,
const vpColor &color,
double smallalpha,
640 double highalpha,
unsigned int thickness)
642 double a = 0., b = 0., e = 0.;
644 if (use_normalized_centered_moments) {
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;
653 if (d <= std::numeric_limits<double>::epsilon()) {
655 a = b = 2.0 * sqrt(n20_p);
657 e = atan2(2.0 * n11_p, num) / 2.0;
660 a = sqrt(2.0 * (num + d));
661 b = sqrt(2.0 * (num - d));
674 double angle = highalpha - smallalpha;
676 double t = (a - b) / (a + b);
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));
683 double incr = angle / nbpoints;
685 double u0 = center.
get_u();
686 double v0 = center.
get_v();
687 double cose = cos(e);
688 double sine = sin(e);
690 double u = a * cos(smallalpha);
691 double v = b * sin(smallalpha);
697 iP11.
set_uv(u0 + cose * u - sine * v, v0 + sine * u + cose * v);
700 for (
unsigned int i = 0; i < nbpoints; i++) {
707 iP22.
set_uv(u0 + cose * u - sine * v, v0 + sine * u + cose * v);
709 drawLine(I, iP11, iP22, color, thickness);
731 double size,
unsigned char color,
unsigned int thickness,
const vpImagePoint &offset)
748 drawArrow(I, ipo + offset, ip1 + offset, color, 4 * thickness, 2 * thickness, thickness);
751 drawArrow(I, ipo + offset, ip1 + offset, color, 4 * thickness, 2 * thickness, thickness);
754 drawArrow(I, ipo + offset, ip1 + offset, color, 4 * thickness, 2 * thickness, thickness);
802 drawArrow(I, ipo + offset, ip1 + offset, color, 4 * thickness, 2 * thickness, thickness);
805 drawArrow(I, ipo + offset, ip1 + offset, color, 4 * thickness, 2 * thickness, thickness);
808 drawArrow(I, ipo + offset, ip1 + offset, color, 4 * thickness, 2 * thickness, thickness);
820 unsigned char color,
unsigned int thickness)
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);
834 unsigned int thickness)
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);
848 unsigned int thickness)
874 unsigned int thickness,
bool closed)
876 DrawPolygon(I, vip, color, thickness, closed);
888 unsigned int thickness,
bool closed)
890 DrawPolygon(I, vip,
vpRGBa(color.
R, color.
G, color.
B), thickness, closed);
906 unsigned int thickness)
909 DrawFilledRectangle(I, rectangle, color);
911 DrawRectangle(I, rectangle, color, thickness);
928 unsigned int thickness)
931 DrawFilledRectangle(I, rectangle,
vpRGBa(color.
R, color.
G, color.
B));
933 DrawRectangle(I, rectangle,
vpRGBa(color.
R, color.
G, color.
B), thickness);
Generic class defining intrinsic camera parameters.
Class to define RGB colors available for display functionnalities.
static const vpColor none
static const vpColor blue
static const vpColor green
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 ¢er, 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 ¢er, 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 ...
static double distance(const vpImagePoint &iP1, const vpImagePoint &iP2)
void set_uv(double u, double v)
Definition of the vpImage class member functions.
unsigned int getWidth() const
unsigned int getHeight() const
static double sqr(double x)
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 ...
unsigned char B
Blue component.
unsigned char R
Red component.
unsigned char G
Green component.
Defines a rectangle in the plane.