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