Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpFeatureDepth.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/vpFeatureDepth.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
58
59
60attributes and members directly related to the vpBasicFeature needs
61other functionalities ar useful but not mandatory
62
63
64
65
66
67*/
68
73{
74 // feature dimension
75 dim_s = 1;
76 nbParameters = 3;
77
78 // memory allocation
79 s.resize(dim_s);
80 if (flags == NULL)
81 flags = new bool[nbParameters];
82 for (unsigned int i = 0; i < nbParameters; i++)
83 flags[i] = false;
84 x = y = 0.;
85 Z = 1.;
86}
87
91vpFeatureDepth::vpFeatureDepth() : x(0), y(0), Z(1.) { init(); }
92
99void vpFeatureDepth::set_LogZoverZstar(double LogZoverZstar) { s[0] = LogZoverZstar; }
100
107double vpFeatureDepth::get_LogZoverZstar() const { return s[0]; }
108
116{
117 this->x = x_;
118 flags[0] = true;
119}
120
127double vpFeatureDepth::get_x() const { return x; }
128
136{
137 this->y = y_;
138 flags[1] = true;
139}
140
147double vpFeatureDepth::get_y() const { return y; }
148
156{
157 this->Z = Z_;
158 flags[2] = true;
159}
160
167double vpFeatureDepth::get_Z() const { return Z; }
168
181void vpFeatureDepth::set_xyZLogZoverZstar(double x_, double y_, double Z_, double LogZoverZstar)
182{
183 set_x(x_);
184 set_y(y_);
185 set_Z(Z_);
186 set_LogZoverZstar(LogZoverZstar);
187 for (unsigned int i = 0; i < nbParameters; i++)
188 flags[i] = true;
189}
190
217{
218 vpMatrix L;
219
221 for (unsigned int i = 0; i < nbParameters; i++) {
222 if (flags[i] == false) {
223 switch (i) {
224 case 0:
225 vpTRACE("Warning !!! The interaction matrix is computed but x was "
226 "not set yet");
227 break;
228 case 1:
229 vpTRACE("Warning !!! The interaction matrix is computed but y was "
230 "not set yet");
231 break;
232 case 2:
233 vpTRACE("Warning !!! The interaction matrix is computed but z was "
234 "not set yet");
235 break;
236 default:
237 vpTRACE("Problem during the reading of the variable flags");
238 }
239 }
240 }
241 resetFlags();
242 }
243
244 L.resize(1, 6);
245
246 double x_ = get_x();
247 double y_ = get_y();
248 double Z_ = get_Z();
249
250 if (Z_ < 0) {
251 vpERROR_TRACE("Point is behind the camera ");
252 std::cout << "Z = " << Z_ << std::endl;
253
254 throw(vpFeatureException(vpFeatureException::badInitializationError, "Point is behind the camera "));
255 }
256
257 if (fabs(Z_) < 1e-6) {
258 vpERROR_TRACE("Point Z coordinates is null ");
259 std::cout << "Z = " << Z_ << std::endl;
260
261 throw(vpFeatureException(vpFeatureException::badInitializationError, "Point Z coordinates is null"));
262 }
263
264 if (FEATURE_ALL & select) {
265 L = 0;
266 L[0][0] = 0;
267 L[0][1] = 0;
268 L[0][2] = -1 / Z_;
269 L[0][3] = -y_;
270 L[0][4] = x_;
271 L[0][5] = 0;
272 }
273
274 return L;
275}
276
313vpColVector vpFeatureDepth::error(const vpBasicFeature &s_star, unsigned int select)
314{
315
316 if (fabs(s_star.get_s().sumSquare()) > 1e-6) {
317 vpERROR_TRACE("s* should be zero ! ");
319 }
320
321 vpColVector e(1);
322 if (FEATURE_ALL & select) {
323 e[0] = s[0];
324 }
325
326 return e;
327}
328
344void vpFeatureDepth::print(unsigned int select) const
345{
346 if (FEATURE_ALL & select) {
347 std::cout << "Point: x=" << get_x();
348 std::cout << " Point: y=" << get_y();
349 std::cout << " Point: Z=" << get_Z();
350
351 std::cout << " log(Z/Z*)=" << get_LogZoverZstar();
352
353 std::cout << std::endl;
354 }
355}
356
368void vpFeatureDepth::buildFrom(double x_, double y_, double Z_, double LogZoverZstar)
369{
370
371 s[0] = LogZoverZstar;
372
373 this->x = x_;
374 this->y = y_;
375 this->Z = Z_;
376
377 if (Z < 0) {
378 vpERROR_TRACE("Point is behind the camera ");
379 std::cout << "Z = " << Z << std::endl;
380
381 throw(vpFeatureException(vpFeatureException::badInitializationError, "Point is behind the camera "));
382 }
383
384 if (fabs(Z) < 1e-6) {
385 vpERROR_TRACE("Point Z coordinates is null ");
386 std::cout << "Z = " << Z << std::endl;
387
388 throw(vpFeatureException(vpFeatureException::badInitializationError, "Point Z coordinates is null"));
389 }
390
391 for (unsigned int i = 0; i < nbParameters; i++)
392 flags[i] = true;
393}
394
406{
407 vpFeatureDepth *feature = new vpFeatureDepth;
408 return feature;
409}
410
417 const vpColor & /* color */, unsigned int /* thickness */) const
418{
419 static int firsttime = 0;
420
421 if (firsttime == 0) {
422 firsttime = 1;
423 vpERROR_TRACE("not implemented");
424 // Do not throw and error since it is not subject
425 // to produce a failure
426 }
427}
433void vpFeatureDepth::display(const vpCameraParameters & /* cam */, const vpImage<vpRGBa> & /* I */,
434 const vpColor & /* color */, unsigned int /* thickness */) const
435{
436 static int firsttime = 0;
437
438 if (firsttime == 0) {
439 firsttime = 1;
440 vpERROR_TRACE("not implemented");
441 // Do not throw and error since it is not subject
442 // to produce a failure
443 }
444}
445
446/*
447 * Local variables:
448 * c-basic-offset: 2
449 * End:
450 */
class that defines what is a visual feature
vpColVector s
State of the visual feature.
unsigned int nbParameters
Number of parameters needed to compute the interaction matrix.
vpColVector get_s(unsigned int select=FEATURE_ALL) const
Get the feature vector .
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.
double sumSquare() const
void resize(unsigned int i, bool flagNullify=true)
Class to define RGB colors available for display functionalities.
Definition vpColor.h:152
Class that defines a 3D point visual feature which is composed by one parameters that is that defin...
void display(const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1) const
void set_x(double x)
void buildFrom(double x, double y, double Z, double LogZoverZstar)
double get_y() const
vpMatrix interaction(unsigned int select=FEATURE_ALL)
void set_y(double y)
double get_LogZoverZstar() const
void set_LogZoverZstar(double LogZoverZstar)
void set_xyZLogZoverZstar(double x, double y, double Z, double logZZs)
vpFeatureDepth * duplicate() const
vpColVector error(const vpBasicFeature &s_star, unsigned int select=FEATURE_ALL)
void print(unsigned int select=FEATURE_ALL) const
double get_Z() const
void set_Z(double Z)
double get_x() const
Error that can be emitted by the vpBasicFeature class and its derivates.
@ badInitializationError
Wrong feature initialization.
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