C++ Vector::push_back方法代码示例

bool YarpJointDev::getDOF ( yarp::sig::Vector& curDof ) {

    curDof.clear();

    for ( unsigned i = 0; i < _chain->GetNumberOfJoints(); ++i )
        curDof.push_back ( 1 );

    return true;

}

bool skinManager::bottleToVector(const yarp::os::Bottle& b, yarp::sig::Vector& v){
    for(int i=0; i<b.size(); i++)
        if(b.get(i).isDouble() || b.get(i).isInt())
            v.push_back(b.get(i).asDouble());
        else
            return false;
    return true;
}

void HOGThread::computeHOG(IplImage* img, yarp::sig::Vector &currHist, double Th, int aperture, bool show)
{
    IplImage * contrast= cvCreateImage(cvGetSize(img), IPL_DEPTH_32F, 1);
    IplImage * arctan= cvCreateImage(cvGetSize(img), IPL_DEPTH_32F, 1);
    IplImage * mask= cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 1);
    IplImage * edges= cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 1);

    double max =0;
    double min =0;
    IplImage *img_t = cvCreateImage(cvGetSize(img),img->depth,1);
    IplImage *img_f = cvCreateImage(cvGetSize(img),32,1);

    IplImage * derivativeX= cvCreateImage(cvGetSize(img),32,1);
    IplImage * derivativeY= cvCreateImage(cvGetSize(img),32,1);

    if(img->nChannels>1)
        cvCvtColor(img,img_t,CV_RGB2GRAY);
    else
        cvCopy(img,img_t,0);

    cvMinMaxLoc(img_t,&min,&max,NULL,NULL,NULL);
    cvConvertScale(img_t,img_f,1.0/max,0);

    cvSobel(img_f,derivativeX,1,0,aperture);
    cvSobel(img_f,derivativeY,0,1,aperture);

    cvZero(arctan);
    cvZero(contrast);
    cvCartToPolar(derivativeX, derivativeY, contrast, arctan, 0);

    cvThreshold(contrast,contrast,Th,1,CV_THRESH_BINARY);
    
    cvConvertScale(contrast,mask,255,0);

    int n_bins[]={nbins};
    float range[]={0,2*CV_PI };
    float * ranges[] = {range};
    CvHistogram* histTemp=cvCreateHist(1, n_bins, CV_HIST_ARRAY, ranges,1);
    cvCalcHist(&arctan, histTemp, 0, mask);
    cvNormalizeHist(histTemp,1);

    for(int i=0; i<nbins; i++)
    {   
        float bin_value=cvGetReal1D(histTemp->bins,i);
        currHist.push_back(bin_value);
    }

    cvReleaseImage(&img_f);
    cvReleaseImage(&derivativeX);
    cvReleaseImage(&derivativeY);
    cvReleaseImage(&img_t);
    cvReleaseImage(&contrast);
    cvReleaseImage(&arctan);
    cvReleaseImage(&mask);
    cvReleaseImage(&edges);
    cvReleaseHist(&histTemp);
}

bool JoypadControlClient::getRawTouch(unsigned int touch_id, yarp::sig::Vector& value)
{
    value.clear();
    if(m_rpc_only)
    {
        Bottle cmd, response;

        cmd.addVocab(VOCAB_IJOYPADCTRL);
        cmd.addVocab(VOCAB_GET);
        cmd.addVocab(VOCAB_TOUCH);
        cmd.addVocab(VOCAB_VALUE);
        cmd.addInt32(touch_id);
        m_rpcPort.write(cmd, response);
        if(response.get(0).asVocab() == VOCAB_OK && response .get(1).isFloat64() && response.get(2).isFloat64())
        {
            value.push_back(response.get(1).asFloat64());
            value.push_back(response.get(2).asFloat64());
            return true;
        }
        else
        {
            return false;
        }
    }
    else
    {
        LockGuard l(m_touchPort.mutex);
        if(touch_id < m_touchPort.storage.size()/2)
        {
            value.push_back(m_touchPort.storage[touch_id * 2]);
            value.push_back(m_touchPort.storage[touch_id * 2 + 1]);
            return true;
        }
        else
        {
            return false;
        }
    }
}

void YarpJointDev::PosEulerSingle_To_PosAxisAngle ( const std::vector< float >& pos,
        yarp::sig::Vector& xd,
        yarp::sig::Vector& od ) {

    assert ( pos.size() == 6 );

    xd.clear();
    od.clear();

    xd.push_back ( pos[0] );
    xd.push_back ( pos[1] );
    xd.push_back ( pos[2] );

    float xa, ya, za, theta;

    Euler_To_AxisAngle ( pos[3], pos[4], pos[5], xa, ya, za, theta );

    od.push_back ( xa );
    od.push_back ( ya );
    od.push_back ( za );
    od.push_back ( theta );

}

int laserHokuyo::readData(const Laser_mode_type laser_mode, const char* text_data, const int text_data_len, int current_line, yarp::sig::Vector& data)
{ 
    static char data_block [4000];

    if (text_data_len==0)
    {
        return  HOKUYO_STATUS_ERROR_NOTHING_RECEIVED;
    }

    long int timestamp = 0 ;

    // termination complete check
    if (text_data_len == 1 &&
        text_data[0] == '\n')
        {
            //Decode the data
            int len = strlen(data_block);
            for (int value_counter =0; value_counter < len; value_counter+=3)
            {
                int value = decodeDataValue(data_block+value_counter, 3);
                if (value<sensor_properties.DMIN && error_codes==1)
                {
                    value=sensor_properties.DMAX;
                }
                //units are m
                 data.push_back(value/1000.0); break;
            }
            return HOKUYO_STATUS_ACQUISITION_COMPLETE;
        }

    // check in the first line if it is a valid answer to GD command
    if (current_line == 0) 
    {
        data_block[0]='\0'; //resets the datablock; 
        if ((laser_mode == MD_MODE && (text_data[0] != 'M' || text_data[1] != 'D')) ||
            (laser_mode == GD_MODE && (text_data[0] != 'G' || text_data[1] != 'D')))
            {
                #if LASER_DEBUG
                yDebug("Invalid answer to a MD command: %s\n", text_data);
                #endif
                return HOKUYO_STATUS_ERROR_INVALID_COMMAND;
            }
            else
                return  HOKUYO_STATUS_OK;
    }

    // check in the second line if the status of the sensor is ok
    if (current_line == 1)
    {
        if ((laser_mode == MD_MODE && (text_data[0] != '9' || text_data[1] != '9' || text_data[2] != 'b' )) ||
            (laser_mode == GD_MODE && (text_data[0] != '0' || text_data[1] != '0' || text_data[2] != 'P' )))
            {
                #if LASER_DEBUG
                yDebug("Invalid sensor status: %s\n", text_data);
                #endif
                return HOKUYO_STATUS_ERROR_BUSY;
            }
            else
                return HOKUYO_STATUS_OK;
    }

    // verify the checksum for all the following lines
    if (current_line >= 2)
    {
        char expected_checksum = text_data[text_data_len - 2];
        if (calculateCheckSum(text_data, text_data_len - 2, expected_checksum) < 0)
        {
            #if LASER_DEBUG
            yDebug("Cheksum error, line: %d: %s\n", current_line, text_data);
            #endif
            return HOKUYO_STATUS_ERROR_INVALID_CHECKSUM;
        }
    }

    // read the sensor time stamp in the third line
    if (current_line == 2)
    {
        timestamp = decodeDataValue(text_data, 4);
    }

    // read the data in the next lines
    if (current_line >= 3)
    {
        strncat (data_block, text_data, text_data_len-2 );
    }

    //increments the lines counter
    //current_line++;

    return HOKUYO_STATUS_OK;
}

bool JoypadControlClient::getRawStick(unsigned int stick_id, yarp::sig::Vector& value, JoypadCtrl_coordinateMode coordinate_mode)
{
    value.clear();
    if(m_rpc_only || coordinate_mode == IJoypadController::JypCtrlcoord_POLAR)
    {
        Bottle cmd, response;
        int    dof, coordmode;

        coordmode = coordinate_mode == IJoypadController::JypCtrlcoord_CARTESIAN ? VOCAB_CARTESIAN : VOCAB_POLAR;
        cmd.addVocab(VOCAB_IJOYPADCTRL);
        cmd.addVocab(VOCAB_GET);
        cmd.addVocab(VOCAB_STICKDOF);
        cmd.addVocab(VOCAB_VALUE);
        cmd.addInt32(stick_id);
        m_rpcPort.write(cmd, response);

        if(response.get(0).asVocab() == VOCAB_OK && response.get(1).isInt32())
        {
            dof = response.get(1).asInt32();
        }
        else
        {
            return false;
        }
        cmd.addVocab(VOCAB_IJOYPADCTRL);
        cmd.addVocab(VOCAB_GET);
        cmd.addVocab(VOCAB_STICK);
        cmd.addVocab(VOCAB_VALUE);
        cmd.addVocab(coordmode);
        cmd.addInt32(stick_id);
        m_rpcPort.write(cmd, response);
        if(response.get(0).asVocab() == VOCAB_OK)
        {
            for(int i = 0; i < dof; i++)
            {
                if(response.get(i).isFloat64())
                {
                    value.push_back(response.get(i).asFloat64());
                }
                else
                {
                    return false;
                }
            }
            return true;
        }
        else
        {
            return false;
        }
    }
    else
    {
        LockGuard l(m_stickPort.mutex);
        int offset = 0;

        unsigned int i;
        if(getStickCount(i), stick_id >= i)
        {
            yError() << "JoypadControlCLient: GetStick() error.. Stick_id out of bound";
            return false;
        }
        for(size_t j = 0; j < stick_id; j++)
        {
            offset += m_stickDof[j];
        }

        for(size_t i = 0; i < m_stickDof[stick_id]; ++i)
        {
            value.push_back(m_stickPort.storage[offset + i]);
        }

        return true;
    }
}