Visual Servoing Platform  version 3.2.0 under development (2019-01-22)
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 
43 {
44 
45  oP.resize(4);
46  cP.resize(4);
47 
48  p.resize(5);
49 }
50 
51 void vpSphere::setWorldCoordinates(const vpColVector &oP_) { this->oP = oP_; }
52 
53 void vpSphere::setWorldCoordinates(const double X0, const double Y0, const double Z0, const double R)
54 {
55  oP[0] = X0;
56  oP[1] = Y0;
57  oP[2] = Z0;
58  oP[3] = R;
59 }
60 
62 
64 {
65  init();
67 }
68 
69 vpSphere::vpSphere(const double X0, const double Y0, const double Z0, const double R)
70 {
71  init();
72  setWorldCoordinates(X0, Y0, Z0, R);
73 }
74 
76 
79 
82 {
83  double x0, y0, z0; // variables intermediaires
84  // double k0, k1, k2, k3, k4; //variables intermediaires
85  double E, A, B; // variables intermediaires
86 
87  // calcul des parametres M20, M11, M02 de l'ellipse
88  double s, r; // variables intermediaires
89  r = cP_[3];
90 
91  x0 = cP_[0];
92  y0 = cP_[1];
93  z0 = cP_[2];
94 
95  s = r * r - y0 * y0 - z0 * z0;
96 
97  // k0 = (r*r - x0*x0 -z0*z0)/s;
98  // k1 = (x0*y0)/s;
99  // k2 = (x0*z0)/s;
100  // k3 = (y0*z0)/s;
101  // k4 = (r*r - x0*x0 -y0*y0)/s;
102 
103  if ((s = z0 * z0 - r * r) < 0.0) {
104  vpERROR_TRACE("sphere derriere le plan image\n");
105  }
106 
107  p_[0] = x0 * z0 / s; // x
108  p_[1] = y0 * z0 / s; // y
109 
110  if (fabs(x0) > 1e-6) {
111  // vpERROR_TRACE(" %f",r) ;
112  double e = y0 / x0;
113  double b = r / sqrt(s);
114  double a = x0 * x0 + y0 * y0 + z0 * z0 - r * r;
115  if (a < 0.0) {
116  vpERROR_TRACE("sphere derriere le plan image\n");
117  }
118  a = r * sqrt(a) / s;
119  if (fabs(e) <= 1.0) {
120  E = e;
121  A = a;
122  B = b;
123  } else {
124  E = -1.0 / e;
125  A = b;
126  B = a;
127  }
128  } else {
129  // vpERROR_TRACE(" %f",r) ;
130  E = 0.0;
131  A = r / sqrt(s);
132  B = r * sqrt(y0 * y0 + z0 * z0 - r * r) / s;
133  }
134 
135  p_[2] = (A * A + B * B * E * E) / (1.0 + E * E); // mu20
136  p_[3] = (A * A - B * B) * E / (1.0 + E * E); // mu11
137  p_[4] = (B * B + A * A * E * E) / (1.0 + E * E); // mu02
138 
139  // vpERROR_TRACE(" %f",r) ;
140 
141  // std::cout << p.t() ;
142 }
145 
148 {
149  double x0, y0, z0; // variables intermediaires
150 
151  x0 = cMo[0][0] * oP[0] + cMo[0][1] * oP[1] + cMo[0][2] * oP[2] + cMo[0][3];
152  y0 = cMo[1][0] * oP[0] + cMo[1][1] * oP[1] + cMo[1][2] * oP[2] + cMo[1][3];
153  z0 = cMo[2][0] * oP[0] + cMo[2][1] * oP[1] + cMo[2][2] * oP[2] + cMo[2][3];
154 
155  cP_[3] = oP[3];
156 
157  cP_[0] = x0;
158  cP_[1] = y0;
159  cP_[2] = z0;
160 }
161 
164 {
165  vpSphere *feature = new vpSphere(*this);
166  return feature;
167 }
168 
169 // non destructive wrt. cP and p
171  const vpColor &color, const unsigned int thickness)
172 {
173  vpColVector _cP, _p;
174  changeFrame(cMo, _cP);
175  projection(_cP, _p);
176  vpFeatureDisplay::displayEllipse(_p[0], _p[1], _p[2], _p[3], _p[4], cam, I, color, thickness);
177 }
178 
179 void vpSphere::display(const vpImage<unsigned char> &I, const vpCameraParameters &cam, const vpColor &color,
180  const unsigned int thickness)
181 {
182  vpFeatureDisplay::displayEllipse(p[0], p[1], p[2], p[3], p[4], cam, I, color, thickness);
183 }
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)
Implementation of an homogeneous matrix and operations on such kind of matrices.
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP)
Perspective projection of the circle.
Definition: vpSphere.cpp:147
vpSphere * duplicate() const
for memory issue (used by the vpServo class only)
Definition: vpSphere.cpp:163
#define vpERROR_TRACE
Definition: vpDebug.h:393
Class to define colors available for display functionnalities.
Definition: vpColor.h:120
virtual ~vpSphere()
Definition: vpSphere.cpp:75
Class that defines what is a sphere.
Definition: vpSphere.h:60
void setWorldCoordinates(const vpColVector &oP)
Definition: vpSphere.cpp:51
vpColVector cP
Definition: vpTracker.h:75
void projection()
perspective projection of the sphere
Definition: vpSphere.cpp:78
void display(const vpImage< unsigned char > &I, const vpCameraParameters &cam, const vpColor &color=vpColor::green, const unsigned int thickness=1)
Definition: vpSphere.cpp:179
Generic class defining intrinsic camera parameters.
vpSphere()
Definition: vpSphere.cpp:61
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
void init()
Definition: vpSphere.cpp:42
vpColVector p
Definition: vpTracker.h:71
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:244