poly.cpp

Go to the documentation of this file.

#include "utils/poly.hpp"
 
#include "utils/logger.hpp"
#include <exception>
 
using namespace ::std;
using namespace ::galaxis;
using namespace ::galaxis::utils;
using namespace ::galaxis::control;
 
std::ostream& galaxis::operator<<(std::ostream& strm, polynom const& poly) {
  std::string str = "";
 
  if(poly.size() >= 1) {
    str = std::to_string(poly[0]);
    str += "x^0";
  }
  for(unsigned int i = 1; i < poly.size(); ++i) {
    str += " + ";
    str += std::to_string(poly[i]);
    str += "x^";
    str += std::to_string(i);
  }
 
  return strm << str;
}
 
std::ostream& galaxis::operator<<(std::ostream& strm, interval const& interval) {
  return strm << "[" << std::get<0>(interval) << ", " << std::get<1>(interval) << "]";
}
 
double galaxis::utils::evaluate_polynom(std::vector<double> const& p_polynomRef, double p_x) {
  float value = 0.0;
 
  float iValue;
  for(size_t i = 0; i < p_polynomRef.size(); ++i) {
    iValue = 1;
 
    // x^i
    for(size_t j = 0; j < i; ++j) {
      iValue *= p_x;
    }
 
    value += p_polynomRef[i] * iValue;
  }
 
  return value;
}
 
float galaxis::utils::evaluate_polynom(polynom const& p_polynomRef, float p_x) {
  std::vector<double> poly;
  for(auto const& elt : p_polynomRef) {
    poly.push_back(elt);
  }
 
  return static_cast<float>(evaluate_polynom(poly,p_x));
}
 
 
float galaxis::utils::calculate_gradient(std::vector<float> const& p_polynomRef, float p_x) {
  float value = 0.0;
 
  float iValue;
  for(size_t i = 1; i < p_polynomRef.size(); ++i) {
    iValue = 1;
 
    // i*x^(i-1)
    for(size_t j = 0; j < i - 1; ++j) {
      iValue *= p_x;
    }
 
    value += p_polynomRef[i] * i * iValue;
  }
 
  return value;
}
 
double galaxis::utils::calculate_gradient(std::vector<double> const& p_polynomRef, double p_x) {
  float value = 0.0;
 
  float iValue;
  for(size_t i = 1; i < p_polynomRef.size(); ++i) {
    iValue = 1;
 
    // i*x^(i-1)
    for(size_t j = 0; j < i - 1; ++j) {
      iValue *= p_x;
    }
 
    value += p_polynomRef[i] * i * iValue;
  }
 
  return value;
}
 
double galaxis::utils::calculate_gradient(DynamicPolynom p_polynomVecSPtr, double p_x) {
  float value = 0.0;
 
  float iValue;
  for(size_t i = 1; i < p_polynomVecSPtr->size(); ++i) {
    iValue = 1;
 
    // i*x^(i-1)
    for(size_t j = 0; j < i - 1; ++j) {
      iValue *= p_x;
    }
 
    value += p_polynomVecSPtr->at(i) * i * iValue;
  }
 
  return value;
}
 
namespace galaxis {
 
  float calculate_error_between_polyspline_point_square(shared_ptr<PolySpline> p_polySpline, Eigen::Vector2f& p_point) {
    // TODO: maxbe calc correct distance between polynom and
    // log_warning("poly.hpp: galaxis::utils::calculate_error_between_polyspline_point - correct distance implementation missing");
 
    float splinevalue = p_polySpline->evaluate(static_cast<float>(p_point[0]));
    float error       = p_point[1] - splinevalue;
 
    return error * error;
  }
 
  float calculate_error_between_polyspline_point(PolySpline p_polySpline, Eigen::Vector2f& p_point) {
    // TODO: maxbe calc correct distance between polynom and
    // log_warning("poly.hpp: galaxis::utils::calculate_error_between_polyspline_point - correct distance implementation missing");
 
    float splinevalue = p_polySpline.evaluate(static_cast<float>(p_point[0]));
    float error       = p_point[1] - splinevalue;
 
    return error;
  }
 
  float calculate_error_between_polyspline_poseseries(shared_ptr<PolySpline> p_polySplineSPtr, shared_ptr<galaxis::control::CarPoseSeries> p_poseSeriesSPtr,
                                                      shared_ptr<line_markings_reference_t> p_lineMarkings,
                                                      float cross_angle_weight, float cross_line_weight) {
    float error = 0;
    // Compute regular error function based on y difference between polynom and series
    for(shared_ptr<CarPose> pose: *p_poseSeriesSPtr) {
      error += calculate_error_between_polyspline_point_square(p_polySplineSPtr, pose->position);
    }
 
  
    // // Compute Angle at each planning point between polynom and series
    // for(shared_ptr<CarPose> pose: *p_poseSeriesSPtr) {
      
    //   // We dont use the spline part of polyspline
    //   float poly_gradient = calculate_gradient(p_polySplineSPtr->m_polynomVec.at(0), pose->position[0]);
      
    //   // Convert gradient to orientation
    //   float poly_orientation = std::tanh(poly_gradient);
    //   float angle_error = poly_orientation - pose->orientation;
 
    //   error += cross_angle_weight * (angle_error * angle_error);
    // }
 
    if(p_lineMarkings != nullptr){
      for(shared_ptr<CarPose> pose: *p_poseSeriesSPtr) {
        //TODO: Check that the pose is in the well defined interval for the line polynoms (tend to be bad outside of the interval)
        if(p_lineMarkings->isRightEnabled) {
          float r_error = calculate_error_between_polyspline_point(p_lineMarkings->right, pose->position);
          
          if(r_error > 0){
            error += (r_error * r_error) * cross_line_weight;
          }
        }
 
        if(p_lineMarkings->isLeftEnabled) {
          float l_error = calculate_error_between_polyspline_point(p_lineMarkings->left, pose->position);
          
          if(l_error < 0){
            error += (l_error * l_error) * cross_line_weight;
          }
        }
      }
    }
 
    return error;
  }
 
 
} // namespace galaxis
 
// --- START PolySpline ---
PolySpline::PolySpline() {}
 
PolySpline::PolySpline(std::array<double,4> coeff, double max, double min) {
  m_polynomVec.resize(2);
  for(uint32_t i = 0; i < coeff.size(); i ++){
    m_polynomVec.at(0).push_back(coeff.at(i));
  }
 
  m_intervalVec.push_back(std::tuple<float, float>(min,max));
}
 
void PolySpline::add_polynom(interval const& p_interval, polynom const& p_polynom) {
  // TODO: add interval check
  m_intervalVec.push_back(p_interval);
  m_polynomVec.push_back(polynom(p_polynom));
}
 
void PolySpline::add_polynom(float lowerBound, float upperBound, polynom const& polynom) {
  interval interv = std::make_tuple(lowerBound, upperBound);
  add_polynom(interv, polynom);
}
 
float PolySpline::evaluate(float p_x) const {
  for(size_t i = 0; i < m_intervalVec.size(); ++i) {
    if(p_x >= std::get<0>(m_intervalVec[i]) && p_x <= std::get<1>(m_intervalVec[i])) {
      return utils::evaluate_polynom(m_polynomVec[i], p_x);
    }
  }
  // log_error("No polynominterval for x.");
  return 0;
}
 
void PolySpline::clear() {
  m_intervalVec.clear();
  m_polynomVec.clear();
}
// --- END PolySpline ---