package biomav;
import java.awt.image.BufferedImage;
import java.io.File;
import java.io.IOException;
import javax.imageio.ImageIO;
public class PoleDetector implements PolePositionPollInterface
{
public final int R = 255;
public final int G = 144;
public final int B = 0;
int[] image;
byte[] imageBytes;
boolean usingByteImage;
int width = VideoPollInterface.FRONT_VIDEO_FRAME_WIDTH;
int height= VideoPollInterface.FRONT_VIDEO_FRAME_HEIGHT;
int[] orangePixelCountColumns;
double RG;
double RB;
double GB;
double allowedDelta = 0.2;
double cameraAngle = 93;
double poleWidth = 0.15;
int minimalOrangePixelsForPoleColumn = 15;
double[] angleDistancePole = new double[2];
public static void main(String args[])
{
PoleDetector pd = new PoleDetector();
int[] image = pd.readImage("C:/Users/Laurie/Desktop/simulatorPole.PNG");
pd.setImage(image);
System.out.println("pole angle in radions from function: " + pd.getPositionPole());
}
{
orangePixelCountColumns = new int[width];
calculateRatios(R, G, B);
}
{
if (g == 0)
g = 0.01;
if(b == 0)
b = 0.01;
RG = (double) r / (double) g;
RB = (double) r / (double) b;
GB = (double) g / (double) b;
}
{
angleDistancePole = new double[2];
setImage(image);
detectOrangePixels(image);
getPositionPole();
return angleDistancePole;
}
{
double nColumns = 0;
int[] columnNumbers = new int[orangePixelCountColumns.length];
double meanColumn;
boolean leftOfMiddle;
double percentagePoleInView = 0;
double viewWidthAtPole = 0;
double viewDistanceAtPole = 0;
double poleDistance = 0;
double poleAngle = 0;
double pixelsFromCenter = 0;
double distanceFromCenter = 0;
for(int i=0;i<orangePixelCountColumns.length; i++)
{
if(orangePixelCountColumns[i] > minimalOrangePixelsForPoleColumn)
{
columnNumbers[(int) nColumns] = i;
nColumns++;
}
}
meanColumn = 0;
for(int i=0;i<nColumns;i++)
{
meanColumn += columnNumbers[i];
}
meanColumn /= nColumns;
leftOfMiddle = meanColumn <= (double) (width/2);
if(Math.abs(meanColumn - (double) width/2) < nColumns/2 )
{
percentagePoleInView = nColumns / (double) (width);
viewWidthAtPole = poleWidth / percentagePoleInView;
viewDistanceAtPole = (viewWidthAtPole/2.0) / Math.tan(Math.toRadians(cameraAngle/2.0));
pixelsFromCenter = Math.abs(((double) width/2) - meanColumn);
distanceFromCenter = pixelsFromCenter * (viewWidthAtPole / ((double) width));
poleDistance = Math.sqrt(Math.pow(viewDistanceAtPole, 2) + Math.pow(distanceFromCenter, 2));
poleAngle = Math.atan(distanceFromCenter/viewDistanceAtPole);
}
else
{
percentagePoleInView = nColumns / (double) (width / 2);
viewWidthAtPole = poleWidth / percentagePoleInView;
viewDistanceAtPole = viewWidthAtPole / Math.tan(Math.toRadians(cameraAngle / 2.0));
pixelsFromCenter = Math.abs(((double) width/2) - meanColumn);
distanceFromCenter = pixelsFromCenter * (viewWidthAtPole / ((double) width / 2.0));
poleDistance = Math.sqrt(Math.pow(viewDistanceAtPole, 2) + Math.pow(distanceFromCenter, 2));
poleAngle = Math.atan(distanceFromCenter/viewDistanceAtPole);
}
angleDistancePole[0] = poleAngle;
angleDistancePole[1] = poleAngle;
if(leftOfMiddle)
{
return -poleAngle;
}
else
{
return poleAngle;
}
}
{
orangePixelCountColumns = new int[width];
for(int i=0; i<width; i++)
{
for(int j=0; j<height; j++)
{
if(isOrange(i, j, image))
{
orangePixelCountColumns[i]++;
}
}
}
}
{
orangePixelCountColumns = new int[width];
for(int i=0; i<width; i++)
{
for(int j=0; j<height; j++)
{
if(isOrange(i, j, image))
{
orangePixelCountColumns[i]++;
}
}
}
}
public boolean isOrange(
int i,
int j,
byte[]image)
{
double r, g, b;
r = image[(j*width + i) * 3];
g = image[(j*width + i) * 3 + 1];
b = image[(j*width + i) * 3 + 2];
if(r < 0)
{
r += 256;
}
if(g < 0)
{
g += 256;
}
if(b < 0)
{
b += 256;
}
if(g == 0)
{
g = 0.01;
}
if(b == 0)
{
b = 0.01;
}
double rg, rb, gb;
rg = r/g;
rb = r/b;
gb = g/b;
if((Math.abs(rg - RG) < allowedDelta) && (Math.abs(rb - RB) < allowedDelta) && (Math.abs(gb - GB) < allowedDelta))
{
return true;
}
return false;
}
public boolean isOrange(
int i,
int j,
int[]image)
{
double r, g, b;
int pixel = image[j*width + i];
r = ((pixel & 0x00ff0000) >> 16);
g = ((pixel & 0x0000ff00) >> 8);
b = pixel & 0x000000ff;
if(g == 0)
{
g = 0.01;
}
if(b == 0)
{
b = 0.01;
}
double rg, rb, gb;
rg = r/g;
rb = r/b;
gb = g/b;
if((Math.abs(rg - RG) < allowedDelta) && (Math.abs(rb - RB) < allowedDelta) && (Math.abs(gb - GB) < allowedDelta))
{
return true;
}
return false;
}
{
try
{
File file = new File(filename);
BufferedImage tempImage = ImageIO.read(file);
width = tempImage.getWidth();
height = tempImage.getHeight();
image = tempImage.getRGB(0, 0, width, height, image, 0, width);
usingByteImage = false;
return image;
}
catch (IOException e) {}
return null;
}
{
image = _image;
usingByteImage = false;
detectOrangePixels(image);
}
{
imageBytes = _image;
usingByteImage = true;
detectOrangePixels(imageBytes);
}
@Override
return 0;
}
@Override
Double [] polePositions = new Double[1];
if( Double.isNaN(getPositionPole()))
return new Double[0];
polePositions[0] = getPositionPole();
return polePositions;
}
}