60 #include <visp3/core/vpConfig.h> 61 #include <visp3/core/vpDebug.h> 66 #if (defined (VISP_HAVE_AFMA6) && defined (VISP_HAVE_DC1394)) 70 #include <visp3/sensor/vp1394TwoGrabber.h> 71 #include <visp3/core/vpImage.h> 72 #include <visp3/core/vpImagePoint.h> 73 #include <visp3/core/vpMath.h> 74 #include <visp3/core/vpHomogeneousMatrix.h> 75 #include <visp3/visual_features/vpFeaturePoint.h> 76 #include <visp3/core/vpPoint.h> 77 #include <visp3/vs/vpServo.h> 78 #include <visp3/visual_features/vpFeatureBuilder.h> 79 #include <visp3/robot/vpRobotAfma6.h> 80 #include <visp3/core/vpException.h> 81 #include <visp3/vs/vpServoDisplay.h> 82 #include <visp3/blob/vpDot.h> 83 #include <visp3/vision/vpPose.h> 84 #include <visp3/io/vpImageIo.h> 85 #include <visp3/core/vpDisplay.h> 86 #include <visp3/gui/vpDisplayX.h> 87 #include <visp3/gui/vpDisplayOpenCV.h> 88 #include <visp3/gui/vpDisplayGTK.h> 111 vpDisplayX display(I,100,100,
"Current image") ;
112 #elif defined(VISP_HAVE_OPENCV) 114 #elif defined(VISP_HAVE_GTK) 121 std::cout << std::endl ;
122 std::cout <<
"-------------------------------------------------------" << std::endl ;
123 std::cout <<
" Test program for vpServo " <<std::endl ;
124 std::cout <<
" Eye-to-hand task control" << std::endl ;
125 std::cout <<
" Simulation " << std::endl ;
126 std::cout <<
" task : servo a point " << std::endl ;
127 std::cout <<
"-------------------------------------------------------" << std::endl ;
128 std::cout << std::endl ;
135 for (i=0 ; i < nbPoint ; i++)
162 for (i=0 ; i < nbPoint ; i++)
177 std::cout << cMo << std::endl ;
185 std::cout <<
" Learning 0/1 " <<std::endl ;
186 char name[FILENAME_MAX] ;
187 sprintf(name,
"cdMo.dat") ;
189 std::cin >> learning ;
193 vpTRACE(
"Save the location of the object in a file cdMo.dat") ;
194 std::ofstream f(name) ;
202 vpTRACE(
"Loading desired location from cdMo.dat") ;
203 std::ifstream f(
"cdMo.dat") ;
212 for (i=0 ; i < nbPoint ; i++)
227 vpTRACE(
"\t we want an eye-in-hand control law") ;
228 vpTRACE(
"\t robot is controlled in the camera frame") ;
233 for (i=0 ; i < nbPoint ; i++)
239 vpTRACE(
"Display task information " ) ;
246 double convergence_threshold = 0.00;
251 unsigned int iter=0 ;
259 oMcamrobot[0][3] = -0.05 ;
264 double lambda_av =0.1;
268 std::cout <<
"alpha 0.7" << std::endl;
270 std::cout <<
"beta 5" << std::endl;
272 std::list<vpImagePoint> Lcog ;
274 while(error > convergence_threshold)
276 std::cout <<
"---------------------------------------------" << iter++ <<std::endl ;
288 for (i=0 ; i < nbPoint ; i++)
291 Lcog.push_back( dot[i].getCog() );
296 vpTRACE(
"Error detected while tracking visual features") ;
304 for (i=0 ; i < nbPoint ; i++)
330 cMe = cMo *oMcamrobot * camrobotMe ;
344 if (std::fabs(alpha) <= std::numeric_limits<double>::epsilon())
348 gain = alpha * exp (-beta * ( task.
getError() ).sumSquare() ) + lambda_av;
351 else gain = lambda_av ;
355 vpTRACE(
"%f %f %f %f %f",alpha, beta, lambda_av, ( task.
getError() ).sumSquare(), gain) ;
362 for (std::list<vpImagePoint>::const_iterator it_cog = Lcog.begin(); it_cog != Lcog.end(); ++it_cog)
369 error = ( task.
getError() ).sumSquare() ;
370 std::cout <<
"|| s - s* || = "<< error<<std::endl ;
374 vpTRACE(
"Error detected while tracking visual features") ;
383 if ((SAVE==1) && (iter %3==0))
387 sprintf(name,
"/tmp/marchand/image.%04d.ppm",it++) ;
407 vpERROR_TRACE(
"You do not have an afma6 robot or a firewire framegrabber connected to your computer...");
Implementation of a matrix and operations on matrices.
void getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height) const
void projection(const vpColVector &_cP, vpColVector &_p)
Projection onto the image plane of a point. Input: the 3D coordinates in the camera frame _cP...
void get_eJe(vpMatrix &_eJe)
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
Implementation of an homogeneous matrix and operations on such kind of matrices.
void set_eJe(const vpMatrix &eJe_)
void display(const vpImage< unsigned char > &I, const vpCameraParameters &cam, const vpColor &color=vpColor::green, const unsigned int thickness=1)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
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)
void print() const
Print the matrix as a pose vector .
void track(const vpImage< unsigned char > &I)
void set_x(const double x)
Set the point x coordinate in the image plane.
static void displayPoint(const vpImage< unsigned char > &I, const vpImagePoint &ip, const vpColor &color, unsigned int thickness=1)
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
Point coordinates conversion from pixel coordinates to normalized coordinates in meter...
void get_cMe(vpHomogeneousMatrix &_cMe) const
static const vpColor green
void acquire(vpImage< unsigned char > &I)
static void flush(const vpImage< unsigned char > &I)
void set_y(const double y)
void load(std::ifstream &f)
Control of Irisa's gantry robot named Afma6.
Class that defines what is a point.
void set_x(const double x)
static void write(const vpImage< unsigned char > &I, const std::string &filename)
void open(vpImage< unsigned char > &I)
void set_i(const double ii)
vpImagePoint getCog() const
Initialize the velocity controller.
vpColVector computeControlLaw()
static void display(const vpImage< unsigned char > &I)
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
Generic class defining intrinsic camera parameters.
void set_y(const double y)
Set the point y coordinate in the image plane.
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(vpHomogeneousMatrix *)=NULL)
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
static void getImage(const vpImage< unsigned char > &Is, vpImage< vpRGBa > &Id)
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
void set_j(const double jj)
void setWorldCoordinates(const double oX, const double oY, const double oZ)
Implementation of column vector and the associated operations.
void setGraphics(const bool activate)
void set_cVe(const vpVelocityTwistMatrix &cVe_)
void setFramerate(vp1394TwoFramerateType fps)
void setVideoMode(vp1394TwoVideoModeType videomode)
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
This tracker is meant to track a dot (connected pixels with same gray level) on a vpImage...
vpColVector getError() const
Class for firewire ieee1394 video devices using libdc1394-2.x api.
void save(std::ofstream &f) const
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &_cP)
void addPoint(const vpPoint &P)
void setServo(const vpServoType &servo_type)
void initTracking(const vpImage< unsigned char > &I)
static void display(const vpServo &s, const vpCameraParameters &cam, const vpImage< unsigned char > &I, vpColor currentColor=vpColor::green, vpColor desiredColor=vpColor::red, unsigned int thickness=1)
static const vpColor blue