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

new constructor test which gets rid of robust loss now passes!

release/4.3a0
lcarlone 2020-11-27 19:00:08 -05:00
parent 7ce0641b43
commit 556fa83e9f
1 changed files with 12 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
tests/testGncOptimizer.cpp
View File

@ -115,14 +115,17 @@ public:
state_(initialValues), params_(params) { state_(initialValues), params_(params) {
// make sure all noiseModels are Gaussian or convert to Gaussian // make sure all noiseModels are Gaussian or convert to Gaussian
nfg_.resize(graph.size());
for (size_t i = 0; i < graph.size(); i++) { for (size_t i = 0; i < graph.size(); i++) {
if(graph[i]){ if(graph[i]){
auto &factor = boost::dynamic_pointer_cast<NoiseModelFactor>(nfg_[i]); NoiseModelFactor::shared_ptr factor = boost::dynamic_pointer_cast<NoiseModelFactor>(graph[i]);
auto &robust = boost::dynamic_pointer_cast<noiseModel::Robust>(factor->noiseModel()); 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: if(robust){ // if the factor has a robust loss, we have to change it:
nfg_.push_back(factor->cloneWithNewNoiseModel(factor->noiseModel())); SharedNoiseModel gaussianNoise = robust->noise();
}{ // else we directly push it back NoiseModelFactor::shared_ptr gaussianFactor = factor->cloneWithNewNoiseModel(gaussianNoise);
nfg_.push_back(factor); nfg_[i] = gaussianFactor;
} else{ // else we directly push it back
nfg_[i] = factor;
} }
} }
} }
@ -274,7 +277,7 @@ TEST(GncOptimizer, gncConstructor) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(GncOptimizer, gncConstructorWithRobustGraphAsInput) { TEST(GncOptimizer, gncConstructorWithRobustGraphAsInput) {
// simple graph with Gaussian noise model // simple graph with Gaussian noise model
auto fg = example::createReallyNonlinearFactorGraph(); auto fg = example::sharedNonRobustFactorGraphWithOutliers();
// same graph with robust noise model // same graph with robust noise model
auto fg_robust = example::sharedRobustFactorGraphWithOutliers(); auto fg_robust = example::sharedRobustFactorGraphWithOutliers();
@ -285,7 +288,9 @@ TEST(GncOptimizer, gncConstructorWithRobustGraphAsInput) {
LevenbergMarquardtParams lmParams; LevenbergMarquardtParams lmParams;
GncParams<LevenbergMarquardtParams> gncParams(lmParams); GncParams<LevenbergMarquardtParams> gncParams(lmParams);
auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg_robust, initial, gncParams); auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg_robust, initial, gncParams);
// fg.print("fg\n");
// fg_robust.print("fg_robust\n");
// gnc.getFactors().print("gnc\n");
// make sure that when parsing the graph is transformed into one without robust loss // make sure that when parsing the graph is transformed into one without robust loss
CHECK( fg.equals(gnc.getFactors()) ); CHECK( fg.equals(gnc.getFactors()) );
} }