Visual Servoing Platform  version 3.4.0
testDisplacement.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  * Tests transformation within various representations of rotation.
33  *
34  * Authors:
35  * Eric Marchand
36  *
37  *****************************************************************************/
38 
48 #include <stdio.h>
49 #include <stdlib.h>
50 #include <visp3/core/vpDebug.h>
51 #include <visp3/core/vpMath.h>
52 #include <visp3/core/vpRotationMatrix.h>
53 #include <visp3/core/vpThetaUVector.h>
54 #include <visp3/vision/vpHomography.h>
55 
56 bool test(const std::string &s, const vpHomography &H, const std::vector<double> &bench)
57 {
58  static unsigned int cpt = 0;
59  std::cout << "** Test " << ++cpt << std::endl;
60  std::cout << s << "(" << H.getRows() << "," << H.getCols() << ") = \n[" << H << "]" << std::endl;
61  if (bench.size() != H.size()) {
62  std::cout << "Test fails: bad size wrt bench" << std::endl;
63  return false;
64  }
65  for (unsigned int i = 0; i < H.size(); i++) {
66  if (std::fabs(H.data[i] - bench[i]) > std::fabs(H.data[i]) * std::numeric_limits<double>::epsilon()) {
67  std::cout << "Test fails: bad content" << std::endl;
68  return false;
69  }
70  }
71 
72  return true;
73 }
74 
75 int main()
76 {
77 #if (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
78  try {
79  {
80  vpHomography H;
81  H.eye();
82  std::vector<double> bench(9, 0);
83  bench[0] = bench[4] = bench[8] = 1.;
84  int err = 1;
85  if (test("H", H, bench) == false)
86  return err;
87  if (test("H", H / H[2][2], bench) == false)
88  return err;
89  }
90  {
92 
93  std::cout << "Initialization " << std::endl;
94  // std::cout << tu << std::endl ;
95 
96  std::cout << "From vpThetaUVector to vpRotationMatrix " << std::endl;
97  vpRotationMatrix R(tu);
98 
99  // pure rotation
101  M.insert(R);
102 
103  std::cout << "M" << std::endl << M << std::endl;
104  vpPlane p(0, 0, 1, 1);
105 
106  vpHomography H(M, p);
107 
108  std::cout << "H" << std::endl << H << std::endl;
109 
110  vpColVector n;
112 
113  H.computeDisplacement(R, T, n);
114 
115  std::cout << "R" << std::endl << R;
116  std::cout << "T" << std::endl << T.t() << std::endl;
117  std::cout << "n" << std::endl << n.t() << std::endl;
118  }
119  std::cout << "------------------------------------------------------" << std::endl;
120 
121  {
123 
124  std::cout << "Initialization " << std::endl;
125  // std::cout << tu << std::endl ;
126 
127  std::cout << "From vpThetaUVector to vpRotationMatrix " << std::endl;
128  vpRotationMatrix R(tu);
129 
130  // pure rotation
132  M.insert(R);
133 
134  M[0][3] = 0.21;
135  M[1][3] = 0.31;
136  M[2][3] = 0.5;
137 
138  std::cout << "M" << std::endl << M << std::endl;
139  vpPlane p(0, 0, 1, 1);
140 
141  vpHomography H(M, p);
142 
143  std::cout << "H" << std::endl << H << std::endl;
144 
145  vpColVector n;
147 
148  H.computeDisplacement(R, T, n);
149 
150  std::cout << "R" << std::endl << R;
151  std::cout << "T" << std::endl << T.t() << std::endl;
152  std::cout << "n" << std::endl << n.t() << std::endl;
153  }
154 
155  std::cout << "------------------------------------------------------" << std::endl;
156  {
157  vpThetaUVector tu(vpMath::rad(-190), vpMath::rad(12), vpMath::rad(-45));
158 
159  vpRotationMatrix R(tu);
160 
161  // pure rotation
163  M.insert(R);
164 
165  M[0][3] = 0.21;
166  M[1][3] = -0.31;
167  M[2][3] = 0.5;
168 
169  std::cout << "M" << std::endl << M << std::endl;
170  vpPlane p(0.4, -0.5, 0.5, 1);
171 
172  vpHomography H(M, p);
173 
174  std::cout << "H" << std::endl << H << std::endl;
175 
176  vpColVector n;
178  H.computeDisplacement(R, T, n);
179 
180  std::cout << "R" << std::endl << R;
181  std::cout << "T" << std::endl << T.t() << std::endl;
182  std::cout << "n" << std::endl << n.t() << std::endl;
183 
184  vpPlane p1(n[0], n[1], n[2], 1.0);
185  H.buildFrom(R, T, p1);
186  std::cout << "H" << std::endl << H << std::endl;
187  }
188  std::cout << "All tests succeed" << std::endl;
189  return EXIT_SUCCESS;
190  } catch (const vpException &e) {
191  std::cout << "Catch an exception: " << e << std::endl;
192  return EXIT_FAILURE;
193  }
194 #else
195  std::cout << "Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
196  return EXIT_SUCCESS;
197 #endif
198 }
void buildFrom(const vpRotationMatrix &aRb, const vpTranslationVector &atb, const vpPlane &bP)
Construction from Translation and rotation and a plane.
Implementation of an homogeneous matrix and operations on such kind of matrices.
error that can be emited by ViSP classes.
Definition: vpException.h:71
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:145
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:291
unsigned int getCols() const
Definition: vpArray2D.h:279
void computeDisplacement(vpRotationMatrix &aRb, vpTranslationVector &atb, vpColVector &n)
Implementation of a rotation matrix and operations on such kind of matrices.
void insert(const vpRotationMatrix &R)
Implementation of an homography and operations on homographies.
Definition: vpHomography.h:174
vpRowVector t() const
vpRowVector t() const
unsigned int getRows() const
Definition: vpArray2D.h:289
static double rad(double deg)
Definition: vpMath.h:110
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
This class defines the container for a plane geometrical structure.
Definition: vpPlane.h:58
Class that consider the case of a translation vector.
Implementation of a rotation vector as axis-angle minimal representation.