poly.hpp

Go to the documentation of this file.

#ifndef _POLY_HPP_
#define _POLY_HPP_
 
#include "control/pose.hpp"
 
#include <vector>
#include <memory>
#include <tuple>
#include <Eigen/Dense>
#include <string>
 
using namespace ::std;
using namespace ::galaxis;
using namespace ::galaxis::control;
 
typedef std::shared_ptr<std::vector<double>> DynamicPolynom;
 
namespace galaxis {
 
  typedef std::vector<float> polynom;
  typedef std::tuple<float, float> interval;
 
  std::ostream& operator<<(std::ostream& strm, polynom const& poly);
  std::ostream& operator<<(std::ostream& strm, interval const& interval);
 
  namespace utils {
    float evaluate_polynom(polynom const& p_polynomRef, float p_x);
    double evaluate_polynom(std::vector<double> const& p_polynomRef, double p_x);
    
    float calculate_gradient(std::vector<float> const& p_polynomRef, float p_x);
    double calculate_gradient(std::vector<double> const& p_polynomRef, double p_x);
    double calculate_gradient(DynamicPolynom p_polynomVecSPtr, double p_x);
  }
 
 
 
  class PolySpline {
  public:
    std::vector<galaxis::interval> m_intervalVec;
    std::vector<galaxis::polynom> m_polynomVec; // vector[0] + vector[1] * x^1 + ... + vector[n] * x^n
  
    PolySpline();
    PolySpline(std::array<double,4> coeff, double max, double min);
    void add_polynom(interval const& p_interval, polynom const& p_polynom);
    void add_polynom(float lowerBound, float upperBound, polynom const& polynom);
    float evaluate(float p_x) const;
    void clear();
  
 
    // TODO: add getter for polynom in specific interval
  };
 
  struct line_markings_reference_t{
    PolySpline center;
    PolySpline left;
    PolySpline right;
    bool isLeftEnabled = false;
    bool isCenterEnabled = false;
    bool isRightEnabled = false;
  };
 
  float calculate_error_between_polyspline_point(shared_ptr<PolySpline> p_polySpline, Eigen::Vector2f& p_point);
  float calculate_error_between_polyspline_point_square(shared_ptr<PolySpline> p_polySpline, Eigen::Vector2f const& p_point);
  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);
} // namespace galaxis
#endif