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 <L, vpColVector <R, 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;
}
vpCDEBUG(12) << "v: " << ptuSpeedInterface[0]
<< " " << ptuSpeedInterface[1] << std::endl;
ptu.move(ptuSpeedInterface, ptuFrameInterface);
return;
}
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);
// init q_dot_saturated
q_dot_sat = q_dot;
for (unsigned int i = 0 ; i < vpBiclops::ndof; ++ i) // q1 and q2
{
if (fabs (q_dot[i]) > max)
{
norm = true;
max = fabs (q_dot[i]);
vpERROR_TRACE ("Excess velocity: ROTATION "
"(axe nr.%d).", i);
}
}
// Rotations velocities normalisation
if (norm == true) {
max = vpBiclops::speedLimit / max;
q_dot_sat = q_dot * max;
}
vpCDEBUG(12) << "send velocity: " << q_dot_sat.t() << std::endl;
vpRobotBiclopsController::shmType shm;
vpDEBUG_TRACE (12, "Lock mutex vpShm_mutex");
pthread_mutex_lock(&vpShm_mutex);
shm = controller.readShm();
for (unsigned int i=0; i < vpBiclops::ndof; i ++)
shm.q_dot[i] = q_dot[i];
controller.writeShm(shm);
vpDEBUG_TRACE (12, "unlock mutex vpShm_mutex");
pthread_mutex_unlock(&vpShm_mutex);
return;
}
// Fill in the plot message for publishing data corresponding to 2 tasks
void makePlotMSG(std_msgs::Float64MultiArray &plotMsg,
const vpColVector &taskError_1, //error of task 1
const vpColVector &qdot_1, //qdot computed to accomplish task 1
const vpColVector &taskError_2, //error of task 2
const vpColVector &qdot_2, //qdot computed to accomplish task 2
const vpColVector &q1dot_1, //q1dot of task1
const vpColVector &q2dot_1, //q2dot of task1
const vpColVector &q1dot_2, //q1dot of task2
const vpColVector &q2dot_2, //q2dot of task2
double x_midPoint, //normalized coordinate x of midpoint(hand marker, object marker)
double y_midPoint) //normalized coordinate y of midpoint(hand marker, object marker)
{
/////////////////////////////////////////////////////////////////
// Task 1
/////////////////////////////////////////////////////////////////
// add the errors
for(unsigned int i=0; i<taskError_1.getRows(); i++)
{
//ROS_INFO_STREAM("=> Adding taskError_1 at position " << plotMsg.data.size());
plotMsg.data.push_back(taskError_1[i]);
}
// add qdot - total joint velocity
for(unsigned int i=0; i<qdot_1.getRows(); i++)
{
//ROS_INFO_STREAM("=> Adding qdot of task 1 at position " << plotMsg.data.size());
plotMsg.data.push_back(qdot_1[i]);
}
/////////////////////////////////////////////////////////////////
// Task 2
/////////////////////////////////////////////////////////////////
// add the errors
for(unsigned int i=0; i<taskError_2.getRows(); i++)
{
//ROS_INFO_STREAM("=> Adding taskError_2 at position " << plotMsg.data.size());
plotMsg.data.push_back(taskError_2[i]);
}
// add qdot - total joint velocity
for(unsigned int i=0; i<qdot_2.getRows(); i++)
{
//ROS_INFO_STREAM("=> Adding qdot of task 2 at position " << plotMsg.data.size());
plotMsg.data.push_back(qdot_2[i]);
}
/////////////////////////////////////////////////////////////////
// Task 1: q1dot and q2dot
/////////////////////////////////////////////////////////////////
// add qdot - joint velocities of primary task of 1
for(unsigned int i=0; i<q1dot_1.getRows(); i++)
{
//ROS_INFO_STREAM("=> Adding q1dot of task 1 at position " << plotMsg.data.size());
plotMsg.data.push_back(q1dot_1[i]);
}
// add qdot - joint velocities of secondary task of 1
for(unsigned int i=0; i<q2dot_1.getRows(); i++)
{
//ROS_INFO_STREAM("=> Adding q2dot of task 1 at position " << plotMsg.data.size());
plotMsg.data.push_back(q2dot_1[i]);
}
/////////////////////////////////////////////////////////////////
// Task 2: q1dot and q2dot
/////////////////////////////////////////////////////////////////
// add qdot - joint velocities of primary task of 2
for(unsigned int i=0; i<q1dot_2.getRows(); i++)
{
//ROS_INFO_STREAM("=> Adding q1dot of task 2 at position " << plotMsg.data.size());
plotMsg.data.push_back(q1dot_2[i]);
}
// add qdot - joint velocities of secondary task of 2
for(unsigned int i=0; i<q2dot_2.getRows(); i++)
{
//ROS_INFO_STREAM("=> Adding q2dot of task 2 at position " << plotMsg.data.size());
plotMsg.data.push_back(q2dot_2[i]);
}
//add midpoint normalized coordinates
plotMsg.data.push_back(x_midPoint);
plotMsg.data.push_back(y_midPoint);
}
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;
}
// ===================================================================
void vpRobust::MEstimator(const vpRobustEstimatorType method,
const vpColVector &residues,
vpColVector &weights)
{
double med=0; // median
double normmedian=0; // Normalized median
double sigma=0;// Standard Deviation
// resize vector only if the size of residue vector has changed
unsigned int n_data = residues.getRows();
resize(n_data);
sorted_residues = residues;
unsigned int ind_med = (unsigned int)(ceil(n_data/2.0))-1;
// Calculate median
med = select(sorted_residues, 0, (int)n_data-1, (int)ind_med/*(int)n_data/2*/);
//residualMedian = med ;
// Normalize residues
for(unsigned int i=0; i<n_data; i++)
{
normres[i] = (fabs(residues[i]- med));
sorted_normres[i] = (fabs(sorted_residues[i]- med));
}
// Calculate MAD
normmedian = select(sorted_normres, 0, (int)n_data-1, (int)ind_med/*(int)n_data/2*/);
//normalizedResidualMedian = normmedian ;
// 1.48 keeps scale estimate consistent for a normal probability dist.
sigma = 1.4826*normmedian; // median Absolute Deviation
// Set a minimum threshold for sigma
// (when sigma reaches the level of noise in the image)
if(sigma < NoiseThreshold)
{
sigma= NoiseThreshold;
}
switch (method)
{
case TUKEY :
{
psiTukey(sigma, normres,weights);
vpCDEBUG(2) << "Tukey's function computed" << std::endl;
break ;
}
case CAUCHY :
{
psiCauchy(sigma, normres,weights);
break ;
}
/* case MCLURE :
{
psiMcLure(sigma, normres);
break ;
}*/
case HUBER :
{
psiHuber(sigma, normres,weights);
break ;
}
}
}
