bool EdgeSE3Offset::read(std::istream& is) {
int pidFrom, pidTo;
is >> pidFrom >> pidTo ;
if (! setParameterId(0,pidFrom))
return false;
if (! setParameterId(1,pidTo))
return false;
Vector7d meas;
for (int i=0; i<7; i++)
is >> meas[i];
// normalize the quaternion to recover numerical precision lost by storing as human readable text
Vector4d::MapType(meas.data()+3).normalize();
setMeasurement(internal::fromVectorQT(meas));
if (is.bad()) {
return false;
}
for ( int i=0; i<information().rows() && is.good(); i++)
for (int j=i; j<information().cols() && is.good(); j++){
is >> information()(i,j);
if (i!=j)
information()(j,i)=information()(i,j);
}
if (is.bad()) {
// we overwrite the information matrix with the Identity
information().setIdentity();
}
return true;
}
bool EdgeSE3PointXYZDisparity::read(std::istream& is) {
// measured keypoint
int pid;
is >> pid;
setParameterId(0,pid);
Vector3d meas;
for (int i=0; i<3; i++) is >> meas[i];
setMeasurement(meas);
if (is.bad())
return false;
for ( int i=0; i<information().rows() && is.good(); i++)
for (int j=i; j<information().cols() && is.good(); j++){
is >> information()(i,j);
if (i!=j)
information()(j,i)=information()(i,j);
}
if (is.bad()) {
// we overwrite the information matrix
information().setIdentity();
information()(2,2)=1000.;
}
//_cacheIds[0] = _paramId;
return true;
}
bool EdgeProjectXYZ2UV::read(std::istream& is) {
int paramId;
is >> paramId;
setParameterId(0, paramId);
for (int i=0; i<2; i++) {
is >> _measurement[i];
}
for (int i=0; i<2; i++)
for (int j=i; j<2; j++) {
is >> information()(i,j);
if (i!=j)
information()(j,i)=information()(i,j);
}
return true;
}
bool EdgeSE3PointXYZ::read(std::istream& is) {
int pId;
is >> pId;
setParameterId(0, pId);
// measured keypoint
Vector3d meas;
for (int i=0; i<3; i++) is >> meas[i];
setMeasurement(meas);
// information matrix is the identity for features, could be changed to allow arbitrary covariances
if (is.bad()) {
return false;
}
for ( int i=0; i<information().rows() && is.good(); i++)
for (int j=i; j<information().cols() && is.good(); j++){
is >> information()(i,j);
if (i!=j)
information()(j,i)=information()(i,j);
}
if (is.bad()) {
// we overwrite the information matrix
information().setIdentity();
}
return true;
}
