NonlinearOptimizer constructor now requires shared_ptr to prevent storing pointers to stack and temporary variables. Code that uses it will need to be modified, but CitySLAM is already done.
Richard Roberts
parent
40889e8f50
commit
2c8d8dbde4
|
|
@ -44,9 +44,9 @@ namespace gtsam {
|
|||
// Constructors
|
||||
/* ************************************************************************* */
|
||||
template<class G, class C>
|
||||
NonlinearOptimizer<G, C>::NonlinearOptimizer(const G& graph,
|
||||
const Ordering& ordering, shared_config config, double lambda) :
|
||||
graph_(&graph), ordering_(&ordering), config_(config), error_(graph.error(
|
||||
NonlinearOptimizer<G, C>::NonlinearOptimizer(shared_graph graph,
|
||||
shared_ordering ordering, shared_config config, double lambda) :
|
||||
graph_(graph), ordering_(ordering), config_(config), error_(graph->error(
|
||||
*config)), lambda_(lambda) {
|
||||
}
|
||||
|
||||
|
|
@ -88,7 +88,7 @@ namespace gtsam {
|
|||
if (verbosity >= CONFIG)
|
||||
newConfig->print("newConfig");
|
||||
|
||||
return NonlinearOptimizer(*graph_, *ordering_, newConfig);
|
||||
return NonlinearOptimizer(graph_, ordering_, newConfig);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -139,14 +139,14 @@ namespace gtsam {
|
|||
newConfig->print("config");
|
||||
|
||||
// create new optimization state with more adventurous lambda
|
||||
NonlinearOptimizer next(*graph_, *ordering_, newConfig, lambda_ / factor);
|
||||
NonlinearOptimizer next(graph_, ordering_, newConfig, 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_, lambda_ * factor);
|
||||
NonlinearOptimizer cautious(graph_, ordering_, config_, lambda_ * factor);
|
||||
return cautious.try_lambda(linear, verbosity, factor);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -31,6 +31,8 @@ namespace gtsam {
|
|||
|
||||
// For performance reasons in recursion, we store configs in a shared_ptr
|
||||
typedef boost::shared_ptr<const Config> shared_config;
|
||||
typedef boost::shared_ptr<const FactorGraph> shared_graph;
|
||||
typedef boost::shared_ptr<const Ordering> shared_ordering;
|
||||
|
||||
enum verbosityLevel {
|
||||
SILENT,
|
||||
|
|
@ -49,17 +51,17 @@ namespace gtsam {
|
|||
|
||||
// keep a reference to const versions of the graph and ordering
|
||||
// These normally do not change
|
||||
const FactorGraph* graph_;
|
||||
const Ordering* ordering_;
|
||||
const shared_graph graph_;
|
||||
const shared_ordering ordering_;
|
||||
|
||||
// keep a configuration and its error
|
||||
// These typically change once per iteration (in a functional way)
|
||||
shared_config config_;
|
||||
double error_;
|
||||
const shared_config config_;
|
||||
const double error_;
|
||||
|
||||
// keep current lambda for use within LM only
|
||||
// TODO: red flag, should we have an LM class ?
|
||||
double lambda_;
|
||||
const double lambda_;
|
||||
|
||||
// Recursively try to do tempered Gauss-Newton steps until we succeed
|
||||
NonlinearOptimizer try_lambda(const GaussianFactorGraph& linear,
|
||||
|
|
@ -70,9 +72,16 @@ namespace gtsam {
|
|||
/**
|
||||
* Constructor
|
||||
*/
|
||||
NonlinearOptimizer(const FactorGraph& graph, const Ordering& ordering,
|
||||
NonlinearOptimizer(shared_graph graph, shared_ordering ordering,
|
||||
shared_config config, double lambda = 1e-5);
|
||||
|
||||
/**
|
||||
* Copy constructor
|
||||
*/
|
||||
NonlinearOptimizer(const NonlinearOptimizer<FactorGraph, Config> &optimizer) :
|
||||
graph_(optimizer.graph_), ordering_(optimizer.ordering_), config_(optimizer.config_),
|
||||
error_(optimizer.error_), lambda_(optimizer.lambda_) {}
|
||||
|
||||
/**
|
||||
* Return current error
|
||||
*/
|
||||
|
|
|
|||
|
|
@ -12,6 +12,9 @@ using namespace boost::assign;
|
|||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include <boost/shared_ptr.hpp>
|
||||
using namespace boost;
|
||||
|
||||
#include "Matrix.h"
|
||||
#include "Ordering.h"
|
||||
#include "smallExample.h"
|
||||
|
|
@ -27,7 +30,7 @@ typedef NonlinearOptimizer<ExampleNonlinearFactorGraph,VectorConfig> Optimizer;
|
|||
/* ************************************************************************* */
|
||||
TEST( NonlinearOptimizer, delta )
|
||||
{
|
||||
ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph();
|
||||
shared_ptr<ExampleNonlinearFactorGraph> fg(new ExampleNonlinearFactorGraph(createNonlinearFactorGraph()));
|
||||
Optimizer::shared_config initial = sharedNoisyConfig();
|
||||
|
||||
// Expected configuration is the difference between the noisy config
|
||||
|
|
@ -47,22 +50,22 @@ TEST( NonlinearOptimizer, delta )
|
|||
expected.insert("x2", dx2);
|
||||
|
||||
// Check one ordering
|
||||
Ordering ord1;
|
||||
ord1 += "x2","l1","x1";
|
||||
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
|
||||
Ordering ord2;
|
||||
ord2 += "x1","x2","l1";
|
||||
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...
|
||||
Ordering ord3;
|
||||
ord3 += "l1","x1","x2";
|
||||
shared_ptr<Ordering> ord3(new Ordering());
|
||||
*ord3 += "l1","x1","x2";
|
||||
Optimizer optimizer3(fg, ord3, initial);
|
||||
VectorConfig actual3 = optimizer3.linearizeAndOptimizeForDelta();
|
||||
CHECK(assert_equal(actual3,expected));
|
||||
|
|
@ -72,7 +75,7 @@ TEST( NonlinearOptimizer, delta )
|
|||
TEST( NonlinearOptimizer, iterateLM )
|
||||
{
|
||||
// really non-linear factor graph
|
||||
ExampleNonlinearFactorGraph fg = createReallyNonlinearFactorGraph();
|
||||
shared_ptr<ExampleNonlinearFactorGraph> fg(new ExampleNonlinearFactorGraph(createReallyNonlinearFactorGraph()));
|
||||
|
||||
// config far from minimum
|
||||
Vector x0 = Vector_(1, 3.0);
|
||||
|
|
@ -80,8 +83,8 @@ TEST( NonlinearOptimizer, iterateLM )
|
|||
config->insert("x", x0);
|
||||
|
||||
// ordering
|
||||
Ordering ord;
|
||||
ord.push_back("x");
|
||||
shared_ptr<Ordering> ord(new Ordering());
|
||||
ord->push_back("x");
|
||||
|
||||
// create initial optimization state, with lambda=0
|
||||
Optimizer optimizer(fg, ord, config, 0);
|
||||
|
|
@ -107,23 +110,23 @@ TEST( NonlinearOptimizer, iterateLM )
|
|||
/* ************************************************************************* */
|
||||
TEST( NonlinearOptimizer, optimize )
|
||||
{
|
||||
ExampleNonlinearFactorGraph fg = createReallyNonlinearFactorGraph();
|
||||
shared_ptr<ExampleNonlinearFactorGraph> fg(new ExampleNonlinearFactorGraph(createReallyNonlinearFactorGraph()));
|
||||
|
||||
// test error at minimum
|
||||
Vector xstar = Vector_(1, 0.0);
|
||||
VectorConfig cstar;
|
||||
cstar.insert("x", xstar);
|
||||
DOUBLES_EQUAL(0.0,fg.error(cstar),0.0);
|
||||
DOUBLES_EQUAL(0.0,fg->error(cstar),0.0);
|
||||
|
||||
// test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 =
|
||||
Vector x0 = Vector_(1, 3.0);
|
||||
boost::shared_ptr<VectorConfig> c0(new VectorConfig);
|
||||
c0->insert("x", x0);
|
||||
DOUBLES_EQUAL(199.0,fg.error(*c0),1e-3);
|
||||
DOUBLES_EQUAL(199.0,fg->error(*c0),1e-3);
|
||||
|
||||
// optimize parameters
|
||||
Ordering ord;
|
||||
ord.push_back("x");
|
||||
shared_ptr<Ordering> ord(new Ordering());
|
||||
ord->push_back("x");
|
||||
double relativeThreshold = 1e-5;
|
||||
double absoluteThreshold = 1e-5;
|
||||
|
||||
|
|
|
|||
|
|
@ -6,6 +6,7 @@
|
|||
#include <iostream>
|
||||
#include <boost/shared_ptr.hpp>
|
||||
#include <boost/assign/std/list.hpp>
|
||||
using namespace boost;
|
||||
using namespace boost::assign;
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
|
@ -84,9 +85,9 @@ TEST( Pose2Graph, linerization )
|
|||
TEST(Pose2Graph, optimize) {
|
||||
|
||||
// create a Pose graph with one equality constraint and one measurement
|
||||
Pose2Graph fg;
|
||||
fg.addConstraint("p0", Pose2(0,0,0));
|
||||
fg.add("p0", "p1", Pose2(1,2,M_PI_2), covariance);
|
||||
shared_ptr<Pose2Graph> fg(new Pose2Graph);
|
||||
fg->addConstraint("p0", Pose2(0,0,0));
|
||||
fg->add("p0", "p1", Pose2(1,2,M_PI_2), covariance);
|
||||
|
||||
// Create initial config
|
||||
boost::shared_ptr<Pose2Config> initial(new Pose2Config());
|
||||
|
|
@ -94,13 +95,13 @@ TEST(Pose2Graph, optimize) {
|
|||
initial->insert("p1", Pose2(0,0,0));
|
||||
|
||||
// Choose an ordering and optimize
|
||||
Ordering ordering;
|
||||
ordering += "p0","p1";
|
||||
shared_ptr<Ordering> ordering(new Ordering);
|
||||
*ordering += "p0","p1";
|
||||
typedef NonlinearOptimizer<Pose2Graph, Pose2Config> Optimizer;
|
||||
Optimizer optimizer(fg, ordering, initial);
|
||||
Optimizer optimizer0(fg, ordering, initial);
|
||||
Optimizer::verbosityLevel verbosity = Optimizer::SILENT;
|
||||
//Optimizer::verbosityLevel verbosity = Optimizer::ERROR;
|
||||
optimizer = optimizer.levenbergMarquardt(1e-15, 1e-15, verbosity);
|
||||
Optimizer optimizer = optimizer0.levenbergMarquardt(1e-15, 1e-15, verbosity);
|
||||
|
||||
// Check with expected config
|
||||
Pose2Config expected;
|
||||
|
|
@ -118,15 +119,15 @@ TEST(Pose2Graph, optimizeCircle) {
|
|||
Pose2 p0 = hexagon["p0"], p1 = hexagon["p1"];
|
||||
|
||||
// create a Pose graph with one equality constraint and one measurement
|
||||
Pose2Graph fg;
|
||||
fg.addConstraint("p0", p0);
|
||||
shared_ptr<Pose2Graph> fg(new Pose2Graph);
|
||||
fg->addConstraint("p0", p0);
|
||||
Pose2 delta = between(p0,p1);
|
||||
fg.add("p0", "p1", delta, covariance);
|
||||
fg.add("p1", "p2", delta, covariance);
|
||||
fg.add("p2", "p3", delta, covariance);
|
||||
fg.add("p3", "p4", delta, covariance);
|
||||
fg.add("p4", "p5", delta, covariance);
|
||||
fg.add("p5", "p0", delta, covariance);
|
||||
fg->add("p0", "p1", delta, covariance);
|
||||
fg->add("p1", "p2", delta, covariance);
|
||||
fg->add("p2", "p3", delta, covariance);
|
||||
fg->add("p3", "p4", delta, covariance);
|
||||
fg->add("p4", "p5", delta, covariance);
|
||||
fg->add("p5", "p0", delta, covariance);
|
||||
|
||||
// Create initial config
|
||||
boost::shared_ptr<Pose2Config> initial(new Pose2Config());
|
||||
|
|
@ -138,13 +139,13 @@ TEST(Pose2Graph, optimizeCircle) {
|
|||
initial->insert("p5", expmap(hexagon["p5"],Vector_(3,-0.1, 0.1,-0.1)));
|
||||
|
||||
// Choose an ordering and optimize
|
||||
Ordering ordering;
|
||||
ordering += "p0","p1","p2","p3","p4","p5";
|
||||
shared_ptr<Ordering> ordering(new Ordering);
|
||||
*ordering += "p0","p1","p2","p3","p4","p5";
|
||||
typedef NonlinearOptimizer<Pose2Graph, Pose2Config> Optimizer;
|
||||
Optimizer optimizer(fg, ordering, initial);
|
||||
Optimizer optimizer0(fg, ordering, initial);
|
||||
Optimizer::verbosityLevel verbosity = Optimizer::SILENT;
|
||||
// Optimizer::verbosityLevel verbosity = Optimizer::ERROR;
|
||||
optimizer = optimizer.levenbergMarquardt(1e-15, 1e-15, verbosity);
|
||||
Optimizer optimizer = optimizer0.levenbergMarquardt(1e-15, 1e-15, verbosity);
|
||||
|
||||
Pose2Config actual = *optimizer.config();
|
||||
|
||||
|
|
|
|||
|
|
@ -8,6 +8,7 @@
|
|||
#include <iostream>
|
||||
#include <boost/shared_ptr.hpp>
|
||||
#include <boost/assign/std/list.hpp>
|
||||
using namespace boost;
|
||||
using namespace boost::assign;
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
|
@ -32,15 +33,15 @@ TEST(Pose3Graph, optimizeCircle) {
|
|||
Pose3 p0 = hexagon["p0"], p1 = hexagon["p1"];
|
||||
|
||||
// create a Pose graph with one equality constraint and one measurement
|
||||
Pose3Graph fg;
|
||||
fg.addConstraint("p0", p0);
|
||||
shared_ptr<Pose3Graph> fg(new Pose3Graph);
|
||||
fg->addConstraint("p0", p0);
|
||||
Pose3 delta = between(p0,p1);
|
||||
fg.add("p0", "p1", delta, covariance);
|
||||
fg.add("p1", "p2", delta, covariance);
|
||||
fg.add("p2", "p3", delta, covariance);
|
||||
fg.add("p3", "p4", delta, covariance);
|
||||
fg.add("p4", "p5", delta, covariance);
|
||||
fg.add("p5", "p0", delta, covariance);
|
||||
fg->add("p0", "p1", delta, covariance);
|
||||
fg->add("p1", "p2", delta, covariance);
|
||||
fg->add("p2", "p3", delta, covariance);
|
||||
fg->add("p3", "p4", delta, covariance);
|
||||
fg->add("p4", "p5", delta, covariance);
|
||||
fg->add("p5", "p0", delta, covariance);
|
||||
|
||||
// Create initial config
|
||||
boost::shared_ptr<Pose3Config> initial(new Pose3Config());
|
||||
|
|
@ -52,13 +53,13 @@ TEST(Pose3Graph, optimizeCircle) {
|
|||
initial->insert("p5", expmap(hexagon["p5"],Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
|
||||
|
||||
// Choose an ordering and optimize
|
||||
Ordering ordering;
|
||||
ordering += "p0","p1","p2","p3","p4","p5";
|
||||
shared_ptr<Ordering> ordering(new Ordering);
|
||||
*ordering += "p0","p1","p2","p3","p4","p5";
|
||||
typedef NonlinearOptimizer<Pose3Graph, Pose3Config> Optimizer;
|
||||
Optimizer optimizer(fg, ordering, initial);
|
||||
Optimizer optimizer0(fg, ordering, initial);
|
||||
Optimizer::verbosityLevel verbosity = Optimizer::SILENT;
|
||||
// Optimizer::verbosityLevel verbosity = Optimizer::ERROR;
|
||||
optimizer = optimizer.levenbergMarquardt(1e-15, 1e-15, verbosity);
|
||||
Optimizer optimizer = optimizer0.levenbergMarquardt(1e-15, 1e-15, verbosity);
|
||||
|
||||
Pose3Config actual = *optimizer.config();
|
||||
|
||||
|
|
|
|||
|
|
@ -9,6 +9,7 @@
|
|||
#include <boost/assign/std/list.hpp> // for operator +=
|
||||
#include <boost/assign/std/map.hpp> // for insert
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/shared_ptr.hpp>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <GaussianFactorGraph.h>
|
||||
#include <NonlinearFactor.h>
|
||||
|
|
@ -32,6 +33,7 @@
|
|||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
using namespace boost;
|
||||
using namespace boost::assign;
|
||||
|
||||
// trick from some reading group
|
||||
|
|
@ -286,11 +288,11 @@ TEST (SQP, two_pose_truth ) {
|
|||
shared f2(new Simulated2DMeasurement(z2, sigma2, "x2", "l1"));
|
||||
|
||||
// construct the graph
|
||||
NLGraph graph;
|
||||
graph.push_back(ef1);
|
||||
graph.push_back(ef2);
|
||||
graph.push_back(f1);
|
||||
graph.push_back(f2);
|
||||
shared_ptr<NLGraph> graph(new NLGraph());
|
||||
graph->push_back(ef1);
|
||||
graph->push_back(ef2);
|
||||
graph->push_back(f1);
|
||||
graph->push_back(f2);
|
||||
|
||||
// create an initial estimate
|
||||
boost::shared_ptr<VectorConfig> initialEstimate(new VectorConfig(feas)); // must start with feasible set
|
||||
|
|
@ -298,8 +300,8 @@ TEST (SQP, two_pose_truth ) {
|
|||
//initialEstimate->insert("l1", Vector_(2, 1.2, 5.6)); // with small error
|
||||
|
||||
// optimize the graph
|
||||
Ordering ordering;
|
||||
ordering += "x1", "x2", "l1";
|
||||
shared_ptr<Ordering> ordering(new Ordering());
|
||||
*ordering += "x1", "x2", "l1";
|
||||
Optimizer optimizer(graph, ordering, initialEstimate, 1e-5);
|
||||
|
||||
// display solution
|
||||
|
|
@ -498,27 +500,27 @@ TEST (SQP, stereo_truth ) {
|
|||
truthConfig->addLandmarkPoint(1, landmark);
|
||||
|
||||
// create graph
|
||||
VSLAMGraph graph;
|
||||
shared_ptr<VSLAMGraph> graph(new VSLAMGraph());
|
||||
|
||||
// create equality constraints for poses
|
||||
graph.addCameraConstraint(1, camera1.pose());
|
||||
graph.addCameraConstraint(2, camera2.pose());
|
||||
graph->addCameraConstraint(1, camera1.pose());
|
||||
graph->addCameraConstraint(2, camera2.pose());
|
||||
|
||||
// create VSLAM factors
|
||||
Point2 z1 = camera1.project(landmark);
|
||||
if (verbose) z1.print("z1");
|
||||
shared_vf vf1(new VSLAMFactor(z1, 1.0, 1, 1, shK));
|
||||
graph.push_back(vf1);
|
||||
graph->push_back(vf1);
|
||||
Point2 z2 = camera2.project(landmark);
|
||||
if (verbose) z2.print("z2");
|
||||
shared_vf vf2(new VSLAMFactor(z2, 1.0, 2, 1, shK));
|
||||
graph.push_back(vf2);
|
||||
graph->push_back(vf2);
|
||||
|
||||
if (verbose) graph.print("Graph after construction");
|
||||
if (verbose) graph->print("Graph after construction");
|
||||
|
||||
// create ordering
|
||||
Ordering ord;
|
||||
ord += "x1", "x2", "l1";
|
||||
shared_ptr<Ordering> ord(new Ordering());
|
||||
*ord += "x1", "x2", "l1";
|
||||
|
||||
// create optimizer
|
||||
VOptimizer optimizer(graph, ord, truthConfig, 1e-5);
|
||||
|
|
@ -571,40 +573,40 @@ TEST (SQP, stereo_truth_noisy ) {
|
|||
noisyConfig->addLandmarkPoint(1, landmarkNoisy);
|
||||
|
||||
// create graph
|
||||
VSLAMGraph graph;
|
||||
shared_ptr<VSLAMGraph> graph(new VSLAMGraph());
|
||||
|
||||
// create equality constraints for poses
|
||||
graph.addCameraConstraint(1, camera1.pose());
|
||||
graph.addCameraConstraint(2, camera2.pose());
|
||||
graph->addCameraConstraint(1, camera1.pose());
|
||||
graph->addCameraConstraint(2, camera2.pose());
|
||||
|
||||
// create VSLAM factors
|
||||
Point2 z1 = camera1.project(landmark);
|
||||
if (verbose) z1.print("z1");
|
||||
shared_vf vf1(new VSLAMFactor(z1, 1.0, 1, 1, shK));
|
||||
graph.push_back(vf1);
|
||||
graph->push_back(vf1);
|
||||
Point2 z2 = camera2.project(landmark);
|
||||
if (verbose) z2.print("z2");
|
||||
shared_vf vf2(new VSLAMFactor(z2, 1.0, 2, 1, shK));
|
||||
graph.push_back(vf2);
|
||||
graph->push_back(vf2);
|
||||
|
||||
if (verbose) {
|
||||
graph.print("Graph after construction");
|
||||
graph->print("Graph after construction");
|
||||
noisyConfig->print("Initial config");
|
||||
}
|
||||
|
||||
// create ordering
|
||||
Ordering ord;
|
||||
ord += "x1", "x2", "l1";
|
||||
shared_ptr<Ordering> ord(new Ordering());
|
||||
*ord += "x1", "x2", "l1";
|
||||
|
||||
// create optimizer
|
||||
VOptimizer optimizer(graph, ord, noisyConfig, 1e-5);
|
||||
VOptimizer optimizer0(graph, ord, noisyConfig, 1e-5);
|
||||
|
||||
if (verbose)
|
||||
cout << "Initial Error: " << optimizer.error() << endl;
|
||||
cout << "Initial Error: " << optimizer0.error() << endl;
|
||||
|
||||
// use Levenberg-Marquardt optimization
|
||||
double relThresh = 1e-5, absThresh = 1e-5;
|
||||
optimizer = optimizer.levenbergMarquardt(relThresh, absThresh, VOptimizer::SILENT);
|
||||
VOptimizer optimizer(optimizer0.levenbergMarquardt(relThresh, absThresh, VOptimizer::SILENT));
|
||||
|
||||
// verify
|
||||
DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
|
||||
|
|
|
|||
|
|
@ -8,6 +8,8 @@
|
|||
*/
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <boost/shared_ptr.hpp>
|
||||
using namespace boost;
|
||||
|
||||
#include "VSLAMGraph.h"
|
||||
#include "NonlinearFactorGraph-inl.h"
|
||||
|
|
@ -66,11 +68,11 @@ VSLAMGraph testGraph() {
|
|||
TEST( VSLAMGraph, optimizeLM)
|
||||
{
|
||||
// build a graph
|
||||
VSLAMGraph graph = testGraph();
|
||||
shared_ptr<VSLAMGraph> graph(new VSLAMGraph(testGraph()));
|
||||
// add 3 landmark constraints
|
||||
graph.addLandmarkConstraint(1, landmark1);
|
||||
graph.addLandmarkConstraint(2, landmark2);
|
||||
graph.addLandmarkConstraint(3, landmark3);
|
||||
graph->addLandmarkConstraint(1, landmark1);
|
||||
graph->addLandmarkConstraint(2, landmark2);
|
||||
graph->addLandmarkConstraint(3, landmark3);
|
||||
|
||||
// Create an initial configuration corresponding to the ground truth
|
||||
boost::shared_ptr<VSLAMConfig> initialEstimate(new VSLAMConfig);
|
||||
|
|
@ -89,7 +91,7 @@ TEST( VSLAMGraph, optimizeLM)
|
|||
keys.push_back("l4");
|
||||
keys.push_back("x1");
|
||||
keys.push_back("x2");
|
||||
Ordering ordering(keys);
|
||||
shared_ptr<Ordering> ordering(new Ordering(keys));
|
||||
|
||||
// Create an optimizer and check its error
|
||||
// We expect the initial to be zero because config is the ground truth
|
||||
|
|
@ -110,10 +112,10 @@ TEST( VSLAMGraph, optimizeLM)
|
|||
TEST( VSLAMGraph, optimizeLM2)
|
||||
{
|
||||
// build a graph
|
||||
VSLAMGraph graph = testGraph();
|
||||
shared_ptr<VSLAMGraph> graph(new VSLAMGraph(testGraph()));
|
||||
// add 2 camera constraints
|
||||
graph.addCameraConstraint(1, camera1);
|
||||
graph.addCameraConstraint(2, camera2);
|
||||
graph->addCameraConstraint(1, camera1);
|
||||
graph->addCameraConstraint(2, camera2);
|
||||
|
||||
// Create an initial configuration corresponding to the ground truth
|
||||
boost::shared_ptr<VSLAMConfig> initialEstimate(new VSLAMConfig);
|
||||
|
|
@ -133,7 +135,7 @@ TEST( VSLAMGraph, optimizeLM2)
|
|||
keys.push_back("l4");
|
||||
keys.push_back("x1");
|
||||
keys.push_back("x2");
|
||||
Ordering ordering(keys);
|
||||
shared_ptr<Ordering> ordering(new Ordering(keys));
|
||||
|
||||
// Create an optimizer and check its error
|
||||
// We expect the initial to be zero because config is the ground truth
|
||||
|
|
@ -154,10 +156,10 @@ TEST( VSLAMGraph, optimizeLM2)
|
|||
TEST( VSLAMGraph, CHECK_ORDERING)
|
||||
{
|
||||
// build a graph
|
||||
VSLAMGraph graph = testGraph();
|
||||
shared_ptr<VSLAMGraph> graph(new VSLAMGraph(testGraph()));
|
||||
// add 2 camera constraints
|
||||
graph.addCameraConstraint(1, camera1);
|
||||
graph.addCameraConstraint(2, camera2);
|
||||
graph->addCameraConstraint(1, camera1);
|
||||
graph->addCameraConstraint(2, camera2);
|
||||
|
||||
// Create an initial configuration corresponding to the ground truth
|
||||
boost::shared_ptr<VSLAMConfig> initialEstimate(new VSLAMConfig);
|
||||
|
|
@ -168,7 +170,7 @@ TEST( VSLAMGraph, CHECK_ORDERING)
|
|||
initialEstimate->addLandmarkPoint(3, landmark3);
|
||||
initialEstimate->addLandmarkPoint(4, landmark4);
|
||||
|
||||
Ordering ordering = graph.getOrdering();
|
||||
shared_ptr<Ordering> ordering(new Ordering(graph->getOrdering()));
|
||||
|
||||
// Create an optimizer and check its error
|
||||
// We expect the initial to be zero because config is the ground truth
|
||||
|
|
|
|||
Loading…
Reference in New Issue