cipher982

// estimate longitudinal position of a car (s)
check_car_s += ((double)prev_size * 0.02 * check_speed);

if (car_lane == lane) // if the other car is in 'lane' (our lane)
{
  // is it within 30m ahead of us?
  car_ahead |= check_car_s > car_s && check_car_s - car_s < 30; 
}
else if (car_lane - lane == -1) // if left of us
{

cipher982

if (d > 0 && d < 4) // d represents location in meters from left-most edge
{
  car_lane = 0; // 0-4 meters = lane 0, (left-lane)
}
else if (d > 4 && d < 8)
{
  car_lane = 1; // middle-lane
}
else if (d > 8 && d < 12)
{

cipher982

// simulate latency compensation
const double latency = 0.1;

px = px + v * cos(psi) * latency;
py = py + v * sin(psi) * latency;

cipher982

// actuator shock absorber
if (t < N - 2) {
  //steering
  fg[0] += 30000.0*CppAD::pow(vars[delta_start + t + 1] - vars[delta_start + t], 2);

  //acceleration
  fg[0] += CppAD::pow(vars[a_start + t + 1] - vars[a_start + t], 2);    
}

cipher982

// Fit a polynomial.
// Adapted from
// https://github.com/JuliaMath/Polynomials.jl/blob/master/src/Polynomials.jl#L676-L716
Eigen::VectorXd polyfit(Eigen::VectorXd xvals, Eigen::VectorXd yvals, int order) {
  assert(xvals.size() == yvals.size());
  assert(order >= 1 && order <= xvals.size() - 1);
  Eigen::MatrixXd A(xvals.size(), order + 1);

  for (int i = 0; i < xvals.size(); i++) {
    A(i, 0) = 1.0;

cipher982

double deg2rad(double x) { return x * pi() / 180; }
double rad2deg(double x) { return x * 180 / pi(); } 

cipher982

      fg[1 + x_start + t]    = x1    - (x0 + v0 * CppAD::cos(psi0) * dt);
      fg[1 + y_start + t]    = y1    - (y0 + v0 * CppAD::sin(psi0) * dt);
      fg[1 + psi_start + t]  = psi1  - (psi0 + v0 * delta0 / Lf * dt);
      fg[1 + v_start + t]    = v1    - (v0 + a0 * dt);
      fg[1 + cte_start + t]  = cte1  - ((f0 - y0) + (v0 * CppAD::sin(epsi0) * dt));
      fg[1 + epsi_start + t] = epsi1 - ((psi0 - psides0) + v0 * delta0 / Lf * dt);

cipher982

// j[1] is the data JSON object
vector<double> ptsx = j[1]["ptsx"];
vector<double> ptsy = j[1]["ptsy"];
double px  = j[1]["x"];
double py  = j[1]["y"];
double psi = j[1]["psi"];
double v   = j[1]["speed"];

cipher982

  """
  Add polygon and Overlay and Warp
  """
def polygonOverlay(src):
  
  binary_warped = src
  # Create an image to draw the lines on
  warp_zero = np.zeros_like(binary_warped).astype(np.uint8)
  color_warp = np.dstack((warp_zero, warp_zero, warp_zero))

cipher982

def regressionLanesTracker(src):
    
    binary_warped = src
    # Assuming you have created a warped binary image called "binary_warped"
    # Take a histogram of the bottom half of the image
    histogram = np.sum(binary_warped[int(binary_warped.shape[0]/2):,:], axis=0)
    # Create an output image to draw on and  visualize the result
    out_img = np.dstack((binary_warped, binary_warped, binary_warped))*255
    # Find the peak of the left and right halves of the histogram
    # These will be the starting point for the left and right lines