3D pose estimation algorithm (coplanar case).

The class implements an algorithm for 3D object's pose estimation from it's 2D coordinates obtained by perspective projection, when the object is described coplanar points. The idea of the implemented math and algorithm is described in "Iterative Pose Estimation using Coplanar Feature Points" paper written by Oberkampf, Daniel DeMenthon and Larry Davis (the implementation of the algorithm is very close translation of the pseudo code given by the paper, so should be easy to follow).

At this point the implementation works only with models described by 4 points, which is the minimum number of points enough for 3D pose estimation.

The 4 model's point are supposed to be coplanar, i.e. supposed to reside all within same planer. See Posit for none coplanar case.

Read 3D Pose Estimation article for additional information and samples.

Sample usage:

// points of real object - model Vector3[] copositObject = new Vector3[4] { new Vector3( -56.5f, 0, 56.5f ), new Vector3( 56.5f, 0, 56.5f ), new Vector3( 56.5f, 0, -56.5f ), new Vector3( -56.5f, 0, -56.5f ), }; // focal length of camera used to capture the object float focalLength = 640; // depends on your camera or projection system // initialize CoPOSIT object CoplanarPosit coposit = new CoplanarPosit( copositObject, focalLength ); // 2D points of te object - projection Accord.Point[] projectedPoints = new Accord.Point[4] { new Accord.Point( -77, 48 ), new Accord.Point( 44, 66 ), new Accord.Point( 75, -36 ), new Accord.Point( -61, -58 ), }; // estimate pose Matrix3x3 rotationMatrix; Vector3 translationVector; coposit.EstimatePose( projectedPoints, out rotationMatrix, out translationVector );
CoplanarPosit Class Documentation
示例#1
0
        public MainForm( )
        {
            InitializeComponent( );

            posit = new Posit( positObject, -200 );
            coposit = new CoplanarPosit( copositObject, 200 );
        }
示例#2
0
        // 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);
            }
        }