using System;
namespace Metric3DCalibration
{
using ExtensionMethods;
class Program
{
static void Main(string[] args)
{
Console.Write("Estimation of homography and affine transformation (correcting an inclined laser plane)\n\n");
var piece = GetAQS12Piece();
var rangeMap =
Image.FromFile(
@"%CVB%\Tutorial\Metric\Images\RangeMapCalibrationPattern.tif");
Console.Write("Rangemap loaded with size of " + rangeMap.Width.ToString() + " x " + rangeMap.Height.ToString() + " from ");
Console.Write(@"%CVB%\Tutorial\Metric\Images\RangeMapCalibrationPattern.tif" + "\n");
if (calibrator.CorrectionOfLaserPlaneInclination.Item2 != null)
calibrator.CorrectionOfLaserPlaneInclination.Item2.Value.ShowTransformation();
Console.Write("Residuals:\n");
residuals.ShowXYZArray();
double desiredAccuracy = 0.05;
if (!residuals.CheckAccuracy(desiredAccuracy))
{
Console.Write("Results do not have desired accuracy. Check face segmentation and extracted AQS12 points...\n");
var facesAqs12 = segmentor.FaceSegmentationFromPiece(rangeMap.Planes[0]);
var pointsAqs12 = segmentor.ExtractProjectedPointsFromPiece(rangeMap.Planes[0]);
Console.Write("Extracted points from range map:\n");
pointsAqs12.ShowXYZArray();
}
else
{
var calibratedCloud = calibrator.CreatePointCloud(rangeMap,
PointCloudFlags.Float);
Console.Write("The calibration was sucessful and accuracy is < " + desiredAccuracy.ToString() + " mm. :)\n");
}
}
public static AQS12Piece GetAQS12Piece()
{
var points = new Point3Dd[]
{
new Point3Dd(20.0018, 44.9941, 15.0000),
new Point3Dd(24.0018, 39.9942, 14.9994),
new Point3Dd(23.9994, 24.9972, 15.0001),
new Point3Dd(20.0021, 20.0035, 15.0011),
new Point3Dd(15.9994, 25.0079, 15.0016),
new Point3Dd(16.0000, 39.9919, 15.0010),
new Point3Dd(20.0095, 59.9985, 4.9902),
new Point3Dd(32.0093, 44.9958, 4.9909),
new Point3Dd(32.0052, 19.9925, 4.9920),
new Point3Dd(20.0021, 4.9961, 4.9939),
new Point3Dd( 8.0024, 19.9980, 5.0009),
new Point3Dd( 8.0065, 45.0009, 4.9984)
};
return new AQS12Piece(points, 0);
}
}
namespace ExtensionMethods
{
public static class Extensions
{
public static void ShowTransformation(this AffineMatrix3D trafo)
{
System.Text.StringBuilder sMessage = new System.Text.StringBuilder();
sMessage.Append("Translation:\n");
sMessage.Append("\n\nTransformation matrix:\n");
sMessage.Append("\n\n");
Console.Write(sMessage.ToString());
}
public static void ShowTransformation(this AffineTransformationParameters trafoPara)
{
System.Text.StringBuilder sMessage = new System.Text.StringBuilder();
sMessage.Append("Rotation angles in [degree] about\n");
sMessage.Append("Inclination of laser plane in [degree] about\n");
sMessage.Append(
"X: " + trafoPara.
InclinationX.ToString(
"N4") +
"\n");
sMessage.Append(
"Z: " + trafoPara.
InclinationZ.ToString(
"N4") +
"\n\n");
sMessage.Append("Shear\n");
sMessage.Append(
"Syz: " + trafoPara.
Syz.ToString(
"N5") +
"\n");
sMessage.Append(
"Syx: " + trafoPara.
Syx.ToString(
"N5") +
"\n\n");
sMessage.Append("Scale\n");
sMessage.Append(
"X: " + trafoPara.
Scale.
X.ToString(
"N3") +
"\n");
sMessage.Append(
"Y: " + trafoPara.
Scale.
Y.ToString(
"N3") +
"\n");
sMessage.Append(
"Z: " + trafoPara.
Scale.
Z.ToString(
"N3") +
"\n\n");
Console.Write(sMessage.ToString());
}
public static void ShowXYZArray(this Point3Dd[] xyz)
{
System.Text.StringBuilder sMessage = new System.Text.StringBuilder();
sMessage.Append("Point \t X \t Y \t Z \n");
for (int i = 0; i < 12; i++)
sMessage.Append(i + 1 + "\t" + xyz[i].X.ToString("N4") + " " + xyz[i].Y.ToString("N4") + " " + xyz[i].Z.ToString("N4") + "\n");
sMessage.Append("\n");
Console.Write(sMessage.ToString());
}
public static bool CheckAccuracy(this Point3Dd[] residuals, double desiredAccuracy)
{
foreach (Point3Dd residual in residuals)
{
if (residual.
X > desiredAccuracy || residual.
Y > desiredAccuracy || residual.
Z > desiredAccuracy)
return false;
}
return true;
}
}
}
}
static CalibrationConfiguration FromAQS12Piece(AQS12Piece piece)
static LaserPlaneHomographyCalibrator3D CalibratorFromAqs12Piece(ImagePlane rangeMapPlane, AQS12RangeMapSegmentor segmentor, CalibrationConfiguration config, out Point3Dd[] residuals)
Tuple< AffineMatrix3D?, AffineTransformationParameters?> CorrectionOfLaserPlaneInclination
override string ToString()
override string ToString()