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

starting to create test and code for gncParams

release/4.3a0
lcarlone 2020-11-25 20:11:04 -05:00
parent 7e29944f95
commit b5d06b5878
1 changed files with 57 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

105
tests/testGncOptimizer.cpp
View File

@ -12,7 +12,7 @@
/** /**
* @file testGncOptimizer.cpp * @file testGncOptimizer.cpp
* @brief Unit tests for GncOptimizer class * @brief Unit tests for GncOptimizer class
* @author Jignnan Shi * @author Jingnan Shi
* @author Luca Carlone * @author Luca Carlone
* @author Frank Dellaert * @author Frank Dellaert
*/ */
@ -21,12 +21,21 @@
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <tests/smallExample.h> #include <tests/smallExample.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
using symbol_shorthand::X;
using symbol_shorthand::L;
/* ************************************************************************* */ /* ************************************************************************* */
template <class BaseOptimizerParameters> template <class BaseOptimizerParameters>
class GncParams { class GncParams {
using BaseOptimizer = BaseOptimizerParameters::OptimizerType; public:
GncParams(const BaseOptimizerParameters& baseOptimizerParams)
: baseOptimizerParams(baseOptimizerParams) {} // using BaseOptimizer = BaseOptimizerParameters::OptimizerType;
GncParams(const BaseOptimizerParameters& baseOptimizerParams): baseOptimizerParams(baseOptimizerParams) {}
BaseOptimizerParameters baseOptimizerParams; BaseOptimizerParameters baseOptimizerParams;
@ -34,64 +43,64 @@ class GncParams {
}; };
/* ************************************************************************* */ /* ************************************************************************* */
template <class GncParameters> //template <class GncParameters>
class GncOptimizer { //class GncOptimizer {
public: // public:
// types etc // // types etc
//
// private:
// FG INITIAL GncParameters params_;
//
// public:
// GncOptimizer(FG, INITIAL, const GncParameters& params) : params(params) {
// // Check that all noise models are Gaussian
// }
//
// Values optimize() const {
// NonlinearFactorGraph currentGraph = graph_;
// for (i : {1, 2, 3}) {
// BaseOptimizer::Optimizer baseOptimizer(currentGraph, initial);
// VALUES currentSolution = baseOptimizer.optimize();
// if (converged) {
// return currentSolution;
// }
// graph_i = this->makeGraph(currentSolution);
// }
// }
//
// NonlinearFactorGraph makeGraph(const Values& currentSolution) const {
// // calculate some weights
// this->calculateWeights();
// // copy the graph with new weights
//
// }
//};
private: ///* ************************************************************************* */
FG INITIAL GncParameters params_; //TEST(GncOptimizer, calculateWeights) {
//}
public: //
GncOptimizer(FG, INITIAL, const GncParameters& params) : params(params) { ///* ************************************************************************* */
// Check that all noise models are Gaussian //TEST(GncOptimizer, copyGraph) {
} //}
Values optimize() const {
NonlinearFactorGraph currentGraph = graph_;
for (i : {1, 2, 3}) {
BaseOptimizer::Optimizer baseOptimizer(currentGraph, initial);
VALUES currentSolution = baseOptimizer.optimize();
if (converged) {
return currentSolution;
}
graph_i = this->makeGraph(currentSolution);
}
}
NonlinearFactorGraph makeGraph(const Values& currentSolution) const {
// calculate some weights
this->calculateWeights();
// copy the graph with new weights
}
};
/* ************************************************************************* */
TEST(GncOptimizer, calculateWeights) {
}
/* ************************************************************************* */
TEST(GncOptimizer, copyGraph) {
}
/* ************************************************************************* */ /* ************************************************************************* */
TEST(GncOptimizer, makeGraph) { TEST(GncOptimizer, makeGraph) {
// has to have Gaussian noise models ! // has to have Gaussian noise models !
auto fg = example::createReallyNonlinearFactorGraph(); auto fg = example::createReallyNonlinearFactorGraph(); // just a unary factor on a 2D point
Point2 p0(3, 3); Point2 p0(3, 3);
Values initial; Values initial;
initial.insert(X(1), p0); initial.insert(X(1), p0);
LevenbergMarquardtParams lmParams; LevenbergMarquardtParams lmParams;
GncParams gncParams(lmParams); GncParams<LevenbergMarquardtParams> gncParams(lmParams);
auto gnc = GncOptimizer(fg, initial, gncParams); // auto gnc = GncOptimizer(fg, initial, gncParams);
NonlinearFactorGraph actual = gnc.makeGraph(initial); // NonlinearFactorGraph actual = gnc.makeGraph(initial);
} }
/* ************************************************************************* */ /* ************************************************************************* *
TEST(GncOptimizer, optimize) { TEST(GncOptimizer, optimize) {
// has to have Gaussian noise models ! // has to have Gaussian noise models !
auto fg = example::createReallyNonlinearFactorGraph(); auto fg = example::createReallyNonlinearFactorGraph();