diff --git a/nonlinear/FusionTupleConfig.h b/nonlinear/FusionTupleConfig.h index e6719c107..3846b9351 100644 --- a/nonlinear/FusionTupleConfig.h +++ b/nonlinear/FusionTupleConfig.h @@ -348,6 +348,7 @@ struct FusionTupleConfig1 : public FusionTupleConfig > { 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(this->base_tuple_); } @@ -362,6 +363,7 @@ struct FusionTupleConfig2 : public FusionTupleConfig 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(this->base_tuple_); } @@ -377,6 +379,7 @@ struct FusionTupleConfig3 : public FusionTupleConfig #include -#include -#include -#include -#include #include #include +#include "BearingRangeFactor.h" +#include "PriorFactor.h" +#include "BetweenFactor.h" + +#include #include +#include #include using namespace boost; @@ -471,6 +473,46 @@ TEST( testFusionTupleConfig, configN) EXPECT(assert_equal(cfg2, cfg3)); } +/* ************************************************************************* */ +TEST( testFusionTupleConfig, basic_factor) +{ + // planar example system + typedef FusionTupleConfig2 Config; // pair config + typedef FusionTupleConfig1 TestPoseConfig; + typedef NonlinearFactorGraph Graph; + typedef NonlinearOptimizer Optimizer; + + // Factors +// typedef PriorFactor Prior; // fails to add to graph + typedef PriorFactor Prior; + typedef BetweenFactor Odometry; + typedef BearingRangeFactor 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); } /* ************************************************************************* */