C++ Matrix::resize方法代码示例

void iCubShoulderConstr::appendMatrixRow(yarp::sig::Matrix &dest,
                                         const yarp::sig::Vector &row)
{
    yarp::sig::Matrix tmp;

    // if dest is already filled with something
    if (dest.rows())
    {   
        // exit if lengths do not match     
        if (row.length()!=dest.cols())
            return;

        tmp.resize(dest.rows()+1,dest.cols());

        // copy the content of dest in temp
        for (int i=0; i<dest.rows(); i++)
            for (int j=0; j<dest.cols(); j++)
                tmp(i,j)=dest(i,j);

        // reassign dest
        dest=tmp;
    }
    else
        dest.resize(1,row.length());

    // append the last row
    for (int i=0; i<dest.cols(); i++)
        dest(dest.rows()-1,i)=row[i];
}

bool KDLtoYarp(const KDL::Rotation & kdlRotation, yarp::sig::Matrix & yarpMatrix3_3)
{
    if( yarpMatrix3_3.rows() != 3 || yarpMatrix3_3.cols() != 3 ) { yarpMatrix3_3.resize(3,3); }
    //Both kdl and yarp store the rotation matrix in row major order
    memcpy(yarpMatrix3_3.data(),kdlRotation.data,3*3*sizeof(double));
    return true;
}

bool Matrix_read(const std::string file_name, yarp::sig::Matrix & mat)
{
    FILE * fp;
    uint32_t rows,cols;
    double elem;
    
    fp = fopen(file_name.c_str(),"rb");
    if( fp == NULL ) return false;
    
    //reading dimensions information
    fread(&rows,sizeof(uint32_t),1,fp);
    fread(&cols,sizeof(uint32_t),1,fp);
    
    mat.resize(rows,cols);
    
    for(size_t i=0;i<rows;i++) {
        for(size_t j=0;j<cols;j++ ) {
             fread(&elem,sizeof(double),1,fp);
             mat(i,j) = elem;
         }
     }
    
    /*
    if( gsl_matrix_fread(fp,(gsl_matrix*)mat.getGslMatrix()) == GSL_EFAILED ) {
        fclose(fp);
        return false;
    }
    */
    
    
    fclose(fp);
    return true;
}

bool yarpWholeBodyModelV1::convertBasePose(const Frame &xBase, yarp::sig::Matrix & H_world_base)
{
    if( H_world_base.cols() != 4 || H_world_base.rows() != 4 )
        H_world_base.resize(4,4);
    xBase.get4x4Matrix(H_world_base.data());
    return true;
}

void RobotInterface::getVelocityState(yarp::sig::Matrix& outputVelocity)
{
	outputVelocity.resize(velocityState.rows(), velocityState.cols());
	for(int i = 0; i < outputVelocity.rows(); i++)
	{
		for(int d = 0; d < outputVelocity.cols(); d++) outputVelocity(i, d) = velocityState(i, d);
	}
}

void RobotInterface::getPositionState(yarp::sig::Matrix& outputPosition)
{
	outputPosition.resize(positionState.rows(), positionState.cols());
	for(int i = 0; i < outputPosition.rows(); i++)
	{
		for(int d = 0; d < outputPosition.cols(); d++) outputPosition(i, d) = positionState(i, d);
	}
}

void RobotInterface::getPressureState(yarp::sig::Matrix& outputPressure)
{
	outputPressure.resize(pressureState.rows(), pressureState.cols());
	for(int i = 0; i < outputPressure.rows(); i++)
	{
		for(int d = 0; d < outputPressure.cols(); d++) outputPressure(i, d) = pressureState(i, d);
	}
}

void buildRotMat2(const double &theta,yarp::sig::Matrix &rot)
{
    rot.resize(2,2);
    rot(0,0)=cos(theta);
    rot(0,1)=sin(theta);
    rot(1,0)=-sin(theta);
    rot(1,1)=cos(theta);
}

void idyntree2yarp(const iDynTreeMatrixType & idyntreeMatrix,
                   yarp::sig::Matrix & yarpMatrix)
{
    yarpMatrix.resize(idyntreeMatrix.rows(),idyntreeMatrix.cols());
    for(size_t row=0; row < idyntreeMatrix.rows(); row++)
    {
        for(size_t col=0; col < idyntreeMatrix.cols(); col++)
        {
            yarpMatrix(row,col) = idyntreeMatrix(row,col);
        }
    }
}

void HandIKModule::fillMatrixFromBottle(const Bottle* b, yarp::sig::Matrix &m, int rows, int cols)
{
    int k=0;
    m.resize(rows,cols);
    for (int i=0; i<rows; i++)
    {
        for (int j=0; j<cols; j++)
        {
            m(i,j)=b->get(k).asDouble();
            k++;
        }
    }
}

void buildRotMat3(const double &alpha,const double &beta,const double &gamma,yarp::sig::Matrix &rot)
{
    rot.resize(3,3);
    rot(0,0)=cos(beta)*cos(gamma);
    rot(0,1)=-cos(beta)*sin(gamma);
    rot(0,2)=sin(beta);
    rot(1,0)=sin(alpha)*sin(beta)*cos(gamma)+cos(alpha)*sin(gamma);
    rot(1,1)=-sin(alpha)*sin(beta)*sin(gamma)+cos(alpha)*cos(gamma);
    rot(1,2)=-sin(alpha)*cos(beta);
    rot(2,0)=-cos(alpha)*sin(beta)*cos(gamma)+sin(alpha)*sin(gamma);
    rot(2,1)=cos(alpha)*sin(beta)*sin(gamma)+sin(alpha)*cos(gamma);
    rot(2,2)=cos(alpha)*cos(beta);
}

bool insituFTSensorCalibrationThread::getCalibration(yarp::sig::Matrix & mat)
{
    mat.resize(3,6);

    bool ret = true;

    std::string err;
    ret = estimator.computeForceCalibrationMatrixEstimation(err);
    if( !ret )
    {
        yError("insituFTSensorCalibrationThread CalibrationMatrixEstimation failed %s",err.c_str());
        return false;
    }

    return estimator.getEstimatedForceCalibrationMatrix(InSituFTCalibration::wrapMat(mat));
}

bool KDLtoYarp_position(const KDL::Frame & kdlFrame, yarp::sig::Matrix & yarpMatrix4_4 )
{
    yarp::sig::Matrix R(3,3);
    yarp::sig::Vector p(3);

    KDLtoYarp(kdlFrame.M,R);
    KDLtoYarp(kdlFrame.p,p);

    if( yarpMatrix4_4.rows() != 4 || yarpMatrix4_4.cols() != 4 ) { yarpMatrix4_4.resize(4,4); }
    yarpMatrix4_4.zero();

    yarpMatrix4_4.setSubmatrix(R,0,0);
    yarpMatrix4_4.setSubcol(p,0,3);
    yarpMatrix4_4(3,3) = 1;

    return true;
}

    //---------------------------------------------------------
    bool eigenToYarpMatrix(const Eigen::MatrixXd& eigenMatrix, yarp::sig::Matrix& yarpMatrix)
    {
        if((yarpMatrix.cols() == 0)||(yarpMatrix.rows() == 0))
        {
            cout<<"ERROR: input matrix is empty (eigenToYarpMatrix)"<<endl;
            return false;
        }

        //resize and fill eigen vector with yarp vector elements
        yarpMatrix.resize(eigenMatrix.rows(),eigenMatrix.cols());
        
        for(unsigned int i = 0; i < yarpMatrix.rows(); i++)
            for(unsigned int j = 0; j < yarpMatrix.cols(); j++)
                yarpMatrix(i,j) = eigenMatrix(i,j);
            
        return true;
    };

bool Matrix_read(const std::string file_name, yarp::sig::Matrix & mat)
{
    FILE * fp;
    uint32_t rows,cols;
    double elem;

    std::cerr << "Opening matrix " << file_name << std::endl;


    fp = fopen(file_name.c_str(),"rb");
    if( fp == NULL ) {
        std::cerr << "Error opening matrix " << file_name << std::endl;
        return false;
    }

    //reading dimensions information
    fread(&rows,sizeof(uint32_t),1,fp);
    fread(&cols,sizeof(uint32_t),1,fp);

    mat.resize(rows,cols);

    for(size_t i=0; i<rows; i++) {
        for(size_t j=0; j<cols; j++ ) {
            fread(&elem,sizeof(double),1,fp);
            std::cout << "Read element " << i << " " << j << " : " << elem << std::endl;
            mat(i,j) = elem;
        }
    }

    /*
    if( gsl_matrix_fread(fp,(gsl_matrix*)mat.getGslMatrix()) == GSL_EFAILED ) {
        fclose(fp);
        return false;
    }
    */


    fclose(fp);
    return true;
}