Visual Servoing Platform version 3.5.0
testPoseFeatures.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 * 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
58#ifndef DOXYGEN_SHOULD_SKIP_THIS
59
60#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
61class vp_createPointClass
62{
63public:
64 int value;
65
66 vp_createPointClass() : value(0) {}
67
68 int vp_createPoint(vpFeaturePoint &fp, const vpPoint &v)
69 {
70 value += 1;
72 return value;
73 }
74};
75
76void vp_createPoint(vpFeaturePoint &fp, const vpPoint &v) { vpFeatureBuilder::create(fp, v); }
77
78void vp_createLine(vpFeatureLine &fp, const vpLine &v) { vpFeatureBuilder::create(fp, v); }
79#endif
80#endif
81
82int 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)
163 vpFeatureLine fl;
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
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
212int 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}
Class that defines a 3D circle in the object frame and allows forward projection of a 3D circle in th...
Definition: vpCircle.h:92
void setWorldCoordinates(const vpColVector &oP)
Definition: vpCircle.cpp:60
error that can be emited by ViSP classes.
Definition: vpException.h:72
const std::string & getStringMessage() const
Send a reference (constant) related the error message (can be empty).
Definition: vpException.cpp:92
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
Class that defines a 2D line visual feature which is composed by two parameters that are and ,...
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
Class that defines a 2D segment visual features. This class allow to consider two sets of visual feat...
Implementation of an homogeneous matrix and operations on such kind of matrices.
Class that defines a 3D line in the object frame and allows forward projection of the line in the cam...
Definition: vpLine.h:105
void setWorldCoordinates(const double &oA1, const double &oB1, const double &oC1, const double &oD1, const double &oA2, const double &oB2, const double &oC2, const double &oD2)
Definition: vpLine.cpp:85
static double rad(double deg)
Definition: vpMath.h:110
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:82
Tools for pose computation from any feature.
void addFeatureVanishingPoint(const vpPoint &)
void setVerbose(const bool &mode)
void setVVSIterMax(const unsigned int &val)
void addFeaturePoint3D(const vpPoint &)
void addSpecificFeature(RetType(*fct_ptr)(ArgsFunc...), Args &&... args)
void addFeaturePoint(const vpPoint &)
void setCovarianceComputation(const bool &flag)
void addFeatureEllipse(const vpCircle &)
void setLambda(const double &val)
void computePose(vpHomogeneousMatrix &cMo, const vpPoseFeaturesMethodType &type=VIRTUAL_VS)
vpMatrix getCovarianceMatrix() const
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:152
vpRowVector t() const
vpPoseVector buildFrom(double tx, double ty, double tz, double tux, double tuy, double tuz)