48 #define PRINT_CONDITION_NUMBER
50 #include <visp/vpDebug.h>
51 #include <visp/vpConfig.h>
53 #include <visp/vpHomogeneousMatrix.h>
54 #include <visp/vpMomentObject.h>
55 #include <visp/vpMomentDatabase.h>
56 #include <visp/vpMomentCommon.h>
57 #include <visp/vpFeatureMomentCommon.h>
58 #include <visp/vpDisplayX.h>
59 #include <visp/vpDisplayGTK.h>
60 #include <visp/vpDisplayGDI.h>
61 #include <visp/vpDisplayOpenCV.h>
62 #include <visp/vpCameraParameters.h>
63 #include <visp/vpIoTools.h>
64 #include <visp/vpMath.h>
65 #include <visp/vpHomogeneousMatrix.h>
66 #include <visp/vpServo.h>
67 #include <visp/vpDebug.h>
68 #include <visp/vpFeatureBuilder.h>
69 #include <visp/vpFeaturePoint.h>
70 #include <visp/vpRobotCamera.h>
71 #include <visp/vpImageSimulator.h>
72 #include <visp/vpPlane.h>
73 #include <visp/vpPoseVector.h>
74 #include <visp/vpPlot.h>
76 #if !defined(_WIN32) && !defined(VISP_HAVE_PTHREAD)
80 std::cout <<
"Can't run this example since vpSimulatorAfma6 capability is not available." << std::endl;
81 std::cout <<
"You should install pthread third-party library." << std::endl;
84 #elif !defined(VISP_HAVE_X11) && !defined(VISP_HAVE_OPENCV) && !defined(VISP_HAVE_GDI) && !defined(VISP_HAVE_D3D9) && !defined(VISP_HAVE_GTK)
87 std::cout <<
"Can't run this example since no display capability is available." << std::endl;
88 std::cout <<
"You should install one of the following third-party library: X11, OpenCV, GDI, GTK." << std::endl;
103 void execute(
unsigned int nbIter);
106 void planeToABC(
vpPlane& pl,
double& A,
double& B,
double& C);
109 void init_visp_plot(
vpPlot& );
126 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
177 for (
int i = 0; i < 4; i++) X[i].resize(3);
199 imsim_start.
init(tmp_start_img, X);
201 imsim_start.
getImage(start_img,cam);
204 imsim.
init(tmp_img, X);
234 displayInt.
init(Iint,700,0,
"Visual servoing with moments") ;
245 double A;
double B;
double C;
246 double Ad;
double Bd;
double Cd;
253 planeToABC(pl,A,B,C);
257 planeToABC(pl,Ad,Bd,Cd);
289 void execute(
unsigned int nbIter){
292 init_visp_plot(ViSP_plot);
299 vpTRACE(
"Display task information " ) ;
307 float sampling_time = 0.010f;
315 while(iter++<nbIter ){
327 planeToABC(pl,A,B,C);
351 err_features = task.
error;
357 ViSP_plot.
plot(0,iter, v);
358 ViSP_plot.
plot(1,iter,currentpose);
359 ViSP_plot.
plot(2, iter,err_features);
361 _error = ( task.
getError() ).sumSquare();
363 #if defined(PRINT_CONDITION_NUMBER)
370 Linteraction.
svd(singularvals, tmpry);
372 std::cout<<
"Condition Number: "<<condno<<std::endl;
379 vpTRACE(
"\n\nClick in the internal view window to end...");
384 delete featureMoments;
385 delete featureMomentsDes;
389 double error(){
return _error;}
393 void planeToABC(
vpPlane& pl,
double& A,
double& B,
double& C){
394 if(fabs(pl.
getD())<std::numeric_limits<double>::epsilon()){
395 std::cout <<
"Invalid position:" << std::endl;
396 std::cout << cMo << std::endl;
397 std::cout <<
"Cannot put plane in the form 1/Z=Ax+By+C." << std::endl;
410 init_visp_plot(
vpPlot& ViSP_plot) {
415 const unsigned int NbGraphs = 3;
416 const unsigned int NbCurves_in_graph[NbGraphs] = {6,6,6};
418 ViSP_plot.
init(NbGraphs , 800, 800, 10, 10,
"Visual Servoing results...");
426 for (
unsigned int p = 0; p<NbGraphs; p++) {
427 ViSP_plot.
initGraph(p,NbCurves_in_graph[p]);
428 for (
unsigned int c = 0; c<NbCurves_in_graph[p]; c++)
432 ViSP_plot.
setTitle(0,
"Robot velocities");
440 ViSP_plot.
setTitle(1,
"Camera pose cMo");
448 ViSP_plot.
setTitle(2,
"Error in visual features: ");
Definition of the vpMatrix class.
void init(const vpImage< unsigned char > &I, vpColVector *X)
void setColor(const unsigned int graphNum, const unsigned int curveNum, vpColor color)
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
void setPosition(const vpHomogeneousMatrix &cMw)
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.
double getMinValue() const
Define the X11 console to display images.
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.
void init(vpImage< unsigned char > &I, int winx=-1, int winy=-1, const char *title=NULL)
void setLegend(const unsigned int graphNum, const unsigned int curveNum, const char *legend)
void plot(const unsigned int graphNum, const unsigned int curveNum, const double x, const double y)
static double measureTimeMs()
void setTitle(const unsigned int graphNum, const char *title)
static int wait(double t0, double t)
static const vpColor green
static void flush(const vpImage< unsigned char > &I)
Class that defines a RGB 32 bits structure.
vpPoseVector buildFrom(const vpHomogeneousMatrix &M)
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.
void setCameraPosition(const vpHomogeneousMatrix &cMt)
vpColVector getError() const
static const vpColor cyan
void svd(vpColVector &w, vpMatrix &v)
vpColVector computeControlLaw()
vpFeatureMomentAlpha & getFeatureAlpha()
void updateAll(double A, double B, double C)
void setInterpolationType(const vpInterpolationType interplt)
void changeFrame(const vpHomogeneousMatrix &cMo)
Class that defines the simplest robot: a free flying camera.
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.
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+ library version 1.2.
void extract(vpRotationMatrix &R) const
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 char *title=NULL)
vpServoIteractionMatrixType
static double getSurface(vpMomentObject &object)
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
vpFeatureMomentCInvariant & getFeatureCInvariant()
static double rad(double deg)
void updateAll(vpMomentObject &object)
void initGraph(unsigned int graphNum, unsigned int curveNbr)
void getPosition(vpHomogeneousMatrix &cMw) const
This class initializes and allows access to commonly used moments.
static double getAlpha(vpMomentObject &object)
double getMaxValue() const
Class that provides a data structure for the column vectors as well as a set of operations on these v...
The pose is a complete representation of every rigid motion in the euclidian space.
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...
virtual bool getClick(bool blocking=true)=0
This class defines the container for a plane geometrical structure.
static const vpColor purple
vpFeatureMomentAreaNormalized & getFeatureAn()
void setServo(const vpServoType &servo_type)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &v)
Class that consider the case of a translation vector.
static const vpColor blue