Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
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
59void vpSphere::setWorldCoordinates(const vpColVector &oP_) { this->oP = oP_; }
60
70void 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
106vpSphere::vpSphere(double oX, double oY, double oZ, double R)
107{
108 init();
109 setWorldCoordinates(oX, oY, oZ, R);
110}
111
116
125
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
285 unsigned int thickness)
286{
287 vpFeatureDisplay::displayEllipse(p[0], p[1], p[2], p[3], p[4], cam, I, color, thickness);
288}
289
298void 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.
void resize(unsigned int i, bool flagNullify=true)
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.
Definition of the vpImage class member functions.
Definition vpImage.h:135
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
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