public void Init(int nparams, int nerrs, CvTermCriteria criteria, bool completeSymmFlag)
{
if (Param != null || /*Param.Rows != nparams ||*/ nerrs != ((Err != null) ? Err.Rows : 0))
{
Clear();
}
Mask = new CvMat(nparams, 1, MatrixType.U8C1);
Cv.Set(Mask, CvScalar.ScalarAll(1));
PrevParam = new CvMat(nparams, 1, MatrixType.F64C1);
Param = new CvMat(nparams, 1, MatrixType.F64C1);
JtJ = new CvMat(nparams, nparams, MatrixType.F64C1);
JtJN = new CvMat(nparams, nparams, MatrixType.F64C1);
JtJV = new CvMat(nparams, nparams, MatrixType.F64C1);
JtJW = new CvMat(nparams, 1, MatrixType.F64C1);
JtErr = new CvMat(nparams, 1, MatrixType.F64C1);
if (nerrs > 0)
{
J = new CvMat(nerrs, nparams, MatrixType.F64C1);
Err = new CvMat(nerrs, 1, MatrixType.F64C1);
}
PrevErrNorm = double.MaxValue;
LambdaLg10 = -3;
Criteria = criteria;
if ((criteria.Type & CriteriaType.Iteration) == CriteriaType.Iteration)
{
criteria.MaxIter = Math.Min(Math.Max(criteria.MaxIter, 1), 1000);
}
else
{
criteria.MaxIter = 30;
}
if ((criteria.Type & CriteriaType.Epsilon) == CriteriaType.Epsilon)
{
criteria.Epsilon = Math.Max(criteria.Epsilon, 0);
}
else
{
criteria.Epsilon = double.Epsilon;
}
State = LevMarqState.Started;
Iters = 0;
CompleteSymmFlag = completeSymmFlag;
}
public unsafe bool UpdateAlt(out CvMat _param, out CvMat _JtJ, out CvMat _JtErr, out double _errNorm)
{
_JtJ = null;
_JtErr = null;
_errNorm = 0;
double change;
if (State == LevMarqState.Done)
{
_param = Param;
return(false);
}
if (State == LevMarqState.Started)
{
_param = Param;
Cv.Zero(JtJ);
Cv.Zero(JtErr);
_errNorm = 0;
_JtJ = JtJ;
_JtErr = JtErr;
_errNorm = ErrNorm;
State = LevMarqState.CalcJ;
return(true);
}
if (State == LevMarqState.CalcJ)
{
Cv.Copy(Param, PrevParam);
Step();
_param = Param;
PrevErrNorm = ErrNorm;
ErrNorm = 0;
_errNorm = ErrNorm;
State = LevMarqState.CheckErr;
return(true);
}
Debug.Assert(State == LevMarqState.CheckErr);
if (ErrNorm > PrevErrNorm)
{
LambdaLg10++;
Step();
_param = Param;
ErrNorm = 0;
_errNorm = ErrNorm;
State = LevMarqState.CheckErr;
return(true);
}
LambdaLg10 = Math.Max(LambdaLg10 - 1, -16);
if (++Iters >= Criteria.MaxIter ||
(change = Cv.Norm(Param, PrevParam, NormType.RelativeL2)) < Criteria.Epsilon)
{
_param = Param;
State = LevMarqState.Done;
return(false);
}
PrevErrNorm = ErrNorm;
Cv.Zero(JtJ);
Cv.Zero(JtErr);
_param = Param;
_JtJ = JtJ;
_JtErr = JtErr;
State = LevMarqState.CalcJ;
return(true);
}
public bool Update(out CvMat param, out CvMat matJ, out CvMat err)
{
double change;
matJ = err = null;
if (State == LevMarqState.Done)
{
param = Param;
return(false);
}
if (State == LevMarqState.Started)
{
param = Param;
Cv.Zero(J);
Cv.Zero(Err);
matJ = J;
err = Err;
State = LevMarqState.CalcJ;
return(true);
}
if (State == LevMarqState.CalcJ)
{
Cv.MulTransposed(J, JtJ, true);
Cv.GEMM(J, Err, 1, null, 0, JtErr, GemmOperation.A_T);
Cv.Copy(Param, PrevParam);
Step();
if (Iters == 0)
{
PrevErrNorm = Cv.Norm(Err, null, NormType.L2);
}
param = Param;
Cv.Zero(Err);
err = Err;
State = LevMarqState.CheckErr;
return(true);
}
if (State != LevMarqState.CheckErr)
{
throw new OpenCvSharpException();
}
ErrNorm = Cv.Norm(Err, null, NormType.L2);
if (ErrNorm > PrevErrNorm)
{
LambdaLg10++;
Step();
param = Param;
Cv.Zero(Err);
err = Err;
State = LevMarqState.CalcJ;
return(true);
}
LambdaLg10 = Math.Max(LambdaLg10 - 1, -16);
if (++Iters >= Criteria.MaxIter ||
(change = Cv.Norm(Param, PrevParam, NormType.RelativeL2)) < Criteria.Epsilon)
{
param = Param;
State = LevMarqState.Done;
return(true);
}
PrevErrNorm = ErrNorm;
param = Param;
Cv.Zero(J);
matJ = J;
err = Err;
State = LevMarqState.CalcJ;
return(true);
}
public unsafe bool UpdateAlt(out CvMat _param, out CvMat _JtJ, out CvMat _JtErr, out double _errNorm)
{
_JtJ = null;
_JtErr = null;
_errNorm = 0;
double change;
if (State == LevMarqState.Done)
{
_param = Param;
return false;
}
if (State == LevMarqState.Started)
{
_param = Param;
Cv.Zero(JtJ);
Cv.Zero(JtErr);
_errNorm = 0;
_JtJ = JtJ;
_JtErr = JtErr;
_errNorm = ErrNorm;
State = LevMarqState.CalcJ;
return true;
}
if (State == LevMarqState.CalcJ)
{
Cv.Copy(Param, PrevParam);
Step();
_param = Param;
PrevErrNorm = ErrNorm;
ErrNorm = 0;
_errNorm = ErrNorm;
State = LevMarqState.CheckErr;
return true;
}
Debug.Assert(State == LevMarqState.CheckErr);
if (ErrNorm > PrevErrNorm)
{
LambdaLg10++;
Step();
_param = Param;
ErrNorm = 0;
_errNorm = ErrNorm;
State = LevMarqState.CheckErr;
return true;
}
LambdaLg10 = Math.Max(LambdaLg10 - 1, -16);
if (++Iters >= Criteria.MaxIter ||
(change = Cv.Norm(Param, PrevParam, NormType.RelativeL2)) < Criteria.Epsilon)
{
_param = Param;
State = LevMarqState.Done;
return false;
}
PrevErrNorm = ErrNorm;
Cv.Zero(JtJ);
Cv.Zero(JtErr);
_param = Param;
_JtJ = JtJ;
_JtErr = JtErr;
State = LevMarqState.CalcJ;
return true;
}
public bool Update(out CvMat param, out CvMat matJ, out CvMat err)
{
double change;
matJ = err = null;
if (State == LevMarqState.Done)
{
param = Param;
return false;
}
if (State == LevMarqState.Started)
{
param = Param;
Cv.Zero(J);
Cv.Zero(Err);
matJ = J;
err = Err;
State = LevMarqState.CalcJ;
return true;
}
if (State == LevMarqState.CalcJ)
{
Cv.MulTransposed(J, JtJ, true);
Cv.GEMM(J, Err, 1, null, 0, JtErr, GemmOperation.A_T);
Cv.Copy(Param, PrevParam);
Step();
if (Iters == 0)
PrevErrNorm = Cv.Norm(Err, null, NormType.L2);
param = Param;
Cv.Zero(Err);
err = Err;
State = LevMarqState.CheckErr;
return true;
}
if (State != LevMarqState.CheckErr)
{
throw new OpenCvSharpException();
}
ErrNorm = Cv.Norm(Err, null, NormType.L2);
if (ErrNorm > PrevErrNorm)
{
LambdaLg10++;
Step();
param = Param;
Cv.Zero(Err);
err = Err;
State = LevMarqState.CalcJ;
return true;
}
LambdaLg10 = Math.Max(LambdaLg10 - 1, -16);
if (++Iters >= Criteria.MaxIter ||
(change = Cv.Norm(Param, PrevParam, NormType.RelativeL2)) < Criteria.Epsilon)
{
param = Param;
State = LevMarqState.Done;
return true;
}
PrevErrNorm = ErrNorm;
param = Param;
Cv.Zero(J);
matJ = J;
err = Err;
State = LevMarqState.CalcJ;
return true;
}
public void Init(int nparams, int nerrs, CvTermCriteria criteria, bool completeSymmFlag)
{
if (Param != null || /*Param.Rows != nparams ||*/ nerrs != ((Err != null) ? Err.Rows : 0))
Clear();
Mask = new CvMat(nparams, 1, MatrixType.U8C1);
Cv.Set(Mask, CvScalar.ScalarAll(1));
PrevParam = new CvMat(nparams, 1, MatrixType.F64C1);
Param = new CvMat(nparams, 1, MatrixType.F64C1);
JtJ = new CvMat(nparams, nparams, MatrixType.F64C1);
JtJN = new CvMat(nparams, nparams, MatrixType.F64C1);
JtJV = new CvMat(nparams, nparams, MatrixType.F64C1);
JtJW = new CvMat(nparams, 1, MatrixType.F64C1);
JtErr = new CvMat(nparams, 1, MatrixType.F64C1);
if (nerrs > 0)
{
J = new CvMat(nerrs, nparams, MatrixType.F64C1);
Err = new CvMat(nerrs, 1, MatrixType.F64C1);
}
PrevErrNorm = double.MaxValue;
LambdaLg10 = -3;
Criteria = criteria;
if ((criteria.Type & CriteriaType.Iteration) == CriteriaType.Iteration)
criteria.MaxIter = Math.Min(Math.Max(criteria.MaxIter, 1), 1000);
else
criteria.MaxIter = 30;
if ((criteria.Type & CriteriaType.Epsilon) == CriteriaType.Epsilon)
criteria.Epsilon = Math.Max(criteria.Epsilon, 0);
else
criteria.Epsilon = double.Epsilon;
State = LevMarqState.Started;
Iters = 0;
CompleteSymmFlag = completeSymmFlag;
}
