Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpFeaturePoint.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 point visual feature.
33 *
34*****************************************************************************/
35
41#include <visp3/visual_features/vpBasicFeature.h>
42#include <visp3/visual_features/vpFeaturePoint.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// math
52#include <visp3/core/vpMath.h>
53
54#include <visp3/core/vpFeatureDisplay.h>
55
56/*
57
58attributes and members directly related to the vpBasicFeature needs
59other functionalities ar useful but not mandatory
60
61*/
62
67{
68 // feature dimension
69 dim_s = 2;
70 nbParameters = 3;
71
72 // memory allocation
73 s.resize(dim_s);
74 if (flags == NULL)
75 flags = new bool[nbParameters];
76 for (unsigned int i = 0; i < nbParameters; i++)
77 flags[i] = false;
78
79 // default value Z (1 meters)
80 Z = 1;
81}
82
87
94void vpFeaturePoint::set_Z(double Z_)
95{
96 this->Z = Z_;
97 flags[2] = true;
98}
99
106double vpFeaturePoint::get_Z() const { return Z; }
107
116{
117 s[0] = x;
118 flags[0] = true;
119}
120
127double vpFeaturePoint::get_x() const { return s[0]; }
128
136{
137 s[1] = y;
138 flags[1] = true;
139}
140
147double vpFeaturePoint::get_y() const { return s[1]; }
148
160void vpFeaturePoint::set_xyZ(double x_, double y_, double Z_)
161{
162 set_x(x_);
163 set_y(y_);
164 set_Z(Z_);
165 for (unsigned int i = 0; i < nbParameters; i++)
166 flags[i] = true;
167}
168
213{
214 vpMatrix L;
215
216 L.resize(0, 6);
217
219 for (unsigned int i = 0; i < nbParameters; i++) {
220 if (flags[i] == false) {
221 switch (i) {
222 case 0:
223 vpTRACE("Warning !!! The interaction matrix is computed but x was "
224 "not set yet");
225 break;
226 case 1:
227 vpTRACE("Warning !!! The interaction matrix is computed but y was "
228 "not set yet");
229 break;
230 case 2:
231 vpTRACE("Warning !!! The interaction matrix is computed but Z was "
232 "not set yet");
233 break;
234 default:
235 vpTRACE("Problem during the reading of the variable flags");
236 }
237 }
238 }
239 resetFlags();
240 }
241
242 double x_ = get_x();
243 double y_ = get_y();
244 double Z_ = get_Z();
245
246 if (Z_ < 0) {
247 vpERROR_TRACE("Point is behind the camera ");
248 std::cout << "Z = " << Z_ << std::endl;
249
250 throw(vpFeatureException(vpFeatureException::badInitializationError, "Point is behind the camera "));
251 }
252
253 if (fabs(Z_) < 1e-6) {
254 vpERROR_TRACE("Point Z coordinates is null ");
255 std::cout << "Z = " << Z_ << std::endl;
256
257 throw(vpFeatureException(vpFeatureException::badInitializationError, "Point Z coordinates is null"));
258 }
259
260 if (vpFeaturePoint::selectX() & select) {
261 vpMatrix Lx(1, 6);
262 Lx = 0;
263
264 Lx[0][0] = -1 / Z_;
265 Lx[0][1] = 0;
266 Lx[0][2] = x_ / Z_;
267 Lx[0][3] = x_ * y_;
268 Lx[0][4] = -(1 + x_ * x_);
269 Lx[0][5] = y_;
270
271 L = vpMatrix::stack(L, Lx);
272 }
273
274 if (vpFeaturePoint::selectY() & select) {
275 vpMatrix Ly(1, 6);
276 Ly = 0;
277
278 Ly[0][0] = 0;
279 Ly[0][1] = -1 / Z_;
280 Ly[0][2] = y_ / Z_;
281 Ly[0][3] = 1 + y_ * y_;
282 Ly[0][4] = -x_ * y_;
283 Ly[0][5] = -x_;
284
285 L = vpMatrix::stack(L, Ly);
286 }
287 return L;
288}
289
326vpColVector vpFeaturePoint::error(const vpBasicFeature &s_star, unsigned int select)
327{
328 vpColVector e(0);
329
330 try {
331 if (vpFeaturePoint::selectX() & select) {
332 vpColVector ex(1);
333 ex[0] = s[0] - s_star[0];
334
335 e = vpColVector::stack(e, ex);
336 }
337
338 if (vpFeaturePoint::selectY() & select) {
339 vpColVector ey(1);
340 ey[0] = s[1] - s_star[1];
341 e = vpColVector::stack(e, ey);
342 }
343 } catch (...) {
344 throw;
345 }
346
347 return e;
348}
349
369void vpFeaturePoint::print(unsigned int select) const
370{
371
372 std::cout << "Point: Z=" << get_Z();
373 if (vpFeaturePoint::selectX() & select)
374 std::cout << " x=" << get_x();
375 if (vpFeaturePoint::selectY() & select)
376 std::cout << " y=" << get_y();
377 std::cout << std::endl;
378}
379
392void vpFeaturePoint::buildFrom(double x_, double y_, double Z_)
393{
394
395 s[0] = x_;
396 s[1] = y_;
397
398 this->Z = Z_;
399
400 if (Z_ < 0) {
401 vpERROR_TRACE("Point is behind the camera ");
402 std::cout << "Z = " << Z_ << std::endl;
403
404 throw(vpFeatureException(vpFeatureException::badInitializationError, "Point is behind the camera "));
405 }
406
407 if (fabs(Z_) < 1e-6) {
408 vpERROR_TRACE("Point Z coordinates is null ");
409 std::cout << "Z = " << Z_ << std::endl;
410
411 throw(vpFeatureException(vpFeatureException::badInitializationError, "Point Z coordinates is null"));
412 }
413
414 for (unsigned int i = 0; i < nbParameters; i++)
415 flags[i] = true;
416}
417
429 unsigned int thickness) const
430{
431 try {
432 double x, y;
433 x = get_x();
434 y = get_y();
435
436 vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness);
437
438 } catch (...) {
439 vpERROR_TRACE("Error caught");
440 throw;
441 }
442}
443
455 unsigned int thickness) const
456{
457 try {
458 double x, y;
459 x = get_x();
460 y = get_y();
461
462 vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness);
463
464 } catch (...) {
465 vpERROR_TRACE("Error caught");
466 throw;
467 }
468}
469
481{
482 vpFeaturePoint *feature = new vpFeaturePoint;
483 return feature;
484}
485
504unsigned int vpFeaturePoint::selectX() { return FEATURE_LINE[0]; }
505
524unsigned 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.
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 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 emitted by the vpBasicFeature class and its derivates.
@ badInitializationError
Wrong feature initialization.
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)
Definition of the vpImage class member functions.
Definition vpImage.h:135
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
void stack(const vpMatrix &A)
#define vpTRACE
Definition vpDebug.h:411
#define vpERROR_TRACE
Definition vpDebug.h:388