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

yay! only the final monster to go!

release/4.3a0
lcarlone 2020-11-27 19:29:42 -05:00
parent 556fa83e9f
commit 9e3263f2b1
1 changed files with 45 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

64
tests/testGncOptimizer.cpp
View File

@ -147,7 +147,7 @@ public:
weights = calculateWeights(result, mu); weights = calculateWeights(result, mu);
// variable/values update // variable/values update
NonlinearFactorGraph graph_iter = this->makeGraph(weights); NonlinearFactorGraph graph_iter = this->makeWeightedGraph(weights);
GaussNewtonOptimizer baseOptimizer_iter(graph_iter, state_); GaussNewtonOptimizer baseOptimizer_iter(graph_iter, state_);
Values result = baseOptimizer.optimize(); Values result = baseOptimizer.optimize();
@ -202,8 +202,25 @@ public:
} }
/// create a graph where each factor is weighted by the gnc weights /// create a graph where each factor is weighted by the gnc weights
NonlinearFactorGraph makeGraph(const Vector& weights) const { NonlinearFactorGraph makeWeightedGraph(const Vector& weights) const {
return NonlinearFactorGraph(nfg_); // make sure all noiseModels are Gaussian or convert to Gaussian
NonlinearFactorGraph newGraph;
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());
if(noiseModel){
Matrix newInfo = weights[i] * noiseModel->information();
SharedNoiseModel newNoiseModel = noiseModel::Gaussian::Information(newInfo);
newGraph[i] = factor->cloneWithNewNoiseModel(newNoiseModel);
}else{
throw std::runtime_error(
"GncOptimizer::makeWeightedGraph: unexpected non-Gaussian noise model.");
}
}
}
return newGraph;
} }
/// calculate gnc weights /// calculate gnc weights
@ -384,22 +401,31 @@ TEST(GncOptimizer, calculateWeights) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(GncOptimizer, makeGraph) { TEST(GncOptimizer, makeWeightedGraph) {
// // simple graph with Gaussian noise model // create original factor
// auto fg = example::createReallyNonlinearFactorGraph(); double sigma1 = 0.1;
// // same graph with robust noise model NonlinearFactorGraph nfg = example::nonlinearFactorGraphWithGivenSigma(sigma1);
// auto fg_robust = example::sharedRobustFactorGraphWithOutliers();
// // create expected
// Point2 p0(3, 3); double sigma2 = 10;
// Values initial; NonlinearFactorGraph expected = example::nonlinearFactorGraphWithGivenSigma(sigma2);
// initial.insert(X(1), p0);
// // create weights
// LevenbergMarquardtParams lmParams; Vector weights = Vector::Ones(1); // original info:1/0.1^2 = 100. New info: 1/10^2 = 0.01. Ratio is 10-4
// GncParams<LevenbergMarquardtParams> gncParams(lmParams); weights[0] = 1e-4;
// auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg_robust, initial, gncParams);
// // create actual
// // make sure that when parsing the graph is transformed into one without robust loss Point2 p0(3, 3);
// CHECK( fg.equals(gnc.getFactors()) ); Values initial;
initial.insert(X(1), p0);
LevenbergMarquardtParams lmParams;
GncParams<LevenbergMarquardtParams> gncParams(lmParams);
auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(nfg, initial, gncParams);
NonlinearFactorGraph actual = gnc.makeWeightedGraph(weights);
// check it's all good
CHECK(assert_equal(expected, actual));
} }
/* ************************************************************************* */ /* ************************************************************************* */