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)
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
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. 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