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>
80 void execute(
unsigned int nbIter);
83 void _planeToABC(
vpPlane& pl,
double& A,
double& B,
double& C);
86 #if !defined(WIN32) && !defined(VISP_HAVE_PTHREAD)
90 std::cout <<
"Can't run this example since vpSimulatorAfma6 capability is not available." << std::endl;
91 std::cout <<
"You should install pthread third-party library." << std::endl;
94 #elif !defined(VISP_HAVE_X11) && !defined(VISP_HAVE_OPENCV) && !defined(VISP_HAVE_GDI) && !defined(VISP_HAVE_D3D9) && !defined(VISP_HAVE_GTK)
97 std::cout <<
"Can't run this example since no display capability is available." << std::endl;
98 std::cout <<
"You should install one of the following third-party library: X11, OpenCV, GDI, GTK." << std::endl;
114 #if defined VISP_HAVE_X11
116 #elif defined VISP_HAVE_OPENCV
118 #elif defined VISP_HAVE_GDI
120 #elif defined VISP_HAVE_D3D9
122 #elif defined VISP_HAVE_GTK
151 vector<vpPoint> src_pts;
152 vector<vpPoint> dst_pts;
154 double x[5] = { 0.2, 0.2,-0.2,-0.2, 0.2 };
155 double y[5] = {-0.1, 0.1, 0.1,-0.1,-0.1 };
158 for (
int i = 0 ; i < nbpoints ; i++){
162 src_pts.push_back(p);
167 for (
int i = 0 ; i < nbpoints ; i++){
171 dst_pts.push_back(p);
179 double x[5] = { 0.2, 0.2,-0.2,-0.2, 0.2 };
180 double y[5] = {-0.1, 0.1, 0.1,-0.1,-0.1 };
182 vector<vpPoint> cur_pts;
184 for (
int i = 0 ; i < nbpoints ; i++){
188 cur_pts.push_back(p);
198 displayInt.
init(Iint,700,0,
"Visual servoing with moments") ;
209 double A;
double B;
double C;
210 double Ad;
double Bd;
double Cd;
217 _planeToABC(pl,A,B,C);
221 _planeToABC(pl,Ad,Bd,Cd);
253 void execute(
unsigned int nbIter){
259 vpTRACE(
"Display task information " ) ;
263 robot.getInternalView(Iint);
268 while(iter++<nbIter ){
272 cMo = robot.get_cMo();
278 _planeToABC(pl,A,B,C);
288 robot.getInternalView(Iint);
299 _error = ( task.
getError() ).sumSquare();
304 vpTRACE(
"\n\nClick in the internal view window to end...");
309 delete featureMoments;
310 delete featureMomentsDes;
314 double error(){
return _error;}
336 void _planeToABC(
vpPlane& pl,
double& A,
double& B,
double& C){
337 if(fabs(pl.
getD())<std::numeric_limits<double>::epsilon()){
338 std::cout <<
"Invalid position:" << std::endl;
339 std::cout << cMo << std::endl;
340 std::cout <<
"Cannot put plane in the form 1/Z=Ax+By+C." << std::endl;
354 removeJointLimits(robot);
The object displayed at the desired position is the same than the scene object defined in vpSceneObje...
void setDesiredCameraPosition(const vpHomogeneousMatrix cdMo)
void initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desiredObject)
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)
static double getAlpha(vpMomentObject &objec)
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)
create a new ste of two visual features
This class allows to access common vpFeatureMoments in a pre-filled database.
void setLambda(double _lambda)
set the gain lambda
Initialize the position controller.
error that can be emited by ViSP classes.
void setJointLimit(const vpColVector &limitMin, const vpColVector &limitMax)
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)
void setType(vpObjectType type)
void setABCD(const double A, const double B, const double C, const double D)
static void flush(const vpImage< unsigned char > &I)
vpRobot::vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Class that defines what is a point.
Display for windows using Direct3D.
void kill()
destruction (memory deallocation if required)
vpColVector getError() const
vpColVector computeControlLaw()
compute the desired control law
vpFeatureMomentAlpha & getFeatureAlpha()
void updateAll(double A, double B, double C)
void initialiseObjectRelativeToCamera(const vpHomogeneousMatrix &cMo)
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
void init(vpAfma6::vpAfma6ToolType tool, vpCameraParameters::vpCameraParametersProjType projModel=vpCameraParameters::perspectiveProjWithoutDistortion)
vpServoIteractionMatrixType
void setCurrentViewColor(const vpColor col)
void fromVector(std::vector< vpPoint > &points)
static double getSurface(vpMomentObject &object)
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Set the type of the interaction matrix (current, mean, desired, user).
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.
Class that provides a data structure for the column vectors as well as a set of operations on these v...
vpFeatureMomentGravityCenterNormalized & getFeatureGravityNormalized()
void setDesiredViewColor(const vpColor col)
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
void getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height)
virtual bool getClick(bool blocking=true)=0
This class defines the container for a plane geometrical structure.
Class required to compute the visual servoing control law descbribed in and .
vpFeatureMomentAreaNormalized & getFeatureAn()
Class that consider the case of a translation vector.
void setServo(vpServoType _servo_type)
Choice of the visual servoing control law.
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...