116 lines
3.3 KiB
C++
116 lines
3.3 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 Jignnan Shi
|
|
* @author Luca Carlone
|
|
* @author Frank Dellaert
|
|
*/
|
|
|
|
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
|
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
|
#include <tests/smallExample.h>
|
|
|
|
/* ************************************************************************* */
|
|
template <class BaseOptimizerParameters>
|
|
class GncParams {
|
|
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();
|
|
|
|
Point2 p0(3, 3);
|
|
Values initial;
|
|
initial.insert(X(1), p0);
|
|
|
|
LevenbergMarquardtParams lmParams;
|
|
GncParams 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);
|
|
}
|
|
/* ************************************************************************* */
|