48 #define PRINT_CONDITION_NUMBER
50 #include <visp/vpDebug.h>
51 #include <visp/vpConfig.h>
53 #include <visp/vpHomogeneousMatrix.h>
54 #include <visp/vpMomentObject.h>
55 #include <visp/vpMomentDatabase.h>
56 #include <visp/vpMomentCommon.h>
57 #include <visp/vpFeatureMomentCommon.h>
58 #include <visp/vpDisplayX.h>
59 #include <visp/vpDisplayGTK.h>
60 #include <visp/vpDisplayGDI.h>
61 #include <visp/vpDisplayOpenCV.h>
62 #include <visp/vpCameraParameters.h>
63 #include <visp/vpIoTools.h>
64 #include <visp/vpMath.h>
65 #include <visp/vpHomogeneousMatrix.h>
66 #include <visp/vpServo.h>
67 #include <visp/vpDebug.h>
68 #include <visp/vpFeatureBuilder.h>
69 #include <visp/vpFeaturePoint.h>
70 #include <visp/vpRobotCamera.h>
71 #include <visp/vpImageSimulator.h>
72 #include <visp/vpPlane.h>
73 #include <visp/vpPoseVector.h>
74 #include <visp/vpPlot.h>
87 void execute(
unsigned int nbIter);
90 void _planeToABC(
vpPlane& pl,
double& A,
double& B,
double& C);
93 #if !defined(WIN32) && !defined(VISP_HAVE_PTHREAD)
97 std::cout <<
"Can't run this example since vpSimulatorAfma6 capability is not available." << std::endl;
98 std::cout <<
"You should install pthread third-party library." << std::endl;
101 #elif !defined(VISP_HAVE_X11) && !defined(VISP_HAVE_OPENCV) && !defined(VISP_HAVE_GDI) && !defined(VISP_HAVE_D3D9) && !defined(VISP_HAVE_GTK)
104 std::cout <<
"Can't run this example since no display capability is available." << std::endl;
105 std::cout <<
"You should install one of the following third-party library: X11, OpenCV, GDI, GTK." << std::endl;
108 void init_visp_plot(
vpPlot& );
126 #if defined VISP_HAVE_X11
128 #elif defined VISP_HAVE_OPENCV
130 #elif defined VISP_HAVE_GDI
132 #elif defined VISP_HAVE_D3D9
134 #elif defined VISP_HAVE_GTK
170 for (
int i = 0; i < 4; i++) X[i].resize(3);
192 imsim_start.
init(tmp_start_img, X);
194 imsim_start.
getImage(start_img,cam);
197 imsim.
init(tmp_img, X);
227 displayInt.
init(Iint,700,0,
"Visual servoing with moments") ;
238 double A;
double B;
double C;
239 double Ad;
double Bd;
double Cd;
246 _planeToABC(pl,A,B,C);
250 _planeToABC(pl,Ad,Bd,Cd);
282 void execute(
unsigned int nbIter){
285 init_visp_plot(ViSP_plot);
292 vpTRACE(
"Display task information " ) ;
300 float sampling_time = 0.010f;
308 while(iter++<nbIter ){
320 _planeToABC(pl,A,B,C);
344 err_features = task.
error;
350 ViSP_plot.
plot(0,iter, v);
351 ViSP_plot.
plot(1,iter,currentpose);
352 ViSP_plot.
plot(2, iter,err_features);
354 _error = ( task.
getError() ).sumSquare();
356 #if defined(PRINT_CONDITION_NUMBER)
363 Linteraction.
svd(singularvals, tmpry);
365 std::cout<<
"Condition Number: "<<condno<<std::endl;
372 vpTRACE(
"\n\nClick in the internal view window to end...");
377 delete featureMoments;
378 delete featureMomentsDes;
382 double error(){
return _error;}
386 void _planeToABC(
vpPlane& pl,
double& A,
double& B,
double& C){
387 if(fabs(pl.
getD())<std::numeric_limits<double>::epsilon()){
388 std::cout <<
"Invalid position:" << std::endl;
389 std::cout << cMo << std::endl;
390 std::cout <<
"Cannot put plane in the form 1/Z=Ax+By+C." << std::endl;
403 init_visp_plot(
vpPlot& ViSP_plot) {
408 const unsigned int NbGraphs = 3;
409 const unsigned int NbCurves_in_graph[NbGraphs] = {6,6,6};
411 ViSP_plot.
init(NbGraphs , 800, 800, 10, 10,
"Visual Servoing results...");
419 for (
unsigned int p = 0; p<NbGraphs; p++) {
420 ViSP_plot.
initGraph(p,NbCurves_in_graph[p]);
421 for (
unsigned int c = 0; c<NbCurves_in_graph[p]; c++)
425 ViSP_plot.
setTitle(0,
"Robot velocities");
433 ViSP_plot.
setTitle(1,
"Camera pose cMo");
441 ViSP_plot.
setTitle(2,
"Error in visual features: ");
Definition of the vpMatrix class.
void setColor(const unsigned int graphNum, const unsigned int curveNum, vpColor color)
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)
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.
double getMinValue() const
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
error that can be emited by ViSP classes.
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)
static double measureTimeMs()
void setTitle(const unsigned int graphNum, const char *title)
static int wait(double t0, double t)
double sumSquare() const
return sum of the Aij^2 (for all i, for all j)
static const vpColor green
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)
Class that defines a RGB 32 bits structure.
vpPoseVector buildFrom(const vpHomogeneousMatrix &M)
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 kill()
destruction (memory deallocation if required)
vpColVector getError() const
static const vpColor cyan
void svd(vpColVector &w, vpMatrix &v)
vpColVector computeControlLaw()
compute the desired control law
vpFeatureMomentAlpha & getFeatureAlpha()
void updateAll(double A, double B, double C)
void changeFrame(const vpHomogeneousMatrix &cMo)
Class that defines the simplest robot: a free flying camera.
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.
void init(const vpImage< unsigned char > &I, vpColVector *_X)
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
void setInterpolationType(const vpInterpolationType interp)
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)
Set the type of the interaction matrix (current, mean, desired, user).
vpFeatureMomentCInvariant & getFeatureCInvariant()
static double rad(double deg)
void updateAll(vpMomentObject &object)
void initGraph(unsigned int graphNum, unsigned int curveNbr)
void getPosition(vpHomogeneousMatrix &cMw) const
This class initializes and allows access to commonly used moments.
double getMaxValue() const
Class that provides a data structure for the column vectors as well as a set of operations on these v...
The pose is a complete representation of every rigid motion in the euclidian space.
vpFeatureMomentGravityCenterNormalized & getFeatureGravityNormalized()
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...
virtual bool getClick(bool blocking=true)=0
This class defines the container for a plane geometrical structure.
void setCameraPosition(const vpHomogeneousMatrix &_cMt)
Class required to compute the visual servoing control law descbribed in and .
static const vpColor purple
vpFeatureMomentAreaNormalized & getFeatureAn()
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &v)
Class that consider the case of a translation vector.
void setServo(vpServoType _servo_type)
Choice of the visual servoing control law.
static const vpColor blue