Visual Servoing Platform version 3.5.0
vpFeatureLine.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 * 2D line visual feature.
33 *
34 * Authors:
35 * Eric Marchand
36 *
37 *****************************************************************************/
38
44#include <visp3/visual_features/vpBasicFeature.h>
45#include <visp3/visual_features/vpFeatureLine.h>
46
47// Exception
48#include <visp3/core/vpException.h>
49#include <visp3/visual_features/vpFeatureException.h>
50
51// Debug trace
52#include <visp3/core/vpDebug.h>
53
54// simple math function (round)
55#include <visp3/core/vpMath.h>
56
57// Display Issue
58
59// Meter/pixel conversion
60#include <visp3/core/vpCameraParameters.h>
61
62// Color / image / display
63#include <visp3/core/vpColor.h>
64#include <visp3/core/vpImage.h>
65
66#include <visp3/core/vpFeatureDisplay.h>
67
68/*
69attributes and members directly related to the vpBasicFeature needs
70other functionalities ar useful but not mandatory
71*/
72
77{
78 // feature dimension
79 dim_s = 2;
80 nbParameters = 6;
81
82 // memory allocation
83 // x cos(theta) + y sin(theta) - rho = 0
84 // s[0] = rho
85 // s[1] = theta
86 s.resize(dim_s);
87 if (flags == NULL)
88 flags = new bool[nbParameters];
89 for (unsigned int i = 0; i < nbParameters; i++)
90 flags[i] = false;
91
92 A = B = C = D = 0.0;
93}
94
98vpFeatureLine::vpFeatureLine() : A(0), B(0), C(0), D(0) { init(); }
99
107void vpFeatureLine::setRhoTheta(double rho, double theta)
108{
109 s[0] = rho;
110 s[1] = theta;
111 for (int i = 0; i < 2; i++)
112 flags[i] = true;
113}
114
129void vpFeatureLine::setABCD(double A_, double B_, double C_, double D_)
130{
131 this->A = A_;
132 this->B = B_;
133 this->C = C_;
134 this->D = D_;
135 for (unsigned int i = 2; i < nbParameters; i++)
136 flags[i] = true;
137}
138
191{
192 vpMatrix L;
193
195 for (unsigned int i = 0; i < nbParameters; i++) {
196 if (flags[i] == false) {
197 switch (i) {
198 case 0:
199 vpTRACE("Warning !!! The interaction matrix is computed but rho "
200 "was not set yet");
201 break;
202 case 1:
203 vpTRACE("Warning !!! The interaction matrix is computed but theta "
204 "was not set yet");
205 break;
206 case 2:
207 vpTRACE("Warning !!! The interaction matrix is computed but A was "
208 "not set yet");
209 break;
210 case 3:
211 vpTRACE("Warning !!! The interaction matrix is computed but B was "
212 "not set yet");
213 break;
214 case 4:
215 vpTRACE("Warning !!! The interaction matrix is computed but C was "
216 "not set yet");
217 break;
218 case 5:
219 vpTRACE("Warning !!! The interaction matrix is computed but D was "
220 "not set yet");
221 break;
222 default:
223 vpTRACE("Problem during the reading of the variable flags");
224 }
225 }
226 }
227 resetFlags();
228 }
229 double rho = s[0];
230 double theta = s[1];
231
232 double co = cos(theta);
233 double si = sin(theta);
234
235 if (fabs(D) < 1e-6) {
236 vpERROR_TRACE("Incorrect plane coordinates D is null, D = %f", D);
237
238 throw(vpFeatureException(vpFeatureException::badInitializationError, "Incorrect plane coordinates D"));
239 }
240
241 double lambda_theta = (A * si - B * co) / D;
242 double lambda_rho = (C + rho * A * co + rho * B * si) / D;
243
244 if (vpFeatureLine::selectRho() & select) {
245 vpMatrix Lrho(1, 6);
246
247 Lrho[0][0] = co * lambda_rho;
248 Lrho[0][1] = si * lambda_rho;
249 Lrho[0][2] = -rho * lambda_rho;
250 Lrho[0][3] = si * (1.0 + rho * rho);
251 Lrho[0][4] = -co * (1.0 + rho * rho);
252 Lrho[0][5] = 0.0;
253
254 L.stack(Lrho);
255 }
256
257 if (vpFeatureLine::selectTheta() & select) {
258 vpMatrix Ltheta(1, 6);
259
260 Ltheta[0][0] = co * lambda_theta;
261 Ltheta[0][1] = si * lambda_theta;
262 Ltheta[0][2] = -rho * lambda_theta;
263 Ltheta[0][3] = -rho * co;
264 Ltheta[0][4] = -rho * si;
265 Ltheta[0][5] = -1.0;
266
267 L.stack(Ltheta);
268 }
269 return L;
270}
271
310vpColVector vpFeatureLine::error(const vpBasicFeature &s_star, unsigned int select)
311{
312 vpColVector e(0);
313
314 try {
315 if (vpFeatureLine::selectRho() & select) {
316 vpColVector erho(1);
317 erho[0] = s[0] - s_star[0];
318
319 e = vpColVector::stack(e, erho);
320 }
321
322 if (vpFeatureLine::selectTheta() & select) {
323
324 double err = s[1] - s_star[1];
325 while (err < -M_PI)
326 err += 2 * M_PI;
327 while (err > M_PI)
328 err -= 2 * M_PI;
329
330 vpColVector etheta(1);
331 etheta[0] = err;
332 e = vpColVector::stack(e, etheta);
333 }
334 } catch (...) {
335 throw;
336 }
337
338 return e;
339}
340
361void vpFeatureLine::print(unsigned int select) const
362{
363
364 std::cout << "Line:\t " << A << "X+" << B << "Y+" << C << "Z +" << D << "=0" << std::endl;
365 ;
366 if (vpFeatureLine::selectRho() & select)
367 std::cout << " \trho=" << s[0];
368 if (vpFeatureLine::selectTheta() & select)
369 std::cout << " \ttheta=" << s[1];
370 std::cout << std::endl;
371}
372
387void vpFeatureLine::buildFrom(double rho, double theta)
388{
389 s[0] = rho;
390 s[1] = theta;
391 for (int i = 0; i < 2; i++)
392 flags[i] = true;
393}
394
423void vpFeatureLine::buildFrom(double rho, double theta, double A_, double B_, double C_, double D_)
424{
425 s[0] = rho;
426 s[1] = theta;
427 this->A = A_;
428 this->B = B_;
429 this->C = C_;
430 this->D = D_;
431 for (unsigned int i = 0; i < nbParameters; i++)
432 flags[i] = true;
433}
434
446{
447 vpFeatureLine *feature = new vpFeatureLine;
448 return feature;
449}
450
462 unsigned int thickness) const
463{
464 try {
465 double rho, theta;
466 rho = getRho();
467 theta = getTheta();
468
469 vpFeatureDisplay::displayLine(rho, theta, cam, I, color, thickness);
470
471 } catch (...) {
472 vpERROR_TRACE("Error caught");
473 throw;
474 }
475}
476
487void vpFeatureLine::display(const vpCameraParameters &cam, const vpImage<vpRGBa> &I, const vpColor &color,
488 unsigned int thickness) const
489{
490 try {
491 double rho, theta;
492 rho = getRho();
493 theta = getTheta();
494
495 vpFeatureDisplay::displayLine(rho, theta, cam, I, color, thickness);
496
497 } catch (...) {
498 vpERROR_TRACE("Error caught");
499 throw;
500 }
501}
502
521unsigned int vpFeatureLine::selectRho() { return FEATURE_LINE[0]; }
522
542unsigned 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.
Definition: vpColVector.h:131
void stack(double d)
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:158
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 emited by the vpBasicFeature class and its derivates.
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
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
#define vpTRACE
Definition: vpDebug.h:416
#define vpERROR_TRACE
Definition: vpDebug.h:393