diff options
Diffstat (limited to 'unsupported/Eigen')
-rw-r--r-- | unsupported/Eigen/src/NonLinearOptimization/HybridNonLinearSolver.h | 8 | ||||
-rw-r--r-- | unsupported/Eigen/src/NonLinearOptimization/LevenbergMarquardt.h | 12 |
2 files changed, 17 insertions, 3 deletions
diff --git a/unsupported/Eigen/src/NonLinearOptimization/HybridNonLinearSolver.h b/unsupported/Eigen/src/NonLinearOptimization/HybridNonLinearSolver.h index 35dc332e0..d75b1407c 100644 --- a/unsupported/Eigen/src/NonLinearOptimization/HybridNonLinearSolver.h +++ b/unsupported/Eigen/src/NonLinearOptimization/HybridNonLinearSolver.h @@ -194,6 +194,8 @@ template<typename FunctorType, typename Scalar> HybridNonLinearSolverSpace::Status HybridNonLinearSolver<FunctorType,Scalar>::solveOneStep(FVectorType &x) { + assert(x.size()==n); // check the caller is not cheating us + int j; std::vector<PlanarRotation<Scalar> > v_givens(n), w_givens(n); @@ -350,6 +352,8 @@ HybridNonLinearSolverSpace::Status HybridNonLinearSolver<FunctorType,Scalar>::solve(FVectorType &x) { HybridNonLinearSolverSpace::Status status = solveInit(x); + if (status==HybridNonLinearSolverSpace::ImproperInputParameters) + return status; while (status==HybridNonLinearSolverSpace::Running) status = solveOneStep(x); return status; @@ -429,6 +433,8 @@ template<typename FunctorType, typename Scalar> HybridNonLinearSolverSpace::Status HybridNonLinearSolver<FunctorType,Scalar>::solveNumericalDiffOneStep(FVectorType &x) { + assert(x.size()==n); // check the caller is not cheating us + int j; std::vector<PlanarRotation<Scalar> > v_givens(n), w_givens(n); @@ -587,6 +593,8 @@ HybridNonLinearSolverSpace::Status HybridNonLinearSolver<FunctorType,Scalar>::solveNumericalDiff(FVectorType &x) { HybridNonLinearSolverSpace::Status status = solveNumericalDiffInit(x); + if (status==HybridNonLinearSolverSpace::ImproperInputParameters) + return status; while (status==HybridNonLinearSolverSpace::Running) status = solveNumericalDiffOneStep(x); return status; diff --git a/unsupported/Eigen/src/NonLinearOptimization/LevenbergMarquardt.h b/unsupported/Eigen/src/NonLinearOptimization/LevenbergMarquardt.h index 8bae1e131..f99366bbc 100644 --- a/unsupported/Eigen/src/NonLinearOptimization/LevenbergMarquardt.h +++ b/unsupported/Eigen/src/NonLinearOptimization/LevenbergMarquardt.h @@ -161,6 +161,8 @@ LevenbergMarquardtSpace::Status LevenbergMarquardt<FunctorType,Scalar>::minimize(FVectorType &x) { LevenbergMarquardtSpace::Status status = minimizeInit(x); + if (status==LevenbergMarquardtSpace::ImproperInputParameters) + return status; do { status = minimizeOneStep(x); } while (status==LevenbergMarquardtSpace::Running); @@ -214,7 +216,7 @@ template<typename FunctorType, typename Scalar> LevenbergMarquardtSpace::Status LevenbergMarquardt<FunctorType,Scalar>::minimizeOneStep(FVectorType &x) { - int j; + assert(x.size()==n); // check the caller is not cheating us /* calculate the jacobian matrix. */ int df_ret = functor.df(x, fjac); @@ -235,7 +237,7 @@ LevenbergMarquardt<FunctorType,Scalar>::minimizeOneStep(FVectorType &x) /* to the norms of the columns of the initial jacobian. */ if (iter == 1) { if (!useExternalScaling) - for (j = 0; j < n; ++j) + for (int j = 0; j < n; ++j) diag[j] = (wa2[j]==0.)? 1. : wa2[j]; /* on the first iteration, calculate the norm of the scaled x */ @@ -255,7 +257,7 @@ LevenbergMarquardt<FunctorType,Scalar>::minimizeOneStep(FVectorType &x) /* compute the norm of the scaled gradient. */ gnorm = 0.; if (fnorm != 0.) - for (j = 0; j < n; ++j) + for (int j = 0; j < n; ++j) if (wa2[permutation.indices()[j]] != 0.) gnorm = std::max(gnorm, ei_abs( fjac.col(j).head(j+1).dot(qtf.head(j+1)/fnorm) / wa2[permutation.indices()[j]])); @@ -431,6 +433,8 @@ template<typename FunctorType, typename Scalar> LevenbergMarquardtSpace::Status LevenbergMarquardt<FunctorType,Scalar>::minimizeOptimumStorageOneStep(FVectorType &x) { + assert(x.size()==n); // check the caller is not cheating us + int i, j; bool sing; @@ -606,6 +610,8 @@ LevenbergMarquardtSpace::Status LevenbergMarquardt<FunctorType,Scalar>::minimizeOptimumStorage(FVectorType &x) { LevenbergMarquardtSpace::Status status = minimizeOptimumStorageInit(x); + if (status==LevenbergMarquardtSpace::ImproperInputParameters) + return status; do { status = minimizeOptimumStorageOneStep(x); } while (status==LevenbergMarquardtSpace::Running); |