| Directory: | ./ |
|---|---|
| File: | include/pinocchio/math/gram-schmidt-orthonormalisation.hpp |
| Date: | 2025-02-12 21:03:38 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 10 | 10 | 100.0% |
| Branches: | 14 | 48 | 29.2% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // | ||
| 2 | // Copyright (c) 2024 INRIA | ||
| 3 | // | ||
| 4 | |||
| 5 | #ifndef __pinocchio_math_gram_schmidt_orthonormalisation_hpp__ | ||
| 6 | #define __pinocchio_math_gram_schmidt_orthonormalisation_hpp__ | ||
| 7 | |||
| 8 | #include "pinocchio/math/fwd.hpp" | ||
| 9 | #include <Eigen/Core> | ||
| 10 | |||
| 11 | namespace pinocchio | ||
| 12 | { | ||
| 13 | /// \brief Perform the Gram-Schmidt orthonormalisation on the input/output vector for a given | ||
| 14 | /// input basis | ||
| 15 | /// | ||
| 16 | /// \param[in] basis Orthonormal basis | ||
| 17 | /// \param[in,out] vec Vector to orthonomarlize wrt the input basis | ||
| 18 | /// | ||
| 19 | template<typename MatrixType, typename VectorType> | ||
| 20 | 100000 | void orthonormalisation( | |
| 21 | const Eigen::MatrixBase<MatrixType> & basis, const Eigen::MatrixBase<VectorType> & vec_) | ||
| 22 | { | ||
| 23 | typedef typename VectorType::Scalar Scalar; | ||
| 24 | 100000 | VectorType & vec = vec_.const_cast_derived(); | |
| 25 | |||
| 26 |
1/24✗ Branch 2 not taken.
✓ Branch 3 taken 100000 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 35 not taken.
✗ Branch 36 not taken.
✗ Branch 38 not taken.
✗ Branch 39 not taken.
|
100000 | PINOCCHIO_CHECK_ARGUMENT_SIZE(basis.rows(), vec.size()); |
| 27 |
4/8✓ Branch 1 taken 100000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100000 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 100000 times.
✗ Branch 9 not taken.
✓ Branch 10 taken 100000 times.
✗ Branch 11 not taken.
|
100000 | assert((basis.transpose() * basis).isIdentity() && "The input basis is not orthonormal."); |
| 28 | |||
| 29 |
2/2✓ Branch 1 taken 1000000 times.
✓ Branch 2 taken 100000 times.
|
1100000 | for (Eigen::DenseIndex col_id = 0; col_id < basis.cols(); ++col_id) |
| 30 | { | ||
| 31 |
1/2✓ Branch 1 taken 1000000 times.
✗ Branch 2 not taken.
|
1000000 | const auto col = basis.col(col_id); |
| 32 |
1/2✓ Branch 1 taken 1000000 times.
✗ Branch 2 not taken.
|
1000000 | const Scalar alpha = col.dot(vec); |
| 33 |
2/4✓ Branch 1 taken 1000000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1000000 times.
✗ Branch 5 not taken.
|
1000000 | vec -= alpha * col; |
| 34 | } | ||
| 35 | |||
| 36 |
3/6✓ Branch 2 taken 100000 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 100000 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 100000 times.
|
100000 | assert((basis.transpose() * vec).isZero()); |
| 37 | 100000 | } | |
| 38 | } // namespace pinocchio | ||
| 39 | |||
| 40 | #endif // ifndef __pinocchio_math_gram_schmidt_orthonormalisation_hpp__ | ||
| 41 |