Visual Servoing Platform  version 3.4.1 under development (2021-10-26)
vpSphere.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  * Sphere feature.
33  *
34  * Authors:
35  * Eric Marchand
36  *
37  *****************************************************************************/
38 
39 #include <visp3/core/vpFeatureDisplay.h>
40 #include <visp3/core/vpSphere.h>
41 
46 {
47  oP.resize(4);
48  cP.resize(4);
49 
50  p.resize(5);
51 }
52 
62 void vpSphere::setWorldCoordinates(const vpColVector &oP_) { this->oP = oP_; }
63 
73 void vpSphere::setWorldCoordinates(double oX, double oY, double oZ, double R)
74 {
75  oP[0] = oX;
76  oP[1] = oY;
77  oP[2] = oZ;
78  oP[3] = R;
79 }
80 
85 
95 {
96  init();
98 }
99 
109 vpSphere::vpSphere(double oX, double oY, double oZ, double R)
110 {
111  init();
112  setWorldCoordinates(oX, oY, oZ, R);
113 }
114 
119 
128 
143 void vpSphere::projection(const vpColVector &cP_, vpColVector &p_) const
144 {
145  p_.resize(5, false);
146  double x0, y0, z0;
147  double E, A, B;
148 
149  // calcul des parametres M20, M11, M02 de l'ellipse
150  double s, r;
151  r = cP_[3];
152 
153  x0 = cP_[0];
154  y0 = cP_[1];
155  z0 = cP_[2];
156 
157  s = r * r - y0 * y0 - z0 * z0;
158 
159  if ((s = z0 * z0 - r * r) < 0.0) {
160  vpERROR_TRACE("Error: Sphere is behind image plane\n");
161  }
162 
163  p_[0] = x0 * z0 / s; // x
164  p_[1] = y0 * z0 / s; // y
165 
166  if (fabs(x0) > 1e-6) {
167  double e = y0 / x0;
168  double b = r / sqrt(s);
169  double a = x0 * x0 + y0 * y0 + z0 * z0 - r * r;
170  if (a < 0.0) {
171  vpERROR_TRACE("Error: Sphere is behind image plane\n");
172  }
173  a = r * sqrt(a) / s;
174  if (fabs(e) <= 1.0) {
175  E = e;
176  A = a;
177  B = b;
178  } else {
179  E = -1.0 / e;
180  A = b;
181  B = a;
182  }
183  } else {
184  E = 0.0;
185  A = r / sqrt(s);
186  B = r * sqrt(y0 * y0 + z0 * z0 - r * r) / s;
187  }
188 
189  // Chaumette PhD Thesis 1990, eq 2.72 divided by 4 since n_ij = mu_ij_chaumette_thesis / 4
190  double det = 4 * (1.0 + vpMath::sqr(E));
191  double n20 = (vpMath::sqr(A) + vpMath::sqr(B * E)) / det;
192  double n11 = (vpMath::sqr(A) - vpMath::sqr(B)) * E / det;
193  double n02 = (vpMath::sqr(B) + vpMath::sqr(A * E)) / det;
194 
195  p_[2] = n20;
196  p_[3] = n11;
197  p_[4] = n02;
198 }
199 
207 
216 {
217  cP_.resize(4, false);
218 
219  double x0, y0, z0; // variables intermediaires
220 
221  x0 = cMo[0][0] * oP[0] + cMo[0][1] * oP[1] + cMo[0][2] * oP[2] + cMo[0][3];
222  y0 = cMo[1][0] * oP[0] + cMo[1][1] * oP[1] + cMo[1][2] * oP[2] + cMo[1][3];
223  z0 = cMo[2][0] * oP[0] + cMo[2][1] * oP[1] + cMo[2][2] * oP[2] + cMo[2][3];
224 
225  cP_[3] = oP[3];
226 
227  cP_[0] = x0;
228  cP_[1] = y0;
229  cP_[2] = z0;
230 }
231 
234 {
235  vpSphere *feature = new vpSphere(*this);
236  return feature;
237 }
238 
251  const vpColor &color, unsigned int thickness)
252 {
253  vpColVector _cP, _p;
254  changeFrame(cMo, _cP);
255  projection(_cP, _p);
256  vpFeatureDisplay::displayEllipse(_p[0], _p[1], _p[2], _p[3], _p[4], cam, I, color, thickness);
257 }
258 
271  const vpColor &color, unsigned int thickness)
272 {
273  vpColVector _cP, _p;
274  changeFrame(cMo, _cP);
275  projection(_cP, _p);
276  vpFeatureDisplay::displayEllipse(_p[0], _p[1], _p[2], _p[3], _p[4], cam, I, color, thickness);
277 }
278 
287 void vpSphere::display(const vpImage<unsigned char> &I, const vpCameraParameters &cam, const vpColor &color,
288  unsigned int thickness)
289 {
290  vpFeatureDisplay::displayEllipse(p[0], p[1], p[2], p[3], p[4], cam, I, color, thickness);
291 }
292 
301 void vpSphere::display(const vpImage<vpRGBa> &I, const vpCameraParameters &cam, const vpColor &color,
302  unsigned int thickness)
303 {
304  vpFeatureDisplay::displayEllipse(p[0], p[1], p[2], p[3], p[4], cam, I, color, thickness);
305 }
Implementation of an homogeneous matrix and operations on such kind of matrices.
#define vpERROR_TRACE
Definition: vpDebug.h:393
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:157
vpSphere * duplicate() const
For memory issue (used by the vpServo class only).
Definition: vpSphere.cpp:233
virtual ~vpSphere()
Definition: vpSphere.cpp:118
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const
Definition: vpSphere.cpp:215
Class that defines a 3D sphere in the object frame and allows forward projection of a 3D sphere in th...
Definition: vpSphere.h:82
void setWorldCoordinates(const vpColVector &oP)
Definition: vpSphere.cpp:62
vpColVector cP
Definition: vpTracker.h:77
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)
void projection()
Definition: vpSphere.cpp:127
static double sqr(double x)
Definition: vpMath.h:116
Generic class defining intrinsic camera parameters.
vpSphere()
Definition: vpSphere.cpp:84
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
void init()
Definition: vpSphere.cpp:45
void display(const vpImage< unsigned char > &I, const vpCameraParameters &cam, const vpColor &color=vpColor::green, unsigned int thickness=1)
Definition: vpSphere.cpp:287
vpColVector p
Definition: vpTracker.h:73