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
            };

            var trafo = Metric.CalculateRigidBodyTransformationFromAqs12Piece(cloud as DensePointCloud, segmentor, piece, out Point3Dd[] 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(20.0018, 44.9941, 15.0000),
        new(24.0018, 39.9942, 14.9994),
        new(23.9994, 24.9972, 15.0001),
        new(20.0021, 20.0035, 15.0011),
        new(15.9994, 25.0079, 15.0016),
        new(16.0000, 39.9919, 15.0010),
        new(20.0095, 59.9985,  4.9902),
        new(32.0093, 44.9958,  4.9909),
        new(32.0052, 19.9925,  4.9920),
        new(20.0021,  4.9961,  4.9939),
        new( 8.0024, 19.9980,  5.0009),
        new( 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
}