 |
Visual Servoing Platform
version 3.2.0
|
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;
82 #elif !defined(VISP_HAVE_X11) && !defined(VISP_HAVE_OPENCV) && !defined(VISP_HAVE_GDI) && !defined(VISP_HAVE_D3D9) && \
83 !defined(VISP_HAVE_GTK)
86 std::cout <<
"Can't run this example since no display capability is available." << std::endl;
87 std::cout <<
"You should install one of the following third-party library: "
88 "X11, OpenCV, GDI, GTK."
105 void execute(
unsigned int nbIter);
108 void planeToABC(
vpPlane &pl,
double &A,
double &B,
double &C);
111 void init_visp_plot(
vpPlot &);
127 std::cout <<
"Catch an exception: " << e << std::endl;
133 #if defined VISP_HAVE_X11
135 #elif defined VISP_HAVE_OPENCV
137 #elif defined VISP_HAVE_GDI
139 #elif defined VISP_HAVE_D3D9
141 #elif defined VISP_HAVE_GTK
179 for (
int i = 0; i < 4; i++)
202 imsim_start.
init(tmp_start_img, X);
204 imsim_start.
getImage(start_img, cam);
207 imsim.
init(tmp_img, X);
236 displayInt.
init(Iint, 700, 0,
"Visual servoing with moments");
260 planeToABC(pl, A, B, C);
264 planeToABC(pl, Ad, Bd, Cd);
286 featureMomentsDes->
updateAll(Ad, Bd, Cd);
296 (1 << 10) | (1 << 11));
302 void execute(
unsigned int nbIter)
306 init_visp_plot(ViSP_plot);
313 vpTRACE(
"Display task information ");
318 unsigned int iter = 0;
324 float sampling_time = 0.010f;
332 while (iter++ < nbIter) {
345 planeToABC(pl, A, B, C);
370 err_features = task.
error;
371 std::cout <<
" || s - s* || = " << task.
error.
sumSquare() << std::endl;
376 ViSP_plot.
plot(0, iter, v);
377 ViSP_plot.
plot(1, iter, currentpose);
378 ViSP_plot.
plot(2, iter, err_features);
380 _error = (task.
getError()).sumSquare();
382 #if defined(PRINT_CONDITION_NUMBER)
389 Linteraction.
svd(singularvals, tmpry);
391 std::cout <<
"Condition Number: " << condno << std::endl;
397 vpTRACE(
"\n\nClick in the internal view window to end...");
402 delete featureMoments;
403 delete featureMomentsDes;
407 double error() {
return _error; }
409 void planeToABC(
vpPlane &pl,
double &A,
double &B,
double &C)
411 if (fabs(pl.
getD()) < std::numeric_limits<double>::epsilon()) {
412 std::cout <<
"Invalid position:" << std::endl;
413 std::cout << cMo << std::endl;
414 std::cout <<
"Cannot put plane in the form 1/Z=Ax+By+C." << std::endl;
424 void init_visp_plot(
vpPlot &ViSP_plot)
430 const unsigned int NbGraphs = 3;
431 const unsigned int NbCurves_in_graph[NbGraphs] = {6, 6, 6};
433 ViSP_plot.
init(NbGraphs, 800, 800, 10, 10,
"Visual Servoing results...");
438 for (
unsigned int p = 0; p < NbGraphs; p++) {
439 ViSP_plot.
initGraph(p, NbCurves_in_graph[p]);
440 for (
unsigned int c = 0; c < NbCurves_in_graph[p]; c++)
441 ViSP_plot.
setColor(p, c, Colors[c]);
444 ViSP_plot.
setTitle(0,
"Robot velocities");
452 ViSP_plot.
setTitle(1,
"Camera pose cMo");
460 ViSP_plot.
setTitle(2,
"Error in visual features: ");
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
This class allows to access common vpFeatureMoments in a pre-filled database.
VISP_EXPORT int wait(double t0, double t)
void plot(const unsigned int graphNum, const unsigned int curveNum, const double x, const double y)
void setType(vpObjectType input_type)
static const vpColor orange
void setInterpolationType(const vpInterpolationType interplt)
static double rad(double deg)
vpMatrix L
Interaction matrix.
Generic class defining intrinsic camera parameters.
void updateAll(vpMomentObject &object)
vpFeatureMomentAlpha & getFeatureAlpha()
void setCameraPosition(const vpHomogeneousMatrix &cMt)
This class initializes and allows access to commonly used moments.
void setPosition(const vpHomogeneousMatrix &wMc)
Display for windows using GDI (available on any windows 32 platform).
void init(const vpImage< unsigned char > &I, vpColVector *X)
vpPoseVector buildFrom(const double tx, const double ty, const double tz, const double tux, const double tuy, const double tuz)
Class that consider the case of a translation vector.
Class which enables to project an image in the 3D space and get the view of a virtual camera.
static const vpColor purple
void setABCD(const double a, const double b, const double c, const double d)
Display for windows using Direct3D 3rd party. Thus to enable this class Direct3D should be installed....
Class that defines the simplest robot: a free flying camera.
Implementation of column vector and the associated operations.
void extract(vpRotationMatrix &R) const
static const vpColor cyan
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
Implementation of a matrix and operations on matrices.
void setServo(const vpServoType &servo_type)
VISP_EXPORT double measureTimeMs()
vpHomogeneousMatrix getPosition() const
static const vpColor green
vpFeatureMomentGravityCenterNormalized & getFeatureGravityNormalized()
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
void fromImage(const vpImage< unsigned char > &image, unsigned char threshold, const vpCameraParameters &cam)
static void display(const vpImage< unsigned char > &I)
void setTitle(const unsigned int graphNum, const std::string &title)
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
Implementation of a pose vector and operations on poses.
vpColVector getError() const
void updateAll(double A, double B, double C)
static std::vector< double > getMu3(vpMomentObject &object)
void init(vpImage< unsigned char > &I, int winx=-1, int winy=-1, const std::string &title="")
Class for generic objects.
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="")
This class defines the container for a plane geometrical structure.
void changeFrame(const vpHomogeneousMatrix &cMo)
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
void getImage(vpImage< unsigned char > &I, const vpCameraParameters &cam)
static const vpColor blue
vpServoIteractionMatrixType
vpColVector computeControlLaw()
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, const unsigned int select=vpBasicFeature::FEATURE_ALL)
static void flush(const vpImage< unsigned char > &I)
virtual void setSamplingTime(const double &delta_t)
vpHomogeneousMatrix inverse() const
void initGraph(unsigned int graphNum, unsigned int curveNbr)
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
Class to define colors available for display functionnalities.
Implementation of an homogeneous matrix and operations on such kind of matrices.
void setLegend(const unsigned int graphNum, const unsigned int curveNum, const std::string &legend)
static double getSurface(vpMomentObject &object)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
void setColor(const unsigned int graphNum, const unsigned int curveNum, vpColor color)
vpFeatureMomentCInvariant & getFeatureCInvariant()
error that can be emited by ViSP classes.
This class enables real time drawing of 2D or 3D graphics. An instance of the class open a window whi...
void svd(vpColVector &w, vpMatrix &V)
static double getAlpha(vpMomentObject &object)
vpFeatureMomentAreaNormalized & getFeatureAn()