diff --git a/.cproject b/.cproject index 02f7b71c6..ed0be7891 100644 --- a/.cproject +++ b/.cproject @@ -315,14 +315,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -349,6 +341,7 @@ make + tests/testBayesTree.run true false @@ -356,6 +349,7 @@ make + testBinaryBayesNet.run true false @@ -403,6 +397,7 @@ make + testSymbolicBayesNet.run true false @@ -410,6 +405,7 @@ make + tests/testSymbolicFactor.run true false @@ -417,6 +413,7 @@ make + testSymbolicFactorGraph.run true false @@ -432,11 +429,20 @@ make + tests/testBayesTree true false true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j5 @@ -525,22 +531,6 @@ false true - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - make -j2 @@ -557,6 +547,22 @@ true true + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + make -j2 @@ -581,6 +587,62 @@ true true + + make + -j2 + all + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j5 + testGeneralSFMFactor.run + true + true + true + + + make + -j5 + testProjectionFactor.run + true + true + true + + + make + -j5 + testGeneralSFMFactor_Cal3Bundler.run + true + true + true + + + make + -j6 -j8 + testAntiFactor.run + true + true + true + make -j5 @@ -613,110 +675,6 @@ true true - - make - -j5 - testGeneralSFMFactor.run - true - true - true - - - make - -j5 - testPlanarSLAM.run - true - true - true - - - make - -j5 - testPose2SLAM.run - true - true - true - - - make - -j5 - testPose3SLAM.run - true - true - true - - - make - -j5 - testSimulated2D.run - true - true - true - - - make - -j5 - testSimulated2DOriented.run - true - true - true - - - make - -j5 - testVisualSLAM.run - true - true - true - - - make - -j5 - testProjectionFactor.run - true - true - true - - - make - -j5 - testSerializationSLAM.run - true - true - true - - - make - -j5 - testGeneralSFMFactor_Cal3Bundler.run - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - make -j5 @@ -1103,6 +1061,7 @@ make + testGraph.run true false @@ -1110,6 +1069,7 @@ make + testJunctionTree.run true false @@ -1117,6 +1077,7 @@ make + testSymbolicBayesNetB.run true false @@ -1284,6 +1245,7 @@ make + testErrors.run true false @@ -1329,14 +1291,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -1417,6 +1371,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -1723,7 +1685,6 @@ make - testSimulated2DOriented.run true false @@ -1763,7 +1724,6 @@ make - testSimulated2D.run true false @@ -1771,7 +1731,6 @@ make - testSimulated3D.run true false @@ -1987,7 +1946,6 @@ make - tests/testGaussianISAM2 true false @@ -2009,102 +1967,6 @@ true true - - make - -j2 - testRot3.run - true - true - true - - - make - -j2 - testRot2.run - true - true - true - - - make - -j2 - testPose3.run - true - true - true - - - make - -j2 - timeRot3.run - true - true - true - - - make - -j2 - testPose2.run - true - true - true - - - make - -j2 - testCal3_S2.run - true - true - true - - - make - -j2 - testSimpleCamera.run - true - true - true - - - make - -j2 - testHomography2.run - true - true - true - - - make - -j2 - testCalibratedCamera.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - testPoint2.run - true - true - true - make -j3 @@ -2306,6 +2168,7 @@ cpack + -G DEB true false @@ -2313,6 +2176,7 @@ cpack + -G RPM true false @@ -2320,6 +2184,7 @@ cpack + -G TGZ true false @@ -2327,6 +2192,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -2468,34 +2334,98 @@ true true - + make - -j5 - testSpirit.run + -j2 + testRot3.run true true true - + make - -j5 - testWrap.run + -j2 + testRot2.run true true true - + make - -j5 - check.wrap + -j2 + testPose3.run true true true - + make - -j5 - wrap + -j2 + timeRot3.run + true + true + true + + + make + -j2 + testPose2.run + true + true + true + + + make + -j2 + testCal3_S2.run + true + true + true + + + make + -j2 + testSimpleCamera.run + true + true + true + + + make + -j2 + testHomography2.run + true + true + true + + + make + -j2 + testCalibratedCamera.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + testPoint2.run true true true @@ -2539,6 +2469,38 @@ false true + + make + -j5 + testSpirit.run + true + true + true + + + make + -j5 + testWrap.run + true + true + true + + + make + -j5 + check.wrap + true + true + true + + + make + -j5 + wrap + true + true + true + diff --git a/gtsam/slam/AntiFactor.h b/gtsam/slam/AntiFactor.h index fe1b93bde..ae794db6a 100644 --- a/gtsam/slam/AntiFactor.h +++ b/gtsam/slam/AntiFactor.h @@ -47,7 +47,7 @@ namespace gtsam { AntiFactor() {} /** constructor - Creates the equivalent AntiFactor from an existing factor */ - AntiFactor(NonlinearFactor::shared_ptr factor) : Base(factor->begin(), factor->end()), factor_(factor) {} + AntiFactor(NonlinearFactor::shared_ptr factor) : Base(factor->keys()), factor_(factor) {} virtual ~AntiFactor() {} @@ -94,11 +94,10 @@ namespace gtsam { * returns a Jacobian instead of a full Hessian), but with the opposite sign. This * effectively cancels the effect of the original factor on the factor graph. */ - boost::shared_ptr - linearize(const Values& c, const Ordering& ordering) const { + boost::shared_ptr linearize(const Values& c) const { // Generate the linearized factor from the contained nonlinear factor - GaussianFactor::shared_ptr gaussianFactor = factor_->linearize(c, ordering); + GaussianFactor::shared_ptr gaussianFactor = factor_->linearize(c); // return the negated version of the factor return gaussianFactor->negate(); diff --git a/gtsam/slam/tests/testAntiFactor.cpp b/gtsam/slam/tests/testAntiFactor.cpp index aba2b82b7..1a455001b 100644 --- a/gtsam/slam/tests/testAntiFactor.cpp +++ b/gtsam/slam/tests/testAntiFactor.cpp @@ -23,6 +23,10 @@ #include #include #include +#include +#include +#include +#include #include using namespace std; @@ -40,21 +44,15 @@ TEST( AntiFactor, NegativeHessian) SharedNoiseModel sigma(noiseModel::Unit::Create(Pose3::Dim())); // Create a configuration corresponding to the ground truth - boost::shared_ptr values(new Values()); - values->insert(1, pose1); - values->insert(2, pose2); - - // Define an elimination ordering - Ordering::shared_ptr ordering(new Ordering()); - ordering->insert(1, 0); - ordering->insert(2, 1); - + Values values; + values.insert(1, pose1); + values.insert(2, pose2); // Create a "standard" factor BetweenFactor::shared_ptr originalFactor(new BetweenFactor(1, 2, z, sigma)); // Linearize it into a Jacobian Factor - GaussianFactor::shared_ptr originalJacobian = originalFactor->linearize(*values, *ordering); + GaussianFactor::shared_ptr originalJacobian = originalFactor->linearize(values); // Convert it to a Hessian Factor HessianFactor::shared_ptr originalHessian = HessianFactor::shared_ptr(new HessianFactor(*originalJacobian)); @@ -63,7 +61,7 @@ TEST( AntiFactor, NegativeHessian) AntiFactor::shared_ptr antiFactor(new AntiFactor(originalFactor)); // Linearize the AntiFactor into a Hessian Factor - GaussianFactor::shared_ptr antiGaussian = antiFactor->linearize(*values, *ordering); + GaussianFactor::shared_ptr antiGaussian = antiFactor->linearize(values); HessianFactor::shared_ptr antiHessian = boost::dynamic_pointer_cast(antiGaussian); @@ -96,39 +94,39 @@ TEST( AntiFactor, EquivalentBayesNet) Pose3 z(Rot3(), Point3(1, 1, 1)); SharedNoiseModel sigma(noiseModel::Unit::Create(Pose3::Dim())); - NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph()); - graph->add(PriorFactor(1, pose1, sigma)); - graph->add(BetweenFactor(1, 2, pose1.between(pose2), sigma)); + NonlinearFactorGraph graph; + graph.push_back(PriorFactor(1, pose1, sigma)); + graph.push_back(BetweenFactor(1, 2, pose1.between(pose2), sigma)); // Create a configuration corresponding to the ground truth - Values::shared_ptr values(new Values()); - values->insert(1, pose1); - values->insert(2, pose2); + Values values; + values.insert(1, pose1); + values.insert(2, pose2); // Define an elimination ordering - Ordering::shared_ptr ordering = graph->orderingCOLAMD(*values); + Ordering ordering = graph.orderingCOLAMD(); // Eliminate into a BayesNet - GaussianSequentialSolver solver1(*graph->linearize(*values, *ordering)); - GaussianBayesNet::shared_ptr expectedBayesNet = solver1.eliminate(); + GaussianFactorGraph lin_graph = *graph.linearize(values); + GaussianBayesNet::shared_ptr expectedBayesNet = lin_graph.eliminateSequential(ordering); // Back-substitute to find the optimal deltas - VectorValues expectedDeltas = optimize(*expectedBayesNet); + VectorValues expectedDeltas = expectedBayesNet->optimize(); // Add an additional factor between Pose1 and Pose2 BetweenFactor::shared_ptr f1(new BetweenFactor(1, 2, z, sigma)); - graph->push_back(f1); + graph.push_back(f1); // Add the corresponding AntiFactor between Pose1 and Pose2 AntiFactor::shared_ptr f2(new AntiFactor(f1)); - graph->push_back(f2); + graph.push_back(f2); // Again, Eliminate into a BayesNet - GaussianSequentialSolver solver2(*graph->linearize(*values, *ordering)); - GaussianBayesNet::shared_ptr actualBayesNet = solver2.eliminate(); + GaussianFactorGraph lin_graph1 = *graph.linearize(values); + GaussianBayesNet::shared_ptr actualBayesNet = lin_graph1.eliminateSequential(ordering); // Back-substitute to find the optimal deltas - VectorValues actualDeltas = optimize(*actualBayesNet); + VectorValues actualDeltas = actualBayesNet->optimize(); // Verify the BayesNets are identical CHECK(assert_equal(*expectedBayesNet, *actualBayesNet, 1e-5));