Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
testFeatureSegment.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2023 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 https://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 * Visual feature manipulation (segment).
33 *
34*****************************************************************************/
35
36#include <fstream>
37#include <iostream>
38#include <numeric>
39#include <vector>
40
41#include <visp3/core/vpConfig.h>
42
43#if defined(VISP_HAVE_MODULE_ROBOT) && \
44 (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
45
46#include <visp3/core/vpCameraParameters.h>
47#include <visp3/core/vpDisplay.h>
48#include <visp3/core/vpHomogeneousMatrix.h>
49#include <visp3/core/vpImage.h>
50#include <visp3/core/vpMath.h>
51#include <visp3/core/vpPoint.h>
52#include <visp3/gui/vpDisplayGDI.h>
53#include <visp3/gui/vpDisplayX.h>
54#include <visp3/gui/vpPlot.h>
55#include <visp3/io/vpParseArgv.h>
56#include <visp3/robot/vpSimulatorCamera.h>
57#include <visp3/visual_features/vpFeatureBuilder.h>
58#include <visp3/visual_features/vpFeatureSegment.h>
59#include <visp3/vs/vpServo.h> //visual servoing task
60
68int main(int argc, const char **argv)
69{
70 try {
71#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI))
72 int opt_no_display = 0;
73 int opt_curves = 1;
74#endif
75 int opt_normalized = 1;
76
77 // Parse the command line to set the variables
78 vpParseArgv::vpArgvInfo argTable [] = {
79#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI))
80 {"-d", vpParseArgv::ARGV_CONSTANT_INT, 0, (char *)&opt_no_display, "Disable display and graphics viewer."},
81#endif
82 {"-normalized", vpParseArgv::ARGV_INT, (char *)NULL, (char *)&opt_normalized,
83 "1 to use normalized features, 0 for non normalized."},
84 {"-h", vpParseArgv::ARGV_HELP, (char *)NULL, (char *)NULL, "Print the help."},
85 {(char *)NULL, vpParseArgv::ARGV_END, (char *)NULL, (char *)NULL, (char *)NULL}
86 };
87
88 // Read the command line options
89 if (vpParseArgv::parse(&argc, argv, argTable,
92 return (false);
93 }
94
95 std::cout << "Used options: " << std::endl;
96#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI))
97 opt_curves = (opt_no_display == 0) ? 1 : 0;
98 std::cout << " - no display: " << opt_no_display << std::endl;
99 std::cout << " - curves : " << opt_curves << std::endl;
100#endif
101 std::cout << " - normalized: " << opt_normalized << std::endl;
102
103 vpCameraParameters cam(640., 480., 320., 240.);
104
105#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)
106 vpDisplay *display = NULL;
107 if (!opt_no_display) {
108#if defined(VISP_HAVE_X11)
109 display = new vpDisplayX;
110#elif defined(VISP_HAVE_GDI)
111 display = new vpDisplayGDI;
112#endif
113 }
114#endif
115 vpImage<unsigned char> I(480, 640, 0);
116
117#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI))
118 if (!opt_no_display)
119 display->init(I);
120#endif
121
122 vpHomogeneousMatrix wMo; // Set to identity. Robot world frame is equal to object frame
123 vpHomogeneousMatrix cMo(-0.5, 0.5, 2., vpMath::rad(10), vpMath::rad(20), vpMath::rad(30));
124 vpHomogeneousMatrix cdMo(0., 0., 1., vpMath::rad(0), vpMath::rad(0), vpMath::rad(0));
125 vpHomogeneousMatrix wMc; // Camera location in the robot world frame
126
127 vpPoint P[4]; // 4 points in the object frame
128 P[0].setWorldCoordinates(.1, .1, 0.);
129 P[1].setWorldCoordinates(-.1, .1, 0.);
130 P[2].setWorldCoordinates(-.1, -.1, 0.);
131 P[3].setWorldCoordinates(.1, -.1, 0.);
132
133 vpPoint Pd[4]; // 4 points in the desired camera frame
134 for (int i = 0; i < 4; i++) {
135 Pd[i] = P[i];
136 Pd[i].project(cdMo);
137 }
138 vpPoint Pc[4]; // 4 points in the current camera frame
139 for (int i = 0; i < 4; i++) {
140 Pc[i] = P[i];
141 Pc[i].project(cMo);
142 }
143
144 vpFeatureSegment seg_cur[2], seg_des[2]; // Current and desired features
145 for (int i = 0; i < 2; i++) {
146 if (opt_normalized) {
147 seg_cur[i].setNormalized(true);
148 seg_des[i].setNormalized(true);
149 }
150 else {
151 seg_cur[i].setNormalized(false);
152 seg_des[i].setNormalized(false);
153 }
154 vpFeatureBuilder::create(seg_cur[i], Pc[i * 2], Pc[i * 2 + 1]);
155 vpFeatureBuilder::create(seg_des[i], Pd[i * 2], Pd[i * 2 + 1]);
156 seg_cur[i].print();
157 seg_des[i].print();
158 }
159
160 // define visual servoing task
161 vpServo task;
164 task.setLambda(2.);
165
166 for (int i = 0; i < 2; i++)
167 task.addFeature(seg_cur[i], seg_des[i]);
168
169#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI))
170 if (!opt_no_display) {
172 for (int i = 0; i < 2; i++) {
173 seg_cur[i].display(cam, I, vpColor::red);
174 seg_des[i].display(cam, I, vpColor::green);
176 }
177 }
178#endif
179
180#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI))
181 vpPlot *graph = NULL;
182 if (opt_curves) {
183 // Create a window (700 by 700) at position (100, 200) with two graphics
184 graph = new vpPlot(2, 500, 500, 700, 10, "Curves...");
185
186 // The first graphic contains 3 curve and the second graphic contains 3
187 // curves
188 graph->initGraph(0, 6);
189 graph->initGraph(1, 8);
190 // graph->setTitle(0, "Velocities");
191 // graph->setTitle(1, "Error s-s*");
192 }
193#endif
194
195 // param robot
196 vpSimulatorCamera robot;
197 float sampling_time = 0.02f; // Sampling period in seconds
198 robot.setSamplingTime(sampling_time);
201 wMc = wMo * cMo.inverse();
202 robot.setPosition(wMc);
203 int iter = 0;
204
205 do {
206 double t = vpTime::measureTimeMs();
207 wMc = robot.getPosition();
208 cMo = wMc.inverse() * wMo;
209 for (int i = 0; i < 4; i++)
210 Pc[i].project(cMo);
211
212 for (int i = 0; i < 2; i++)
213 vpFeatureBuilder::create(seg_cur[i], Pc[i * 2], Pc[i * 2 + 1]);
214
215#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI))
216 if (!opt_no_display) {
218 for (int i = 0; i < 2; i++) {
219 seg_cur[i].display(cam, I, vpColor::red);
220 seg_des[i].display(cam, I, vpColor::green);
222 }
223 }
224#endif
225
228
229#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI))
230 if (opt_curves) {
231 graph->plot(0, iter, v); // plot velocities applied to the robot
232 graph->plot(1, iter, task.getError()); // plot error vector
233 }
234#endif
235
236 vpTime::wait(t, sampling_time * 1000); // Wait 10 ms
237 iter++;
238
239 } while ((task.getError()).sumSquare() > 0.0005);
240
241#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI))
242 if (graph != NULL)
243 delete graph;
244#endif
245#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI))
246 if (!opt_no_display && display != NULL)
247 delete display;
248#endif
249
250 std::cout << "final error=" << (task.getError()).sumSquare() << std::endl;
251 return EXIT_SUCCESS;
252 }
253 catch (const vpException &e) {
254 std::cout << "Catch an exception: " << e << std::endl;
255 return EXIT_FAILURE;
256 }
257}
258
259#elif !(defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
260int main()
261{
262 std::cout << "Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
263 return EXIT_SUCCESS;
264}
265#else
266int main()
267{
268 std::cout << "Test empty since visp_robot module is not available.\n" << std::endl;
269 return EXIT_SUCCESS;
270}
271
272#endif
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
static const vpColor red
Definition vpColor.h:211
static const vpColor green
Definition vpColor.h:214
Display for windows using GDI (available on any windows 32 platform).
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition vpDisplayX.h:132
Class that defines generic functionalities for display.
Definition vpDisplay.h:173
static void display(const vpImage< unsigned char > &I)
static void flush(const vpImage< unsigned char > &I)
error that can be emitted by ViSP classes.
Definition vpException.h:59
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
Class that defines a 2D segment visual features. This class allow to consider two sets of visual feat...
void print(unsigned int select=FEATURE_ALL) const
void setNormalized(bool normalized)
void display(const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1) const
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
Definition of the vpImage class member functions.
Definition vpImage.h:135
static double rad(double deg)
Definition vpMath.h:116
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
@ ARGV_NO_DEFAULTS
No default options like -help.
@ ARGV_NO_LEFTOVERS
Print an error message if an option is not in the argument list.
@ ARGV_INT
Argument is associated to an int.
@ ARGV_CONSTANT_INT
Stand alone argument associated to an int var that is set to 1.
@ ARGV_END
End of the argument list.
@ ARGV_HELP
Argument is for help displaying.
This class enables real time drawing of 2D or 3D graphics. An instance of the class open a window whi...
Definition vpPlot.h:113
void initGraph(unsigned int graphNum, unsigned int curveNbr)
Definition vpPlot.cpp:202
void plot(unsigned int graphNum, unsigned int curveNum, double x, double y)
Definition vpPlot.cpp:269
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition vpPoint.h:77
void setWorldCoordinates(double oX, double oY, double oZ)
Definition vpPoint.cpp:110
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
@ CAMERA_FRAME
Definition vpRobot.h:80
void setMaxRotationVelocity(double maxVr)
Definition vpRobot.cpp:257
void setMaxTranslationVelocity(double maxVt)
Definition vpRobot.cpp:236
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition vpServo.cpp:564
@ EYEINHAND_CAMERA
Definition vpServo.h:151
void setLambda(double c)
Definition vpServo.h:403
void setServo(const vpServoType &servo_type)
Definition vpServo.cpp:210
vpColVector getError() const
Definition vpServo.h:276
vpColVector computeControlLaw()
Definition vpServo.cpp:930
@ CURRENT
Definition vpServo.h:179
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition vpServo.cpp:487
Class that defines the simplest robot: a free flying camera.
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT double measureTimeMs()