using System;
namespace Metric3DCalibration
{
using ExtensionMethods;
class Program
{
static void Main(string[] args)
{
Console.Write("Estimation of an affine transformation(correcting an inclined laser plane)\n\n");
var piece = GetAQS12Piece();
config.CalculateHomography = false;
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");
Console.Write("Dense point cloud created from rangemap and calibration file with " + cloud.NumPoints + " points.\n\n");
calibrator.CorrectionOfLaserPlaneInclination.Item1.Value.ShowTransformation();
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(cloud as
DensePointCloud);
var pointsAqs12 = segmentor.ExtractProjectedPointsFromPiece(cloud as
DensePointCloud);
Console.Write("Extracted points from point cloud:\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;
}
}
}
}
PointCloud CreatePointCloud(Image rangeMap)
static Calibrator3D FromFile(string fileName)
static CalibrationConfiguration FromAQS12Piece(AQS12Piece piece)
static Tuple< AffineMatrix3D, AffineTransformationParameters?> CalculateCorrectionOfLaserPlaneInclinationFromAqs12Piece(DensePointCloud cloud, AQS12DensePointCloudSegmentor segmentor, CalibrationConfiguration config, out Point3Dd[] residuals)
override string ToString()
override string ToString()