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