diff --git a/.cproject b/.cproject index f8214daad..cee43f9a7 100644 --- a/.cproject +++ b/.cproject @@ -5,46 +5,47 @@ - - - - - - - - - - + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -298,1008 +299,1048 @@ - - -make - -all -true -true -true - - -make - -clean -true -true -true - - -make - -check -true -true -true - - -make - -testGaussianConditional.run -true -true -true - - -make - -testGaussianFactor.run -true -true -true - - -make - -timeGaussianFactor.run -true -true -true - - -make - -timeVectorConfig.run -true -true -true - - -make - -testVectorBTree.run -true -true -true - - -make - -testVectorMap.run -true -true -true - - -make - -testNoiseModel.run -true -true -true - - -make - -testBayesNetPreconditioner.run -true -true -true - - -make - -testErrors.run -true -false -true - - -make -check -true -true -true - - -make -tests/testSPQRUtil.run -true -true -true - - -make -clean -true -true -true - - -make - -check -true -true -true - - -make - -tests/testGaussianJunctionTree.run -true -true -true - - -make -check -true -true -true - - -make -clean -true -true -true - - -make -testBTree.run -true -true -true - - -make -testDSF.run -true -true -true - - -make -testDSFVector.run -true -true -true - - -make -testMatrix.run -true -true -true - - -make -testSPQRUtil.run -true -true -true - - -make -testVector.run -true -true -true - - -make -timeMatrix.run -true -true -true - - -make -all -true -true -true - - -make - -all -true -true -true - - -make - -clean -true -true -true - - -make --k -check -true -false -true - - -make - -testBayesTree.run -true -false -true - - -make - -testBinaryBayesNet.run -true -false -true - - -make - -testFactorGraph.run -true -true -true - - -make - -testISAM.run -true -true -true - - -make - -testJunctionTree.run -true -true -true - - -make - -testKey.run -true -true -true - - -make - -testOrdering.run -true -true -true - - -make - -testSymbolicBayesNet.run -true -false -true - - -make - -testSymbolicFactor.run -true -false -true - - -make - -testSymbolicFactorGraph.run -true -false -true - - -make - -timeSymbolMaps.run -true -true -true - - -make - -check -true -true -true - - -make - -testClusterTree.run -true -true -true - - -make - -testJunctionTree.run -true -true -true - - -make - -testEliminationTree.run -true -true -true - - -make -check -true -true -true - - -make -testGaussianFactorGraph.run -true -true -true - - -make -testGaussianISAM.run -true -true -true - - -make -testGaussianISAM2.run -true -true -true - - -make -testGraph.run -true -false -true - - -make -testIterative.run -true -true -true - - -make -testNonlinearEquality.run -true -true -true - - -make -testNonlinearFactor.run -true -true -true - - -make -testNonlinearFactorGraph.run -true -true -true - - -make -testNonlinearOptimizer.run -true -true -true - - -make -testSQP.run -true -true -true - - -make -testSubgraphPreconditioner.run -true -true -true - - -make -testTupleConfig.run -true -true -true - - -make -timeGaussianFactorGraph.run -true -true -true - - -make -testBayesNetPreconditioner.run -true -true -true - - -make -testConstraintOptimizer.run -true -true -true - - -make -testInference.run -true -false -true - - -make -testGaussianBayesNet.run -true -false -true - - -make -testGaussianFactor.run -true -false -true - - -make -testJunctionTree.run -true -false -true - - -make -testSymbolicBayesNet.run -true -false -true - - -make -testSymbolicFactorGraph.run -true -false -true - - -make - -clean -true -true -true - - -make - -all -true -true -true - - -make - -testNonlinearConstraint.run -true -true -true - - -make - -testLieConfig.run -true -true -true - - -make - -testConstraintOptimizer.run -true -true -true - - -make - -install -true -true -true - - -make - -clean -true -true -true - - -make -all -true -true -true - - -make -clean -true -true -true - - -make -all -true -true -true - - -make -clean -true -true -true - - -make -all -true -true -true - - -make -clean -true -true -true - - -make - -all -true -true -true - - -make - -clean -true -true -true - - -make -clean all -true -true -true - - -make -all -true -true -true - - -make -check -true -true -true - - -make -clean -true -true -true - - -make -testPlanarSLAM.run -true -true -true - - -make -testPose2Config.run -true -true -true - - -make -testPose2Factor.run -true -true -true - - -make -testPose2Prior.run -true -true -true - - -make -testPose2SLAM.run -true -true -true - - -make -testPose3Config.run -true -true -true - - -make -testPose3SLAM.run -true -true -true - - -make -testSimulated2DOriented.run -true -false -true - - -make -testVSLAMConfig.run -true -true -true - - -make -testVSLAMFactor.run -true -true -true - - -make -testVSLAMGraph.run -true -true -true - - -make -testPose3Factor.run -true -true -true - - -make -testSimulated2D.run -true -false -true - - -make -testSimulated3D.run -true -false -true - - -make -check -true -true -true - - -make - -timeCalibratedCamera.run -true -true -true - - -make - -timeRot3.run -true -true -true - - -make - -clean -true -true -true - - -make -check -true -true -true - - -make --j2 -install -true -true -true - - -make --j2 -check -true -true -true - - -make - -clean -true -true -true - - -make -check -true -true -true - - -make -testRot3.run -true -true -true - - -make -testRot2.run -true -true -true - - -make -testPose3.run -true -true -true - - -make -timeRot3.run -true -true -true - - -make -testPose2.run -true -true -true - - -make -testCal3_S2.run -true -true -true - - -make -testSimpleCamera.run -true -true -true - - -make -testHomography2.run -true -true -true - - -make -testCalibratedCamera.run -true -true -true - - -make -check -true -true -true - - -make -clean -true -true -true - - -make -testPoint2.run -true -true -true - - -make --j2 -install -true -true -true - - -make --j2 -clean -true -true -true - - -make --j2 -check -true -true -true - - -make -all -true -true -true - - -make -dist -true -true -true - - -make - -check -true -true -true - - -make - -testGaussianJunctionTree.run -true -true -true - - -make - -testGaussianFactorGraph.run -true -true -true - - -make - -timeGaussianFactorGraph.run -true -true -true - - -make - -check -true -true -true - - -make - -clean -true -true -true - - -make - -install -true -true -true - - -make - -all -true -true -true - - - - + + + make + all + true + true + true + + + make + clean + true + true + true + + + make + check + true + true + true + + + make + testGaussianConditional.run + true + true + true + + + make + testGaussianFactor.run + true + true + true + + + make + timeGaussianFactor.run + true + true + true + + + make + timeVectorConfig.run + true + true + true + + + make + testVectorBTree.run + true + true + true + + + make + testVectorMap.run + true + true + true + + + make + testNoiseModel.run + true + true + true + + + make + testBayesNetPreconditioner.run + true + true + true + + + make + testErrors.run + true + false + true + + + make + + check + true + true + true + + + make + + tests/testSPQRUtil.run + true + true + true + + + make + + clean + true + true + true + + + make + check + true + true + true + + + make + tests/testGaussianJunctionTree.run + true + true + true + + + make + + check + true + true + true + + + make + + clean + true + true + true + + + make + + testBTree.run + true + true + true + + + make + + testDSF.run + true + true + true + + + make + + testDSFVector.run + true + true + true + + + make + + testMatrix.run + true + true + true + + + make + + testSPQRUtil.run + true + true + true + + + make + + testVector.run + true + true + true + + + make + + timeMatrix.run + true + true + true + + + make + + all + true + true + true + + + make + all + true + true + true + + + make + clean + true + true + true + + + make + -k + check + true + false + true + + + make + testBayesTree.run + true + false + true + + + make + testBinaryBayesNet.run + true + false + true + + + make + testFactorGraph.run + true + true + true + + + make + testISAM.run + true + true + true + + + make + testJunctionTree.run + true + true + true + + + make + testKey.run + true + true + true + + + make + testOrdering.run + true + true + true + + + make + testSymbolicBayesNet.run + true + false + true + + + make + testSymbolicFactor.run + true + false + true + + + make + testSymbolicFactorGraph.run + true + false + true + + + make + timeSymbolMaps.run + true + true + true + + + make + check + true + true + true + + + make + testClusterTree.run + true + true + true + + + make + testJunctionTree.run + true + true + true + + + make + testEliminationTree.run + true + true + true + + + make + + check + true + true + true + + + make + + testGaussianFactorGraph.run + true + true + true + + + make + + testGaussianISAM.run + true + true + true + + + make + + testGaussianISAM2.run + true + true + true + + + make + + testGraph.run + true + false + true + + + make + + testIterative.run + true + true + true + + + make + + testNonlinearEquality.run + true + true + true + + + make + + testNonlinearFactor.run + true + true + true + + + make + + testNonlinearFactorGraph.run + true + true + true + + + make + + testNonlinearOptimizer.run + true + true + true + + + make + + testSQP.run + true + true + true + + + make + + testSubgraphPreconditioner.run + true + true + true + + + make + + testTupleConfig.run + true + true + true + + + make + + timeGaussianFactorGraph.run + true + true + true + + + make + + testBayesNetPreconditioner.run + true + true + true + + + make + + testConstraintOptimizer.run + true + true + true + + + make + + testInference.run + true + false + true + + + make + + testGaussianBayesNet.run + true + false + true + + + make + + testGaussianFactor.run + true + false + true + + + make + + testJunctionTree.run + true + false + true + + + make + + testSymbolicBayesNet.run + true + false + true + + + make + + testSymbolicFactorGraph.run + true + false + true + + + make + clean + true + true + true + + + make + all + true + true + true + + + make + testNonlinearConstraint.run + true + true + true + + + make + testLieConfig.run + true + true + true + + + make + testConstraintOptimizer.run + true + true + true + + + make + install + true + true + true + + + make + clean + true + true + true + + + make + + all + true + true + true + + + make + + clean + true + true + true + + + make + + all + true + true + true + + + make + + clean + true + true + true + + + make + + all + true + true + true + + + make + + clean + true + true + true + + + make + all + true + true + true + + + make + clean + true + true + true + + + make + + clean all + true + true + true + + + make + + all + true + true + true + + + make + + check + true + true + true + + + make + + clean + true + true + true + + + make + + testPlanarSLAM.run + true + true + true + + + make + + testPose2Config.run + true + true + true + + + make + + testPose2Factor.run + true + true + true + + + make + + testPose2Prior.run + true + true + true + + + make + + testPose2SLAM.run + true + true + true + + + make + + testPose3Config.run + true + true + true + + + make + + testPose3SLAM.run + true + true + true + + + make + + testSimulated2DOriented.run + true + false + true + + + make + + testVSLAMConfig.run + true + true + true + + + make + + testVSLAMFactor.run + true + true + true + + + make + + testVSLAMGraph.run + true + true + true + + + make + + testPose3Factor.run + true + true + true + + + make + + testSimulated2D.run + true + false + true + + + make + + testSimulated3D.run + true + false + true + + + make + + check + true + true + true + + + make + timeCalibratedCamera.run + true + true + true + + + make + timeRot3.run + true + true + true + + + make + clean + true + true + true + + + make + + check + true + true + true + + + make + + check + true + true + true + + + make + -j2 + install + true + true + true + + + make + -j2 + check + true + true + true + + + make + clean + true + true + true + + + make + -j2 + install + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + check + true + true + true + + + make + + all + true + true + true + + + make + + dist + true + true + true + + + make + + testRot3.run + true + true + true + + + make + + testRot2.run + true + true + true + + + make + + testPose3.run + true + true + true + + + make + + timeRot3.run + true + true + true + + + make + + testPose2.run + true + true + true + + + make + + testCal3_S2.run + true + true + true + + + make + + testSimpleCamera.run + true + true + true + + + make + + testHomography2.run + true + true + true + + + make + + testCalibratedCamera.run + true + true + true + + + make + + check + true + true + true + + + make + + clean + true + true + true + + + make + + testPoint2.run + true + true + true + + + make + check + true + true + true + + + make + testGaussianJunctionTree.run + true + true + true + + + make + testGaussianFactorGraph.run + true + true + true + + + make + timeGaussianFactorGraph.run + true + true + true + + + make + + testTupleConfig.run + true + true + true + + + make + + testFusionTupleConfig.run + true + true + true + + + make + check + true + true + true + + + make + clean + true + true + true + + + make + install + true + true + true + + + make + all + true + true + true + + + + - - + + diff --git a/nonlinear/FusionTupleConfig.h b/nonlinear/FusionTupleConfig.h new file mode 100644 index 000000000..fdc3a6371 --- /dev/null +++ b/nonlinear/FusionTupleConfig.h @@ -0,0 +1,107 @@ +/** + * @file FusionTupleConfig.h + * @brief Experimental tuple config using boost.Fusion + * @author Alex Cunningham + */ + +#pragma once + +#include +#include +#include +#include +#include + +#include + +namespace gtsam { + +/** + * This config uses a real tuple to store types + * The template parameter should be a boost fusion structure, like + * set + */ +template +class FusionTupleConfig { + +public: + /** useful types */ + typedef Configs BaseTuple; + +protected: + /** the underlying tuple storing everything */ + Configs base_tuple_; + +public: + /** create an empty config */ + FusionTupleConfig() {} + + /** copy constructor */ + FusionTupleConfig(const FusionTupleConfig& other) : base_tuple_(other.base_tuple_) {} + + /** direct initialization of the underlying structure */ + FusionTupleConfig(const Configs& cfg_set) : base_tuple_(cfg_set) {} + + /** initialization by slicing a larger config */ + template + FusionTupleConfig(const FusionTupleConfig& other) { + // use config subinsert and fusion::foreach + } + + virtual ~FusionTupleConfig() {} + + /** insertion */ + template + void insert(const Key& j, const Value& x) { + boost::fusion::at_key >(base_tuple_).insert(j,x); + } + + /** retrieve a point */ + template + const typename Key::Value_t & at(const Key& j) const { + return boost::fusion::at_key >(base_tuple_).at(j); + } + + /** retrieve a full config */ + template + const Config & config() const { + return boost::fusion::at_key(base_tuple_); + } + + /** size of the config - sum all sizes from subconfigs */ + size_t size() const { + return boost::fusion::accumulate(base_tuple_, 0, FusionTupleConfig::size_helper()); + } + + /** returns true if the config is empty */ + bool empty() const { + return boost::fusion::all(base_tuple_, FusionTupleConfig::empty_helper()); + } + +private: + /** helper structs to make use of fusion algorithms */ + struct size_helper { + typedef size_t result_type; + + template + size_t operator()(const T& t, const size_t& s) const + { + return s + t.size(); + } + }; + + struct empty_helper + { + template + bool operator()(T t) const + { + return t.empty(); + } + }; + + +}; + + +} // \namespace gtsam + diff --git a/nonlinear/Makefile.am b/nonlinear/Makefile.am index 5ae04a057..0cf839716 100644 --- a/nonlinear/Makefile.am +++ b/nonlinear/Makefile.am @@ -12,7 +12,8 @@ check_PROGRAMS = #---------------------------------------------------------------------------------------------------- # Lie Groups -headers += LieConfig.h LieConfig-inl.h TupleConfig.h TupleConfig-inl.h +headers += LieConfig.h LieConfig-inl.h TupleConfig.h TupleConfig-inl.h +headers += FusionTupleConfig.h check_PROGRAMS += tests/testLieConfig # Nonlinear nonlinear diff --git a/tests/Makefile.am b/tests/Makefile.am index b5feb1655..0a03c3382 100644 --- a/tests/Makefile.am +++ b/tests/Makefile.am @@ -13,6 +13,9 @@ check_PROGRAMS += testSymbolicBayesNet testSymbolicFactorGraph testTupleConfig check_PROGRAMS += testNonlinearEqualityConstraint testBoundingConstraint check_PROGRAMS += testTransformConstraint testLinearApproxFactor +# experimental +check_PROGRAMS += testFusionTupleConfig + if USE_LDL check_PROGRAMS += testConstraintOptimizer endif diff --git a/tests/testFusionTupleConfig.cpp b/tests/testFusionTupleConfig.cpp new file mode 100644 index 000000000..087549e1d --- /dev/null +++ b/tests/testFusionTupleConfig.cpp @@ -0,0 +1,119 @@ +/** + * @file testFusionTupleConfig.cpp + * @author Alex Cunningham + */ + +#include + +#include +#include +#include +#include +#include "Key.h" +#include + +#include + +using namespace boost; +using namespace gtsam; +using namespace std; + +static const double tol = 1e-5; + +typedef TypedSymbol PoseKey; +typedef TypedSymbol PointKey; +typedef LieConfig PoseConfig; +typedef LieConfig PointConfig; + +// some generic poses, points and keys +PoseKey x1(1), x2(2); +Pose2 x1_val(1.0, 2.0, 0.3), x2_val(2.0, 3.0, 0.4); +PointKey l1(1), l2(2); +Point2 l1_val(1.0, 2.0), l2_val(3.0, 4.0); + +typedef FusionTupleConfig > TupPointConfig; +typedef FusionTupleConfig > TupPoseConfig; +typedef FusionTupleConfig > TupPairConfig; + +/* ************************************************************************* */ +TEST( testFusionTupleConfig, basic_config ) { + + // some initialized configs to start with + PoseConfig poseConfig1; + poseConfig1.insert(x1, x1_val); + PointConfig pointConfig1; + pointConfig1.insert(l1, l1_val); + + // basic initialization + TupPointConfig cfg1A, cfg2A(pointConfig1); + EXPECT(cfg1A.size() == 0); + EXPECT(cfg2A.size() == 1); + EXPECT(cfg1A.empty()); + EXPECT(!cfg2A.empty()); + + TupPoseConfig cfg1B, cfg2B(poseConfig1); + EXPECT(cfg1B.size() == 0); + EXPECT(cfg2B.size() == 1); + EXPECT(cfg1B.empty()); + EXPECT(!cfg2B.empty()); + + TupPairConfig cfg1, cfg2(fusion::make_set(poseConfig1, pointConfig1)); + EXPECT(cfg1.size() == 0); + EXPECT(cfg1.empty()); + EXPECT(cfg2.size() == 2); + EXPECT(!cfg2.empty()); + + // insertion + cfg1A.insert(l1, l1_val); + EXPECT(cfg1A.size() == 1); + cfg1A.insert(l2, l2_val); + EXPECT(cfg1A.size() == 2); + cfg2A.insert(l2, l2_val); + EXPECT(cfg2A.size() == 2); + + cfg1B.insert(x1, x1_val); + EXPECT(cfg1B.size() == 1); + cfg1B.insert(x2, x2_val); + EXPECT(cfg1B.size() == 2); + cfg2B.insert(x2, x2_val); + EXPECT(cfg2B.size() == 2); + + cfg1.insert(x1, x1_val); + cfg1.insert(x2, x2_val); + cfg1.insert(l1, l1_val); + cfg1.insert(l2, l2_val); + EXPECT(cfg1.size() == 4); + + // retrieval of elements + EXPECT(assert_equal(l1_val, cfg1A.at(l1))); + EXPECT(assert_equal(l2_val, cfg1A.at(l2))); + + EXPECT(assert_equal(x1_val, cfg1B.at(x1))); + EXPECT(assert_equal(x2_val, cfg1B.at(x2))); + + EXPECT(assert_equal(l1_val, cfg1.at(l1))); + EXPECT(assert_equal(l2_val, cfg1.at(l2))); + EXPECT(assert_equal(x1_val, cfg1.at(x1))); + EXPECT(assert_equal(x2_val, cfg1.at(x2))); + + // config extraction + PointConfig expPointConfig; + expPointConfig.insert(l1, l1_val); + expPointConfig.insert(l2, l2_val); + + PoseConfig expPoseConfig; + expPoseConfig.insert(x1, x1_val); + expPoseConfig.insert(x2, x2_val); + + EXPECT(assert_equal(expPointConfig, cfg1A.config())); + EXPECT(assert_equal(expPoseConfig, cfg1B.config())); + + EXPECT(assert_equal(expPointConfig, cfg1.config())); + EXPECT(assert_equal(expPoseConfig, cfg1.config())); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ + +