C# (CSharp) Accord.Math.Geometry CoplanarPosit.EstimatePose Examples

EstimatePose() public method

Estimate pose of a model from it's projected 2D coordinates.

Because of the Coplanar POSIT algorithm's nature, it provides two pose estimations, which are valid from the algorithm's math point of view. For each pose an error is calculated, which specifies how good estimation fits to the specified real 2D coordinated. The method provides the best estimation through its output parameters rotation and translation. This may be enough for many of the pose estimation application. For those, who require checking the alternate pose estimation, it can be obtained using AlternateEstimatedRotation and AlternateEstimatedTranslation properties. The calculated error is provided for both estimations through BestEstimationError and AlternateEstimationError properties.

4 points must be be given for pose estimation.

public EstimatePose ( Point points, AForge.Math.Matrix3x3 &rotation, Vector3 &translation ) : void
points	Point	4 2D points of the model's projection.
rotation	AForge.Math.Matrix3x3	Gets best estimation of object's rotation.
translation	Vector3	Gets best estimation of object's translation.
return	void

Example #1

File: MainForm.cs Project: accord-net/framework

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