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

double
vpPose::computeResidual(vpColVector &x, vpColVector &M, vpColVector &d)
{

  unsigned int i ;
  unsigned int n = x.getRows()/5 ;

  vpPoint *p;
  p = new vpPoint [n] ;
  {
    //    firsttime=1 ;
    for( i=0 ; i < n ; i++)
    {
      p[i].setWorldCoordinates(x[5*i+2],x[5*i+3], x[5*i+4]) ;
    }
  }

  vpHomogeneousMatrix cMo ;
  for (i=0 ; i <16 ; i++)
  {
    cMo.data[i] = M[i];
  }


  d.resize(n) ;
  vpColVector cP, xy ;

  for( i=0 ; i < n ; i++)
  {
    p[i].changeFrame(cMo,cP) ;
    p[i].projection(cP,xy) ;
    d[i] = sqrt(vpMath::sqr(x[5*i]-xy[0])+vpMath::sqr(x[5*i+1]-xy[1])) ;
  }

  delete [] p;

  return 0 ;
}

// Calculate the modes of the density for the distribution
// and their associated errors
double
vpScale::MeanShift(vpColVector &error)
{

  int n = error.getRows()/dimension;
  vpColVector density(n);
  vpColVector density_gradient(n);
  vpColVector mean_shift(n);

  int increment=1;

  // choose smallest error as start point
  int i=0;
  while(error[i]<0 && error[i]<error[i+1])
    i++;

  // Do mean shift until no shift
  while(increment >= 1 && i<n)
  {
    increment=0;
    density[i] = KernelDensity(error, i);
    density_gradient[i] = KernelDensityGradient(error, i);
    mean_shift[i]=vpMath::sqr(bandwidth)*density_gradient[i]/((dimension+2)*density[i]);

    double tmp_shift = mean_shift[i];

    // Do mean shift
    while(tmp_shift>0 && tmp_shift>error[i]-error[i+1])
    {
      i++;
      increment++;
      tmp_shift-=(error[i]-error[i-1]);
    }
  }

  return error[i];

}

/*!

  Operator that allows to multiply a twist transformation matrix by a
  column vector.

  Operation c = V * v (V, the velocity skew transformation matrix is unchanged,
  c and v are 6 dimension vectors).

  \param v : Velocity skew vector.

  \exception vpMatrixException::incorrectMatrixSizeError If v is not a 6
  dimension vector.

*/
vpColVector
vpVelocityTwistMatrix::operator*(const vpColVector &v) const
{
  vpColVector c(6);

  if (6 != v.getRows())
    {
      vpERROR_TRACE("vpVelocityTwistMatrix mismatch in vpVelocityTwistMatrix/vector multiply") ;
      throw(vpMatrixException::incorrectMatrixSizeError) ;
    }

  c = 0.0;

  for (int i=0;i<6;i++) {
    for (int j=0;j<6;j++) {
      {
 	c[i]+=rowPtrs[i][j] * v[j];
      }
    }
  }

  return c ;
}

/*!
  Compute the skew symmetric matrix \f$[{\bf v}]_\times\f$ of vector v.

  \f[ \mbox{if} \quad  {\bf V} =  \left( \begin{array}{c} x \\ y \\  z
  \end{array}\right), \quad \mbox{then} \qquad
  [{\bf v}]_\times = \left( \begin{array}{ccc}
  0 & -z & y \\
  z & 0 & -x \\
  -y & x & 0
  \end{array}\right)
  \f]

  \param v : Input vector used to compute the skew symmetric matrix.
*/
vpMatrix
vpColVector::skew(const vpColVector &v)
{

  vpMatrix M ;
  if (v.getRows() != 3)
  {
    vpERROR_TRACE("Cannot compute skew kinematic matrix,"
		"v has not 3 rows") ;
    throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
			    "\n\t\t Cannot compute skew kinematic matrix"
			    "v has not 3 rows")) ;
  }


  M.resize(3,3) ;
  M[0][0] = 0 ;     M[0][1] = -v[2] ;  M[0][2] = v[1] ;
  M[1][0] = v[2] ;  M[1][1] = 0 ;      M[1][2] = -v[0] ;
  M[2][0] = -v[1] ; M[2][1] = v[0] ;   M[2][2] = 0 ;


  return M ;
}

// Fit model to this random selection of data points.
void
vpHomography::computeTransformation(vpColVector &x, unsigned int *ind, vpColVector &M)
{
  unsigned int i ;
  unsigned int n = x.getRows()/4 ;
  double xa[4], xb[4];
  double ya[4], yb[4];
  unsigned int n2 = n * 2;
  unsigned int ind2;
  for(i=0 ; i < 4 ; i++)
    {
      ind2 = 2 * ind[i];
      xb[i] = x[ind2] ;
      yb[i] = x[ind2+1] ;

      xa[i] = x[n2+ind2] ;
      ya[i] = x[n2+ind2+1] ;
    }

  vpHomography aHb ;
  try {
    vpHomography::HLM(4,xb, yb, xa, ya, true, aHb);
    //vpHomography::HLM(8, xb, yb, xa, ya, false, aHb); //modified 13/09
  }
  catch(...)
    {
      aHb.setIdentity();
    }

  M.resize(9);
  for (i=0 ; i <9 ; i++)
    {
      M[i] = aHb.data[i] ;
    }
  aHb /= aHb[2][2] ;
}

/*!
  \brief calculation of McLure's influence function

  \param sigma : sigma parameters
  \param x : normalized residue vector
  \param weights : weight vector
*/
void vpRobust::psiMcLure(double sig, vpColVector &r,vpColVector &weights)
{
  unsigned int n_data = r.getRows();

  //McLure's function
  for(unsigned int i=0; i<n_data; i++)
  {
    weights[i] = 1/(vpMath::sqr(1+vpMath::sqr(r[i]/sig)));
    //w[i] = 2*mad/vpMath::sqr((mad+r[i]*r[i]));//odobez

    // If one coordinate is an outlier the other is too!
    // w[i] < 0.01 is a threshold to be set
    /*if(w[i] < 0.01)
      {
      if(i%2 == 0)
      {
      w[i+1] = w[i];
      i++;
      }
      else
      w[i-1] = w[i];
      }*/
  }
}

void vpRobust::psiCauchy(double sig, vpColVector &x, vpColVector &weights)
{
  unsigned int n_data = x.getRows();
  double const_sig = 2.3849*sig;

  //Calculate Cauchy's equation
  for(unsigned int i=0; i<n_data; i++)
  {
    weights[i] = 1/(1+vpMath::sqr(x[i]/(const_sig)));

    // If one coordinate is an outlier the other is too!
    // w[i] < 0.01 is a threshold to be set
    /*if(w[i] < 0.01)
      {
      if(i%2 == 0)
      {
      w[i+1] = w[i];
      i++;
      }
      else
      w[i-1] = w[i];
      }*/
  }
}

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;

}

static void
calcul_masques(vpColVector &angle, // definitions des angles theta
	       unsigned int n,             // taille masques (PAIRE ou IMPAIRE Ok)
         vpMatrix *M)        // resultat M[theta](n,n)
{
  // Le coef |a| = |1/2n| n'est pas incorpore dans M(i,j) (=> que des int)

  unsigned int i_theta,  // indice (boucle sur les masques)
       i,j;      // indices de boucle sur M(i,j)
  double X,Y,   // point correspondant/centre du masque
    theta, cos_theta, sin_theta, tan_theta,
    moitie = ((double)n)/2.0; // moitie REELLE du masque
  point P1,Q1,P,Q;  // clippe Droite(theta) P1,Q1 -> P,Q
  int    sgn;       // signe de M(i,j)
  double v;         // ponderation de M(i,j)

 unsigned int nb_theta = angle.getRows() ;

 for(i_theta=0; i_theta<nb_theta; i_theta++)
 {
   theta = M_PI/180*angle[i_theta]; // indice i -> theta(i) en radians
   																//  angle[] dans [0,180[
   cos_theta = cos(theta);        // vecteur directeur de l'ECM
   sin_theta = sin(theta);        //  associe au masque

   // PRE-CALCULE 2 POINTS DE D(theta) BIEN EN DEHORS DU MASQUE
   // =========================================================
   //if( angle[i_theta]==90 )                     // => tan(theta) infinie !
   if( std::fabs(angle[i_theta]-90) <= vpMath::maximum(std::fabs(angle[i_theta]), 90.)*std::numeric_limits<double>::epsilon() )                     // => tan(theta) infinie !
   {
     P1.x=0; P1.y=-(int)n;
     Q1.x=0; Q1.y= n;
   }
   else
   {
     tan_theta = sin_theta/cos_theta;       // pente de la droite D(theta)
     P1.x=-(int)n; P1.y=tan_theta*(-(int)n);
     Q1.x=n;  Q1.y=tan_theta*n;
   }

   // CALCULE MASQUE M(theta)
   // ======================
   M[i_theta].resize(n,n);  // allocation (si necessaire)

   for(i=0,Y=-moitie+0.5 ;   i<n  ; i++,Y++)
   {
     for(j=0,X=-moitie+0.5 ;   j<n  ; j++,X++)
     {
       // produit vectoriel dir_droite*(X,Y)
       sgn = vpMath::sign(cos_theta*Y - sin_theta*X);

       // Resultat = P,Q
       if( clipping(P1,Q1, X-0.5,Y-0.5,X+0.5,Y+0.5, P,Q) )
       {
	 // v dans [0,1]
	 v=S_relative(P,Q, X-0.5,Y-0.5,X+0.5,Y+0.5);
       }
       else
	 v=1; // PQ ne coupe pas le pixel(i,j)

       M[i_theta][i][j] = vpMath::round(100*sgn*v);

       // 2 chiffres significatifs
       // M(i,j) sans incorporer le coef a
     }
   }
 }

}

/*!
  Return the direct geometric model of the end effector: fMe

  \param q : Articular position for pan and tilt axis.

  \return fMe, the homogeneous matrix corresponding to the direct geometric
  model of the end effector. Describes the transformation between the robot reference
  frame (called fixed) and the end effector frame.

*/
vpHomogeneousMatrix
vpBiclops::get_fMe (const vpColVector & q)
{
  vpHomogeneousMatrix fMe;

  if (q.getRows() != 2) {
    vpERROR_TRACE("Bad dimension for biclops articular vector");
    throw(vpException(vpException::dimensionError, "Bad dimension for biclops articular vector"));
  }

  double            q1 = q[0]; // pan
  double            q2 = q[1]; // tilt

  double            c1 = cos(q1);
  double            s1 = sin(q1);
  double            c2 = cos(q2);
  double            s2 = sin(q2);

  if (dh_model_ == DH1)
  {
    fMe[0][0] = -c1*s2;
    fMe[0][1] = -s1;
    fMe[0][2] = c1*c2;
    fMe[0][3] = 0;

    fMe[1][0] = -s1*s2;
    fMe[1][1] = c1;
    fMe[1][2] = s1*c2;
    fMe[1][3] = 0;

    fMe[2][0] = -c2;
    fMe[2][1] = 0;
    fMe[2][2] = -s2;
    fMe[2][3] = 0;

    fMe[3][0] = 0;
    fMe[3][1] = 0;
    fMe[3][2] = 0;
    fMe[3][3] = 1;
  }
  else
  {
    fMe[0][0] = c1*s2;
    fMe[0][1] = -s1;
    fMe[0][2] = c1*c2;
    fMe[0][3] = 0;

    fMe[1][0] = s1*s2;
    fMe[1][1] = c1;
    fMe[1][2] = s1*c2;
    fMe[1][3] = 0;

    fMe[2][0] = -c2;
    fMe[2][1] = 0;
    fMe[2][2] = s2;
    fMe[2][3] = 0;

    fMe[3][0] = 0;
    fMe[3][1] = 0;
    fMe[3][2] = 0;
    fMe[3][3] = 1;
  }

  return fMe;
}

void
vpMbKltTracker::computeVVSPoseEstimation(const unsigned int iter, vpMatrix &L,
    const vpColVector &w, vpMatrix &L_true, vpMatrix &LVJ_true, double &normRes, double &normRes_1, vpColVector &w_true,
    vpColVector &R, vpMatrix &LTL, vpColVector &LTR, vpColVector &error_prev, vpColVector &v, double &mu,
    vpHomogeneousMatrix &cMoPrev, vpHomogeneousMatrix &ctTc0_Prev) {
  m_error = R;
  if(computeCovariance){
    L_true = L;
    if(!isoJoIdentity){
       vpVelocityTwistMatrix cVo;
       cVo.buildFrom(cMo);
       LVJ_true = (L*cVo*oJo);
    }
  }

  normRes_1 = normRes;
  normRes = 0;
  for (unsigned int i = 0; i < static_cast<unsigned int>(R.getRows()); i += 1){
    w_true[i] = w[i];
    R[i] = R[i] * w[i];
    normRes += R[i];
  }

  if((iter == 0) || compute_interaction){
    for(unsigned int i=0; i<static_cast<unsigned int>(R.getRows()); i++){
      for(unsigned int j=0; j<6; j++){
        L[i][j] *= w[i];
      }
    }
  }

  if(isoJoIdentity){
      LTL = L.AtA();
      computeJTR(L, R, LTR);

      switch(m_optimizationMethod){
      case vpMbTracker::LEVENBERG_MARQUARDT_OPT:
      {
        vpMatrix LMA(LTL.getRows(), LTL.getCols());
        LMA.eye();
        vpMatrix LTLmuI = LTL + (LMA*mu);
        v = -lambda*LTLmuI.pseudoInverse(LTLmuI.getRows()*std::numeric_limits<double>::epsilon())*LTR;

        if(iter != 0)
          mu /= 10.0;

        error_prev = m_error;
        break;
      }
      case vpMbTracker::GAUSS_NEWTON_OPT:
      default:
        v = -lambda * LTL.pseudoInverse(LTL.getRows()*std::numeric_limits<double>::epsilon()) * LTR;
      }
  }
  else{
      vpVelocityTwistMatrix cVo;
      cVo.buildFrom(cMo);
      vpMatrix LVJ = (L*cVo*oJo);
      vpMatrix LVJTLVJ = (LVJ).AtA();
      vpColVector LVJTR;
      computeJTR(LVJ, R, LVJTR);

      switch(m_optimizationMethod){
      case vpMbTracker::LEVENBERG_MARQUARDT_OPT:
      {
        vpMatrix LMA(LVJTLVJ.getRows(), LVJTLVJ.getCols());
        LMA.eye();
        vpMatrix LTLmuI = LVJTLVJ + (LMA*mu);
        v = -lambda*LTLmuI.pseudoInverse(LTLmuI.getRows()*std::numeric_limits<double>::epsilon())*LVJTR;
        v = cVo * v;

        if(iter != 0)
          mu /= 10.0;

        error_prev = m_error;
        break;
      }
      case vpMbTracker::GAUSS_NEWTON_OPT:
      default:
      {
        v = -lambda*LVJTLVJ.pseudoInverse(LVJTLVJ.getRows()*std::numeric_limits<double>::epsilon())*LVJTR;
        v = cVo * v;
        break;
      }
      }
  }

  cMoPrev = cMo;
  ctTc0_Prev = ctTc0;
  ctTc0 = vpExponentialMap::direct(v).inverse() * ctTc0;
  cMo = ctTc0 * c0Mo;
}

void
vpRobotPtu46::setVelocity (const vpRobot::vpControlFrameType frame,
			   const vpColVector & v)
{
  TPtuFrame ptuFrameInterface;

  if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ())
  {
    vpERROR_TRACE ("Cannot send a velocity to the robot "
		 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
    throw vpRobotException (vpRobotException::wrongStateError,
			    "Cannot send a velocity to the robot "
			    "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
  }

  switch(frame)
  {
  case vpRobot::CAMERA_FRAME :
    {
      ptuFrameInterface = PTU_CAMERA_FRAME;
      if ( v.getRows() != 2) {
	vpERROR_TRACE ("Bad dimension fo speed vector in camera frame");
	throw vpRobotException (vpRobotException::wrongStateError,
				"Bad dimension for speed vector "
				"in camera frame");
      }
      break ;
    }
  case vpRobot::ARTICULAR_FRAME :
    {
      ptuFrameInterface = PTU_ARTICULAR_FRAME;
      if ( v.getRows() != 2) {
	vpERROR_TRACE ("Bad dimension fo speed vector in articular frame");
	throw vpRobotException (vpRobotException::wrongStateError,
				"Bad dimension for speed vector "
				"in articular frame");
      }
      break ;
    }
  case vpRobot::REFERENCE_FRAME :
    {
      vpERROR_TRACE ("Cannot send a velocity to the robot "
		   "in the reference frame: "
		   "functionality not implemented");
      throw vpRobotException (vpRobotException::wrongStateError,
			      "Cannot send a velocity to the robot "
			      "in the reference frame:"
			      "functionality not implemented");
      break ;
    }
  case vpRobot::MIXT_FRAME :
    {
      vpERROR_TRACE ("Cannot send a velocity to the robot "
		   "in the mixt frame: "
		   "functionality not implemented");
      throw vpRobotException (vpRobotException::wrongStateError,
			      "Cannot send a velocity to the robot "
			      "in the mixt frame:"
			      "functionality not implemented");
      break ;
    }
  default:
    {
      vpERROR_TRACE ("Error in spec of vpRobot. "
		   "Case not taken in account.");
      throw vpRobotException (vpRobotException::wrongStateError,
			      "Cannot send a velocity to the robot ");
    }
  }

  vpDEBUG_TRACE (12, "Velocity limitation.");
  bool norm = false; // Flag to indicate when velocities need to be nomalized
  double ptuSpeedInterface[2];

  switch(frame) {
  case vpRobot::ARTICULAR_FRAME :
  case vpRobot::CAMERA_FRAME : {
    double max = this ->maxRotationVelocity;
    for (unsigned int i = 0 ; i < 2; ++ i) // rx and ry of the camera
    {
      if (fabs (v[i]) > max)
      {
	norm = true;
	max = fabs (v[i]);
	vpERROR_TRACE ("Excess velocity: ROTATION "
		     "(axe nr.%d).", i);
      }
    }
    // Rotations velocities normalisation
    if (norm == true) {
      max =  this ->maxRotationVelocity / max;
      for (unsigned int i = 0 ; i < 2; ++ i)
	ptuSpeedInterface [i] = v[i]*max;
    }
    break;
  }
  default:
    // Should never occur
    break;

//.........这里部分代码省略.........

bool
vpHomography::degenerateConfiguration(vpColVector &x, unsigned int *ind,
				      double threshold_area)
{

  unsigned int i, j, k;

  for (i=1 ; i < 4 ; i++)
    for (j=0 ; j<i ; j++)
      if (ind[i]==ind[j]) return true ;

  unsigned int n = x.getRows()/4 ;
  double pa[4][3] ;
  double pb[4][3] ;



  for(i = 0 ; i < 4 ; i++)
  {
    pb[i][0] = x[2*ind[i]] ;
    pb[i][1] = x[2*ind[i]+1] ;
    pb[i][2] = 1;

    pa[i][0] = x[2*n+2*ind[i]] ;
    pa[i][1] = x[2*n+2*ind[i]+1] ;
    pa[i][2] = 1;
  }

  i = 0, j = 1, k = 2;

  double area012 = (-pa[j][0]*pa[i][1] + pa[k][0]*pa[i][1] +
		    pa[i][0]*pa[j][1] - pa[k][0]*pa[j][1] +
		    -pa[i][0]*pa[k][1] + pa[1][j]*pa[k][1]);

  i = 0; j = 1, k = 3;
  double area013 = (-pa[j][0]*pa[i][1] + pa[k][0]*pa[i][1] +
		    pa[i][0]*pa[j][1] - pa[k][0]*pa[j][1] +
		    -pa[i][0]*pa[k][1] + pa[1][j]*pa[k][1]);

  i = 0; j = 2, k = 3;
  double area023 = (-pa[j][0]*pa[i][1] + pa[k][0]*pa[i][1] +
		    pa[i][0]*pa[j][1] - pa[k][0]*pa[j][1] +
		    -pa[i][0]*pa[k][1] + pa[1][j]*pa[k][1]);

  i = 1; j = 2, k = 3;
  double area123 = (-pa[j][0]*pa[i][1] + pa[k][0]*pa[i][1] +
		    pa[i][0]*pa[j][1] - pa[k][0]*pa[j][1] +
		    -pa[i][0]*pa[k][1] + pa[1][j]*pa[k][1]);

  double sum_area = area012 + area013 + area023 + area123;

  return ((sum_area < threshold_area) ||
	  (iscolinear(pa[0],pa[1],pa[2]) ||
	   iscolinear(pa[0],pa[1],pa[3]) ||
	   iscolinear(pa[0],pa[2],pa[3]) ||
	   iscolinear(pa[1],pa[2],pa[3]) ||
	   iscolinear(pb[0],pb[1],pb[2]) ||
	   iscolinear(pb[0],pb[1],pb[3]) ||
	   iscolinear(pb[0],pb[2],pb[3]) ||
	   iscolinear(pb[1],pb[2],pb[3])));
}

/*!

  Send a velocity on each axis.

  \param frame : Control frame. This biclops head can only be controlled in
  articular. Be aware, the camera frame (vpRobot::CAMERA_FRAME), the reference
  frame (vpRobot::REFERENCE_FRAME) and the mixt frame (vpRobot::MIXT_FRAME) are
  not implemented.

  \param q_dot : The desired articular velocity of the axis in rad/s. \f$ \dot
  {r} = [\dot{q}_1, \dot{q}_2]^t \f$ with \f$ \dot{q}_1 \f$ the pan of the
  camera and \f$ \dot{q}_2\f$ the tilt of the camera.

  \exception vpRobotException::wrongStateError : If a the robot is not
  configured to handle a velocity. The robot can handle a velocity only if the
  velocity control mode is set. For that, call setRobotState(
  vpRobot::STATE_VELOCITY_CONTROL) before setVelocity().

  \exception vpRobotException::wrongStateError : If a not supported frame type
  (vpRobot::CAMERA_FRAME, vpRobot::REFERENCE_FRAME or vpRobot::MIXT_FRAME) is
  given.

  \warning Velocities could be saturated if one of them exceed the maximal
  autorized speed (see vpRobot::maxRotationVelocity).

*/
void
vpRobotBiclops::setVelocity (const vpRobot::vpControlFrameType frame,
			     const vpColVector & q_dot)
{
  if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ())
  {
    vpERROR_TRACE ("Cannot send a velocity to the robot "
		 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
    throw vpRobotException (vpRobotException::wrongStateError,
			    "Cannot send a velocity to the robot "
			    "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
  }

  switch(frame)
  {
  case vpRobot::CAMERA_FRAME :
    {
      vpERROR_TRACE ("Cannot send a velocity to the robot "
		   "in the camera frame: "
		   "functionality not implemented");
      throw vpRobotException (vpRobotException::wrongStateError,
			      "Cannot send a velocity to the robot "
			      "in the camera frame:"
			      "functionality not implemented");
      break ;
    }
  case vpRobot::ARTICULAR_FRAME :
    {
      if ( q_dot.getRows() != 2) {
	vpERROR_TRACE ("Bad dimension fo speed vector in articular frame");
	throw vpRobotException (vpRobotException::wrongStateError,
				"Bad dimension for speed vector "
				"in articular frame");
      }
      break ;
    }
  case vpRobot::REFERENCE_FRAME :
    {
      vpERROR_TRACE ("Cannot send a velocity to the robot "
		   "in the reference frame: "
		   "functionality not implemented");
      throw vpRobotException (vpRobotException::wrongStateError,
			      "Cannot send a velocity to the robot "
			      "in the reference frame:"
			      "functionality not implemented");
      break ;
    }
  case vpRobot::MIXT_FRAME :
    {
      vpERROR_TRACE ("Cannot send a velocity to the robot "
		   "in the mixt frame: "
		   "functionality not implemented");
      throw vpRobotException (vpRobotException::wrongStateError,
			      "Cannot send a velocity to the robot "
			      "in the mixt frame:"
			      "functionality not implemented");
      break ;
    }
  default:
    {
      vpERROR_TRACE ("Error in spec of vpRobot. "
		   "Case not taken in account.");
      throw vpRobotException (vpRobotException::wrongStateError,
			      "Cannot send a velocity to the robot ");
    }
  }

  vpDEBUG_TRACE (12, "Velocity limitation.");
  bool norm = false; // Flag to indicate when velocities need to be nomalized

  // Saturate articular speed
  double max = vpBiclops::speedLimit;
  vpColVector q_dot_sat(vpBiclops::ndof);

//.........这里部分代码省略.........

double vpRobust::computeNormalizedMedian(vpColVector &all_normres,
					 const vpColVector &residues,
					 const vpColVector &all_residues,
					 const vpColVector & weights
					 )
{
  double med=0;
  double normmedian=0;

  unsigned int n_all_data = all_residues.getRows();
  unsigned int n_data = residues.getRows();
  
  // resize vector only if the size of residue vector has changed
  resize(n_data);
    
  sorted_residues = residues;
  vpColVector no_null_weight_residues;
  no_null_weight_residues.resize(n_data);
  
  //all_normres.resize(n_all_data); // Normalized Residue
  //vpColVector sorted_normres(n_data); // Normalized Residue
  //vpColVector sorted_residues = residues;
  //vpColVector sorted_residues;
  

  unsigned int index =0;
  for(unsigned int j=0;j<n_data;j++)
  {
    //if(weights[j]!=0)
    if(std::fabs(weights[j]) > std::numeric_limits<double>::epsilon())
    {
      no_null_weight_residues[index]=residues[j];
      index++;
    }
  }
  sorted_residues.resize(index);
  memcpy(sorted_residues.data,no_null_weight_residues.data,index*sizeof(double));
  n_data=index;

  vpCDEBUG(2) << "vpRobust MEstimator reached. No. data = " << n_data
	      << std::endl;

  // Calculate Median
  // Be careful to not use the rejected residues for the
  // calculation.
  
  unsigned int ind_med = (unsigned int)(ceil(n_data/2.0))-1;
  med = select(sorted_residues, 0, (int)n_data-1, (int)ind_med/*(int)n_data/2*/);

  unsigned int i;
  // Normalize residues
  for(i=0; i<n_all_data; i++)
  {
    all_normres[i] = (fabs(all_residues[i]- med));
  }

  for(i=0; i<n_data; i++)
  {
    sorted_normres[i] = (fabs(sorted_residues[i]- med));
  }
  // MAD calculated only on first iteration

  //normmedian = Median(normres, weights);
  //normmedian = Median(normres);
  normmedian = select(sorted_normres, 0, (int)n_data-1, (int)ind_med/*(int)n_data/2*/);

  return normmedian;
}