Commit 2bda112d by jcarpent

### [C++] Cholesky decomposition now handles matrices rhs operand

parent 698c2cfb
 ... ... @@ -56,7 +56,7 @@ namespace se3 /// /// \param[in] model The model structure of the rigid body system. /// \param[in] data The data structure of the rigid body system. /// \param[inout] y The input vector to inverse which also contains the result \f$x\f$ of the inversion. /// \param[inout] y The input matrix to inverse which also contains the result \f$x\f$ of the inversion. /// template Mat & solve(const Model & model, const Data & data, ... ... @@ -67,7 +67,7 @@ namespace se3 /// /// \param[in] model The model structure of the rigid body system. /// \param[in] data The data structure of the rigid body system. /// \param[inout] v The input vector to multiply with data.M and storing the result. /// \param[inout] v The input matrix to multiply with data.M and storing the result. /// \param[in] usingCholesky If true, use the Cholesky decomposition stored in data. Exploit the sparsity of the kinematic tree. /// /// \return A reference to the result of \f$Mv \f$. ... ... @@ -82,7 +82,7 @@ namespace se3 /// /// \param[in] model The model structure of the rigid body system. /// \param[in] data The data structure of the rigid body system. /// \param[inout] v The input vector to multiply with data.U and also storing the result. /// \param[inout] v The input matrix to multiply with data.U and also storing the result. /// /// \return A reference to the result of \f$Uv \f$ stored in v. /// ... ... @@ -96,7 +96,7 @@ namespace se3 /// /// \param[in] model The model structure of the rigid body system. /// \param[in] data The data structure of the rigid body system. /// \param[inout] v The input vector to multiply with data.U.tranpose() and also storing the result. /// \param[inout] v The input matrix to multiply with data.U.tranpose() and also storing the result. /// /// \return A reference to the result of \f$U^{\top}v \f$ stored in v. /// ... ... @@ -110,7 +110,7 @@ namespace se3 /// /// \param[in] model The model structure of the rigid body system. /// \param[in] data The data structure of the rigid body system. /// \param[inout] v The input vector to multiply with data.U^{-1} and also storing the result. /// \param[inout] v The input matrix to multiply with data.U^{-1} and also storing the result. /// /// \return A reference to the result of \f$U^{-1}v \f$ stored in v. /// ... ... @@ -126,7 +126,7 @@ namespace se3 /// /// \param[in] model The model structure of the rigid body system. /// \param[in] data The data structure of the rigid body system. /// \param[inout] v The input vector to multiply with data.U^{-\top} and also storing the result. /// \param[inout] v The input matrix to multiply with data.U^{-\top} and also storing the result. /// /// \return A reference to the result of \f$U^{-\top}v \f$ stored in v. /// ... ... @@ -142,7 +142,7 @@ namespace se3 /// /// \param[in] model The model structure of the rigid body system. /// \param[in] data The data structure of the rigid body system. /// \param[inout] v The input vector to multiply with data.M^{-1} and also storing the result. /// \param[inout] v The input matrix to multiply with data.M^{-1} and also storing the result. /// /// \return A reference to the result of \f$M^{-1}v \f$ stored in v. /// ... ...
 ... ... @@ -71,13 +71,13 @@ namespace se3 const Data & data, Eigen::MatrixBase & v) { assert(v.size() == model.nv); EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat); assert(v.rows() == model.nv); const Eigen::MatrixXd & U = data.U; const std::vector & nvt = data.nvSubtree_fromRow; for(int k=0;k < model.nv-1;++k) // You can stop one step before nv v[k] += U.row(k).segment(k+1,nvt[(Model::Index)k]-1) * v.segment(k+1,nvt[(Model::Index)k]-1); v.row(k) += U.row(k).segment(k+1,nvt[(Model::Index)k]-1) * v.middleRows(k+1,nvt[(Model::Index)k]-1); return v.derived(); } ... ... @@ -88,13 +88,12 @@ namespace se3 const Data & data, Eigen::MatrixBase & v) { assert(v.size() == model.nv); EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat); assert(v.rows() == model.nv); const Eigen::MatrixXd & U = data.U; const std::vector & nvt = data.nvSubtree_fromRow; for( int i=model.nv-2;i>=0;--i ) // You can start from nv-2 (no child in nv-1) v.segment(i+1,nvt[(Model::Index)i]-1) += U.row(i).segment(i+1,nvt[(Model::Index)i]-1).transpose()*v[i]; for( int k=model.nv-2;k>=0;--k ) // You can start from nv-2 (no child in nv-1) v.middleRows(k+1,nvt[(Model::Index)k]-1) += U.row(k).segment(k+1,nvt[(Model::Index)k]-1).transpose()*v.row(k); return v.derived(); } ... ... @@ -109,14 +108,13 @@ namespace se3 { /* We search y s.t. v = U y. * For any k, v_k = y_k + U_{k,k+1:} y_{k+1:} */ assert(v.size() == model.nv); EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat); assert(v.rows() == model.nv); const Eigen::MatrixXd & U = data.U; const std::vector & 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[(Model::Index)k]-1) * v.segment(k+1,nvt[(Model::Index)k]-1); v.row(k) -= U.row(k).segment(k+1,nvt[(Model::Index)k]-1) * v.middleRows(k+1,nvt[(Model::Index)k]-1); return v.derived(); } ... ... @@ -127,13 +125,12 @@ namespace se3 { /* We search y s.t. v = U' y. * For any k, v_k = y_k + sum_{m \in parent{k}} U(m,k) v(k). */ assert(v.size() == model.nv); EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat); assert(v.rows() == model.nv); const Eigen::MatrixXd & U = data.U; const std::vector & nvt = data.nvSubtree_fromRow; for( int i=0;i & v) { assert(v.size() == model.nv); EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat); assert(v.rows() == model.nv); const Eigen::MatrixXd & M = data.M; const std::vector & nvt = data.nvSubtree_fromRow; ... ... @@ -154,8 +150,8 @@ namespace se3 for(int k=model.nv-1;k>=0;--k) { res[k] = M.row(k).segment(k,nvt[(Model::Index)k]) * v.segment(k,nvt[(Model::Index)k]); res.segment(k+1,nvt[(Model::Index)k]-1) += M.row(k).segment(k+1,nvt[(Model::Index)k]-1).transpose()*v[k]; res.row(k) = M.row(k).segment(k,nvt[(Model::Index)k]) * v.middleRows(k,nvt[(Model::Index)k]); res.middleRows(k+1,nvt[(Model::Index)k]-1) += M.row(k).segment(k+1,nvt[(Model::Index)k]-1).transpose()*v.row(k); } return res; ... ... @@ -167,7 +163,7 @@ namespace se3 Eigen::MatrixBase & v) { Utv(model,data,v); for( int k=0;k & v) { Uiv(model,data,v); for(int k=0;k
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!