public void TestDistortion() { // Create a checkerboard image var img = Microsoft.Psi.Imaging.Image.Create(1024, 1024, Microsoft.Psi.Imaging.PixelFormat.BGR_24bpp); unsafe { byte *row = (byte *)img.ImageData.ToPointer(); for (int i = 0; i < img.Height; i++) { byte *col = row; for (int j = 0; j < img.Width; j++) { if ((i / 20 + j / 20) % 2 == 0) { col[0] = 255; col[1] = 0; col[2] = 0; } else { col[0] = 0; col[1] = 0; col[2] = 255; } col += img.BitsPerPixel / 8; } row += img.Stride; } #if DUMP_IMAGES SaveColorToBMP("checkerboard.bmp", img); #endif // DUMP_IMAGES } // Setup our distortion coefficients double[] distortionCoefficients = new double[6] { 1.10156359448570129, -0.049757665717193485, -0.0018714899575029596, 0.0, 0.0, 0.0 }; double[] tangentialCoefficients = new double[2] { 0.0083588278483703853, 0.0 }; // Next run distort on the image var distortedImage = Microsoft.Psi.Imaging.Image.Create(img.Width, img.Height, img.PixelFormat); var intrinsicMat = CreateMatrix.Dense <double>(3, 3, new double[9] { 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0 }); var ci = new CameraIntrinsics( img.Width, img.Height, intrinsicMat, Vector <double> .Build.DenseOfArray(distortionCoefficients), Vector <double> .Build.DenseOfArray(tangentialCoefficients)); unsafe { byte *dstrow = (byte *)distortedImage.ImageData.ToPointer(); for (int i = 0; i < distortedImage.Height; i++) { byte *dstcol = dstrow; for (int j = 0; j < distortedImage.Width; j++) { MathNet.Spatial.Euclidean.Point2D pixelCoord = new MathNet.Spatial.Euclidean.Point2D((i - 512.0) / 1024.0, (j - 512.0) / 1024.0); MathNet.Spatial.Euclidean.Point2D distortedPixelCoord; ci.DistortPoint(pixelCoord, out distortedPixelCoord); int px = (int)(distortedPixelCoord.X * 1024.0 + 512.0); int py = (int)(distortedPixelCoord.Y * 1024.0 + 512.0); if (px >= 0 && px < img.Width && py >= 0 && py < img.Height) { byte *src = (byte *)img.ImageData.ToPointer() + py * distortedImage.Stride + px * distortedImage.BitsPerPixel / 8; dstcol[0] = src[0]; dstcol[1] = src[1]; dstcol[2] = src[2]; } dstcol += distortedImage.BitsPerPixel / 8; } dstrow += distortedImage.Stride; } #if DUMP_IMAGES SaveColorToBMP("distorted.bmp", distortedImage); #endif // DUMP_IMAGES } // Finally run undistort on the result var undistortedImage = Microsoft.Psi.Imaging.Image.Create(img.Width, img.Height, img.PixelFormat); unsafe { double err = 0.0; int numPts = 0; byte * dstrow = (byte *)undistortedImage.ImageData.ToPointer(); for (int i = 0; i < undistortedImage.Height; i++) { byte *dstcol = dstrow; for (int j = 0; j < undistortedImage.Width; j++) { MathNet.Spatial.Euclidean.Point2D pixelCoord = new MathNet.Spatial.Euclidean.Point2D((i - 512.0) / 1024.0, (j - 512.0) / 1024.0); MathNet.Spatial.Euclidean.Point2D distortedPixelCoord; ci.DistortPoint(pixelCoord, out distortedPixelCoord); var undistortedPixelCoord = ci.UndistortPoint(distortedPixelCoord); int px = (int)(undistortedPixelCoord.X * 1024.0 + 512.0); int py = (int)(undistortedPixelCoord.Y * 1024.0 + 512.0); if (px >= 0 && px < img.Width && py >= 0 && py < img.Height) { byte *src = (byte *)img.ImageData.ToPointer() + py * img.Stride + px * img.BitsPerPixel / 8; dstcol[0] = src[0]; dstcol[1] = src[1]; dstcol[2] = src[2]; byte * src2 = (byte *)img.ImageData.ToPointer() + i * img.Stride + j * img.BitsPerPixel / 8; double dx = (double)src2[0] - (double)src[0]; double dy = (double)src2[1] - (double)src[1]; double dz = (double)src2[2] - (double)src[2]; err += dx * dx + dy * dy + dz * dz; numPts++; } dstcol += undistortedImage.BitsPerPixel / 8; } dstrow += undistortedImage.Stride; } double rmse = Math.Sqrt(err / (double)numPts); if (rmse > 100) { throw new AssertFailedException("Distort/Undistort returned incorrect results."); } #if DUMP_IMAGES SaveColorToBMP("undistorted.bmp", undistortedImage); #endif // DUMP_IMAGES } }
public void StRayAlgo()//straight ray algorithm { if (_result.Count == 0) { _result.Clear(); } int n_d = _initialDiffusivity.Count; //he number of cells, it equals the number of columns of matrix A int n_t = _tProcessed.Count; //the number of travel times, it equals the number of rows of matrix A // b=Ax, x->x2 // consider b as time, A as distance, x as speed double xMax = 1.0 / (Math.Sqrt(_iP.MinD)); // set the constraint, the max of x, x=1/(root of diffusivity) double xMin = 1.0 / (Math.Sqrt(_iP.MaxD)); // set the constraint, the min of x, x=1/(root of diffusivity) List <double> estimatedD = _initialDiffusivity.ToList(); Vector <double> x = Vector <double> .Build.Dense(n_d, 1); for (int i = 0; i < n_d; i++) { x[i] = 1.0 / (Math.Sqrt(estimatedD[i]));//the speed vector, x=1/(root of diffusivity) } #region int iter = 0; double RMS = _iP.CriRMS + 1; while (iter <_iP.StrRay& RMS> _iP.CriRMS) { //STEP 1 SIRT_Result_StraightRay Record = new SIRT_Result_StraightRay(); Record.Iter = iter; Record.OldProcessedD = x.ToList(); //==================================================== //STEP 2 : Set matrix A Matrix <double> A = Matrix <double> .Build.Dense(n_t, n_d, 0); for (int row = 0; row < n_t; row++) { Inv_SRInfoLine infoLine = (Inv_SRInfoLine)_table.SRInfoLineList[row]; MathNet.Spatial.Euclidean.Point2D PStart = new MathNet.Spatial.Euclidean.Point2D(infoLine.Start.Coor[0], infoLine.Start.Coor[2]); MathNet.Spatial.Euclidean.Point2D PEnd = new MathNet.Spatial.Euclidean.Point2D(infoLine.End.Coor[0], infoLine.End.Coor[2]); MathNet.Spatial.Euclidean.Line2D Traj = new MathNet.Spatial.Euclidean.Line2D(PStart, PEnd); for (int col = 0; col < n_d; col++) { CohenSutherland coh = new CohenSutherland(); List <MathNet.Spatial.Euclidean.Point2D> Ps = coh.CohenSutherlandLineClip(_fRs[col], PStart, PEnd).ToList(); if (Ps.Count > 1) { A[row, col] = Ps[0].DistanceTo(Ps[1]); } } } Record.A = A.Clone(); Record.ProcessedTime = (A * x).ToList(); //==================================================== //STEP 3 : set the start node and the end node from Record foreach (object obj in _table.SRInfoLineList) { Inv_SRInfoLine infoLine = (Inv_SRInfoLine)obj; Record.Start.Add(infoLine.Start); Record.End.Add(infoLine.End); } //==================================================== //STEP 4 : cimmino calculation Vector <double> b = Vector <double> .Build.DenseOfArray(_tProcessed.ToArray()); SIRT_Options so = new SIRT_Options(b, A, x, 1);// b=Ax Vector <double> x2 = so.XUpdaterDROP(x, 1); //==================================================== //STEP 7 : update x and diffusivity for next iteration x = x2.Clone(); for (int i = 0; i < n_d; i++) { if (x[i] < conMinX[i]) { x[i] = conMinX[i]; } if (x[i] > conMaxX[i]) { x[i] = conMaxX[i]; } } Record.NewProcessedD = x.ToList(); //==================================================== _result.Add(Record);//output iter = iter + 1; } #endregion }