Esempio n. 1
0
        //called when data for any output pin is requested
        public void Evaluate(int SpreadMax)
        {
            if (FPinInDo[0])
            {
                int nPointsPerImage = FPinInObject.SliceCount;
                if (nPointsPerImage == 0)
                {
                    FStatus[0] = "Insufficient points";
                    return;
                }
                int nImages = Math.Max(FPinInImage1.SliceCount, FPinInImage2.SliceCount) / nPointsPerImage;

                CALIB_TYPE        flags        = CALIB_TYPE.DEFAULT;
                MCvTermCriteria   termCrit     = new MCvTermCriteria(100, 0.001);
                MCvPoint3D32f[][] objectPoints = new MCvPoint3D32f[nImages][];
                PointF[][]        imagePoints1 = new PointF[nImages][];
                PointF[][]        imagePoints2 = new PointF[nImages][];
                Size imageSize = new Size((int)FPinInSensorSize[0].x, (int)FPinInSensorSize[0].y);
                ExtrinsicCameraParameters interCameraExtrinsics;
                Matrix <double>           foundamentalMatrix;
                Matrix <double>           essentialMatrix;
                IntrinsicCameraParameters intrinsics1 = FPinInIntrinsics1[0].intrinsics;
                IntrinsicCameraParameters intrinsics2 = FPinInIntrinsics2[0].intrinsics;

                imagePoints1 = MatrixUtils.ImagePoints(FPinInImage1, nPointsPerImage);
                imagePoints2 = MatrixUtils.ImagePoints(FPinInImage2, nPointsPerImage);

                for (int i = 0; i < nImages; i++)
                {
                    objectPoints[i] = MatrixUtils.ObjectPoints(FPinInObject);
                }

                try
                {
                    CameraCalibration.StereoCalibrate(objectPoints, imagePoints1, imagePoints2, intrinsics1, intrinsics2, imageSize, flags, termCrit, out interCameraExtrinsics, out foundamentalMatrix, out essentialMatrix);

                    Extrinsics extrinsics = new Extrinsics(interCameraExtrinsics);
                    FPinOutExtrinsics[0] = extrinsics;
                    FPinOutTransform[0]  = extrinsics.Matrix;

                    FPinOutSuccess[0] = true;
                    FStatus[0]        = "OK";
                }
                catch (Exception e)  {
                    FPinOutSuccess[0] = false;
                    FStatus[0]        = e.Message;
                }
            }
        }
        //called when data for any output pin is requested
        public void Evaluate(int SpreadMax)
        {
            SpreadMax = Math.Max(FPinInObject.SliceCount, FPinInImage.SliceCount);

            FPinOutExtrinsics.SliceCount = SpreadMax;
            FPinOutStatus.SliceCount     = SpreadMax;

            for (int i = 0; i < SpreadMax; i++)
            {
                try
                {
                    if (FPinInObject[i].SliceCount == 0 || FPinInImage[i].SliceCount == 0)
                    {
                        throw new Exception("No datapoints");
                    }
                    if (FPinInImage[i].SliceCount == 1)
                    {
                        throw new Exception("Only 1 image point is being input per board, check SliceCount!");
                    }
                    if (FPinInObject[i].SliceCount == 1)
                    {
                        throw new Exception("Only 1 object point is being input per board, check SliceCount!");
                    }
                    if (FPinInIntrinsics[i].intrinsics == null)
                    {
                        throw new Exception("Waiting for camera calibration intrinsics");
                    }

                    ExtrinsicCameraParameters extrinsics = CameraCalibration.FindExtrinsicCameraParams2(MatrixUtils.ObjectPoints(FPinInObject[i]), MatrixUtils.ImagePoints(FPinInImage[i]), FPinInIntrinsics[i].intrinsics);
                    FPinOutExtrinsics[i] = new Extrinsics(extrinsics);
                    FPinOutStatus[i]     = "OK";
                }
                catch (Exception e)
                {
                    FPinOutStatus[i] = e.Message;
                }
            }
        }
        //called when data for any output pin is requested
        public void Evaluate(int SpreadMax)
        {
            if (FPinInDo[0])
            {
                int nPointsPerImage = FPinInObject.SliceCount;
                if (nPointsPerImage == 0)
                {
                    FStatus[0] = "Insufficient points";
                    return;
                }
                int nImages = FPinInImage.SliceCount / nPointsPerImage;

                MCvPoint3D32f[][] objectPoints = new MCvPoint3D32f[nImages][];
                PointF[][]        imagePoints  = new PointF[nImages][];
                Size       imageSize           = new Size((int)FPinInSensorSize[0].x, (int)FPinInSensorSize[0].y);
                CALIB_TYPE flags = new CALIB_TYPE();
                IntrinsicCameraParameters   intrinsicParam = new IntrinsicCameraParameters();
                ExtrinsicCameraParameters[] extrinsicsPerView;
                GetFlags(out flags);

                if (flags.HasFlag(CALIB_TYPE.CV_CALIB_USE_INTRINSIC_GUESS))
                {
                    if (FPinInIntrinsics[0] == null)
                    {
                        Matrix <double> mat = intrinsicParam.IntrinsicMatrix;
                        mat[0, 0] = FPinInSensorSize[0].x;
                        mat[1, 1] = FPinInSensorSize[0].y;
                        mat[0, 2] = FPinInSensorSize[0].x / 2.0d;
                        mat[1, 2] = FPinInSensorSize[0].y / 2.0d;
                        mat[2, 2] = 1;
                    }
                    else
                    {
                        intrinsicParam = FPinInIntrinsics[0].intrinsics;
                    }
                }

                imagePoints = MatrixUtils.ImagePoints(FPinInImage, nPointsPerImage);

                for (int i = 0; i < nImages; i++)
                {
                    objectPoints[i] = new MCvPoint3D32f[nPointsPerImage];

                    for (int j = 0; j < nPointsPerImage; j++)
                    {
                        objectPoints[i] = MatrixUtils.ObjectPoints(FPinInObject);
                    }
                }

                try
                {
                    FPinOutError[0] = CameraCalibration.CalibrateCamera(objectPoints, imagePoints, imageSize, intrinsicParam, flags, out extrinsicsPerView);

                    Intrinsics intrinsics = new Intrinsics(intrinsicParam, imageSize);
                    FPinOutIntrinsics[0] = intrinsics;
                    FPinOutProjection[0] = intrinsics.Matrix;

                    FPinOutExtrinsics.SliceCount = nImages;
                    FPinOutView.SliceCount       = nImages;
                    for (int i = 0; i < nImages; i++)
                    {
                        Extrinsics extrinsics = new Extrinsics(extrinsicsPerView[i]);
                        FPinOutExtrinsics[i] = extrinsics;
                        FPinOutView[i]       = extrinsics.Matrix;
                    }

                    if (FPinInCoordSystem[0] == TCoordinateSystem.VVVV)
                    {
                        for (int i = 0; i < FPinOutView.SliceCount; i++)
                        {
                            FPinOutView[i] = MatrixUtils.ConvertToVVVV(FPinOutView[i]);
                        }
                    }

                    FPinOutSuccess[0] = true;
                    FStatus[0]        = "OK";
                }
                catch (Exception e)  {
                    FPinOutSuccess[0] = false;
                    FStatus[0]        = e.Message;
                }
            }
        }