Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
testPoseFeatures.cpp
1/*
2 * ViSP, open source Visual Servoing Platform software.
3 * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
4 *
5 * This software is free software; you can redistribute it and/or modify
6 * it under the terms of the GNU General Public License as published by
7 * the Free Software Foundation; either version 2 of the License, or
8 * (at your option) any later version.
9 * See the file LICENSE.txt at the root directory of this source
10 * distribution for additional information about the GNU GPL.
11 *
12 * For using ViSP with software that can not be combined with the GNU
13 * GPL, please contact Inria about acquiring a ViSP Professional
14 * Edition License.
15 *
16 * See https://visp.inria.fr for more information.
17 *
18 * This software was developed at:
19 * Inria Rennes - Bretagne Atlantique
20 * Campus Universitaire de Beaulieu
21 * 35042 Rennes Cedex
22 * France
23 *
24 * If you have questions regarding the use of this file, please contact
25 * Inria at visp@inria.fr
26 *
27 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29 *
30 * Description:
31 * Compute the pose from visual features by virtual visual servoing.
32 */
33
34#include <iostream>
35#include <limits>
36#include <vector>
37
38#include <visp3/core/vpCameraParameters.h>
39#include <visp3/core/vpConfig.h>
40#include <visp3/core/vpHomogeneousMatrix.h>
41#include <visp3/core/vpImage.h>
42#include <visp3/core/vpPoint.h>
43#include <visp3/vision/vpPose.h>
44#include <visp3/vision/vpPoseFeatures.h>
45
53#ifndef DOXYGEN_SHOULD_SKIP_THIS
54
55#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
56class vp_createPointClass
57{
58public:
59 int value;
60
61 vp_createPointClass() : value(0) {}
62
63 int vp_createPoint(vpFeaturePoint &fp, const vpPoint &v)
64 {
65 value += 1;
67 return value;
68 }
69};
70
71void vp_createPoint(vpFeaturePoint &fp, const vpPoint &v) { vpFeatureBuilder::create(fp, v); }
72
73void vp_createLine(vpFeatureLine &fp, const vpLine &v) { vpFeatureBuilder::create(fp, v); }
74#endif
75#endif
76
77int test_pose(bool use_robust)
78{
79 if (use_robust)
80 std::cout << "** Test robust pose estimation from features\n" << std::endl;
81 else
82 std::cout << "** Test pose estimation from features\n" << std::endl;
83
84 vpImage<unsigned char> I(600, 600);
85
86 vpHomogeneousMatrix cMo_ref(0., 0., 1., vpMath::rad(0), vpMath::rad(0), vpMath::rad(60));
87 vpPoseVector pose_ref = vpPoseVector(cMo_ref);
88
89 std::cout << "Reference pose used to create the visual features : " << std::endl;
90 std::cout << pose_ref.t() << std::endl;
91
92 vpPoseFeatures pose;
93
94 std::vector<vpPoint> pts;
95
96 double val = 0.25;
97 double val2 = 0.0;
98
99 // 2D Point Feature
100 pts.push_back(vpPoint(0.0, -val, val2));
101 pts.push_back(vpPoint(0.0, val, val2));
102 pts.push_back(vpPoint(-val, val, val2));
103
104 // Segment Feature
105 pts.push_back(vpPoint(-val, -val / 2.0, val2));
106 pts.push_back(vpPoint(val, val / 2.0, val2));
107
108 // 3D point Feature
109 pts.push_back(vpPoint(0.0, 0.0, -1.5));
110
111 // Line Feature
112 vpLine line;
113 line.setWorldCoordinates(0.0, 1.0, 0.0, .0, 0.0, 0.0, 1.0, 0.0);
114
115 // Vanishing Point Feature
116 vpLine l1;
117 l1.setWorldCoordinates(0.0, 1.0, 0.2, 0.0, 1.0, 0.0, 0.0, -0.25);
118
119 vpLine l2;
120 l2.setWorldCoordinates(0.0, 1.0, 0.2, 0.0, -1.0, 0.0, 0.0, -0.25);
121
122 // Ellipse Feature
123 vpCircle circle;
124 circle.setWorldCoordinates(0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.25);
125
126 pts[0].project(cMo_ref);
127 pts[1].project(cMo_ref);
128 pts[2].project(cMo_ref);
129
130 pts[3].project(cMo_ref);
131 pts[4].project(cMo_ref);
132
133 pts[5].project(cMo_ref);
134
135 line.project(cMo_ref);
136
137 l1.project(cMo_ref);
138 l2.project(cMo_ref);
139
140 circle.project(cMo_ref);
141
142 pose.addFeaturePoint(pts[0]);
143 // pose.addFeaturePoint(pts[1]);
144 pose.addFeaturePoint(pts[2]);
145
146 pose.addFeaturePoint3D(pts[5]);
147
148 pose.addFeatureVanishingPoint(l1, l2);
149
150 // pose.addFeatureSegment(pts[3],pts[4]);
151 //
152 // pose.addFeatureLine(line);
153
154 pose.addFeatureEllipse(circle);
155
156#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
158 vpFeatureLine fl;
161 vp_createPointClass cpClass;
162 int (vp_createPointClass::*ptrClass)(vpFeaturePoint &, const vpPoint &) = &vp_createPointClass::vp_createPoint;
163 pose.addSpecificFeature(&cpClass, ptrClass, fp, pts[1]);
164 pose.addSpecificFeature(&vp_createLine, fl, line);
165 pose.addSpecificFeature(ptr, fs, pts[3], pts[4]);
166#endif
167
168 pose.setVerbose(true);
169 pose.setLambda(0.6);
170 pose.setVVSIterMax(200);
171 pose.setCovarianceComputation(true);
172
173 vpHomogeneousMatrix cMo_est(0.4, 0.3, 1.5, vpMath::rad(0), vpMath::rad(0), vpMath::rad(0));
174 vpPoseVector pose_est = vpPoseVector(cMo_est);
175 std::cout << "\nPose used as initialisation of the pose computation : " << std::endl;
176 std::cout << pose_est.t() << std::endl;
177
178 if (!use_robust)
179 pose.computePose(cMo_est);
180 else
182
183 if (!use_robust)
184 std::cout << "\nEstimated pose from visual features : " << std::endl;
185 else
186 std::cout << "\nRobust estimated pose from visual features : " << std::endl;
187
188 pose_est.buildFrom(cMo_est);
189 std::cout << pose_est.t() << std::endl;
190
191 std::cout << "\nResulting covariance (Diag): " << std::endl;
192 vpMatrix covariance = pose.getCovarianceMatrix();
193 std::cout << covariance[0][0] << " " << covariance[1][1] << " " << covariance[2][2] << " " << covariance[3][3] << " "
194 << covariance[4][4] << " " << covariance[5][5] << " " << std::endl;
195
196 int test_fail = 0;
197 for (unsigned int i = 0; i < 6; i++) {
198 if (std::fabs(pose_ref[i] - pose_est[i]) > 0.001)
199 test_fail = 1;
200 }
201
202 std::cout << "\nPose is " << (test_fail ? "badly" : "well") << " estimated\n" << std::endl;
203
204 return test_fail;
205}
206
207int main()
208{
209#if (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
210 try {
211 if (test_pose(false))
212 return EXIT_FAILURE;
213
214 if (test_pose(true))
215 return EXIT_FAILURE;
216
217 return EXIT_SUCCESS;
218 } catch (const vpException &e) {
219 std::cout << "Catch an exception: " << e.getStringMessage() << std::endl;
220 return EXIT_FAILURE;
221 }
222#else
223 std::cout << "Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
224 return EXIT_SUCCESS;
225#endif
226}
Class that defines a 3D circle in the object frame and allows forward projection of a 3D circle in th...
Definition vpCircle.h:87
void setWorldCoordinates(const vpColVector &oP)
Definition vpCircle.cpp:57
error that can be emitted by ViSP classes.
Definition vpException.h:59
const std::string & getStringMessage() const
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.
Definition of the vpImage class member functions.
Definition vpImage.h:135
Class that defines a 3D line in the object frame and allows forward projection of the line in the cam...
Definition vpLine.h:100
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:82
static double rad(double deg)
Definition vpMath.h:116
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition vpPoint.h:77
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 addFeaturePoint(const vpPoint &)
void setCovarianceComputation(const bool &flag)
void addFeatureEllipse(const vpCircle &)
void addSpecificFeature(RetType(*fct_ptr)(ArgsFunc...), Args &&...args)
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.
vpRowVector t() const
vpPoseVector buildFrom(double tx, double ty, double tz, double tux, double tuy, double tuz)