handPoseEstimationModule Class Reference

Class handPoseEstimationModule. More...

#include <handPoseEstimationModule.h>

Inheritance diagram for handPoseEstimationModule:

Public Member Functions
virtual bool	configure (yarp::os::ResourceFinder &rf)
virtual bool	interruptModule ()
virtual bool	close ()
virtual bool	updateModule ()
virtual double	getPeriod ()
bool	attach (yarp::os::RpcServer &source)
bool	start ()
	Start (re-start) the hand pose estimation. More...
bool	stop ()
	Stop the hand pose estimation. More...
bool	pause ()
	Pause the hand pose estimation. More...
bool	resume ()
	Resume the hand pose estimation. More...
yarp::os::Bottle	lastOffsets ()
	Ask for the last estimated angular Offsets on the arm (7DoF) More...
bool	quit ()
	Quit the module. More...
virtual bool	read (yarp::os::ConnectionReader &connection) YARP_OVERRIDE
virtual std::vector< std::string >	help (const std::string &functionName="--all")

Protected Member Functions
cv::Mat	processImages (cv::Mat inputImage)
	The function applies a canny edge detector and a distance transform. More...
bool	initializeSMCVariables ()
	Initialize the variables structs needed for the SMC to run. More...
bool	initSMC ()
	Initialize the variables values for the SMC. More...
bool	runSMCIteration ()
	Run one iteration of the Sequential Monte Carlo Parameters estimation. More...
void	mergeAndFlipImages ()
	merge the left and right images. More...
void	kernelDensityEstimation ()
	Perform the Kernel Density estimation with a multivariate gaussian kernel.
bool	readArmJoints ()
	Read the arm joints. More...
bool	readHeadJoints ()
	Read the head joints. More...
bool	systematic_resampling (CvMat *oldParticlesState, CvMat *oldParticlesWeights, CvMat *newParticlesState, CvMat *cumWeight, float sum2)
	perform the systematic resampling step of the SMC algorithm More...

Detailed Description

Class handPoseEstimationModule.

Definition at line 53 of file handPoseEstimationModule.h.

Member Function Documentation

bool handPoseEstimationModule::initializeSMCVariables ( )

protected

Initialize the variables structs needed for the SMC to run.

Returns

true/false on success/failure

Definition at line 92 of file handPoseEstimationModule.cpp.

{
    nParticles=200; // HardCoded since the Internal Model should be recompiled to support a different amount of particles

    //allocate memory for the particles;
    particles=cvCreateMat(8,nParticles,CV_32FC1); // 0-6 Betas 7- likelihood
    //fill the memory with zeros
    cvSetZero(particles);

    //define ways of accessing the particles:
    // Beta1
    particles1 = cvCreateMatHeader( 1,nParticles, CV_32FC1);
    cvInitMatHeader( particles1, 1, nParticles, CV_32FC1, particles->data.ptr, particles->step );
    // Beta2
    particles2 = cvCreateMatHeader( 1,nParticles, CV_32FC1);
    cvInitMatHeader( particles2, 1, nParticles, CV_32FC1, particles->data.ptr + particles->step*1, particles->step );
    // Beta3
    particles3 = cvCreateMatHeader( 1,nParticles, CV_32FC1);
    cvInitMatHeader( particles3, 1, nParticles, CV_32FC1, particles->data.ptr + particles->step*2, particles->step );
    // Beta4
    particles4 = cvCreateMatHeader( 1,nParticles, CV_32FC1);
    cvInitMatHeader( particles4, 1, nParticles, CV_32FC1, particles->data.ptr + particles->step*3, particles->step );
    // Beta5
    particles5 = cvCreateMatHeader( 1,nParticles, CV_32FC1);
    cvInitMatHeader( particles5, 1, nParticles, CV_32FC1, particles->data.ptr + particles->step*4, particles->step );
    // Beta6
    particles6 = cvCreateMatHeader( 1,nParticles, CV_32FC1);
    cvInitMatHeader( particles6, 1, nParticles, CV_32FC1, particles->data.ptr + particles->step*5, particles->step );
    // Beta7
    particles7 = cvCreateMatHeader( 1,nParticles, CV_32FC1);
    cvInitMatHeader( particles7, 1, nParticles, CV_32FC1, particles->data.ptr + particles->step*6, particles->step );
    // likelihood
    particles8 = cvCreateMatHeader( 1,nParticles, CV_32FC1);
    cvInitMatHeader( particles8, 1, nParticles, CV_32FC1, particles->data.ptr + particles->step*7, particles->step );

    //theta1-theta7
    particles1to7 = cvCreateMatHeader( 7,nParticles, CV_32FC1);
    cvInitMatHeader( particles1to7, 7, nParticles, CV_32FC1, particles->data.ptr, particles->step );


    newParticles=cvCreateMat(8,nParticles,CV_32FC1);
    newParticles1to7 = cvCreateMatHeader( 7,nParticles, CV_32FC1);
    cvInitMatHeader( newParticles1to7, 7, nParticles, CV_32FC1, newParticles->data.ptr, newParticles->step );
    // Resampling stuff
    cumWeight =cvCreateMat(1,nParticles+1,CV_32FC1);
    noise=cvCreateMat(7,nParticles,CV_32FC1);
    cvSetZero(noise);   
    return true;
}

bool handPoseEstimationModule::initSMC ( )

protected

Initialize the variables values for the SMC.

Returns

true/false on success/failure

Definition at line 142 of file handPoseEstimationModule.cpp.

{
    // Generate random particles

    srand((unsigned int)time(0)); //make sure random numbers are really random.
    rngState = cvRNG(rand());

    cvRandArr( &rngState, particles1, CV_RAND_NORMAL, cvScalar(initialMean), cvScalar(initialStdDev));
    //initialize Theta2
    cvRandArr( &rngState, particles2, CV_RAND_NORMAL, cvScalar(initialMean), cvScalar(initialStdDev));
    //initialize Theta3
    cvRandArr( &rngState, particles3, CV_RAND_NORMAL, cvScalar(initialMean), cvScalar(initialStdDev));
    //initialize Theta4
    cvRandArr( &rngState, particles4, CV_RAND_NORMAL, cvScalar(initialMean), cvScalar(initialStdDev));
    //initialize Theta5
    cvRandArr( &rngState, particles5, CV_RAND_NORMAL, cvScalar(initialMean), cvScalar(initialStdDev));
    //initialize Theta6
    cvRandArr( &rngState, particles6, CV_RAND_NORMAL, cvScalar(initialMean), cvScalar(initialStdDev));
    //initialize Theta7
    cvRandArr( &rngState, particles7, CV_RAND_NORMAL, cvScalar(initialMean), cvScalar(initialStdDev));

    // Artificial Noise Initialization
    artifNoiseStdDev = initialArtificialNoiseStdDev;
    // Setting first particle as Zero offset    
    for (unsigned int joint=0;joint<8;joint++) 
    {
        cvmSet(particles,joint,0,0.0);
    }
    return true;
}

yarp::os::Bottle handPoseEstimationModule::lastOffsets ( )

virtual

Ask for the last estimated angular Offsets on the arm (7DoF)

Returns

Bottle with last angular Offsets

Reimplemented from handPoseEstimation_IDL.

Definition at line 675 of file handPoseEstimationModule.cpp.

{
    yInfo("lastOffsets command received");
    Bottle reply;
    reply = lastBestOffset;
    return reply;
}

void handPoseEstimationModule::mergeAndFlipImages ( )

protected

merge the left and right images.

i.e., concatenate horizontally.

Definition at line 402 of file handPoseEstimationModule.cpp.

{
    cv::hconcat(imageProcL, imageProcR,concatenatedImage);
    cv::flip(concatenatedImage,concatenatedImage,0);
    cvtColor(concatenatedImage,concatenatedImage,CV_GRAY2BGR);
}

bool handPoseEstimationModule::pause ( )

virtual

Pause the hand pose estimation.

The filter stop the generation of particles and waits for a resume command

Returns

true/false on success/failure

Reimplemented from handPoseEstimation_IDL.

Definition at line 661 of file handPoseEstimationModule.cpp.

{
    paused = true;
    yInfo("pause command received");
    return true;
}

Mat handPoseEstimationModule::processImages ( cv::Mat  inputImage )

protected

The function applies a canny edge detector and a distance transform.

Returns

the processed image

Definition at line 552 of file handPoseEstimationModule.cpp.

{
    Mat edges,dtImage;
    Mat dtImage2,dtImage2_8;
    cvtColor(inputImage,edges,CV_RGB2GRAY);

    // Image
    blur( edges, edges, Size(3,3) );
    Canny(edges,edges,65,3*65,3);
    threshold(edges,edges,100,255,THRESH_BINARY_INV);
    distanceTransform(edges,dtImage,CV_DIST_L2,CV_DIST_MASK_5);
    cvtColor(dtImage,dtImage2,CV_GRAY2BGR);
    dtImage.convertTo(dtImage2_8,CV_8UC3);
    return dtImage2_8;
}

bool handPoseEstimationModule::quit ( )

virtual

Quit the module.

Returns

true/false on success/failure

Reimplemented from handPoseEstimation_IDL.

Definition at line 683 of file handPoseEstimationModule.cpp.

{
    yInfo("quit command received");
    closing = true;
    return true;
}

bool handPoseEstimationModule::readArmJoints ( )

protected

Read the arm joints.

Returns

true/false on success/failure

Definition at line 568 of file handPoseEstimationModule.cpp.

{
    // read Arm joint angles
    Bottle *receive = armPort.read();

    string s1 = receive->toString();
    char *str = new char[s1.size()+1];
    strcpy(str, s1.c_str());
    char * pch;
    pch = strtok ( (char*) str," ");
    int i=0;
    while (pch != NULL) 
    {
        encodersArm[i]=atof(pch);
        pch = strtok (NULL, " ");
        i++;
    }
}

bool handPoseEstimationModule::readHeadJoints ( )

protected

Read the head joints.

Returns

true/false on success/failure

Definition at line 587 of file handPoseEstimationModule.cpp.

{
    //Read Head Joint Angles    
    Bottle *receive = headPort.read();
    string s2 = receive->toString();
    char *str = new char[s2.size()+1];
    strcpy(str, s2.c_str());
    char * pch;
    pch = strtok ( (char*) str," ");
    int i=0;
    while (pch != NULL) 
    {
        encodersHead[i]=atof(pch);
        pch = strtok (NULL, " ");
        i++;
    }
}

bool handPoseEstimationModule::resume ( )

virtual

Resume the hand pose estimation.

The filter resumes the generation of particles after a pause command

Returns

true/false on success/failure

Reimplemented from handPoseEstimation_IDL.

Definition at line 668 of file handPoseEstimationModule.cpp.

{
    paused = false;
    yInfo("resume command received");
    return true;
}

bool handPoseEstimationModule::runSMCIteration ( )

protected

Run one iteration of the Sequential Monte Carlo Parameters estimation.

Returns

true/false on success/failure

Definition at line 173 of file handPoseEstimationModule.cpp.

{
    // tmp variables
    double maxLikelihood=0.0;
    double sumLikelihood=0.0;
    double likelihood=0.0;

    // prepare containers to send data through YARP
    ImageOf<PixelBgr> &yarpReturnImage = LRimageOutputPort.prepare();
    Bottle &outputParticles = particlesOutPort.prepare();
    Bottle &outputHead= headOutPort.prepare();
    // prepare concatenated Image
    concatenatedImage.convertTo(concatenatedImage,CV_8UC3);
    yarpReturnImage.resize(concatenatedImage.cols,concatenatedImage.rows);
    concatenatedImage.copyTo( cvarrToMat( static_cast<IplImage*> ( yarpReturnImage.getIplImage() ) ) );
    // Fill Bottle with offsets+encoders to generate
    for(int index=0;index < nParticles;index++) 
    {
        // Arm + offsets
        for (unsigned int joint=0;joint<7;joint++) 
        {
            outputParticles.addDouble(encodersArm[joint]+cvmGet(particles,joint,index));
            if(index==0)
            {
                yInfo() << cvmGet(particles,joint,index);
            }
        }
        // Fingers
        for(unsigned int joint=7;joint<16;joint++) 
        {
            outputParticles.addDouble(encodersArm[joint]);
        }
    }
    outputParticles.addInt(nParticles); // n_particles

    // Fill Bottle with head encoders
    for(int k=0;k<6;k++)
    {
        outputHead.addDouble(encodersHead[k]);
    }

    //Send data to InternalModel
    particlesOutPort.write();
    headOutPort.write();  
    LRimageOutputPort.write();

    // Waitint for results
    yInfo("Waiting for Hypotheses generation and evaluation");
    Bottle *receivedLikelihood = likelihoodPort.read();
    yInfo(" DONE");
    
    // Save the likelihood of each particle
    for(int index=0;index<nParticles;index++) 
    {
        likelihood = receivedLikelihood->pop().asDouble();
        cvmSet(particles,7,index,likelihood);
        sumLikelihood+=likelihood;
        if(likelihood>maxLikelihood) 
        {
            maxLikelihood=likelihood;
        }
    }
    cvmSet(particles,7,0,0.0);
    kernelDensityEstimation();


    yInfo("Best likelihood: %f", (float) cvmGet(particles,7,maxWeightIndex));

    if(iteration>minimumIteration) // START sending offsets
    {
        // Send Best Particle
        Bottle &bestOffset = offsetsPort.prepare();
        bestOffset.clear();
        for(int i=0;i<7;i++) 
        {
            bestOffset.addDouble(cvmGet(particles,i,maxWeightIndex));
        }
        lastBestOffset.clear();
        lastBestOffset = bestOffset;
        bestOffset.addDouble(iteration);
        offsetsPort.write();
    }
    // Resampling or not Resampling. That's the Question

    if(maxLikelihood>minimumLikelihood) 
    {
        systematic_resampling(particles1to7,particles8,newParticles,cumWeight, sumLikelihood);
        artifNoiseStdDev=artifNoiseStdDev*decreasedMultiplier;
    }
    else  //I can't apply a resampling with all weights equal to 0! 
    {
        cvCopy(particles,newParticles);
        artifNoiseStdDev=artifNoiseStdDev*increasedMultiplier;
    }
    if(artifNoiseStdDev > upperBoundNoise) 
    {
       artifNoiseStdDev = upperBoundNoise;
    }
    // Apply artificial Dynamics

    CvMat* A = cvCreateMat(8,8,CV_32FC1);
    cvSetIdentity(A); //
    cvMatMul(A,newParticles,particles);

    float mean = 0;
    CvMat* noiseSingle;
    noiseSingle = cvCreateMat(1,nParticles,CV_32FC1);
    cvSetZero(noiseSingle);
    if(artifNoiseStdDev < lowerBoundNoise)  // lowerbound of artificial noise
    {
        artifNoiseStdDev = lowerBoundNoise;
    }
    cvRandArr( &rngState, noise, CV_RAND_NORMAL, cvScalar(mean), cvScalar(artifNoiseStdDev));
    cvAdd(particles1to7,noise,particles1to7);
    yInfo() << "ArtNoise: " << artifNoiseStdDev;
    // Setting first particle as Zero offset
    for (unsigned int joint=0;joint<8;joint++) 
    {
        cvmSet(particles,joint,0,0.0);
    }
    
    return true;
}

bool handPoseEstimationModule::start ( )

virtual

Start (re-start) the hand pose estimation.

The filter generates particles and update the particle distribution accordingly

Returns

true/false on success/failure

Reimplemented from handPoseEstimation_IDL.

Definition at line 644 of file handPoseEstimationModule.cpp.

{
    iteration=0;
    initSMC(); // Generated new particles
    stopped = false;
    yInfo("start command received");
    return true;
}

bool handPoseEstimationModule::stop ( )

virtual

Stop the hand pose estimation.

The filter stop the generation of particles and waits for a start command

Returns

true/false on success/failure

Reimplemented from handPoseEstimation_IDL.

Definition at line 653 of file handPoseEstimationModule.cpp.

{
    stopped = true;
    yInfo("stop command received");
    return true;

}

bool handPoseEstimationModule::systematic_resampling ( CvMat *  oldParticlesState, CvMat *  oldParticlesWeights, CvMat *  newParticlesState, CvMat *  cumWeight, float  sum2  )

protected

perform the systematic resampling step of the SMC algorithm

Returns

true/false on success/failure

Definition at line 329 of file handPoseEstimationModule.cpp.

{
    double u; //random number [0,1)
    double sum;
    int c1;
    int rIndex;  //index of the randomized array
    int cIndex;  //index of the cumulative weight array. cIndex -1 indicates which particle we think of resampling.
    int npIndex; //%new particle index, tells me how many particles have been created so far.

    //%N is the number of particles.
    //[lines, N] = size(oldParticlesWeight);
    //in CPP, _nParticles is the number of particles.


    //%NORMALIZE THE WEIGHTS, so that sum(oldParticles)=1.
    //oldParticlesWeight = oldParticlesWeight / sum(oldParticlesWeight);
    sum=0;

    for(c1=0;c1<nParticles;c1++) 
    {
        ((float*)(oldParticlesWeights->data.ptr + oldParticlesWeights->step*0))[c1] = (((float*)(oldParticlesWeights->data.ptr + oldParticlesWeights->step*0))[c1])/(float)sum2;
    }
    for(c1=0;c1<nParticles;c1++) 
    {
        sum+=((float*)(oldParticlesWeights->data.ptr + oldParticlesWeights->step*0))[c1];
    }

    //%GENERATE N RANDOM VALUES
    //u = rand(1)/N; %random value [0,1/N)
    u=1/(double)nParticles*((double)rand()/(double)RAND_MAX);

    //%the randomized values are going to be u, u+1/N, u+2/N, etc.
    //%instead of accessing this vector, the elements are computed on the fly:
    //%randomVector(a)= (a-1)/N+u.

    ((float*)(cumWeight->data.ptr))[0]=0.0;
    for(c1=0;c1<nParticles;c1++) 
    {
        ((float*)(cumWeight->data.ptr))[c1+1]=((float*)(cumWeight->data.ptr))[c1]+((float*)(oldParticlesWeights->data.ptr + oldParticlesWeights->step*0))[c1];
    }

    if(((float*)(cumWeight->data.ptr))[nParticles]!=1) 
    {
        ((float*)(cumWeight->data.ptr))[nParticles]=1;
    }

    //%PERFORM THE ACTUAL RESAMPLING
    rIndex=0; //index of the randomized array
    cIndex=1; //index of the cumulative weight array. cIndex -1 indicates which particle we think of resampling.
    npIndex=0; //new particle index, tells me how many particles have been created so far.

    while(npIndex < nParticles) 
    {
        if(((float*)(cumWeight->data.ptr))[cIndex]>=(double)rIndex/(double)nParticles+u) {
            ((float*)(newParticlesState->data.ptr + newParticlesState->step*0))[npIndex]=((float*)(oldParticlesState->data.ptr + oldParticlesState->step*0))[cIndex-1];
            ((float*)(newParticlesState->data.ptr + newParticlesState->step*1))[npIndex]=((float*)(oldParticlesState->data.ptr + oldParticlesState->step*1))[cIndex-1];
            ((float*)(newParticlesState->data.ptr + newParticlesState->step*2))[npIndex]=((float*)(oldParticlesState->data.ptr + oldParticlesState->step*2))[cIndex-1];
            ((float*)(newParticlesState->data.ptr + newParticlesState->step*3))[npIndex]=((float*)(oldParticlesState->data.ptr + oldParticlesState->step*3))[cIndex-1];
            ((float*)(newParticlesState->data.ptr + newParticlesState->step*4))[npIndex]=((float*)(oldParticlesState->data.ptr + oldParticlesState->step*4))[cIndex-1];
            ((float*)(newParticlesState->data.ptr + newParticlesState->step*5))[npIndex]=((float*)(oldParticlesState->data.ptr + oldParticlesState->step*5))[cIndex-1];
            ((float*)(newParticlesState->data.ptr + newParticlesState->step*6))[npIndex]=((float*)(oldParticlesState->data.ptr + oldParticlesState->step*6))[cIndex-1];
            ((float*)(newParticlesState->data.ptr + newParticlesState->step*7))[npIndex]=0; //initializing weight
            rIndex=rIndex+1;
            npIndex=npIndex+1;
        }
        else 
        {
            cIndex=cIndex+1;
        }
    }
    return false;
}

---

The documentation for this class was generated from the following files:

• /home/pvicente/software/robot-code/git-vicentepedro/Online-Body-Schema-Adaptation/modules/handPoseEstimation/include/handPoseEstimationModule.h
• /home/pvicente/software/robot-code/git-vicentepedro/Online-Body-Schema-Adaptation/modules/handPoseEstimation/src/handPoseEstimationModule.cpp