public void CreateEpiGeometry_Normalized()
{
var ChL = new DenseVector(4);
_C_L.CopySubVectorTo(ChL, 0, 0, 3);
ChL[3] = 1.0;
var ChR = new DenseVector(4);
_C_R.CopySubVectorTo(ChR, 0, 0, 3);
ChR[3] = 1.0;
// Find e_R = P_R*C_L, e_L = P_L*C_R
_epi_R = _CMN_R * (_Nr * ChL);
_epi_L = _CMN_L * (_Nr * ChR);
_epi_R.DivideThis(_epi_R[2]);
_epi_L.DivideThis(_epi_L[2]);
_epiX_L = new DenseMatrix(3, 3);
_epiX_L[0, 0] = 0.0;
_epiX_L[1, 0] = _epi_L[2];
_epiX_L[2, 0] = -_epi_L[1];
_epiX_L[0, 1] = -_epi_L[2];
_epiX_L[1, 1] = 0.0;
_epiX_L[2, 1] = _epi_L[0];
_epiX_L[0, 2] = _epi_L[1];
_epiX_L[1, 2] = -_epi_L[0];
_epiX_L[2, 2] = 0.0;
_epiX_R = new DenseMatrix(3, 3);
_epiX_R[0, 0] = 0.0;
_epiX_R[1, 0] = _epi_R[2];
_epiX_R[2, 0] = -_epi_R[1];
_epiX_R[0, 1] = -_epi_R[2];
_epiX_R[1, 1] = 0.0;
_epiX_R[2, 1] = _epi_R[0];
_epiX_R[0, 2] = _epi_R[1];
_epiX_R[1, 2] = -_epi_R[0];
_epiX_R[2, 2] = 0.0;
// F = [er]x * Pr * pseudoinv(Pl)
_F = _epiX_R * (_CMN_R * _CMN_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]);
}
private void FindDisparitiesCosts(IntVector2 pb, Vector<double> epiLine)
{
epiLine.DivideThis(epiLine.At(1));
IntVector2 pm = new IntVector2();
int xmax = FindXmax(epiLine);
int x0 = FindX0(epiLine);
xmax = xmax - 1;
for(int xm = x0 + 1; xm < xmax; ++xm)
{
double ym0 = FindYd(xm, epiLine);
double ym1 = FindYd(xm + 1, epiLine);
for(int ym = (int)ym0; ym < (int)ym1; ++ym)
{
if(ImageMatched.HaveValueAt(ym, xm))
{
pm.X = xm;
pm.Y = ym;
double cost = CostComp.GetCost_Border(CurrentPixel, pm);
DispComp.StoreDisparity(CurrentPixel, pm, cost);
}
}
}
}
public Matrix<double> ComputeShearingMatrix_My(int imgWidth, int imgHeight, Matrix<double> H)
{
// Transform corner points
_shTopLeft = H * new DenseVector(new double[3] { 0.0, 0.0, 1.0 }); ;
_shTopRight = H * new DenseVector(new double[3] { imgWidth - 1, 0.0, 1.0 }); ;
_shBotLeft = H * new DenseVector(new double[3] { 0.0, imgHeight - 1, 1.0 }); ;
_shBotRight = H * new DenseVector(new double[3] { imgWidth - 1, imgHeight - 1, 1.0 }); ;
// Scale so that weights are 1
_shTopLeft.DivideThis(_shTopLeft.At(2));
_shTopRight.DivideThis(_shTopRight.At(2));
_shBotLeft.DivideThis(_shBotLeft.At(2));
_shBotRight.DivideThis(_shBotRight.At(2));
double s1 = 1.0, s2 = 0.0;
MultivariateFunctionMinimalisation miniAlg = new MultivariateFunctionMinimalisation();
miniAlg.DoComputeDerivativesNumerically = true;
miniAlg.Function = GetErrorFunction_NewS_Beta;
miniAlg.IterationInit = UpdateShearingParams_Beta;
miniAlg.MaximumIterations = 100;
miniAlg.NumericalDerivativeStep = 1e-3;
miniAlg.UseBFGSMethod = false;
miniAlg.DoComputeDerivativesNumerically = true;
miniAlg.InitialParameters = new DenseVector(new double[2] { s1, s2 });
miniAlg.Process();
s1 = miniAlg.MinimalParameters.At(0);
s2 = miniAlg.MinimalParameters.At(1);
var Hs = DenseMatrix.CreateIdentity(3);
Hs[0, 0] = s1;
Hs[0, 1] = s2;
_shTopLeft = Hs * _shTopLeft;
_shTopRight = Hs * _shTopRight;
_shBotLeft = Hs * _shBotLeft;
_shBotRight = Hs * _shBotRight;
double wt = (_shTopLeft - _shTopRight).L2Norm();
double wb = (_shBotLeft - _shBotRight).L2Norm();
double hl = (_shTopLeft - _shBotLeft).L2Norm();
double hr = (_shTopRight - _shBotRight).L2Norm();
return Hs;
}
