public MarkerPosition3D(float realSizeMarker, float focalLength
, int imgW, int imgH)
{
imageWidth = imgW;
imageHeight = imgH;
positAlgorithm = new CoplanarPosit(
new Vector3[]
{ new Vector3( -realSizeMarker/2, 0, realSizeMarker/2 ),
new Vector3( realSizeMarker/2, 0, realSizeMarker/2 ),
new Vector3( realSizeMarker/2, 0, -realSizeMarker/2 ),
new Vector3( -realSizeMarker/2, 0, -realSizeMarker/2 ),
}
, focalLength);
}
// 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 );
}
}
private void CreatePosit( )
{
if ( glyphSize != 0 )
{
// create glyph's model
float sizeHalf = glyphSize / 2;
Vector3[] glyphModel = new Vector3[]
{
new Vector3( -sizeHalf, 0, sizeHalf ),
new Vector3( sizeHalf, 0, sizeHalf ),
new Vector3( sizeHalf, 0, -sizeHalf ),
new Vector3( -sizeHalf, 0, -sizeHalf ),
};
posit = new CoplanarPosit( glyphModel, cameraFocalLength );
}
else
{
posit = null;
}
}
public MainForm( )
{
InitializeComponent( );
posit = new Posit( positObject, -200 );
coposit = new CoplanarPosit( copositObject, 200 );
}
/// <summary>
/// Estemate the 3D position of te glyph.
/// </summary>
/// <param name="imageSize">Image size is more like size of the coordinate system.</param>
/// <param name="useCoplanarPosit"></param>
/// <param name="yaw"></param>
/// <param name="pitch"></param>
/// <param name="roll"></param>
public void EstimateOrientation(bool useCoplanarPosit, out float yaw, out float pitch, out float roll)
{
AForge.Point[] tmpQuadrilateral = new AForge.Point[this.Quadrilateral.Count];
for (int index = 0; index < this.Quadrilateral.Count; index++)
{
tmpQuadrilateral[index] = new AForge.Point(this.Quadrilateral[index].X, this.Quadrilateral[index].Y);
}
//
this.focalLength = this.CoordinateSystemSize.Width;
// Scale the coordinates
for (int index = 0; index < tmpQuadrilateral.Length; index++)
{
float x = System.Math.Max(0, System.Math.Min(tmpQuadrilateral[index].X, this.CoordinateSystemSize.Width - 1));
float y = System.Math.Max(0, System.Math.Min(tmpQuadrilateral[index].Y, this.CoordinateSystemSize.Height - 1));
tmpQuadrilateral[index] = new AForge.Point(x - this.CoordinateSystemSize.Width / 2, this.CoordinateSystemSize.Height / 2 - y);
}
// 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.
this.ModelRadius = 0;
foreach (Vector3 modelPoint in modelPoints)
{
float distanceToCenter = (modelPoint - modelCenter).Norm;
if (distanceToCenter > this.ModelRadius)
{
this.ModelRadius = distanceToCenter;
}
}
if (!useCoplanarPosit)
{
Posit posit = new Posit(modelPoints, focalLength);
posit.EstimatePose(tmpQuadrilateral, out rotationMatrix, out translationVector);
}
else
{
CoplanarPosit coposit = new CoplanarPosit(modelPoints, this.focalLength);
coposit.EstimatePose(tmpQuadrilateral, out rotationMatrix, out translationVector);
this.bestRotationMatrix = coposit.BestEstimatedRotation;
this.bestTranslationVector = coposit.BestEstimatedTranslation;
this.alternateRotationMatrix = coposit.AlternateEstimatedRotation;
this.alternateTranslationVector = coposit.AlternateEstimatedTranslation;
}
// Get the rotation.
this.rotationMatrix.ExtractYawPitchRoll(out yaw, out pitch, out roll);
// Transform to degree.
yaw *= (float)(180.0 / System.Math.PI);
pitch *= (float)(180.0 / System.Math.PI);
roll *= (float)(180.0 / System.Math.PI);
}
