53 #include <visp3/core/vpHomogeneousMatrix.h>
54 #include <visp3/core/vpMath.h>
55 #include <visp3/core/vpSphere.h>
56 #include <visp3/gui/vpDisplayD3D.h>
57 #include <visp3/gui/vpDisplayGDI.h>
58 #include <visp3/gui/vpDisplayGTK.h>
59 #include <visp3/gui/vpDisplayOpenCV.h>
60 #include <visp3/gui/vpDisplayX.h>
61 #include <visp3/gui/vpProjectionDisplay.h>
62 #include <visp3/io/vpParseArgv.h>
63 #include <visp3/robot/vpSimulatorCamera.h>
64 #include <visp3/visual_features/vpFeatureBuilder.h>
65 #include <visp3/visual_features/vpFeatureEllipse.h>
66 #include <visp3/vs/vpServo.h>
67 #include <visp3/vs/vpServoDisplay.h>
70 #define GETOPTARGS "cdho"
72 void usage(
const char *name,
const char *badparam);
73 bool getOptions(
int argc,
const char **argv,
bool &click_allowed,
bool &display);
83 void usage(
const char *name,
const char *badparam)
86 Simulation of a 2D visual servoing on a sphere:\n\
87 - eye-in-hand control law,\n\
88 - velocity computed in the camera frame,\n\
89 - display the camera view,\n\
90 - a secondary task is the added.\n\
93 %s [-c] [-d] [-o] [-h]\n", name);
99 Disable the mouse click. Useful to automaze the \n\
100 execution of this program without humain intervention.\n\
103 Turn off the display.\n\
106 Disable new projection operator usage for secondary task.\n\
112 fprintf(stdout,
"\nERROR: Bad parameter [%s]\n", badparam);
128 bool getOptions(
int argc,
const char **argv,
bool &click_allowed,
bool &display,
bool &new_proj_operator)
136 click_allowed =
false;
142 new_proj_operator =
false;
145 usage(argv[0], NULL);
149 usage(argv[0], optarg_);
154 if ((c == 1) || (c == -1)) {
156 usage(argv[0], NULL);
157 std::cerr <<
"ERROR: " << std::endl;
158 std::cerr <<
" Bad argument " << optarg_ << std::endl << std::endl;
165 int main(
int argc,
const char **argv)
167 #if (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
169 bool opt_display =
true;
170 bool opt_click_allowed =
true;
171 bool opt_new_proj_operator =
true;
174 if (getOptions(argc, argv, opt_click_allowed, opt_display, opt_new_proj_operator) ==
false) {
175 return(EXIT_FAILURE);
182 #ifdef VISP_HAVE_DISPLAY
183 # if defined VISP_HAVE_X11
186 # elif defined VISP_HAVE_GTK
189 # elif defined VISP_HAVE_GDI
192 # elif defined VISP_HAVE_OPENCV
195 # elif defined VISP_HAVE_D3D9
202 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)
204 displayI.
init(I, 100, 100,
"Camera view...");
205 displayExt.
init(Iext, 130 +
static_cast<int>(I.
getWidth()), 100,
"External view");
218 #ifdef VISP_HAVE_DISPLAY
222 double px = 600, py = 600;
237 robot.getPosition(wMc);
249 #ifdef VISP_HAVE_DISPLAY
250 externalview.
insert(sphere);
269 std::cout << std::endl;
280 #ifdef VISP_HAVE_DISPLAY
289 if (opt_display && opt_click_allowed) {
295 unsigned int iter = 0;
297 bool start_secondary_task =
false;
300 while (iter++ < 2000 && !stop) {
301 std::cout <<
"---------------------------------------------" << iter << std::endl;
304 robot.getPosition(wMc);
316 #ifdef VISP_HAVE_DISPLAY
326 start_secondary_task =
true;
329 if (start_secondary_task) {
338 std::cout <<
"de2dt :" << de2dt.t() << std::endl;
340 std::cout <<
"(I-WpW)de2dt :" << sec.
t() << std::endl;
344 if (opt_display && opt_click_allowed) {
345 std::stringstream ss;
346 ss << std::string(
"New projection operator: ") + (opt_new_proj_operator ? std::string(
"yes (use option -o to use old one)") :
std::string(
"no"));
352 if (opt_display && opt_click_allowed) {
360 std::cout <<
"|| s - s* || = " << (task.
getError()).sumSquare() << std::endl;
372 if (opt_display && opt_click_allowed) {
384 std::cout <<
"Catch a ViSP exception: " << e << std::endl;
390 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="")
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 emited 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)
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 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()
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
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)