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 * Compute the pose from visual features by virtual visual servoing.
33 *
34 * Authors:
35 * Aurelien Yol
36 *
37 *****************************************************************************/
38
39 #include <iostream>
40 #include <limits>
41 #include <vector>
42
43 #include <visp3/core/vpCameraParameters.h>
44 #include <visp3/core/vpConfig.h>
45 #include <visp3/core/vpHomogeneousMatrix.h>
46 #include <visp3/core/vpImage.h>
47 #include <visp3/core/vpPoint.h>
48 #include <visp3/vision/vpPose.h>
49 #include <visp3/vision/vpPoseFeatures.h>
50
51 /*!
52 \example testPoseFeatures.cpp
53
54 Compute the pose from different visual features.
55
56 */
57
58 #ifndef DOXYGEN_SHOULD_SKIP_THIS
59
60 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
61 class vp_createPointClass
62 {
63 public:
64 int value;
65
vp_createPointClass()66 vp_createPointClass() : value(0) {}
67
vp_createPoint(vpFeaturePoint & fp,const vpPoint & v)68 int vp_createPoint(vpFeaturePoint &fp, const vpPoint &v)
69 {
70 value += 1;
71 vpFeatureBuilder::create(fp, v);
72 return value;
73 }
74 };
75
vp_createPoint(vpFeaturePoint & fp,const vpPoint & v)76 void vp_createPoint(vpFeaturePoint &fp, const vpPoint &v) { vpFeatureBuilder::create(fp, v); }
77
vp_createLine(vpFeatureLine & fp,const vpLine & v)78 void vp_createLine(vpFeatureLine &fp, const vpLine &v) { vpFeatureBuilder::create(fp, v); }
79 #endif
80 #endif
81
test_pose(bool use_robust)82 int test_pose(bool use_robust)
83 {
84 if (use_robust)
85 std::cout << "** Test robust pose estimation from features\n" << std::endl;
86 else
87 std::cout << "** Test pose estimation from features\n" << std::endl;
88
89 vpImage<unsigned char> I(600, 600);
90
91 vpHomogeneousMatrix cMo_ref(0., 0., 1., vpMath::rad(0), vpMath::rad(0), vpMath::rad(60));
92 vpPoseVector pose_ref = vpPoseVector(cMo_ref);
93
94 std::cout << "Reference pose used to create the visual features : " << std::endl;
95 std::cout << pose_ref.t() << std::endl;
96
97 vpPoseFeatures pose;
98
99 std::vector<vpPoint> pts;
100
101 double val = 0.25;
102 double val2 = 0.0;
103
104 // 2D Point Feature
105 pts.push_back(vpPoint(0.0, -val, val2));
106 pts.push_back(vpPoint(0.0, val, val2));
107 pts.push_back(vpPoint(-val, val, val2));
108
109 // Segment Feature
110 pts.push_back(vpPoint(-val, -val / 2.0, val2));
111 pts.push_back(vpPoint(val, val / 2.0, val2));
112
113 // 3D point Feature
114 pts.push_back(vpPoint(0.0, 0.0, -1.5));
115
116 // Line Feature
117 vpLine line;
118 line.setWorldCoordinates(0.0, 1.0, 0.0, .0, 0.0, 0.0, 1.0, 0.0);
119
120 // Vanishing Point Feature
121 vpLine l1;
122 l1.setWorldCoordinates(0.0, 1.0, 0.2, 0.0, 1.0, 0.0, 0.0, -0.25);
123
124 vpLine l2;
125 l2.setWorldCoordinates(0.0, 1.0, 0.2, 0.0, -1.0, 0.0, 0.0, -0.25);
126
127 // Ellipse Feature
128 vpCircle circle;
129 circle.setWorldCoordinates(0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.25);
130
131 pts[0].project(cMo_ref);
132 pts[1].project(cMo_ref);
133 pts[2].project(cMo_ref);
134
135 pts[3].project(cMo_ref);
136 pts[4].project(cMo_ref);
137
138 pts[5].project(cMo_ref);
139
140 line.project(cMo_ref);
141
142 l1.project(cMo_ref);
143 l2.project(cMo_ref);
144
145 circle.project(cMo_ref);
146
147 pose.addFeaturePoint(pts[0]);
148 // pose.addFeaturePoint(pts[1]);
149 pose.addFeaturePoint(pts[2]);
150
151 pose.addFeaturePoint3D(pts[5]);
152
153 pose.addFeatureVanishingPoint(l1, l2);
154
155 // pose.addFeatureSegment(pts[3],pts[4]);
156 //
157 // pose.addFeatureLine(line);
158
159 pose.addFeatureEllipse(circle);
160
161 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
162 vpFeaturePoint fp;
163 vpFeatureLine fl;
164 vpFeatureSegment fs;
165 void (*ptr)(vpFeatureSegment &, vpPoint &, vpPoint &) = &vpFeatureBuilder::create;
166 vp_createPointClass cpClass;
167 int (vp_createPointClass::*ptrClass)(vpFeaturePoint &, const vpPoint &) = &vp_createPointClass::vp_createPoint;
168 pose.addSpecificFeature(&cpClass, ptrClass, fp, pts[1]);
169 pose.addSpecificFeature(&vp_createLine, fl, line);
170 pose.addSpecificFeature(ptr, fs, pts[3], pts[4]);
171 #endif
172
173 pose.setVerbose(true);
174 pose.setLambda(0.6);
175 pose.setVVSIterMax(200);
176 pose.setCovarianceComputation(true);
177
178 vpHomogeneousMatrix cMo_est(0.4, 0.3, 1.5, vpMath::rad(0), vpMath::rad(0), vpMath::rad(0));
179 vpPoseVector pose_est = vpPoseVector(cMo_est);
180 std::cout << "\nPose used as initialisation of the pose computation : " << std::endl;
181 std::cout << pose_est.t() << std::endl;
182
183 if (!use_robust)
184 pose.computePose(cMo_est);
185 else
186 pose.computePose(cMo_est, vpPoseFeatures::ROBUST_VIRTUAL_VS);
187
188 if (!use_robust)
189 std::cout << "\nEstimated pose from visual features : " << std::endl;
190 else
191 std::cout << "\nRobust estimated pose from visual features : " << std::endl;
192
193 pose_est.buildFrom(cMo_est);
194 std::cout << pose_est.t() << std::endl;
195
196 std::cout << "\nResulting covariance (Diag): " << std::endl;
197 vpMatrix covariance = pose.getCovarianceMatrix();
198 std::cout << covariance[0][0] << " " << covariance[1][1] << " " << covariance[2][2] << " " << covariance[3][3] << " "
199 << covariance[4][4] << " " << covariance[5][5] << " " << std::endl;
200
201 int test_fail = 0;
202 for (unsigned int i = 0; i < 6; i++) {
203 if (std::fabs(pose_ref[i] - pose_est[i]) > 0.001)
204 test_fail = 1;
205 }
206
207 std::cout << "\nPose is " << (test_fail ? "badly" : "well") << " estimated\n" << std::endl;
208
209 return test_fail;
210 }
211
main()212 int main()
213 {
214 #if (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
215 try {
216 if (test_pose(false))
217 return -1;
218
219 if (test_pose(true))
220 return -1;
221
222 return 0;
223 } catch (const vpException &e) {
224 std::cout << "Catch an exception: " << e.getStringMessage() << std::endl;
225 return -1;
226 }
227 #else
228 std::cout << "Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
229 return EXIT_SUCCESS;
230 #endif
231 }
232