Visual Servoing Platform version 3.5.0
vpFeatureTranslation.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 translation 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/vpFeatureTranslation.h>
42
43#include <visp3/core/vpMath.h>
44
45// Exception
46#include <visp3/core/vpException.h>
47#include <visp3/visual_features/vpFeatureException.h>
48
49// Debug trace
50#include <visp3/core/vpDebug.h>
51
56/*
57
58attributes and members directly related to the vpBasicFeature needs
59other functionalities are useful but not mandatory
60
61*/
62
69{
70 // feature dimension
71 dim_s = 3;
72 nbParameters = 1;
73
74 // memory allocation
75 s.resize(dim_s);
76 if (flags == NULL)
77 flags = new bool[nbParameters];
78 for (unsigned int i = 0; i < nbParameters; i++)
79 flags[i] = false;
80}
81
89
98{
99 init();
100}
101
115 : f2Mf1(), translation(r)
116{
117 init();
118
119 buildFrom(f2Mf1_);
120}
121
132{
133 this->f2Mf1 = f2Mf1_;
134 s[0] = f2Mf1[0][3];
135 s[1] = f2Mf1[1][3];
136 s[2] = f2Mf1[2][3];
137
138 flags[0] = true;
139}
140
151{
152 translation = r;
153}
154
164void vpFeatureTranslation::set_Tx(double t_x) { s[0] = t_x; }
174void vpFeatureTranslation::set_Ty(double t_y) { s[1] = t_y; }
184void vpFeatureTranslation::set_Tz(double t_z) { s[2] = t_z; }
185
196{
197 return translation;
198}
199
205double vpFeatureTranslation::get_Tx() const { return s[0]; }
206
212double vpFeatureTranslation::get_Ty() const { return s[1]; }
213
219double vpFeatureTranslation::get_Tz() const { return s[2]; }
220
303{
304
305 vpMatrix L;
306 L.resize(0, 6);
307
309 for (unsigned int i = 0; i < nbParameters; i++) {
310 if (flags[i] == false) {
311 switch (i) {
312 case 0:
313 vpTRACE("Warning !!! The interaction matrix is computed but f2Mf1 "
314 "was not set yet");
315 break;
316 default:
317 vpTRACE("Problem during the reading of the variable flags");
318 }
319 }
320 }
321 resetFlags();
322 }
323
324 if (translation == cdMc) {
325 // This version is a simplification
326 if (vpFeatureTranslation::selectTx() & select) {
327 vpMatrix Lx(1, 6);
328
329 for (int i = 0; i < 3; i++)
330 Lx[0][i] = f2Mf1[0][i];
331 Lx[0][3] = 0;
332 Lx[0][4] = 0;
333 Lx[0][5] = 0;
334
335 L = vpMatrix::stack(L, Lx);
336 }
337
338 if (vpFeatureTranslation::selectTy() & select) {
339 vpMatrix Ly(1, 6);
340
341 for (int i = 0; i < 3; i++)
342 Ly[0][i] = f2Mf1[1][i];
343 Ly[0][3] = 0;
344 Ly[0][4] = 0;
345 Ly[0][5] = 0;
346
347 L = vpMatrix::stack(L, Ly);
348 }
349
350 if (vpFeatureTranslation::selectTz() & select) {
351 vpMatrix Lz(1, 6);
352
353 for (int i = 0; i < 3; i++)
354 Lz[0][i] = f2Mf1[2][i];
355 Lz[0][3] = 0;
356 Lz[0][4] = 0;
357 Lz[0][5] = 0;
358
359 L = vpMatrix::stack(L, Lz);
360 }
361 }
362 if (translation == cMcd) {
363 // This version is a simplification
364 if (vpFeatureTranslation::selectTx() & select) {
365 vpMatrix Lx(1, 6);
366 Lx[0][0] = -1;
367 Lx[0][1] = 0;
368 Lx[0][2] = 0;
369 Lx[0][3] = 0;
370 Lx[0][4] = -s[2];
371 Lx[0][5] = s[1];
372
373 L = vpMatrix::stack(L, Lx);
374 }
375
376 if (vpFeatureTranslation::selectTy() & select) {
377 vpMatrix Ly(1, 6);
378 Ly[0][0] = 0;
379 Ly[0][1] = -1;
380 Ly[0][2] = 0;
381 Ly[0][3] = s[2];
382 Ly[0][4] = 0;
383 Ly[0][5] = -s[0];
384
385 L = vpMatrix::stack(L, Ly);
386 }
387
388 if (vpFeatureTranslation::selectTz() & select) {
389 vpMatrix Lz(1, 6);
390 Lz[0][0] = 0;
391 Lz[0][1] = 0;
392 Lz[0][2] = -1;
393 Lz[0][3] = -s[1];
394 Lz[0][4] = s[0];
395 Lz[0][5] = 0;
396
397 L = vpMatrix::stack(L, Lz);
398 }
399 }
400
401 if (translation == cMo) {
402 // This version is a simplification
403 if (vpFeatureTranslation::selectTx() & select) {
404 vpMatrix Lx(1, 6);
405 Lx[0][0] = -1;
406 Lx[0][1] = 0;
407 Lx[0][2] = 0;
408 Lx[0][3] = 0;
409 Lx[0][4] = -s[2];
410 Lx[0][5] = s[1];
411
412 L = vpMatrix::stack(L, Lx);
413 }
414
415 if (vpFeatureTranslation::selectTy() & select) {
416 vpMatrix Ly(1, 6);
417 Ly[0][0] = 0;
418 Ly[0][1] = -1;
419 Ly[0][2] = 0;
420 Ly[0][3] = s[2];
421 Ly[0][4] = 0;
422 Ly[0][5] = -s[0];
423
424 L = vpMatrix::stack(L, Ly);
425 }
426
427 if (vpFeatureTranslation::selectTz() & select) {
428 vpMatrix Lz(1, 6);
429 Lz[0][0] = 0;
430 Lz[0][1] = 0;
431 Lz[0][2] = -1;
432 Lz[0][3] = -s[1];
433 Lz[0][4] = s[0];
434 Lz[0][5] = 0;
435
436 L = vpMatrix::stack(L, Lz);
437 }
438 }
439
440 return L;
441}
442
509vpColVector vpFeatureTranslation::error(const vpBasicFeature &s_star, unsigned int select)
510{
511 vpColVector e(0);
512
513 if (translation == cdMc || translation == cMcd) {
514 if (s_star.get_s().sumSquare() > 1e-6) {
515 vpERROR_TRACE("s* should be zero ! ");
517 }
518 }
519
520 if (vpFeatureTranslation::selectTx() & select) {
521 vpColVector ex(1);
522 ex[0] = s[0] - s_star[0];
523 e = vpColVector::stack(e, ex);
524 }
525
526 if (vpFeatureTranslation::selectTy() & select) {
527 vpColVector ey(1);
528 ey[0] = s[1] - s_star[1];
529 e = vpColVector::stack(e, ey);
530 }
531
532 if (vpFeatureTranslation::selectTz() & select) {
533 vpColVector ez(1);
534 ez[0] = s[2] - s_star[2];
535 e = vpColVector::stack(e, ez);
536 }
537
538 return e;
539}
540
564void vpFeatureTranslation::print(unsigned int select) const
565{
566 std::cout << "Translation 3D:";
567 if (vpFeatureTranslation::selectTx() & select) {
568 std::cout << " tx=" << s[0];
569 }
570 if (vpFeatureTranslation::selectTy() & select) {
571 std::cout << " ty=" << s[1];
572 }
573 if (vpFeatureTranslation::selectTz() & select) {
574 std::cout << " tz=" << s[2];
575 }
576 std::cout << std::endl;
577}
578
593{
594 vpFeatureTranslation *feature = NULL;
595 if (translation == cdMc)
596 feature = new vpFeatureTranslation(cdMc);
597 if (translation == cMo)
598 feature = new vpFeatureTranslation(cMo);
599 if (translation == cMcd)
600 feature = new vpFeatureTranslation(cMcd);
601 return feature;
602}
603
610 const vpColor & /* color */, unsigned int /* thickness */) const
611{
612 static int firsttime = 0;
613
614 if (firsttime == 0) {
615 firsttime = 1;
616 vpERROR_TRACE("not implemented");
617 // Do not throw and error since it is not subject
618 // to produce a failure
619 }
620}
627 const vpColor & /* color */, unsigned int /* thickness */) const
628{
629 static int firsttime = 0;
630
631 if (firsttime == 0) {
632 firsttime = 1;
633 vpERROR_TRACE("not implemented");
634 // Do not throw and error since it is not subject
635 // to produce a failure
636 }
637}
638
683unsigned int vpFeatureTranslation::selectTx() { return FEATURE_LINE[0]; }
684
728unsigned int vpFeatureTranslation::selectTy() { return FEATURE_LINE[1]; }
729
773unsigned 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.
Definition: vpColVector.h:131
double sumSquare() const
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 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.
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
void stack(const vpMatrix &A)
Definition: vpMatrix.cpp:5879
#define vpTRACE
Definition: vpDebug.h:416
#define vpERROR_TRACE
Definition: vpDebug.h:393