Foundation/Cvb.Net/Metric3DCalibrationRigidBodyTransform

This example program is located in your CVB installation under %CVB%Tutorial/Foundation/Cvb.Net/Metric3DCalibrationRigidBodyTransform.

Program.cs:

// Example for a **rigid-body transformation** (extrinsic calibration) using the AQS12 target.

 
// ---------------------------------------------------------------------------
using Stemmer.Cvb;
using Stemmer.Cvb.Foundation;
// ---------------------------------------------------------------------------
 
 
using System;
 
namespace Metric3DCalibration
{
    using ExtensionMethods;
 
    class Program
    {
        static void Main(string[] args)
        {
            Console.Write("Estimation of a rigid body transformation (rotation and translation)\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");

            var calibrator = Calibrator3D.FromFile(@"%CVB%\Tutorial\Metric\Images\SICalibration.json");
            var cloud = calibrator.CreatePointCloud(rangeMap, PointCloudFlags.Float);
            Console.Write("Dense point cloud created from rangemap and calibration file with " + cloud.NumPoints + " points.\n\n");

            var segmentor = new AQS12DensePointCloudSegmentor(SegmentationMethod.KmeansClustering)
            {
                // set a valid window size
                WindowSize = 5
            };

            Point3Dd[] residuals;
            var trafo = Metric.CalculateRigidBodyTransformationFromAqs12Piece(cloud as DensePointCloud, segmentor, piece, out residuals);
            calibrator.ExtrinsicMatrix = trafo.Item1;

            trafo.Item1.ShowTransformation();
            trafo.Item2.ShowTransformation();
            Console.Write("Residuals:\n");
            residuals.ShowXYZArray();

            var calibratedCloud = calibrator.CreatePointCloud(rangeMap, PointCloudFlags.Float);
 
            Console.Write("Number of points: " + calibratedCloud.NumPoints + "\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(trafo.Translation.ToString());
                sMessage.Append("\n\nTransformation matrix:\n");
                sMessage.Append(trafo.Matrix.ToString());
                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("X: " + trafoPara.RotationAngles[0].ToString("N4") + "\n");
                sMessage.Append("Y: " + trafoPara.RotationAngles[1].ToString("N4") + "\n");
                sMessage.Append("Z: " + trafoPara.RotationAngles[2].ToString("N4") + "\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());
            }
        }
    } // namespace ExtensionMethods
}