int main(int argc, char *argv[]) { Node root("root"); Node etc("etc"); root.add(&etc); Leaf hosts("hosts"); etc.add(&hosts); Node apache("apache"); etc.add(&apache); Leaf httpd("httpd.conf"); apache.add(&httpd); Node home("home"); root.add(&home); Node admin("admin"); home.add(&admin); Leaf doc("document.txt"); admin.add(&doc); Node piero("piero"); home.add(&piero); Leaf song("song.mp3"); piero.add(&song); root.dump(); }
void FileTest::testIsDirectory() { /* test signature () const */ ibrcommon::File etc("/etc"); CPPUNIT_ASSERT(etc.isDirectory()); ibrcommon::File resolve("/etc/resolv.conf"); CPPUNIT_ASSERT(!resolve.isDirectory()); }
QString Location::top() const { QString str = filePath(); if (lineNo() >= 1) { str += QLatin1Char(':'); str += QString::number(lineNo()); } if (etc()) str += QLatin1String(" (etc.)"); return str; }
QString Location::top() const { QString str = filePath(); if (!QDir::isAbsolutePath(str)) { QDir path(str); str = path.absolutePath(); } if (lineNo() >= 1) { str += QLatin1Char(':'); str += QString::number(lineNo()); } if (etc()) str += QLatin1String(" (etc.)"); return str; }
QString Location::top() const { QString str = filePath(); if (lineNo() >= 1) { str += QLatin1Char(':'); str += QString::number(lineNo()); #if 0 if (columnNo() >= 1) str += ":" + QString::number(columnNo()); #endif } if (etc()) str += QLatin1String(" (etc.)"); return str; }
SortServiceModel::SortServiceModel(QObject *parent) : QSortFilterProxyModel(parent) { /* testing purpose */ QString testPath(getenv("SERVICES_PRIORITY")); if (!testPath.isEmpty()) { QDir testDir(testPath); QStringList testPriorityList = testDir.entryList(QDir::Files); foreach(QString filename, testPriorityList) createPriorities(testDir.filePath(filename)); } else { QDir etc(QDir::rootPath() + QLatin1String("/etc/accounts-ui/service-sorting.d")); QDir home(QDir::homePath() + QLatin1String("/.config/accounts-ui/service-sorting.d")); QStringList etcPriorityList = etc.entryList(QDir::Files); foreach(QString filename, etcPriorityList) createPriorities(etc.filePath(filename)); QStringList homePriorityList = home.entryList(QDir::Files); foreach(QString filename, homePriorityList) createPriorities(home.filePath(filename)); } }
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 ; } }
// ###################################################################### static int submain(const int argc, char** argv) { MYLOGVERB = LOG_INFO; // suppress debug messages // Instantiate a ModelManager: ModelManager manager("test get eye position"); nub::soft_ref<EyeTrackerConfigurator> etc(new EyeTrackerConfigurator(manager)); manager.addSubComponent(etc); nub::soft_ref<EventLog> el(new EventLog(manager)); manager.addSubComponent(el); nub::soft_ref<PsychoDisplay> d(new PsychoDisplay(manager)); manager.addSubComponent(d); manager.setOptionValString(&OPT_EventLogFileName, "psychodata.psy"); manager.setOptionValString(&OPT_EyeTrackerType, "ISCAN"); // Parse command-line: if (manager.parseCommandLine(argc, argv, "", 0, 0) == false) return(1); // hook our various babies up and do post-command-line configs: nub::soft_ref<EyeTracker> et = etc->getET(); et->setEventLog(el); // let's get all our ModelComponent instances started: manager.start(); d->setEyeTracker(et); d->setEventLog(el); el->pushEvent(std::string("===== Trial 1: =====")); //lets time stamp event log and start the eyetracker //et->track(true); //here we will start testing the get pos LINFO("eyescanner started going to start reading stuff"); // int i=0; d->clearScreen(); d->displayISCANcalib(); d->waitForKey(); Image< PixRGB<byte> > displayImage (1920,1080, ZEROS); drawCircle(displayImage, Point2D<int>(1920/2,1080/2), 5, PixRGB<byte>(255,0,0)); SDL_Surface *surf = d->makeBlittableSurface(displayImage, true); d->displaySurface(surf, -2); CalibrationTransform::Data pts; AffineTransform a; Image<double> txf; Point2D<int> testpoint; Point2D<double> testpointCalib,testpointD; LINFO("precalib"); pts = et->getCalibrationSet(d); LINFO("postcalib"); txf = a.computeTransform(pts); LINFO("transform is..."); std::cerr << txf << std::endl; testpoint = et->getEyePos(); testpointD = Point2D<double>(testpoint); LINFO("\n testpoint %d,%d",testpoint.i,testpoint.j); testpointCalib = a.getCalibrated(testpointD); LINFO("\n testpoint %d,%d, calibrated %f,%f",testpoint.i,testpoint.j,testpointCalib.i,testpointCalib.j); d->clearScreen(); //now for a live demo of real time eyetracking char tmp[40]; //SDL_Surface *surf2; while(d->checkForKey() < 0) { displayImage.clear(); testpoint = et->getEyePos(); testpointD = Point2D<double>(testpoint); testpointCalib = a.getCalibrated(testpointD); LINFO("\n testpoint %d,%d, calibrated %f,%f",testpoint.i,testpoint.j,testpointCalib.i,testpointCalib.j); drawCircle(displayImage, Point2D<int>(testpointCalib), 2, PixRGB<byte>(255,0,0)); sprintf(tmp,"(%d,%d)",(int)testpointCalib.i,(int)testpointCalib.j); writeText(displayImage,Point2D<int>(testpointCalib),tmp,PixRGB<byte>(255,0,0),PixRGB<byte>(0,0,0),SimpleFont::FIXED(8)); // d->displayImage(displayImage, true, PixRGB<byte>(), -2); SDL_Surface *surf2 = d->makeBlittableSurface(displayImage, true); d->displaySurface(surf2, -2); SDL_FreeSurface(surf2); // i++; } // struct EyeposEvent // { // uint64; // float x, y; // }; // std::vector<EyeposEvent> evs; // evs.reserve(100000); // stop the eye tracker: usleep(50000); et->track(false); // stop all our ModelComponents manager.stop(); // all done! return 0; }