bbiwarg-original/bbiwarg/Utility/Projection3DTo3D.cs

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Drawing;
using System.Threading.Tasks;
using Emgu.CV;
using Emgu.CV.Structure;

namespace bbiwarg.Utility
{
    class Projection3DTo3D
    {
        private ImageSize sizeA;
        private ImageSize sizeB;
        private List<Vector3D> calibrationPoints3D;
        private List<Vector2D> calibrationPointsA;
        private List<Vector2D> calibrationPointsB;
        private int numPointsForCalibration;

        private IntrinsicCameraParameters intrinsicParam1;
        private IntrinsicCameraParameters intrinsicParam2;
        private ExtrinsicCameraParameters[] extrinsicParams;
        private Matrix<double> foundamentalMatrix;
        private Matrix<double> essentialMatrix;

        public bool IsCalibrated { get; private set; }

        public Projection3DTo3D(ImageSize sizeA, ImageSize sizeB, int numPointsForCalibration = 8)
        {
            this.sizeA = sizeA;
            this.sizeB = sizeB;
            this.numPointsForCalibration = numPointsForCalibration;

            reset();
        }

        public void reset()
        {
            IsCalibrated = false;
            calibrationPoints3D = new List<Vector3D>();
            calibrationPointsA = new List<Vector2D>();
            calibrationPointsB = new List<Vector2D>();

            intrinsicParam1 = new IntrinsicCameraParameters();
            intrinsicParam2 = new IntrinsicCameraParameters();
        }

        public void addCalibrationPoints(Vector3D p3D, Vector2D pA, Vector2D pB)
        {
            calibrationPoints3D.Add(p3D);
            calibrationPointsA.Add(pA);
            calibrationPointsB.Add(sizeA.getAbsolutePoint(sizeB.getRelativePoint(pB)));

            if (calibrationPointsA.Count == numPointsForCalibration)
                calibrate();
        }

        private void calibrate()
        {
            MCvPoint3D32f[][] points3D = new MCvPoint3D32f[numPointsForCalibration][];
            PointF[][] pointsA = new PointF[numPointsForCalibration][];
            PointF[][] pointsB = new PointF[numPointsForCalibration][];

            for (int i = 0; i < numPointsForCalibration; i++)
            {
                points3D[i] = new MCvPoint3D32f[1] { new MCvPoint3D32f(calibrationPoints3D[i].X, calibrationPoints3D[i].Y, calibrationPoints3D[i].Z) };
                pointsA[i] = new PointF[1] { calibrationPointsA[i] };
                pointsB[i] = new PointF[1] { calibrationPointsB[i] };
            }
            /*
            CameraCalibration.StereoCalibrate(points3D, pointsA, pointsB, intrinsicParam1, intrinsicParam2, new Size(sizeA.Width, sizeB.Width), Emgu.CV.CvEnum.CALIB_TYPE.DEFAULT, new MCvTermCriteria(0.1e5), out extrinsicParams, out foundamentalMatrix, out essentialMatrix);

            Console.WriteLine("EXTRINSICS:");
            Console.WriteLine(extrinsicParams.RotationVector.ToString());
            Console.WriteLine(extrinsicParams.TranslationVector.ToString());*/

            
            //CameraCalibration.CalibrateCamera(points3D, pointsB, new Size(sizeA.Width, sizeB.Width), intrinsicParam2, Emgu.CV.CvEnum.CALIB_TYPE.DEFAULT, new MCvTermCriteria(0.1e5), out extrinsicParams);
        }
    }
}