51 #include <visp/vpDebug.h>
53 #include <visp/vpImage.h>
54 #include <visp/vpImageIo.h>
55 #include <visp/vpImageTools.h>
57 #include <visp/vpCameraParameters.h>
58 #include <visp/vpTime.h>
59 #include <visp/vpRobotCamera.h>
61 #include <visp/vpMath.h>
62 #include <visp/vpHomogeneousMatrix.h>
63 #include <visp/vpDisplayGTK.h>
64 #include <visp/vpDisplayGDI.h>
65 #include <visp/vpDisplayOpenCV.h>
66 #include <visp/vpDisplayD3D.h>
67 #include <visp/vpDisplayX.h>
69 #include <visp/vpFeatureLuminance.h>
70 #include <visp/vpParseArgv.h>
72 #include <visp/vpImageSimulator.h>
76 #include <visp/vpParseArgv.h>
77 #include <visp/vpIoTools.h>
80 #define GETOPTARGS "cdi:n:h"
92 void usage(
const char *name,
const char *badparam, std::string ipath,
int niter)
95 Tracking of Surf key-points.\n\
98 %s [-i <input image path>] [-c] [-d] [-n <number of iterations>] [-h]\n", name);
102 -i <input image path> %s\n\
103 Set image input path.\n\
104 From this path read \"ViSP-images/doisneau/doisneau.jpg\"\n\
106 Setting the VISP_INPUT_IMAGE_PATH environment\n\
107 variable produces the same behaviour than using\n\
111 Disable the mouse click. Useful to automaze the \n\
112 execution of this program without humain intervention.\n\
115 Turn off the display.\n\
118 Number of iterations.\n\
122 ipath.c_str(), niter);
125 fprintf(stdout,
"\nERROR: Bad parameter [%s]\n", badparam);
141 bool getOptions(
int argc,
const char **argv, std::string &ipath,
142 bool &click_allowed,
bool &display,
int &niter)
149 case 'c': click_allowed =
false;
break;
150 case 'd': display =
false;
break;
151 case 'i': ipath = optarg;
break;
152 case 'n': niter = atoi(optarg);
break;
153 case 'h': usage(argv[0], NULL, ipath, niter);
return false;
break;
156 usage(argv[0], optarg, ipath, niter);
161 if ((c == 1) || (c == -1)) {
163 usage(argv[0], NULL, ipath, niter);
164 std::cerr <<
"ERROR: " << std::endl;
165 std::cerr <<
" Bad argument " << optarg << std::endl << std::endl;
175 main(
int argc,
const char ** argv)
177 std::string env_ipath;
178 std::string opt_ipath;
180 std::string filename;
181 bool opt_click_allowed =
true;
182 bool opt_display =
true;
186 char *ptenv = getenv(
"VISP_INPUT_IMAGE_PATH");
191 if (! env_ipath.empty())
196 if (getOptions(argc, argv, opt_ipath, opt_click_allowed,
197 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
210 <<
"WARNING: " << std::endl;
211 std::cout <<
" Since -i <visp image path=" << ipath <<
"> "
212 <<
" is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
213 <<
" we skip the environment variable." << std::endl;
218 if (opt_ipath.empty() && env_ipath.empty()){
219 usage(argv[0], NULL, ipath, opt_niter);
220 std::cerr << std::endl
221 <<
"ERROR:" << std::endl;
222 std::cerr <<
" Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
224 <<
" environment variable to specify the location of the " << std::endl
225 <<
" image path where test images are located." << std::endl << std::endl;
234 for (
int i = 0; i < 4; i++) X[i].resize(3);
258 sim.
init(Itexture, X);
281 #if defined VISP_HAVE_X11
283 #elif defined VISP_HAVE_GDI
285 #elif defined VISP_HAVE_GTK
289 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK)
291 d.
init(I, 20, 10,
"Photometric visual servoing : s") ;
295 if (opt_display && opt_click_allowed) {
296 std::cout <<
"Click in the image to continue..." << std::endl;
315 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK)
320 if (opt_display && opt_click_allowed) {
321 std::cout <<
"Click in the image to continue..." << std::endl;
334 #if defined VISP_HAVE_X11
336 #elif defined VISP_HAVE_GDI
338 #elif defined VISP_HAVE_GTK
341 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK)
343 d1.
init(Idiff, 40+(
int)I.
getWidth(), 10,
"photometric visual servoing : s-s* ") ;
395 for(
unsigned int i = 0 ; i < n ; i++) diagHsd[i][i] = Hsd[i][i];
429 double normeError = 0;
433 std::cout <<
"--------------------------------------------" << iter++ << std::endl ;
439 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK)
446 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK)
456 sI.
error(sId,error) ;
459 std::cout <<
"|e| "<<normeError <<std::endl ;
476 e = H * Lsd.
t() *error ;
481 std::cout <<
"lambda = " << lambda <<
" mu = " << mu ;
482 std::cout <<
" |Tc| = " << sqrt(v.
sumSquare()) << std::endl;
489 while(normeError > 10000 && iter < opt_niter);