Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/dynamic-graph/eigen-io.h	Lines:	49	49	100.0 %
Date:	2023-03-15 12:04:10	Branches:	162	250	64.8 %


//
// Copyright 2016 CNRS
//
// Author: Rohan Budhiraja
//

#ifndef DYNAMIC_GRAPH_EIGEN_IO_H
#define DYNAMIC_GRAPH_EIGEN_IO_H

#include <dynamic-graph/exception-signal.h>
#include <dynamic-graph/linear-algebra.h>

#include <Eigen/Geometry>
#include <boost/format.hpp>
#include <boost/numeric/conversion/cast.hpp>

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: [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) {
  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);
    } 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;
}

/* \brief Eigen Matrix input from istream
 *
 * 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) {
  unsigned int _colsize;
  unsigned int _rowsize;
  double _dbl_val;
  char _ch;
  boost::format fmt(
      "Failed to enter %s as matrix. Reenter as "
      "((val11,val12,val13,...,val1N),"
      "...,(valM1,valM2,...,valMN))");
  MatrixXd _tmp_matrix;
  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 {
      _tmp_matrix.resize(_rowsize, _colsize);
      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 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();
              _tmp_matrix(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());
        }
      }
    }
  }
  inst = _tmp_matrix;
  return iss;
}

inline std::istringstream &operator>>(std::istringstream &iss,
                                      Transform<double, 3, Affine> &inst) {
  MatrixXd M;
  iss >> M;
  inst.matrix() = M;
  return iss;
}

/* \brief Eigen Homogeneous Matrix output
 *
 * 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))
 */

inline std::ostream &operator<<(std::ostream &os,
                                Transform<double, 3, Affine> MH) {
  IOFormat boostFmt(StreamPrecision, DontAlignCols, ",", ",", "(", ")", "(",
                    ")");

  os << "[4,4]" << MH.matrix().format(boostFmt);
  return os;
}

inline std::ostream &operator<<(std::ostream &os, AngleAxisd quat) {
  VectorXd v(4);
  v(0) = quat.angle();
  v.tail<3>() = quat.axis();
  os << v;
  return os;
}

inline std::istringstream &operator>>(std::istringstream &iss,
                                      AngleAxisd &inst) {
  VectorXd v(4);
  iss >> v;
  inst.angle() = v(0);
  inst.axis() = v.tail<3>();
  return iss;
}

}  // namespace Eigen

#endif  // DYNAMIC_GRAPH_EIGEN_IO_H


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			//
2			// Copyright 2016 CNRS
3			//
4			// Author: Rohan Budhiraja
5			//
6
7			#ifndef DYNAMIC_GRAPH_EIGEN_IO_H
8			#define DYNAMIC_GRAPH_EIGEN_IO_H
9
10			#include <dynamic-graph/exception-signal.h>
11			#include <dynamic-graph/linear-algebra.h>
12
13			#include <Eigen/Geometry>
14			#include <boost/format.hpp>
15			#include <boost/numeric/conversion/cast.hpp>
16
17			using dynamicgraph::ExceptionSignal;
18
19			// TODO: Eigen 3.3 onwards has a global Eigen::Index definition.
20			// If Eigen version is updated, use Eigen::Index instead of this macro.
21
22			/* \brief Eigen Vector input from istream
23			*
24			* Input Vector format: [N](val1,val2,val3,...,valN)
25			* e.g. [5](1,23,32.2,12.12,32)
26			*/
27			namespace Eigen {
28			typedef EIGEN_DEFAULT_DENSE_INDEX_TYPE eigen_index;
29
30		11	inline std::istringstream &operator>>(std::istringstream &iss,
31			dynamicgraph::Vector &inst) {
32			unsigned int _size;
33			double _dbl_val;
34			char _ch;
35			boost::format fmt(
36			"Failed to enter %s as vector."
37	✓✗	16	" Reenter as [N](val1,val2,val3,...,valN)");
38	✓✗✓✗	11	fmt % iss.str();
39	✓✗✓✗ ✓✗✓✓ ✓✓	11	if (iss >> _ch && _ch != '[') {
40	✓✗✓✗	1	throw ExceptionSignal(ExceptionSignal::GENERIC, fmt.str());
41			} else {
42	✓✗✓✗ ✓✓✓✗ ✓✗✓✓	10	if (iss >> _size && !iss.fail()) {
43	✓✗	9	inst.resize(_size);
44			} else
45	✓✗✓✗	1	throw ExceptionSignal(ExceptionSignal::GENERIC, fmt.str());
46	✓✗✓✗ ✓✗✓✓ ✓✓	9	if (iss >> _ch && _ch != ']')
47	✓✗✓✗	1	throw ExceptionSignal(ExceptionSignal::GENERIC, fmt.str());
48			else {
49	✓✗✓✗ ✓✗✓✓ ✓✓	8	if (iss >> _ch && _ch != '(')
50	✓✗✓✗	1	throw ExceptionSignal(ExceptionSignal::GENERIC, fmt.str());
51			else {
52	✓✓	42	for (unsigned int i = 0; i < _size; i++) {
53	✓✗	35	iss >> _dbl_val;
54	✓✗✓✓ ✓✗✗✓ ✓✓✓✗	35	if (iss.peek() == ',' \|\| iss.peek() == ' ') iss.ignore();
55	✓✗	35	inst(i) = _dbl_val;
56			}
57	✓✗✓✗ ✓✓✓✓ ✓✓	7	if (iss >> _ch && _ch != ')')
58	✓✗✓✗	1	throw ExceptionSignal(ExceptionSignal::GENERIC, fmt.str());
59			}
60			}
61			}
62		12	return iss;
63			}
64
65			/* \brief Eigen Matrix input from istream
66			*
67			* Matrix format: [M,N]((val11,val12,val13,...,val1N),...,
68			* (valM1,valM2,...,valMN))
69			* e.g. [2,5]((1 23 32.2 12.12 32),(2 32 23 92.01 19.2))
70			*/
71
72			template <typename Derived>
73		12	inline std::istringstream &operator>>(std::istringstream &iss,
74			DenseBase<Derived> &inst) {
75			unsigned int _colsize;
76			unsigned int _rowsize;
77			double _dbl_val;
78			char _ch;
79	✓✗	24	boost::format fmt(
80			"Failed to enter %s as matrix. Reenter as "
81			"((val11,val12,val13,...,val1N),"
82			"...,(valM1,valM2,...,valMN))");
83	✓✗	23	MatrixXd _tmp_matrix;
84	✓✗✓✗	12	fmt % iss.str();
85	✓✗✓✗ ✓✗✓✓ ✓✓	12	if (iss >> _ch && _ch != '[') {
86	✓✗✓✗	1	throw ExceptionSignal(ExceptionSignal::GENERIC, fmt.str());
87			} else {
88	✓✗	11	iss >> _rowsize;
89	✓✗✓✓ ✓✗✗✓ ✓✓✓✗	11	if (iss.peek() == ',' \|\| iss.peek() == ' ') iss.ignore();
90	✓✗	11	iss >> _colsize;
91	✓✗✓✓	11	if (iss.fail())
92	✓✗✓✗	3	throw ExceptionSignal(ExceptionSignal::GENERIC, fmt.str());
93			else {
94	✓✗	8	_tmp_matrix.resize(_rowsize, _colsize);
95	✓✗✓✗ ✓✗✓✓ ✓✓	8	if (iss >> _ch && _ch != ']')
96	✓✗✓✗	1	throw ExceptionSignal(ExceptionSignal::GENERIC, fmt.str());
97			else {
98	✓✗✓✗ ✓✗✓✓ ✓✓	7	if (iss >> _ch && _ch != '(')
99	✓✗✓✗	1	throw ExceptionSignal(ExceptionSignal::GENERIC, fmt.str());
100			else {
101	✓✓	19	for (unsigned int j = 0; j < _rowsize; j++) {
102	✓✗✓✗ ✓✓✓✓ ✓✓	17	if (iss >> _ch && _ch != '(')
103	✓✗✓✗	2	throw ExceptionSignal(ExceptionSignal::GENERIC, fmt.str());
104	✓✓	42	for (unsigned int i = 0; i < _colsize; i++) {
105	✓✗	27	iss >> _dbl_val;
106	✓✗✓✓ ✓✗✗✓ ✓✓✓✗	27	if (iss.peek() == ',' \|\| iss.peek() == ' ') iss.ignore();
107	✓✗	27	_tmp_matrix(j, i) = _dbl_val;
108			}
109	✓✗✓✗ ✓✓✓✓ ✓✓	15	if (iss >> _ch && _ch != ')')
110	✓✗✓✗	2	throw ExceptionSignal(ExceptionSignal::GENERIC, fmt.str());
111	✓✗✓✓ ✓✗✗✓ ✓✓✓✗	13	if (iss.peek() == ',' \|\| iss.peek() == ' ') iss.ignore();
112			}
113	✓✗✓✗ ✓✓✓✗ ✓✓	2	if (iss >> _ch && _ch != ')')
114	✓✗✓✗	1	throw ExceptionSignal(ExceptionSignal::GENERIC, fmt.str());
115			}
116			}
117			}
118			}
119	✓✗	1	inst = _tmp_matrix;
120		2	return iss;
121			}
122
123			inline std::istringstream &operator>>(std::istringstream &iss,
124			Transform<double, 3, Affine> &inst) {
125			MatrixXd M;
126			iss >> M;
127			inst.matrix() = M;
128			return iss;
129			}
130
131			/* \brief Eigen Homogeneous Matrix output
132			*
133			* Matrix format: [M,N]((val11,val12,val13,...,val1N),...,
134			* (valM1,valM2,...,valMN))
135			* e.g. [2,5]((1 23 32.2 12.12 32),(2 32 23 92.01 19.2))
136			*/
137
138			inline std::ostream &operator<<(std::ostream &os,
139			Transform<double, 3, Affine> MH) {
140			IOFormat boostFmt(StreamPrecision, DontAlignCols, ",", ",", "(", ")", "(",
141			")");
142
143			os << "[4,4]" << MH.matrix().format(boostFmt);
144			return os;
145			}
146
147			inline std::ostream &operator<<(std::ostream &os, AngleAxisd quat) {
148			VectorXd v(4);
149			v(0) = quat.angle();
150			v.tail<3>() = quat.axis();
151			os << v;
152			return os;
153			}
154
155			inline std::istringstream &operator>>(std::istringstream &iss,
156			AngleAxisd &inst) {
157			VectorXd v(4);
158			iss >> v;
159			inst.angle() = v(0);
160			inst.axis() = v.tail<3>();
161			return iss;
162			}
163
164			} // namespace Eigen
165
166			#endif // DYNAMIC_GRAPH_EIGEN_IO_H