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
57 /*!
58
59 Initialise the memory space requested for a 3D point visual
60 feature.
61
62 By default this feature is initialized to \f${\bf X} = (0, 0, 1)\f$.
63 */
init()64 void vpFeaturePoint3D::init()
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
83 /*!
84
85 Default constructor that build a 3D point visual feature and
86 initialize it to \f${\bf X} = (0, 0, 1)\f$.
87
88 */
vpFeaturePoint3D()89 vpFeaturePoint3D::vpFeaturePoint3D() { init(); }
90
91 /*!
92
93 Initialise the \f$X\f$ coordinate in the camera frame of the 3D Point
94 visual feature \f${\bf X} = (X,Y,Z)\f$.
95
96 \param X : \f$X\f$ coordinate of the visual feature.
97 \sa get_X()
98
99 */
set_X(double X)100 void vpFeaturePoint3D::set_X(double X)
101 {
102 s[0] = X;
103 flags[0] = true;
104 }
105
106 /*!
107
108 Initialise the \f$Y\f$ coordinate in the camera frame of the 3D Point
109 visual feature \f${\bf X} = (X,Y,Z)\f$.
110
111 \param Y : \f$Y\f$ coordinate of the visual feature.
112 \sa get_Y()
113
114 */
set_Y(double Y)115 void vpFeaturePoint3D::set_Y(double Y)
116 {
117 s[1] = Y;
118 flags[1] = true;
119 }
120
121 /*!
122
123 Initialise the \f$Z\f$ coordinate in the camera frame of the 3D Point
124 visual feature \f${\bf X} = (X,Y,Z)\f$.
125
126 \param Z : \f$Z\f$ coordinate or depth of the visual feature.
127 \sa get_Z()
128
129 */
set_Z(double Z)130 void vpFeaturePoint3D::set_Z(double Z)
131 {
132 s[2] = Z;
133 flags[2] = true;
134 }
135
136 /*!
137 Initialize the 3D point coordinates.
138
139 \param X,Y,Z : \f$(X,Y,Z)\f$ coordinates in the camera frame of the
140 3D point visual feature.
141
142 \sa set_X(), set_Y(), set_Z()
143 */
set_XYZ(double X,double Y,double Z)144 void 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
154 //! Return the \f$X\f$ coordinate in the camera frame of the 3D point.
get_X() const155 double vpFeaturePoint3D::get_X() const { return s[0]; }
156
157 //! Return the \f$Y\f$ coordinate in the camera frame of the 3D point.
get_Y() const158 double vpFeaturePoint3D::get_Y() const { return s[1]; }
159
160 //! Return the \f$Z\f$ coordinate in the camera frame of the 3D point.
get_Z() const161 double vpFeaturePoint3D::get_Z() const { return s[2]; }
162
163 /*!
164 Compute and return the interaction matrix \f$ L \f$ associated to a subset
165 of the possible 3D point features \f$(X,Y,Z)\f$ that
166 represent the 3D point coordinates expressed in the camera frame.
167
168 \f[
169 L = \left[
170 \begin{array}{rrrrrr}
171 -1 & 0 & 0 & 0 & -Z & Y \\
172 0 & -1 & 0 & Z & 0 & -X \\
173 0 & 0 & -1 & -Y & X & 0 \\
174 \end{array}
175 \right]
176 \f]
177
178
179 \param select : Selection of a subset of the possible 3D point coordinate
180 features.
181 - To compute the interaction matrix for all the three
182 subset features \f$(X,Y,Z)\f$ use vpBasicFeature::FEATURE_ALL. In
183 that case the dimension of the interaction matrix is \f$ [3 \times
184 6] \f$
185 - To compute the interaction matrix for only one of the
186 subset (\f$X, Y,Z\f$) use
187 one of the corresponding function selectX(), selectY() or
188 selectZ(). In that case the returned interaction matrix is \f$ [1
189 \times 6] \f$ dimension.
190
191 \return The interaction matrix computed from the 3D point coordinate
192 features.
193
194 The code below shows how to compute the interaction matrix
195 associated to the visual feature \f$s = X \f$.
196
197 \code
198 vpPoint point;
199 ...
200 // Creation of the current feature s
201 vpFeaturePoint3D s;
202 s.buildFrom(point);
203
204 vpMatrix L_X = s.interaction( vpFeaturePoint3D::selectX() );
205 \endcode
206
207 The code below shows how to compute the interaction matrix
208 associated to the \f$s = (X,Y) \f$
209 subset visual feature:
210
211 \code
212 vpMatrix L_XY = s.interaction( vpFeaturePoint3D::selectX() | vpFeaturePoint3D::selectY() );
213 \endcode
214
215 L_XY is here now a 2 by 6 matrix. The first line corresponds to
216 the \f$ X \f$ visual feature while the second one to the \f$
217 Y \f$ visual feature.
218
219 It is also possible to build the interaction matrix from all the
220 3D point coordinates by:
221
222 \code
223 vpMatrix L_XYZ = s.interaction( vpBasicFeature::FEATURE_ALL );
224 \endcode
225
226 In that case, L_XYZ is a 3 by 6 interaction matrix where the last
227 line corresponds to the \f$ Z \f$ visual feature.
228
229 */
interaction(unsigned int select)230 vpMatrix vpFeaturePoint3D::interaction(unsigned int select)
231 {
232 vpMatrix L;
233
234 L.resize(0, 6);
235
236 if (deallocate == vpBasicFeature::user) {
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
307 /*!
308 Compute the error \f$ (s-s^*)\f$ between the current and the desired
309 visual features from a subset of the possible features.
310
311 \param s_star : Desired 3D point visual feature.
312
313 \param select : The error can be computed for a selection of a
314 subset of the possible 3D point coordinate features.
315 - To compute the error for all the three coordinates use
316 vpBasicFeature::FEATURE_ALL. In that case the error vector is a 3
317 dimension column vector.
318 - To compute the error for only one of the coordinate
319 feature \f$(X,Y, or Z)\f$ use one of the
320 corresponding function selectX(), selectY() or selectZ(). In
321 that case the error vector is a 1 dimension column vector.
322
323 \return The error \f$ (s-s^*)\f$ between the current and the desired
324 visual feature.
325
326 The code below shows how to use this method to manipulate the \f$
327 Z \f$ subset:
328
329 \code
330 // Creation of the current feature s
331 vpFeaturePoint3D s;
332 s.set_Z(0.8); // Initialization of the current Z feature
333
334 // Creation of the desired feature s*.
335 vpFeatureTranslation s_star;
336 s_star.set_Z(1); // Initialization of the current Z* feature to Z*=1 meter
337
338 // Compute the interaction matrix for the Z coordinate feature
339 vpMatrix L_Z = s.interaction( vpFeaturePoint3D::selectZ() );
340
341 // Compute the error vector (s-s*) for the Z feature
342 s.error(s_star, vpFeaturePoint3D::selectZ());
343 \endcode
344
345 To manipulate the subset features \f$s=(Y, Z)\f$,
346 the code becomes:
347 \code
348 // Compute the interaction matrix for the Y, Z feature coordinates
349 vpMatrix L_YZ = s.interaction( vpFeaturePoint3D::selectY() |
350 vpFeaturePoint3D::selectZ() );
351
352 // Compute the error vector e = (s-s*) for the Y, Z feature coordinates
353 vpColVector e = s.error(s_star, vpFeaturePoint3D::selectY() |
354 vpFeaturePoint3D::selectZ());
355 \endcode
356
357 */
error(const vpBasicFeature & s_star,unsigned int select)358 vpColVector 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
388 /*!
389
390 Build a 3D point visual feature from the camera frame coordinates
391 \f$(X,Y,Z)\f$ of a point.
392
393 \param p : A point with camera frame coordinates \f${^c}P=(X,Y,Z)\f$
394 up to date (see vpPoint class).
395
396 \exception vpFeatureException::badInitializationError: If the depth
397 (\f$Z\f$ coordinate) is negative. That means that the 3D point is
398 behind the camera which is not possible.
399
400 \exception vpFeatureException::badInitializationError: If the depth
401 (\f$Z\f$ coordinate) is null. That means that the 3D point is
402 on the camera which is not possible.
403 */
buildFrom(const vpPoint & p)404 void vpFeaturePoint3D::buildFrom(const vpPoint &p)
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
432 /*!
433
434 Build a 3D point visual feature from the camera frame coordinates
435 \f$(X,Y,Z)\f$ of a point.
436
437 \param X,Y,Z : Camera frame coordinates \f$(X,Y,Z)\f$ of a 3D point.
438
439 \exception vpFeatureException::badInitializationError: If the depth
440 (\f$Z\f$ coordinate) is negative. That means that the 3D point is
441 on the camera which is not possible.
442
443 \exception vpFeatureException::badInitializationError: If the depth
444 (\f$Z\f$ coordinate) is null. That means that the 3D point is
445 on the camera which is not possible.
446
447 */
buildFrom(double X,double Y,double Z)448 void 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
473 /*!
474 Print to stdout the values of the current visual feature \f$ s \f$.
475
476 \param select : Selection of a subset of the possible 3D point
477 feature coordinates.
478 - To print all the three coordinates used as features use
479 vpBasicFeature::FEATURE_ALL.
480 - To print only one of the coordinate
481 feature \f$(X,Y,Z)\f$ use one of the
482 corresponding function selectX(), selectX() or selectZ().
483
484 \code
485 vpPoint point;
486
487 // Creation of the current feature s
488 vpFeaturePoint3D s;
489 s.buildFrom(point);
490
491 s.print(); // print all the 3 components of the translation feature
492 s.print(vpBasicFeature::FEATURE_ALL); // same behavior then previous line
493 s.print(vpFeaturePoint3D::selectZ()); // print only the Z component
494 \endcode
495 */
print(unsigned int select) const496 void 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
509 /*!
510
511 Create an object with the same type.
512
513 \code
514 vpBasicFeature *s_star;
515 vpFeaturePoint3D s;
516 s_star = s.duplicate(); // s_star is now a vpFeaturePoint3D
517 \endcode
518
519 */
duplicate() const520 vpFeaturePoint3D *vpFeaturePoint3D::duplicate() const
521 {
522 vpFeaturePoint3D *feature = new vpFeaturePoint3D;
523 return feature;
524 }
525
526 /*!
527
528 Not implemented.
529 */
display(const vpCameraParameters &,const vpImage<unsigned char> &,const vpColor &,unsigned int) const530 void vpFeaturePoint3D::display(const vpCameraParameters & /*cam*/, const vpImage<unsigned char> & /* I */,
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
543 /*!
544
545 Not implemented.
546 */
display(const vpCameraParameters &,const vpImage<vpRGBa> &,const vpColor &,unsigned int) const547 void vpFeaturePoint3D::display(const vpCameraParameters & /*cam*/, const vpImage<vpRGBa> & /* I */,
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 }
559 /*!
560
561 Function used to select the \f$ X\f$ subset coordinate of the 3D point
562 visual feature.
563
564 This function is to use in conjunction with interaction() in order
565 to compute the interaction matrix associated to \f$ X\f$ feature.
566
567 See the interaction() method for an usage example.
568
569 This function is also useful in the vpServo class to indicate that
570 a subset of the visual feature is to use in the control law:
571
572 \code
573 vpFeaturePoint3D p;
574 vpServo task;
575 ...
576 // Add the (X,Y) subset coordinates features from a 3D point to the task
577 task.addFeature(p, vpFeaturePoint3D::selectX() |
578 vpFeaturePoint3D::selectY());
579 \endcode
580
581 \sa selectY(), selectZ()
582
583 */
selectX()584 unsigned int vpFeaturePoint3D::selectX() { return FEATURE_LINE[0]; }
585
586 /*!
587
588 Function used to select the \f$ Y\f$ subset coordinate of the 3D point
589 visual feature.
590
591 This function is to use in conjunction with interaction() in order
592 to compute the interaction matrix associated to \f$ Y\f$ feature.
593
594 See the interaction() method for an usage example.
595
596 This function is also useful in the vpServo class to indicate that
597 a subset of the visual feature is to use in the control law:
598
599 \code
600 vpFeaturePoint3D p;
601 vpServo task;
602 ...
603 // Add the (X,Y) subset coordinates features from a 3D point to the task
604 task.addFeature(p, vpFeaturePoint3D::selectX() |
605 vpFeaturePoint3D::selectY());
606 \endcode
607
608 \sa selectX(), selectZ()
609
610 */
selectY()611 unsigned int vpFeaturePoint3D::selectY() { return FEATURE_LINE[1]; }
612
613 /*!
614
615 Function used to select the \f$ Z\f$ subset coordinate of the 3D point
616 visual feature.
617
618 This function is to use in conjunction with interaction() in order
619 to compute the interaction matrix associated to \f$ Z\f$ feature.
620
621 See the interaction() method for an usage example.
622
623 This function is also useful in the vpServo class to indicate that
624 a subset of the visual feature is to use in the control law:
625
626 \code
627 vpFeaturePoint3D p;
628 vpServo task;
629 ...
630 // Add the (Z) subset coordinate feature from a 3D point to the task
631 task.addFeature(p, vpFeaturePoint3D::selectZ());
632 \endcode
633
634 \sa selectX(), selectY()
635
636 */
selectZ()637 unsigned int vpFeaturePoint3D::selectZ() { return FEATURE_LINE[2]; }
638