#include <stdio.h>
#include <stdlib.h>
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpMath.h>
#include <visp3/core/vpSphere.h>
#include <visp3/gui/vpDisplayFactory.h>
#include <visp3/gui/vpProjectionDisplay.h>
#include <visp3/io/vpParseArgv.h>
#include <visp3/robot/vpSimulatorCamera.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/visual_features/vpFeatureEllipse.h>
#include <visp3/vs/vpServo.h>
#include <visp3/vs/vpServoDisplay.h>
#define GETOPTARGS "cdho"
#ifdef ENABLE_VISP_NAMESPACE
#endif
void usage(const char *name, const char *badparam);
bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display);
void usage(const char *name, const char *badparam)
{
fprintf(stdout, "\n\
Simulation of a 2D visual servoing on a sphere:\n\
- eye-in-hand control law,\n\
- velocity computed in the camera frame,\n\
- display the camera view,\n\
- a secondary task is the added.\n\
\n\
SYNOPSIS\n\
%s [-c] [-d] [-o] [-h]\n",
name);
fprintf(stdout, "\n\
OPTIONS: Default\n\
\n\
-c\n\
Disable the mouse click. Useful to automate the \n\
execution of this program without human intervention.\n\
\n\
-d \n\
Turn off the display.\n\
\n\
-o \n\
Disable new projection operator usage for secondary task.\n\
\n\
-h\n\
Print the help.\n");
if (badparam)
fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
}
bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display, bool &new_proj_operator)
{
const char *optarg_;
int c;
switch (c) {
case 'c':
click_allowed = false;
break;
case 'd':
display = false;
break;
case 'o':
new_proj_operator = false;
break;
case 'h':
usage(argv[0], nullptr);
return false;
default:
usage(argv[0], optarg_);
return false;
}
}
if ((c == 1) || (c == -1)) {
usage(argv[0], nullptr);
std::cerr << "ERROR: " << std::endl;
std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
return false;
}
return true;
}
int main(int argc, const char **argv)
{
#if (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
std::shared_ptr<vpDisplay> displayI;
std::shared_ptr<vpDisplay> displayExt;
#else
#endif
try {
bool opt_display = true;
bool opt_click_allowed = true;
bool opt_new_proj_operator = true;
if (getOptions(argc, argv, opt_click_allowed, opt_display, opt_new_proj_operator) == false) {
return (EXIT_FAILURE);
}
if (opt_display) {
#if defined(VISP_HAVE_DISPLAY)
#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
#else
#endif
#endif
}
#ifdef VISP_HAVE_DISPLAY
#endif
double px = 600, py = 600;
cMo[0][3] = 0.1;
cMo[1][3] = 0.2;
cMo[2][3] = 2;
wMo = wMc * cMo;
cMod[0][3] = 0;
cMod[1][3] = 0;
cMod[2][3] = 1;
#ifdef VISP_HAVE_DISPLAY
#endif
std::cout << std::endl;
#ifdef VISP_HAVE_DISPLAY
#endif
if (opt_display && opt_click_allowed) {
}
unsigned int iter = 0;
bool stop = false;
bool start_secondary_task = false;
while (iter++ < 2000 && !stop) {
std::cout << "---------------------------------------------" << iter << std::endl;
robot.getPosition(wMc);
if (opt_display) {
#ifdef VISP_HAVE_DISPLAY
#endif
}
start_secondary_task = true;
}
if (start_secondary_task) {
de2dt[0] = 0.50;
de2dt[1] = 0.25;
de2dt[2] = 1;
std::cout << "de2dt :" << de2dt.t() << std::endl;
std::cout <<
"(I-WpW)de2dt :" << sec.
t() << std::endl;
v += sec;
if (opt_display && opt_click_allowed) {
std::stringstream ss;
ss << std::string("New projection operator: ") +
(opt_new_proj_operator ? std::string("yes (use option -o to use old one)") : std::string("no"));
}
}
else {
if (opt_display && opt_click_allowed) {
}
}
std::cout <<
"|| s - s* || = " << (task.
getError()).sumSquare() << std::endl;
if (opt_display) {
stop = true;
}
}
}
if (opt_display && opt_click_allowed) {
}
#if (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11)
if (displayI != nullptr) {
delete displayI;
}
if (displayExt != nullptr) {
delete displayExt;
}
#endif
return EXIT_SUCCESS;
}
std::cout << "Catch a ViSP exception: " << e << std::endl;
#if (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11)
if (displayI != nullptr) {
delete displayI;
}
if (displayExt != nullptr) {
delete displayExt;
}
#endif
return EXIT_FAILURE;
}
#else
(void)argc;
(void)argv;
std::cout << "Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
return EXIT_SUCCESS;
#endif
}
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 double rad(double deg)
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
interface with the image for feature display
void insert(vpForwardProjection &fp)
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 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
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.