From a4aa7b9f45cc2dae6f74016b03344bbeaa00972c Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 13 Feb 2015 16:07:32 +0100 Subject: [PATCH 01/16] Some comments --- gtsam/inference/EliminateableFactorGraph.h | 3 ++- gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp | 4 ++++ 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/gtsam/inference/EliminateableFactorGraph.h b/gtsam/inference/EliminateableFactorGraph.h index 5fb5fbdb1..f5431db3d 100644 --- a/gtsam/inference/EliminateableFactorGraph.h +++ b/gtsam/inference/EliminateableFactorGraph.h @@ -128,7 +128,8 @@ namespace gtsam { OptionalOrderingType orderingType = boost::none) const; /** Do multifrontal elimination of all variables to produce a Bayes tree. If an ordering is not - * provided, the ordering provided by COLAMD will be used. + * provided, the ordering will be computed using either COLAMD or METIS, dependeing on + * the parameter orderingType (Ordering::COLAMD or Ordering::METIS) * * Example - Full Cholesky elimination in COLAMD order: * \code diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index 473caa35e..1c781f173 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -242,8 +242,10 @@ void LevenbergMarquardtOptimizer::iterate() { bool systemSolvedSuccessfully; try { + // ============ Solve is where most computation happens !! ================= delta = solve(dampedSystem, state_.values, params_); systemSolvedSuccessfully = true; + // ========================================================================= } catch (IndeterminantLinearSystemException) { systemSolvedSuccessfully = false; } @@ -267,7 +269,9 @@ void LevenbergMarquardtOptimizer::iterate() { if (linearizedCostChange >= 0) { // step is valid // update values gttic(retract); + // ============ This is where the solution is updated ==================== newValues = state_.values.retract(delta); + // ======================================================================= gttoc(retract); // compute new error From d86ac21b0e9f3f58189f7ed51bd2b235e44e42ee Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 13 Feb 2015 16:54:43 +0100 Subject: [PATCH 02/16] add testOrdering target --- .cproject | 3322 ++++++++++++++++++++++++++--------------------------- 1 file changed, 1659 insertions(+), 1663 deletions(-) diff --git a/.cproject b/.cproject index 756116cfb..b77415840 100644 --- a/.cproject +++ b/.cproject @@ -1,7 +1,5 @@ - - - + @@ -486,26 +484,265 @@ - + make -j5 - SmartProjectionFactorExample_kitti_nonbatch.run + testCombinedImuFactor.run true true true - + make -j5 - SmartProjectionFactorExample_kitti.run + testImuFactor.run true true true - + make -j5 - SmartProjectionFactorTesting.run + testAHRSFactor.run + true + true + true + + + make + -j8 + testAttitudeFactor.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + testNonlinearConstraint.run + true + true + true + + + make + -j2 + testLieConfig.run + true + true + true + + + make + -j2 + testConstraintOptimizer.run + true + true + true + + + make + -j2 + testGaussianFactor.run + true + true + true + + + make + -j4 + testImuFactor.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + testBTree.run + true + true + true + + + make + -j2 + testDSF.run + true + true + true + + + make + -j2 + testDSFVector.run + true + true + true + + + make + -j2 + testMatrix.run + true + true + true + + + make + -j2 + testSPQRUtil.run + true + true + true + + + make + -j2 + testVector.run + true + true + true + + + make + -j2 + timeMatrix.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j5 + wrap + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + install + true + true + true + + + make + -j2 + all + true + true + true + + + cmake + .. + true + false + true + + + make + -j2 + check + true + true + true + + + make + -j2 + tests/testLieConfig.run true true true @@ -534,6 +771,70 @@ true true + + make + -j2 + all + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j5 + all + true + false + true + + + make + -j5 + check + true + false + true + + + make + -j5 + SmartProjectionFactorExample_kitti_nonbatch.run + true + true + true + + + make + -j5 + SmartProjectionFactorExample_kitti.run + true + true + true + + + make + -j5 + SmartProjectionFactorTesting.run + true + true + true + make -j2 @@ -558,7 +859,159 @@ true true - + + make + -j5 + testCal3Bundler.run + true + true + true + + + make + -j5 + testCal3DS2.run + true + true + true + + + make + -j5 + testCalibratedCamera.run + true + true + true + + + make + -j5 + testEssentialMatrix.run + true + true + true + + + make + -j1 VERBOSE=1 + testHomography2.run + true + false + true + + + make + -j5 + testPinholeCamera.run + true + true + true + + + make + -j5 + testPoint2.run + true + true + true + + + make + -j5 + testPoint3.run + true + true + true + + + make + -j5 + testPose2.run + true + true + true + + + make + -j5 + testPose3.run + true + true + true + + + make + -j5 + testRot3M.run + true + true + true + + + make + -j5 + testSphere2.run + true + true + true + + + make + -j5 + testStereoCamera.run + true + true + true + + + make + -j5 + testCal3Unified.run + true + true + true + + + make + -j5 + testRot2.run + true + true + true + + + make + -j5 + testRot3Q.run + true + true + true + + + make + -j5 + testRot3.run + true + true + true + + + make + -j4 + testSO3.run + true + true + true + + + make + -j4 + testQuaternion.run + true + true + true + + make -j2 all @@ -566,7 +1019,7 @@ true true - + make -j2 clean @@ -574,143 +1027,23 @@ true true - - make - -k - check - true - false - true - - - make - - tests/testBayesTree.run - true - false - true - - - make - - testBinaryBayesNet.run - true - false - true - - + make -j2 - testFactorGraph.run + clean all true true true - + make -j2 - testISAM.run + install true true true - - make - -j2 - testJunctionTree.run - true - true - true - - - make - -j2 - testKey.run - true - true - true - - - make - -j2 - testOrdering.run - true - true - true - - - make - - testSymbolicBayesNet.run - true - false - true - - - make - - tests/testSymbolicFactor.run - true - false - true - - - make - - testSymbolicFactorGraph.run - true - false - true - - - make - -j2 - timeSymbolMaps.run - true - true - true - - - make - - tests/testBayesTree - true - false - true - - - make - -j2 - tests/testPose2.run - true - true - true - - - make - -j2 - tests/testPose3.run - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - check - true - true - true - - + make -j2 clean @@ -862,154 +1195,146 @@ true true - + make - -j5 - testGaussianFactorGraphUnordered.run + -j2 + all true true true - + make - -j5 - testGaussianBayesNetUnordered.run + -j2 + check true true true - + make - -j5 - testGaussianConditional.run - true - true - true - - - make - -j5 - testGaussianDensity.run - true - true - true - - - make - -j5 - testGaussianJunctionTree.run - true - true - true - - - make - -j5 - testHessianFactor.run - true - true - true - - - make - -j5 - testJacobianFactor.run - true - true - true - - - make - -j5 - testKalmanFilter.run - true - true - true - - - make - -j5 - testNoiseModel.run - true - true - true - - - make - -j5 - testSampler.run - true - true - true - - - make - -j5 - testSerializationLinear.run - true - true - true - - - make - -j5 - testVectorValues.run - true - true - true - - - make - -j5 - testGaussianBayesTree.run - true - true - true - - - make - -j5 - testCombinedImuFactor.run - true - true - true - - - make - -j5 - testImuFactor.run - true - true - true - - - make - -j5 - testAHRSFactor.run - true - true - true - - - make - -j8 - testAttitudeFactor.run - true - true - true - - - make - -j5 + -j2 clean true true true - + make - -j5 - all + -j2 + testPlanarSLAM.run + true + true + true + + + make + -j2 + testPose2Config.run + true + true + true + + + make + -j2 + testPose2Factor.run + true + true + true + + + make + -j2 + testPose2Prior.run + true + true + true + + + make + -j2 + testPose2SLAM.run + true + true + true + + + make + -j2 + testPose3Config.run + true + true + true + + + make + -j2 + testPose3SLAM.run + true + true + true + + + make + + testSimulated2DOriented.run + true + false + true + + + make + -j2 + testVSLAMConfig.run + true + true + true + + + make + -j2 + testVSLAMFactor.run + true + true + true + + + make + -j2 + testVSLAMGraph.run + true + true + true + + + make + -j2 + testPose3Factor.run + true + true + true + + + make + + testSimulated2D.run + true + false + true + + + make + + testSimulated3D.run + true + false + true + + + make + -j2 + tests/testGaussianISAM2 true true true @@ -1104,477 +1429,95 @@ make - testErrors.run true false true - - make - -j2 - check - true - true - true - - - make - -j2 - tests/testGaussianJunctionTree.run - true - true - true - - - make - -j2 - tests/testGaussianFactor.run - true - true - true - - - make - -j2 - tests/testGaussianConditional.run - true - true - true - - - make - -j2 - tests/timeSLAMlike.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - testBTree.run - true - true - true - - - make - -j2 - testDSF.run - true - true - true - - - make - -j2 - testDSFVector.run - true - true - true - - - make - -j2 - testMatrix.run - true - true - true - - - make - -j2 - testSPQRUtil.run - true - true - true - - - make - -j2 - testVector.run - true - true - true - - - make - -j2 - timeMatrix.run - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - testClusterTree.run - true - true - true - - - make - -j2 - testJunctionTree.run - true - true - true - - - make - -j2 - tests/testEliminationTree.run - true - true - true - - - make - -j2 - tests/testSymbolicFactor.run - true - true - true - - - make - -j2 - tests/testVariableSlots.run - true - true - true - - - make - -j2 - tests/testConditional.run - true - true - true - - - make - -j2 - tests/testSymbolicFactorGraph.run - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - testNonlinearConstraint.run - true - true - true - - - make - -j2 - testLieConfig.run - true - true - true - - - make - -j2 - testConstraintOptimizer.run - true - true - true - - - make - -j2 - check - true - true - true - - + make -j5 - testBTree.run + testAntiFactor.run true true true - + make -j5 - testDSF.run + testPriorFactor.run true true true - + make -j5 - testDSFMap.run + testDataset.run true true true - + make -j5 - testDSFVector.run + testEssentialMatrixFactor.run true true true - + make -j5 - testFixedVector.run + 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 - -j2 - testPlanarSLAM.run - true - true - true - - - make - -j2 - testPose2Config.run - true - true - true - - - make - -j2 - testPose2Factor.run - true - true - true - - - make - -j2 - testPose2Prior.run - true - true - true - - - make - -j2 - testPose2SLAM.run - true - true - true - - - make - -j2 - testPose3Config.run - true - true - true - - - make - -j2 - testPose3SLAM.run - true - true - true - - - make - testSimulated2DOriented.run - true - false - true - - - make - -j2 - testVSLAMConfig.run - true - true - true - - - make - -j2 - testVSLAMFactor.run - true - true - true - - - make - -j2 - testVSLAMGraph.run - true - true - true - - - make - -j2 - testPose3Factor.run - true - true - true - - - make - testSimulated2D.run - true - false - true - - - make - testSimulated3D.run - true - false - true - - - make - -j2 - tests/testGaussianISAM2 - true - true - true - - + make -j5 - testEliminationTree.run + testGeneralSFMFactor.run true true true - + make -j5 - testInference.run + testProjectionFactor.run true true true - + make -j5 - testKey.run + testRotateFactor.run true true true - + make - -j1 - testSymbolicBayesTree.run + -j5 + testPoseRotationPrior.run true - false + true true - + make - -j1 - testSymbolicSequentialSolver.run + -j5 + testImplicitSchurFactor.run true - false + true true - + make -j4 - testLabeledSymbol.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - tests/testLieConfig.run + testRangeFactor.run true true true @@ -1780,7 +1723,6 @@ cpack - -G DEB true false @@ -1788,7 +1730,6 @@ cpack - -G RPM true false @@ -1796,7 +1737,6 @@ cpack - -G TGZ true false @@ -1804,7 +1744,6 @@ cpack - --config CPackSourceConfig.cmake true false @@ -1986,7 +1925,7 @@ false true - + make -j2 check @@ -1994,38 +1933,55 @@ true true - + make - -j2 - clean - true - true - true - - - make - -j2 - install - true - true - true - - - make - -j2 - all - true - true - true - - - cmake - .. + + tests/testGaussianISAM2 true false true - + + make + -j2 + check + true + true + true + + + make + -j2 + tests/testGaussianJunctionTree.run + true + true + true + + + make + -j2 + tests/testGaussianFactor.run + true + true + true + + + make + -j2 + tests/testGaussianConditional.run + true + true + true + + + make + -j2 + tests/timeSLAMlike.run + true + true + true + + make -j2 testGaussianFactor.run @@ -2033,418 +1989,26 @@ true true - + make -j5 - testCal3Bundler.run + testParticleFactor.run true true true - + make -j5 - testCal3DS2.run + testExpressionMeta.run true true true - - make - -j5 - testCalibratedCamera.run - true - true - true - - - make - -j5 - testEssentialMatrix.run - true - true - true - - - make - -j1 VERBOSE=1 - testHomography2.run - true - false - true - - - make - -j5 - testPinholeCamera.run - true - true - true - - - make - -j5 - testPoint2.run - true - true - true - - - make - -j5 - testPoint3.run - true - true - true - - - make - -j5 - testPose2.run - true - true - true - - - make - -j5 - testPose3.run - true - true - true - - - make - -j5 - testRot3M.run - true - true - true - - - make - -j5 - testSphere2.run - true - true - true - - - make - -j5 - testStereoCamera.run - true - true - true - - - make - -j5 - testCal3Unified.run - true - true - true - - - make - -j5 - testRot2.run - true - true - true - - - make - -j5 - testRot3Q.run - true - true - true - - - make - -j5 - testRot3.run - true - true - true - - + make -j4 - testSO3.run - true - true - true - - - make - -j4 - testQuaternion.run - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j5 - all - true - false - true - - - make - -j5 - check - true - false - true - - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - clean all - true - true - true - - - make - -j5 - testIMUSystem.run - true - true - true - - - make - -j5 - testPoseRTV.run - true - true - true - - - make - -j5 - testVelocityConstraint.run - true - true - true - - - make - -j5 - testVelocityConstraint3.run - true - true - true - - - make - -j1 - testDiscreteBayesTree.run - true - false - true - - - make - -j5 - testDiscreteConditional.run - true - true - true - - - make - -j5 - testDiscreteFactor.run - true - true - true - - - make - -j5 - testDiscreteFactorGraph.run - true - true - true - - - make - -j5 - testDiscreteMarginals.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - timeCalibratedCamera.run - true - true - true - - - make - -j2 - timeRot3.run - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j5 - testWrap.run - true - true - true - - - make - -j5 - testSpirit.run - true - true - true - - - make - -j5 - check.wrap - true - true - true - - - make - -j5 - testMethod.run - true - true - true - - - make - -j5 - testClass.run - true - true - true - - - make - -j4 - testType.run - true - true - true - - - make - -j4 - testArgument.run - true - true - true - - - make - -j4 - testReturnValue.run - true - true - true - - - make - -j4 - testTemplate.run - true - true - true - - - make - -j4 - testGlobalFunction.run + testCustomChartExpression.run true true true @@ -2497,327 +2061,145 @@ true true - + make -j2 - vSFMexample.run + all true true true - + make -j2 - testVSLAMGraph - true - true - true - - - make - -j5 - testMatrix.run - true - true - true - - - make - -j5 - testVector.run - true - true - true - - - make - -j5 - testNumericalDerivative.run - true - true - true - - - make - -j5 - testVerticalBlockMatrix.run - true - true - true - - - make - -j4 - testOptionalJacobian.run - true - true - true - - - make - -j5 - check.tests - true - true - true - - - make - -j2 - timeGaussianFactorGraph.run - true - true - true - - - make - -j5 - testMarginals.run - true - true - true - - - make - -j5 - testGaussianISAM2.run - true - true - true - - - make - -j5 - testSymbolicFactorGraphB.run - true - true - true - - - make - -j2 - timeSequentialOnDataset.run - true - true - true - - - make - -j5 - testGradientDescentOptimizer.run - true - true - true - - - make - -j2 - testGaussianFactor.run - true - true - true - - - make - -j2 - testNonlinearOptimizer.run - true - true - true - - - make - -j2 - testGaussianBayesNet.run - true - true - true - - - make - -j2 - testNonlinearISAM.run - true - true - true - - - make - -j2 - testNonlinearEquality.run - true - true - true - - - make - -j2 - testExtendedKalmanFilter.run - true - true - true - - - make - -j5 - timing.tests - true - true - true - - - make - -j5 - testNonlinearFactor.run - true - true - true - - - make - -j5 clean true true true - + make - -j5 - testGaussianJunctionTreeB.run - true - true - true - - - make - - testGraph.run + -k + check true false true - + make - + tests/testBayesTree.run + true + false + true + + + make + testBinaryBayesNet.run + true + false + true + + + make + -j2 + testFactorGraph.run + true + true + true + + + make + -j2 + testISAM.run + true + true + true + + + make + -j2 testJunctionTree.run true - false + true true - + make - - testSymbolicBayesNetB.run + -j2 + testKey.run + true + true + true + + + make + -j2 + testOrdering.run + true + true + true + + + make + testSymbolicBayesNet.run true false true - + make - -j5 - testGaussianISAM.run + tests/testSymbolicFactor.run true - true + false true - + make - -j5 - testDoglegOptimizer.run + testSymbolicFactorGraph.run true - true + false true - - make - -j5 - testNonlinearFactorGraph.run - true - true - true - - - make - -j5 - testIterative.run - true - true - true - - - make - -j5 - testSubgraphSolver.run - true - true - true - - - make - -j5 - testGaussianFactorGraphB.run - true - true - true - - - make - -j5 - testSummarization.run - true - true - true - - - make - -j5 - testManifold.run - true - true - true - - - make - -j5 - testParticleFactor.run - true - true - true - - - make - -j5 - testExpressionMeta.run - true - true - true - - - make - -j4 - testCustomChartExpression.run - true - true - true - - + make -j2 - testGaussianFactor.run + timeSymbolMaps.run true true true - + + make + tests/testBayesTree + true + false + true + + make -j2 - install + check true true true - + + make + -j2 + timeCalibratedCamera.run + true + true + true + + + make + -j2 + timeRot3.run + true + true + true + + make -j2 clean @@ -2825,122 +2207,18 @@ true true - + make -j2 - all + tests/testPose2.run true true true - + make -j2 - clean - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j5 - testAntiFactor.run - true - true - true - - - make - -j5 - testPriorFactor.run - true - true - true - - - make - -j5 - testDataset.run - true - true - true - - - make - -j5 - testEssentialMatrixFactor.run - true - true - true - - - make - -j5 - testGeneralSFMFactor_Cal3Bundler.run - true - true - true - - - make - -j5 - testGeneralSFMFactor.run - true - true - true - - - make - -j5 - testProjectionFactor.run - true - true - true - - - make - -j5 - testRotateFactor.run - true - true - true - - - make - -j5 - testPoseRotationPrior.run - true - true - true - - - make - -j5 - testImplicitSchurFactor.run - true - true - true - - - make - -j4 - testRangeFactor.run + tests/testPose3.run true true true @@ -3137,181 +2415,6 @@ true true - - make - -j5 - testLago.run - true - true - true - - - make - -j5 - testLinearContainerFactor.run - true - true - true - - - make - -j5 - testOrdering.run - true - true - true - - - make - -j5 - testValues.run - true - true - true - - - make - -j5 - testWhiteNoiseFactor.run - true - true - true - - - make - -j4 - testExpression.run - true - true - true - - - make - -j4 - testAdaptAutoDiff.run - true - true - true - - - make - -j4 - testCallRecord.run - true - true - true - - - make - -j4 - testExpressionFactor.run - true - true - true - - - make - -j4 - testImuFactor.run - true - true - true - - - make - -j5 - timeCalibratedCamera.run - true - true - true - - - make - -j5 - timePinholeCamera.run - true - true - true - - - make - -j5 - timeStereoCamera.run - true - true - true - - - make - -j5 - timeLago.run - true - true - true - - - make - -j5 - timePose3.run - true - true - true - - - make - -j4 - timeAdaptAutoDiff.run - true - true - true - - - make - -j4 - timeCameraExpression.run - true - true - true - - - make - -j4 - timeOneCameraExpression.run - true - true - true - - - make - -j4 - timeSFMExpressions.run - true - true - true - - - make - -j4 - timeIncremental.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - tests/testGaussianISAM2 - true - false - true - make -j2 @@ -3408,10 +2511,903 @@ true true - + + make + -j1 + testDiscreteBayesTree.run + true + false + true + + make -j5 - wrap + testDiscreteConditional.run + true + true + true + + + make + -j5 + testDiscreteFactor.run + true + true + true + + + make + -j5 + testDiscreteFactorGraph.run + true + true + true + + + make + -j5 + testDiscreteMarginals.run + true + true + true + + + make + -j5 + testEliminationTree.run + true + true + true + + + make + -j5 + testInference.run + true + true + true + + + make + -j5 + testKey.run + true + true + true + + + make + -j1 + testSymbolicBayesTree.run + true + false + true + + + make + -j1 + testSymbolicSequentialSolver.run + true + false + true + + + make + -j4 + testLabeledSymbol.run + true + true + true + + + make + -j4 + testOrdering.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + testClusterTree.run + true + true + true + + + make + -j2 + testJunctionTree.run + true + true + true + + + make + -j2 + tests/testEliminationTree.run + true + true + true + + + make + -j2 + tests/testSymbolicFactor.run + true + true + true + + + make + -j2 + tests/testVariableSlots.run + true + true + true + + + make + -j2 + tests/testConditional.run + true + true + true + + + make + -j2 + tests/testSymbolicFactorGraph.run + true + true + true + + + make + -j5 + testLago.run + true + true + true + + + make + -j5 + testLinearContainerFactor.run + true + true + true + + + make + -j5 + testOrdering.run + true + true + true + + + make + -j5 + testValues.run + true + true + true + + + make + -j5 + testWhiteNoiseFactor.run + true + true + true + + + make + -j4 + testExpression.run + true + true + true + + + make + -j4 + testAdaptAutoDiff.run + true + true + true + + + make + -j4 + testCallRecord.run + true + true + true + + + make + -j4 + testExpressionFactor.run + true + true + true + + + make + -j5 + testIMUSystem.run + true + true + true + + + make + -j5 + testPoseRTV.run + true + true + true + + + make + -j5 + testVelocityConstraint.run + true + true + true + + + make + -j5 + testVelocityConstraint3.run + true + true + true + + + make + -j5 + timeCalibratedCamera.run + true + true + true + + + make + -j5 + timePinholeCamera.run + true + true + true + + + make + -j5 + timeStereoCamera.run + true + true + true + + + make + -j5 + timeLago.run + true + true + true + + + make + -j5 + timePose3.run + true + true + true + + + make + -j4 + timeAdaptAutoDiff.run + true + true + true + + + make + -j4 + timeCameraExpression.run + true + true + true + + + make + -j4 + timeOneCameraExpression.run + true + true + true + + + make + -j4 + timeSFMExpressions.run + true + true + true + + + make + -j4 + timeIncremental.run + true + 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 + all + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j5 + testGaussianFactorGraphUnordered.run + true + true + true + + + make + -j5 + testGaussianBayesNetUnordered.run + true + true + true + + + make + -j5 + testGaussianConditional.run + true + true + true + + + make + -j5 + testGaussianDensity.run + true + true + true + + + make + -j5 + testGaussianJunctionTree.run + true + true + true + + + make + -j5 + testHessianFactor.run + true + true + true + + + make + -j5 + testJacobianFactor.run + true + true + true + + + make + -j5 + testKalmanFilter.run + true + true + true + + + make + -j5 + testNoiseModel.run + true + true + true + + + make + -j5 + testSampler.run + true + true + true + + + make + -j5 + testSerializationLinear.run + true + true + true + + + make + -j5 + testVectorValues.run + true + true + true + + + make + -j5 + testGaussianBayesTree.run + true + true + true + + + make + -j5 + testWrap.run + true + true + true + + + make + -j5 + testSpirit.run + true + true + true + + + make + -j5 + check.wrap + true + true + true + + + make + -j5 + testMethod.run + true + true + true + + + make + -j5 + testClass.run + true + true + true + + + make + -j4 + testType.run + true + true + true + + + make + -j4 + testArgument.run + true + true + true + + + make + -j4 + testReturnValue.run + true + true + true + + + make + -j4 + testTemplate.run + true + true + true + + + make + -j4 + testGlobalFunction.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j5 + testMatrix.run + true + true + true + + + make + -j5 + testVector.run + true + true + true + + + make + -j5 + testNumericalDerivative.run + true + true + true + + + make + -j5 + testVerticalBlockMatrix.run + true + true + true + + + make + -j4 + testOptionalJacobian.run + true + true + true + + + make + -j5 + check.tests + true + true + true + + + make + -j2 + timeGaussianFactorGraph.run + true + true + true + + + make + -j5 + testMarginals.run + true + true + true + + + make + -j5 + testGaussianISAM2.run + true + true + true + + + make + -j5 + testSymbolicFactorGraphB.run + true + true + true + + + make + -j2 + timeSequentialOnDataset.run + true + true + true + + + make + -j5 + testGradientDescentOptimizer.run + true + true + true + + + make + -j2 + testGaussianFactor.run + true + true + true + + + make + -j2 + testNonlinearOptimizer.run + true + true + true + + + make + -j2 + testGaussianBayesNet.run + true + true + true + + + make + -j2 + testNonlinearISAM.run + true + true + true + + + make + -j2 + testNonlinearEquality.run + true + true + true + + + make + -j2 + testExtendedKalmanFilter.run + true + true + true + + + make + -j5 + timing.tests + true + true + true + + + make + -j5 + testNonlinearFactor.run + true + true + true + + + make + -j5 + clean + true + true + true + + + make + -j5 + testGaussianJunctionTreeB.run + true + true + true + + + make + testGraph.run + true + false + true + + + make + testJunctionTree.run + true + false + true + + + make + testSymbolicBayesNetB.run + true + false + true + + + make + -j5 + testGaussianISAM.run + true + true + true + + + make + -j5 + testDoglegOptimizer.run + true + true + true + + + make + -j5 + testNonlinearFactorGraph.run + true + true + true + + + make + -j5 + testIterative.run + true + true + true + + + make + -j5 + testSubgraphSolver.run + true + true + true + + + make + -j5 + testGaussianFactorGraphB.run + true + true + true + + + make + -j5 + testSummarization.run + true + true + true + + + make + -j5 + testManifold.run + true + true + true + + + make + -j2 + vSFMexample.run + true + true + true + + + make + -j5 + clean + true + true + true + + + make + -j5 + all + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + testVSLAMGraph true true true From 8d19f45825b8f4c3a322b1e84112f0131cc0910d Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 13 Feb 2015 16:55:04 +0100 Subject: [PATCH 03/16] named constructor "Create" --- gtsam/inference/Ordering.h | 22 ++++++++++ gtsam/inference/tests/testOrdering.cpp | 59 +++++++++++++++++++------- 2 files changed, 66 insertions(+), 15 deletions(-) diff --git a/gtsam/inference/Ordering.h b/gtsam/inference/Ordering.h index e25590578..92f011c7d 100644 --- a/gtsam/inference/Ordering.h +++ b/gtsam/inference/Ordering.h @@ -170,6 +170,28 @@ namespace gtsam { /// @} + /// @name Named Constructors @{ + + template + static Ordering Create(OrderingType orderingType, + const FactorGraph& graph) { + + switch (orderingType) { + case COLAMD: + return colamd(graph); + case METIS: + return metis(graph); + case CUSTOM: + throw std::runtime_error( + "Ordering::Create error: called with CUSTOM ordering type."); + default: + throw std::runtime_error( + "Ordering::Create error: called with unknown ordering type."); + } + } + + /// @} + /// @name Testable @{ GTSAM_EXPORT void print(const std::string& str = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; diff --git a/gtsam/inference/tests/testOrdering.cpp b/gtsam/inference/tests/testOrdering.cpp index 013f569e0..85b585503 100644 --- a/gtsam/inference/tests/testOrdering.cpp +++ b/gtsam/inference/tests/testOrdering.cpp @@ -28,16 +28,22 @@ using namespace std; using namespace gtsam; using namespace boost::assign; +namespace example { +SymbolicFactorGraph symbolicChain() { + SymbolicFactorGraph sfg; + sfg.push_factor(0, 1); + sfg.push_factor(1, 2); + sfg.push_factor(2, 3); + sfg.push_factor(3, 4); + sfg.push_factor(4, 5); + return sfg; +} +} /* ************************************************************************* */ TEST(Ordering, constrained_ordering) { - SymbolicFactorGraph sfg; // create graph with wanted variable set = 2, 4 - sfg.push_factor(0,1); - sfg.push_factor(1,2); - sfg.push_factor(2,3); - sfg.push_factor(3,4); - sfg.push_factor(4,5); + SymbolicFactorGraph sfg = example::symbolicChain(); // unconstrained version Ordering actUnconstrained = Ordering::colamd(sfg); @@ -57,16 +63,11 @@ TEST(Ordering, constrained_ordering) { /* ************************************************************************* */ TEST(Ordering, grouped_constrained_ordering) { - SymbolicFactorGraph sfg; // create graph with constrained groups: // 1: 2, 4 // 2: 5 - sfg.push_factor(0,1); - sfg.push_factor(1,2); - sfg.push_factor(2,3); - sfg.push_factor(3,4); - sfg.push_factor(4,5); + SymbolicFactorGraph sfg = example::symbolicChain(); // constrained version - push one set to the end FastMap constraints; @@ -140,7 +141,6 @@ TEST(Ordering, csr_format_2) { EXPECT(xadjExpected == mi.xadj()); EXPECT(adjExpected.size() == mi.adj().size()); EXPECT(adjExpected == mi.adj()); - } /* ************************************************************************* */ @@ -170,7 +170,6 @@ TEST(Ordering, csr_format_3) { EXPECT(xadjExpected == mi.xadj()); EXPECT(adjExpected.size() == mi.adj().size()); EXPECT(adjExpected == adjAcutal); - } /* ************************************************************************* */ @@ -207,7 +206,6 @@ TEST(Ordering, csr_format_4) { sfg.push_factor(Symbol('x', 4), Symbol('l', 1)); Ordering metOrder2 = Ordering::metis(sfg); - } /* ************************************************************************* */ @@ -231,6 +229,37 @@ TEST(Ordering, metis) { Ordering metis = Ordering::metis(sfg); } + +/* ************************************************************************* */ +TEST(Ordering, Create) { + + // create graph with wanted variable set = 2, 4 + SymbolicFactorGraph sfg = example::symbolicChain(); + + // COLAMD + { + Ordering actual = Ordering::Create(Ordering::COLAMD,sfg); + Ordering expected = Ordering(list_of(0)(1)(2)(3)(4)(5)); + EXPECT(assert_equal(expected, actual)); + } + + // METIS + { + Ordering actual = Ordering::Create(Ordering::METIS,sfg); + // 2 + // 0 + // 1 + // 4 + // 3 + // 5 + Ordering expected = Ordering(list_of(5)(3)(4)(1)(0)(2)); + EXPECT(assert_equal(expected, actual)); + } + + // CUSTOM + CHECK_EXCEPTION(Ordering::Create(Ordering::CUSTOM,sfg),runtime_error); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ From 674794d387108f4b5ce94ff88f285d17e08d0ff7 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 13 Feb 2015 16:59:36 +0100 Subject: [PATCH 04/16] Added test of metis for a loop --- gtsam/inference/tests/testOrdering.cpp | 24 +++++++++++++++++++++++- 1 file changed, 23 insertions(+), 1 deletion(-) diff --git a/gtsam/inference/tests/testOrdering.cpp b/gtsam/inference/tests/testOrdering.cpp index 85b585503..ee8751f47 100644 --- a/gtsam/inference/tests/testOrdering.cpp +++ b/gtsam/inference/tests/testOrdering.cpp @@ -230,10 +230,32 @@ TEST(Ordering, metis) { Ordering metis = Ordering::metis(sfg); } +/* ************************************************************************* */ +TEST(Ordering, MetisLoop) { + + // create linear graph + SymbolicFactorGraph sfg = example::symbolicChain(); + + // add loop closure + sfg.push_factor(0,5); + + // METIS + { + Ordering actual = Ordering::Create(Ordering::METIS,sfg); + // 0,3 + // 1 + // 2 + // 4 + // 5 + Ordering expected = Ordering(list_of(5)(4)(2)(1)(0)(3)); + EXPECT(assert_equal(expected, actual)); + } +} + /* ************************************************************************* */ TEST(Ordering, Create) { - // create graph with wanted variable set = 2, 4 + // create chain graph SymbolicFactorGraph sfg = example::symbolicChain(); // COLAMD From 56456a2396fe0c059a8a6fa7bb42c6743e5dc9a2 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 13 Feb 2015 17:16:38 +0100 Subject: [PATCH 05/16] Formatting to default BORG format --- gtsam/inference/Ordering.cpp | 424 ++++++++++++------------- gtsam/inference/Ordering.h | 382 +++++++++++----------- gtsam/inference/tests/testOrdering.cpp | 118 +++---- 3 files changed, 469 insertions(+), 455 deletions(-) diff --git a/gtsam/inference/Ordering.cpp b/gtsam/inference/Ordering.cpp index 5ae25d543..0882b87d1 100644 --- a/gtsam/inference/Ordering.cpp +++ b/gtsam/inference/Ordering.cpp @@ -29,242 +29,236 @@ using namespace std; namespace gtsam { - /* ************************************************************************* */ - FastMap Ordering::invert() const - { - FastMap inverted; - for(size_t pos = 0; pos < this->size(); ++pos) - inverted.insert(make_pair((*this)[pos], pos)); - return inverted; +/* ************************************************************************* */ +FastMap Ordering::invert() const { + FastMap inverted; + for (size_t pos = 0; pos < this->size(); ++pos) + inverted.insert(make_pair((*this)[pos], pos)); + return inverted; +} + +/* ************************************************************************* */ +Ordering Ordering::colamd(const VariableIndex& variableIndex) { + // Call constrained version with all groups set to zero + vector dummy_groups(variableIndex.size(), 0); + return Ordering::colamdConstrained(variableIndex, dummy_groups); +} + +/* ************************************************************************* */ +Ordering Ordering::colamdConstrained(const VariableIndex& variableIndex, + std::vector& cmember) { + gttic(Ordering_COLAMDConstrained); + + gttic(Prepare); + size_t nEntries = variableIndex.nEntries(), nFactors = + variableIndex.nFactors(), nVars = variableIndex.size(); + // Convert to compressed column major format colamd wants it in (== MATLAB format!) + size_t Alen = ccolamd_recommended((int) nEntries, (int) nFactors, + (int) nVars); /* colamd arg 3: size of the array A */ + vector A = vector(Alen); /* colamd arg 4: row indices of A, of size Alen */ + vector p = vector(nVars + 1); /* colamd arg 5: column pointers of A, of size n_col+1 */ + + // Fill in input data for COLAMD + p[0] = 0; + int count = 0; + vector keys(nVars); // Array to store the keys in the order we add them so we can retrieve them in permuted order + size_t index = 0; + BOOST_FOREACH(const VariableIndex::value_type key_factors, variableIndex) { + // Arrange factor indices into COLAMD format + const VariableIndex::Factors& column = key_factors.second; + size_t lastFactorId = numeric_limits::max(); + BOOST_FOREACH(size_t factorIndex, column) { + if (lastFactorId != numeric_limits::max()) + assert(factorIndex > lastFactorId); + A[count++] = (int) factorIndex; // copy sparse column + } + p[index + 1] = count; // column j (base 1) goes from A[j-1] to A[j]-1 + // Store key in array and increment index + keys[index] = key_factors.first; + ++index; } - /* ************************************************************************* */ - Ordering Ordering::colamd(const VariableIndex& variableIndex) - { - // Call constrained version with all groups set to zero - vector dummy_groups(variableIndex.size(), 0); - return Ordering::colamdConstrained(variableIndex, dummy_groups); + assert((size_t)count == variableIndex.nEntries()); + + //double* knobs = NULL; /* colamd arg 6: parameters (uses defaults if NULL) */ + double knobs[CCOLAMD_KNOBS]; + ccolamd_set_defaults(knobs); + knobs[CCOLAMD_DENSE_ROW] = -1; + knobs[CCOLAMD_DENSE_COL] = -1; + + int stats[CCOLAMD_STATS]; /* colamd arg 7: colamd output statistics and error codes */ + + gttoc(Prepare); + + // call colamd, result will be in p + /* returns (1) if successful, (0) otherwise*/ + if (nVars > 0) { + gttic(ccolamd); + int rv = ccolamd((int) nFactors, (int) nVars, (int) Alen, &A[0], &p[0], + knobs, stats, &cmember[0]); + if (rv != 1) + throw runtime_error( + (boost::format("ccolamd failed with return value %1%") % rv).str()); } - /* ************************************************************************* */ - Ordering Ordering::colamdConstrained( - const VariableIndex& variableIndex, std::vector& cmember) - { - gttic(Ordering_COLAMDConstrained); + // ccolamd_report(stats); - gttic(Prepare); - size_t nEntries = variableIndex.nEntries(), nFactors = variableIndex.nFactors(), nVars = variableIndex.size(); - // Convert to compressed column major format colamd wants it in (== MATLAB format!) - size_t Alen = ccolamd_recommended((int)nEntries, (int)nFactors, (int)nVars); /* colamd arg 3: size of the array A */ - vector A = vector(Alen); /* colamd arg 4: row indices of A, of size Alen */ - vector p = vector(nVars + 1); /* colamd arg 5: column pointers of A, of size n_col+1 */ + gttic(Fill_Ordering); + // Convert elimination ordering in p to an ordering + Ordering result; + result.resize(nVars); + for (size_t j = 0; j < nVars; ++j) + result[j] = keys[p[j]]; + gttoc(Fill_Ordering); - // Fill in input data for COLAMD - p[0] = 0; - int count = 0; - vector keys(nVars); // Array to store the keys in the order we add them so we can retrieve them in permuted order - size_t index = 0; - BOOST_FOREACH(const VariableIndex::value_type key_factors, variableIndex) { - // Arrange factor indices into COLAMD format - const VariableIndex::Factors& column = key_factors.second; - size_t lastFactorId = numeric_limits::max(); - BOOST_FOREACH(size_t factorIndex, column) { - if(lastFactorId != numeric_limits::max()) - assert(factorIndex > lastFactorId); - A[count++] = (int)factorIndex; // copy sparse column - } - p[index+1] = count; // column j (base 1) goes from A[j-1] to A[j]-1 - // Store key in array and increment index - keys[index] = key_factors.first; - ++ index; - } + return result; +} - assert((size_t)count == variableIndex.nEntries()); +/* ************************************************************************* */ +Ordering Ordering::colamdConstrainedLast(const VariableIndex& variableIndex, + const std::vector& constrainLast, bool forceOrder) { + gttic(Ordering_COLAMDConstrainedLast); - //double* knobs = NULL; /* colamd arg 6: parameters (uses defaults if NULL) */ - double knobs[CCOLAMD_KNOBS]; - ccolamd_set_defaults(knobs); - knobs[CCOLAMD_DENSE_ROW]=-1; - knobs[CCOLAMD_DENSE_COL]=-1; + size_t n = variableIndex.size(); + std::vector cmember(n, 0); - int stats[CCOLAMD_STATS]; /* colamd arg 7: colamd output statistics and error codes */ + // Build a mapping to look up sorted Key indices by Key + FastMap keyIndices; + size_t j = 0; + BOOST_FOREACH(const VariableIndex::value_type key_factors, variableIndex) + keyIndices.insert(keyIndices.end(), make_pair(key_factors.first, j++)); - gttoc(Prepare); + // If at least some variables are not constrained to be last, constrain the + // ones that should be constrained. + int group = (constrainLast.size() != n ? 1 : 0); + BOOST_FOREACH(Key key, constrainLast) { + cmember[keyIndices.at(key)] = group; + if (forceOrder) + ++group; + } - // call colamd, result will be in p - /* returns (1) if successful, (0) otherwise*/ - if(nVars > 0) { - gttic(ccolamd); - int rv = ccolamd((int)nFactors, (int)nVars, (int)Alen, &A[0], &p[0], knobs, stats, &cmember[0]); - if(rv != 1) - throw runtime_error((boost::format("ccolamd failed with return value %1%")%rv).str()); - } + return Ordering::colamdConstrained(variableIndex, cmember); +} - // ccolamd_report(stats); +/* ************************************************************************* */ +Ordering Ordering::colamdConstrainedFirst(const VariableIndex& variableIndex, + const std::vector& constrainFirst, bool forceOrder) { + gttic(Ordering_COLAMDConstrainedFirst); - gttic(Fill_Ordering); - // Convert elimination ordering in p to an ordering - Ordering result; - result.resize(nVars); - for(size_t j = 0; j < nVars; ++j) - result[j] = keys[p[j]]; - gttoc(Fill_Ordering); + const int none = -1; + size_t n = variableIndex.size(); + std::vector cmember(n, none); + // Build a mapping to look up sorted Key indices by Key + FastMap keyIndices; + size_t j = 0; + BOOST_FOREACH(const VariableIndex::value_type key_factors, variableIndex) + keyIndices.insert(keyIndices.end(), make_pair(key_factors.first, j++)); + + // If at least some variables are not constrained to be last, constrain the + // ones that should be constrained. + int group = 0; + BOOST_FOREACH(Key key, constrainFirst) { + cmember[keyIndices.at(key)] = group; + if (forceOrder) + ++group; + } + + if (!forceOrder && !constrainFirst.empty()) + ++group; + BOOST_FOREACH(int& c, cmember) + if (c == none) + c = group; + + return Ordering::colamdConstrained(variableIndex, cmember); +} + +/* ************************************************************************* */ +Ordering Ordering::colamdConstrained(const VariableIndex& variableIndex, + const FastMap& groups) { + gttic(Ordering_COLAMDConstrained); + size_t n = variableIndex.size(); + std::vector cmember(n, 0); + + // Build a mapping to look up sorted Key indices by Key + FastMap keyIndices; + size_t j = 0; + BOOST_FOREACH(const VariableIndex::value_type key_factors, variableIndex) + keyIndices.insert(keyIndices.end(), make_pair(key_factors.first, j++)); + + // Assign groups + typedef FastMap::value_type key_group; + BOOST_FOREACH(const key_group& p, groups) { + // FIXME: check that no groups are skipped + cmember[keyIndices.at(p.first)] = p.second; + } + + return Ordering::colamdConstrained(variableIndex, cmember); +} + +/* ************************************************************************* */ +Ordering Ordering::metis(const MetisIndex& met) { + gttic(Ordering_METIS); + + vector xadj = met.xadj(); + vector adj = met.adj(); + vector perm, iperm; + + idx_t size = met.nValues(); + for (idx_t i = 0; i < size; i++) { + perm.push_back(0); + iperm.push_back(0); + } + + int outputError; + + outputError = METIS_NodeND(&size, &xadj[0], &adj[0], NULL, NULL, &perm[0], + &iperm[0]); + Ordering result; + + if (outputError != METIS_OK) { + std::cout << "METIS failed during Nested Dissection ordering!\n"; return result; } - /* ************************************************************************* */ - Ordering Ordering::colamdConstrainedLast( - const VariableIndex& variableIndex, const std::vector& constrainLast, bool forceOrder) - { - gttic(Ordering_COLAMDConstrainedLast); - - size_t n = variableIndex.size(); - std::vector cmember(n, 0); - - // Build a mapping to look up sorted Key indices by Key - FastMap keyIndices; - size_t j = 0; - BOOST_FOREACH(const VariableIndex::value_type key_factors, variableIndex) - keyIndices.insert(keyIndices.end(), make_pair(key_factors.first, j++)); - - // If at least some variables are not constrained to be last, constrain the - // ones that should be constrained. - int group = (constrainLast.size() != n ? 1 : 0); - BOOST_FOREACH(Key key, constrainLast) { - cmember[keyIndices.at(key)] = group; - if(forceOrder) - ++ group; - } - - return Ordering::colamdConstrained(variableIndex, cmember); + result.resize(size); + for (size_t j = 0; j < (size_t) size; ++j) { + // We have to add the minKey value back to obtain the original key in the Values + result[j] = met.intToKey(perm[j]); } + return result; +} - /* ************************************************************************* */ - Ordering Ordering::colamdConstrainedFirst( - const VariableIndex& variableIndex, const std::vector& constrainFirst, bool forceOrder) - { - gttic(Ordering_COLAMDConstrainedFirst); - - const int none = -1; - size_t n = variableIndex.size(); - std::vector cmember(n, none); - - // Build a mapping to look up sorted Key indices by Key - FastMap keyIndices; - size_t j = 0; - BOOST_FOREACH(const VariableIndex::value_type key_factors, variableIndex) - keyIndices.insert(keyIndices.end(), make_pair(key_factors.first, j++)); - - // If at least some variables are not constrained to be last, constrain the - // ones that should be constrained. - int group = 0; - BOOST_FOREACH(Key key, constrainFirst) { - cmember[keyIndices.at(key)] = group; - if(forceOrder) - ++ group; - } - - if(!forceOrder && !constrainFirst.empty()) - ++ group; - BOOST_FOREACH(int& c, cmember) - if(c == none) - c = group; - - return Ordering::colamdConstrained(variableIndex, cmember); - } - - /* ************************************************************************* */ - Ordering Ordering::colamdConstrained(const VariableIndex& variableIndex, - const FastMap& groups) - { - gttic(Ordering_COLAMDConstrained); - size_t n = variableIndex.size(); - std::vector cmember(n, 0); - - // Build a mapping to look up sorted Key indices by Key - FastMap keyIndices; - size_t j = 0; - BOOST_FOREACH(const VariableIndex::value_type key_factors, variableIndex) - keyIndices.insert(keyIndices.end(), make_pair(key_factors.first, j++)); - - // Assign groups - typedef FastMap::value_type key_group; - BOOST_FOREACH(const key_group& p, groups) { - // FIXME: check that no groups are skipped - cmember[keyIndices.at(p.first)] = p.second; - } - - return Ordering::colamdConstrained(variableIndex, cmember); - } - - - /* ************************************************************************* */ - Ordering Ordering::metis(const MetisIndex& met) - { - gttic(Ordering_METIS); - - vector xadj = met.xadj(); - vector adj = met.adj(); - vector perm, iperm; - - idx_t size = met.nValues(); - for (idx_t i = 0; i < size; i++) - { - perm.push_back(0); - iperm.push_back(0); - } - - int outputError; - - outputError = METIS_NodeND(&size, &xadj[0], &adj[0], NULL, NULL, &perm[0], &iperm[0]); - Ordering result; - - if (outputError != METIS_OK) - { - std::cout << "METIS failed during Nested Dissection ordering!\n"; - return result; - } - - result.resize(size); - for (size_t j = 0; j < (size_t)size; ++j){ - // We have to add the minKey value back to obtain the original key in the Values - result[j] = met.intToKey(perm[j]); - } - return result; - } - - /* ************************************************************************* */ - void Ordering::print(const std::string& str, const KeyFormatter& keyFormatter) const - { - cout << str; - // Print ordering in index order - // Print the ordering with varsPerLine ordering entries printed on each line, - // for compactness. - static const size_t varsPerLine = 10; - bool endedOnNewline = false; - for(size_t i = 0; i < size(); ++i) { - if(i % varsPerLine == 0) - cout << "Position " << i << ": "; - if(i % varsPerLine != 0) - cout << ", "; - cout << keyFormatter(at(i)); - if(i % varsPerLine == varsPerLine - 1) { - cout << "\n"; - endedOnNewline = true; - } else { - endedOnNewline = false; - } - } - if(!endedOnNewline) +/* ************************************************************************* */ +void Ordering::print(const std::string& str, + const KeyFormatter& keyFormatter) const { + cout << str; + // Print ordering in index order + // Print the ordering with varsPerLine ordering entries printed on each line, + // for compactness. + static const size_t varsPerLine = 10; + bool endedOnNewline = false; + for (size_t i = 0; i < size(); ++i) { + if (i % varsPerLine == 0) + cout << "Position " << i << ": "; + if (i % varsPerLine != 0) + cout << ", "; + cout << keyFormatter(at(i)); + if (i % varsPerLine == varsPerLine - 1) { cout << "\n"; - cout.flush(); + endedOnNewline = true; + } else { + endedOnNewline = false; + } } + if (!endedOnNewline) + cout << "\n"; + cout.flush(); +} - /* ************************************************************************* */ - bool Ordering::equals(const Ordering& other, double tol) const - { - return (*this) == other; - } +/* ************************************************************************* */ +bool Ordering::equals(const Ordering& other, double tol) const { + return (*this) == other; +} } diff --git a/gtsam/inference/Ordering.h b/gtsam/inference/Ordering.h index 92f011c7d..e4cc2c55d 100644 --- a/gtsam/inference/Ordering.h +++ b/gtsam/inference/Ordering.h @@ -30,192 +30,210 @@ namespace gtsam { - class Ordering : public std::vector { - protected: - typedef std::vector Base; +class Ordering: public std::vector { +protected: + typedef std::vector Base; - public: +public: - /// Type of ordering to use - enum OrderingType { - COLAMD, METIS, CUSTOM - }; - - typedef Ordering This; ///< Typedef to this class - typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class - - /// Create an empty ordering - GTSAM_EXPORT Ordering() {} - - /// Create from a container - template - explicit Ordering(const KEYS& keys) : Base(keys.begin(), keys.end()) {} - - /// Create an ordering using iterators over keys - template - Ordering(ITERATOR firstKey, ITERATOR lastKey) : Base(firstKey, lastKey) {} - - /// Add new variables to the ordering as ordering += key1, key2, ... Equivalent to calling - /// push_back. - boost::assign::list_inserter > - operator+=(Key key) { - return boost::assign::make_list_inserter(boost::assign_detail::call_push_back(*this))(key); - } - - /// Invert (not reverse) the ordering - returns a map from key to order position - FastMap invert() const; - - /// @name Fill-reducing Orderings @{ - - /// Compute a fill-reducing ordering using COLAMD from a factor graph (see details for note on - /// performance). This internally builds a VariableIndex so if you already have a VariableIndex, - /// it is faster to use COLAMD(const VariableIndex&) - template - static Ordering colamd(const FactorGraph& graph) { - return colamd(VariableIndex(graph)); } - - /// Compute a fill-reducing ordering using COLAMD from a VariableIndex. - static GTSAM_EXPORT Ordering colamd(const VariableIndex& variableIndex); - - /// Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details - /// for note on performance). This internally builds a VariableIndex so if you already have a - /// VariableIndex, it is faster to use COLAMD(const VariableIndex&). This function constrains - /// the variables in \c constrainLast to the end of the ordering, and orders all other variables - /// before in a fill-reducing ordering. If \c forceOrder is true, the variables in \c - /// constrainLast will be ordered in the same order specified in the vector \c - /// constrainLast. If \c forceOrder is false, the variables in \c constrainLast will be - /// ordered after all the others, but will be rearranged by CCOLAMD to reduce fill-in as well. - template - static Ordering colamdConstrainedLast(const FactorGraph& graph, - const std::vector& constrainLast, bool forceOrder = false) { - return colamdConstrainedLast(VariableIndex(graph), constrainLast, forceOrder); } - - /// Compute a fill-reducing ordering using constrained COLAMD from a VariableIndex. This - /// function constrains the variables in \c constrainLast to the end of the ordering, and orders - /// all other variables before in a fill-reducing ordering. If \c forceOrder is true, the - /// variables in \c constrainLast will be ordered in the same order specified in the vector - /// \c constrainLast. If \c forceOrder is false, the variables in \c constrainLast will be - /// ordered after all the others, but will be rearranged by CCOLAMD to reduce fill-in as well. - static GTSAM_EXPORT Ordering colamdConstrainedLast(const VariableIndex& variableIndex, - const std::vector& constrainLast, bool forceOrder = false); - - /// Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details - /// for note on performance). This internally builds a VariableIndex so if you already have a - /// VariableIndex, it is faster to use COLAMD(const VariableIndex&). This function constrains - /// the variables in \c constrainLast to the end of the ordering, and orders all other variables - /// before in a fill-reducing ordering. If \c forceOrder is true, the variables in \c - /// constrainLast will be ordered in the same order specified in the vector \c - /// constrainLast. If \c forceOrder is false, the variables in \c constrainFirst will be - /// ordered after all the others, but will be rearranged by CCOLAMD to reduce fill-in as well. - template - static Ordering colamdConstrainedFirst(const FactorGraph& graph, - const std::vector& constrainFirst, bool forceOrder = false) { - return colamdConstrainedFirst(VariableIndex(graph), constrainFirst, forceOrder); } - - /// Compute a fill-reducing ordering using constrained COLAMD from a VariableIndex. This - /// function constrains the variables in \c constrainFirst to the front of the ordering, and - /// orders all other variables after in a fill-reducing ordering. If \c forceOrder is true, the - /// variables in \c constrainFirst will be ordered in the same order specified in the - /// vector \c constrainFirst. If \c forceOrder is false, the variables in \c - /// constrainFirst will be ordered after all the others, but will be rearranged by CCOLAMD to - /// reduce fill-in as well. - static GTSAM_EXPORT Ordering colamdConstrainedFirst(const VariableIndex& variableIndex, - const std::vector& constrainFirst, bool forceOrder = false); - - /// Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details - /// for note on performance). This internally builds a VariableIndex so if you already have a - /// VariableIndex, it is faster to use COLAMD(const VariableIndex&). In this function, a group - /// for each variable should be specified in \c groups, and each group of variables will appear - /// in the ordering in group index order. \c groups should be a map from Key to group index. - /// The group indices used should be consecutive starting at 0, but may appear in \c groups in - /// arbitrary order. Any variables not present in \c groups will be assigned to group 0. This - /// function simply fills the \c cmember argument to CCOLAMD with the supplied indices, see the - /// CCOLAMD documentation for more information. - template - static Ordering colamdConstrained(const FactorGraph& graph, - const FastMap& groups) { - return colamdConstrained(VariableIndex(graph), groups); } - - /// Compute a fill-reducing ordering using constrained COLAMD from a VariableIndex. In this - /// function, a group for each variable should be specified in \c groups, and each group of - /// variables will appear in the ordering in group index order. \c groups should be a map from - /// Key to group index. The group indices used should be consecutive starting at 0, but may - /// appear in \c groups in arbitrary order. Any variables not present in \c groups will be - /// assigned to group 0. This function simply fills the \c cmember argument to CCOLAMD with the - /// supplied indices, see the CCOLAMD documentation for more information. - static GTSAM_EXPORT Ordering colamdConstrained(const VariableIndex& variableIndex, - const FastMap& groups); - - /// Return a natural Ordering. Typically used by iterative solvers - template - static Ordering Natural(const FactorGraph &fg) { - FastSet src = fg.keys(); - std::vector keys(src.begin(), src.end()); - std::stable_sort(keys.begin(), keys.end()); - return Ordering(keys); - } - - /// METIS Formatting function - template - static GTSAM_EXPORT void CSRFormat(std::vector& xadj, std::vector& adj, const FactorGraph& graph); - - /// Compute an ordering determined by METIS from a VariableIndex - static GTSAM_EXPORT Ordering metis(const MetisIndex& met); - - template - static Ordering metis(const FactorGraph& graph) - { - return metis(MetisIndex(graph)); - } - - /// @} - - /// @name Named Constructors @{ - - template - static Ordering Create(OrderingType orderingType, - const FactorGraph& graph) { - - switch (orderingType) { - case COLAMD: - return colamd(graph); - case METIS: - return metis(graph); - case CUSTOM: - throw std::runtime_error( - "Ordering::Create error: called with CUSTOM ordering type."); - default: - throw std::runtime_error( - "Ordering::Create error: called with unknown ordering type."); - } - } - - /// @} - - /// @name Testable @{ - - GTSAM_EXPORT void print(const std::string& str = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; - - GTSAM_EXPORT bool equals(const Ordering& other, double tol = 1e-9) const; - - /// @} - - private: - /// Internal COLAMD function - static GTSAM_EXPORT Ordering colamdConstrained( - const VariableIndex& variableIndex, std::vector& cmember); - - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - } + /// Type of ordering to use + enum OrderingType { + COLAMD, METIS, CUSTOM }; - /// traits - template<> struct traits : public Testable {}; + typedef Ordering This; ///< Typedef to this class + typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class + + /// Create an empty ordering + GTSAM_EXPORT + Ordering() { + } + + /// Create from a container + template + explicit Ordering(const KEYS& keys) : + Base(keys.begin(), keys.end()) { + } + + /// Create an ordering using iterators over keys + template + Ordering(ITERATOR firstKey, ITERATOR lastKey) : + Base(firstKey, lastKey) { + } + + /// Add new variables to the ordering as ordering += key1, key2, ... Equivalent to calling + /// push_back. + boost::assign::list_inserter > operator+=( + Key key) { + return boost::assign::make_list_inserter( + boost::assign_detail::call_push_back(*this))(key); + } + + /// Invert (not reverse) the ordering - returns a map from key to order position + FastMap invert() const; + + /// @name Fill-reducing Orderings @{ + + /// Compute a fill-reducing ordering using COLAMD from a factor graph (see details for note on + /// performance). This internally builds a VariableIndex so if you already have a VariableIndex, + /// it is faster to use COLAMD(const VariableIndex&) + template + static Ordering colamd(const FactorGraph& graph) { + return colamd(VariableIndex(graph)); + } + + /// Compute a fill-reducing ordering using COLAMD from a VariableIndex. + static GTSAM_EXPORT Ordering colamd(const VariableIndex& variableIndex); + + /// Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details + /// for note on performance). This internally builds a VariableIndex so if you already have a + /// VariableIndex, it is faster to use COLAMD(const VariableIndex&). This function constrains + /// the variables in \c constrainLast to the end of the ordering, and orders all other variables + /// before in a fill-reducing ordering. If \c forceOrder is true, the variables in \c + /// constrainLast will be ordered in the same order specified in the vector \c + /// constrainLast. If \c forceOrder is false, the variables in \c constrainLast will be + /// ordered after all the others, but will be rearranged by CCOLAMD to reduce fill-in as well. + template + static Ordering colamdConstrainedLast(const FactorGraph& graph, + const std::vector& constrainLast, bool forceOrder = false) { + return colamdConstrainedLast(VariableIndex(graph), constrainLast, + forceOrder); + } + + /// Compute a fill-reducing ordering using constrained COLAMD from a VariableIndex. This + /// function constrains the variables in \c constrainLast to the end of the ordering, and orders + /// all other variables before in a fill-reducing ordering. If \c forceOrder is true, the + /// variables in \c constrainLast will be ordered in the same order specified in the vector + /// \c constrainLast. If \c forceOrder is false, the variables in \c constrainLast will be + /// ordered after all the others, but will be rearranged by CCOLAMD to reduce fill-in as well. + static GTSAM_EXPORT Ordering colamdConstrainedLast( + const VariableIndex& variableIndex, const std::vector& constrainLast, + bool forceOrder = false); + + /// Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details + /// for note on performance). This internally builds a VariableIndex so if you already have a + /// VariableIndex, it is faster to use COLAMD(const VariableIndex&). This function constrains + /// the variables in \c constrainLast to the end of the ordering, and orders all other variables + /// before in a fill-reducing ordering. If \c forceOrder is true, the variables in \c + /// constrainLast will be ordered in the same order specified in the vector \c + /// constrainLast. If \c forceOrder is false, the variables in \c constrainFirst will be + /// ordered after all the others, but will be rearranged by CCOLAMD to reduce fill-in as well. + template + static Ordering colamdConstrainedFirst(const FactorGraph& graph, + const std::vector& constrainFirst, bool forceOrder = false) { + return colamdConstrainedFirst(VariableIndex(graph), constrainFirst, + forceOrder); + } + + /// Compute a fill-reducing ordering using constrained COLAMD from a VariableIndex. This + /// function constrains the variables in \c constrainFirst to the front of the ordering, and + /// orders all other variables after in a fill-reducing ordering. If \c forceOrder is true, the + /// variables in \c constrainFirst will be ordered in the same order specified in the + /// vector \c constrainFirst. If \c forceOrder is false, the variables in \c + /// constrainFirst will be ordered after all the others, but will be rearranged by CCOLAMD to + /// reduce fill-in as well. + static GTSAM_EXPORT Ordering colamdConstrainedFirst( + const VariableIndex& variableIndex, + const std::vector& constrainFirst, bool forceOrder = false); + + /// Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details + /// for note on performance). This internally builds a VariableIndex so if you already have a + /// VariableIndex, it is faster to use COLAMD(const VariableIndex&). In this function, a group + /// for each variable should be specified in \c groups, and each group of variables will appear + /// in the ordering in group index order. \c groups should be a map from Key to group index. + /// The group indices used should be consecutive starting at 0, but may appear in \c groups in + /// arbitrary order. Any variables not present in \c groups will be assigned to group 0. This + /// function simply fills the \c cmember argument to CCOLAMD with the supplied indices, see the + /// CCOLAMD documentation for more information. + template + static Ordering colamdConstrained(const FactorGraph& graph, + const FastMap& groups) { + return colamdConstrained(VariableIndex(graph), groups); + } + + /// Compute a fill-reducing ordering using constrained COLAMD from a VariableIndex. In this + /// function, a group for each variable should be specified in \c groups, and each group of + /// variables will appear in the ordering in group index order. \c groups should be a map from + /// Key to group index. The group indices used should be consecutive starting at 0, but may + /// appear in \c groups in arbitrary order. Any variables not present in \c groups will be + /// assigned to group 0. This function simply fills the \c cmember argument to CCOLAMD with the + /// supplied indices, see the CCOLAMD documentation for more information. + static GTSAM_EXPORT Ordering colamdConstrained( + const VariableIndex& variableIndex, const FastMap& groups); + + /// Return a natural Ordering. Typically used by iterative solvers + template + static Ordering Natural(const FactorGraph &fg) { + FastSet src = fg.keys(); + std::vector keys(src.begin(), src.end()); + std::stable_sort(keys.begin(), keys.end()); + return Ordering(keys); + } + + /// METIS Formatting function + template + static GTSAM_EXPORT void CSRFormat(std::vector& xadj, + std::vector& adj, const FactorGraph& graph); + + /// Compute an ordering determined by METIS from a VariableIndex + static GTSAM_EXPORT Ordering metis(const MetisIndex& met); + + template + static Ordering metis(const FactorGraph& graph) { + return metis(MetisIndex(graph)); + } + + /// @} + + /// @name Named Constructors @{ + + template + static Ordering Create(OrderingType orderingType, + const FactorGraph& graph) { + + switch (orderingType) { + case COLAMD: + return colamd(graph); + case METIS: + return metis(graph); + case CUSTOM: + throw std::runtime_error( + "Ordering::Create error: called with CUSTOM ordering type."); + default: + throw std::runtime_error( + "Ordering::Create error: called with unknown ordering type."); + } + } + + /// @} + + /// @name Testable @{ + + GTSAM_EXPORT + void print(const std::string& str = "", const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const; + + GTSAM_EXPORT + bool equals(const Ordering& other, double tol = 1e-9) const; + + /// @} + +private: + /// Internal COLAMD function + static GTSAM_EXPORT Ordering colamdConstrained( + const VariableIndex& variableIndex, std::vector& cmember); + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + } +}; + +/// traits +template<> struct traits : public Testable { +}; } diff --git a/gtsam/inference/tests/testOrdering.cpp b/gtsam/inference/tests/testOrdering.cpp index ee8751f47..26efb65b2 100644 --- a/gtsam/inference/tests/testOrdering.cpp +++ b/gtsam/inference/tests/testOrdering.cpp @@ -56,7 +56,8 @@ TEST(Ordering, constrained_ordering) { EXPECT(assert_equal(expConstrained, actConstrained)); // constrained version - push one set to the start - Ordering actConstrained2 = Ordering::colamdConstrainedFirst(sfg, list_of(2)(4)); + Ordering actConstrained2 = Ordering::colamdConstrainedFirst(sfg, + list_of(2)(4)); Ordering expConstrained2 = Ordering(list_of(2)(4)(0)(1)(3)(5)); EXPECT(assert_equal(expConstrained2, actConstrained2)); } @@ -82,43 +83,41 @@ TEST(Ordering, grouped_constrained_ordering) { /* ************************************************************************* */ TEST(Ordering, csr_format) { - // Example in METIS manual - SymbolicFactorGraph sfg; - sfg.push_factor(0, 1); - sfg.push_factor(1, 2); - sfg.push_factor(2, 3); - sfg.push_factor(3, 4); - sfg.push_factor(5, 6); - sfg.push_factor(6, 7); - sfg.push_factor(7, 8); - sfg.push_factor(8, 9); - sfg.push_factor(10, 11); - sfg.push_factor(11, 12); - sfg.push_factor(12, 13); - sfg.push_factor(13, 14); + // Example in METIS manual + SymbolicFactorGraph sfg; + sfg.push_factor(0, 1); + sfg.push_factor(1, 2); + sfg.push_factor(2, 3); + sfg.push_factor(3, 4); + sfg.push_factor(5, 6); + sfg.push_factor(6, 7); + sfg.push_factor(7, 8); + sfg.push_factor(8, 9); + sfg.push_factor(10, 11); + sfg.push_factor(11, 12); + sfg.push_factor(12, 13); + sfg.push_factor(13, 14); - sfg.push_factor(0, 5); - sfg.push_factor(5, 10); - sfg.push_factor(1, 6); - sfg.push_factor(6, 11); - sfg.push_factor(2, 7); - sfg.push_factor(7, 12); - sfg.push_factor(3, 8); - sfg.push_factor(8, 13); - sfg.push_factor(4, 9); - sfg.push_factor(9, 14); + sfg.push_factor(0, 5); + sfg.push_factor(5, 10); + sfg.push_factor(1, 6); + sfg.push_factor(6, 11); + sfg.push_factor(2, 7); + sfg.push_factor(7, 12); + sfg.push_factor(3, 8); + sfg.push_factor(8, 13); + sfg.push_factor(4, 9); + sfg.push_factor(9, 14); - MetisIndex mi(sfg); + MetisIndex mi(sfg); - vector xadjExpected, adjExpected; - xadjExpected += 0, 2, 5, 8, 11, 13, 16, 20, 24, 28, 31, 33, 36, 39, 42, 44; - adjExpected += 1, 5, 0, 2, 6, 1, 3, 7, 2, 4, 8, 3, 9, 0, 6, 10, 1, 5, 7, 11, - 2, 6, 8, 12, 3, 7, 9, 13, 4, 8, 14, 5, 11, 6, 10, 12, 7, 11, - 13, 8, 12, 14, 9, 13 ; + vector xadjExpected, adjExpected; + xadjExpected += 0, 2, 5, 8, 11, 13, 16, 20, 24, 28, 31, 33, 36, 39, 42, 44; + adjExpected += 1, 5, 0, 2, 6, 1, 3, 7, 2, 4, 8, 3, 9, 0, 6, 10, 1, 5, 7, 11, 2, 6, 8, 12, 3, 7, 9, 13, 4, 8, 14, 5, 11, 6, 10, 12, 7, 11, 13, 8, 12, 14, 9, 13; - EXPECT(xadjExpected == mi.xadj()); - EXPECT(adjExpected.size() == mi.adj().size()); - EXPECT(adjExpected == mi.adj()); + EXPECT(xadjExpected == mi.xadj()); + EXPECT(adjExpected.size() == mi.adj().size()); + EXPECT(adjExpected == mi.adj()); } /* ************************************************************************* */ @@ -136,7 +135,7 @@ TEST(Ordering, csr_format_2) { vector xadjExpected, adjExpected; xadjExpected += 0, 1, 4, 6, 8, 10; - adjExpected += 1, 0, 2, 4, 1, 3, 2, 4, 1, 3; + adjExpected += 1, 0, 2, 4, 1, 3, 2, 4, 1, 3; EXPECT(xadjExpected == mi.xadj()); EXPECT(adjExpected.size() == mi.adj().size()); @@ -237,18 +236,18 @@ TEST(Ordering, MetisLoop) { SymbolicFactorGraph sfg = example::symbolicChain(); // add loop closure - sfg.push_factor(0,5); + sfg.push_factor(0, 5); // METIS { - Ordering actual = Ordering::Create(Ordering::METIS,sfg); - // 0,3 - // 1 - // 2 - // 4 - // 5 - Ordering expected = Ordering(list_of(5)(4)(2)(1)(0)(3)); - EXPECT(assert_equal(expected, actual)); + Ordering actual = Ordering::Create(Ordering::METIS, sfg); + // 0,3 + // 1,3 + // 2 + // 4,0 + // 5 + Ordering expected = Ordering(list_of(5)(4)(2)(1)(0)(3)); + EXPECT(assert_equal(expected, actual)); } } @@ -260,28 +259,31 @@ TEST(Ordering, Create) { // COLAMD { - Ordering actual = Ordering::Create(Ordering::COLAMD,sfg); - Ordering expected = Ordering(list_of(0)(1)(2)(3)(4)(5)); - EXPECT(assert_equal(expected, actual)); + Ordering actual = Ordering::Create(Ordering::COLAMD, sfg); + Ordering expected = Ordering(list_of(0)(1)(2)(3)(4)(5)); + EXPECT(assert_equal(expected, actual)); } // METIS { - Ordering actual = Ordering::Create(Ordering::METIS,sfg); - // 2 - // 0 - // 1 - // 4 - // 3 - // 5 - Ordering expected = Ordering(list_of(5)(3)(4)(1)(0)(2)); - EXPECT(assert_equal(expected, actual)); + Ordering actual = Ordering::Create(Ordering::METIS, sfg); + // 2 + // 0 + // 1 + // 4 + // 3 + // 5 + Ordering expected = Ordering(list_of(5)(3)(4)(1)(0)(2)); + EXPECT(assert_equal(expected, actual)); } // CUSTOM - CHECK_EXCEPTION(Ordering::Create(Ordering::CUSTOM,sfg),runtime_error); + CHECK_EXCEPTION(Ordering::Create(Ordering::CUSTOM, sfg), runtime_error); } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ From 0be63753bc3ffe31563cbb1fd975a53d94470389 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 13 Feb 2015 17:17:11 +0100 Subject: [PATCH 06/16] Call Ordering::Create to make sure Metis is executed when asked (was a bug!) --- gtsam/nonlinear/DoglegOptimizer.cpp | 4 ++-- gtsam/nonlinear/GaussNewtonOptimizer.cpp | 7 +++---- gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp | 8 ++------ 3 files changed, 7 insertions(+), 12 deletions(-) diff --git a/gtsam/nonlinear/DoglegOptimizer.cpp b/gtsam/nonlinear/DoglegOptimizer.cpp index e148bd65d..a91515e9c 100644 --- a/gtsam/nonlinear/DoglegOptimizer.cpp +++ b/gtsam/nonlinear/DoglegOptimizer.cpp @@ -95,8 +95,8 @@ void DoglegOptimizer::iterate(void) { /* ************************************************************************* */ DoglegParams DoglegOptimizer::ensureHasOrdering(DoglegParams params, const NonlinearFactorGraph& graph) const { - if(!params.ordering) - params.ordering = Ordering::colamd(graph); + if (!params.ordering) + params.ordering = Ordering::Create(params.orderingType, graph); return params; } diff --git a/gtsam/nonlinear/GaussNewtonOptimizer.cpp b/gtsam/nonlinear/GaussNewtonOptimizer.cpp index 7a115e1c4..e420567ec 100644 --- a/gtsam/nonlinear/GaussNewtonOptimizer.cpp +++ b/gtsam/nonlinear/GaussNewtonOptimizer.cpp @@ -46,10 +46,9 @@ void GaussNewtonOptimizer::iterate() { /* ************************************************************************* */ GaussNewtonParams GaussNewtonOptimizer::ensureHasOrdering( - GaussNewtonParams params, const NonlinearFactorGraph& graph) const -{ - if(!params.ordering) - params.ordering = Ordering::colamd(graph); + GaussNewtonParams params, const NonlinearFactorGraph& graph) const { + if (!params.ordering) + params.ordering = Ordering::Create(params.orderingType, graph); return params; } diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index 1c781f173..ef1f1eff1 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -344,12 +344,8 @@ void LevenbergMarquardtOptimizer::iterate() { /* ************************************************************************* */ LevenbergMarquardtParams LevenbergMarquardtOptimizer::ensureHasOrdering( LevenbergMarquardtParams params, const NonlinearFactorGraph& graph) const { - if (!params.ordering){ - if (params.orderingType == Ordering::METIS) - params.ordering = Ordering::metis(graph); - else - params.ordering = Ordering::colamd(graph); - } + if (!params.ordering) + params.ordering = Ordering::Create(params.orderingType, graph); return params; } From 36426fade45d985ceedd9690aba24619fccb8dcb Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 13 Feb 2015 20:22:52 +0100 Subject: [PATCH 07/16] Some more tests and comments about associated Bayes trees. All succeed on Mac. --- .cproject | 26 +++++++-- gtsam/inference/tests/testOrdering.cpp | 23 ++++---- .../symbolic/tests/testSymbolicBayesTree.cpp | 57 +++++++++++++++++++ 3 files changed, 91 insertions(+), 15 deletions(-) diff --git a/.cproject b/.cproject index b77415840..7086ce250 100644 --- a/.cproject +++ b/.cproject @@ -1277,7 +1277,6 @@ make - testSimulated2DOriented.run true false @@ -1317,7 +1316,6 @@ make - testSimulated2D.run true false @@ -1325,7 +1323,6 @@ make - testSimulated3D.run true false @@ -1429,6 +1426,7 @@ make + testErrors.run true false @@ -1723,6 +1721,7 @@ cpack + -G DEB true false @@ -1730,6 +1729,7 @@ cpack + -G RPM true false @@ -1737,6 +1737,7 @@ cpack + -G TGZ true false @@ -1744,6 +1745,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -1935,7 +1937,6 @@ make - tests/testGaussianISAM2 true false @@ -2087,6 +2088,7 @@ make + tests/testBayesTree.run true false @@ -2094,6 +2096,7 @@ make + testBinaryBayesNet.run true false @@ -2141,6 +2144,7 @@ make + testSymbolicBayesNet.run true false @@ -2148,6 +2152,7 @@ make + tests/testSymbolicFactor.run true false @@ -2155,6 +2160,7 @@ make + testSymbolicFactorGraph.run true false @@ -2170,6 +2176,7 @@ make + tests/testBayesTree true false @@ -2671,6 +2678,14 @@ true true + + make + -j4 + testSymbolicBayesTree.run + true + true + true + make -j5 @@ -3289,6 +3304,7 @@ make + testGraph.run true false @@ -3296,6 +3312,7 @@ make + testJunctionTree.run true false @@ -3303,6 +3320,7 @@ make + testSymbolicBayesNetB.run true false diff --git a/gtsam/inference/tests/testOrdering.cpp b/gtsam/inference/tests/testOrdering.cpp index 26efb65b2..6320ca748 100644 --- a/gtsam/inference/tests/testOrdering.cpp +++ b/gtsam/inference/tests/testOrdering.cpp @@ -241,11 +241,10 @@ TEST(Ordering, MetisLoop) { // METIS { Ordering actual = Ordering::Create(Ordering::METIS, sfg); - // 0,3 - // 1,3 - // 2 - // 4,0 - // 5 + // - P( 1 0 3) + // | - P( 4 | 0 3) + // | | - P( 5 | 0 4) + // | - P( 2 | 1 3) Ordering expected = Ordering(list_of(5)(4)(2)(1)(0)(3)); EXPECT(assert_equal(expected, actual)); } @@ -259,6 +258,11 @@ TEST(Ordering, Create) { // COLAMD { + //- P( 4 5) + //| - P( 3 | 4) + //| | - P( 2 | 3) + //| | | - P( 1 | 2) + //| | | | - P( 0 | 1) Ordering actual = Ordering::Create(Ordering::COLAMD, sfg); Ordering expected = Ordering(list_of(0)(1)(2)(3)(4)(5)); EXPECT(assert_equal(expected, actual)); @@ -267,12 +271,9 @@ TEST(Ordering, Create) { // METIS { Ordering actual = Ordering::Create(Ordering::METIS, sfg); - // 2 - // 0 - // 1 - // 4 - // 3 - // 5 + //- P( 1 0 2) + //| - P( 3 4 | 2) + //| | - P( 5 | 4) Ordering expected = Ordering(list_of(5)(3)(4)(1)(0)(2)); EXPECT(assert_equal(expected, actual)); } diff --git a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp index 3c02f6dbd..979786515 100644 --- a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp @@ -691,6 +691,63 @@ TEST(SymbolicBayesTree, complicatedMarginal) } } + +/* ************************************************************************* */ +TEST(SymbolicBayesTree, COLAMDvsMETIS) { + + // create circular graph + SymbolicFactorGraph sfg; + sfg.push_factor(0, 1); + sfg.push_factor(1, 2); + sfg.push_factor(2, 3); + sfg.push_factor(3, 4); + sfg.push_factor(4, 5); + sfg.push_factor(0, 5); + + // COLAMD + { + Ordering ordering = Ordering::Create(Ordering::COLAMD, sfg); + EXPECT(assert_equal(Ordering(list_of(0)(5)(1)(4)(2)(3)), ordering)); + + // - P( 4 2 3) + // | - P( 1 | 2 4) + // | | - P( 5 | 1 4) + // | | | - P( 0 | 1 5) + SymbolicBayesTree expected; + expected.insertRoot( + MakeClique(list_of(4)(2)(3), 3, + list_of( + MakeClique(list_of(1)(2)(4), 1, + list_of( + MakeClique(list_of(5)(1)(4), 1, + list_of(MakeClique(list_of(0)(1)(5), 1)))))))); + + SymbolicBayesTree actual = *sfg.eliminateMultifrontal(ordering); + EXPECT(assert_equal(expected, actual)); + } + + // METIS + { + Ordering ordering = Ordering::Create(Ordering::METIS, sfg); + EXPECT(assert_equal(Ordering(list_of(5)(4)(2)(1)(0)(3)), ordering)); + + // - P( 1 0 3) + // | - P( 4 | 0 3) + // | | - P( 5 | 0 4) + // | - P( 2 | 1 3) + SymbolicBayesTree expected; + expected.insertRoot( + MakeClique(list_of(1)(0)(3), 3, + list_of( + MakeClique(list_of(4)(0)(3), 1, + list_of(MakeClique(list_of(5)(0)(4), 1))))( + MakeClique(list_of(2)(1)(3), 1)))); + + SymbolicBayesTree actual = *sfg.eliminateMultifrontal(ordering); + EXPECT(assert_equal(expected, actual)); + } +} + /* ************************************************************************* */ int main() { TestResult tr; From a753f763c0f0d8c92ae5473d9725f0e82f325cdf Mon Sep 17 00:00:00 2001 From: Paul Drews Date: Fri, 13 Feb 2015 22:19:44 -0500 Subject: [PATCH 08/16] Expect different ordering on Linux --- gtsam/inference/tests/testOrdering.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/gtsam/inference/tests/testOrdering.cpp b/gtsam/inference/tests/testOrdering.cpp index 6320ca748..37b6ce3c6 100644 --- a/gtsam/inference/tests/testOrdering.cpp +++ b/gtsam/inference/tests/testOrdering.cpp @@ -239,6 +239,17 @@ TEST(Ordering, MetisLoop) { sfg.push_factor(0, 5); // METIS +#ifdef LINUX + { + Ordering actual = Ordering::Create(Ordering::METIS, sfg); + // - P( 0 4 1) + // | - P( 2 | 4 1) + // | | - P( 3 | 4 2) + // | - P( 5 | 0 1) + Ordering expected = Ordering(list_of(3)(2)(5)(0)(4)(1)); + EXPECT(assert_equal(expected, actual)); + } +#else { Ordering actual = Ordering::Create(Ordering::METIS, sfg); // - P( 1 0 3) @@ -248,6 +259,7 @@ TEST(Ordering, MetisLoop) { Ordering expected = Ordering(list_of(5)(4)(2)(1)(0)(3)); EXPECT(assert_equal(expected, actual)); } +#endif } /* ************************************************************************* */ From 8d89529c98b14d3af40ef5df780057c17296daab Mon Sep 17 00:00:00 2001 From: pdrews Date: Fri, 13 Feb 2015 22:49:15 -0500 Subject: [PATCH 09/16] Metis ordering difference between linux/mac --- gtsam/inference/tests/testOrdering.cpp | 2 +- .../symbolic/tests/testSymbolicBayesTree.cpp | 21 +++++++++++++++---- 2 files changed, 18 insertions(+), 5 deletions(-) diff --git a/gtsam/inference/tests/testOrdering.cpp b/gtsam/inference/tests/testOrdering.cpp index 37b6ce3c6..8f2c417d3 100644 --- a/gtsam/inference/tests/testOrdering.cpp +++ b/gtsam/inference/tests/testOrdering.cpp @@ -239,7 +239,7 @@ TEST(Ordering, MetisLoop) { sfg.push_factor(0, 5); // METIS -#ifdef LINUX +#if !defined(__APPLE__) { Ordering actual = Ordering::Create(Ordering::METIS, sfg); // - P( 0 4 1) diff --git a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp index 979786515..93c38b324 100644 --- a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp @@ -729,20 +729,33 @@ TEST(SymbolicBayesTree, COLAMDvsMETIS) { // METIS { Ordering ordering = Ordering::Create(Ordering::METIS, sfg); +// Linux and Mac split differently when using mettis +#if !defined(__APPLE__) + EXPECT(assert_equal(Ordering(list_of(3)(2)(5)(0)(4)(1)), ordering)); +#else EXPECT(assert_equal(Ordering(list_of(5)(4)(2)(1)(0)(3)), ordering)); +#endif // - P( 1 0 3) // | - P( 4 | 0 3) // | | - P( 5 | 0 4) // | - P( 2 | 1 3) SymbolicBayesTree expected; +#if !defined(__APPLE__) expected.insertRoot( - MakeClique(list_of(1)(0)(3), 3, + MakeClique(list_of(2)(4)(1), 3, list_of( - MakeClique(list_of(4)(0)(3), 1, + MakeClique(list_of(0)(1)(4), 1, list_of(MakeClique(list_of(5)(0)(4), 1))))( - MakeClique(list_of(2)(1)(3), 1)))); - + MakeClique(list_of(3)(2)(4), 1)))); +#else + expected.insertRoot( + MakeClique(list_of(1)(0)(3), 3, + list_of( + MakeClique(list_of(4)(0)(3), 1, + list_of(MakeClique(list_of(5)(0)(4), 1))))( + MakeClique(list_of(2)(1)(3), 1)))); +#endif SymbolicBayesTree actual = *sfg.eliminateMultifrontal(ordering); EXPECT(assert_equal(expected, actual)); } From 3b1c6b1b1e2f86eaca0a67e9c179fcd3a40434b0 Mon Sep 17 00:00:00 2001 From: zhaoyang Date: Wed, 18 Feb 2015 23:36:31 -0500 Subject: [PATCH 10/16] add in a COLAMD vs METIS exmaple --- examples/SFMExample_bal_COLAMD_METIS.cpp | 145 +++++++++++++++++++++++ 1 file changed, 145 insertions(+) create mode 100644 examples/SFMExample_bal_COLAMD_METIS.cpp diff --git a/examples/SFMExample_bal_COLAMD_METIS.cpp b/examples/SFMExample_bal_COLAMD_METIS.cpp new file mode 100644 index 000000000..59e4a087f --- /dev/null +++ b/examples/SFMExample_bal_COLAMD_METIS.cpp @@ -0,0 +1,145 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file SFMExample.cpp + * @brief This file is to compare the ordering performance for COLAMD vs METIS. + * Example problem is to solve a structure-from-motion problem from a "Bundle Adjustment in the Large" file. + * @author Frank Dellaert, Zhaoyang Lv + */ + +// For an explanation of headers, see SFMExample.cpp +#include +#include +#include +#include +#include +#include // for loading BAL datasets ! +#include + +using namespace std; +using namespace gtsam; +using symbol_shorthand::C; +using symbol_shorthand::P; + +// We will be using a projection factor that ties a SFM_Camera to a 3D point. +// An SFM_Camera is defined in datase.h as a camera with unknown Cal3Bundler calibration +// and has a total of 9 free parameters +typedef GeneralSFMFactor MyFactor; + +/* ************************************************************************* */ +int main (int argc, char* argv[]) { + + // Find default file, but if an argument is given, try loading a file + string filename = findExampleDataFile("dubrovnik-3-7-pre"); + if (argc>1) filename = string(argv[1]); + + // Load the SfM data from file + SfM_data mydata; + readBAL(filename, mydata); + cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.number_tracks() % mydata.number_cameras(); + + // Create a factor graph + NonlinearFactorGraph graph; + + // We share *one* noiseModel between all projection factors + noiseModel::Isotropic::shared_ptr noise = + noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v + + // Add measurements to the factor graph + size_t j = 0; + BOOST_FOREACH(const SfM_Track& track, mydata.tracks) { + BOOST_FOREACH(const SfM_Measurement& m, track.measurements) { + size_t i = m.first; + Point2 uv = m.second; + graph.push_back(MyFactor(uv, noise, C(i), P(j))); // note use of shorthand symbols C and P + } + j += 1; + } + + // Add a prior on pose x1. This indirectly specifies where the origin is. + // and a prior on the position of the first landmark to fix the scale + graph.push_back(PriorFactor(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1))); + graph.push_back(PriorFactor (P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1))); + + // Create initial estimate + Values initial; + size_t i = 0; j = 0; + BOOST_FOREACH(const SfM_Camera& camera, mydata.cameras) initial.insert(C(i++), camera); + BOOST_FOREACH(const SfM_Track& track, mydata.tracks) initial.insert(P(j++), track.p); + + /** ---------------------------------------------------**/ + + /* With COLAMD, optimize the graph and print the results */ + cout << "Optimize with COLAMD..." << endl; + + Values result_COLAMD; + try { + double tic_t = clock(); + + LevenbergMarquardtParams params_using_COLAMD; + params_using_COLAMD.setVerbosity("ERROR"); + params_using_COLAMD.ordering = Ordering::Create(Ordering::COLAMD, graph); + + double toc_t = (clock() - tic_t)/CLOCKS_PER_SEC; + + tic_t = clock(); + + LevenbergMarquardtOptimizer lm(graph, initial, params_using_COLAMD); + result_COLAMD = lm.optimize(); + + tic_t = clock(); + + cout << "Ordering: " << toc_t << "seconds" << endl; + cout << "Solving: " << (clock() - tic_t)/CLOCKS_PER_SEC << "seconds" << endl; + + } catch (exception& e) { + cout << e.what(); + } + + // To see the error, check SFMExample_bal.cpp file + //cout << "final error: " << graph.error(result_COLAMD) << endl; + + /** ---------------------------------------------------**/ + + /* with METIS, optimize the graph and print the results */ + cout << "Optimize with METIS" << endl; + + Values results_METIS; + try { + double tic_t = clock(); + + LevenbergMarquardtParams params_using_METIS; + params_using_METIS.setVerbosity("ERROR"); + params_using_METIS.ordering = Ordering::Create(Ordering::METIS, graph); + + double toc_t = (clock() - tic_t)/CLOCKS_PER_SEC; + + tic_t = clock(); + + LevenbergMarquardtOptimizer lm(graph, initial, params_using_METIS); + results_METIS = lm.optimize(); + + tic_t = clock(); + + cout << "Ordering: " << toc_t << "seconds" << endl; + cout << "Solving: " << (clock() - tic_t)/CLOCKS_PER_SEC << "seconds" << endl; + + } catch (exception& e) { + cout << e.what(); + } + + + + return 0; +} +/* ************************************************************************* */ + From c2a223ddbb0ff605dfef2899c4991693202537d8 Mon Sep 17 00:00:00 2001 From: zhaoyang Date: Wed, 18 Feb 2015 23:58:53 -0500 Subject: [PATCH 11/16] copy the nonlinearfactor graph for two independent tests --- examples/SFMExample_bal_COLAMD_METIS.cpp | 26 +++++++++++++++--------- 1 file changed, 16 insertions(+), 10 deletions(-) diff --git a/examples/SFMExample_bal_COLAMD_METIS.cpp b/examples/SFMExample_bal_COLAMD_METIS.cpp index 59e4a087f..e2dff2cb2 100644 --- a/examples/SFMExample_bal_COLAMD_METIS.cpp +++ b/examples/SFMExample_bal_COLAMD_METIS.cpp @@ -48,7 +48,7 @@ int main (int argc, char* argv[]) { cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.number_tracks() % mydata.number_cameras(); // Create a factor graph - NonlinearFactorGraph graph; + NonlinearFactorGraph graph_for_COLAMD; // We share *one* noiseModel between all projection factors noiseModel::Isotropic::shared_ptr noise = @@ -60,15 +60,15 @@ int main (int argc, char* argv[]) { BOOST_FOREACH(const SfM_Measurement& m, track.measurements) { size_t i = m.first; Point2 uv = m.second; - graph.push_back(MyFactor(uv, noise, C(i), P(j))); // note use of shorthand symbols C and P + graph_for_COLAMD.push_back(MyFactor(uv, noise, C(i), P(j))); // note use of shorthand symbols C and P } j += 1; } // Add a prior on pose x1. This indirectly specifies where the origin is. // and a prior on the position of the first landmark to fix the scale - graph.push_back(PriorFactor(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1))); - graph.push_back(PriorFactor (P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1))); + graph_for_COLAMD.push_back(PriorFactor(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1))); + graph_for_COLAMD.push_back(PriorFactor (P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1))); // Create initial estimate Values initial; @@ -76,7 +76,11 @@ int main (int argc, char* argv[]) { BOOST_FOREACH(const SfM_Camera& camera, mydata.cameras) initial.insert(C(i++), camera); BOOST_FOREACH(const SfM_Track& track, mydata.tracks) initial.insert(P(j++), track.p); - /** ---------------------------------------------------**/ + NonlinearFactorGraph graph_for_METIS = graph_for_COLAMD.clone(); + + + /** --------------- COMPARISON -----------------------**/ + /** ----------------------------------------------------**/ /* With COLAMD, optimize the graph and print the results */ cout << "Optimize with COLAMD..." << endl; @@ -87,13 +91,13 @@ int main (int argc, char* argv[]) { LevenbergMarquardtParams params_using_COLAMD; params_using_COLAMD.setVerbosity("ERROR"); - params_using_COLAMD.ordering = Ordering::Create(Ordering::COLAMD, graph); + params_using_COLAMD.ordering = Ordering::Create(Ordering::COLAMD, graph_for_COLAMD); double toc_t = (clock() - tic_t)/CLOCKS_PER_SEC; tic_t = clock(); - LevenbergMarquardtOptimizer lm(graph, initial, params_using_COLAMD); + LevenbergMarquardtOptimizer lm(graph_for_COLAMD, initial, params_using_COLAMD); result_COLAMD = lm.optimize(); tic_t = clock(); @@ -105,12 +109,14 @@ int main (int argc, char* argv[]) { cout << e.what(); } + cout << endl << endl; + // To see the error, check SFMExample_bal.cpp file //cout << "final error: " << graph.error(result_COLAMD) << endl; /** ---------------------------------------------------**/ - /* with METIS, optimize the graph and print the results */ + cout << "Optimize with METIS" << endl; Values results_METIS; @@ -119,13 +125,13 @@ int main (int argc, char* argv[]) { LevenbergMarquardtParams params_using_METIS; params_using_METIS.setVerbosity("ERROR"); - params_using_METIS.ordering = Ordering::Create(Ordering::METIS, graph); + params_using_METIS.ordering = Ordering::Create(Ordering::METIS, graph_for_METIS); double toc_t = (clock() - tic_t)/CLOCKS_PER_SEC; tic_t = clock(); - LevenbergMarquardtOptimizer lm(graph, initial, params_using_METIS); + LevenbergMarquardtOptimizer lm(graph_for_METIS, initial, params_using_METIS); results_METIS = lm.optimize(); tic_t = clock(); From 55729e0e6901dab73b84702e884ad9db9e0f96bc Mon Sep 17 00:00:00 2001 From: zhaoyang Date: Thu, 19 Feb 2015 16:00:21 -0500 Subject: [PATCH 12/16] fix bugs in timing, duplicate graph --- examples/SFMExample_bal_COLAMD_METIS.cpp | 52 +++++++++++++----------- 1 file changed, 29 insertions(+), 23 deletions(-) diff --git a/examples/SFMExample_bal_COLAMD_METIS.cpp b/examples/SFMExample_bal_COLAMD_METIS.cpp index e2dff2cb2..429853ed0 100644 --- a/examples/SFMExample_bal_COLAMD_METIS.cpp +++ b/examples/SFMExample_bal_COLAMD_METIS.cpp @@ -40,7 +40,7 @@ int main (int argc, char* argv[]) { // Find default file, but if an argument is given, try loading a file string filename = findExampleDataFile("dubrovnik-3-7-pre"); - if (argc>1) filename = string(argv[1]); + if (argc>1) filename = findExampleDataFile(string(argv[1])); // Load the SfM data from file SfM_data mydata; @@ -48,7 +48,7 @@ int main (int argc, char* argv[]) { cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.number_tracks() % mydata.number_cameras(); // Create a factor graph - NonlinearFactorGraph graph_for_COLAMD; + NonlinearFactorGraph graph; // We share *one* noiseModel between all projection factors noiseModel::Isotropic::shared_ptr noise = @@ -60,15 +60,15 @@ int main (int argc, char* argv[]) { BOOST_FOREACH(const SfM_Measurement& m, track.measurements) { size_t i = m.first; Point2 uv = m.second; - graph_for_COLAMD.push_back(MyFactor(uv, noise, C(i), P(j))); // note use of shorthand symbols C and P + graph.push_back(MyFactor(uv, noise, C(i), P(j))); // note use of shorthand symbols C and P } j += 1; } // Add a prior on pose x1. This indirectly specifies where the origin is. // and a prior on the position of the first landmark to fix the scale - graph_for_COLAMD.push_back(PriorFactor(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1))); - graph_for_COLAMD.push_back(PriorFactor (P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1))); + graph.push_back(PriorFactor(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1))); + graph.push_back(PriorFactor (P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1))); // Create initial estimate Values initial; @@ -76,9 +76,6 @@ int main (int argc, char* argv[]) { BOOST_FOREACH(const SfM_Camera& camera, mydata.cameras) initial.insert(C(i++), camera); BOOST_FOREACH(const SfM_Track& track, mydata.tracks) initial.insert(P(j++), track.p); - NonlinearFactorGraph graph_for_METIS = graph_for_COLAMD.clone(); - - /** --------------- COMPARISON -----------------------**/ /** ----------------------------------------------------**/ @@ -86,31 +83,28 @@ int main (int argc, char* argv[]) { cout << "Optimize with COLAMD..." << endl; Values result_COLAMD; + double t_COLAMD_ordering, t_COLAMD_solving; try { double tic_t = clock(); LevenbergMarquardtParams params_using_COLAMD; params_using_COLAMD.setVerbosity("ERROR"); - params_using_COLAMD.ordering = Ordering::Create(Ordering::COLAMD, graph_for_COLAMD); + params_using_COLAMD.ordering = Ordering::Create(Ordering::COLAMD, graph); - double toc_t = (clock() - tic_t)/CLOCKS_PER_SEC; + t_COLAMD_ordering = (clock() - tic_t)/CLOCKS_PER_SEC; tic_t = clock(); - LevenbergMarquardtOptimizer lm(graph_for_COLAMD, initial, params_using_COLAMD); + LevenbergMarquardtOptimizer lm(graph, initial, params_using_COLAMD); result_COLAMD = lm.optimize(); - tic_t = clock(); - - cout << "Ordering: " << toc_t << "seconds" << endl; - cout << "Solving: " << (clock() - tic_t)/CLOCKS_PER_SEC << "seconds" << endl; + t_COLAMD_solving = (clock() - tic_t)/CLOCKS_PER_SEC; } catch (exception& e) { cout << e.what(); } cout << endl << endl; - // To see the error, check SFMExample_bal.cpp file //cout << "final error: " << graph.error(result_COLAMD) << endl; @@ -120,30 +114,42 @@ int main (int argc, char* argv[]) { cout << "Optimize with METIS" << endl; Values results_METIS; + double t_METIS_ordering, t_METIS_solving; try { double tic_t = clock(); LevenbergMarquardtParams params_using_METIS; params_using_METIS.setVerbosity("ERROR"); - params_using_METIS.ordering = Ordering::Create(Ordering::METIS, graph_for_METIS); + params_using_METIS.ordering = Ordering::Create(Ordering::METIS, graph); - double toc_t = (clock() - tic_t)/CLOCKS_PER_SEC; + t_METIS_ordering = (clock() - tic_t)/CLOCKS_PER_SEC; tic_t = clock(); - LevenbergMarquardtOptimizer lm(graph_for_METIS, initial, params_using_METIS); + LevenbergMarquardtOptimizer lm(graph, initial, params_using_METIS); results_METIS = lm.optimize(); - tic_t = clock(); - - cout << "Ordering: " << toc_t << "seconds" << endl; - cout << "Solving: " << (clock() - tic_t)/CLOCKS_PER_SEC << "seconds" << endl; + t_METIS_solving = (clock() - tic_t)/CLOCKS_PER_SEC; } catch (exception& e) { cout << e.what(); } + { + // printing the result + cout << "Time comparison by solving " << filename << " results:" << endl; + cout << boost::format("read %1% tracks on %2% cameras\n") \ + % mydata.number_tracks() % mydata.number_cameras() \ + << endl; + cout << "COLAMD: " << endl; + cout << "Ordering: " << t_COLAMD_ordering << "seconds" << endl; + cout << "Solving: " << t_COLAMD_solving << "seconds" << endl; + + cout << "METIS: " << endl; + cout << "Ordering: " << t_METIS_ordering << "seconds" << endl; + cout << "Solving: " << t_METIS_solving << "seconds" << endl; + } return 0; } From 92f2e8e168b1f6bb2a735f37bd8aef0cd7f2ff40 Mon Sep 17 00:00:00 2001 From: zhaoyang Date: Fri, 20 Feb 2015 23:45:13 -0500 Subject: [PATCH 13/16] add in a natural ordering for testing. Test this code on other machines. --- examples/SFMExample_bal_COLAMD_METIS.cpp | 121 ++++++++++++++--------- gtsam/inference/Ordering.h | 6 +- gtsam/linear/IterativeSolver.cpp | 2 +- gtsam/linear/SubgraphPreconditioner.cpp | 2 +- 4 files changed, 80 insertions(+), 51 deletions(-) diff --git a/examples/SFMExample_bal_COLAMD_METIS.cpp b/examples/SFMExample_bal_COLAMD_METIS.cpp index 429853ed0..7d9457b9e 100644 --- a/examples/SFMExample_bal_COLAMD_METIS.cpp +++ b/examples/SFMExample_bal_COLAMD_METIS.cpp @@ -18,6 +18,7 @@ // For an explanation of headers, see SFMExample.cpp #include +#include #include #include #include @@ -40,7 +41,7 @@ int main (int argc, char* argv[]) { // Find default file, but if an argument is given, try loading a file string filename = findExampleDataFile("dubrovnik-3-7-pre"); - if (argc>1) filename = findExampleDataFile(string(argv[1])); + if (argc>1) filename = string(argv[1]); // Load the SfM data from file SfM_data mydata; @@ -79,76 +80,102 @@ int main (int argc, char* argv[]) { /** --------------- COMPARISON -----------------------**/ /** ----------------------------------------------------**/ + /** ---------------------------------------------------**/ + + double t_COLAMD_ordering, t_METIS_ordering; //, t_NATURAL_ordering; + LevenbergMarquardtParams params_using_COLAMD, params_using_METIS, params_using_NATURAL; + try { + double tic_t = clock(); + params_using_METIS.setVerbosity("ERROR"); + params_using_METIS.ordering = Ordering::Create(Ordering::METIS, graph); + t_METIS_ordering = (clock() - tic_t)/CLOCKS_PER_SEC; + + tic_t = clock(); + params_using_COLAMD.setVerbosity("ERROR"); + params_using_COLAMD.ordering = Ordering::Create(Ordering::COLAMD, graph); + t_COLAMD_ordering = (clock() - tic_t)/CLOCKS_PER_SEC; + +// tic_t = clock(); +// params_using_NATURAL.setVerbosity("ERROR"); +// params_using_NATURAL.ordering = Ordering::Create(Ordering::NATURAL, graph); +// t_NATURAL_ordering = (clock() - tic_t)/CLOCKS_PER_SEC; + + } catch (exception& e) { + cout << e.what(); + } + + // expect they have different ordering results + if(params_using_COLAMD.ordering == params_using_METIS.ordering) { + cout << "COLAMD and METIS produce the same ordering. " + << "Problem here!!!" << endl; + } + + /* with METIS, optimize the graph and print the results */ + cout << "Optimize with METIS" << endl; + + Values result_METIS; + double t_METIS_solving; + try { + double tic_t = clock(); + LevenbergMarquardtOptimizer lm_METIS(graph, initial, params_using_COLAMD); + result_METIS = lm_METIS.optimize(); + t_METIS_solving = (clock() - tic_t)/CLOCKS_PER_SEC; + } catch (exception& e) { + cout << e.what(); + } + /* With COLAMD, optimize the graph and print the results */ cout << "Optimize with COLAMD..." << endl; Values result_COLAMD; - double t_COLAMD_ordering, t_COLAMD_solving; + double t_COLAMD_solving; try { double tic_t = clock(); - - LevenbergMarquardtParams params_using_COLAMD; - params_using_COLAMD.setVerbosity("ERROR"); - params_using_COLAMD.ordering = Ordering::Create(Ordering::COLAMD, graph); - - t_COLAMD_ordering = (clock() - tic_t)/CLOCKS_PER_SEC; - - tic_t = clock(); - - LevenbergMarquardtOptimizer lm(graph, initial, params_using_COLAMD); - result_COLAMD = lm.optimize(); - + LevenbergMarquardtOptimizer lm_COLAMD(graph, initial, params_using_COLAMD); + result_COLAMD = lm_COLAMD.optimize(); t_COLAMD_solving = (clock() - tic_t)/CLOCKS_PER_SEC; - } catch (exception& e) { cout << e.what(); } + // disable optimizer with NATURAL since it doesn't converge on large problem + /* Use Natural ordering and solve both respectively */ + // cout << "Solving with natural ordering: " << endl; + + // Values result_NATURAL; + // double t_NATURAL_solving; + // try { + // double tic_t = clock(); + // LevenbergMarquardtOptimizer lm_NATURAL(graph, initial, params_using_NATURAL); + // result_NATURAL = lm_NATURAL.optimize(); + // t_NATURAL_solving = (clock() - tic_t)/CLOCKS_PER_SEC; + // } catch (exception& e) { + // cout << e.what(); + // } + cout << endl << endl; - // To see the error, check SFMExample_bal.cpp file - //cout << "final error: " << graph.error(result_COLAMD) << endl; - - /** ---------------------------------------------------**/ - /* with METIS, optimize the graph and print the results */ - - cout << "Optimize with METIS" << endl; - - Values results_METIS; - double t_METIS_ordering, t_METIS_solving; - try { - double tic_t = clock(); - - LevenbergMarquardtParams params_using_METIS; - params_using_METIS.setVerbosity("ERROR"); - params_using_METIS.ordering = Ordering::Create(Ordering::METIS, graph); - - t_METIS_ordering = (clock() - tic_t)/CLOCKS_PER_SEC; - - tic_t = clock(); - - LevenbergMarquardtOptimizer lm(graph, initial, params_using_METIS); - results_METIS = lm.optimize(); - - t_METIS_solving = (clock() - tic_t)/CLOCKS_PER_SEC; - - } catch (exception& e) { - cout << e.what(); - } { // printing the result cout << "Time comparison by solving " << filename << " results:" << endl; - cout << boost::format("read %1% tracks on %2% cameras\n") \ + cout << boost::format("%1% point tracks and %2% cameras\n") \ % mydata.number_tracks() % mydata.number_cameras() \ << endl; cout << "COLAMD: " << endl; cout << "Ordering: " << t_COLAMD_ordering << "seconds" << endl; - cout << "Solving: " << t_COLAMD_solving << "seconds" << endl; + cout << "Solving: " << t_COLAMD_solving << "seconds" << endl; + cout << "final error: " << graph.error(result_COLAMD) << endl; cout << "METIS: " << endl; cout << "Ordering: " << t_METIS_ordering << "seconds" << endl; - cout << "Solving: " << t_METIS_solving << "seconds" << endl; + cout << "Solving: " << t_METIS_solving << "seconds" << endl; + cout << "final error: " << graph.error(result_METIS) << endl; + +// cout << "Natural: " << endl; +// cout << "Ordering: " << t_NATURAL_ordering << "seconds" << endl; +// cout << "Solving: " << t_NATURAL_solving << "seconds" << endl; +// cout << "final error: " << graph.error(result_NATURAL) << endl; } return 0; diff --git a/gtsam/inference/Ordering.h b/gtsam/inference/Ordering.h index e4cc2c55d..109fa1784 100644 --- a/gtsam/inference/Ordering.h +++ b/gtsam/inference/Ordering.h @@ -38,7 +38,7 @@ public: /// Type of ordering to use enum OrderingType { - COLAMD, METIS, CUSTOM + COLAMD, METIS, CUSTOM, NATURAL }; typedef Ordering This; ///< Typedef to this class @@ -163,7 +163,7 @@ public: /// Return a natural Ordering. Typically used by iterative solvers template - static Ordering Natural(const FactorGraph &fg) { + static Ordering natural(const FactorGraph &fg) { FastSet src = fg.keys(); std::vector keys(src.begin(), src.end()); std::stable_sort(keys.begin(), keys.end()); @@ -196,6 +196,8 @@ public: return colamd(graph); case METIS: return metis(graph); + case NATURAL: + return natural(graph); case CUSTOM: throw std::runtime_error( "Ordering::Create error: called with CUSTOM ordering type."); diff --git a/gtsam/linear/IterativeSolver.cpp b/gtsam/linear/IterativeSolver.cpp index ab3ccde13..89486962b 100644 --- a/gtsam/linear/IterativeSolver.cpp +++ b/gtsam/linear/IterativeSolver.cpp @@ -86,7 +86,7 @@ KeyInfo::KeyInfo(const GaussianFactorGraph &fg, const Ordering &ordering) /****************************************************************************/ KeyInfo::KeyInfo(const GaussianFactorGraph &fg) - : ordering_(Ordering::Natural(fg)) { + : ordering_(Ordering::natural(fg)) { initialize(fg); } diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp index f5ee4ddfc..af4e9dd4f 100644 --- a/gtsam/linear/SubgraphPreconditioner.cpp +++ b/gtsam/linear/SubgraphPreconditioner.cpp @@ -366,7 +366,7 @@ std::vector SubgraphBuilder::sample(const std::vector &weights, Subgraph::shared_ptr SubgraphBuilder::operator() (const GaussianFactorGraph &gfg) const { const SubgraphBuilderParameters &p = parameters_; - const Ordering inverse_ordering = Ordering::Natural(gfg); + const Ordering inverse_ordering = Ordering::natural(gfg); const FastMap forward_ordering = inverse_ordering.invert(); const size_t n = inverse_ordering.size(), t = n * p.complexity_ ; From 1d8157289496b09d1d9c622c8c7fd721c83bac17 Mon Sep 17 00:00:00 2001 From: zhaoyang Date: Sat, 21 Feb 2015 13:16:03 -0500 Subject: [PATCH 14/16] Capitalize static methods in ordering.h This commit involves the API change. Related files in gtsam have been changed. All the tests examples run without issue. --- examples/SFMExample_bal_COLAMD_METIS.cpp | 2 - examples/SolverComparer.cpp | 2 +- .../inference/EliminateableFactorGraph-inst.h | 20 ++++----- gtsam/inference/ISAM-inst.h | 2 +- gtsam/inference/Ordering.cpp | 20 ++++----- gtsam/inference/Ordering.h | 42 +++++++++---------- gtsam/inference/tests/testOrdering.cpp | 14 +++---- gtsam/linear/IterativeSolver.cpp | 2 +- gtsam/linear/SubgraphPreconditioner.cpp | 2 +- gtsam/nonlinear/ISAM2.cpp | 8 ++-- gtsam/nonlinear/NonlinearFactorGraph.cpp | 4 +- .../nonlinear/BatchFixedLagSmoother.cpp | 2 +- .../nonlinear/ConcurrentBatchFilter.cpp | 4 +- .../nonlinear/ConcurrentBatchSmoother.cpp | 2 +- tests/testNonlinearFactorGraph.cpp | 4 +- 15 files changed, 64 insertions(+), 66 deletions(-) diff --git a/examples/SFMExample_bal_COLAMD_METIS.cpp b/examples/SFMExample_bal_COLAMD_METIS.cpp index 7d9457b9e..3ccdf9b72 100644 --- a/examples/SFMExample_bal_COLAMD_METIS.cpp +++ b/examples/SFMExample_bal_COLAMD_METIS.cpp @@ -80,8 +80,6 @@ int main (int argc, char* argv[]) { /** --------------- COMPARISON -----------------------**/ /** ----------------------------------------------------**/ - /** ---------------------------------------------------**/ - double t_COLAMD_ordering, t_METIS_ordering; //, t_NATURAL_ordering; LevenbergMarquardtParams params_using_COLAMD, params_using_METIS, params_using_NATURAL; try { diff --git a/examples/SolverComparer.cpp b/examples/SolverComparer.cpp index 923b0b9de..0f61a4220 100644 --- a/examples/SolverComparer.cpp +++ b/examples/SolverComparer.cpp @@ -589,7 +589,7 @@ void runStats() { cout << "Gathering statistics..." << endl; GaussianFactorGraph linear = *datasetMeasurements.linearize(initial); - GaussianJunctionTree jt(GaussianEliminationTree(linear, Ordering::colamd(linear))); + GaussianJunctionTree jt(GaussianEliminationTree(linear, Ordering::Colamd(linear))); treeTraversal::ForestStatistics statistics = treeTraversal::GatherStatistics(jt); ofstream file; diff --git a/gtsam/inference/EliminateableFactorGraph-inst.h b/gtsam/inference/EliminateableFactorGraph-inst.h index 5e261e200..b4fc3b3a6 100644 --- a/gtsam/inference/EliminateableFactorGraph-inst.h +++ b/gtsam/inference/EliminateableFactorGraph-inst.h @@ -55,9 +55,9 @@ namespace gtsam { // have a VariableIndex already here because we computed one if needed in the previous 'else' // block. if (orderingType == Ordering::METIS) - return eliminateSequential(Ordering::metis(asDerived()), function, variableIndex, orderingType); - else - return eliminateSequential(Ordering::colamd(*variableIndex), function, variableIndex, orderingType); + return eliminateSequential(Ordering::Metis(asDerived()), function, variableIndex, orderingType); + else + return eliminateSequential(Ordering::Colamd(*variableIndex), function, variableIndex, orderingType); } } @@ -93,9 +93,9 @@ namespace gtsam { // have a VariableIndex already here because we computed one if needed in the previous 'else' // block. if (orderingType == Ordering::METIS) - return eliminateMultifrontal(Ordering::metis(asDerived()), function, variableIndex, orderingType); + return eliminateMultifrontal(Ordering::Metis(asDerived()), function, variableIndex, orderingType); else - return eliminateMultifrontal(Ordering::colamd(*variableIndex), function, variableIndex, orderingType); + return eliminateMultifrontal(Ordering::Colamd(*variableIndex), function, variableIndex, orderingType); } } @@ -125,7 +125,7 @@ namespace gtsam { if(variableIndex) { gttic(eliminatePartialSequential); // Compute full ordering - Ordering fullOrdering = Ordering::colamdConstrainedFirst(*variableIndex, variables); + Ordering fullOrdering = Ordering::ColamdConstrainedFirst(*variableIndex, variables); // Split off the part of the ordering for the variables being eliminated Ordering ordering(fullOrdering.begin(), fullOrdering.begin() + variables.size()); @@ -163,7 +163,7 @@ namespace gtsam { if(variableIndex) { gttic(eliminatePartialMultifrontal); // Compute full ordering - Ordering fullOrdering = Ordering::colamdConstrainedFirst(*variableIndex, variables); + Ordering fullOrdering = Ordering::ColamdConstrainedFirst(*variableIndex, variables); // Split off the part of the ordering for the variables being eliminated Ordering ordering(fullOrdering.begin(), fullOrdering.begin() + variables.size()); @@ -216,7 +216,7 @@ namespace gtsam { boost::get(&variables) : boost::get&>(&variables); Ordering totalOrdering = - Ordering::colamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered); + Ordering::ColamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered); // Split up ordering const size_t nVars = variablesOrOrdering->size(); @@ -275,7 +275,7 @@ namespace gtsam { boost::get(&variables) : boost::get&>(&variables); Ordering totalOrdering = - Ordering::colamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered); + Ordering::ColamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered); // Split up ordering const size_t nVars = variablesOrOrdering->size(); @@ -301,7 +301,7 @@ namespace gtsam { if(variableIndex) { // Compute a total ordering for all variables - Ordering totalOrdering = Ordering::colamdConstrainedLast(*variableIndex, variables); + Ordering totalOrdering = Ordering::ColamdConstrainedLast(*variableIndex, variables); // Split out the part for the marginalized variables Ordering marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - variables.size()); diff --git a/gtsam/inference/ISAM-inst.h b/gtsam/inference/ISAM-inst.h index 7cb3d9817..aebd3325a 100644 --- a/gtsam/inference/ISAM-inst.h +++ b/gtsam/inference/ISAM-inst.h @@ -46,7 +46,7 @@ namespace gtsam { const VariableIndex varIndex(factors); const FastSet newFactorKeys = newFactors.keys(); const Ordering constrainedOrdering = - Ordering::colamdConstrainedLast(varIndex, std::vector(newFactorKeys.begin(), newFactorKeys.end())); + Ordering::ColamdConstrainedLast(varIndex, std::vector(newFactorKeys.begin(), newFactorKeys.end())); Base bayesTree = *factors.eliminateMultifrontal(constrainedOrdering, function, varIndex); this->roots_.insert(this->roots_.end(), bayesTree.roots().begin(), bayesTree.roots().end()); this->nodes_.insert(bayesTree.nodes().begin(), bayesTree.nodes().end()); diff --git a/gtsam/inference/Ordering.cpp b/gtsam/inference/Ordering.cpp index 0882b87d1..9f4a81d05 100644 --- a/gtsam/inference/Ordering.cpp +++ b/gtsam/inference/Ordering.cpp @@ -38,14 +38,14 @@ FastMap Ordering::invert() const { } /* ************************************************************************* */ -Ordering Ordering::colamd(const VariableIndex& variableIndex) { +Ordering Ordering::Colamd(const VariableIndex& variableIndex) { // Call constrained version with all groups set to zero vector dummy_groups(variableIndex.size(), 0); - return Ordering::colamdConstrained(variableIndex, dummy_groups); + return Ordering::ColamdConstrained(variableIndex, dummy_groups); } /* ************************************************************************* */ -Ordering Ordering::colamdConstrained(const VariableIndex& variableIndex, +Ordering Ordering::ColamdConstrained(const VariableIndex& variableIndex, std::vector& cmember) { gttic(Ordering_COLAMDConstrained); @@ -115,7 +115,7 @@ Ordering Ordering::colamdConstrained(const VariableIndex& variableIndex, } /* ************************************************************************* */ -Ordering Ordering::colamdConstrainedLast(const VariableIndex& variableIndex, +Ordering Ordering::ColamdConstrainedLast(const VariableIndex& variableIndex, const std::vector& constrainLast, bool forceOrder) { gttic(Ordering_COLAMDConstrainedLast); @@ -137,11 +137,11 @@ Ordering Ordering::colamdConstrainedLast(const VariableIndex& variableIndex, ++group; } - return Ordering::colamdConstrained(variableIndex, cmember); + return Ordering::ColamdConstrained(variableIndex, cmember); } /* ************************************************************************* */ -Ordering Ordering::colamdConstrainedFirst(const VariableIndex& variableIndex, +Ordering Ordering::ColamdConstrainedFirst(const VariableIndex& variableIndex, const std::vector& constrainFirst, bool forceOrder) { gttic(Ordering_COLAMDConstrainedFirst); @@ -170,11 +170,11 @@ Ordering Ordering::colamdConstrainedFirst(const VariableIndex& variableIndex, if (c == none) c = group; - return Ordering::colamdConstrained(variableIndex, cmember); + return Ordering::ColamdConstrained(variableIndex, cmember); } /* ************************************************************************* */ -Ordering Ordering::colamdConstrained(const VariableIndex& variableIndex, +Ordering Ordering::ColamdConstrained(const VariableIndex& variableIndex, const FastMap& groups) { gttic(Ordering_COLAMDConstrained); size_t n = variableIndex.size(); @@ -193,11 +193,11 @@ Ordering Ordering::colamdConstrained(const VariableIndex& variableIndex, cmember[keyIndices.at(p.first)] = p.second; } - return Ordering::colamdConstrained(variableIndex, cmember); + return Ordering::ColamdConstrained(variableIndex, cmember); } /* ************************************************************************* */ -Ordering Ordering::metis(const MetisIndex& met) { +Ordering Ordering::Metis(const MetisIndex& met) { gttic(Ordering_METIS); vector xadj = met.xadj(); diff --git a/gtsam/inference/Ordering.h b/gtsam/inference/Ordering.h index 109fa1784..0ec5774e5 100644 --- a/gtsam/inference/Ordering.h +++ b/gtsam/inference/Ordering.h @@ -38,7 +38,7 @@ public: /// Type of ordering to use enum OrderingType { - COLAMD, METIS, CUSTOM, NATURAL + COLAMD, METIS, NATURAL, CUSTOM }; typedef Ordering This; ///< Typedef to this class @@ -78,12 +78,12 @@ public: /// performance). This internally builds a VariableIndex so if you already have a VariableIndex, /// it is faster to use COLAMD(const VariableIndex&) template - static Ordering colamd(const FactorGraph& graph) { - return colamd(VariableIndex(graph)); + static Ordering Colamd(const FactorGraph& graph) { + return Colamd(VariableIndex(graph)); } /// Compute a fill-reducing ordering using COLAMD from a VariableIndex. - static GTSAM_EXPORT Ordering colamd(const VariableIndex& variableIndex); + static GTSAM_EXPORT Ordering Colamd(const VariableIndex& variableIndex); /// Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details /// for note on performance). This internally builds a VariableIndex so if you already have a @@ -94,9 +94,9 @@ public: /// constrainLast. If \c forceOrder is false, the variables in \c constrainLast will be /// ordered after all the others, but will be rearranged by CCOLAMD to reduce fill-in as well. template - static Ordering colamdConstrainedLast(const FactorGraph& graph, + static Ordering ColamdConstrainedLast(const FactorGraph& graph, const std::vector& constrainLast, bool forceOrder = false) { - return colamdConstrainedLast(VariableIndex(graph), constrainLast, + return ColamdConstrainedLast(VariableIndex(graph), constrainLast, forceOrder); } @@ -106,7 +106,7 @@ public: /// variables in \c constrainLast will be ordered in the same order specified in the vector /// \c constrainLast. If \c forceOrder is false, the variables in \c constrainLast will be /// ordered after all the others, but will be rearranged by CCOLAMD to reduce fill-in as well. - static GTSAM_EXPORT Ordering colamdConstrainedLast( + static GTSAM_EXPORT Ordering ColamdConstrainedLast( const VariableIndex& variableIndex, const std::vector& constrainLast, bool forceOrder = false); @@ -119,9 +119,9 @@ public: /// constrainLast. If \c forceOrder is false, the variables in \c constrainFirst will be /// ordered after all the others, but will be rearranged by CCOLAMD to reduce fill-in as well. template - static Ordering colamdConstrainedFirst(const FactorGraph& graph, + static Ordering ColamdConstrainedFirst(const FactorGraph& graph, const std::vector& constrainFirst, bool forceOrder = false) { - return colamdConstrainedFirst(VariableIndex(graph), constrainFirst, + return ColamdConstrainedFirst(VariableIndex(graph), constrainFirst, forceOrder); } @@ -132,7 +132,7 @@ public: /// vector \c constrainFirst. If \c forceOrder is false, the variables in \c /// constrainFirst will be ordered after all the others, but will be rearranged by CCOLAMD to /// reduce fill-in as well. - static GTSAM_EXPORT Ordering colamdConstrainedFirst( + static GTSAM_EXPORT Ordering ColamdConstrainedFirst( const VariableIndex& variableIndex, const std::vector& constrainFirst, bool forceOrder = false); @@ -146,9 +146,9 @@ public: /// function simply fills the \c cmember argument to CCOLAMD with the supplied indices, see the /// CCOLAMD documentation for more information. template - static Ordering colamdConstrained(const FactorGraph& graph, + static Ordering ColamdConstrained(const FactorGraph& graph, const FastMap& groups) { - return colamdConstrained(VariableIndex(graph), groups); + return ColamdConstrained(VariableIndex(graph), groups); } /// Compute a fill-reducing ordering using constrained COLAMD from a VariableIndex. In this @@ -158,12 +158,12 @@ public: /// appear in \c groups in arbitrary order. Any variables not present in \c groups will be /// assigned to group 0. This function simply fills the \c cmember argument to CCOLAMD with the /// supplied indices, see the CCOLAMD documentation for more information. - static GTSAM_EXPORT Ordering colamdConstrained( + static GTSAM_EXPORT Ordering ColamdConstrained( const VariableIndex& variableIndex, const FastMap& groups); /// Return a natural Ordering. Typically used by iterative solvers template - static Ordering natural(const FactorGraph &fg) { + static Ordering Natural(const FactorGraph &fg) { FastSet src = fg.keys(); std::vector keys(src.begin(), src.end()); std::stable_sort(keys.begin(), keys.end()); @@ -176,11 +176,11 @@ public: std::vector& adj, const FactorGraph& graph); /// Compute an ordering determined by METIS from a VariableIndex - static GTSAM_EXPORT Ordering metis(const MetisIndex& met); + static GTSAM_EXPORT Ordering Metis(const MetisIndex& met); template - static Ordering metis(const FactorGraph& graph) { - return metis(MetisIndex(graph)); + static Ordering Metis(const FactorGraph& graph) { + return Metis(MetisIndex(graph)); } /// @} @@ -193,11 +193,11 @@ public: switch (orderingType) { case COLAMD: - return colamd(graph); + return Colamd(graph); case METIS: - return metis(graph); + return Metis(graph); case NATURAL: - return natural(graph); + return Natural(graph); case CUSTOM: throw std::runtime_error( "Ordering::Create error: called with CUSTOM ordering type."); @@ -222,7 +222,7 @@ public: private: /// Internal COLAMD function - static GTSAM_EXPORT Ordering colamdConstrained( + static GTSAM_EXPORT Ordering ColamdConstrained( const VariableIndex& variableIndex, std::vector& cmember); /** Serialization function */ diff --git a/gtsam/inference/tests/testOrdering.cpp b/gtsam/inference/tests/testOrdering.cpp index 8f2c417d3..d789da9b0 100644 --- a/gtsam/inference/tests/testOrdering.cpp +++ b/gtsam/inference/tests/testOrdering.cpp @@ -46,17 +46,17 @@ TEST(Ordering, constrained_ordering) { SymbolicFactorGraph sfg = example::symbolicChain(); // unconstrained version - Ordering actUnconstrained = Ordering::colamd(sfg); + Ordering actUnconstrained = Ordering::Colamd(sfg); Ordering expUnconstrained = Ordering(list_of(0)(1)(2)(3)(4)(5)); EXPECT(assert_equal(expUnconstrained, actUnconstrained)); // constrained version - push one set to the end - Ordering actConstrained = Ordering::colamdConstrainedLast(sfg, list_of(2)(4)); + Ordering actConstrained = Ordering::ColamdConstrainedLast(sfg, list_of(2)(4)); Ordering expConstrained = Ordering(list_of(0)(1)(5)(3)(4)(2)); EXPECT(assert_equal(expConstrained, actConstrained)); // constrained version - push one set to the start - Ordering actConstrained2 = Ordering::colamdConstrainedFirst(sfg, + Ordering actConstrained2 = Ordering::ColamdConstrainedFirst(sfg, list_of(2)(4)); Ordering expConstrained2 = Ordering(list_of(2)(4)(0)(1)(3)(5)); EXPECT(assert_equal(expConstrained2, actConstrained2)); @@ -76,7 +76,7 @@ TEST(Ordering, grouped_constrained_ordering) { constraints[4] = 1; constraints[5] = 2; - Ordering actConstrained = Ordering::colamdConstrained(sfg, constraints); + Ordering actConstrained = Ordering::ColamdConstrained(sfg, constraints); Ordering expConstrained = list_of(0)(1)(3)(2)(4)(5); EXPECT(assert_equal(expConstrained, actConstrained)); } @@ -195,7 +195,7 @@ TEST(Ordering, csr_format_4) { EXPECT(adjExpected.size() == mi.adj().size()); EXPECT(adjExpected == adjAcutal); - Ordering metOrder = Ordering::metis(sfg); + Ordering metOrder = Ordering::Metis(sfg); // Test different symbol types sfg.push_factor(Symbol('l', 1)); @@ -204,7 +204,7 @@ TEST(Ordering, csr_format_4) { sfg.push_factor(Symbol('x', 3), Symbol('l', 1)); sfg.push_factor(Symbol('x', 4), Symbol('l', 1)); - Ordering metOrder2 = Ordering::metis(sfg); + Ordering metOrder2 = Ordering::Metis(sfg); } /* ************************************************************************* */ @@ -226,7 +226,7 @@ TEST(Ordering, metis) { EXPECT(adjExpected.size() == mi.adj().size()); EXPECT(adjExpected == mi.adj()); - Ordering metis = Ordering::metis(sfg); + Ordering metis = Ordering::Metis(sfg); } /* ************************************************************************* */ diff --git a/gtsam/linear/IterativeSolver.cpp b/gtsam/linear/IterativeSolver.cpp index 89486962b..ab3ccde13 100644 --- a/gtsam/linear/IterativeSolver.cpp +++ b/gtsam/linear/IterativeSolver.cpp @@ -86,7 +86,7 @@ KeyInfo::KeyInfo(const GaussianFactorGraph &fg, const Ordering &ordering) /****************************************************************************/ KeyInfo::KeyInfo(const GaussianFactorGraph &fg) - : ordering_(Ordering::natural(fg)) { + : ordering_(Ordering::Natural(fg)) { initialize(fg); } diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp index af4e9dd4f..f5ee4ddfc 100644 --- a/gtsam/linear/SubgraphPreconditioner.cpp +++ b/gtsam/linear/SubgraphPreconditioner.cpp @@ -366,7 +366,7 @@ std::vector SubgraphBuilder::sample(const std::vector &weights, Subgraph::shared_ptr SubgraphBuilder::operator() (const GaussianFactorGraph &gfg) const { const SubgraphBuilderParameters &p = parameters_; - const Ordering inverse_ordering = Ordering::natural(gfg); + const Ordering inverse_ordering = Ordering::Natural(gfg); const FastMap forward_ordering = inverse_ordering.invert(); const size_t n = inverse_ordering.size(), t = n * p.complexity_ ; diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 3b3d76285..97735b5d5 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -341,7 +341,7 @@ boost::shared_ptr > ISAM2::recalculate(const FastSet& markedKe Ordering order; if(constrainKeys) { - order = Ordering::colamdConstrained(variableIndex_, *constrainKeys); + order = Ordering::ColamdConstrained(variableIndex_, *constrainKeys); } else { @@ -351,11 +351,11 @@ boost::shared_ptr > ISAM2::recalculate(const FastSet& markedKe FastMap constraintGroups; BOOST_FOREACH(Key var, observedKeys) constraintGroups[var] = 1; - order = Ordering::colamdConstrained(variableIndex_, constraintGroups); + order = Ordering::ColamdConstrained(variableIndex_, constraintGroups); } else { - order = Ordering::colamd(variableIndex_); + order = Ordering::Colamd(variableIndex_); } } gttoc(ordering); @@ -481,7 +481,7 @@ boost::shared_ptr > ISAM2::recalculate(const FastSet& markedKe // Generate ordering gttic(Ordering); - Ordering ordering = Ordering::colamdConstrained(affectedFactorsVarIndex, constraintGroups); + Ordering ordering = Ordering::ColamdConstrained(affectedFactorsVarIndex, constraintGroups); gttoc(Ordering); ISAM2BayesTree::shared_ptr bayesTree = ISAM2JunctionTree(GaussianEliminationTree( diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp index 238d3bc56..99a58a989 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.cpp +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -282,13 +282,13 @@ FastSet NonlinearFactorGraph::keys() const { /* ************************************************************************* */ Ordering NonlinearFactorGraph::orderingCOLAMD() const { - return Ordering::colamd(*this); + return Ordering::Colamd(*this); } /* ************************************************************************* */ Ordering NonlinearFactorGraph::orderingCOLAMDConstrained(const FastMap& constraints) const { - return Ordering::colamdConstrained(*this, constraints); + return Ordering::ColamdConstrained(*this, constraints); } /* ************************************************************************* */ diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp index 05b0bb49e..952b134b5 100644 --- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp @@ -191,7 +191,7 @@ void BatchFixedLagSmoother::reorder(const std::set& marginalizeKeys) { } // COLAMD groups will be used to place marginalize keys in Group 0, and everything else in Group 1 - ordering_ = Ordering::colamdConstrainedFirst(factors_, std::vector(marginalizeKeys.begin(), marginalizeKeys.end())); + ordering_ = Ordering::ColamdConstrainedFirst(factors_, std::vector(marginalizeKeys.begin(), marginalizeKeys.end())); if(debug) { ordering_.print("New Ordering: "); diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp index fcaae9626..40d1746ac 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp @@ -362,9 +362,9 @@ void ConcurrentBatchFilter::reorder(const boost::optional >& keysT // COLAMD groups will be used to place marginalize keys in Group 0, and everything else in Group 1 if(keysToMove && keysToMove->size() > 0) { - ordering_ = Ordering::colamdConstrainedFirst(factors_, std::vector(keysToMove->begin(), keysToMove->end())); + ordering_ = Ordering::ColamdConstrainedFirst(factors_, std::vector(keysToMove->begin(), keysToMove->end())); }else{ - ordering_ = Ordering::colamd(factors_); + ordering_ = Ordering::Colamd(factors_); } } diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp index 0f6515056..c410ad528 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp @@ -231,7 +231,7 @@ void ConcurrentBatchSmoother::reorder() { variableIndex_ = VariableIndex(factors_); FastList separatorKeys = separatorValues_.keys(); - ordering_ = Ordering::colamdConstrainedLast(variableIndex_, std::vector(separatorKeys.begin(), separatorKeys.end())); + ordering_ = Ordering::ColamdConstrainedLast(variableIndex_, std::vector(separatorKeys.begin(), separatorKeys.end())); } diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp index f398a33fe..8e6b033e9 100644 --- a/tests/testNonlinearFactorGraph.cpp +++ b/tests/testNonlinearFactorGraph.cpp @@ -79,14 +79,14 @@ TEST( NonlinearFactorGraph, GET_ORDERING) { Ordering expected; expected += L(1), X(2), X(1); // For starting with l1,x1,x2 NonlinearFactorGraph nlfg = createNonlinearFactorGraph(); - Ordering actual = Ordering::colamd(nlfg); + Ordering actual = Ordering::Colamd(nlfg); EXPECT(assert_equal(expected,actual)); // Constrained ordering - put x2 at the end Ordering expectedConstrained; expectedConstrained += L(1), X(1), X(2); FastMap constraints; constraints[X(2)] = 1; - Ordering actualConstrained = Ordering::colamdConstrained(nlfg, constraints); + Ordering actualConstrained = Ordering::ColamdConstrained(nlfg, constraints); EXPECT(assert_equal(expectedConstrained, actualConstrained)); } From 0b9758d88c363bfe649d1b56a4fbdf44928a14bf Mon Sep 17 00:00:00 2001 From: zhaoyang Date: Mon, 23 Feb 2015 10:24:34 -0500 Subject: [PATCH 15/16] change to GTSAM timing --- examples/SFMExample_bal_COLAMD_METIS.cpp | 83 ++++++++---------------- 1 file changed, 26 insertions(+), 57 deletions(-) diff --git a/examples/SFMExample_bal_COLAMD_METIS.cpp b/examples/SFMExample_bal_COLAMD_METIS.cpp index 3ccdf9b72..1e36c4b7e 100644 --- a/examples/SFMExample_bal_COLAMD_METIS.cpp +++ b/examples/SFMExample_bal_COLAMD_METIS.cpp @@ -24,6 +24,9 @@ #include #include #include // for loading BAL datasets ! + +#include + #include using namespace std; @@ -80,24 +83,17 @@ int main (int argc, char* argv[]) { /** --------------- COMPARISON -----------------------**/ /** ----------------------------------------------------**/ - double t_COLAMD_ordering, t_METIS_ordering; //, t_NATURAL_ordering; - LevenbergMarquardtParams params_using_COLAMD, params_using_METIS, params_using_NATURAL; + LevenbergMarquardtParams params_using_COLAMD, params_using_METIS; try { - double tic_t = clock(); params_using_METIS.setVerbosity("ERROR"); + gttic_(METIS_ORDERING); params_using_METIS.ordering = Ordering::Create(Ordering::METIS, graph); - t_METIS_ordering = (clock() - tic_t)/CLOCKS_PER_SEC; + gttoc_(METIS_ORDERING); - tic_t = clock(); params_using_COLAMD.setVerbosity("ERROR"); + gttic_(COLAMD_ORDERING); params_using_COLAMD.ordering = Ordering::Create(Ordering::COLAMD, graph); - t_COLAMD_ordering = (clock() - tic_t)/CLOCKS_PER_SEC; - -// tic_t = clock(); -// params_using_NATURAL.setVerbosity("ERROR"); -// params_using_NATURAL.ordering = Ordering::Create(Ordering::NATURAL, graph); -// t_NATURAL_ordering = (clock() - tic_t)/CLOCKS_PER_SEC; - + gttoc_(COLAMD_ORDERING); } catch (exception& e) { cout << e.what(); } @@ -108,49 +104,23 @@ int main (int argc, char* argv[]) { << "Problem here!!!" << endl; } - /* with METIS, optimize the graph and print the results */ - cout << "Optimize with METIS" << endl; + /* Optimize the graph with METIS and COLAMD and time the results */ - Values result_METIS; - double t_METIS_solving; + Values result_METIS, result_COLAMD; try { - double tic_t = clock(); - LevenbergMarquardtOptimizer lm_METIS(graph, initial, params_using_COLAMD); + gttic_(OPTIMIZE_WITH_METIS); + LevenbergMarquardtOptimizer lm_METIS(graph, initial, params_using_METIS); result_METIS = lm_METIS.optimize(); - t_METIS_solving = (clock() - tic_t)/CLOCKS_PER_SEC; - } catch (exception& e) { - cout << e.what(); - } + gttoc_(OPTIMIZE_WITH_METIS); - /* With COLAMD, optimize the graph and print the results */ - cout << "Optimize with COLAMD..." << endl; - - Values result_COLAMD; - double t_COLAMD_solving; - try { - double tic_t = clock(); + gttic_(OPTIMIZE_WITH_COLAMD); LevenbergMarquardtOptimizer lm_COLAMD(graph, initial, params_using_COLAMD); result_COLAMD = lm_COLAMD.optimize(); - t_COLAMD_solving = (clock() - tic_t)/CLOCKS_PER_SEC; + gttoc_(OPTIMIZE_WITH_COLAMD); } catch (exception& e) { cout << e.what(); } - // disable optimizer with NATURAL since it doesn't converge on large problem - /* Use Natural ordering and solve both respectively */ - // cout << "Solving with natural ordering: " << endl; - - // Values result_NATURAL; - // double t_NATURAL_solving; - // try { - // double tic_t = clock(); - // LevenbergMarquardtOptimizer lm_NATURAL(graph, initial, params_using_NATURAL); - // result_NATURAL = lm_NATURAL.optimize(); - // t_NATURAL_solving = (clock() - tic_t)/CLOCKS_PER_SEC; - // } catch (exception& e) { - // cout << e.what(); - // } - cout << endl << endl; { @@ -160,22 +130,21 @@ int main (int argc, char* argv[]) { % mydata.number_tracks() % mydata.number_cameras() \ << endl; - cout << "COLAMD: " << endl; - cout << "Ordering: " << t_COLAMD_ordering << "seconds" << endl; - cout << "Solving: " << t_COLAMD_solving << "seconds" << endl; - cout << "final error: " << graph.error(result_COLAMD) << endl; + cout << "COLAMD final error: " << graph.error(result_COLAMD) << endl; + cout << "METIS final error: " << graph.error(result_METIS) << endl; - cout << "METIS: " << endl; - cout << "Ordering: " << t_METIS_ordering << "seconds" << endl; - cout << "Solving: " << t_METIS_solving << "seconds" << endl; - cout << "final error: " << graph.error(result_METIS) << endl; + tictoc_print_(); -// cout << "Natural: " << endl; -// cout << "Ordering: " << t_NATURAL_ordering << "seconds" << endl; -// cout << "Solving: " << t_NATURAL_solving << "seconds" << endl; -// cout << "final error: " << graph.error(result_NATURAL) << endl; +// cout << "COLAMD: " << endl; +// cout << "Ordering: " << t_COLAMD_ordering << "seconds" << endl; +// cout << "Solving: " << t_COLAMD_solving << "seconds" << endl; + +// cout << "METIS: " << endl; +// cout << "Ordering: " << t_METIS_ordering << "seconds" << endl; +// cout << "Solving: " << t_METIS_solving << "seconds" << endl; } + return 0; } /* ************************************************************************* */ From 48d549f38384f0fcc4a44c7b35155c3fed60ea08 Mon Sep 17 00:00:00 2001 From: zhaoyang Date: Mon, 23 Feb 2015 10:31:25 -0500 Subject: [PATCH 16/16] remove unuseful comments --- examples/SFMExample_bal_COLAMD_METIS.cpp | 21 +++++++-------------- 1 file changed, 7 insertions(+), 14 deletions(-) diff --git a/examples/SFMExample_bal_COLAMD_METIS.cpp b/examples/SFMExample_bal_COLAMD_METIS.cpp index 1e36c4b7e..4bbaac3ef 100644 --- a/examples/SFMExample_bal_COLAMD_METIS.cpp +++ b/examples/SFMExample_bal_COLAMD_METIS.cpp @@ -121,27 +121,20 @@ int main (int argc, char* argv[]) { cout << e.what(); } - cout << endl << endl; - { - // printing the result + { // printing the result + + cout << "COLAMD final error: " << graph.error(result_COLAMD) << endl; + cout << "METIS final error: " << graph.error(result_METIS) << endl; + + cout << endl << endl; + cout << "Time comparison by solving " << filename << " results:" << endl; cout << boost::format("%1% point tracks and %2% cameras\n") \ % mydata.number_tracks() % mydata.number_cameras() \ << endl; - cout << "COLAMD final error: " << graph.error(result_COLAMD) << endl; - cout << "METIS final error: " << graph.error(result_METIS) << endl; - tictoc_print_(); - -// cout << "COLAMD: " << endl; -// cout << "Ordering: " << t_COLAMD_ordering << "seconds" << endl; -// cout << "Solving: " << t_COLAMD_solving << "seconds" << endl; - -// cout << "METIS: " << endl; -// cout << "Ordering: " << t_METIS_ordering << "seconds" << endl; -// cout << "Solving: " << t_METIS_solving << "seconds" << endl; }