Patch for inputing Eigen::Transform as Matrix4d

Author: Rohan Budhiraja

include/dynamic-graph/value.h

@@ -30,8 +30,6 @@ namespace dynamicgraph {
     class Value;
     class DYNAMIC_GRAPH_DLLAPI EitherType {
     public:
-      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
       EitherType(const Value& value);
       ~EitherType();
       operator bool () const;
@@ -41,16 +39,14 @@ namespace dynamicgraph {
       operator double () const;
       operator std::string () const;
       operator Vector () const;
-      operator Matrix () const;
+      operator Eigen::MatrixXd () const;
+      operator Eigen::Matrix4d () const;
     private:
       const Value* value_;
     };
 
     class DYNAMIC_GRAPH_DLLAPI Value {
     public:
-
-      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
       enum Type {
 	NONE,
 	BOOL,
@@ -61,6 +57,7 @@ namespace dynamicgraph {
 	STRING,
 	VECTOR,
 	MATRIX,
+	MATRIX4D,
 	NB_TYPES
       };
       ~Value();
@@ -72,7 +69,8 @@ namespace dynamicgraph {
       explicit Value(const double& value);
       explicit Value(const std::string& value);
       explicit Value(const Vector& value);
-      explicit Value(const Matrix& value);
+      explicit Value(const Eigen::MatrixXd& value);
+      explicit Value(const Eigen::Matrix4d& value);
       /// Copy constructor
       Value(const Value& value);
       // Construct an empty value (None)
@@ -108,7 +106,8 @@ namespace dynamicgraph {
       double doubleValue() const;
       std::string stringValue() const;
       Vector vectorValue() const;
-      Matrix matrixValue() const;
+      Eigen::MatrixXd matrixXdValue() const;
+      Eigen::Matrix4d matrix4dValue() const;
       Type type_;
       const void* const value_;
     };

src/command/command.cpp

@@ -50,7 +50,7 @@ namespace dynamicgraph {
       for (unsigned int iParam=0; iParam < values.size(); iParam++) {
 	if (values[iParam].type() != paramTypes[iParam]) {
 	  std::stringstream ss;
-	  ss << "argument " << iParam << "is of wrong type: "
+	  ss << "argument " << iParam << " is of wrong type: "
 	     << Value::typeName(paramTypes[iParam]) << " expected, got "
 	     << Value::typeName(values[iParam].type());
 	  throw ExceptionAbstract(ExceptionAbstract::TOOLS, ss.str());

src/command/value.cpp

@@ -61,9 +61,14 @@ namespace dynamicgraph {
     {
       return value_->vectorValue();
     }
-    EitherType::operator Matrix() const
+    EitherType::operator Eigen::MatrixXd() const
     {
-      return value_->matrixValue();
+      return value_->matrixXdValue();
+    }
+
+    EitherType::operator Eigen::Matrix4d() const
+    {
+      return value_->matrix4dValue();
     }
 
     void Value::deleteValue ()
@@ -91,7 +96,10 @@ namespace dynamicgraph {
 	delete(const Vector*)value_;
 	break;
       case MATRIX:
-	delete(const Matrix*)value_;
+	delete(const Eigen::MatrixXd*)value_;
+	break;
+      case MATRIX4D:
+	delete(const Eigen::Matrix4d*)value_;
 	break;
       default:;
       }
@@ -129,8 +137,12 @@ namespace dynamicgraph {
 					value_(new Vector(value))
     {
     }
-    Value::Value(const Matrix& value) : type_(MATRIX),
-					value_(new Matrix(value))
+    Value::Value(const Eigen::MatrixXd& value) : type_(MATRIX),
+						 value_(new Eigen::MatrixXd(value))
+    {
+    }
+    Value::Value(const Eigen::Matrix4d& value) : type_(MATRIX4D),
+						 value_(new Eigen::Matrix4d(value))
     {
     }
 
@@ -168,7 +180,10 @@ namespace dynamicgraph {
 	copy = new Vector(value.vectorValue());
 	break;
       case Value::MATRIX:
-	copy = new Matrix(value.matrixValue());
+	copy = new Eigen::MatrixXd(value.matrixXdValue());
+	break;
+      case Value::MATRIX4D:
+	copy = new Eigen::Matrix4d(value.matrix4dValue());
 	break;
       default:
 	abort();
@@ -262,12 +277,20 @@ namespace dynamicgraph {
 			      "value is not an vector");
     }
 
-    Matrix Value::matrixValue() const
+    Eigen::MatrixXd Value::matrixXdValue() const
     {
       if(type_ == MATRIX)
-	return *((const Matrix*)value_);
+	return *((const Eigen::MatrixXd*)value_);
+      throw ExceptionAbstract(ExceptionAbstract::TOOLS,
+			      "value is not a Eigen matrixXd");
+    }
+
+    Eigen::Matrix4d Value::matrix4dValue() const
+    {
+      if(type_ == MATRIX4D)
+	return *((const Eigen::Matrix4d*)value_);
       throw ExceptionAbstract(ExceptionAbstract::TOOLS,
-			      "value is not a matrix");
+			      "value is not a Eigen matrix4d");
     }
 
     std::string Value::typeName(Type type)
@@ -288,7 +311,9 @@ namespace dynamicgraph {
       case VECTOR:
 	return std::string("vector");
       case MATRIX:
-	return std::string("matrix");
+	return std::string("matrixXd");
+      case MATRIX4D:
+	return std::string("matrix4d");
       default:
 	return std::string("unknown");
       }
@@ -321,7 +346,10 @@ namespace dynamicgraph {
 	os << value.vectorValue();
 	break;
       case Value::MATRIX:
-	os << value.matrixValue();
+	os << value.matrixXdValue();
+	break;
+      case Value::MATRIX4D:
+	os << value.matrix4dValue();
 	break;
       default:
 	return os;
@@ -336,7 +364,8 @@ namespace dynamicgraph {
     template<> const Value::Type ValueHelper<double>::TypeID = Value::DOUBLE;
     template<> const Value::Type ValueHelper<std::string>::TypeID = Value::STRING;
     template<> const Value::Type ValueHelper<Vector>::TypeID = Value::VECTOR;
-    template<> const Value::Type ValueHelper<Matrix>::TypeID = Value::MATRIX;
+    template<> const Value::Type ValueHelper<Eigen::MatrixXd>::TypeID = Value::MATRIX;
+    template<> const Value::Type ValueHelper<Eigen::Matrix4d>::TypeID = Value::MATRIX4D;
 
   } // namespace command
 } //namespace dynamicgraph