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

FusionTupleConfigs work correctly for optimization

release/4.3a0
Alex Cunningham 2010-08-16 19:19:50 +00:00
parent 977155d133
commit 30c3f46b52
2 changed files with 49 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
nonlinear/FusionTupleConfig.h
View File

@ -348,6 +348,7 @@ struct FusionTupleConfig1 : public FusionTupleConfig<boost::fusion::set<C1> > {
typedef C1 Config1;
FusionTupleConfig1() {}
FusionTupleConfig1(const Base& c) : Base(c) {}
FusionTupleConfig1(const C1& c1) : Base(boost::fusion::make_set(c1)) {}
const Config1& first() const { return boost::fusion::at_key<C1>(this->base_tuple_); }
@ -362,6 +363,7 @@ struct FusionTupleConfig2 : public FusionTupleConfig<boost::fusion::set<C1, C2>
typedef C2 Config2;
FusionTupleConfig2() {}
FusionTupleConfig2(const Base& c) : Base(c) {}
FusionTupleConfig2(const C1& c1, const C2& c2) : Base(boost::fusion::make_set(c1, c2)) {}
const Config1& first() const { return boost::fusion::at_key<C1>(this->base_tuple_); }
@ -377,6 +379,7 @@ struct FusionTupleConfig3 : public FusionTupleConfig<boost::fusion::set<C1, C2,
typedef C3 Config3;
FusionTupleConfig3() {}
FusionTupleConfig3(const Base& c) : Base(c) {}
FusionTupleConfig3(const C1& c1, const C2& c2, const C3& c3)
: Base(boost::fusion::make_set(c1, c2, c3)) {}

50
tests/testFusionTupleConfig.cpp
View File

@ -11,14 +11,16 @@
#include <Pose2.h>
#include <Point2.h>
#include <Pose3.h>
#include <Point3.h>
#include <Key.h>
#include <Vector.h>
#include <Key.h>
#include <VectorConfig.h>
#include "BearingRangeFactor.h"
#include "PriorFactor.h"
#include "BetweenFactor.h"
#include <NonlinearFactorGraph-inl.h>
#include <LieConfig-inl.h>
#include <NonlinearOptimizer-inl.h>
#include <FusionTupleConfig.h>
using namespace boost;
@ -471,6 +473,46 @@ TEST( testFusionTupleConfig, configN)
EXPECT(assert_equal(cfg2, cfg3));
}
/* ************************************************************************* */
TEST( testFusionTupleConfig, basic_factor)
{
// planar example system
typedef FusionTupleConfig2<PoseConfig, PointConfig> Config; // pair config
typedef FusionTupleConfig1<PoseConfig> TestPoseConfig;
typedef NonlinearFactorGraph<Config> Graph;
typedef NonlinearOptimizer<Graph,Config> Optimizer;
// Factors
// typedef PriorFactor<TestPoseConfig, PoseKey, Pose2> Prior; // fails to add to graph
typedef PriorFactor<Config, PoseKey, Pose2> Prior;
typedef BetweenFactor<Config, PoseKey, Pose2> Odometry;
typedef BearingRangeFactor<Config, PoseKey, PointKey> BearingRange;
PoseKey pose1k(1), pose2k(2), pose3k(3);
Pose2 pose1, pose2(2.0, 0.0, 0.0), pose3(4.0, 0.0, 0.0);
SharedDiagonal prior_model = noiseModel::Isotropic::Sigma(3, 0.1);
SharedDiagonal odom_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1));
Graph graph;
graph.add(Prior(pose1k, pose1, prior_model));
graph.add(Odometry(pose1k, pose2k, between(pose1, pose2), odom_model));
graph.add(Odometry(pose2k, pose3k, between(pose2, pose3), odom_model));
Config init;
init.insert(pose1k, Pose2(0.2, 0.4, 0.0));
init.insert(pose2k, Pose2(1.8,-0.4, 0.3));
init.insert(pose3k, Pose2(4.1, 0.0,-0.2));
Optimizer::shared_config actual = Optimizer::optimizeLM(graph, init);
Config expected;
expected.insert(pose1k, pose1);
expected.insert(pose2k, pose2);
expected.insert(pose3k, pose3);
EXPECT(assert_equal(expected, *actual, tol));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */