/// <summary> /// Writes the provided image to the stream. /// </summary> /// <param name="image">Image to write.</param> /// <returns>True, if the operation was successful, false otherwise.</returns> protected unsafe override bool WriteInternal(IImage image) { bool isSuccessful; lock (syncObj) { if (image.ColorInfo.NumberOfChannels == 3 && !ColorFrames) { throw new Exception("Image must be color!"); } if (image.ColorInfo.NumberOfChannels == 1 && ColorFrames) { throw new Exception("Image must be grayscale!"); } if (!image.Size.Equals(FrameSize)) { throw new Exception("Input image must be the same size as defined frame size!"); } this.Position++; var iplImg = image.AsOpenCvImage(); IplImage *iplImgPtr = (IplImage *)&iplImg; isSuccessful = CvHighGuiInvoke.cvWriteFrame(videoObjPtr, (IntPtr)iplImgPtr); } return(isSuccessful); }
private void Calibrate(IEnumerable <Image <Gray, byte> > images, int nx, int ny, int useUncalibrated, float _squareSize /* Chessboard square size in cm */) { int displayCorners = 1; int showUndistorted = 1; bool isVerticalStereo = false;//OpenCV can handle left-right //or up-down camera arrangements const int maxScale = 1; //FILE* f = fopen(imageList, "rt"); int i, j, lr, nframes, n = nx * ny, N = 0; List <MCvPoint3D32f> objectPoints; var points = new List <MCvPoint2D64f> [2]; List <int> npoints; List <char>[] active = new List <char> [2]; var temp = new PointF[n]; var imageSize = new System.Drawing.Size(0, 0); // ARRAY AND VECTOR STORAGE: Matrix <double> _M1 = new Matrix <double>(3, 3); Matrix <double> _M2 = new Matrix <double>(3, 3); Matrix <double> _D1 = new Matrix <double>(1, 5); Matrix <double> _D2 = new Matrix <double>(1, 5); Matrix <double> _R = new Matrix <double>(3, 3); Matrix <double> _T = new Matrix <double>(3, 1); Matrix <double> _E = new Matrix <double>(3, 3); Matrix <double> _F = new Matrix <double>(3, 3); Matrix <double> _Q = new Matrix <double>(4, 4); double [,] M1 = _M1.Data; double [,] M2 = _M2.Data; double [,] D1 = _D1.Data; double [,] D2 = _D2.Data; double [,] R = _R.Data; double [,] T = _T.Data; double [,] E = _E.Data; double [,] F = _F.Data; double [,] Q = _Q.Data; for (i = 0; i < images.Count(); i++) { int count = 0, result = 0; lr = i % 2; List <MCvPoint2D64f> pts = points[lr]; var img = images.ElementAt(i); imageSize = img.Size; //FIND CHESSBOARDS AND CORNERS THEREIN: for (int s = 1; s <= maxScale; s++) { var timg = img; if (s > 1) { timg = new Image <Gray, byte>(img.Width * s, img.Height * s); img.Resize(timg.Width, timg.Height, Emgu.CV.CvEnum.INTER.CV_INTER_CUBIC); } temp = Emgu.CV.CameraCalibration.FindChessboardCorners(timg, new Size(nx, ny), Emgu.CV.CvEnum.CALIB_CB_TYPE.ADAPTIVE_THRESH | Emgu.CV.CvEnum.CALIB_CB_TYPE.NORMALIZE_IMAGE); Emgu.CV.CameraCalibration. result = temp.Count(); if (timg != img) { timg.Dispose(); } if (result == 0 || s == maxScale) { for (j = 0; j < count; j++) { temp[j].X /= s; temp[j].Y /= s; } } if (result) { break; } } if (displayCorners) { printf("%s\n", buf); IplImage *cimg = cvCreateImage(imageSize, 8, 3); cvCvtColor(img, cimg, CV_GRAY2BGR); cvDrawChessboardCorners(cimg, cvSize(nx, ny), &temp[0], count, result); cvShowImage("corners", cimg); cvReleaseImage(&cimg); if (cvWaitKey(0) == 27) //Allow ESC to quit { exit(-1); } } else { putchar('.'); } N = pts.size(); pts.resize(N + n, cvPoint2D32f(0, 0)); active[lr].push_back((uchar)result); //assert( result != 0 ); if (result) { //Calibration will suffer without subpixel interpolation cvFindCornerSubPix(img, &temp[0], count, cvSize(11, 11), cvSize(-1, -1), cvTermCriteria(CV_TERMCRIT_ITER + CV_TERMCRIT_EPS, 30, 0.01)); copy(temp.begin(), temp.end(), pts.begin() + N); } cvReleaseImage(&img); } fclose(f); printf("\n"); // HARVEST CHESSBOARD 3D OBJECT POINT LIST: nframes = active[0].size();//Number of good chessboads found objectPoints.resize(nframes * n); for (i = 0; i < ny; i++) { for (j = 0; j < nx; j++) { objectPoints[i * nx + j] = cvPoint3D32f(i * squareSize, j * squareSize, 0); } } for (i = 1; i < nframes; i++) { copy(objectPoints.begin(), objectPoints.begin() + n, objectPoints.begin() + i * n); } npoints.resize(nframes, n); N = nframes * n; CvMat _objectPoints = cvMat(1, N, CV_32FC3, &objectPoints[0]); CvMat _imagePoints1 = cvMat(1, N, CV_32FC2, &points[0][0]); CvMat _imagePoints2 = cvMat(1, N, CV_32FC2, &points[1][0]); CvMat _npoints = cvMat(1, npoints.size(), CV_32S, &npoints[0]); cvSetIdentity(&_M1); cvSetIdentity(&_M2); cvZero(&_D1); cvZero(&_D2); // CALIBRATE THE STEREO CAMERAS printf("Running stereo calibration ..."); fflush(stdout); cvStereoCalibrate(&_objectPoints, &_imagePoints1, &_imagePoints2, &_npoints, &_M1, &_D1, &_M2, &_D2, imageSize, &_R, &_T, &_E, &_F, cvTermCriteria(CV_TERMCRIT_ITER + CV_TERMCRIT_EPS, 100, 1e-5), CV_CALIB_FIX_ASPECT_RATIO + CV_CALIB_ZERO_TANGENT_DIST + CV_CALIB_SAME_FOCAL_LENGTH); printf(" done\n"); // CALIBRATION QUALITY CHECK // because the output fundamental matrix implicitly // includes all the output information, // we can check the quality of calibration using the // epipolar geometry constraint: m2^t*F*m1=0 vector <CvPoint3D32f> lines[2]; points[0].resize(N); points[1].resize(N); _imagePoints1 = cvMat(1, N, CV_32FC2, &points[0][0]); _imagePoints2 = cvMat(1, N, CV_32FC2, &points[1][0]); lines[0].resize(N); lines[1].resize(N); CvMat _L1 = cvMat(1, N, CV_32FC3, &lines[0][0]); CvMat _L2 = cvMat(1, N, CV_32FC3, &lines[1][0]); //Always work in undistorted space cvUndistortPoints(&_imagePoints1, &_imagePoints1, &_M1, &_D1, 0, &_M1); cvUndistortPoints(&_imagePoints2, &_imagePoints2, &_M2, &_D2, 0, &_M2); cvComputeCorrespondEpilines(&_imagePoints1, 1, &_F, &_L1); cvComputeCorrespondEpilines(&_imagePoints2, 2, &_F, &_L2); double avgErr = 0; for (i = 0; i < N; i++) { double err = fabs(points[0][i].x * lines[1][i].x + points[0][i].y * lines[1][i].y + lines[1][i].z) + fabs(points[1][i].x * lines[0][i].x + points[1][i].y * lines[0][i].y + lines[0][i].z); avgErr += err; } printf("avg err = %g\n", avgErr / (nframes * n)); //COMPUTE AND DISPLAY RECTIFICATION if (showUndistorted) { CvMat *mx1 = cvCreateMat(imageSize.height, imageSize.width, CV_32F); CvMat *my1 = cvCreateMat(imageSize.height, imageSize.width, CV_32F); CvMat *mx2 = cvCreateMat(imageSize.height, imageSize.width, CV_32F); CvMat *my2 = cvCreateMat(imageSize.height, imageSize.width, CV_32F); CvMat *img1r = cvCreateMat(imageSize.height, imageSize.width, CV_8U); CvMat *img2r = cvCreateMat(imageSize.height, imageSize.width, CV_8U); CvMat *disp = cvCreateMat(imageSize.height, imageSize.width, CV_16S); CvMat *vdisp = cvCreateMat(imageSize.height, imageSize.width, CV_8U); CvMat *pair; double R1[3][3], R2[3][3], P1[3][4], P2[3][4];
public unsafe static extern void cvPutText(IplImage *img, [MarshalAs(UnmanagedType.LPStr)] String text, Point org, ref Font font, CvScalar color);
public unsafe static extern void cvFillConvexPoly(IplImage *img, [In] Point[] pts, int npts, CvScalar color, LineTypes lineType, int shift);
public unsafe static extern void cvDrawContours(IplImage *img, IntPtr contour, CvScalar externalColor, CvScalar holeColor, int maxLevel, int thickness, LineTypes lineType, Point offset);
public unsafe static extern void cvEllipse(IplImage *img, Point center, Size axes, double angle, double startAngle, double endAngle, CvScalar color, int thickness, LineTypes lineType, int shift);
public unsafe static extern void cvCircle(IplImage *img, Point center, int radius, CvScalar color, int thickness, LineTypes lineType, int shift);
public unsafe static extern void cvRectangleR(IplImage *img, Rectangle rect, CvScalar color, int thickness, LineTypes lineType, int shift);
public unsafe static extern void cvPolyLine(IplImage *img, [In] IntPtr[] pts, [In] int[] npts, int contours, [MarshalAs(UnmanagedType.Bool)] bool isClosed, CvScalar color, int thickness, LineTypes lineType, int shift);
public unsafe static extern void cvLine(IplImage *img, Point pt1, Point pt2, CvScalar color, int thickness, LineTypes lineType, int shift);
public unsafe static extern void cvAddWeighted(IplImage *src1, double alpha, IplImage *src2, double beta, double gamma, IplImage *dst);
public unsafe static extern IplImage *cvCloneImage(IplImage *image);
public unsafe static extern bool cvSaveImage([MarshalAs(UnmanagedType.LPStr)] String filename, IplImage *image, IntPtr parameters);
public unsafe static extern void cvReleaseImage(ref IplImage *image);