From 7ce0641b4382c2d97cd8cd181a577bb5f3ad841d Mon Sep 17 00:00:00 2001 From: lcarlone Date: Fri, 27 Nov 2020 18:28:31 -0500 Subject: [PATCH] working on make graph --- tests/testGncOptimizer.cpp | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp index a2dcafc81..e80ad7974 100644 --- a/tests/testGncOptimizer.cpp +++ b/tests/testGncOptimizer.cpp @@ -112,14 +112,18 @@ private: public: GncOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues, const GncParameters& params = GncParameters()) : - nfg_(graph), state_(initialValues), params_(params) { + state_(initialValues), params_(params) { // make sure all noiseModels are Gaussian or convert to Gaussian - for (size_t i = 0; i < nfg_.size(); i++) { - if(nfg_[i]){ - // NonlinearFactor factor = nfg_[i]->clone(); - nfg_[i]-> - + for (size_t i = 0; i < graph.size(); i++) { + if(graph[i]){ + auto &factor = boost::dynamic_pointer_cast(nfg_[i]); + auto &robust = boost::dynamic_pointer_cast(factor->noiseModel()); + if(robust){ // if the factor has a robust loss, we have to change it: + nfg_.push_back(factor->cloneWithNewNoiseModel(factor->noiseModel())); + }{ // else we directly push it back + nfg_.push_back(factor); + } } } }