// // Copyright (c) 2015 CNRS // // This file is part of Pinocchio // Pinocchio is free software: you can redistribute it // and/or modify it under the terms of the GNU Lesser General Public // License as published by the Free Software Foundation, either version // 3 of the License, or (at your option) any later version. // // Pinocchio is distributed in the hope that it will be // useful, but WITHOUT ANY WARRANTY; without even the implied warranty // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU // General Lesser Public License for more details. You should have // received a copy of the GNU Lesser General Public License along with // Pinocchio If not, see // <http://www.gnu.org/licenses/>. #include <iostream> #include <Eigen/Core> #include <pinocchio/spatial/skew.hpp> #define BOOST_TEST_DYN_LINK #define BOOST_TEST_MODULE UdutTest #include <boost/test/unit_test.hpp> template<int N> void udut( Eigen::Matrix<double,N,N> & M ) { typedef Eigen::Matrix<double,N,N> MatrixNd; typedef Eigen::Matrix<double,1,N> VectorNd; typedef typename MatrixNd::DiagonalReturnType D_t; typedef typename MatrixNd::template TriangularViewReturnType<Eigen::StrictlyUpper>::Type U_t; VectorNd tmp; D_t D = M.diagonal(); U_t U = M.template triangularView<Eigen::StrictlyUpper>(); for(int j=N-1;j>=0;--j ) { typename VectorNd::SegmentReturnType DUt = tmp.tail(N-j-1); if( j<N-1 ) DUt = M.row(j).tail(N-j-1).transpose().cwiseProduct( D.tail(N-j-1) ); D[j] -= M.row(j).tail(N-j-1).dot(DUt); for(int i=j-1;i>=0;--i) { U(i,j) -= M.row(i).tail(N-j-1).dot(DUt); U(i,j) /= D[j]; } } } BOOST_AUTO_TEST_SUITE ( Udut ) BOOST_AUTO_TEST_CASE ( udut ) { using namespace Eigen; Matrix<double,6,6> A = Matrix<double,6,6>::Random(); A = A*A.transpose(); double m = A(1,1); Matrix3d I = A.bottomRightCorner<3,3>(); Vector3d c = A.block<3,1>(0,3); A.topLeftCorner<3,3>() = Matrix3d::Identity()*m; A.topRightCorner<3,3>() = se3::skew(c).transpose(); A.bottomLeftCorner<3,3>() = se3::skew(c); A.bottomRightCorner<3,3>() -= A.bottomLeftCorner<3,3>()*A.bottomLeftCorner<3,3>(); std::cout << "A = [\n" << A << "];" << std::endl; udut(A); std::cout << "U = [\n" << A << "];" << std::endl; } BOOST_AUTO_TEST_SUITE_END()