public EstimatePose ( Point points, AForge.Math.Matrix3x3 &rotation, Vector3 &translation ) : void | ||
points | Point | 4 2D points of the |
rotation | AForge.Math.Matrix3x3 | Gets best estimation of object's rotation. |
translation | Vector3 | Gets best estimation of object's translation. |
return | void |
// 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); } }