Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Improve performance by precomputing sincos values #340

Open
VRichardJP opened this issue Dec 14, 2021 · 0 comments
Open

Improve performance by precomputing sincos values #340

VRichardJP opened this issue Dec 14, 2021 · 0 comments

Comments

@VRichardJP
Copy link

VRichardJP commented Dec 14, 2021

As it has been pointed out before in another discussion (#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;
};
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

No branches or pull requests

1 participant