move ordering into the solver, and the nonlinear optimizer is now exact <G, T, L>
Kai Ni
parent
03ec3e3e62
commit
3bf15333af
|
|
@ -228,17 +228,18 @@ namespace gtsam {
|
|||
*/
|
||||
template <class NonlinearGraph, class Config>
|
||||
class Factorization {
|
||||
public:
|
||||
Factorization() {}
|
||||
private:
|
||||
boost::shared_ptr<const Ordering> ordering_;
|
||||
|
||||
Factorization(const NonlinearGraph& g, const Config& config) {}
|
||||
public:
|
||||
Factorization(boost::shared_ptr<const Ordering> ordering) : ordering_(ordering) {}
|
||||
|
||||
/**
|
||||
* solve for the optimal displacement in the tangent space, and then solve
|
||||
* the resulted linear system
|
||||
*/
|
||||
VectorConfig optimize(GaussianFactorGraph& fg, const Ordering& ordering) const {
|
||||
return fg.optimize(ordering);
|
||||
VectorConfig optimize(GaussianFactorGraph& fg) const {
|
||||
return fg.optimize(*ordering_);
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -48,8 +48,8 @@ namespace gtsam {
|
|||
/* ************************************************************************* */
|
||||
template<class G, class C, class L, class S>
|
||||
NonlinearOptimizer<G, C, L, S>::NonlinearOptimizer(shared_graph graph,
|
||||
shared_ordering ordering, shared_config config, shared_solver solver, double lambda) :
|
||||
graph_(graph), ordering_(ordering), config_(config), error_(graph->error(
|
||||
shared_config config, shared_solver solver, double lambda) :
|
||||
graph_(graph), config_(config), error_(graph->error(
|
||||
*config)), lambda_(lambda), solver_(solver) {
|
||||
}
|
||||
|
||||
|
|
@ -59,7 +59,7 @@ namespace gtsam {
|
|||
template<class G, class C, class L, class S>
|
||||
VectorConfig NonlinearOptimizer<G, C, L, S>::linearizeAndOptimizeForDelta() const {
|
||||
L linearized = solver_->linearize(*graph_, *config_);
|
||||
return solver_->optimize(linearized, *ordering_);
|
||||
return solver_->optimize(linearized);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -82,7 +82,7 @@ namespace gtsam {
|
|||
if (verbosity >= CONFIG)
|
||||
newConfig->print("newConfig");
|
||||
|
||||
NonlinearOptimizer newOptimizer = NonlinearOptimizer(graph_, ordering_, newConfig, solver_);
|
||||
NonlinearOptimizer newOptimizer = NonlinearOptimizer(graph_, newConfig, solver_);
|
||||
|
||||
if (verbosity >= ERROR)
|
||||
cout << "error: " << newOptimizer.error_ << endl;
|
||||
|
|
@ -128,7 +128,7 @@ namespace gtsam {
|
|||
damped.print("damped");
|
||||
|
||||
// solve
|
||||
VectorConfig delta = solver_->optimize(damped, *ordering_);
|
||||
VectorConfig delta = solver_->optimize(damped);
|
||||
if (verbosity >= TRYDELTA)
|
||||
delta.print("delta");
|
||||
|
||||
|
|
@ -138,14 +138,14 @@ namespace gtsam {
|
|||
newConfig->print("config");
|
||||
|
||||
// create new optimization state with more adventurous lambda
|
||||
NonlinearOptimizer next(graph_, ordering_, newConfig, solver_, lambda_ / factor);
|
||||
NonlinearOptimizer next(graph_, newConfig, solver_, lambda_ / factor);
|
||||
|
||||
// if error decreased, return the new state
|
||||
if (next.error_ <= error_)
|
||||
return next;
|
||||
else {
|
||||
// TODO: can we avoid copying the config ?
|
||||
NonlinearOptimizer cautious(graph_, ordering_, config_, solver_, lambda_ * factor);
|
||||
NonlinearOptimizer cautious(graph_, config_, solver_, lambda_ * factor);
|
||||
return cautious.try_lambda(linear, verbosity, factor);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -33,8 +33,8 @@ namespace gtsam {
|
|||
// For performance reasons in recursion, we store configs in a shared_ptr
|
||||
typedef boost::shared_ptr<const T> shared_config;
|
||||
typedef boost::shared_ptr<const G> shared_graph;
|
||||
typedef boost::shared_ptr<const Ordering> shared_ordering;
|
||||
typedef boost::shared_ptr<const S> shared_solver;
|
||||
typedef const S solver;
|
||||
|
||||
enum verbosityLevel {
|
||||
SILENT,
|
||||
|
|
@ -51,10 +51,9 @@ namespace gtsam {
|
|||
|
||||
private:
|
||||
|
||||
// keep a reference to const versions of the graph and ordering
|
||||
// keep a reference to const version of the graph
|
||||
// These normally do not change
|
||||
const shared_graph graph_;
|
||||
const shared_ordering ordering_;
|
||||
|
||||
// keep a configuration and its error
|
||||
// These typically change once per iteration (in a functional way)
|
||||
|
|
@ -77,15 +76,14 @@ namespace gtsam {
|
|||
/**
|
||||
* Constructor
|
||||
*/
|
||||
NonlinearOptimizer(shared_graph graph, shared_ordering ordering,
|
||||
shared_config config, shared_solver solver = shared_solver(new S),
|
||||
NonlinearOptimizer(shared_graph graph, shared_config config, shared_solver solver,
|
||||
double lambda = 1e-5);
|
||||
|
||||
/**
|
||||
* Copy constructor
|
||||
*/
|
||||
NonlinearOptimizer(const NonlinearOptimizer<G, T, L, S> &optimizer) :
|
||||
graph_(optimizer.graph_), ordering_(optimizer.ordering_), config_(optimizer.config_),
|
||||
graph_(optimizer.graph_), config_(optimizer.config_),
|
||||
error_(optimizer.error_), lambda_(optimizer.lambda_), solver_(optimizer.solver_) {}
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -59,6 +59,8 @@ namespace gtsam {
|
|||
Point2 t() const { return t_; }
|
||||
Rot2 r() const { return r_; }
|
||||
|
||||
static inline size_t dim() { return 3; };
|
||||
|
||||
}; // Pose2
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -24,7 +24,7 @@ namespace gtsam {
|
|||
SubgraphPCG<G, T>::SubgraphPCG(const G& g, const T& theta0) :
|
||||
maxIterations_(100), verbose_(false), epsilon_(1e-4), epsilon_abs_(1e-5) {
|
||||
|
||||
// generate spanning tree and create ordering
|
||||
// generate spanning tree
|
||||
PredecessorMap<Key> tree = g.template findMinimumSpanningTree<Key, Constraint>();
|
||||
list<Key> keys = predecessorMap2Keys(tree);
|
||||
|
||||
|
|
@ -57,10 +57,10 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
template<class G, class T>
|
||||
VectorConfig SubgraphPCG<G, T>::optimize(SubgraphPreconditioner& system, const Ordering& ordering) const {
|
||||
VectorConfig SubgraphPCG<G, T>::optimize(SubgraphPreconditioner& system) const {
|
||||
//TODO: 3 is hard coded here
|
||||
VectorConfig zeros;
|
||||
BOOST_FOREACH(const Symbol& j, ordering) zeros.insert(j,zero(3));
|
||||
BOOST_FOREACH(const Symbol& j, *ordering_) zeros.insert(j,zero(Pose::dim()));
|
||||
|
||||
// Solve the subgraph PCG
|
||||
VectorConfig ybar = conjugateGradients<SubgraphPreconditioner, VectorConfig,
|
||||
|
|
|
|||
|
|
@ -90,9 +90,6 @@ namespace gtsam {
|
|||
G T_, C_;
|
||||
|
||||
public:
|
||||
// kai: this constructor is for compatible with Factorization
|
||||
SubgraphPCG() { throw std::runtime_error("SubgraphPCG: this constructor is only for compatibility!");}
|
||||
|
||||
SubgraphPCG(const G& g, const T& config);
|
||||
|
||||
boost::shared_ptr<Ordering> ordering() const { return ordering_; }
|
||||
|
|
@ -108,7 +105,7 @@ namespace gtsam {
|
|||
* solve for the optimal displacement in the tangent space, and then solve
|
||||
* the resulted linear system
|
||||
*/
|
||||
VectorConfig optimize(SubgraphPreconditioner& system, const Ordering& ordering) const;
|
||||
VectorConfig optimize(SubgraphPreconditioner& system) const;
|
||||
};
|
||||
} // nsamespace gtsam
|
||||
|
||||
|
|
|
|||
|
|
@ -35,152 +35,157 @@ using namespace example;
|
|||
typedef NonlinearOptimizer<Graph,Config> Optimizer;
|
||||
|
||||
/* ************************************************************************* */
|
||||
//TEST( NonlinearOptimizer, delta )
|
||||
//{
|
||||
// shared_ptr<Graph> fg(new Graph(
|
||||
// createNonlinearFactorGraph()));
|
||||
// Optimizer::shared_config initial = sharedNoisyConfig();
|
||||
//
|
||||
// // Expected configuration is the difference between the noisy config
|
||||
// // and the ground-truth config. One step only because it's linear !
|
||||
// VectorConfig expected;
|
||||
// Vector dl1(2);
|
||||
// dl1(0) = -0.1;
|
||||
// dl1(1) = 0.1;
|
||||
// expected.insert("l1", dl1);
|
||||
// Vector dx1(2);
|
||||
// dx1(0) = -0.1;
|
||||
// dx1(1) = -0.1;
|
||||
// expected.insert("x1", dx1);
|
||||
// Vector dx2(2);
|
||||
// dx2(0) = 0.1;
|
||||
// dx2(1) = -0.2;
|
||||
// expected.insert("x2", dx2);
|
||||
//
|
||||
// // Check one ordering
|
||||
// shared_ptr<Ordering> ord1(new Ordering());
|
||||
// *ord1 += "x2","l1","x1";
|
||||
// Optimizer optimizer1(fg, ord1, initial);
|
||||
// VectorConfig actual1 = optimizer1.linearizeAndOptimizeForDelta();
|
||||
// CHECK(assert_equal(actual1,expected));
|
||||
//
|
||||
// // Check another
|
||||
// shared_ptr<Ordering> ord2(new Ordering());
|
||||
// *ord2 += "x1","x2","l1";
|
||||
// Optimizer optimizer2(fg, ord2, initial);
|
||||
// VectorConfig actual2 = optimizer2.linearizeAndOptimizeForDelta();
|
||||
// CHECK(assert_equal(actual2,expected));
|
||||
//
|
||||
// // And yet another...
|
||||
// shared_ptr<Ordering> ord3(new Ordering());
|
||||
// *ord3 += "l1","x1","x2";
|
||||
// Optimizer optimizer3(fg, ord3, initial);
|
||||
// VectorConfig actual3 = optimizer3.linearizeAndOptimizeForDelta();
|
||||
// CHECK(assert_equal(actual3,expected));
|
||||
//}
|
||||
//
|
||||
///* ************************************************************************* */
|
||||
//TEST( NonlinearOptimizer, iterateLM )
|
||||
//{
|
||||
// // really non-linear factor graph
|
||||
// shared_ptr<Graph> fg(new Graph(
|
||||
// createReallyNonlinearFactorGraph()));
|
||||
//
|
||||
// // config far from minimum
|
||||
// Point2 x0(3,0);
|
||||
// boost::shared_ptr<Config> config(new Config);
|
||||
// config->insert(simulated2D::PoseKey(1), x0);
|
||||
//
|
||||
// // ordering
|
||||
// shared_ptr<Ordering> ord(new Ordering());
|
||||
// ord->push_back("x1");
|
||||
//
|
||||
// // create initial optimization state, with lambda=0
|
||||
// Optimizer::shared_solver solver(new Factorization<
|
||||
// Graph, Config> );
|
||||
// Optimizer optimizer(fg, ord, config, solver, 0.);
|
||||
//
|
||||
// // normal iterate
|
||||
// Optimizer iterated1 = optimizer.iterate();
|
||||
//
|
||||
// // LM iterate with lambda 0 should be the same
|
||||
// Optimizer iterated2 = optimizer.iterateLM();
|
||||
//
|
||||
// // Try successive iterates. TODO: ugly pointers, better way ?
|
||||
// Optimizer *pointer = new Optimizer(iterated2);
|
||||
// for (int i=0;i<10;i++) {
|
||||
// Optimizer* newOptimizer = new Optimizer(pointer->iterateLM());
|
||||
// delete pointer;
|
||||
// pointer = newOptimizer;
|
||||
// }
|
||||
// delete(pointer);
|
||||
//
|
||||
// CHECK(assert_equal(*iterated1.config(), *iterated2.config(), 1e-9));
|
||||
//}
|
||||
//
|
||||
///* ************************************************************************* */
|
||||
//TEST( NonlinearOptimizer, optimize )
|
||||
//{
|
||||
// shared_ptr<Graph> fg(new Graph(
|
||||
// createReallyNonlinearFactorGraph()));
|
||||
//
|
||||
// // test error at minimum
|
||||
// Point2 xstar(0,0);
|
||||
// Config cstar;
|
||||
// cstar.insert(simulated2D::PoseKey(1), xstar);
|
||||
// DOUBLES_EQUAL(0.0,fg->error(cstar),0.0);
|
||||
//
|
||||
// // test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 =
|
||||
// Point2 x0(3,3);
|
||||
// boost::shared_ptr<Config> c0(new Config);
|
||||
// c0->insert(simulated2D::PoseKey(1), x0);
|
||||
// DOUBLES_EQUAL(199.0,fg->error(*c0),1e-3);
|
||||
//
|
||||
// // optimize parameters
|
||||
// shared_ptr<Ordering> ord(new Ordering());
|
||||
// ord->push_back("x1");
|
||||
// double relativeThreshold = 1e-5;
|
||||
// double absoluteThreshold = 1e-5;
|
||||
//
|
||||
// // initial optimization state is the same in both cases tested
|
||||
// Optimizer optimizer(fg, ord, c0);
|
||||
//
|
||||
// // Gauss-Newton
|
||||
// Optimizer actual1 = optimizer.gaussNewton(relativeThreshold,
|
||||
// absoluteThreshold);
|
||||
// DOUBLES_EQUAL(0,fg->error(*(actual1.config())),1e-3);
|
||||
//
|
||||
// // Levenberg-Marquardt
|
||||
// Optimizer actual2 = optimizer.levenbergMarquardt(relativeThreshold,
|
||||
// absoluteThreshold, Optimizer::SILENT);
|
||||
// DOUBLES_EQUAL(0,fg->error(*(actual2.config())),1e-3);
|
||||
//}
|
||||
//
|
||||
///* ************************************************************************* */
|
||||
//TEST( NonlinearOptimizer, Factorization )
|
||||
//{
|
||||
// typedef NonlinearOptimizer<Pose2Graph, Pose2Config, GaussianFactorGraph, Factorization<Pose2Graph, Pose2Config> > Optimizer;
|
||||
//
|
||||
// boost::shared_ptr<Pose2Config> config(new Pose2Config);
|
||||
// config->insert(1, Pose2(0.,0.,0.));
|
||||
// config->insert(2, Pose2(1.5,0.,0.));
|
||||
//
|
||||
// boost::shared_ptr<Pose2Graph> graph(new Pose2Graph);
|
||||
// graph->addPrior(1, Pose2(0.,0.,0.), Isotropic::Sigma(3, 1e-10));
|
||||
// graph->addConstraint(1,2, Pose2(1.,0.,0.), Isotropic::Sigma(3, 1));
|
||||
//
|
||||
// boost::shared_ptr<Ordering> ordering(new Ordering);
|
||||
// ordering->push_back(Pose2Config::Key(1));
|
||||
// ordering->push_back(Pose2Config::Key(2));
|
||||
//
|
||||
// Optimizer optimizer(graph, ordering, config);
|
||||
// Optimizer optimized = optimizer.iterateLM();
|
||||
//
|
||||
// Pose2Config expected;
|
||||
// expected.insert(1, Pose2(0.,0.,0.));
|
||||
// expected.insert(2, Pose2(1.,0.,0.));
|
||||
// CHECK(assert_equal(expected, *optimized.config(), 1e-5));
|
||||
//}
|
||||
TEST( NonlinearOptimizer, delta )
|
||||
{
|
||||
shared_ptr<Graph> fg(new Graph(
|
||||
createNonlinearFactorGraph()));
|
||||
Optimizer::shared_config initial = sharedNoisyConfig();
|
||||
|
||||
// Expected configuration is the difference between the noisy config
|
||||
// and the ground-truth config. One step only because it's linear !
|
||||
VectorConfig expected;
|
||||
Vector dl1(2);
|
||||
dl1(0) = -0.1;
|
||||
dl1(1) = 0.1;
|
||||
expected.insert("l1", dl1);
|
||||
Vector dx1(2);
|
||||
dx1(0) = -0.1;
|
||||
dx1(1) = -0.1;
|
||||
expected.insert("x1", dx1);
|
||||
Vector dx2(2);
|
||||
dx2(0) = 0.1;
|
||||
dx2(1) = -0.2;
|
||||
expected.insert("x2", dx2);
|
||||
|
||||
// Check one ordering
|
||||
shared_ptr<Ordering> ord1(new Ordering());
|
||||
*ord1 += "x2","l1","x1";
|
||||
Optimizer::shared_solver solver;
|
||||
solver = Optimizer::shared_solver(new Optimizer::solver(ord1));
|
||||
Optimizer optimizer1(fg, initial, solver);
|
||||
VectorConfig actual1 = optimizer1.linearizeAndOptimizeForDelta();
|
||||
CHECK(assert_equal(actual1,expected));
|
||||
|
||||
// Check another
|
||||
shared_ptr<Ordering> ord2(new Ordering());
|
||||
*ord2 += "x1","x2","l1";
|
||||
solver = Optimizer::shared_solver(new Optimizer::solver(ord2));
|
||||
Optimizer optimizer2(fg, initial, solver);
|
||||
VectorConfig actual2 = optimizer2.linearizeAndOptimizeForDelta();
|
||||
CHECK(assert_equal(actual2,expected));
|
||||
|
||||
// And yet another...
|
||||
shared_ptr<Ordering> ord3(new Ordering());
|
||||
*ord3 += "l1","x1","x2";
|
||||
solver = Optimizer::shared_solver(new Optimizer::solver(ord3));
|
||||
Optimizer optimizer3(fg, initial, solver);
|
||||
VectorConfig actual3 = optimizer3.linearizeAndOptimizeForDelta();
|
||||
CHECK(assert_equal(actual3,expected));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( NonlinearOptimizer, iterateLM )
|
||||
{
|
||||
// really non-linear factor graph
|
||||
shared_ptr<Graph> fg(new Graph(
|
||||
createReallyNonlinearFactorGraph()));
|
||||
|
||||
// config far from minimum
|
||||
Point2 x0(3,0);
|
||||
boost::shared_ptr<Config> config(new Config);
|
||||
config->insert(simulated2D::PoseKey(1), x0);
|
||||
|
||||
// ordering
|
||||
shared_ptr<Ordering> ord(new Ordering());
|
||||
ord->push_back("x1");
|
||||
|
||||
// create initial optimization state, with lambda=0
|
||||
Optimizer::shared_solver solver(new Optimizer::solver(ord));
|
||||
Optimizer optimizer(fg, config, solver, 0.);
|
||||
|
||||
// normal iterate
|
||||
Optimizer iterated1 = optimizer.iterate();
|
||||
|
||||
// LM iterate with lambda 0 should be the same
|
||||
Optimizer iterated2 = optimizer.iterateLM();
|
||||
|
||||
// Try successive iterates. TODO: ugly pointers, better way ?
|
||||
Optimizer *pointer = new Optimizer(iterated2);
|
||||
for (int i=0;i<10;i++) {
|
||||
Optimizer* newOptimizer = new Optimizer(pointer->iterateLM());
|
||||
delete pointer;
|
||||
pointer = newOptimizer;
|
||||
}
|
||||
delete(pointer);
|
||||
|
||||
CHECK(assert_equal(*iterated1.config(), *iterated2.config(), 1e-9));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( NonlinearOptimizer, optimize )
|
||||
{
|
||||
shared_ptr<Graph> fg(new Graph(
|
||||
createReallyNonlinearFactorGraph()));
|
||||
|
||||
// test error at minimum
|
||||
Point2 xstar(0,0);
|
||||
Config cstar;
|
||||
cstar.insert(simulated2D::PoseKey(1), xstar);
|
||||
DOUBLES_EQUAL(0.0,fg->error(cstar),0.0);
|
||||
|
||||
// test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 =
|
||||
Point2 x0(3,3);
|
||||
boost::shared_ptr<Config> c0(new Config);
|
||||
c0->insert(simulated2D::PoseKey(1), x0);
|
||||
DOUBLES_EQUAL(199.0,fg->error(*c0),1e-3);
|
||||
|
||||
// optimize parameters
|
||||
shared_ptr<Ordering> ord(new Ordering());
|
||||
ord->push_back("x1");
|
||||
double relativeThreshold = 1e-5;
|
||||
double absoluteThreshold = 1e-5;
|
||||
|
||||
// initial optimization state is the same in both cases tested
|
||||
Optimizer::shared_solver solver(new Optimizer::solver(ord));
|
||||
Optimizer optimizer(fg, c0, solver);
|
||||
|
||||
// Gauss-Newton
|
||||
Optimizer actual1 = optimizer.gaussNewton(relativeThreshold,
|
||||
absoluteThreshold);
|
||||
DOUBLES_EQUAL(0,fg->error(*(actual1.config())),1e-3);
|
||||
|
||||
// Levenberg-Marquardt
|
||||
Optimizer actual2 = optimizer.levenbergMarquardt(relativeThreshold,
|
||||
absoluteThreshold, Optimizer::SILENT);
|
||||
DOUBLES_EQUAL(0,fg->error(*(actual2.config())),1e-3);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( NonlinearOptimizer, Factorization )
|
||||
{
|
||||
typedef NonlinearOptimizer<Pose2Graph, Pose2Config, GaussianFactorGraph, Factorization<Pose2Graph, Pose2Config> > Optimizer;
|
||||
|
||||
boost::shared_ptr<Pose2Config> config(new Pose2Config);
|
||||
config->insert(1, Pose2(0.,0.,0.));
|
||||
config->insert(2, Pose2(1.5,0.,0.));
|
||||
|
||||
boost::shared_ptr<Pose2Graph> graph(new Pose2Graph);
|
||||
graph->addPrior(1, Pose2(0.,0.,0.), Isotropic::Sigma(3, 1e-10));
|
||||
graph->addConstraint(1,2, Pose2(1.,0.,0.), Isotropic::Sigma(3, 1));
|
||||
|
||||
boost::shared_ptr<Ordering> ordering(new Ordering);
|
||||
ordering->push_back(Pose2Config::Key(1));
|
||||
ordering->push_back(Pose2Config::Key(2));
|
||||
Optimizer::shared_solver solver(new Factorization<Pose2Graph, Pose2Config>(ordering));
|
||||
|
||||
Optimizer optimizer(graph, config, solver);
|
||||
Optimizer optimized = optimizer.iterateLM();
|
||||
|
||||
Pose2Config expected;
|
||||
expected.insert(1, Pose2(0.,0.,0.));
|
||||
expected.insert(2, Pose2(1.,0.,0.));
|
||||
CHECK(assert_equal(expected, *optimized.config(), 1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( NonlinearOptimizer, SubgraphPCG )
|
||||
|
|
@ -195,14 +200,10 @@ TEST( NonlinearOptimizer, SubgraphPCG )
|
|||
graph->addPrior(1, Pose2(0.,0.,0.), Isotropic::Sigma(3, 1e-10));
|
||||
graph->addConstraint(1,2, Pose2(1.,0.,0.), Isotropic::Sigma(3, 1));
|
||||
|
||||
boost::shared_ptr<Ordering> ordering(new Ordering);
|
||||
ordering->push_back(Pose2Config::Key(1));
|
||||
ordering->push_back(Pose2Config::Key(2));
|
||||
|
||||
double relativeThreshold = 1e-5;
|
||||
double absoluteThreshold = 1e-5;
|
||||
Optimizer::shared_solver solver(new SubgraphPCG<Pose2Graph, Pose2Config>(*graph, *config));
|
||||
Optimizer optimizer(graph, ordering, config, solver);
|
||||
Optimizer optimizer(graph, config, solver);
|
||||
Optimizer optimized = optimizer.gaussNewton(relativeThreshold, absoluteThreshold, Optimizer::SILENT);
|
||||
|
||||
Pose2Config expected;
|
||||
|
|
|
|||
|
|
@ -101,7 +101,8 @@ TEST(Pose2Graph, optimize) {
|
|||
shared_ptr<Ordering> ordering(new Ordering);
|
||||
*ordering += "x0","x1";
|
||||
typedef NonlinearOptimizer<Pose2Graph, Pose2Config> Optimizer;
|
||||
Optimizer optimizer0(fg, ordering, initial);
|
||||
Optimizer::shared_solver solver(new Optimizer::solver(ordering));
|
||||
Optimizer optimizer0(fg, initial, solver);
|
||||
Optimizer::verbosityLevel verbosity = Optimizer::SILENT;
|
||||
//Optimizer::verbosityLevel verbosity = Optimizer::ERROR;
|
||||
Optimizer optimizer = optimizer0.levenbergMarquardt(1e-15, 1e-15, verbosity);
|
||||
|
|
@ -145,7 +146,8 @@ TEST(Pose2Graph, optimizeCircle) {
|
|||
shared_ptr<Ordering> ordering(new Ordering);
|
||||
*ordering += "x0","x1","x2","x3","x4","x5";
|
||||
typedef NonlinearOptimizer<Pose2Graph, Pose2Config> Optimizer;
|
||||
Optimizer optimizer0(fg, ordering, initial);
|
||||
Optimizer::shared_solver solver(new Optimizer::solver(ordering));
|
||||
Optimizer optimizer0(fg, initial, solver);
|
||||
Optimizer::verbosityLevel verbosity = Optimizer::SILENT;
|
||||
// Optimizer::verbosityLevel verbosity = Optimizer::ERROR;
|
||||
Optimizer optimizer = optimizer0.levenbergMarquardt(1e-15, 1e-15, verbosity);
|
||||
|
|
|
|||
|
|
@ -58,7 +58,8 @@ TEST(Pose3Graph, optimizeCircle) {
|
|||
shared_ptr<Ordering> ordering(new Ordering);
|
||||
*ordering += "x0","x1","x2","x3","x4","x5";
|
||||
typedef NonlinearOptimizer<Pose3Graph, Pose3Config> Optimizer;
|
||||
Optimizer optimizer0(fg, ordering, initial);
|
||||
Optimizer::shared_solver solver(new Optimizer::solver(ordering));
|
||||
Optimizer optimizer0(fg, initial, solver);
|
||||
Optimizer::verbosityLevel verbosity = Optimizer::SILENT;
|
||||
// Optimizer::verbosityLevel verbosity = Optimizer::ERROR;
|
||||
Optimizer optimizer = optimizer0.levenbergMarquardt(1e-15, 1e-15, verbosity);
|
||||
|
|
|
|||
|
|
@ -296,7 +296,8 @@ TEST (SQP, two_pose_truth ) {
|
|||
// optimize the graph
|
||||
shared_ptr<Ordering> ordering(new Ordering());
|
||||
*ordering += "x1", "x2", "l1";
|
||||
Optimizer optimizer(graph, ordering, initialEstimate);
|
||||
Optimizer::shared_solver solver(new Optimizer::solver(ordering));
|
||||
Optimizer optimizer(graph, initialEstimate, solver);
|
||||
|
||||
// display solution
|
||||
double relativeThreshold = 1e-5;
|
||||
|
|
@ -533,7 +534,8 @@ TEST (SQP, stereo_truth ) {
|
|||
*ord += "x1", "x2", "l1";
|
||||
|
||||
// create optimizer
|
||||
VOptimizer optimizer(graph, ord, truthConfig);
|
||||
VOptimizer::shared_solver solver(new VOptimizer::solver(ord));
|
||||
VOptimizer optimizer(graph, truthConfig, solver);
|
||||
|
||||
// optimize
|
||||
VOptimizer afterOneIteration = optimizer.iterate();
|
||||
|
|
@ -609,7 +611,8 @@ TEST (SQP, stereo_truth_noisy ) {
|
|||
*ord += "x1", "x2", "l1";
|
||||
|
||||
// create optimizer
|
||||
VOptimizer optimizer0(graph, ord, noisyConfig);
|
||||
VOptimizer::shared_solver solver(new VOptimizer::solver(ord));
|
||||
VOptimizer optimizer0(graph, noisyConfig, solver);
|
||||
|
||||
if (verbose)
|
||||
cout << "Initial Error: " << optimizer0.error() << endl;
|
||||
|
|
|
|||
|
|
@ -100,7 +100,8 @@ TEST( Graph, optimizeLM)
|
|||
|
||||
// Create an optimizer and check its error
|
||||
// We expect the initial to be zero because config is the ground truth
|
||||
Optimizer optimizer(graph, ordering, initialEstimate);
|
||||
Optimizer::shared_solver solver(new Optimizer::solver(ordering));
|
||||
Optimizer optimizer(graph, initialEstimate, solver);
|
||||
DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
|
||||
|
||||
// Iterate once, and the config should not have changed because we started
|
||||
|
|
@ -144,7 +145,8 @@ TEST( Graph, optimizeLM2)
|
|||
|
||||
// Create an optimizer and check its error
|
||||
// We expect the initial to be zero because config is the ground truth
|
||||
Optimizer optimizer(graph, ordering, initialEstimate);
|
||||
Optimizer::shared_solver solver(new Optimizer::solver(ordering));
|
||||
Optimizer optimizer(graph, initialEstimate, solver);
|
||||
DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
|
||||
|
||||
// Iterate once, and the config should not have changed because we started
|
||||
|
|
@ -179,7 +181,8 @@ TEST( Graph, CHECK_ORDERING)
|
|||
|
||||
// Create an optimizer and check its error
|
||||
// We expect the initial to be zero because config is the ground truth
|
||||
Optimizer optimizer(graph, ordering, initialEstimate);
|
||||
Optimizer::shared_solver solver(new Optimizer::solver(ordering));
|
||||
Optimizer optimizer(graph, initialEstimate, solver);
|
||||
DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
|
||||
|
||||
// Iterate once, and the config should not have changed because we started
|
||||
|
|
|
|||
Loading…
Reference in New Issue