レベル4：OpenCVでレーンを検出する

        # sample
        roi_vertices = calc_roi_vertices(cv_bgr,
                                         top_width_rate=0.3,top_height_position=0.15,
                                         bottom_width_rate=0.8,bottom_height_position=0.9)

        # robocar camera demo_lane
        roi_vertices = calc_roi_vertices(cv_bgr,
                                         top_width_rate=0.9,top_height_position=0.15,
                                         bottom_width_rate=2.0,bottom_height_position=1)

def to_roi(cv_bgr, vertices):
    """
    Region Of Interest
    頂点座標でmaskを作り、入力画像に適用する
    args:
        cv_bgr: OpenCV BGR画像データ
        vertices: 領域の頂点座標
    return:
        cv_bgr_result: 領域外を黒くしたOpenCV BGR画像データ
    """
    mask = np.zeros_like(cv_bgr)
    if len(mask.shape)==2:
        cv2.fillPoly(mask, vertices, 255)
    else:
        cv2.fillPoly(mask, vertices, (255,)*mask.shape[2]) # in case, the input image has a channel dimension
    return cv2.bitwise_and(cv_bgr, mask)

        # robocar camera demo_lane
        ipm_vertices = calc_ipm_vertices(cv_bgr,
                                         top_width_rate=0.9,top_height_position=0.15,
                                         bottom_width_rate=2.0,bottom_height_position=1)

def to_ipm(cv_bgr,ipm_vertices):
    '''
    Inverse Perspective Mapping
    TopViewに画像を変換する
    args:
        cv_bgr: OpenCV BGR画像データ
        ipm_vertices: 視点変換座標
    return:
        cv_bgr_ipm: 変換後のOpenCV BGR画像データ
    '''
    rows, cols = cv_bgr.shape[:2]

    offset = cols*.25

    src = ipm_vertices
    dst = np.float32([[offset, 0], [cols - offset, 0], [cols - offset, rows], [offset, rows]])

    # srcとdst座標に基づいて変換行列を作成する
    matrix = cv2.getPerspectiveTransform(src, dst)

    # 変換行列から画像をTopViewに変換する
    cv_bgr_ipm = cv2.warpPerspective(cv_bgr, matrix, (cols, rows))

    return cv_bgr_ipm

def reverse_ipm(cv_bgr,ipm_vertices):
    '''
    IPM逆変換を行う
    args:
        cv_bgr: OpenCV BGR画像データ
        ipm_vertices: 変換時に使ったIPM座標
    return:
        cv_bgr_ipm_reverse: OpenCV BGR画像データ
    '''
    rows, cols = cv_bgr.shape[:2]

    offset = cols*.25

    dst = ipm_vertices
    src = np.float32([[offset, 0], [cols - offset, 0], [cols - offset, rows], [offset, rows]])

    matrix = cv2.getPerspectiveTransform(src, dst)

    cv_bgr_ipm_reverse = cv2.warpPerspective(cv_bgr, matrix, (cols,rows))
    return cv_bgr_ipm_reverse

def to_white(cv_bgr):
    '''
    白色だけを抽出する
    args:
        cv_bgr: OpenCV BGR画像データ
    return:
        cv_bgr_result: OpenCV BGR画像データ
    '''
    print("to_white()")
    t0 = time.time()
    cv_hsv = cv2.cvtColor(cv_bgr, cv2.COLOR_BGR2HSV)
    # 取得する色の範囲を指定する
    lower1_color = np.array([0,0,120])
    upper1_color = np.array([45,40,255])
    lower2_color = np.array([50,0,200])
    upper2_color = np.array([100,20,255])
    lower3_color = np.array([45,0,225])
    upper3_color = np.array([100,40,255])
    lower4_color = np.array([50,0,180])
    upper4_color = np.array([75,25,200])

    # 指定した色に基づいたマスク画像の生成
    white1_mask = cv2.inRange(cv_hsv,lower1_color,upper1_color)
    white2_mask = cv2.inRange(cv_hsv,lower2_color,upper2_color)
    white3_mask = cv2.inRange(cv_hsv,lower3_color,upper3_color)
    white4_mask = cv2.inRange(cv_hsv,lower4_color,upper4_color)
    img_mask = cv2.bitwise_or(white1_mask, white2_mask)
    img_mask = cv2.bitwise_or(img_mask, white3_mask)
    img_mask = cv2.bitwise_or(img_mask, white4_mask)
    # フレーム画像とマスク画像の共通の領域を抽出する
    cv_bgr_result = cv2.bitwise_and(cv_bgr,cv_bgr,mask=img_mask)
    t1 = time.time()
    dt_cv = t1-t0
    print("Conversion took {:.5} seconds".format(dt_cv))

    return cv_bgr_result

def to_bin(cv_bgr):
    '''
    画像を2値化する
    args:
        cv_bgr: OpenCV BGR画像データ
    return:
        cv_bin: OpenCV 2値化したグレースケール画像データ
    '''
    print("to_bin()")
    t0 = time.time()
    # ガウスぼかしで境界線の弱い部分を消す
    cv_gauss = cv2.GaussianBlur(cv_bgr,(5,5),0) # サイズは奇数
    cv_gray = cv2.cvtColor(cv_gauss, cv2.COLOR_BGR2GRAY)
    #plt.title('gray')
    #plt.imshow(cv_gray)
    #plt.show()

    # 色の薄い部分を削除する
    ret, mask = cv2.threshold(cv_gray, 20, 255, cv2.THRESH_BINARY)
    mask = cv2.bitwise_and(cv_gray,cv_gray,mask=mask)
    cv_gray = cv2.bitwise_and(cv_gray,cv_gray,mask=mask)
    #plt.title('gray')
    #plt.imshow(cv_gray)
    #plt.show()

    # 入力画像，閾値，maxVal，閾値処理手法
    ret,cv_bin = cv2.threshold(cv_gray,0,255,cv2.THRESH_BINARY|cv2.THRESH_OTSU);

    t1 = time.time()
    dt_cv = t1-t0
    print("Conversion took {:.5} seconds".format(dt_cv))
    return cv_bin

    # 画面下半分のピクセル数をカウントする
    histogram = np.sum(cv_bin[int(rows/2):,:], axis=0)

def sliding_windows(cv_bin):
    '''
    sliding windowを行い、左右レーンを構成するピクセル座標を求める
    args:
        cv_bin: 2値化したレーン画像のOpenCV grayscale画像データ
    returns:
        cv_rgb_sliding_windows: sliding window処理のOpenCV RGB画像データ
        histogram: 入力画像の下半分の列毎のピクセル総数の配列(1,col)
        left_x: 左レーンを構成するピクセルのx座標群
        left_y: 左レーンを構成するピクセルのy座標群
        right_x: 右レーンを構成するピクセルのx座標群
        right_y: 右レーンを構成するピクセルのy座標群
    '''

def polynormal_fit(pts_y,pts_x):
    '''
    曲線を構成する点から二次多項式を求める
    (y軸の値は均等に取るのでxを求める式となる)
    args:
        pts_x: x座標配列
        pts_y: y座標配列
    returns:
        polyfit_const: 二次曲線x=ay**2+by+cの[a,b,c]の値
    '''
    polyfit_const = np.polyfit(pts_y, pts_x, 2)
    return polyfit_const

                # 左右センターの二次多項式と座標を求める
                left_polyfit_const, right_polyfit_const, center_polyfit_const, \
                    pts_left, pts_right, pts_center = calc_lr_curve_lane(left_x,left_y,right_x,right_y,plot_y)

def calc_lr_curve_lane(left_x,left_y,right_x,right_y,plot_y):
...
    # 左右の二次多項式を求める
    left_polyfit_const = polynormal_fit(left_y,left_x)
    right_polyfit_const = polynormal_fit(right_y,right_x)
    # センターの二次多項式を求める
    center_polyfit_const = [(left_polyfit_const[0]+right_polyfit_const[0])/2,(left_polyfit_const[1]+right_polyfit_const[1])/2,(left_polyfit_const[2]+right_polyfit_const[2])/2]

            # 等間隔なy座標を生成する
            plot_y = np.linspace(0, rows-1, rows)

def calc_lr_curve_lane(left_x,left_y,right_x,right_y,plot_y):
...
    # y軸に対する左右の二次多項式上のx座標を求める
    left_plot_x = left_polyfit_const[0]*plot_y**2 + left_polyfit_const[1]*plot_y + left_polyfit_const[2]
    right_plot_x = right_polyfit_const[0]*plot_y**2 + right_polyfit_const[1]*plot_y + right_polyfit_const[2]
    # y軸に対するセンターの二次多項式上のx座標を求める
    center_plot_x = center_polyfit_const[0]*plot_y**2 + center_polyfit_const[1]*plot_y + center_polyfit_const[2]

    '''
    x,y座標を[x,y]配列に変換する
    左右間の領域描画用に、右側座標は逆順にする
    pts_centerは中間線上の座標
    '''
    pts_left = np.int32(np.array([np.transpose(np.vstack([left_plot_x, plot_y]))]))
    pts_right = np.int32(np.array([np.flipud(np.transpose(np.vstack([right_plot_x, plot_y])))]))
    pts_center = np.int32(np.array([np.transpose(np.vstack([center_plot_x, plot_y]))]))

    return left_polyfit_const, right_polyfit_const, center_polyfit_const, pts_left, pts_right, pts_center

def calc_curve(curve_y0,curve_y1,curve_polyfit_const):
    '''
    曲線を計算する
    args:
        curve_y0: 曲線上y座標
        curve_y1: 曲線下y座標
        curve_ployfit_const: 曲線の定数
    returns:
        x: 円の中心x座標
        y: 円の中心y座標
        r: 円の半径r (曲率半径r)
        rotate_deg: 弧の回転角度
        angle_deg: 弧の描画角度
        curve_tilt_deg: y軸との傾き角度
    '''
    # 中間点における曲率半径Rを求める
    curve_y = curve_y1-curve_y0
    curve_r = calc_curvature_radius(curve_polyfit_const,curve_y)

    # x座標を求める
    curve_x0 = curve_polyfit_const[0]*curve_y0**2 + curve_polyfit_const[1]*curve_y0 + curve_polyfit_const[2]
    curve_x1 = curve_polyfit_const[0]*curve_y1**2 + curve_polyfit_const[1]*curve_y1 + curve_polyfit_const[2]

    # 2点と半径と曲線の定数から円の中心座標を求める
    py = curve_y1
    px = curve_x1
    qy = curve_y0
    qx = curve_x0
    r = curve_r
    x,y = calc_circle_center_point(px,py,qx,qy,r,curve_polyfit_const[0])

    # 弧の描画角度を求める
    rotate_deg, angle_deg = calc_ellipse_angle(py,px,qy,qx,r,x,y,curve_polyfit_const[0])
    print("py={},px={},qy={},qx={},x={},y={},r={}".format(py,px,qy,qx,x,y,r))
    print("rotate_deg={} angle_deg={}".format(rotate_deg,angle_deg))

    # 垂直方向との傾き角を求める
    # プラスなら左カーブ、マイナスなら右カーブ
    curve_tilt_rad = math.atan((px-qx)/(py-qy))
    curve_tilt_deg = math.degrees(curve_tilt_rad)
    print("curve_tilt_deg={}".format(curve_tilt_deg))

    return x,y,r,rotate_deg,angle_deg,curve_tilt_deg

def calc_circle_center_point(px,py,qx,qy,r,const):
    '''
    2点と半径rから円の中心座標を求める
    args:
        py: 円上の点Pのy座標
        px: 円上の点Pのx座標
        qy: 円上の点Qのy座標
        qx: 円上の点Qのx座標
        r: 円の半径r
        const: 候補2点の識別子
    return:
        x: 円の中心点x座標
        y: 円の中心点y座標
    '''
    if const > 0:
        x=((py - qy)*(np.sqrt(-(px**2 - 2*px*qx + py**2 - 2*py*qy + qx**2 + qy**2)*(px**2 - 2*px*qx + py**2 - 2*py*qy + qx**2 + qy**2 - 4*r**2))*(px - qx) - (py + qy)*(px**2 - 2*px*qx + py**2 - 2*py*qy + qx**2 + qy**2)) + (px**2 + py**2 - qx**2 - qy**2)*(px**2 - 2*px*qx + py**2 - 2*py*qy + qx**2 + qy**2))/(2*(px - qx)*(px**2 - 2*px*qx + py**2 - 2*py*qy + qx**2 + qy**2))
        y=(np.sqrt(-(px**2 - 2*px*qx + py**2 - 2*py*qy + qx**2 + qy**2)*(px**2 - 2*px*qx + py**2 - 2*py*qy + qx**2 + qy**2 - 4*r**2))*(-px + qx)/2 + (py + qy)*(px**2 - 2*px*qx + py**2 - 2*py*qy + qx**2 + qy**2)/2)/(px**2 - 2*px*qx + py**2 - 2*py*qy + qx**2 + qy**2)
    else:
        x=(-(py - qy)*(np.sqrt(-(px**2 - 2*px*qx + py**2 - 2*py*qy + qx**2 + qy**2)*(px**2 - 2*px*qx + py**2 - 2*py*qy + qx**2 + qy**2 - 4*r**2))*(px - qx) + (py + qy)*(px**2 - 2*px*qx + py**2 - 2*py*qy + qx**2 + qy**2)) + (px**2 + py**2 - qx**2 - qy**2)*(px**2 - 2*px*qx + py**2 - 2*py*qy + qx**2 + qy**2))/(2*(px - qx)*(px**2 - 2*px*qx + py**2 - 2*py*qy + qx**2 + qy**2))
        y=(np.sqrt(-(px**2 - 2*px*qx + py**2 - 2*py*qy + qx**2 + qy**2)*(px**2 - 2*px*qx + py**2 - 2*py*qy + qx**2 + qy**2 - 4*r**2))*(px - qx)/2 + (py + qy)*(px**2 - 2*px*qx + py**2 - 2*py*qy + qx**2 + qy**2)/2)/(px**2 - 2*px*qx + py**2 - 2*py*qy + qx**2 + qy**2)

    return x,y

import sympy
import time
start_time = time.time()
x=sympy.Symbol('x') # 変数xを出力時の文字'x'として用意する
y=sympy.Symbol('y') # 変数yを出力時の文字'y'として用意する
r=sympy.Symbol('r') # 変数rを出力時の文字'r'として用意する
px=sympy.Symbol('px')
py=sympy.Symbol('py')
qx=sympy.Symbol('qx')
qy=sympy.Symbol('qy')

"""
方程式: solve
連立方程式 (x-px)**2+(y-py)**2=r**2, (x-qx)**2+(y-qy)**2=r**2 の解を求める
"""
b= sympy.solve ([(x-px)**2+(y-py)**2-r**2, (x-qx)**2+(y-qy)**2-r**2],[x,y])  # 連立方程式を解く
print("time:{}".format(time.time() - start_time))
print(b)

    # IPM変換後の画像におけるx,yメートル(黒い部分も含む)
    X_METER=3
    Y_METER=1.5
...
                ''''
                実測値 メートル座標系における計算
                '''
                # ピクセルをメートルに変換
                ym_per_pix = 1.0*Y_METER/rows
                xm_per_pix = 1.0*X_METER/cols
                # 等間隔なy座標を生成する
                plot_ym = np.linspace(0, rows-1, rows)*ym_per_pix
                # 左右センターの二次多項式と座標を求める
                left_polyfit_const, right_polyfit_const, center_polyfit_const, \
                    _pts_left, _pts_right, _pts_center = calc_lr_curve_lane(left_x*xm_per_pix,left_y*ym_per_pix,right_x*xm_per_pix,right_y*ym_per_pix,plot_ym)

                # 中央線までの距離を計算する
                # 最も下の位置で計算する
                bottom_y = np.max(plot_ym)
                bottom_x = center_polyfit_const[0]*bottom_y**2 + center_polyfit_const[1]*bottom_y + center_polyfit_const[2]
                meters_from_center = bottom_x - (cols/2)*xm_per_pix

    pts_ellipse = np.array(pts_center[:,int(pts_center.shape[1]/2):,:]).astype(int)
    pts_ellipse = np.concatenate((pts_ellipse,np.array([[[x,y]]]).astype(int)),axis=1)
    cv2.fillPoly(cv_rgb_ellipse, [pts_ellipse], (255,0,0))