From 198c54d6ef1f9a665e5af4266acbbc2b46abaf2f Mon Sep 17 00:00:00 2001
From: Rohan Budhiraja <budhiraja@laas.fr>
Date: Fri, 29 Jan 2016 20:57:36 +0100
Subject: [PATCH] Patch for inputing Eigen::Transform as Matrix4d
---
include/dynamic-graph/value.h | 15 +++++-----
src/command/command.cpp | 2 +-
src/command/value.cpp | 53 +++++++++++++++++++++++++++--------
3 files changed, 49 insertions(+), 21 deletions(-)
diff --git a/include/dynamic-graph/value.h b/include/dynamic-graph/value.h
index d8adc30..ffa43c0 100644
--- a/include/dynamic-graph/value.h
+++ b/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_;
};
diff --git a/src/command/command.cpp b/src/command/command.cpp
index 7cd2b51..350a6af 100644
--- a/src/command/command.cpp
+++ b/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());
diff --git a/src/command/value.cpp b/src/command/value.cpp
index ed77090..4ff3123 100644
--- a/src/command/value.cpp
+++ b/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
--
GitLab