precompute sin/cos to improve performance

include/teb_local_planner/g2o_types/edge_acceleration.h

@@ -127,10 +127,10 @@ class EdgeAcceleration : public BaseTebMultiEdge<2, double>
 
 
     // consider directions
-//     vel1 *= g2o::sign(diff1[0]*cos(pose1->theta()) + diff1[1]*sin(pose1->theta())); 
-//     vel2 *= g2o::sign(diff2[0]*cos(pose2->theta()) + diff2[1]*sin(pose2->theta())); 
-    vel1 *= fast_sigmoid( 100*(diff1.x()*cos(pose1->theta()) + diff1.y()*sin(pose1->theta())) ); 
-    vel2 *= fast_sigmoid( 100*(diff2.x()*cos(pose2->theta()) + diff2.y()*sin(pose2->theta())) ); 
+//     vel1 *= g2o::sign(diff1[0]*pose1->theta().cos() + diff1[1]*pose1->theta().sin());
+//     vel2 *= g2o::sign(diff2[0]*pose2->theta().cos() + diff2[1]*pose2->theta().sin());
+    vel1 *= fast_sigmoid( 100*(diff1.x()*pose1->theta().cos() + diff1.y()*pose1->theta().sin()) );
+    vel2 *= fast_sigmoid( 100*(diff2.x()*pose2->theta().cos() + diff2.y()*pose2->theta().sin()) );
 
     const double acc_lin  = (vel2 - vel1)*2 / ( dt1->dt() + dt2->dt() );
 
@@ -326,8 +326,8 @@ class EdgeAccelerationStart : public BaseTebMultiEdge<2, const geometry_msgs::Tw
     double vel2 = dist / dt->dt();
 
     // consider directions
-    //vel2 *= g2o::sign(diff[0]*cos(pose1->theta()) + diff[1]*sin(pose1->theta())); 
-    vel2 *= fast_sigmoid( 100*(diff.x()*cos(pose1->theta()) + diff.y()*sin(pose1->theta())) ); 
+    //vel2 *= g2o::sign(diff[0]*pose1->theta().cos() + diff[1]*pose1->theta().sin());
+    vel2 *= fast_sigmoid( 100*(diff.x()*pose1->theta().cos() + diff.y()*pose1->theta().sin()) );
 
     const double acc_lin  = (vel2 - vel1) / dt->dt();
 
@@ -418,8 +418,8 @@ class EdgeAccelerationGoal : public BaseTebMultiEdge<2, const geometry_msgs::Twi
     const double vel2 = _measurement->linear.x;
 
     // consider directions
-    //vel1 *= g2o::sign(diff[0]*cos(pose_pre_goal->theta()) + diff[1]*sin(pose_pre_goal->theta())); 
-    vel1 *= fast_sigmoid( 100*(diff.x()*cos(pose_pre_goal->theta()) + diff.y()*sin(pose_pre_goal->theta())) ); 
+    //vel1 *= g2o::sign(diff[0]*pose_pre_goal->theta().cos() + diff[1]*pose_pre_goal->theta().sin());
+    vel1 *= fast_sigmoid( 100*(diff.x()*pose_pre_goal->theta().cos() + diff.y()*pose_pre_goal->theta().sin()) );
 
     const double acc_lin  = (vel2 - vel1) / dt->dt();
 
@@ -499,10 +499,10 @@ class EdgeAccelerationHolonomic : public BaseTebMultiEdge<3, double>
     Eigen::Vector2d diff1 = pose2->position() - pose1->position();
     Eigen::Vector2d diff2 = pose3->position() - pose2->position();
 
-    double cos_theta1 = std::cos(pose1->theta());
-    double sin_theta1 = std::sin(pose1->theta()); 
-    double cos_theta2 = std::cos(pose2->theta());
-    double sin_theta2 = std::sin(pose2->theta()); 
+    double cos_theta1 = pose1->theta().cos();
+    double sin_theta1 = pose1->theta().sin();
+    double cos_theta2 = pose2->theta().cos();
+    double sin_theta2 = pose2->theta().sin();
 
     // transform pose2 into robot frame pose1 (inverse 2d rotation matrix)
     double p1_dx =  cos_theta1*diff1.x() + sin_theta1*diff1.y();
@@ -589,8 +589,8 @@ class EdgeAccelerationHolonomicStart : public BaseTebMultiEdge<3, const geometry
     // VELOCITY & ACCELERATION
     Eigen::Vector2d diff = pose2->position() - pose1->position();
 
-    double cos_theta1 = std::cos(pose1->theta());
-    double sin_theta1 = std::sin(pose1->theta()); 
+    double cos_theta1 = pose1->theta().cos();
+    double sin_theta1 = pose1->theta().sin();
 
     // transform pose2 into robot frame pose1 (inverse 2d rotation matrix)
     double p1_dx =  cos_theta1*diff.x() + sin_theta1*diff.y();
@@ -681,8 +681,8 @@ class EdgeAccelerationHolonomicGoal : public BaseTebMultiEdge<3, const geometry_
 
     Eigen::Vector2d diff = pose_goal->position() - pose_pre_goal->position();    
 
-    double cos_theta1 = std::cos(pose_pre_goal->theta());
-    double sin_theta1 = std::sin(pose_pre_goal->theta()); 
+    double cos_theta1 = pose_pre_goal->theta().cos();
+    double sin_theta1 = pose_pre_goal->theta().sin();
 
     // transform pose2 into robot frame pose1 (inverse 2d rotation matrix)
     double p1_dx =  cos_theta1*diff.x() + sin_theta1*diff.y();

include/teb_local_planner/g2o_types/edge_kinematics.h

@@ -94,10 +94,10 @@ class EdgeKinematicsDiffDrive : public BaseTebBinaryEdge<2, double, VertexPose,
     Eigen::Vector2d deltaS = conf2->position() - conf1->position();
 
     // non holonomic constraint
-    _error[0] = fabs( ( cos(conf1->theta())+cos(conf2->theta()) ) * deltaS[1] - ( sin(conf1->theta())+sin(conf2->theta()) ) * deltaS[0] );
+    _error[0] = fabs( ( conf1->theta().cos()+conf2->theta().cos() ) * deltaS[1] - ( conf1->theta().sin()+conf2->theta().sin() ) * deltaS[0] );
 
     // positive-drive-direction constraint
-    Eigen::Vector2d angle_vec ( cos(conf1->theta()), sin(conf1->theta()) );	   
+    Eigen::Vector2d angle_vec ( conf1->theta().cos(), conf1->theta().sin() );
     _error[1] = penaltyBoundFromBelow(deltaS.dot(angle_vec), 0,0);
     // epsilon=0, otherwise it pushes the first bandpoints away from start
 
@@ -117,19 +117,19 @@ class EdgeKinematicsDiffDrive : public BaseTebBinaryEdge<2, double, VertexPose,
 
     Eigen::Vector2d deltaS = conf2->position() - conf1->position();
 
-    double cos1 = cos(conf1->theta());
-    double cos2 = cos(conf2->theta());
-    double sin1 = sin(conf1->theta());
-    double sin2 = sin(conf2->theta());
+    double cos1 = conf1->theta().cos();
+    double cos2 = conf2->theta().cos();
+    double sin1 = conf1->theta().sin();
+    double sin2 = conf2->theta().sin();
     double aux1 = sin1 + sin2;
     double aux2 = cos1 + cos2;
 
     double dd_error_1 = deltaS[0]*cos1;
     double dd_error_2 = deltaS[1]*sin1;
     double dd_dev = penaltyBoundFromBelowDerivative(dd_error_1+dd_error_2, 0,0);
 
-    double dev_nh_abs = g2o::sign( ( cos(conf1->theta())+cos(conf2->theta()) ) * deltaS[1] - 
-	      ( sin(conf1->theta())+sin(conf2->theta()) ) * deltaS[0] );
+    double dev_nh_abs = g2o::sign( ( conf1->theta().cos()+conf2->theta().cos() ) * deltaS[1] -
+	      ( conf1->theta().sin()+conf2->theta().sin() ) * deltaS[0] );
 
     // conf1
     _jacobianOplusXi(0,0) = aux1 * dev_nh_abs; // nh x1
@@ -203,7 +203,7 @@ class EdgeKinematicsCarlike : public BaseTebBinaryEdge<2, double, VertexPose, Ve
     Eigen::Vector2d deltaS = conf2->position() - conf1->position();
 
     // non holonomic constraint
-    _error[0] = fabs( ( cos(conf1->theta())+cos(conf2->theta()) ) * deltaS[1] - ( sin(conf1->theta())+sin(conf2->theta()) ) * deltaS[0] );
+    _error[0] = fabs( ( conf1->theta().cos()+conf2->theta().cos() ) * deltaS[1] - (conf1->theta().sin()+conf2->theta().sin() ) * deltaS[0] );
 
     // limit minimum turning radius
     double angle_diff = g2o::normalize_theta( conf2->theta() - conf1->theta() );

include/teb_local_planner/g2o_types/edge_velocity.h

@@ -105,8 +105,8 @@ class EdgeVelocity : public BaseTebMultiEdge<2, double>
     }
     double vel = dist / deltaT->estimate();
 
-//     vel *= g2o::sign(deltaS[0]*cos(conf1->theta()) + deltaS[1]*sin(conf1->theta())); // consider direction
-    vel *= fast_sigmoid( 100 * (deltaS.x()*cos(conf1->theta()) + deltaS.y()*sin(conf1->theta())) ); // consider direction
+//     vel *= g2o::sign(deltaS[0]*conf1->theta().cos() + deltaS[1]*conf1->theta().sin()); // consider direction
+    vel *= fast_sigmoid( 100 * (deltaS.x()*conf1->theta().cos() + deltaS.y()*conf1->theta().sin()) ); // consider direction
 
     const double omega = angle_diff / deltaT->estimate();
 
@@ -241,8 +241,8 @@ class EdgeVelocityHolonomic : public BaseTebMultiEdge<3, double>
     const VertexTimeDiff* deltaT = static_cast<const VertexTimeDiff*>(_vertices[2]);
     Eigen::Vector2d deltaS = conf2->position() - conf1->position();
 
-    double cos_theta1 = std::cos(conf1->theta());
-    double sin_theta1 = std::sin(conf1->theta()); 
+    double cos_theta1 = conf1->theta().cos();
+    double sin_theta1 = conf1->theta().sin();
 
     // transform conf2 into current robot frame conf1 (inverse 2d rotation matrix)
     double r_dx =  cos_theta1*deltaS.x() + sin_theta1*deltaS.y();

include/teb_local_planner/g2o_types/edge_velocity_obstacle_ratio.h

@@ -98,7 +98,7 @@ class EdgeVelocityObstacleRatio : public BaseTebMultiEdge<2, const Obstacle*>
     }
     double vel = dist / deltaT->estimate();
 
-    vel *= fast_sigmoid( 100 * (deltaS.x()*cos(conf1->theta()) + deltaS.y()*sin(conf1->theta())) ); // consider direction
+    vel *= fast_sigmoid( 100 * (deltaS.x()*(conf1->theta().cos() + deltaS.y()*conf1->theta().sin())) ); // consider direction
 
     const double omega = angle_diff / deltaT->estimate();
 

include/teb_local_planner/g2o_types/vertex_pose.h

@@ -170,13 +170,13 @@ class VertexPose : public g2o::BaseVertex<3, PoseSE2 >
     * @brief Access the orientation part (yaw angle) of the pose
     * @return reference to the yaw angle
     */ 
-  inline double& theta() {return _estimate.theta();}
+  inline Theta& theta() {return _estimate.theta();}
 
   /**
     * @brief Access the orientation part (yaw angle) of the pose (read-only)
     * @return const reference to the yaw angle
     */ 
-  inline const double& theta() const {return _estimate.theta();}
+  inline const Theta& theta() const {return _estimate.theta();}
 
   /**
     * @brief Set the underlying estimate (2D vector) to zero.

include/teb_local_planner/pose_se2.h

@@ -49,6 +49,82 @@
 namespace teb_local_planner
 {
 
+/**
+  * @class Theta
+  * @brief This class wraps an angle value (in radian) and lazily evaluates its sin/cos
+  */
+class Theta
+{
+public:
+  Theta(): Theta(0.0) {}
+
+  Theta(const Theta& other)
+    : _rad(other._rad)
+    , _sin(other._sin)
+    , _cos(other._cos)
+    , _init(other._init)
+  {}
+
+  Theta(const double rad)
+    : _rad(rad)
+    , _init(false)
+  {}
+
+  Theta& operator=(const double rad) {
+    _rad = rad;
+    _init = false;
+    return *this;
+  }
+
+  Theta& operator*=(const double x) {
+    _rad *= x;
+    _init = false;
+    return *this;
+  }
+
+  friend std::ostream& operator<<(std::ostream& os, const Theta& obj) {
+    os << obj._rad;
+    return os;
+  }
+
+  friend std::istream& operator>>(std::istream& is, Theta& obj) {
+    is >> obj._rad;
+    obj._init = false;
+    return is;
+  }
+
+  operator double() const {return rad();}
+
+  const double& rad() const {return _rad;}
+
+  const double& sin() const {
+    if (!_init) {
+      const_cast<Theta*>(this)->eval_sincos();
+    }
+    return _sin;
+  }
+
+  const double& cos() const {
+    if (!_init) {
+      const_cast<Theta*>(this)->eval_sincos();
+    }
+    return _cos;
+  }
+
+private:
+
+  void eval_sincos() {
+    _sin = std::sin(_rad);
+    _cos = std::cos(_rad);
+    _init = true;
+  }
+
+  double _rad;
+  double _sin;
+  double _cos;
+  bool _init; // whether sin/cos values have been evaluated already
+};
+
 /**
   * @class PoseSE2
   * @brief This class implements a pose in the domain SE2: \f$ \mathbb{R}^2 \times S^1 \f$
@@ -179,13 +255,13 @@ class PoseSE2
     * @brief Access the orientation part (yaw angle) of the pose
     * @return reference to the yaw angle
     */ 
-  double& theta() {return _theta;}
+  Theta& theta() {return _theta;}
 
   /**
     * @brief Access the orientation part (yaw angle) of the pose (read-only)
     * @return const reference to the yaw angle
     */ 
-  const double& theta() const {return _theta;}
+  const Theta& theta() const {return _theta;}
 
   /**
     * @brief Set pose to [0,0,0]
@@ -212,7 +288,7 @@ class PoseSE2
    * @brief Return the unit vector of the current orientation
    * @returns [cos(theta), sin(theta))]^T
    */  
-  Eigen::Vector2d orientationUnitVec() const {return Eigen::Vector2d(std::cos(_theta), std::sin(_theta));}
+  Eigen::Vector2d orientationUnitVec() const {return Eigen::Vector2d(_theta.cos(), _theta.sin());}
 
   ///@}
 
@@ -394,7 +470,7 @@ class PoseSE2
 private:
 
   Eigen::Vector2d _position; 
-  double _theta;
+  Theta _theta;
 
 public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW  

include/teb_local_planner/robot_footprint_model.h

@@ -609,8 +609,8 @@ class LineRobotFootprint : public BaseRobotFootprintModel
     */
   void transformToWorld(const PoseSE2& current_pose, Eigen::Vector2d& line_start_world, Eigen::Vector2d& line_end_world) const
   {
-    double cos_th = std::cos(current_pose.theta());
-    double sin_th = std::sin(current_pose.theta());
+    double cos_th = current_pose.theta().cos();
+    double sin_th = current_pose.theta().sin();
     line_start_world.x() = current_pose.x() + cos_th * line_start_.x() - sin_th * line_start_.y();
     line_start_world.y() = current_pose.y() + sin_th * line_start_.x() + cos_th * line_start_.y();
     line_end_world.x() = current_pose.x() + cos_th * line_end_.x() - sin_th * line_end_.y();
@@ -756,8 +756,8 @@ class PolygonRobotFootprint : public BaseRobotFootprintModel
     */
   void transformToWorld(const PoseSE2& current_pose, Point2dContainer& polygon_world) const
   {
-    double cos_th = std::cos(current_pose.theta());
-    double sin_th = std::sin(current_pose.theta());
+    double cos_th = current_pose.theta().cos();
+    double sin_th = current_pose.theta().sin();
     for (std::size_t i=0; i<vertices_.size(); ++i)
     {
       polygon_world[i].x() = current_pose.x() + cos_th * vertices_[i].x() - sin_th * vertices_[i].y();

src/graph_search.cpp

@@ -176,7 +176,7 @@ void lrKeyPointGraph::createGraph(const PoseSE2& start, const PoseSE2& goal, dou
         {
           Eigen::Vector2d keypoint_dist = graph_[*it_j].pos-start.position();
           keypoint_dist.normalize();
-          Eigen::Vector2d start_orient_vec( cos(start.theta()), sin(start.theta()) ); // already normalized
+          Eigen::Vector2d start_orient_vec( start.theta().cos(), start.theta().sin() ); // already normalized
           // check angle
           if (start_orient_vec.dot(keypoint_dist) <= obstacle_heading_threshold)
           {

src/optimal_planner.cpp

@@ -1114,7 +1114,7 @@ void TebOptimalPlanner::extractVelocity(const PoseSE2& pose1, const PoseSE2& pos
 
   if (cfg_->robot.max_vel_y == 0) // nonholonomic robot
   {
-    Eigen::Vector2d conf1dir( cos(pose1.theta()), sin(pose1.theta()) );
+    Eigen::Vector2d conf1dir( pose1.theta().cos(), pose1.theta().sin() );
     // translational velocity
     double dir = deltaS.dot(conf1dir);
     vx = (double) g2o::sign(dir) * deltaS.norm()/dt;
@@ -1125,8 +1125,8 @@ void TebOptimalPlanner::extractVelocity(const PoseSE2& pose1, const PoseSE2& pos
     // transform pose 2 into the current robot frame (pose1)
     // for velocities only the rotation of the direction vector is necessary.
     // (map->pose1-frame: inverse 2d rotation matrix)
-    double cos_theta1 = std::cos(pose1.theta());
-    double sin_theta1 = std::sin(pose1.theta());
+    double cos_theta1 = pose1.theta().cos();
+    double sin_theta1 = pose1.theta().sin();
     double p1_dx =  cos_theta1*deltaS.x() + sin_theta1*deltaS.y();
     double p1_dy = -sin_theta1*deltaS.x() + cos_theta1*deltaS.y();
     vx = p1_dx / dt;