ViSP
 All Classes Functions Variables Enumerations Enumerator Friends Groups Pages
testPoseFeatures.cpp
1 /****************************************************************************
2  *
3  * $Id: testPoseFeatures.cpp 4056 2013-01-05 13:04:42Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2013 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 from visual features by virtual visual servoing.
36  *
37  * Authors:
38  * Aurelien Yol
39  *
40  *****************************************************************************/
41 
42 #include <visp/vpConfig.h>
43 #include <visp/vpHomogeneousMatrix.h>
44 #include <visp/vpPoint.h>
45 #include <visp/vpDisplay.h>
46 #include <visp/vpDisplayX.h>
47 #include <visp/vpImage.h>
48 #include <visp/vpCameraParameters.h>
49 #include <visp/vpPoseFeatures.h>
50 #include <visp/vpPose.h>
51 #include <iostream>
52 #include <vector>
53 #include <visp/vpPose.h>
54 #include <limits>
55 
63 #ifndef DOXYGEN_SHOULD_SKIP_THIS
64 #ifdef VISP_HAVE_CPP11_COMPATIBILITY
65 class vp_createPointClass{
66 public:
67  int value;
68 
69  vp_createPointClass() : value(0){}
70 
71  int vp_createPoint(vpFeaturePoint &fp,const vpPoint &v){
72  value += 1;
74  return value;
75  }
76 };
77 
78 void vp_createPoint(vpFeaturePoint &fp,const vpPoint &v){
80 }
81 
82 void vp_createLine(vpFeatureLine &fp,const vpLine &v){
84 }
85 #endif
86 #endif
87 
88 int main()
89 {
90  vpImage<unsigned char> I(600,600);
91 
92  vpHomogeneousMatrix cMo_ref(0., 0., 1., vpMath::rad(0), vpMath::rad(0), vpMath::rad(60));
93  vpPoseVector pose_ref = vpPoseVector(cMo_ref);
94 
95  std::cout << "Reference pose used to create the visual features : " << std::endl;
96  std::cout << pose_ref.t() << std::endl;
97 
98  vpPoseFeatures pose;
99 
100  vpPoint pts[6];
101 
102  double val = 0.25;
103  double val2 = 0.0;
104 
105  //2D Point Feature
106  pts[0].setWorldCoordinates(0.0,-val,val2);
107  pts[1].setWorldCoordinates(0.0,val,val2);
108  pts[2].setWorldCoordinates(-val,val,val2);
109 
110  //Segment Feature
111  pts[3].setWorldCoordinates(-val,-val/2.0,val2);
112  pts[4].setWorldCoordinates(val,val/2.0,val2);
113 
114  //3D point Feature
115  pts[5].setWorldCoordinates(0.0,0.0,-1.5);
116 
117  //Line Feature
118  vpLine line;
119  line.setWorldCoordinates(0.0,1.0,0.0,.0,
120  0.0,0.0,1.0,0.0);
121 
122  //Vanishing Point Feature
123  vpLine l1;
124  l1.setWorldCoordinates(0.0,1.0,0.2,0.0,
125  1.0,0.0,0.0,-0.25);
126 
127  vpLine l2;
128  l2.setWorldCoordinates(0.0,1.0,0.2,0.0,
129  -1.0,0.0,0.0,-0.25);
130 
131  //Ellipse Feature
132  vpCircle circle;
133  circle.setWorldCoordinates(0.0, 0.0, 1.0 , 0.0, 0.0, 0.0, 0.25);
134 
135  pts[0].project(cMo_ref);
136  pts[1].project(cMo_ref);
137  pts[2].project(cMo_ref);
138 
139  pts[3].project(cMo_ref);
140  pts[4].project(cMo_ref);
141 
142  pts[5].project(cMo_ref);
143 
144  line.project(cMo_ref);
145 
146  l1.project(cMo_ref);
147  l2.project(cMo_ref);
148 
149  circle.project(cMo_ref);
150 
151  pose.addFeaturePoint(pts[0]);
152 // pose.addFeaturePoint(pts[1]);
153  pose.addFeaturePoint(pts[2]);
154 
155  pose.addFeaturePoint3D(pts[5]);
156 
157  pose.addFeatureVanishingPoint(l1,l2);
158 
159 // pose.addFeatureSegment(pts[3],pts[4]);
160 //
161 // pose.addFeatureLine(line);
162 
163  pose.addFeatureEllipse(circle);
164 
165 #ifdef VISP_HAVE_CPP11_COMPATIBILITY
166  vpFeaturePoint fp;
167  vpFeatureLine fl;
168  vpFeatureSegment fs;
169  void (*ptr)(vpFeatureSegment&, vpPoint&, vpPoint&) = &vpFeatureBuilder::create;
170  vp_createPointClass cpClass;
171  int (vp_createPointClass::*ptrClass)(vpFeaturePoint&, const vpPoint&) = &vp_createPointClass::vp_createPoint;
172  pose.addSpecificFeature(&cpClass, ptrClass, fp, pts[1]);
173  pose.addSpecificFeature(&vp_createLine, fl, line);
174  pose.addSpecificFeature(ptr, fs, pts[3], pts[4]);
175 #endif
176 
177  pose.setVerbose(true);
178  pose.setLambda(0.6);
179  pose.setVVSIterMax(200);
180  pose.setCovarianceComputation(true);
181 
182  vpHomogeneousMatrix cMo_est(0.4, 0.3, 1.5, vpMath::rad(0), vpMath::rad(0), vpMath::rad(0));
183  vpPoseVector pose_est = vpPoseVector(cMo_est);
184  std::cout << "\nPose used as initialisation of the pose computation : " << std::endl;
185  std::cout << pose_est.t() << std::endl;
186 
187  pose.computePose(cMo_est);
188 // pose.computePose(cMo_est, vpPoseFeatures::ROBUST_VIRTUAL_VS);
189 
190 
191  std::cout << "\nEstimated pose from visual features : " << std::endl;
192  pose_est.buildFrom(cMo_est);
193  std::cout << pose_est.t() << std::endl;
194 
195  std::cout << "\nResulting covariance (Diag): " << std::endl;
196  vpMatrix covariance = pose.getCovarianceMatrix();
197  std::cout << covariance[0][0] << " "
198  << covariance[1][1] << " "
199  << covariance[2][2] << " "
200  << covariance[3][3] << " "
201  << covariance[4][4] << " "
202  << covariance[5][5] << " " << std::endl;
203 
204  int test_fail = 0;
205  for(unsigned int i=0; i<6; i++) {
206  if (std::fabs(pose_ref[i]-pose_est[i]) > 0.001)
207  test_fail = 1;
208  }
209 
210  std::cout << "\nPose is " << (test_fail ? "badly" : "well") << " estimated" << std::endl;
211 
212  return test_fail;
213 }