I wrote some experiment program with Eigen, Geometry module for performance check.
There are 800-times forward kinematics of 6-dof serial manipulator.
I read most ways to speed up Eigen. ex) optimization, SSE2, etc.
However, I couldn't get higher performance rather than my lab's own c++ geometry library.
Is there a special point to speed up Eigen when using Geometry module.
Here are my parts of code.

Code: Select all

// In main.cpp
double q[6];
Isometry3d T;
robot bot;
for (int i=0; i<800; i++)
{
   for (int j=0; j<6; j++)
      q[j] = rand();
   T = bot.Cal_FKin(q);
}
// In robot.h
class{
public:
   const Isometry3d&  Cal_FKin(double q[6]);
private:
   link m_link[6];
   Isometry3d m_tmEE;
};
// In robot.cpp
const Isometry3d& robot::Cal_FKin(double q[6])
{
   m_tmEE = m_link[0].Cal_T(q[0]);
   for (int i=1; i<6; i++)
      m_tmEE = m_tmEE*m_link[i].Cal_T(q_[i]);
   return m_tmEE;
}
// In link.h
class link{
public:
   const Isometry3d& Cal_T(double q);
private:
   double m_a, m_alpha, m_d, m_theta;
   Isometry3d m_T;
};
// In link.cpp
const Isometry3d& link::Cal_T(double q)
{
   m_T = AngleAxisd(m_alpha, Vector3d::UnitX())*Translation3d(m_a, 0, m_d)*AngleAxisd(m_theta+q, Vector3d::UnitZ());
   return m_T;
}



I compiled in Release mode and use SSE2 mode.

The bottleneck is currently the Cal_T function for which SSE cannot help. However, you can exploit the sparsity of the two involved rotations and translation to compute by hand the resulting isometry. You can even store cos(alpha) and sin(alpha) instead of alpha to avoid recomputing these trigonometric functions anytime.

Yes, I know I can derive equations in Cal_T. However, the problem is that I compared the performance with the other geometry library in the same condition that I didn't derive equations in Cal_T. If I have to derive these kind of equations everytime, I don't have any reason to use Eigen Geometry Module. I would just write code by my hands.
In many articles comparing Eigen with other library, there is noalias(?) function in matrix manipulation enhancing Eigen's performance. However, there isn't any function like noalias in the Geometry module. I want to know new methods or skills that can enhance the performance also in the Geometry module.

noalias, unrolling, and vectorization can speedup matrix products. Here we would need advanced symbolic computation or special in-plane rotations to optimize the assembly and application of the rotations.