Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpFeatureLine.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 * 2D line visual feature.
33 *
34*****************************************************************************/
35
41#include <visp3/visual_features/vpBasicFeature.h>
42#include <visp3/visual_features/vpFeatureLine.h>
43
44// Exception
45#include <visp3/core/vpException.h>
46#include <visp3/visual_features/vpFeatureException.h>
47
48// Debug trace
49#include <visp3/core/vpDebug.h>
50
51// simple math function (round)
52#include <visp3/core/vpMath.h>
53
54// Display Issue
55
56// Meter/pixel conversion
57#include <visp3/core/vpCameraParameters.h>
58
59// Color / image / display
60#include <visp3/core/vpColor.h>
61#include <visp3/core/vpImage.h>
62
63#include <visp3/core/vpFeatureDisplay.h>
64
65/*
66attributes and members directly related to the vpBasicFeature needs
67other functionalities ar useful but not mandatory
68*/
69
74{
75 // feature dimension
76 dim_s = 2;
77 nbParameters = 6;
78
79 // memory allocation
80 // x cos(theta) + y sin(theta) - rho = 0
81 // s[0] = rho
82 // s[1] = theta
83 s.resize(dim_s);
84 if (flags == NULL)
85 flags = new bool[nbParameters];
86 for (unsigned int i = 0; i < nbParameters; i++)
87 flags[i] = false;
88
89 A = B = C = D = 0.0;
90}
91
95vpFeatureLine::vpFeatureLine() : A(0), B(0), C(0), D(0) { init(); }
96
104void vpFeatureLine::setRhoTheta(double rho, double theta)
105{
106 s[0] = rho;
107 s[1] = theta;
108 for (int i = 0; i < 2; i++)
109 flags[i] = true;
110}
111
126void vpFeatureLine::setABCD(double A_, double B_, double C_, double D_)
127{
128 this->A = A_;
129 this->B = B_;
130 this->C = C_;
131 this->D = D_;
132 for (unsigned int i = 2; i < nbParameters; i++)
133 flags[i] = true;
134}
135
188{
189 vpMatrix L;
190
192 for (unsigned int i = 0; i < nbParameters; i++) {
193 if (flags[i] == false) {
194 switch (i) {
195 case 0:
196 vpTRACE("Warning !!! The interaction matrix is computed but rho "
197 "was not set yet");
198 break;
199 case 1:
200 vpTRACE("Warning !!! The interaction matrix is computed but theta "
201 "was not set yet");
202 break;
203 case 2:
204 vpTRACE("Warning !!! The interaction matrix is computed but A was "
205 "not set yet");
206 break;
207 case 3:
208 vpTRACE("Warning !!! The interaction matrix is computed but B was "
209 "not set yet");
210 break;
211 case 4:
212 vpTRACE("Warning !!! The interaction matrix is computed but C was "
213 "not set yet");
214 break;
215 case 5:
216 vpTRACE("Warning !!! The interaction matrix is computed but D was "
217 "not set yet");
218 break;
219 default:
220 vpTRACE("Problem during the reading of the variable flags");
221 }
222 }
223 }
224 resetFlags();
225 }
226 double rho = s[0];
227 double theta = s[1];
228
229 double co = cos(theta);
230 double si = sin(theta);
231
232 if (fabs(D) < 1e-6) {
233 vpERROR_TRACE("Incorrect plane coordinates D is null, D = %f", D);
234
235 throw(vpFeatureException(vpFeatureException::badInitializationError, "Incorrect plane coordinates D"));
236 }
237
238 double lambda_theta = (A * si - B * co) / D;
239 double lambda_rho = (C + rho * A * co + rho * B * si) / D;
240
241 if (vpFeatureLine::selectRho() & select) {
242 vpMatrix Lrho(1, 6);
243
244 Lrho[0][0] = co * lambda_rho;
245 Lrho[0][1] = si * lambda_rho;
246 Lrho[0][2] = -rho * lambda_rho;
247 Lrho[0][3] = si * (1.0 + rho * rho);
248 Lrho[0][4] = -co * (1.0 + rho * rho);
249 Lrho[0][5] = 0.0;
250
251 L.stack(Lrho);
252 }
253
254 if (vpFeatureLine::selectTheta() & select) {
255 vpMatrix Ltheta(1, 6);
256
257 Ltheta[0][0] = co * lambda_theta;
258 Ltheta[0][1] = si * lambda_theta;
259 Ltheta[0][2] = -rho * lambda_theta;
260 Ltheta[0][3] = -rho * co;
261 Ltheta[0][4] = -rho * si;
262 Ltheta[0][5] = -1.0;
263
264 L.stack(Ltheta);
265 }
266 return L;
267}
268
307vpColVector vpFeatureLine::error(const vpBasicFeature &s_star, unsigned int select)
308{
309 vpColVector e(0);
310
311 try {
312 if (vpFeatureLine::selectRho() & select) {
313 vpColVector erho(1);
314 erho[0] = s[0] - s_star[0];
315
316 e = vpColVector::stack(e, erho);
317 }
318
319 if (vpFeatureLine::selectTheta() & select) {
320
321 double err = s[1] - s_star[1];
322 while (err < -M_PI)
323 err += 2 * M_PI;
324 while (err > M_PI)
325 err -= 2 * M_PI;
326
327 vpColVector etheta(1);
328 etheta[0] = err;
329 e = vpColVector::stack(e, etheta);
330 }
331 } catch (...) {
332 throw;
333 }
334
335 return e;
336}
337
358void vpFeatureLine::print(unsigned int select) const
359{
360
361 std::cout << "Line:\t " << A << "X+" << B << "Y+" << C << "Z +" << D << "=0" << std::endl;
362 ;
363 if (vpFeatureLine::selectRho() & select)
364 std::cout << " \trho=" << s[0];
365 if (vpFeatureLine::selectTheta() & select)
366 std::cout << " \ttheta=" << s[1];
367 std::cout << std::endl;
368}
369
384void vpFeatureLine::buildFrom(double rho, double theta)
385{
386 s[0] = rho;
387 s[1] = theta;
388 for (int i = 0; i < 2; i++)
389 flags[i] = true;
390}
391
420void vpFeatureLine::buildFrom(double rho, double theta, double A_, double B_, double C_, double D_)
421{
422 s[0] = rho;
423 s[1] = theta;
424 this->A = A_;
425 this->B = B_;
426 this->C = C_;
427 this->D = D_;
428 for (unsigned int i = 0; i < nbParameters; i++)
429 flags[i] = true;
430}
431
443{
444 vpFeatureLine *feature = new vpFeatureLine;
445 return feature;
446}
447
459 unsigned int thickness) const
460{
461 try {
462 double rho, theta;
463 rho = getRho();
464 theta = getTheta();
465
466 vpFeatureDisplay::displayLine(rho, theta, cam, I, color, thickness);
467
468 } catch (...) {
469 vpERROR_TRACE("Error caught");
470 throw;
471 }
472}
473
484void vpFeatureLine::display(const vpCameraParameters &cam, const vpImage<vpRGBa> &I, const vpColor &color,
485 unsigned int thickness) const
486{
487 try {
488 double rho, theta;
489 rho = getRho();
490 theta = getTheta();
491
492 vpFeatureDisplay::displayLine(rho, theta, cam, I, color, thickness);
493
494 } catch (...) {
495 vpERROR_TRACE("Error caught");
496 throw;
497 }
498}
499
518unsigned int vpFeatureLine::selectRho() { return FEATURE_LINE[0]; }
519
539unsigned int vpFeatureLine::selectTheta() { return FEATURE_LINE[1]; }
class that defines what is a visual feature
vpColVector s
State of the visual feature.
static const unsigned int FEATURE_LINE[32]
unsigned int nbParameters
Number of parameters needed to compute the interaction matrix.
unsigned int dim_s
Dimension of the visual feature.
vpBasicFeatureDeallocatorType deallocate
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
void stack(double d)
void resize(unsigned int i, bool flagNullify=true)
Class to define RGB colors available for display functionalities.
Definition vpColor.h:152
static void displayLine(double rho, double theta, const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1)
Error that can be emitted by the vpBasicFeature class and its derivates.
@ badInitializationError
Wrong feature initialization.
Class that defines a 2D line visual feature which is composed by two parameters that are and ,...
void setRhoTheta(double rho, double theta)
void display(const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1) const
double getTheta() const
static unsigned int selectRho()
vpFeatureLine * duplicate() const
void buildFrom(double rho, double theta)
void print(unsigned int select=FEATURE_ALL) const
void setABCD(double A, double B, double C, double D)
static unsigned int selectTheta()
vpMatrix interaction(unsigned int select=FEATURE_ALL)
vpColVector error(const vpBasicFeature &s_star, unsigned int select=FEATURE_ALL)
double getRho() const
Definition of the vpImage class member functions.
Definition vpImage.h:135
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
#define vpTRACE
Definition vpDebug.h:411
#define vpERROR_TRACE
Definition vpDebug.h:388