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()
{
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 ;
}
}
