コード例 #1
0
bool Vector3Seq::loadPlainFormat(const std::string& filename)
{
    PlainSeqFileLoader loader;

    if(!loader.load(filename)){
        setIoErrorMessage(loader.errorMessage());
        return false;
    }

    if(loader.numParts() < 3){
        setIoErrorMessage(filename + "does not have 3 columns for 3d vector elements");
        return false;
    }
  
    setNumFrames(loader.numFrames());
    setFrameRate(1.0 / loader.timeStep());

    int frame = 0;
    for(PlainSeqFileLoader::iterator it = loader.begin(); it != loader.end(); ++it){
        vector<double>& data = *it;
        (*this)[frame++] << data[1], data[2], data[3];
    }

    return true;
}
コード例 #2
0
bool MultiAffine3Seq::loadPlainFormat(const std::string& filename)
{
    clearSeqMessage();
    PlainSeqFileLoader loader;

    if(!loader.load(filename)){
        addSeqMessage(loader.errorMessage());
        return false;
    }

    if(loader.numParts() < 12){
        addSeqMessage(filename +
                      "does not have 12-columns "
                      "(3 for position vectors, 9 for attitde matrices)");
        return false;
    }
  
    setDimension(loader.numFrames(), 1);
    setTimeStep(loader.timeStep());

    int i = 0;
    Part base = part(0);
    for(PlainSeqFileLoader::iterator it = loader.begin(); it != loader.end(); ++it){
        vector<double>& data = *it;
        base[i].translation() << data[1], data[2], data[3];
        base[i].linear() <<
            data[ 4], data[ 5], data[ 6],
            data[ 7], data[ 8], data[ 9],
            data[10], data[11], data[12];
        ++i;
    }

    return true;
}
コード例 #3
0
ファイル: MultiSE3Seq.cpp プロジェクト: kayusawa/choreonoid
bool MultiSE3Seq::loadPlainMatrixFormat(const std::string& filename)
{
    clearSeqMessage();
    PlainSeqFileLoader loader;

    if(!loader.load(filename)){
        addSeqMessage(loader.errorMessage());
        return false;
    }

    int n = loader.numParts();
    if(n < 12 || (n % 12) != 0){
        addSeqMessage(filename +
                      "does not have elements in multiple of twelve "
                      "(each 3 for position vectors, 9 for attitde matrices)");
        return false;
    }
    int m = n / 12;
  
    setDimension(loader.numFrames(), m);
    setTimeStep(loader.timeStep());

    int f = 0;
    Part base = part(0);
    for(PlainSeqFileLoader::iterator it = loader.begin(); it != loader.end(); ++it){
        vector<double>& data = *it;
        int i = 0;
        Frame frame = MultiSE3Seq::frame(f++);
        for(int j=0; j < m; ++j){
            SE3& x = frame[j];
            x.translation() << data[i], data[i+1], data[i+2];
            i += 3;
            Matrix3 R;
            R <<
                data[i],   data[i+1], data[i+2],
                data[i+3], data[i+4], data[i+5],
                data[i+6], data[i+7], data[i+8];
            i += 9;
            x.rotation() = R;
        }
    }

    return true;
}
コード例 #4
0
bool MultiValueSeq::loadPlainFormat(const std::string& filename, std::ostream& os)
{
    PlainSeqFileLoader loader;

    if(!loader.load(filename, os)){
        return false;
    }

    setDimension(loader.numFrames(), loader.numParts());
    setFrameRate(1.0 / loader.timeStep());

    int i = 0;
    for(PlainSeqFileLoader::iterator it = loader.begin(); it != loader.end(); ++it){
        copy((it->begin() + 1), it->end(), frame(i++).begin());
    }

    return true;
}
コード例 #5
0
ファイル: MultiSE3Seq.cpp プロジェクト: kayusawa/choreonoid
bool MultiSE3Seq::loadPlainRpyFormat(const std::string& filename)
{
    clearSeqMessage();
    PlainSeqFileLoader loader;

    if(!loader.load(filename)){
        addSeqMessage(loader.errorMessage());
        return false;
    }

    int n = loader.numParts();
    if(n != 3){
        addSeqMessage(filename +
                      "does not have a multiple of 3 elements (R,P,Y)");
        return false;
    }

    setDimension(loader.numFrames(), 1);
    setTimeStep(loader.timeStep());

    int f = 0;
    Part base = part(0);
    for(PlainSeqFileLoader::iterator it = loader.begin(); it != loader.end(); ++it){
        vector<double>& data = *it;
        Frame frame = MultiSE3Seq::frame(f++);
        SE3& x = frame[0];
        x.translation() << 0, 0, 0;
        double r, p, y;
        //First element is time
        r = data[1];
        p = data[2];
        y = data[3];
        Matrix3 R = rotFromRpy(r, p, y);
        x.rotation() = R;
    }

    return true;
}