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: ");