Visual Servoing Platform version 3.5.0
vpMbtMeEllipse.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 * Moving edges.
33 *
34 * Authors:
35 * Eric Marchand
36 *
37 *****************************************************************************/
38
39#ifndef DOXYGEN_SHOULD_SKIP_THIS
40
41#include <visp3/mbt/vpMbtMeEllipse.h>
42
43#include <visp3/core/vpDebug.h>
44#include <visp3/core/vpImagePoint.h>
45#include <visp3/core/vpRobust.h>
46#include <visp3/core/vpTrackingException.h>
47#include <visp3/me/vpMe.h>
48
49#include <algorithm> // (std::min)
50#include <cmath> // std::fabs
51#include <limits> // numeric_limits
52
56vpMbtMeEllipse::vpMbtMeEllipse()
57 : vpMeEllipse()
58{
59}
60
64vpMbtMeEllipse::vpMbtMeEllipse(const vpMbtMeEllipse &me_ellipse)
65 : vpMeEllipse(me_ellipse)
66{
67}
71vpMbtMeEllipse::~vpMbtMeEllipse()
72{
73}
74
90void vpMbtMeEllipse::computeProjectionError(const vpImage<unsigned char> &I, double &sumErrorRad,
91 unsigned int &nbFeatures,
92 const vpMatrix &SobelX, const vpMatrix &SobelY,
93 bool display, unsigned int length,
94 unsigned int thickness)
95{
96 sumErrorRad = 0;
97 nbFeatures = 0;
98
99 double offset = static_cast<double>(std::floor(SobelX.getRows() / 2.0f));
100 int height = static_cast<int>(I.getHeight());
101 int width = static_cast<int>(I.getWidth());
102
103 double max_iImg = height - 1.;
104 double max_jImg = width - 1.;
105
106 vpColVector vecSite(2);
107 vpColVector vecGrad(2);
108
109 for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end(); ++it) {
110 double iSite = it->ifloat;
111 double jSite = it->jfloat;
112
113 if (!outOfImage(vpMath::round(iSite), vpMath::round(jSite), 0, height, width)) { // Check if necessary
114 // The tangent angle to the ellipse at a site
115 double theta = computeTheta(vpImagePoint(iSite, jSite));
116
117 vecSite[0] = cos(theta);
118 vecSite[1] = sin(theta);
119 vecSite.normalize();
120
121 double gradientX = 0;
122 double gradientY = 0;
123
124 for (unsigned int i = 0; i < SobelX.getRows(); i++) {
125 double iImg = iSite + (i - offset);
126 for (unsigned int j = 0; j < SobelX.getCols(); j++) {
127 double jImg = jSite + (j - offset);
128
129 if (iImg < 0)
130 iImg = 0.0;
131 if (jImg < 0)
132 jImg = 0.0;
133
134 if (iImg > max_iImg)
135 iImg = max_iImg;
136 if (jImg > max_jImg)
137 jImg = max_jImg;
138
139 gradientX += SobelX[i][j] * I((unsigned int)iImg, (unsigned int)jImg);
140 }
141 }
142
143 for (unsigned int i = 0; i < SobelY.getRows(); i++) {
144 double iImg = iSite + (i - offset);
145 for (unsigned int j = 0; j < SobelY.getCols(); j++) {
146 double jImg = jSite + (j - offset);
147
148 if (iImg < 0)
149 iImg = 0.0;
150 if (jImg < 0)
151 jImg = 0.0;
152
153 if (iImg > max_iImg)
154 iImg = max_iImg;
155 if (jImg > max_jImg)
156 jImg = max_jImg;
157
158 gradientY += SobelY[i][j] * I((unsigned int)iImg, (unsigned int)jImg);
159 }
160 }
161
162 double angle = atan2(gradientY, gradientX);
163 while (angle < 0)
164 angle += M_PI;
165 while (angle > M_PI)
166 angle -= M_PI;
167
168 vecGrad[0] = cos(angle);
169 vecGrad[1] = sin(angle);
170 vecGrad.normalize();
171
172 double angle1 = acos(vecSite * vecGrad);
173 double angle2 = acos(vecSite * (-vecGrad));
174
175 if (display) {
176 vpDisplay::displayArrow(I, it->get_i(), it->get_j(), static_cast<int>(it->get_i() + length*cos(theta)),
177 static_cast<int>(it->get_j() + length*sin(theta)), vpColor::blue,
178 length >= 20 ? length/5 : 4, length >= 20 ? length/10 : 2, thickness);
179 if (angle1 < angle2) {
180 vpDisplay::displayArrow(I, it->get_i(), it->get_j(), static_cast<int>(it->get_i() + length*cos(angle)),
181 static_cast<int>(it->get_j() + length*sin(angle)), vpColor::red,
182 length >= 20 ? length/5 : 4, length >= 20 ? length/10 : 2, thickness);
183 } else {
184 vpDisplay::displayArrow(I, it->get_i(), it->get_j(), static_cast<int>(it->get_i() + length*cos(angle+M_PI)),
185 static_cast<int>(it->get_j() + length*sin(angle+M_PI)), vpColor::red,
186 length >= 20 ? length/5 : 4, length >= 20 ? length/10 : 2, thickness);
187 }
188 }
189
190 sumErrorRad += (std::min)(angle1, angle2);
191
192 nbFeatures++;
193 }
194 }
195}
196
197void vpMbtMeEllipse::initTracking(const vpImage<unsigned char> &I, const vpImagePoint &center_p,
198 double n20_p, double n11_p, double n02_p, bool doNotTrack,
199 vpImagePoint *pt1, const vpImagePoint *pt2)
200{
201 if (pt1 != NULL && pt2 != NULL) {
202 m_trackArc = true;
203 }
204
205 // useful for sample(I) : uc, vc, a, b, e, Ki, alpha1, alpha2
206 m_uc = center_p.get_u();
207 m_vc = center_p.get_v();
208 m_n20 = n20_p;
209 m_n11 = n11_p;
210 m_n02 = n02_p;
211
212 computeAbeFromNij();
213 computeKiFromNij();
214
215 if (m_trackArc) {
216 alpha1 = computeAngleOnEllipse(*pt1);
217 alpha2 = computeAngleOnEllipse(*pt2);
218 if ((alpha2 <= alpha1) || (std::fabs(alpha2 - alpha1) < m_arcEpsilon)) {
219 alpha2 += 2.0 * M_PI;
220 }
221 // useful for track(I)
222 iP1 = *pt1;
223 iP2 = *pt2;
224 }
225 else {
226 alpha1 = 0.0;
227 alpha2 = 2.0 * M_PI;
228 // useful for track(I)
229 vpImagePoint ip;
230 computePointOnEllipse(alpha1, ip);
231 iP1 = iP2 = ip;
232 }
233 // useful for display(I) so useless if no display before track(I)
234 iPc.set_uv(m_uc, m_vc);
235
236 sample(I, doNotTrack);
237
238 try {
239 if (!doNotTrack)
240 track(I);
241 } catch (const vpException &exception) {
242 throw(exception);
243 }
244}
245
251void vpMbtMeEllipse::track(const vpImage<unsigned char> &I)
252{
253 try {
255 if (m_mask != NULL) {
256 // Expected density could be modified if some vpMeSite are no more tracked because they are outside the mask.
257 m_expectedDensity = static_cast<unsigned int>(list.size());
258 }
259 } catch (const vpException &exception) {
260 throw(exception);
261 }
262}
263
267void vpMbtMeEllipse::updateParameters(const vpImage<unsigned char> &I, const vpImagePoint &center_p,
268 double n20_p, double n11_p, double n02_p)
269{
270 m_uc = center_p.get_u();
271 m_vc = center_p.get_v();
272 m_n20 = n20_p;
273 m_n11 = n11_p;
274 m_n02 = n02_p;
275
276 computeAbeFromNij();
277 computeKiFromNij();
278
279 suppressPoints();
280 reSample(I);
281
282 // remet a jour l'angle delta pour chaque point de la liste
283 updateTheta();
284}
285
299void vpMbtMeEllipse::reSample(const vpImage<unsigned char> &I)
300{
301 if (!me) {
302 vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
303 throw(vpTrackingException(vpTrackingException::initializationError, "Moving edges not initialized"));
304 }
305
306 unsigned int n = numberOfSignal();
307 if ((double)n < 0.9 * m_expectedDensity) {
308 sample(I);
310 }
311}
312
325void vpMbtMeEllipse::sample(const vpImage<unsigned char> &I, bool doNotTrack)
326{
327 // Warning: similar code in vpMeEllipse::sample() except for display that is removed here
328 if (!me) {
329 throw(vpException(vpException::fatalError, "Moving edges on ellipse not initialized"));
330 }
331 // Delete old lists
332 list.clear();
333 angle.clear();
334
335 int nbrows = static_cast<int>(I.getHeight());
336 int nbcols = static_cast<int>(I.getWidth());
337
338 if (std::fabs(me->getSampleStep()) <= std::numeric_limits<double>::epsilon()) {
339 std::cout << "In vpMeEllipse::sample: ";
340 std::cout << "function called with sample step = 0, set to 10 dg";
341 me->setSampleStep(10.0);
342 }
343 double incr = vpMath::rad(me->getSampleStep()); // angle increment
344 // alpha2 - alpha1 = 2 * M_PI for a complete ellipse
345 m_expectedDensity = static_cast<unsigned int>(floor((alpha2 - alpha1) / incr));
346#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
347 expecteddensity = static_cast<double>(m_expectedDensity);
348#endif
349
350 // starting angle for sampling
351 double ang = alpha1 + ((alpha2 - alpha1) - static_cast<double>(m_expectedDensity) * incr)/2.0;
352 // sample positions
353 for (unsigned int i = 0; i < m_expectedDensity; i++) {
354 vpImagePoint iP;
355 computePointOnEllipse(ang,iP);
356 // If point is in the image, add to the sample list
357 // Check done in (i,j) frame)
358 if (!outOfImage(vpMath::round(iP.get_i()), vpMath::round(iP.get_j()), 0, nbrows, nbcols)) {
359 double theta = computeTheta(iP);
360 vpMeSite pix;
361 // (i,j) frame used for vpMeSite
362 pix.init(iP.get_i(), iP.get_j(), theta);
363 pix.setDisplay(selectDisplay);
365 list.push_back(pix);
366 angle.push_back(ang);
367 }
368 ang += incr;
369 }
370 if (!doNotTrack) {
372 }
373}
374
379void vpMbtMeEllipse::suppressPoints()
380{
381 // Loop through list of sites to track
382 for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end();) {
383 vpMeSite s = *it; // current reference pixel
385 it = list.erase(it);
386 else
387 ++it;
388 }
389}
390
391#endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
unsigned int getCols() const
Definition: vpArray2D.h:279
unsigned int getRows() const
Definition: vpArray2D.h:289
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
static const vpColor red
Definition: vpColor.h:217
static const vpColor blue
Definition: vpColor.h:223
static void displayArrow(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color=vpColor::white, unsigned int w=4, unsigned int h=2, unsigned int thickness=1)
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ fatalError
Fatal error.
Definition: vpException.h:96
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
double get_j() const
Definition: vpImagePoint.h:214
double get_u() const
Definition: vpImagePoint.h:262
void set_uv(double u, double v)
Definition: vpImagePoint.h:247
double get_i() const
Definition: vpImagePoint.h:203
double get_v() const
Definition: vpImagePoint.h:273
unsigned int getWidth() const
Definition: vpImage.h:246
unsigned int getHeight() const
Definition: vpImage.h:188
static double rad(double deg)
Definition: vpMath.h:110
static int round(double x)
Definition: vpMath.h:247
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
Class that tracks an ellipse using moving edges.
Definition: vpMeEllipse.h:98
Performs search in a given direction(normal) for a given distance(pixels) for a given 'site'....
Definition: vpMeSite.h:72
@ NO_SUPPRESSION
Point used by the tracker.
Definition: vpMeSite.h:78
void setDisplay(vpMeSiteDisplayType select)
Definition: vpMeSite.h:139
void init()
Definition: vpMeSite.cpp:66
vpMeSiteState getState() const
Definition: vpMeSite.h:190
void setState(const vpMeSiteState &flag)
Definition: vpMeSite.h:176
void initTracking(const vpImage< unsigned char > &I)
void track(const vpImage< unsigned char > &I)
Track sampled pixels.
Error that can be emited by the vpTracker class and its derivates.
#define vpDERROR_TRACE
Definition: vpDebug.h:464