Visual Servoing Platform  version 3.6.1 under development (2024-04-22)
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 
120 
135 void vpSphere::projection(const vpColVector &cP_, vpColVector &p_) const
136 {
137  p_.resize(5, false);
138  double x0, y0, z0;
139  double E, A, B;
140 
141  // calcul des parametres M20, M11, M02 de l'ellipse
142  double s, r;
143  r = cP_[3];
144 
145  x0 = cP_[0];
146  y0 = cP_[1];
147  z0 = cP_[2];
148 
149  s = (r * r) - (y0 * y0) - (z0 * z0);
150 
151  if ((s = ((z0 * z0) - (r * r))) < 0.0) {
152  vpERROR_TRACE("Error: Sphere is behind image plane\n");
153  }
154 
155  p_[0] = (x0 * z0) / s; // x
156  p_[1] = (y0 * z0) / s; // y
157 
158  if (fabs(x0) > 1e-6) {
159  double e = y0 / x0;
160  double b = r / sqrt(s);
161  double a = ((x0 * x0) + (y0 * y0) + (z0 * z0)) - (r * r);
162  if (a < 0.0) {
163  vpERROR_TRACE("Error: Sphere is behind image plane\n");
164  }
165  a = (r * sqrt(a)) / s;
166  if (fabs(e) <= 1.0) {
167  E = e;
168  A = a;
169  B = b;
170  }
171  else {
172  E = -1.0 / e;
173  A = b;
174  B = a;
175  }
176  }
177  else {
178  E = 0.0;
179  A = r / sqrt(s);
180  B = (r * sqrt(((y0 * y0) + (z0 * z0)) - (r * r))) / s;
181  }
182 
183  // Chaumette PhD Thesis 1990, eq 2.72 divided by 4 since n_ij = mu_ij_chaumette_thesis / 4
184  double det = 4 * (1.0 + vpMath::sqr(E));
185  double n20 = (vpMath::sqr(A) + vpMath::sqr(B * E)) / det;
186  double n11 = ((vpMath::sqr(A) - vpMath::sqr(B)) * E) / det;
187  double n02 = (vpMath::sqr(B) + vpMath::sqr(A * E)) / det;
188 
189  p_[2] = n20;
190  p_[3] = n11;
191  p_[4] = n02;
192 }
193 
201 
210 {
211  cP_.resize(4, false);
212 
213  double x0, y0, z0; // variables intermediaires
214 
215  x0 = (cMo[0][0] * oP[0]) + (cMo[0][1] * oP[1]) + (cMo[0][2] * oP[2]) + cMo[0][3];
216  y0 = (cMo[1][0] * oP[0]) + (cMo[1][1] * oP[1]) + (cMo[1][2] * oP[2]) + cMo[1][3];
217  z0 = (cMo[2][0] * oP[0]) + (cMo[2][1] * oP[1]) + (cMo[2][2] * oP[2]) + cMo[2][3];
218 
219  cP_[3] = oP[3];
220 
221  cP_[0] = x0;
222  cP_[1] = y0;
223  cP_[2] = z0;
224 }
225 
228 {
229  vpSphere *feature = new vpSphere(*this);
230  return feature;
231 }
232 
245  const vpColor &color, unsigned int thickness)
246 {
247  vpColVector v_cP, v_p;
248  changeFrame(cMo, v_cP);
249  projection(v_cP, v_p);
250  vpFeatureDisplay::displayEllipse(v_p[0], v_p[1], v_p[2], v_p[3], v_p[4], cam, I, color, thickness);
251 }
252 
265  const vpColor &color, unsigned int thickness)
266 {
267  vpColVector v_cP, v_p;
268  changeFrame(cMo, v_cP);
269  projection(v_cP, v_p);
270  vpFeatureDisplay::displayEllipse(v_p[0], v_p[1], v_p[2], v_p[3], v_p[4], cam, I, color, thickness);
271 }
272 
281 void vpSphere::display(const vpImage<unsigned char> &I, const vpCameraParameters &cam, const vpColor &color,
282  unsigned int thickness)
283 {
284  vpFeatureDisplay::displayEllipse(p[0], p[1], p[2], p[3], p[4], cam, I, color, thickness);
285 }
286 
295 void vpSphere::display(const vpImage<vpRGBa> &I, const vpCameraParameters &cam, const vpColor &color,
296  unsigned int thickness)
297 {
298  vpFeatureDisplay::displayEllipse(p[0], p[1], p[2], p[3], p[4], cam, I, color, thickness);
299 }
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1056
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:201
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 projection() vp_override
Definition: vpSphere.cpp:119
void setWorldCoordinates(const vpColVector &oP) vp_override
Definition: vpSphere.cpp:59
void init() vp_override
Definition: vpSphere.cpp:42
vpSphere * duplicate() const vp_override
For memory issue (used by the vpServo class only).
Definition: vpSphere.cpp:227
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const vp_override
Definition: vpSphere.cpp:209
vpSphere()
Definition: vpSphere.cpp:81
void display(const vpImage< unsigned char > &I, const vpCameraParameters &cam, const vpColor &color=vpColor::green, unsigned int thickness=1) vp_override
Definition: vpSphere.cpp:281
vpColVector cP
Definition: vpTracker.h:71
vpColVector p
Definition: vpTracker.h:67
#define vpERROR_TRACE
Definition: vpDebug.h:382