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