diff --git a/include/roboptim/core/detail/structured-input.hh b/include/roboptim/core/detail/structured-input.hh new file mode 100644 index 000000000..24042415a --- /dev/null +++ b/include/roboptim/core/detail/structured-input.hh @@ -0,0 +1,134 @@ +// Copyright (C) 2015 by Grégoire Duchemin, AIST, CNRS, EPITA +// +// This file is part of the roboptim. +// +// roboptim 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. +// +// roboptim 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 Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with roboptim. If not, see . + +#ifndef ROBOPTIM_CORE_DETAIL_STRUCTURED_INPUT_HH +# define ROBOPTIM_CORE_DETAIL_STRUCTURED_INPUT_HH +# include +# include + +# include + +# include + +namespace roboptim +{ + namespace detail + { + /// \brief Gives access to a std::vector of std::pair representing blocks of input + /// Those blocks are needed even if the function is not differentiable, + /// making it easier for us to define it in a exterior class to then use it transparently. + struct BlockProvider + { + /// \brief stores the blocks defined by the function as a pair of integers. + /// The first element is the first index of the block, the second element + /// repersetn the total size of the block. + std::vector > blocks; + }; + + /// \brief This class gives access to the getJacobianBlock() method, which implementation + /// is to be spacialized according to the type of matrix handled by the function. + template + struct StructuredInputJacobianInternal : public BlockProvider + {}; + + /// \brief DenseMatrix specialization of the getJacobianBlock() method provider. + template + struct StructuredInputJacobianInternal : public BlockProvider + { + /// \brief return type of the getJacobianBlock() method + typedef typename FuncType::jacobian_ref JacBlock; + /// \brief input type of the jacobian given by the user + typedef typename FuncType::jacobian_ref InputJacBlock; + + /// \brief retrieve a specific block of the jacobian given as an input + /// \param jacobian the jacobian to operate upon + /// \param blockInd the index of the block to be retrieved + JacBlock getJacobianBlock(InputJacBlock jacobian, size_t blockInd) const; + }; + + /// \brief SparseMatrix specialization of the getJacobianBlock() method provider. + template + struct StructuredInputJacobianInternal : public BlockProvider + { + /// \brief return type of the getJacobianBlock() method + // Why is the InputPanel set to false ? According to the documentation, the + // InputPanel parameter allows eigen to check if aligend access to the memory is feasible. + // In the Sparse specialization of the BlockImpl class, we can see that if it is + // set to false, it will use the default template implementation, which is explicitly + // made read-only. Therefore, we should want it to be set to true, so as to enable + // the read-access we want for the returned block of the jacobian matrix. + // + // ...only, the compiler expects a inputpanel value of 0 (false), here, and actually tells + // us, for a value of one, that there is no way of writing anythig to the extracted + // submatrix. + // + // This may be due to some inheritance shenanigans we missed in the documentation that + // reverse the meaning of the InputPanel attribute, but for lack of a better explanation, + // we just use the magical value "false" here. Everything compiles and works in both + // RowMajor and ColMajor mode, so we get the result we expected. + typedef typename Eigen::Block JacBlock; + /// \brief input type of the jacobian given by the user + typedef typename FuncType::jacobian_ref InputJacBlock; + + /// \brief retrieve a specific block of the jacobian given as an input + /// \param jacobian the jacobian to operate upon + /// \param blockInd the index of the block to be retrieved + JacBlock getJacobianBlock(InputJacBlock jacobian, size_t blockInd) const; + }; + + /// \brief Provides utility methods to describe the input format of a function + /// + /// Inheriting from this class will allow a function to define "blocks" of input. + /// You can then retrieve a specific block from the whole input, or the specific + /// part of the jacobian depending on this block. + /// + /// To inherit from this class, please specify as the template argument the roboptim + /// function type of the function (Function, DifferentiableFunction, LinearFunction, + /// et cetera) + template + class StructuredInput + // This boost::conditional allows us to not define the getJacobianBlock method + // if the function is not differentiable. + : public boost::conditional< + boost::is_base_of, FuncType>::value, + StructuredInputJacobianInternal, + BlockProvider> + ::type + { + public: + /// \brief Adds a new block of input to the function + /// \param size size of the block to add + void addBlock(size_t size); + /// \brief Returns the number of blocks defined by the function + size_t getNumBlocks() const; + + /// \brief return type of the getInputBlock() method + typedef typename Eigen::Ref::ConstSegmentReturnType ConstSegment; + + /// \brief Reads a specified block of data from an input argument + /// + /// \param input the input data to be read + /// \param blockInd the index of the block to be read + ConstSegment getInputBlock(typename FuncType::const_argument_ref input, size_t blockInd) const; + }; + + } // end of namespace detail. +} // end of namespace roboptim. + +# include + +#endif //! ROBOPTIM_CORE_DETAIL_STRUCTURED_INPUT_HH diff --git a/include/roboptim/core/detail/structured-input.hxx b/include/roboptim/core/detail/structured-input.hxx new file mode 100644 index 000000000..33303c98d --- /dev/null +++ b/include/roboptim/core/detail/structured-input.hxx @@ -0,0 +1,91 @@ +// Copyright (C) 2015 by Grégoire Duchemin, AIST, CNRS, EPITA +// +// This file is part of the roboptim. +// +// roboptim 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. +// +// roboptim 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 Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with roboptim. If not, see . + +#ifndef ROBOPTIM_CORE_DETAIL_STRUCTURED_INPUT_HXX +# define ROBOPTIM_CORE_DETAIL_STRUCTURED_INPUT_HXX + +namespace roboptim +{ + namespace detail + { + // This is the only way we found to get a submatrix from a Sparse matrix + // while still granting us writing rights to said submatrix. + // This will become useless when Eigen will let us write in .block() + // obtained from Sparse matrices. + template + Eigen::Block + getBlock (Eigen::SparseMatrixBase& m, int colStart, int colLength) + { + return Eigen::Block (m.derived (), 0, colStart, m.rows (), colLength); + } + + template + void StructuredInput::addBlock(size_t size) + { + size_t startingIndex = 0; + + if (BlockProvider::blocks.size() != 0) + { + std::pair lastPair = BlockProvider::blocks[BlockProvider::blocks.size()-1]; + startingIndex = lastPair.first + lastPair.second; + } + + BlockProvider::blocks.push_back(std::make_pair(startingIndex, size)); + } + + template + size_t StructuredInput::getNumBlocks() const + { + return BlockProvider::blocks.size(); + } + + template + typename StructuredInput::ConstSegment StructuredInput::getInputBlock(typename U::const_argument_ref input, size_t blockInd) const + { + assert(blockInd < BlockProvider::blocks.size()); + + return input.segment(static_cast(BlockProvider::blocks[blockInd].first), static_cast(BlockProvider::blocks[blockInd].second)); + } + + template + typename StructuredInputJacobianInternal::JacBlock + StructuredInputJacobianInternal::getJacobianBlock + (typename StructuredInputJacobianInternal::InputJacBlock jacobian, + size_t blockInd) + const + { + assert(blockInd < blocks.size()); + + return jacobian.middleCols(static_cast(blocks[blockInd].first), static_cast(blocks[blockInd].second)); + } + + template + typename StructuredInputJacobianInternal::JacBlock + StructuredInputJacobianInternal::getJacobianBlock + (typename StructuredInputJacobianInternal::InputJacBlock jacobian, + size_t blockInd) + const + { + assert(blockInd < blocks.size()); + + return getBlock(jacobian, static_cast(blocks[blockInd].first), static_cast(blocks[blockInd].second)); + } + + } // end of namespace detail. +} // end of namespace roboptim. + +#endif //! ROBOPTIM_CORE_DETAIL_STRUCTURED_INPUT_HXX diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 7388b0c51..be7c04bee 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -77,6 +77,7 @@ ROBOPTIM_CORE_TEST(interval) ROBOPTIM_CORE_TEST(util) ROBOPTIM_CORE_TEST(indent) ROBOPTIM_CORE_TEST(detail-utility) +ROBOPTIM_CORE_TEST(detail-structured-input) ROBOPTIM_CORE_TEST(cache) ROBOPTIM_CORE_TEST(function) ROBOPTIM_CORE_TEST(derivable-function) diff --git a/tests/detail-structured-input.cc b/tests/detail-structured-input.cc new file mode 100644 index 000000000..eda24a87b --- /dev/null +++ b/tests/detail-structured-input.cc @@ -0,0 +1,184 @@ +// Copyright (C) 2015 by Grégoire Duchemin, AIST, CNRS, EPITA +// +// This file is part of the roboptim. +// +// roboptim 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. +// +// roboptim 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 Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with roboptim. If not, see . + +#include "shared-tests/fixture.hh" + +#include + +#include +#include + +#include + +typedef boost::mpl::list< ::roboptim::EigenMatrixDense + ,::roboptim::EigenMatrixSparse + > functionTypes_t; + +template +struct F : public roboptim::GenericDifferentiableFunction, + public roboptim::detail::StructuredInput > +{ + ROBOPTIM_DIFFERENTIABLE_FUNCTION_FWD_TYPEDEFS_ + (roboptim::GenericDifferentiableFunction); + + F () : roboptim::GenericDifferentiableFunction (22, 10, "f_n (x) = n * x") + { + this->addBlock(5); + this->addBlock(12); + this->addBlock(5); + } + + void impl_compute (result_ref res, const_argument_ref argument) const + { + res.setZero (); + for (size_type i = 0; i < this->outputSize (); ++i) + for (size_type j = 0; j < 3; ++j) + { + res[i] += (value_type)i * argument[19 + j]; + } + } + + void impl_gradient (gradient_ref, const_argument_ref, + size_type) const + { + } + + void impl_jacobian (jacobian_ref jac, const_argument_ref input) const + { + for(int i = 0; i < this->outputSize(); ++i) + { + this->getJacobianBlock(jac, 0).row(i) << this->getInputBlock(input, 2).transpose(); + this->getJacobianBlock(jac, 1).row(i) << this->getInputBlock(input, 1).transpose(); + this->getJacobianBlock(jac, 2).row(i) << this->getInputBlock(input, 0).transpose(); + } + } + +}; + +template <> +inline void +F::impl_gradient(gradient_ref grad, const_argument_ref, size_type functionId) const +{ + grad.setZero (); + for (size_type j = 0; j < 3; ++j) + { + grad.insert (19 + j) += (value_type)functionId; + } +} + +template <> +inline void +F::impl_jacobian (jacobian_ref jac, const_argument_ref input) const + { + for(int i = 0; i < this->outputSize(); ++i) + { + for(int j = 0; j < 5; ++j) + { + this->getJacobianBlock(jac, 0).coeffRef(i, j) = this->getInputBlock(input, 2)[j]; + } + + for(int j = 5; j < 12 + 5; ++j) + { + this->getJacobianBlock(jac, 0).coeffRef(i, j) = this->getInputBlock(input, 1)[j - 5]; + } + + for(int j = 12 + 5; j < 5 + 12 + 5; ++j) + { + this->getJacobianBlock(jac, 0).coeffRef(i, j) = this->getInputBlock(input, 0)[j - (12 + 5)]; + } + + + } + } + +template +struct G : public roboptim::GenericFunction, + public roboptim::detail::StructuredInput > +{ + ROBOPTIM_FUNCTION_FWD_TYPEDEFS_ + (roboptim::GenericFunction); + + G () : roboptim::GenericFunction (22, 10, "g_n (x) = n * x") + { + this->addBlock(5); + this->addBlock(12); + this->addBlock(5); + } + + void impl_compute (result_ref res, const_argument_ref argument) const + { + res.setZero (); + for (size_type i = 0; i < this->outputSize (); ++i) + for (size_type j = 0; j < 3; ++j) + { + res[i] += (value_type)i * argument[19 + j]; + } + } + +}; + +boost::shared_ptr output; + +BOOST_FIXTURE_TEST_SUITE (core, TestSuiteConfiguration) + +BOOST_AUTO_TEST_CASE_TEMPLATE (manifold_map_test_0, T, functionTypes_t) +{ + F f; + + // F is a DifferentiableFunction, and as such exposes the getJacobianBlock() method + BOOST_STATIC_ASSERT(boost::is_base_of::parent_t, typename F::traits_t>, F >::value); + // G is just a Function, and as such does not expose the getJacobianBlock() method + BOOST_STATIC_ASSERT(!boost::is_base_of::parent_t, typename G::traits_t>, G >::value); + + typename F::argument_t arg(22); + for(int i = 0; i < arg.size(); ++i) + { + arg(i) = i + 1; + } + + typename F::jacobian_t jacobian(10, 22); + jacobian.setZero(); + + f.jacobian(jacobian, arg); + + Eigen::MatrixXd denseJac(jacobian); + + for (int i = 0; i < f.outputSize(); ++i) + { + int j = 0; + + for(; j < 5; ++j) + { + BOOST_CHECK(denseJac(i, j) == 12 + 5 + 1 + j); + } + + for(; j < 5 + 12; ++j) + { + BOOST_CHECK(denseJac(i, j) == j + 1); + } + + for(; j < 5 + 12 + 5; ++j) + { + BOOST_CHECK(denseJac(i, j) == j - (12 + 5) + 1); + } + + } + + +} + +BOOST_AUTO_TEST_SUITE_END ()