Visual Servoing Platform  version 3.1.0
vpFeatureBuilderEllipse.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 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  * Conversion between tracker and visual feature ellipse.
33  *
34  * Authors:
35  * Eric Marchand
36  *
37  *****************************************************************************/
38 
45 #include <visp3/core/vpMath.h>
46 #include <visp3/visual_features/vpFeatureBuilder.h>
47 
63 {
64  try {
65 
66  // 3D data
67  double alpha = t.cP[0];
68  double beta = t.cP[1];
69  double gamma = t.cP[2];
70 
71  double X0 = t.cP[3];
72  double Y0 = t.cP[4];
73  double Z0 = t.cP[5];
74 
75  // equation p 318 prior eq (39)
76  double d = alpha * X0 + beta * Y0 + gamma * Z0;
77 
78  double A = alpha / d;
79  double B = beta / d;
80  double C = gamma / d;
81 
82  s.setABC(A, B, C);
83 
84  // 2D data
85  s.buildFrom(t.p[0], t.p[1], t.p[2], t.p[3], t.p[4]);
86 
87  } catch (...) {
88  vpERROR_TRACE("Error caught");
89  throw;
90  }
91 }
92 
108 {
109  try {
110 
111  // 3D data
112  double X0 = t.cP[0];
113  double Y0 = t.cP[1];
114  double Z0 = t.cP[2];
115  double R = t.cP[3];
116 
117  double d = vpMath::sqr(X0) + vpMath::sqr(Y0) + vpMath::sqr(Z0) - vpMath::sqr(R);
118 
119  double A = X0 / d;
120  double B = Y0 / d;
121  double C = Z0 / d;
122 
123  s.setABC(A, B, C);
124 
125  // 2D data
126  s.buildFrom(t.p[0], t.p[1], t.p[2], t.p[3], t.p[4]);
127 
128  } catch (...) {
129  vpERROR_TRACE("Error caught");
130  throw;
131  }
132 }
133 
134 #ifdef VISP_HAVE_MODULE_BLOB
135 
153 {
154  try {
155 
156  unsigned int order = 3;
157  vpMatrix mp(order, order);
158  mp = 0;
159  vpMatrix m(order, order);
160  m = 0;
161 
162  mp[0][0] = t.m00;
163  mp[1][0] = t.m10;
164  mp[0][1] = t.m01;
165  mp[2][0] = t.m20;
166  mp[1][1] = t.m11;
167  mp[0][2] = t.m02;
168 
169  vpPixelMeterConversion::convertMoment(cam, order, mp, m);
170 
171  double m00 = m[0][0];
172  double m01 = m[0][1];
173  double m10 = m[1][0];
174  double m02 = m[0][2];
175  double m11 = m[1][1];
176  double m20 = m[2][0];
177 
178  double xc = m10 / m00; // sum j /S
179  double yc = m01 / m00; // sum i /S
180 
181  double mu20 = 4 * (m20 - m00 * vpMath::sqr(xc)) / (m00);
182  double mu02 = 4 * (m02 - m00 * vpMath::sqr(yc)) / (m00);
183  double mu11 = 4 * (m11 - m00 * xc * yc) / (m00);
184 
185  s.buildFrom(xc, yc, mu20, mu11, mu02);
186  } catch (...) {
187  vpERROR_TRACE("Error caught");
188  throw;
189  }
190 }
191 
210 {
211  try {
212 
213  unsigned int order = 3;
214  vpMatrix mp(order, order);
215  mp = 0;
216  vpMatrix m(order, order);
217  m = 0;
218 
219  mp[0][0] = t.m00;
220  mp[1][0] = t.m10;
221  mp[0][1] = t.m01;
222  mp[2][0] = t.m20;
223  mp[1][1] = t.m11;
224  mp[0][2] = t.m02;
225 
226  vpPixelMeterConversion::convertMoment(cam, order, mp, m);
227 
228  double m00 = m[0][0];
229  double m01 = m[0][1];
230  double m10 = m[1][0];
231  double m02 = m[0][2];
232  double m11 = m[1][1];
233  double m20 = m[2][0];
234 
235  double xc = m10 / m00; // sum j /S
236  double yc = m01 / m00; // sum i /S
237 
238  double mu20 = 4 * (m20 - m00 * vpMath::sqr(xc)) / (m00);
239  double mu02 = 4 * (m02 - m00 * vpMath::sqr(yc)) / (m00);
240  double mu11 = 4 * (m11 - m00 * xc * yc) / (m00);
241 
242  s.buildFrom(xc, yc, mu20, mu11, mu02);
243  } catch (...) {
244  vpERROR_TRACE("Error caught");
245  throw;
246  }
247 }
248 #endif //#ifdef VISP_HAVE_MODULE_BLOB
249 
250 #ifdef VISP_HAVE_MODULE_ME
251 
269 {
270  try {
271 
272  unsigned int order = 3;
273  vpMatrix mp(order, order);
274  mp = 0;
275  vpMatrix m(order, order);
276  m = 0;
277 
278  // The opposite of vpDot and vpDot2 because moments in vpMeEllipse
279  // are computed in the ij coordinate system whereas the moments in vpDot
280  // and vpDot2 are computed in the uv coordinate system
281  mp[0][0] = t.get_m00();
282  mp[1][0] = t.get_m01();
283  mp[0][1] = t.get_m10();
284  mp[2][0] = t.get_m02();
285  mp[1][1] = t.get_m11();
286  mp[0][2] = t.get_m20();
287 
288  vpPixelMeterConversion::convertMoment(cam, order, mp, m);
289 
290  double m00 = m[0][0];
291  double m01 = m[0][1];
292  double m10 = m[1][0];
293  double m02 = m[0][2];
294  double m11 = m[1][1];
295  double m20 = m[2][0];
296 
297  double xc = m10 / m00; // sum j /S
298  double yc = m01 / m00; // sum i /S
299 
300  double mu20 = 4 * (m20 - m00 * vpMath::sqr(xc)) / (m00);
301  double mu02 = 4 * (m02 - m00 * vpMath::sqr(yc)) / (m00);
302  double mu11 = 4 * (m11 - m00 * xc * yc) / (m00);
303 
304  s.buildFrom(xc, yc, mu20, mu11, mu02);
305  } catch (...) {
306  vpERROR_TRACE("Error caught");
307  throw;
308  }
309 }
310 #endif //#ifdef VISP_HAVE_MODULE_ME
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:104
double m02
Definition: vpDot2.h:391
double get_m00() const
Definition: vpMeEllipse.h:146
double m10
Definition: vpDot.h:146
#define vpERROR_TRACE
Definition: vpDebug.h:393
double m00
Definition: vpDot.h:132
double get_m02() const
Definition: vpMeEllipse.h:186
Class that tracks an ellipse moving edges.
Definition: vpMeEllipse.h:106
double m11
Definition: vpDot2.h:375
Class that defines what is a sphere.
Definition: vpSphere.h:60
This tracker is meant to track a blob (connex pixels with same gray level) on a vpImage.
Definition: vpDot2.h:126
double m01
Definition: vpDot2.h:367
double m20
Definition: vpDot.h:162
double get_m10() const
Definition: vpMeEllipse.h:154
double m11
Definition: vpDot.h:153
vpColVector cP
Definition: vpTracker.h:75
double get_m11() const
Definition: vpMeEllipse.h:170
double m01
Definition: vpDot.h:139
double m20
Definition: vpDot2.h:382
static double sqr(double x)
Definition: vpMath.h:108
Generic class defining intrinsic camera parameters.
double get_m20() const
Definition: vpMeEllipse.h:178
void setABC(const double A, const double B, const double C)
double m02
Definition: vpDot.h:171
static void convertMoment(const vpCameraParameters &cam, unsigned int order, const vpMatrix &moment_pixel, vpMatrix &moment_meter)
This tracker is meant to track a dot (connected pixels with same gray level) on a vpImage...
Definition: vpDot.h:117
double m10
Definition: vpDot2.h:359
void buildFrom(const double x, const double y, const double mu20, const double mu11, const double mu02)
double m00
Definition: vpDot2.h:351
Class that defines 2D ellipse visual feature.
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
Class that defines what is a circle.
Definition: vpCircle.h:58
double get_m01() const
Definition: vpMeEllipse.h:162
vpColVector p
Definition: vpTracker.h:71