4 #include <visp/vpCameraParameters.h>
5 #include <visp/vpDisplayX.h>
6 #include <visp/vpDot2.h>
7 #include <visp/vpFeatureBuilder.h>
8 #include <visp/vpFeatureDepth.h>
9 #include <visp/vpFeaturePoint.h>
10 #include <visp/vpHomogeneousMatrix.h>
11 #include <visp/vpImage.h>
12 #include <visp/vpImageConvert.h>
13 #include <visp/vpServo.h>
14 #include <visp/vpVelocityTwistMatrix.h>
19 #if defined( VISP_HAVE_DC1394_2 ) && defined( VISP_HAVE_X11 )
20 #define TEST_COULD_BE_ACHIEVED
43 #ifdef TEST_COULD_BE_ACHIEVED
45 main(
int argc,
char **argv )
49 vpImage< unsigned char > I;
52 double coef = 1. / 6.77;
56 robot.
init( argc, argv );
62 vpTime::sleepMs( 3000 );
64 std::cout <<
"Robot connected" << std::endl;
67 vpCameraParameters cam;
77 cam.initPersProjWithoutDistortion( 600, 600, I.getWidth() / 2, I.getHeight() / 2 );
82 vpDisplayX
d( I, 10, 10,
"Current frame" );
83 vpDisplay::display( I );
84 vpDisplay::flush( I );
88 dot.setGraphics(
true );
89 dot.setComputeMoments(
true );
90 dot.setEllipsoidShapePrecision( 0. );
91 dot.setGrayLevelPrecision( 0.9 );
92 dot.setEllipsoidBadPointsPercentage( 0.5 );
93 dot.initTracking( I );
94 vpDisplay::flush( I );
97 task.setServo( vpServo::EYEINHAND_L_cVe_eJe );
98 task.setInteractionMatrixType( vpServo::DESIRED, vpServo::PSEUDO_INVERSE );
99 task.setLambda( lambda );
100 vpVelocityTwistMatrix cVe;
101 cVe = robot.get_cVe();
104 std::cout <<
"cVe: \n" << cVe << std::endl;
109 std::cout <<
"eJe: \n" << eJe << std::endl;
112 vpFeaturePoint s_x, s_xd;
115 vpFeatureBuilder::create( s_x, cam,
dot );
118 s_xd.buildFrom( 0, 0, depth );
121 task.addFeature( s_x, s_xd );
124 vpFeatureDepth s_Z, s_Zd;
126 double surface = 1. / sqrt(
dot.m00 / ( cam.get_px() * cam.get_py() ) );
133 std::cout <<
"Z " << Z << std::endl;
134 s_Z.buildFrom( s_x.get_x(), s_x.get_y(), Z, 0 );
135 s_Zd.buildFrom( s_x.get_x(), s_x.get_y(), Zd, 0 );
138 task.addFeature( s_Z, s_Zd );
148 vpDisplay::display( I );
153 vpFeatureBuilder::create( s_x, cam,
dot );
156 surface = 1. / sqrt(
dot.m00 / ( cam.get_px() * cam.get_py() ) );
158 s_Z.buildFrom( s_x.get_x(), s_x.get_y(), Z, log( Z / Zd ) );
160 robot.get_cVe( cVe );
167 v = task.computeControlLaw();
169 std::cout <<
"Send velocity to the pionner: " << v[0] <<
" m/s " << vpMath::deg( v[1] ) <<
" deg/s" << std::endl;
175 vpDisplay::displayLine( I, 0, 320, 479, 320, vpColor::red );
176 vpDisplay::flush( I );
179 if ( vpDisplay::getClick( I,
false ) )
183 std::cout <<
"Ending robot thread..." << std::endl;
189 catch ( vpException e )
191 std::cout <<
"Catch an exception: " << e << std::endl;
199 std::cout <<
"You don't have the right 3rd party libraries to run this example..." << std::endl;