13 #ifndef H_QPMAD_UTILS_MISC 14 #define H_QPMAD_UTILS_MISC 45 M.setRandom(size, size);
46 M = M.transpose()*M + Eigen::MatrixXd::Identity(size, size);
57 template <
typename t_Derived>
60 if (std::numeric_limits<qpmad_utils::DefaultScalar>::has_quiet_NaN)
62 matrix.setConstant(std::numeric_limits<qpmad_utils::DefaultScalar>::quiet_NaN());
82 template<
class t_DerivedOutput,
class t_DerivedInput>
84 getATA( Eigen::PlainObjectBase<t_DerivedOutput> &result,
85 const Eigen::DenseBase<t_DerivedInput> &A)
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)
91 result.block(i, i, num_el-i, 1).noalias() = A.transpose().bottomRows(num_el-i) * A.col(i);
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)
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)
121 result.block( offset,
124 A_num_col).block(i, i, A_num_col-i, 1).noalias() = A.transpose().bottomRows(A_num_col-i) * A.col(i);
140 template<
class t_DerivedOutput,
class t_DerivedInput>
142 addATA( Eigen::DenseBase<t_DerivedOutput> &result,
143 const Eigen::DenseBase<t_DerivedInput> &A)
145 std::ptrdiff_t num_el = A.cols();
146 for (std::ptrdiff_t i = 0; i < num_el; ++i)
148 result.block(i, i, num_el-i, 1).noalias() += A.transpose().bottomRows(num_el-i) * A.col(i);
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)
171 std::ptrdiff_t A_num_col = A.cols();
172 for (std::ptrdiff_t i = 0; i < A_num_col; ++i)
174 result.block( offset,
177 A_num_col).block(i, i, A_num_col-i, 1).noalias() += A.transpose().bottomRows(A_num_col-i) * A.col(i);
190 template<
class t_Derived>
194 std::ptrdiff_t num_el = matrix.cols();
195 for (std::ptrdiff_t i = 0; i < num_el-1; ++i)
197 matrix.block(i, i+1, 1, num_el-i-1) = matrix.transpose().block(i, i+1, 1, num_el-i-1);
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)
225 return ((rotation*matrix).colwise() + translation);
242 template<
typename t_Scalar,
248 makeBlockDiagonal(
const std::vector<Eigen::Matrix<t_Scalar, t_rows, t_cols, t_flags> > &input_matrices)
250 switch (input_matrices.size())
256 return (input_matrices[0]);
260 std::ptrdiff_t row_size = 0;
261 std::ptrdiff_t col_size = 0;
263 for(std::size_t i = 0; i < input_matrices.size(); ++i)
265 row_size += input_matrices[i].rows();
266 col_size += input_matrices[i].cols();
269 output.setZero(row_size, col_size);
272 std::ptrdiff_t cumulative_row = 0;
273 std::ptrdiff_t cumulative_col = 0;
275 for(std::size_t i = 0; i < input_matrices.size(); ++i)
277 int current_num_rows = input_matrices[i].rows();
278 int current_num_cols = input_matrices[i].cols();
280 output.block(cumulative_row, cumulative_col, current_num_rows, current_num_cols) = input_matrices[i];
282 cumulative_row += current_num_rows;
283 cumulative_col += current_num_cols;
301 template<
class t_DerivedInput>
304 makeBlockDiagonal(
const Eigen::DenseBase<t_DerivedInput> &input_matrix,
305 const std::ptrdiff_t num_copies)
313 return (input_matrix);
319 output.setZero(num_copies * input_matrix.rows(),
320 num_copies * input_matrix.cols());
323 for(std::ptrdiff_t i = 0; i < num_copies; ++i)
325 output.block(i*input_matrix.rows(), i*input_matrix.cols(),
326 input_matrix.rows(), input_matrix.cols()) = input_matrix;
346 template<
class t_DerivedOutput,
353 const std::vector<Eigen::Matrix<t_Scalar, t_rows, t_cols, t_flags> > &matrices)
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();
358 if (number_of_matrices > 0)
360 std::ptrdiff_t total_number_of_rows = 0;
361 std::ptrdiff_t number_of_cols = 0;
363 for(std::size_t i = 0; i < number_of_matrices; ++i)
365 if(!nonempty_matrices[i].size())
367 nonempty_matrices.erase(nonempty_matrices.begin() + i);
368 --number_of_matrices;
372 if(!number_of_matrices)
378 number_of_cols = nonempty_matrices[0].cols();
379 for (std::size_t i = 0; i < number_of_matrices; ++i)
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();
385 result.resize(total_number_of_rows, number_of_cols);
387 std::ptrdiff_t row_index = 0;
388 for (std::size_t i = 0; i < number_of_matrices; ++i)
390 result.block(row_index, 0, nonempty_matrices[i].rows(), number_of_cols) = nonempty_matrices[i];
391 row_index += nonempty_matrices[i].rows();
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)
421 if (matrix1.rows() == 0)
427 if (matrix2.rows() == 0)
433 std::ptrdiff_t number_of_rows = matrix1.rows();
434 std::ptrdiff_t number_of_cols = matrix1.cols() + matrix2.cols();
436 QPMAD_UTILS_ASSERT(number_of_rows == matrix2.rows(),
"Inconsistent size of input matrices.");
438 result.resize(number_of_rows, number_of_cols);
439 result << matrix1, matrix2;
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)
470 if (matrix1.rows() == 0)
476 if (matrix2.rows() == 0)
482 if (matrix3.rows() == 0)
488 std::ptrdiff_t number_of_rows = matrix1.rows();
489 std::ptrdiff_t number_of_cols = matrix1.cols() + matrix2.cols() + matrix3.cols();
492 && (number_of_rows == matrix3.rows()),
493 "Inconsistent size of input matrices.");
495 result.resize(number_of_rows, number_of_cols);
496 result << matrix1, matrix2, matrix3;
515 template<
class t_Derived>
518 const std::ptrdiff_t row_to_remove)
520 QPMAD_UTILS_ASSERT(row_to_remove <= matrix.rows(),
"The index of a removed row is greater than the size of the matrix.");
522 std::ptrdiff_t number_of_rows = matrix.rows()-1;
523 std::ptrdiff_t number_of_cols = matrix.cols();
525 if( row_to_remove < number_of_rows )
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);
531 matrix.conservativeResize(number_of_rows, number_of_cols);
547 template<
class t_Derived>
550 const std::ptrdiff_t column_to_remove)
552 QPMAD_UTILS_ASSERT(column_to_remove <= matrix.rows(),
"The index of a removed column is greater than the size of the matrix.");
554 std::ptrdiff_t number_of_rows = matrix.rows();
555 std::ptrdiff_t number_of_cols = matrix.cols()-1;
557 if( column_to_remove < number_of_cols )
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);
563 matrix.conservativeResize(number_of_rows, number_of_cols);
580 template<
class t_Derived>
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)
593 std::size_t num_blocks_vert = input.rows() / input_block_rows;
594 std::size_t num_blocks_hor = input.cols() / input_block_cols;
596 std::size_t output_block_rows = identity_size*input_block_rows;
597 std::size_t output_block_cols = identity_size*input_block_cols;
599 output.setZero(identity_size * input.rows(),
600 identity_size * input.cols());
602 for (std::size_t i = 0; i < num_blocks_vert; ++i)
604 for (std::size_t j = 0; j < num_blocks_hor; ++j)
606 for(std::size_t k = 0; k < identity_size; ++k)
608 output.block( i*output_block_rows + k*input_block_rows,
609 j*output_block_cols + k*input_block_cols,
612 = input.block(i*input_block_rows, j*input_block_cols, input_block_rows, input_block_cols);
638 template<
class t_Derived>
641 Eigen::Stride<Eigen::Dynamic, Eigen::Dynamic> >
644 const std::size_t row_step,
645 const std::size_t first_row = 0)
648 matrix.data() + first_row,
649 ceil( static_cast<double> (matrix.rows() - first_row)/row_step),
651 Eigen::Stride<Eigen::Dynamic, Eigen::Dynamic>(matrix.rows(), row_step)));
660 template<
class t_Derived>
663 Eigen::Stride<Eigen::Dynamic, Eigen::Dynamic> >
665 const Eigen::PlainObjectBase<t_Derived> &);
680 const std::size_t first_index)
682 step_size_ = step_size;
683 first_index_ = first_index;
698 template<
class t_Derived>
701 Eigen::Stride<Eigen::Dynamic, Eigen::Dynamic> >
704 const Eigen::PlainObjectBase<t_Derived> & matrix)
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> 717 BOOST_MPL_HAS_XXX_TRAIT_DEF(Scalar)
718 BOOST_MPL_HAS_XXX_TRAIT_DEF(Index)
719 BOOST_MPL_HAS_XXX_TRAIT_DEF(StorageKind)
736 struct is_eigen_type : boost::mpl::and_<detail::has_Scalar<T>,
737 detail::has_Index<T>,
738 detail::has_StorageKind<T> >
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 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)
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; ...].
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 ...].
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, :).
void QPMAD_UTILS_VISIBILITY_ATTRIBUTE convertLLTtoSymmetric(Eigen::PlainObjectBase< t_Derived > &matrix)
Converts left lower triangular matrix to a symmetric matrix.
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,...
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.
void getRandomPositiveDefinititeMatrix(Eigen::MatrixXd &M, const std::size_t size)
#define QPMAD_UTILS_VISIBILITY_ATTRIBUTE
#define QPMAD_UTILS_ASSERT(condition, message)
void unsetMatrix(Eigen::DenseBase< t_Derived > &matrix)
Unset matrix (initialize to NaN)
void QPMAD_UTILS_VISIBILITY_ATTRIBUTE addATA(Eigen::DenseBase< t_DerivedOutput > &result, const Eigen::DenseBase< t_DerivedInput > &A)
result += A^T * A
QPMAD_UTILS_DYNAMIC_MATRIX(t_Scalar) QPMAD_UTILS_VISIBILITY_ATTRIBUTE makeBlockDiagonal(const std
Create a diagonal matrix consisting of the input matrices.
void QPMAD_UTILS_VISIBILITY_ATTRIBUTE getATA(Eigen::PlainObjectBase< t_DerivedOutput > &result, const Eigen::DenseBase< t_DerivedInput > &A)
result = A^T * A
SelectionMatrix(const std::size_t step_size, const std::size_t first_index)
Constructore.
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.