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);
82 y1 = std::min<int>(std::max<int>(y1, 0), h);
83 y2 = std::min<int>(std::max<int>(y2, 0), h);
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);
107 if ((y1 < 0 && y2 < 0) || (y1 > h && y2 > h)) {
131 const bool inverse = ((std::abs(y2 - y1) - std::abs(x2 - x1)) > 0);
142 const double dx = x2 - x1;
143 const double dy =
static_cast<double>(std::abs(y2 - y1));
145 double error = dx / 2.0f;
146 const int ystep = (y1 < y2) ? 1 : -1;
147 int y0 = y1 -
static_cast<int>(width) / 2;
149 for (
int x = x1; x <= x2; x++) {
150 for (
int i = 0; i < static_cast<int>(width); i++) {
155 canvas[x][y] = color;
160 canvas[y][x] = color;
174 template <
class Type>
176 unsigned int width = 1)
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++) {
182 double cx = radius * ::cos(a) + center.
get_u();
183 double cy = radius * ::sin(a) + center.
get_v();
185 DrawLine(canvas,
static_cast<int>(cx),
static_cast<int>(cy),
static_cast<int>(px),
static_cast<int>(py), color,
193 template <
class Type>
void DrawFilledRectangle(
vpImage<Type> &canvas,
vpRect rect,
const Type &color)
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++) {
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)
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);
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);
223 template <
class Type>
224 void DrawRectangle(
vpImage<Type> &canvas,
const vpRect &rect,
const Type &color,
unsigned int width = 1)
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);
246 unsigned char color,
unsigned int w,
unsigned int h,
unsigned int thickness)
252 if ((std::fabs(a) <= std::numeric_limits<double>::epsilon()) &&
253 (std::fabs(b) <= std::numeric_limits<double>::epsilon())) {
269 drawLine(I, ip2, ip4, color, thickness);
276 drawLine(I, ip2, ip4, color, thickness);
279 drawLine(I, ip1, ip2, color, thickness);
292 unsigned int w,
unsigned int h,
unsigned int thickness)
298 if ((std::fabs(a) <= std::numeric_limits<double>::epsilon()) &&
299 (std::fabs(b) <= std::numeric_limits<double>::epsilon())) {
315 drawLine(I, ip2, ip4, color, thickness);
322 drawLine(I, ip2, ip4, color, thickness);
325 drawLine(I, ip1, ip2, color, thickness);
337 unsigned char color,
unsigned int thickness)
339 DrawCircle(I, circle.
getCenter(),
static_cast<int>(circle.
getRadius()), color, thickness);
351 unsigned char color,
unsigned int thickness)
353 DrawCircle(I, center,
static_cast<int>(radius), color, thickness);
364 unsigned int thickness)
378 unsigned int thickness)
380 DrawCircle(I, center,
static_cast<int>(radius),
vpRGBa(color.
R, color.
G, color.
B), thickness);
392 unsigned int thickness)
395 top.set_i(ip.
get_i() - size / 2);
396 top.set_j(ip.
get_j());
403 drawLine(I, top, bottom, color, thickness);
404 drawLine(I, left, right, color, thickness);
416 unsigned int thickness)
419 top.set_i(ip.
get_i() - size / 2);
420 top.set_j(ip.
get_j());
427 drawLine(I, top, bottom, color, thickness);
428 drawLine(I, left, right, color, thickness);
439 unsigned char color,
unsigned int thickness)
446 bool vertical_line =
static_cast<int>(ip2_.
get_j()) ==
static_cast<int>(ip1_.
get_j());
449 std::swap(ip1_, ip2_);
453 std::swap(ip1_, ip2_);
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();
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();
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;
486 const vpColor &color,
unsigned int thickness)
493 bool vertical_line =
static_cast<int>(ip2_.
get_j()) ==
static_cast<int>(ip1_.
get_j());
496 std::swap(ip1_, ip2_);
500 std::swap(ip1_, ip2_);
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();
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();
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;
562 double coef3,
bool use_normalized_centered_moments,
unsigned char color,
563 double smallalpha,
double highalpha,
unsigned int thickness)
565 double a = 0., b = 0., e = 0.;
567 if (use_normalized_centered_moments) {
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;
576 if (d <= std::numeric_limits<double>::epsilon()) {
578 a = b = 2.0 * sqrt(n20_p);
581 e = atan2(2.0 * n11_p, num) / 2.0;
584 a = sqrt(2.0 * (num + d));
585 b = sqrt(2.0 * (num - d));
599 double angle = highalpha - smallalpha;
601 double t = (a - b) / (a + b);
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));
608 double incr = angle / nbpoints;
610 double u0 = center.
get_u();
611 double v0 = center.
get_v();
612 double cose = cos(e);
613 double sine = sin(e);
615 double u = a * cos(smallalpha);
616 double v = b * sin(smallalpha);
622 iP11.
set_uv(u0 + cose * u - sine * v, v0 + sine * u + cose * v);
625 for (
unsigned int i = 0; i < nbpoints; i++) {
632 iP22.
set_uv(u0 + cose * u - sine * v, v0 + sine * u + cose * v);
634 drawLine(I, iP11, iP22, color, thickness);
677 bool use_normalized_centered_moments,
const vpColor &color,
double smallalpha,
678 double highalpha,
unsigned int thickness)
680 double a = 0., b = 0., e = 0.;
682 if (use_normalized_centered_moments) {
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;
691 if (d <= std::numeric_limits<double>::epsilon()) {
693 a = b = 2.0 * sqrt(n20_p);
696 e = atan2(2.0 * n11_p, num) / 2.0;
699 a = sqrt(2.0 * (num + d));
700 b = sqrt(2.0 * (num - d));
714 double angle = highalpha - smallalpha;
716 double t = (a - b) / (a + b);
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));
723 double incr = angle / nbpoints;
725 double u0 = center.
get_u();
726 double v0 = center.
get_v();
727 double cose = cos(e);
728 double sine = sin(e);
730 double u = a * cos(smallalpha);
731 double v = b * sin(smallalpha);
737 iP11.
set_uv(u0 + cose * u - sine * v, v0 + sine * u + cose * v);
740 for (
unsigned int i = 0; i < nbpoints; i++) {
747 iP22.
set_uv(u0 + cose * u - sine * v, v0 + sine * u + cose * v);
749 drawLine(I, iP11, iP22, color, thickness);
771 double size,
unsigned char color,
unsigned int thickness,
const vpImagePoint &offset)
788 drawArrow(I, ipo + offset, ip1 + offset, color, 4 * thickness, 2 * thickness, thickness);
791 drawArrow(I, ipo + offset, ip1 + offset, color, 4 * thickness, 2 * thickness, thickness);
794 drawArrow(I, ipo + offset, ip1 + offset, color, 4 * thickness, 2 * thickness, thickness);
843 drawArrow(I, ipo + offset, ip1 + offset, color, 4 * thickness, 2 * thickness, thickness);
846 drawArrow(I, ipo + offset, ip1 + offset, color, 4 * thickness, 2 * thickness, thickness);
849 drawArrow(I, ipo + offset, ip1 + offset, color, 4 * thickness, 2 * thickness, thickness);
861 unsigned char color,
unsigned int thickness)
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);
875 unsigned int thickness)
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);
889 unsigned int thickness)
915 unsigned int thickness,
bool closed)
917 DrawPolygon(I, vip, color, thickness, closed);
929 unsigned int thickness,
bool closed)
931 DrawPolygon(I, vip,
vpRGBa(color.
R, color.
G, color.
B), thickness, closed);
947 unsigned int thickness)
950 DrawFilledRectangle(I, rectangle, color);
953 DrawRectangle(I, rectangle, color, thickness);
970 unsigned int thickness)
973 DrawFilledRectangle(I, rectangle,
vpRGBa(color.
R, color.
G, color.
B));
976 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 functionalities.
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.
Class that defines a 2D circle in an image.
vpImagePoint getCenter() const
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 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 ...
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.