From bfafccbef0b1823c310f6203a6d4e213650c70e6 Mon Sep 17 00:00:00 2001
From: Rohan Budhiraja <budhiraja@laas.fr>
Date: Thu, 10 Dec 2015 16:10:53 +0100
Subject: [PATCH] [eigen] Replace jrl-mal with eigen

---
 .gitmodules                                |   4 +-
 .travis.yml                                |   1 +
 CMakeLists.txt                             |   7 +-
 include/CMakeLists.txt                     |   1 +
 include/dynamic-graph/eigen-io.h           | 126 +++++++++++++++++++++
 include/dynamic-graph/linear-algebra.h     |  11 +-
 include/dynamic-graph/signal-cast-helper.h |   6 +-
 include/dynamic-graph/value.h              |   7 +-
 src/CMakeLists.txt                         |   7 +-
 src/command/value.cpp                      |   1 -
 src/signal/signal-cast-helper.cpp          |  10 +-
 tests/CMakeLists.txt                       |   6 -
 tests/signal-cast-registerer-libA.hh       |   5 +-
 tests/signal-cast-registerer-libB.hh       |   5 +-
 tests/signal-cast-registerer.cpp           |  25 ++--
 15 files changed, 173 insertions(+), 49 deletions(-)
 create mode 100644 include/dynamic-graph/eigen-io.h

diff --git a/.gitmodules b/.gitmodules
index 5a0f6c9..6dc3ed2 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -1,6 +1,6 @@
 [submodule "jrl-cmakemodules"]
-	path = jrl-cmakemodules
-	url = git://github.com/jrl-umi3218/jrl-cmakemodules.git
+	   path = jrl-cmakemodules
+	   url = git://github.com/jrl-umi3218/jrl-cmakemodules.git
 [submodule "cmake"]
 	path = cmake
 	url = git://github.com/jrl-umi3218/jrl-cmakemodules.git
diff --git a/.travis.yml b/.travis.yml
index 786db3a..9cd6f2c 100644
--- a/.travis.yml
+++ b/.travis.yml
@@ -1,4 +1,5 @@
 language: cpp
+sudo: required
 compiler:
   - gcc
   - clang
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 3266697..b4cbf66 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -17,6 +17,7 @@ CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
 
 INCLUDE(cmake/base.cmake)
 INCLUDE(cmake/boost.cmake)
+INCLUDE(cmake/eigen.cmake)
 INCLUDE(cmake/pthread.cmake)
 INCLUDE(cmake/cpack.cmake)
 
@@ -31,9 +32,6 @@ SET(LIBRARY_OUTPUT_PATH ${PROJECT_BINARY_DIR}/lib)
 
 SETUP_PROJECT()
 
-# Trigger dependency to jrl-mal
-ADD_REQUIRED_DEPENDENCY("jrl-mal >= 1.8.0")
-
 # Add configuration headers for plug-ins.
 GENERATE_CONFIGURATION_HEADER(
   ${HEADER_DIR} config-tracer.hh DG_TRACER tracer_EXPORTS)
@@ -62,6 +60,9 @@ PKG_CONFIG_APPEND_LIBS("dynamic-graph")
 SEARCH_FOR_BOOST()
 SEARCH_FOR_PTHREAD()
 
+# Search for Boost.
+SEARCH_FOR_EIGEN()
+
 ADD_SUBDIRECTORY(src)
 ADD_SUBDIRECTORY(include)
 ADD_SUBDIRECTORY(doc)
diff --git a/include/CMakeLists.txt b/include/CMakeLists.txt
index 079ed6d..ec6d20b 100644
--- a/include/CMakeLists.txt
+++ b/include/CMakeLists.txt
@@ -67,6 +67,7 @@ tracer.h
 tracer-real-time.h
 
 command.h
+eigen-io.h
 linear-algebra.h
 value.h
 
diff --git a/include/dynamic-graph/eigen-io.h b/include/dynamic-graph/eigen-io.h
new file mode 100644
index 0000000..f3ee994
--- /dev/null
+++ b/include/dynamic-graph/eigen-io.h
@@ -0,0 +1,126 @@
+//
+// Copyright 2010 CNRS
+//
+// Author: Rohan Budhiraja
+//
+// This file is part of dynamic-graph.
+// dynamic-graph is free software: you can redistribute it and/or
+// modify it under the terms of the GNU Lesser General Public License
+// as published by the Free Software Foundation, either version 3 of
+// the License, or (at your option) any later version.
+// dynamic-graph is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.  You should
+// have received a copy of the GNU Lesser General Public License along
+// with dynamic-graph.  If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef DYNAMIC_GRAPH_EIGEN_IO_H
+#define DYNAMIC_GRAPH_EIGEN_IO_H
+
+#include <iostream>
+#include <boost/format.hpp>
+#include <boost/numeric/conversion/cast.hpp>
+
+#include <dynamic-graph/exception-signal.h>
+#include <dynamic-graph/linear-algebra.h>
+#include <Eigen/Geometry>
+
+using dynamicgraph::ExceptionSignal;
+
+//TODO: Eigen 3.3 onwards has a global Eigen::Index definition.
+//If Eigen version is updated, use Eigen::Index instead of this macro.
+
+
+  /* \brief Eigen Vector input from istream
+   *
+   * Input Vector format: val1 val2 val3 ... valN
+   * e.g. 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()) );
+      }
+      catch (boost::bad_numeric_cast&) {
+	throw ExceptionSignal(ExceptionSignal::GENERIC, fmt.str() );
+      }
+      
+      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]]
+   */
+
+  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 != ']'){
+	  throw ExceptionSignal(ExceptionSignal::GENERIC, fmt.str());
+	}
+	else {
+	  try {
+	    inst.resize(boost::numeric_cast<eigen_index> (_stdmat.size()), _vec_size);
+	  }
+	  catch (boost::bad_numeric_cast&) {
+	    throw ExceptionSignal(ExceptionSignal::GENERIC, fmt.str());
+	  }
+	  for (unsigned int i =0;i<_stdmat.size(); i++) {
+	    inst.row(i) = _stdmat[i];
+	  }
+	}
+      }
+      return iss;
+    }
+}
+
+
+#endif //DYNAMIC_GRAPH_EIGEN_IO_H
diff --git a/include/dynamic-graph/linear-algebra.h b/include/dynamic-graph/linear-algebra.h
index 62cc76d..7605815 100644
--- a/include/dynamic-graph/linear-algebra.h
+++ b/include/dynamic-graph/linear-algebra.h
@@ -17,15 +17,12 @@
 
 #ifndef DYNAMIC_GRAPH_LINEAR_ALGEBRA_H
 #define DYNAMIC_GRAPH_LINEAR_ALGEBRA_H
-
-#include <jrl/mal/boost.hh>
-#include <boost/numeric/ublas/matrix.hpp>
+#include <Eigen/Core>
 
 namespace dynamicgraph {
-  typedef maal::boost::Vector Vector;
-  typedef maal::boost::Matrix Matrix;
+  typedef Eigen::MatrixXd Matrix;
+  typedef Eigen::VectorXd Vector;
 }
 
-#endif //DYNAMIC_GRAPH_LINEAR_ALGEBRA_H
-
 
+#endif //DYNAMIC_GRAPH_LINEAR_ALGEBRA_H
diff --git a/include/dynamic-graph/signal-cast-helper.h b/include/dynamic-graph/signal-cast-helper.h
index f7704db..4024e9a 100644
--- a/include/dynamic-graph/signal-cast-helper.h
+++ b/include/dynamic-graph/signal-cast-helper.h
@@ -28,6 +28,8 @@
 # include <boost/lexical_cast.hpp>
 # include <boost/tuple/tuple.hpp>
 
+#include <dynamic-graph/eigen-io.h>
+
 # include <dynamic-graph/dynamic-graph-api.h>
 # include "dynamic-graph/exception-signal.h"
 # include "dynamic-graph/signal-caster.h"
@@ -99,8 +101,8 @@ namespace dynamicgraph
     class SignalCast
     {
     public:
-      static T cast( std::istringstream& stringValue ) { throw 1;}
-      static void disp( const T& t,std::ostream& os )  { throw 1;  }
+      static T cast( std::istringstream& ) { throw 1;}
+      static void disp( const T&,std::ostream&)  { throw 1;  }
       static void trace( const T& t,std::ostream& os ) { disp(t,os); }
     public:
       // adapter functions for SignalCast
diff --git a/include/dynamic-graph/value.h b/include/dynamic-graph/value.h
index f3614bb..d8adc30 100644
--- a/include/dynamic-graph/value.h
+++ b/include/dynamic-graph/value.h
@@ -23,13 +23,15 @@
 #include <cassert>
 #include <typeinfo>
 #include "dynamic-graph/dynamic-graph-api.h"
-#include "dynamic-graph/linear-algebra.h"
+#include <dynamic-graph/linear-algebra.h>
 
 namespace dynamicgraph {
   namespace command {
     class Value;
     class DYNAMIC_GRAPH_DLLAPI EitherType {
     public:
+      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
       EitherType(const Value& value);
       ~EitherType();
       operator bool () const;
@@ -46,6 +48,9 @@ namespace dynamicgraph {
 
     class DYNAMIC_GRAPH_DLLAPI Value {
     public:
+
+      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
       enum Type {
 	NONE,
 	BOOL,
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index de7b7f8..9209da6 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -50,7 +50,6 @@ ADD_LIBRARY(${LIBRARY_NAME}
 
 SET_TARGET_PROPERTIES(${LIBRARY_NAME} PROPERTIES SOVERSION ${PROJECT_VERSION})
 
-PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} jrl-mal)
 
 IF (UNIX)
   TARGET_LINK_LIBRARIES(${LIBRARY_NAME} ${CMAKE_DL_LIBS} pthread)
@@ -58,9 +57,9 @@ ENDIF (UNIX)
 
 TARGET_LINK_LIBRARIES(${LIBRARY_NAME} ${Boost_LIBRARIES})
 
-IF (UNIX AND NOT APPLE)
-  TARGET_LINK_LIBRARIES(${LIBRARY_NAME} ${JRL_MAL_LDFLAGS_OTHER})
-ENDIF (UNIX AND NOT APPLE)
+#IF (UNIX AND NOT APPLE)
+#  TARGET_LINK_LIBRARIES(${LIBRARY_NAME} ${JRL_MAL_LDFLAGS_OTHER})
+#ENDIF (UNIX AND NOT APPLE)
 
 INSTALL(TARGETS ${LIBRARY_NAME}
   DESTINATION ${CMAKE_INSTALL_LIBDIR})
diff --git a/src/command/value.cpp b/src/command/value.cpp
index 92a032d..ed77090 100644
--- a/src/command/value.cpp
+++ b/src/command/value.cpp
@@ -15,7 +15,6 @@
 // have received a copy of the GNU Lesser General Public License along
 // with dynamic-graph.  If not, see <http://www.gnu.org/licenses/>.
 
-#include <boost/numeric/ublas/io.hpp>
 #include "dynamic-graph/value.h"
 #include "dynamic-graph/exception-abstract.h"
 
diff --git a/src/signal/signal-cast-helper.cpp b/src/signal/signal-cast-helper.cpp
index 00a683b..376bfe4 100644
--- a/src/signal/signal-cast-helper.cpp
+++ b/src/signal/signal-cast-helper.cpp
@@ -24,7 +24,6 @@
 #include <sstream>
 #include <algorithm>
 #include <dynamic-graph/exception-signal.h>
-
 #include <dynamic-graph/linear-algebra.h>
 
 namespace dynamicgraph
@@ -41,6 +40,7 @@ namespace dynamicgraph
   // To workaround this problem, parse special values manually
   // (the strings used are the one produces by displaying special
   // values on a stream).
+
   template <>
   inline boost::any
   DefaultCastRegisterer<double>::cast (std::istringstream& iss)
@@ -68,13 +68,14 @@ namespace dynamicgraph
     }
 
   /* Specialize Matrix and Vector traces. */
+
   template <>
   void
   DefaultCastRegisterer<dynamicgraph::Vector>::
   trace(const boost::any& object, std::ostream& os)
   {
     const dynamicgraph::Vector & v = boost::any_cast<dynamicgraph::Vector> (object);
-    for( unsigned int i=0;i<v.size();++i )
+    for( int i=0;i<v.size();++i )
       { os << "\t" << v(i); }
   }
   template <>
@@ -83,13 +84,12 @@ namespace dynamicgraph
   trace(const boost::any& object, std::ostream& os)
   {
     const dynamicgraph::Matrix & m = boost::any_cast<dynamicgraph::Matrix> (object);
-    for( unsigned int i=0;i<m.nbRows();++i )
-      for( unsigned int j=0;j<m.nbCols();++j )
+    for(int i=0;i<m.rows();++i )
+      for(int j=0;j<m.cols();++j )
 	{ os << "\t" << m(i,j); }
   }
 
 
-
   /// Registers useful casts
   namespace
   {
diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
index 723b69b..4d14693 100644
--- a/tests/CMakeLists.txt
+++ b/tests/CMakeLists.txt
@@ -43,12 +43,6 @@ MACRO(DYNAMIC_GRAPH_TEST NAME)
   # Link against Boost.
   TARGET_LINK_LIBRARIES(${NAME} ${Boost_LIBRARIES})
 
-  # Search for jrl-mal.
-  INCLUDE_DIRECTORIES(${JRL_MAL_INCLUDE_DIRS})
-  LINK_DIRECTORIES(${JRL_MAL_LIBRARY_DIRS})
-  IF(UNIX AND NOT APPLE)
-    TARGET_LINK_LIBRARIES(${NAME} ${JRL_MAL_LDFLAGS})
-  ENDIF(UNIX AND NOT APPLE)
 ENDMACRO(DYNAMIC_GRAPH_TEST)
 
 
diff --git a/tests/signal-cast-registerer-libA.hh b/tests/signal-cast-registerer-libA.hh
index 7dcdd3b..f0065e2 100644
--- a/tests/signal-cast-registerer-libA.hh
+++ b/tests/signal-cast-registerer-libA.hh
@@ -13,8 +13,7 @@
 // You should have received a copy of the GNU Lesser General Public License
 // along with dynamic-graph.  If not, see <http://www.gnu.org/licenses/>.
 
-#include <boost/numeric/ublas/vector.hpp>
-#include <boost/numeric/ublas/io.hpp>
+#include <Eigen/Dense>
 
-typedef boost::numeric::ublas::vector<double> vec_type;
+typedef Eigen::VectorXd vec_type;
 extern vec_type vA;
diff --git a/tests/signal-cast-registerer-libB.hh b/tests/signal-cast-registerer-libB.hh
index ea04629..1c600a5 100644
--- a/tests/signal-cast-registerer-libB.hh
+++ b/tests/signal-cast-registerer-libB.hh
@@ -13,8 +13,7 @@
 // You should have received a copy of the GNU Lesser General Public License
 // along with dynamic-graph.  If not, see <http://www.gnu.org/licenses/>.
 
-#include <boost/numeric/ublas/vector.hpp>
-#include <boost/numeric/ublas/io.hpp>
+#include <Eigen/Core>
 
-typedef boost::numeric::ublas::vector<double> vec_type;
+typedef Eigen::VectorXd vec_type;
 extern vec_type vB;
diff --git a/tests/signal-cast-registerer.cpp b/tests/signal-cast-registerer.cpp
index cc61c49..78dba1e 100644
--- a/tests/signal-cast-registerer.cpp
+++ b/tests/signal-cast-registerer.cpp
@@ -17,13 +17,15 @@
 
 #include <boost/foreach.hpp>
 #include <boost/format.hpp>
-#include <boost/numeric/ublas/vector.hpp>
-#include <boost/numeric/ublas/io.hpp>
+
+#include <Eigen/Dense>
 
 #include <dynamic-graph/debug.h>
 #include <dynamic-graph/entity.h>
 #include <dynamic-graph/factory.h>
 #include <dynamic-graph/pool.h>
+#include <dynamic-graph/eigen-io.h>
+#include <dynamic-graph/linear-algebra.h>
 #include <dynamic-graph/signal-caster.h>
 #include <dynamic-graph/signal.h>
 #include <dynamic-graph/signal-cast-helper.h>
@@ -39,14 +41,14 @@
 using boost::test_tools::output_test_stream;
 
 
-typedef boost::numeric::ublas::vector<double> Vector;
+typedef Eigen::VectorXd Vector;
 
 
-struct BoostNumericsCastRegisterer : public dynamicgraph::SignalCastRegisterer
+struct EigenCastRegisterer : public dynamicgraph::SignalCastRegisterer
 {
-  typedef boost::numeric::ublas::vector<double> bnuVector;
+  typedef Vector bnuVector;
 
-  BoostNumericsCastRegisterer () :
+  EigenCastRegisterer () :
     SignalCastRegisterer
     (typeid(bnuVector), dispVector, castVector, traceVector)
   {}
@@ -62,7 +64,7 @@ struct BoostNumericsCastRegisterer : public dynamicgraph::SignalCastRegisterer
   {
     const bnuVector& v = boost::any_cast<bnuVector> (object);
     os << "[ ";
-    for (unsigned int i = 0; i < v.size (); ++i)
+    for (int i = 0; i < v.size (); ++i)
       os << v(i) << " ";
     os << " ];" << std::endl;
   }
@@ -70,13 +72,13 @@ struct BoostNumericsCastRegisterer : public dynamicgraph::SignalCastRegisterer
   static void traceVector (const boost::any& object, std::ostream& os)
   {
     const bnuVector& v = boost::any_cast<bnuVector> (object);
-    for (unsigned int i = 0; i < v.size (); ++i)
+    for (int i = 0; i < v.size (); ++i)
       os << v(i) << " ";
     os << std::endl;
   }
 };
 
-BoostNumericsCastRegisterer myVectorCast;
+EigenCastRegisterer myVectorCast;
 
 // Define a new cast with a type that supports streaming operators to
 // and from it (this could be automated with macros).
@@ -132,9 +134,8 @@ BOOST_AUTO_TEST_CASE (standard_double_registerer)
 // Check a custom cast registerer for Boost uBLAS vectors.
 BOOST_AUTO_TEST_CASE (custom_vector_registerer)
 {
-  namespace ublas = boost::numeric::ublas;
 
-  dynamicgraph::Signal<Vector, int> myVectorSignal("vector");
+  dynamicgraph::Signal<dynamicgraph::Vector, int> myVectorSignal("vector");
 
   // Print the signal name.
   {
@@ -145,7 +146,7 @@ BOOST_AUTO_TEST_CASE (custom_vector_registerer)
 
   for (unsigned int i = 0; i < 5; ++i)
     {
-      ublas::unit_vector<double> v (5, i);
+      Vector  v = Vector::Unit(5,i) ;
       std::ostringstream os;
       os << v;
       std::istringstream ss (os.str ());
-- 
GitLab