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

working on make graph

release/4.3a0
lcarlone 2020-11-27 18:28:31 -05:00
parent 5db6894b66
commit 7ce0641b43
1 changed files with 10 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
tests/testGncOptimizer.cpp
View File

@ -112,14 +112,18 @@ private:
public: public:
GncOptimizer(const NonlinearFactorGraph& graph, GncOptimizer(const NonlinearFactorGraph& graph,
const Values& initialValues, const GncParameters& params = GncParameters()) : 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 // make sure all noiseModels are Gaussian or convert to Gaussian
for (size_t i = 0; i < nfg_.size(); i++) { for (size_t i = 0; i < graph.size(); i++) {
if(nfg_[i]){ if(graph[i]){
// NonlinearFactor factor = nfg_[i]->clone(); auto &factor = boost::dynamic_pointer_cast<NoiseModelFactor>(nfg_[i]);
nfg_[i]-> auto &robust = boost::dynamic_pointer_cast<noiseModel::Robust>(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);
}
} }
} }
} }