diff --git a/.cproject b/.cproject index 79ad1df26..7d302b39a 100644 --- a/.cproject +++ b/.cproject @@ -600,6 +600,7 @@ make + tests/testBayesTree.run true false @@ -607,6 +608,7 @@ make + testBinaryBayesNet.run true false @@ -654,6 +656,7 @@ make + testSymbolicBayesNet.run true false @@ -661,6 +664,7 @@ make + tests/testSymbolicFactor.run true false @@ -668,6 +672,7 @@ make + testSymbolicFactorGraph.run true false @@ -683,6 +688,7 @@ make + tests/testBayesTree true false @@ -1034,6 +1040,7 @@ make + testErrors.run true false @@ -1263,6 +1270,46 @@ true true + + make + -j5 + testBTree.run + true + true + true + + + make + -j5 + testDSF.run + true + true + true + + + make + -j5 + testDSFMap.run + true + true + true + + + make + -j5 + testDSFVector.run + true + true + true + + + make + -j5 + testFixedVector.run + true + true + true + make -j2 @@ -1345,7 +1392,6 @@ make - testSimulated2DOriented.run true false @@ -1385,7 +1431,6 @@ make - testSimulated2D.run true false @@ -1393,7 +1438,6 @@ make - testSimulated3D.run true false @@ -1407,46 +1451,6 @@ true true - - make - -j5 - testBTree.run - true - true - true - - - make - -j5 - testDSF.run - true - true - true - - - make - -j5 - testDSFMap.run - true - true - true - - - make - -j5 - testDSFVector.run - true - true - true - - - make - -j5 - testFixedVector.run - true - true - true - make -j5 @@ -1704,6 +1708,7 @@ cpack + -G DEB true false @@ -1711,6 +1716,7 @@ cpack + -G RPM true false @@ -1718,6 +1724,7 @@ cpack + -G TGZ true false @@ -1725,6 +1732,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -2451,6 +2459,7 @@ make + testGraph.run true false @@ -2458,6 +2467,7 @@ make + testJunctionTree.run true false @@ -2465,6 +2475,7 @@ make + testSymbolicBayesNetB.run true false @@ -2534,10 +2545,10 @@ true true - + make -j5 - testBADFactor.run + testExpressionFactor.run true true true @@ -2952,7 +2963,6 @@ make - tests/testGaussianISAM2 true false diff --git a/gtsam_unstable/examples/Pose2SLAMExampleExpressions.cpp b/gtsam_unstable/examples/Pose2SLAMExampleExpressions.cpp index 590e83b70..936c9957b 100644 --- a/gtsam_unstable/examples/Pose2SLAMExampleExpressions.cpp +++ b/gtsam_unstable/examples/Pose2SLAMExampleExpressions.cpp @@ -19,7 +19,7 @@ // The two new headers that allow using our Automatic Differentiation Expression framework #include -#include +#include // Header order is close to far #include @@ -42,19 +42,19 @@ int main(int argc, char** argv) { // 2a. Add a prior on the first pose, setting it to the origin noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1)); - graph.push_back(BADFactor(priorNoise, Pose2(0, 0, 0), x1)); + graph.push_back(ExpressionFactor(priorNoise, Pose2(0, 0, 0), x1)); // For simplicity, we will use the same noise model for odometry and loop closures noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1)); // 2b. Add odometry factors - graph.push_back(BADFactor(model, Pose2(2, 0, 0 ), between(x1,x2))); - graph.push_back(BADFactor(model, Pose2(2, 0, M_PI_2), between(x2,x3))); - graph.push_back(BADFactor(model, Pose2(2, 0, M_PI_2), between(x3,x4))); - graph.push_back(BADFactor(model, Pose2(2, 0, M_PI_2), between(x4,x5))); + graph.push_back(ExpressionFactor(model, Pose2(2, 0, 0 ), between(x1,x2))); + graph.push_back(ExpressionFactor(model, Pose2(2, 0, M_PI_2), between(x2,x3))); + graph.push_back(ExpressionFactor(model, Pose2(2, 0, M_PI_2), between(x3,x4))); + graph.push_back(ExpressionFactor(model, Pose2(2, 0, M_PI_2), between(x4,x5))); // 2c. Add the loop closure constraint - graph.push_back(BADFactor(model, Pose2(2, 0, M_PI_2), between(x5,x2))); + graph.push_back(ExpressionFactor(model, Pose2(2, 0, M_PI_2), between(x5,x2))); graph.print("\nFactor Graph:\n"); // print // 3. Create the data structure to hold the initialEstimate estimate to the solution diff --git a/gtsam_unstable/examples/SFMExampleExpressions.cpp b/gtsam_unstable/examples/SFMExampleExpressions.cpp index c59c4f497..1537af1d9 100644 --- a/gtsam_unstable/examples/SFMExampleExpressions.cpp +++ b/gtsam_unstable/examples/SFMExampleExpressions.cpp @@ -24,7 +24,7 @@ // The two new headers that allow using our Automatic Differentiation Expression framework #include -#include +#include // Header order is close to far #include @@ -57,10 +57,10 @@ int main(int argc, char* argv[]) { // Specify uncertainty on first pose prior noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1))); - // Here we don't use a PriorFactor but directly the BADFactor class + // Here we don't use a PriorFactor but directly the ExpressionFactor class // The object x0 is an Expression, and we create a factor wanting it to be equal to poses[0] Pose3_ x0('x',0); - graph.push_back(BADFactor(poseNoise, poses[0], x0)); + graph.push_back(ExpressionFactor(poseNoise, poses[0], x0)); // We create a constant Expression for the calibration here Cal3_S2_ cK(K); @@ -74,14 +74,14 @@ int main(int argc, char* argv[]) { // Below an expression for the prediction of the measurement: Point3_ p('l', j); Point2_ prediction = uncalibrate(cK, project(transform_to(x, p))); - // Again, here we use a BADFactor - graph.push_back(BADFactor(measurementNoise, measurement, prediction)); + // Again, here we use a ExpressionFactor + graph.push_back(ExpressionFactor(measurementNoise, measurement, prediction)); } } - // Add prior on first point to constrain scale, again with BADFActor + // Add prior on first point to constrain scale, again with ExpressionFactor noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1); - graph.push_back(BADFactor(pointNoise, points[0], Point3_('l', 0))); + graph.push_back(ExpressionFactor(pointNoise, points[0], Point3_('l', 0))); // Create perturbed initial Values initialEstimate; diff --git a/gtsam_unstable/nonlinear/BADFactor.h b/gtsam_unstable/nonlinear/ExpressionFactor.h similarity index 90% rename from gtsam_unstable/nonlinear/BADFactor.h rename to gtsam_unstable/nonlinear/ExpressionFactor.h index c26e31b33..14e9c54ba 100644 --- a/gtsam_unstable/nonlinear/BADFactor.h +++ b/gtsam_unstable/nonlinear/ExpressionFactor.h @@ -24,10 +24,10 @@ namespace gtsam { /** - * BAD Factor that supports arbitrary expressions via AD + * Factor that supports arbitrary expressions via AD */ template -class BADFactor: public NoiseModelFactor { +class ExpressionFactor: public NoiseModelFactor { const T measurement_; const Expression expression_; @@ -35,7 +35,7 @@ class BADFactor: public NoiseModelFactor { public: /// Constructor - BADFactor(const SharedNoiseModel& noiseModel, // + ExpressionFactor(const SharedNoiseModel& noiseModel, // const T& measurement, const Expression& expression) : NoiseModelFactor(noiseModel, expression.keys()), // measurement_(measurement), expression_(expression) { @@ -61,7 +61,7 @@ public: } }; -// BADFactor +// ExpressionFactor } diff --git a/gtsam_unstable/nonlinear/tests/testExpression.cpp b/gtsam_unstable/nonlinear/tests/testExpression.cpp index 3747ed078..04f456ef1 100644 --- a/gtsam_unstable/nonlinear/tests/testExpression.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpression.cpp @@ -77,7 +77,7 @@ TEST(Expression, leaf) { // EXPECT(assert_equal(expected.at(67),a.jacobians().at(67))); //} /* ************************************************************************* */ - +// Binary(Leaf,Unary(Binary(Leaf,Leaf))) TEST(Expression, test) { // Test Constant expression @@ -95,7 +95,7 @@ TEST(Expression, test) { Expression uv_hat(uncalibrate, K, projection); // Check keys - std::set expectedKeys; + set expectedKeys; expectedKeys.insert(1); expectedKeys.insert(2); expectedKeys.insert(3); @@ -111,7 +111,7 @@ TEST(Expression, compose1) { Expression R3 = R1 * R2; // Check keys - std::set expectedKeys; + set expectedKeys; expectedKeys.insert(1); expectedKeys.insert(2); EXPECT(expectedKeys == R3.keys()); @@ -126,7 +126,7 @@ TEST(Expression, compose2) { Expression R3 = R1 * R2; // Check keys - std::set expectedKeys; + set expectedKeys; expectedKeys.insert(1); EXPECT(expectedKeys == R3.keys()); } @@ -140,7 +140,7 @@ TEST(Expression, compose3) { Expression R3 = R1 * R2; // Check keys - std::set expectedKeys; + set expectedKeys; expectedKeys.insert(3); EXPECT(expectedKeys == R3.keys()); } @@ -167,7 +167,7 @@ TEST(Expression, ternary) { Expression ABC(composeThree, A, B, C); // Check keys - std::set expectedKeys; + set expectedKeys; expectedKeys.insert(1); expectedKeys.insert(2); expectedKeys.insert(3); diff --git a/gtsam_unstable/nonlinear/tests/testBADFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp similarity index 89% rename from gtsam_unstable/nonlinear/tests/testBADFactor.cpp rename to gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp index 635a19235..87da6fde9 100644 --- a/gtsam_unstable/nonlinear/tests/testBADFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file testBADFactor.cpp + * @file testExpressionFactor.cpp * @date September 18, 2014 * @author Frank Dellaert * @author Paul Furgale @@ -18,7 +18,7 @@ */ #include -#include +#include #include #include #include @@ -35,7 +35,7 @@ SharedNoiseModel model = noiseModel::Unit::Create(2); /* ************************************************************************* */ // Leaf -TEST(BADFactor, leaf) { +TEST(ExpressionFactor, leaf) { // Create some values Values values; @@ -49,7 +49,7 @@ TEST(BADFactor, leaf) { Point2_ p(2); // Try concise version - BADFactor f(model, Point2(0, 0), p); + ExpressionFactor f(model, Point2(0, 0), p); EXPECT_LONGS_EQUAL(2, f.dim()); boost::shared_ptr gf = f.linearize(values); boost::shared_ptr jf = // @@ -59,7 +59,7 @@ TEST(BADFactor, leaf) { /* ************************************************************************* */ // non-zero noise model -TEST(BADFactor, model) { +TEST(ExpressionFactor, model) { // Create some values Values values; @@ -75,7 +75,7 @@ TEST(BADFactor, model) { // Try concise version SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.01)); - BADFactor f(model, Point2(0, 0), p); + ExpressionFactor f(model, Point2(0, 0), p); EXPECT_LONGS_EQUAL(2, f.dim()); boost::shared_ptr gf = f.linearize(values); boost::shared_ptr jf = // @@ -85,7 +85,7 @@ TEST(BADFactor, model) { /* ************************************************************************* */ // Unary(Leaf)) -TEST(BADFactor, test) { +TEST(ExpressionFactor, test) { // Create some values Values values; @@ -99,7 +99,7 @@ TEST(BADFactor, test) { Point3_ p(2); // Try concise version - BADFactor f(model, measured, project(p)); + ExpressionFactor f(model, measured, project(p)); EXPECT_LONGS_EQUAL(2, f.dim()); boost::shared_ptr gf = f.linearize(values); boost::shared_ptr jf = // @@ -109,7 +109,7 @@ TEST(BADFactor, test) { /* ************************************************************************* */ // Unary(Binary(Leaf,Leaf)) -TEST(BADFactor, test1) { +TEST(ExpressionFactor, test1) { // Create some values Values values; @@ -127,7 +127,7 @@ TEST(BADFactor, test1) { Point3_ p(2); // Try concise version - BADFactor f2(model, measured, project(transform_to(x, p))); + ExpressionFactor f2(model, measured, project(transform_to(x, p))); EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9); EXPECT_LONGS_EQUAL(2, f2.dim()); boost::shared_ptr gf2 = f2.linearize(values); @@ -136,7 +136,7 @@ TEST(BADFactor, test1) { /* ************************************************************************* */ // Binary(Leaf,Unary(Binary(Leaf,Leaf))) -TEST(BADFactor, test2) { +TEST(ExpressionFactor, test2) { // Create some values Values values; @@ -160,14 +160,14 @@ TEST(BADFactor, test2) { Point2_ uv_hat(K, &Cal3_S2::uncalibrate, xy_hat); // Create factor and check value, dimension, linearization - BADFactor f(model, measured, uv_hat); + ExpressionFactor f(model, measured, uv_hat); EXPECT_DOUBLES_EQUAL(expected_error, f.error(values), 1e-9); EXPECT_LONGS_EQUAL(2, f.dim()); boost::shared_ptr gf = f.linearize(values); EXPECT( assert_equal(*expected, *gf, 1e-9)); // Try concise version - BADFactor f2(model, measured, + ExpressionFactor f2(model, measured, uncalibrate(K, project(transform_to(x, p)))); EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9); EXPECT_LONGS_EQUAL(2, f2.dim()); @@ -177,7 +177,7 @@ TEST(BADFactor, test2) { TernaryExpression::Function fff = project6; // Try ternary version - BADFactor f3(model, measured, project3(x, p, K)); + ExpressionFactor f3(model, measured, project3(x, p, K)); EXPECT_DOUBLES_EQUAL(expected_error, f3.error(values), 1e-9); EXPECT_LONGS_EQUAL(2, f3.dim()); boost::shared_ptr gf3 = f3.linearize(values); @@ -186,14 +186,14 @@ TEST(BADFactor, test2) { /* ************************************************************************* */ -TEST(BADFactor, compose1) { +TEST(ExpressionFactor, compose1) { // Create expression Rot3_ R1(1), R2(2); Rot3_ R3 = R1 * R2; // Create factor - BADFactor f(noiseModel::Unit::Create(3), Rot3(), R3); + ExpressionFactor f(noiseModel::Unit::Create(3), Rot3(), R3); // Create some values Values values; @@ -216,14 +216,14 @@ TEST(BADFactor, compose1) { /* ************************************************************************* */ // Test compose with arguments referring to the same rotation -TEST(BADFactor, compose2) { +TEST(ExpressionFactor, compose2) { // Create expression Rot3_ R1(1), R2(1); Rot3_ R3 = R1 * R2; // Create factor - BADFactor f(noiseModel::Unit::Create(3), Rot3(), R3); + ExpressionFactor f(noiseModel::Unit::Create(3), Rot3(), R3); // Create some values Values values; @@ -245,14 +245,14 @@ TEST(BADFactor, compose2) { /* ************************************************************************* */ // Test compose with one arguments referring to a constant same rotation -TEST(BADFactor, compose3) { +TEST(ExpressionFactor, compose3) { // Create expression Rot3_ R1(Rot3::identity()), R2(3); Rot3_ R3 = R1 * R2; // Create factor - BADFactor f(noiseModel::Unit::Create(3), Rot3(), R3); + ExpressionFactor f(noiseModel::Unit::Create(3), Rot3(), R3); // Create some values Values values; @@ -287,14 +287,14 @@ Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, return R1 * (R2 * R3); } -TEST(BADFactor, composeTernary) { +TEST(ExpressionFactor, composeTernary) { // Create expression Rot3_ A(1), B(2), C(3); Rot3_ ABC(composeThree, A, B, C); // Create factor - BADFactor f(noiseModel::Unit::Create(3), Rot3(), ABC); + ExpressionFactor f(noiseModel::Unit::Create(3), Rot3(), ABC); // Create some values Values values; diff --git a/gtsam_unstable/timing/timeCameraExpression.cpp b/gtsam_unstable/timing/timeCameraExpression.cpp index 97c939272..c87d1919b 100644 --- a/gtsam_unstable/timing/timeCameraExpression.cpp +++ b/gtsam_unstable/timing/timeCameraExpression.cpp @@ -17,7 +17,7 @@ */ #include -#include +#include #include #include #include @@ -76,17 +76,17 @@ int main() { GeneralSFMFactor2 f1(z, model, 1, 2, 3); time("GeneralSFMFactor2 : ", f1, values); - // BADFactor + // ExpressionFactor // Oct 3, 2014, Macbook Air // 20.3 musecs/call - BADFactor f2(model, z, + ExpressionFactor f2(model, z, uncalibrate(K, project(transform_to(x, p)))); time("Bin(Leaf,Un(Bin(Leaf,Leaf))): ", f2, values); - // BADFactor ternary + // ExpressionFactor ternary // Oct 3, 2014, Macbook Air // 20.3 musecs/call - BADFactor f3(model, z, project3(x, p, K)); + ExpressionFactor f3(model, z, project3(x, p, K)); time("Ternary(Leaf,Leaf,Leaf) : ", f3, values); // CALIBRATED @@ -97,19 +97,19 @@ int main() { GenericProjectionFactor g1(z, model, 1, 2, fixedK); time("GenericProjectionFactor: ", g1, values); - // BADFactor + // ExpressionFactor // Oct 3, 2014, Macbook Air // 16.0 musecs/call - BADFactor g2(model, z, + ExpressionFactor g2(model, z, uncalibrate(Cal3_S2_(*fixedK), project(transform_to(x, p)))); time("Bin(Cnst,Un(Bin(Leaf,Leaf))): ", g2, values); - // BADFactor, optimized + // ExpressionFactor, optimized // Oct 3, 2014, Macbook Air // 9.0 musecs/call typedef PinholeCamera Camera; typedef Expression Camera_; - BADFactor g3(model, z, Point2_(myProject, x, p)); + ExpressionFactor g3(model, z, Point2_(myProject, x, p)); time("Binary(Leaf,Leaf) : ", g3, values); return 0; } diff --git a/gtsam_unstable/timing/timeOneCameraExpression.cpp b/gtsam_unstable/timing/timeOneCameraExpression.cpp index 61cb51bfd..8a1fb5b1b 100644 --- a/gtsam_unstable/timing/timeOneCameraExpression.cpp +++ b/gtsam_unstable/timing/timeOneCameraExpression.cpp @@ -17,7 +17,7 @@ */ #include -#include +#include #include #include @@ -55,14 +55,14 @@ int main() { values.insert(2, Point3(0, 0, 1)); values.insert(3, Cal3_S2()); - // BADFactor + // ExpressionFactor // Oct 3, 2014, Macbook Air // 20.3 musecs/call //#define TERNARY #ifdef TERNARY - BADFactor f(model, z, project3(x, p, K)); + ExpressionFactor f(model, z, project3(x, p, K)); #else - BADFactor f(model, z, uncalibrate(K, project(transform_to(x, p)))); + ExpressionFactor f(model, z, uncalibrate(K, project(transform_to(x, p)))); #endif time(f, values);