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