correct formatting
lcarlone
parent
c4644a0d61
commit
7699f04820
|
|
@ -51,10 +51,13 @@ public:
|
|||
using BaseOptimizer = GaussNewtonOptimizer; // BaseOptimizerParameters::OptimizerType;
|
||||
|
||||
GncParams(const BaseOptimizerParameters& baseOptimizerParams) :
|
||||
baseOptimizerParams(baseOptimizerParams) {}
|
||||
baseOptimizerParams(baseOptimizerParams) {
|
||||
}
|
||||
|
||||
// default constructor
|
||||
GncParams(): baseOptimizerParams() {}
|
||||
GncParams() :
|
||||
baseOptimizerParams() {
|
||||
}
|
||||
|
||||
BaseOptimizerParameters baseOptimizerParams;
|
||||
/// any other specific GNC parameters:
|
||||
|
|
@ -65,16 +68,24 @@ public:
|
|||
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 */
|
||||
|
||||
void setLossType(const RobustLossType type){ lossType = type; }
|
||||
void setLossType(const RobustLossType type) {
|
||||
lossType = type;
|
||||
}
|
||||
void setMaxIterations(const size_t maxIter) {
|
||||
std::cout
|
||||
<< "setMaxIterations: changing the max nr of iters might lead to less accurate solutions and is not recommended! "
|
||||
<< std::endl;
|
||||
maxIterations = maxIter;
|
||||
}
|
||||
void setInlierThreshold(const double inth){ barcSq = inth; }
|
||||
void setMuStep(const double step){ muStep = step; }
|
||||
void setVerbosityGNC(const VerbosityGNC verbosity) { verbosityGNC = verbosity; }
|
||||
void setInlierThreshold(const double inth) {
|
||||
barcSq = inth;
|
||||
}
|
||||
void setMuStep(const double step) {
|
||||
muStep = step;
|
||||
}
|
||||
void setVerbosityGNC(const VerbosityGNC verbosity) {
|
||||
verbosityGNC = verbosity;
|
||||
}
|
||||
void setKnownInliers(const std::vector<size_t> knownIn) {
|
||||
for (size_t i = 0; i < knownIn.size(); i++)
|
||||
knownInliers.push_back(knownIn[i]);
|
||||
|
|
@ -83,8 +94,7 @@ public:
|
|||
/// equals
|
||||
bool equals(const GncParams& other, double tol = 1e-9) const {
|
||||
return baseOptimizerParams.equals(other.baseOptimizerParams)
|
||||
&& lossType == other.lossType
|
||||
&& maxIterations == other.maxIterations
|
||||
&& lossType == other.lossType && maxIterations == other.maxIterations
|
||||
&& std::fabs(barcSq - other.barcSq) <= tol
|
||||
&& std::fabs(muStep - other.muStep) <= tol
|
||||
&& verbosityGNC == other.verbosityGNC
|
||||
|
|
@ -95,10 +105,11 @@ public:
|
|||
void print(const std::string& str) const {
|
||||
std::cout << str << "\n";
|
||||
switch (lossType) {
|
||||
case GM: std::cout << "lossType: Geman McClure" << "\n"; break;
|
||||
case GM:
|
||||
std::cout << "lossType: Geman McClure" << "\n";
|
||||
break;
|
||||
default:
|
||||
throw std::runtime_error(
|
||||
"GncParams::print: unknown loss type.");
|
||||
throw std::runtime_error("GncParams::print: unknown loss type.");
|
||||
}
|
||||
std::cout << "maxIterations: " << maxIterations << "\n";
|
||||
std::cout << "barcSq: " << barcSq << "\n";
|
||||
|
|
@ -123,19 +134,22 @@ private:
|
|||
Vector weights_; // this could be a local variable in optimize, but it is useful to make it accessible from outside
|
||||
|
||||
public:
|
||||
GncOptimizer(const NonlinearFactorGraph& graph,
|
||||
const Values& initialValues, const GncParameters& params = GncParameters()) :
|
||||
GncOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues,
|
||||
const GncParameters& params = GncParameters()) :
|
||||
state_(initialValues), params_(params) {
|
||||
|
||||
// make sure all noiseModels are Gaussian or convert to Gaussian
|
||||
nfg_.resize(graph.size());
|
||||
for (size_t i = 0; i < graph.size(); i++) {
|
||||
if (graph[i]) {
|
||||
NoiseModelFactor::shared_ptr factor = boost::dynamic_pointer_cast<NoiseModelFactor>(graph[i]);
|
||||
noiseModel::Robust::shared_ptr robust = boost::dynamic_pointer_cast<noiseModel::Robust>(factor->noiseModel());
|
||||
NoiseModelFactor::shared_ptr factor = boost::dynamic_pointer_cast<
|
||||
NoiseModelFactor>(graph[i]);
|
||||
noiseModel::Robust::shared_ptr robust = boost::dynamic_pointer_cast<
|
||||
noiseModel::Robust>(factor->noiseModel());
|
||||
if (robust) { // if the factor has a robust loss, we have to change it:
|
||||
SharedNoiseModel gaussianNoise = robust->noise();
|
||||
NoiseModelFactor::shared_ptr gaussianFactor = factor->cloneWithNewNoiseModel(gaussianNoise);
|
||||
NoiseModelFactor::shared_ptr gaussianFactor =
|
||||
factor->cloneWithNewNoiseModel(gaussianNoise);
|
||||
nfg_[i] = gaussianFactor;
|
||||
} else { // else we directly push it back
|
||||
nfg_[i] = factor;
|
||||
|
|
@ -145,10 +159,18 @@ public:
|
|||
}
|
||||
|
||||
/// getter functions
|
||||
NonlinearFactorGraph getFactors() const { return NonlinearFactorGraph(nfg_); }
|
||||
Values getState() const { return Values(state_); }
|
||||
GncParameters getParams() const { return GncParameters(params_); }
|
||||
Vector getWeights() const {return weights_;}
|
||||
NonlinearFactorGraph getFactors() const {
|
||||
return NonlinearFactorGraph(nfg_);
|
||||
}
|
||||
Values getState() const {
|
||||
return Values(state_);
|
||||
}
|
||||
GncParameters getParams() const {
|
||||
return GncParameters(params_);
|
||||
}
|
||||
Vector getWeights() const {
|
||||
return weights_;
|
||||
}
|
||||
|
||||
/// implement GNC main loop, including graduating nonconvexity with mu
|
||||
Values optimize() {
|
||||
|
|
@ -238,11 +260,15 @@ public:
|
|||
newGraph.resize(nfg_.size());
|
||||
for (size_t i = 0; i < nfg_.size(); i++) {
|
||||
if (nfg_[i]) {
|
||||
NoiseModelFactor::shared_ptr factor = boost::dynamic_pointer_cast<NoiseModelFactor>(nfg_[i]);
|
||||
noiseModel::Gaussian::shared_ptr noiseModel = boost::dynamic_pointer_cast<noiseModel::Gaussian>(factor->noiseModel());
|
||||
NoiseModelFactor::shared_ptr factor = boost::dynamic_pointer_cast<
|
||||
NoiseModelFactor>(nfg_[i]);
|
||||
noiseModel::Gaussian::shared_ptr noiseModel =
|
||||
boost::dynamic_pointer_cast<noiseModel::Gaussian>(
|
||||
factor->noiseModel());
|
||||
if (noiseModel) {
|
||||
Matrix newInfo = weights[i] * noiseModel->information();
|
||||
SharedNoiseModel newNoiseModel = noiseModel::Gaussian::Information(newInfo);
|
||||
SharedNoiseModel newNoiseModel = noiseModel::Gaussian::Information(
|
||||
newInfo);
|
||||
newGraph[i] = factor->cloneWithNewNoiseModel(newNoiseModel);
|
||||
} else {
|
||||
throw std::runtime_error(
|
||||
|
|
@ -259,7 +285,9 @@ public:
|
|||
|
||||
// do not update the weights that the user has decided are known inliers
|
||||
std::vector<size_t> allWeights;
|
||||
for (size_t k = 0; k < nfg_.size(); k++) {allWeights.push_back(k);}
|
||||
for (size_t k = 0; k < nfg_.size(); k++) {
|
||||
allWeights.push_back(k);
|
||||
}
|
||||
std::vector<size_t> unknownWeights;
|
||||
std::set_difference(allWeights.begin(), allWeights.end(),
|
||||
params_.knownInliers.begin(), params_.knownInliers.end(),
|
||||
|
|
@ -271,7 +299,8 @@ public:
|
|||
for (size_t k : unknownWeights) {
|
||||
if (nfg_[k]) {
|
||||
double u2_k = nfg_[k]->error(currentEstimate); // squared (and whitened) residual
|
||||
weights[k] = std::pow( ( mu*params_.barcSq )/( u2_k + mu*params_.barcSq ) , 2);
|
||||
weights[k] = std::pow(
|
||||
(mu * params_.barcSq) / (u2_k + mu * params_.barcSq), 2);
|
||||
}
|
||||
}
|
||||
return weights;
|
||||
|
|
@ -284,7 +313,6 @@ public:
|
|||
|
||||
/* ************************************************************************* */
|
||||
TEST(GncOptimizer, gncParamsConstructor) {
|
||||
|
||||
//check params are correctly parsed
|
||||
LevenbergMarquardtParams lmParams;
|
||||
GncParams<LevenbergMarquardtParams> gncParams1(lmParams);
|
||||
|
|
@ -324,7 +352,8 @@ TEST(GncOptimizer, gncConstructor) {
|
|||
|
||||
LevenbergMarquardtParams lmParams;
|
||||
GncParams<LevenbergMarquardtParams> gncParams(lmParams);
|
||||
auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial, gncParams);
|
||||
auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial,
|
||||
gncParams);
|
||||
|
||||
CHECK(gnc.getFactors().equals(fg));
|
||||
CHECK(gnc.getState().equals(initial));
|
||||
|
|
@ -333,7 +362,6 @@ TEST(GncOptimizer, gncConstructor) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
TEST(GncOptimizer, gncConstructorWithRobustGraphAsInput) {
|
||||
// simple graph with Gaussian noise model
|
||||
auto fg = example::sharedNonRobustFactorGraphWithOutliers();
|
||||
// same graph with robust noise model
|
||||
auto fg_robust = example::sharedRobustFactorGraphWithOutliers();
|
||||
|
|
@ -344,7 +372,8 @@ TEST(GncOptimizer, gncConstructorWithRobustGraphAsInput) {
|
|||
|
||||
LevenbergMarquardtParams lmParams;
|
||||
GncParams<LevenbergMarquardtParams> gncParams(lmParams);
|
||||
auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg_robust, initial, gncParams);
|
||||
auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg_robust,
|
||||
initial, gncParams);
|
||||
|
||||
// make sure that when parsing the graph is transformed into one without robust loss
|
||||
CHECK(fg.equals(gnc.getFactors()));
|
||||
|
|
@ -352,7 +381,6 @@ TEST(GncOptimizer, gncConstructorWithRobustGraphAsInput) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
TEST(GncOptimizer, initializeMu) {
|
||||
// has to have Gaussian noise models !
|
||||
auto fg = example::createReallyNonlinearFactorGraph();
|
||||
|
||||
Point2 p0(3, 3);
|
||||
|
|
@ -361,8 +389,10 @@ TEST(GncOptimizer, initializeMu) {
|
|||
|
||||
LevenbergMarquardtParams lmParams;
|
||||
GncParams<LevenbergMarquardtParams> gncParams(lmParams);
|
||||
gncParams.setLossType(GncParams<LevenbergMarquardtParams>::RobustLossType::GM);
|
||||
auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial, gncParams);
|
||||
gncParams.setLossType(
|
||||
GncParams<LevenbergMarquardtParams>::RobustLossType::GM);
|
||||
auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial,
|
||||
gncParams);
|
||||
EXPECT_DOUBLES_EQUAL(gnc.initializeMu(), 2 * 198.999, 1e-3); // according to rmk 5 in the gnc paper: m0 = 2 rmax^2 / barcSq (barcSq=1 in this example)
|
||||
}
|
||||
|
||||
|
|
@ -377,8 +407,10 @@ TEST(GncOptimizer, updateMu) {
|
|||
|
||||
LevenbergMarquardtParams lmParams;
|
||||
GncParams<LevenbergMarquardtParams> gncParams(lmParams);
|
||||
gncParams.setLossType(GncParams<LevenbergMarquardtParams>::RobustLossType::GM);
|
||||
auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial, gncParams);
|
||||
gncParams.setLossType(
|
||||
GncParams<LevenbergMarquardtParams>::RobustLossType::GM);
|
||||
auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial,
|
||||
gncParams);
|
||||
|
||||
double mu = 5.0;
|
||||
EXPECT_DOUBLES_EQUAL(gnc.updateMu(mu), mu / 1.4, tol);
|
||||
|
|
@ -399,8 +431,10 @@ TEST(GncOptimizer, checkMuConvergence) {
|
|||
|
||||
LevenbergMarquardtParams lmParams;
|
||||
GncParams<LevenbergMarquardtParams> gncParams(lmParams);
|
||||
gncParams.setLossType(GncParams<LevenbergMarquardtParams>::RobustLossType::GM);
|
||||
auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial, gncParams);
|
||||
gncParams.setLossType(
|
||||
GncParams<LevenbergMarquardtParams>::RobustLossType::GM);
|
||||
auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial,
|
||||
gncParams);
|
||||
|
||||
double mu = 1.0;
|
||||
CHECK(gnc.checkMuConvergence(mu));
|
||||
|
|
@ -408,7 +442,6 @@ TEST(GncOptimizer, checkMuConvergence) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
TEST(GncOptimizer, calculateWeights) {
|
||||
// has to have Gaussian noise models !
|
||||
auto fg = example::sharedNonRobustFactorGraphWithOutliers();
|
||||
|
||||
Point2 p0(0, 0);
|
||||
|
|
@ -433,7 +466,8 @@ TEST(GncOptimizer, calculateWeights) {
|
|||
double barcSq = 5.0;
|
||||
weights_expected[3] = std::pow(mu * barcSq / (50.0 + mu * barcSq), 2); // outlier, error = 50
|
||||
gncParams.setInlierThreshold(barcSq);
|
||||
auto gnc2 = GncOptimizer<GncParams<GaussNewtonParams>>(fg, initial, gncParams);
|
||||
auto gnc2 = GncOptimizer<GncParams<GaussNewtonParams>>(fg, initial,
|
||||
gncParams);
|
||||
weights_actual = gnc2.calculateWeights(initial, mu);
|
||||
CHECK(assert_equal(weights_expected, weights_actual, tol));
|
||||
}
|
||||
|
|
@ -442,11 +476,13 @@ TEST(GncOptimizer, calculateWeights) {
|
|||
TEST(GncOptimizer, makeWeightedGraph) {
|
||||
// create original factor
|
||||
double sigma1 = 0.1;
|
||||
NonlinearFactorGraph nfg = example::nonlinearFactorGraphWithGivenSigma(sigma1);
|
||||
NonlinearFactorGraph nfg = example::nonlinearFactorGraphWithGivenSigma(
|
||||
sigma1);
|
||||
|
||||
// create expected
|
||||
double sigma2 = 10;
|
||||
NonlinearFactorGraph expected = example::nonlinearFactorGraphWithGivenSigma(sigma2);
|
||||
NonlinearFactorGraph expected = example::nonlinearFactorGraphWithGivenSigma(
|
||||
sigma2);
|
||||
|
||||
// create weights
|
||||
Vector weights = Vector::Ones(1); // original info:1/0.1^2 = 100. New info: 1/10^2 = 0.01. Ratio is 10-4
|
||||
|
|
@ -459,7 +495,8 @@ TEST(GncOptimizer, makeWeightedGraph) {
|
|||
|
||||
LevenbergMarquardtParams lmParams;
|
||||
GncParams<LevenbergMarquardtParams> gncParams(lmParams);
|
||||
auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(nfg, initial, gncParams);
|
||||
auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(nfg, initial,
|
||||
gncParams);
|
||||
NonlinearFactorGraph actual = gnc.makeWeightedGraph(weights);
|
||||
|
||||
// check it's all good
|
||||
|
|
@ -468,7 +505,6 @@ TEST(GncOptimizer, makeWeightedGraph) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
TEST(GncOptimizer, optimizeSimple) {
|
||||
// has to have Gaussian noise models !
|
||||
auto fg = example::createReallyNonlinearFactorGraph();
|
||||
|
||||
Point2 p0(3, 3);
|
||||
|
|
@ -477,7 +513,8 @@ TEST(GncOptimizer, optimizeSimple) {
|
|||
|
||||
LevenbergMarquardtParams lmParams;
|
||||
GncParams<LevenbergMarquardtParams> gncParams(lmParams);
|
||||
auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial, gncParams);
|
||||
auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial,
|
||||
gncParams);
|
||||
|
||||
Values actual = gnc.optimize();
|
||||
DOUBLES_EQUAL(0, fg.error(actual), tol);
|
||||
|
|
@ -485,7 +522,6 @@ TEST(GncOptimizer, optimizeSimple) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
TEST(GncOptimizer, optimize) {
|
||||
// has to have Gaussian noise models !
|
||||
auto fg = example::sharedNonRobustFactorGraphWithOutliers();
|
||||
|
||||
Point2 p0(1, 0);
|
||||
|
|
@ -516,7 +552,6 @@ TEST(GncOptimizer, optimize) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
TEST(GncOptimizer, optimizeWithKnownInliers) {
|
||||
// has to have Gaussian noise models !
|
||||
auto fg = example::sharedNonRobustFactorGraphWithOutliers();
|
||||
|
||||
Point2 p0(1, 0);
|
||||
|
|
|
|||
Loading…
Reference in New Issue