AForge.Math.Geometry.Posit.EstimatePose C# (CSharp) Method

        public void EstimatePose( Point[] points, out Matrix3x3 rotation, out Vector3 translation )
        {
            if ( points.Length != 4 )
            {
                throw new ArgumentException( "4 points must be be given for pose estimation." );
            }

            float Z0 = 0, scale = 1;

            Vector3 X0 = new Vector3( points[0].X );
            Vector3 Y0 = new Vector3( points[0].Y );

            Vector3 XI = new Vector3( points[1].X, points[2].X, points[3].X );
            Vector3 YI = new Vector3( points[1].Y, points[2].Y, points[3].Y );

            int count = 0;

            Vector3 iVector = new Vector3( );
            Vector3 jVector = new Vector3( );
            Vector3 kVector = new Vector3( );
            Vector3 imageXs = new Vector3( );
            Vector3 imageYs = new Vector3( );

            Vector3 eps = new Vector3( 1 );

            for ( ; count < 100; count++ )
            {
                // calculate new scale orthographic projection (SOP)
                imageXs = XI * eps - X0;
                imageYs = YI * eps - Y0;

                // calculate I and J vectors
                iVector = modelPseudoInverse * imageXs;
                jVector = modelPseudoInverse * imageYs;
                // convert them to unit vectors i and j
                float iNorm = iVector.Normalize( );
                float jNorm = jVector.Normalize( );
                // scale of projection
                scale = ( iNorm + jNorm ) / 2;
                // calculate n vector k
                kVector = Vector3.Cross( iVector, jVector );
                // z-coordinate Z0 of the translation vector
                Z0 = focalLength / scale;

                // calculate new epsilon values
                Vector3 oldEps = eps;
                eps = ( modelVectors * kVector ) / Z0 + 1;

                // check if it is time to stop
                if ( ( eps - oldEps ).Abs( ).Max < stop_epsilon )
                    break;
            }

            // create rotation matrix
            rotation = Matrix3x3.CreateFromRows( iVector, jVector, kVector );

            // create translation vector
            Vector3 temp = rotation * modelPoints[0];
            translation = new Vector3(
                points[0].X / scale - temp.X,
                points[0].Y / scale - temp.Y,
                focalLength / scale );
        }
    }

        // Estimate 3D position
        private void EstimatePose( )
        {
            try
            {
                // check if all image coordinates are specified
                if ( ( string.IsNullOrEmpty( imagePoint1Box.Text ) ) ||
                     ( string.IsNullOrEmpty( imagePoint2Box.Text ) ) ||
                     ( string.IsNullOrEmpty( imagePoint3Box.Text ) ) ||
                     ( string.IsNullOrEmpty( imagePoint4Box.Text ) ) )
                {
                    throw new ApplicationException( "Some image coordinates are not specified." );
                }

                // check if all model coordnates are specified
                if ( ( string.IsNullOrEmpty( modelPoint1xBox.Text ) ) ||
                     ( string.IsNullOrEmpty( modelPoint2xBox.Text ) ) ||
                     ( string.IsNullOrEmpty( modelPoint3xBox.Text ) ) ||
                     ( string.IsNullOrEmpty( modelPoint4xBox.Text ) ) ||
                     ( string.IsNullOrEmpty( modelPoint1yBox.Text ) ) ||
                     ( string.IsNullOrEmpty( modelPoint2yBox.Text ) ) ||
                     ( string.IsNullOrEmpty( modelPoint3yBox.Text ) ) ||
                     ( string.IsNullOrEmpty( modelPoint4yBox.Text ) ) ||
                     ( ( !useCoplanarPosit ) && (
                       ( string.IsNullOrEmpty( modelPoint1zBox.Text ) ) ||
                       ( string.IsNullOrEmpty( modelPoint2zBox.Text ) ) ||
                       ( string.IsNullOrEmpty( modelPoint3zBox.Text ) ) ||
                       ( string.IsNullOrEmpty( modelPoint4zBox.Text ) ) ) ) )
                {
                    throw new ApplicationException( "Some model coordinates are not specified." );
                }

                // calculate model's center
                Vector3 modelCenter = new Vector3(
                    ( modelPoints[0].X + modelPoints[1].X + modelPoints[2].X + modelPoints[3].X ) / 4,
                    ( modelPoints[0].Y + modelPoints[1].Y + modelPoints[2].Y + modelPoints[3].Y ) / 4,
                    ( modelPoints[0].Z + modelPoints[1].Z + modelPoints[2].Z + modelPoints[3].Z ) / 4
                );

                // calculate ~ model's radius
                modelRadius = 0;
                foreach ( Vector3 modelPoint in modelPoints )
                {
                    float distanceToCenter = ( modelPoint - modelCenter ).Norm;
                    if ( distanceToCenter > modelRadius )
                    {
                        modelRadius = distanceToCenter;
                    }
                }

                if ( !useCoplanarPosit )
                {
                    Posit posit = new Posit( modelPoints, focalLength );
                    posit.EstimatePose( imagePoints, out rotationMatrix, out translationVector );

                    bestPoseButton.Visible = alternatePoseButton.Visible = false;
                }
                else
                {
                    CoplanarPosit coposit = new CoplanarPosit( modelPoints, focalLength );
                    coposit.EstimatePose( imagePoints, out rotationMatrix, out translationVector );

                    bestRotationMatrix = coposit.BestEstimatedRotation;
                    bestTranslationVector = coposit.BestEstimatedTranslation;

                    alternateRotationMatrix = coposit.AlternateEstimatedRotation;
                    alternateTranslationVector = coposit.AlternateEstimatedTranslation;

                    bestPoseButton.Visible = alternatePoseButton.Visible = true;
                }

                isPoseEstimated = true;
                UpdateEstimationInformation( );
                pictureBox.Invalidate( );
            }
            catch ( ApplicationException ex )
            {
                MessageBox.Show( ex.Message, "Error", MessageBoxButtons.OK, MessageBoxIcon.Error );
            }
        }