Demonstration of the wireframe simulator with a simple visual servoing.
#include <stdlib.h>
#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpCylinder.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpMath.h>
#include <visp3/core/vpTime.h>
#include <visp3/core/vpVelocityTwistMatrix.h>
#include <visp3/gui/vpDisplayD3D.h>
#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
#include <visp3/io/vpImageIo.h>
#include <visp3/io/vpParseArgv.h>
#include <visp3/robot/vpSimulatorCamera.h>
#include <visp3/robot/vpWireFrameSimulator.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/vs/vpServo.h>
#define GETOPTARGS "dh"
#ifdef VISP_HAVE_DISPLAY
void usage(const char *name, const char *badparam);
bool getOptions(int argc, const char **argv, bool &display);
void usage(const char *name, const char *badparam)
{
fprintf(stdout, "\n\
Demonstration of the wireframe simulator with a simple visual servoing.\n\
\n\
The visual servoing consists in bringing the camera at a desired position\n\
from the object.\n\
\n\
The visual features used to compute the pose of the camera and \n\
thus the control law are two lines. These features are computed thanks \n\
to the equation of a cylinder.\n\
\n\
This demonstration explains also how to move the object around a world \n\
reference frame. Here, the movment is a rotation around the x and y axis \n\
at a given distance from the world frame. In fact the object trajectory \n\
is on a sphere whose center is the origin of the world frame.\n\
\n\
SYNOPSIS\n\
%s [-d] [-h]\n", name);
fprintf(stdout, "\n\
OPTIONS: \n\
-d \n\
Turn off the display.\n\
\n\
-h\n\
Print the help.\n");
if (badparam)
fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
}
bool getOptions(int argc, const char **argv, bool &display)
{
const char *optarg_;
int c;
switch (c) {
case 'd':
display = false;
break;
case 'h':
usage(argv[0], NULL);
return false;
break;
default:
usage(argv[0], optarg_);
return false;
break;
}
}
if ((c == 1) || (c == -1)) {
usage(argv[0], NULL);
std::cerr << "ERROR: " << std::endl;
std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
return false;
}
return true;
}
int main(int argc, const char **argv)
{
try {
bool opt_display = true;
if (getOptions(argc, argv, opt_display) == false) {
exit(-1);
}
#if defined VISP_HAVE_X11
#elif defined VISP_HAVE_OPENCV
#elif defined VISP_HAVE_GDI
#elif defined VISP_HAVE_D3D9
#elif defined VISP_HAVE_GTK
#endif
if (opt_display) {
display[0].
init(Iint, 100, 100,
"The internal view");
display[1].
init(Iext, 100, 100,
"The first external view");
}
float sampling_time = 0.040f;
cylinder.track(cMo);
cylinder.track(cdMo);
for (int i = 0; i < 2; i++)
int stop = 10;
if (opt_display) {
stop = 2500;
std::cout << "Click on a display" << std::endl;
};
}
int iter = 0;
e1 = 0;
e2 = 0;
iter = 0;
double rapport = 0;
double vitesse = 0.3;
int tempo = 600;
while (iter++ < stop) {
if (opt_display) {
}
cylinder.track(cMo);
if (iter % tempo < 200 && iter % tempo >= 0) {
e2 = 0;
e1[0] = -fabs(vitesse);
rapport = -vitesse / proj_e1[0];
proj_e1 *= rapport;
v += proj_e1;
}
if (iter % tempo < 300 && iter % tempo >= 200) {
e1 = 0;
e2[1] = -fabs(vitesse);
rapport = -vitesse / proj_e2[1];
proj_e2 *= rapport;
v += proj_e2;
}
if (iter % tempo < 500 && iter % tempo >= 300) {
e2 = 0;
e1[0] = -fabs(vitesse);
rapport = vitesse / proj_e1[0];
proj_e1 *= rapport;
v += proj_e1;
}
if (iter % tempo < 600 && iter % tempo >= 500) {
e1 = 0;
e2[1] = -fabs(vitesse);
rapport = vitesse / proj_e2[1];
proj_e2 *= rapport;
v += proj_e2;
}
if (opt_display) {
;
}
std::cout <<
"|| s - s* || = " << (task.
getError()).sumSquare() << std::endl;
}
return EXIT_SUCCESS;
std::cout << "Catch an exception: " << e << std::endl;
return EXIT_FAILURE;
}
}
#else
int main()
{
std::cout << "You do not have X11, or GDI (Graphical Device Interface), or GTK functionalities to display images..." << std::endl;
std::cout << "Tip if you are on a unix-like system:" << std::endl;
std::cout << "- Install X11, configure again ViSP using cmake and build again this example" << std::endl;
std::cout << "Tip if you are on a windows-like system:" << std::endl;
std::cout << "- Install GDI, configure again ViSP using cmake and build again this example" << std::endl;
return EXIT_SUCCESS;
}
#endif