47 #include <visp/vpDebug.h>
48 #include <visp/vpConfig.h>
50 #include <visp/vpHomogeneousMatrix.h>
51 #include <visp/vpMomentObject.h>
52 #include <visp/vpMomentDatabase.h>
53 #include <visp/vpMomentCommon.h>
54 #include <visp/vpFeatureMomentCommon.h>
55 #include <visp/vpDisplayX.h>
56 #include <visp/vpDisplayGTK.h>
57 #include <visp/vpDisplayGDI.h>
58 #include <visp/vpCameraParameters.h>
59 #include <visp/vpIoTools.h>
60 #include <visp/vpMath.h>
61 #include <visp/vpHomogeneousMatrix.h>
62 #include <visp/vpServo.h>
63 #include <visp/vpDebug.h>
64 #include <visp/vpFeatureBuilder.h>
65 #include <visp/vpFeaturePoint.h>
66 #include <visp/vpSimulatorAfma6.h>
67 #include <visp/vpPlane.h>
69 #if !defined(_WIN32) && !defined(VISP_HAVE_PTHREAD)
73 std::cout <<
"Can't run this example since vpSimulatorAfma6 capability is not available." << std::endl;
74 std::cout <<
"You should install pthread third-party library." << std::endl;
77 #elif !defined(VISP_HAVE_X11) && !defined(VISP_HAVE_OPENCV) && !defined(VISP_HAVE_GDI) && !defined(VISP_HAVE_D3D9) && !defined(VISP_HAVE_GTK)
80 std::cout <<
"Can't run this example since no display capability is available." << std::endl;
81 std::cout <<
"You should install one of the following third-party library: X11, OpenCV, GDI, GTK." << std::endl;
96 void execute(
unsigned int nbIter);
99 void planeToABC(
vpPlane& pl,
double& A,
double& B,
double& C);
116 std::cout <<
"Catch an exception: " << e << std::endl;
122 #if defined VISP_HAVE_X11
124 #elif defined VISP_HAVE_OPENCV
126 #elif defined VISP_HAVE_GDI
128 #elif defined VISP_HAVE_D3D9
130 #elif defined VISP_HAVE_GTK
159 vector<vpPoint> src_pts;
160 vector<vpPoint> dst_pts;
162 double x[5] = { 0.2, 0.2,-0.2,-0.2, 0.2 };
163 double y[5] = {-0.1, 0.1, 0.1,-0.1,-0.1 };
166 for (
int i = 0 ; i < nbpoints ; i++){
170 src_pts.push_back(p);
175 for (
int i = 0 ; i < nbpoints ; i++){
179 dst_pts.push_back(p);
187 double x[5] = { 0.2, 0.2,-0.2,-0.2, 0.2 };
188 double y[5] = {-0.1, 0.1, 0.1,-0.1,-0.1 };
190 vector<vpPoint> cur_pts;
192 for (
int i = 0 ; i < nbpoints ; i++){
196 cur_pts.push_back(p);
206 displayInt.
init(Iint,700,0,
"Visual servoing with moments") ;
217 double A;
double B;
double C;
218 double Ad;
double Bd;
double Cd;
225 planeToABC(pl,A,B,C);
229 planeToABC(pl,Ad,Bd,Cd);
261 void execute(
unsigned int nbIter){
267 vpTRACE(
"Display task information " ) ;
271 robot.getInternalView(Iint);
276 while(iter++<nbIter ){
280 cMo = robot.get_cMo();
286 planeToABC(pl,A,B,C);
296 robot.getInternalView(Iint);
307 _error = ( task.
getError() ).sumSquare();
312 vpTRACE(
"\n\nClick in the internal view window to end...");
317 delete featureMoments;
318 delete featureMomentsDes;
322 double error(){
return _error;}
345 void planeToABC(
vpPlane& pl,
double& A,
double& B,
double& C){
346 if(fabs(pl.
getD())<std::numeric_limits<double>::epsilon()){
347 std::cout <<
"Invalid position:" << std::endl;
348 std::cout << cMo << std::endl;
349 std::cout <<
"Cannot put plane in the form 1/Z=Ax+By+C." << std::endl;
360 robot.setCurrentViewColor(
vpColor(150,150,150));
361 robot.setDesiredViewColor(
vpColor(200,200,200));
363 removeJointLimits(robot);
366 robot.initialiseObjectRelativeToCamera(cMo);
369 robot.setDesiredCameraPosition(cdMo);
370 robot.getCameraParameters(cam,Iint);
The object displayed at the desired position is the same than the scene object defined in vpSceneObje...
Perspective projection without distortion model.
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
void setPosition(const vpHomogeneousMatrix &cMw)
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.
Initialize the position controller.
error that can be emited by ViSP classes.
void setJointLimit(const vpColVector &limitMin, const vpColVector &limitMax)
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 track(const vpHomogeneousMatrix &cMo)
static double measureTimeMs()
static int wait(double t0, double t)
static void flush(const vpImage< unsigned char > &I)
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Class that defines what is a point.
Display for windows using Direct3D.
vpColVector getError() const
vpColVector computeControlLaw()
vpFeatureMomentAlpha & getFeatureAlpha()
void updateAll(double A, double B, double C)
void changeFrame(const vpHomogeneousMatrix &cMo)
static std::vector< double > getMu3(vpMomentObject &object)
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...
Simulator of Irisa's gantry robot named Afma6.
The vpDisplayGTK allows to display image using the GTK+ library version 1.2.
void extract(vpRotationMatrix &R) const
vpServoIteractionMatrixType
void fromVector(std::vector< vpPoint > &points)
static double getSurface(vpMomentObject &object)
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
vpFeatureMomentCInvariant & getFeatureCInvariant()
static double rad(double deg)
A 40cm by 40cm plate with 4 points at coordinates (-0.07,-0.05,0), (0.07,0.05,0), (0...
void updateAll(vpMomentObject &object)
This class initializes and allows access to commonly used moments.
static double getAlpha(vpMomentObject &object)
Class that provides a data structure for the column vectors as well as a set of operations on these v...
vpFeatureMomentGravityCenterNormalized & getFeatureGravityNormalized()
void setType(vpObjectType input_type)
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
virtual bool getClick(bool blocking=true)=0
This class defines the container for a plane geometrical structure.
vpFeatureMomentAreaNormalized & getFeatureAn()
void setServo(const vpServoType &servo_type)
Class that consider the case of a translation vector.
void setWorldCoordinates(const double ox, const double oy, const double oz)
Set the point world coordinates. We mean here the coordinates of the point in the object frame...