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