44 #define PRINT_CONDITION_NUMBER 46 #include <visp3/core/vpDebug.h> 47 #include <visp3/core/vpConfig.h> 49 #include <visp3/core/vpHomogeneousMatrix.h> 50 #include <visp3/core/vpMomentObject.h> 51 #include <visp3/core/vpMomentDatabase.h> 52 #include <visp3/core/vpMomentCommon.h> 53 #include <visp3/visual_features/vpFeatureMomentCommon.h> 54 #include <visp3/gui/vpDisplayX.h> 55 #include <visp3/gui/vpDisplayGTK.h> 56 #include <visp3/gui/vpDisplayGDI.h> 57 #include <visp3/gui/vpDisplayOpenCV.h> 58 #include <visp3/core/vpCameraParameters.h> 59 #include <visp3/core/vpIoTools.h> 60 #include <visp3/core/vpMath.h> 61 #include <visp3/core/vpHomogeneousMatrix.h> 62 #include <visp3/vs/vpServo.h> 63 #include <visp3/core/vpDebug.h> 64 #include <visp3/visual_features/vpFeatureBuilder.h> 65 #include <visp3/visual_features/vpFeaturePoint.h> 66 #include <visp3/robot/vpSimulatorCamera.h> 67 #include <visp3/robot/vpImageSimulator.h> 68 #include <visp3/core/vpPlane.h> 69 #include <visp3/core/vpPoseVector.h> 70 #include <visp3/gui/vpPlot.h> 72 #if !defined(_WIN32) && !defined(VISP_HAVE_PTHREAD) 76 std::cout <<
"Can't run this example since vpSimulatorAfma6 capability is not available." << std::endl;
77 std::cout <<
"You should install pthread third-party library." << std::endl;
80 #elif !defined(VISP_HAVE_X11) && !defined(VISP_HAVE_OPENCV) && !defined(VISP_HAVE_GDI) && !defined(VISP_HAVE_D3D9) && !defined(VISP_HAVE_GTK) 83 std::cout <<
"Can't run this example since no display capability is available." << std::endl;
84 std::cout <<
"You should install one of the following third-party library: X11, OpenCV, GDI, GTK." << std::endl;
99 void execute(
unsigned int nbIter);
102 void planeToABC(
vpPlane& pl,
double& A,
double& B,
double& C);
105 void init_visp_plot(
vpPlot& );
122 std::cout <<
"Catch an exception: " << e << std::endl;
129 #if defined VISP_HAVE_X11 131 #elif defined VISP_HAVE_OPENCV 133 #elif defined VISP_HAVE_GDI 135 #elif defined VISP_HAVE_D3D9 137 #elif defined VISP_HAVE_GTK 173 for (
int i = 0; i < 4; i++) X[i].resize(3);
195 imsim_start.
init(tmp_start_img, X);
197 imsim_start.
getImage(start_img,cam);
200 imsim.
init(tmp_img, X);
230 displayInt.
init(Iint,700,0,
"Visual servoing with moments") ;
241 double A;
double B;
double C;
242 double Ad;
double Bd;
double Cd;
249 planeToABC(pl,A,B,C);
253 planeToABC(pl,Ad,Bd,Cd);
285 void execute(
unsigned int nbIter){
288 init_visp_plot(ViSP_plot);
295 vpTRACE(
"Display task information " ) ;
307 float sampling_time = 0.010f;
315 while(iter++<nbIter ){
328 planeToABC(pl,A,B,C);
352 err_features = task.
error;
358 ViSP_plot.
plot(0,iter, v);
359 ViSP_plot.
plot(1,iter,currentpose);
360 ViSP_plot.
plot(2, iter,err_features);
362 _error = ( task.
getError() ).sumSquare();
364 #if defined(PRINT_CONDITION_NUMBER) 371 Linteraction.
svd(singularvals, tmpry);
373 std::cout<<
"Condition Number: "<<condno<<std::endl;
380 vpTRACE(
"\n\nClick in the internal view window to end...");
385 delete featureMoments;
386 delete featureMomentsDes;
390 double error(){
return _error;}
394 void planeToABC(
vpPlane& pl,
double& A,
double& B,
double& C){
395 if(fabs(pl.
getD())<std::numeric_limits<double>::epsilon()){
396 std::cout <<
"Invalid position:" << std::endl;
397 std::cout << cMo << std::endl;
398 std::cout <<
"Cannot put plane in the form 1/Z=Ax+By+C." << std::endl;
411 init_visp_plot(
vpPlot& ViSP_plot) {
416 const unsigned int NbGraphs = 3;
417 const unsigned int NbCurves_in_graph[NbGraphs] = {6,6,6};
419 ViSP_plot.
init(NbGraphs , 800, 800, 10, 10,
"Visual Servoing results...");
427 for (
unsigned int p = 0; p<NbGraphs; p++) {
428 ViSP_plot.
initGraph(p,NbCurves_in_graph[p]);
429 for (
unsigned int c = 0; c<NbCurves_in_graph[p]; c++)
433 ViSP_plot.
setTitle(0,
"Robot velocities");
441 ViSP_plot.
setTitle(1,
"Camera pose cMo");
449 ViSP_plot.
setTitle(2,
"Error in visual features: ");
void setPosition(const vpHomogeneousMatrix &wMc)
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)
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.
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)
void setTitle(const unsigned int graphNum, const char *title)
static const vpColor green
static void flush(const vpImage< unsigned char > &I)
Class that defines a RGB 32 bits structure.
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)
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
vpHomogeneousMatrix getPosition() 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)
VISP_EXPORT double measureTimeMs()
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.
vpHomogeneousMatrix inverse() const
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...
vpPoseVector buildFrom(const double tx, const double ty, const double tz, const double tux, const double tuy, const double tuz)
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)
Class that consider the case of a translation vector.
static const vpColor blue