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