You signed in with another tab or window. Reload to refresh your session.You signed out in another tab or window. Reload to refresh your session.You switched accounts on another tab or window. Reload to refresh your session.Dismiss alert
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:
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 ati
andi+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 inEdgeAcceleration
,EdgeVelocity
andEdgeKinematics
we can findcos(conf1->theta())
orcos(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 fromdouble
to theTheta
class below and replaced allcos(pose1->theta())
by the correspondingpose1->theta().cos()
. With these simple changes the number of calls tosincos
is reduced by 1/3 and teb computation time is reduced by 10% in my custom benchmark:The text was updated successfully, but these errors were encountered: