public MorphologyCraterCallback(VertexShaderCrater vShaderCrater)
{
p1 = new polynomial();
p2 = new polynomial();
g = new gaussian();
mVShaderCrater = vShaderCrater;
}
// this should only be a casting, but c# sucks!
public static void convertToFloatArray(float x,float y, polynomial mP1,polynomial mP2,
gaussian mG,ref float[] o)
{
// float[]
if(o==null)
o = new float[21];
o[0] = 0;
o[1] = x;
o[2] = y;
o[3] = mP1.min;
o[4] = mP1.max;
o[5] = mP1.w0;
o[6] = mP1.w1;
o[7] = mP1.w2;
o[8] = mP1.w3;
// second poly
o[9] = mP2.min;
o[10] = mP2.max;
o[11] = mP2.w0;
o[12] = mP2.w1;
o[13] = mP2.w2;
o[14] = mP2.w3;
// first gaussian
o[15] = mG.min ;
o[16] = mG.max ;
o[17] = mG.mean;
o[18] = mG.sigma;
o[19] = mG.h ;
o[20] = mG.hs ;
//return o;
}
public void Reset()
{
survey = null;
survey_updates = 0;
minimum_rms_error = double.MaxValue;
best_fit_curve = null;
}
private void FindBestCurve(int tx, int ty)
{
for (int x = 0; x < survey.GetLength(0); x++)
{
for (int y = 0; y < survey.GetLength(1); y++)
{
polynomial p = survey[x, y];
if (p != null)
{
p.Solve();
double rms_error = p.GetRMSerror();
if (rms_error < minimum_rms_error)
{
best_fit_curve = p;
minimum_rms_error = rms_error;
centre_of_distortion_x = tx + (x / 2.0f);
centre_of_distortion_y = ty + (y / 2.0f);
}
}
}
}
Console.WriteLine("Minimum RMS error: " + minimum_rms_error.ToString());
}
/// <summary>
/// parse an xml node to extract camera calibration parameters
/// </summary>
/// <param name="xnod"></param>
/// <param name="level"></param>
public void LoadFromXml(XmlNode xnod, int level)
{
XmlNode xnodWorking;
if (xnod.Name == "FieldOfViewDegrees")
camera_FOV_degrees = Convert.ToInt32(xnod.InnerText);
if (xnod.Name == "ImageDimensions")
{
IFormatProvider format = new System.Globalization.CultureInfo("en-GB");
String[] dimStr = xnod.InnerText.Split(',');
image_width = Convert.ToInt32(dimStr[0], format);
image_height = Convert.ToInt32(dimStr[1], format);
}
if (xnod.Name == "CentreOfDistortion")
{
IFormatProvider format = new System.Globalization.CultureInfo("en-GB");
String[] centreStr = xnod.InnerText.Split(',');
centre_of_distortion = new calibration_point(
Convert.ToSingle(centreStr[0], format),
Convert.ToSingle(centreStr[1], format));
}
if (xnod.Name == "DistortionCoefficients")
{
if (xnod.InnerText != "")
{
IFormatProvider format = new System.Globalization.CultureInfo("en-GB");
String[] coeffStr = xnod.InnerText.Split(',');
fitter = new polynomial();
fitter.SetDegree(coeffStr.Length - 1);
for (int i = 0; i < coeffStr.Length; i++)
fitter.SetCoeff(i, Convert.ToSingle(coeffStr[i], format));
}
}
if (xnod.Name == "Scale")
{
scale = Convert.ToSingle(xnod.InnerText);
}
if (xnod.Name == "RotationDegrees")
{
rotation = Convert.ToSingle(xnod.InnerText) / 180.0f * (float)Math.PI;
}
if (xnod.Name == "RMSerror")
{
min_RMS_error = Convert.ToSingle(xnod.InnerText);
// update the calibration lookup table
updateCalibrationMap();
}
// if this is an element, extract any attributes
/*
if (xnod.NodeType == XmlNodeType.Element)
{
XmlNamedNodeMap mapAttributes = xnod.Attributes;
for (int i = 0; i < mapAttributes.Count; i += 1)
{
//Console.WriteLine(pad + " " + mapAttributes.Item(i).Name
// + " = " + mapAttributes.Item(i).Value);
}
}
*/
// call recursively on all children of the current node
if (xnod.HasChildNodes)
{
xnodWorking = xnod.FirstChild;
while (xnodWorking != null)
{
LoadFromXml(xnodWorking, level + 1);
xnodWorking = xnodWorking.NextSibling;
}
}
}
/// <summary>
/// update the calibration lookup table, which maps pixels
/// in the rectified image into the original image
/// </summary>
/// <param name="width"></param>
/// <param name="height"></param>
private void updateCalibrationMap(int width, int height, polynomial curve, float scale, float rotation)
{
temp_calibration_map = new int[width * height];
temp_calibration_map_inverse = new int[width, height, 2];
for (int x = 0; x < width; x++)
{
float dx = x - centre_of_distortion.x;
for (int y = 0; y < height; y++)
{
float dy = y - centre_of_distortion.y;
float radial_dist_rectified = (float)Math.Sqrt((dx * dx) + (dy * dy));
if (radial_dist_rectified >= 0.01f)
{
float radial_dist_original = curve.RegVal(radial_dist_rectified);
if (radial_dist_original > 0)
{
float ratio = radial_dist_original / radial_dist_rectified;
float x2 = (float)Math.Round(centre_of_distortion.x + (dx * ratio));
x2 = (x2 - (width / 2)) * scale;
float y2 = (float)Math.Round(centre_of_distortion.y + (dy * ratio));
y2 = (y2 - (height / 2)) * scale;
// apply rotation
float x3=x2, y3=y2;
rotatePoint(x2, y2, -rotation, ref x3, ref y3);
x3 += (width / 2);
y3 += (height / 2);
if (((int)x3 > -1) && ((int)x3 < width) && ((int)y3 > -1) && ((int)y3 < height))
{
int n = (y * width) + x;
int n2 = ((int)y3 * width) + (int)x3;
temp_calibration_map[n] = n2;
temp_calibration_map_inverse[(int)x3, (int)y3, 0] = x;
temp_calibration_map_inverse[(int)x3, (int)y3, 1] = y;
}
}
}
}
}
}
private void detectLensDistortion(int width, int height,
int grid_x, int grid_y)
{
if (grid != null)
{
if (grid[grid_x, grid_y] != null)
{
// field of vision in radians
float FOV_horizontal = camera_FOV_degrees * (float)Math.PI / 180.0f;
float FOV_vertical = FOV_horizontal *height / (float)width;
// center point of the grid within the image
pattern_centre_x = (int)grid[grid_x, grid_y].x;
pattern_centre_y = (int)grid[grid_x, grid_y].y;
// calculate the distance to the centre grid point on the ground plane
float ground_dist_to_point = camera_dist_to_pattern_centre_mm;
// line of sight distance between the camera lens and the centre point
float camera_to_point_dist = (float)Math.Sqrt((ground_dist_to_point * ground_dist_to_point) +
(camera_height_mm * camera_height_mm));
distance_to_pattern_centre = camera_to_point_dist;
// tilt angle at the centre point
float centre_tilt = (float)Math.Asin(camera_height_mm / camera_to_point_dist);
// angle subtended by one grid spacing at the centre
float point_pan = (float)Math.Asin(calibration_pattern_spacing_mm / camera_to_point_dist);
// grid width at the centre point
float x1 = pattern_centre_x + (point_pan * width / FOV_horizontal);
// calculate the distance to the observed grid point on the ground plane
ground_dist_to_point = camera_dist_to_pattern_centre_mm + ((grid_y + 2) * calibration_pattern_spacing_mm);
// line of sight distance between the camera lens and the observed point
camera_to_point_dist = (float)Math.Sqrt((ground_dist_to_point * ground_dist_to_point) +
(camera_height_mm * camera_height_mm));
// tilt angle
float point_tilt = (float)Math.Asin(camera_height_mm / camera_to_point_dist);
// angle subtended by one grid spacing
point_pan = (float)Math.Asin(calibration_pattern_spacing_mm / camera_to_point_dist);
// calc the position of the grid point within the image after rectification
float x2 = pattern_centre_x + (point_pan * width / FOV_horizontal);
float y2 = pattern_centre_y + ((point_tilt - centre_tilt) * height / FOV_vertical);
// calc the gradient
float grad = (x2 - x1) / (float)(y2 - pattern_centre_y);
float baseline_fraction = baseline_offset / (float)(calibration_pattern_spacing_mm);
centre_of_distortion = new calibration_point(0, 0);
int hits = 0;
float cx = 0, cy = 0;
for (int x = 0; x < grid.GetLength(0); x++)
{
for (int y = 0; y < grid.GetLength(1); y++)
{
if (grid[x, y] != null)
{
// calculate the distance to the observed grid point on the ground plane
ground_dist_to_point = camera_dist_to_pattern_centre_mm + ((grid_y - y) * calibration_pattern_spacing_mm);
// line of sight distance between the camera lens and the observed point
camera_to_point_dist = (float)Math.Sqrt((ground_dist_to_point * ground_dist_to_point) +
(camera_height_mm * camera_height_mm));
// tilt angle
point_tilt = (float)Math.Asin(camera_height_mm / camera_to_point_dist);
// distance to the point on the ground plave along the x (horizontal axis)
float ground_dist_to_point_x = ((x - grid_x) * calibration_pattern_spacing_mm) + baseline_offset;
// pan angle
point_pan = (float)Math.Asin(ground_dist_to_point_x / camera_to_point_dist);
// calc the position of the grid point within the image after rectification
float w = ((x1 - pattern_centre_x) + ((grid[x, y].y - pattern_centre_y) * grad));
float wbaseline = baseline_fraction * (grid[x, y].y - pattern_centre_y) * grad;
float rectified_x = pattern_centre_x + (w * (x - grid_x)) - wbaseline;
float rectified_y = pattern_centre_y + ((point_tilt - centre_tilt) * height / FOV_vertical);
grid[x, y].rectified_x = rectified_x;
grid[x, y].rectified_y = rectified_y;
cx += (grid[x, y].x - rectified_x);
cy += (grid[x, y].y - rectified_y);
hits++;
}
}
}
if (hits > 0)
{
// a ballpack figure for the centre of distortion
centre_of_distortion.x = (width / 2) + (cx / (float)hits);
centre_of_distortion.y = (height / 2) + (cy / (float)hits);
float winner_x = centre_of_distortion.x;
float winner_y = centre_of_distortion.y;
float min_rms_err = 999999;
int radius = 5;
for (int search_x = (int)centre_of_distortion.x - radius; search_x <= (int)centre_of_distortion.x + radius; search_x++)
{
for (int search_y = (int)centre_of_distortion.y - radius; search_y <= (int)centre_of_distortion.y + radius; search_y++)
{
polynomial curvefit = new polynomial();
curvefit.SetDegree(2);
for (int x = 0; x < grid.GetLength(0); x++)
{
for (int y = 0; y < grid.GetLength(1); y++)
{
if (grid[x, y] != null)
{
addDataPoint(grid[x, y].x, grid[x, y].y,
grid[x, y].rectified_x, grid[x, y].rectified_y,
curvefit);
// intermediary points
if (y > 0)
{
if (grid[x, y - 1] != null)
{
float xx = grid[x, y - 1].x + ((grid[x, y].x - grid[x, y - 1].x) / 2);
float yy = grid[x, y - 1].y + ((grid[x, y].y - grid[x, y - 1].y) / 2);
float rectified_xx = grid[x, y - 1].rectified_x + ((grid[x, y].rectified_x - grid[x, y - 1].rectified_x) / 2);
float rectified_yy = grid[x, y - 1].rectified_y + ((grid[x, y].rectified_y - grid[x, y - 1].rectified_y) / 2);
addDataPoint(xx, yy,
rectified_xx, rectified_yy,
curvefit);
}
}
if (x > 0)
{
if (grid[x-1, y] != null)
{
float xx = grid[x-1, y].x + ((grid[x, y].x - grid[x-1, y].x) / 2);
float yy = grid[x-1, y].y + ((grid[x, y].y - grid[x-1, y].y) / 2);
float rectified_xx = grid[x-1, y].rectified_x + ((grid[x, y].rectified_x - grid[x-1, y].rectified_x) / 2);
float rectified_yy = grid[x-1, y].rectified_y + ((grid[x, y].rectified_y - grid[x-1, y].rectified_y) / 2);
addDataPoint(xx, yy,
rectified_xx, rectified_yy,
curvefit);
}
}
}
}
}
curvefit.Solve();
float rms_err = curvefit.GetRMSerror();
if (rms_err < min_rms_err)
{
min_rms_err = rms_err;
winner_x = search_x;
winner_y = search_y;
fitter = curvefit;
}
}
}
centre_of_distortion.x = winner_x;
centre_of_distortion.y = winner_y;
}
}
}
}
/// <summary>
/// add a data point to the given curve fitter
/// </summary>
/// <param name="x"></param>
/// <param name="y"></param>
/// <param name="rectified_x"></param>
/// <param name="rectified_y"></param>
/// <param name="polycurve"></param>
private void addDataPoint(float x, float y, float rectified_x, float rectified_y,
polynomial polycurve)
{
float dx = rectified_x - centre_of_distortion.x;
float dy = rectified_y - centre_of_distortion.y;
float radial_dist_rectified = (float)Math.Sqrt((dx * dx) + (dy * dy));
dx = x - centre_of_distortion.x;
dy = y - centre_of_distortion.y;
float radial_dist_original = (float)Math.Sqrt((dx * dx) + (dy * dy));
polycurve.AddPoint(radial_dist_rectified, radial_dist_original);
}
public void updatePolinomial(polynomial p1,polynomial p2,gaussian g)
{
mP1 = p1;
mP2 = p2;
mG = g;
// SetFloat ("radious", p2.max);
}
public void Update(
int image_width,
int image_height,
CalibrationDot[,] grid)
{
if (survey_updates < test_interval)
{
int cx = image_width / 2;
int cy = image_height / 2;
int radius_pixels = image_width * radius_percent / 100;
int radius_pixels_sqr = radius_pixels * radius_pixels;
int diameter_pixels = radius_pixels * 2;
int tx = cx - radius_pixels;
int bx = tx + diameter_pixels;
int ty = cy - radius_pixels;
int by = ty + diameter_pixels;
if (survey == null)
{
survey = new polynomial[(diameter_pixels * 2) + 1, (diameter_pixels * 2) + 1];
}
if (survey.GetLength(0) != diameter_pixels)
{
survey = new polynomial[(diameter_pixels * 2) + 1, (diameter_pixels * 2) + 1];
}
for (float centre_x = tx; centre_x <= bx; centre_x += 0.5f)
{
float dcx = centre_x - cx;
dcx *= dcx;
for (float centre_y = ty; centre_y <= by; centre_y += 0.5f)
{
float dcy = centre_y - cy;
dcy *= dcy;
float r = dcx * dcx + dcy * dcy;
if (r < radius_pixels_sqr)
{
int xx = (int)((centre_x - tx) * 2);
int yy = (int)((centre_y - ty) * 2);
// get the curve associated with this possible centre of distortion
if (survey[xx, yy] == null)
{
polynomial p = new polynomial();
p.SetDegree(degree);
survey[xx, yy] = p;
}
polynomial curve = survey[xx, yy];
for (int grid_x = 0; grid_x < grid.GetLength(0); grid_x++)
{
for (int grid_y = 0; grid_y < grid.GetLength(1); grid_y++)
{
CalibrationDot dot = grid[grid_x, grid_y];
if (dot != null)
{
if (dot.rectified_x > 0)
{
double dx = dot.x - centre_x;
double dy = dot.y - centre_y;
double actual_radial_dist = Math.Sqrt(dx * dx + dy * dy);
dx = dot.rectified_x - centre_x;
dy = dot.rectified_y - centre_y;
double rectified_radial_dist = Math.Sqrt(dx * dx + dy * dy);
curve.AddPoint(rectified_radial_dist, actual_radial_dist);
}
}
}
}
}
}
}
survey_updates++;
if (survey_updates >= test_interval)
{
FindBestCurve(tx, ty);
survey = null;
survey_updates = 0;
}
}
}