C++ vpColVector::t方法代码示例

/*!

  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;
}