Visual Servoing Platform version 3.5.0
vpFeaturePoint.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 point visual feature.
33 *
34 * Authors:
35 * Eric Marchand
36 *
37 *****************************************************************************/
38
44#include <visp3/visual_features/vpBasicFeature.h>
45#include <visp3/visual_features/vpFeaturePoint.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// math
55#include <visp3/core/vpMath.h>
56
57#include <visp3/core/vpFeatureDisplay.h>
58
59/*
60
61attributes and members directly related to the vpBasicFeature needs
62other functionalities ar useful but not mandatory
63
64*/
65
70{
71 // feature dimension
72 dim_s = 2;
73 nbParameters = 3;
74
75 // memory allocation
76 s.resize(dim_s);
77 if (flags == NULL)
78 flags = new bool[nbParameters];
79 for (unsigned int i = 0; i < nbParameters; i++)
80 flags[i] = false;
81
82 // default value Z (1 meters)
83 Z = 1;
84}
85
90
97void vpFeaturePoint::set_Z(double Z_)
98{
99 this->Z = Z_;
100 flags[2] = true;
101}
102
109double vpFeaturePoint::get_Z() const { return Z; }
110
119{
120 s[0] = x;
121 flags[0] = true;
122}
123
130double vpFeaturePoint::get_x() const { return s[0]; }
131
139{
140 s[1] = y;
141 flags[1] = true;
142}
143
150double vpFeaturePoint::get_y() const { return s[1]; }
151
163void vpFeaturePoint::set_xyZ(double x_, double y_, double Z_)
164{
165 set_x(x_);
166 set_y(y_);
167 set_Z(Z_);
168 for (unsigned int i = 0; i < nbParameters; i++)
169 flags[i] = true;
170}
171
216{
217 vpMatrix L;
218
219 L.resize(0, 6);
220
222 for (unsigned int i = 0; i < nbParameters; i++) {
223 if (flags[i] == false) {
224 switch (i) {
225 case 0:
226 vpTRACE("Warning !!! The interaction matrix is computed but x was "
227 "not set yet");
228 break;
229 case 1:
230 vpTRACE("Warning !!! The interaction matrix is computed but y was "
231 "not set yet");
232 break;
233 case 2:
234 vpTRACE("Warning !!! The interaction matrix is computed but Z was "
235 "not set yet");
236 break;
237 default:
238 vpTRACE("Problem during the reading of the variable flags");
239 }
240 }
241 }
242 resetFlags();
243 }
244
245 double x_ = get_x();
246 double y_ = get_y();
247 double Z_ = get_Z();
248
249 if (Z_ < 0) {
250 vpERROR_TRACE("Point is behind the camera ");
251 std::cout << "Z = " << Z_ << std::endl;
252
253 throw(vpFeatureException(vpFeatureException::badInitializationError, "Point is behind the camera "));
254 }
255
256 if (fabs(Z_) < 1e-6) {
257 vpERROR_TRACE("Point Z coordinates is null ");
258 std::cout << "Z = " << Z_ << std::endl;
259
260 throw(vpFeatureException(vpFeatureException::badInitializationError, "Point Z coordinates is null"));
261 }
262
263 if (vpFeaturePoint::selectX() & select) {
264 vpMatrix Lx(1, 6);
265 Lx = 0;
266
267 Lx[0][0] = -1 / Z_;
268 Lx[0][1] = 0;
269 Lx[0][2] = x_ / Z_;
270 Lx[0][3] = x_ * y_;
271 Lx[0][4] = -(1 + x_ * x_);
272 Lx[0][5] = y_;
273
274 L = vpMatrix::stack(L, Lx);
275 }
276
277 if (vpFeaturePoint::selectY() & select) {
278 vpMatrix Ly(1, 6);
279 Ly = 0;
280
281 Ly[0][0] = 0;
282 Ly[0][1] = -1 / Z_;
283 Ly[0][2] = y_ / Z_;
284 Ly[0][3] = 1 + y_ * y_;
285 Ly[0][4] = -x_ * y_;
286 Ly[0][5] = -x_;
287
288 L = vpMatrix::stack(L, Ly);
289 }
290 return L;
291}
292
329vpColVector vpFeaturePoint::error(const vpBasicFeature &s_star, unsigned int select)
330{
331 vpColVector e(0);
332
333 try {
334 if (vpFeaturePoint::selectX() & select) {
335 vpColVector ex(1);
336 ex[0] = s[0] - s_star[0];
337
338 e = vpColVector::stack(e, ex);
339 }
340
341 if (vpFeaturePoint::selectY() & select) {
342 vpColVector ey(1);
343 ey[0] = s[1] - s_star[1];
344 e = vpColVector::stack(e, ey);
345 }
346 } catch (...) {
347 throw;
348 }
349
350 return e;
351}
352
372void vpFeaturePoint::print(unsigned int select) const
373{
374
375 std::cout << "Point: Z=" << get_Z();
376 if (vpFeaturePoint::selectX() & select)
377 std::cout << " x=" << get_x();
378 if (vpFeaturePoint::selectY() & select)
379 std::cout << " y=" << get_y();
380 std::cout << std::endl;
381}
382
395void vpFeaturePoint::buildFrom(double x_, double y_, double Z_)
396{
397
398 s[0] = x_;
399 s[1] = y_;
400
401 this->Z = Z_;
402
403 if (Z_ < 0) {
404 vpERROR_TRACE("Point is behind the camera ");
405 std::cout << "Z = " << Z_ << std::endl;
406
407 throw(vpFeatureException(vpFeatureException::badInitializationError, "Point is behind the camera "));
408 }
409
410 if (fabs(Z_) < 1e-6) {
411 vpERROR_TRACE("Point Z coordinates is null ");
412 std::cout << "Z = " << Z_ << std::endl;
413
414 throw(vpFeatureException(vpFeatureException::badInitializationError, "Point Z coordinates is null"));
415 }
416
417 for (unsigned int i = 0; i < nbParameters; i++)
418 flags[i] = true;
419}
420
432 unsigned int thickness) const
433{
434 try {
435 double x, y;
436 x = get_x();
437 y = get_y();
438
439 vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness);
440
441 } catch (...) {
442 vpERROR_TRACE("Error caught");
443 throw;
444 }
445}
446
458 unsigned int thickness) const
459{
460 try {
461 double x, y;
462 x = get_x();
463 y = get_y();
464
465 vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness);
466
467 } catch (...) {
468 vpERROR_TRACE("Error caught");
469 throw;
470 }
471}
472
484{
485 vpFeaturePoint *feature = new vpFeaturePoint;
486 return feature;
487}
488
507unsigned int vpFeaturePoint::selectX() { return FEATURE_LINE[0]; }
508
527unsigned int vpFeaturePoint::selectY() { 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 displayPoint(double x, double y, 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 point visual feature which is composed by two parameters that are the cartes...
void buildFrom(double x, double y, double Z)
static unsigned int selectX()
void set_xyZ(double x, double y, double Z)
vpColVector error(const vpBasicFeature &s_star, unsigned int select=FEATURE_ALL)
static unsigned int selectY()
void set_y(double y)
double get_y() const
double get_x() const
void set_x(double x)
void display(const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1) const
void print(unsigned int select=FEATURE_ALL) const
vpFeaturePoint * duplicate() const
double get_Z() const
void set_Z(double Z)
vpMatrix interaction(unsigned int select=FEATURE_ALL)
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
void stack(const vpMatrix &A)
Definition: vpMatrix.cpp:5879
#define vpTRACE
Definition: vpDebug.h:416
#define vpERROR_TRACE
Definition: vpDebug.h:393