49 #include <visp3/core/vpHomogeneousMatrix.h>
50 #include <visp3/core/vpMath.h>
51 #include <visp3/core/vpSphere.h>
52 #include <visp3/gui/vpDisplayD3D.h>
53 #include <visp3/gui/vpDisplayGDI.h>
54 #include <visp3/gui/vpDisplayGTK.h>
55 #include <visp3/gui/vpDisplayOpenCV.h>
56 #include <visp3/gui/vpDisplayX.h>
57 #include <visp3/gui/vpProjectionDisplay.h>
58 #include <visp3/io/vpParseArgv.h>
59 #include <visp3/robot/vpSimulatorCamera.h>
60 #include <visp3/visual_features/vpFeatureBuilder.h>
61 #include <visp3/visual_features/vpFeatureEllipse.h>
62 #include <visp3/vs/vpServo.h>
63 #include <visp3/vs/vpServoDisplay.h>
66 #define GETOPTARGS "cdho"
68 void usage(
const char *name,
const char *badparam);
69 bool getOptions(
int argc,
const char **argv,
bool &click_allowed,
bool &display);
79 void usage(
const char *name,
const char *badparam)
82 Simulation of a 2D visual servoing on a sphere:\n\
83 - eye-in-hand control law,\n\
84 - velocity computed in the camera frame,\n\
85 - display the camera view,\n\
86 - a secondary task is the added.\n\
89 %s [-c] [-d] [-o] [-h]\n",
96 Disable the mouse click. Useful to automate the \n\
97 execution of this program without human intervention.\n\
100 Turn off the display.\n\
103 Disable new projection operator usage for secondary task.\n\
109 fprintf(stdout,
"\nERROR: Bad parameter [%s]\n", badparam);
125 bool getOptions(
int argc,
const char **argv,
bool &click_allowed,
bool &display,
bool &new_proj_operator)
133 click_allowed =
false;
139 new_proj_operator =
false;
142 usage(argv[0],
nullptr);
146 usage(argv[0], optarg_);
151 if ((c == 1) || (c == -1)) {
153 usage(argv[0],
nullptr);
154 std::cerr <<
"ERROR: " << std::endl;
155 std::cerr <<
" Bad argument " << optarg_ << std::endl << std::endl;
162 int main(
int argc,
const char **argv)
164 #if (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
166 bool opt_display =
true;
167 bool opt_click_allowed =
true;
168 bool opt_new_proj_operator =
true;
171 if (getOptions(argc, argv, opt_click_allowed, opt_display, opt_new_proj_operator) ==
false) {
172 return (EXIT_FAILURE);
179 #ifdef VISP_HAVE_DISPLAY
180 #if defined(VISP_HAVE_X11)
183 #elif defined(VISP_HAVE_GTK)
186 #elif defined(VISP_HAVE_GDI)
189 #elif defined(HAVE_OPENCV_HIGHGUI)
192 #elif defined(VISP_HAVE_D3D9)
199 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)
201 displayI.
init(I, 100, 100,
"Camera view...");
202 displayExt.
init(Iext, 130 +
static_cast<int>(I.
getWidth()), 100,
"External view");
215 #ifdef VISP_HAVE_DISPLAY
219 double px = 600, py = 600;
234 robot.getPosition(wMc);
246 #ifdef VISP_HAVE_DISPLAY
247 externalview.
insert(sphere);
266 std::cout << std::endl;
277 #ifdef VISP_HAVE_DISPLAY
286 if (opt_display && opt_click_allowed) {
292 unsigned int iter = 0;
294 bool start_secondary_task =
false;
297 while (iter++ < 2000 && !stop) {
298 std::cout <<
"---------------------------------------------" << iter << std::endl;
301 robot.getPosition(wMc);
313 #ifdef VISP_HAVE_DISPLAY
323 start_secondary_task =
true;
326 if (start_secondary_task) {
336 std::cout <<
"de2dt :" << de2dt.t() << std::endl;
338 std::cout <<
"(I-WpW)de2dt :" << sec.
t() << std::endl;
342 if (opt_display && opt_click_allowed) {
343 std::stringstream ss;
344 ss << std::string(
"New projection operator: ") +
345 (opt_new_proj_operator ? std::string(
"yes (use option -o to use old one)") : std::string(
"no"));
350 if (opt_display && opt_click_allowed) {
358 std::cout <<
"|| s - s* || = " << (task.
getError()).sumSquare() << std::endl;
370 if (opt_display && opt_click_allowed) {
382 std::cout <<
"Catch a ViSP exception: " << e << std::endl;
388 std::cout <<
"Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
static const vpColor white
Display for windows using Direct3D 3rd party. Thus to enable this class Direct3D should be installed....
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)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
error that can be emitted by ViSP classes.
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
Class that defines 2D ellipse visual feature.
void track(const vpHomogeneousMatrix &cMo)
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
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)
interface with the image for feature display
void display(vpImage< unsigned char > &I, const vpHomogeneousMatrix &cextMo, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &color, const bool &displayTraj=false, unsigned int thickness=1)
void insert(vpForwardProjection &fp)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel) vp_override
static void display(const vpServo &s, const vpCameraParameters &cam, const vpImage< unsigned char > &I, vpColor currentColor=vpColor::green, vpColor desiredColor=vpColor::red, unsigned int thickness=1)
void addFeature(vpBasicFeature &s_cur, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
vpColVector secondaryTask(const vpColVector &de2dt, const bool &useLargeProjectionOperator=false)
void setServo(const vpServoType &servo_type)
vpColVector getError() const
vpColVector computeControlLaw()
Class that defines the simplest robot: a free flying camera.
Class that defines a 3D sphere in the object frame and allows forward projection of a 3D sphere in th...
void setWorldCoordinates(const vpColVector &oP) vp_override
void display(vpImage< unsigned char > &I, const std::string &title)
Display a gray-scale image.