Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpFeaturePoint3D.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 * 3D point visual feature.
33 *
34*****************************************************************************/
35
36#include <visp3/visual_features/vpBasicFeature.h>
37#include <visp3/visual_features/vpFeaturePoint3D.h>
38
39// Exception
40#include <visp3/core/vpException.h>
41#include <visp3/visual_features/vpFeatureException.h>
42
43// Debug trace
44#include <visp3/core/vpDebug.h>
45
46/*
47
48 attributes and members directly related to the vpBasicFeature needs
49 other functionalities are useful but not mandatory
50
51*/
52
61{
62 // feature dimension
63 dim_s = 3;
64 nbParameters = 3;
65
66 // memory allocation
67 s.resize(dim_s);
68 if (flags == NULL)
69 flags = new bool[nbParameters];
70 for (unsigned int i = 0; i < nbParameters; i++)
71 flags[i] = false;
72
73 // default value XYZ
74 s[0] = 0;
75 s[1] = 0;
76 s[2] = 1;
77}
78
86
97{
98 s[0] = X;
99 flags[0] = true;
100}
101
112{
113 s[1] = Y;
114 flags[1] = true;
115}
116
127{
128 s[2] = Z;
129 flags[2] = true;
130}
131
140void vpFeaturePoint3D::set_XYZ(double X, double Y, double Z)
141{
142 set_X(X);
143 set_Y(Y);
144 set_Z(Z);
145
146 for (unsigned int i = 0; i < nbParameters; i++)
147 flags[i] = true;
148}
149
151double vpFeaturePoint3D::get_X() const { return s[0]; }
152
154double vpFeaturePoint3D::get_Y() const { return s[1]; }
155
157double vpFeaturePoint3D::get_Z() const { return s[2]; }
158
227{
228 vpMatrix L;
229
230 L.resize(0, 6);
231
233 for (unsigned int i = 0; i < nbParameters; i++) {
234 if (flags[i] == false) {
235 switch (i) {
236 case 0:
237 vpTRACE("Warning !!! The interaction matrix is computed but X was "
238 "not set yet");
239 break;
240 case 1:
241 vpTRACE("Warning !!! The interaction matrix is computed but Y was "
242 "not set yet");
243 break;
244 case 2:
245 vpTRACE("Warning !!! The interaction matrix is computed but Z was "
246 "not set yet");
247 break;
248 default:
249 vpTRACE("Problem during the reading of the variable flags");
250 }
251 }
252 }
253 resetFlags();
254 }
255
256 double X = get_X();
257 double Y = get_Y();
258 double Z = get_Z();
259
260 if (vpFeaturePoint3D::selectX() & select) {
261 vpMatrix Lx(1, 6);
262 Lx = 0;
263
264 Lx[0][0] = -1;
265 Lx[0][1] = 0;
266 Lx[0][2] = 0;
267 Lx[0][3] = 0;
268 Lx[0][4] = -Z;
269 Lx[0][5] = Y;
270
271 L = vpMatrix::stack(L, Lx);
272 }
273
274 if (vpFeaturePoint3D::selectY() & select) {
275 vpMatrix Ly(1, 6);
276 Ly = 0;
277
278 Ly[0][0] = 0;
279 Ly[0][1] = -1;
280 Ly[0][2] = 0;
281 Ly[0][3] = Z;
282 Ly[0][4] = 0;
283 Ly[0][5] = -X;
284
285 L = vpMatrix::stack(L, Ly);
286 }
287 if (vpFeaturePoint3D::selectZ() & select) {
288 vpMatrix Lz(1, 6);
289 Lz = 0;
290
291 Lz[0][0] = 0;
292 Lz[0][1] = 0;
293 Lz[0][2] = -1;
294 Lz[0][3] = -Y;
295 Lz[0][4] = X;
296 Lz[0][5] = 0;
297
298 L = vpMatrix::stack(L, Lz);
299 }
300 return L;
301}
302
354vpColVector vpFeaturePoint3D::error(const vpBasicFeature &s_star, unsigned int select)
355{
356 vpColVector e(0);
357
358 try {
359 if (vpFeaturePoint3D::selectX() & select) {
360 vpColVector ex(1);
361 ex[0] = s[0] - s_star[0];
362
363 e = vpColVector::stack(e, ex);
364 }
365
366 if (vpFeaturePoint3D::selectY() & select) {
367 vpColVector ey(1);
368 ey[0] = s[1] - s_star[1];
369 e = vpColVector::stack(e, ey);
370 }
371
372 if (vpFeaturePoint3D::selectZ() & select) {
373 vpColVector ez(1);
374 ez[0] = s[2] - s_star[2];
375 e = vpColVector::stack(e, ez);
376 }
377 } catch (...) {
378 throw;
379 }
380
381 return e;
382}
383
401{
402
403 // cP is expressed in homogeneous coordinates
404 // we devide by the fourth coordinate
405 s[0] = p.cP[0] / p.cP[3];
406 s[1] = p.cP[1] / p.cP[3];
407 s[2] = p.cP[2] / p.cP[3];
408
409 double Z = s[2];
410 if (Z < 0) {
411 vpERROR_TRACE("Point is behind the camera ");
412 std::cout << "Z = " << Z << std::endl;
413
414 throw(vpFeatureException(vpFeatureException::badInitializationError, "Point is behind the camera "));
415 }
416
417 if (fabs(Z) < 1e-6) {
418 vpERROR_TRACE("Point Z coordinates is null ");
419 std::cout << "Z = " << Z << std::endl;
420
421 throw(vpFeatureException(vpFeatureException::badInitializationError, "Point Z coordinates is null"));
422 }
423
424 for (unsigned int i = 0; i < nbParameters; i++)
425 flags[i] = true;
426}
427
444void vpFeaturePoint3D::buildFrom(double X, double Y, double Z)
445{
446
447 s[0] = X;
448 s[1] = Y;
449 s[2] = Z;
450
451 if (Z < 0) {
452 vpERROR_TRACE("Point is behind the camera ");
453 std::cout << "Z = " << Z << std::endl;
454
455 throw(vpFeatureException(vpFeatureException::badInitializationError, "Point is behind the camera "));
456 }
457
458 if (fabs(Z) < 1e-6) {
459 vpERROR_TRACE("Point Z coordinates is null ");
460 std::cout << "Z = " << Z << std::endl;
461
462 throw(vpFeatureException(vpFeatureException::badInitializationError, "Point Z coordinates is null"));
463 }
464
465 for (unsigned int i = 0; i < nbParameters; i++)
466 flags[i] = true;
467}
468
492void vpFeaturePoint3D::print(unsigned int select) const
493{
494
495 std::cout << "Point3D: ";
496 if (vpFeaturePoint3D::selectX() & select)
497 std::cout << " X=" << get_X();
498 if (vpFeaturePoint3D::selectY() & select)
499 std::cout << " Y=" << get_Y();
500 if (vpFeaturePoint3D::selectZ() & select)
501 std::cout << " Z=" << get_Z();
502 std::cout << std::endl;
503}
504
517{
518 vpFeaturePoint3D *feature = new vpFeaturePoint3D;
519 return feature;
520}
521
527 const vpColor & /* color */, unsigned int /* thickness */) const
528{
529 static int firsttime = 0;
530
531 if (firsttime == 0) {
532 firsttime = 1;
533 vpERROR_TRACE("not implemented");
534 // Do not throw and error since it is not subject
535 // to produce a failure
536 }
537}
538
544 const vpColor & /* color */, unsigned int /* thickness */) const
545{
546 static int firsttime = 0;
547
548 if (firsttime == 0) {
549 firsttime = 1;
550 vpERROR_TRACE("not implemented");
551 // Do not throw and error since it is not subject
552 // to produce a failure
553 }
554}
580unsigned int vpFeaturePoint3D::selectX() { return FEATURE_LINE[0]; }
581
607unsigned int vpFeaturePoint3D::selectY() { return FEATURE_LINE[1]; }
608
633unsigned int vpFeaturePoint3D::selectZ() { return FEATURE_LINE[2]; }
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
Error that can be emitted by the vpBasicFeature class and its derivates.
@ badInitializationError
Wrong feature initialization.
Class that defines the 3D point visual feature.
void print(unsigned int select=FEATURE_ALL) const
static unsigned int selectX()
double get_X() const
Return the coordinate in the camera frame of the 3D point.
void set_XYZ(double X, double Y, double Z)
double get_Y() const
Return the coordinate in the camera frame of the 3D point.
static unsigned int selectZ()
vpMatrix interaction(unsigned int select=FEATURE_ALL)
vpFeaturePoint3D * duplicate() const
void buildFrom(const vpPoint &p)
static unsigned int selectY()
void display(const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1) const
vpColVector error(const vpBasicFeature &s_star, unsigned int select=FEATURE_ALL)
void set_X(double X)
double get_Z() const
Return the coordinate in the camera frame of the 3D point.
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)
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition vpPoint.h:77
vpColVector cP
Definition vpTracker.h:72
#define vpTRACE
Definition vpDebug.h:411
#define vpERROR_TRACE
Definition vpDebug.h:388