diff --git a/.cproject b/.cproject index cee43f9a7..42bf72538 100644 --- a/.cproject +++ b/.cproject @@ -302,6 +302,7 @@ make + all true true @@ -309,6 +310,7 @@ make + clean true true @@ -316,6 +318,7 @@ make + check true true @@ -323,6 +326,7 @@ make + testGaussianConditional.run true true @@ -330,6 +334,7 @@ make + testGaussianFactor.run true true @@ -337,6 +342,7 @@ make + timeGaussianFactor.run true true @@ -344,6 +350,7 @@ make + timeVectorConfig.run true true @@ -351,6 +358,7 @@ make + testVectorBTree.run true true @@ -358,6 +366,7 @@ make + testVectorMap.run true true @@ -365,6 +374,7 @@ make + testNoiseModel.run true true @@ -372,6 +382,7 @@ make + testBayesNetPreconditioner.run true true @@ -379,6 +390,7 @@ make + testErrors.run true false @@ -386,7 +398,6 @@ make - check true true @@ -394,7 +405,6 @@ make - tests/testSPQRUtil.run true true @@ -402,7 +412,6 @@ make - clean true true @@ -410,6 +419,7 @@ make + check true true @@ -417,6 +427,7 @@ make + tests/testGaussianJunctionTree.run true true @@ -424,7 +435,6 @@ make - check true true @@ -432,7 +442,6 @@ make - clean true true @@ -440,7 +449,6 @@ make - testBTree.run true true @@ -448,7 +456,6 @@ make - testDSF.run true true @@ -456,7 +463,6 @@ make - testDSFVector.run true true @@ -464,7 +470,6 @@ make - testMatrix.run true true @@ -472,7 +477,6 @@ make - testSPQRUtil.run true true @@ -480,7 +484,6 @@ make - testVector.run true true @@ -488,7 +491,6 @@ make - timeMatrix.run true true @@ -496,7 +498,6 @@ make - all true true @@ -504,6 +505,7 @@ make + all true true @@ -511,6 +513,7 @@ make + clean true true @@ -526,6 +529,7 @@ make + testBayesTree.run true false @@ -533,6 +537,7 @@ make + testBinaryBayesNet.run true false @@ -540,6 +545,7 @@ make + testFactorGraph.run true true @@ -547,6 +553,7 @@ make + testISAM.run true true @@ -554,6 +561,7 @@ make + testJunctionTree.run true true @@ -561,6 +569,7 @@ make + testKey.run true true @@ -568,6 +577,7 @@ make + testOrdering.run true true @@ -575,6 +585,7 @@ make + testSymbolicBayesNet.run true false @@ -582,6 +593,7 @@ make + testSymbolicFactor.run true false @@ -589,6 +601,7 @@ make + testSymbolicFactorGraph.run true false @@ -596,6 +609,7 @@ make + timeSymbolMaps.run true true @@ -603,6 +617,7 @@ make + check true true @@ -610,6 +625,7 @@ make + testClusterTree.run true true @@ -617,6 +633,7 @@ make + testJunctionTree.run true true @@ -624,6 +641,7 @@ make + testEliminationTree.run true true @@ -631,7 +649,6 @@ make - check true true @@ -639,7 +656,6 @@ make - testGaussianFactorGraph.run true true @@ -647,7 +663,6 @@ make - testGaussianISAM.run true true @@ -655,7 +670,6 @@ make - testGaussianISAM2.run true true @@ -663,7 +677,6 @@ make - testGraph.run true false @@ -671,7 +684,6 @@ make - testIterative.run true true @@ -679,7 +691,6 @@ make - testNonlinearEquality.run true true @@ -687,7 +698,6 @@ make - testNonlinearFactor.run true true @@ -695,7 +705,6 @@ make - testNonlinearFactorGraph.run true true @@ -703,7 +712,6 @@ make - testNonlinearOptimizer.run true true @@ -711,7 +719,6 @@ make - testSQP.run true true @@ -719,7 +726,6 @@ make - testSubgraphPreconditioner.run true true @@ -727,7 +733,6 @@ make - testTupleConfig.run true true @@ -735,7 +740,6 @@ make - timeGaussianFactorGraph.run true true @@ -743,7 +747,6 @@ make - testBayesNetPreconditioner.run true true @@ -751,7 +754,6 @@ make - testConstraintOptimizer.run true true @@ -759,7 +761,6 @@ make - testInference.run true false @@ -767,7 +768,6 @@ make - testGaussianBayesNet.run true false @@ -775,7 +775,6 @@ make - testGaussianFactor.run true false @@ -783,7 +782,6 @@ make - testJunctionTree.run true false @@ -791,7 +789,6 @@ make - testSymbolicBayesNet.run true false @@ -799,7 +796,6 @@ make - testSymbolicFactorGraph.run true false @@ -807,6 +803,7 @@ make + clean true true @@ -814,6 +811,7 @@ make + all true true @@ -821,6 +819,7 @@ make + testNonlinearConstraint.run true true @@ -828,6 +827,7 @@ make + testLieConfig.run true true @@ -835,6 +835,7 @@ make + testConstraintOptimizer.run true true @@ -842,6 +843,7 @@ make + install true true @@ -849,6 +851,7 @@ make + clean true true @@ -856,7 +859,6 @@ make - all true true @@ -864,7 +866,6 @@ make - clean true true @@ -872,7 +873,6 @@ make - all true true @@ -880,7 +880,6 @@ make - clean true true @@ -888,7 +887,6 @@ make - all true true @@ -896,7 +894,6 @@ make - clean true true @@ -904,6 +901,7 @@ make + all true true @@ -911,6 +909,7 @@ make + clean true true @@ -918,7 +917,6 @@ make - clean all true true @@ -926,7 +924,6 @@ make - all true true @@ -934,7 +931,6 @@ make - check true true @@ -942,7 +938,6 @@ make - clean true true @@ -950,7 +945,6 @@ make - testPlanarSLAM.run true true @@ -958,7 +952,6 @@ make - testPose2Config.run true true @@ -966,7 +959,6 @@ make - testPose2Factor.run true true @@ -974,7 +966,6 @@ make - testPose2Prior.run true true @@ -982,7 +973,6 @@ make - testPose2SLAM.run true true @@ -990,7 +980,6 @@ make - testPose3Config.run true true @@ -998,7 +987,6 @@ make - testPose3SLAM.run true true @@ -1006,7 +994,6 @@ make - testSimulated2DOriented.run true false @@ -1014,7 +1001,6 @@ make - testVSLAMConfig.run true true @@ -1022,7 +1008,6 @@ make - testVSLAMFactor.run true true @@ -1030,7 +1015,6 @@ make - testVSLAMGraph.run true true @@ -1038,7 +1022,6 @@ make - testPose3Factor.run true true @@ -1046,7 +1029,6 @@ make - testSimulated2D.run true false @@ -1054,7 +1036,6 @@ make - testSimulated3D.run true false @@ -1062,7 +1043,6 @@ make - check true true @@ -1070,6 +1050,7 @@ make + timeCalibratedCamera.run true true @@ -1077,6 +1058,7 @@ make + timeRot3.run true true @@ -1084,6 +1066,7 @@ make + clean true true @@ -1091,15 +1074,6 @@ make - - check - true - true - true - - - make - check true true @@ -1123,11 +1097,111 @@ make + clean true true true + + make + check + true + true + true + + + make + + tests/testLieConfig.run + 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 @@ -1154,7 +1228,6 @@ make - all true true @@ -1162,110 +1235,14 @@ 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 @@ -1273,6 +1250,7 @@ make + testGaussianJunctionTree.run true true @@ -1280,6 +1258,7 @@ make + testGaussianFactorGraph.run true true @@ -1287,6 +1266,7 @@ make + timeGaussianFactorGraph.run true true @@ -1294,7 +1274,6 @@ make - testTupleConfig.run true true @@ -1302,7 +1281,6 @@ make - testFusionTupleConfig.run true true @@ -1310,6 +1288,7 @@ make + check true true @@ -1317,6 +1296,7 @@ make + clean true true @@ -1324,6 +1304,7 @@ make + install true true @@ -1331,6 +1312,7 @@ make + all true true diff --git a/nonlinear/FusionTupleConfig.h b/nonlinear/FusionTupleConfig.h index b9298b29c..e96162200 100644 --- a/nonlinear/FusionTupleConfig.h +++ b/nonlinear/FusionTupleConfig.h @@ -14,11 +14,11 @@ #include #include #include +#include #include #include #include - #include "Testable.h" #include "LieConfig.h" @@ -50,13 +50,11 @@ public: /** direct initialization of the underlying structure */ FusionTupleConfig(const Configs& cfg_set) : base_tuple_(cfg_set) {} - /** initialization by slicing a larger config - BROKEN */ + /** initialization from arbitrary other configs */ template FusionTupleConfig(const FusionTupleConfig& other) - //: base_tuple_(other.base_tuple()) // fails + : base_tuple_(boost::fusion::fold(other.base_tuple(), Configs(), assign_outer())) { -// this->base_tuple_ = other.base_tuple(); // fails -// Configs val = other.base_tuple(); // also fails } virtual ~FusionTupleConfig() {} @@ -102,15 +100,16 @@ public: /** equals */ bool equals(const FusionTupleConfig& other, double tol=1e-9) const { + FusionTupleConfig::equals_helper helper(tol); return boost::fusion::all( boost::fusion::zip( boost::fusion::as_vector(base_tuple_), boost::fusion::as_vector(other.base_tuple_)), - FusionTupleConfig::equals_helper(tol)); + helper); } /** direct access to the underlying fusion set - don't use this */ - const Configs & base_tuple() const { return base_tuple_; } + const BaseTuple & base_tuple() const { return base_tuple_; } private: @@ -155,6 +154,33 @@ private: } }; + /** two separate function objects for arbitrary copy construction */ + template + struct assign_inner { + typedef Ret result_type; + + Config config; + assign_inner(const Config& cfg) : config(cfg) {} + + template + result_type operator()(const T& t, const result_type& s) const { + result_type new_s(s); + T new_cfg(config); + if (!new_cfg.empty()) boost::fusion::at_key(new_s) = new_cfg; + return new_s; + } + }; + + template + struct assign_outer { + typedef Ret result_type; + + template // T is the config from the "other" config + Ret operator()(const T& t, const Ret& s) const { + assign_inner helper(t); + return boost::fusion::fold(s, s, helper); // loop over the "self" config + } + }; }; diff --git a/nonlinear/LieConfig.h b/nonlinear/LieConfig.h index cd59a2cfa..ef93aae62 100644 --- a/nonlinear/LieConfig.h +++ b/nonlinear/LieConfig.h @@ -58,6 +58,8 @@ namespace gtsam { LieConfig() {} LieConfig(const LieConfig& config) : values_(config.values_) {} + template + LieConfig(const LieConfig& other) {} // do nothing when initializing with wrong type virtual ~LieConfig() {} /** print */ diff --git a/nonlinear/tests/testLieConfig.cpp b/nonlinear/tests/testLieConfig.cpp index e55126f08..0fc824573 100644 --- a/nonlinear/tests/testLieConfig.cpp +++ b/nonlinear/tests/testLieConfig.cpp @@ -230,6 +230,29 @@ TEST(LieConfig, update) expected.insert("v2", Vector_(1, -2.)); CHECK(assert_equal(expected,config0)); } + +/* ************************************************************************* */ +TEST(LieConfig, dummy_initialization) +{ + typedef TypedSymbol Key; + typedef LieConfig Config1; + typedef LieConfig Config2; + + Config1 init1; + init1.insert(Key(1), Vector_(2, 1.0, 2.0)); + init1.insert(Key(2), Vector_(2, 4.0, 3.0)); + + Config2 init2; + init2.insert("v1", Vector_(2, 1.0, 2.0)); + init2.insert("v2", Vector_(2, 4.0, 3.0)); + + Config1 actual1(init2); + Config2 actual2(init1); + + EXPECT(actual1.empty()); + EXPECT(actual2.empty()); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/tests/testFusionTupleConfig.cpp b/tests/testFusionTupleConfig.cpp index 07a348532..789c25ecb 100644 --- a/tests/testFusionTupleConfig.cpp +++ b/tests/testFusionTupleConfig.cpp @@ -3,13 +3,15 @@ * @author Alex Cunningham */ +#include + #include #include #include #include #include -#include "Key.h" +#include #include #include @@ -126,8 +128,8 @@ TEST( testFusionTupleConfig, slicing ) { cfg_pair.insert(l2, l2_val); // initialize configs by slicing a larger config - TupPointConfig cfg_point(cfg_pair); TupPoseConfig cfg_pose(cfg_pair); + TupPointConfig cfg_point(cfg_pair); PointConfig expPointConfig; expPointConfig.insert(l1, l1_val); @@ -137,9 +139,19 @@ TEST( testFusionTupleConfig, slicing ) { expPoseConfig.insert(x1, x1_val); expPoseConfig.insert(x2, x2_val); - // verify - slicing at initialization isn't implemented -// EXPECT(assert_equal(expPointConfig, cfg_point.config())); -// EXPECT(assert_equal(expPoseConfig, cfg_pose.config())); + // verify + EXPECT(assert_equal(expPointConfig, cfg_point.config())); + EXPECT(assert_equal(expPoseConfig, cfg_pose.config())); +} + +/* ************************************************************************* */ +TEST( testFusionTupleConfig, upgrading ) { + TupPoseConfig small; + small.insert(x1, x1_val); + + TupPairConfig actual(small); + EXPECT(actual.size() == 1); + EXPECT(assert_equal(x1_val, actual.at(x1), tol)); } /* ************************************************************************* */ @@ -167,29 +179,6 @@ TEST( testFusionTupleConfig, equals ) { EXPECT(assert_equal(c5, c6, 1e-5)); } -/* ************************************************************************* */ -TEST( testFusionTupleConfig, fusion_slicing ) { - typedef fusion::set Set1; - typedef fusion::set Set2; - Set1 s1(1, 2.0, true); - - // assignment - EXPECT(fusion::at_key(s1) == 1); - fusion::at_key(s1) = 2; - EXPECT(fusion::at_key(s1) == 2); - - // slicing: big set sliced to a small set - Set2 s2 = s1; // by assignment - EXPECT_DOUBLES_EQUAL(2.0, fusion::at_key(s2), tol); - - Set2 s3(s1); // by initialization - EXPECT_DOUBLES_EQUAL(2.0, fusion::at_key(s3), tol); - - // upgrading: small set initializing a big set -// Set2 s3(5); -// Set1 s4 = s3; // apparently can't do this -} - /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */