[eigen] add ostream and istream operators for Eigen/Geoemetry classes.

modify dg::Vector and dg::Matrix istream operators.

Author: Rohan Budhiraja

ChangeLog

@@ -3,6 +3,11 @@ CHANGELOG
 
 [Current]
 
+[v3.0.0]
+ * Replace jrl-mal with eigen
+ * Add ostream and isteram operators for Eigen Classes
+
+
 [v2.5.5]
  * Improve Travis support
 

include/dynamic-graph/eigen-io.h

@@ -1,5 +1,5 @@
 //
-// Copyright 2010 CNRS
+// Copyright 2016 CNRS
 //
 // Author: Rohan Budhiraja
 //
@@ -34,92 +34,126 @@ using dynamicgraph::ExceptionSignal;
 
   /* \brief Eigen Vector input from istream
    *
-   * Input Vector format: val1 val2 val3 ... valN
-   * e.g. 1 23 32.2 12.12 32
+   * Input Vector format: [N](val1,val2,val3,...,valN)
+   * e.g. [5](1,23,32.2,12.12,32)
    */
 namespace Eigen {
   typedef EIGEN_DEFAULT_DENSE_INDEX_TYPE eigen_index;
   inline std::istringstream& operator >> (std::istringstream &iss, 
-					  dynamicgraph::Vector &inst)
-    {
-      std::vector<double> _stdvec;
-      double _dbl_val;
-      
-      boost::format fmt ("Failed to enter %s as vector. Reenter as [val1 val2 ... valN]");
-      fmt %iss.str();
-      
-      while(iss >> _dbl_val && !iss.fail()) {
-	_stdvec.push_back(_dbl_val);
-      }
-      try {
-	inst = Eigen::VectorXd::Map (_stdvec.data(),
-				     boost::numeric_cast<eigen_index> (_stdvec.size()) );
+					  dynamicgraph::Vector &inst) {
+    unsigned int _size;
+    double _dbl_val;
+    char _ch;
+    boost::format fmt ("Failed to enter %s as vector. Reenter as [N](val1,val2,val3,...,valN)");
+    fmt %iss.str();
+    if(iss>> _ch && _ch != '['){
+      throw ExceptionSignal(ExceptionSignal::GENERIC, fmt.str());
+    }
+    else {
+      if(iss >> _size && !iss.fail()){
+	inst.resize(_size);
       }
-      catch (boost::bad_numeric_cast&) {
-	throw ExceptionSignal(ExceptionSignal::GENERIC, fmt.str() );
+      else
+	throw ExceptionSignal(ExceptionSignal::GENERIC, fmt.str());
+      if(iss >> _ch && _ch != ']')
+	throw ExceptionSignal(ExceptionSignal::GENERIC, fmt.str());
+      else {
+	if(iss>> _ch && _ch != '(')
+	  throw ExceptionSignal(ExceptionSignal::GENERIC, fmt.str());
+	else {
+	  for (unsigned int i=0;i<_size; i++){
+	    iss >> _dbl_val;
+	    if (iss.peek() == ',' || iss.peek() == ' ')
+	      iss.ignore();
+	    inst(i) = _dbl_val;
+	  }
+	  if(iss >> _ch && _ch != ')')
+	    throw ExceptionSignal(ExceptionSignal::GENERIC, fmt.str());
+	}
       }
-      
-      return iss;
     }
+    return iss;
+  }
 
   /* \brief Eigen Matrix input from istream
    *
-   * Matrix format: [[val11 val12 val13 ... val1N] ... [valM1 valM2 ... valMN]]
-   * e.g. [[1 23 32.2 12.12 32][2 32 23 92.01 19.2]]
+   * Matrix format: [M,N]((val11,val12,val13,...,val1N),...,(valM1,valM2,...,valMN))
+   * e.g. [2,5]((1 23 32.2 12.12 32),(2 32 23 92.01 19.2))
    */
 
   template<typename Derived>
   inline std::istringstream& operator >> (std::istringstream &iss, 
-					  DenseBase<Derived> &inst)
-    {
-      std::vector<dynamicgraph::Vector> _stdmat;
-      char _ch;
-      int _vec_size;
-      bool _vec_size_set = false;
-
-      boost::format fmt 
-	("Failed to enter %s as matrix. Reenter as [[val11 val12 ... val1N]...[valM1 valM2 ... valMN]]. Check that vector sizes are consistent.");
-      fmt %iss.str();
-      
-      if(iss>> _ch && _ch != '['){
-	throw ExceptionSignal(ExceptionSignal::GENERIC, fmt.str());
-      }
-      else{
-	dynamicgraph::Vector _eigvec;
-	while(iss >> _eigvec && !iss.fail()){
-	  if (!_vec_size_set) {
-	    try {
-	      _vec_size = boost::numeric_cast <int> (_eigvec.size());
-	    }
-	    catch (boost::bad_numeric_cast&) {
-	      throw ExceptionSignal(ExceptionSignal::GENERIC, fmt.str());
-	  }
-	    _vec_size_set = true;
-	  }
-	  else {
-	    if (_eigvec.size() != _vec_size) {
-	      throw ExceptionSignal(ExceptionSignal::GENERIC, fmt.str());
-	    }
-	  }
-	  _stdmat.push_back(_eigvec);
-	}
-	if(iss>> _ch && _ch != ']'){
+					  DenseBase<Derived> &inst) {
+    unsigned int _colsize;
+    unsigned int _rowsize;
+    double _dbl_val;
+    char _ch;
+    boost::format fmt ("Failed to enter %s as vector. Reenter as [N](val1,val2,val3,...,valN)");
+    fmt %iss.str();
+    if(iss>> _ch && _ch != '['){
+      throw ExceptionSignal(ExceptionSignal::GENERIC, fmt.str());
+    }
+    else {
+      iss >>_rowsize;
+      if (iss.peek() == ',' || iss.peek() == ' ')
+	iss.ignore();
+      iss >> _colsize;
+      if (iss.fail())
+	throw ExceptionSignal(ExceptionSignal::GENERIC,fmt.str());
+      else {
+	inst.resize(_rowsize,_colsize);
+	if(iss >> _ch && _ch != ']')
 	  throw ExceptionSignal(ExceptionSignal::GENERIC, fmt.str());
-	}
 	else {
-	  try {
-	    inst.resize(boost::numeric_cast<eigen_index> (_stdmat.size()), _vec_size);
-	  }
-	  catch (boost::bad_numeric_cast&) {
+	  if(iss>> _ch && _ch != '(')
 	    throw ExceptionSignal(ExceptionSignal::GENERIC, fmt.str());
-	  }
-	  for (unsigned int i =0;i<_stdmat.size(); i++) {
-	    inst.row(i) = _stdmat[i];
+	  else {
+	    for (unsigned int j=0;j<_rowsize; j++){
+	      if(iss>> _ch && _ch != '(')
+		throw ExceptionSignal(ExceptionSignal::GENERIC, fmt.str());
+	      for (unsigned int i=0;i<_colsize; i++){
+		iss >> _dbl_val;
+		if (iss.peek() == ',' || iss.peek() == ' ')
+		  iss.ignore();
+		inst(j,i) = _dbl_val;
+	      }
+	      if(iss >> _ch && _ch != ')')
+		throw ExceptionSignal(ExceptionSignal::GENERIC, fmt.str());
+	      if (iss.peek() == ',' || iss.peek() == ' ')
+		iss.ignore();
+	    }
+	    if(iss >> _ch && _ch != ')')
+	      throw ExceptionSignal(ExceptionSignal::GENERIC, fmt.str());
 	  }
 	}
       }
-      return iss;
     }
+    return iss;
+  }
+
+  
+  inline std::istringstream& operator >> (std::istringstream &iss, 
+					  Transform<double,3,Affine> &inst) {
+    Matrix4d M;    iss >> M;    inst = M;    return iss;  }
+  
+  
+  
+  inline std::ostream& operator << (std::ostream &os, 
+				    Transform<double,3,Affine> MH) {
+    os << MH.matrix();      return os;   }
+  
+  
+  inline std::ostream& operator << (std::ostream &os, 
+				    AngleAxisd quat) {
+    Vector4d v;      v(0) = quat.angle();      v.tail<3>() = quat.axis();
+    os << v;      return os;   }
+  
+  inline std::istringstream& operator >> (std::istringstream &iss, 
+					  AngleAxisd &inst) {
+    Vector4d v;    iss >>v;
+    inst.angle() = v(0);    inst.axis() = v.tail<3>();
+    return iss;  }
+  
 }
 
 

tests/signal-cast-registerer.cpp

@@ -149,7 +149,7 @@ BOOST_AUTO_TEST_CASE (custom_vector_registerer)
       Vector  v = Vector::Unit(5,i) ;
       std::ostringstream os;
       os << v;
-      std::istringstream ss (os.str ());
+      std::istringstream ss ("[5]("+os.str ()+")");
 
       // Set signal value.
       myVectorSignal.set (ss);