RALPH examples

This notebook implements a version of the RALPH algorithm for road following with some examples.

Definitions

Turn off annoying warnings

Off[Plot::"plnr"];

Extract trapezoidal region

extractTrapezoid[
          m_,  {ymin_,  dmin_}, {ymax_,  
            dmax_}] := 
Module[{m2,  xsize,  
            tbl}, 
m2  =  
            Reverse[m]; 
xsize  =  
            Length[m  //  
                First]; 
tbl  =  
DeleteCases[
              Table[If[i  <  ymin  ||  i  >  ymax, 
                  Null, 
{i, 
                    Round[{xsize/2  -  
                          dmin/2 - (i - ymin)/(ymax - ymin) (dmax - 
                                  dmin)/2,  
                        xsize/2  +  
                          dmin/2 + (i - ymin)/(ymax - ymin) (dmax - 
                                  dmin)/2}]}], {i, 1,  Length[m]}], 
              Null]; 
Take[
                m2[[#[[1]]]], #[[2]]] &  /@  
            tbl
];

Subsample a line of an image

subSampleLine[zl_,  
          n_] := 
Module[{len,  
            di}, 
len  =  
            Length[zl]; 
di  =  (len - 1)/(n - 
                  1); 
Table[
            zl[[Round[i]]], {i, 1,  len,  
              di}]
];

Subsample an extracted trapezoidal region

subSampleTrapezoid[z_,  
          n_] := 
Module[{z1}, 
z1  =  subSampleLine[#,  n] &  /@  z; 
subSampleLine[z1,  
            n]
];

Subsample a trapezoidal region from an image

subSampleImage[m_,  {ymin_,  dmin_}, {ymax_,  dmax_},  
          n_] := 
Module[{z1, 
            z2}, 
z1  =  
            extractTrapezoid[
              m,  {ymin,  dmin},  {ymax,  
                dmax}]; 
z2  =  
            subSampleTrapezoid[z1, n]
];

Rectify one processed line

rectifyOne[
          l_] := 
Module[{a, 
            b}, 
{a, b}  =  l; 
Drop[
            RotateRight[PadLeft[b, Abs[a] + Length[b], -1], a], 
            Abs[a]]
];

Rectify an image based on a given curvature

The function curv[y] is expected to return an x value for y values between 0 and 1 that corresponds to that lines shift. A value of 0.5 corresponds to no shift; negative values correspond to left shift, positive values correspond to right shift.

rectify[mat_,  
          curv_] := 
Module[{n,  m,  
            tbl}, 
{n, m}  =  
            Dimensions[mat]; 
tbl = 
            Table[{Round[m*(curv[i  //  N] -  .5)],  
                mat[[n - i*(n - 1)]]}, {i, 0, 1, 
                1/(n - 1)}]; 
 
          Reverse[rectifyOne  /@  tbl]
];

Compute vertical scan line summations

scanLines[
          mat_] := 
Module[{m2}, 
m2  =  DeleteCases[#,  -1] &  /@  
              Transpose[
                mat]; 
((Plus  @@  #)/
                  Length[#]) &  /@  m2];

Compute measure

measure[
          mat_] := 
Plus  @@  (Abs[#[[1]] - #[[2]]] &  /@  Partition[scanLines[mat], 2, 1])  //  
          N;

Sloped line curvature

lineCurv[y_,  a_] :=  0.5  +  (a - 0.5) y

PlotLineCurv[
          a_] := 
Module[{x, y, 
            res}, 
res  = 
            y  /.  (Solve[x  ==  lineCurv[y,  a], y]  //  
                  Flatten); 
Plot[res, {x,  0, 1},  
            PlotRange -> {{0, 1}, {0, 1}},  AspectRatio -> 1,  
            Frame -> True]];

Circle curvature

curveCurv[y_,  a_] := Sqrt[(a - 0.5)^2  -  y^2] + a

PlotCurveCurv[
          a_] := 
Module[{x, y, 
            res}, 
res  = 
            y  /.  (Solve[x  ==  curveCurv[y, a], 
                    y][[2]]); 
          Plot[res, {x,  0, 1},  PlotRange -> {{0, 1}, {0, 1}}, 
            AspectRatio -> 1,  
            Frame -> True]
];

Straight road image example

Load in sample image

Be sure that the directory points to the image to be loaded in.

SetDirectory["/Users/nechyba/public_html/eel5840/neural_networks/\
data"];

Timing[g  =  
          Import["straight_road_bw.jpg", "JPEG"];]

Show sample image

Show[g];

Subsample trapezoidal region

m  =  g[[1, 1]];
tst  =  subSampleImage[m,  {48, 118}, {110, 236}, 50];
Show[Graphics[Raster[Reverse[tst/255]]],  
        AspectRatio -> Automatic,  
        ImageSize -> {300, 300}];

Test different straight line hypotheses

f[a_] := measure[rectify[tst, lineCurv[#, a] &]];
result  =  Table[{i, f[i]}, {i, 0, 1, 1/21}];
ListPlot[result,  PlotJoined -> True, Frame -> True, 
        GridLines -> {{0.5}, Automatic}];

Best result

bestResult  =  0.45;

Best straight line hypothesis

PlotLineCurv[1 - bestResult];

Illustrate corresponding shifted image

tst2  =  rectify[tst, lineCurv[#, bestResult] &];
Show[Graphics[Raster[Reverse[tst2/255]]],  
        AspectRatio -> Automatic,  
        ImageSize -> {300, 300}];

Compare to original

Show[Graphics[Raster[Reverse[tst/255]]],  
        AspectRatio -> Automatic,  
        ImageSize -> {300, 300}];

Show scan line function for best curvature hypothesis

bestScanLine  =  scanLines[tst2];
g1  =  
        ListPlot[bestScanLine,  PlotJoined -> True, 
          Frame -> True,  PlotRange -> All,  
          PlotStyle -> {RGBColor[1, 0, 0]}];

Compare to baseline straight-line, zero-orientation hypothesis

originalScanLine  =  scanLines[tst];
g2  =  
        ListPlot[originalScanLine,  PlotJoined -> True, 
          Frame -> True,  PlotRange -> All, 
          PlotStyle -> {RGBColor[0, 0, 1]},  
          DisplayFunction -> Identity];
Show[g1, g2];

Curved road image example

Load in sample image

Be sure that the directory points to the image to be loaded in.

Timing[g  =  
          Import["curved_road_bw.jpg", "JPEG"];]

Show sample image

Show[g];

Subsample trapezoidal region

m  =  g[[1, 1]];
tst  =  subSampleImage[m,  {48, 118}, {110, 236}, 50];
Show[Graphics[Raster[Reverse[tst/255]]],  
        AspectRatio -> Automatic,  
        ImageSize -> {300, 300}];

Test different straight line hypotheses

f[a_] := measure[rectify[tst, curveCurv[#, a] &]];
result  =  
        Table[{-Log[-i], f[i]}, {i, -10, -1,  
            1/4}];
ListPlot[result,  PlotJoined -> True, Frame -> True, 
        GridLines -> Automatic];

Best result

bestResult  =  -Exp[0.7]

Best circle curvature hypothesis (reflected)

PlotCurveCurv[bestResult];

Illustrate corresponding shifted image

tst2  =  rectify[tst, curveCurv[#, bestResult] &];
Show[Graphics[Raster[Reverse[tst2/255]]],  
        AspectRatio -> Automatic,  
        ImageSize -> {300, 300}];

Compare to original image

Show[Graphics[Raster[Reverse[tst/255]]],  
        AspectRatio -> Automatic,  
        ImageSize -> {300, 300}];

Show scan line function for best curvature hypothesis

bestScanLine  =  scanLines[tst2];
g1  =  
        ListPlot[bestScanLine,  PlotJoined -> True, 
          Frame -> True,  PlotRange -> All,  
          PlotStyle -> {RGBColor[1, 0, 0]}];

Compare to baseline straight-line, zero-orientation hypothesis

originalScanLine  =  scanLines[tst];
g2  =  
        ListPlot[originalScanLine,  PlotJoined -> True, 
          Frame -> True,  PlotRange -> All, 
          PlotStyle -> {RGBColor[0, 0, 1]},  
          DisplayFunction -> Identity];
Show[g1, g2];