12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

moved gncLossType outside params

release/4.3a0
lcarlone 2020-12-29 21:59:21 -05:00
parent dfdd206708
commit 24672385b3
3 changed files with 36 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
gtsam/nonlinear/GncOptimizer.h
View File

@ -176,10 +176,10 @@ class GncOptimizer {
} }
// set initial mu // set initial mu
switch (params_.lossType) { switch (params_.lossType) {
case GncParameters::GM: case GncLossType::GM:
// surrogate cost is convex for large mu // surrogate cost is convex for large mu
return 2 * rmax_sq / params_.barcSq; // initial mu return 2 * rmax_sq / params_.barcSq; // initial mu
case GncParameters::TLS: case GncLossType::TLS:
/* initialize mu to the value specified in Remark 5 in GNC paper. /* initialize mu to the value specified in Remark 5 in GNC paper.
surrogate cost is convex for mu close to zero surrogate cost is convex for mu close to zero
degenerate case: 2 * rmax_sq - params_.barcSq < 0 (handled in the main loop) degenerate case: 2 * rmax_sq - params_.barcSq < 0 (handled in the main loop)
@ -198,10 +198,10 @@ class GncOptimizer {
/// Update the gnc parameter mu to gradually increase nonconvexity. /// Update the gnc parameter mu to gradually increase nonconvexity.
double updateMu(const double mu) const { double updateMu(const double mu) const {
switch (params_.lossType) { switch (params_.lossType) {
case GncParameters::GM: case GncLossType::GM:
// reduce mu, but saturate at 1 (original cost is recovered for mu -> 1) // reduce mu, but saturate at 1 (original cost is recovered for mu -> 1)
return std::max(1.0, mu / params_.muStep); return std::max(1.0, mu / params_.muStep);
case GncParameters::TLS: case GncLossType::TLS:
// increases mu at each iteration (original cost is recovered for mu -> inf) // increases mu at each iteration (original cost is recovered for mu -> inf)
return mu * params_.muStep; return mu * params_.muStep;
default: default:
@ -214,10 +214,10 @@ class GncOptimizer {
bool checkMuConvergence(const double mu) const { bool checkMuConvergence(const double mu) const {
bool muConverged = false; bool muConverged = false;
switch (params_.lossType) { switch (params_.lossType) {
case GncParameters::GM: case GncLossType::GM:
muConverged = 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; break;
case GncParameters::TLS: case GncLossType::TLS:
muConverged = false; // for TLS there is no stopping condition on mu (it must tend to infinity) muConverged = false; // for TLS there is no stopping condition on mu (it must tend to infinity)
break; break;
default: default:
@ -242,10 +242,10 @@ class GncOptimizer {
bool checkWeightsConvergence(const Vector& weights) const { bool checkWeightsConvergence(const Vector& weights) const {
bool weightsConverged = false; bool weightsConverged = false;
switch (params_.lossType) { switch (params_.lossType) {
case GncParameters::GM: case GncLossType::GM:
weightsConverged = false; // for GM, there is no clear binary convergence for the weights weightsConverged = false; // for GM, there is no clear binary convergence for the weights
break; break;
case GncParameters::TLS: case GncLossType::TLS:
weightsConverged = true; weightsConverged = true;
for (size_t i = 0; i < weights.size(); i++) { for (size_t i = 0; i < weights.size(); i++) {
if (std::fabs(weights[i] - std::round(weights[i])) if (std::fabs(weights[i] - std::round(weights[i]))
@ -315,7 +315,7 @@ class GncOptimizer {
// update weights of known inlier/outlier measurements // update weights of known inlier/outlier measurements
switch (params_.lossType) { switch (params_.lossType) {
case GncParameters::GM: { // use eq (12) in GNC paper case GncLossType::GM: { // use eq (12) in GNC paper
for (size_t k : unknownWeights) { for (size_t k : unknownWeights) {
if (nfg_[k]) { if (nfg_[k]) {
double u2_k = nfg_[k]->error(currentEstimate); // squared (and whitened) residual double u2_k = nfg_[k]->error(currentEstimate); // squared (and whitened) residual
@ -325,7 +325,7 @@ class GncOptimizer {
} }
return weights; return weights;
} }
case GncParameters::TLS: { // use eq (14) in GNC paper case GncLossType::TLS: { // use eq (14) in GNC paper
double upperbound = (mu + 1) / mu * params_.barcSq; double upperbound = (mu + 1) / mu * params_.barcSq;
double lowerbound = mu / (mu + 1) * params_.barcSq; double lowerbound = mu / (mu + 1) * params_.barcSq;
for (size_t k : unknownWeights) { for (size_t k : unknownWeights) {

12
gtsam/nonlinear/GncParams.h
View File

@ -32,6 +32,12 @@
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
/// Choice of robust loss function for GNC.
enum GncLossType {
GM /*Geman McClure*/,
TLS /*Truncated least squares*/
};
template<class BaseOptimizerParameters> template<class BaseOptimizerParameters>
class GncParams { class GncParams {
public: public:
@ -45,12 +51,6 @@ class GncParams {
VALUES VALUES
}; };
/// Choice of robust loss function for GNC.
enum GncLossType {
GM /*Geman McClure*/,
TLS /*Truncated least squares*/
};
/// Constructor. /// Constructor.
GncParams(const BaseOptimizerParameters& baseOptimizerParams) GncParams(const BaseOptimizerParameters& baseOptimizerParams)
: baseOptimizerParams(baseOptimizerParams) { : baseOptimizerParams(baseOptimizerParams) {

40
tests/testGncOptimizer.cpp
View File

@ -66,7 +66,7 @@ TEST(GncOptimizer, gncParamsConstructor) {
// change something at the gncParams level // change something at the gncParams level
GncParams<GaussNewtonParams> gncParams2c(gncParams2b); GncParams<GaussNewtonParams> gncParams2c(gncParams2b);
gncParams2c.setLossType(GncParams<GaussNewtonParams>::GncLossType::GM); gncParams2c.setLossType(GncLossType::GM);
CHECK(!gncParams2c.equals(gncParams2b.baseOptimizerParams)); CHECK(!gncParams2c.equals(gncParams2b.baseOptimizerParams));
} }
@ -119,7 +119,7 @@ TEST(GncOptimizer, initializeMu) {
// testing GM mu initialization // testing GM mu initialization
GncParams<LevenbergMarquardtParams> gncParams; GncParams<LevenbergMarquardtParams> gncParams;
gncParams.setLossType(GncParams<LevenbergMarquardtParams>::GncLossType::GM); gncParams.setLossType(GncLossType::GM);
auto gnc_gm = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial, auto gnc_gm = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial,
gncParams); gncParams);
// according to rmk 5 in the gnc paper: m0 = 2 rmax^2 / barcSq // according to rmk 5 in the gnc paper: m0 = 2 rmax^2 / barcSq
@ -127,7 +127,7 @@ TEST(GncOptimizer, initializeMu) {
EXPECT_DOUBLES_EQUAL(gnc_gm.initializeMu(), 2 * 198.999, 1e-3); EXPECT_DOUBLES_EQUAL(gnc_gm.initializeMu(), 2 * 198.999, 1e-3);
// testing TLS mu initialization // testing TLS mu initialization
gncParams.setLossType(GncParams<LevenbergMarquardtParams>::GncLossType::TLS); gncParams.setLossType(GncLossType::TLS);
auto gnc_tls = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial, auto gnc_tls = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial,
gncParams); gncParams);
// according to rmk 5 in the gnc paper: m0 = barcSq / (2 * rmax^2 - barcSq) // according to rmk 5 in the gnc paper: m0 = barcSq / (2 * rmax^2 - barcSq)
@ -145,7 +145,7 @@ TEST(GncOptimizer, updateMuGM) {
initial.insert(X(1), p0); initial.insert(X(1), p0);
GncParams<LevenbergMarquardtParams> gncParams; GncParams<LevenbergMarquardtParams> gncParams;
gncParams.setLossType(GncParams<LevenbergMarquardtParams>::GncLossType::GM); gncParams.setLossType(GncLossType::GM);
gncParams.setMuStep(1.4); gncParams.setMuStep(1.4);
auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial, auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial,
gncParams); gncParams);
@ -169,7 +169,7 @@ TEST(GncOptimizer, updateMuTLS) {
GncParams<LevenbergMarquardtParams> gncParams; GncParams<LevenbergMarquardtParams> gncParams;
gncParams.setMuStep(1.4); gncParams.setMuStep(1.4);
gncParams.setLossType(GncParams<LevenbergMarquardtParams>::GncLossType::TLS); gncParams.setLossType(GncLossType::TLS);
auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial, auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial,
gncParams); gncParams);
@ -188,7 +188,7 @@ TEST(GncOptimizer, checkMuConvergence) {
{ {
GncParams<LevenbergMarquardtParams> gncParams; GncParams<LevenbergMarquardtParams> gncParams;
gncParams.setLossType(GncParams<LevenbergMarquardtParams>::GncLossType::GM); gncParams.setLossType(GncLossType::GM);
auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial, auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial,
gncParams); gncParams);
@ -198,7 +198,7 @@ TEST(GncOptimizer, checkMuConvergence) {
{ {
GncParams<LevenbergMarquardtParams> gncParams; GncParams<LevenbergMarquardtParams> gncParams;
gncParams.setLossType( gncParams.setLossType(
GncParams<LevenbergMarquardtParams>::GncLossType::TLS); GncLossType::TLS);
auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial, auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial,
gncParams); gncParams);
@ -251,7 +251,7 @@ TEST(GncOptimizer, checkWeightsConvergence) {
{ {
GncParams<LevenbergMarquardtParams> gncParams; GncParams<LevenbergMarquardtParams> gncParams;
gncParams.setLossType(GncParams<LevenbergMarquardtParams>::GncLossType::GM); gncParams.setLossType(GncLossType::GM);
auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial, auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial,
gncParams); gncParams);
@ -261,7 +261,7 @@ TEST(GncOptimizer, checkWeightsConvergence) {
{ {
GncParams<LevenbergMarquardtParams> gncParams; GncParams<LevenbergMarquardtParams> gncParams;
gncParams.setLossType( gncParams.setLossType(
GncParams<LevenbergMarquardtParams>::GncLossType::TLS); GncLossType::TLS);
auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial, auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial,
gncParams); gncParams);
@ -272,7 +272,7 @@ TEST(GncOptimizer, checkWeightsConvergence) {
{ {
GncParams<LevenbergMarquardtParams> gncParams; GncParams<LevenbergMarquardtParams> gncParams;
gncParams.setLossType( gncParams.setLossType(
GncParams<LevenbergMarquardtParams>::GncLossType::TLS); GncLossType::TLS);
auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial, auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial,
gncParams); gncParams);
@ -283,7 +283,7 @@ TEST(GncOptimizer, checkWeightsConvergence) {
{ {
GncParams<LevenbergMarquardtParams> gncParams; GncParams<LevenbergMarquardtParams> gncParams;
gncParams.setLossType( gncParams.setLossType(
GncParams<LevenbergMarquardtParams>::GncLossType::TLS); GncLossType::TLS);
gncParams.setWeightsTol(0.1); gncParams.setWeightsTol(0.1);
auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial, auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial,
gncParams); gncParams);
@ -305,7 +305,7 @@ TEST(GncOptimizer, checkConvergenceTLS) {
GncParams<LevenbergMarquardtParams> gncParams; GncParams<LevenbergMarquardtParams> gncParams;
gncParams.setRelativeCostTol(1e-5); gncParams.setRelativeCostTol(1e-5);
gncParams.setLossType(GncParams<LevenbergMarquardtParams>::GncLossType::TLS); gncParams.setLossType(GncLossType::TLS);
auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial, auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial,
gncParams); gncParams);
@ -331,7 +331,7 @@ TEST(GncOptimizer, calculateWeightsGM) {
GaussNewtonParams gnParams; GaussNewtonParams gnParams;
GncParams<GaussNewtonParams> gncParams(gnParams); GncParams<GaussNewtonParams> gncParams(gnParams);
gncParams.setLossType(GncParams<GaussNewtonParams>::GncLossType::GM); gncParams.setLossType(GncLossType::GM);
auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(fg, initial, gncParams); auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(fg, initial, gncParams);
double mu = 1.0; double mu = 1.0;
Vector weights_actual = gnc.calculateWeights(initial, mu); Vector weights_actual = gnc.calculateWeights(initial, mu);
@ -364,7 +364,7 @@ TEST(GncOptimizer, calculateWeightsTLS) {
GaussNewtonParams gnParams; GaussNewtonParams gnParams;
GncParams<GaussNewtonParams> gncParams(gnParams); GncParams<GaussNewtonParams> gncParams(gnParams);
gncParams.setLossType(GncParams<GaussNewtonParams>::GncLossType::TLS); gncParams.setLossType(GncLossType::TLS);
auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(fg, initial, gncParams); auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(fg, initial, gncParams);
double mu = 1.0; double mu = 1.0;
Vector weights_actual = gnc.calculateWeights(initial, mu); Vector weights_actual = gnc.calculateWeights(initial, mu);
@ -397,7 +397,7 @@ TEST(GncOptimizer, calculateWeightsTLS2) {
// actual: // actual:
GaussNewtonParams gnParams; GaussNewtonParams gnParams;
GncParams<GaussNewtonParams> gncParams(gnParams); GncParams<GaussNewtonParams> gncParams(gnParams);
gncParams.setLossType(GncParams<GaussNewtonParams>::GncLossType::TLS); gncParams.setLossType(GncLossType::TLS);
gncParams.setInlierCostThreshold(0.51); // if inlier threshold is slightly larger than 0.5, then measurement is inlier gncParams.setInlierCostThreshold(0.51); // if inlier threshold is slightly larger than 0.5, then measurement is inlier
auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(nfg, initial, auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(nfg, initial,
gncParams); gncParams);
@ -413,7 +413,7 @@ TEST(GncOptimizer, calculateWeightsTLS2) {
// actual: // actual:
GaussNewtonParams gnParams; GaussNewtonParams gnParams;
GncParams<GaussNewtonParams> gncParams(gnParams); GncParams<GaussNewtonParams> gncParams(gnParams);
gncParams.setLossType(GncParams<GaussNewtonParams>::GncLossType::TLS); gncParams.setLossType(GncLossType::TLS);
gncParams.setInlierCostThreshold(0.49); // if inlier threshold is slightly below 0.5, then measurement is outlier gncParams.setInlierCostThreshold(0.49); // if inlier threshold is slightly below 0.5, then measurement is outlier
auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(nfg, initial, auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(nfg, initial,
gncParams); gncParams);
@ -429,7 +429,7 @@ TEST(GncOptimizer, calculateWeightsTLS2) {
// actual: // actual:
GaussNewtonParams gnParams; GaussNewtonParams gnParams;
GncParams<GaussNewtonParams> gncParams(gnParams); GncParams<GaussNewtonParams> gncParams(gnParams);
gncParams.setLossType(GncParams<GaussNewtonParams>::GncLossType::TLS); gncParams.setLossType(GncLossType::TLS);
gncParams.setInlierCostThreshold(0.5); // if inlier threshold is slightly below 0.5, then measurement is outlier gncParams.setInlierCostThreshold(0.5); // if inlier threshold is slightly below 0.5, then measurement is outlier
auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(nfg, initial, auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(nfg, initial,
gncParams); gncParams);
@ -538,7 +538,7 @@ TEST(GncOptimizer, optimizeWithKnownInliers) {
{ {
GncParams<GaussNewtonParams> gncParams; GncParams<GaussNewtonParams> gncParams;
gncParams.setKnownInliers(knownInliers); gncParams.setKnownInliers(knownInliers);
gncParams.setLossType(GncParams<GaussNewtonParams>::GncLossType::GM); gncParams.setLossType(GncLossType::GM);
//gncParams.setVerbosityGNC(GncParams<GaussNewtonParams>::Verbosity::SUMMARY); //gncParams.setVerbosityGNC(GncParams<GaussNewtonParams>::Verbosity::SUMMARY);
auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(fg, initial, auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(fg, initial,
gncParams); gncParams);
@ -555,7 +555,7 @@ TEST(GncOptimizer, optimizeWithKnownInliers) {
{ {
GncParams<GaussNewtonParams> gncParams; GncParams<GaussNewtonParams> gncParams;
gncParams.setKnownInliers(knownInliers); gncParams.setKnownInliers(knownInliers);
gncParams.setLossType(GncParams<GaussNewtonParams>::GncLossType::TLS); gncParams.setLossType(GncLossType::TLS);
// gncParams.setVerbosityGNC(GncParams<GaussNewtonParams>::Verbosity::SUMMARY); // gncParams.setVerbosityGNC(GncParams<GaussNewtonParams>::Verbosity::SUMMARY);
auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(fg, initial, auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(fg, initial,
gncParams); gncParams);
@ -574,7 +574,7 @@ TEST(GncOptimizer, optimizeWithKnownInliers) {
// if we set the threshold large, they are all inliers // if we set the threshold large, they are all inliers
GncParams<GaussNewtonParams> gncParams; GncParams<GaussNewtonParams> gncParams;
gncParams.setKnownInliers(knownInliers); gncParams.setKnownInliers(knownInliers);
gncParams.setLossType(GncParams<GaussNewtonParams>::GncLossType::TLS); gncParams.setLossType(GncLossType::TLS);
//gncParams.setVerbosityGNC(GncParams<GaussNewtonParams>::Verbosity::VALUES); //gncParams.setVerbosityGNC(GncParams<GaussNewtonParams>::Verbosity::VALUES);
gncParams.setInlierCostThreshold(100.0); gncParams.setInlierCostThreshold(100.0);
auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(fg, initial, auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(fg, initial,