125 lines
3.6 KiB
C++
125 lines
3.6 KiB
C++
/* ----------------------------------------------------------------------------
|
|
|
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
|
* Atlanta, Georgia 30332-0415
|
|
* All Rights Reserved
|
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
|
|
|
* See LICENSE for the license information
|
|
|
|
* -------------------------------------------------------------------------- */
|
|
|
|
/**
|
|
* @file testGncOptimizer.cpp
|
|
* @brief Unit tests for GncOptimizer class
|
|
* @author Jingnan Shi
|
|
* @author Luca Carlone
|
|
* @author Frank Dellaert
|
|
*/
|
|
|
|
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
|
#include <gtsam/nonlinear/NonlinearFactorGraph.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>
|
|
class GncParams {
|
|
public:
|
|
|
|
// using BaseOptimizer = BaseOptimizerParameters::OptimizerType;
|
|
GncParams(const BaseOptimizerParameters& baseOptimizerParams): baseOptimizerParams(baseOptimizerParams) {}
|
|
|
|
BaseOptimizerParameters baseOptimizerParams;
|
|
|
|
/// any other specific GNC parameters:
|
|
};
|
|
|
|
/* ************************************************************************* */
|
|
//template <class GncParameters>
|
|
//class GncOptimizer {
|
|
// public:
|
|
// // 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
|
|
//
|
|
// }
|
|
//};
|
|
|
|
///* ************************************************************************* */
|
|
//TEST(GncOptimizer, calculateWeights) {
|
|
//}
|
|
//
|
|
///* ************************************************************************* */
|
|
//TEST(GncOptimizer, copyGraph) {
|
|
//}
|
|
|
|
/* ************************************************************************* */
|
|
TEST(GncOptimizer, makeGraph) {
|
|
// has to have Gaussian noise models !
|
|
auto fg = example::createReallyNonlinearFactorGraph(); // just a unary factor on a 2D point
|
|
|
|
Point2 p0(3, 3);
|
|
Values initial;
|
|
initial.insert(X(1), p0);
|
|
|
|
LevenbergMarquardtParams lmParams;
|
|
GncParams<LevenbergMarquardtParams> gncParams(lmParams);
|
|
// auto gnc = GncOptimizer(fg, initial, gncParams);
|
|
|
|
// NonlinearFactorGraph actual = gnc.makeGraph(initial);
|
|
}
|
|
|
|
/* ************************************************************************* *
|
|
TEST(GncOptimizer, optimize) {
|
|
// has to have Gaussian noise models !
|
|
auto fg = example::createReallyNonlinearFactorGraph();
|
|
|
|
Point2 p0(3, 3);
|
|
Values initial;
|
|
initial.insert(X(1), p0);
|
|
|
|
LevenbergMarquardtParams lmParams;
|
|
GncParams gncParams(lmParams);
|
|
auto gnc = GncOptimizer(fg, initial, gncParams);
|
|
Values actual = gnc.optimize();
|
|
DOUBLES_EQUAL(0, fg.error(actual2), tol);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
int main() {
|
|
TestResult tr;
|
|
return TestRegistry::runAllTests(tr);
|
|
}
|
|
/* ************************************************************************* */
|