qpmad
Eigen-based C++ QP solver.
eigenut_misc.h
Go to the documentation of this file.
1 /**
2  @file
3  @author Alexander Sherikov
4  @author Jan Michalczyk
5  @copyright 2019 Alexander Sherikov. Licensed under the Apache License, Version 2.0.
6  @copyright 2014-2017 INRIA. Licensed under the Apache License, Version 2.0.
7  (see @ref LICENSE or http://www.apache.org/licenses/LICENSE-2.0)
8 
9  @brief
10 */
11 
12 
13 #ifndef H_QPMAD_UTILS_MISC
14 #define H_QPMAD_UTILS_MISC
15 
16 #include <vector>
17 
18 
19 /**
20  * @page eigenut eigenut
21  *
22  * @anchor eigenut_casting_hack
23  *
24  * @note Since an attempt to write to a temporary object may result in a
25  * compilation error, it is not possible to use the result of matrix.block() as
26  * an output parameter of a function. However, there is a workaround: the
27  * parameter can be declared as const and then casted to non-const as suggested
28  * in Eigen's documentation:
29  * https://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html.
30  * This hack is useful when it is necessary to handle matrix blocks and
31  * matrices identically.
32  */
33 
34 
35 /**
36  * @defgroup eigenut eigenut
37  * @brief Utility functions based on Eigen.
38  */
39 
40 /// @ingroup eigenut
41 namespace qpmad_utils
42 {
43  inline void getRandomPositiveDefinititeMatrix(Eigen::MatrixXd &M, const std::size_t size)
44  {
45  M.setRandom(size, size);
46  M = M.transpose()*M + Eigen::MatrixXd::Identity(size, size);
47  }
48 
49 
50  /**
51  * @brief Unset matrix (initialize to NaN)
52  *
53  * @tparam t_Derived matrix data type
54  *
55  * @param[in,out] matrix
56  */
57  template <typename t_Derived>
58  void unsetMatrix (Eigen::DenseBase< t_Derived > &matrix)
59  {
60  if (std::numeric_limits<qpmad_utils::DefaultScalar>::has_quiet_NaN)
61  {
62  matrix.setConstant(std::numeric_limits<qpmad_utils::DefaultScalar>::quiet_NaN());
63  }
64  else
65  {
66  matrix.setZero();
67  }
68  }
69 
70 
71  /**
72  * @brief result = A^T * A
73  *
74  * @tparam t_DerivedOutput Eigen parameter
75  * @tparam t_DerivedInput Eigen parameter
76  *
77  * @param[out] result
78  * @param[in] A
79  *
80  * @attention Only the left lower triangular part of the result is initialized.
81  */
82  template<class t_DerivedOutput, class t_DerivedInput>
84  getATA( Eigen::PlainObjectBase<t_DerivedOutput> &result,
85  const Eigen::DenseBase<t_DerivedInput> &A)
86  {
87  std::ptrdiff_t num_el = A.cols();
88  result.resize(num_el,num_el);
89  for (std::ptrdiff_t i = 0; i < num_el; ++i)
90  {
91  result.block(i, i, num_el-i, 1).noalias() = A.transpose().bottomRows(num_el-i) * A.col(i);
92  }
93  }
94 
95 
96  /**
97  * @brief result.diagonalBlock() = A^T * A
98  *
99  * @tparam t_DerivedOutput Eigen parameter
100  * @tparam t_DerivedInput Eigen parameter
101  *
102  * @param[out] result
103  * @param[in] A
104  * @param[in] offset offset of A in result
105  * @param[in] num_el size of result
106  *
107  * @attention Only the left lower triangular part of the result is initialized.
108  */
109  template<class t_DerivedOutput, class t_DerivedInput>
111  getATA( Eigen::PlainObjectBase<t_DerivedOutput> &result,
112  const Eigen::DenseBase<t_DerivedInput> &A,
113  const std::ptrdiff_t offset,
114  const std::ptrdiff_t num_el)
115  {
116  std::ptrdiff_t A_num_col = A.cols();
117  result.resize(num_el, num_el);
118  result.template triangularView<Eigen::Lower>().setZero();
119  for (std::ptrdiff_t i = 0; i < A_num_col; ++i)
120  {
121  result.block( offset,
122  offset,
123  A_num_col,
124  A_num_col).block(i, i, A_num_col-i, 1).noalias() = A.transpose().bottomRows(A_num_col-i) * A.col(i);
125  }
126  }
127 
128 
129  /**
130  * @brief result += A^T * A
131  *
132  * @tparam t_DerivedOutput Eigen parameter
133  * @tparam t_DerivedInput Eigen parameter
134  *
135  * @param[in,out] result
136  * @param[in] A
137  *
138  * @attention Only the left lower triangular part of the result is initialized.
139  */
140  template<class t_DerivedOutput, class t_DerivedInput>
142  addATA( Eigen::DenseBase<t_DerivedOutput> &result,
143  const Eigen::DenseBase<t_DerivedInput> &A)
144  {
145  std::ptrdiff_t num_el = A.cols();
146  for (std::ptrdiff_t i = 0; i < num_el; ++i)
147  {
148  result.block(i, i, num_el-i, 1).noalias() += A.transpose().bottomRows(num_el-i) * A.col(i);
149  }
150  }
151 
152 
153  /**
154  * @brief result.diagonalBlock() += A^T * A
155  *
156  * @tparam t_DerivedOutput Eigen parameter
157  * @tparam t_DerivedInput Eigen parameter
158  *
159  * @param[in,out] result
160  * @param[in] A
161  * @param[in] offset offset of A in result
162  *
163  * @attention Only the left lower triangular part of the result is initialized.
164  */
165  template<class t_DerivedOutput, class t_DerivedInput>
167  addATA( Eigen::DenseBase<t_DerivedOutput> &result,
168  const Eigen::DenseBase<t_DerivedInput> &A,
169  const std::ptrdiff_t offset)
170  {
171  std::ptrdiff_t A_num_col = A.cols();
172  for (std::ptrdiff_t i = 0; i < A_num_col; ++i)
173  {
174  result.block( offset,
175  offset,
176  A_num_col,
177  A_num_col).block(i, i, A_num_col-i, 1).noalias() += A.transpose().bottomRows(A_num_col-i) * A.col(i);
178  }
179  }
180 
181 
182 
183  /**
184  * @brief Converts left lower triangular matrix to a symmetric matrix.
185  *
186  * @tparam t_Derived Eigen parameter
187  *
188  * @param[in,out] matrix
189  */
190  template<class t_Derived>
192  convertLLTtoSymmetric(Eigen::PlainObjectBase<t_Derived> &matrix)
193  {
194  std::ptrdiff_t num_el = matrix.cols();
195  for (std::ptrdiff_t i = 0; i < num_el-1; ++i)
196  {
197  matrix.block(i, i+1, 1, num_el-i-1) = matrix.transpose().block(i, i+1, 1, num_el-i-1);
198  }
199  }
200 
201 
202  /**
203  * @brief Transform the input positions given as a concatenated set of 2d/3d
204  * vectors, given M = [v1, v2, ...], returns M_new = [R*v1 + t, R*v2 + t, ...]
205  *
206  * @tparam t_DerivedMatrix Eigen parameter
207  * @tparam t_DerivedRotation Eigen parameter
208  * @tparam t_DerivedTranslation Eigen parameter
209  *
210  * @param[in] matrix matrix containing vectors (M)
211  * @param[in] rotation rotation matrix (R)
212  * @param[in] translation translation vector (t)
213  *
214  * @return matrix of transformed vectors
215  */
216  template <class t_DerivedMatrix, class t_DerivedRotation, class t_DerivedTranslation>
217  inline Eigen::Matrix< typename Eigen::MatrixBase<t_DerivedMatrix>::Scalar,
218  Eigen::MatrixBase<t_DerivedMatrix>::RowsAtCompileTime,
219  Eigen::MatrixBase<t_DerivedMatrix>::ColsAtCompileTime>
221  transform( const Eigen::MatrixBase<t_DerivedMatrix> &matrix,
222  const Eigen::MatrixBase<t_DerivedRotation> &rotation,
223  const Eigen::MatrixBase<t_DerivedTranslation> &translation)
224  {
225  return ((rotation*matrix).colwise() + translation);
226  }
227 
228 
229 
230  /**
231  * @brief Create a diagonal matrix consisting of the input matrices
232  *
233  * @tparam t_Scalar Eigen parameter (input)
234  * @tparam t_rows Eigen parameter (input)
235  * @tparam t_cols Eigen parameter (input)
236  * @tparam t_flags Eigen parameter (input)
237  *
238  * @param[in] input_matrices vector of smaller matrices to be placed into a large block diagonal matrix
239  *
240  * @return matrix with each input element as a block in the diagonal
241  */
242  template< typename t_Scalar,
243  int t_rows,
244  int t_cols,
245  int t_flags>
248  makeBlockDiagonal(const std::vector<Eigen::Matrix<t_Scalar, t_rows, t_cols, t_flags> > &input_matrices)
249  {
250  switch (input_matrices.size())
251  {
252  case 0:
253  return (QPMAD_UTILS_DYNAMIC_MATRIX(t_Scalar)());
254 
255  case 1:
256  return (input_matrices[0]);
257 
258  default:
259  // Initialize the output diagonal matrix
260  std::ptrdiff_t row_size = 0;
261  std::ptrdiff_t col_size = 0;
262 
263  for(std::size_t i = 0; i < input_matrices.size(); ++i)
264  {
265  row_size += input_matrices[i].rows();
266  col_size += input_matrices[i].cols();
267  }
268  QPMAD_UTILS_DYNAMIC_MATRIX(t_Scalar) output;
269  output.setZero(row_size, col_size);
270 
271  // Add in the input matrices
272  std::ptrdiff_t cumulative_row = 0;
273  std::ptrdiff_t cumulative_col = 0;
274 
275  for(std::size_t i = 0; i < input_matrices.size(); ++i)
276  {
277  int current_num_rows = input_matrices[i].rows();
278  int current_num_cols = input_matrices[i].cols();
279 
280  output.block(cumulative_row, cumulative_col, current_num_rows, current_num_cols) = input_matrices[i];
281 
282  cumulative_row += current_num_rows;
283  cumulative_col += current_num_cols;
284  }
285  return output;
286  }
287  }
288 
289 
290 
291  /**
292  * @brief Create a diagonal matrix replicating the input matrix
293  *
294  * @tparam t_DerivedInput Eigen parameter (input)
295  *
296  * @param[in] input_matrix matrix to be replicated into block diagonal
297  * @param[in] num_copies determines the output size
298  *
299  * @return matrix with each input element as a block in the diagonal
300  */
301  template<class t_DerivedInput>
302  QPMAD_UTILS_DYNAMIC_MATRIX(typename Eigen::DenseBase<t_DerivedInput>::Scalar)
304  makeBlockDiagonal( const Eigen::DenseBase<t_DerivedInput> &input_matrix,
305  const std::ptrdiff_t num_copies)
306  {
307  switch (num_copies)
308  {
309  case 0:
310  return (QPMAD_UTILS_DYNAMIC_MATRIX(typename Eigen::DenseBase<t_DerivedInput>::Scalar)());
311 
312  case 1:
313  return (input_matrix);
314 
315  default:
316  // Initialize the output diagonal matrix
317  QPMAD_UTILS_DYNAMIC_MATRIX(typename Eigen::DenseBase<t_DerivedInput>::Scalar) output;
318 
319  output.setZero(num_copies * input_matrix.rows(),
320  num_copies * input_matrix.cols());
321 
322  // Replicate the input matrix into block diagonal form
323  for(std::ptrdiff_t i = 0; i < num_copies; ++i)
324  {
325  output.block(i*input_matrix.rows(), i*input_matrix.cols(),
326  input_matrix.rows(), input_matrix.cols()) = input_matrix;
327  }
328  return (output);
329  }
330  }
331 
332 
333 
334  /**
335  * @brief Concatenate matrices vertically, [A;B;C; ...]
336  *
337  * @tparam t_DerivedOutput Eigen parameter
338  * @tparam t_Scalar Eigen parameter (input)
339  * @tparam t_rows Eigen parameter (input)
340  * @tparam t_cols Eigen parameter (input)
341  * @tparam t_flags Eigen parameter (input)
342  *
343  * @param[out] result result of concatenation
344  * @param[in] matrices matrices
345  */
346  template< class t_DerivedOutput,
347  typename t_Scalar,
348  int t_rows,
349  int t_cols,
350  int t_flags>
352  concatenateMatricesVertically( Eigen::PlainObjectBase<t_DerivedOutput> &result,
353  const std::vector<Eigen::Matrix<t_Scalar, t_rows, t_cols, t_flags> > &matrices)
354  {
355  std::vector< Eigen::Matrix<t_Scalar, t_rows, t_cols, t_flags> > nonempty_matrices = matrices;
356  std::size_t number_of_matrices = nonempty_matrices.size();
357 
358  if (number_of_matrices > 0)
359  {
360  std::ptrdiff_t total_number_of_rows = 0;
361  std::ptrdiff_t number_of_cols = 0;
362 
363  for(std::size_t i = 0; i < number_of_matrices; ++i)
364  {
365  if(!nonempty_matrices[i].size())
366  {
367  nonempty_matrices.erase(nonempty_matrices.begin() + i);
368  --number_of_matrices;
369  }
370  }
371 
372  if(!number_of_matrices)
373  {
374  result.resize(0, 0);
375  return;
376  }
377 
378  number_of_cols = nonempty_matrices[0].cols();
379  for (std::size_t i = 0; i < number_of_matrices; ++i)
380  {
381  QPMAD_UTILS_ASSERT(number_of_cols == nonempty_matrices[i].cols(), "Inconsistent size of input matrices.");
382  total_number_of_rows += nonempty_matrices[i].rows();
383  }
384 
385  result.resize(total_number_of_rows, number_of_cols);
386 
387  std::ptrdiff_t row_index = 0;
388  for (std::size_t i = 0; i < number_of_matrices; ++i)
389  {
390  result.block(row_index, 0, nonempty_matrices[i].rows(), number_of_cols) = nonempty_matrices[i];
391  row_index += nonempty_matrices[i].rows();
392  }
393  }
394  else
395  {
396  result.resize(0, 0);
397  }
398  }
399 
400 
401  /**
402  * @brief Concatenate matrices horizontally, [A B C ...]
403  *
404  * @tparam t_DerivedOutput Eigen parameter
405  * @tparam t_DerivedInput1 Eigen parameter
406  * @tparam t_DerivedInput2 Eigen parameter
407  *
408  * @param[out] result result of concatenation
409  * @param[in] matrix1
410  * @param[in] matrix2
411  */
412  template< class t_DerivedOutput,
413  class t_DerivedInput1,
414  class t_DerivedInput2>
417  Eigen::PlainObjectBase<t_DerivedOutput> &result,
418  const Eigen::DenseBase<t_DerivedInput1> &matrix1,
419  const Eigen::DenseBase<t_DerivedInput2> &matrix2)
420  {
421  if (matrix1.rows() == 0)
422  {
423  result = matrix2;
424  }
425  else
426  {
427  if (matrix2.rows() == 0)
428  {
429  result = matrix1;
430  }
431  else
432  {
433  std::ptrdiff_t number_of_rows = matrix1.rows();
434  std::ptrdiff_t number_of_cols = matrix1.cols() + matrix2.cols();
435 
436  QPMAD_UTILS_ASSERT(number_of_rows == matrix2.rows(), "Inconsistent size of input matrices.");
437 
438  result.resize(number_of_rows, number_of_cols);
439  result << matrix1, matrix2;
440  }
441  }
442  }
443 
444 
445 
446  /**
447  * @brief Concatenate matrices horizontally, [A B C ...]
448  *
449  * @tparam t_DerivedOutput Eigen parameter
450  * @tparam t_DerivedInput1 Eigen parameter
451  * @tparam t_DerivedInput2 Eigen parameter
452  * @tparam t_DerivedInput3 Eigen parameter
453  *
454  * @param[out] result result of concatenation
455  * @param[in] matrix1
456  * @param[in] matrix2
457  * @param[in] matrix3
458  */
459  template< class t_DerivedOutput,
460  class t_DerivedInput1,
461  class t_DerivedInput2,
462  class t_DerivedInput3>
465  Eigen::PlainObjectBase<t_DerivedOutput> &result,
466  const Eigen::DenseBase<t_DerivedInput1> &matrix1,
467  const Eigen::DenseBase<t_DerivedInput2> &matrix2,
468  const Eigen::DenseBase<t_DerivedInput3> &matrix3)
469  {
470  if (matrix1.rows() == 0)
471  {
472  concatenateMatricesHorizontally(result, matrix2, matrix3);
473  }
474  else
475  {
476  if (matrix2.rows() == 0)
477  {
478  concatenateMatricesHorizontally(result, matrix1, matrix3);
479  }
480  else
481  {
482  if (matrix3.rows() == 0)
483  {
484  concatenateMatricesHorizontally(result, matrix1, matrix2);
485  }
486  else
487  {
488  std::ptrdiff_t number_of_rows = matrix1.rows();
489  std::ptrdiff_t number_of_cols = matrix1.cols() + matrix2.cols() + matrix3.cols();
490 
491  QPMAD_UTILS_ASSERT( (number_of_rows == matrix2.rows())
492  && (number_of_rows == matrix3.rows()),
493  "Inconsistent size of input matrices.");
494 
495  result.resize(number_of_rows, number_of_cols);
496  result << matrix1, matrix2, matrix3;
497  }
498  }
499  }
500  }
501 
502 
503 
504  /**
505  * @brief Remove a row with the specified index.
506  *
507  * @tparam t_Derived Eigen parameter
508  *
509  * @param[in,out] matrix matrix
510  * @param[in] row_to_remove index of a row
511  *
512  * Based on
513  * http://stackoverflow.com/questions/13290395/how-to-remove-a-certain-row-or-column-while-using-eigen-library-c
514  */
515  template<class t_Derived>
517  removeRow( Eigen::PlainObjectBase<t_Derived> & matrix,
518  const std::ptrdiff_t row_to_remove)
519  {
520  QPMAD_UTILS_ASSERT(row_to_remove <= matrix.rows(), "The index of a removed row is greater than the size of the matrix.");
521 
522  std::ptrdiff_t number_of_rows = matrix.rows()-1;
523  std::ptrdiff_t number_of_cols = matrix.cols();
524 
525  if( row_to_remove < number_of_rows )
526  {
527  matrix.block(row_to_remove, 0, number_of_rows - row_to_remove, number_of_cols) =
528  matrix.block(row_to_remove + 1, 0 , number_of_rows - row_to_remove, number_of_cols);
529  }
530 
531  matrix.conservativeResize(number_of_rows, number_of_cols);
532  }
533 
534 
535 
536  /**
537  * @brief Remove a column with the specified index.
538  *
539  * @tparam t_Derived Eigen parameter
540  *
541  * @param[in,out] matrix matrix
542  * @param[in] column_to_remove index of a column
543  *
544  * Based on
545  * http://stackoverflow.com/questions/13290395/how-to-remove-a-certain-row-or-column-while-using-eigen-library-c
546  */
547  template<class t_Derived>
549  removeColumn( Eigen::PlainObjectBase<t_Derived> & matrix,
550  const std::ptrdiff_t column_to_remove)
551  {
552  QPMAD_UTILS_ASSERT(column_to_remove <= matrix.rows(), "The index of a removed column is greater than the size of the matrix.");
553 
554  std::ptrdiff_t number_of_rows = matrix.rows();
555  std::ptrdiff_t number_of_cols = matrix.cols()-1;
556 
557  if( column_to_remove < number_of_cols )
558  {
559  matrix.block(0, column_to_remove, number_of_rows, number_of_cols - column_to_remove) =
560  matrix.block(0, column_to_remove + 1 ,number_of_rows, number_of_cols - column_to_remove);
561  }
562 
563  matrix.conservativeResize(number_of_rows, number_of_cols);
564  }
565 
566 
567  /**
568  * @brief Custom Kronecker product: blocks of the input matrix are treated as
569  * single elements.
570  *
571  * @tparam t_Derived Eigen parameter
572  *
573  * @param[in] input input matrix
574  * @param[in] input_block_rows rows in a block
575  * @param[in] input_block_cols cols in a block
576  * @param[in] identity_size size of the identity matrix used in the product
577  *
578  * @return result of the product
579  */
580  template<class t_Derived>
581  QPMAD_UTILS_DYNAMIC_MATRIX(typename Eigen::DenseBase<t_Derived>::Scalar)
583  multiplyBlockKroneckerIdentity (const Eigen::DenseBase<t_Derived> & input,
584  const std::size_t input_block_rows,
585  const std::size_t input_block_cols,
586  const std::size_t identity_size)
587  {
588  QPMAD_UTILS_ASSERT(input.rows() % input_block_rows == 0, "Wrong block size.");
589  QPMAD_UTILS_ASSERT(input.cols() % input_block_cols == 0, "Wrong block size.");
590 
591  QPMAD_UTILS_DYNAMIC_MATRIX(typename Eigen::DenseBase<t_Derived>::Scalar) output;
592 
593  std::size_t num_blocks_vert = input.rows() / input_block_rows;
594  std::size_t num_blocks_hor = input.cols() / input_block_cols;
595 
596  std::size_t output_block_rows = identity_size*input_block_rows;
597  std::size_t output_block_cols = identity_size*input_block_cols;
598 
599  output.setZero(identity_size * input.rows(),
600  identity_size * input.cols());
601 
602  for (std::size_t i = 0; i < num_blocks_vert; ++i)
603  {
604  for (std::size_t j = 0; j < num_blocks_hor; ++j)
605  {
606  for(std::size_t k = 0; k < identity_size; ++k)
607  {
608  output.block( i*output_block_rows + k*input_block_rows,
609  j*output_block_cols + k*input_block_cols,
610  input_block_rows,
611  input_block_cols)
612  = input.block(i*input_block_rows, j*input_block_cols, input_block_rows, input_block_cols);
613  }
614  }
615  }
616 
617  return(output);
618  }
619 
620 
621  // ===========================================================================
622  // Selections
623  // ===========================================================================
624 
625 
626  /**
627  * @brief Select rows from a matrix, in Matlab notation the result is
628  * M(first:step:end, :).
629  *
630  * @tparam t_Derived Eigen parameter
631  *
632  * @param[in] matrix input matrix
633  * @param[in] row_step each 'row_step' is selected from the input matrix
634  * @param[in] first_row starting from 'first_row'
635  *
636  * @return Matrix consisting of selected rows.
637  */
638  template<class t_Derived>
639  inline Eigen::Map< const QPMAD_UTILS_DYNAMIC_MATRIX(typename Eigen::PlainObjectBase<t_Derived>::Scalar),
640  Eigen::Unaligned,
641  Eigen::Stride<Eigen::Dynamic, Eigen::Dynamic> >
643  selectRows( const Eigen::PlainObjectBase<t_Derived> &matrix,
644  const std::size_t row_step,
645  const std::size_t first_row = 0)
646  {
647  return (QPMAD_UTILS_DYNAMIC_MATRIX(typename Eigen::PlainObjectBase<t_Derived>::Scalar)::Map(
648  matrix.data() + first_row,
649  ceil( static_cast<double> (matrix.rows() - first_row)/row_step),
650  matrix.cols(),
651  Eigen::Stride<Eigen::Dynamic, Eigen::Dynamic>(matrix.rows(), row_step)));
652  }
653 
654 
655  /**
656  * @brief Selection matrix
657  */
659  {
660  template<class t_Derived>
661  friend Eigen::Map< const QPMAD_UTILS_DYNAMIC_MATRIX( typename Eigen::PlainObjectBase<t_Derived>::Scalar ),
662  Eigen::Unaligned,
663  Eigen::Stride<Eigen::Dynamic, Eigen::Dynamic> >
664  operator*( const SelectionMatrix &,
665  const Eigen::PlainObjectBase<t_Derived> &);
666 
667 
668  private:
669  std::size_t step_size_;
670  std::size_t first_index_;
671 
672  public:
673  /**
674  * @brief Constructore
675  *
676  * @param[in] step_size step size of selection: 3 -- each third element
677  * @param[in] first_index index of the first element for selection
678  */
679  SelectionMatrix( const std::size_t step_size,
680  const std::size_t first_index)
681  {
682  step_size_ = step_size;
683  first_index_ = first_index;
684  }
685  };
686 
687 
688  /**
689  * @brief Multiply selection matrix by a generic Eigen matrix (select rows)
690  *
691  * @tparam t_Derived Eigen parameter
692  *
693  * @param[in] selector
694  * @param[in] matrix
695  *
696  * @return selected rows
697  */
698  template<class t_Derived>
699  inline Eigen::Map< const QPMAD_UTILS_DYNAMIC_MATRIX( typename Eigen::PlainObjectBase<t_Derived>::Scalar ),
700  Eigen::Unaligned,
701  Eigen::Stride<Eigen::Dynamic, Eigen::Dynamic> >
703  operator*( const SelectionMatrix & selector,
704  const Eigen::PlainObjectBase<t_Derived> & matrix)
705  {
706  return(selectRows(matrix, selector.step_size_, selector.first_index_));
707  }
708 
709 
710  #ifdef QPMAD_UTILS_ENABLE_EIGENTYPE_DETECTION
711  #include <boost/utility/enable_if.hpp>
712  #include <boost/mpl/has_xxx.hpp>
713  #include <boost/mpl/and.hpp>
714 
715  namespace detail
716  {
717  BOOST_MPL_HAS_XXX_TRAIT_DEF(Scalar)
718  BOOST_MPL_HAS_XXX_TRAIT_DEF(Index)
719  BOOST_MPL_HAS_XXX_TRAIT_DEF(StorageKind)
720  }
721 
722 
723  /**
724  * Traits for checking if T is indeed an Eigen Type
725  * @tparam T any Type
726  *
727  * Example Usage:
728  * is_eigen_type<int>::value // evaluates to false
729  * is_eigen_type<int>::type // evaluates to false_type
730  * is_eigen_type<Eigen::Vector2d>::value // evaluates to true
731  * is_eigen_type<Eigen::Vector2d>::type // true_type
732  *
733  * @see https://forum.kde.org/viewtopic.php?f=74&t=121280
734  */
735  template<typename T>
736  struct is_eigen_type : boost::mpl::and_<detail::has_Scalar<T>,
737  detail::has_Index<T>,
738  detail::has_StorageKind<T> >
739  {
740  };
741 
742  #define QPMAD_UTILS_EIGENTYPE_ENABLER_TYPE(DataType) \
743  const typename boost::enable_if_c< qpmad_utils::is_eigen_type<DataType>::value >::type
744  #define QPMAD_UTILS_EIGENTYPE_DISABLER_TYPE(DataType) \
745  const typename boost::enable_if_c< !(qpmad_utils::is_eigen_type<DataType>::value) >::type
746  #endif
747 } // eigenut
748 
749 #endif
Eigen::Map< const QPMAD_UTILS_DYNAMIC_MATRIX(typename Eigen::PlainObjectBase< t_Derived >::Scalar), Eigen::Unaligned, Eigen::Stride< Eigen::Dynamic, Eigen::Dynamic > > QPMAD_UTILS_VISIBILITY_ATTRIBUTE operator *(const SelectionMatrix &selector, const Eigen::PlainObjectBase< t_Derived > &matrix)
Multiply selection matrix by a generic Eigen matrix (select rows)
Definition: eigenut_misc.h:703
void QPMAD_UTILS_VISIBILITY_ATTRIBUTE concatenateMatricesVertically(Eigen::PlainObjectBase< t_DerivedOutput > &result, const std::vector< Eigen::Matrix< t_Scalar, t_rows, t_cols, t_flags > > &matrices)
Concatenate matrices vertically, [A;B;C; ...].
Definition: eigenut_misc.h:352
void QPMAD_UTILS_VISIBILITY_ATTRIBUTE concatenateMatricesHorizontally(Eigen::PlainObjectBase< t_DerivedOutput > &result, const Eigen::DenseBase< t_DerivedInput1 > &matrix1, const Eigen::DenseBase< t_DerivedInput2 > &matrix2)
Concatenate matrices horizontally, [A B C ...].
Definition: eigenut_misc.h:416
Eigen::Map< const QPMAD_UTILS_DYNAMIC_MATRIX(typename Eigen::PlainObjectBase< t_Derived >::Scalar), Eigen::Unaligned, Eigen::Stride< Eigen::Dynamic, Eigen::Dynamic > > QPMAD_UTILS_VISIBILITY_ATTRIBUTE selectRows(const Eigen::PlainObjectBase< t_Derived > &matrix, const std::size_t row_step, const std::size_t first_row=0)
Select rows from a matrix, in Matlab notation the result is M(first:step:end, :).
Definition: eigenut_misc.h:643
void QPMAD_UTILS_VISIBILITY_ATTRIBUTE convertLLTtoSymmetric(Eigen::PlainObjectBase< t_Derived > &matrix)
Converts left lower triangular matrix to a symmetric matrix.
Definition: eigenut_misc.h:192
Eigen::Matrix< typename Eigen::MatrixBase< t_DerivedMatrix >::Scalar, Eigen::MatrixBase< t_DerivedMatrix >::RowsAtCompileTime, Eigen::MatrixBase< t_DerivedMatrix >::ColsAtCompileTime > QPMAD_UTILS_VISIBILITY_ATTRIBUTE transform(const Eigen::MatrixBase< t_DerivedMatrix > &matrix, const Eigen::MatrixBase< t_DerivedRotation > &rotation, const Eigen::MatrixBase< t_DerivedTranslation > &translation)
Transform the input positions given as a concatenated set of 2d/3d vectors, given M = [v1,...
Definition: eigenut_misc.h:221
void QPMAD_UTILS_VISIBILITY_ATTRIBUTE removeRow(Eigen::PlainObjectBase< t_Derived > &matrix, const std::ptrdiff_t row_to_remove)
Remove a row with the specified index.
Definition: eigenut_misc.h:517
void getRandomPositiveDefinititeMatrix(Eigen::MatrixXd &M, const std::size_t size)
Definition: eigenut_misc.h:43
#define QPMAD_UTILS_VISIBILITY_ATTRIBUTE
#define QPMAD_UTILS_ASSERT(condition, message)
void unsetMatrix(Eigen::DenseBase< t_Derived > &matrix)
Unset matrix (initialize to NaN)
Definition: eigenut_misc.h:58
void QPMAD_UTILS_VISIBILITY_ATTRIBUTE addATA(Eigen::DenseBase< t_DerivedOutput > &result, const Eigen::DenseBase< t_DerivedInput > &A)
result += A^T * A
Definition: eigenut_misc.h:142
QPMAD_UTILS_DYNAMIC_MATRIX(t_Scalar) QPMAD_UTILS_VISIBILITY_ATTRIBUTE makeBlockDiagonal(const std
Create a diagonal matrix consisting of the input matrices.
Definition: eigenut_misc.h:246
void QPMAD_UTILS_VISIBILITY_ATTRIBUTE getATA(Eigen::PlainObjectBase< t_DerivedOutput > &result, const Eigen::DenseBase< t_DerivedInput > &A)
result = A^T * A
Definition: eigenut_misc.h:84
SelectionMatrix(const std::size_t step_size, const std::size_t first_index)
Constructore.
Definition: eigenut_misc.h:679
void QPMAD_UTILS_VISIBILITY_ATTRIBUTE removeColumn(Eigen::PlainObjectBase< t_Derived > &matrix, const std::ptrdiff_t column_to_remove)
Remove a column with the specified index.
Definition: eigenut_misc.h:549