/// <summary>
/// update the calibration lookup table, which maps pixels
/// in the rectified image into the original image
/// </summary>
/// <param name="image_width"></param>
/// <param name="image_height"></param>
/// <param name="curve">polynomial curve describing the lens distortion</param>
/// <param name="scale"></param>
/// <param name="rotation"></param>
/// <param name="centre_of_distortion_x"></param>
/// <param name="centre_of_distortion_y"></param>
/// <param name="calibration_map"></param>
/// <param name="calibration_map_inverse"></param>
private static void updateCalibrationMap(int image_width,
int image_height,
polynomial curve,
float scale,
float rotation,
float centre_of_distortion_x,
float centre_of_distortion_y,
ref int[] calibration_map,
ref int[, ,] calibration_map_inverse)
{
int half_width = image_width / 2;
int half_height = image_height / 2;
calibration_map = new int[image_width * image_height];
calibration_map_inverse = new int[image_width, image_height, 2];
for (int x = 0; x < image_width; x++)
{
float dx = x - centre_of_distortion_x;
for (int y = 0; y < image_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)
{
double radial_dist_original = curve.RegVal(radial_dist_rectified);
if (radial_dist_original > 0)
{
double ratio = radial_dist_original / radial_dist_rectified;
float x2 = (float)Math.Round(centre_of_distortion_x + (dx * ratio));
x2 = (x2 - (image_width / 2)) * scale;
float y2 = (float)Math.Round(centre_of_distortion_y + (dy * ratio));
y2 = (y2 - (image_height / 2)) * scale;
// apply rotation
double x3 = x2, y3 = y2;
rotatePoint(x2, y2, -rotation, ref x3, ref y3);
x3 += half_width;
y3 += half_height;
if (((int)x3 > -1) && ((int)x3 < image_width) &&
((int)y3 > -1) && ((int)y3 < image_height))
{
int n = (y * image_width) + x;
int n2 = ((int)y3 * image_width) + (int)x3;
calibration_map[n] = n2;
calibration_map_inverse[(int)x3, (int)y3, 0] = x;
calibration_map_inverse[(int)x3, (int)y3, 1] = y;
}
}
}
}
}
}
/// <summary>
/// shows the lens distortion model
/// </summary>
/// <param name="img_width">width of the image</param>
/// <param name="img_height">height of the image</param>
/// <param name="centre_of_distortion">coordinates for the centre of distortion</param>
/// <param name="curve">distortion curve</param>
/// <param name="FOV_degrees">horizontal field of view in degrees</param>
/// <param name="increment_degrees">increment for concentric circles in degrees</param>
/// <param name="output_filename">filename to save as</param>
private static void ShowLensDistortion(int img_width, int img_height,
CalibrationDot centre_of_distortion,
polynomial curve,
float FOV_degrees, float increment_degrees,
string output_filename)
{
byte[] img = new byte[img_width * img_height * 3];
for (int i = 0; i < img.Length; i++) img[i] = 255;
float max_radius = img_width / 2;
float total_difference = 0;
for (float r = 0; r < max_radius * 1.0f; r += 0.2f)
{
float diff = (float)Math.Abs(r - curve.RegVal(r));
total_difference += diff;
}
if (total_difference > 0)
{
int rr, gg, bb;
float diff_sum = 0;
for (float r = 0; r < max_radius * 1.8f; r += 0.2f)
{
float original_r = (float)curve.RegVal(r);
float d1 = r - original_r;
diff_sum += Math.Abs(d1);
float diff = diff_sum / total_difference;
if (diff > 1.0f) diff = 1.0f;
byte difference = (byte)(50 + (diff * 205));
if (d1 >= 0)
{
rr = difference;
gg = difference;
bb = difference;
}
else
{
rr = difference;
gg = difference;
bb = difference;
}
drawing.drawCircle(img, img_width, img_height,
(float)centre_of_distortion.x,
(float)centre_of_distortion.y,
r, rr, gg, bb, 1, 300);
}
}
float increment = max_radius * increment_degrees / FOV_degrees;
float angle_degrees = increment_degrees;
for (float r = increment; r <= max_radius * 150 / 100; r += increment)
{
float radius = (float)curve.RegVal(r);
drawing.drawCircle(img, img_width, img_height,
(float)centre_of_distortion.x,
(float)centre_of_distortion.y,
radius, 0, 0, 0, 0, 360);
drawing.AddText(img, img_width, img_height, angle_degrees.ToString(),
"Courier New", 10, 0, 0, 0,
(int)centre_of_distortion.x + (int)radius + 10, (int)centre_of_distortion.y);
angle_degrees += increment_degrees;
}
int incr = img_width / 20;
for (int x = 0; x < img_width; x += incr)
{
for (int y = 0; y < img_height; y += incr)
{
float dx = x - (float)centre_of_distortion.x;
float dy = y - (float)centre_of_distortion.y;
float radius = (float)Math.Sqrt(dx * dx + dy * dy);
float tot = 0;
for (float r = 0; r < radius; r += 0.2f)
{
float diff = (float)Math.Abs(r - curve.RegVal(r));
tot += diff;
}
float r1 = (float)Math.Abs((radius - 2f) - curve.RegVal(radius - 2f));
float r2 = (float)Math.Abs(radius - curve.RegVal(radius));
float fraction = 1.0f + (Math.Abs(r2 - r1) * img_width * 1.5f / total_difference);
int x2 = (int)(centre_of_distortion.x + (dx * fraction));
int y2 = (int)(centre_of_distortion.y + (dy * fraction));
drawing.drawLine(img, img_width, img_height, x, y, x2, y2, 255, 0, 0, 0, false);
}
}
drawing.drawLine(img, img_width, img_height, 0, (int)centre_of_distortion.y, img_width - 1, (int)centre_of_distortion.y, 0, 0, 0, 0, false);
drawing.drawLine(img, img_width, img_height, (int)centre_of_distortion.x, 0, (int)centre_of_distortion.x, img_height - 1, 0, 0, 0, 0, false);
Bitmap output_bmp = new Bitmap(img_width, img_height, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
BitmapArrayConversions.updatebitmap_unsafe(img, output_bmp);
if (output_filename.ToLower().EndsWith("jpg"))
output_bmp.Save(output_filename, System.Drawing.Imaging.ImageFormat.Jpeg);
if (output_filename.ToLower().EndsWith("bmp"))
output_bmp.Save(output_filename, System.Drawing.Imaging.ImageFormat.Bmp);
}
/// <summary>
/// applies the given curve to the lines to produce rectified coordinates
/// </summary>
/// <param name="lines"></param>
/// <param name="distortion_curve"></param>
/// <param name="centre_of_distortion"></param>
/// <returns></returns>
private static List<double> RectifyDots(List<double> dots,
int image_width, int image_height,
polynomial curve,
double centre_of_distortion_x,
double centre_of_distortion_y,
double rotation,
double scale)
{
List<double> rectified_dots = new List<double>();
float half_width = image_width / 2;
float half_height = image_height / 2;
List<double> rectified_line = new List<double>();
for (int j = 0; j < dots.Count; j += 2)
{
double x = dots[j];
double y = dots[j + 1];
double dx = x - centre_of_distortion_x;
double dy = y - centre_of_distortion_y;
double radial_dist_rectified = Math.Sqrt((dx * dx) + (dy * dy));
if (radial_dist_rectified >= 0.01f)
{
double radial_dist_original = curve.RegVal(radial_dist_rectified);
if (radial_dist_original > 0)
{
double ratio = radial_dist_rectified / radial_dist_original;
double x2 = Math.Round(centre_of_distortion_x + (dx * ratio));
x2 = (x2 - (image_width / 2)) * scale;
double y2 = Math.Round(centre_of_distortion_y + (dy * ratio));
y2 = (y2 - (image_height / 2)) * scale;
// apply rotation
double rectified_x = x2, rectified_y = y2;
rotatePoint(x2, y2, -rotation, ref rectified_x, ref rectified_y);
rectified_x += half_width;
rectified_y += half_height;
rectified_dots.Add(rectified_x);
rectified_dots.Add(rectified_y);
}
}
}
return (rectified_dots);
}
private static bool ValidCurve(polynomial curve, int image_width)
{
int max_radius = image_width / 2;
int half_radius = max_radius / 2;
double diff1 = curve.RegVal(half_radius) - half_radius;
double diff2 = curve.RegVal(max_radius) - max_radius;
if (diff2 > diff1)
return (false);
else
return (true);
}
/// <summary>
/// applies the given curve to the lines to produce rectified coordinates
/// </summary>
/// <param name="lines"></param>
/// <param name="distortion_curve"></param>
/// <param name="centre_of_distortion"></param>
/// <returns></returns>
private static List<List<double>> RectifyLines(List<List<double>> lines,
int image_width, int image_height,
polynomial curve,
CalibrationDot centre_of_distortion,
double rotation,
double scale)
{
List<List<double>> rectified_lines = new List<List<double>>();
float half_width = image_width / 2;
float half_height = image_height / 2;
for (int i = 0; i < lines.Count; i++)
{
List<double> line = lines[i];
List<double> rectified_line = new List<double>();
for (int j = 0; j < line.Count; j += 2)
{
double x = line[j];
double y = line[j + 1];
double dx = x - centre_of_distortion.x;
double dy = y - centre_of_distortion.y;
double radial_dist_rectified = Math.Sqrt((dx * dx) + (dy * dy));
if (radial_dist_rectified >= 0.01f)
{
double radial_dist_original = curve.RegVal(radial_dist_rectified);
if (radial_dist_original > 0)
{
double ratio = radial_dist_rectified / radial_dist_original;
double x2 = Math.Round(centre_of_distortion.x + (dx * ratio));
x2 = (x2 - (image_width / 2)) * scale;
double y2 = Math.Round(centre_of_distortion.y + (dy * ratio));
y2 = (y2 - (image_height / 2)) * scale;
// apply rotation
double rectified_x = x2, rectified_y = y2;
rotatePoint(x2, y2, -rotation, ref rectified_x, ref rectified_y);
rectified_x += half_width;
rectified_y += half_height;
rectified_line.Add(rectified_x);
rectified_line.Add(rectified_y);
}
}
}
if (rectified_line.Count > 6)
rectified_lines.Add(rectified_line);
}
return (rectified_lines);
}
