#include <visp3/core/vpConfig.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpImageTools.h>
#include <visp3/io/vpImageIo.h>
#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpTime.h>
#include <visp3/robot/vpSimulatorCamera.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpMath.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/vpParseArgv.h>
#include <visp3/visual_features/vpFeatureLuminance.h>
#include <visp3/vs/vpServo.h>
#include <stdlib.h>
#include <visp3/robot/vpImageSimulator.h>
#define Z 1
#include <visp3/core/vpIoTools.h>
#include <visp3/io/vpParseArgv.h>
#define GETOPTARGS "cdi:n:h"
#ifdef ENABLE_VISP_NAMESPACE
#endif
void usage(const char *name, const char *badparam, std::string ipath, int niter);
bool getOptions(int argc, const char **argv, std::string &ipath, bool &click_allowed, bool &display, int &niter);
void usage(const char *name, const char *badparam, std::string ipath, int niter)
{
fprintf(stdout, "\n\
Tracking of Surf key-points.\n\
\n\
SYNOPSIS\n\
%s [-i <input image path>] [-c] [-d] [-n <number of iterations>] [-h]\n",
name);
fprintf(stdout, "\n\
OPTIONS: Default\n\
-i <input image path> %s\n\
Set image input path.\n\
From this path read \"doisneau/doisneau.jpg\"\n\
images. \n\
Setting the VISP_INPUT_IMAGE_PATH environment\n\
variable produces the same behaviour than using\n\
this option.\n\
\n\
-c\n\
Disable the mouse click. Useful to automate the \n\
execution of this program without human intervention.\n\
\n\
-d \n\
Turn off the display.\n\
\n\
-n %%d %d\n\
Number of iterations.\n\
\n\
-h\n\
Print the help.\n",
ipath.c_str(), niter);
if (badparam)
fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
}
bool getOptions(int argc, const char **argv, std::string &ipath, bool &click_allowed, bool &display, int &niter)
{
const char *optarg_;
int c;
switch (c) {
case 'c':
click_allowed = false;
break;
case 'd':
display = false;
break;
case 'i':
ipath = optarg_;
break;
case 'n':
niter = atoi(optarg_);
break;
case 'h':
usage(argv[0], nullptr, ipath, niter);
return false;
default:
usage(argv[0], optarg_, ipath, niter);
return false;
}
}
if ((c == 1) || (c == -1)) {
usage(argv[0], nullptr, ipath, niter);
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)
{
#if (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
try {
std::string env_ipath;
std::string opt_ipath;
std::string ipath;
std::string filename;
bool opt_click_allowed = true;
bool opt_display = true;
int opt_niter = 400;
if (!env_ipath.empty())
ipath = env_ipath;
if (getOptions(argc, argv, opt_ipath, opt_click_allowed, opt_display, opt_niter) == false) {
return EXIT_FAILURE;
}
if (!opt_ipath.empty())
ipath = opt_ipath;
if (!opt_ipath.empty() && !env_ipath.empty()) {
if (ipath != env_ipath) {
std::cout << std::endl << "WARNING: " << std::endl;
std::cout << " Since -i <visp image path=" << ipath << "> "
<< " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
<< " we skip the environment variable." << std::endl;
}
}
if (opt_ipath.empty() && env_ipath.empty()) {
usage(argv[0], nullptr, ipath, opt_niter);
std::cerr << std::endl << "ERROR:" << std::endl;
std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
<< " environment variable to specify the location of the " << std::endl
<< " image path where test images are located." << std::endl
<< std::endl;
return EXIT_FAILURE;
}
for (int i = 0; i < 4; i++)
X[i].resize(3);
X[0][0] = -0.3;
X[0][1] = -0.215;
X[0][2] = 0;
X[1][0] = 0.3;
X[1][1] = -0.215;
X[1][2] = 0;
X[2][0] = 0.3;
X[2][1] = 0.215;
X[2][2] = 0;
X[3][0] = -0.3;
X[3][1] = 0.215;
X[3][2] = 0;
cdMo[2][3] = 1;
Id = I;
#if defined(VISP_HAVE_X11)
vpDisplayX d;
#elif defined(VISP_HAVE_GDI)
#elif defined(VISP_HAVE_GTK)
#elif defined(HAVE_OPENCV_HIGHGUI)
#endif
#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_OPENCV)
if (opt_display) {
d.
init(I, 20, 10,
"Photometric visual servoing : s");
}
if (opt_display && opt_click_allowed) {
std::cout << "Click in the image to continue..." << std::endl;
}
#endif
I = 0u;
#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK)
if (opt_display) {
}
if (opt_display && opt_click_allowed) {
std::cout << "Click in the image to continue..." << std::endl;
}
#endif
Idiff = I;
#if defined(VISP_HAVE_X11)
vpDisplayX d1;
#elif defined(VISP_HAVE_GDI)
#elif defined(VISP_HAVE_GTK)
#endif
#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK)
if (opt_display) {
d1.
init(Idiff, 40 +
static_cast<int>(I.
getWidth()), 10,
"photometric visual servoing : s-s* ");
}
#endif
robot.setSamplingTime(0.04);
int iter = 1;
double normError = 0;
do {
std::cout << "--------------------------------------------" << iter++ << std::endl;
#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK)
if (opt_display) {
}
#endif
#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK)
if (opt_display) {
}
#endif
std::cout << " |e| = " << normError << std::endl;
std::cout <<
" |v| = " << sqrt(v.
sumSquare()) << std::endl;
wMc = robot.
getPosition();
} while (normError > 10000 && iter < opt_niter);
std::cout <<
"Time to convergence: " << chrono.
getDurationMs() <<
" ms" << std::endl;
v = 0;
return EXIT_SUCCESS;
}
std::cout << "Catch an exception: " << e << std::endl;
return EXIT_FAILURE;
}
#else
(void)argc;
(void)argv;
std::cout << "Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
return EXIT_SUCCESS;
#endif
}
Generic class defining intrinsic camera parameters.
void start(bool reset=true)
Implementation of column vector and the associated operations.
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...
void init(vpImage< unsigned char > &I, int win_x=-1, int win_y=-1, const std::string &win_title="") VP_OVERRIDE
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
void init(vpImage< unsigned char > &I, int winx=-1, int winy=-1, const std::string &title="") VP_OVERRIDE
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)
error that can be emitted by ViSP classes.
Class that defines the image luminance visual feature.
vpFeatureLuminance & buildFrom(vpImage< unsigned char > &I)
void init(unsigned int _nbr, unsigned int _nbc, double _Z)
void setCameraParameters(const vpCameraParameters &_cam)
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix & buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
vpHomogeneousMatrix inverse() const
static void read(vpImage< unsigned char > &I, const std::string &filename, int backend=IO_DEFAULT_BACKEND)
Class which enables to project an image in the 3D space and get the view of a virtual camera.
void getImage(vpImage< unsigned char > &I, const vpCameraParameters &cam)
void init(const vpImage< unsigned char > &I, vpColVector *X)
void setInterpolationType(const vpInterpolationType interplt)
void setCameraPosition(const vpHomogeneousMatrix &cMt)
unsigned int getWidth() const
unsigned int getHeight() const
static double rad(double deg)
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel) VP_OVERRIDE
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
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 setServo(const vpServoType &servo_type)
vpColVector getError() const
vpColVector computeControlLaw()
Class that defines the simplest robot: a free flying camera.