void computeCurrentPositionUsingCoders(PidMotion* pidMotion) {
PidComputationValues* computationValues = &(pidMotion->computationValues);
PidCurrentValues* thetaCurrentValues = &(computationValues->currentValues[THETA]);
PidCurrentValues* alphaCurrentValues = &(computationValues->currentValues[ALPHA]);
// 2 dependant Wheels (direction + angle)
float value0 = (float)getCoderValue(CODER_LEFT);
float value1 = (float)getCoderValue(CODER_RIGHT);
// Compute real position of wheel
thetaCurrentValues->position = computeTheta(value0, value1);
alphaCurrentValues->position = computeAlpha(value0, value1);
}
/*!
Compute the \f$ theta \f$ angle for each vpMeSite.
\note The \f$ theta \f$ angle is useful during the tracking part.
*/
void
vpMeEllipse::updateTheta()
{
vpMeSite p_me;
double theta;
for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
p_me = *it;
vpImagePoint iP;
iP.set_i(p_me.ifloat);
iP.set_j(p_me.jfloat);
computeTheta(theta, K, iP) ;
p_me.alpha = theta ;
*it = p_me;
}
}
/**
* Go to a position
*/
void gotoSimplePosition(PidMotion* pidMotion, float leftMM, float rightMM, float a, float speed, OutputStream* notificationOutputStream) {
// determine the type of motion
enum MotionParameterType motionParameterType = getMotionParameterType(leftMM, rightMM);
// Alpha / Theta
float thetaNextPosition = computeTheta(leftMM, rightMM);
float alphaNextPosition = computeAlpha(leftMM, rightMM);
PidMotionDefinition* motionDefinition = pidMotionGetNextToWritePidMotionDefinition(pidMotion);
motionDefinition->motionType = MOTION_TYPE_NORMAL;
motionDefinition->notificationOutputStream = notificationOutputStream;
setNextPosition(motionDefinition, THETA, motionParameterType, thetaNextPosition, a, speed);
setNextPosition(motionDefinition, ALPHA, motionParameterType, alphaNextPosition, a, speed);
// All main information are defined
motionDefinition->state = PID_MOTION_DEFINITION_STATE_SET;
}
void RcompoundpoissonPape_useGamma(double *gamma,
double *hitdistribution, int *nseq, int *lseq, int * mhit, int *mclump) {
int seqlen, i;
int maxclumpsize, maxhits;
double lambda;
double *theta, extention[3];
if (!Rpwm||!Rcpwm||!Rstation||!Rtrans) {
error("load forground and background properly");
return;
}
if (!gamma||!hitdistribution||!nseq||!lseq||!mhit||!mclump) {
error("parameters are null");
return;
}
if (Rgran==0.0 || Rsiglevel==0.0) {
error("call mdist.option first");
return;
}
seqlen=0;
for (i=0; i<*nseq; i++) {
seqlen+=lseq[i]-Rpwm->nrow+1;
}
maxclumpsize=(double)mclump[0];
maxhits=(double)mhit[0];
memset(extention, 0, 3*sizeof(double));
gamma[Rpwm->nrow]=(gamma[Rpwm->nrow]+EPSILON)/(1+2*EPSILON);
computeExtentionFactorsPape(extention, gamma, Rpwm->nrow);
theta=initTheta(maxclumpsize);
if (theta==NULL) {
error("Memory-allocation in initTheta failed");
}
computeInitialClump(theta, gamma,Rpwm->nrow);
computeTheta(maxclumpsize, theta, extention, Rpwm->nrow);
lambda=computePoissonParameter(seqlen, Rpwm->nrow,
maxclumpsize, gamma[0],theta);
computeCompoundPoissonDistributionKemp(lambda, maxhits,
maxclumpsize, theta, hitdistribution);
deleteTheta(theta);
}
/*!
Construct a list of vpMeSite moving edges at a particular sampling
step between the two extremities. The two extremities are defined by
the points with the smallest and the biggest \f$ alpha \f$ angle.
\param I : Image in which the ellipse appears.
\exception vpTrackingException::initializationError : Moving edges not initialized.
*/
void
vpMeEllipse::sample(const vpImage<unsigned char> & I)
{
vpCDEBUG(1) <<"begin vpMeEllipse::sample() : "<<std::endl ;
if (!me) {
vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
throw(vpTrackingException(vpTrackingException::initializationError,
"Moving edges not initialized")) ;
}
int height = (int)I.getHeight() ;
int width = (int)I.getWidth() ;
double n_sample;
//if (me->getSampleStep()==0)
if (std::fabs(me->getSampleStep()) <= std::numeric_limits<double>::epsilon())
{
std::cout << "In vpMeEllipse::sample: " ;
std::cout << "function called with sample step = 0" ;
//return fatalError ;
}
double j, i;//, j11, i11;
vpImagePoint iP11;
j = i = 0.0 ;
double incr = vpMath::rad(me->getSampleStep()) ; // angle increment en degree
vpColor col = vpColor::red ;
getParameters() ;
// Delete old list
list.clear();
angle.clear();
// sample positions
double k = alpha1 ;
while (k<alpha2)
{
// j = a *cos(k) ; // equation of an ellipse
// i = b *sin(k) ; // equation of an ellipse
j = a *sin(k) ; // equation of an ellipse
i = b *cos(k) ; // equation of an ellipse
// (i,j) are the coordinates on the origin centered ellipse ;
// a rotation by "e" and a translation by (xci,jc) are done
// to get the coordinates of the point on the shifted ellipse
// iP11.set_j( iPc.get_j() + ce *j - se *i );
// iP11.set_i( iPc.get_i() -( se *j + ce *i) );
iP11.set_j( iPc.get_j() + ce *j + se *i );
iP11.set_i( iPc.get_i() - se *j + ce *i );
vpDisplay::displayCross(I, iP11, 5, col) ;
double theta ;
computeTheta(theta, K, iP11) ;
// If point is in the image, add to the sample list
if(!outOfImage(vpMath::round(iP11.get_i()), vpMath::round(iP11.get_j()), 0, height, width))
{
vpMeSite pix ;
pix.init((int)iP11.get_i(), (int)iP11.get_j(), theta) ;
pix.setDisplay(selectDisplay) ;
pix.setState(vpMeSite::NO_SUPPRESSION);
if(vpDEBUG_ENABLE(3))
{
vpDisplay::displayCross(I,iP11, 5, vpColor::blue);
}
list.push_back(pix);
angle.push_back(k);
}
k += incr ;
}
vpMeTracker::initTracking(I) ;
n_sample = (unsigned int)list.size() ;
vpCDEBUG(1) << "end vpMeEllipse::sample() : " ;
vpCDEBUG(1) << n_sample << " point inserted in the list " << std::endl ;
}
void Rcompoundpoisson_useBeta(double *alpha, double *beta,
double *beta3p, double *beta5p,
double *hitdistribution, int *nseq,
int *lseq, int * mhit, int *mclump, int *sstrand) {
int seqlen, i;
int maxclumpsize, maxhits, singlestranded;
double lambda;
double *theta, extention[3];
double *delta, *deltap;
if (!Rpwm||!Rcpwm||!Rstation||!Rtrans) {
error("load forground and background properly");
return;
}
if (!alpha||!beta||!beta3p||!beta5p||
!hitdistribution||!nseq||!lseq||!mhit||!mclump) {
error("parameters are null");
return;
}
if (Rgran==0.0 || Rsiglevel==0.0) {
error("call mdist.option first");
return;
}
//compute the total length of the sequence
seqlen=0;
for (i=0; i<*nseq; i++) {
seqlen+=lseq[i]-Rpwm->nrow+1;
}
//init the maximal clump size and the max number of hits
maxclumpsize=(double)mclump[0];
maxhits=(double)mhit[0];
singlestranded=*sstrand;
delta=Calloc(Rpwm->nrow,double);
deltap=Calloc(Rpwm->nrow,double);
if (delta==NULL||deltap==NULL) {
error("Memory-allocation in Rcompoundpoisson_useBeta failed");
}
// initialize the extention factors
memset(extention, 0, 3*sizeof(double));
if (singlestranded==1) {
computeDeltasSingleStranded(delta, beta, Rpwm->nrow);
computeExtentionFactorsKoppSingleStranded(extention, beta, Rpwm->nrow);
theta=initThetaSingleStranded(maxclumpsize);
if (theta==NULL) {
error("Memory-allocation in initTheta failed");
}
computeInitialClumpKoppSingleStranded(theta, delta, Rpwm->nrow);
computeThetaSingleStranded(maxclumpsize, theta, extention, Rpwm->nrow);
lambda=computePoissonParameterSingleStranded(seqlen, Rpwm->nrow,
maxclumpsize, alpha[0],theta);
computeCompoundPoissonDistributionKempSingleStranded(lambda, maxhits,
maxclumpsize, theta, hitdistribution);
} else {
beta3p[0]=(beta3p[0]+EPSILON)/(1+2*EPSILON);
computeDeltas(delta, deltap, beta, beta3p,beta5p,Rpwm->nrow);
computeExtentionFactorsKopp(extention, delta, deltap, beta,
beta3p, beta5p, Rpwm->nrow);
theta=initTheta(maxclumpsize);
if (theta==NULL) {
error("Memory-allocation in initTheta failed");
}
computeInitialClumpKopp(theta, beta3p,delta, deltap, Rpwm->nrow);
computeTheta(maxclumpsize, theta, extention, Rpwm->nrow);
lambda=computePoissonParameter(seqlen, Rpwm->nrow, maxclumpsize,
alpha[0],theta);
computeCompoundPoissonDistributionKemp(lambda, maxhits, maxclumpsize,
theta, hitdistribution);
}
deleteTheta(theta);
Free(delta);
Free(deltap);
}
