| 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 | |
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." );
}
Matrix3x3 rotation1, rotation2;
Vector3 translation1, translation2;
// find initial rotation
POS( points, new Vector3( 1 ), out rotation1, out rotation2, out translation1, out translation2 );
// iterate further and fine tune the solution
float error1 = Iterate( points, ref rotation1, ref translation1 );
float error2 = Iterate( points, ref rotation2, ref translation2 );
// take the best found pose
if ( error1 < error2 )
{
bestRotation = rotation1;
bestTranslation = translation1;
bestPoseError = error1;
alternateRotation = rotation2;
alternateTranslation = translation2;
alternatePoseError = error2;
}
else
{
bestRotation = rotation2;
bestTranslation = translation2;
bestPoseError = error2;
alternateRotation = rotation1;
alternateTranslation = translation1;
alternatePoseError = error1;
}
rotation = bestRotation;
translation = bestTranslation;
}
// 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);
}
}