Visual Servoing Platform  version 3.6.1 under development (2023-12-02)
vpFeatureDisplay.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  * Interface with the image for feature display.
33  *
34 *****************************************************************************/
35 
36 #include <visp3/core/vpFeatureDisplay.h>
37 
38 // Meter/pixel conversion
39 #include <visp3/core/vpMeterPixelConversion.h>
40 
41 // display
42 #include <visp3/core/vpDisplay.h>
43 
44 // Debug trace
45 #include <visp3/core/vpDebug.h>
46 
47 // math
48 #include <visp3/core/vpMath.h>
49 
50 #include <visp3/core/vpImagePoint.h>
51 
63 void vpFeatureDisplay::displayPoint(double x, double y, const vpCameraParameters &cam, const vpImage<unsigned char> &I,
64  const vpColor &color, unsigned int thickness)
65 {
66  vpImagePoint ip; // pixel coordinates in float
68  vpDisplay::displayCross(I, ip, 15, color, thickness);
69 }
70 
81 void vpFeatureDisplay::displayLine(double rho, double theta, const vpCameraParameters &cam,
82  const vpImage<unsigned char> &I, const vpColor &color, unsigned int thickness)
83 {
84  // x cos(theta) + y sin(theta) - rho = 0
85  double rhop, thetap;
86  vpMeterPixelConversion::convertLine(cam, rho, theta, rhop, thetap);
87 
88  // u cos(thetap) + v sin(thetap) - rhop = 0
89  double co = cos(thetap);
90  double si = sin(thetap);
91  double c = -rhop;
92 
93  double a = si;
94  double b = co;
95  vpImagePoint ip1, ip2;
96 
97  if (fabs(a) < fabs(b)) {
98  ip1.set_ij(0, (-c) / b);
99  double h = I.getHeight() - 1;
100  ip2.set_ij(h, (-c - a * h) / b);
101  vpDisplay::displayLine(I, ip1, ip2, color, thickness);
102  } else {
103  ip1.set_ij((-c) / a, 0);
104  double w = I.getWidth() - 1;
105  ip2.set_ij((-c - b * w) / a, w);
106  vpDisplay::displayLine(I, ip1, ip2, color, thickness);
107  }
108 }
109 
121 void vpFeatureDisplay::displayCylinder(double rho1, double theta1, double rho2, double theta2,
122  const vpCameraParameters &cam, const vpImage<unsigned char> &I,
123  const vpColor &color, unsigned int thickness)
124 {
125  displayLine(rho1, theta1, cam, I, color, thickness);
126  displayLine(rho2, theta2, cam, I, color, thickness);
127 }
128 
146 void vpFeatureDisplay::displayEllipse(double x, double y, double n20, double n11, double n02,
147  const vpCameraParameters &cam, const vpImage<unsigned char> &I,
148  const vpColor &color, unsigned int thickness)
149 {
150  vpImagePoint center;
151  double n20_p, n11_p, n02_p;
152  vpCircle circle;
153  circle.p[0] = x;
154  circle.p[1] = y;
155  circle.p[2] = n20;
156  circle.p[3] = n11;
157  circle.p[4] = n02;
158 
159  vpMeterPixelConversion::convertEllipse(cam, circle, center, n20_p, n11_p, n02_p);
160  vpDisplay::displayEllipse(I, center, n20_p, n11_p, n02_p, true, color, thickness);
161 }
162 
174 void vpFeatureDisplay::displayPoint(double x, double y, const vpCameraParameters &cam, const vpImage<vpRGBa> &I,
175  const vpColor &color, unsigned int thickness)
176 {
177  vpImagePoint ip; // pixel coordinates in float
179 
180  vpDisplay::displayCross(I, ip, 15, color, thickness);
181 }
182 
193 void vpFeatureDisplay::displayLine(double rho, double theta, const vpCameraParameters &cam, const vpImage<vpRGBa> &I,
194  const vpColor &color, unsigned int thickness)
195 {
196  // x cos(theta) + y sin(theta) - rho = 0
197  double rhop, thetap;
198  vpMeterPixelConversion::convertLine(cam, rho, theta, rhop, thetap);
199 
200  // u cos(thetap) + v sin(thetap) - rhop = 0
201  double co = cos(thetap);
202  double si = sin(thetap);
203  double c = -rhop;
204 
205  double a = si;
206  double b = co;
207  vpImagePoint ip1, ip2;
208 
209  if (fabs(a) < fabs(b)) {
210  ip1.set_ij(0, (-c) / b);
211  double h = I.getHeight() - 1;
212  ip2.set_ij(h, (-c - a * h) / b);
213  vpDisplay::displayLine(I, ip1, ip2, color, thickness);
214  } else {
215  ip1.set_ij((-c) / a, 0);
216  double w = I.getWidth() - 1;
217  ip2.set_ij((-c - b * w) / a, w);
218  vpDisplay::displayLine(I, ip1, ip2, color, thickness);
219  }
220 }
221 
233 void vpFeatureDisplay::displayCylinder(double rho1, double theta1, double rho2, double theta2,
234  const vpCameraParameters &cam, const vpImage<vpRGBa> &I, const vpColor &color,
235  unsigned int thickness)
236 {
237  displayLine(rho1, theta1, cam, I, color, thickness);
238  displayLine(rho2, theta2, cam, I, color, thickness);
239 }
240 
258 void vpFeatureDisplay::displayEllipse(double x, double y, double n20, double n11, double n02,
259  const vpCameraParameters &cam, const vpImage<vpRGBa> &I, const vpColor &color,
260  unsigned int thickness)
261 {
262  vpImagePoint center;
263  double n20_p, n11_p, n02_p;
264  vpCircle circle;
265  circle.p[0] = x;
266  circle.p[1] = y;
267  circle.p[2] = n20;
268  circle.p[3] = n11;
269  circle.p[4] = n02;
270 
271  vpMeterPixelConversion::convertEllipse(cam, circle, center, n20_p, n11_p, n02_p);
272  vpDisplay::displayEllipse(I, center, n20_p, n11_p, n02_p, true, color, thickness);
273 }
Generic class defining intrinsic camera parameters.
Class that defines a 3D circle in the object frame and allows forward projection of a 3D circle in th...
Definition: vpCircle.h:86
Class to define RGB colors available for display functionalities.
Definition: vpColor.h:152
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
static void displayEllipse(const vpImage< unsigned char > &I, const vpImagePoint &center, const double &coef1, const double &coef2, const double &coef3, bool use_normalized_centered_moments, const vpColor &color, unsigned int thickness=1, bool display_center=false, bool display_arc=false)
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
static void displayCylinder(double rho1, double theta1, double rho2, double theta2, const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1)
static void displayLine(double rho, double theta, const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1)
static void displayEllipse(double x, double y, double n20, double n11, double n02, const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1)
static void displayPoint(double x, double y, const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, 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_ij(double ii, double jj)
Definition: vpImagePoint.h:305
unsigned int getWidth() const
Definition: vpImage.h:240
unsigned int getHeight() const
Definition: vpImage.h:182
static void convertLine(const vpCameraParameters &cam, const double &rho_m, const double &theta_m, double &rho_p, double &theta_p)
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
static void convertEllipse(const vpCameraParameters &cam, const vpSphere &sphere, vpImagePoint &center_p, double &n20_p, double &n11_p, double &n02_p)
vpColVector p
Definition: vpTracker.h:67