Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpFeatureTranslation.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 translation visual feature.
33 *
34*****************************************************************************/
35
36#include <visp3/visual_features/vpBasicFeature.h>
37#include <visp3/visual_features/vpFeatureTranslation.h>
38
39#include <visp3/core/vpMath.h>
40
41// Exception
42#include <visp3/core/vpException.h>
43#include <visp3/visual_features/vpFeatureException.h>
44
45// Debug trace
46#include <visp3/core/vpDebug.h>
47
52/*
53
54attributes and members directly related to the vpBasicFeature needs
55other functionalities are useful but not mandatory
56
57*/
58
65{
66 // feature dimension
67 dim_s = 3;
68 nbParameters = 1;
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
85
97
111 : f2Mf1(), translation(r)
112{
113 init();
114
115 buildFrom(f2Mf1_);
116}
117
128{
129 this->f2Mf1 = f2Mf1_;
130 s[0] = f2Mf1[0][3];
131 s[1] = f2Mf1[1][3];
132 s[2] = f2Mf1[2][3];
133
134 flags[0] = true;
135}
136
150
160void vpFeatureTranslation::set_Tx(double t_x) { s[0] = t_x; }
170void vpFeatureTranslation::set_Ty(double t_y) { s[1] = t_y; }
180void vpFeatureTranslation::set_Tz(double t_z) { s[2] = t_z; }
181
195
201double vpFeatureTranslation::get_Tx() const { return s[0]; }
202
208double vpFeatureTranslation::get_Ty() const { return s[1]; }
209
215double vpFeatureTranslation::get_Tz() const { return s[2]; }
216
299{
300
301 vpMatrix L;
302 L.resize(0, 6);
303
305 for (unsigned int i = 0; i < nbParameters; i++) {
306 if (flags[i] == false) {
307 switch (i) {
308 case 0:
309 vpTRACE("Warning !!! The interaction matrix is computed but f2Mf1 "
310 "was not set yet");
311 break;
312 default:
313 vpTRACE("Problem during the reading of the variable flags");
314 }
315 }
316 }
317 resetFlags();
318 }
319
320 if (translation == cdMc) {
321 // This version is a simplification
322 if (vpFeatureTranslation::selectTx() & select) {
323 vpMatrix Lx(1, 6);
324
325 for (int i = 0; i < 3; i++)
326 Lx[0][i] = f2Mf1[0][i];
327 Lx[0][3] = 0;
328 Lx[0][4] = 0;
329 Lx[0][5] = 0;
330
331 L = vpMatrix::stack(L, Lx);
332 }
333
334 if (vpFeatureTranslation::selectTy() & select) {
335 vpMatrix Ly(1, 6);
336
337 for (int i = 0; i < 3; i++)
338 Ly[0][i] = f2Mf1[1][i];
339 Ly[0][3] = 0;
340 Ly[0][4] = 0;
341 Ly[0][5] = 0;
342
343 L = vpMatrix::stack(L, Ly);
344 }
345
346 if (vpFeatureTranslation::selectTz() & select) {
347 vpMatrix Lz(1, 6);
348
349 for (int i = 0; i < 3; i++)
350 Lz[0][i] = f2Mf1[2][i];
351 Lz[0][3] = 0;
352 Lz[0][4] = 0;
353 Lz[0][5] = 0;
354
355 L = vpMatrix::stack(L, Lz);
356 }
357 }
358 if (translation == cMcd) {
359 // This version is a simplification
360 if (vpFeatureTranslation::selectTx() & select) {
361 vpMatrix Lx(1, 6);
362 Lx[0][0] = -1;
363 Lx[0][1] = 0;
364 Lx[0][2] = 0;
365 Lx[0][3] = 0;
366 Lx[0][4] = -s[2];
367 Lx[0][5] = s[1];
368
369 L = vpMatrix::stack(L, Lx);
370 }
371
372 if (vpFeatureTranslation::selectTy() & select) {
373 vpMatrix Ly(1, 6);
374 Ly[0][0] = 0;
375 Ly[0][1] = -1;
376 Ly[0][2] = 0;
377 Ly[0][3] = s[2];
378 Ly[0][4] = 0;
379 Ly[0][5] = -s[0];
380
381 L = vpMatrix::stack(L, Ly);
382 }
383
384 if (vpFeatureTranslation::selectTz() & select) {
385 vpMatrix Lz(1, 6);
386 Lz[0][0] = 0;
387 Lz[0][1] = 0;
388 Lz[0][2] = -1;
389 Lz[0][3] = -s[1];
390 Lz[0][4] = s[0];
391 Lz[0][5] = 0;
392
393 L = vpMatrix::stack(L, Lz);
394 }
395 }
396
397 if (translation == cMo) {
398 // This version is a simplification
399 if (vpFeatureTranslation::selectTx() & select) {
400 vpMatrix Lx(1, 6);
401 Lx[0][0] = -1;
402 Lx[0][1] = 0;
403 Lx[0][2] = 0;
404 Lx[0][3] = 0;
405 Lx[0][4] = -s[2];
406 Lx[0][5] = s[1];
407
408 L = vpMatrix::stack(L, Lx);
409 }
410
411 if (vpFeatureTranslation::selectTy() & select) {
412 vpMatrix Ly(1, 6);
413 Ly[0][0] = 0;
414 Ly[0][1] = -1;
415 Ly[0][2] = 0;
416 Ly[0][3] = s[2];
417 Ly[0][4] = 0;
418 Ly[0][5] = -s[0];
419
420 L = vpMatrix::stack(L, Ly);
421 }
422
423 if (vpFeatureTranslation::selectTz() & select) {
424 vpMatrix Lz(1, 6);
425 Lz[0][0] = 0;
426 Lz[0][1] = 0;
427 Lz[0][2] = -1;
428 Lz[0][3] = -s[1];
429 Lz[0][4] = s[0];
430 Lz[0][5] = 0;
431
432 L = vpMatrix::stack(L, Lz);
433 }
434 }
435
436 return L;
437}
438
505vpColVector vpFeatureTranslation::error(const vpBasicFeature &s_star, unsigned int select)
506{
507 vpColVector e(0);
508
509 if (translation == cdMc || translation == cMcd) {
510 if (s_star.get_s().sumSquare() > 1e-6) {
511 vpERROR_TRACE("s* should be zero ! ");
513 }
514 }
515
516 if (vpFeatureTranslation::selectTx() & select) {
517 vpColVector ex(1);
518 ex[0] = s[0] - s_star[0];
519 e = vpColVector::stack(e, ex);
520 }
521
522 if (vpFeatureTranslation::selectTy() & select) {
523 vpColVector ey(1);
524 ey[0] = s[1] - s_star[1];
525 e = vpColVector::stack(e, ey);
526 }
527
528 if (vpFeatureTranslation::selectTz() & select) {
529 vpColVector ez(1);
530 ez[0] = s[2] - s_star[2];
531 e = vpColVector::stack(e, ez);
532 }
533
534 return e;
535}
536
560void vpFeatureTranslation::print(unsigned int select) const
561{
562 std::cout << "Translation 3D:";
563 if (vpFeatureTranslation::selectTx() & select) {
564 std::cout << " tx=" << s[0];
565 }
566 if (vpFeatureTranslation::selectTy() & select) {
567 std::cout << " ty=" << s[1];
568 }
569 if (vpFeatureTranslation::selectTz() & select) {
570 std::cout << " tz=" << s[2];
571 }
572 std::cout << std::endl;
573}
574
589{
590 vpFeatureTranslation *feature = NULL;
591 if (translation == cdMc)
592 feature = new vpFeatureTranslation(cdMc);
593 if (translation == cMo)
594 feature = new vpFeatureTranslation(cMo);
595 if (translation == cMcd)
596 feature = new vpFeatureTranslation(cMcd);
597 return feature;
598}
599
606 const vpColor & /* color */, unsigned int /* thickness */) const
607{
608 static int firsttime = 0;
609
610 if (firsttime == 0) {
611 firsttime = 1;
612 vpERROR_TRACE("not implemented");
613 // Do not throw and error since it is not subject
614 // to produce a failure
615 }
616}
623 const vpColor & /* color */, unsigned int /* thickness */) const
624{
625 static int firsttime = 0;
626
627 if (firsttime == 0) {
628 firsttime = 1;
629 vpERROR_TRACE("not implemented");
630 // Do not throw and error since it is not subject
631 // to produce a failure
632 }
633}
634
679unsigned int vpFeatureTranslation::selectTx() { return FEATURE_LINE[0]; }
680
724unsigned int vpFeatureTranslation::selectTy() { return FEATURE_LINE[1]; }
725
769unsigned int vpFeatureTranslation::selectTz() { 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.
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 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 translation visual feature .
static unsigned int selectTz()
vpMatrix interaction(unsigned int select=FEATURE_ALL)
void buildFrom(const vpHomogeneousMatrix &f2Mf1)
vpColVector error(const vpBasicFeature &s_star, unsigned int select=FEATURE_ALL)
void display(const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1) const
void setFeatureTranslationType(const vpFeatureTranslationRepresentationType r)
vpFeatureTranslation * duplicate() const
Feature duplication.
static unsigned int selectTx()
vpFeatureTranslationRepresentationType getFeatureTranslationType() const
static unsigned int selectTy()
void print(unsigned int select=FEATURE_ALL) const
Implementation of an homogeneous matrix and operations on such kind of matrices.
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