- fixed stopping conditions
- handled degenerate case in mu initialization - set TLS as default - added more unit testsrelease/4.3a0
parent
c57174436f
commit
dc5c769e7c
|
@ -62,11 +62,12 @@ public:
|
|||
/// GNC parameters
|
||||
BaseOptimizerParameters baseOptimizerParams; /*optimization parameters used to solve the weighted least squares problem at each GNC iteration*/
|
||||
/// any other specific GNC parameters:
|
||||
RobustLossType lossType = GM; /* default loss*/
|
||||
RobustLossType lossType = TLS; /* default loss*/
|
||||
size_t maxIterations = 100; /* maximum number of iterations*/
|
||||
double barcSq = 1.0; /* a factor is considered an inlier if factor.error() < barcSq. Note that factor.error() whitens by the covariance*/
|
||||
double muStep = 1.4; /* multiplicative factor to reduce/increase the mu in gnc */
|
||||
double relativeCostTol = 1e-5; ///< The maximum relative cost change to stop iterating
|
||||
double relativeCostTol = 1e-5; ///< if relative cost change if below this threshold, stop iterating
|
||||
double weightsTol = 1e-4; ///< if the weights are within weightsTol from being binary, stop iterating (only for TLS)
|
||||
VerbosityGNC verbosityGNC = SILENT; /* verbosity level */
|
||||
std::vector<size_t> knownInliers = std::vector<size_t>(); /* slots in the factor graph corresponding to measurements that we know are inliers */
|
||||
|
||||
|
@ -97,6 +98,9 @@ public:
|
|||
/// Set the maximum relative difference in mu values to stop iterating
|
||||
void setRelativeCostTol(double value) { relativeCostTol = value;
|
||||
}
|
||||
/// Set the maximum difference between the weights and their rounding in {0,1} to stop iterating
|
||||
void setWeightsTol(double value) { weightsTol = value;
|
||||
}
|
||||
/// Set the verbosity level
|
||||
void setVerbosityGNC(const VerbosityGNC verbosity) {
|
||||
verbosityGNC = verbosity;
|
||||
|
@ -136,6 +140,8 @@ public:
|
|||
std::cout << "maxIterations: " << maxIterations << "\n";
|
||||
std::cout << "barcSq: " << barcSq << "\n";
|
||||
std::cout << "muStep: " << muStep << "\n";
|
||||
std::cout << "relativeCostTol: " << relativeCostTol << "\n";
|
||||
std::cout << "weightsTol: " << weightsTol << "\n";
|
||||
std::cout << "verbosityGNC: " << verbosityGNC << "\n";
|
||||
for (size_t i = 0; i < knownInliers.size(); i++)
|
||||
std::cout << "knownInliers: " << knownInliers[i] << "\n";
|
||||
|
@ -205,8 +211,8 @@ public:
|
|||
BaseOptimizer baseOptimizer(nfg_, state_);
|
||||
Values result = baseOptimizer.optimize();
|
||||
double mu = initializeMu();
|
||||
double cost = calculateWeightedCost();
|
||||
double prev_cost = cost;
|
||||
double prev_cost = nfg_.error(result);
|
||||
double cost = 0.0; // this will be updated in the main loop
|
||||
|
||||
// handle the degenerate case that corresponds to small
|
||||
// maximum residual errors at initialization
|
||||
|
@ -215,16 +221,21 @@ public:
|
|||
if (mu <= 0) {
|
||||
if (params_.verbosityGNC >= GncParameters::VerbosityGNC::SUMMARY) {
|
||||
std::cout << "GNC Optimizer stopped because maximum residual at "
|
||||
"initialization is small." << std::endl;
|
||||
"initialization is small." << std::endl;
|
||||
}
|
||||
if (params_.verbosityGNC >= GncParameters::VerbosityGNC::VALUES) {
|
||||
result.print("result\n");
|
||||
std::cout << "mu: " << mu << std::endl;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
for (size_t iter = 0; iter < params_.maxIterations; iter++) {
|
||||
size_t iter;
|
||||
for (iter = 0; iter < params_.maxIterations; iter++) {
|
||||
|
||||
// display info
|
||||
if (params_.verbosityGNC >= GncParameters::VerbosityGNC::VALUES) {
|
||||
std::cout << "iter: " << iter << std::endl;
|
||||
result.print("result\n");
|
||||
std::cout << "mu: " << mu << std::endl;
|
||||
std::cout << "weights: " << weights_ << std::endl;
|
||||
|
@ -232,28 +243,34 @@ public:
|
|||
// weights update
|
||||
weights_ = calculateWeights(result, mu);
|
||||
|
||||
// update cost
|
||||
prev_cost = cost;
|
||||
cost = calculateWeightedCost();
|
||||
|
||||
// variable/values update
|
||||
NonlinearFactorGraph graph_iter = this->makeWeightedGraph(weights_);
|
||||
BaseOptimizer baseOptimizer_iter(graph_iter, state_);
|
||||
result = baseOptimizer_iter.optimize();
|
||||
|
||||
// stopping condition
|
||||
if (checkConvergence(mu, cost, prev_cost)) {
|
||||
// display info
|
||||
if (params_.verbosityGNC >= GncParameters::VerbosityGNC::SUMMARY) {
|
||||
std::cout << "final iterations: " << iter << std::endl;
|
||||
std::cout << "final mu: " << mu << std::endl;
|
||||
std::cout << "final weights: " << weights_ << std::endl;
|
||||
}
|
||||
break;
|
||||
}
|
||||
cost = graph_iter.error(result);
|
||||
if (checkConvergence(mu, weights_, cost, prev_cost)) { break; }
|
||||
|
||||
// update mu
|
||||
mu = updateMu(mu);
|
||||
|
||||
// get ready for next iteration
|
||||
prev_cost = cost;
|
||||
|
||||
// display info
|
||||
if (params_.verbosityGNC >= GncParameters::VerbosityGNC::VALUES) {
|
||||
std::cout << "previous cost: " << prev_cost << std::endl;
|
||||
std::cout << "current cost: " << cost << std::endl;
|
||||
}
|
||||
}
|
||||
// display info
|
||||
if (params_.verbosityGNC >= GncParameters::VerbosityGNC::SUMMARY) {
|
||||
std::cout << "final iterations: " << iter << std::endl;
|
||||
std::cout << "final mu: " << mu << std::endl;
|
||||
std::cout << "final weights: " << weights_ << std::endl;
|
||||
std::cout << "previous cost: " << prev_cost << std::endl;
|
||||
std::cout << "current cost: " << cost << std::endl;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
@ -276,7 +293,9 @@ public:
|
|||
// initialize mu to the value specified in Remark 5 in GNC paper.
|
||||
// surrogate cost is convex for mu close to zero
|
||||
// degenerate case: 2 * rmax_sq - params_.barcSq < 0 (handled in the main loop)
|
||||
return params_.barcSq / (2 * rmax_sq - params_.barcSq);
|
||||
// according to remark mu = params_.barcSq / (2 * rmax_sq - params_.barcSq) = params_.barcSq/ excessResidual
|
||||
// however, if the denominator is 0 or negative, we return mu = -1 which leads to termination of the main GNC loop
|
||||
return (2 * rmax_sq - params_.barcSq) > 0 ? params_.barcSq / (2 * rmax_sq - params_.barcSq) : -1;
|
||||
default:
|
||||
throw std::runtime_error(
|
||||
"GncOptimizer::initializeMu: called with unknown loss type.");
|
||||
|
@ -298,53 +317,68 @@ public:
|
|||
}
|
||||
}
|
||||
|
||||
/// calculated sum of weighted squared residuals
|
||||
double calculateWeightedCost() const {
|
||||
double cost = 0;
|
||||
for (size_t i = 0; i < nfg_.size(); i++) {
|
||||
cost += weights_[i] * nfg_[i]->error(state_);
|
||||
}
|
||||
return cost;
|
||||
}
|
||||
|
||||
/// check if we have reached the value of mu for which the surrogate loss matches the original loss
|
||||
bool checkMuConvergence(const double mu) const {
|
||||
bool muConverged = false;
|
||||
switch (params_.lossType) {
|
||||
case GncParameters::GM:
|
||||
return std::fabs(mu - 1.0) < 1e-9; // mu=1 recovers the original GM function
|
||||
muConverged = std::fabs(mu - 1.0) < 1e-9; // mu=1 recovers the original GM function
|
||||
break;
|
||||
case GncParameters::TLS:
|
||||
muConverged = false; // for TLS there is no stopping condition on mu (it must tend to infinity)
|
||||
break;
|
||||
default:
|
||||
throw std::runtime_error(
|
||||
"GncOptimizer::checkMuConvergence: called with unknown loss type.");
|
||||
}
|
||||
if (muConverged && params_.verbosityGNC >= GncParameters::VerbosityGNC::SUMMARY)
|
||||
std::cout << "muConverged = true " << std::endl;
|
||||
return muConverged;
|
||||
}
|
||||
|
||||
/// check convergence of relative cost differences
|
||||
bool checkCostConvergence(const double cost, const double prev_cost) const {
|
||||
switch (params_.lossType) {
|
||||
case GncParameters::TLS:
|
||||
return std::fabs(cost - prev_cost) < params_.relativeCostTol;
|
||||
default:
|
||||
throw std::runtime_error(
|
||||
"GncOptimizer::checkMuConvergence: called with unknown loss type.");
|
||||
}
|
||||
bool costConverged = std::fabs(cost - prev_cost) / std::max(prev_cost,1e-7) < params_.relativeCostTol;
|
||||
if (costConverged && params_.verbosityGNC >= GncParameters::VerbosityGNC::SUMMARY)
|
||||
std::cout << "checkCostConvergence = true " << std::endl;
|
||||
return costConverged;
|
||||
}
|
||||
|
||||
/// check convergence of weights to binary values
|
||||
bool checkWeightsConvergence(const Vector& weights) const {
|
||||
bool weightsConverged = false;
|
||||
switch (params_.lossType) {
|
||||
case GncParameters::GM:
|
||||
weightsConverged = false; // for GM, there is no clear binary convergence for the weights
|
||||
break;
|
||||
case GncParameters::TLS:
|
||||
weightsConverged = true;
|
||||
for(size_t i=0; i<weights.size(); i++){
|
||||
if( std::fabs ( weights[i] - std::round(weights[i]) ) > params_.weightsTol ){
|
||||
weightsConverged = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
break;
|
||||
default:
|
||||
throw std::runtime_error(
|
||||
"GncOptimizer::checkWeightsConvergence: called with unknown loss type.");
|
||||
}
|
||||
if (weightsConverged && params_.verbosityGNC >= GncParameters::VerbosityGNC::SUMMARY)
|
||||
std::cout << "weightsConverged = true " << std::endl;
|
||||
return weightsConverged;
|
||||
}
|
||||
|
||||
/// check for convergence between consecutive GNC iterations
|
||||
bool checkConvergence(const double mu,
|
||||
const Vector& weights,
|
||||
const double cost,
|
||||
const double prev_cost) const {
|
||||
switch (params_.lossType) {
|
||||
case GncParameters::GM:
|
||||
return checkMuConvergence(mu);
|
||||
case GncParameters::TLS:
|
||||
return checkCostConvergence(cost, prev_cost);
|
||||
default:
|
||||
throw std::runtime_error(
|
||||
"GncOptimizer::checkMuConvergence: called with unknown loss type.");
|
||||
}
|
||||
return checkCostConvergence(cost,prev_cost) ||
|
||||
checkWeightsConvergence(weights) ||
|
||||
checkMuConvergence(mu);
|
||||
}
|
||||
|
||||
|
||||
/// create a graph where each factor is weighted by the gnc weights
|
||||
NonlinearFactorGraph makeWeightedGraph(const Vector& weights) const {
|
||||
// make sure all noiseModels are Gaussian or convert to Gaussian
|
||||
|
|
|
@ -66,7 +66,7 @@ TEST(GncOptimizer, gncParamsConstructor) {
|
|||
|
||||
// change something at the gncParams level
|
||||
GncParams<GaussNewtonParams> gncParams2c(gncParams2b);
|
||||
gncParams2c.setLossType(GncParams<GaussNewtonParams>::RobustLossType::TLS);
|
||||
gncParams2c.setLossType(GncParams<GaussNewtonParams>::RobustLossType::GM);
|
||||
CHECK(!gncParams2c.equals(gncParams2b.baseOptimizerParams));
|
||||
}
|
||||
|
||||
|
@ -186,7 +186,7 @@ TEST(GncOptimizer, updateMuTLS) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(GncOptimizer, checkMuConvergenceGM) {
|
||||
TEST(GncOptimizer, checkMuConvergence) {
|
||||
// has to have Gaussian noise models !
|
||||
auto fg = example::createReallyNonlinearFactorGraph();
|
||||
|
||||
|
@ -194,6 +194,7 @@ TEST(GncOptimizer, checkMuConvergenceGM) {
|
|||
Values initial;
|
||||
initial.insert(X(1), p0);
|
||||
|
||||
{
|
||||
LevenbergMarquardtParams lmParams;
|
||||
GncParams<LevenbergMarquardtParams> gncParams(lmParams);
|
||||
gncParams.setLossType(
|
||||
|
@ -203,6 +204,112 @@ TEST(GncOptimizer, checkMuConvergenceGM) {
|
|||
|
||||
double mu = 1.0;
|
||||
CHECK(gnc.checkMuConvergence(mu));
|
||||
}
|
||||
{
|
||||
LevenbergMarquardtParams lmParams;
|
||||
GncParams<LevenbergMarquardtParams> gncParams(lmParams);
|
||||
gncParams.setLossType(
|
||||
GncParams<LevenbergMarquardtParams>::RobustLossType::TLS);
|
||||
auto gnc =
|
||||
GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial, gncParams);
|
||||
|
||||
double mu = 1.0;
|
||||
CHECK(!gnc.checkMuConvergence(mu)); //always false for TLS
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(GncOptimizer, checkCostConvergence) {
|
||||
// has to have Gaussian noise models !
|
||||
auto fg = example::createReallyNonlinearFactorGraph();
|
||||
|
||||
Point2 p0(3, 3);
|
||||
Values initial;
|
||||
initial.insert(X(1), p0);
|
||||
|
||||
{
|
||||
LevenbergMarquardtParams lmParams;
|
||||
GncParams<LevenbergMarquardtParams> gncParams(lmParams);
|
||||
gncParams.setRelativeCostTol(0.49);
|
||||
auto gnc =
|
||||
GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial, gncParams);
|
||||
|
||||
double prev_cost = 1.0;
|
||||
double cost = 0.5;
|
||||
// relative cost reduction = 0.5 > 0.49, hence checkCostConvergence = false
|
||||
CHECK(!gnc.checkCostConvergence(cost, prev_cost));
|
||||
}
|
||||
{
|
||||
LevenbergMarquardtParams lmParams;
|
||||
GncParams<LevenbergMarquardtParams> gncParams(lmParams);
|
||||
gncParams.setRelativeCostTol(0.51);
|
||||
auto gnc =
|
||||
GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial, gncParams);
|
||||
|
||||
double prev_cost = 1.0;
|
||||
double cost = 0.5;
|
||||
// relative cost reduction = 0.5 < 0.51, hence checkCostConvergence = true
|
||||
CHECK(gnc.checkCostConvergence(cost, prev_cost));
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(GncOptimizer, checkWeightsConvergence) {
|
||||
// has to have Gaussian noise models !
|
||||
auto fg = example::createReallyNonlinearFactorGraph();
|
||||
|
||||
Point2 p0(3, 3);
|
||||
Values initial;
|
||||
initial.insert(X(1), p0);
|
||||
|
||||
{
|
||||
LevenbergMarquardtParams lmParams;
|
||||
GncParams<LevenbergMarquardtParams> gncParams(lmParams);
|
||||
gncParams.setLossType(
|
||||
GncParams<LevenbergMarquardtParams>::RobustLossType::GM);
|
||||
auto gnc =
|
||||
GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial, gncParams);
|
||||
|
||||
Vector weights = Vector::Ones(fg.size());
|
||||
CHECK(!gnc.checkWeightsConvergence(weights)); //always false for GM
|
||||
}
|
||||
{
|
||||
LevenbergMarquardtParams lmParams;
|
||||
GncParams<LevenbergMarquardtParams> gncParams(lmParams);
|
||||
gncParams.setLossType(
|
||||
GncParams<LevenbergMarquardtParams>::RobustLossType::TLS);
|
||||
auto gnc =
|
||||
GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial, gncParams);
|
||||
|
||||
Vector weights = Vector::Ones(fg.size());
|
||||
// weights are binary, so checkWeightsConvergence = true
|
||||
CHECK(gnc.checkWeightsConvergence(weights));
|
||||
}
|
||||
{
|
||||
LevenbergMarquardtParams lmParams;
|
||||
GncParams<LevenbergMarquardtParams> gncParams(lmParams);
|
||||
gncParams.setLossType(
|
||||
GncParams<LevenbergMarquardtParams>::RobustLossType::TLS);
|
||||
auto gnc =
|
||||
GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial, gncParams);
|
||||
|
||||
Vector weights = Vector::Ones(fg.size());
|
||||
weights[0] = 0.9; // more than weightsTol = 1e-4 from 1, hence checkWeightsConvergence = false
|
||||
CHECK(!gnc.checkWeightsConvergence(weights));
|
||||
}
|
||||
{
|
||||
LevenbergMarquardtParams lmParams;
|
||||
GncParams<LevenbergMarquardtParams> gncParams(lmParams);
|
||||
gncParams.setLossType(
|
||||
GncParams<LevenbergMarquardtParams>::RobustLossType::TLS);
|
||||
gncParams.setWeightsTol(0.1);
|
||||
auto gnc =
|
||||
GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial, gncParams);
|
||||
|
||||
Vector weights = Vector::Ones(fg.size());
|
||||
weights[0] = 0.9; // exactly weightsTol = 0.1 from 1, hence checkWeightsConvergence = true
|
||||
CHECK(gnc.checkWeightsConvergence(weights));
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
@ -455,9 +562,12 @@ TEST(GncOptimizer, optimizeWithKnownInliers) {
|
|||
knownInliers.push_back(2);
|
||||
|
||||
// nonconvexity with known inliers
|
||||
{
|
||||
GncParams<GaussNewtonParams> gncParams;
|
||||
gncParams.setKnownInliers(knownInliers);
|
||||
// gncParams.setVerbosityGNC(GncParams<GaussNewtonParams>::VerbosityGNC::VALUES);
|
||||
gncParams.setLossType(
|
||||
GncParams<GaussNewtonParams>::RobustLossType::GM);
|
||||
//gncParams.setVerbosityGNC(GncParams<GaussNewtonParams>::VerbosityGNC::SUMMARY);
|
||||
auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(fg, initial, gncParams);
|
||||
|
||||
Values gnc_result = gnc.optimize();
|
||||
|
@ -468,6 +578,45 @@ TEST(GncOptimizer, optimizeWithKnownInliers) {
|
|||
DOUBLES_EQUAL(1.0, finalWeights[0], tol);
|
||||
DOUBLES_EQUAL(1.0, finalWeights[1], tol);
|
||||
DOUBLES_EQUAL(1.0, finalWeights[2], tol);
|
||||
}
|
||||
{
|
||||
GncParams<GaussNewtonParams> gncParams;
|
||||
gncParams.setKnownInliers(knownInliers);
|
||||
gncParams.setLossType(
|
||||
GncParams<GaussNewtonParams>::RobustLossType::TLS);
|
||||
// gncParams.setVerbosityGNC(GncParams<GaussNewtonParams>::VerbosityGNC::SUMMARY);
|
||||
auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(fg, initial, gncParams);
|
||||
|
||||
Values gnc_result = gnc.optimize();
|
||||
CHECK(assert_equal(Point2(0.0, 0.0), gnc_result.at<Point2>(X(1)), 1e-3));
|
||||
|
||||
// check weights were actually fixed:
|
||||
Vector finalWeights = gnc.getWeights();
|
||||
DOUBLES_EQUAL(1.0, finalWeights[0], tol);
|
||||
DOUBLES_EQUAL(1.0, finalWeights[1], tol);
|
||||
DOUBLES_EQUAL(1.0, finalWeights[2], tol);
|
||||
DOUBLES_EQUAL(0.0, finalWeights[3], tol);
|
||||
}
|
||||
{
|
||||
// if we set the threshold large, they are all inliers
|
||||
GncParams<GaussNewtonParams> gncParams;
|
||||
gncParams.setKnownInliers(knownInliers);
|
||||
gncParams.setLossType(
|
||||
GncParams<GaussNewtonParams>::RobustLossType::TLS);
|
||||
//gncParams.setVerbosityGNC(GncParams<GaussNewtonParams>::VerbosityGNC::VALUES);
|
||||
gncParams.setInlierCostThreshold( 100.0 );
|
||||
auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(fg, initial, gncParams);
|
||||
|
||||
Values gnc_result = gnc.optimize();
|
||||
CHECK(assert_equal(Point2(0.25, 0.0), gnc_result.at<Point2>(X(1)), 1e-3));
|
||||
|
||||
// check weights were actually fixed:
|
||||
Vector finalWeights = gnc.getWeights();
|
||||
DOUBLES_EQUAL(1.0, finalWeights[0], tol);
|
||||
DOUBLES_EQUAL(1.0, finalWeights[1], tol);
|
||||
DOUBLES_EQUAL(1.0, finalWeights[2], tol);
|
||||
DOUBLES_EQUAL(1.0, finalWeights[3], tol);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
Loading…
Reference in New Issue