Script to test performance of code-generation with A*B and A.transpose() * B

test-cg.cpp

#include <Eigen/Core>
#include <cppad/cg/support/cppadcg_eigen.hpp>
#include <Eigen/Eigenvalues>

#include <iostream>

// define dimension
#define ND 50

#ifdef WIN32
#include <Windows.h>
#include <stdint.h> // portable: uint64_t   MSVC: __int64

int gettimeofday(struct timeval* tp, struct timezone* tzp)
{
  // Note: some broken versions only have 8 trailing zero's, the correct epoch has 9 trailing zero's
  // This magic number is the number of 100 nanosecond intervals since January 1, 1601 (UTC)
  // until 00:00:00 January 1, 1970
  static const uint64_t EPOCH = ((uint64_t)116444736000000000ULL);

  SYSTEMTIME  system_time;
  FILETIME    file_time;
  uint64_t    time;

  GetSystemTime(&system_time);
  SystemTimeToFileTime(&system_time, &file_time);
  time = ((uint64_t)file_time.dwLowDateTime);
  time += ((uint64_t)file_time.dwHighDateTime) << 32;

  tp->tv_sec = (long)((time - EPOCH) / 10000000L);
  tp->tv_usec = (long)(system_time.wMilliseconds * 1000);
  return 0;
}
#else
#include <sys/time.h>
#endif
#include <iostream>
#include <stack>

#define SMOOTH(s) for(size_t _smooth=0;_smooth<s;++_smooth) 

/* Return the time spent in secs. */
inline double operator-(const struct timeval & t1,const struct timeval & t0)
{
  /* TODO: double check the double conversion from long (on 64x). */
  return double(t1.tv_sec - t0.tv_sec)+1e-6*double(t1.tv_usec - t0.tv_usec);
}

struct PinocchioTicToc
{
  enum Unit { S = 1, MS = 1000, US = 1000000, NS = 1000000000 };
  Unit DEFAULT_UNIT;
  
  static std::string unitName(Unit u)
  {
    switch(u) { case S: return "s"; case MS: return "ms"; case US: return "us"; case NS: return "ns"; }
    return "";
  }
  
  std::stack<struct timeval> stack;
  mutable struct timeval t0;
  
  PinocchioTicToc( Unit def = MS ) : DEFAULT_UNIT(def) {}
  
  inline void tic() {
    stack.push(t0);
    gettimeofday(&(stack.top()),NULL);
  }
  
  inline double toc() { return toc(DEFAULT_UNIT); };
  
  inline double toc(const Unit factor)
  {
    gettimeofday(&t0,NULL);
    double dt = (t0-stack.top())*factor;
    stack.pop();
    return dt;
  }
  
  inline void toc(std::ostream & os, double SMOOTH=1)
  {
    os << toc(DEFAULT_UNIT)/SMOOTH << " " << unitName(DEFAULT_UNIT) << std::endl;
  }
};

template <typename Scalar>
struct TmpData {
  typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixXs;
  typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> VectorXs;
  MatrixXs OSIMinv_tmp;
  MatrixXs U1inv;
  TmpData() : OSIMinv_tmp(MatrixXs::Zero(ND, ND)), U1inv(MatrixXs::Zero(ND, ND)) {}

  template <typename NewScalar>
  TmpData<NewScalar> cast() const {
    typedef TmpData<NewScalar> ReturnType;
    ReturnType res;
    res.OSIMinv_tmp = OSIMinv_tmp.template cast<NewScalar>();
    res.U1inv = U1inv.template cast<NewScalar>();
    return res;
  }
};

template <typename Scalar>
void calc(const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &in, Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &out,
          TmpData<Scalar> &tmp) {
  typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixXs;
  const Eigen::Map<const MatrixXs> M(in.data(), ND, ND);
  Eigen::Map<MatrixXs> osim(out.data(), ND, ND);

  tmp.U1inv.setIdentity();
  M.template triangularView<Eigen::UnitUpper>().solveInPlace(tmp.U1inv);
  osim.noalias() = tmp.U1inv;
  tmp.OSIMinv_tmp.noalias() = -tmp.U1inv.transpose() * M.diagonal().asDiagonal();
  osim.noalias() = tmp.OSIMinv_tmp * tmp.U1inv;
}

int main(void) {
  typedef double Scalar;

  typedef CppAD::cg::CG<Scalar> CGScalar;
  typedef CppAD::AD<CGScalar> ADScalar;
  typedef CppAD::ADFun<CGScalar> ADFun;
  typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> VectorXs;
  typedef Eigen::Matrix<ADScalar, Eigen::Dynamic, 1> ADVectorXs;
  //*********Setting up code gen variables and functions*********
  ADVectorXs ad_X;
  ADVectorXs ad_Y;

  TmpData<Scalar> scalar_tmp;
  TmpData<ADScalar> ad_tmp = scalar_tmp.cast<ADScalar>();

  ad_X.resize(ND * ND);
  ad_Y.resize(ND * ND);

  ADFun ad_fun;
  CppAD::Independent(ad_X);
  calc(ad_X, ad_Y, ad_tmp);
  ad_fun.Dependent(ad_X, ad_Y);
  ad_fun.optimize("no_compare_op");

  /******************Preparing Library********************/
  std::string function_name = "calc_cg";
  // Initialize Library
  CppAD::cg::ModelCSourceGen<Scalar> cgen(ad_fun, "calc_cg");
  cgen.setCreateForwardZero(true);
  cgen.setCreateJacobian(false);
  // cgen.setCreateForwardZero(true);
  CppAD::cg::ModelLibraryCSourceGen<Scalar> libcgen(cgen);
  CppAD::cg::DynamicModelLibraryProcessor<Scalar> dynamicLibManager(libcgen, "am_lib");
  // Compile Library
  CppAD::cg::ClangCompiler<Scalar> compiler;
  std::vector<std::string> compile_options = compiler.getCompileFlags();
  compile_options[0] = "-Ofast";
  compiler.setCompileFlags(compile_options);
  dynamicLibManager.createDynamicLibrary(compiler, false);
  std::unique_ptr<CppAD::cg::DynamicLib<Scalar>> dynamicLib;
  // load Library
  const auto it = dynamicLibManager.getOptions().find("dlOpenMode");
  if (it == dynamicLibManager.getOptions().end()) {
    dynamicLib.reset(new CppAD::cg::LinuxDynamicLib<Scalar>(dynamicLibManager.getLibraryName() +
                                                            CppAD::cg::system::SystemInfo<>::DYNAMIC_LIB_EXTENSION));
  } else {
    int dlOpenMode = std::stoi(it->second);
    dynamicLib.reset(new CppAD::cg::LinuxDynamicLib<Scalar>(
        dynamicLibManager.getLibraryName() + CppAD::cg::system::SystemInfo<>::DYNAMIC_LIB_EXTENSION, dlOpenMode));
  }
  std::unique_ptr<CppAD::cg::GenericModel<Scalar>> generatedFun_ptr = dynamicLib->model(function_name.c_str());
  // Evaluate Function

  /******************************Computation*************************/
  typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixXs;

  MatrixXs Mrandom(MatrixXs::Random(ND, ND));

  MatrixXs Min(Eigen::RealSchur<MatrixXs>(Mrandom).matrixU());

  VectorXs v_in, v_out_cg, v_out;
  v_in.resize(ND * ND);
  v_out.resize(ND * ND);
  v_out_cg.resize(ND * ND);

  // M = pinocchio::SE3Tpl<Scalar>::Random();
  v_in = Eigen::Map<const VectorXs>(Min.data(), Min.size());

  PinocchioTicToc timer(PinocchioTicToc::US);
  const int NBT = 1000 * 100;

  timer.tic();
  SMOOTH(NBT) { generatedFun_ptr->ForwardZero(v_in, v_out_cg); }
  std::cout << "calc code-gen = \t\t";
  timer.toc(std::cout, NBT);

  timer.tic();
  SMOOTH(NBT) { calc(v_in, v_out, scalar_tmp); }
  std::cout << "calc = \t\t";
  timer.toc(std::cout, NBT);

  std::cerr << "error in computations:" << (v_out - v_out_cg).norm() << std::endl;
}