Demonstration of the wireframe simulator with a simple visual servoing.
#include <stdlib.h>
#include <visp3/core/vpCameraParameters.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/visual_features/vpFeaturePoint.h>
#include <visp3/vs/vpServo.h>
#define GETOPTARGS "dh"
#ifdef VISP_HAVE_DISPLAY
void usage(const char *name, std::string ipath, const char *badparam);
bool getOptions(int argc, const char **argv, std::string &ipath, bool &display);
void usage(const char *name, std::string ipath, 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 \n\
position from the object.\n\
\n\
The visual features used to compute the pose of the camera and \n\
thus the control law are four points.\n\
\n\
This demonstration explains also how to move the object around a world\n\
reference frame. Here, the movement 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: Default\n\
-i <input image path> %s\n\
Set mire.pgm image input path.\n\
From this path read \"mire/mire.pgm\" image.\n\
Setting the VISP_INPUT_IMAGE_PATH environment variable \n\
produces the same behaviour than using this option.\n\
\n\
-d \n\
Turn off the display.\n\
\n\
-h\n\
Print the help.\n", ipath.c_str());
if (badparam)
fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
}
bool getOptions(int argc, const char **argv, std::string &ipath, bool &display)
{
const char *optarg_;
int c;
switch (c) {
case 'i':
ipath = optarg_;
break;
case 'd':
display = false;
break;
case 'h':
usage(argv[0], ipath, NULL);
return false;
break;
default:
usage(argv[0], ipath, optarg_);
return false;
break;
}
}
if ((c == 1) || (c == -1)) {
usage(argv[0], ipath, 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;
std::string opt_ipath;
std::string env_ipath;
std::string ipath;
std::string filename;
if (getOptions(argc, argv, opt_ipath, 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(Iext1, 100, 100,
"The first external view");
display[2].
init(Iext2, 100, 100,
"The second external view");
}
float sampling_time = 0.040f;
for (int i = 0; i < 4; i++)
point[i].track(cMo);
for (int i = 0; i < 4; i++)
for (int i = 0; i < 4; i++)
point[i].track(cdMo);
for (int i = 0; i < 4; i++)
for (int i = 0; i < 4; i++)
std::list<vpImageSimulator> list;
for (int i = 0; i < 4; i++)
X[i].resize(3);
X[0][0] = -0.2;
X[0][1] = -0.2;
X[0][2] = 0;
X[1][0] = 0.2;
X[1][1] = -0.2;
X[1][2] = 0;
X[2][0] = 0.2;
X[2][1] = 0.2;
X[2][2] = 0;
X[3][0] = -0.2;
X[3][1] = 0.2;
X[3][2] = 0;
if (!env_ipath.empty())
ipath = env_ipath;
if (!opt_ipath.empty())
ipath = opt_ipath;
imsim.
init(filename.c_str(), X);
list.push_back(imsim);
int stop = 10;
if (opt_display) {
stop = 2500;
std::cout << "Click on a display" << std::endl;
};
}
int iter = 0;
while (iter++ < stop) {
if (opt_display) {
}
for (int i = 0; i < 4; i++) {
}
wMo = b * c * a;
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