Improve performance by precomputing sincos values

[VRichardJP]

As it has been pointed out before in another discussion (https://github.com/rst-tu-dortmund/teb_local_planner/pull/234), teb spends quite a lot of time computing sin/cos while optimizing the graph. Using google profiling tools, I can see on my machine that teb spends roughly 40% of its time in sincos.

Going through the code, I observe that many of these sin/cos calculation is actually duplicated. For example EdgeKinematics reads 2 vertices at i and i+1, and computes the sin/cos of both pose angle. This means that each vertex has its angle sin/cos computed twice. Another typical pattern is that because the edges explore the same vertices they end up recomputing many times the same sin/cos values. For example in EdgeAcceleration, EdgeVelocity and EdgeKinematics we can find cos(conf1->theta()) or cos(pose1->theta()), which correspond to the same vertex.

One way to tackle this issue is to simply precompute the sin/cos values for each pose. For example I have changed PoseSE2::_theta value from double to the Theta class below and replaced all cos(pose1->theta()) by the corresponding pose1->theta().cos(). With these simple changes the number of calls to sincos is reduced by 1/3 and teb computation time is reduced by 10% in my custom benchmark:

class Theta
{
public:
  Theta() {
    _theta = 0;
    update_sincos();
  }

  Theta& operator=(const double theta) {
    _theta = theta;
    update_sincos();
  }

  Theta& operator*=(const double x) {
    _theta *= x;
    update_sincos();
  }

  friend std::ostream& operator<<(std::ostream& os, const Theta& obj) {
    os << obj._theta;
    return os;
  }

  friend std::istream& operator>>(std::istream& is, Theta& obj) {
    is >> obj._theta;
    obj.update_sincos();
    return is;
  }

  operator double() const {return theta();}

  const double& theta() const {return _theta;};
  const double& sin() const {return _sin;};
  const double& cos() const {return _cos;};

private:

  void update_sincos() {
    _sin = std::sin(_theta);
    _cos = std::cos(_theta);
  }

  double _theta;
  double _sin;
  double _cos;
};