Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/pinocchio/algorithm/cholesky.hxx	Lines:	146	146	100.0 %
Date:	2024-04-26 13:14:21	Branches:	117	462	25.3 %


//
// Copyright (c) 2015-2019 CNRS INRIA
//

#ifndef __pinocchio_cholesky_hxx__
#define __pinocchio_cholesky_hxx__

#include "pinocchio/algorithm/check.hpp"

/// @cond DEV

namespace pinocchio
{
  namespace cholesky
  {

    template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
    inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::MatrixXs &
    decompose(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
              DataTpl<Scalar,Options,JointCollectionTpl> & data)
    {
      /*
       *    D = zeros(n,1);
       *    U = eye(n);
       *    for j=n:-1:1
       *      subtree = j+1:tree(j);
       *      D(j) = M(j,j) - U(j,subtree)*diag(D(subtree))*U(j,subtree)';
       *      i=parent(j);
       *      while i>0
       *          U(i,j) = (M(i,j) - U(i,subtree)*diag(D(subtree))*U(j,subtree)') / D(j);
       *          i=parent(i);
       *      end
       *    end
       */
      PINOCCHIO_UNUSED_VARIABLE(model);
      assert(model.check(data) && "data is not consistent with model.");

      typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;

      const typename Data::MatrixXs & M = data.M;
      typename Data::MatrixXs & U = data.U;
      typename Data::VectorXs & D = data.D;
      typename Data::VectorXs & Dinv = data.Dinv;

      for(int j=model.nv-1;j>=0;--j )
      {
        const int NVT = data.nvSubtree_fromRow[(size_t)j]-1;
        typename Data::VectorXs::SegmentReturnType DUt = data.tmp.head(NVT);
        if(NVT)
          DUt.noalias() = U.row(j).segment(j+1,NVT).transpose()
          .cwiseProduct(D.segment(j+1,NVT));

        D[j] = M(j,j) - U.row(j).segment(j+1,NVT).dot(DUt);
        Dinv[j] = Scalar(1)/D[j];
        for(int _i = data.parents_fromRow[(size_t)j]; _i >= 0;_i = data.parents_fromRow[(size_t)_i])
          U(_i,j) = (M(_i,j) - U.row(_i).segment(j+1,NVT).dot(DUt)) * Dinv[j];
      }

      return data.U;
    }

    namespace internal
    {
      template<typename Mat, int ColsAtCompileTime = Mat::ColsAtCompileTime>
      struct Uv
      {
        template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
        static void run(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                        const DataTpl<Scalar,Options,JointCollectionTpl> & data,
                        const Eigen::MatrixBase<Mat> & m)
        {
          Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,m);
          for(int k = 0; k < m_.cols(); ++k)
            cholesky::Uv(model,data,m_.col(k));
        }
      };

      template<typename Mat>
      struct Uv<Mat,1>
      {
        template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
        static void run(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                        const DataTpl<Scalar,Options,JointCollectionTpl> & data,
                        const Eigen::MatrixBase<Mat> & v)
        {
          typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;

          EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat)
          PINOCCHIO_UNUSED_VARIABLE(model);
          assert(model.check(data) && "data is not consistent with model.");
          PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv);

          Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,v);

          const typename Data::MatrixXs & U = data.U;
          const std::vector<int> & nvt = data.nvSubtree_fromRow;

          for(int k=0;k < model.nv-1;++k) // You can stop one step before nv
            v_.row(k) += U.row(k).segment(k+1,nvt[(size_t)k]-1) * v_.middleRows(k+1,nvt[(size_t)k]-1);
        }
      };

    } // namespace internal

    /* Compute U*v.
     * Nota: there is no smart way of doing U*D*v, so it is not proposed. */
    template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Mat>
    Mat & Uv(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
             const DataTpl<Scalar,Options,JointCollectionTpl> & data,
             const Eigen::MatrixBase<Mat> & m)
    {
      Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,m);
      internal::Uv<Mat,Mat::ColsAtCompileTime>::run(model,data,m_);
      return m_.derived();
    }

    namespace internal
    {
      template<typename Mat, int ColsAtCompileTime = Mat::ColsAtCompileTime>
      struct Utv
      {
        template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
        static void run(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                        const DataTpl<Scalar,Options,JointCollectionTpl> & data,
                        const Eigen::MatrixBase<Mat> & m)
        {
          Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,m);
          for(int k = 0; k < m_.cols(); ++k)
            cholesky::Utv(model,data,m_.col(k));
        }
      };

      template<typename Mat>
      struct Utv<Mat,1>
      {
        template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
        static void run(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                        const DataTpl<Scalar,Options,JointCollectionTpl> & data,
                        const Eigen::MatrixBase<Mat> & v)
        {
          typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;

          EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat)

          PINOCCHIO_UNUSED_VARIABLE(model);
          assert(model.check(data) && "data is not consistent with model.");
          PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv);
          Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,v);

          const typename Data::MatrixXs & U = data.U;
          const std::vector<int> & nvt = data.nvSubtree_fromRow;

          for( int k=model.nv-2;k>=0;--k ) // You can start from nv-2 (no child in nv-1)
            v_.segment(k+1,nvt[(size_t)k]-1) += U.row(k).segment(k+1,nvt[(size_t)k]-1).transpose()*v_[k];
        }
      };

    } // namespace internal

    /* Compute U'*v */
    template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Mat>
    Mat & Utv(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
              const DataTpl<Scalar,Options,JointCollectionTpl> & data,
              const Eigen::MatrixBase<Mat> & m)
    {
      Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,m);
      internal::Utv<Mat,Mat::ColsAtCompileTime>::run(model,data,m_);
      return m_.derived();
    }

    namespace internal
    {
      template<typename Mat, int ColsAtCompileTime = Mat::ColsAtCompileTime>
      struct Uiv
      {
        template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
        static void run(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                        const DataTpl<Scalar,Options,JointCollectionTpl> & data,
                        const Eigen::MatrixBase<Mat> & m)
        {
          Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,m);
          for(int k = 0; k < m_.cols(); ++k)
            cholesky::Uiv(model,data,m_.col(k));
        }
      };

      template<typename Mat>
      struct Uiv<Mat,1>
      {
        template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
        static void run(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                        const DataTpl<Scalar,Options,JointCollectionTpl> & data,
                        const Eigen::MatrixBase<Mat> & v)
        {
          typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;

          EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat)
          assert(model.check(data) && "data is not consistent with model.");
          PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv);
          Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,v);

          const typename Data::MatrixXs & U = data.U;
          const std::vector<int> & nvt = data.nvSubtree_fromRow;

          for(int k=model.nv-2;k>=0;--k) // You can start from nv-2 (no child in nv-1)
            v_[k] -= U.row(k).segment(k+1,nvt[(size_t)k]-1).dot(v_.segment(k+1,nvt[(size_t)k]-1));
        }
      };

    } // namespace internal

    /* Compute U^{-1}*v
     * Nota: there is no efficient way to compute D^{-1}U^{-1}v
     * in a single loop, so algorithm is not proposed.*/
    template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Mat>
    Mat & Uiv(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
              const DataTpl<Scalar,Options,JointCollectionTpl> & data,
              const Eigen::MatrixBase<Mat> & m)
    {
      Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,m);
      internal::Uiv<Mat,Mat::ColsAtCompileTime>::run(model,data,m_);
      return m_.derived();
    }

    namespace internal
    {
      template<typename Mat, int ColsAtCompileTime = Mat::ColsAtCompileTime>
      struct Utiv
      {
        template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
        static void run(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                        const DataTpl<Scalar,Options,JointCollectionTpl> & data,
                        const Eigen::MatrixBase<Mat> & m)
        {
          Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,m);
          for(int k = 0; k < m_.cols(); ++k)
            cholesky::Utiv(model,data,m_.col(k));
        }
      };

      template<typename Mat>
      struct Utiv<Mat,1>
      {
        template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
        static void run(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                        const DataTpl<Scalar,Options,JointCollectionTpl> & data,
                        const Eigen::MatrixBase<Mat> & v)
        {
          typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;

          EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat)
          assert(model.check(data) && "data is not consistent with model.");
          PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv);
          Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,v);

          const typename Data::MatrixXs & U = data.U;
          const std::vector<int> & nvt = data.nvSubtree_fromRow;

          for(int k=0; k<model.nv-1; ++k) // You can stop one step before nv.
            v_.segment(k+1,nvt[(size_t)k]-1) -= U.row(k).segment(k+1,nvt[(size_t)k]-1).transpose() * v_[k];
        }
      };

    } // namespace internal

    template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Mat>
    Mat & Utiv(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
               const DataTpl<Scalar,Options,JointCollectionTpl> & data,
               const Eigen::MatrixBase<Mat> & m)
    {
      Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,m);
      internal::Utiv<Mat,Mat::ColsAtCompileTime>::run(model,data,m_);
      return m_.derived();
    }

    namespace internal
    {
      template<typename Mat, typename MatRes, int ColsAtCompileTime = Mat::ColsAtCompileTime>
      struct Mv
      {
        template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
        static void run(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                        const DataTpl<Scalar,Options,JointCollectionTpl> & data,
                        const Eigen::MatrixBase<Mat> & min,
                        const Eigen::MatrixBase<MatRes> & mout
                        )
        {
          MatRes & mout_ = PINOCCHIO_EIGEN_CONST_CAST(MatRes,mout);
          for(int k = 0; k < min.cols(); ++k)
            cholesky::Mv(model,data,min.col(k),mout_.col(k));
        }
      };

      template<typename Mat, typename MatRes>
      struct Mv<Mat,MatRes,1>
      {
        template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
        static void run(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                        const DataTpl<Scalar,Options,JointCollectionTpl> & data,
                        const Eigen::MatrixBase<Mat> & vin,
                        const Eigen::MatrixBase<MatRes> & vout)
        {
          typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;

          EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat)
          EIGEN_STATIC_ASSERT_VECTOR_ONLY(MatRes)

          PINOCCHIO_UNUSED_VARIABLE(model);
          assert(model.check(data) && "data is not consistent with model.");
          PINOCCHIO_CHECK_ARGUMENT_SIZE(vin.size(), model.nv);
          PINOCCHIO_CHECK_ARGUMENT_SIZE(vout.size(), model.nv);

          MatRes & vout_ = PINOCCHIO_EIGEN_CONST_CAST(MatRes,vout);

          const typename Data::MatrixXs & M = data.M;
          const std::vector<int> & nvt = data.nvSubtree_fromRow;

          for(int k=model.nv-1;k>=0;--k)
          {
            vout_[k] = M.row(k).segment(k,nvt[(size_t)k]) * vin.segment(k,nvt[(size_t)k]);
            vout_.segment(k+1,nvt[(size_t)k]-1) += M.row(k).segment(k+1,nvt[(size_t)k]-1).transpose()*vin[k];
          }
        }
      };

    } // namespace internal

    template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Mat, typename MatRes>
    MatRes & Mv(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                const DataTpl<Scalar,Options,JointCollectionTpl> & data,
                const Eigen::MatrixBase<Mat> & min,
                const Eigen::MatrixBase<MatRes> & mout)
    {
      MatRes & mout_ = PINOCCHIO_EIGEN_CONST_CAST(MatRes,mout);
      internal::Mv<Mat,MatRes,Mat::ColsAtCompileTime>::run(model,data,min.derived(),mout_);
      return mout_.derived();
    }

    template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Mat>
    typename PINOCCHIO_EIGEN_PLAIN_TYPE(Mat) Mv(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                                      const DataTpl<Scalar,Options,JointCollectionTpl> & data,
                                      const Eigen::MatrixBase<Mat> & min)
    {
      typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Mat) ReturnType;
      ReturnType res(model.nv,min.cols());
      return Mv(model,data,min,res);
    }

    namespace internal
    {
      template<typename Mat, int ColsAtCompileTime = Mat::ColsAtCompileTime>
      struct UDUtv
      {
        template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
        static void run(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                        const DataTpl<Scalar,Options,JointCollectionTpl> & data,
                        const Eigen::MatrixBase<Mat> & m)
        {
          Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,m);
          for(int k = 0; k < m_.cols(); ++k)
            cholesky::UDUtv(model,data,m_.col(k));
        }
      };

      template<typename Mat>
      struct UDUtv<Mat,1>
      {
        template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
        static void run(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                        const DataTpl<Scalar,Options,JointCollectionTpl> & data,
                        const Eigen::MatrixBase<Mat> & v)
        {
          EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat)

          assert(model.check(data) && "data is not consistent with model.");
          PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv);

          Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,v);

          cholesky::Utv(model,data,v_);
          v_.array() *= data.D.array();
          cholesky::Uv(model,data,v_);
        }
      };

    } // namespace internal

    template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Mat>
    Mat & UDUtv(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                const DataTpl<Scalar,Options,JointCollectionTpl> & data,
                const Eigen::MatrixBase<Mat> & m)
    {
      Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,m);

      internal::UDUtv<Mat>::run(model,data,m_);
      return m_;
    }

    namespace internal
    {
      template<typename Mat, int ColsAtCompileTime = Mat::ColsAtCompileTime>
      struct solve
      {
        template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
        static void run(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                        const DataTpl<Scalar,Options,JointCollectionTpl> & data,
                        const Eigen::MatrixBase<Mat> & m)
        {
          Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,m);
          for(int k = 0; k < m_.cols(); ++k)
            cholesky::solve(model,data,m_.col(k));
        }
      };

      template<typename Mat>
      struct solve<Mat,1>
      {
        template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
        static void run(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                        const DataTpl<Scalar,Options,JointCollectionTpl> & data,
                        const Eigen::MatrixBase<Mat> & v)
        {
          EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat)

          assert(model.check(data) && "data is not consistent with model.");

          Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,v);

          cholesky::Uiv(model,data,v_);
          v_.array() *= data.Dinv.array();
          cholesky::Utiv(model,data,v_);
        }
      };

    } // namespace internal

    template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Mat>
    Mat & solve(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                const DataTpl<Scalar,Options,JointCollectionTpl> & data,
                const Eigen::MatrixBase<Mat> & m)
    {
      Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,m);
      internal::solve<Mat,Mat::ColsAtCompileTime>::run(model,data,m_);
      return m_.derived();
    }

    namespace internal
    {
      template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Mat>
      Mat & Miunit(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                   const DataTpl<Scalar,Options,JointCollectionTpl> & data,
                   const int col,
                   const Eigen::MatrixBase<Mat> & v)
      {
        EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat);

        PINOCCHIO_UNUSED_VARIABLE(model);
        assert(model.check(data) && "data is not consistent with model.");
        PINOCCHIO_CHECK_INPUT_ARGUMENT(col < model.nv && col >= 0);
        PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv);

        typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;

        const typename Data::MatrixXs & U = data.U;
        const std::vector<int> & nvt = data.nvSubtree_fromRow;
        Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,v);

        const int last_col = std::min<int>(col-1,model.nv-2); // You can start from nv-2 (no child in nv-1)
        v_.tail(model.nv - col - 1).setZero();
        v_[col] = 1.;
        for( int k=last_col;k>=0;--k )
        {
          int nvt_max = std::min<int>(col,nvt[(size_t)k]-1);
          v_[k] = -U.row(k).segment(k+1,nvt_max).dot(v_.segment(k+1,nvt_max));
        }

        v_.head(col+1).array() *= data.Dinv.head(col+1).array();

        for( int k=0;k<model.nv-1;++k ) // You can stop one step before nv.
        {
          int nvt_max = nvt[(size_t)k]-1;
          v_.segment(k+1,nvt_max) -= U.row(k).segment(k+1,nvt_max).transpose() * v_[k];
        }

        return v_.derived();
      }
    }// namespace internal

    template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Mat>
    Mat & computeMinv(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                      const DataTpl<Scalar,Options,JointCollectionTpl> & data,
                      const Eigen::MatrixBase<Mat> & Minv)
    {
      PINOCCHIO_CHECK_ARGUMENT_SIZE(Minv.rows(), model.nv);
      PINOCCHIO_CHECK_ARGUMENT_SIZE(Minv.cols(), model.nv);

      assert(model.check(data) && "data is not consistent with model.");

      Mat & Minv_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,Minv);

      for(int k = 0; k < model.nv; ++k)
        internal::Miunit(model,data,k,Minv_.col(k));

      return Minv_.derived();
    }

  } //   namespace cholesky
} // namespace pinocchio

/// @endcond

#endif // ifndef __pinocchio_cholesky_hxx__


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2015-2019 CNRS INRIA
3			//
4
5			#ifndef __pinocchio_cholesky_hxx__
6			#define __pinocchio_cholesky_hxx__
7
8			#include "pinocchio/algorithm/check.hpp"
9
10			/// @cond DEV
11
12			namespace pinocchio
13			{
14			namespace cholesky
15			{
16
17			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
18			inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::MatrixXs &
19		206	decompose(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
20			DataTpl<Scalar,Options,JointCollectionTpl> & data)
21			{
22			/*
23			* D = zeros(n,1);
24			* U = eye(n);
25			* for j=n:-1:1
26			* subtree = j+1:tree(j);
27			* D(j) = M(j,j) - U(j,subtree)diag(D(subtree))U(j,subtree)';
28			* i=parent(j);
29			* while i>0
30			* U(i,j) = (M(i,j) - U(i,subtree)diag(D(subtree))U(j,subtree)') / D(j);
31			* i=parent(i);
32			* end
33			* end
34			*/
35			PINOCCHIO_UNUSED_VARIABLE(model);
36	✓✗	206	assert(model.check(data) && "data is not consistent with model.");
37
38			typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
39
40		206	const typename Data::MatrixXs & M = data.M;
41		206	typename Data::MatrixXs & U = data.U;
42		206	typename Data::VectorXs & D = data.D;
43		206	typename Data::VectorXs & Dinv = data.Dinv;
44
45	✓✓	6784	for(int j=model.nv-1;j>=0;--j )
46			{
47		6578	const int NVT = data.nvSubtree_fromRow[(size_t)j]-1;
48	✓✗	6578	typename Data::VectorXs::SegmentReturnType DUt = data.tmp.head(NVT);
49	✓✓	6578	if(NVT)
50	✓✗✓✗ ✓✗✓✗ ✓✗✓✗	11508	DUt.noalias() = U.row(j).segment(j+1,NVT).transpose()
51	✓✗	5754	.cwiseProduct(D.segment(j+1,NVT));
52
53	✓✗✓✗ ✓✗✓✗ ✓✗	6578	D[j] = M(j,j) - U.row(j).segment(j+1,NVT).dot(DUt);
54	✓✗✓✗	6578	Dinv[j] = Scalar(1)/D[j];
55	✓✓	59157	for(int _i = data.parents_fromRow[(size_t)j]; _i >= 0;_i = data.parents_fromRow[(size_t)_i])
56	✓✗✓✗ ✓✗✓✗ ✓✗✓✗	52579	U(_i,j) = (M(_i,j) - U.row(_i).segment(j+1,NVT).dot(DUt)) * Dinv[j];
57			}
58
59		206	return data.U;
60			}
61
62			namespace internal
63			{
64			template<typename Mat, int ColsAtCompileTime = Mat::ColsAtCompileTime>
65			struct Uv
66			{
67			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
68			static void run(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
69			const DataTpl<Scalar,Options,JointCollectionTpl> & data,
70			const Eigen::MatrixBase<Mat> & m)
71			{
72			Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,m);
73			for(int k = 0; k < m_.cols(); ++k)
74			cholesky::Uv(model,data,m_.col(k));
75			}
76			};
77
78			template<typename Mat>
79			struct Uv<Mat,1>
80			{
81			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
82		4	static void run(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
83			const DataTpl<Scalar,Options,JointCollectionTpl> & data,
84			const Eigen::MatrixBase<Mat> & v)
85			{
86			typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
87
88			EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat)
89			PINOCCHIO_UNUSED_VARIABLE(model);
90	✓✗	4	assert(model.check(data) && "data is not consistent with model.");
91	✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗	4	PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv);
92
93		4	Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,v);
94
95		4	const typename Data::MatrixXs & U = data.U;
96		4	const std::vector<int> & nvt = data.nvSubtree_fromRow;
97
98	✓✓	128	for(int k=0;k < model.nv-1;++k) // You can stop one step before nv
99	✓✗✓✗ ✓✗✓✗ ✓✗	124	v_.row(k) += U.row(k).segment(k+1,nvt[(size_t)k]-1) * v_.middleRows(k+1,nvt[(size_t)k]-1);
100		4	}
101			};
102
103			} // namespace internal
104
105			/* Compute U*v.
106			* Nota: there is no smart way of doing UDv, so it is not proposed. */
107			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Mat>
108		4	Mat & Uv(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
109			const DataTpl<Scalar,Options,JointCollectionTpl> & data,
110			const Eigen::MatrixBase<Mat> & m)
111			{
112		4	Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,m);
113		4	internal::Uv<Mat,Mat::ColsAtCompileTime>::run(model,data,m_);
114		4	return m_.derived();
115			}
116
117			namespace internal
118			{
119			template<typename Mat, int ColsAtCompileTime = Mat::ColsAtCompileTime>
120			struct Utv
121			{
122			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
123			static void run(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
124			const DataTpl<Scalar,Options,JointCollectionTpl> & data,
125			const Eigen::MatrixBase<Mat> & m)
126			{
127			Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,m);
128			for(int k = 0; k < m_.cols(); ++k)
129			cholesky::Utv(model,data,m_.col(k));
130			}
131			};
132
133			template<typename Mat>
134			struct Utv<Mat,1>
135			{
136			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
137		3	static void run(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
138			const DataTpl<Scalar,Options,JointCollectionTpl> & data,
139			const Eigen::MatrixBase<Mat> & v)
140			{
141			typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
142
143			EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat)
144
145			PINOCCHIO_UNUSED_VARIABLE(model);
146	✓✗	3	assert(model.check(data) && "data is not consistent with model.");
147	✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗	3	PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv);
148		3	Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,v);
149
150		3	const typename Data::MatrixXs & U = data.U;
151		3	const std::vector<int> & nvt = data.nvSubtree_fromRow;
152
153	✓✓	96	for( int k=model.nv-2;k>=0;--k ) // You can start from nv-2 (no child in nv-1)
154	✓✗✓✗ ✓✗✓✗ ✓✗	93	v_.segment(k+1,nvt[(size_t)k]-1) += U.row(k).segment(k+1,nvt[(size_t)k]-1).transpose()*v_[k];
155		3	}
156			};
157
158			} // namespace internal
159
160			/* Compute U'v /
161			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Mat>
162		3	Mat & Utv(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
163			const DataTpl<Scalar,Options,JointCollectionTpl> & data,
164			const Eigen::MatrixBase<Mat> & m)
165			{
166		3	Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,m);
167		3	internal::Utv<Mat,Mat::ColsAtCompileTime>::run(model,data,m_);
168		3	return m_.derived();
169			}
170
171			namespace internal
172			{
173			template<typename Mat, int ColsAtCompileTime = Mat::ColsAtCompileTime>
174			struct Uiv
175			{
176			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
177		205	static void run(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
178			const DataTpl<Scalar,Options,JointCollectionTpl> & data,
179			const Eigen::MatrixBase<Mat> & m)
180			{
181		205	Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,m);
182	✓✓	1501	for(int k = 0; k < m_.cols(); ++k)
183	✓✗	1296	cholesky::Uiv(model,data,m_.col(k));
184		205	}
185			};
186
187			template<typename Mat>
188			struct Uiv<Mat,1>
189			{
190			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
191		3652	static void run(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
192			const DataTpl<Scalar,Options,JointCollectionTpl> & data,
193			const Eigen::MatrixBase<Mat> & v)
194			{
195			typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
196
197			EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat)
198	✓✗	3652	assert(model.check(data) && "data is not consistent with model.");
199	✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗	3652	PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv);
200		3652	Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,v);
201
202		3652	const typename Data::MatrixXs & U = data.U;
203		3652	const std::vector<int> & nvt = data.nvSubtree_fromRow;
204
205	✓✓	116472	for(int k=model.nv-2;k>=0;--k) // You can start from nv-2 (no child in nv-1)
206	✓✗✓✗ ✓✗✓✗	112820	v_[k] -= U.row(k).segment(k+1,nvt[(size_t)k]-1).dot(v_.segment(k+1,nvt[(size_t)k]-1));
207		3652	}
208			};
209
210			} // namespace internal
211
212			/* Compute U^{-1}*v
213			* Nota: there is no efficient way to compute D^{-1}U^{-1}v
214			* in a single loop, so algorithm is not proposed.*/
215			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Mat>
216		4062	Mat & Uiv(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
217			const DataTpl<Scalar,Options,JointCollectionTpl> & data,
218			const Eigen::MatrixBase<Mat> & m)
219			{
220		4062	Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,m);
221		4062	internal::Uiv<Mat,Mat::ColsAtCompileTime>::run(model,data,m_);
222		4062	return m_.derived();
223			}
224
225			namespace internal
226			{
227			template<typename Mat, int ColsAtCompileTime = Mat::ColsAtCompileTime>
228			struct Utiv
229			{
230			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
231			static void run(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
232			const DataTpl<Scalar,Options,JointCollectionTpl> & data,
233			const Eigen::MatrixBase<Mat> & m)
234			{
235			Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,m);
236			for(int k = 0; k < m_.cols(); ++k)
237			cholesky::Utiv(model,data,m_.col(k));
238			}
239			};
240
241			template<typename Mat>
242			struct Utiv<Mat,1>
243			{
244			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
245		671	static void run(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
246			const DataTpl<Scalar,Options,JointCollectionTpl> & data,
247			const Eigen::MatrixBase<Mat> & v)
248			{
249			typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
250
251			EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat)
252	✓✗	671	assert(model.check(data) && "data is not consistent with model.");
253	✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗	671	PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv);
254		671	Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,v);
255
256		671	const typename Data::MatrixXs & U = data.U;
257		671	const std::vector<int> & nvt = data.nvSubtree_fromRow;
258
259	✓✓	21416	for(int k=0; k<model.nv-1; ++k) // You can stop one step before nv.
260	✓✗✓✗ ✓✗✓✗ ✓✗	20745	v_.segment(k+1,nvt[(size_t)k]-1) -= U.row(k).segment(k+1,nvt[(size_t)k]-1).transpose() * v_[k];
261		671	}
262			};
263
264			} // namespace internal
265
266			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Mat>
267		671	Mat & Utiv(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
268			const DataTpl<Scalar,Options,JointCollectionTpl> & data,
269			const Eigen::MatrixBase<Mat> & m)
270			{
271		671	Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,m);
272		671	internal::Utiv<Mat,Mat::ColsAtCompileTime>::run(model,data,m_);
273		671	return m_.derived();
274			}
275
276			namespace internal
277			{
278			template<typename Mat, typename MatRes, int ColsAtCompileTime = Mat::ColsAtCompileTime>
279			struct Mv
280			{
281			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
282			static void run(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
283			const DataTpl<Scalar,Options,JointCollectionTpl> & data,
284			const Eigen::MatrixBase<Mat> & min,
285			const Eigen::MatrixBase<MatRes> & mout
286			)
287			{
288			MatRes & mout_ = PINOCCHIO_EIGEN_CONST_CAST(MatRes,mout);
289			for(int k = 0; k < min.cols(); ++k)
290			cholesky::Mv(model,data,min.col(k),mout_.col(k));
291			}
292			};
293
294			template<typename Mat, typename MatRes>
295			struct Mv<Mat,MatRes,1>
296			{
297			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
298		2	static void run(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
299			const DataTpl<Scalar,Options,JointCollectionTpl> & data,
300			const Eigen::MatrixBase<Mat> & vin,
301			const Eigen::MatrixBase<MatRes> & vout)
302			{
303			typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
304
305			EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat)
306			EIGEN_STATIC_ASSERT_VECTOR_ONLY(MatRes)
307
308			PINOCCHIO_UNUSED_VARIABLE(model);
309	✓✗	2	assert(model.check(data) && "data is not consistent with model.");
310	✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗	2	PINOCCHIO_CHECK_ARGUMENT_SIZE(vin.size(), model.nv);
311	✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗	2	PINOCCHIO_CHECK_ARGUMENT_SIZE(vout.size(), model.nv);
312
313		2	MatRes & vout_ = PINOCCHIO_EIGEN_CONST_CAST(MatRes,vout);
314
315		2	const typename Data::MatrixXs & M = data.M;
316		2	const std::vector<int> & nvt = data.nvSubtree_fromRow;
317
318	✓✓	66	for(int k=model.nv-1;k>=0;--k)
319			{
320	✓✗✓✗ ✓✗✓✗ ✓✗	64	vout_[k] = M.row(k).segment(k,nvt[(size_t)k]) * vin.segment(k,nvt[(size_t)k]);
321	✓✗✓✗ ✓✗✓✗ ✓✗	64	vout_.segment(k+1,nvt[(size_t)k]-1) += M.row(k).segment(k+1,nvt[(size_t)k]-1).transpose()*vin[k];
322			}
323		2	}
324			};
325
326			} // namespace internal
327
328			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Mat, typename MatRes>
329		2	MatRes & Mv(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
330			const DataTpl<Scalar,Options,JointCollectionTpl> & data,
331			const Eigen::MatrixBase<Mat> & min,
332			const Eigen::MatrixBase<MatRes> & mout)
333			{
334		2	MatRes & mout_ = PINOCCHIO_EIGEN_CONST_CAST(MatRes,mout);
335		2	internal::Mv<Mat,MatRes,Mat::ColsAtCompileTime>::run(model,data,min.derived(),mout_);
336		2	return mout_.derived();
337			}
338
339			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Mat>
340		2	typename PINOCCHIO_EIGEN_PLAIN_TYPE(Mat) Mv(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
341			const DataTpl<Scalar,Options,JointCollectionTpl> & data,
342			const Eigen::MatrixBase<Mat> & min)
343			{
344			typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Mat) ReturnType;
345	✓✗✓✗	4	ReturnType res(model.nv,min.cols());
346	✓✗✓✗	4	return Mv(model,data,min,res);
347			}
348
349			namespace internal
350			{
351			template<typename Mat, int ColsAtCompileTime = Mat::ColsAtCompileTime>
352			struct UDUtv
353			{
354			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
355			static void run(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
356			const DataTpl<Scalar,Options,JointCollectionTpl> & data,
357			const Eigen::MatrixBase<Mat> & m)
358			{
359			Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,m);
360			for(int k = 0; k < m_.cols(); ++k)
361			cholesky::UDUtv(model,data,m_.col(k));
362			}
363			};
364
365			template<typename Mat>
366			struct UDUtv<Mat,1>
367			{
368			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
369		2	static void run(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
370			const DataTpl<Scalar,Options,JointCollectionTpl> & data,
371			const Eigen::MatrixBase<Mat> & v)
372			{
373			EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat)
374
375	✓✗	2	assert(model.check(data) && "data is not consistent with model.");
376	✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗	2	PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv);
377
378		2	Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,v);
379
380		2	cholesky::Utv(model,data,v_);
381	✓✗✓✗	2	v_.array() *= data.D.array();
382		2	cholesky::Uv(model,data,v_);
383		2	}
384			};
385
386			} // namespace internal
387
388			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Mat>
389		2	Mat & UDUtv(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
390			const DataTpl<Scalar,Options,JointCollectionTpl> & data,
391			const Eigen::MatrixBase<Mat> & m)
392			{
393		2	Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,m);
394
395		2	internal::UDUtv<Mat>::run(model,data,m_);
396		2	return m_;
397			}
398
399			namespace internal
400			{
401			template<typename Mat, int ColsAtCompileTime = Mat::ColsAtCompileTime>
402			struct solve
403			{
404			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
405		3	static void run(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
406			const DataTpl<Scalar,Options,JointCollectionTpl> & data,
407			const Eigen::MatrixBase<Mat> & m)
408			{
409		3	Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,m);
410	✓✓	99	for(int k = 0; k < m_.cols(); ++k)
411	✓✗	96	cholesky::solve(model,data,m_.col(k));
412		3	}
413			};
414
415			template<typename Mat>
416			struct solve<Mat,1>
417			{
418			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
419		638	static void run(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
420			const DataTpl<Scalar,Options,JointCollectionTpl> & data,
421			const Eigen::MatrixBase<Mat> & v)
422			{
423			EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat)
424
425	✓✗	638	assert(model.check(data) && "data is not consistent with model.");
426
427		638	Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,v);
428
429		638	cholesky::Uiv(model,data,v_);
430	✓✗✓✗	638	v_.array() *= data.Dinv.array();
431		638	cholesky::Utiv(model,data,v_);
432		638	}
433			};
434
435			} // namespace internal
436
437			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Mat>
438		533	Mat & solve(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
439			const DataTpl<Scalar,Options,JointCollectionTpl> & data,
440			const Eigen::MatrixBase<Mat> & m)
441			{
442		533	Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,m);
443		533	internal::solve<Mat,Mat::ColsAtCompileTime>::run(model,data,m_);
444		533	return m_.derived();
445			}
446
447			namespace internal
448			{
449			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Mat>
450		256	Mat & Miunit(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
451			const DataTpl<Scalar,Options,JointCollectionTpl> & data,
452			const int col,
453			const Eigen::MatrixBase<Mat> & v)
454			{
455			EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat);
456
457			PINOCCHIO_UNUSED_VARIABLE(model);
458	✓✗	256	assert(model.check(data) && "data is not consistent with model.");
459	✓✗✗✓ ✗✗	256	PINOCCHIO_CHECK_INPUT_ARGUMENT(col < model.nv && col >= 0);
460	✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗	256	PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv);
461
462			typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
463
464		256	const typename Data::MatrixXs & U = data.U;
465		256	const std::vector<int> & nvt = data.nvSubtree_fromRow;
466		256	Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,v);
467
468		256	const int last_col = std::min<int>(col-1,model.nv-2); // You can start from nv-2 (no child in nv-1)
469	✓✗	256	v_.tail(model.nv - col - 1).setZero();
470		256	v_[col] = 1.;
471	✓✓	4224	for( int k=last_col;k>=0;--k )
472			{
473		3968	int nvt_max = std::min<int>(col,nvt[(size_t)k]-1);
474	✓✗✓✗ ✓✗✓✗	3968	v_[k] = -U.row(k).segment(k+1,nvt_max).dot(v_.segment(k+1,nvt_max));
475			}
476
477	✓✗✓✗ ✓✗✓✗	256	v_.head(col+1).array() *= data.Dinv.head(col+1).array();
478
479	✓✓	8192	for( int k=0;k<model.nv-1;++k ) // You can stop one step before nv.
480			{
481		7936	int nvt_max = nvt[(size_t)k]-1;
482	✓✗✓✗ ✓✗✓✗ ✓✗	7936	v_.segment(k+1,nvt_max) -= U.row(k).segment(k+1,nvt_max).transpose() * v_[k];
483			}
484
485		256	return v_.derived();
486			}
487			}// namespace internal
488
489			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Mat>
490		3	Mat & computeMinv(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
491			const DataTpl<Scalar,Options,JointCollectionTpl> & data,
492			const Eigen::MatrixBase<Mat> & Minv)
493			{
494	✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗	3	PINOCCHIO_CHECK_ARGUMENT_SIZE(Minv.rows(), model.nv);
495	✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗	3	PINOCCHIO_CHECK_ARGUMENT_SIZE(Minv.cols(), model.nv);
496
497	✗✗	3	assert(model.check(data) && "data is not consistent with model.");
498
499		3	Mat & Minv_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,Minv);
500
501	✗✗	99	for(int k = 0; k < model.nv; ++k)
502	✗✗	96	internal::Miunit(model,data,k,Minv_.col(k));
503
504		3	return Minv_.derived();
505			}
506
507			} // namespace cholesky
508			} // namespace pinocchio
509
510			/// @endcond
511
512			#endif // ifndef __pinocchio_cholesky_hxx__