[Bench] Add benchmark of Cholesky algo

benchmark/CMakeLists.txt

@@ -42,6 +42,10 @@ ENDMACRO(ADD_TEST_CFLAGS)
 # 
 ADD_BENCH(timings TRUE)
 
+# timings
+# 
+ADD_BENCH(timings-cholesky TRUE)
+
 # timings derivatives
 # 
 ADD_BENCH(timings-derivatives TRUE)

benchmark/timings-cholesky.cpp

+//
+// Copyright (c) 2018 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 "pinocchio/spatial/fwd.hpp"
+#include "pinocchio/spatial/se3.hpp"
+#include "pinocchio/multibody/visitor.hpp"
+#include "pinocchio/multibody/model.hpp"
+#include "pinocchio/algorithm/crba.hpp"
+#include "pinocchio/algorithm/cholesky.hpp"
+#include "pinocchio/parsers/urdf.hpp"
+#include "pinocchio/parsers/sample-models.hpp"
+
+#include <iostream>
+
+#include "pinocchio/tools/timer.hpp"
+
+#include <Eigen/StdVector>
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::VectorXd)
+
+int main(int argc, const char ** argv)
+{
+  using namespace Eigen;
+  using namespace se3;
+
+  StackTicToc timer(StackTicToc::US);
+  #ifdef NDEBUG
+  const int NBT = 1000*100;
+  #else
+    const int NBT = 1;
+    std::cout << "(the time score in debug mode is not relevant) " << std::endl;
+  #endif
+    
+  se3::Model model;
+
+  std::string filename = PINOCCHIO_SOURCE_DIR"/models/simple_humanoid.urdf";
+  if(argc>1) filename = argv[1];
+  if( filename == "HS") 
+    se3::buildModels::humanoidSimple(model,true);
+  else if( filename == "H2" )
+    se3::buildModels::humanoid2d(model);
+  else
+    se3::urdf::buildModel(filename,JointModelFreeFlyer(),model);
+  std::cout << "nq = " << model.nq << std::endl;
+
+  se3::Data data(model);
+  VectorXd q = VectorXd::Random(model.nq);
+  VectorXd qdot = VectorXd::Random(model.nv);
+  VectorXd qddot = VectorXd::Random(model.nv);
+  
+  MatrixXd A(model.nv,model.nv), B(model.nv,model.nv);
+  A.setZero(); B.setRandom();
+
+  std::vector<VectorXd> qs     (NBT);
+  std::vector<VectorXd> lhs  (NBT);
+  std::vector<VectorXd> rhs (NBT);
+  for(size_t i=0;i<NBT;++i)
+    {
+      qs[i]     = Eigen::VectorXd::Random(model.nq);
+      lhs[i] = Eigen::VectorXd::Zero(model.nv);
+      rhs[i] = Eigen::VectorXd::Random(model.nv);
+    }
+  
+  double total = 0;
+  SMOOTH(NBT)
+    {
+      crba(model,data,qs[_smooth]);
+      timer.tic();
+      cholesky::decompose(model,data);
+      total += timer.toc(timer.DEFAULT_UNIT);
+    }
+  std::cout << "Cholesky = \t" << (total/NBT) 
+	    << " " << timer.unitName(timer.DEFAULT_UNIT) <<std::endl;
+
+  timer.tic();
+  SMOOTH(NBT)
+  {
+    cholesky::solve(model,data,rhs[_smooth]);
+  }
+  std::cout << "Cholesky solve vector = \t\t"; timer.toc(std::cout,NBT);
+  
+  timer.tic();
+  SMOOTH(NBT)
+  {
+    cholesky::Mv(model,data,rhs[_smooth],true);
+  }
+  std::cout << "UDUtv = \t\t"; timer.toc(std::cout,NBT);
+  
+  MatrixXd Minv(model.nv,model.nv); Minv.setZero();
+  timer.tic();
+  SMOOTH(NBT)
+  {
+    cholesky::computeMinv(model,data,Minv);
+  }
+  std::cout << "Minv from cholesky = \t\t"; timer.toc(std::cout,NBT);
+  
+  timer.tic();
+  SMOOTH(NBT)
+  {
+    cholesky::solve(model,data,Minv.col(10));
+  }
+  std::cout << "Cholesky solve column = \t\t"; timer.toc(std::cout,NBT);
+  
+  timer.tic();
+  SMOOTH(NBT)
+  {
+    lhs[_smooth].noalias() = Minv*rhs[_smooth];
+  }
+  std::cout << "Minv*v = \t\t"; timer.toc(std::cout,NBT);
+  
+  timer.tic();
+  SMOOTH(NBT)
+  {
+    A.noalias() = Minv*B;
+  }
+  std::cout << "A = Minv*B = \t\t"; timer.toc(std::cout,NBT);
+  
+  return 0;
+}