Visual Servoing Platform version 3.5.0
vpFeaturePoint3D.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 * 3D point visual feature.
33 *
34 * Authors:
35 * Eric Marchand
36 * Fabien Spindler
37 *
38 *****************************************************************************/
39
40#include <visp3/visual_features/vpBasicFeature.h>
41#include <visp3/visual_features/vpFeaturePoint3D.h>
42
43// Exception
44#include <visp3/core/vpException.h>
45#include <visp3/visual_features/vpFeatureException.h>
46
47// Debug trace
48#include <visp3/core/vpDebug.h>
49
50/*
51
52 attributes and members directly related to the vpBasicFeature needs
53 other functionalities are useful but not mandatory
54
55*/
56
65{
66 // feature dimension
67 dim_s = 3;
68 nbParameters = 3;
69
70 // memory allocation
71 s.resize(dim_s);
72 if (flags == NULL)
73 flags = new bool[nbParameters];
74 for (unsigned int i = 0; i < nbParameters; i++)
75 flags[i] = false;
76
77 // default value XYZ
78 s[0] = 0;
79 s[1] = 0;
80 s[2] = 1;
81}
82
90
101{
102 s[0] = X;
103 flags[0] = true;
104}
105
116{
117 s[1] = Y;
118 flags[1] = true;
119}
120
131{
132 s[2] = Z;
133 flags[2] = true;
134}
135
144void vpFeaturePoint3D::set_XYZ(double X, double Y, double Z)
145{
146 set_X(X);
147 set_Y(Y);
148 set_Z(Z);
149
150 for (unsigned int i = 0; i < nbParameters; i++)
151 flags[i] = true;
152}
153
155double vpFeaturePoint3D::get_X() const { return s[0]; }
156
158double vpFeaturePoint3D::get_Y() const { return s[1]; }
159
161double vpFeaturePoint3D::get_Z() const { return s[2]; }
162
231{
232 vpMatrix L;
233
234 L.resize(0, 6);
235
237 for (unsigned int i = 0; i < nbParameters; i++) {
238 if (flags[i] == false) {
239 switch (i) {
240 case 0:
241 vpTRACE("Warning !!! The interaction matrix is computed but X was "
242 "not set yet");
243 break;
244 case 1:
245 vpTRACE("Warning !!! The interaction matrix is computed but Y was "
246 "not set yet");
247 break;
248 case 2:
249 vpTRACE("Warning !!! The interaction matrix is computed but Z was "
250 "not set yet");
251 break;
252 default:
253 vpTRACE("Problem during the reading of the variable flags");
254 }
255 }
256 }
257 resetFlags();
258 }
259
260 double X = get_X();
261 double Y = get_Y();
262 double Z = get_Z();
263
264 if (vpFeaturePoint3D::selectX() & select) {
265 vpMatrix Lx(1, 6);
266 Lx = 0;
267
268 Lx[0][0] = -1;
269 Lx[0][1] = 0;
270 Lx[0][2] = 0;
271 Lx[0][3] = 0;
272 Lx[0][4] = -Z;
273 Lx[0][5] = Y;
274
275 L = vpMatrix::stack(L, Lx);
276 }
277
278 if (vpFeaturePoint3D::selectY() & select) {
279 vpMatrix Ly(1, 6);
280 Ly = 0;
281
282 Ly[0][0] = 0;
283 Ly[0][1] = -1;
284 Ly[0][2] = 0;
285 Ly[0][3] = Z;
286 Ly[0][4] = 0;
287 Ly[0][5] = -X;
288
289 L = vpMatrix::stack(L, Ly);
290 }
291 if (vpFeaturePoint3D::selectZ() & select) {
292 vpMatrix Lz(1, 6);
293 Lz = 0;
294
295 Lz[0][0] = 0;
296 Lz[0][1] = 0;
297 Lz[0][2] = -1;
298 Lz[0][3] = -Y;
299 Lz[0][4] = X;
300 Lz[0][5] = 0;
301
302 L = vpMatrix::stack(L, Lz);
303 }
304 return L;
305}
306
358vpColVector vpFeaturePoint3D::error(const vpBasicFeature &s_star, unsigned int select)
359{
360 vpColVector e(0);
361
362 try {
363 if (vpFeaturePoint3D::selectX() & select) {
364 vpColVector ex(1);
365 ex[0] = s[0] - s_star[0];
366
367 e = vpColVector::stack(e, ex);
368 }
369
370 if (vpFeaturePoint3D::selectY() & select) {
371 vpColVector ey(1);
372 ey[0] = s[1] - s_star[1];
373 e = vpColVector::stack(e, ey);
374 }
375
376 if (vpFeaturePoint3D::selectZ() & select) {
377 vpColVector ez(1);
378 ez[0] = s[2] - s_star[2];
379 e = vpColVector::stack(e, ez);
380 }
381 } catch (...) {
382 throw;
383 }
384
385 return e;
386}
387
405{
406
407 // cP is expressed in homogeneous coordinates
408 // we devide by the fourth coordinate
409 s[0] = p.cP[0] / p.cP[3];
410 s[1] = p.cP[1] / p.cP[3];
411 s[2] = p.cP[2] / p.cP[3];
412
413 double Z = s[2];
414 if (Z < 0) {
415 vpERROR_TRACE("Point is behind the camera ");
416 std::cout << "Z = " << Z << std::endl;
417
418 throw(vpFeatureException(vpFeatureException::badInitializationError, "Point is behind the camera "));
419 }
420
421 if (fabs(Z) < 1e-6) {
422 vpERROR_TRACE("Point Z coordinates is null ");
423 std::cout << "Z = " << Z << std::endl;
424
425 throw(vpFeatureException(vpFeatureException::badInitializationError, "Point Z coordinates is null"));
426 }
427
428 for (unsigned int i = 0; i < nbParameters; i++)
429 flags[i] = true;
430}
431
448void vpFeaturePoint3D::buildFrom(double X, double Y, double Z)
449{
450
451 s[0] = X;
452 s[1] = Y;
453 s[2] = Z;
454
455 if (Z < 0) {
456 vpERROR_TRACE("Point is behind the camera ");
457 std::cout << "Z = " << Z << std::endl;
458
459 throw(vpFeatureException(vpFeatureException::badInitializationError, "Point is behind the camera "));
460 }
461
462 if (fabs(Z) < 1e-6) {
463 vpERROR_TRACE("Point Z coordinates is null ");
464 std::cout << "Z = " << Z << std::endl;
465
466 throw(vpFeatureException(vpFeatureException::badInitializationError, "Point Z coordinates is null"));
467 }
468
469 for (unsigned int i = 0; i < nbParameters; i++)
470 flags[i] = true;
471}
472
496void vpFeaturePoint3D::print(unsigned int select) const
497{
498
499 std::cout << "Point3D: ";
500 if (vpFeaturePoint3D::selectX() & select)
501 std::cout << " X=" << get_X();
502 if (vpFeaturePoint3D::selectY() & select)
503 std::cout << " Y=" << get_Y();
504 if (vpFeaturePoint3D::selectZ() & select)
505 std::cout << " Z=" << get_Z();
506 std::cout << std::endl;
507}
508
521{
522 vpFeaturePoint3D *feature = new vpFeaturePoint3D;
523 return feature;
524}
525
531 const vpColor & /* color */, unsigned int /* thickness */) const
532{
533 static int firsttime = 0;
534
535 if (firsttime == 0) {
536 firsttime = 1;
537 vpERROR_TRACE("not implemented");
538 // Do not throw and error since it is not subject
539 // to produce a failure
540 }
541}
542
548 const vpColor & /* color */, unsigned int /* thickness */) const
549{
550 static int firsttime = 0;
551
552 if (firsttime == 0) {
553 firsttime = 1;
554 vpERROR_TRACE("not implemented");
555 // Do not throw and error since it is not subject
556 // to produce a failure
557 }
558}
584unsigned int vpFeaturePoint3D::selectX() { return FEATURE_LINE[0]; }
585
611unsigned int vpFeaturePoint3D::selectY() { return FEATURE_LINE[1]; }
612
637unsigned 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.
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
Error that can be emited by the vpBasicFeature class and its derivates.
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)
void set_Y(double Y)
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_Z(double Z)
void set_X(double X)
double get_Z() const
Return the coordinate in the camera frame of the 3D point.
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
void stack(const vpMatrix &A)
Definition: vpMatrix.cpp:5879
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:82
vpColVector cP
Definition: vpTracker.h:77
#define vpTRACE
Definition: vpDebug.h:416
#define vpERROR_TRACE
Definition: vpDebug.h:393