55 #include <visp3/core/vpCameraParameters.h>
56 #include <visp3/core/vpConfig.h>
57 #include <visp3/core/vpCylinder.h>
58 #include <visp3/core/vpHomogeneousMatrix.h>
59 #include <visp3/core/vpImage.h>
60 #include <visp3/core/vpMath.h>
61 #include <visp3/gui/vpDisplayFactory.h>
62 #include <visp3/gui/vpProjectionDisplay.h>
63 #include <visp3/io/vpParseArgv.h>
64 #include <visp3/robot/vpSimulatorCamera.h>
65 #include <visp3/visual_features/vpFeatureBuilder.h>
66 #include <visp3/visual_features/vpFeatureLine.h>
67 #include <visp3/vs/vpServo.h>
68 #include <visp3/vs/vpServoDisplay.h>
71 #define GETOPTARGS "cdho"
73 #ifdef ENABLE_VISP_NAMESPACE
77 void usage(
const char *name,
const char *badparam);
78 bool getOptions(
int argc,
const char **argv,
bool &click_allowed,
bool &display);
88 void usage(
const char *name,
const char *badparam)
91 Simulation of a 2D visual servoing on a cylinder:\n\
92 - eye-in-hand control law,\n\
93 - velocity computed in the camera frame,\n\
94 - display the camera view.\n\
97 %s [-c] [-d] [-o] [-h]\n",
104 Disable the mouse click. Useful to automate the \n\
105 execution of this program without human intervention.\n\
108 Turn off the display.\n\
111 Disable new projection operator usage for secondary task.\n\
117 fprintf(stdout,
"\nERROR: Bad parameter [%s]\n", badparam);
132 bool getOptions(
int argc,
const char **argv,
bool &click_allowed,
bool &display,
bool &new_proj_operator)
140 click_allowed =
false;
146 new_proj_operator =
false;
149 usage(argv[0],
nullptr);
153 usage(argv[0], optarg_);
158 if ((c == 1) || (c == -1)) {
160 usage(argv[0],
nullptr);
161 std::cerr <<
"ERROR: " << std::endl;
162 std::cerr <<
" Bad argument " << optarg_ << std::endl << std::endl;
169 int main(
int argc,
const char **argv)
172 #ifdef VISP_HAVE_DISPLAY
173 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
174 std::shared_ptr<vpDisplay> displayInt;
175 std::shared_ptr<vpDisplay> displayExt;
181 #if (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
183 bool opt_display =
true;
184 bool opt_click_allowed =
true;
185 bool opt_new_proj_operator =
true;
188 if (getOptions(argc, argv, opt_click_allowed, opt_display, opt_new_proj_operator) ==
false) {
196 #ifdef VISP_HAVE_DISPLAY
199 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
218 #ifdef VISP_HAVE_DISPLAY
237 robot.getPosition(wMc);
248 #ifdef VISP_HAVE_DISPLAY
249 externalview.
insert(cylinder);
252 cylinder.track(cMod);
258 for (
unsigned int i = 0; i < 2; i++)
269 for (
unsigned int i = 0; i < 2; i++) {
296 #ifdef VISP_HAVE_DISPLAY
305 if (opt_display && opt_click_allowed) {
317 unsigned int iter = 0;
319 bool start_secondary_task =
false;
322 std::cout <<
"---------------------------------------------" << iter++ << std::endl;
325 robot.getPosition(wMc);
337 for (
unsigned int i = 0; i < 2; i++) {
346 #ifdef VISP_HAVE_DISPLAY
356 start_secondary_task =
true;
359 if (start_secondary_task) {
373 static unsigned int iter_sec = 0;
375 double vitesse = 0.5;
376 unsigned int tempo = 800;
378 if (iter_sec > tempo) {
382 if (iter_sec % tempo < 200) {
384 e1[0] = fabs(vitesse);
386 rapport = vitesse / proj_e1[0];
391 if (iter_sec % tempo < 400 && iter_sec % tempo >= 200) {
393 e2[1] = fabs(vitesse);
395 rapport = vitesse / proj_e2[1];
400 if (iter_sec % tempo < 600 && iter_sec % tempo >= 400) {
402 e1[0] = -fabs(vitesse);
404 rapport = -vitesse / proj_e1[0];
409 if (iter_sec % tempo < 800 && iter_sec % tempo >= 600) {
411 e2[1] = -fabs(vitesse);
413 rapport = -vitesse / proj_e2[1];
418 if (opt_display && opt_click_allowed) {
419 std::stringstream ss;
420 ss << std::string(
"New projection operator: ") +
421 (opt_new_proj_operator ? std::string(
"yes (use option -o to use old one)") : std::string(
"no"));
429 if (opt_display && opt_click_allowed) {
437 std::cout <<
"|| s - s* || = " << (task.
getError()).sumSquare() << std::endl;
451 if (opt_display && opt_click_allowed) {
461 #if (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11) && defined(VISP_HAVE_DISPLAY)
462 if (displayInt !=
nullptr) {
465 if (displayExt !=
nullptr) {
472 std::cout <<
"Catch a ViSP exception: " << e << std::endl;
473 #if (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11) && defined(VISP_HAVE_DISPLAY)
474 if (displayInt !=
nullptr) {
477 if (displayExt !=
nullptr) {
486 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...
Class that defines generic functionalities for display.
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 emitted by ViSP classes.
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpImagePoint &t)
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 VP_OVERRIDE
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 insert(vpForwardProjection &fp)
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 setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel) VP_OVERRIDE
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 addFeature(vpBasicFeature &s_cur, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
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()
Class that defines the simplest robot: a free flying camera.
std::shared_ptr< vpDisplay > createDisplay()
Return a smart pointer vpDisplay specialization if a GUI library is available or nullptr otherwise.
vpDisplay * allocateDisplay()
Return a newly allocated vpDisplay specialization if a GUI library is available or nullptr otherwise.