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

cipher982

plt.suptitle('After Sobel Binary', fontsize=14, fontweight='bold')
plt.imshow(step2)
plt.show()

histogram = np.sum(step2[step2.shape[0]//2:,:], axis=0)
plt.suptitle('Histogram of Pixels', fontsize=14, fontweight='bold')
plt.plot(histogram)
plt.show()

cipher982

# for a given sobel kernel size and threshold values
def toMagSobel(img, sobel_kernel=3, mag_thresh=(0, 255)):
    # Convert to grayscale
    if len(np.shape(img)) > 2:
        gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
    else:
        gray = img
    # Take both Sobel x and y gradients
    sobelx = cv2.Sobel(gray, cv2.CV_64F, 1, 0, ksize=sobel_kernel)
    sobely = cv2.Sobel(gray, cv2.CV_64F, 0, 1, ksize=sobel_kernel)