/*! Return the position of each axis. - In positionning control mode, call vpRobotBiclopsController::getPosition() - In speed control mode, call vpRobotBiclopsController::getActualPosition() \param frame : Control frame. This biclops head can only be controlled in articular. \param q : The position of the axis in radians. \exception vpRobotException::wrongStateError : If a not supported frame type is given. */ void vpRobotBiclops::getPosition (const vpRobot::vpControlFrameType frame, vpColVector & q) { switch(frame) { case vpRobot::CAMERA_FRAME : vpERROR_TRACE ("Cannot get position in camera frame: not implemented"); throw vpRobotException (vpRobotException::wrongStateError, "Cannot get position in camera frame: " "not implemented"); break; case vpRobot::REFERENCE_FRAME: vpERROR_TRACE ("Cannot get position in reference frame: " "not implemented"); throw vpRobotException (vpRobotException::wrongStateError, "Cannot get position in reference frame: " "not implemented"); break; case vpRobot::MIXT_FRAME: vpERROR_TRACE ("Cannot get position in mixt frame: " "not implemented"); throw vpRobotException (vpRobotException::wrongStateError, "Cannot get position in mixt frame: " "not implemented"); break; case vpRobot::ARTICULAR_FRAME: break ; } vpRobot::vpRobotStateType state; state = vpRobot::getRobotState(); switch (state) { case STATE_STOP: case STATE_POSITION_CONTROL: q = controller.getPosition(); break; case STATE_VELOCITY_CONTROL: case STATE_ACCELERATION_CONTROL: default: q.resize(vpBiclops::ndof); vpDEBUG_TRACE (12, "Lock mutex vpMeasure_mutex"); pthread_mutex_lock(&vpMeasure_mutex); // Wait until a position is available vpRobotBiclopsController::shmType shm; vpDEBUG_TRACE (12, "Lock mutex vpShm_mutex"); pthread_mutex_lock(&vpShm_mutex); shm = controller.readShm(); vpDEBUG_TRACE (12, "unlock mutex vpShm_mutex"); pthread_mutex_unlock(&vpShm_mutex); for (unsigned int i=0; i < vpBiclops::ndof; i ++) { q[i] = shm.actual_q[i]; } vpCDEBUG(11) << "++++++++ Measure actuals: " << q.t(); vpDEBUG_TRACE (12, "unlock mutex vpMeasure_mutex"); pthread_mutex_unlock(&vpMeasure_mutex); // A position is available break; } }
static void lagrange (vpMatrix &a, vpMatrix &b, vpColVector &x1, vpColVector &x2) { if (DEBUG_LEVEL1) std::cout << "begin (CLagrange.cc)Lagrange(...) " << std::endl; try{ int i,imin; vpMatrix ata ; // A^T A ata = a.t()*a ; vpMatrix btb ; // B^T B btb = b.t()*b ; vpMatrix bta ; // B^T A bta = b.t()*a ; vpMatrix btb1 ; // (B^T B)^(-1) if (b.getRows() >= b.getCols()) btb1 = btb.inverseByLU() ; else btb1 = btb.pseudoInverse(); if (DEBUG_LEVEL1) { std::cout << " BTB1 * BTB : " << std::endl << btb1*btb << std::endl; std::cout << " BTB * BTB1 : " << std::endl << btb*btb1 << std::endl; } vpMatrix r ; // (B^T B)^(-1) B^T A r = btb1*bta ; vpMatrix e ; // - A^T B (B^T B)^(-1) B^T A e = - (a.t()*b) *r ; e += ata ; // calcul E = A^T A - A^T B (B^T B)^(-1) B^T A if (DEBUG_LEVEL1) { std::cout << " E :" << std::endl << e << std::endl; } // vpColVector sv ; // vpMatrix v ; e.svd(x1,ata) ;// destructif sur e // calcul du vecteur propre de E correspondant a la valeur propre min. /* calcul de SVmax */ imin = 0; // FC : Pourquoi calculer SVmax ?????? // double svm = 0.0; // for (i=0;i<x1.getRows();i++) // { // if (x1[i] > svm) { svm = x1[i]; imin = i; } // } // svm *= EPS; /* pour le rang */ for (i=0;i<x1.getRows();i++) if (x1[i] < x1[imin]) imin = i; if (DEBUG_LEVEL1) { printf("SV(E) : %.15lf %.15lf %.15lf\n",x1[0],x1[1],x1[2]); std::cout << " i_min " << imin << std::endl; } for (i=0;i<x1.getRows();i++) x1[i] = ata[i][imin]; x2 = - (r*x1) ; // X_2 = - (B^T B)^(-1) B^T A X_1 if (DEBUG_LEVEL1) { std::cout << " X1 : " << x1.t() << std::endl; std::cout << " V : " << std::endl << ata << std::endl; } } catch(...) { vpERROR_TRACE(" ") ; throw ; } if (DEBUG_LEVEL1) std::cout << "end (CLagrange.cc)Lagrange(...) " << std::endl; }
/*! Compute the covariance matrix of the parameters x from a least squares minimisation defined as: Ax = b \param A : Matrix A from Ax = b. \param x : Vector x from Ax = b corresponding to the parameters to estimate. \param b : Vector b from Ax = b. */ vpMatrix vpMatrix::computeCovarianceMatrix(const vpMatrix &A, const vpColVector &x, const vpColVector &b) { double sigma2 = ( ((b.t())*b) - ( (b.t())*A*x ) ); return (A.t()*A).pseudoInverse()*sigma2; }