Main MRPT website > C++ reference for MRPT 1.4.0
eigen_plugins_impl.h
Go to the documentation of this file.
1/* +---------------------------------------------------------------------------+
2 | Mobile Robot Programming Toolkit (MRPT) |
3 | http://www.mrpt.org/ |
4 | |
5 | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file |
6 | See: http://www.mrpt.org/Authors - All rights reserved. |
7 | Released under BSD License. See details in http://www.mrpt.org/License |
8 +---------------------------------------------------------------------------+ */
9
10#ifndef MRPT_EIGEN_PLUGINS_IMPL_H
11#define MRPT_EIGEN_PLUGINS_IMPL_H
12
13// -------------------------------------------------------------------------
14// This file implements some templates which had to be left only declared
15// in the "plug-in" headers "eigen_plugins.h" within Eigen::MatrixBase<>
16// -------------------------------------------------------------------------
17
19{
20 // Generic version for all kind of matrices:
21 template<int R, int C>
23 {
24 template <typename S, int Opt, int MaxR, int MaxC>
25 static inline void doit(Eigen::Matrix<S,R,C,Opt,MaxR,MaxC> &mat, size_t new_rows,size_t new_cols)
26 {
27 ::mrpt::math::detail::TAuxResizer<Eigen::Matrix<S,R,C,Opt,MaxR,MaxC>,Eigen::Matrix<S,R,C,Opt,MaxR,MaxC>::SizeAtCompileTime>::internal_resize(mat,new_rows,new_cols);
28 }
29 };
30 // Specialization for column matrices:
31 template<int R>
32 struct MatOrVecResizer<R,1>
33 {
34 template <typename S, int Opt, int MaxR, int MaxC>
35 static inline void doit(Eigen::Matrix<S,R,1,Opt,MaxR,MaxC> &mat, size_t new_rows,size_t )
36 {
37 ::mrpt::math::detail::TAuxResizer<Eigen::Matrix<S,R,1,Opt,MaxR,MaxC>,Eigen::Matrix<S,R,1,Opt,MaxR,MaxC>::SizeAtCompileTime>::internal_resize(mat,new_rows);
38 }
39 };
40 // Specialization for row matrices:
41 template<int C>
42 struct MatOrVecResizer<1,C>
43 {
44 template <typename S, int Opt, int MaxR, int MaxC>
45 static inline void doit(Eigen::Matrix<S,1,C,Opt,MaxR,MaxC> &mat, size_t ,size_t new_cols)
46 {
47 ::mrpt::math::detail::TAuxResizer<Eigen::Matrix<S,1,C,Opt,MaxR,MaxC>,Eigen::Matrix<S,1,C,Opt,MaxR,MaxC>::SizeAtCompileTime>::internal_resize(mat,new_cols);
48 }
49 };
50 template<>
51 struct MatOrVecResizer<1,1>
52 {
53 template <typename S, int Opt, int MaxR, int MaxC>
54 static inline void doit(Eigen::Matrix<S,1,1,Opt,MaxR,MaxC> &mat, size_t ,size_t new_cols)
55 {
56 ::mrpt::math::detail::TAuxResizer<Eigen::Matrix<S,1,1,Opt,MaxR,MaxC>,Eigen::Matrix<S,1,1,Opt,MaxR,MaxC>::SizeAtCompileTime>::internal_resize(mat,new_cols);
57 }
58 };
59}
60
61/** Compute the eigenvectors and eigenvalues, both returned as matrices: eigenvectors are the columns, and eigenvalues
62 */
63template <class Derived>
64template <class MATRIX1,class MATRIX2>
65EIGEN_STRONG_INLINE void Eigen::MatrixBase<Derived>::eigenVectors( MATRIX1 & eVecs, MATRIX2 & eVals ) const
66{
67 Matrix<Scalar,Dynamic,1> evals;
68 eigenVectorsVec(eVecs,evals);
69 eVals.resize(evals.size(),evals.size());
70 eVals.setZero();
71 eVals.diagonal()=evals;
72}
73
74/** Compute the eigenvectors and eigenvalues, both returned as matrices: eigenvectors are the columns, and eigenvalues
75 */
76template <class Derived>
77template <class MATRIX1,class VECTOR1>
78EIGEN_STRONG_INLINE void Eigen::MatrixBase<Derived>::eigenVectorsVec( MATRIX1 & eVecs, VECTOR1 & eVals ) const
79{
80 Eigen::EigenSolver< Derived > es(*this, true);
81 eVecs = es.eigenvectors().real(); // Keep only the real part of complex matrix
82 eVals = es.eigenvalues().real(); // Keep only the real part of complex matrix
83
84 // Sort by ascending eigenvalues:
85 std::vector<std::pair<Scalar,Index> > D;
86 D.reserve(eVals.size());
87 for (Index i=0;i<eVals.size();i++)
88 D.push_back(std::pair<Scalar,Index>(eVals.coeff(i,0),i));
89 std::sort(D.begin(),D.end());
90 MATRIX1 sortedEigs;
91 sortedEigs.resizeLike(eVecs);
92 for (int i=0;i<eVals.size();i++)
93 {
94 eVals.coeffRef(i,0)=D[i].first;
95 sortedEigs.col(i)=eVecs.col(D[i].second);
96 }
97 eVecs = sortedEigs;
98}
99
100/** Compute the eigenvectors and eigenvalues, both returned as matrices: eigenvectors are the columns, and eigenvalues
101 */
102template <class Derived>
103template <class MATRIX1,class MATRIX2>
104EIGEN_STRONG_INLINE void Eigen::MatrixBase<Derived>::eigenVectorsSymmetric( MATRIX1 & eVecs, MATRIX2 & eVals ) const
105{
106 Matrix<Scalar,Dynamic,1> evals;
107 eigenVectorsSymmetricVec(eVecs,evals);
108 eVals.resize(evals.size(),evals.size());
109 eVals.setZero();
110 eVals.diagonal()=evals;
111}
112
113/** Compute the eigenvectors and eigenvalues, both returned as matrices: eigenvectors are the columns, and eigenvalues
114 */
115template <class Derived>
116template <class MATRIX1,class VECTOR1>
117EIGEN_STRONG_INLINE void Eigen::MatrixBase<Derived>::eigenVectorsSymmetricVec( MATRIX1 & eVecs, VECTOR1 & eVals ) const
118{
119 // This solver returns the eigenvectors already sorted.
120 Eigen::SelfAdjointEigenSolver<Derived> eigensolver(*this);
121 eVecs = eigensolver.eigenvectors();
122 eVals = eigensolver.eigenvalues();
123}
124
125
126template <class Derived>
127bool Eigen::MatrixBase<Derived>::fromMatlabStringFormat(const std::string &s, std::ostream *dump_errors_here)
128{
129 // Start with a (0,0) matrix:
130 if ( Derived::RowsAtCompileTime==Eigen::Dynamic)
131 (*this) = Derived();
132
133 // Look for starting "[".
134 size_t ini = s.find_first_not_of(" \t\r\n");
135 if (ini==std::string::npos || s[ini]!='[') { return false; }
136
137 size_t end = s.find_last_not_of(" \t\r\n");
138 if (end==std::string::npos || s[end]!=']') return false;
139
140 if (ini>end) return false;
141
142 std::vector<Scalar> lstElements;
143
144 size_t i = ini+1;
145 size_t nRow = 0;
146
147 while (i<end)
148 {
149 // Extract one row:
150 size_t end_row = s.find_first_of(";]",i);
151 if (end_row==std::string::npos) { return false; }
152
153 // We have one row in s[ i : (end_row-1) ]
154 std::stringstream ss(s.substr(i, end_row-i ));
155 lstElements.clear();
156 try
157 {
158 while (!ss.eof())
159 {
160 Scalar val;
161 ss >> val;
162 if (ss.bad() || ss.fail()) break;
163 lstElements.push_back(val);
164 }
165 } catch (...) { } // end of line
166
167 // Empty row? Only for the first row, then this is an empty matrix:
168 if (lstElements.empty())
169 {
170 if (nRow>0)
171 return false;
172 else
173 {
174 // Else, this may be an empty matrix... if there is no next row, we'll return with a (0,0) matrix
175 if ( Derived::RowsAtCompileTime==Eigen::Dynamic )
176 (*this) = Derived();
177 }
178 }
179 else
180 {
181 const size_t N = lstElements.size();
182
183 // Check valid width: All rows must have the same width
184 if ((nRow>0 && size_t(cols())!=N) ||
185 (nRow==0 && Derived::ColsAtCompileTime!=Eigen::Dynamic && Derived::ColsAtCompileTime!=int(N)) )
186 {
187 if (dump_errors_here)
188 (*dump_errors_here) << "[fromMatlabStringFormat] Row " << nRow+1 << " has invalid number of columns.\n";
189 return false;
190 }
191
192 // Append to the matrix:
193 if ( Derived::RowsAtCompileTime==Eigen::Dynamic || Derived::ColsAtCompileTime==Eigen::Dynamic )
195 else if (Derived::RowsAtCompileTime!=Eigen::Dynamic && int(nRow)>=Derived::RowsAtCompileTime)
196 {
197 if (dump_errors_here)
198 (*dump_errors_here) << "[fromMatlabStringFormat] Read more rows than the capacity of the fixed sized matrix.\n";
199 return false;
200 }
201
202 for (size_t q=0;q<N;q++)
203 coeffRef(nRow,q) = lstElements[q];
204
205 // Go for the next row:
206 nRow++;
207 }
208
209 i = end_row+1;
210 }
211 // For fixed sized matrices, check size:
212 if (Derived::RowsAtCompileTime!=Eigen::Dynamic && int(nRow)!=Derived::RowsAtCompileTime)
213 {
214 if (dump_errors_here)
215 (*dump_errors_here) << "[fromMatlabStringFormat] Read less rows than the capacity of the fixed sized matrix.\n";
216 return false;
217 }
218 return true; // Ok
219}
220
221template <class Derived>
222std::string Eigen::MatrixBase<Derived>::inMatlabFormat(const size_t decimal_digits) const
223{
224 std::stringstream s;
225 s << "[" << std::scientific;
226 s.precision(decimal_digits);
227 for (Index i=0;i<rows();i++)
228 {
229 for (Index j=0;j<cols();j++)
230 s << coeff(i,j) << " ";
231 if (i<rows()-1) s << ";";
232 }
233 s << "]";
234 return s.str();
235}
236
237template <class Derived>
238void Eigen::MatrixBase<Derived>::saveToTextFile(
239 const std::string &file,
241 bool appendMRPTHeader,
242 const std::string &userHeader
243 ) const
244{
245#if defined(_MSC_VER) && (_MSC_VER>=1400) // Use a secure version in Visual Studio 2005+
246 FILE *f;
247 if (0!=::fopen_s(&f,file.c_str(),"wt")) f= NULL;
248#else
249 FILE *f= ::fopen(file.c_str(),"wt");
250#endif
251 if (!f)
252 throw std::runtime_error(std::string("saveToTextFile: Error opening file ")+file+std::string("' for writing a matrix as text."));
253
254 if (!userHeader.empty())
255 fprintf(f,"%s",userHeader.c_str() );
256
257 if (appendMRPTHeader)
258 {
259 time_t rawtime;
260 ::time(&rawtime);
261#if defined(_MSC_VER) && (_MSC_VER>=1400) // Use a secure version in Visual Studio 2005+
262 struct tm timeinfo_data;
263 struct tm * timeinfo;
264 if (0!=::localtime_s(&timeinfo_data,&rawtime)) timeinfo=NULL;
265 else timeinfo = &timeinfo_data;
266#else
267 struct tm * timeinfo = ::localtime(&rawtime);
268#endif
269
270#if defined(_MSC_VER) && (_MSC_VER>=1400) // Use a secure version in Visual Studio 2005+
271 char strTimeBuf[100];
272 if (0!=asctime_s(strTimeBuf,sizeof(strTimeBuf),timeinfo)) strTimeBuf[0]='\0';
273 char *strTime = &strTimeBuf[0];
274#else
275 char *strTime = asctime(timeinfo);
276#endif
277 fprintf(f,"%% File generated with %s at %s\n%%-----------------------------------------------------------------\n",
279 strTime);
280 }
281
282 for (Index i=0; i < rows(); i++)
283 {
284 for (Index j=0; j < cols(); j++)
285 {
286 switch(fileFormat)
287 {
288 case mrpt::math::MATRIX_FORMAT_ENG: ::fprintf(f,"%.16e",static_cast<double>(coeff(i,j))); break;
289 case mrpt::math::MATRIX_FORMAT_FIXED: ::fprintf(f,"%.16f",static_cast<double>(coeff(i,j))); break;
290 case mrpt::math::MATRIX_FORMAT_INT: ::fprintf(f,"%i",static_cast<int>(coeff(i,j))); break;
291 default:
292 throw std::runtime_error("Unsupported value for the parameter 'fileFormat'!");
293 };
294 // Separating blank space
295 if (j<(cols()-1)) ::fprintf(f," ");
296 }
297 ::fprintf(f,"\n");
298 }
299 ::fclose(f);
300}
301
302
303template <class Derived>
304void Eigen::MatrixBase<Derived>::loadFromTextFile(const std::string &file)
305{
306 std::ifstream f(file.c_str());
307 if (f.fail()) throw std::runtime_error(std::string("loadFromTextFile: can't open file:") + file);
309}
310
311template <class Derived>
312void Eigen::MatrixBase<Derived>::loadFromTextFile(std::istream &f)
313{
314 // This matrix is NROWS x NCOLS
315 std::string str;
316 std::vector<double> fil(512);
317 size_t nRows = 0;
318 while ( !f.eof() && !f.fail() )
319 {
320 std::getline(f,str);
321 if (str.size() && str[0]!='#' && str[0]!='%')
322 {
323 // Parse row to floats:
324 const char *ptr = str.c_str();
325 char *ptrEnd = NULL;
326 size_t i=0;
327 // Process each number in this row:
328 while ( ptr[0] && ptr!=ptrEnd )
329 {
330 // Find next number: (non white-space character):
331 while (ptr[0] && (ptr[0]==' ' || ptr[0]==',' || ptr[0]=='\t' || ptr[0]=='\r' || ptr[0]=='\n'))
332 ptr++;
333 if (fil.size()<=i) fil.resize(fil.size()+ (fil.size()>>1));
334 // Convert to "double":
335 fil[i] = strtod(ptr,&ptrEnd);
336 // A valid conversion has been done?
337 if (ptr!=ptrEnd)
338 {
339 i++; // Yes
340 ptr = ptrEnd;
341 ptrEnd = NULL;
342 }
343 }; // end while procesing this row
344
345 // "i": # of columns:
346 if ((Derived::ColsAtCompileTime!=Eigen::Dynamic && Index(i)!=Derived::ColsAtCompileTime) )
347 throw std::runtime_error("loadFromTextFile: The matrix in the text file does not match fixed matrix size");
348 if (Derived::ColsAtCompileTime==Eigen::Dynamic && nRows>0 && Index(i)!=cols() )
349 throw std::runtime_error("loadFromTextFile: The matrix in the text file does not have the same number of columns in all rows");
350
351 // Append to the matrix:
352 if ( Derived::RowsAtCompileTime==Eigen::Dynamic || Derived::ColsAtCompileTime==Eigen::Dynamic )
353 {
354 if (rows()<static_cast<int>(nRows+1) || cols()<static_cast<int>(i))
355 {
356 const size_t extra_rows = std::max(static_cast<size_t>(1), nRows >> 1 );
358 }
359 }
360 else if (Derived::RowsAtCompileTime!=Eigen::Dynamic && int(nRows)>=Derived::RowsAtCompileTime)
361 throw std::runtime_error("loadFromTextFile: Read more rows than the capacity of the fixed sized matrix.");
362
363 for (size_t q=0;q<i;q++)
364 coeffRef(nRows,q) = Scalar(fil[q]);
365
366 nRows++;
367 } // end if fgets
368 } // end while not feof
369
370 // Final resize to the real size (in case we allocated space in advance):
371 if ( Derived::RowsAtCompileTime==Eigen::Dynamic || Derived::ColsAtCompileTime==Eigen::Dynamic )
373
374 // Report error as exception
375 if (!nRows) throw std::runtime_error("loadFromTextFile: Error loading from text file");
376}
377
378
379#endif // guard define
EIGEN_STRONG_INLINE void eigenVectorsSymmetricVec(MATRIX1 &eVecs, VECTOR1 &eVals) const
[For symmetric matrices only] Compute the eigenvectors and eigenvalues (in no particular order),...
void loadFromTextFile(const std::string &file)
Load matrix from a text file, compatible with MATLAB text format.
EIGEN_STRONG_INLINE iterator end()
Definition: eigen_plugins.h:27
EIGEN_STRONG_INLINE void eigenVectorsVec(MATRIX1 &eVecs, VECTOR1 &eVals) const
[For square matrices only] Compute the eigenvectors and eigenvalues (sorted), eigenvectors are the co...
int BASE_IMPEXP fprintf(FILE *fil, const char *format,...) MRPT_NO_THROWS MRPT_printf_format_check(2
An OS-independent version of fprintf.
std::string BASE_IMPEXP MRPT_getVersion()
Returns a string describing the MRPT version.
TMatrixTextFileFormat
Definition: math_frwds.h:65
@ MATRIX_FORMAT_ENG
engineering format 'e'
Definition: math_frwds.h:66
@ MATRIX_FORMAT_FIXED
fixed floating point 'f'
Definition: math_frwds.h:67
@ MATRIX_FORMAT_INT
intergers 'i'
Definition: math_frwds.h:68
static void doit(Eigen::Matrix< S, 1, 1, Opt, MaxR, MaxC > &mat, size_t, size_t new_cols)
static void doit(Eigen::Matrix< S, 1, C, Opt, MaxR, MaxC > &mat, size_t, size_t new_cols)
static void doit(Eigen::Matrix< S, R, 1, Opt, MaxR, MaxC > &mat, size_t new_rows, size_t)
static void doit(Eigen::Matrix< S, R, C, Opt, MaxR, MaxC > &mat, size_t new_rows, size_t new_cols)
Internal resize which compiles to nothing on fixed-size matrices.
Definition: math_frwds.h:50



Page generated by Doxygen 1.9.5 for MRPT 1.4.0 SVN: at Sun Nov 27 03:17:04 UTC 2022