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());
}
Exemple #3
0
QString Location::top() const
{
    QString str = filePath();
    if (lineNo() >= 1) {
        str += QLatin1Char(':'); 
        str += QString::number(lineNo());
    }
    if (etc())
        str += QLatin1String(" (etc.)");
    return str;
}
Exemple #4
0
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;
}
Exemple #5
0
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 ;
  }
}
Exemple #9
0
// ######################################################################
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;


}