48 #include <visp3/core/vpConfig.h>
49 #include <visp3/core/vpHomogeneousMatrix.h>
50 #include <visp3/core/vpMath.h>
51 #include <visp3/core/vpSphere.h>
52 #include <visp3/gui/vpDisplayFactory.h>
53 #include <visp3/io/vpParseArgv.h>
54 #include <visp3/robot/vpSimulatorCamera.h>
55 #include <visp3/visual_features/vpFeatureBuilder.h>
56 #include <visp3/visual_features/vpFeatureEllipse.h>
57 #include <visp3/vs/vpServo.h>
58 #include <visp3/vs/vpServoDisplay.h>
61 #define GETOPTARGS "cdh"
63 #ifdef ENABLE_VISP_NAMESPACE
67 void usage(
const char *name,
const char *badparam);
68 bool getOptions(
int argc,
const char **argv,
bool &click_allowed,
bool &display);
78 void usage(
const char *name,
const char *badparam)
81 Simulation of a 2D visual servoing on a sphere:\n\
82 - eye-in-hand control law,\n\
83 - velocity computed in the camera frame,\n\
84 - display the camera view.\n\
94 Disable the mouse click. Useful to automate the \n\
95 execution of this program without human intervention.\n\
98 Turn off the display.\n\
104 fprintf(stdout,
"\nERROR: Bad parameter [%s]\n", badparam);
119 bool getOptions(
int argc,
const char **argv,
bool &click_allowed,
bool &display)
127 click_allowed =
false;
133 usage(argv[0],
nullptr);
137 usage(argv[0], optarg_);
142 if ((c == 1) || (c == -1)) {
144 usage(argv[0],
nullptr);
145 std::cerr <<
"ERROR: " << std::endl;
146 std::cerr <<
" Bad argument " << optarg_ << std::endl << std::endl;
153 int main(
int argc,
const char **argv)
155 #if (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
156 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
157 std::shared_ptr<vpDisplay> display;
162 bool opt_display =
true;
163 bool opt_click_allowed =
true;
166 if (getOptions(argc, argv, opt_click_allowed, opt_display) ==
false) {
167 return (EXIT_FAILURE);
173 #if defined(VISP_HAVE_DISPLAY)
175 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
190 double px = 600, py = 600;
205 robot.getPosition(wMc);
242 unsigned int iter = 0;
244 while (iter++ < 200) {
245 std::cout <<
"---------------------------------------------" << iter << std::endl;
249 robot.getPosition(wMc);
266 std::cout <<
"Task rank: " << task.
getTaskRank() << std::endl;
270 std::cout <<
"|| s - s* || = " << (task.
getError()).sumSquare() << std::endl;
273 if (opt_display && opt_click_allowed) {
281 #if (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11)
282 if (display !=
nullptr) {
289 std::cout <<
"Catch a ViSP exception: " << e << std::endl;
290 #if (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11)
291 if (display !=
nullptr) {
300 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
Class that defines generic functionalities for display.
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 vpImagePoint &t)
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 bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
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)
unsigned int getTaskRank() const
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
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
std::shared_ptr< vpDisplay > createDisplay()
Return a smart pointer vpDisplay specialization if a GUI library is available or nullptr otherwise.
vpDisplay * allocateDisplay()
Return a newly allocated vpDisplay specialization if a GUI library is available or nullptr otherwise.