43 #include <visp3/core/vpImage.h>
44 #include <visp3/core/vpImageTools.h>
45 #include <visp3/io/vpImageIo.h>
47 #include <visp3/core/vpCameraParameters.h>
48 #include <visp3/core/vpTime.h>
49 #include <visp3/robot/vpSimulatorCamera.h>
51 #include <visp3/core/vpHomogeneousMatrix.h>
52 #include <visp3/core/vpMath.h>
53 #include <visp3/gui/vpDisplayD3D.h>
54 #include <visp3/gui/vpDisplayGDI.h>
55 #include <visp3/gui/vpDisplayGTK.h>
56 #include <visp3/gui/vpDisplayOpenCV.h>
57 #include <visp3/gui/vpDisplayX.h>
59 #include <visp3/io/vpParseArgv.h>
60 #include <visp3/visual_features/vpFeatureLuminance.h>
61 #include <visp3/vs/vpServo.h>
64 #include <visp3/robot/vpImageSimulator.h>
67 #include <visp3/core/vpIoTools.h>
68 #include <visp3/io/vpParseArgv.h>
71 #define GETOPTARGS "cdi:n:h"
73 void usage(
const char *name,
const char *badparam, std::string ipath,
int niter);
74 bool getOptions(
int argc,
const char **argv, std::string &ipath,
bool &click_allowed,
bool &display,
int &niter);
86 void usage(
const char *name,
const char *badparam, std::string ipath,
int niter)
89 Tracking of Surf key-points.\n\
92 %s [-i <input image path>] [-c] [-d] [-n <number of iterations>] [-h]\n",
97 -i <input image path> %s\n\
98 Set image input path.\n\
99 From this path read \"doisneau/doisneau.jpg\"\n\
101 Setting the VISP_INPUT_IMAGE_PATH environment\n\
102 variable produces the same behaviour than using\n\
106 Disable the mouse click. Useful to automaze the \n\
107 execution of this program without humain intervention.\n\
110 Turn off the display.\n\
113 Number of iterations.\n\
117 ipath.c_str(), niter);
120 fprintf(stdout,
"\nERROR: Bad parameter [%s]\n", badparam);
136 bool getOptions(
int argc,
const char **argv, std::string &ipath,
bool &click_allowed,
bool &display,
int &niter)
144 click_allowed =
false;
153 niter = atoi(optarg_);
156 usage(argv[0], NULL, ipath, niter);
160 usage(argv[0], optarg_, ipath, niter);
165 if ((c == 1) || (c == -1)) {
167 usage(argv[0], NULL, ipath, niter);
168 std::cerr <<
"ERROR: " << std::endl;
169 std::cerr <<
" Bad argument " << optarg_ << std::endl << std::endl;
176 int main(
int argc,
const char **argv)
178 #if (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
180 std::string env_ipath;
181 std::string opt_ipath;
183 std::string filename;
184 bool opt_click_allowed =
true;
185 bool opt_display =
true;
193 if (!env_ipath.empty())
197 if (getOptions(argc, argv, opt_ipath, opt_click_allowed, opt_display, opt_niter) ==
false) {
202 if (!opt_ipath.empty())
207 if (!opt_ipath.empty() && !env_ipath.empty()) {
208 if (ipath != env_ipath) {
209 std::cout << std::endl <<
"WARNING: " << std::endl;
210 std::cout <<
" Since -i <visp image path=" << ipath <<
"> "
211 <<
" is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
212 <<
" we skip the environment variable." << std::endl;
217 if (opt_ipath.empty() && env_ipath.empty()) {
218 usage(argv[0], NULL, ipath, opt_niter);
219 std::cerr << std::endl <<
"ERROR:" << std::endl;
220 std::cerr <<
" Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
221 <<
" environment variable to specify the location of the " << std::endl
222 <<
" image path where test images are located." << std::endl
232 for (
int i = 0; i < 4; i++)
257 sim.
init(Itexture, X);
276 #if defined VISP_HAVE_X11
278 #elif defined VISP_HAVE_GDI
280 #elif defined VISP_HAVE_GTK
282 #elif defined VISP_HAVE_OPENCV
286 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_OPENCV)
288 d.
init(I, 20, 10,
"Photometric visual servoing : s");
292 if (opt_display && opt_click_allowed) {
293 std::cout <<
"Click in the image to continue..." << std::endl;
313 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK)
318 if (opt_display && opt_click_allowed) {
319 std::cout <<
"Click in the image to continue..." << std::endl;
330 #if defined VISP_HAVE_X11
332 #elif defined VISP_HAVE_GDI
334 #elif defined VISP_HAVE_GTK
337 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK)
339 d1.
init(Idiff, 40 +
static_cast<int>(I.
getWidth()), 10,
"photometric visual servoing : s-s* ");
346 robot.setSamplingTime(0.04);
348 robot.setPosition(wMc);
385 double normError = 0;
391 std::cout <<
"--------------------------------------------" << iter++ << std::endl;
396 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK)
403 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK)
415 std::cout <<
" |e| = " << normError << std::endl;
416 std::cout <<
" |v| = " << sqrt(v.
sumSquare()) << std::endl;
420 wMc = robot.getPosition();
422 }
while (normError > 10000 && iter < opt_niter);
425 std::cout <<
"Time to convergence: " << chrono.
getDurationMs() <<
" ms" << std::endl;
432 std::cout <<
"Catch an exception: " << e << std::endl;
438 std::cout <<
"Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
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...
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 winx=-1, int winy=-1, const std::string &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)
error that can be emited by ViSP classes.
Class that defines the image luminance visual feature.
void setCameraParameters(vpCameraParameters &_cam)
void buildFrom(vpImage< unsigned char > &I)
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
static void read(vpImage< unsigned char > &I, const std::string &filename)
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)
@ 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 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.