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
}
