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;
}
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;
}
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;
}
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;
}
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;
}
