public List<Sphere> FindSpheres(RangeImage rangeData)
{
int maxLp = 0;
List<Sphere> sphereList = new List<Sphere>();
do {
dataProcess(rangeData);
subsTol = subsTol + .05;
maxLp++;
} while (groupNum <= 6 && maxLp < 9);
for (int i = 1; i < groupNum; i++)
{
if (spBl[i] == 1)
{
Sphere updtSphere = new Sphere(spCr[i],(float).11777);
sphereList.Add(updtSphere);
}
}
return sphereList;
}
public HeightMap(RangeImage RangeImage)
{
m_RangeImage = RangeImage;
occ_count = -1;
GRID_WIDTH_Z = 0;
GRID_WIDTH_Z = 0;
}
//Detect the Edges!
//Returns a bi-dimensional array sized as RangeImage width and height + 1
//with an "1" for every pixel (x,y) where an edge is detected
//Parameters
// RangeImage object, double with threshold to determine "layers" in the picture
// (How separated the object has to be from the background
// to be considered an object and not part of the background)
/**
* @brief Detect edges:
* @param threshold A float
* @param rangeData A RangeImage object - @brief Double with threshold to determine "layers" in the picture
* @brief (How separated the object has to be from the background
* @brief to be considered an object and not part of the background)
* @returns edgesGen - A float[,]
* @brief A bi-dimensional array sized as RangeImage width and height + 1
*/
public float[,] detectEdges(RangeImage rangeData, float threshold)
{
int cont = 0;
float[,] edgesGen = new float[rangeData.Width + 1, rangeData.Height + 1];
for (int i = 2; i < rangeData.Width - 3; i++)
{
for (int j = 2; j < rangeData.Height - 3; j++)
{
//Calculate neighboring pixels
float wZ = rangeData.GetCustom(i, j - 1);
float eZ = rangeData.GetCustom(i, j + 1);
float nZ = rangeData.GetCustom(i - 1, j);
float sZ = rangeData.GetCustom(i + 1, j);
double diffX = Math.Abs(wZ - eZ);
double diffY = Math.Abs(nZ - sZ);
//If we have a match and its not neighboring other edges, we mark it as edge
if ((diffX > threshold * rangeData.GetCustom(i, j) && edgesGen[i, j - 1] == 0 && edgesGen[i, j - 2] == 0) || (diffY > threshold * rangeData.GetCustom(i, j) && edgesGen[i - 1, j] == 0 && edgesGen[i - 2, j] == 0))
//if ((diffX > threshold && edgesGen[i, j - 1] == 0 && edgesGen[i, j - 2] == 0) || (diffY > threshold && edgesGen[i - 1, j] == 0 && edgesGen[i - 2, j] == 0))
//if ((diffX > threshold && edgesGen[i, j - 1] == 0) || (diffY > threshold && edgesGen[i - 1, j] == 0))
//if (diffX > threshold || diffY > threshold)
{
//Console.WriteLine("Edge at: ("+i+","+j+")");
cont++;
edgesGen[i, j] = 1;
}
}
}
return edgesGen;
}
public void StartRecognition(RangeImage rangeImage)
{
rangeData = rangeImage;
height = rangeImage.Height;
width = rangeImage.Width;
bm = new Bitmap(width, height);
g = Graphics.FromImage(bm);
image.BackgroundImage = bm;
image.BackgroundImageLayout = ImageLayout.None;
image.Size = new Size(width, height);
g.Clear(Color.White);
SphereRecognition sr = new SphereRecognition();
//System.Windows.Forms.MessageBox.Show("W/H = " + width + "/" + height);
image.Bounds = new Rectangle(0, 0, width, height);
paintImage();
edges = sr.detectEdges(rangeData, float.Parse(conf1.Text));
//doFix();
/**/
paintEdges();
List<Sphere> spheres = sr.FindSpheres(rangeData, edges, int.Parse(maxRadius.Text), int.Parse(minRadius.Text), float.Parse(conf1.Text), float.Parse(conf2.Text));
Sphere actual;
for (int i = 0; i < spheres.Count; i++)
{
actual = spheres[i];
//int tmp = actual.center.X;
actual.center.Y = height - actual.center.Y;
//actual.center.Y = tmp;
//actual.center.X += (int)(actual.radius*1.35f);
g.DrawEllipse(Pens.Blue, (int)actual.center.X - actual.radius, ((int)actual.center.Y - actual.radius), (int)actual.radius * 2, (int)actual.radius * 2);
g.DrawLine(Pens.Blue, (actual.center.X - 1), (actual.center.Y - 1) - 5, (actual.center.X - 1), (actual.center.Y - 1) + 5);
g.DrawLine(Pens.Blue, (actual.center.X - 1) - 5, (actual.center.Y - 1), (actual.center.X - 1) + 5, (actual.center.Y - 1));
image.Refresh();
//g.DrawEllipse(Pens.Blue, (int)actual.center.Y - actual.radius, ((int)actual.center.X - actual.radius), (int)actual.radius * 2, (int)actual.radius * 2);
//g.DrawLine(Pens.Blue, (actual.center.Y - 1), (actual.center.X - 1) - 5, (actual.center.Y - 1), (actual.center.X - 1) + 5);
//g.DrawLine(Pens.Blue, (actual.center.Y - 1) - 5, (actual.center.X - 1), (actual.center.Y - 1) + 5, (actual.center.X - 1));
float formula2 = ((17f / 9 * (float)actual.radius) - 94f / 9f) / 1000f;
String algo = "haha " + actual.radius * (Math.Cos(89.9 * (Math.PI / 180)) * actual.avgDistance) + "\n" + (actual.midPoints - actual.avgDistance) + "\n" + Math.Abs(actual.midPoints - actual.avgDistance) + "\n" + formula2 + "\n" + (Math.Abs(actual.midPoints - actual.avgDistance) - formula2);
//System.Windows.Forms.MessageBox.Show("ABSOLUTE=\nX="+actual.absolutePosition.X+"\nY="+actual.absolutePosition.Y+"\nZ="+actual.absolutePosition.Z+"\n\nx=" + actual.center.X + "\ny=" + actual.center.Y + "\n\nR=" + actual.radius + "\nDiffC=" + actual.diffCircumference + "\nS=" + actual.score + "\nAVGD=" + actual.avgDistance + "\nAVGM=" + actual.midPoints + "\nAVGE=" + actual.avgEdges + "\n\n\n" + algo);
//paintImage();
//paintEdges();
}
/**/
}
private void StartRecognition(String filename)
{
rangeData = new RangeImage();
String extension = filename.Substring(filename.LastIndexOf(".")+1).ToLower();
switch (extension)
{
case "rangeimage":
rangeData = RangeImage.Read(filename);
width = rangeData.Width;
height = rangeData.Height;
button2.Enabled = true;
break;
case "scan":
FileStream f = File.Open(filename, FileMode.Open);
BinaryReader br = new BinaryReader(f);
width = br.ReadInt32();
height = br.ReadInt32();
rangeData.setWidth(width);
rangeData.setHeight(height);
if (rangeData.initializeRangeValues(width, height))
{
for (int i = 0; i < height * width; i++)
{
try
{
rangeData.addValue(i, br.ReadSingle());
}
catch (Exception E) { }
}
//System.Windows.Forms.MessageBox.Show("W/H = " + width+"/"+height);
filename = filename.Replace(".scan", ".rangeimage");
rangeData.Write(filename);
/*if (value > max)
max = value;
if (value < min || min == 0)
min = value;
int val = (int)Math.Min(255, Math.Abs((value / 75f) * 255f));
SolidBrush sb = new SolidBrush(Color.FromArgb(255, val, val, val));
g.FillRectangle(sb, i/width, i%width, 1, 1);
*/
//System.Windows.Forms.MessageBox.Show("avg" + avg/(height*width));
//System.Windows.Forms.MessageBox.Show("min" + min);
//System.Windows.Forms.MessageBox.Show("max" + max);
f.Close();
}
button2.Enabled = true;
break;
default:
System.Windows.Forms.MessageBox.Show("The format of the data is not recognized");
return;
break;
}
StartRecognition(rangeData);
/*
TextWriter tw = new StreamWriter("c:\\GROUP.txt");
for(int i=0;i<width*height;i++)
{
tw.WriteLine(i/width);
tw.WriteLine(i % width);
tw.WriteLine(rangeData.Get(i));
}
*/
}
private void LoadCommand_Click(object sender, EventArgs e)
{
if (OpenFileDialog.ShowDialog() == DialogResult.OK)
{
m_RangeImage = RangeImage.Read(OpenFileDialog.FileName);
InitRangeImage();
GenerateFlatImage();
Canvas.Invalidate();
}
}
private void dataProcess(RangeImage rangeData)
{
int inc = 0;
numP = 0;
groupNum = 0;
TextWriter nw = new StreamWriter("spTesting.txt");
//nw.WriteLine(subsTol);
//Load data into a matrixv and initialize variables
Vector3[] pointLine = rangeData.ConvertToPoints(float.MaxValue);
for (int i = 0; i<VERTICAL_RESOLUTION; i++) {
for (int j = 0; j<HORIZONTAL_RESOLUTION; j++) {
points[i,j] = pointLine[inc];
inc++;
//nw.WriteLine(pointLine[inc].X);
//nw.WriteLine(pointLine[inc].Y);
//nw.WriteLine(pointLine[inc].Z);
gpNm[i,j] = 0; //Initializes the group number for clustering
if (Math.Sqrt(Math.Pow(points[i,j].X,2)+Math.Pow(points[i,j].Y,2)+Math.Pow(points[i,j].Z,2)) < 30.0) {
spPt[i,j] = 0; //Assign the value 0 to all the points (for sphere detection)
} else {
spPt[i,j] = 5;
}
}
}
//preProcess data to filter points that can be potential spheres
int iPr = 0;
int jPr = 0;
for (int i=0;i<VERTICAL_RESOLUTION;i++) {
for (int j=1;j<HORIZONTAL_RESOLUTION;j++) {
if (spPt[i,j] != 5) {
if (iPr != i) {
iPr = i;
jPr = 0;
}
if (Math.Sqrt(Math.Pow(points[i,j].X-points[iPr,jPr].X,2)+Math.Pow(points[i,j].Y-points[iPr,jPr].Y,2)+Math.Pow(points[i,j].Z-points[iPr,jPr].Z,2)) > 2.0*.117775 || jPr == 0) {
spPt[i,j] = 1;
spPt[iPr,jPr] = 1;
//nw.WriteLine(points[i,j].X);
//nw.WriteLine(points[i,j].Y);
//nw.WriteLine(points[i,j].Z);
}
if (Math.Sqrt(Math.Pow(points[i,j].X-points[i,j-1].X,2)+Math.Pow(points[i,j].Y-points[i,j-1].Y,2)+Math.Pow(points[i,j].Z-points[i,j-1].Z,2)) > 2.0*.117775) {
iPr = i;
jPr = j;
}
if (j==400 || j==401 || j==0 || j== 1) {
spPt[i,j] = 1;
}
}
} // End of "j loop"
} //End of "i loop"
//Process data and find points that belong to spheres. Use the sphere equation to process each point and its neighbors
double rad = .11777;
double mag = 0.0;
double[] nrm = {0.0,0.0,1.0};
int sum = 0;
int cond = 0;
int maxSum = 0;
int iMax = 0;
int jMax = 0;
double[] center=new double[3];
double tol = .001;
double z = 0.0;
double ptAng; //Angle used to obtain resolution
int ptRes; //Point resolution
double newPtRes;
int newim;
int newjn;
for (int i=0;i<VERTICAL_RESOLUTION;i++) {
for (int j=10;j<HORIZONTAL_RESOLUTION;j++) {
sum = 0;
cond = 0;
mag = Math.Sqrt(Math.Pow(points[i,j].X,2)+Math.Pow(points[i,j].Y,2)+Math.Pow(points[i,j].Z,2));
nrm[0] = points[i,j].X/mag;
nrm[1] = points[i,j].Y/mag;
nrm[2] = points[i,j].Z/mag;
//nw.WriteLine(nrm[0]);
//nw.WriteLine(nrm[1]);
//nw.WriteLine(nrm[2]);
if (spPt[i,j] == 0) {
center[0] = points[i,j].X + rad*nrm[0];
center[1] = points[i,j].Y + rad*nrm[1];
center[2] = points[i,j].Z + rad*nrm[2];
//nw.WriteLine(center[0]);
//nw.WriteLine(center[1]);
//nw.WriteLine(center[2]);
//Obtaining point resolution based on distance to point
ptAng = Math.Asin(.09/Math.Sqrt(Math.Pow(points[i,j].X,2)+Math.Pow(points[i,j].Y,2)+Math.Pow(points[i,j].Z,2)));
ptRes = (int)Math.Floor(ptAng/.0043633);
//nw.WriteLine(ptAng);
//nw.WriteLine(ptRes);
for (int m=-ptRes;m<ptRes;m++) {
for (int n=-ptRes;n<ptRes;n++) {
newim = i + m;
newjn = j + n;
if (newim < 0) { newim = 0; }
if (newjn < 0) { newjn = 0; }
if (newim > 359) { newim = 359; }
if (newjn > 399) { newjn = 399; }
if (Math.Sqrt(Math.Pow(points[newim,newjn].X-points[i,j].X,2)+Math.Pow(points[newim,newjn].Y-points[i,j].Y,2)+Math.Pow(points[newim,newjn].Z-points[i,j].Z,2)) < rad) {
if (Math.Pow(center[2]-points[newim,newjn].Z,2)+Math.Pow(points[newim,newjn].X-center[0],2)+Math.Pow(points[newim,newjn].Y-center[1],2)-Math.Pow(rad,2) < tol) {
sum++;
}
}
}//End of 'n loop'
}//End of 'm loop'
newPtRes = Math.Pow(2.0*(double)ptRes,2) - subsTol*Math.Pow(2.0*(double)ptRes,2);
if (newPtRes > 2.0) {
//nw.WriteLine(sum);
if ((double)sum > newPtRes && Math.Sqrt(Math.Pow(points[i,j].X,2)+Math.Pow(points[i,j].Y,2)+Math.Pow(points[i,j].Z,2)) < 7) {
spPt[i,j] = 3;
//nw.WriteLine(points[i,j].X);
//nw.WriteLine(points[i,j].Y);
//nw.WriteLine(points[i,j].Z);
}
}
}
} //End of 'i loop'
} //End of 'j loop'
//Code to postprocess data
groupNum = 1;
int mMin = 0;
int nMin = 0;
double dis;
double minDis = 10000.0;
for (int i = 0; i < VERTICAL_RESOLUTION; i++)
{
for (int j = 0; j < HORIZONTAL_RESOLUTION; j++)
{
if (spPt[i,j] == 3)
{
for (int m = 0; m < VERTICAL_RESOLUTION; m++)
{
for (int n = 0; n < HORIZONTAL_RESOLUTION; n++)
{
if (spPt[m,n] == 3)
{
dis = Math.Sqrt(Math.Pow(points[i,j].X - points[m,n].X, 2) + Math.Pow(points[i,j].Y - points[m,n].Y, 2) + Math.Pow(points[i,j].Z - points[m,n].Z, 2));
//Look at distance between points
if (dis < .117775 && dis > 0)
{
if (gpNm[i,j] > 0)
{
gpNm[m,n] = gpNm[i,j];
}
if (gpNm[m,n] > 0)
{
gpNm[i,j] = gpNm[m,n];
}
if (gpNm[i,j] == 0 && gpNm[m,n] == 0)
{
gpNm[i,j] = groupNum;
gpNm[m,n] = groupNum;
groupNum++;
}
}
}
}//End of "m loop"
} //End of "n loop"
}
} //End of "j loop"
}//End of "i loop"
//Assign group number to single points that are not part of a cluster
for (int i = 0; i < VERTICAL_RESOLUTION; i++)
{
for (int j = 0; j < HORIZONTAL_RESOLUTION; j++)
{
if (spPt[i,j] == 3 && gpNm[i,j] == 0)
{
gpNm[i,j] = groupNum;
groupNum++;
}
}
}
//Find the centers of the clusters
// TextWriter nw = new StreamWriter("spTesting.txt");
double[] nrm2 = new double[3];
double mag2 = 0;
//double rad = .11;
double[] sum2 = new double[100];
for (int i=0;i<100;i++) {
spCr[i].X = 0;
spCr[i].Y = 0;
spCr[i].Z = 0;
sum2[i] = 0;
}
for (int i=0;i<VERTICAL_RESOLUTION;i++) {
for (int j=0;j<HORIZONTAL_RESOLUTION;j++) {
if (gpNm[i,j] > 0) {
spCr[gpNm[i,j]].X = (spCr[gpNm[i,j]].X + points[i,j].X);
spCr[gpNm[i,j]].Y = (spCr[gpNm[i,j]].Y + points[i,j].Y);
spCr[gpNm[i,j]].Z = (spCr[gpNm[i,j]].Z + points[i,j].Z);
sum2[gpNm[i,j]] = sum2[gpNm[i,j]] + 1;
}
}
}
//Obtain average center of point clusters
for (int i=0;i<groupNum;i++) {
spCr[i].X = spCr[i].X/(float)(sum2[i]);
spCr[i].Y = spCr[i].Y/(float)(sum2[i]);
spCr[i].Z = spCr[i].Z/(float)(sum2[i]);
mag2 = Math.Sqrt(Math.Pow(spCr[i].X,2)+Math.Pow(spCr[i].Y,2)+Math.Pow(spCr[i].Z,2));
nrm2[0] = spCr[i].X/mag2;
nrm2[1] = spCr[i].Y/mag2;
nrm2[2] = spCr[i].Z/mag2;
spCr[i].X = spCr[i].X + (float)(rad*nrm2[0]);
spCr[i].Y = spCr[i].Y + (float)(rad*nrm2[1]);
spCr[i].Z = spCr[i].Z + (float)(rad*nrm2[2]);
nw.WriteLine(spCr[i].X);
nw.WriteLine(spCr[i].Y);
nw.WriteLine(spCr[i].Z);
}
nw.Close();
//Obtain the centers of the spheres
dis = 0;
for (int i=0;i<100;i++) {
spBl[i] = 0;
}
for (int i=0;i<groupNum;i++) {
for (int j=0;j<groupNum;j++) {
dis = Math.Sqrt(Math.Pow(spCr[i].X-spCr[j].Y,2)+Math.Pow(spCr[i].Y-spCr[j].Y,2)+Math.Pow(spCr[i].Z-spCr[j].Z,2));
if (dis > .35 && dis < .55) {
spBl[i] = 1;
spBl[j] = 1;
}
}
}
}
private void UpdateRangeImage()
{
if (m_Robot.LatestRangeImage != null && m_Robot.LatestRangeImage != m_CurrentRangeImage)
{
//if (m_RangeImageViewer != null)
// m_RangeImageViewer.RangeImage = m_Robot.LatestRangeImage;
//if (m_Navigation != null)
// m_Navigation.Map = m_Robot.Map;
if (m_Control != null)
{
m_Control.Map = m_Robot.Map;
m_Control.processRangeImage();
}
m_CurrentRangeImage = m_Robot.LatestRangeImage;
}
}
private void toolStripButton12_Click(object sender, EventArgs e)
{
if (openFileDialog1.ShowDialog() == DialogResult.OK)
{
m_RangeImage = RangeImage.Read(openFileDialog1.FileName);
InitRangeImage();
GenerateFlatImage();
Canvas.Invalidate();
}
}
/**
* @brief Reads the given reader.
* \exception "InvalidOperationException" @brief Thrown when the requested operation is invalid (The RangeImage version is not recognized).
* @param Reader A BinaryReader
* @returns A RangeImage
*/
public static RangeImage Read(BinaryReader Reader)
{
Reader.BaseStream.Position = 0;
uint Version = Reader.ReadUInt32();
RangeImage I = new RangeImage();
switch (Version)
{
case 0x0104:
//RangeImage I = new RangeImage();
I.m_Width = Reader.ReadInt32();
I.m_Height = Reader.ReadInt32();
I.m_ScannerLocation = new PointF(Reader.ReadSingle(), Reader.ReadSingle());
I.m_ScannerPrimaryAxis = new PointF(Reader.ReadSingle(), Reader.ReadSingle());
I.m_HorizontalFieldOfView = Reader.ReadSingle();
I.m_VerticalFieldOfView = Reader.ReadSingle();
long l = Reader.ReadInt64();
I.m_AquiredOn = DateTime.Now;
I.m_SequenceNumber = Reader.ReadInt32();
I.m_Range = Reader.ReadSingle();
break;
case 0x0200:
//RangeImage I = new RangeImage();
Int32 length = Reader.ReadInt32();
Int32 bytesRead = 8;
float locX=0.0f, locY=0.0f;
float oriX=0.0f, oriY=0.0f;
float temp;
while (bytesRead < length)
{
Byte key = Reader.ReadByte();
bytesRead++;
switch (key)
{
case WIDTH_KEY:
I.m_Width = Reader.ReadInt32();
bytesRead += 4;
break;
case HEIGHT_KEY:
I.m_Height = Reader.ReadInt32();
bytesRead += 4;
break;
case POSITION_X_KEY:
temp = Reader.ReadSingle();
bytesRead += 4;
I.m_ScannerLocation = new PointF(temp, locY);
break;
case POSITION_Y_KEY:
temp = Reader.ReadSingle();
bytesRead += 4;
I.m_ScannerLocation = new PointF(locX, temp);
break;
case ORIENTATION_X_KEY:
temp = Reader.ReadSingle();
I.m_ScannerPrimaryAxis = new PointF(oriX, temp);
bytesRead += 4;
break;
case ORIENTATION_Y_KEY:
temp = Reader.ReadSingle();
I.m_ScannerPrimaryAxis = new PointF(temp, oriY);
bytesRead += 4;
break;
case HORIZONTAL_FOV_KEY:
I.m_HorizontalFieldOfView = Reader.ReadSingle();
bytesRead += 4;
break;
case VERTICAL_FOV_KEY:
I.m_VerticalFieldOfView = Reader.ReadSingle();
bytesRead += 4;
break;
case ACQUISITION_TIME_KEY:
// XXX: should probably actually use this data
long discarded = Reader.ReadInt64();
bytesRead+=8;
I.m_AquiredOn = DateTime.Now;
break;
case SEQUENCE_NUMBER_KEY:
I.m_SequenceNumber = Reader.ReadInt32();
bytesRead += 4;
break;
case MAX_RANGE_VALUE_KEY:
I.m_Range = Reader.ReadSingle();
bytesRead += 4;
break;
case VERTICAL_BIAS_KEY:
I.m_VerticalBias = Reader.ReadSingle();
bytesRead += 4;
break;
case HORIZONTAL_BIAS_KEY:
I.m_HorizontalBias = Reader.ReadSingle();
bytesRead += 4;
break;
}
}
break;
default:
throw new InvalidOperationException("The RangeImage version is not recognized");
}
I.m_RangeValues = new float[I.m_Width * I.m_Height];
for (int i = 0; i < I.m_RangeValues.Length; i++)
I.m_RangeValues[i] = Reader.ReadSingle();
return I;
}
//Returns a list of Sphere objects
//Parameters:
// RangeImage object
// Array containing the edges of the image with an edge being a "1" and no-edge "0"
// THIS CAN BE NULL
// Maximum Radius
// Minimum Radius
// Edge threshold (see detectEdges method)
// Percentile, which percentaje of the possible spheres will be discarded (based on their score)
/**
* @brief Returns a list of Sphere objects
* @param rangeData A RangeImage Object
* @param edges A float[,] - Array containing the edges of the image with an edge being a "1" and no-edge "0". THIS CAN BE NULL.
* @param maxRad An integer - Maximum Radius
* @param minRad An integer - Minimum Radius
* @param edgeThreshold A float - Edge threshold (see @sa detectEdges method)
* @param percentile A float - Which percentage of the possible spheres will be discarded (based on their score)
* @returns The found spheres
*/
public List<Sphere> FindSpheres(RangeImage rangeData, float[,] edges, int maxRad, int minRad, float edgeThreshold, float percentile)
{
DebugTimer dt = new DebugTimer();
dt.debug("Find spheres!");
//Determine the range of radii that we will search for
int diffRadii = maxRad - minRad;
//Create the array that will hold the edge data
//Normal bi-dimensional array x,y
if (edges == null)
{
edges = new float[rangeData.Width + 1, rangeData.Height + 1];
edges = detectEdges(rangeData, edgeThreshold);
//edges = detectEdgesRestricted(spheres, float.Parse(edgeTop.Text), float.Parse(edgeBottom.Text));
}
//Initialize the array that will hold the accumulation data
//First two dimensions are the are a x,y plane for the accumulation
//Third dimension is the radius for which that accumulation is done
//Forh dimension is only 2 items long:
// -The first one is the accumulation value
// -The second is an average of all the distances (z) to the point
// in the edge array that projects a circle over this particular
// x,y,k(radius) point
accumulation = new float[rangeData.Width + 1, rangeData.Height + 1, diffRadii, 4];
//Initialize an array for recording the local MAX and MIN from each accumulation for
//all the different radii. The first dimension of the array is the computed radius,
//and on the second dimension the item 0 is the minimum value, and the item 1 is the max.
float[,] localMaxMin = new float[maxRad - minRad, 2];
float tmpHeight = 0f;
dt.debug("Starting Hough transform algorithm...");
for (int i = 0; i < edges.GetLength(0); i++)
{
for (int j = 0; j < edges.GetLength(1); j++)
{
//If we have an edge in this point on the edges array, we proceed
if (edges[i, j] == 1)
{
//For every radius
for (int k = minRad; k < maxRad; k++)
{
//statusText.Text = "Generating circles (x,y,r)("+i+","+j+","+k+")";
for (int a = 0; a < 360; a += 5)
{
//Circle with 'k' radius to be traced on layer 'k'
//Points on the circumference
int x = (int)(i + Math.Sin(a * (Math.PI / 180)) * k);
int y = (int)(j + Math.Cos(a * (Math.PI / 180)) * k);
//If we are withing the bounds of the image
if (x > 0 && x < edges.GetLength(0) && y > 0 && y < edges.GetLength(1))
{
tmpHeight = accumulation[x, y, maxRad - k - 1, 0] + 1F;
//ACCUMULATE!
accumulation[x, y, maxRad - k - 1, 0] = tmpHeight;
//Update the average of distances of the circumference
accumulation[x, y, maxRad - k - 1, 1] += rangeData.GetCustom(i, j);
//Update the closest point in the circumference
if (rangeData.GetCustom(i, j) < accumulation[x, y, maxRad - k - 1, 2] || accumulation[x, y, maxRad - k - 1, 2] == 0)
accumulation[x, y, maxRad - k - 1, 2] = rangeData.GetCustom(i, j);
if (rangeData.GetCustom(i, j) > accumulation[x, y, maxRad - k - 1, 2])
accumulation[x, y, maxRad - k - 1, 3] = rangeData.GetCustom(i, j);
//accumulation[x, y, maxRad - k - 1, 1] = (accumulation[x, y, maxRad - k - 1, 1] + rangeData.GetCustom(i, j)) / 2;
//If we have a local (in the "layer", for a specific radius) MINIMUM we record it
if (tmpHeight < localMaxMin[maxRad - k - 1, 0] || localMaxMin[maxRad - k - 1, 0] == 0)
localMaxMin[maxRad - k - 1, 0] = tmpHeight;
//If we have a local (in the "layer", for a specific radius) MAXIMUM we record it
if (tmpHeight > localMaxMin[maxRad - k - 1, 1])
localMaxMin[maxRad - k - 1, 1] = tmpHeight;
}
}
}
}
}
}
dt.debugEnlapsed("Done accumulating...");
dt.debug("Starting tests...");
//Global list of positions, radius, accumulatedValues (SCORE), the average distance from center to midpoints, and average to edge points of spheres
List<Point> centers = new List<Point>();
List<int> radii = new List<int>();
List<float> accumulatedValues = new List<float>();
List<double> midPoints = new List<double>();
List<double> avgEdges = new List<double>();
List<double> avgDistances = new List<double>();
List<double> diffCircumferences = new List<double>();
//Initialize variables
float tmpPerc = 0;
Point tmpCenter = new Point(0, 0);
double avgDistance, distance, formula1, formula2, valN, valE, valS, valW, avgPoints, avgEdge, pointDifference, offset, diffCircumference;
float pointDifferenteThreshold = 0.03f;//0.02
float midPointDifferenteThreshold = 0.02f;//0.01
//DEBUG
List<String> trash = new List<String>();
for (int k = 0; k < accumulation.GetLength(2); k++)
{
//Calculate half the radius for sphere double cheching in line 163-179
offset = (maxRad - (float)k) / 2F;
for (int i = 0; i < rangeData.Width - 1; i++)
{
for (int j = 0; j < rangeData.Height - 1; j++)
{
//Calculate if the point is in the predefined percentile
tmpPerc = Math.Abs(accumulation[i, j, k, 0] - localMaxMin[k, 0]) / localMaxMin[k, 1];
//Average distance to the center of the sphere
avgDistance = (rangeData.GetCustom((i + 1 > rangeData.Width - 2) ? i : i + 1, j) + rangeData.GetCustom((i < 1) ? i + 1 : i - 1, j) + rangeData.GetCustom(i, (j + 1 > rangeData.Height - 2) ? j : j + 1) + rangeData.GetCustom(i, (j < 1) ? j + 1 : j - 1)) / 4f;
//Average edge distance
avgEdge = accumulation[i, j, k, 1] / accumulation[i, j, k, 0];
//Distance from center to edges
//(difference between average distance at the circumference, minus the center)
distance = avgEdge - avgDistance;
//Formula that approximates de excpected distance difference for a sphere of radius k
//formula1 = (7f / 1800f) * (maxRad - k) + (122f / 900f);
//Threshold for difference between expected and optimal
//float thresholdDistance = 0.2f;
//IF the point is in the predefined percentile and
//the distance from edges of the circumference and the center is possitive and
//the difference between the average distance at circumference minus the formula for k is
// bigger tan threshold
//if (tmpPerc > percentile / 100 && distance > 0 && (distance - formula1) < thresholdDistance)
//-Math.Min(0,(maxRad-k-15))
if (tmpPerc > (percentile / 100) || (tmpPerc >= (percentile / 100) - ((float)(maxRad - k) / 90)))
{
//Get distances of four points in the sphere surface
valE = rangeData.GetCustom((i - offset < 0) ? i : (int)(i - offset), j);
valS = rangeData.GetCustom((i + offset > rangeData.Width - 1) ? i : (int)(i + offset), j);
valW = rangeData.GetCustom(i, (j - offset < 0) ? j : (int)(j - offset));
valN = rangeData.GetCustom(i, (j + offset > rangeData.Height - 1) ? j : (int)(j + offset));
//Calculate the difference between the closest point, and the farther away
pointDifference = Math.Max(Math.Max(valE, valN), Math.Max(valS, valW)) - Math.Min(Math.Min(valE, valN), Math.Min(valS, valW));
//Average of all four points minus the distance to center
avgPoints = ((valE + valS + valW + valN) / 4);
//Formula for expected difference between center distance and distance of four points at k/2 for sphere with radius k
formula2 = ((17f / 9 * (float)(maxRad - k)) - 94f / 9f) / 1000f;
//Console.WriteLine("----------------\nSPHERE ID="+centers.Count+"\nradius="+(maxRad-k)+"\ndistance edges=" + accumulation[i, j, k, 1] + "\ndistanceAvg=" + avgDistance + "\n(distance edges)-distanceAvg=" + distance + "\n(Midpoint points avg distance)-avgDistance(center)=" + avgPoints + "\nNEWNUMBER***=" + formula2 + "\npointDifference=" + pointDifference+"\n--------------");
diffCircumference = accumulation[i, j, k, 3] - accumulation[i, j, k, 2];
//if (avgEdges > 1.6 && avgEdges < 1.9)
float diffToOptimal = Math.Abs(((float)(maxRad - k) / 800f) - (float)(avgPoints - avgDistance));
//if (pointDifference < 0.02d && avgPoints - formula2 > 0 && avgPoints - formula2 < 0.002d)
// && Math.Abs((avgPoints-avgDistance)-formula2)<0.1f
float tmptmp = (float)(avgEdge - (avgPoints + (float)(((float)(maxRad - k) * 2) / 100f)));
float tmptmptmp = (float)(maxRad - k) / 100;
double radiusMeters = (float)(maxRad - k) * (Math.Cos(89.75 * (Math.PI / 180)) * avgDistance);
float circumferenceThreshold = 0.1f;
float distanceCenterPointsThreshold = 0.005f;
//&& tmptmp < tmptmptmp//&& tmptmp < tmptmptmp
if (diffCircumference < circumferenceThreshold &&
avgPoints - avgDistance > distanceCenterPointsThreshold &&
pointDifference < pointDifferenteThreshold &&
diffToOptimal < midPointDifferenteThreshold &&
avgEdge > avgPoints && avgPoints > avgDistance &&
avgEdge - avgDistance > radiusMeters &&
avgEdge < avgDistance * 3 //&&
// Magic Number!
//avgEdge - avgDistance < radiusMeters * 3
)
{
//Console.WriteLine("----------------\nSPHERE ID=" + centers.Count + "\nradius=" + (maxRad - k) + "\ndistance edges=" + avgEdge + "\ndistanceAvg=" + avgDistance + "\n(distance edges)-distanceAvg=" + distance + "\n(Midpoint points avg distance)-avgDistance(center)=" + avgPoints + "\nNEWNUMBER***=" + formula2 + "\npointDifference=" + pointDifference + "\nSCORE=" + accumulation[i, j, k, 0] + "\n--------------");
//trash.Add("distance edges=" + accumulation[i, j, k, 1] + "\ndistanceAvg=" + avgDistance + "\ndiference=" + distance + "\ndiff2 - avgDistance=" + avgPoints + "\nNEWNUMBER***="+formula2+"\npointDifference=" + pointDifference+ "\nRESULT=" + (avgPoints - distance * Math.Sin(45 * (Math.PI / 180))));
//Add the values of this sphere to the global lists
centers.Add(new Point(i, j));
radii.Add(maxRad - k);
accumulatedValues.Add(accumulation[i, j, k, 0]);
midPoints.Add(avgPoints);
avgEdges.Add(avgEdge);
avgDistances.Add(avgDistance);
diffCircumferences.Add(diffCircumference);
}
}
}
}
}
dt.debugEnlapsed("Done with tests...");
List<Sphere> repeatedList = new List<Sphere>();
double deg2rad = Math.PI / 180;
double realRadius, theta, phi;
for (int i = 0; i < centers.Count; i++)
{
Vector3 absolutePosition = new Vector3();
//double theta = Math.Asin(((centers[i].X - rangeData.Width / 2) * Math.Cos(89.75 * deg2rad)) / avgDistances[i]);
//double phi = Math.Asin(((centers[i].Y - rangeData.Height / 2) * Math.Cos(89.75 * deg2rad)) / avgDistances[i]);
theta = Math.Asin((centers[i].X - rangeData.Width / 2) * Math.Cos(89.75 * deg2rad));
phi = Math.Asin((centers[i].Y - rangeData.Height / 2) * Math.Cos(89.75 * deg2rad));
absolutePosition.X = (float)(avgDistances[i] * Math.Cos(phi) * Math.Sin(theta));
absolutePosition.Y = (float)(avgDistances[i] * Math.Sin(phi) * Math.Sin(theta));
absolutePosition.Z = (float)(avgDistances[i] * Math.Cos(phi));
realRadius = avgDistances[i] * Math.Cos(89.75 * deg2rad) * radii[i];
Sphere a = new Sphere(absolutePosition, centers[i], radii[i], (float)realRadius, accumulatedValues[i], (float)avgEdges[i], (float)avgDistances[i], (float)midPoints[i], (float)diffCircumferences[i]);
repeatedList.Add(a);
}
List<Sphere> finalList = new List<Sphere>();
dt.debug("Start sphere merging algorithm...");
//Minimum dstance to consider that two centers are the same sphere
int minDistanceThreshold = 16;
//finalList = repeatedList;
/**/
List<Sphere> toMerge = new List<Sphere>();
IEnumerator<Sphere> enu = repeatedList.GetEnumerator();
IEnumerator<Sphere> enu2;
List<Sphere> tmpSpheres = new List<Sphere>(repeatedList);
int limit = repeatedList.Count - 1;
Sphere actual, test;
while (enu.MoveNext())
{
actual = enu.Current;
toMerge.Clear();
toMerge.Add(actual);
if (tmpSpheres.Contains(actual))
{
enu2 = tmpSpheres.GetEnumerator();
while (enu2.MoveNext())
{
test = enu2.Current;
if (actual != test && pointDistance(actual.center, test.center) < minDistanceThreshold)
toMerge.Add(test);
}
foreach (Sphere sph in toMerge)
tmpSpheres.Remove(sph);
if (toMerge.Count > 1)
finalList.Add(mergeSpheres(toMerge));
else
finalList.Add(actual);
}
}
dt.debugEnlapsed("Done with merging...");
/**/
List<Sphere> balancedFinalList = new List<Sphere>(finalList);
/**/
dt.debug("Finally filtering by score...");
foreach (Sphere sph in finalList)
{
if (sph.score < sph.radius * Math.PI)
balancedFinalList.Remove(sph);
}
/**/
dt.debugEnlapsed("Final list prepared.");
dt.debug("Returning list of spheres (" + balancedFinalList.Count + "). Done.");
//return repeatedList;
return balancedFinalList;
}
