1 /****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2019 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 http://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 * Authors:
35 * Eric Marchand
36 * Fabien Spindler
37 *
38 *****************************************************************************/
39
40 #include <visp3/core/vpFeatureDisplay.h>
41
42 // Meter/pixel conversion
43 #include <visp3/core/vpMeterPixelConversion.h>
44
45 // display
46 #include <visp3/core/vpDisplay.h>
47
48 // Debug trace
49 #include <visp3/core/vpDebug.h>
50
51 // math
52 #include <visp3/core/vpMath.h>
53
54 #include <visp3/core/vpImagePoint.h>
55
56 /*!
57 Display a 2D point with coordinates (x, y) expressed in the image plane.
58 These coordinates are obtained after perspective projection of the point.
59
60 \param x, y : Point coordinates in the image plane.
61 \param cam : Camera intrinsic parameters.
62 \param I : Image.
63
64 \param color : Color to use to display the feature.
65 \param thickness : Thickness of the feature representation.
66 */
displayPoint(double x,double y,const vpCameraParameters & cam,const vpImage<unsigned char> & I,const vpColor & color,unsigned int thickness)67 void vpFeatureDisplay::displayPoint(double x, double y, const vpCameraParameters &cam, const vpImage<unsigned char> &I,
68 const vpColor &color, unsigned int thickness)
69 {
70 vpImagePoint ip; // pixel coordinates in float
71 vpMeterPixelConversion::convertPoint(cam, x, y, ip);
72 vpDisplay::displayCross(I, ip, 15, color, thickness);
73 }
74
75 /*!
76 Display a 2D line with coordinates \f$(\rho, \theta)\f$ expressed in the image plane.
77 These coordinates are obtained after perspective projection of the line.
78
79 \param rho, theta : Line parameters \f$(\rho, \theta)\f$ expressed in the image plane.
80 \param cam : Camera intrinsic parameters.
81 \param I : Image.
82 \param color : Color to use to display the feature.
83 \param thickness : Thickness of the feature representation.
84 */
displayLine(double rho,double theta,const vpCameraParameters & cam,const vpImage<unsigned char> & I,const vpColor & color,unsigned int thickness)85 void vpFeatureDisplay::displayLine(double rho, double theta, const vpCameraParameters &cam,
86 const vpImage<unsigned char> &I, const vpColor &color, unsigned int thickness)
87 {
88 // x cos(theta) + y sin(theta) - rho = 0
89 double rhop, thetap;
90 vpMeterPixelConversion::convertLine(cam, rho, theta, rhop, thetap);
91
92 // u cos(thetap) + v sin(thetap) - rhop = 0
93 double co = cos(thetap);
94 double si = sin(thetap);
95 double c = -rhop;
96
97 double a = si;
98 double b = co;
99 vpImagePoint ip1, ip2;
100
101 if (fabs(a) < fabs(b)) {
102 ip1.set_ij(0, (-c) / b);
103 double h = I.getHeight() - 1;
104 ip2.set_ij(h, (-c - a * h) / b);
105 vpDisplay::displayLine(I, ip1, ip2, color, thickness);
106 } else {
107 ip1.set_ij((-c) / a, 0);
108 double w = I.getWidth() - 1;
109 ip2.set_ij((-c - b * w) / a, w);
110 vpDisplay::displayLine(I, ip1, ip2, color, thickness);
111 }
112 }
113
114 /*!
115 Display cylinder limbs as two 2D lines with coordinates \f$(\rho, \theta)\f$ expressed in the image plane.
116 These coordinates are obtained after perspective projection of the cylinder.
117
118 \param rho1, theta1 : Cylinder first limb parameters \f$(\rho, \theta)\f$ expressed in the image plane.
119 \param rho2, theta2 : Cylinder second limb parameters \f$(\rho, \theta)\f$ expressed in the image plane.
120 \param cam : Camera intrinsic parameters.
121 \param I : Image.
122 \param color : Color to use to display the feature.
123 \param thickness : Thickness of the feature representation.
124 */
displayCylinder(double rho1,double theta1,double rho2,double theta2,const vpCameraParameters & cam,const vpImage<unsigned char> & I,const vpColor & color,unsigned int thickness)125 void vpFeatureDisplay::displayCylinder(double rho1, double theta1, double rho2, double theta2,
126 const vpCameraParameters &cam, const vpImage<unsigned char> &I,
127 const vpColor &color, unsigned int thickness)
128 {
129 displayLine(rho1, theta1, cam, I, color, thickness);
130 displayLine(rho2, theta2, cam, I, color, thickness);
131 }
132
133 /*!
134 Display an ellipse with parameters \f$(x, y, n_{20}, n_{11}, n_{02})\f$ expressed in the image plane.
135 These parameters are obtained after perspective projection of a 3D circle (see vpCircle) or a sphere (see vpSphere).
136
137 \param x, y, n20, n11, n02 : Ellipse parameters where:
138 - \f$(x,y)\f$ are the normalized coordinates of the ellipse center,
139 respectively along the horizontal and vertical axis in the image plane.
140 - \f$n_{20}, n_{11}, n_{02}\f$ are the second order centered moments of the ellipse normalized
141 by its area (i.e., such that \f$n_{ij} = \mu_{ij}/a\f$ where
142 \f$\mu_{ij}\f$ are the centered moments and a the area) expressed in the image plane.
143 \param cam : Camera intrinsic parameters.
144 \param I : Image.
145 \param color : Color to use to display the feature.
146 \param thickness : Thickness of the feature representation.
147
148 \sa vpDisplay::displayEllipse()
149 */
displayEllipse(double x,double y,double n20,double n11,double n02,const vpCameraParameters & cam,const vpImage<unsigned char> & I,const vpColor & color,unsigned int thickness)150 void vpFeatureDisplay::displayEllipse(double x, double y, double n20, double n11, double n02,
151 const vpCameraParameters &cam, const vpImage<unsigned char> &I,
152 const vpColor &color, unsigned int thickness)
153 {
154 vpImagePoint center;
155 double n20_p, n11_p, n02_p;
156 vpCircle circle;
157 circle.p[0] = x;
158 circle.p[1] = y;
159 circle.p[2] = n20;
160 circle.p[3] = n11;
161 circle.p[4] = n02;
162
163 vpMeterPixelConversion::convertEllipse(cam, circle, center, n20_p, n11_p, n02_p);
164 vpDisplay::displayEllipse(I, center, n20_p, n11_p, n02_p, true, color, thickness);
165 }
166
167 /*!
168 Display a 2D point with coordinates (x, y) expressed in the image plane.
169 These coordinates are obtained after perspective projection of the point.
170
171 \param x, y : Point coordinates in the image plane.
172 \param cam : Camera intrinsic parameters.
173 \param I : Image.
174
175 \param color : Color to use to display the feature.
176 \param thickness : Thickness of the feature representation.
177 */
displayPoint(double x,double y,const vpCameraParameters & cam,const vpImage<vpRGBa> & I,const vpColor & color,unsigned int thickness)178 void vpFeatureDisplay::displayPoint(double x, double y, const vpCameraParameters &cam, const vpImage<vpRGBa> &I,
179 const vpColor &color, unsigned int thickness)
180 {
181 vpImagePoint ip; // pixel coordinates in float
182 vpMeterPixelConversion::convertPoint(cam, x, y, ip);
183
184 vpDisplay::displayCross(I, ip, 15, color, thickness);
185 }
186
187 /*!
188 Display a 2D line with coordinates \f$(\rho, \theta)\f$ expressed in the image plane.
189 These coordinates are obtained after perspective projection of the line.
190
191 \param rho, theta : Line parameters \f$(\rho, \theta)\f$ expressed in the image plane.
192 \param cam : Camera intrinsic parameters.
193 \param I : Image.
194 \param color : Color to use to display the feature.
195 \param thickness : Thickness of the feature representation.
196 */
displayLine(double rho,double theta,const vpCameraParameters & cam,const vpImage<vpRGBa> & I,const vpColor & color,unsigned int thickness)197 void vpFeatureDisplay::displayLine(double rho, double theta, const vpCameraParameters &cam, const vpImage<vpRGBa> &I,
198 const vpColor &color, unsigned int thickness)
199 {
200 // x cos(theta) + y sin(theta) - rho = 0
201 double rhop, thetap;
202 vpMeterPixelConversion::convertLine(cam, rho, theta, rhop, thetap);
203
204 // u cos(thetap) + v sin(thetap) - rhop = 0
205 double co = cos(thetap);
206 double si = sin(thetap);
207 double c = -rhop;
208
209 double a = si;
210 double b = co;
211 vpImagePoint ip1, ip2;
212
213 if (fabs(a) < fabs(b)) {
214 ip1.set_ij(0, (-c) / b);
215 double h = I.getHeight() - 1;
216 ip2.set_ij(h, (-c - a * h) / b);
217 vpDisplay::displayLine(I, ip1, ip2, color, thickness);
218 } else {
219 ip1.set_ij((-c) / a, 0);
220 double w = I.getWidth() - 1;
221 ip2.set_ij((-c - b * w) / a, w);
222 vpDisplay::displayLine(I, ip1, ip2, color, thickness);
223 }
224 }
225
226 /*!
227 Display cylinder limbs as two 2D lines with coordinates \f$(\rho, \theta)\f$ expressed in the image plane.
228 These coordinates are obtained after perspective projection of the cylinder.
229
230 \param rho1, theta1 : Cylinder first limb parameters \f$(\rho, \theta)\f$ expressed in the image plane.
231 \param rho2, theta2 : Cylinder second limb parameters \f$(\rho, \theta)\f$ expressed in the image plane.
232 \param cam : Camera intrinsic parameters.
233 \param I : Image.
234 \param color : Color to use to display the feature.
235 \param thickness : Thickness of the feature representation.
236 */
displayCylinder(double rho1,double theta1,double rho2,double theta2,const vpCameraParameters & cam,const vpImage<vpRGBa> & I,const vpColor & color,unsigned int thickness)237 void vpFeatureDisplay::displayCylinder(double rho1, double theta1, double rho2, double theta2,
238 const vpCameraParameters &cam, const vpImage<vpRGBa> &I, const vpColor &color,
239 unsigned int thickness)
240 {
241 displayLine(rho1, theta1, cam, I, color, thickness);
242 displayLine(rho2, theta2, cam, I, color, thickness);
243 }
244
245 /*!
246 Display an ellipse with parameters \f$(x, y, n_{20}, n_{11}, n_{02})\f$ expressed in the image plane.
247 These parameters are obtained after perspective projection of a 3D circle (see vpCircle) or a sphere (see vpSphere).
248
249 \param x, y, n20, n11, n02 : Ellipse parameters where:
250 - \f$(x,y)\f$ are the normalized coordinates of the ellipse center,
251 respectively along the horizontal and vertical axis in the image plane.
252 - \f$n_{20}, n_{11}, n_{02}\f$ are the second order centered moments of the ellipse normalized
253 by its area (i.e., such that \f$n_{ij} = \mu_{ij}/a\f$ where
254 \f$\mu_{ij}\f$ are the centered moments and a the area) expressed in the image plane.
255 \param cam : Camera intrinsic parameters.
256 \param I : Image.
257 \param color : Color to use to display the feature.
258 \param thickness : Thickness of the feature representation.
259
260 \sa vpDisplay::displayEllipse()
261 */
displayEllipse(double x,double y,double n20,double n11,double n02,const vpCameraParameters & cam,const vpImage<vpRGBa> & I,const vpColor & color,unsigned int thickness)262 void vpFeatureDisplay::displayEllipse(double x, double y, double n20, double n11, double n02,
263 const vpCameraParameters &cam, const vpImage<vpRGBa> &I, const vpColor &color,
264 unsigned int thickness)
265 {
266 vpImagePoint center;
267 double n20_p, n11_p, n02_p;
268 vpCircle circle;
269 circle.p[0] = x;
270 circle.p[1] = y;
271 circle.p[2] = n20;
272 circle.p[3] = n11;
273 circle.p[4] = n02;
274
275 vpMeterPixelConversion::convertEllipse(cam, circle, center, n20_p, n11_p, n02_p);
276 vpDisplay::displayEllipse(I, center, n20_p, n11_p, n02_p, true, color, thickness);
277 }
278