Visual Servoing Platform  version 3.2.0 under development (2019-01-22)
vpFeatureDisplay.cpp
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 
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
72 
73  vpDisplay::displayCross(I, ip, 15, color, thickness);
74 }
75 
86 void vpFeatureDisplay::displayLine(double rho, double theta, const vpCameraParameters &cam,
87  const vpImage<unsigned char> &I, const vpColor &color, unsigned int thickness)
88 {
89  // x cos(theta) + y sin(theta) - rho = 0
90  double rhop, thetap;
91  vpMeterPixelConversion::convertLine(cam, rho, theta, rhop, thetap);
92 
93  // u cos(thetap) + v sin(thetap) - rhop = 0
94  double co = cos(thetap);
95  double si = sin(thetap);
96  double c = -rhop;
97 
98  double a = si;
99  double b = co;
100  vpImagePoint ip1, ip2;
101 
102  if (fabs(a) < fabs(b)) {
103  ip1.set_ij(0, (-c) / b);
104  double h = I.getHeight() - 1;
105  ip2.set_ij(h, (-c - a * h) / b);
106  vpDisplay::displayLine(I, ip1, ip2, color, thickness);
107  } else {
108  ip1.set_ij((-c) / a, 0);
109  double w = I.getWidth() - 1;
110  ip2.set_ij((-c - b * w) / a, w);
111  vpDisplay::displayLine(I, ip1, ip2, color, thickness);
112  }
113 }
114 
126 void vpFeatureDisplay::displayCylinder(double rho1, double theta1, double rho2, double theta2,
127  const vpCameraParameters &cam, const vpImage<unsigned char> &I,
128  const vpColor &color, unsigned int thickness)
129 {
130  displayLine(rho1, theta1, cam, I, color, thickness);
131  displayLine(rho2, theta2, cam, I, color, thickness);
132 }
133 
149 void vpFeatureDisplay::displayEllipse(double x, double y, double mu20, double mu11, double mu02,
150  const vpCameraParameters &cam, const vpImage<unsigned char> &I,
151  const vpColor &color, unsigned int thickness)
152 {
153  vpImagePoint center;
154  double mu20_p, mu11_p, mu02_p;
155  vpCircle circle;
156  circle.p[0] = x;
157  circle.p[1] = y;
158  circle.p[2] = mu20;
159  circle.p[3] = mu11;
160  circle.p[4] = mu02;
161 
162  vpMeterPixelConversion::convertEllipse(cam, circle, center, mu20_p, mu11_p, mu02_p);
163  vpDisplay::displayEllipse(I, center, mu20_p, mu11_p, mu02_p, true, color, thickness);
164 }
165 
177 void vpFeatureDisplay::displayPoint(double x, double y, const vpCameraParameters &cam, const vpImage<vpRGBa> &I,
178  const vpColor &color, unsigned int thickness)
179 {
180  vpImagePoint ip; // pixel coordinates in float
182 
183  vpDisplay::displayCross(I, ip, 15, color, thickness);
184 }
185 
196 void vpFeatureDisplay::displayLine(double rho, double theta, const vpCameraParameters &cam, const vpImage<vpRGBa> &I,
197  const vpColor &color, unsigned int thickness)
198 {
199  // x cos(theta) + y sin(theta) - rho = 0
200  double rhop, thetap;
201  vpMeterPixelConversion::convertLine(cam, rho, theta, rhop, thetap);
202 
203  // u cos(thetap) + v sin(thetap) - rhop = 0
204  double co = cos(thetap);
205  double si = sin(thetap);
206  double c = -rhop;
207 
208  double a = si;
209  double b = co;
210  vpImagePoint ip1, ip2;
211 
212  if (fabs(a) < fabs(b)) {
213  ip1.set_ij(0, (-c) / b);
214  double h = I.getHeight() - 1;
215  ip2.set_ij(h, (-c - a * h) / b);
216  vpDisplay::displayLine(I, ip1, ip2, color, thickness);
217  } else {
218  ip1.set_ij((-c) / a, 0);
219  double w = I.getWidth() - 1;
220  ip2.set_ij((-c - b * w) / a, w);
221  vpDisplay::displayLine(I, ip1, ip2, color, thickness);
222  }
223 }
224 
236 void vpFeatureDisplay::displayCylinder(double rho1, double theta1, double rho2, double theta2,
237  const vpCameraParameters &cam, const vpImage<vpRGBa> &I, const vpColor &color,
238  unsigned int thickness)
239 {
240  displayLine(rho1, theta1, cam, I, color, thickness);
241  displayLine(rho2, theta2, cam, I, color, thickness);
242 }
243 
259 void vpFeatureDisplay::displayEllipse(double x, double y, double mu20, double mu11, double mu02,
260  const vpCameraParameters &cam, const vpImage<vpRGBa> &I, const vpColor &color,
261  unsigned int thickness)
262 {
263  vpImagePoint center;
264  double mu20_p, mu11_p, mu02_p;
265  vpCircle circle;
266  circle.p[0] = x;
267  circle.p[1] = y;
268  circle.p[2] = mu20;
269  circle.p[3] = mu11;
270  circle.p[4] = mu02;
271 
272  vpMeterPixelConversion::convertEllipse(cam, circle, center, mu20_p, mu11_p, mu02_p);
273  vpDisplay::displayEllipse(I, center, mu20_p, mu11_p, mu02_p, true, color, thickness);
274 }
static void displayEllipse(const vpImage< unsigned char > &I, const vpImagePoint &center, const double &coef1, const double &coef2, const double &coef3, bool use_centered_moments, const vpColor &color, unsigned int thickness=1)
static void convertEllipse(const vpCameraParameters &cam, const vpSphere &sphere, vpImagePoint &center, double &mu20_p, double &mu11_p, double &mu02_p)
static void displayEllipse(double x, double y, double mu20, double mu11, double m02, const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1)
unsigned int getWidth() const
Definition: vpImage.h:239
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 convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
Class to define colors available for display functionnalities.
Definition: vpColor.h:120
static void displayPoint(double x, double y, const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1)
Generic class defining intrinsic camera parameters.
static void convertLine(const vpCameraParameters &cam, const double &rho_m, const double &theta_m, double &rho_p, double &theta_p)
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, 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)
unsigned int getHeight() const
Definition: vpImage.h:178
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1)
Class that defines what is a circle.
Definition: vpCircle.h:58
vpColVector p
Definition: vpTracker.h:71
void set_ij(const double ii, const double jj)
Definition: vpImagePoint.h:189