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