Commit 55c51cbd authored by Rohan Budhiraja's avatar Rohan Budhiraja Committed by Olivier Stasse

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

modify dg::Vector and dg::Matrix istream operators.
parent bfafccbe
......@@ -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
......
//
// 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; }
}
......
......@@ -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);
......
Markdown is supported
0%
or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment