Completed the side algorithms of Cholesky.

src/algorithm/cholesky.hpp

@@ -75,8 +75,8 @@ namespace se3
       const Eigen::MatrixXd & U = data.U;
       const std::vector<int> & nvt = data.nvSubtree_fromRow;
 
-      for( int j=0;j<model.nv-1;++j ) // You can stop one step before nv
-	v[j] += U.row(j).segment(j+1,nvt[j]-1) * v.segment(j+1,nvt[j]-1);
+      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[k]-1) * v.segment(k+1,nvt[k]-1);
 
       return v.derived();
     }
@@ -106,7 +106,7 @@ namespace se3
     Mat &
     Uiv( const Model &                  model, 
 	 const Data&                          data ,
-	  Eigen::MatrixBase<Mat> &   v)
+	 Eigen::MatrixBase<Mat> &   v)
     {
       /* We search y s.t. v = U y. 
        * For any k, v_k = y_k + U_{k,k+1:} y_{k+1:} */
@@ -141,7 +141,53 @@ namespace se3
       return v.derived();
     }
 
-    
+    namespace internal
+    {
+      template<typename Mat>
+      Mat
+      Mv( const Model &                  model, 
+	  const  Data&                          data ,
+	  const Eigen::MatrixBase<Mat> &   v)
+      {
+	assert(v.size() == model.nv);
+	EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat);
+      
+	const Eigen::MatrixXd & M = data.M;
+	const std::vector<int> & nvt = data.nvSubtree_fromRow;
+	Mat res(model.nv);
+
+	for( int k=model.nv-1;k>=0;--k ) 
+	  {
+	    res[k] = M.row(k).segment(k,nvt[k]) * v.segment(k,nvt[k]);
+	    res.segment(k+1,nvt[k]-1) += M.row(k).segment(k+1,nvt[k]-1).transpose()*v[k];
+	  }
+
+	return v.derived();
+      }
+
+      template<typename Mat>
+      Mat &
+      UDUtv( const Model &              model, 
+	     const Data&                data ,
+	     Eigen::MatrixBase<Mat> &   v)
+      {
+	Utv(model,data,v);
+	for( int k=0;k<model.nv;++k ) v[k] *= data.D[k];
+	return Uv(model,data,v);
+      }
+    } // internal
+  
+    template<typename Mat>
+    Mat &
+    Mv( const Model &                  model, 
+	const  Data&                          data ,
+	Eigen::MatrixBase<Mat> &   v,
+	bool usingCholesky = false)
+    {
+      if(usingCholesky) return internal::UDUtv(model,data,v);
+      else return v = internal::Mv(model,data,v);
+    }
+
     template<typename Mat>
     Mat &
     solve( const Model &              model, 

unittest/cholesky.cpp

 #ifdef NDEBUG
 #pragma GCC diagnostic ignored "-Wunused-but-set-variable"
+#pragma GCC diagnostic ignored "-Wunused-variable"
 #endif
 
 #include "pinocchio/spatial/fwd.hpp"
@@ -18,14 +19,16 @@
 #include "pinocchio/tools/timer.hpp"
 #include "pinocchio/multibody/parser/sample-models.hpp"
 
-//#define __SSE3__
-#include <fenv.h>
-#ifdef __SSE3__
-#include <pmmintrin.h>
-#endif
-
+#include <boost/utility/binary.hpp>
 
-void timings(const se3::Model & model, se3::Data& data, int flag = 1)
+/* The flag triger the following timers:
+ * 00001: sparse UDUt cholesky
+ * 00010: dense Eigen LDLt cholesky (with pivot)
+ * 00100: sparse resolution 
+ * 01000: sparse U*v multiplication
+ * 10000: sparse U'*v multiplication
+ */
+void timings(const se3::Model & model, se3::Data& data, long flag)
 {
   StackTicToc timer(StackTicToc::US); 
 #ifdef NDEBUG
@@ -34,9 +37,10 @@ void timings(const se3::Model & model, se3::Data& data, int flag = 1)
   const int NBT = 1;
 #endif
 
-  bool verbose = flag & (flag - 1); // True is two or more binaries of the flag are 1.
+  bool verbose = flag & (flag-1) ; // True is two or more binaries of the flag are 1.
+  if(verbose) std::cout <<"--" << std::endl;
 
-  if( flag & 1 )
+  if( flag >> 0 & 1 )
     {
       timer.tic();
       SMOOTH(NBT)
@@ -47,7 +51,7 @@ void timings(const se3::Model & model, se3::Data& data, int flag = 1)
       timer.toc(std::cout,NBT);
     }
 
-  if( flag & 2 )
+  if( flag >> 1 & 1 )
     {
       timer.tic();
       Eigen::VectorXd v = Eigen::VectorXd::Random(model.nv);
@@ -61,26 +65,69 @@ void timings(const se3::Model & model, se3::Data& data, int flag = 1)
       timer.toc(std::cout,NBT);
     }
 
-  if( flag & 4 )
+
+  if( flag >> 2 & 31 )
     {
       std::vector<Eigen::VectorXd> randvec(NBT);
       for(int i=0;i<NBT;++i ) randvec[i] = Eigen::VectorXd::Random(model.nv);
       Eigen::VectorXd zero = Eigen::VectorXd(model.nv);
       Eigen::VectorXd res (model.nv);
-      timer.tic();
-      SMOOTH(NBT)
-      {
-	//se3::cholesky::Uv(model,data,randvec[_smooth]);
-	// se3::cholesky::Utv(model,data,randvec[_smooth]);
-	//se3::cholesky::Uiv(model,data,randvec[_smooth]);
-	//se3::cholesky::Utiv(model,data,randvec[_smooth]);
-	se3::cholesky::solve(model,data,randvec[_smooth]);
-      }
-      if(verbose) std::cout << "Uv =\t\t";
-      timer.toc(std::cout,NBT);
-    }
 
 
+      if( flag >> 2 & 1 )
+	{
+	  timer.tic();
+	  SMOOTH(NBT)
+	  {
+	    se3::cholesky::solve(model,data,randvec[_smooth]);
+	  }
+	  if(verbose) std::cout << "solve =\t\t";
+	  timer.toc(std::cout,NBT);
+	}
+
+      if( flag >> 3 & 1 )
+	{
+	  timer.tic();
+	  SMOOTH(NBT)
+	  {
+	    se3::cholesky::Uv(model,data,randvec[_smooth]);
+	  }
+	  if(verbose) std::cout << "Uv =\t\t";
+	  timer.toc(std::cout,NBT);
+	}
+
+      if( flag >> 4 & 1 )
+	{
+	  timer.tic();
+	  SMOOTH(NBT)
+	  {
+	    se3::cholesky::Uiv(model,data,randvec[_smooth]);
+	  }
+	  if(verbose) std::cout << "Uiv =\t\t";
+	  timer.toc(std::cout,NBT);
+	}
+      if( flag >> 5 & 1 )
+	{
+	  timer.tic();
+	  Eigen::VectorXd res;
+	  SMOOTH(NBT)
+	  {
+	    res = se3::cholesky::Mv(model,data,randvec[_smooth]);
+	  }
+	  if(verbose) std::cout << "Mv =\t\t";
+	  timer.toc(std::cout,NBT);
+	}
+      if( flag >> 6 & 1 )
+	{
+	  timer.tic();
+	  SMOOTH(NBT)
+	  {
+	    se3::cholesky::Mv(model,data,randvec[_smooth],true);
+	  }
+	  if(verbose) std::cout << "UDUtv =\t\t";
+	  timer.toc(std::cout,NBT);
+	}
+    }
 }
 
 void assertValues(const se3::Model & model, se3::Data& data)
@@ -120,31 +167,23 @@ void assertValues(const se3::Model & model, se3::Data& data)
   assert( Miv.isApprox(M.inverse()*v));
 }
 
-int main()//int argc, const char ** argv)
+int main()
 {
-#ifdef __SSE3__
-  _MM_SET_DENORMALS_ZERO_MODE(_MM_DENORMALS_ZERO_ON);
-#endif
-
-
   using namespace Eigen;
   using namespace se3;
 
-  SE3::Matrix3 I3 = SE3::Matrix3::Identity();
-
   se3::Model model;
   se3::buildModels::humanoidSimple(model,true);
-  //se3::buildModels::humanoid2d(model);
 
   se3::Data data(model);
   VectorXd q = VectorXd::Zero(model.nq);
-  data.M.fill(0);
+  data.M.fill(0); // Only nonzero coeff of M are initialized by CRBA.
   crba(model,data,q);
 
 #ifndef NDEBUG 
   assertValues(model,data);
 #else
-  timings(model,data,1|4|2);
+  timings(model,data,BOOST_BINARY(1000101));
 #endif
 
   return 0;