pinocchio  3.7.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
 
Loading...
Searching...
No Matches
gram-schmidt-orthonormalisation.hpp
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
11namespace pinocchio
12{
19 template<typename MatrixType, typename VectorType>
21 const Eigen::MatrixBase<MatrixType> & basis, const Eigen::MatrixBase<VectorType> & vec_)
22 {
23 typedef typename VectorType::Scalar Scalar;
24 VectorType & vec = vec_.const_cast_derived();
25
26 PINOCCHIO_CHECK_ARGUMENT_SIZE(basis.rows(), vec.size());
27 assert((basis.transpose() * basis).isIdentity() && "The input basis is not orthonormal.");
28
29 for (Eigen::DenseIndex col_id = 0; col_id < basis.cols(); ++col_id)
30 {
31 const auto col = basis.col(col_id);
32 const Scalar alpha = col.dot(vec);
33 vec -= alpha * col;
34 }
35
36 assert((basis.transpose() * vec).isZero());
37 }
38} // namespace pinocchio
39
40#endif // ifndef __pinocchio_math_gram_schmidt_orthonormalisation_hpp__
Main pinocchio namespace.
Definition treeview.dox:11
void orthonormalisation(const Eigen::MatrixBase< MatrixType > &basis, const Eigen::MatrixBase< VectorType > &vec_)
Perform the Gram-Schmidt orthonormalisation on the input/output vector for a given input basis.