Visual Servoing Platform  version 3.6.0 under development (2023-09-29)
vpSphere.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  * Sphere feature.
33  *
34 *****************************************************************************/
35 
36 #include <visp3/core/vpFeatureDisplay.h>
37 #include <visp3/core/vpSphere.h>
38 
43 {
44  oP.resize(4);
45  cP.resize(4);
46 
47  p.resize(5);
48 }
49 
59 void vpSphere::setWorldCoordinates(const vpColVector &oP_) { this->oP = oP_; }
60 
70 void vpSphere::setWorldCoordinates(double oX, double oY, double oZ, double R)
71 {
72  oP[0] = oX;
73  oP[1] = oY;
74  oP[2] = oZ;
75  oP[3] = R;
76 }
77 
82 
92 {
93  init();
95 }
96 
106 vpSphere::vpSphere(double oX, double oY, double oZ, double R)
107 {
108  init();
109  setWorldCoordinates(oX, oY, oZ, R);
110 }
111 
116 
125 
140 void vpSphere::projection(const vpColVector &cP_, vpColVector &p_) const
141 {
142  p_.resize(5, false);
143  double x0, y0, z0;
144  double E, A, B;
145 
146  // calcul des parametres M20, M11, M02 de l'ellipse
147  double s, r;
148  r = cP_[3];
149 
150  x0 = cP_[0];
151  y0 = cP_[1];
152  z0 = cP_[2];
153 
154  s = r * r - y0 * y0 - z0 * z0;
155 
156  if ((s = z0 * z0 - r * r) < 0.0) {
157  vpERROR_TRACE("Error: Sphere is behind image plane\n");
158  }
159 
160  p_[0] = x0 * z0 / s; // x
161  p_[1] = y0 * z0 / s; // y
162 
163  if (fabs(x0) > 1e-6) {
164  double e = y0 / x0;
165  double b = r / sqrt(s);
166  double a = x0 * x0 + y0 * y0 + z0 * z0 - r * r;
167  if (a < 0.0) {
168  vpERROR_TRACE("Error: Sphere is behind image plane\n");
169  }
170  a = r * sqrt(a) / s;
171  if (fabs(e) <= 1.0) {
172  E = e;
173  A = a;
174  B = b;
175  } else {
176  E = -1.0 / e;
177  A = b;
178  B = a;
179  }
180  } else {
181  E = 0.0;
182  A = r / sqrt(s);
183  B = r * sqrt(y0 * y0 + z0 * z0 - r * r) / s;
184  }
185 
186  // Chaumette PhD Thesis 1990, eq 2.72 divided by 4 since n_ij = mu_ij_chaumette_thesis / 4
187  double det = 4 * (1.0 + vpMath::sqr(E));
188  double n20 = (vpMath::sqr(A) + vpMath::sqr(B * E)) / det;
189  double n11 = (vpMath::sqr(A) - vpMath::sqr(B)) * E / det;
190  double n02 = (vpMath::sqr(B) + vpMath::sqr(A * E)) / det;
191 
192  p_[2] = n20;
193  p_[3] = n11;
194  p_[4] = n02;
195 }
196 
204 
213 {
214  cP_.resize(4, false);
215 
216  double x0, y0, z0; // variables intermediaires
217 
218  x0 = cMo[0][0] * oP[0] + cMo[0][1] * oP[1] + cMo[0][2] * oP[2] + cMo[0][3];
219  y0 = cMo[1][0] * oP[0] + cMo[1][1] * oP[1] + cMo[1][2] * oP[2] + cMo[1][3];
220  z0 = cMo[2][0] * oP[0] + cMo[2][1] * oP[1] + cMo[2][2] * oP[2] + cMo[2][3];
221 
222  cP_[3] = oP[3];
223 
224  cP_[0] = x0;
225  cP_[1] = y0;
226  cP_[2] = z0;
227 }
228 
231 {
232  vpSphere *feature = new vpSphere(*this);
233  return feature;
234 }
235 
248  const vpColor &color, unsigned int thickness)
249 {
250  vpColVector _cP, _p;
251  changeFrame(cMo, _cP);
252  projection(_cP, _p);
253  vpFeatureDisplay::displayEllipse(_p[0], _p[1], _p[2], _p[3], _p[4], cam, I, color, thickness);
254 }
255 
268  const vpColor &color, unsigned int thickness)
269 {
270  vpColVector _cP, _p;
271  changeFrame(cMo, _cP);
272  projection(_cP, _p);
273  vpFeatureDisplay::displayEllipse(_p[0], _p[1], _p[2], _p[3], _p[4], cam, I, color, thickness);
274 }
275 
284 void vpSphere::display(const vpImage<unsigned char> &I, const vpCameraParameters &cam, const vpColor &color,
285  unsigned int thickness)
286 {
287  vpFeatureDisplay::displayEllipse(p[0], p[1], p[2], p[3], p[4], cam, I, color, thickness);
288 }
289 
298 void vpSphere::display(const vpImage<vpRGBa> &I, const vpCameraParameters &cam, const vpColor &color,
299  unsigned int thickness)
300 {
301  vpFeatureDisplay::displayEllipse(p[0], p[1], p[2], p[3], p[4], cam, I, color, thickness);
302 }
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:167
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:351
Class to define RGB colors available for display functionalities.
Definition: vpColor.h:152
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)
Implementation of an homogeneous matrix and operations on such kind of matrices.
static double sqr(double x)
Definition: vpMath.h:124
Class that defines a 3D sphere in the object frame and allows forward projection of a 3D sphere in th...
Definition: vpSphere.h:78
void init()
Definition: vpSphere.cpp:42
void display(const vpImage< unsigned char > &I, const vpCameraParameters &cam, const vpColor &color=vpColor::green, unsigned int thickness=1)
Definition: vpSphere.cpp:284
void setWorldCoordinates(const vpColVector &oP)
Definition: vpSphere.cpp:59
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const
Definition: vpSphere.cpp:212
void projection()
Definition: vpSphere.cpp:124
vpSphere()
Definition: vpSphere.cpp:81
virtual ~vpSphere()
Definition: vpSphere.cpp:115
vpSphere * duplicate() const
For memory issue (used by the vpServo class only).
Definition: vpSphere.cpp:230
vpColVector cP
Definition: vpTracker.h:72
vpColVector p
Definition: vpTracker.h:68
#define vpERROR_TRACE
Definition: vpDebug.h:388