private void RectifyImages(object sender, RoutedEventArgs e)
{
if(_camImageFirst.ImageSource == null || _camImageSec.ImageSource == null)
{
MessageBox.Show("Images must be set");
return;
}
if(_camImageFirst.ImageSource.PixelWidth != _camImageSec.ImageSource.PixelWidth ||
_camImageFirst.ImageSource.PixelHeight != _camImageSec.ImageSource.PixelHeight)
{
MessageBox.Show("Images must have same size");
return;
}
if(CalibrationData.Data.IsCamLeftCalibrated == false ||
CalibrationData.Data.IsCamRightCalibrated == false)
{
MessageBox.Show("Cameras must be calibrated");
return;
}
ImageRectification_ZhangLoop rectifier = new ImageRectification_ZhangLoop();
RectificationTransformation rectTransformation = new RectificationTransformation();
rectTransformation.Rectifier = rectifier;
ImageTransformer transformer = new ImageTransformer();
transformer.Transformation = rectTransformation;
transformer.UsedInterpolationMethod = ImageTransformer.InterpolationMethod.Quadratic;
rectifier.ImageHeight = _camImageFirst.ImageSource.PixelHeight;
rectifier.ImageWidth = _camImageFirst.ImageSource.PixelWidth;
rectifier.EpiCrossLeft = CalibrationData.Data.EpipoleCrossLeft;
rectifier.EpiCrossRight = CalibrationData.Data.EpipoleCrossRight;
rectifier.EpipoleLeft = CalibrationData.Data.EpipoleLeft;
rectifier.EpipoleRight = CalibrationData.Data.EpipoleRight;
rectifier.IsEpiLeftInInfinity = CalibrationData.Data.EpiLeftInInfinity;
rectifier.IsEpiRightInInfinity = CalibrationData.Data.EpiRightInInfinity;
rectifier.FundamentalMatrix = CalibrationData.Data.Fundamental;
rectifier.ComputeRectificationMatrices();
ColorImage imgLeft = new ColorImage();
imgLeft.FromBitmapSource(_camImageFirst.ImageSource);
ColorImage imgRight = new ColorImage();
imgRight.FromBitmapSource(_camImageSec.ImageSource);
rectTransformation.WhichImage = RectificationTransformation.ImageIndex.Left;
ColorImage rectLeft = transformer.TransfromImageBackwards(imgLeft, true);
rectTransformation.WhichImage = RectificationTransformation.ImageIndex.Right;
ColorImage rectRight = transformer.TransfromImageBackwards(imgRight, true);
_camImageFirst.ImageSource = rectLeft.ToBitmapSource();
_camImageSec.ImageSource = rectRight.ToBitmapSource();
}
public void Test_Rectification()
{
var Fi = new DenseMatrix(3); // Target F
Fi[1, 2] = -1.0;
Fi[2, 1] = 1.0;
var K_l = new DenseMatrix(3, 3);
K_l[0, 0] = 10.0; // fx
K_l[1, 1] = 10.0; // fy
K_l[0, 1] = 0.0; // s
K_l[0, 2] = 300.0; // x0
K_l[1, 2] = 250.0; // y0
K_l[2, 2] = 1.0; // 1
var K_r = new DenseMatrix(3, 3);
K_r[0, 0] = 12.0; // fx
K_r[1, 1] = 12.5; // fy
K_r[0, 1] = 0.0; // s
K_r[0, 2] = 300.0; // x0
K_r[1, 2] = 200.0; // y0
K_r[2, 2] = 1.0; // 1
var R_l = DenseMatrix.CreateIdentity(3);
var R_r = DenseMatrix.CreateIdentity(3);
Vector<double> C_l = new DenseVector(3);
C_l[0] = 50.0;
C_l[1] = 50.0;
C_l[2] = 0.0;
Vector<double> C_r = new DenseVector(3);
C_r[0] = 40.0;
C_r[1] = 40.0;
C_r[2] = 10.0;
Matrix<double> Ext_l = new DenseMatrix(3, 4);
Ext_l.SetSubMatrix(0, 0, R_l);
Ext_l.SetColumn(3, -R_l * C_l);
Matrix<double> Ext_r = new DenseMatrix(3, 4);
Ext_r.SetSubMatrix(0, 0, R_r);
Ext_r.SetColumn(3, -R_r * C_r);
var CM_l = K_l * Ext_l;
var CM_r = K_r * Ext_r;
// Find e_R = P_R*C_L, e_L = P_L*C_R
var epi_r = CM_r * new DenseVector(new double[] { C_l[0], C_l[1], C_l[2], 1.0 });
var epi_l = CM_l * new DenseVector(new double[] { C_r[0], C_r[1], C_r[2], 1.0 });
var ex_l = new DenseMatrix(3, 3);
ex_l[0, 0] = 0.0;
ex_l[1, 0] = epi_l[2];
ex_l[2, 0] = -epi_l[1];
ex_l[0, 1] = -epi_l[2];
ex_l[1, 1] = 0.0;
ex_l[2, 1] = epi_l[0];
ex_l[0, 2] = epi_l[1];
ex_l[1, 2] = -epi_l[0];
ex_l[2, 2] = 0.0;
var ex_r = new DenseMatrix(3, 3);
ex_r[0, 0] = 0.0;
ex_r[1, 0] = epi_r[2];
ex_r[2, 0] = -epi_r[1];
ex_r[0, 1] = -epi_r[2];
ex_r[1, 1] = 0.0;
ex_r[2, 1] = epi_r[0];
ex_r[0, 2] = epi_r[1];
ex_r[1, 2] = -epi_r[0];
ex_r[2, 2] = 0.0;
// F = [er]x * Pr * pseudoinv(Pl)
var F = ex_r * (CM_r * CM_l.PseudoInverse());
int rank = F.Rank();
if(rank == 3)
{
// Need to ensure rank 2, so set smallest singular value to 0
var svd = F.Svd();
var E = svd.W;
E[2, 2] = 0;
var oldF = F;
F = svd.U * E * svd.VT;
var diff = F - oldF; // Difference should be very small if all is correct
}
// Scale F, so that F33 = 1
F = F.Divide(F[2, 2]);
ImageRectification_ZhangLoop rect = new ImageRectification_ZhangLoop();
// Assume image of size 640x480
rect.ImageHeight = 480;
rect.ImageWidth = 640;
rect.FundamentalMatrix = F;
rect.EpiCrossLeft = ex_l;
rect.ComputeRectificationMatrices();
// Test H'^T * Fi * H should be very close to F
var H_r = rect.RectificationRight;
var H_l = rect.RectificationLeft;
var eF = H_r.Transpose() * Fi * H_l;
double err = (eF - F).FrobeniusNorm();
Assert.IsTrue(err < 1e-6);
}