Projection2DTo2D.cs 4.5 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137
  1. using System;
  2. using System.Collections.Generic;
  3. using System.Linq;
  4. using System.Drawing;
  5. using System.Text;
  6. using System.Threading.Tasks;
  7. using Emgu.CV;
  8. using Emgu.CV.Structure;
  9. namespace bbiwarg.Utility
  10. {
  11. /// <summary>
  12. /// Computes and stores a homography matrix and provides functions to export it and project points.
  13. /// </summary>
  14. class Projection2DTo2D
  15. {
  16. /// <summary>
  17. /// size of the image the original points are in
  18. /// </summary>
  19. private ImageSize sizeA;
  20. /// <summary>
  21. /// size of the image the projected points are in
  22. /// </summary>
  23. private ImageSize sizeB;
  24. /// <summary>
  25. /// calibration points in the first image (match points in calibrationPointsB)
  26. /// </summary>
  27. private List<PointF> calibrationPointsA;
  28. /// <summary>
  29. /// calibration points in the second image (match points in calibrationPointsA)
  30. /// </summary>
  31. private List<PointF> calibrationPointsB;
  32. /// <summary>
  33. /// number of points used for the calibration
  34. /// </summary>
  35. private int numPointsForCalibration;
  36. /// <summary>
  37. /// homography matrix used to compute the projected points
  38. /// </summary>
  39. private HomographyMatrix homography;
  40. /// <summary>
  41. /// true iff the calibration is finished
  42. /// </summary>
  43. public bool IsCalibrated { get; private set; }
  44. /// <summary>
  45. /// Constructs a Projection2DTo2D.
  46. /// </summary>
  47. /// <param name="sizeA">size of the image the original points are in</param>
  48. /// <param name="sizeB">size of the image the projected points are in</param>
  49. /// <param name="numPointsForCalibration">number of points used for the calibration</param>
  50. public Projection2DTo2D(ImageSize sizeA, ImageSize sizeB, int numPointsForCalibration = 4) {
  51. this.sizeA = sizeA;
  52. this.sizeB = sizeB;
  53. this.numPointsForCalibration = numPointsForCalibration;
  54. reset();
  55. }
  56. /// <summary>
  57. /// Resets the calibration.
  58. /// </summary>
  59. public void reset() {
  60. homography = null;
  61. IsCalibrated = false;
  62. calibrationPointsA = new List<PointF>();
  63. calibrationPointsB = new List<PointF>();
  64. }
  65. /// <summary>
  66. /// Adds a pair of calibration points.
  67. /// </summary>
  68. /// <param name="pointA">point in the first image</param>
  69. /// <param name="pointB">point in the second image</param>
  70. public void addCalibrationPoints(Vector2D pointA, Vector2D pointB) {
  71. calibrationPointsA.Add(sizeA.getRelativePoint(pointA));
  72. calibrationPointsB.Add(sizeB.getRelativePoint(pointB));
  73. if (calibrationPointsA.Count == numPointsForCalibration)
  74. calibrate();
  75. }
  76. /// <summary>
  77. /// Projects a point.
  78. /// </summary>
  79. /// <param name="pointA">the point to project</param>
  80. /// <returns>projected point</returns>
  81. public Vector2D projectPoint(Vector2D pointA) {
  82. PointF[] pointfsB = new PointF[1] {sizeA.getRelativePoint(pointA)};
  83. homography.ProjectPoints(pointfsB);
  84. return sizeB.getAbsolutePoint(new Vector2D(pointfsB[0]));
  85. }
  86. /// <summary>
  87. /// Computes the homography from the lists of calibration points.
  88. /// </summary>
  89. private void calibrate() {
  90. homography = CameraCalibration.FindHomography(calibrationPointsA.ToArray(), calibrationPointsB.ToArray(), Emgu.CV.CvEnum.HOMOGRAPHY_METHOD.DEFAULT, 0.995);
  91. calibrationPointsA.Clear();
  92. calibrationPointsB.Clear();
  93. IsCalibrated = true;
  94. exportHomography();
  95. }
  96. /// <summary>
  97. /// Writes the homography to a file.
  98. /// </summary>
  99. private void exportHomography() {
  100. String[] fileData = new String[homography.Size.Height];
  101. StringBuilder sb = new StringBuilder();
  102. for (int r = 0; r < homography.Size.Height; r++)
  103. {
  104. for (int c = 0; c < homography.Size.Width; c++)
  105. {
  106. sb.Append(homography.Data[r, c]);
  107. sb.Append(" ");
  108. }
  109. fileData[r] = sb.ToString();
  110. sb.Clear();
  111. }
  112. System.IO.File.WriteAllLines(Parameters.HomographyFileName, fileData);
  113. }
  114. }
  115. }