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