39 #include <visp3/core/vpDebug.h>
41 #include <visp3/core/vpImage.h>
42 #include <visp3/core/vpImageTools.h>
43 #include <visp3/io/vpImageIo.h>
45 #include <visp3/core/vpCameraParameters.h>
46 #include <visp3/core/vpTime.h>
47 #include <visp3/robot/vpSimulatorCamera.h>
49 #include <visp3/core/vpHomogeneousMatrix.h>
50 #include <visp3/core/vpMath.h>
51 #include <visp3/gui/vpDisplayD3D.h>
52 #include <visp3/gui/vpDisplayGDI.h>
53 #include <visp3/gui/vpDisplayGTK.h>
54 #include <visp3/gui/vpDisplayOpenCV.h>
55 #include <visp3/gui/vpDisplayX.h>
57 #include <visp3/io/vpParseArgv.h>
58 #include <visp3/visual_features/vpFeatureLuminance.h>
61 #include <visp3/robot/vpImageSimulator.h>
64 #include <visp3/core/vpIoTools.h>
65 #include <visp3/io/vpParseArgv.h>
68 #define GETOPTARGS "cdi:n:h"
70 void usage(
const char *name,
const char *badparam, std::string ipath,
int niter);
71 bool getOptions(
int argc,
const char **argv, std::string &ipath,
bool &click_allowed,
bool &display,
int &niter);
83 void usage(
const char *name,
const char *badparam, std::string ipath,
int niter)
86 Tracking of Surf key-points.\n\
89 %s [-i <input image path>] [-c] [-d] [-n <number of iterations>] [-h]\n",
94 -i <input image path> %s\n\
95 Set image input path.\n\
96 From this path read \"doisneau/doisneau.jpg\"\n\
98 Setting the VISP_INPUT_IMAGE_PATH environment\n\
99 variable produces the same behaviour than using\n\
103 Disable the mouse click. Useful to automate the \n\
104 execution of this program without human intervention.\n\
107 Turn off the display.\n\
110 Number of iterations.\n\
114 ipath.c_str(), niter);
117 fprintf(stdout,
"\nERROR: Bad parameter [%s]\n", badparam);
133 bool getOptions(
int argc,
const char **argv, std::string &ipath,
bool &click_allowed,
bool &display,
int &niter)
141 click_allowed =
false;
150 niter = atoi(optarg_);
153 usage(argv[0],
nullptr, ipath, niter);
157 usage(argv[0], optarg_, ipath, niter);
162 if ((c == 1) || (c == -1)) {
164 usage(argv[0],
nullptr, ipath, niter);
165 std::cerr <<
"ERROR: " << std::endl;
166 std::cerr <<
" Bad argument " << optarg_ << std::endl << std::endl;
173 int main(
int argc,
const char **argv)
175 #if (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
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;
190 if (!env_ipath.empty())
194 if (getOptions(argc, argv, opt_ipath, opt_click_allowed, opt_display, opt_niter) ==
false) {
199 if (!opt_ipath.empty())
204 if (!opt_ipath.empty() && !env_ipath.empty()) {
205 if (ipath != env_ipath) {
206 std::cout << std::endl <<
"WARNING: " << std::endl;
207 std::cout <<
" Since -i <visp image path=" << ipath <<
"> "
208 <<
" is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
209 <<
" we skip the environment variable." << std::endl;
214 if (opt_ipath.empty() && env_ipath.empty()) {
215 usage(argv[0],
nullptr, ipath, opt_niter);
216 std::cerr << std::endl <<
"ERROR:" << std::endl;
217 std::cerr <<
" Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
218 <<
" environment variable to specify the location of the " << std::endl
219 <<
" image path where test images are located." << std::endl
229 for (
int i = 0; i < 4; i++)
254 sim.
init(Itexture, X);
273 #if defined(VISP_HAVE_X11)
275 #elif defined(VISP_HAVE_GDI)
277 #elif defined(VISP_HAVE_GTK)
279 #elif defined(HAVE_OPENCV_HIGHGUI)
283 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_OPENCV)
285 d.
init(I, 20, 10,
"Photometric visual servoing : s");
289 if (opt_display && opt_click_allowed) {
290 std::cout <<
"Click in the image to continue..." << std::endl;
310 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK)
315 if (opt_display && opt_click_allowed) {
316 std::cout <<
"Click in the image to continue..." << std::endl;
327 #if defined(VISP_HAVE_X11)
329 #elif defined(VISP_HAVE_GDI)
331 #elif defined(VISP_HAVE_GTK)
334 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK)
336 d1.
init(Idiff, 40 +
static_cast<int>(I.
getWidth()), 10,
"photometric visual servoing : s-s* ");
343 robot.setSamplingTime(0.04);
345 robot.setPosition(wMc);
385 for (
unsigned int i = 0; i < n; i++)
386 diagHsd[i][i] = Hsd[i][i];
411 double normeError = 0;
416 std::cout <<
"--------------------------------------------" << iter++ << std::endl;
421 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK)
428 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK)
438 sI.
error(sId, error);
441 std::cout <<
"|e| " << normeError << std::endl;
457 e = H * Lsd.
t() * error;
462 std::cout <<
"lambda = " << lambda <<
" mu = " << mu;
463 std::cout <<
" |Tc| = " << sqrt(v.
sumSquare()) << std::endl;
467 wMc = robot.getPosition();
469 }
while (normeError > 10000 && iter < opt_niter);
472 std::cout <<
"Time to convergence: " << chrono.
getDurationMs() <<
" ms" << std::endl;
480 std::cout <<
"Catch an exception: " << e << std::endl;
486 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 win_x=-1, int win_y=-1, const std::string &win_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.
void init(unsigned int _nbr, unsigned int _nbc, double _Z)
void setCameraParameters(vpCameraParameters &_cam)
vpColVector error(const vpBasicFeature &s_star, unsigned int select=FEATURE_ALL) vp_override
void buildFrom(vpImage< unsigned char > &I)
vpMatrix interaction(unsigned int select=FEATURE_ALL) vp_override
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, 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)
Implementation of a matrix and operations on matrices.
vpMatrix inverseByLU() const
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)
Class that defines the simplest robot: a free flying camera.
void display(vpImage< unsigned char > &I, const std::string &title)
Display a gray-scale image.