45 #define PRINT_CONDITION_NUMBER 48 #include <visp3/core/vpCameraParameters.h> 49 #include <visp3/core/vpConfig.h> 50 #include <visp3/core/vpDebug.h> 51 #include <visp3/core/vpHomogeneousMatrix.h> 52 #include <visp3/core/vpIoTools.h> 53 #include <visp3/core/vpMath.h> 54 #include <visp3/core/vpMomentCommon.h> 55 #include <visp3/core/vpMomentDatabase.h> 56 #include <visp3/core/vpMomentObject.h> 57 #include <visp3/core/vpPlane.h> 58 #include <visp3/core/vpPoseVector.h> 59 #include <visp3/gui/vpDisplayGDI.h> 60 #include <visp3/gui/vpDisplayGTK.h> 61 #include <visp3/gui/vpDisplayOpenCV.h> 62 #include <visp3/gui/vpDisplayX.h> 63 #include <visp3/gui/vpPlot.h> 64 #include <visp3/robot/vpImageSimulator.h> 65 #include <visp3/robot/vpSimulatorCamera.h> 66 #include <visp3/visual_features/vpFeatureBuilder.h> 67 #include <visp3/visual_features/vpFeatureMomentCommon.h> 68 #include <visp3/visual_features/vpFeaturePoint.h> 69 #include <visp3/vs/vpServo.h> 71 #if !defined(_WIN32) && !defined(VISP_HAVE_PTHREAD) 75 std::cout <<
"Can't run this example since vpSimulatorAfma6 capability is " 78 std::cout <<
"You should install pthread third-party library." << std::endl;
81 #elif !defined(VISP_HAVE_X11) && !defined(VISP_HAVE_OPENCV) && !defined(VISP_HAVE_GDI) && !defined(VISP_HAVE_D3D9) && \ 82 !defined(VISP_HAVE_GTK) 85 std::cout <<
"Can't run this example since no display capability is available." << std::endl;
86 std::cout <<
"You should install one of the following third-party library: " 87 "X11, OpenCV, GDI, GTK." 103 void execute(
unsigned int nbIter);
106 void planeToABC(
vpPlane &pl,
double &A,
double &B,
double &C);
109 void init_visp_plot(
vpPlot &);
125 std::cout <<
"Catch an exception: " << e << std::endl;
131 #if defined VISP_HAVE_X11 133 #elif defined VISP_HAVE_OPENCV 135 #elif defined VISP_HAVE_GDI 137 #elif defined VISP_HAVE_D3D9 139 #elif defined VISP_HAVE_GTK 177 for (
int i = 0; i < 4; i++)
200 imsim_start.
init(tmp_start_img, X);
202 imsim_start.
getImage(start_img, cam);
205 imsim.
init(tmp_img, X);
234 displayInt.
init(Iint, 700, 0,
"Visual servoing with moments");
258 planeToABC(pl, A, B, C);
262 planeToABC(pl, Ad, Bd, Cd);
284 featureMomentsDes->
updateAll(Ad, Bd, Cd);
294 (1 << 10) | (1 << 11));
300 void execute(
unsigned int nbIter)
304 init_visp_plot(ViSP_plot);
311 vpTRACE(
"Display task information ");
316 unsigned int iter = 0;
322 float sampling_time = 0.010f;
330 while (iter++ < nbIter) {
343 planeToABC(pl, A, B, C);
368 err_features = task.
error;
369 std::cout <<
" || s - s* || = " << task.
error.
sumSquare() << std::endl;
374 ViSP_plot.
plot(0, iter, v);
375 ViSP_plot.
plot(1, iter, currentpose);
376 ViSP_plot.
plot(2, iter, err_features);
378 _error = (task.
getError()).sumSquare();
380 #if defined(PRINT_CONDITION_NUMBER) 387 Linteraction.
svd(singularvals, tmpry);
389 std::cout <<
"Condition Number: " << condno << std::endl;
395 vpTRACE(
"\n\nClick in the internal view window to end...");
400 delete featureMoments;
401 delete featureMomentsDes;
405 double error() {
return _error; }
407 void planeToABC(
vpPlane &pl,
double &A,
double &B,
double &C)
409 if (fabs(pl.
getD()) < std::numeric_limits<double>::epsilon()) {
410 std::cout <<
"Invalid position:" << std::endl;
411 std::cout << cMo << std::endl;
412 std::cout <<
"Cannot put plane in the form 1/Z=Ax+By+C." << std::endl;
422 void init_visp_plot(
vpPlot &ViSP_plot)
428 const unsigned int NbGraphs = 3;
429 const unsigned int NbCurves_in_graph[NbGraphs] = {6, 6, 6};
431 ViSP_plot.
init(NbGraphs, 800, 800, 10, 10,
"Visual Servoing results...");
436 for (
unsigned int p = 0; p < NbGraphs; p++) {
437 ViSP_plot.
initGraph(p, NbCurves_in_graph[p]);
438 for (
unsigned int c = 0; c < NbCurves_in_graph[p]; c++)
439 ViSP_plot.
setColor(p, c, Colors[c]);
442 ViSP_plot.
setTitle(0,
"Robot velocities");
450 ViSP_plot.
setTitle(1,
"Camera pose cMo");
458 ViSP_plot.
setTitle(2,
"Error in visual features: ");
void setPosition(const vpHomogeneousMatrix &wMc)
void svd(vpColVector &w, vpMatrix &V)
Implementation of a matrix and operations on matrices.
VISP_EXPORT int wait(double t0, double t)
void init(const vpImage< unsigned char > &I, vpColVector *X)
void setColor(const unsigned int graphNum, const unsigned int curveNum, vpColor color)
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
Implementation of an homogeneous matrix and operations on such kind of matrices.
Class that defines the simplest robot: a free flying camera.
void getImage(vpImage< unsigned char > &I, const vpCameraParameters &cam)
Display for windows using GDI (available on any windows 32 platform).
Class to define colors available for display functionnalities.
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, const unsigned int select=vpBasicFeature::FEATURE_ALL)
This class allows to access common vpFeatureMoments in a pre-filled database.
error that can be emited by ViSP classes.
void setABCD(const double a, const double b, const double c, const double d)
Class for generic objects.
vpHomogeneousMatrix inverse() const
void plot(const unsigned int graphNum, const unsigned int curveNum, const double x, const double y)
vpHomogeneousMatrix getPosition() const
void extract(vpRotationMatrix &R) const
static const vpColor green
static void flush(const vpImage< unsigned char > &I)
void setTitle(const unsigned int graphNum, const std::string &title)
VISP_EXPORT double measureTimeMs()
void fromImage(const vpImage< unsigned char > &image, unsigned char threshold, const vpCameraParameters &cam)
static const vpColor orange
virtual void setSamplingTime(const double &delta_t)
Display for windows using Direct3D 3rd party. Thus to enable this class Direct3D should be installed...
void setCameraPosition(const vpHomogeneousMatrix &cMt)
static const vpColor cyan
vpColVector computeControlLaw()
vpFeatureMomentAlpha & getFeatureAlpha()
void updateAll(double A, double B, double C)
void setInterpolationType(const vpInterpolationType interplt)
void changeFrame(const vpHomogeneousMatrix &cMo)
static std::vector< double > getMu3(vpMomentObject &object)
vpMatrix L
Interaction matrix.
static void display(const vpImage< unsigned char > &I)
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
Generic class defining intrinsic camera parameters.
Class which enables to project an image in the 3D space and get the view of a virtual camera...
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
void init(const unsigned int nbGraph, const unsigned int height=700, const unsigned int width=700, const int x=-1, const int y=-1, const std::string &title="")
vpServoIteractionMatrixType
static double getSurface(vpMomentObject &object)
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
vpFeatureMomentCInvariant & getFeatureCInvariant()
static double rad(double deg)
void init(vpImage< unsigned char > &I, int winx=-1, int winy=-1, const std::string &title="")
void setLegend(const unsigned int graphNum, const unsigned int curveNum, const std::string &legend)
void updateAll(vpMomentObject &object)
void initGraph(unsigned int graphNum, unsigned int curveNbr)
This class initializes and allows access to commonly used moments.
static double getAlpha(vpMomentObject &object)
Implementation of column vector and the associated operations.
Implementation of a pose vector and operations on poses.
vpFeatureMomentGravityCenterNormalized & getFeatureGravityNormalized()
void setType(vpObjectType input_type)
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
This class enables real time drawing of 2D or 3D graphics. An instance of the class open a window whi...
vpColVector getError() const
vpPoseVector buildFrom(const double tx, const double ty, const double tz, const double tux, const double tuy, const double tuz)
This class defines the container for a plane geometrical structure.
static const vpColor purple
vpFeatureMomentAreaNormalized & getFeatureAn()
void setServo(const vpServoType &servo_type)
Class that consider the case of a translation vector.
static const vpColor blue