ViSP  2.6.2
testPose.cpp
1 /****************************************************************************
2  *
3  * $Id: testPose.cpp 3619 2012-03-09 17:28:57Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2012 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  *
34  * Description:
35  * Compute the pose of a 3D object using the Dementhon, Lagrange and
36  * Non-Linear approach.
37  *
38  * Authors:
39  * Eric Marchand
40  * Fabien Spindler
41  *
42  *****************************************************************************/
43 
44 #include <visp/vpPose.h>
45 #include <visp/vpPoint.h>
46 #include <visp/vpMath.h>
47 #include <visp/vpTranslationVector.h>
48 #include <visp/vpRxyzVector.h>
49 #include <visp/vpRotationMatrix.h>
50 #include <visp/vpHomogeneousMatrix.h>
51 #include <visp/vpDebug.h>
52 #include <visp/vpParseArgv.h>
53 
54 #include <stdlib.h>
55 #include <stdio.h>
56 
57 // List of allowed command line options
58 #define GETOPTARGS "h"
59 
60 #define L 0.035
61 
74 void usage(const char *name, const char *badparam)
75 {
76  fprintf(stdout, "\n\
77 Compute the pose of a 3D object using the Dementhon, Lagrange and\n\
78 Non-Linear approach.\n\
79 \n\
80 SYNOPSIS\n\
81  %s [-h]\n", name);
82 
83  fprintf(stdout, "\n\
84 OPTIONS: Default\n\
85  -h\n\
86  Print the help.\n");
87 
88  if (badparam)
89  fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
90 }
98 bool getOptions(int argc, const char **argv)
99 {
100  const char *optarg;
101  int c;
102  while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg)) > 1) {
103 
104  switch (c) {
105  case 'h': usage(argv[0], NULL); return false; break;
106 
107  default:
108  usage(argv[0], optarg);
109  return false; break;
110  }
111  }
112 
113  if ((c == 1) || (c == -1)) {
114  // standalone param or error
115  usage(argv[0], NULL);
116  std::cerr << "ERROR: " << std::endl;
117  std::cerr << " Bad argument " << optarg << std::endl << std::endl;
118  return false;
119  }
120 
121  return true;
122 }
123 
124 
125 int
126 main(int argc, const char ** argv)
127 {
128  // Read the command line options
129  if (getOptions(argc, argv) == false) {
130  exit (-1);
131  }
132 
133 
134  vpPoint P[4] ; // Point to be tracked
135  vpPose pose ;
136  pose.clearPoint() ;
137 
138 
139  P[0].setWorldCoordinates(-L,-L, 0 ) ;
140  P[1].setWorldCoordinates(L,-L, 0 ) ;
141  P[2].setWorldCoordinates(L,L, 0 ) ;
142  P[3].setWorldCoordinates(-L,L, 0 ) ;
143 
144  vpRotationMatrix cRo;
146  vpRxyzVector cro;
147  vpHomogeneousMatrix cMo_ref(0.1,0.2,1,vpMath::rad(10),0,vpMath::rad(10)) ;
148  int i ;
149  for(i=0 ; i < 4 ; i++)
150  P[i].project(cMo_ref) ;
151 
152 
153  for (i=0 ; i < 4 ; i++)
154  {
155  pose.addPoint(P[i]) ; // and added to the pose computation class
156  }
157 
158  // Let's go ...
159  vpHomogeneousMatrix cMo ;
160 
161  std::cout <<"-------------------------------------------------"<<std::endl ;
162  pose.computePose(vpPose::LAGRANGE, cMo) ;
163  vpTRACE("LAGRANGE pose : ") ;
164  std::cout << cMo << std::endl ;
165  std::cout << "Lagrange residual term: " << pose.computeResidual(cMo) <<std::endl ;
166  cMo.extract(cto);
167  cMo.extract(cRo);
168  cro.buildFrom(cRo);
169  double residual = pose.computeResidual(cMo);
170 
171  std::cout << "\nPose Lagrange "
172  << "(residual: " << residual << ")\n "
173  << "cdto[0] = " << cto[0] << ";\n "
174  << "cdto[1] = " << cto[1] << ";\n "
175  << "cdto[2] = " << cto[2] << ";\n "
176  << "cdro[0] = vpMath::rad(" << vpMath::deg(cro[0]) << ");\n "
177  << "cdro[1] = vpMath::rad(" << vpMath::deg(cro[1]) << ");\n "
178  << "cdro[2] = vpMath::rad(" << vpMath::deg(cro[2]) << ");\n"
179  << std::endl;
180 
181 
182  std::cout <<"--------------------------------------------------"<<std::endl ;
183  pose.computePose(vpPose::LOWE, cMo) ;
184  vpTRACE( "LOWE pose :" ) ;
185  std::cout << cMo << std::endl ;
186  std::cout << "Lowe residual term: " <<pose.computeResidual(cMo) <<std::endl ;
187 
188  cMo.extract(cto);
189  cMo.extract(cRo);
190  cro.buildFrom(cRo);
191  residual = pose.computeResidual(cMo);
192 
193  std::cout << "\nPose Lowe "
194  << "(residual: " << residual << ")\n "
195  << "cdto[0] = " << cto[0] << ";\n "
196  << "cdto[1] = " << cto[1] << ";\n "
197  << "cdto[2] = " << cto[2] << ";\n "
198  << "cdro[0] = vpMath::rad(" << vpMath::deg(cro[0]) << ");\n "
199  << "cdro[1] = vpMath::rad(" << vpMath::deg(cro[1]) << ");\n "
200  << "cdro[2] = vpMath::rad(" << vpMath::deg(cro[2]) << ");\n"
201  << std::endl;
202 
203  std::cout <<"--------------------------------------------------"<<std::endl ;
204  pose.computePose(vpPose::DEMENTHON, cMo) ;
205  vpTRACE( "DEMENTHON pose :" ) ;
206  std::cout << cMo << std::endl ;
207  std::cout << "Dementhon residual term: " << pose.computeResidual(cMo)
208  << std::endl ;
209 
210  cMo.extract(cto);
211  cMo.extract(cRo);
212  cro.buildFrom(cRo);
213  residual = pose.computeResidual(cMo);
214 
215  std::cout << "\nPose Dementhon "
216  << "(residual: " << residual << ")\n "
217  << "cdto[0] = " << cto[0] << ";\n "
218  << "cdto[1] = " << cto[1] << ";\n "
219  << "cdto[2] = " << cto[2] << ";\n "
220  << "cdro[0] = vpMath::rad(" << vpMath::deg(cro[0]) << ");\n "
221  << "cdro[1] = vpMath::rad(" << vpMath::deg(cro[1]) << ");\n "
222  << "cdro[2] = vpMath::rad(" << vpMath::deg(cro[2]) << ");\n"
223  << std::endl;
224 
225  std::cout <<"--------------------------------------------------"<<std::endl ;
226  pose.computePose(vpPose::LOWE, cMo) ;
227  std::cout << "Lowe residual term: " <<pose.computeResidual(cMo) <<std::endl ;
228  // vpTRACE( "Pose LOWE" ) ;
229  // std::cout << cMo << std::endl ;
230  // std::cout << "residu Lowe " << pose.computeResidual(cMo) <<std::endl ;
231 
232  cMo.extract(cto);
233  cMo.extract(cRo);
234  cro.buildFrom(cRo);
235  residual = pose.computeResidual(cMo);
236 
237  std::cout << "\nPose lowe "
238  << "(residual: " << residual << ")\n "
239  << "cdto[0] = " << cto[0] << ";\n "
240  << "cdto[1] = " << cto[1] << ";\n "
241  << "cdto[2] = " << cto[2] << ";\n "
242  << "cdro[0] = vpMath::rad(" << vpMath::deg(cro[0]) << ");\n "
243  << "cdro[1] = vpMath::rad(" << vpMath::deg(cro[1]) << ");\n "
244  << "cdro[2] = vpMath::rad(" << vpMath::deg(cro[2]) << ");\n"
245  << std::endl;
246 
247  std::cout <<std::endl << std::endl ;
248  std::cout <<"--------------------------------------------------"<<std::endl ;
249  std::cout << "Virtual Visual servoing " << std::endl ;
250 
251  std::cout <<"--------------------------------------------------"<<std::endl ;
252  pose.computePose(vpPose::LAGRANGE, cMo) ;
253  vpTRACE("LAGRANGE pose : ") ;
254  std::cout << cMo << std::endl ;
255  std::cout << "Lagrange residual term: " << pose.computeResidual(cMo)
256  <<std::endl ;
257 
258  cMo.extract(cto);
259  cMo.extract(cRo);
260  cro.buildFrom(cRo);
261  residual = pose.computeResidual(cMo);
262 
263  std::cout << "\nPose Lagrange "
264  << "(residual: " << residual << ")\n "
265  << "cdto[0] = " << cto[0] << ";\n "
266  << "cdto[1] = " << cto[1] << ";\n "
267  << "cdto[2] = " << cto[2] << ";\n "
268  << "cdro[0] = vpMath::rad(" << vpMath::deg(cro[0]) << ");\n "
269  << "cdro[1] = vpMath::rad(" << vpMath::deg(cro[1]) << ");\n "
270  << "cdro[2] = vpMath::rad(" << vpMath::deg(cro[2]) << ");\n"
271  << std::endl;
272 
273 
274  std::cout <<"--------------------------------------------------"<<std::endl ;
275  pose.computePose(vpPose::VIRTUAL_VS, cMo) ;
276  vpTRACE( "VIRTUAL_VS pose :" ) ;
277  std::cout << cMo << std::endl ;
278  std::cout << "vvs residual term: " <<pose.computeResidual(cMo) <<std::endl ;
279 
280  cMo.extract(cto);
281  cMo.extract(cRo);
282  cro.buildFrom(cRo);
283  residual = pose.computeResidual(cMo);
284 
285  std::cout << "\nPose VVS "
286  << "(residual: " << residual << ")\n "
287  << "cdto[0] = " << cto[0] << ";\n "
288  << "cdto[1] = " << cto[1] << ";\n "
289  << "cdto[2] = " << cto[2] << ";\n "
290  << "cdro[0] = vpMath::rad(" << vpMath::deg(cro[0]) << ");\n "
291  << "cdro[1] = vpMath::rad(" << vpMath::deg(cro[1]) << ");\n "
292  << "cdro[2] = vpMath::rad(" << vpMath::deg(cro[2]) << ");\n"
293  << std::endl;
294 
295  std::cout <<"-------------------------------------------------"<<std::endl ;
296  pose.computePose(vpPose::DEMENTHON, cMo) ;
297  vpTRACE( "DEMENTHON pose :" ) ;
298  std::cout << cMo << std::endl ;
299  std::cout << "Dementhon residual term: " << pose.computeResidual(cMo)
300  <<std::endl ;
301 
302  cMo.extract(cto);
303  cMo.extract(cRo);
304  cro.buildFrom(cRo);
305  residual = pose.computeResidual(cMo);
306 
307  std::cout << "\nPose Dementhon "
308  << "(residual: " << residual << ")\n "
309  << "cdto[0] = " << cto[0] << ";\n "
310  << "cdto[1] = " << cto[1] << ";\n "
311  << "cdto[2] = " << cto[2] << ";\n "
312  << "cdro[0] = vpMath::rad(" << vpMath::deg(cro[0]) << ");\n "
313  << "cdro[1] = vpMath::rad(" << vpMath::deg(cro[1]) << ");\n "
314  << "cdro[2] = vpMath::rad(" << vpMath::deg(cro[2]) << ");\n"
315  << std::endl;
316 
317  std::cout <<"-------------------------------------------------"<<std::endl ;
318  pose.computePose(vpPose::VIRTUAL_VS, cMo) ;
319  std::cout << "vvs residual term: " <<pose.computeResidual(cMo) <<std::endl ;
320 
321  cMo.extract(cto);
322  cMo.extract(cRo);
323  cro.buildFrom(cRo);
324  residual = pose.computeResidual(cMo);
325 
326  std::cout << "\nPose VVS "
327  << "(residual: " << residual << ")\n "
328  << "cdto[0] = " << cto[0] << ";\n "
329  << "cdto[1] = " << cto[1] << ";\n "
330  << "cdto[2] = " << cto[2] << ";\n "
331  << "cdro[0] = vpMath::rad(" << vpMath::deg(cro[0]) << ");\n "
332  << "cdro[1] = vpMath::rad(" << vpMath::deg(cro[1]) << ");\n "
333  << "cdro[2] = vpMath::rad(" << vpMath::deg(cro[2]) << ");\n"
334  << std::endl;
335  return 0;
336 }
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
#define vpTRACE
Definition: vpDebug.h:401
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Definition: vpParseArgv.cpp:79
Class that defines what is a point.
Definition: vpPoint.h:65
The vpRotationMatrix considers the particular case of a rotation matrix.
Class used for pose computation from N points (pose from point only).
Definition: vpPose.h:80
double computeResidual(vpHomogeneousMatrix &cMo)
Compute and return the residual expressed in meter for the pose matrix 'cMo'.
Definition: vpPose.cpp:255
void extract(vpRotationMatrix &R) const
static double rad(double deg)
Definition: vpMath.h:100
static double deg(double rad)
Definition: vpMath.h:93
Class that consider the case of the Euler angle using the x-y-z convention, where are respectively ...
Definition: vpRxyzVector.h:152
void computePose(vpPoseMethodType methode, vpHomogeneousMatrix &cMo)
compute the pose for a given method
Definition: vpPose.cpp:298
void addPoint(const vpPoint &P)
Add a new point in this array.
Definition: vpPose.cpp:148
void buildFrom(const double phi, const double theta, const double psi)
Definition: vpRxyzVector.h:188
Class that consider the case of a translation vector.
void setWorldCoordinates(const double ox, const double oy, const double oz)
Set the point world coordinates. We mean here the coordinates of the point in the object frame...
Definition: vpPoint.cpp:74
void clearPoint()
suppress all the point in the array of point
Definition: vpPose.cpp:126