Merge pull request RainerKuemmerle#234 from opcode81/slam-3d-read-imp…

…rovements

Improvements in read methods of ParameterSE3Offset and EdgeSE3Prior

g2o/types/slam3d/edge_se3_prior.cpp

@@ -63,20 +63,15 @@ namespace g2o {
     setMeasurement(internal::fromVectorQT(meas));
     // don't need this if we don't use it in error calculation (???)
     // information matrix is the identity for features, could be changed to allow arbitrary covariances    
-    if (is.bad()) {
-      return false;
+    if (is.good()) {
+      for ( int i=0; i<information().rows(); i++)
+        for (int j=i; j<information().cols(); j++){
+          is >> information()(i,j);
+          if (i!=j)
+            information()(j,i)=information()(i,j);
+        }
     }
-    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;
+    return !is.fail();
   }
 
   bool EdgeSE3Prior::write(std::ostream& os) const {

g2o/types/slam3d/parameter_se3_offset.cpp

@@ -52,7 +52,7 @@ namespace g2o {
     // normalize the quaternion to recover numerical precision lost by storing as human readable text
     Vector4::MapType(off.data()+3).normalize();
     setOffset(internal::fromVectorQT(off));
-    return is.good();
+    return !is.fail();
   }
 
   bool ParameterSE3Offset::write(std::ostream& os) const {