int test(double x,double y,double z,double alpha){
//intial pose
vpHomogeneousMatrix cMo(x,y,z,-vpMath::rad(0),vpMath::rad(0),alpha);
//Desired pose
vpHomogeneousMatrix cdMo(vpHomogeneousMatrix(0.0,0.0,1.0,vpMath::rad(0),vpMath::rad(0),-vpMath::rad(0)));
//source and destination objects for moment manipulation
vpMomentObject src(6);
vpMomentObject dst(6);
//init and run the simulation
initScene(cMo, cdMo, src, dst); //initialize graphical scene (for interface)
vpMatrix mat = execute(cMo, cdMo, src, dst);
if(fabs(mat[0][0]-(-1)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
if(fabs(mat[0][1]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
if(fabs(mat[0][2]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
if(fabs(mat[1][0]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
if(fabs(mat[1][1]-(-1)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
if(fabs(mat[1][2]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
if(fabs(mat[2][0]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
if(fabs(mat[2][1]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
if(fabs(mat[2][2]-(-1)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
if(fabs(mat[2][5]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
if(fabs(mat[3][0]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
if(fabs(mat[3][1]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
if(fabs(mat[3][2]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
if(fabs(mat[3][5]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
if(fabs(mat[4][0]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
if(fabs(mat[4][1]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
if(fabs(mat[4][2]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
if(fabs(mat[4][5]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
if(fabs(mat[5][0]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
if(fabs(mat[5][1]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
if(fabs(mat[5][2]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
if(fabs(mat[5][5]-(-1)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
return 0;
}
int main()
{
try {
// We want to calibrate the hand to eye extrinsic camera parameters from 6 couple of poses: cMo and wMe
const unsigned int N = 15;
// Input: six couple of poses used as input in the calibration proces
std::vector<vpHomogeneousMatrix> cMo(N) ; // eye (camera) to object transformation. The object frame is attached to the calibrartion grid
std::vector<vpHomogeneousMatrix> wMe(N) ; // world to hand (end-effector) transformation
// Output: Result of the calibration
vpHomogeneousMatrix eMc; // hand (end-effector) to eye (camera) transformation
// Initialize an eMc transformation used to produce the simulated input transformations cMo and wMe
//vpTranslationVector etc(0.1, 0.2, 0.3);
vpTranslationVector etc(0.05, 0, 0.2);
vpThetaUVector erc;
/*erc[0] = vpMath::rad(10); // 10 deg
erc[1] = vpMath::rad(-10); // -10 deg
erc[2] = vpMath::rad(25); */// 25 deg
erc[0] = vpMath::rad(0); // 10 deg
erc[1] = vpMath::rad(0); // -10 deg
erc[2] = vpMath::rad(0);
/*eMc.buildFrom(etc, erc);
std::cout << "Simulated hand to eye transformation: eMc " << std::endl ;
std::cout << eMc << std::endl ;
std::cout << "Theta U rotation: " << vpMath::deg(erc[0]) << " " << vpMath::deg(erc[1]) << " " << vpMath::deg(erc[2]) << std::endl;*/
vpColVector v_c(6) ; // camera velocity used to produce 6 simulated poses
/*Estimated pose on input data 0: 0.1569787825 0.1461834835 0.5301261328 -0.1929363152 -0.05977090224 -3.086850305
Estimated pose on input data 1: 0.1644980938 -0.01189407408 0.5553413668 0.2166557182 -0.05044182731 3.088872343
Estimated pose on input data 2: -0.08150101233 -0.06427513577 0.8229730225 0.6375887635 0.2929315537 -3.0412121
Estimated pose on input data 3: -0.07399590334 0.2463715544 0.8084672335 -0.5425092233 0.9201825297 2.951574841
Estimated pose on input data 4: 0.1182872853 0.06179490234 0.5342609156 0.290296242 0.8278289665 2.878703416
Estimated pose on input data 5: 0.07240424921 0.05818089849 0.5080845294 0.8878156838 -0.03319297731 2.92028639
Estimated pose on input data 6: 0.128864062 0.01258769986 0.5791513843 -0.01963916351 1.400887526 -2.710874876
Estimated pose on input data 7: 0.1331430092 0.1285754873 0.5841843523 -0.07130638491 1.664699711 2.571484173
Estimated pose on input data 8: 0.1475986772 0.08083976421 0.4742733255 0.3274904787 1.251618151 2.748407772
Estimated pose on input data 9: 0.06660012206 0.04565492383 0.9904218065 -0.2386496107 0.5378999186 3.049756264
Estimated pose on input data 10: 0.135244671 0.006638499199 0.5525444996 -0.1161145018 1.197898419 -2.754131838
Estimated pose on input data 11: 0.1133506203 0.05656750283 0.56323877 0.01858298003 0.670425518 -2.802539113
Estimated pose on input data 12: 0.09667463795 0.03492932371 0.7513400654 -0.2549253893 1.239009966 2.848669567
Estimated pose on input data 13: 0.111754616 0.0352982772 0.6037117746 -0.2635997725 1.343112867 -2.740843596
Estimated pose on input data 14: 0.1304978931 0.05777044613 0.4837973463 0.09932414819 0.6587037148 3.055926128 */
/*Estimated pose on input data 0: 0.1570413653 0.1462814767 0.5296167593 -0.1941770803 -0.05941143538 -3.086854663
Estimated pose on input data 1: 0.1645591308 -0.01178757497 0.5548852438 0.2174172067 -0.05050699552 3.088872578
Estimated pose on input data 2: -0.08140123626 -0.06415861852 0.8225177987 0.6387268082 0.2934568774 -3.041051984
Estimated pose on input data 3: -0.07389514878 0.2465329982 0.8080147574 -0.5427311886 0.9203214735 2.951500448
Estimated pose on input data 4: 0.1183504128 0.06189642877 0.5338128543 0.290828389 0.8278402557 2.878681592
Estimated pose on input data 5: 0.07245283491 0.05827456634 0.5074674243 0.8885114659 -0.03282148657 2.920083464
Estimated pose on input data 6: 0.12894082 0.01269200688 0.5787056135 -0.01986987293 1.400704506 -2.711025518
Estimated pose on input data 7: 0.133226549 0.1287034045 0.5838253899 -0.07109766974 1.664064991 2.571849871
Estimated pose on input data 8: 0.1476536754 0.08092558365 0.4738385251 0.32787776 1.251768622 2.748314156
Estimated pose on input data 9: 0.06674342233 0.04580250865 0.9896494674 -0.2363088805 0.5380505543 3.049988544
Estimated pose on input data 10: 0.1353199539 0.006734768036 0.552106057 -0.1165157893 1.197817012 -2.754244687
Estimated pose on input data 11: 0.1134173579 0.05667957541 0.562798548 0.01827158984 0.6700808586 -2.802593791
Estimated pose on input data 12: 0.09678548583 0.03505801317 0.75085561 -0.2546886466 1.238795585 2.848785012
Estimated pose on input data 13: 0.1118341527 0.03541269363 0.6032772496 -0.2637376516 1.3428496 -2.740983394
Estimated pose on input data 14: 0.1305641085 0.05786662886 0.483384278 0.0992989732 0.6584820877 3.055982463 */
cMo[0].buildFrom(0.1570413653 , 0.1462814767 , 0.5296167593 , -0.1941770803 , -0.05941143538 , -3.086854663);
cMo[1].buildFrom(0.1645591308 , -0.01178757497 , 0.5548852438 , 0.2174172067 , -0.05050699552 , 3.088872578);
cMo[2].buildFrom(-0.08140123626 , -0.06415861852 , 0.8225177987 , 0.6387268082 , 0.2934568774 , -3.041051984);
cMo[3].buildFrom(-0.07389514878 , 0.2465329982 , 0.8080147574 , -0.5427311886 , 0.9203214735 , 2.951500448 );
cMo[4].buildFrom(0.1183504128 , 0.06189642877 , 0.5338128543 , 0.290828389 , 0.8278402557 , 2.878681592 );
cMo[5].buildFrom(0.07245283491 , 0.05827456634 , 0.5074674243, 0.8885114659 , -0.03282148657 , 2.920083464 );
cMo[6].buildFrom( 0.12894082 , 0.01269200688 , 0.5787056135 , -0.01986987293 , 1.400704506 , -2.711025518 );
cMo[7].buildFrom(0.133226549 , 0.1287034045 , 0.5838253899 , -0.07109766974 , 1.664064991 , 2.571849871 );
cMo[8].buildFrom( 0.1476536754 , 0.08092558365 , 0.4738385251 , 0.32787776 , 1.251768622, 2.748314156 );
cMo[9].buildFrom(0.06674342233 , 0.04580250865 , 0.9896494674, -0.2363088805 , 0.5380505543 , 3.049988544 );
cMo[10].buildFrom(0.1353199539 , 0.006734768036 , 0.552106057 , -0.1165157893 , 1.197817012 , -2.754244687 );
cMo[11].buildFrom(0.1134173579 , 0.05667957541 , 0.562798548, 0.01827158984 , 0.6700808586, -2.802593791 );
cMo[12].buildFrom(0.09678548583 , 0.03505801317 , 0.75085561 , -0.2546886466 , 1.238795585 , 2.848785012 );
cMo[13].buildFrom(0.1118341527 , 0.03541269363 , 0.6032772496 , -0.2637376516 , 1.3428496 , -2.740983394 );
cMo[14].buildFrom(0.1305641085 , 0.05786662886, 0.483384278 , 0.0992989732 , 0.6584820877 , 3.055982463 );
/*rvec0 [0.1606333657728691; 0.1554771292435871; 0.523395656853085] [-0.2028827068163479; -0.07146025385754685; -3.087095760857872]
rvec1 [0.1681941750976325; -0.001966200118312488; 0.5488226016042185] [0.2330312513835265; -0.04491445863364658; 3.086889567871931]
rvec2 [-0.07535065781353516; -0.0494231619041778; 0.8121574680164582] [0.6269708574795875; 0.2466336555047598; -3.047031344161096]
rvec3 [-0.06802549373045808; 0.2595903683998133; 0.7964257887135282] [0.5288740859743205; -0.9590380501349665; -2.946336745490846]
rvec4 [0.12187613626985; 0.07101719070919629; 0.5278476386226427] [0.2978167684302557; 0.856630660575602; 2.869907478228704]
rvec5 [0.07595327201575928; 0.06701357139477045; 0.5027460905513198] [0.8912948190468262; -0.008142441201194372; 2.913745579204999]
rvec6 [0.132733282475315; 0.02272610788496898; 0.5730526114864242] [-0.03007532729248057; 1.379964566058304; -2.724720351422941]
rvec7 [0.1370725415848025; 0.1385564871667145; 0.5767143586165552] [-0.06277331632480748; 1.69196990365581; 2.558673516092988]
rvec8 [0.1507250495868063; 0.08898453438435538; 0.4678466347770635] [0.3364169076342961; 1.279870522980729; 2.736186215384778]
rvec9 [0.07331123838561378; 0.06286631535354439; 0.980297548164113] [-0.2302529418021804; 0.5684685241373887; 3.047030697639466]
rvec10 [0.1389003764503458; 0.01635260272872881; 0.5463305723417353] [-0.1266843616136658; 1.176396083254107; -2.765891437223603]
rvec11 [0.1170763510447753; 0.06632308915598384; 0.5567537655740936] [0.007781882768882385; 0.6513072031809303; -2.808179904285889]
rvec12 [0.1017381935405406; 0.04799307809762116; 0.743317825159861] [-0.2461148227376482; 1.266603710301319; 2.841674404874994]
rvec13 [0.1157802508889457; 0.04581540120783949; 0.5972930741156408] [-0.2739393358455662; 1.322462552325598; -2.754638766920345]
rvec14 [0.1336917033827539; 0.0661776007059256; 0.4775114452700749] [0.1113893043239393; 0.6867963031591187; 3.050765919593558]*/
/* rvec0 [0.1597930883553831; 0.1468007367442987; 0.5282961701119935] [-0.1968521018387337; -0.05964684515549047; -3.086641073146096]
rvec1 [0.1673763925455965; -0.01114616076427374; 0.553279655621999] [0.2239915080197494; -0.05005752718967757; 3.088376811445953]
rvec2 [-0.07709719572166263; -0.06311291825387691; 0.8202092073793362] [0.6219896900804657; 0.2876488737041963; -3.044963439387781]
rvec3 [-0.06973561289067365; 0.2471255554578212; 0.805719925349877] [-0.5362409014255265; 0.9272355894724263; 2.951290732839171]
rvec4 [0.1210411206055177; 0.06238084798406756; 0.5324010655079093] [0.2973959444235151; 0.8322373133058337; 2.878093597238552]
rvec5 [0.07508944088821654; 0.05879690189843891; 0.5070670063652497] [0.8932206395919406; -0.03117966337021329; 2.918242087859566]
rvec6 [0.1318496740636458; 0.0132932944215828; 0.5772437747839489] [-0.02665105916432204; 1.401478703953037; -2.713171896524004]
rvec7 [0.1361734623849172; 0.1291969099332706; 0.5818604734340637] [-0.06441618020387928; 1.666431046373741; 2.573408993461815]
rvec8 [0.1500497707905874; 0.0813631742783348; 0.4722210658185951] [0.3346209323627573; 1.254500083452833; 2.748388621696409]
rvec9 [0.07173548938696282; 0.04679841960380481; 0.9882799057094008] [-0.2295202979093593; 0.5438325909344705; 3.050345194607081]
rvec10 [0.1380846144556481; 0.007346052126947021; 0.5505733029088267] [-0.1232907800995376; 1.197896788755784; -2.756013443872642]
rvec11 [0.1162531426061057; 0.05721641313429964; 0.5614795673624928] [0.01109493482067169; 0.6727074488226285; -2.803240757451364]
rvec12 [0.1005589170954649; 0.03581030081212384; 0.7496742244991828] [-0.2478478271228602; 1.242473587369607; 2.850021720345595]
rvec13 [0.1148657511867909; 0.03601171150638215; 0.6019740529052237] [-0.2706978393095179; 1.343857646300739; -2.742391791707475]
rvec14 [0.1329692185754732; 0.05829478232313912; 0.4817358432812112] [0.1078725939205944; 0.6637727816622113; 3.055743294810342]*/
cMo[0].buildFrom(0.1597930883553831, 0.1468007367442987, 0.5282961701119935,-0.1968521018387337, -0.05964684515549047, -3.086641073146096);
cMo[1].buildFrom(0.1673763925455965, -0.01114616076427374, 0.553279655621999,0.2239915080197494, -0.05005752718967757, 3.088376811445953);
cMo[2].buildFrom(-0.07709719572166263, -0.06311291825387691, 0.8202092073793362,0.6219896900804657, 0.2876488737041963, -3.044963439387781);
cMo[3].buildFrom(-0.06973561289067365, 0.2471255554578212, 0.805719925349877,-0.5362409014255265, 0.9272355894724263, 2.951290732839171);
cMo[4].buildFrom(0.1210411206055177, 0.06238084798406756, 0.5324010655079093,0.2973959444235151, 0.8322373133058337, 2.878093597238552);
cMo[5].buildFrom(0.07508944088821654, 0.05879690189843891, 0.5070670063652497,0.8932206395919406, -0.03117966337021329, 2.918242087859566);
cMo[6].buildFrom(0.1318496740636458, 0.0132932944215828, 0.5772437747839489,-0.02665105916432204, 1.401478703953037, -2.713171896524004);
cMo[7].buildFrom(0.1361734623849172, 0.1291969099332706, 0.5818604734340637,-0.06441618020387928, 1.666431046373741, 2.573408993461815);
cMo[8].buildFrom(0.1500497707905874, 0.0813631742783348, 0.4722210658185951,0.3346209323627573, 1.254500083452833, 2.748388621696409);
cMo[9].buildFrom(0.07173548938696282, 0.04679841960380481, 0.9882799057094008,-0.2295202979093593, 0.5438325909344705, 3.050345194607081);
cMo[10].buildFrom(0.1380846144556481, 0.007346052126947021, 0.5505733029088267,-0.1232907800995376, 1.197896788755784, -2.756013443872642);
cMo[11].buildFrom(0.1162531426061057, 0.05721641313429964, 0.5614795673624928,0.01109493482067169, 0.6727074488226285, -2.803240757451364);
cMo[12].buildFrom(0.1005589170954649, 0.03581030081212384, 0.7496742244991828,-0.2478478271228602, 1.242473587369607, 2.850021720345595);
cMo[13].buildFrom(0.1148657511867909,0.03601171150638215, 0.6019740529052237,-0.2706978393095179,1.343857646300739, -2.742391791707475);
cMo[14].buildFrom(0.1329692185754732, 0.05829478232313912, 0.4817358432812112,0.1078725939205944, 0.6637727816622113, 3.055743294810342);
/* rvec0 [0.1670271366433932; 0.1412310989392187; 0.5291817104298804] [-0.2158501122137233; -0.04102562316131209; -3.086462501033879]
rvec1 [0.174732459685928; -0.01679913253246702; 0.5522224159363196] [0.2461084308599157; -0.06243825852964164; 3.087091867254426]
rvec2 [-0.06616300409680606; -0.07144422525798637; 0.822412920334844] [0.6163890473443646; 0.2990544162277649; -3.047056418830492]
rvec3 [-0.05870935563512666; 0.2389436813297934; 0.8107597728870847] [0.5213771113951537; -0.9121090301250817; -2.960378152694366]
rvec4 [0.1281525936746428; 0.05693383934516558; 0.5327733518837586] [0.3179210001194651; 0.8200242142582296; 2.88367687151767]
rvec5 [0.08180651786871981; 0.05366036316002259; 0.5077531214512626] [0.9150117372925481; -0.04466326358959161; 2.914221695870326]
rvec6 [0.1395093199674059; 0.007415567899493163; 0.577117662504006] [-0.04261329013241258; 1.41745949022017; -2.711336977155535]
rvec7 [0.1440317716370368; 0.1231050167128023; 0.5827247954590138] [-0.04665269265730402; 1.658836044602898; 2.585772326995824]
rvec8 [0.1564030388064615; 0.0763727666089834; 0.4722951567730267] [0.354293229181613; 1.24485453932061; 2.756275049720885]
rvec9 [0.08483478780487425; 0.03691454820089227; 0.9903368602303289] [-0.2115118140952619; 0.5254383450695477; 3.056702831376301]
rvec10 [0.1454078173575387; 0.001721636178656781; 0.5504152051438634] [-0.1384204054268272; 1.213040205258829; -2.753677593947966]
rvec11 [0.1237204738872894; 0.05150067983657727; 0.5619672773339737] [-0.002655776810846245; 0.6873760817109954; -2.802941737408393]
rvec12 [0.1105008417439839; 0.02827236018640317; 0.7504407471577624] [-0.2289243995608501; 1.229517790181478; 2.8622921610602]
rvec13 [0.1228660494234161; 0.02992076313030763; 0.6022990540352411] [-0.2881653363559677; 1.359227700111445; -2.738474323741062]
rvec14 [0.1393960912776154; 0.05334088954955449; 0.4818203087724587] [0.1293076026053152; 0.6488927042823395; 3.060803207504738]*/
cMo[0].buildFrom( 0.1670271366433932, 0.1412310989392187, 0.5291817104298804,-0.2158501122137233, -0.04102562316131209, -3.086462501033879);
cMo[1].buildFrom(0.174732459685928, -0.01679913253246702, 0.5522224159363196,0.2461084308599157, -0.06243825852964164, 3.087091867254426);
cMo[2].buildFrom( -0.06616300409680606, -0.07144422525798637, 0.822412920334844,0.6163890473443646, 0.2990544162277649, -3.047056418830492);
cMo[3].buildFrom(-0.05870935563512666, 0.2389436813297934, 0.8107597728870847,0.5213771113951537, -0.9121090301250817, -2.960378152694366);
cMo[4].buildFrom( 0.1281525936746428, 0.05693383934516558, 0.5327733518837586,0.3179210001194651, 0.8200242142582296, 2.88367687151767);
cMo[5].buildFrom(0.08180651786871981, 0.05366036316002259, 0.5077531214512626,0.9150117372925481, -0.04466326358959161, 2.914221695870326);
cMo[6].buildFrom( 0.1395093199674059, 0.007415567899493163, 0.577117662504006,-0.04261329013241258, 1.41745949022017, -2.711336977155535);
cMo[7].buildFrom(0.1440317716370368,0.1231050167128023, 0.5827247954590138,-0.04665269265730402, 1.658836044602898, 2.585772326995824);
cMo[8].buildFrom( 0.1564030388064615, 0.0763727666089834, 0.4722951567730267,0.354293229181613, 1.24485453932061, 2.756275049720885);
cMo[9].buildFrom( 0.08483478780487425, 0.03691454820089227, 0.9903368602303289,-0.2115118140952619, 0.5254383450695477, 3.056702831376301);
cMo[10].buildFrom( 0.1454078173575387, 0.001721636178656781, 0.5504152051438634,-0.1384204054268272,1.213040205258829, -2.753677593947966);
cMo[11].buildFrom( 0.1237204738872894, 0.05150067983657727, 0.5619672773339737,-0.002655776810846245, 0.6873760817109954, -2.802941737408393);
cMo[12].buildFrom( 0.1105008417439839, 0.02827236018640317, 0.7504407471577624,-0.2289243995608501, 1.229517790181478, 2.8622921610602);
cMo[13].buildFrom(0.1228660494234161, 0.02992076313030763, 0.6022990540352411,-0.2881653363559677, 1.359227700111445, -2.738474323741062);
cMo[14].buildFrom( 0.1393960912776154, 0.05334088954955449, 0.4818203087724587,0.1293076026053152, 0.6488927042823395, 3.060803207504738);
/*cMo[0] = cMo[0].inverse();
cMo[1] = cMo[1].inverse();
cMo[2] = cMo[2].inverse();
cMo[3] = cMo[3].inverse();
cMo[4] = cMo[4].inverse();*/
cv::Mat wme(4,4,cv::DataType<double>::type);
/*T_be1 =
-0.4004 -0.0535 0.9148 0.1526
0.0003 -0.9983 -0.0583 0.0044
0.9163 -0.0230 0.3998 0.8041
0 0 0 1.0000
*/
wMe[0][0][0] = -0.4004;
wMe[0][0][1] = -0.0535;
wMe[0][0][2] = 0.9148;
wMe[0][0][3] = 0.1526;
wMe[0][1][0] = 0.0003;
wMe[0][1][1] = -0.9983;
wMe[0][1][2] = -0.0583;
wMe[0][1][3] = 0.0044;
wMe[0][2][0] = 0.9163;
wMe[0][2][1] = -0.0230;
wMe[0][2][2] = 0.3998;
wMe[0][2][3] = 0.8041;
wMe[0] = wMe[0].inverse();
/*T_be2 =
-0.4154 -0.0187 0.9095 0.1792
0.0838 -0.9963 0.0178 0.1368
0.9058 0.0836 0.4154 0.8136
0 0 0 1.0000*/
wMe[1][0][0] = -0.4154;
wMe[1][0][1] = -0.0187;
wMe[1][0][2] = 0.9095;
wMe[1][0][3] = 0.1792;
wMe[1][1][0] = 0.0838;
wMe[1][1][1] = -0.9963;
wMe[1][1][2] = 0.0178;
wMe[1][1][3] = 0.1368;
wMe[1][2][0] = 0.9058 ;
wMe[1][2][1] = 0.0836;
wMe[1][2][2] = 0.4154 ;
wMe[1][2][3] = 0.8136;
wMe[1] = wMe[1].inverse();
/*T_be3 =
0.1052 0.1733 0.9792 0.3298
0.0429 -0.9846 0.1696 0.1947
0.9935 0.0241 -0.1110 0.6216
0 0 0 1.0000*/
wMe[2][0][0] = 0.1052 ;
wMe[2][0][1] = 0.1733 ;
wMe[2][0][2] = 0.9792;
wMe[2][0][3] = 0.3298;
wMe[2][1][0] = 0.0429;
wMe[2][1][1] =-0.9846;
wMe[2][1][2] = 0.1696;
wMe[2][1][3] = 0.1947;
wMe[2][2][0] =0.9935;
wMe[2][2][1] = 0.0241;
wMe[2][2][2] = -0.1110;
wMe[2][2][3] = 0.6216;
wMe[2] = wMe[2].inverse();
/* T_be4 =
0.0678 -0.5774 0.8136 0.3720
-0.0403 -0.8164 -0.5761 -0.0944
0.9969 0.0063 -0.0786 0.6249
0 0 0 1.0000
*/
wMe[3][0][0] = 0.0678;
wMe[3][0][1] = -0.5774 ;
wMe[3][0][2] = 0.8136 ;
wMe[3][0][3] = 0.3720;
wMe[3][1][0] = -0.0403;
wMe[3][1][1] = -0.8164;
wMe[3][1][2] = -0.5761;
wMe[3][1][3] = -0.0944;
wMe[3][2][0] = 0.9969;
wMe[3][2][1] = 0.0063;
wMe[3][2][2] = -0.0786;
wMe[3][2][3] = 0.6249;
wMe[3] = wMe[3].inverse();
/*
T_be5 =
-0.3865 -0.5722 0.7233 0.1988
0.2390 -0.8196 -0.5206 0.0121
0.8908 -0.0284 0.4536 0.7745
0 0 0 1.0000*/
wMe[4][0][0] = -0.3865;
wMe[4][0][1] = -0.5722;
wMe[4][0][2] = 0.7233;
wMe[4][0][3] = 0.1988;
wMe[4][1][0] = 0.2390;
wMe[4][1][1] = -0.8196;
wMe[4][1][2] = -0.5206;
wMe[4][1][3] = 0.0121;
wMe[4][2][0] = 0.8908;
wMe[4][2][1] = -0.0284;
wMe[4][2][2] = 0.4536;
wMe[4][2][3] = 0.7745;
wMe[4] = wMe[4].inverse();
/*T_be6 =
-0.7626 -0.0764 0.6423 0.2651
0.1273 -0.9913 0.0332 0.0548
0.6342 0.1071 0.7657 0.7880
0 0 0 1.0000*/
wMe[5][0][0] = -0.7626;
wMe[5][0][1] = -0.0764 ;
wMe[5][0][2] = 0.6423;
wMe[5][0][3] = 0.2651;
wMe[5][1][0] = 0.1273 ;
wMe[5][1][1] = -0.9913;
wMe[5][1][2] = 0.0332;
wMe[5][1][3] = 0.0548;
wMe[5][2][0] = 0.6342;
wMe[5][2][1] = 0.1071;
wMe[5][2][2] = 0.7657;
wMe[5][2][3] = 0.7880;
wMe[5] = wMe[5].inverse();
/*T_be7 =
-0.2697 0.7872 0.5546 0.1178
-0.0949 -0.5949 0.7982 0.1315
0.9583 0.1627 0.2351 0.7655
0 0 0 1.0000*/
wMe[6][0][0] = -0.2697;
wMe[6][0][1] = 0.7872 ;
wMe[6][0][2] = 0.5546;
wMe[6][0][3] = 0.1178;
wMe[6][1][0] = -0.0949;
wMe[6][1][1] = -0.5949;
wMe[6][1][2] = 0.7982;
wMe[6][1][3] = 0.1315;
wMe[6][2][0] = 0.9583;
wMe[6][2][1] = 0.1627;
wMe[6][2][2] = 0.2351;
wMe[6][2][3] = 0.7655;
wMe[6] = wMe[6].inverse();
/*T_be8 =
-0.1456 -0.9175 0.3700 0.1975
0.0973 -0.3855 -0.9176 -0.0398
0.9845 -0.0976 0.1454 0.7808
0 0 0 1.0000*/
wMe[7][0][0] = -0.1456;
wMe[7][0][1] = -0.9175;
wMe[7][0][2] = 0.3700;
wMe[7][0][3] = 0.1975;
wMe[7][1][0] = 0.0973;
wMe[7][1][1] = -0.3855;
wMe[7][1][2] = -0.9176;
wMe[7][1][3] = -0.0398;
wMe[7][2][0] = 0.9845;
wMe[7][2][1] = -0.0976;
wMe[7][2][2] = 0.1454;
wMe[7][2][3] = 0.7808;
wMe[7] = wMe[7].inverse();
/*T_be9 =
-0.3755 -0.7617 0.5280 0.1443
0.2437 -0.6308 -0.7367 -0.0192
0.8942 -0.1479 0.4224 0.7956
0 0 0 1.0000*/
wMe[8][0][0] = -0.3755;
wMe[8][0][1] = -0.7617;
wMe[8][0][2] = 0.5280 ;
wMe[8][0][3] = 0.1443;
wMe[8][1][0] = 0.2437;
wMe[8][1][1] =-0.6308;
wMe[8][1][2] = -0.7367 ;
wMe[8][1][3] = -0.0192;
wMe[8][2][0] = 0.8942;
wMe[8][2][1] = -0.1479;
wMe[8][2][2] = 0.4224;
wMe[8][2][3] = 0.7956;
wMe[8] = wMe[8].inverse();
/*T_be10 =
-0.1198 -0.3680 0.9221 0.5571
0.0602 -0.9298 -0.3632 0.0788
0.9910 0.0120 0.1335 0.8067
0 0 0 1.0000*/
wMe[9][0][0] = -0.1198;
wMe[9][0][1] = -0.3680;
wMe[9][0][2] = 0.9221 ;
wMe[9][0][3] =0.5571;
wMe[9][1][0] =0.0602;
wMe[9][1][1] = -0.9298 ;
wMe[9][1][2] =-0.3632;
wMe[9][1][3] = 0.0788 ;
wMe[9][2][0] =0.9910;
wMe[9][2][1] =0.0120;
wMe[9][2][2] =0.1335 ;
wMe[9][2][3] = 0.8067;
wMe[9] = wMe[9].inverse();
/*T_be11 =
-0.3123 0.7088 0.6325 0.1088
-0.1564 -0.6951 0.7017 0.1619
0.9370 0.1202 0.3279 0.7734
0 0 0 1.0000*/
wMe[10][0][0] = -0.3123;
wMe[10][0][1] = 0.7088;
wMe[10][0][2] = 0.6325;
wMe[10][0][3] =0.1088;
wMe[10][1][0] =-0.1564 ;
wMe[10][1][1] = -0.6951;
wMe[10][1][2] = 0.7017;
wMe[10][1][3] = 0.1619;
wMe[10][2][0] = 0.9370;
wMe[10][2][1] =0.1202;
wMe[10][2][2] =0.3279;
wMe[10][2][3] = 0.7734;
wMe[10] = wMe[10].inverse();
/*T_be12 =
-0.2224 0.4684 0.8551 0.1112
-0.2306 -0.8774 0.4206 0.1484
0.9473 -0.1037 0.3032 0.7667
0 0 0 1.0000*/
wMe[11][0][0] = -0.2224;
wMe[11][0][1] = 0.4684;
wMe[11][0][2] = 0.8551;
wMe[11][0][3] =0.1112;
wMe[11][1][0] =-0.2306 ;
wMe[11][1][1] = -0.8774 ;
wMe[11][1][2] =0.4206;
wMe[11][1][3] = 0.1484;
wMe[11][2][0] = 0.9473;
wMe[11][2][1] =-0.1037;
wMe[11][2][2] =0.3032 ;
wMe[11][2][3] = 0.7667;
wMe[11] = wMe[11].inverse();
/*T_be13 =
-0.0878 -0.7416 0.6651 0.3389
0.0151 -0.6686 -0.7435 0.0917
0.9960 -0.0552 0.0699 0.7806
0 0 0 1.0000*/
wMe[12][0][0] = -0.0878;
wMe[12][0][1] = -0.7416;
wMe[12][0][2] = 0.6651;
wMe[12][0][3] =0.3389;
wMe[12][1][0] =0.0151;
wMe[12][1][1] =-0.6686 ;
wMe[12][1][2] =-0.7435;
wMe[12][1][3] =0.0917;
wMe[12][2][0] =0.9960;
wMe[12][2][1] =-0.0552;
wMe[12][2][2] =0.0699;
wMe[12][2][3] =0.7806;
wMe[12] = wMe[12].inverse();
/*T_be14 =
-0.4084 0.7378 0.5375 0.1909
-0.1498 -0.6350 0.7578 0.1257
0.9004 0.2290 0.3699 0.7660
0 0 0 1.0000*/
wMe[13][0][0] = -0.4084;
wMe[13][0][1] = 0.7378;
wMe[13][0][2] =0.5375;
wMe[13][0][3] =0.1909;
wMe[13][1][0] =-0.1498 ;
wMe[13][1][1] = -0.6350;
wMe[13][1][2] =0.7578 ;
wMe[13][1][3] = 0.1257;
wMe[13][2][0] =0.9004 ;
wMe[13][2][1] = 0.2290 ;
wMe[13][2][2] =0.3699 ;
wMe[13][2][3] = 0.7660;
wMe[13] = wMe[13].inverse();
/*T_be15 =
-0.3175 -0.4333 0.8435 0.1187
0.0831 -0.8988 -0.4304 0.0654
0.9446 -0.0666 0.3213 0.7620
0 0 0 1.0000*/
wMe[14][0][0] = -0.3175;
wMe[14][0][1] = -0.4333 ;
wMe[14][0][2] = 0.8435;
wMe[14][0][3] =0.1187;
wMe[14][1][0] = 0.0831;
wMe[14][1][1] = -0.8988;
wMe[14][1][2] =-0.4304 ;
wMe[14][1][3] = 0.0654;
wMe[14][2][0] = 0.9446 ;
wMe[14][2][1] =-0.0666;
wMe[14][2][2] =0.3213 ;
wMe[14][2][3] = 0.7620;
wMe[14] = wMe[14].inverse();
/*wMe[6][0][0] = 0.1251;
wMe[6][0][1] = -0.3291;
wMe[6][0][2] =0.9360;
wMe[6][0][3] =0.4350;
wMe[6][1][0] = 0.0764;
wMe[6][1][1] = -0.9374;
wMe[6][1][2] =-0.3398;
wMe[6][1][3] = -0.0258;
wMe[6][2][0] = 0.9892;
wMe[6][2][1] = 0.1140;
wMe[6][2][2] = -0.0922;
wMe[6][2][3] = 0.6420;
wMe[6] = wMe[6].inverse();*/
/**/
/*wMe[0].buildFrom(0, 0, 0, 10*M_PI/180., -10*M_PI/180.,0);
wMe[1].buildFrom(0, 0, 0, 20*M_PI/180.,-10*M_PI/180.,0);
wMe[2].buildFrom(0, 0, 0, 30*M_PI/180.,-10*M_PI/180.,0);
wMe[3].buildFrom(0, 0, 0, 10*M_PI/180.,-20*M_PI/180.,0);
wMe[4].buildFrom(0, 0, 0, 20*M_PI/180.,-20*M_PI/180.,0);
wMe[5].buildFrom(0, 0, 0, 30*M_PI/180.,-20*M_PI/180.,0);
wMe[6].buildFrom(0, 0, 0, 10*M_PI/180.,-30*M_PI/180.,0);
wMe[7].buildFrom(0, 0, 0, 20*M_PI/180.,-30*M_PI/180.,0);
wMe[8].buildFrom(0, 0, 0, 30*M_PI/180.,-30*M_PI/180.,0);*/
/*for (unsigned int i=0 ; i < N ; i++)
{
v_c = 0 ;
if (i==0) {
// Initialize first poses
cMo[0].buildFrom(0, 0, 0.5, 0, 0, 0); // z=0.5 m
wMe[0].buildFrom(0, 0, 0, 0, 0, 0); // Id
}
else if (i==1)
v_c[3] = M_PI/8 ;
else if (i==2)
v_c[4] = M_PI/8 ;
else if (i==3)
v_c[5] = M_PI/10 ;
else if (i==4)
v_c[0] = 0.5 ;
else if (i==5)
v_c[1] = 0.8 ;
vpHomogeneousMatrix cMc; // camera displacement
cMc = vpExponentialMap::direct(v_c) ; // Compute the camera displacement due to the velocity applied to the camera
if (i > 0) {
// From the camera displacement cMc, compute the wMe and cMo matrices
cMo[i] = cMc.inverse() * cMo[i-1];
wMe[i] = wMe[i-1] * eMc * cMc * eMc.inverse();
}
}
if (0) {
for (unsigned int i=0 ; i < N ; i++) {
vpHomogeneousMatrix wMo;
wMo = wMe[i] * eMc * cMo[i];
std::cout << std::endl << "wMo[" << i << "] " << std::endl ;
std::cout << wMo << std::endl ;
std::cout << "cMo[" << i << "] " << std::endl ;
std::cout << cMo[i] << std::endl ;
std::cout << "wMe[" << i << "] " << std::endl ;
std::cout << wMe[i] << std::endl ;
}
}*/
// Reset the eMc matrix to eye
eMc.eye();
vpHomogeneousMatrix c1Mb,c2Mb;
/*-0.2627605227 0.0151902301 0.9647415015 -0.8826177717
-0.01615928012 -0.9998051093 0.01134111962 0.0979656024
0.9647257566 -0.01260952965 0.2629547763 0.2804142289
0 0 0 1 */
// Compute the eMc hand to eye transformation from six poses
// - cMo[6]: camera to object poses as six homogeneous transformations
// - wMe[6]: world to hand (end-effector) poses as six homogeneous transformations
vpCalibration::calibrationTsai(cMo, wMe, eMc) ;
/*c1Mb[0][0] =-0.2627605227;
c1Mb[0][1] = 0.0151902301;
c1Mb[0][2] = 0.9647415015 ;
c1Mb[0][3] = -0.8826177717;
c1Mb[1][0] = -0.01615928012;
c1Mb[1][1] = -0.9998051093;
c1Mb[1][2] = 0.01134111962;
c1Mb[1][3] = 0.0979656024;
c1Mb[2][0] = 0.9647257566;
c1Mb[2][1] = -0.01260952965;
c1Mb[2][2] = 0.2629547763;
c1Mb[2][3] = 0.2804142289;*/
/*c1mb 0.09772591515 0.04706152302 0.9941000244 -0.612686742
-0.8656071104 0.4969240654 0.06156950224 0.4536187228
-0.491094671 -0.8665169855 0.08929914801 0.7723205071
0 0 0 1 */
/*-0.2363178473 -0.02587436709 0.9713312474 -0.8784863904
0.05264871432 -0.9985178827 -0.01378951562 0.09275778847
0.9702484156 0.04788063271 0.2373298487 0.2305791122
0 0 0 1 */
/* calib c1mb -0.243548266 -0.00578239501 0.969871541 -0.8739992614
0.05078066467 -0.9986866964 0.006797541299 0.07978538541
0.9685584991 0.05090625089 0.243522047 0.223042619
0 0 0 1 */
c1Mb = eMc.inverse();
std::cout << " calib c1mb " << eMc << " " << c1Mb << std::endl;
/*-0.271553545 0.001448653885 0.9624222429 -0.8821152339
0.02302455283 -0.9997028805 0.008001293053 -0.08084598513
0.9621478796 0.02433212127 0.2714395065 0.2839962488
0 0 0 1 */
/*c2mb = 0.09712912699 0.06303453402 0.9932736683 -0.6227154987
-0.8697303448 0.4905733585 0.05391574121 0.2821384248
-0.4838750458 -0.869117039 0.102472009 0.7628133232
0 0 0 1*/
/*-0.2556456149 -0.01038676616 0.9667147639 -0.8797472439
0.04013060571 -0.9991944352 -0.0001232892871 -0.08052809443
0.965937293 0.03876333066 0.255856503 0.2052943186*/
c2Mb[0][0] =-0.2556456149;
c2Mb[0][1] = -0.01038676616;
c2Mb[0][2] = 0.9667147639;
c2Mb[0][3] = -0.8797472439;
c2Mb[1][0] = 0.04013060571;
c2Mb[1][1] = -0.9991944352;
c2Mb[1][2] = -0.0001232892871;
c2Mb[1][3] = -0.08052809443;
c2Mb[2][0] = 0.965937293;
c2Mb[2][1] =0.03876333066;
c2Mb[2][2] = 0.255856503;
c2Mb[2][3] = 0.2052943186;
vpHomogeneousMatrix rect1,rect2,proj1,proj2;
/*0.999954 0.009229 -0.002472
-0.009128 0.999225 0.038285
0.002823 -0.038260 0.999264*/
rect1[0][0] = 0.999954;
rect1[0][1] = 0.009229;
rect1[0][2] = -0.002472;
rect1[0][3] = 0;
rect1[1][0] = -0.009128;
rect1[1][1] = 0.999225;
rect1[1][2] = 0.038285;
rect1[1][3] = -0.177012987;
rect1[2][0] =0.002823;
rect1[2][1] = -0.038260;
rect1[2][2] = 0.999264;
rect1[2][3] = 0;
/*0.999990 -0.003522 0.002602
0.003453 0.999650 0.026232
-0.002694 -0.026222 0.999653*/
rect2[0][0] = 0.999990;
rect2[0][1] = -0.003522;
rect2[0][2] = 0.002602;
rect2[0][3] = 0;
rect2[1][0] = 0.003453;
rect2[1][1] = 0.999650;
rect2[1][2] = -0.026232;
rect2[1][3] = 0;
rect2[2][0] =-0.002694 ;
rect2[2][1] = -0.026222;
rect2[2][2] = 0.999653;
rect2[2][3] = 0;
/*770.860676 0.000000 342.122410 0.000000
0.000000 770.860676 258.604725 0.000000
0.000000 0.000000 1.000000 0.000000*/
proj1[0][0] = 770.860676;
proj1[0][1] = 0.000000;
proj1[0][2] = 342.122410;
proj1[0][3] = 0;
proj1[1][0] = 0.00;
proj1[1][1] = 770.860676;
proj1[1][2] = 258.604725;
proj1[1][3] = 0;
proj1[2][0] =0 ;
proj1[2][1] = 0;
proj1[2][2] = 1;
proj1[2][3] = 0;
/*770.860676 0.000000 342.122410 0.000000
0.000000 770.860676 258.604725 0.000000
0.000000 0.000000 1.000000 0.000000*/
proj2[0][0] = 770.860676;
proj2[0][1] = 0.000000;
proj2[0][2] = 342.122410;
proj2[0][3] = 0;
proj2[1][0] = 0.00;
proj2[1][1] = 770.860676;
proj2[1][2] = 258.604725;
proj2[1][3] = 0;
proj2[2][0] =0 ;
proj2[2][1] = -136.36;
proj2[2][2] = 1;
proj2[2][3] = 0;
/*0.999962 -0.007298 0.004851
0.006902 0.996990 0.077220
-0.005400 -0.077183 0.997002*/
rect1[0][0] = 0.999962;
rect1[0][1] = -0.007298;
rect1[0][2] = 0.004851;
rect1[0][3] = 0;
rect1[1][0] = 0.006902;
rect1[1][1] = 0.996990;
rect1[1][2] = 0.077220;
rect1[1][3] = 0;
rect1[2][0] = -0.005400;
rect1[2][1] = -0.077183;
rect1[2][2] = 0.997002;
rect1[2][3] = 0;
/*0.999979 -0.003874 -0.005279
0.004258 0.997179 0.074938
0.004974 -0.074959 0.997174*/
rect2[0][0] = 0.999979;
rect2[0][1] = -0.003874;
rect2[0][2] = -0.005279;
rect2[0][3] = 0;
rect2[1][0] = 0.004258;
rect2[1][1] = 0.997179;
rect2[1][2] = 0.074938;
rect2[1][3] = 0;
rect2[2][0] = 0.004974 ;
rect2[2][1] = -0.074959;
rect2[2][2] = 0.997174;
rect2[2][3] = 0;
/*761.095626 0.000000 307.990547 0.000000
0.000000 761.095626 189.171375 0.000000
0.000000 0.000000 1.000000 0.000000*/
proj1[0][0] = 761.095626;
proj1[0][1] = 0.000000;
proj1[0][2] = 307.9905;
proj1[0][3] = 0;
proj1[1][0] = 0.00;
proj1[1][1] = 761.095;
proj1[1][2] = 189.17;
proj1[1][3] = 0;
proj1[2][0] =0 ;
proj1[2][1] = 0;
proj1[2][2] = 1;
proj1[2][3] = 0;
proj1[0][0] = 1;
proj1[0][1] = 0.000000;
proj1[0][2] = 0;
proj1[0][3] = 0;
proj1[1][0] = 0.00;
proj1[1][1] = 1;
proj1[1][2] = 0;
proj1[1][3] = 0;
proj1[2][0] = 0;
proj1[2][1] = 0;
proj1[2][2] = 1;
proj1[2][3] = 0;
/*770.860676 0.000000 342.122410 0.000000
0.000000 770.860676 258.604725 0.000000
0.000000 0.000000 1.000000 0.000000*/
/*proj2[0][0] = 761.095626;
proj2[0][1] = 0.000000;
proj2[0][2] = 307.9905;
proj2[0][3] = 0;
proj2[1][0] = 0.00;
proj2[1][1] = 761.095;
proj2[1][2] = 189.17;
proj2[1][3] = 135.47;
proj2[2][0] =0 ;
proj2[2][1] = 0;
proj2[2][2] = 1;
proj2[2][3] = 0;*/
proj2[0][0] = 1;
proj2[0][1] = 0.000000;
proj2[0][2] = 0;
proj2[0][3] = 0;
proj2[1][0] = 0.00;
proj2[1][1] = 1;
proj2[1][2] = 0;
proj2[1][3] = 135.47/761.09;
proj2[2][0] = 0;
proj2[2][1] = 0;
proj2[2][2] = 1;
proj2[2][3] = 0;
vpRotationMatrix K1;
vpRotationMatrix K2;
/*733.087064 0.000000 315.021816
0.000000 733.060377 240.070864
0.000000 0.000000 1.000000*/
K2[0][0] = 733.08;
K2[0][1] = 0.000000;
K2[0][2] = 315.02;
K2[1][0] = 0.00;
K2[1][1] = 733.06;
K2[1][2] = 240.0708;
K2[2][0] = 0;
K2[2][1] = 0;
K2[2][2] = 1;
/*729.503218 0.000000 303.279696
0.000000 729.585951 259.287176
0.000000 0.000000 1.000000*/
K1[0][0] = 729.50;
K1[0][1] = 0.000000;
K1[0][2] = 303.279;
K1[1][0] = 0.00;
K1[1][1] = 729.5859;
K1[1][2] = 259.2871;
K1[2][0] = 0;
K1[2][1] = 0;
K1[2][2] = 1;
std::cout << " intr matrix 1 " << proj1*rect1.inverse() << std::endl;
std::cout << " intr matrix 2 " << proj1.inverse()*proj2<< std::endl;
std::cout << " external matrix " << rect2*proj2.inverse()*proj1*rect1.inverse() << std::endl;
std::cout << " calib " << c2Mb*c1Mb.inverse() << " " << std::endl;
std::cout << std::endl << "Output: hand to eye calibration result: eMc estimated " << std::endl ;
std::cout << eMc.inverse() << std::endl ;
std::cout << eMc << std::endl ;
eMc.extract(erc);
std::cout << "Theta U rotation: " << vpMath::deg(erc[0]) << " " << vpMath::deg(erc[1]) << " " << vpMath::deg(erc[2]) << std::endl;
return 0 ;
}
catch(vpException e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1 ;
}
}
int main()
{
try {
vpHomogeneousMatrix cdMo(0, 0, 0.75, 0, 0, 0);
vpHomogeneousMatrix cMo(0.15, -0.1, 1., vpMath::rad(10), vpMath::rad(-10), vpMath::rad(50));
// Define the target as 4 points
vpPoint point[4] ;
point[0].setWorldCoordinates(-0.1,-0.1, 0);
point[1].setWorldCoordinates( 0.1,-0.1, 0);
point[2].setWorldCoordinates( 0.1, 0.1, 0);
point[3].setWorldCoordinates(-0.1, 0.1, 0);
#if defined(VISP_HAVE_OGRE)
// Color image used as background texture.
vpImage<unsigned char> background(480, 640, 255);
// Parameters of our camera
vpCameraParameters cam(840, 840, background.getWidth()/2, background.getHeight()/2);
// Our object
// A simulator with the camera parameters defined above,
// and the background image size
vpAROgre ogre;
ogre.setShowConfigDialog(false);
ogre.setCameraParameters(cam);
ogre.addResource("./"); // Add the path to the Sphere.mesh resource
ogre.init(background, false, true);
// Create the scene that contains 4 spheres
// Sphere.mesh contains a sphere with 1 meter radius
std::vector<std::string> name(4);
for (unsigned int i=0; i<4; i++) {
std::ostringstream s; s << "Sphere" << i; name[i] = s.str();
ogre.load(name[i], "Sphere.mesh");
ogre.setScale(name[i], 0.02f, 0.02f, 0.02f); // Rescale the sphere to 2 cm radius
// Set the position of each sphere in the object frame
ogre.setPosition(name[i], vpTranslationVector(point[i].get_oX(), point[i].get_oY(), point[i].get_oZ()));
}
// Add an optional point light source
Ogre::Light * light = ogre.getSceneManager()->createLight();
light->setDiffuseColour(1, 1, 1); // scaled RGB values
light->setSpecularColour(1, 1, 1); // scaled RGB values
light->setPosition((Ogre::Real)cdMo[0][3], (Ogre::Real)cdMo[1][3], (Ogre::Real)(-cdMo[2][3]));
light->setType(Ogre::Light::LT_POINT);
#endif
vpServo task ;
task.setServo(vpServo::EYEINHAND_CAMERA);
task.setInteractionMatrixType(vpServo::CURRENT);
task.setLambda(0.5);
vpFeaturePoint p[4], pd[4] ;
for (int i = 0 ; i < 4 ; i++) {
point[i].track(cdMo);
vpFeatureBuilder::create(pd[i], point[i]);
point[i].track(cMo);
vpFeatureBuilder::create(p[i], point[i]);
task.addFeature(p[i], pd[i]);
}
vpHomogeneousMatrix wMc, wMo;
vpSimulatorCamera robot;
robot.setSamplingTime(0.040);
robot.getPosition(wMc);
wMo = wMc * cMo;
for (unsigned int iter=0; iter < 150; iter ++) {
robot.getPosition(wMc);
cMo = wMc.inverse() * wMo;
for (int i = 0 ; i < 4 ; i++) {
point[i].track(cMo);
vpFeatureBuilder::create(p[i], point[i]);
}
#if defined(VISP_HAVE_OGRE)
// Update the scene from the new camera position
ogre.display(background, cMo);
#endif
vpColVector v = task.computeControlLaw();
robot.setVelocity(vpRobot::CAMERA_FRAME, v);
vpTime::wait( robot.getSamplingTime() * 1000);
}
task.kill();
}
catch(vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
}
catch(...) {
std::cout << "Catch an exception " << std::endl;
}
}
static
void *mainLoop (void *_simu)
{
// pointer copy of the vpSimulator instance
vpSimulator *simu = (vpSimulator *)_simu ;
// Simulation initialization
simu->initMainApplication() ;
/////////////////////////////////////////
// sets the initial camera location
vpHomogeneousMatrix cMo(-0.3,-0.2,3,
vpMath::rad(0),vpMath::rad(0),vpMath::rad(40)) ;
///////////////////////////////////
// Initialize the robot
vpRobotCamera robot ;
robot.setSamplingTime(0.04); // 40ms
robot.setPosition(cMo) ;
// Send the robot position to the visualizator
simu->setCameraPosition(cMo) ;
// Initialize the camera parameters
vpCameraParameters cam ;
simu->getCameraParameters(cam);
////////////////////////////////////////
// Desired visual features initialization
// sets the points coordinates in the object frame (in meter)
vpPoint point[4] ;
point[0].setWorldCoordinates(-0.1,-0.1,0) ;
point[1].setWorldCoordinates(0.1,-0.1,0) ;
point[2].setWorldCoordinates(0.1,0.1,0) ;
point[3].setWorldCoordinates(-0.1,0.1,0) ;
// sets the desired camera location
vpHomogeneousMatrix cMo_d(0,0,1,0,0,0) ;
// computes the 3D point coordinates in the camera frame and its 2D coordinates
for (int i = 0 ; i < 4 ; i++)
point[i].project(cMo_d) ;
// creates the associated features
vpFeaturePoint pd[4] ;
for (int i = 0 ; i < 4 ; i++)
vpFeatureBuilder::create(pd[i],point[i]) ;
///////////////////////////////////////
// Current visual features initialization
// computes the 3D point coordinates in the camera frame and its 2D coordinates
for (int i = 0 ; i < 4 ; i++)
point[i].project(cMo) ;
// creates the associated features
vpFeaturePoint p[4] ;
for (int i = 0 ; i < 4 ; i++)
vpFeatureBuilder::create(p[i],point[i]) ;
/////////////////////////////////
// Task defintion
vpServo task ;
// we want an eye-in-hand control law ;
task.setServo(vpServo::EYEINHAND_L_cVe_eJe) ;
task.setInteractionMatrixType(vpServo::DESIRED,vpServo::PSEUDO_INVERSE) ;
// Set the position of the camera in the end-effector frame
vpHomogeneousMatrix cMe ;
vpVelocityTwistMatrix cVe(cMe) ;
task.set_cVe(cVe) ;
// Set the Jacobian (expressed in the end-effector frame)
vpMatrix eJe ;
robot.get_eJe(eJe) ;
task.set_eJe(eJe) ;
// we want to see a point on a point
for (int i = 0 ; i < 4 ; i++)
task.addFeature(p[i],pd[i]) ;
// Set the gain
task.setLambda(1.0) ;
// Print the current information about the task
task.print();
vpTime::wait(500);
////////////////////////////////////////////////
// The control loop
int k = 0;
while(k++ < 200){
double t = vpTime::measureTimeMs();
// Update the current features
for (int i = 0 ; i < 4 ; i++)
{
point[i].project(cMo) ;
vpFeatureBuilder::create(p[i],point[i]) ;
}
// Update the robot Jacobian
robot.get_eJe(eJe) ;
task.set_eJe(eJe) ;
// Compute the control law
vpColVector v = task.computeControlLaw() ;
// Send the computed velocity to the robot and compute the new robot position
robot.setVelocity(vpRobot::ARTICULAR_FRAME, v) ;
robot.getPosition(cMo) ;
// Send the robot position to the visualizator
simu->setCameraPosition(cMo) ;
// Print the current information about the task
task.print();
// Wait 40 ms
vpTime::wait(t,40);
}
task.kill();
simu->closeMainApplication() ;
void *a=NULL ;
return a ;
// return (void *);
}
int main()
{
// We want to calibrate the hand to eye extrinsic camera parameters from 6 couple of poses: cMo and wMe
const int N = 6;
// Input: six couple of poses used as input in the calibration proces
std::vector<vpHomogeneousMatrix> cMo(N) ; // eye (camera) to object transformation. The object frame is attached to the calibrartion grid
std::vector<vpHomogeneousMatrix> wMe(N) ; // world to hand (end-effector) transformation
// Output: Result of the calibration
vpHomogeneousMatrix eMc; // hand (end-effector) to eye (camera) transformation
// Initialize an eMc transformation used to produce the simulated input transformations cMo and wMe
vpTranslationVector etc(0.1, 0.2, 0.3);
vpThetaUVector erc;
erc[0] = vpMath::rad(10); // 10 deg
erc[1] = vpMath::rad(-10); // -10 deg
erc[2] = vpMath::rad(25); // 25 deg
eMc.buildFrom(etc, erc);
std::cout << "Simulated hand to eye transformation: eMc " << std::endl ;
std::cout << eMc << std::endl ;
std::cout << "Theta U rotation: " << vpMath::deg(erc[0]) << " " << vpMath::deg(erc[1]) << " " << vpMath::deg(erc[2]) << std::endl;
vpColVector v_c(6) ; // camera velocity used to produce 6 simulated poses
for (int i=0 ; i < N ; i++)
{
v_c = 0 ;
if (i==0) {
// Initialize first poses
cMo[0].buildFrom(0, 0, 0.5, 0, 0, 0); // z=0.5 m
wMe[0].buildFrom(0, 0, 0, 0, 0, 0); // Id
}
else if (i==1)
v_c[3] = M_PI/8 ;
else if (i==2)
v_c[4] = M_PI/8 ;
else if (i==3)
v_c[5] = M_PI/10 ;
else if (i==4)
v_c[0] = 0.5 ;
else if (i==5)
v_c[1] = 0.8 ;
vpHomogeneousMatrix cMc; // camera displacement
cMc = vpExponentialMap::direct(v_c) ; // Compute the camera displacement due to the velocity applied to the camera
if (i > 0) {
// From the camera displacement cMc, compute the wMe and cMo matrices
cMo[i] = cMc.inverse() * cMo[i-1];
wMe[i] = wMe[i-1] * eMc * cMc * eMc.inverse();
}
}
if (0) {
for (int i=0 ; i < N ; i++) {
vpHomogeneousMatrix wMo;
wMo = wMe[i] * eMc * cMo[i];
std::cout << std::endl << "wMo[" << i << "] " << std::endl ;
std::cout << wMo << std::endl ;
std::cout << "cMo[" << i << "] " << std::endl ;
std::cout << cMo[i] << std::endl ;
std::cout << "wMe[" << i << "] " << std::endl ;
std::cout << wMe[i] << std::endl ;
}
}
// Reset the eMc matrix to eye
eMc.eye();
// Compute the eMc hand to eye transformation from six poses
// - cMo[6]: camera to object poses as six homogeneous transformations
// - wMe[6]: world to hand (end-effector) poses as six homogeneous transformations
vpCalibration::calibrationTsai(cMo, wMe, eMc) ;
std::cout << std::endl << "Output: hand to eye calibration result: eMc estimated " << std::endl ;
std::cout << eMc << std::endl ;
eMc.extract(erc);
std::cout << "Theta U rotation: " << vpMath::deg(erc[0]) << " " << vpMath::deg(erc[1]) << " " << vpMath::deg(erc[2]) << std::endl;
return 0 ;
}
int
main(int argc, const char ** argv)
{
try {
bool opt_display = true;
bool opt_click_allowed = true;
// Read the command line options
if (getOptions(argc, argv, opt_click_allowed, opt_display) == false) {
exit (-1);
}
vpImage<unsigned char> I(512,512,0) ;
// We open a window using either X11, GTK or GDI.
#if defined VISP_HAVE_X11
vpDisplayX display;
#elif defined VISP_HAVE_GTK
vpDisplayGTK display;
#elif defined VISP_HAVE_GDI
vpDisplayGDI display;
#elif defined VISP_HAVE_OPENCV
vpDisplayOpenCV display;
#endif
if (opt_display) {
try{
// Display size is automatically defined by the image (I) size
display.init(I, 100, 100,"Camera view...") ;
// Display the image
// The image class has a member that specify a pointer toward
// the display that has been initialized in the display declaration
// therefore is is no longuer necessary to make a reference to the
// display variable.
vpDisplay::display(I) ;
vpDisplay::flush(I) ;
}
catch(...)
{
vpERROR_TRACE("Error while displaying the image") ;
exit(-1);
}
}
// Set the camera parameters
double px, py ; px = py = 600 ;
double u0, v0 ; u0 = v0 = 256 ;
vpCameraParameters cam(px,py,u0,v0);
vpServo task ;
vpSimulatorCamera robot ;
// sets the initial camera location
vpHomogeneousMatrix cMo(0.2,0.2,1,
vpMath::rad(45), vpMath::rad(45), vpMath::rad(125));
// Compute the position of the object in the world frame
vpHomogeneousMatrix wMc, wMo;
robot.getPosition(wMc) ;
wMo = wMc * cMo;
// sets the final camera location (for simulation purpose)
vpHomogeneousMatrix cMod(0,0,1,
vpMath::rad(0), vpMath::rad(0), vpMath::rad(0));
int nbline = 4;
// sets the line coordinates (2 planes) in the world frame
vpLine line[4] ;
line[0].setWorldCoordinates(1,0,0,0.05,0,0,1,0);
line[1].setWorldCoordinates(0,1,0,0.05,0,0,1,0);
line[2].setWorldCoordinates(1,0,0,-0.05,0,0,1,0);
line[3].setWorldCoordinates(0,1,0,-0.05,0,0,1,0);
vpFeatureLine ld[4] ;
vpFeatureLine l[4] ;
// sets the desired position of the visual feature
for(int i = 0; i < nbline; i++)
{
line[i].track(cMod) ;
line[i].print() ;
vpFeatureBuilder::create(ld[i],line[i]) ;
}
// computes the line coordinates in the camera frame and its 2D coordinates
// sets the current position of the visual feature
for(int i = 0; i < nbline; i++)
{
line[i].track(cMo) ;
line[i].print() ;
vpFeatureBuilder::create(l[i],line[i]) ;
l[i].print() ;
}
// define the task
// - we want an eye-in-hand control law
// - robot is controlled in the camera frame
task.setServo(vpServo::EYEINHAND_CAMERA) ;
task.setInteractionMatrixType(vpServo::CURRENT, vpServo::PSEUDO_INVERSE);
//It could be also interesting to test the following tasks
//task.setInteractionMatrixType(vpServo::DESIRED, vpServo::PSEUDO_INVERSE);
//task.setInteractionMatrixType(vpServo::MEAN, vpServo::PSEUDO_INVERSE);
// we want to see a four lines on four lines
for(int i = 0; i < nbline; i++)
task.addFeature(l[i],ld[i]) ;
vpDisplay::display(I) ;
vpServoDisplay::display(task,cam,I) ;
vpDisplay::flush(I) ;
// set the gain
task.setLambda(1) ;
// Display task information
task.print() ;
if (opt_display && opt_click_allowed) {
std::cout << "\n\nClick in the camera view window to start..." << std::endl;
vpDisplay::getClick(I) ;
}
unsigned int iter=0 ;
// loop
while(iter++<200)
{
std::cout << "---------------------------------------------" << iter <<std::endl ;
vpColVector v ;
// get the robot position
robot.getPosition(wMc) ;
// Compute the position of the camera wrt the object frame
cMo = wMc.inverse() * wMo;
// new line position: retrieve x,y and Z of the vpLine structure
for(int i = 0; i < nbline; i++)
{
line[i].track(cMo) ;
vpFeatureBuilder::create(l[i],line[i]);
}
if (opt_display) {
vpDisplay::display(I) ;
vpServoDisplay::display(task,cam,I) ;
vpDisplay::flush(I) ;
}
// compute the control law
v = task.computeControlLaw() ;
// send the camera velocity to the controller
robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
std::cout << "|| s - s* || = " << ( task.getError() ).sumSquare() <<std::endl ; ;
}
if (opt_display && opt_click_allowed) {
std::cout << "\nClick in the camera view window to end..." << std::endl;
vpDisplay::getClick(I) ;
}
// Display task information
task.print() ;
task.kill();
return 0;
}
catch(vpException e) {
std::cout << "Catch a ViSP exception: " << e << std::endl;
return 1;
}
}
int
main(int argc, const char ** argv)
{
try {
bool opt_click_allowed = true;
bool opt_display = true;
// Read the command line options
if (getOptions(argc, argv, opt_click_allowed, opt_display) == false) {
exit (-1);
}
// We open two displays, one for the internal camera view, the other one for
// the external view, using either X11, GTK or GDI.
#if defined VISP_HAVE_X11
vpDisplayX displayInt;
#elif defined VISP_HAVE_GDI
vpDisplayGDI displayInt;
#elif defined VISP_HAVE_OPENCV
vpDisplayOpenCV displayInt;
#endif
vpImage<unsigned char> Iint(480, 640, 255);
if (opt_display) {
// open a display for the visualization
displayInt.init(Iint,700,0, "Internal view") ;
}
vpServo task;
std::cout << std::endl ;
std::cout << "----------------------------------------------" << std::endl ;
std::cout << " Test program for vpServo " <<std::endl ;
std::cout << " Eye-in-hand task control, articular velocity are computed"
<< std::endl ;
std::cout << " Simulation " << std::endl ;
std::cout << " task : servo 4 points " << std::endl ;
std::cout << "----------------------------------------------" << std::endl ;
std::cout << std::endl ;
// sets the initial camera location
vpHomogeneousMatrix cMo(-0.05,-0.05,0.7,
vpMath::rad(10), vpMath::rad(10), vpMath::rad(-30));
// sets the point coordinates in the object frame
vpPoint point[4] ;
point[0].setWorldCoordinates(-0.045,-0.045,0) ;
point[3].setWorldCoordinates(-0.045,0.045,0) ;
point[2].setWorldCoordinates(0.045,0.045,0) ;
point[1].setWorldCoordinates(0.045,-0.045,0) ;
// computes the point coordinates in the camera frame and its 2D coordinates
for (unsigned int i = 0 ; i < 4 ; i++)
point[i].track(cMo) ;
// sets the desired position of the point
vpFeaturePoint p[4] ;
for (unsigned int i = 0 ; i < 4 ; i++)
vpFeatureBuilder::create(p[i],point[i]) ; //retrieve x,y and Z of the vpPoint structure
// sets the desired position of the feature point s*
vpFeaturePoint pd[4] ;
// Desired pose
vpHomogeneousMatrix cdMo(vpHomogeneousMatrix(0.0,0.0,0.8,vpMath::rad(0),vpMath::rad(0),vpMath::rad(0)));
// Projection of the points
for (unsigned int i = 0 ; i < 4 ; i++)
point[i].track(cdMo);
for (unsigned int i = 0 ; i < 4 ; i++)
vpFeatureBuilder::create(pd[i], point[i]);
// define the task
// - we want an eye-in-hand control law
// - articular velocity are computed
task.setServo(vpServo::EYEINHAND_CAMERA);
task.setInteractionMatrixType(vpServo::DESIRED) ;
// we want to see a point on a point
for (unsigned int i = 0 ; i < 4 ; i++)
task.addFeature(p[i],pd[i]) ;
// set the gain
task.setLambda(0.8) ;
// Declaration of the robot
vpSimulatorAfma6 robot(opt_display);
// Initialise the robot and especially the camera
robot.init(vpAfma6::TOOL_CCMOP, vpCameraParameters::perspectiveProjWithoutDistortion);
robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
// Initialise the object for the display part*/
robot.initScene(vpWireFrameSimulator::PLATE, vpWireFrameSimulator::D_STANDARD);
// Initialise the position of the object relative to the pose of the robot's camera
robot.initialiseObjectRelativeToCamera(cMo);
// Set the desired position (for the displaypart)
robot.setDesiredCameraPosition(cdMo);
// Get the internal robot's camera parameters
vpCameraParameters cam;
robot.getCameraParameters(cam,Iint);
if (opt_display)
{
//Get the internal view
vpDisplay::display(Iint);
robot.getInternalView(Iint);
vpDisplay::flush(Iint);
}
// Display task information
task.print() ;
unsigned int iter=0 ;
vpTRACE("\t loop") ;
while(iter++<500)
{
std::cout << "---------------------------------------------" << iter <<std::endl ;
vpColVector v ;
// Get the Time at the beginning of the loop
double t = vpTime::measureTimeMs();
// Get the current pose of the camera
cMo = robot.get_cMo();
if (iter==1) {
std::cout <<"Initial robot position with respect to the object frame:\n";
cMo.print();
}
// new point position
for (unsigned int i = 0 ; i < 4 ; i++)
{
point[i].track(cMo) ;
// retrieve x,y and Z of the vpPoint structure
vpFeatureBuilder::create(p[i],point[i]) ;
}
if (opt_display)
{
// Get the internal view and display it
vpDisplay::display(Iint) ;
robot.getInternalView(Iint);
vpDisplay::flush(Iint);
}
if (opt_display && opt_click_allowed && iter == 1)
{
// suppressed for automate test
std::cout << "Click in the internal view window to continue..." << std::endl;
vpDisplay::getClick(Iint) ;
}
// compute the control law
v = task.computeControlLaw() ;
// send the camera velocity to the controller
robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
std::cout << "|| s - s* || " << ( task.getError() ).sumSquare() <<std::endl ;
// The main loop has a duration of 10 ms at minimum
vpTime::wait(t,10);
}
// Display task information
task.print() ;
task.kill();
std::cout <<"Final robot position with respect to the object frame:\n";
cMo.print();
if (opt_display && opt_click_allowed)
{
// suppressed for automate test
std::cout << "Click in the internal view window to end..." << std::endl;
vpDisplay::getClick(Iint) ;
}
return 0;
}
catch(vpException e) {
std::cout << "Catch a ViSP exception: " << e << std::endl;
return 1;
}
}
int main()
{
#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)
try {
vpHomogeneousMatrix cdMo(0, 0, 0.75, 0, 0, 0);
vpHomogeneousMatrix cMo(0.15, -0.1, 1., vpMath::rad(10), vpMath::rad(-10), vpMath::rad(50));
vpImage<unsigned char> I(480, 640, 255);
vpCameraParameters cam(840, 840, I.getWidth()/2, I.getHeight()/2);
std::vector<vpPoint> point(4) ;
point[0].setWorldCoordinates(-0.1,-0.1, 0);
point[1].setWorldCoordinates( 0.1,-0.1, 0);
point[2].setWorldCoordinates( 0.1, 0.1, 0);
point[3].setWorldCoordinates(-0.1, 0.1, 0);
vpServo task ;
task.setServo(vpServo::EYEINHAND_CAMERA);
task.setInteractionMatrixType(vpServo::CURRENT);
task.setLambda(0.5);
vpVirtualGrabber g("./target_square.pgm", cam);
g.acquire(I, cMo);
#if defined(VISP_HAVE_X11)
vpDisplayX d(I, 0, 0, "Current camera view");
#elif defined(VISP_HAVE_GDI)
vpDisplayGDI d(I, 0, 0, "Current camera view");
#elif defined(VISP_HAVE_OPENCV)
vpDisplayOpenCV d(I, 0, 0, "Current camera view");
#else
std::cout << "No image viewer is available..." << std::endl;
#endif
vpDisplay::display(I);
vpDisplay::displayText(I, 10, 10,
"Click in the 4 dots to initialise the tracking and start the servo",
vpColor::red);
vpDisplay::flush(I);
vpFeaturePoint p[4], pd[4];
std::vector<vpDot2> dot(4);
for (unsigned int i = 0 ; i < 4 ; i++) {
point[i].track(cdMo);
vpFeatureBuilder::create(pd[i], point[i]);
dot[i].setGraphics(true);
dot[i].initTracking(I);
vpDisplay::flush(I);
vpFeatureBuilder::create(p[i], cam, dot[i].getCog());
task.addFeature(p[i], pd[i]);
}
vpHomogeneousMatrix wMc, wMo;
vpSimulatorCamera robot;
robot.setSamplingTime(0.040);
robot.getPosition(wMc);
wMo = wMc * cMo;
for (; ; ) {
robot.getPosition(wMc);
cMo = wMc.inverse() * wMo;
g.acquire(I, cMo);
vpDisplay::display(I);
for (unsigned int i = 0 ; i < 4 ; i++) {
dot[i].track(I);
vpFeatureBuilder::create(p[i], cam, dot[i].getCog());
vpColVector cP;
point[i].changeFrame(cMo, cP) ;
p[i].set_Z(cP[2]);
}
vpColVector v = task.computeControlLaw();
display_trajectory(I, dot);
vpServoDisplay::display(task, cam, I, vpColor::green, vpColor::red) ;
robot.setVelocity(vpRobot::CAMERA_FRAME, v);
vpDisplay::flush(I);
if (vpDisplay::getClick(I, false))
break;
vpTime::wait( robot.getSamplingTime() * 1000);
}
task.kill();
}
catch(vpException e) {
std::cout << "Catch an exception: " << e << std::endl;
}
#endif
}
int main()
{
//////////////////////////////////////////
// sets the initial camera location
vpHomogeneousMatrix cMo(0.3,0.2,3,
vpMath::rad(0),vpMath::rad(0),vpMath::rad(40)) ;
///////////////////////////////////
// initialize the robot
vpRobotCamera robot ;
robot.setSamplingTime(0.04); // 40ms
robot.setPosition(cMo) ;
//initialize the camera parameters
vpCameraParameters cam(800,800,240,180);
//Image definition
unsigned int height = 360 ;
unsigned int width = 480 ;
vpImage<unsigned char> I(height,width);
//Display initialization
vpDisplayGTK disp;
disp.init(I,100,100,"Simulation display");
////////////////////////////////////////
// Desired visual features initialization
// sets the points coordinates in the object frame (in meter)
vpPoint point[4] ;
point[0].setWorldCoordinates(-0.1,-0.1,0) ;
point[1].setWorldCoordinates(0.1,-0.1,0) ;
point[2].setWorldCoordinates(0.1,0.1,0) ;
point[3].setWorldCoordinates(-0.1,0.1,0) ;
// sets the desired camera location
vpHomogeneousMatrix cMo_d(0,0,1,0,0,0) ;
// computes the 3D point coordinates in the camera frame and its 2D coordinates
for (int i = 0 ; i < 4 ; i++)
point[i].project(cMo_d) ;
// creates the associated features
vpFeaturePoint pd[4] ;
for (int i = 0 ; i < 4 ; i++)
vpFeatureBuilder::create(pd[i],point[i]) ;
///////////////////////////////////////
// Current visual features initialization
// computes the 3D point coordinates in the camera frame and its 2D coordinates
for (int i = 0 ; i < 4 ; i++)
point[i].project(cMo) ;
// creates the associated features
vpFeaturePoint p[4] ;
for (int i = 0 ; i < 4 ; i++)
vpFeatureBuilder::create(p[i],point[i]) ;
/////////////////////////////////
// Task defintion
vpServo task ;
// we want an eye-in-hand control law ;
task.setServo(vpServo::EYEINHAND_L_cVe_eJe) ;
task.setInteractionMatrixType(vpServo::DESIRED, vpServo::PSEUDO_INVERSE) ;
// Set the position of the camera in the end-effector frame
vpHomogeneousMatrix cMe ;
vpVelocityTwistMatrix cVe(cMe) ;
task.set_cVe(cVe) ;
// Set the Jacobian (expressed in the end-effector frame)
vpMatrix eJe ;
robot.get_eJe(eJe) ;
task.set_eJe(eJe) ;
// we want to see a point on a point
for (int i = 0 ; i < 4 ; i++)
task.addFeature(p[i],pd[i]) ;
// Set the gain
task.setLambda(1.0) ;
// Print the current information about the task
task.print();
////////////////////////////////////////////////
// The control loop
int k = 0;
while(k++ < 200){
double t = vpTime::measureTimeMs();
// Display the image background
vpDisplay::display(I);
// Update the current features
for (int i = 0 ; i < 4 ; i++)
{
point[i].project(cMo) ;
vpFeatureBuilder::create(p[i],point[i]) ;
}
// Display the task features (current and desired)
vpServoDisplay::display(task,cam,I);
vpDisplay::flush(I);
// Update the robot Jacobian
robot.get_eJe(eJe) ;
task.set_eJe(eJe) ;
// Compute the control law
vpColVector v = task.computeControlLaw() ;
// Send the computed velocity to the robot and compute the new robot position
robot.setVelocity(vpRobot::ARTICULAR_FRAME, v) ;
robot.getPosition(cMo) ;
// Print the current information about the task
task.print();
// Wait 40 ms
vpTime::wait(t,40);
}
task.kill();
return 0;
}
int main(int argc, char **argv)
{
#if defined(VISP_HAVE_DC1394) || defined(VISP_HAVE_V4L2) || defined(VISP_HAVE_CMU1394)
#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)
try {
vpImage<unsigned char> I; // Create a gray level image container
double lambda = 0.1;
// Scale parameter used to estimate the depth Z of the blob from its surface
//double coef = 0.9/14.85; // At 0.9m, the blob has a surface of 14.85 (Logitec sphere)
double coef = 1.2/13.0; // At 1m, the blob has a surface of 11.3 (AVT Pike 032C)
double L = 0.21; // 3D horizontal segment length
double Z_d = 0.8; // Desired distance along Z between camera and segment
bool normalized = true; // segment normilized features are used
// Warning: To have a non singular task of rank 3, Y_d should be different from 0 so that
// the optical axis doesn't intersect the horizontal segment
double Y_d = -.11; // Desired distance along Y between camera and segment.
vpColVector qm(2); // Measured head position
qm = 0;
double qm_pan = 0; // Measured pan position (tilt is not handled in that example)
#ifdef USE_REAL_ROBOT
// Initialize the biclops head
vpRobotBiclops biclops("/usr/share/BiclopsDefault.cfg");
biclops.setDenavitHartenbergModel(vpBiclops::DH1);
// Move to the initial position
vpColVector q(2);
q=0;
// q[0] = vpMath::rad(63);
// q[1] = vpMath::rad(12); // introduce a tilt angle to compensate camera sphere tilt so that the camera is parallel to the plane
biclops.setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
biclops.setPosition( vpRobot::ARTICULAR_FRAME, q );
//biclops.setPositioningVelocity(50);
biclops.getPosition(vpRobot::ARTICULAR_FRAME, qm);
qm_pan = qm[0];
// Now the head will be controlled in velocity
biclops.setRobotState(vpRobot::STATE_VELOCITY_CONTROL) ;
// Initialize the pioneer robot
vpRobotPioneer pioneer;
ArArgumentParser parser(&argc, argv);
parser.loadDefaultArguments();
// ArRobotConnector connects to the robot, get some initial data from it such as type and name,
// and then loads parameter files for this robot.
ArRobotConnector robotConnector(&parser, &pioneer);
if(!robotConnector.connectRobot())
{
ArLog::log(ArLog::Terse, "Could not connect to the pioneer robot.");
if(parser.checkHelpAndWarnUnparsed())
{
Aria::logOptions();
Aria::exit(1);
}
}
if (!Aria::parseArgs())
{
Aria::logOptions();
Aria::shutdown();
return false;
}
pioneer.useSonar(false); // disable the sonar device usage
// Wait 3 sec to be sure that the low level Aria thread used to control
// the robot is started. Without this delay we experienced a delay (arround 2.2 sec)
// between the velocity send to the robot and the velocity that is really applied
// to the wheels.
sleep(3);
std::cout << "Pioneer robot connected" << std::endl;
#endif
vpPioneerPan robot_pan; // Generic robot that computes the velocities for the pioneer and the biclops head
// Camera parameters. In this experiment we don't need a precise calibration of the camera
vpCameraParameters cam;
// Create the camera framegrabber
#if defined(VISP_HAVE_V4L2)
// Create a grabber based on v4l2 third party lib (for usb cameras under Linux)
vpV4l2Grabber g;
g.setScale(1);
g.setInput(0);
g.setDevice("/dev/video1");
g.open(I);
// Logitec sphere parameters
cam.initPersProjWithoutDistortion(558, 555, 312, 210);
#elif defined(VISP_HAVE_DC1394)
// Create a grabber based on libdc1394-2.x third party lib (for firewire cameras under Linux)
vp1394TwoGrabber g(false);
g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_640x480_MONO8);
g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_30);
// AVT Pike 032C parameters
cam.initPersProjWithoutDistortion(800, 795, 320, 216);
#elif defined(VISP_HAVE_CMU1394)
// Create a grabber based on CMU 1394 third party lib (for firewire cameras under windows)
vp1394CMUGrabber g;
g.setVideoMode(0, 5); // 640x480 MONO8
g.setFramerate(4); // 30 Hz
g.open(I);
// AVT Pike 032C parameters
cam.initPersProjWithoutDistortion(800, 795, 320, 216);
#endif
// Acquire an image from the grabber
g.acquire(I);
// Create an image viewer
#if defined(VISP_HAVE_X11)
vpDisplayX d(I, 10, 10, "Current frame");
#elif defined(VISP_HAVE_GDI)
vpDisplayGDI d(I, 10, 10, "Current frame");
#endif
vpDisplay::display(I);
vpDisplay::flush(I);
// The 3D segment consists in two horizontal dots
vpDot2 dot[2];
for (int i=0; i <2; i++)
{
dot[i].setGraphics(true);
dot[i].setComputeMoments(true);
dot[i].setEllipsoidShapePrecision(0.); // to track a blob without any constraint on the shape
dot[i].setGrayLevelPrecision(0.9); // to set the blob gray level bounds for binarisation
dot[i].setEllipsoidBadPointsPercentage(0.5); // to be accept 50% of bad inner and outside points with bad gray level
dot[i].initTracking(I);
vpDisplay::flush(I);
}
vpServo task;
task.setServo(vpServo::EYEINHAND_L_cVe_eJe) ;
task.setInteractionMatrixType(vpServo::DESIRED, vpServo::PSEUDO_INVERSE) ;
task.setLambda(lambda) ;
vpVelocityTwistMatrix cVe ; // keep to identity
cVe = robot_pan.get_cVe() ;
task.set_cVe(cVe) ;
std::cout << "cVe: \n" << cVe << std::endl;
vpMatrix eJe;
// Update the robot jacobian that depends on the pan position
robot_pan.set_eJe(qm_pan);
// Get the robot jacobian
eJe = robot_pan.get_eJe() ;
task.set_eJe(eJe) ;
std::cout << "eJe: \n" << eJe << std::endl;
// Define a 3D horizontal segment an its cordinates in the image plane
vpPoint P[2];
P[0].setWorldCoordinates(-L/2, 0, 0);
P[1].setWorldCoordinates( L/2, 0, 0);
// Define the desired camera position
vpHomogeneousMatrix cMo(0, Y_d, Z_d, 0, 0, 0); // Here we are in front of the segment
for (int i=0; i <2; i++)
{
P[i].changeFrame(cMo);
P[i].project(); // Here the x,y parameters obtained by perspective projection are computed
}
// Estimate the depth of the segment extremity points
double surface[2];
double Z[2]; // Depth of the segment points
for (int i=0; i<2; i++)
{
// Surface of the blob estimated from the image moment m00 and converted in meters
surface[i] = 1./sqrt(dot[i].m00/(cam.get_px()*cam.get_py()));
// Initial depth of the blob
Z[i] = coef * surface[i] ;
}
// Use here a feature segment builder
vpFeatureSegment s_segment(normalized), s_segment_d(normalized); // From the segment feature we use only alpha
vpFeatureBuilder::create(s_segment, cam, dot[0], dot[1]);
s_segment.setZ1(Z[0]);
s_segment.setZ2(Z[1]);
// Set the desired feature
vpFeatureBuilder::create(s_segment_d, P[0], P[1]);
s_segment.setZ1( P[0].get_Z() ); // Desired depth
s_segment.setZ2( P[1].get_Z() );
task.addFeature(s_segment, s_segment_d,
vpFeatureSegment::selectXc() |
vpFeatureSegment::selectL() |
vpFeatureSegment::selectAlpha());
#ifdef USE_PLOTTER
//Create a window (500 by 500) at position (700, 10) with two graphics
vpPlot graph(2, 500, 500, 700, 10, "Curves...");
//The first graphic contains 3 curve and the second graphic contains 3 curves
graph.initGraph(0,3);
graph.initGraph(1,3);
graph.setTitle(0, "Velocities");
graph.setTitle(1, "Error s-s*");
graph.setLegend(0, 0, "vx");
graph.setLegend(0, 1, "wz");
graph.setLegend(0, 2, "w_pan");
graph.setLegend(1, 0, "xm/l");
graph.setLegend(1, 1, "1/l");
graph.setLegend(1, 2, "alpha");
#endif
vpColVector v; // vz, wx
unsigned int iter = 0;
try
{
while(1)
{
#ifdef USE_REAL_ROBOT
// Get the new pan position
biclops.getPosition(vpRobot::ARTICULAR_FRAME, qm);
#endif
qm_pan = qm[0];
// Acquire a new image
g.acquire(I);
// Set the image as background of the viewer
vpDisplay::display(I);
// Display the desired position of the segment
for (int i=0; i<2; i++)
P[i].display(I, cam, vpColor::red, 3);
// Does the blob tracking
for (int i=0; i<2; i++)
dot[i].track(I);
for (int i=0; i<2; i++)
{
// Surface of the blob estimated from the image moment m00 and converted in meters
surface[i] = 1./sqrt(dot[i].m00/(cam.get_px()*cam.get_py()));
// Initial depth of the blob
Z[i] = coef * surface[i] ;
}
// Update the features
vpFeatureBuilder::create(s_segment, cam, dot[0], dot[1]);
// Update the depth of the point. Useful only if current interaction matrix is used
// when task.setInteractionMatrixType(vpServo::CURRENT, vpServo::PSEUDO_INVERSE) is set
s_segment.setZ1(Z[0]);
s_segment.setZ2(Z[1]);
robot_pan.get_cVe(cVe);
task.set_cVe(cVe);
// Update the robot jacobian that depends on the pan position
robot_pan.set_eJe(qm_pan);
// Get the robot jacobian
eJe = robot_pan.get_eJe();
// Update the jacobian that will be used to compute the control law
task.set_eJe(eJe);
// Compute the control law. Velocities are computed in the mobile robot reference frame
v = task.computeControlLaw();
// std::cout << "-----" << std::endl;
// std::cout << "v: " << v.t() << std::endl;
// std::cout << "error: " << task.getError().t() << std::endl;
// std::cout << "L:\n " << task.getInteractionMatrix() << std::endl;
// std::cout << "eJe:\n " << task.get_eJe() << std::endl;
// std::cout << "cVe:\n " << task.get_cVe() << std::endl;
// std::cout << "L_cVe_eJe:\n" << task.getInteractionMatrix() * task.get_cVe() * task.get_eJe() << std::endl;
// task.print() ;
if (task.getTaskRank() != 3)
std::cout << "Warning: task is of rank " << task.getTaskRank() << std::endl;
#ifdef USE_PLOTTER
graph.plot(0, iter, v); // plot velocities applied to the robot
graph.plot(1, iter, task.getError()); // plot error vector
#endif
#ifdef USE_REAL_ROBOT
// Send the velocity to the robot
vpColVector v_pioneer(2); // vx, wz
v_pioneer[0] = v[0];
v_pioneer[1] = v[1];
vpColVector v_biclops(2); // qdot pan and tilt
v_biclops[0] = v[2];
v_biclops[1] = 0;
std::cout << "Send velocity to the pionner: " << v_pioneer[0] << " m/s "
<< vpMath::deg(v_pioneer[1]) << " deg/s" << std::endl;
std::cout << "Send velocity to the biclops head: " << vpMath::deg(v_biclops[0]) << " deg/s" << std::endl;
pioneer.setVelocity(vpRobot::REFERENCE_FRAME, v_pioneer);
biclops.setVelocity(vpRobot::ARTICULAR_FRAME, v_biclops) ;
#endif
// Draw a vertical line which corresponds to the desired x coordinate of the dot cog
vpDisplay::displayLine(I, 0, cam.get_u0(), 479, cam.get_u0(), vpColor::red);
vpDisplay::flush(I);
// A click in the viewer to exit
if ( vpDisplay::getClick(I, false) )
break;
iter ++;
//break;
}
}
catch(...)
{
}
#ifdef USE_REAL_ROBOT
std::cout << "Ending robot thread..." << std::endl;
pioneer.stopRunning();
// wait for the thread to stop
pioneer.waitForRunExit();
#endif
// Kill the servo task
task.print() ;
task.kill();
}
catch(vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
#endif
#endif
}
int
main(int argc, const char ** argv)
{
bool opt_display = true;
bool opt_click_allowed = true;
// Read the command line options
if (getOptions(argc, argv, opt_click_allowed, opt_display) == false) {
exit (-1);
}
vpImage<unsigned char> I(512,512,0) ;
// We open a window using either X11, GTK or GDI.
#if defined VISP_HAVE_X11
vpDisplayX display;
#elif defined VISP_HAVE_GTK
vpDisplayGTK display;
#elif defined VISP_HAVE_GDI
vpDisplayGDI display;
#endif
if (opt_display) {
try{
// Display size is automatically defined by the image (I) size
display.init(I, 100, 100,"Camera view...") ;
// Display the image
// The image class has a member that specify a pointer toward
// the display that has been initialized in the display declaration
// therefore is is no longuer necessary to make a reference to the
// display variable.
vpDisplay::display(I) ;
vpDisplay::flush(I) ;
}
catch(...)
{
vpERROR_TRACE("Error while displaying the image") ;
exit(-1);
}
}
double px, py ; px = py = 600 ;
double u0, v0 ; u0 = v0 = 256 ;
vpCameraParameters cam(px,py,u0,v0);
vpServo task ;
vpSimulatorCamera robot ;
// sets the initial camera location
vpHomogeneousMatrix cMo(-0.2,0.1,1,
vpMath::rad(5), vpMath::rad(5), vpMath::rad(90));
// Compute the position of the object in the world frame
vpHomogeneousMatrix wMc, wMo;
robot.getPosition(wMc) ;
wMo = wMc * cMo;
// sets the final camera location (for simulation purpose)
vpHomogeneousMatrix cMod(0,0,1,
vpMath::rad(0), vpMath::rad(0), vpMath::rad(0));
// sets the line coordinates (2 planes) in the world frame
vpColVector plane1(4) ;
vpColVector plane2(4) ;
plane1[0] = 0; // z = 0
plane1[1] = 0;
plane1[2] = 1;
plane1[3] = 0;
plane2[0] = 0; // y =0
plane2[1] = 1;
plane2[2] = 0;
plane2[3] = 0;
vpLine line ;
line.setWorldCoordinates(plane1, plane2) ;
// sets the desired position of the visual feature
line.track(cMod) ;
line.print() ;
vpFeatureLine ld ;
vpFeatureBuilder::create(ld,line) ;
// computes the line coordinates in the camera frame and its 2D coordinates
// sets the current position of the visual feature
line.track(cMo) ;
line.print() ;
vpFeatureLine l ;
vpFeatureBuilder::create(l,line) ;
l.print() ;
// define the task
// - we want an eye-in-hand control law
// - robot is controlled in the camera frame
task.setServo(vpServo::EYEINHAND_CAMERA) ;
// we want to see a line on a line
task.addFeature(l,ld) ;
vpDisplay::display(I) ;
vpServoDisplay::display(task,cam,I) ;
vpDisplay::flush(I) ;
// set the gain
task.setLambda(1) ;
// Display task information " ) ;
task.print() ;
if (opt_display && opt_click_allowed) {
std::cout << "\n\nClick in the camera view window to start..." << std::endl;
vpDisplay::getClick(I) ;
}
unsigned int iter=0 ;
// loop
while(iter++<200)
{
std::cout << "---------------------------------------------" << iter <<std::endl ;
vpColVector v ;
// get the robot position
robot.getPosition(wMc) ;
// Compute the position of the camera wrt the object frame
cMo = wMc.inverse() * wMo;
// new line position
line.track(cMo) ;
// retrieve x,y and Z of the vpLine structure
vpFeatureBuilder::create(l,line);
if (opt_display) {
vpDisplay::display(I) ;
vpServoDisplay::display(task,cam,I) ;
vpDisplay::flush(I) ;
}
// compute the control law
v = task.computeControlLaw() ;
// send the camera velocity to the controller
robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
std::cout << "|| s - s* || = " << ( task.getError() ).sumSquare() <<std::endl ;
}
if (opt_display && opt_click_allowed) {
std::cout << "\nClick in the camera view window to end..." << std::endl;
vpDisplay::getClick(I) ;
}
// Display task information
task.print() ;
task.kill();
}
int main()
{
try {
vpHomogeneousMatrix cdMo(0, 0, 0.75, 0, 0, 0);
vpHomogeneousMatrix cMo(0.15, -0.1, 1.,
vpMath::rad(10), vpMath::rad(-10), vpMath::rad(50));
std::vector<vpPoint> point;
point.push_back( vpPoint(-0.1,-0.1, 0) );
point.push_back( vpPoint( 0.1,-0.1, 0) );
point.push_back( vpPoint( 0.1, 0.1, 0) );
point.push_back( vpPoint(-0.1, 0.1, 0) );
vpServo task ;
task.setServo(vpServo::EYEINHAND_CAMERA);
task.setInteractionMatrixType(vpServo::CURRENT);
task.setLambda(0.5);
vpFeaturePoint p[4], pd[4] ;
for (unsigned int i = 0 ; i < 4 ; i++) {
point[i].track(cdMo);
vpFeatureBuilder::create(pd[i], point[i]);
point[i].track(cMo);
vpFeatureBuilder::create(p[i], point[i]);
task.addFeature(p[i], pd[i]);
}
vpHomogeneousMatrix wMc, wMo;
vpSimulatorCamera robot;
robot.setSamplingTime(0.040);
robot.getPosition(wMc);
wMo = wMc * cMo;
vpImage<unsigned char> Iint(480, 640, 255) ;
vpImage<unsigned char> Iext(480, 640, 255) ;
#if defined(VISP_HAVE_X11)
vpDisplayX displayInt(Iint, 0, 0, "Internal view");
vpDisplayX displayExt(Iext, 670, 0, "External view");
#elif defined(VISP_HAVE_GDI)
vpDisplayGDI displayInt(Iint, 0, 0, "Internal view");
vpDisplayGDI displayExt(Iext, 670, 0, "External view");
#elif defined(VISP_HAVE_OPENCV)
vpDisplayOpenCV displayInt(Iint, 0, 0, "Internal view");
vpDisplayOpenCV displayExt(Iext, 670, 0, "External view");
#else
std::cout << "No image viewer is available..." << std::endl;
#endif
#if defined(VISP_HAVE_DISPLAY)
vpProjectionDisplay externalview;
for (unsigned int i = 0 ; i < 4 ; i++)
externalview.insert(point[i]) ;
#endif
vpCameraParameters cam(840, 840, Iint.getWidth()/2, Iint.getHeight()/2);
vpHomogeneousMatrix cextMo(0,0,3, 0,0,0);
while(1) {
robot.getPosition(wMc);
cMo = wMc.inverse() * wMo;
for (unsigned int i = 0 ; i < 4 ; i++) {
point[i].track(cMo);
vpFeatureBuilder::create(p[i], point[i]);
}
vpColVector v = task.computeControlLaw();
robot.setVelocity(vpRobot::CAMERA_FRAME, v);
vpDisplay::display(Iint) ;
vpDisplay::display(Iext) ;
display_trajectory(Iint, point, cMo, cam);
vpServoDisplay::display(task, cam, Iint, vpColor::green, vpColor::red);
#if defined(VISP_HAVE_DISPLAY)
externalview.display(Iext, cextMo, cMo, cam, vpColor::red, true);
#endif
vpDisplay::flush(Iint);
vpDisplay::flush(Iext);
// A click to exit
if (vpDisplay::getClick(Iint, false) || vpDisplay::getClick(Iext, false))
break;
vpTime::wait( robot.getSamplingTime() * 1000);
}
task.kill();
}
catch(vpException e) {
std::cout << "Catch an exception: " << e << std::endl;
}
}
int
main(int argc, const char ** argv)
{
bool opt_display = true;
bool opt_click_allowed = true;
// Read the command line options
if (getOptions(argc, argv, opt_click_allowed, opt_display) == false) {
exit (-1);
}
vpImage<unsigned char> I(512,512,255) ;
// We open a window using either X11, GTK or GDI.
#if defined VISP_HAVE_X11
vpDisplayX display;
#elif defined VISP_HAVE_GTK
vpDisplayGTK display;
#elif defined VISP_HAVE_GDI
vpDisplayGDI display;
#endif
if (opt_display) {
try{
// Display size is automatically defined by the image (I) size
display.init(I, 100, 100,"Camera view...") ;
// Display the image
// The image class has a member that specify a pointer toward
// the display that has been initialized in the display declaration
// therefore is is no longuer necessary to make a reference to the
// display variable.
vpDisplay::display(I) ;
vpDisplay::flush(I) ;
}
catch(...)
{
vpERROR_TRACE("Error while displaying the image") ;
exit(-1);
}
}
double px, py ; px = py = 600 ;
double u0, v0 ; u0 = v0 = 256 ;
vpCameraParameters cam(px,py,u0,v0);
vpServo task ;
vpRobotCamera robot ;
vpTRACE("sets the initial camera location " ) ;
vpHomogeneousMatrix cMo(-0.2,0.1,2,
vpMath::rad(5), vpMath::rad(5), vpMath::rad(20));
robot.setPosition(cMo) ;
vpTRACE("sets the final camera location (for simulation purpose)" ) ;
vpHomogeneousMatrix cMod(0,0,1,
vpMath::rad(-60), vpMath::rad(0), vpMath::rad(0));
vpTRACE("sets the cylinder coordinates in the world frame " ) ;
vpCylinder cylinder(0,1,0, // direction
0,0,0, // point of the axis
0.1) ; // radius
vpTRACE("sets the desired position of the visual feature ") ;
cylinder.track(cMod) ;
cylinder.print() ;
vpFeatureLine ld[2] ;
int i ;
for(i=0 ; i < 2 ; i++)
vpFeatureBuilder::create(ld[i],cylinder,i) ;
vpTRACE("project : computes the cylinder coordinates in the camera frame and its 2D coordinates" ) ;
vpTRACE("sets the current position of the visual feature ") ;
cylinder.track(cMo) ;
cylinder.print() ;
vpFeatureLine l[2] ;
for(i=0 ; i < 2 ; i++)
{
vpFeatureBuilder::create(l[i],cylinder,i) ;
l[i].print() ;
}
vpTRACE("define the task") ;
vpTRACE("\t we want an eye-in-hand control law") ;
vpTRACE("\t robot is controlled in the camera frame") ;
task.setServo(vpServo::EYEINHAND_CAMERA) ;
// task.setInteractionMatrixType(vpServo::CURRENT, vpServo::PSEUDO_INVERSE) ;
// it can also be interesting to test these possibilities
// task.setInteractionMatrixType(vpServo::MEAN, vpServo::PSEUDO_INVERSE) ;
task.setInteractionMatrixType(vpServo::DESIRED,vpServo::PSEUDO_INVERSE) ;
//task.setInteractionMatrixType(vpServo::DESIRED, vpServo::TRANSPOSE) ;
// task.setInteractionMatrixType(vpServo::CURRENT, vpServo::TRANSPOSE) ;
vpTRACE("\t we want to see 2 lines on 2 lines.") ;
task.addFeature(l[0],ld[0]) ;
task.addFeature(l[1],ld[1]) ;
vpServoDisplay::display(task,cam,I) ;
vpDisplay::flush(I) ;
vpTRACE("Display task information " ) ;
task.print() ;
if (opt_display && opt_click_allowed) {
std::cout << "\n\nClick in the camera view window to start..." << std::endl;
vpDisplay::getClick(I) ;
}
vpTRACE("\t set the gain") ;
task.setLambda(1) ;
vpTRACE("Display task information " ) ;
task.print() ;
int iter=0 ;
vpTRACE("\t loop") ;
do
{
std::cout << "---------------------------------------------" << iter++ <<std::endl ;
vpColVector v ;
if (iter==1) vpTRACE("\t\t get the robot position ") ;
robot.getPosition(cMo) ;
if (iter==1) vpTRACE("\t\t new line position ") ;
//retrieve x,y and Z of the vpLine structure
cylinder.track(cMo) ;
// cylinder.print() ;
for(i=0 ; i < 2 ; i++)
{
vpFeatureBuilder::create(l[i],cylinder,i) ;
// l[i].print() ;
}
if (opt_display) {
vpDisplay::display(I) ;
vpServoDisplay::display(task,cam,I) ;
vpDisplay::flush(I) ;
}
if (iter==1) vpTRACE("\t\t compute the control law ") ;
v = task.computeControlLaw() ;
if (iter==1) vpTRACE("\t\t send the camera velocity to the controller ") ;
robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
vpTRACE("\t\t || s - s* || ") ;
std::cout << task.error.sumSquare() <<std::endl ; ;
// vpDisplay::getClick(I) ;
}
while(task.error.sumSquare() > 1e-9) ;
if (opt_display && opt_click_allowed) {
std::cout << "\nClick in the camera view window to end..." << std::endl;
vpDisplay::getClick(I) ;
}
vpTRACE("Display task information " ) ;
task.print() ;
task.kill();
}
static
void *mainLoop (void *_simu)
{
// pointer copy of the vpSimulator instance
vpSimulator *simu = (vpSimulator *)_simu ;
// Simulation initialization
simu->initMainApplication() ;
///////////////////////////////////
// Set the initial camera location
vpHomogeneousMatrix cMo(0.3,0.2,3,
vpMath::rad(0),vpMath::rad(0),vpMath::rad(40));
///////////////////////////////////
// Initialize the robot
vpRobotCamera robot ;
robot.setSamplingTime(0.04); // 40ms
robot.setPosition(cMo) ;
// Send the robot position to the visualizator
simu->setCameraPosition(cMo) ;
// Initialize the camera parameters
vpCameraParameters cam ;
simu->getCameraParameters(cam);
////////////////////////////////////////
// Desired visual features initialization
// sets the points coordinates in the object frame (in meter)
vpPoint point[4] ;
point[0].setWorldCoordinates(-0.1,-0.1,0) ;
point[1].setWorldCoordinates(0.1,-0.1,0) ;
point[2].setWorldCoordinates(0.1,0.1,0) ;
point[3].setWorldCoordinates(-0.1,0.1,0) ;
// sets the desired camera location
vpHomogeneousMatrix cMo_d(0,0,1,0,0,0) ;
// computes the 3D point coordinates in the camera frame and its 2D coordinates
for (int i = 0 ; i < 4 ; i++)
point[i].project(cMo_d) ;
// creates the associated features
vpFeaturePoint pd[4] ;
for (int i = 0 ; i < 4 ; i++)
vpFeatureBuilder::create(pd[i],point[i]) ;
///////////////////////////////////////
// Current visual features initialization
unsigned int height = simu->getInternalHeight();
unsigned int width = simu->getInternalWidth();
// Create a greyscale image
vpImage<unsigned char> I(height,width);
//Display initialization
#if defined(VISP_HAVE_X11)
vpDisplayX disp;
#elif defined(VISP_HAVE_GDI)
vpDisplayGDI disp;
#elif defined(VISP_HAVE_GTK)
vpDisplayGTK disp;
#endif
disp.init(I,100,100,"Simulation display");
// disp(I);
// Get the current image
vpTime::wait(500); // wait to be sure the image is generated
simu->getInternalImage(I);
// Display the current image
vpDisplay::display(I);
vpDisplay::flush(I);
// Initialize the four dots tracker
std::cout << "A click in the four dots clockwise. " << std::endl;
vpDot2 dot[4];
vpFeaturePoint p[4];
for (int i = 0 ; i < 4 ; i++)
{
dot[i].setGraphics(true);
// Call for a click
std::cout << "A click in the dot " << i << std::endl;
dot[i].initTracking(I);
// Create the associated feature
vpFeatureBuilder::create(p[i],cam,dot[i]);
// flush the display
vpDisplay::flush(I);
}
/////////////////////////////////
// Task defintion
vpServo task ;
// we want an eye-in-hand control law ;
task.setServo(vpServo::EYEINHAND_L_cVe_eJe) ;
task.setInteractionMatrixType(vpServo::DESIRED) ;
// Set the position of the camera in the end-effector frame
vpHomogeneousMatrix cMe ;
vpVelocityTwistMatrix cVe(cMe) ;
task.set_cVe(cVe) ;
// Set the Jacobian (expressed in the end-effector frame)
vpMatrix eJe ;
robot.get_eJe(eJe) ;
task.set_eJe(eJe) ;
// we want to see a point on a point
for (int i = 0 ; i < 4 ; i++)
task.addFeature(p[i],pd[i]) ;
// Set the gain
task.setLambda(1.0) ;
// Print the current information about the task
task.print();
vpTime::wait(500);
////////////////////////////////////////////////
// The control loop
int k = 0;
while(k++ < 200){
double t = vpTime::measureTimeMs();
// Get the current internal camera view and display it
simu->getInternalImage(I);
vpDisplay::display(I);
// Track the four dots and update the associated visual features
for (int i = 0 ; i < 4 ; i++)
{
dot[i].track(I) ;
vpFeatureBuilder::create(p[i],cam,dot[i]) ;
}
// Display the desired and current visual features
vpServoDisplay::display(task,cam,I) ;
vpDisplay::flush(I);
// Update the robot Jacobian
robot.get_eJe(eJe) ;
task.set_eJe(eJe) ;
// Compute the control law
vpColVector v = task.computeControlLaw() ;
// Send the computed velocity to the robot and compute the new robot position
robot.setVelocity(vpRobot::ARTICULAR_FRAME, v) ;
robot.getPosition(cMo) ;
// Send the robot position to the visualizator
simu->setCameraPosition(cMo) ;
// Wait 40 ms
vpTime::wait(t,40);
}
// Print information about the task
task.print();
task.kill();
simu->closeMainApplication() ;
void *a=NULL ;
return a ;
}
int main()
{
#if defined(VISP_HAVE_PTHREAD)
try {
vpHomogeneousMatrix cdMo(0, 0, 0.75, 0, 0, 0);
vpHomogeneousMatrix cMo(0.15, -0.1, 1., vpMath::rad(10), vpMath::rad(-10), vpMath::rad(50));
/*
Top view of the world frame, the camera frame and the object frame
world, also robot base frame : --> w_y
|
\|/
w_x
object :
o_y
/|\
|
o_x <--
camera :
c_y
/|\
|
c_x <--
*/
vpHomogeneousMatrix wMo(vpTranslationVector(0.40, 0, -0.15),
vpRotationMatrix(vpRxyzVector(-M_PI, 0, M_PI/2.)));
std::vector<vpPoint> point;
point.push_back( vpPoint(-0.1,-0.1, 0) );
point.push_back( vpPoint( 0.1,-0.1, 0) );
point.push_back( vpPoint( 0.1, 0.1, 0) );
point.push_back( vpPoint(-0.1, 0.1, 0) );
vpServo task ;
task.setServo(vpServo::EYEINHAND_CAMERA);
task.setInteractionMatrixType(vpServo::CURRENT);
task.setLambda(0.5);
vpFeaturePoint p[4], pd[4] ;
for (unsigned int i = 0 ; i < 4 ; i++) {
point[i].track(cdMo);
vpFeatureBuilder::create(pd[i], point[i]);
point[i].track(cMo);
vpFeatureBuilder::create(p[i], point[i]);
task.addFeature(p[i], pd[i]);
}
vpSimulatorViper850 robot(true);
robot.setVerbose(true);
// Enlarge the default joint limits
vpColVector qmin = robot.getJointMin();
vpColVector qmax = robot.getJointMax();
qmin[0] = -vpMath::rad(180);
qmax[0] = vpMath::rad(180);
qmax[1] = vpMath::rad(0);
qmax[2] = vpMath::rad(270);
qmin[4] = -vpMath::rad(180);
qmax[4] = vpMath::rad(180);
robot.setJointLimit(qmin, qmax);
std::cout << "Robot joint limits: " << std::endl;
for (unsigned int i=0; i< qmin.size(); i ++)
std::cout << "Joint " << i << ": min " << vpMath::deg(qmin[i]) << " max " << vpMath::deg(qmax[i]) << " (deg)" << std::endl;
robot.init(vpViper850::TOOL_PTGREY_FLEA2_CAMERA, vpCameraParameters::perspectiveProjWithoutDistortion);
robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
robot.initScene(vpWireFrameSimulator::PLATE, vpWireFrameSimulator::D_STANDARD);
robot.set_fMo(wMo);
bool ret = true;
#if VISP_VERSION_INT > VP_VERSION_INT(2,7,0)
ret =
#endif
robot.initialiseCameraRelativeToObject(cMo);
if (ret == false)
return 0; // Not able to set the position
robot.setDesiredCameraPosition(cdMo);
// We modify the default external camera position
robot.setExternalCameraPosition(vpHomogeneousMatrix(vpTranslationVector(-0.4, 0.4, 2),
vpRotationMatrix(vpRxyzVector(M_PI/2,0,0))));
vpImage<unsigned char> Iint(480, 640, 255);
#if defined(VISP_HAVE_X11)
vpDisplayX displayInt(Iint, 700, 0, "Internal view");
#elif defined(VISP_HAVE_GDI)
vpDisplayGDI displayInt(Iint, 700, 0, "Internal view");
#elif defined(VISP_HAVE_OPENCV)
vpDisplayOpenCV displayInt(Iint, 700, 0, "Internal view");
#else
std::cout << "No image viewer is available..." << std::endl;
#endif
vpCameraParameters cam(840, 840, Iint.getWidth()/2, Iint.getHeight()/2);
// Modify the camera parameters to match those used in the other simulations
robot.setCameraParameters(cam);
bool start = true;
//for ( ; ; )
for (int iter =0; iter < 275; iter ++)
{
cMo = robot.get_cMo();
for (unsigned int i = 0 ; i < 4 ; i++) {
point[i].track(cMo);
vpFeatureBuilder::create(p[i], point[i]);
}
vpDisplay::display(Iint);
robot.getInternalView(Iint);
if (!start) {
display_trajectory(Iint, point, cMo, cam);
vpDisplay::displayText(Iint, 40, 120, "Click to stop the servo...", vpColor::red);
}
vpDisplay::flush(Iint);
vpColVector v = task.computeControlLaw();
robot.setVelocity(vpRobot::CAMERA_FRAME, v);
// A click to exit
if (vpDisplay::getClick(Iint, false))
break;
if (start) {
start = false;
v = 0;
robot.setVelocity(vpRobot::CAMERA_FRAME, v);
vpDisplay::displayText(Iint, 40, 120, "Click to start the servo...", vpColor::blue);
vpDisplay::flush(Iint);
//vpDisplay::getClick(Iint);
}
vpTime::wait(1000*robot.getSamplingTime());
}
task.kill();
}
catch(vpException e) {
std::cout << "Catch an exception: " << e << std::endl;
}
#endif
}
int
main(int argc, const char ** argv)
{
bool opt_display = true;
bool opt_click_allowed = true;
// Read the command line options
if (getOptions(argc, argv, opt_click_allowed, opt_display) == false) {
exit (-1);
}
vpImage<unsigned char> Iint(512,512,0) ;
vpImage<unsigned char> Iext(512,512,0) ;
// We open a window using either X11, GTK or GDI.
#if defined VISP_HAVE_X11
vpDisplayX displayInt;
vpDisplayX displayExt;
#elif defined VISP_HAVE_GTK
vpDisplayGTK displayInt;
vpDisplayGTK displayExt;
#elif defined VISP_HAVE_GDI
vpDisplayGDI displayInt;
vpDisplayGDI displayExt;
#endif
if (opt_display) {
try{
// Display size is automatically defined by the image (Iint) and (Iext) size
displayInt.init(Iint, 100, 100,"Internal view") ;
displayExt.init(Iext, (int)(130+Iint.getWidth()), 100, "External view") ;
// Display the image
// The image class has a member that specify a pointer toward
// the display that has been initialized in the display declaration
// therefore is is no longuer necessary to make a reference to the
// display variable.
vpDisplay::display(Iint) ;
vpDisplay::display(Iext) ;
vpDisplay::flush(Iint) ;
vpDisplay::flush(Iext) ;
}
catch(...)
{
vpERROR_TRACE("Error while displaying the image") ;
exit(-1);
}
}
vpProjectionDisplay externalview ;
//Set the camera parameters
double px, py ; px = py = 600 ;
double u0, v0 ; u0 = v0 = 256 ;
vpCameraParameters cam(px,py,u0,v0);
vpServo task ;
vpSimulatorCamera robot ;
// sets the initial camera location
vpHomogeneousMatrix cMo(-0.2,0.1,2,
vpMath::rad(5), vpMath::rad(5), vpMath::rad(20));
vpHomogeneousMatrix wMc, wMo;
robot.getPosition(wMc) ;
wMo = wMc * cMo; // Compute the position of the object in the world frame
// sets the final camera location (for simulation purpose)
vpHomogeneousMatrix cMod(0,0,1,
vpMath::rad(0), vpMath::rad(0), vpMath::rad(0));
// sets the cylinder coordinates in the world frame
vpCylinder cylinder(0,1,0, // direction
0,0,0, // point of the axis
0.1) ; // radius
externalview.insert(cylinder) ;
// sets the desired position of the visual feature
cylinder.track(cMod) ;
cylinder.print() ;
//Build the desired line features thanks to the cylinder and especially its paramaters in the image frame
vpFeatureLine ld[2] ;
int i ;
for(i=0 ; i < 2 ; i++)
vpFeatureBuilder::create(ld[i],cylinder,i) ;
// computes the cylinder coordinates in the camera frame and its 2D coordinates
// sets the current position of the visual feature
cylinder.track(cMo) ;
cylinder.print() ;
//Build the current line features thanks to the cylinder and especially its paramaters in the image frame
vpFeatureLine l[2] ;
for(i=0 ; i < 2 ; i++)
{
vpFeatureBuilder::create(l[i],cylinder,i) ;
l[i].print() ;
}
// define the task
// - we want an eye-in-hand control law
// - robot is controlled in the camera frame
task.setServo(vpServo::EYEINHAND_CAMERA) ;
task.setInteractionMatrixType(vpServo::DESIRED,vpServo::PSEUDO_INVERSE) ;
// it can also be interesting to test these possibilities
// task.setInteractionMatrixType(vpServo::CURRENT,vpServo::PSEUDO_INVERSE) ;
// task.setInteractionMatrixType(vpServo::MEAN, vpServo::PSEUDO_INVERSE) ;
// task.setInteractionMatrixType(vpServo::CURRENT, vpServo::PSEUDO_INVERSE) ;
// task.setInteractionMatrixType(vpServo::DESIRED, vpServo::TRANSPOSE) ;
// task.setInteractionMatrixType(vpServo::CURRENT, vpServo::TRANSPOSE) ;
// we want to see 2 lines on 2 lines
task.addFeature(l[0],ld[0]) ;
task.addFeature(l[1],ld[1]) ;
// Set the point of view of the external view
vpHomogeneousMatrix cextMo(0,0,6,
vpMath::rad(40), vpMath::rad(10), vpMath::rad(60)) ;
// Display the initial scene
vpServoDisplay::display(task,cam,Iint) ;
externalview.display(Iext,cextMo, cMo, cam, vpColor::red);
vpDisplay::flush(Iint) ;
vpDisplay::flush(Iext) ;
// Display task information
task.print() ;
if (opt_display && opt_click_allowed) {
std::cout << "\n\nClick in the internal camera view window to start..." << std::endl;
vpDisplay::getClick(Iint) ;
}
// set the gain
task.setLambda(1) ;
// Display task information
task.print() ;
unsigned int iter=0 ;
// The first loop is needed to reach the desired position
do
{
std::cout << "---------------------------------------------" << iter++ <<std::endl ;
vpColVector v ;
// get the robot position
robot.getPosition(wMc) ;
// Compute the position of the camera wrt the object frame
cMo = wMc.inverse() * wMo;
// new line position
// retrieve x,y and Z of the vpLine structure
// Compute the parameters of the cylinder in the camera frame and in the image frame
cylinder.track(cMo) ;
//Build the current line features thanks to the cylinder and especially its paramaters in the image frame
for(i=0 ; i < 2 ; i++)
{
vpFeatureBuilder::create(l[i],cylinder,i) ;
}
// Display the current scene
if (opt_display) {
vpDisplay::display(Iint) ;
vpDisplay::display(Iext) ;
vpServoDisplay::display(task,cam,Iint) ;
externalview.display(Iext,cextMo, cMo, cam, vpColor::red);
vpDisplay::flush(Iint);
vpDisplay::flush(Iext);
}
// compute the control law
v = task.computeControlLaw() ;
// send the camera velocity to the controller
robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
std::cout << "|| s - s* || = " << ( task.getError() ).sumSquare() <<std::endl ;
}
while(( task.getError() ).sumSquare() > 1e-9) ;
// Second loop is to compute the control law while taking into account the secondary task.
// In this example the secondary task is cut in four steps.
// The first one consists in impose a movement of the robot along the x axis of the object frame with a velocity of 0.5.
// The second one consists in impose a movement of the robot along the y axis of the object frame with a velocity of 0.5.
// The third one consists in impose a movement of the robot along the x axis of the object frame with a velocity of -0.5.
// The last one consists in impose a movement of the robot along the y axis of the object frame with a velocity of -0.5.
// Each steps is made during 200 iterations.
vpColVector e1(6) ; e1 = 0 ;
vpColVector e2(6) ; e2 = 0 ;
vpColVector proj_e1 ;
vpColVector proj_e2 ;
iter = 0;
double rapport = 0;
double vitesse = 0.5;
unsigned int tempo = 800;
while(iter < tempo)
{
vpColVector v ;
robot.getPosition(wMc) ;
// Compute the position of the camera wrt the object frame
cMo = wMc.inverse() * wMo;
cylinder.track(cMo) ;
for(i=0 ; i < 2 ; i++)
{
vpFeatureBuilder::create(l[i],cylinder,i) ;
}
if (opt_display)
{
vpDisplay::display(Iint) ;
vpDisplay::display(Iext) ;
vpServoDisplay::display(task,cam,Iint) ;
externalview.display(Iext,cextMo, cMo, cam, vpColor::red);
vpDisplay::flush(Iint);
vpDisplay::flush(Iext);
}
v = task.computeControlLaw() ;
if ( iter%tempo < 200 /*&& iter%tempo >= 0*/)
{
e2 = 0;
e1[0] = fabs(vitesse) ;
proj_e1 = task.secondaryTask(e1);
rapport = vitesse/proj_e1[0];
proj_e1 *= rapport ;
v += proj_e1 ;
}
if ( iter%tempo < 400 && iter%tempo >= 200)
{
e1 = 0;
e2[1] = fabs(vitesse) ;
proj_e2 = task.secondaryTask(e2);
rapport = vitesse/proj_e2[1];
proj_e2 *= rapport ;
v += proj_e2 ;
}
if ( iter%tempo < 600 && iter%tempo >= 400)
{
e2 = 0;
e1[0] = -fabs(vitesse) ;
proj_e1 = task.secondaryTask(e1);
rapport = -vitesse/proj_e1[0];
proj_e1 *= rapport ;
v += proj_e1 ;
}
if ( iter%tempo < 800 && iter%tempo >= 600)
{
e1 = 0;
e2[1] = -fabs(vitesse) ;
proj_e2 = task.secondaryTask(e2);
rapport = -vitesse/proj_e2[1];
proj_e2 *= rapport ;
v += proj_e2 ;
}
robot.setVelocity(vpRobot::CAMERA_FRAME, v);
std::cout << "|| s - s* || = " << ( task.getError() ).sumSquare() <<std::endl ;
iter++;
}
if (opt_display && opt_click_allowed) {
std::cout << "\nClick in the internal camera view window to end..." << std::endl;
vpDisplay::getClick(Iint) ;
}
// Display task information
task.print() ;
task.kill();
}
int
main()
{
try
{
vpImage<unsigned char> I ;
vp1394TwoGrabber g;
g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_640x480_MONO8);
g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_60);
g.open(I) ;
g.acquire(I) ;
#ifdef VISP_HAVE_X11
vpDisplayX display(I,100,100,"Current image") ;
#elif defined(VISP_HAVE_OPENCV)
vpDisplayOpenCV display(I,100,100,"Current image") ;
#elif defined(VISP_HAVE_GTK)
vpDisplayGTK display(I,100,100,"Current image") ;
#endif
vpDisplay::display(I) ;
vpDisplay::flush(I) ;
vpServo task ;
std::cout << std::endl ;
std::cout << "-------------------------------------------------------" << std::endl ;
std::cout << " Test program for vpServo " <<std::endl ;
std::cout << " Eye-in-hand task control, velocity computed in the camera frame" << std::endl ;
std::cout << " Simulation " << std::endl ;
std::cout << " task : servo a point " << std::endl ;
std::cout << "-------------------------------------------------------" << std::endl ;
std::cout << std::endl ;
int i ;
int nbline =2 ;
vpMeLine line[nbline] ;
vpMe me ;
me.setRange(20) ;
me.setPointsToTrack(100) ;
me.setThreshold(2000) ;
me.setSampleStep(10);
//Initialize the tracking of the two edges of the cylinder
for (i=0 ; i < nbline ; i++)
{
line[i].setDisplay(vpMeSite::RANGE_RESULT) ;
line[i].setMe(&me) ;
line[i].initTracking(I) ;
line[i].track(I) ;
}
vpRobotAfma6 robot ;
//robot.move("zero.pos") ;
vpCameraParameters cam ;
// Update camera parameters
robot.getCameraParameters (cam, I);
vpTRACE("sets the current position of the visual feature ") ;
vpFeatureLine p[nbline] ;
for (i=0 ; i < nbline ; i++)
vpFeatureBuilder::create(p[i],cam, line[i]) ;
vpTRACE("sets the desired position of the visual feature ") ;
vpCylinder cyld(0,1,0,0,0,0,0.04);
vpHomogeneousMatrix cMo(0,0,0.5,0,0,vpMath::rad(0));
cyld.project(cMo);
vpFeatureLine pd[nbline] ;
vpFeatureBuilder::create(pd[0],cyld,vpCylinder::line1);
vpFeatureBuilder::create(pd[1],cyld,vpCylinder::line2);
//Those lines are needed to keep the conventions define in vpMeLine (Those in vpLine are less restrictive)
//Another way to have the coordinates of the desired features is to learn them before executing the program.
pd[0].setRhoTheta(-fabs(pd[0].getRho()),0);
pd[1].setRhoTheta(-fabs(pd[1].getRho()),M_PI);
vpTRACE("define the task") ;
vpTRACE("\t we want an eye-in-hand control law") ;
vpTRACE("\t robot is controlled in the camera frame") ;
task.setServo(vpServo::EYEINHAND_CAMERA) ;
task.setInteractionMatrixType(vpServo::DESIRED, vpServo::PSEUDO_INVERSE);
vpTRACE("\t we want to see a point on a point..") ;
std::cout << std::endl ;
for (i=0 ; i < nbline ; i++)
task.addFeature(p[i],pd[i]) ;
vpTRACE("\t set the gain") ;
task.setLambda(0.3) ;
vpTRACE("Display task information " ) ;
task.print() ;
robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL) ;
unsigned int iter=0 ;
vpTRACE("\t loop") ;
vpColVector v ;
vpImage<vpRGBa> Ic ;
double lambda_av =0.05;
double alpha = 0.02;
double beta =3;
double erreur = 1;
//First loop to reach the convergence position
while(erreur > 0.00001)
{
std::cout << "---------------------------------------------" << iter <<std::endl ;
try {
g.acquire(I) ;
vpDisplay::display(I) ;
//Track the two edges and update the features
for (i=0 ; i < nbline ; i++)
{
line[i].track(I) ;
line[i].display(I, vpColor::red) ;
vpFeatureBuilder::create(p[i],cam,line[i]);
p[i].display(cam, I, vpColor::red) ;
pd[i].display(cam, I, vpColor::green) ;
}
vpDisplay::flush(I) ;
//Adaptative gain
double gain ;
{
if (std::fabs(alpha) <= std::numeric_limits<double>::epsilon())
gain = lambda_av ;
else
{
gain = alpha * exp (-beta * ( task.getError() ).sumSquare() ) + lambda_av ;
}
}
task.setLambda(gain) ;
v = task.computeControlLaw() ;
if (iter==0) vpDisplay::getClick(I) ;
}
catch(...)
{
v =0 ;
robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
robot.stopMotion() ;
exit(1) ;
}
robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
erreur = ( task.getError() ).sumSquare();
vpTRACE("\t\t || s - s* || = %f ", ( task.getError() ).sumSquare()) ;
iter++;
}
/**********************************************************************************************/
//Second loop is to compute the control while taking into account the secondary task.
vpColVector e1(6) ; e1 = 0 ;
vpColVector e2(6) ; e2 = 0 ;
vpColVector proj_e1 ;
vpColVector proj_e2 ;
iter = 0;
double rapport = 0;
double vitesse = 0.02;
unsigned int tempo = 1200;
for ( ; ; )
{
std::cout << "---------------------------------------------" << iter <<std::endl ;
try {
g.acquire(I) ;
vpDisplay::display(I) ;
//Track the two edges and update the features
for (i=0 ; i < nbline ; i++)
{
line[i].track(I) ;
line[i].display(I, vpColor::red) ;
vpFeatureBuilder::create(p[i],cam,line[i]);
p[i].display(cam, I, vpColor::red) ;
pd[i].display(cam, I, vpColor::green) ;
}
vpDisplay::flush(I) ;
v = task.computeControlLaw() ;
//Compute the new control law corresponding to the secondary task
if ( iter%tempo < 400 /*&& iter%tempo >= 0*/)
{
e2 = 0;
e1[0] = fabs(vitesse) ;
proj_e1 = task.secondaryTask(e1);
rapport = vitesse/proj_e1[0];
proj_e1 *= rapport ;
v += proj_e1 ;
if ( iter == 199 ) iter+=200; //This line is needed to make on ly an half turn during the first cycle
}
if ( iter%tempo < 600 && iter%tempo >= 400)
{
e1 = 0;
e2[1] = fabs(vitesse) ;
proj_e2 = task.secondaryTask(e2);
rapport = vitesse/proj_e2[1];
proj_e2 *= rapport ;
v += proj_e2 ;
}
if ( iter%tempo < 1000 && iter%tempo >= 600)
{
e2 = 0;
e1[0] = -fabs(vitesse) ;
proj_e1 = task.secondaryTask(e1);
rapport = -vitesse/proj_e1[0];
proj_e1 *= rapport ;
v += proj_e1 ;
}
if ( iter%tempo < 1200 && iter%tempo >= 1000)
{
e1 = 0;
e2[1] = -fabs(vitesse) ;
proj_e2 = task.secondaryTask(e2);
rapport = -vitesse/proj_e2[1];
proj_e2 *= rapport ;
v += proj_e2 ;
}
robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
}
catch(...)
{
v =0 ;
robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
robot.stopMotion() ;
exit(1) ;
}
vpTRACE("\t\t || s - s* || = %f ", ( task.getError() ).sumSquare()) ;
iter++;
}
vpTRACE("Display task information " ) ;
task.print() ;
task.kill();
}
catch (...)
{
vpERROR_TRACE(" Test failed") ;
return 0;
}
}
int main()
{
#ifdef VISP_HAVE_GTK
try {
std::cout << "ViSP geometric features display example" <<std::endl;
unsigned int height = 288;
unsigned int width = 384;
vpImage<unsigned char> I(height,width);
I = 255; // I is a white image
// create a display window
vpDisplayGTK display;
// initialize a display attached to image I
display.init(I,100,100,"ViSP geometric features display");
// camera parameters to digitalize the image plane
vpCameraParameters cam(600,600,width/2,height/2); // px,py,u0,v0
// pose of the camera with reference to the scene
vpTranslationVector t(0,0,1);
vpRxyzVector rxyz(-M_PI/4,0,0);
vpRotationMatrix R(rxyz);
vpHomogeneousMatrix cMo(t, R);
// scene building, geometric features definition
vpPoint point;
point.setWorldCoordinates(0,0,0);// (X0=0,Y0=0,Z0=0)
vpLine line;
line.setWorldCoordinates(1,1,0,0,0,0,1,0); // planes:(X+Y=0)&(Z=0)
vpCylinder cylinder;
cylinder.setWorldCoordinates(1,-1,0,0,0,0,0.1); // alpha=1,beta=-1,gamma=0,
// X0=0,Y0=0,Z0=0,R=0.1
vpCircle circle;
circle.setWorldCoordinates(0,0,1,0,0,0,0.1); // plane:(Z=0),X0=0,Y0=0,Z=0,R=0.1
vpSphere sphere;
sphere.setWorldCoordinates(0,0,0,0.1); // X0=0,Y0=0,Z0=0,R=0.1
// change frame to be the camera frame and project features in the image plane
point.project(cMo);
line.project(cMo);
cylinder.project(cMo);
circle.project(cMo);
sphere.project(cMo);
// display the scene
vpDisplay::display(I); // display I
// draw the projections of the 3D geometric features in the image plane.
point.display(I,cam,vpColor::black); // draw a black cross over I
line.display(I,cam,vpColor::blue); // draw a blue line over I
cylinder.display(I,cam,vpColor::red); // draw two red lines over I
circle.display(I,cam,vpColor::orange); // draw an orange ellipse over I
sphere.display(I,cam,vpColor::black); // draw a black ellipse over I
vpDisplay::flush(I); // flush the display buffer
std::cout << "A click in the display to exit" << std::endl;
vpDisplay::getClick(I); // wait for a click in the display to exit
// save the drawing
vpImage<vpRGBa> Ic;
vpDisplay::getImage(I,Ic);
std::cout << "ViSP creates \"./geometricFeatures.ppm\" B&W image "<< std::endl;
vpImageIo::write(Ic, "./geometricFeatures.ppm");
return 0;
}
catch(vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
#endif
}
/*!
\example testFeatureSegment.cpp
Shows how to build a task with a segment visual feature.
*/
int main(int argc, const char **argv)
{
try {
#if (defined (VISP_HAVE_X11) || defined (VISP_HAVE_GDI))
int opt_no_display = 0;
int opt_curves = 1;
#endif
int opt_normalized = 1;
// Parse the command line to set the variables
vpParseArgv::vpArgvInfo argTable[] =
{
#if (defined (VISP_HAVE_X11) || defined (VISP_HAVE_GDI))
{"-d", vpParseArgv::ARGV_CONSTANT, 0, (char *) &opt_no_display,
"Disable display and graphics viewer."},
#endif
{"-normalized", vpParseArgv::ARGV_INT, (char*) NULL, (char *) &opt_normalized,
"1 to use normalized features, 0 for non normalized."},
{"-h", vpParseArgv::ARGV_HELP, (char*) NULL, (char *) NULL,
"Print the help."},
{(char*) NULL, vpParseArgv::ARGV_END, (char*) NULL, (char*) NULL, (char*) NULL}
} ;
// Read the command line options
if(vpParseArgv::parse(&argc, argv, argTable,
vpParseArgv::ARGV_NO_LEFTOVERS |
vpParseArgv::ARGV_NO_ABBREV |
vpParseArgv::ARGV_NO_DEFAULTS)) {
return (false);
}
std::cout << "Used options: " << std::endl;
#if (defined (VISP_HAVE_X11) || defined (VISP_HAVE_GDI))
opt_curves = (opt_no_display == 0) ? 1 : 0;
std::cout << " - no display: " << opt_no_display << std::endl;
std::cout << " - curves : " << opt_curves << std::endl;
#endif
std::cout << " - normalized: " << opt_normalized << std::endl;
vpCameraParameters cam(640.,480.,320.,240.);
#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)
vpDisplay *display = NULL;
if (!opt_no_display) {
#if defined(VISP_HAVE_X11)
display = new vpDisplayX;
#elif defined VISP_HAVE_GDI
display = new vpDisplayGDI;
#endif
}
#endif
vpImage<unsigned char> I(480,640,0);
#if (defined (VISP_HAVE_X11) || defined (VISP_HAVE_GDI))
if (!opt_no_display)
display->init(I);
#endif
vpHomogeneousMatrix wMo; // Set to indentity. Robot world frame is equal to object frame
vpHomogeneousMatrix cMo (-0.5, 0.5, 2., vpMath::rad(10), vpMath::rad(20), vpMath::rad(30));
vpHomogeneousMatrix cdMo(0., 0., 1., vpMath::rad(0), vpMath::rad(0), vpMath::rad(0));
vpHomogeneousMatrix wMc; // Camera location in the robot world frame
vpPoint P[4]; // 4 points in the object frame
P[0].setWorldCoordinates( .1, .1, 0.);
P[1].setWorldCoordinates(-.1, .1, 0.);
P[2].setWorldCoordinates(-.1, -.1, 0.);
P[3].setWorldCoordinates( .1, -.1, 0.);
vpPoint Pd[4]; // 4 points in the desired camera frame
for (int i=0; i<4; i++) {
Pd[i] = P[i];
Pd[i].project(cdMo);
}
vpPoint Pc[4]; // 4 points in the current camera frame
for (int i=0; i<4; i++) {
Pc[i] = P[i];
Pc[i].project(cMo);
}
vpFeatureSegment seg_cur[2], seg_des[2]; // Current and desired features
for (int i=0; i <2; i++)
{
if (opt_normalized) {
seg_cur[i].setNormalized(true);
seg_des[i].setNormalized(true);
}
else {
seg_cur[i].setNormalized(false);
seg_des[i].setNormalized(false);
}
vpFeatureBuilder::create(seg_cur[i], Pc[i*2], Pc[i*2+1]);
vpFeatureBuilder::create(seg_des[i], Pd[i*2], Pd[i*2+1]);
seg_cur[i].print();
seg_des[i].print();
}
//define visual servoing task
vpServo task;
task.setServo(vpServo::EYEINHAND_CAMERA);
task.setInteractionMatrixType(vpServo::CURRENT);
task.setLambda(2.) ;
for (int i=0; i <2; i++)
task.addFeature(seg_cur[i], seg_des[i]);
#if (defined (VISP_HAVE_X11) || defined(VISP_HAVE_GDI))
if (!opt_no_display) {
vpDisplay::display(I);
for (int i=0; i <2; i++) {
seg_cur[i].display(cam, I, vpColor::red);
seg_des[i].display(cam, I, vpColor::green);
vpDisplay::flush(I);
}
}
#endif
#if (defined (VISP_HAVE_X11) || defined (VISP_HAVE_GDI))
vpPlot *graph = NULL;
if (opt_curves)
{
//Create a window (700 by 700) at position (100, 200) with two graphics
graph = new vpPlot(2, 500, 500, 700, 10, "Curves...");
//The first graphic contains 3 curve and the second graphic contains 3 curves
graph->initGraph(0,6);
graph->initGraph(1,8);
// graph->setTitle(0, "Velocities");
// graph->setTitle(1, "Error s-s*");
}
#endif
//param robot
vpSimulatorCamera robot;
float sampling_time = 0.02f; // Sampling period in seconds
robot.setSamplingTime(sampling_time);
robot.setMaxTranslationVelocity(5.);
robot.setMaxRotationVelocity(vpMath::rad(90.));
wMc = wMo * cMo.inverse();
robot.setPosition(wMc);
int iter=0;
do {
double t = vpTime::measureTimeMs();
wMc = robot.getPosition();
cMo = wMc.inverse() * wMo;
for (int i=0; i <4; i++)
Pc[i].project(cMo);
for (int i=0; i <2; i++)
vpFeatureBuilder::create(seg_cur[i], Pc[i*2], Pc[i*2+1]);
#if (defined (VISP_HAVE_X11) || defined(VISP_HAVE_GDI))
if (!opt_no_display) {
vpDisplay::display(I);
for (int i=0; i <2; i++) {
seg_cur[i].display(cam, I, vpColor::red);
seg_des[i].display(cam, I, vpColor::green);
vpDisplay::flush(I);
}
}
#endif
vpColVector v = task.computeControlLaw();
robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
#if (defined (VISP_HAVE_X11) || defined (VISP_HAVE_GDI))
if (opt_curves)
{
graph->plot(0, iter, v); // plot velocities applied to the robot
graph->plot(1, iter, task.getError()); // plot error vector
}
#endif
vpTime::wait(t, sampling_time * 1000); // Wait 10 ms
iter ++;
} while(( task.getError() ).sumSquare() > 0.0005);
// A call to kill() is requested here to destroy properly the current
// and desired feature lists.
task.kill();
#if (defined (VISP_HAVE_X11) || defined (VISP_HAVE_GDI))
if (graph != NULL)
delete graph;
#endif
#if (defined (VISP_HAVE_X11) || defined (VISP_HAVE_GDI))
if (!opt_no_display && display != NULL)
delete display;
#endif
std::cout << "final error=" << ( task.getError() ).sumSquare() << std::endl;
return 0;
}
catch(vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
}
int
main(int argc, const char ** argv)
{
// Read the command line options
if (getOptions(argc, argv) == false) {
exit (-1);
}
// Log file creation in /tmp/$USERNAME/log.dat
// This file contains by line:
// - the 6 computed camera velocities (m/s, rad/s) to achieve the task
// - the 6 values of s - s*
std::string username;
// Get the user login name
vpIoTools::getUserName(username);
// Create a log filename to save velocities...
std::string logdirname;
#ifdef WIN32
logdirname ="C:/temp/" + username;
#else
logdirname ="/tmp/" + username;
#endif
// Test if the output path exist. If no try to create it
if (vpIoTools::checkDirectory(logdirname) == false) {
try {
// Create the dirname
vpIoTools::makeDirectory(logdirname);
}
catch (...) {
std::cerr << std::endl
<< "ERROR:" << std::endl;
std::cerr << " Cannot create " << logdirname << std::endl;
exit(-1);
}
}
std::string logfilename;
logfilename = logdirname + "/log.dat";
// Open the log file name
std::ofstream flog(logfilename.c_str());
vpServo task ;
vpRobotCamera robot ;
std::cout << std::endl ;
std::cout << "-------------------------------------------------------" << std::endl ;
std::cout << " Test program for vpServo " <<std::endl ;
std::cout << " Eye-in-hand task control, velocity computed in the camera frame" << std::endl ;
std::cout << " Simulation " << std::endl ;
std::cout << " task : 3D visual servoing " << std::endl ;
std::cout << "-------------------------------------------------------" << std::endl ;
std::cout << std::endl ;
// Sets the initial camera location
vpPoseVector c_r_o(// Translation tx,ty,tz
0.1, 0.2, 2,
// ThetaU rotation
vpMath::rad(20), vpMath::rad(10), vpMath::rad(50) ) ;
// From the camera pose build the corresponding homogeneous matrix
vpHomogeneousMatrix cMo(c_r_o) ;
// Set the robot initial position
robot.setPosition(cMo) ;
// Sets the desired camera location
vpPoseVector cd_r_o(// Translation tx,ty,tz
0, 0, 1,
// ThetaU rotation
vpMath::rad(0),vpMath::rad(0),vpMath::rad(0)) ;
// From the camera desired pose build the corresponding homogeneous matrix
vpHomogeneousMatrix cdMo(cd_r_o) ;
// Compute the homogeneous transformation from the desired camera position to the initial one
vpHomogeneousMatrix cdMc ;
cdMc = cdMo*cMo.inverse() ;
// Build the current visual features s = (c*tc, thetaU_c*Rc)^T
vpFeatureTranslation t(vpFeatureTranslation::cdMc) ;
vpFeatureThetaU tu(vpFeatureThetaU::cdRc); // current feature
t.buildFrom(cdMc) ;
tu.buildFrom(cdMc) ;
// Sets the desired rotation (always zero !) since s is the
// rotation that the camera has to achieve. Here s* = (0, 0)^T
vpFeatureTranslation td(vpFeatureTranslation::cdMc) ;
vpFeatureThetaU tud(vpFeatureThetaU::cdRc); // desired feature
// Define the task
// - we want an eye-in-hand control law
// - the robot is controlled in the camera frame
task.setServo(vpServo::EYEINHAND_CAMERA) ;
// - we use here the interaction matrix computed with the
// current features
task.setInteractionMatrixType(vpServo::CURRENT);
// Add the current and desired visual features
task.addFeature(t,td) ; // 3D translation
task.addFeature(tu,tud) ; // 3D rotation
// - set the constant gain to 1.0
task.setLambda(1) ;
// Display task information
task.print() ;
int iter=0 ;
// Start the visual servoing loop. We stop the servo after 200 iterations
while(iter++ < 200) {
std::cout << "-----------------------------------" << iter <<std::endl ;
vpColVector v ;
// get the robot position
robot.getPosition(cMo) ;
// new displacement to achieve
cdMc = cdMo*cMo.inverse() ;
// Update the current visual features
t.buildFrom(cdMc) ;
tu.buildFrom(cdMc) ;
// Compute the control law
v = task.computeControlLaw() ;
// Display task information
if (iter==1) task.print() ;
// Send the camera velocity to the controller
robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
// Retrieve the error
std::cout << task.error.sumSquare() <<std::endl ;
// Save log
flog << v.t() << " " << task.error.t() << std::endl;
}
// Display task information
task.print() ;
// Kill the task
task.kill();
// Close the log file
flog.close();
}
int
main(int argc, const char ** argv)
{
try {
bool opt_click_allowed = true;
bool opt_display = true;
// Read the command line options
if (getOptions(argc, argv, opt_click_allowed, opt_display) == false) {
exit (-1);
}
// We open two displays, one for the internal camera view, the other one for
// the external view, using either X11, GTK or GDI.
#if defined VISP_HAVE_X11
vpDisplayX displayInt;
vpDisplayX displayExt;
#elif defined VISP_HAVE_GTK
vpDisplayGTK displayInt;
vpDisplayGTK displayExt;
#elif defined VISP_HAVE_GDI
vpDisplayGDI displayInt;
vpDisplayGDI displayExt;
#elif defined VISP_HAVE_OPENCV
vpDisplayOpenCV displayInt;
vpDisplayOpenCV displayExt;
#endif
// open a display for the visualization
vpImage<unsigned char> Iint(300, 300, 0) ;
vpImage<unsigned char> Iext(300, 300, 0) ;
if (opt_display) {
displayInt.init(Iint,0,0, "Internal view") ;
displayExt.init(Iext,330,000, "External view") ;
}
vpProjectionDisplay externalview ;
double px, py ; px = py = 500 ;
double u0, v0 ; u0 = 150, v0 = 160 ;
vpCameraParameters cam(px,py,u0,v0);
int i ;
vpServo task ;
vpSimulatorCamera robot ;
std::cout << std::endl ;
std::cout << "----------------------------------------------" << std::endl ;
std::cout << " Test program for vpServo " <<std::endl ;
std::cout << " Eye-in-hand task control, articular velocity are computed"
<< std::endl ;
std::cout << " Simulation " << std::endl ;
std::cout << " task : servo 4 points " << std::endl ;
std::cout << "----------------------------------------------" << std::endl ;
std::cout << std::endl ;
// sets the initial camera location
vpHomogeneousMatrix cMo(-0.1,-0.1,1,
vpMath::rad(40), vpMath::rad(10), vpMath::rad(60)) ;
// Compute the position of the object in the world frame
vpHomogeneousMatrix wMc, wMo;
robot.getPosition(wMc) ;
wMo = wMc * cMo;
vpHomogeneousMatrix cextMo(0,0,2,
0,0,0) ;//vpMath::rad(40), vpMath::rad(10), vpMath::rad(60)) ;
// sets the point coordinates in the object frame
vpPoint point[4] ;
point[0].setWorldCoordinates(-0.1,-0.1,0) ;
point[1].setWorldCoordinates(0.1,-0.1,0) ;
point[2].setWorldCoordinates(0.1,0.1,0) ;
point[3].setWorldCoordinates(-0.1,0.1,0) ;
for (i = 0 ; i < 4 ; i++)
externalview.insert(point[i]) ;
// computes the point coordinates in the camera frame and its 2D coordinates
for (i = 0 ; i < 4 ; i++)
point[i].track(cMo) ;
// sets the desired position of the point
vpFeaturePoint p[4] ;
for (i = 0 ; i < 4 ; i++)
vpFeatureBuilder::create(p[i],point[i]) ; //retrieve x,y and Z of the vpPoint structure
// sets the desired position of the feature point s*
vpFeaturePoint pd[4] ;
pd[0].buildFrom(-0.1,-0.1, 1) ;
pd[1].buildFrom( 0.1,-0.1, 1) ;
pd[2].buildFrom( 0.1, 0.1, 1) ;
pd[3].buildFrom(-0.1, 0.1, 1) ;
// define the task
// - we want an eye-in-hand control law
// - articular velocity are computed
task.setServo(vpServo::EYEINHAND_L_cVe_eJe) ;
task.setInteractionMatrixType(vpServo::MEAN) ;
// Set the position of the camera in the end-effector frame ") ;
vpHomogeneousMatrix cMe ;
vpVelocityTwistMatrix cVe(cMe) ;
task.set_cVe(cVe) ;
// Set the Jacobian (expressed in the end-effector frame)
vpMatrix eJe ;
robot.get_eJe(eJe) ;
task.set_eJe(eJe) ;
// we want to see a point on a point
for (i = 0 ; i < 4 ; i++)
task.addFeature(p[i],pd[i]) ;
// set the gain
task.setLambda(1) ;
// Display task information " ) ;
task.print() ;
unsigned int iter=0 ;
// loop
while(iter++<200)
{
std::cout << "---------------------------------------------" << iter <<std::endl ;
vpColVector v ;
// Set the Jacobian (expressed in the end-effector frame)
// since q is modified eJe is modified
robot.get_eJe(eJe) ;
task.set_eJe(eJe) ;
// get the robot position
robot.getPosition(wMc) ;
// Compute the position of the camera wrt the object frame
cMo = wMc.inverse() * wMo;
// update new point position and corresponding features
for (i = 0 ; i < 4 ; i++)
{
point[i].track(cMo) ;
//retrieve x,y and Z of the vpPoint structure
vpFeatureBuilder::create(p[i],point[i]) ;
}
// since vpServo::MEAN interaction matrix is used, we need also to update the desired features at each iteration
pd[0].buildFrom(-0.1,-0.1, 1) ;
pd[1].buildFrom( 0.1,-0.1, 1) ;
pd[2].buildFrom( 0.1, 0.1, 1) ;
pd[3].buildFrom(-0.1, 0.1, 1) ;
if (opt_display) {
vpDisplay::display(Iint) ;
vpDisplay::display(Iext) ;
vpServoDisplay::display(task,cam,Iint) ;
externalview.display(Iext,cextMo, cMo, cam, vpColor::green) ;
vpDisplay::flush(Iint);
vpDisplay::flush(Iext);
}
// compute the control law
v = task.computeControlLaw() ;
// send the camera velocity to the controller
robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
std::cout << "|| s - s* || = " << ( task.getError() ).sumSquare() <<std::endl ;
}
// Display task information
task.print() ;
task.kill();
std::cout <<"Final robot position with respect to the object frame:\n";
cMo.print();
if (opt_display && opt_click_allowed) {
// suppressed for automate test
std::cout << "\n\nClick in the internal view window to end..." << std::endl;
vpDisplay::getClick(Iint) ;
}
return 0;
}
catch(vpException e) {
std::cout << "Catch a ViSP exception: " << e << std::endl;
return 1;
}
}
int main()
{
try {
vpHomogeneousMatrix cdMo(0, 0, 0.75, 0, 0, 0);
vpHomogeneousMatrix cMo(0.15, -0.1, 1., vpMath::rad(10), vpMath::rad(-10), vpMath::rad(50));
std::vector<vpPoint> point(4) ;
point[0].setWorldCoordinates(-0.1,-0.1, 0);
point[1].setWorldCoordinates( 0.1,-0.1, 0);
point[2].setWorldCoordinates( 0.1, 0.1, 0);
point[3].setWorldCoordinates(-0.1, 0.1, 0);
vpServo task ;
task.setServo(vpServo::EYEINHAND_CAMERA);
task.setInteractionMatrixType(vpServo::CURRENT);
task.setLambda(0.5);
vpFeaturePoint p[4], pd[4] ;
for (unsigned int i = 0 ; i < 4 ; i++) {
point[i].track(cdMo);
vpFeatureBuilder::create(pd[i], point[i]);
point[i].track(cMo);
vpFeatureBuilder::create(p[i], point[i]);
task.addFeature(p[i], pd[i]);
}
vpHomogeneousMatrix wMc, wMo;
vpSimulatorCamera robot;
robot.setSamplingTime(0.040);
robot.getPosition(wMc);
wMo = wMc * cMo;
vpImage<unsigned char> Iint(480, 640, 0) ;
vpImage<unsigned char> Iext(480, 640, 0) ;
#if defined VISP_HAVE_X11
vpDisplayX displayInt(Iint, 0, 0, "Internal view");
vpDisplayX displayExt(Iext, 670, 0, "External view");
#elif defined VISP_HAVE_GDI
vpDisplayGDI displayInt(Iint, 0, 0, "Internal view");
vpDisplayGDI displayExt(Iext, 670, 0, "External view");
#elif defined VISP_HAVE_OPENCV
vpDisplayOpenCV displayInt(Iint, 0, 0, "Internal view");
vpDisplayOpenCV displayExt(Iext, 670, 0, "External view");
#else
std::cout << "No image viewer is available..." << std::endl;
#endif
vpCameraParameters cam(840, 840, Iint.getWidth()/2, Iint.getHeight()/2);
vpHomogeneousMatrix cextMo(0,0,3, 0,0,0);
vpWireFrameSimulator sim;
sim.initScene(vpWireFrameSimulator::PLATE, vpWireFrameSimulator::D_STANDARD);
sim.setCameraPositionRelObj(cMo);
sim.setDesiredCameraPosition(cdMo);
sim.setExternalCameraPosition(cextMo);
sim.setInternalCameraParameters(cam);
sim.setExternalCameraParameters(cam);
while(1) {
robot.getPosition(wMc);
cMo = wMc.inverse() * wMo;
for (unsigned int i = 0 ; i < 4 ; i++) {
point[i].track(cMo);
vpFeatureBuilder::create(p[i], point[i]);
}
vpColVector v = task.computeControlLaw();
robot.setVelocity(vpRobot::CAMERA_FRAME, v);
sim.setCameraPositionRelObj(cMo);
vpDisplay::display(Iint) ;
vpDisplay::display(Iext) ;
sim.getInternalImage(Iint);
sim.getExternalImage(Iext);
display_trajectory(Iint, point, cMo, cam);
vpDisplay::flush(Iint);
vpDisplay::flush(Iext);
// A click in the internal view to exit
if (vpDisplay::getClick(Iint, false))
break;
vpTime::wait(1000*robot.getSamplingTime());
}
task.kill();
}
catch(vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
}
}
int
main()
{
try
{
vpImage<unsigned char> I ;
vp1394TwoGrabber g;
g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_640x480_MONO8);
g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_60);
g.open(I) ;
g.acquire(I) ;
#ifdef VISP_HAVE_X11
vpDisplayX display(I,100,100,"Current image") ;
#elif defined(VISP_HAVE_OPENCV)
vpDisplayOpenCV display(I,100,100,"Current image") ;
#elif defined(VISP_HAVE_GTK)
vpDisplayGTK display(I,100,100,"Current image") ;
#endif
vpDisplay::display(I) ;
vpDisplay::flush(I);
vpServo task ;
std::cout << std::endl ;
std::cout << "-------------------------------------------------------" << std::endl ;
std::cout << " Test program for vpServo " <<std::endl ;
std::cout << " Eye-in-hand task control, velocity computed in the camera frame" << std::endl ;
std::cout << " Simulation " << std::endl ;
std::cout << " task : servo a line " << std::endl ;
std::cout << "-------------------------------------------------------" << std::endl ;
std::cout << std::endl ;
int i ;
int nbline =4 ;
vpMeLine line[nbline] ;
vpMe me ;
me.setRange(10) ;
me.setPointsToTrack(100) ;
me.setThreshold(50000) ;
me.setSampleStep(10);
//Initialize the tracking. Define the four lines to track.
for (i=0 ; i < nbline ; i++)
{
line[i].setDisplay(vpMeSite::RANGE_RESULT) ;
line[i].setMe(&me) ;
line[i].initTracking(I) ;
line[i].track(I) ;
}
vpRobotAfma6 robot ;
//robot.move("zero.pos") ;
vpCameraParameters cam ;
// Update camera parameters
robot.getCameraParameters (cam, I);
vpTRACE("sets the current position of the visual feature ") ;
vpFeatureLine p[nbline] ;
for (i=0 ; i < nbline ; i++)
vpFeatureBuilder::create(p[i],cam, line[i]) ;
vpTRACE("sets the desired position of the visual feature ") ;
vpLine lined[nbline];
lined[0].setWorldCoordinates(1,0,0,0.05,0,0,1,0);
lined[1].setWorldCoordinates(0,1,0,0.05,0,0,1,0);
lined[2].setWorldCoordinates(1,0,0,-0.05,0,0,1,0);
lined[3].setWorldCoordinates(0,1,0,-0.05,0,0,1,0);
vpHomogeneousMatrix cMo(0,0,0.5,0,0,vpMath::rad(0));
lined[0].project(cMo);
lined[1].project(cMo);
lined[2].project(cMo);
lined[3].project(cMo);
//Those lines are needed to keep the conventions define in vpMeLine (Those in vpLine are less restrictive)
//Another way to have the coordinates of the desired features is to learn them before executing the program.
lined[0].setRho(-fabs(lined[0].getRho()));
lined[0].setTheta(0);
lined[1].setRho(-fabs(lined[1].getRho()));
lined[1].setTheta(M_PI/2);
lined[2].setRho(-fabs(lined[2].getRho()));
lined[2].setTheta(M_PI);
lined[3].setRho(-fabs(lined[3].getRho()));
lined[3].setTheta(-M_PI/2);
vpFeatureLine pd[nbline] ;
vpFeatureBuilder::create(pd[0],lined[0]);
vpFeatureBuilder::create(pd[1],lined[1]);
vpFeatureBuilder::create(pd[2],lined[2]);
vpFeatureBuilder::create(pd[3],lined[3]);
vpTRACE("define the task") ;
vpTRACE("\t we want an eye-in-hand control law") ;
vpTRACE("\t robot is controlled in the camera frame") ;
task.setServo(vpServo::EYEINHAND_CAMERA) ;
task.setInteractionMatrixType(vpServo::DESIRED, vpServo::PSEUDO_INVERSE);
vpTRACE("\t we want to see a point on a point..") ;
std::cout << std::endl ;
for (i=0 ; i < nbline ; i++)
task.addFeature(p[i],pd[i]) ;
vpTRACE("\t set the gain") ;
task.setLambda(0.2) ;
vpTRACE("Display task information " ) ;
task.print() ;
robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL) ;
unsigned int iter=0 ;
vpTRACE("\t loop") ;
vpColVector v ;
vpImage<vpRGBa> Ic ;
double lambda_av =0.05;
double alpha = 0.05 ;
double beta =3 ;
for ( ; ; )
{
std::cout << "---------------------------------------------" << iter <<std::endl ;
try {
g.acquire(I) ;
vpDisplay::display(I) ;
//Track the lines and update the features
for (i=0 ; i < nbline ; i++)
{
line[i].track(I) ;
line[i].display(I, vpColor::red) ;
vpFeatureBuilder::create(p[i],cam,line[i]);
p[i].display(cam, I, vpColor::red) ;
pd[i].display(cam, I, vpColor::green) ;
}
double gain ;
{
if (std::fabs(alpha) <= std::numeric_limits<double>::epsilon())
gain = lambda_av ;
else
{
gain = alpha * exp (-beta * ( task.getError() ).sumSquare() ) + lambda_av ;
}
}
task.setLambda(gain) ;
v = task.computeControlLaw() ;
vpDisplay::flush(I) ;
if (iter==0) vpDisplay::getClick(I) ;
if (v.sumSquare() > 0.5)
{
v =0 ;
robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
robot.stopMotion() ;
vpDisplay::getClick(I) ;
}
robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
}
catch(...)
{
v =0 ;
robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
robot.stopMotion() ;
exit(1) ;
}
vpTRACE("\t\t || s - s* || = %f ", ( task.getError() ).sumSquare()) ;
iter++;
}
vpTRACE("Display task information " ) ;
task.print() ;
task.kill();
}
catch (...)
{
vpERROR_TRACE(" Test failed") ;
return 0;
}
}
int
main(int argc, const char ** argv)
{
try {
bool opt_display = true;
bool opt_click_allowed = true;
// Read the command line options
if (getOptions(argc, argv, opt_click_allowed, opt_display) == false) {
exit (-1);
}
vpImage<unsigned char> I(512,512,0) ;
// We open a window using either X11, GTK or GDI.
#if defined VISP_HAVE_X11
vpDisplayX display;
#elif defined VISP_HAVE_GTK
vpDisplayGTK display;
#elif defined VISP_HAVE_GDI
vpDisplayGDI display;
#elif defined VISP_HAVE_OPENCV
vpDisplayOpenCV display;
#endif
if (opt_display) {
try{
// Display size is automatically defined by the image (I) size
display.init(I, 100, 100,"Camera view...") ;
// Display the image
// The image class has a member that specify a pointer toward
// the display that has been initialized in the display declaration
// therefore is is no longuer necessary to make a reference to the
// display variable.
vpDisplay::display(I) ;
vpDisplay::flush(I) ;
}
catch(...)
{
vpERROR_TRACE("Error while displaying the image") ;
exit(-1);
}
}
double px, py ; px = py = 600 ;
double u0, v0 ; u0 = v0 = 256 ;
vpCameraParameters cam(px,py,u0,v0);
vpServo task ;
vpSimulatorCamera robot ;
// sets the initial camera location
vpHomogeneousMatrix cMo(0,0,1,
vpMath::rad(0), vpMath::rad(80), vpMath::rad(30)) ;
vpHomogeneousMatrix wMc, wMo;
robot.getPosition(wMc) ;
wMo = wMc * cMo; // Compute the position of the object in the world frame
vpHomogeneousMatrix cMod(-0.1,-0.1,0.7,
vpMath::rad(40), vpMath::rad(10), vpMath::rad(30)) ;
// sets the circle coordinates in the world frame
vpCircle circle ;
circle.setWorldCoordinates(0,0,1,
0,0,0,
0.1) ;
// sets the desired position of the visual feature
vpFeatureEllipse pd ;
circle.track(cMod) ;
vpFeatureBuilder::create(pd,circle) ;
// project : computes the circle coordinates in the camera frame and its 2D coordinates
// sets the current position of the visual feature
vpFeatureEllipse p ;
circle.track(cMo) ;
vpFeatureBuilder::create(p,circle) ;
// define the task
// - we want an eye-in-hand control law
// - robot is controlled in the camera frame
task.setServo(vpServo::EYEINHAND_CAMERA) ;
task.setInteractionMatrixType(vpServo::DESIRED) ;
// - we want to see a circle on a circle
task.addFeature(p,pd) ;
// - set the gain
task.setLambda(1) ;
// Display task information
task.print() ;
unsigned int iter=0 ;
// loop
while(iter++ < 200)
{
std::cout << "---------------------------------------------" << iter <<std::endl ;
vpColVector v ;
// get the robot position
robot.getPosition(wMc) ;
// Compute the position of the camera wrt the object frame
cMo = wMc.inverse() * wMo;
// new circle position
// retrieve x,y and Z of the vpCircle structure
circle.track(cMo) ;
vpFeatureBuilder::create(p,circle);
circle.print() ;
p.print() ;
if (opt_display) {
vpDisplay::display(I) ;
vpServoDisplay::display(task,cam,I) ;
vpDisplay::flush(I) ;
}
// compute the control law
v = task.computeControlLaw() ;
std::cout << "task rank: " << task.getTaskRank() <<std::endl ;
// send the camera velocity to the controller
robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
std::cout << "|| s - s* || = " << ( task.getError() ).sumSquare() <<std::endl ;
}
// Display task information
task.print() ;
task.kill();
if (opt_display && opt_click_allowed) {
std::cout << "Click in the camera view window to end..." << std::endl;
vpDisplay::getClick(I) ;
}
return 0;
}
catch(vpException e) {
std::cout << "Catch a ViSP exception: " << e << std::endl;
return 1;
}
}
int
main(int argc, const char ** argv)
{
// Read the command line options
if (getOptions(argc, argv) == false) {
exit (-1);
}
// Log file creation in /tmp/$USERNAME/log.dat
// This file contains by line:
// - the 6 computed camera velocities (m/s, rad/s) to achieve the task
// - the 6 values of s - s*
std::string username;
// Get the user login name
vpIoTools::getUserName(username);
// Create a log filename to save velocities...
std::string logdirname;
#ifdef WIN32
logdirname ="C:/temp/" + username;
#else
logdirname ="/tmp/" + username;
#endif
// Test if the output path exist. If no try to create it
if (vpIoTools::checkDirectory(logdirname) == false) {
try {
// Create the dirname
vpIoTools::makeDirectory(logdirname);
}
catch (...) {
std::cerr << std::endl
<< "ERROR:" << std::endl;
std::cerr << " Cannot create " << logdirname << std::endl;
exit(-1);
}
}
std::string logfilename;
logfilename = logdirname + "/log.dat";
// Open the log file name
std::ofstream flog(logfilename.c_str());
vpRobotCamera robot ;
std::cout << std::endl ;
std::cout << "-------------------------------------------------------" << std::endl ;
std::cout << " Test program for vpServo " <<std::endl ;
std::cout << " Eye-in-hand task control, velocity computed in the camera frame" << std::endl ;
std::cout << " Simulation " << std::endl ;
std::cout << " task : 3D visual servoing " << std::endl ;
std::cout << "-------------------------------------------------------" << std::endl ;
std::cout << std::endl ;
// Sets the initial camera location
vpPoseVector c_r_o(// Translation tx,ty,tz
0.1, 0.2, 2,
// ThetaU rotation
vpMath::rad(20), vpMath::rad(10), vpMath::rad(50) ) ;
// From the camera pose build the corresponding homogeneous matrix
vpHomogeneousMatrix cMo(c_r_o) ;
// Set the robot initial position
robot.setPosition(cMo) ;
// Sets the desired camera location
vpPoseVector cd_r_o(// Translation tx,ty,tz
0, 0, 1,
// ThetaU rotation
vpMath::rad(0),vpMath::rad(0),vpMath::rad(0)) ;
// From the camera desired pose build the corresponding homogeneous matrix
vpHomogeneousMatrix cdMo(cd_r_o) ;
vpHomogeneousMatrix cMcd; // Transformation between current and desired camera frame
vpRotationMatrix cRcd; // Rotation between current and desired camera frame
// Set the constant gain of the servo
double lambda = 1;
int iter=0 ;
// Start the visual servoing loop. We stop the servo after 200 iterations
while(iter++ < 200) {
std::cout << "------------------------------------" << iter <<std::endl ;
// get the robot position
robot.getPosition(cMo) ;
// new displacement to achieve
cMcd = cMo*cdMo.inverse() ;
// Extract the translation vector ctc* which is the current
// translational visual feature.
vpTranslationVector ctcd;
cMcd.extract(ctcd);
// Compute the current theta U visual feature
vpThetaUVector tu_cRcd(cMcd);
// Create the identity matrix
vpMatrix I(3,3);
I.setIdentity();
// Compute the camera translational velocity
vpColVector v(3);
v = lambda * ( I - vpColVector::skew(tu_cRcd) ) * ctcd;
// Compute the camera rotational velocity
vpColVector w(3);
w = lambda * tu_cRcd;
// Update the complete camera velocity vector
vpColVector velocity(6);
for (int i=0; i<3; i++) {
velocity[i] = v[i]; // Translational velocity
velocity[i+3] = w[i]; // Rotational velocity
}
// Send the camera velocity to the controller
robot.setVelocity(vpRobot::CAMERA_FRAME, velocity) ;
// Retrieve the error (s-s*)
std::cout << ctcd.t() << " " << tu_cRcd.t() << std::endl;
// Save log
flog << velocity.t() << " " << ctcd.t() << " " << tu_cRcd.t() << std::endl;
}
// Close the log file
flog.close();
}
