58#include <visp3/core/vpCameraParameters.h>
59#include <visp3/core/vpCylinder.h>
60#include <visp3/core/vpHomogeneousMatrix.h>
61#include <visp3/core/vpImage.h>
62#include <visp3/core/vpMath.h>
63#include <visp3/gui/vpDisplayD3D.h>
64#include <visp3/gui/vpDisplayGDI.h>
65#include <visp3/gui/vpDisplayGTK.h>
66#include <visp3/gui/vpDisplayOpenCV.h>
67#include <visp3/gui/vpDisplayX.h>
68#include <visp3/gui/vpProjectionDisplay.h>
69#include <visp3/io/vpParseArgv.h>
70#include <visp3/robot/vpSimulatorCamera.h>
71#include <visp3/visual_features/vpFeatureBuilder.h>
72#include <visp3/visual_features/vpFeatureLine.h>
73#include <visp3/vs/vpServo.h>
74#include <visp3/vs/vpServoDisplay.h>
77#define GETOPTARGS "cdho"
79void usage(
const char *name,
const char *badparam);
80bool getOptions(
int argc,
const char **argv,
bool &click_allowed,
bool &display);
90void usage(
const char *name,
const char *badparam)
93Simulation of a 2D visual servoing on a cylinder:\n\
94- eye-in-hand control law,\n\
95- velocity computed in the camera frame,\n\
96- display the camera view.\n\
99 %s [-c] [-d] [-o] [-h]\n", name);
105 Disable the mouse click. Useful to automaze the \n\
106 execution of this program without humain intervention.\n\
109 Turn off the display.\n\
112 Disable new projection operator usage for secondary task.\n\
118 fprintf(stdout,
"\nERROR: Bad parameter [%s]\n", badparam);
133bool getOptions(
int argc,
const char **argv,
bool &click_allowed,
bool &display,
bool &new_proj_operator)
141 click_allowed =
false;
147 new_proj_operator =
false;
150 usage(argv[0], NULL);
154 usage(argv[0], optarg_);
159 if ((c == 1) || (c == -1)) {
161 usage(argv[0], NULL);
162 std::cerr <<
"ERROR: " << std::endl;
163 std::cerr <<
" Bad argument " << optarg_ << std::endl << std::endl;
170int main(
int argc,
const char **argv)
172#if (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
174 bool opt_display =
true;
175 bool opt_click_allowed =
true;
176 bool opt_new_proj_operator =
true;
179 if (getOptions(argc, argv, opt_click_allowed, opt_display, opt_new_proj_operator) ==
false) {
187#ifdef VISP_HAVE_DISPLAY
188# if defined VISP_HAVE_X11
191# elif defined VISP_HAVE_GTK
194# elif defined VISP_HAVE_GDI
197# elif defined VISP_HAVE_OPENCV
200# elif defined VISP_HAVE_D3D9
207#ifdef VISP_HAVE_DISPLAY
210 displayInt.
init(Iint, 100, 100,
"Internal view");
211 displayExt.
init(Iext, 130 +
static_cast<int>(Iint.getWidth()), 100,
"External view");
224#ifdef VISP_HAVE_DISPLAY
243 robot.getPosition(wMc);
254#ifdef VISP_HAVE_DISPLAY
255 externalview.
insert(cylinder);
258 cylinder.track(cMod);
264 for (
unsigned int i = 0; i < 2; i++)
275 for (
unsigned int i = 0; i < 2; i++) {
302#ifdef VISP_HAVE_DISPLAY
311 if (opt_display && opt_click_allowed) {
323 unsigned int iter = 0;
325 bool start_secondary_task =
false;
328 std::cout <<
"---------------------------------------------" << iter++ << std::endl;
331 robot.getPosition(wMc);
343 for (
unsigned int i = 0; i < 2; i++) {
352#ifdef VISP_HAVE_DISPLAY
362 start_secondary_task =
true;
365 if (start_secondary_task) {
379 static unsigned int iter_sec = 0;
381 double vitesse = 0.5;
382 unsigned int tempo = 800;
384 if (iter_sec > tempo) {
388 if (iter_sec % tempo < 200) {
390 e1[0] = fabs(vitesse);
392 rapport = vitesse / proj_e1[0];
397 if (iter_sec % tempo < 400 && iter_sec % tempo >= 200) {
399 e2[1] = fabs(vitesse);
401 rapport = vitesse / proj_e2[1];
406 if (iter_sec % tempo < 600 && iter_sec % tempo >= 400) {
408 e1[0] = -fabs(vitesse);
410 rapport = -vitesse / proj_e1[0];
415 if (iter_sec % tempo < 800 && iter_sec % tempo >= 600) {
417 e2[1] = -fabs(vitesse);
419 rapport = -vitesse / proj_e2[1];
424 if (opt_display && opt_click_allowed) {
425 std::stringstream ss;
426 ss << std::string(
"New projection operator: ") + (opt_new_proj_operator ? std::string(
"yes (use option -o to use old one)") :
std::string(
"no"));
434 if (opt_display && opt_click_allowed) {
442 std::cout <<
"|| s - s* || = " << (task.
getError()).sumSquare() << std::endl;
456 if (opt_display && opt_click_allowed) {
468 std::cout <<
"Catch a ViSP exception: " << e << std::endl;
474 std::cout <<
"Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
static const vpColor white
Class that defines a 3D cylinder in the object frame and allows forward projection of a 3D cylinder i...
Display for windows using Direct3D 3rd party. Thus to enable this class Direct3D should be installed....
Display for windows using GDI (available on any windows 32 platform).
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
void init(vpImage< unsigned char > &I, int win_x=-1, int win_y=-1, const std::string &win_title="")
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void display(const vpImage< unsigned char > &I)
static void flush(const vpImage< unsigned char > &I)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
error that can be emited by ViSP classes.
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 ,...
void print(unsigned int select=FEATURE_ALL) const
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
static double rad(double deg)
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
interface with the image for feature display
void display(vpImage< unsigned char > &I, const vpHomogeneousMatrix &cextMo, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &color, const bool &displayTraj=false, unsigned int thickness=1)
void insert(vpForwardProjection &fp)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
static void display(const vpServo &s, const vpCameraParameters &cam, const vpImage< unsigned char > &I, vpColor currentColor=vpColor::green, vpColor desiredColor=vpColor::red, unsigned int thickness=1)
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
vpColVector secondaryTask(const vpColVector &de2dt, const bool &useLargeProjectionOperator=false)
void setServo(const vpServoType &servo_type)
vpColVector getError() const
vpColVector computeControlLaw()
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Class that defines the simplest robot: a free flying camera.