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

fixed namespaces

release/4.3a0
Frank Dellaert 2010-03-12 18:52:04 +00:00
parent a4c769fddc
commit 91f020fee1
1 changed files with 18 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
cpp/testNonlinearOptimizer.cpp
View File

@ -30,18 +30,17 @@ using namespace boost;
#include "SubgraphPreconditioner-inl.h" #include "SubgraphPreconditioner-inl.h"
using namespace gtsam; using namespace gtsam;
using namespace example;
const double tol = 1e-5; const double tol = 1e-5;
typedef NonlinearOptimizer<Graph,Config> Optimizer; typedef NonlinearOptimizer<example::Graph,example::Config> Optimizer;
/* ************************************************************************* */ /* ************************************************************************* */
TEST( NonlinearOptimizer, delta ) TEST( NonlinearOptimizer, delta )
{ {
shared_ptr<Graph> fg(new Graph( shared_ptr<example::Graph> fg(new example::Graph(
createNonlinearFactorGraph())); example::createNonlinearFactorGraph()));
Optimizer::shared_config initial = sharedNoisyConfig(); Optimizer::shared_config initial = example::sharedNoisyConfig();
// Expected configuration is the difference between the noisy config // Expected configuration is the difference between the noisy config
// and the ground-truth config. One step only because it's linear ! // and the ground-truth config. One step only because it's linear !
@ -102,12 +101,12 @@ TEST( NonlinearOptimizer, delta )
TEST( NonlinearOptimizer, iterateLM ) TEST( NonlinearOptimizer, iterateLM )
{ {
// really non-linear factor graph // really non-linear factor graph
shared_ptr<Graph> fg(new Graph( shared_ptr<example::Graph> fg(new example::Graph(
createReallyNonlinearFactorGraph())); example::createReallyNonlinearFactorGraph()));
// config far from minimum // config far from minimum
Point2 x0(3,0); Point2 x0(3,0);
boost::shared_ptr<Config> config(new Config); boost::shared_ptr<example::Config> config(new example::Config);
config->insert(simulated2D::PoseKey(1), x0); config->insert(simulated2D::PoseKey(1), x0);
// ordering // ordering
@ -139,18 +138,18 @@ TEST( NonlinearOptimizer, iterateLM )
/* ************************************************************************* */ /* ************************************************************************* */
TEST( NonlinearOptimizer, optimize ) TEST( NonlinearOptimizer, optimize )
{ {
shared_ptr<Graph> fg(new Graph( shared_ptr<example::Graph> fg(new example::Graph(
createReallyNonlinearFactorGraph())); example::createReallyNonlinearFactorGraph()));
// test error at minimum // test error at minimum
Point2 xstar(0,0); Point2 xstar(0,0);
Config cstar; example::Config cstar;
cstar.insert(simulated2D::PoseKey(1), xstar); cstar.insert(simulated2D::PoseKey(1), 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 = // test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 =
Point2 x0(3,3); Point2 x0(3,3);
boost::shared_ptr<Config> c0(new Config); boost::shared_ptr<example::Config> c0(new example::Config);
c0->insert(simulated2D::PoseKey(1), x0); c0->insert(simulated2D::PoseKey(1), x0);
DOUBLES_EQUAL(199.0,fg->error(*c0),1e-3); DOUBLES_EQUAL(199.0,fg->error(*c0),1e-3);
@ -205,22 +204,23 @@ TEST( NonlinearOptimizer, Factorization )
/* ************************************************************************* */ /* ************************************************************************* */
TEST( NonlinearOptimizer, SubgraphPCG ) TEST( NonlinearOptimizer, SubgraphPCG )
{ {
typedef NonlinearOptimizer<Pose2Graph, Pose2Config, SubgraphPreconditioner, using namespace pose2SLAM;
SubgraphPCG<Pose2Graph, Pose2Config> > Optimizer; typedef SubgraphPCG<Graph, Config> Solver;
typedef NonlinearOptimizer<Graph, Config, SubgraphPreconditioner, Solver> Optimizer;
// Create a graph // Create a graph
boost::shared_ptr<Pose2Graph> graph(new Pose2Graph); boost::shared_ptr<Graph> graph(new Graph);
graph->addPrior(1, Pose2(0., 0., 0.), Isotropic::Sigma(3, 1e-10)); graph->addPrior(1, Pose2(0., 0., 0.), Isotropic::Sigma(3, 1e-10));
graph->addConstraint(1, 2, Pose2(1., 0., 0.), Isotropic::Sigma(3, 1)); graph->addConstraint(1, 2, Pose2(1., 0., 0.), Isotropic::Sigma(3, 1));
// Create an initial config // Create an initial config
boost::shared_ptr<Pose2Config> config(new Pose2Config); boost::shared_ptr<Config> config(new Config);
config->insert(1, Pose2(0., 0., 0.)); config->insert(1, Pose2(0., 0., 0.));
config->insert(2, Pose2(1.5, 0., 0.)); config->insert(2, Pose2(1.5, 0., 0.));
// Create solver and optimizer // Create solver and optimizer
Optimizer::shared_solver solver Optimizer::shared_solver solver
(new SubgraphPCG<Pose2Graph, Pose2Config> (*graph, *config)); (new SubgraphPCG<Graph, Config> (*graph, *config));
Optimizer optimizer(graph, config, solver); Optimizer optimizer(graph, config, solver);
// Optimize !!!! // Optimize !!!!
@ -230,7 +230,7 @@ TEST( NonlinearOptimizer, SubgraphPCG )
absoluteThreshold, Optimizer::SILENT); absoluteThreshold, Optimizer::SILENT);
// Check solution // Check solution
Pose2Config expected; Config expected;
expected.insert(1, Pose2(0., 0., 0.)); expected.insert(1, Pose2(0., 0., 0.));
expected.insert(2, Pose2(1., 0., 0.)); expected.insert(2, Pose2(1., 0., 0.));
CHECK(assert_equal(expected, *optimized.config(), 1e-5)); CHECK(assert_equal(expected, *optimized.config(), 1e-5));