diff --git a/.cproject b/.cproject index 3f6979819..b76463977 100644 --- a/.cproject +++ b/.cproject @@ -1,19 +1,17 @@ - - - + - - + + @@ -21,25 +19,33 @@ - + + + + @@ -54,13 +60,13 @@ - - + + @@ -71,20 +77,31 @@ + + - + @@ -99,13 +116,13 @@ - - + + @@ -116,12 +133,18 @@ + + @@ -131,10 +154,14 @@ + + @@ -160,6 +187,8 @@ + + @@ -245,6 +274,135 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -288,228 +446,77 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - + make -j5 - testPoseRTV.run + SmartProjectionFactorExample_kitti_nonbatch.run true true true - + make -j5 - testIMUSystem.run + SmartProjectionFactorExample_kitti.run true true true - - make - -j2 - testGaussianFactor.run - true - true - true - - + make -j5 - schedulingExample.run + SmartProjectionFactorTesting.run true true true - - make - -j5 - testCSP.run - true - true - true - - - make - -j5 - testScheduler.run - true - true - true - - - make - -j5 - schedulingQuals12.run - true - true - true - - - make - -j5 - testSudoku.run - true - true - true - - - make - -j5 - schedulingQuals13.run - true - true - true - - - make - -j5 - testStereoCamera.run - true - true - true - - - make - -j5 - testRot3M.run - true - true - true - - - make - -j5 - testPoint3.run - true - true - true - - - make - -j5 - testCalibratedCamera.run - true - true - true - - - make - -j5 - timeStereoCamera.run - true - true - true - - - make - -j1 VERBOSE=1 - testHomography2.run - true - false - true - - - make - -j5 - testPoint2.run - true - true - true - - - make - -j5 - testPose2.run - true - true - true - - - make - -j5 - testPose3.run - true - true - true - - - make - -j5 - testCal3_S2.run - true - true - true - - - make - -j5 - testSphere2.run - true - true - true - - - make - -j5 - testEssentialMatrix.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 - vSFMexample.run - true - true - true - - - make - -j2 - all - true - true - true - - + make -j2 check @@ -517,7 +524,15 @@ true true - + + make + -j2 + tests/testSPQRUtil.run + true + true + true + + make -j2 clean @@ -525,22 +540,6 @@ true true - - make - -j5 - all - true - false - true - - - make - -j5 - check - true - false - true - make -j2 @@ -661,6 +660,718 @@ false true + + make + -j5 + testPoseRTV.run + true + true + true + + + make + -j5 + testIMUSystem.run + true + true + true + + + make + -j5 + testBTree.run + true + true + true + + + make + -j5 + testDSF.run + true + true + true + + + make + -j5 + testDSFVector.run + true + true + true + + + make + -j5 + testFixedVector.run + true + true + true + + + make + -j5 + testDSFMap.run + true + true + true + + + make + -j2 + tests/testPose2.run + true + true + true + + + make + -j2 + tests/testPose3.run + true + true + true + + + make + -j5 + testValues.run + true + true + true + + + make + -j5 + testOrdering.run + true + true + true + + + make + -j5 + testKey.run + true + true + true + + + make + -j5 + testLinearContainerFactor.run + true + true + true + + + make + -j6 -j8 + testWhiteNoiseFactor.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j5 + schedulingExample.run + true + true + true + + + make + -j5 + testCSP.run + true + true + true + + + make + -j5 + testScheduler.run + true + true + true + + + make + -j5 + schedulingQuals12.run + true + true + true + + + make + -j5 + testSudoku.run + true + true + true + + + make + -j5 + schedulingQuals13.run + 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 + testInvDepthFactor3.run + true + true + true + + + make + -j5 + testPoseTranslationPrior.run + true + true + true + + + make + -j5 + testPoseRotationPrior.run + true + true + true + + + make + -j5 + testReferenceFrameFactor.run + true + true + true + + + make + -j5 + testAHRS.run + true + true + true + + + make + -j8 + testImuFactor.run + true + true + true + + + make + -j5 + testMultiProjectionFactor.run + true + true + true + + + make + -j5 + testSmartProjectionFactor.run + true + true + true + + + make + -j5 + testTSAMFactors.run + true + true + true + + + make + -j5 + clean + true + true + true + + + make + -j5 + all + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + testGaussianConditional.run + true + true + true + + + make + -j2 + testGaussianFactor.run + true + true + true + + + make + -j2 + timeGaussianFactor.run + true + true + true + + + make + -j2 + timeVectorConfig.run + true + true + true + + + make + -j2 + testVectorBTree.run + true + true + true + + + make + -j2 + testVectorMap.run + true + true + true + + + make + -j2 + testNoiseModel.run + true + true + true + + + make + -j2 + testBayesNetPreconditioner.run + true + true + true + + + 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 + -j5 + testParticleFactor.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 + testImuFactor.run + true + true + true + + + make + -j5 + testCombinedImuFactor.run + true + true + true + make -j2 @@ -802,469 +1513,23 @@ true true - - make - -j2 - testVSLAMGraph - 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 - tests/testGaussianISAM2 - true - false - true - - + make -j5 - check.tests + testEliminationTree.run true true true - - make - -j2 - timeGaussianFactorGraph.run - true - true - true - - + make -j5 - testMarginals.run + testInference.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 - -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 - testVectorValues.run - true - true - true - - - make - -j5 - testNoiseModel.run - true - true - true - - - make - -j5 - testHessianFactor.run - true - true - true - - - make - -j5 - testGaussianConditional.run - true - true - true - - - make - -j5 - testGaussianFactorGraph.run - true - true - true - - - make - -j5 - testGaussianJunctionTree.run - true - true - true - - - make - -j5 - testKalmanFilter.run - true - true - true - - - make - -j5 - testGaussianDensity.run - true - true - true - - - make - -j5 - testSerializationLinear.run - true - true - true - - - make - -j5 - testJacobianFactor.run - true - true - true - - - make - -j5 - testBTree.run - true - true - true - - - make - -j5 - testDSF.run - true - true - true - - - make - -j5 - testDSFVector.run - true - true - true - - - make - -j5 - testFixedVector.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j5 - testInvDepthCamera3.run - true - true - true - - - make - -j5 - testTriangulation.run - true - true - true - - - make - -j5 - testValues.run - true - true - true - - - make - -j5 - testOrdering.run - true - true - true - - + make -j5 testKey.run @@ -1272,628 +1537,20 @@ true true - + make - -j5 - testLinearContainerFactor.run + -j1 + testSymbolicBayesTree.run true - true + false true - + make - -j6 -j8 - testWhiteNoiseFactor.run + -j1 + testSymbolicSequentialSolver.run true - true - 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 - -j5 - testVector.run - true - true - true - - - make - -j5 - testMatrix.run - true - true - true - - - make - -j5 - clean - true - true - true - - - make - -j5 - all - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - clean all - true - true - true - - - make - -j2 - install - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j5 - testGeneralSFMFactor.run - true - true - true - - - make - -j5 - testProjectionFactor.run - true - true - true - - - make - -j5 - testGeneralSFMFactor_Cal3Bundler.run - true - true - true - - - make - -j6 -j8 - testAntiFactor.run - true - true - true - - - make - -j6 -j8 - testBetweenFactor.run - true - true - true - - - make - -j5 - testEssentialMatrixFactor.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 - testGaussianFactor.run - true - true - true - - - make - -j2 - testRot3.run - true - true - true - - - make - -j2 - testRot2.run - true - true - true - - - make - -j2 - testPose3.run - true - true - true - - - make - -j2 - timeRot3.run - true - true - true - - - make - -j2 - testPose2.run - true - true - true - - - make - -j2 - testCal3_S2.run - true - true - true - - - make - -j2 - testSimpleCamera.run - true - true - true - - - make - -j2 - testHomography2.run - true - true - true - - - make - -j2 - testCalibratedCamera.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - testPoint2.run - true - true - true - - - make - -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 - clean - true - true - true - - - make - -j5 - testInvDepthFactor3.run - true - true - true - - - make - -j5 - testPoseTranslationPrior.run - true - true - true - - - make - -j5 - testPoseRotationPrior.run - true - true - true - - - make - -j5 - testReferenceFrameFactor.run - true - true - true - - - make - -j5 - testAHRS.run - true - true - true - - - make - -j8 - testImuFactor.run - true - true - true - - - make - -j5 - testMultiProjectionFactor.run - true - true - true - - - make - -j5 - testSmartProjectionFactor.run - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - SimpleRotation.run - true - true - true - - - make - -j5 - CameraResectioning.run - true - true - true - - - make - -j5 - PlanarSLAMExample.run - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - easyPoint2KalmanFilter.run - true - true - true - - - make - -j2 - elaboratePoint2KalmanFilter.run - true - true - true - - - make - -j5 - Pose2SLAMExample.run - true - true - true - - - make - -j2 - Pose2SLAMwSPCG_easy.run - true - true - true - - - make - -j5 - UGM_small.run - true - true - true - - - make - -j5 - LocalizationExample.run - true - true - true - - - make - -j5 - OdometryExample.run - true - true - true - - - make - -j5 - RangeISAMExample_plaza2.run - true - true - true - - - make - -j5 - CreateSFMExampleData.run - true - true + false true @@ -1912,110 +1569,6 @@ true true - - make - -j5 - testInference.run - true - true - true - - - make - -j1 - testSymbolicSequentialSolver.run - true - false - true - - - make - -j5 - testEliminationTree.run - true - true - true - - - make - -j1 - testSymbolicBayesTree.run - true - false - true - - - make - -j5 - testSpirit.run - true - true - true - - - make - -j5 - testWrap.run - true - true - true - - - make - -j5 - check.wrap - true - true - true - - - make - -j5 - wrap - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - make -j3 @@ -2407,39 +1960,15 @@ true true - + make -j5 - testGPSFactor.run + check.tests true true true - - make - -j5 - testAttitudeFactor.run - true - true - true - - - make - -j5 - testMagFactor.run - true - true - true - - - make - -j4 - testImuFactor.run - true - true - true - - + make -j2 check @@ -2447,15 +1976,214 @@ true true - + make -j2 - tests/testSPQRUtil.run + clean true true true - + + make + -j2 + install + true + true + true + + + make + -j2 + all + true + true + true + + + cmake + .. + true + false + true + + + make + -j2 + testGaussianFactor.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 + testGeneralSFMFactor.run + true + true + true + + + make + -j5 + testProjectionFactor.run + true + true + true + + + make + -j5 + testGeneralSFMFactor_Cal3Bundler.run + true + true + true + + + make + -j6 -j8 + testAntiFactor.run + true + true + true + + + make + -j6 -j8 + testBetweenFactor.run + true + true + true + + + make + -j5 + testDataset.run + true + true + true + + + make + -j5 + testEssentialMatrixFactor.run + true + true + true + + + make + -j5 + testRotateFactor.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 @@ -2503,55 +2231,95 @@ true true - + make -j2 - tests/testPose2.run + vSFMexample.run true true true - + make -j2 - tests/testPose3.run + testVSLAMGraph true true true - + make - -j2 - all + -j5 + testInvDepthCamera3.run true true true - + make - -j2 - clean + -j5 + testTriangulation.run true true true - + make - -j2 - check + -j5 + check.tests true true true - + make -j2 - testGaussianConditional.run + 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 @@ -2559,62 +2327,661 @@ true true - + make -j2 - timeGaussianFactor.run + testNonlinearOptimizer.run true true true - + make -j2 - timeVectorConfig.run + testGaussianBayesNet.run true true true - + make -j2 - testVectorBTree.run + testNonlinearISAM.run true true true - + make -j2 - testVectorMap.run + testNonlinearEquality.run true true true - + make -j2 - testNoiseModel.run + testExtendedKalmanFilter.run true true true - + make - -j2 - testBayesNetPreconditioner.run + -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 - testErrors.run + 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 + -j2 + testGaussianFactor.run + true + true + true + + + make + -j2 + install + true + true + true + + + make + -j2 + clean + true + true + 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 + -j5 + testStereoCamera.run + true + true + true + + + make + -j5 + testRot3M.run + true + true + true + + + make + -j5 + testPoint3.run + true + true + true + + + make + -j5 + testCalibratedCamera.run + true + true + true + + + make + -j5 + timeStereoCamera.run + true + true + true + + + make + -j1 VERBOSE=1 + testHomography2.run + true + false + true + + + make + -j5 + testPoint2.run + true + true + true + + + make + -j5 + testPose2.run + true + true + true + + + make + -j5 + testPose3.run + true + true + true + + + make + -j5 + timeCalibratedCamera.run + true + true + true + + + make + -j5 + testPinholeCamera.run + true + true + true + + + make + -j5 + timePinholeCamera.run + true + true + true + + + make + -j5 + testCal3DS2.run + true + true + true + + + make + -j5 + testCal3Bundler.run + true + true + true + + + make + -j5 + testSphere2.run + true + true + true + + + make + -j5 + testEssentialMatrix.run + true + true + true + + + make + -j5 + testVector.run + true + true + true + + + make + -j5 + testMatrix.run + true + true + true + + + make + -j2 + SimpleRotation.run + true + true + true + + + make + -j5 + CameraResectioning.run + true + true + true + + + make + -j5 + PlanarSLAMExample.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + easyPoint2KalmanFilter.run + true + true + true + + + make + -j2 + elaboratePoint2KalmanFilter.run + true + true + true + + + make + -j5 + Pose2SLAMExample.run + true + true + true + + + make + -j2 + Pose2SLAMwSPCG_easy.run + true + true + true + + + make + -j5 + UGM_small.run + true + true + true + + + make + -j5 + LocalizationExample.run + true + true + true + + + make + -j5 + OdometryExample.run + true + true + true + + + make + -j5 + RangeISAMExample_plaza2.run + true + true + true + + + make + -j5 + SelfCalibrationExample.run + true + true + true + + + make + -j5 + SFMExample.run + true + true + true + + + make + -j5 + VisualISAMExample.run + true + true + true + + + make + -j5 + VisualISAM2Example.run + true + true + true + + + make + -j5 + Pose2SLAMExample_graphviz.run + true + true + true + + + make + -j5 + Pose2SLAMExample_graph.run + true + true + true + + + make + -j5 + SFMExample_bal.run + true + true + true + + + make + -j4 + testImuFactor.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + tests/testGaussianISAM2 + true + false + true + + + make + -j2 + testRot3.run + true + true + true + + + make + -j2 + testRot2.run + true + true + true + + + make + -j2 + testPose3.run + true + true + true + + + make + -j2 + timeRot3.run + true + true + true + + + make + -j2 + testPose2.run + true + true + true + + + make + -j2 + testCal3_S2.run + true + true + true + + + make + -j2 + testSimpleCamera.run + true + true + true + + + make + -j2 + testHomography2.run + true + true + true + + + make + -j2 + testCalibratedCamera.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + testPoint2.run + true + true + true + + + make + -j5 + testSpirit.run + true + true + true + + + make + -j5 + testWrap.run + true + true + true + + + make + -j5 + check.wrap + true + true + true + + + make + -j5 + wrap + true + true + true + diff --git a/.gitignore b/.gitignore index 6d6c4d9ad..6d274af09 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,4 @@ -/build/ -/*.DS_Store +/build* +/doc* +*.pyc +*.DS_Store \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index 494417829..d03f11ede 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,10 +3,11 @@ project(GTSAM CXX C) cmake_minimum_required(VERSION 2.6) # Set the version number for the library -set (GTSAM_VERSION_MAJOR 2) -set (GTSAM_VERSION_MINOR 4) +set (GTSAM_VERSION_MAJOR 3) +set (GTSAM_VERSION_MINOR 0) set (GTSAM_VERSION_PATCH 0) - +math (EXPR GTSAM_VERSION_NUMERIC "10000 * ${GTSAM_VERSION_MAJOR} + 100 * ${GTSAM_VERSION_MINOR} + ${GTSAM_VERSION_PATCH}") +set (GTSAM_VERSION_STRING "${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}") ############################################################################### # Gather information, perform checks, set defaults @@ -45,30 +46,26 @@ endif() # Set up options # Configurable Options -option(GTSAM_BUILD_TESTS "Enable/Disable building of tests" ON) option(GTSAM_BUILD_TIMING "Enable/Disable building of timing scripts" OFF) # These do not currently work -option(GTSAM_BUILD_EXAMPLES "Enable/Disable building of examples" ON) if(GTSAM_UNSTABLE_AVAILABLE) option(GTSAM_BUILD_UNSTABLE "Enable/Disable libgtsam_unstable" ON) endif() -option(GTSAM_BUILD_SHARED_LIBRARY "Enable/Disable building of a shared version of gtsam" ON) -option(GTSAM_BUILD_STATIC_LIBRARY "Enable/Disable building of a static version of gtsam" OFF) +option(GTSAM_BUILD_STATIC_LIBRARY "Build a static gtsam library, instead of shared" OFF) option(GTSAM_USE_QUATERNIONS "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices. If enable, Rot3::EXPMAP is enforced by default." OFF) -if(NOT MSVC) - option(GTSAM_BUILD_CONVENIENCE_LIBRARIES "Enable/Disable use of convenience libraries for faster development rebuilds, but slower install" OFF) -endif() option(GTSAM_POSE3_EXPMAP "Enable/Disable using Pose3::EXPMAP as the default mode. If disabled, Pose3::FIRST_ORDER will be used." OFF) option(GTSAM_ROT3_EXPMAP "Ignore if GTSAM_USE_QUATERNIONS is OFF (Rot3::EXPMAP by default). Otherwise, enable Rot3::EXPMAP, or if disabled, use Rot3::CAYLEY." OFF) option(GTSAM_ENABLE_CONSISTENCY_CHECKS "Enable/Disable expensive consistency checks" OFF) +option(GTSAM_WITH_TBB "Use Intel Threaded Building Blocks (TBB) if available" ON) +option(GTSAM_WITH_EIGEN_MKL "Eigen will use Intel MKL if available" ON) +option(GTSAM_WITH_EIGEN_MKL_OPENMP "Eigen, when using Intel MKL, will also use OpenMP for multithreading if available" ON) +option(GTSAM_THROW_CHEIRALITY_EXCEPTION "Throw exception when a triangulated point is behind a camera" ON) # Options relating to MATLAB wrapper # TODO: Check for matlab mex binary before handling building of binaries option(GTSAM_INSTALL_MATLAB_TOOLBOX "Enable/Disable installation of matlab toolbox" OFF) option(GTSAM_BUILD_WRAP "Enable/Disable building of matlab wrap utility (necessary for matlab interface)" ON) -option(GTSAM_INSTALL_WRAP "Enable/Disable installation of wrap utility for wrapping other libraries" ON) # Check / set dependent variables for MATLAB wrapper -set(GTSAM_WRAP_HEADER_PATH "${PROJECT_SOURCE_DIR}/wrap") if(GTSAM_INSTALL_MATLAB_TOOLBOX AND NOT GTSAM_BUILD_WRAP) message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX is enabled, please also enable GTSAM_BUILD_WRAP") endif() @@ -76,33 +73,29 @@ if(GTSAM_INSTALL_WRAP AND NOT GTSAM_BUILD_WRAP) message(FATAL_ERROR "GTSAM_INSTALL_WRAP is enabled, please also enable GTSAM_BUILD_WRAP") endif() +if(GTSAM_INSTALL_MATLAB_TOOLBOX AND GTSAM_BUILD_STATIC_LIBRARY) + message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and GTSAM_BUILD_STATIC_LIBRARY are both enabled. The MATLAB wrapper cannot be compiled with a static GTSAM library because mex modules are themselves shared libraries. If you want a self-contained mex module, enable GTSAM_MEX_BUILD_STATIC_MODULE instead of GTSAM_BUILD_STATIC_LIBRARY.") +endif() + # Flags for choosing default packaging tools set(CPACK_SOURCE_GENERATOR "TGZ" CACHE STRING "CPack Default Source Generator") set(CPACK_GENERATOR "TGZ" CACHE STRING "CPack Default Binary Generator") -# Sanity check building of libraries -if (NOT GTSAM_BUILD_SHARED_LIBRARY AND NOT GTSAM_BUILD_STATIC_LIBRARY) - message(FATAL_ERROR "Both shared and static version of GTSAM library disabled - need to choose at least one!") -endif() - -# Flags to determine whether tests and examples are build during 'make install' -# Note that these remove the targets from the 'all' -option(GTSAM_DISABLE_TESTS_ON_INSTALL "Disables building tests during install" ON) -option(GTSAM_DISABLE_EXAMPLES_ON_INSTALL "Disables building examples during install" OFF) - -# Pull in infrastructure -if (GTSAM_BUILD_TESTS) - enable_testing() - include(Dart) - include(CTest) -endif() - ############################################################################### # Find boost -# If using Boost shared libs, set up auto linking for shared libs -if(MSVC AND NOT Boost_USE_STATIC_LIBS) +# To change the path for boost, you will need to set: +# BOOST_ROOT: path to install prefix for boost +# Boost_NO_SYSTEM_PATHS: set to true to keep the find script from ignoring BOOST_ROOT + +# If using Boost shared libs, disable auto linking +if(MSVC) + # Some libraries, at least Boost Program Options, rely on this to export DLL symbols add_definitions(-DBOOST_ALL_DYN_LINK) + # Disable autolinking + if(NOT Boost_USE_STATIC_LIBS) + add_definitions(-DBOOST_ALL_NO_LIB) + endif() endif() find_package(Boost 1.43 COMPONENTS serialization system filesystem thread program_options date_time regex timer chrono) @@ -115,7 +108,9 @@ endif() option(GTSAM_DISABLE_NEW_TIMERS "Disables using Boost.chrono for timing" OFF) # Allow for not using the timer libraries on boost < 1.48 (GTSAM timing code falls back to old timer library) -set(GTSAM_BOOST_LIBRARIES ${Boost_SERIALIZATION_LIBRARY} ${Boost_SYSTEM_LIBRARY} ${Boost_FILESYSTEM_LIBRARY} ${Boost_THREAD_LIBRARY} ${Boost_DATE_TIME_LIBRARY} ${Boost_REGEX_LIBRARY}) +set(GTSAM_BOOST_LIBRARIES + ${Boost_SERIALIZATION_LIBRARY} ${Boost_SYSTEM_LIBRARY} ${Boost_FILESYSTEM_LIBRARY} + ${Boost_THREAD_LIBRARY} ${Boost_DATE_TIME_LIBRARY} ${Boost_REGEX_LIBRARY}) if (GTSAM_DISABLE_NEW_TIMERS) message("WARNING: GTSAM timing instrumentation manually disabled") add_definitions(-DGTSAM_DISABLE_NEW_TIMERS) @@ -123,7 +118,64 @@ else() if(Boost_TIMER_LIBRARY) list(APPEND GTSAM_BOOST_LIBRARIES ${Boost_TIMER_LIBRARY} ${Boost_CHRONO_LIBRARY}) else() - message("WARNING: Boost older than 1.48 was found, GTSAM timing instrumentation will use the older, less accurate, Boost timer library.") + list(APPEND GTSAM_BOOST_LIBRARIES rt) # When using the header-only boost timer library, need -lrt + message("WARNING: GTSAM timing instrumentation will use the older, less accurate, Boost timer library because boost older than 1.48 was found.") + endif() +endif() + + +############################################################################### +# Find TBB +find_package(TBB) + +# Set up variables if we're using TBB +if(TBB_FOUND AND GTSAM_WITH_TBB) + set(GTSAM_USE_TBB 1) # This will go into config.h + include_directories(BEFORE ${TBB_INCLUDE_DIRS}) + set(GTSAM_TBB_LIBRARIES "") + if(TBB_DEBUG_LIBRARIES) + foreach(lib ${TBB_LIBRARIES}) + list(APPEND GTSAM_TBB_LIBRARIES optimized "${lib}") + endforeach() + foreach(lib ${TBB_DEBUG_LIBRARIES}) + list(APPEND GTSAM_TBB_LIBRARIES debug "${lib}") + endforeach() + else() + set(GTSAM_TBB_LIBRARIES ${TBB_LIBRARIES}) + endif() + list(APPEND GTSAM_ADDITIONAL_LIBRARIES ${GTSAM_TBB_LIBRARIES}) +else() + set(GTSAM_USE_TBB 0) # This will go into config.h +endif() + + +############################################################################### +# Find Google perftools +find_package(GooglePerfTools) + + +############################################################################### +# Find MKL +if(GTSAM_USE_EIGEN_MKL) + find_package(MKL) + + if(MKL_FOUND AND GTSAM_WITH_EIGEN_MKL) + set(GTSAM_USE_EIGEN_MKL 1) # This will go into config.h + set(EIGEN_USE_MKL_ALL 1) # This will go into config.h - it makes Eigen use MKL + include_directories(${MKL_INCLUDE_DIR}) + list(APPEND GTSAM_ADDITIONAL_LIBRARIES ${MKL_LIBRARIES}) + endif() +endif() + + +############################################################################### +# Find OpenMP (if we're also using MKL) +if(GTSAM_USE_EIGEN_MKL AND GTSAM_USE_EIGEN_MKL_OPENMP AND GTSAM_USE_EIGEN_MKL) + find_package(OpenMP) + + if(OPENMP_FOUND AND GTSAM_USE_EIGEN_MKL AND GTSAM_WITH_EIGEN_MKL_OPENMP) + set(GTSAM_USE_EIGEN_MKL_OPENMP 1) # This will go into config.h + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") endif() endif() @@ -164,6 +216,41 @@ install(FILES ${PROJECT_BINARY_DIR}/gtsam/3rdparty/gtsam_eigen_includes.h DESTIN ############################################################################### # Global compile options +# Build list of possible allocators +set(possible_allocators "") +if(GTSAM_USE_TBB) + list(APPEND possible_allocators TBB) + set(preferred_allocator TBB) +else() + list(APPEND possible_allocators BoostPool STL) + set(preferred_allocator STL) +endif() +if(GOOGLE_PERFTOOLS_FOUND) + list(APPEND possible_allocators tcmalloc) +endif() + +# Check if current allocator choice is valid and set cache option +list(FIND possible_allocators "${GTSAM_DEFAULT_ALLOCATOR}" allocator_valid) +if(allocator_valid EQUAL -1) + set(GTSAM_DEFAULT_ALLOCATOR ${preferred_allocator} CACHE STRING "Default allocator" FORCE) +else() + set(GTSAM_DEFAULT_ALLOCATOR ${preferred_allocator} CACHE STRING "Default allocator") +endif() +set_property(CACHE GTSAM_DEFAULT_ALLOCATOR PROPERTY STRINGS ${possible_allocators}) +mark_as_advanced(GTSAM_DEFAULT_ALLOCATOR) + +# Define compile flags depending on allocator +if("${GTSAM_DEFAULT_ALLOCATOR}" STREQUAL "BoostPool") + set(GTSAM_ALLOCATOR_BOOSTPOOL 1) +elseif("${GTSAM_DEFAULT_ALLOCATOR}" STREQUAL "STL") + set(GTSAM_ALLOCATOR_STL 1) +elseif("${GTSAM_DEFAULT_ALLOCATOR}" STREQUAL "TBB") + set(GTSAM_ALLOCATOR_TBB 1) +elseif("${GTSAM_DEFAULT_ALLOCATOR}" STREQUAL "tcmalloc") + set(GTSAM_ALLOCATOR_STL 1) # tcmalloc replaces malloc, so to use it we use the STL allocator + list(APPEND GTSAM_ADDITIONAL_LIBRARIES "tcmalloc") +endif() + # Include boost - use 'BEFORE' so that a specific boost specified to CMake # takes precedence over a system-installed one. include_directories(BEFORE ${Boost_INCLUDE_DIR}) @@ -174,35 +261,25 @@ include_directories(BEFORE ${Boost_INCLUDE_DIR}) include_directories(BEFORE gtsam/3rdparty/UFconfig gtsam/3rdparty/CCOLAMD/Include + gtsam/3rdparty/metis-5.1.0/include + gtsam/3rdparty/metis-5.1.0/libmetis + gtsam/3rdparty/metis-5.1.0/GKlib ${PROJECT_SOURCE_DIR} ${PROJECT_BINARY_DIR} # So we can include generated config header files CppUnitLite) if(MSVC) add_definitions(-D_CRT_SECURE_NO_WARNINGS -D_SCL_SECURE_NO_WARNINGS) - add_definitions(/wd4251 /wd4275 /wd4251) # Disable non-DLL-exported base class warnings + add_definitions(/wd4251 /wd4275 /wd4251 /wd4661 /wd4344) # Disable non-DLL-exported base class and other warnings endif() if(GTSAM_ENABLE_CONSISTENCY_CHECKS) add_definitions(-DGTSAM_EXTRA_CONSISTENCY_CHECKS) -endif() +endif() ############################################################################### # Add components -# Set default library - static or shared, before adding subdirectories -if(GTSAM_BUILD_SHARED_LIBRARY) - set(gtsam-default gtsam-shared) - if(GTSAM_BUILD_UNSTABLE) - set(gtsam_unstable-default gtsam_unstable-shared) - endif() -else() - set(gtsam-default gtsam-static) - if(GTSAM_BUILD_UNSTABLE) - set(gtsam_unstable-default gtsam_unstable-static) - endif() -endif() - # Build CppUnitLite add_subdirectory(CppUnitLite) @@ -218,9 +295,7 @@ add_subdirectory(gtsam) add_subdirectory(tests) # Build examples -if (GTSAM_BUILD_EXAMPLES) - add_subdirectory(examples) -endif(GTSAM_BUILD_EXAMPLES) +add_subdirectory(examples) # Matlab toolbox if (GTSAM_INSTALL_MATLAB_TOOLBOX) @@ -233,7 +308,7 @@ if (GTSAM_BUILD_UNSTABLE) endif(GTSAM_BUILD_UNSTABLE) # Install config and export files -GtsamMakeConfigFile(GTSAM) +GtsamMakeConfigFile(GTSAM "${CMAKE_CURRENT_SOURCE_DIR}/gtsam_extra.cmake.in") export(TARGETS ${GTSAM_EXPORTED_TARGETS} FILE GTSAM-exports.cmake) # Check for doxygen availability - optional dependency @@ -244,13 +319,16 @@ if (DOXYGEN_FOUND) add_subdirectory(doc) endif() +# CMake Tools +add_subdirectory(cmake) + ############################################################################### # Set up CPack set(CPACK_PACKAGE_DESCRIPTION_SUMMARY "GTSAM") set(CPACK_PACKAGE_VENDOR "Frank Dellaert, Georgia Institute of Technology") set(CPACK_PACKAGE_CONTACT "Frank Dellaert, dellaert@cc.gatech.edu") -set(CPACK_PACKAGE_DESCRIPTION_FILE "${CMAKE_CURRENT_SOURCE_DIR}/README") +set(CPACK_PACKAGE_DESCRIPTION_FILE "${CMAKE_CURRENT_SOURCE_DIR}/README.md") set(CPACK_RESOURCE_FILE_LICENSE "${CMAKE_CURRENT_SOURCE_DIR}/LICENSE") set(CPACK_PACKAGE_VERSION_MAJOR ${GTSAM_VERSION_MAJOR}) set(CPACK_PACKAGE_VERSION_MINOR ${GTSAM_VERSION_MINOR}) @@ -274,42 +352,75 @@ set(CPACK_DEBIAN_PACKAGE_DEPENDS "libboost-dev (>= 1.43)") #Example: "libc6 (>= message(STATUS "===============================================================") message(STATUS "================ Configuration Options ======================") message(STATUS "Build flags ") -#print_config_flag(${GTSAM_BUILD_TIMING} "Build Timing scripts ") -print_config_flag(${GTSAM_BUILD_EXAMPLES} "Build Examples ") print_config_flag(${GTSAM_BUILD_TESTS} "Build Tests ") +print_config_flag(${GTSAM_BUILD_EXAMPLES_ALWAYS} "Build examples with 'make all' ") +print_config_flag(${GTSAM_BUILD_TIMING} "Build Timing scripts ") if (DOXYGEN_FOUND) - print_config_flag(${GTSAM_BUILD_DOCS} "Build Docs ") + print_config_flag(${GTSAM_BUILD_DOCS} "Build Docs ") endif() -print_config_flag(${GTSAM_BUILD_SHARED_LIBRARY} "Build shared GTSAM Library ") -print_config_flag(${GTSAM_BUILD_STATIC_LIBRARY} "Build static GTSAM Library ") -if(NOT MSVC) - print_config_flag(${GTSAM_BUILD_CONVENIENCE_LIBRARIES} "Build Convenience Libraries ") -endif() -print_config_flag(${GTSAM_BUILD_TYPE_POSTFIXES} "Put build-type in library name ") +print_config_flag(${GTSAM_BUILD_STATIC_LIBRARY} "Build static GTSAM library instead of shared") +print_config_flag(${GTSAM_BUILD_TYPE_POSTFIXES} "Put build type in library name ") if(GTSAM_UNSTABLE_AVAILABLE) - print_config_flag(${GTSAM_BUILD_UNSTABLE} "Build libgtsam_unstable ") + print_config_flag(${GTSAM_BUILD_UNSTABLE} "Build libgtsam_unstable ") endif() -print_config_flag(${GTSAM_DISABLE_TESTS_ON_INSTALL} "Tests excluded from all/install target ") -print_config_flag(${GTSAM_DISABLE_EXAMPLES_ON_INSTALL} "Examples excluded from all/install target ") string(TOUPPER "${CMAKE_BUILD_TYPE}" cmake_build_type_toupper) -message(STATUS " Build type : ${CMAKE_BUILD_TYPE}") -message(STATUS " C compilation flags : ${CMAKE_C_FLAGS} ${CMAKE_C_FLAGS_${cmake_build_type_toupper}}") -message(STATUS " C++ compilation flags : ${CMAKE_CXX_FLAGS} ${CMAKE_CXX_FLAGS_${cmake_build_type_toupper}}") +if(NOT MSVC AND NOT XCODE_VERSION) + message(STATUS " Build type : ${CMAKE_BUILD_TYPE}") + message(STATUS " C compilation flags : ${CMAKE_C_FLAGS} ${CMAKE_C_FLAGS_${cmake_build_type_toupper}}") + message(STATUS " C++ compilation flags : ${CMAKE_CXX_FLAGS} ${CMAKE_CXX_FLAGS_${cmake_build_type_toupper}}") +endif() +if(GTSAM_USE_TBB) + message(STATUS " Use Intel TBB : Yes") +elseif(TBB_FOUND) + message(STATUS " Use Intel TBB : TBB found but GTSAM_WITH_TBB is disabled") +else() + message(STATUS " Use Intel TBB : TBB not found") +endif() +if(GTSAM_USE_EIGEN_MKL) + message(STATUS " Eigen will use MKL : Yes") +elseif(MKL_FOUND) + message(STATUS " Eigen will use MKL : MKL found but GTSAM_WITH_EIGEN_MKL is disabled") +else() + message(STATUS " Eigen will use MKL : MKL not found") +endif() +if(GTSAM_USE_EIGEN_MKL_OPENMP) + message(STATUS " Eigen will use MKL and OpenMP : Yes") +elseif(OPENMP_FOUND AND NOT GTSAM_WITH_EIGEN_MKL) + message(STATUS " Eigen will use MKL and OpenMP : OpenMP found but GTSAM_WITH_EIGEN_MKL is disabled") +elseif(OPENMP_FOUND AND NOT MKL_FOUND) + message(STATUS " Eigen will use MKL and OpenMP : OpenMP found but MKL not found") +elseif(OPENMP_FOUND) + message(STATUS " Eigen will use MKL and OpenMP : OpenMP found but GTSAM_WITH_EIGEN_MKL_OPENMP is disabled") +else() + message(STATUS " Eigen will use MKL and OpenMP : OpenMP not found") +endif() +message(STATUS " Default allocator : ${GTSAM_DEFAULT_ALLOCATOR}") + message(STATUS "Packaging flags ") -message(STATUS " CPack Source Generator : ${CPACK_SOURCE_GENERATOR}") -message(STATUS " CPack Generator : ${CPACK_GENERATOR}") +message(STATUS " CPack Source Generator : ${CPACK_SOURCE_GENERATOR}") +message(STATUS " CPack Generator : ${CPACK_GENERATOR}") message(STATUS "GTSAM flags ") -print_config_flag(${GTSAM_USE_QUATERNIONS} "Quaternions as default Rot3 ") -print_config_flag(${GTSAM_ENABLE_CONSISTENCY_CHECKS} "Runtime consistency checking ") -print_config_flag(${GTSAM_POSE3_EXPMAP} "Using full expmap as defaul retract for Pose3 ") +print_config_flag(${GTSAM_USE_QUATERNIONS} "Quaternions as default Rot3 ") +print_config_flag(${GTSAM_ENABLE_CONSISTENCY_CHECKS} "Runtime consistency checking ") +print_config_flag(${GTSAM_POSE3_EXPMAP} "Pose3 retract is full ExpMap ") message(STATUS "MATLAB toolbox flags ") -print_config_flag(${GTSAM_INSTALL_MATLAB_TOOLBOX} "Install matlab toolbox ") -print_config_flag(${GTSAM_BUILD_WRAP} "Build Wrap ") -print_config_flag(${GTSAM_INSTALL_WRAP} "Install wrap utility ") +print_config_flag(${GTSAM_INSTALL_MATLAB_TOOLBOX} "Install matlab toolbox ") +print_config_flag(${GTSAM_BUILD_WRAP} "Build Wrap ") message(STATUS "===============================================================") +# Print warnings at the end +if(GTSAM_WITH_TBB AND NOT TBB_FOUND) + message(WARNING "TBB was not found - this is ok, but note that GTSAM parallelization will be disabled. Set GTSAM_WITH_TBB to 'Off' to avoid this warning.") +endif() +if(GTSAM_WITH_EIGEN_MKL AND NOT MKL_FOUND) + message(WARNING "MKL was not found - this is ok, but note that MKL yields better performance. Set GTSAM_WITH_EIGEN_MKL to 'Off' to disable this warning.") +endif() +if(GTSAM_WITH_EIGEN_MKL_OPENMP AND NOT OPENMP_FOUND AND MKL_FOUND) + message(WARNING "Your compiler does not support OpenMP - this is ok, but performance may be improved with OpenMP. Set GTSAM_WITH_EIGEN_MKL_OPENMP to 'Off' to avoid this warning.") +endif() + # Include CPack *after* all flags include(CPack) diff --git a/DEVELOP b/DEVELOP new file mode 100644 index 000000000..483197bc8 --- /dev/null +++ b/DEVELOP @@ -0,0 +1,19 @@ +Information for developers + +Coding Conventions: + +* Classes are Uppercase, methods and functions lowerMixedCase +* We use a modified K&R Style, with 2-space tabs, inserting spaces for tabs +* Use meaningful variable names, e.g., measurement not msm + + +Windows: + +On Windows it is necessary to explicitly export all functions from the library +which should be externally accessible. To do this, include the macro +GTSAM_EXPORT in your class or function definition. + +For example: +class GTSAM_EXPORT MyClass { ... }; + +GTSAM_EXPORT myFunction(); \ No newline at end of file diff --git a/README b/INSTALL similarity index 72% rename from README rename to INSTALL index a20a683a0..c71dcd4f9 100644 --- a/README +++ b/INSTALL @@ -1,6 +1,3 @@ -README - Georgia Tech Smoothing and Mapping library ---------------------------------------------------- - Quickstart In the root library folder execute: @@ -11,42 +8,6 @@ $] cmake .. $] make check (optional, runs unit tests) $] make install ---------------------------------------------------- -What is GTSAM? - - GTSAM is a library of C++ classes that implement smoothing and - mapping (SAM) in robotics and vision, using factor graphs and Bayes - networks as the underlying computing paradigm rather than sparse - matrices. - -GTSAM is not (yet) open source: See COPYING & LICENSE -Please see USAGE for an example on how to use GTSAM. - -The library is organized according to the following directory structure: - - 3rdparty local copies of third party libraries - Eigen3 and CCOLAMD - base provides some base Math and data structures, as well as test-related utilities - geometry points, poses, tensors, etc - inference core graphical model inference such as factor graphs, junction trees, Bayes nets, Bayes trees - linear inference specialized to Gaussian linear case, GaussianFactorGraph etc... - nonlinear non-linear factor graphs and non-linear optimization - slam SLAM and visual SLAM application code - -This library contains unchanged copies of two third party libraries, with documentation -of licensing as follows: - - CCOLAMD 2.73: Tim Davis' constrained column approximate minimum degree ordering library - - http://www.cise.ufl.edu/research/sparse - - Licenced under LGPL v2.1, provided in gtsam/3rdparty/CCOLAMD/Doc/lesser.txt - - Eigen 3.2: General C++ matrix and linear algebra library - - Licenced under MPL2, provided in gtsam/3rdparty/Eigen/COPYING.README (some code that is 3rd-party to - Eigen is BSD and LGPL) - - -There are two supporting libraries: - - CppUnitLite unit test library customized for use with gtsam - wrap code generation utility for the Matlab interface to gtsam - Important Installation Notes ---------------------------- @@ -56,16 +17,30 @@ GTSAM requires the following libraries to be installed on your system: - Cmake version 2.6 or higher - Support for XCode 4.3 command line tools on Mac requires CMake 2.8.8 or higher -Tested compilers - - GCC 4.2-4.7 - - Clang 2.9-3.2 - - OSX GCC 4.2 - - MSVC 2010, 2012 - +Optional dependent libraries: + - If TBB is installed and detectable by CMake GTSAM will use it automatically. + Ensure that CMake prints "Use Intel TBB : Yes". To disable the use of TBB, + disable the CMake flag GTSAM_WITH_TBB (enabled by default). On Ubuntu, TBB + may be installed from the Ubuntu repositories, and for other platforms it + may be downloaded from https://www.threadingbuildingblocks.org/ + +Tested compilers: + +- GCC 4.2-4.7 +- OSX Clang 2.9-5.0 +- OSX GCC 4.2 +- MSVC 2010, 2012 + Tested systems: - - Ubuntu 11.04, 11.10, 12.04, 12.10, 13.04 - - MacOS 10.6, 10.7 - - Windows 7 + +- Ubuntu 11.04 - 13.10 +- MacOS 10.6 - 10.9 +- Windows 7, 8, 8.1 + +Known issues: + +- MSVC 2013 is not yet supported because it cannot build the serialization module + of Boost 1.55 (or earlier). 2) GTSAM makes extensive use of debug assertions, and we highly recommend you work diff --git a/LICENSE b/LICENSE index 81c000246..e7424bbc2 100644 --- a/LICENSE +++ b/LICENSE @@ -1,29 +1,18 @@ -Copyright (c) 2010, Georgia Tech Research Corporation -Atlanta, Georgia 30332-0415 -All Rights Reserved +GTSAM is released under the simplified BSD license, reproduced in the file +LICENSE.BSD in this directory. -See also README for licensing of 3rd-party code included in GTSAM. +GTSAM contains two third party libraries, with documentation of licensing and +modifications as follows: -Redistribution and use in source and binary forms, with or without modification, -are permitted provided that the following conditions are met: - - * Redistributions of source code must retain the above copyright notice, - this list of conditions and the following disclaimer. - - * Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - - * Neither the name of the copyright holders nor the names of its contributors - may be used to endorse or promote products derived from this software without - specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY -EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES -OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT -SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED -TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR -BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY -WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +- CCOLAMD 2.73: Tim Davis' constrained column approximate minimum degree +ordering library + - Included unmodified in gtsam/3rdparty/CCOLAMD and gtsam/3rdparty/UFconfig + - http://www.cise.ufl.edu/research/sparse + - Licenced under LGPL v2.1, provided in gtsam/3rdparty/CCOLAMD/Doc/lesser.txt +- Eigen 3.2: General C++ matrix and linear algebra library + - Modified with 3 patches that have been contributed back to the Eigen team: + - http://eigen.tuxfamily.org/bz/show_bug.cgi?id=704 (Householder QR MKL selection) + - http://eigen.tuxfamily.org/bz/show_bug.cgi?id=705 (Fix MKL LLT return code) + - http://eigen.tuxfamily.org/bz/show_bug.cgi?id=716 (Improved comma initialization) + - Licenced under MPL2, provided in gtsam/3rdparty/Eigen/COPYING.README + - Some code that is 3rd-party to Eigen is BSD and LGPL \ No newline at end of file diff --git a/LICENSE.BSD b/LICENSE.BSD new file mode 100644 index 000000000..406b266b7 --- /dev/null +++ b/LICENSE.BSD @@ -0,0 +1,13 @@ +Copyright (c) 2010, Georgia Tech Research Corporation +Atlanta, Georgia 30332-0415 +All Rights Reserved + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. + +3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. \ No newline at end of file diff --git a/README.md b/README.md new file mode 100644 index 000000000..460f51bf3 --- /dev/null +++ b/README.md @@ -0,0 +1,47 @@ +README - Georgia Tech Smoothing and Mapping library +=================================================== + +What is GTSAM? +-------------- + +GTSAM is a library of C++ classes that implement smoothing and +mapping (SAM) in robotics and vision, using factor graphs and Bayes +networks as the underlying computing paradigm rather than sparse +matrices. + +On top of the C++ library, GTSAM includes a MATLAB interface (enable +GTSAM_INSTALL_MATLAB_TOOLBOX in CMake to build it). A Python interface +is under development. + +Quickstart +---------- + +In the root library folder execute: + +``` +#!bash +$ mkdir build +$ cd build +$ cmake .. +$ make check (optional, runs unit tests) +$ make install +``` + +Prerequisites: + +- [Boost](http://www.boost.org/users/download/) >= 1.43 (Ubuntu: `sudo apt-get install libboost-all-dev`) +- [CMake](http://www.cmake.org/cmake/resources/software.html) >= 2.6 (Ubuntu: `sudo apt-get install cmake`) + +Optional prerequisites - used automatically if findable by CMake: + +- [Intel Threaded Building Blocks (TBB)](http://www.threadingbuildingblocks.org/) (Ubuntu: `sudo apt-get install libtbb-dev`) +- [Intel Math Kernel Library (MKL)](http://software.intel.com/en-us/intel-mkl) + +Additional Information +---------------------- + +See the [`INSTALL`](https://bitbucket.org/gtborg/gtsam/src/develop/INSTALL) file for more detailed installation instructions. + +GTSAM is open source under the BSD license, see the [`LICENSE`](https://bitbucket.org/gtborg/gtsam/src/develop/LICENSE) and [`LICENSE.BSD`](https://bitbucket.org/gtborg/gtsam/src/develop/LICENSE.BSD) files. + +Please see the [`examples/`](https://bitbucket.org/gtborg/gtsam/src/develop/examples) directory and the [`USAGE`](https://bitbucket.org/gtborg/gtsam/src/develop/USAGE) file for examples on how to use GTSAM. diff --git a/USAGE b/USAGE index e5f4dc581..a41b71045 100644 --- a/USAGE +++ b/USAGE @@ -47,6 +47,17 @@ Factors: SLAM example, is a measurement such as a visual reading on a landmark or odometry. +The library is organized according to the following directory structure: + + 3rdparty local copies of third party libraries - Eigen3 and CCOLAMD + base provides some base Math and data structures, as well as test-related utilities + geometry points, poses, tensors, etc + inference core graphical model inference such as factor graphs, junction trees, Bayes nets, Bayes trees + linear inference specialized to Gaussian linear case, GaussianFactorGraph etc... + nonlinear non-linear factor graphs and non-linear optimization + slam SLAM and visual SLAM application code + + VSLAM Example --------------------------------------------------- diff --git a/cmake/CMakeLists.txt b/cmake/CMakeLists.txt new file mode 100644 index 000000000..49069c57f --- /dev/null +++ b/cmake/CMakeLists.txt @@ -0,0 +1,26 @@ +# This file installs the scripts from this directory that may be used in other +# projects. See README.txt in this directory for documentation. + +# Set the install directory depending on the platform so it will be found by +# find_package(GTSAMCMakeTools) +if(WIN32 AND NOT CYGWIN) + set(SCRIPT_INSTALL_DIR "${CMAKE_INSTALL_PREFIX}/CMake") +else() + set(SCRIPT_INSTALL_DIR "${CMAKE_INSTALL_PREFIX}/lib/cmake") +endif() + +# Install scripts +install(FILES + GTSAMCMakeToolsConfig.cmake + Config.cmake.in + dllexport.h.in + GtsamBuildTypes.cmake + GtsamMakeConfigFile.cmake + GtsamMatlabWrap.cmake + GtsamPythonWrap.cmake + GtsamTesting.cmake + GtsamTestingObsolete.cmake + README.html + DESTINATION "${SCRIPT_INSTALL_DIR}/GTSAMCMakeTools") + + diff --git a/cmake/GTSAMCMakeToolsConfig.cmake b/cmake/GTSAMCMakeToolsConfig.cmake new file mode 100644 index 000000000..c79a2f5c2 --- /dev/null +++ b/cmake/GTSAMCMakeToolsConfig.cmake @@ -0,0 +1,3 @@ +# This config file modifies CMAKE_MODULE_PATH so that the GTSAM-CMakeTools files may be included + +list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}") diff --git a/cmake/GtsamBuildTypes.cmake b/cmake/GtsamBuildTypes.cmake index 1fc259724..ee2bbd42a 100644 --- a/cmake/GtsamBuildTypes.cmake +++ b/cmake/GtsamBuildTypes.cmake @@ -10,11 +10,7 @@ if(NOT FIRST_PASS_DONE AND NOT CMAKE_BUILD_TYPE AND NOT MSVC AND NOT XCODE_VERSI endif() # Add option for using build type postfixes to allow installing multiple build modes -if(MSVC OR XCODE_VERSION) - option(GTSAM_BUILD_TYPE_POSTFIXES "Enable/Disable appending the build type to the name of compiled libraries" ON) -else() - option(GTSAM_BUILD_TYPE_POSTFIXES "Enable/Disable appending the build type to the name of compiled libraries" OFF) -endif() +option(GTSAM_BUILD_TYPE_POSTFIXES "Enable/Disable appending the build type to the name of compiled libraries" ON) # Add debugging flags but only on the first pass if(NOT FIRST_PASS_DONE) @@ -66,7 +62,7 @@ endif() # Set up build type library postfixes if(GTSAM_BUILD_TYPE_POSTFIXES) - foreach(build_type Debug Timing RelWithDebInfo MinSizeRel) + foreach(build_type Debug Timing Profiling RelWithDebInfo MinSizeRel) string(TOUPPER "${build_type}" build_type_toupper) set(CMAKE_${build_type_toupper}_POSTFIX ${build_type}) endforeach() @@ -106,7 +102,7 @@ if( NOT cmake_build_type_tolower STREQUAL "" endif() # Mark that first pass is done -set(FIRST_PASS_DONE TRUE CACHE BOOL "Internally used to mark whether cmake has been run multiple times" FORCE) +set(FIRST_PASS_DONE TRUE CACHE INTERNAL "Internally used to mark whether cmake has been run multiple times") mark_as_advanced(FIRST_PASS_DONE) # Enable Visual Studio solution folders diff --git a/cmake/GtsamMakeConfigFile.cmake b/cmake/GtsamMakeConfigFile.cmake index 4b5d96a2e..74081b58a 100644 --- a/cmake/GtsamMakeConfigFile.cmake +++ b/cmake/GtsamMakeConfigFile.cmake @@ -1,5 +1,7 @@ # Writes a config file +set(GTSAM_CONFIG_TEMPLATE_PATH ${CMAKE_CURRENT_LIST_DIR}) + function(GtsamMakeConfigFile PACKAGE_NAME) if(WIN32 AND NOT CYGWIN) @@ -20,7 +22,7 @@ function(GtsamMakeConfigFile PACKAGE_NAME) file(RELATIVE_PATH CONF_REL_INCLUDE_DIR "${CMAKE_INSTALL_PREFIX}/${DEF_INSTALL_CMAKE_DIR}" "${CMAKE_INSTALL_PREFIX}/include") file(RELATIVE_PATH CONF_REL_LIB_DIR "${CMAKE_INSTALL_PREFIX}/${DEF_INSTALL_CMAKE_DIR}" "${CMAKE_INSTALL_PREFIX}/lib") - configure_file(${PROJECT_SOURCE_DIR}/cmake/Config.cmake.in "${PROJECT_BINARY_DIR}/${PACKAGE_NAME}Config.cmake" @ONLY) + configure_file(${GTSAM_CONFIG_TEMPLATE_PATH}/Config.cmake.in "${PROJECT_BINARY_DIR}/${PACKAGE_NAME}Config.cmake" @ONLY) message(STATUS "Wrote ${PROJECT_BINARY_DIR}/${PACKAGE_NAME}Config.cmake") # Install config and exports files (for find scripts) diff --git a/cmake/GtsamMatlabWrap.cmake b/cmake/GtsamMatlabWrap.cmake index d699795fd..98bd7f469 100644 --- a/cmake/GtsamMatlabWrap.cmake +++ b/cmake/GtsamMatlabWrap.cmake @@ -40,19 +40,28 @@ set(MATLAB_ROOT "${MATLAB_ROOT}" CACHE PATH "Path to MATLAB installation root (e # User-friendly wrapping function. Builds a mex module from the provided -# interfaceHeader. For example, for the interface header /path/to/gtsam.h, +# interfaceHeader. For example, for the interface header gtsam.h, # this will build the wrap module 'gtsam'. -# Params: -# interfaceHeader : Absolute or relative path to the interface definition file -# linkLibraries : All dependent CMake target names, library names, or full library paths -# extraIncludeDirs : Extra include directories, in addition to those already passed to include_directories(...) -# extraMexFlags : Any additional compiler flags +# +# Arguments: +# +# interfaceHeader: The relative path to the wrapper interface definition file. +# linkLibraries: Any *additional* libraries to link. Your project library +# (e.g. `lba`), libraries it depends on, and any necessary +# MATLAB libraries will be linked automatically. So normally, +# leave this empty. +# extraIncludeDirs: Any *additional* include paths required by dependent +# libraries that have not already been added by +# include_directories. Again, normally, leave this empty. +# extraMexFlags: Any *additional* flags to pass to the compiler when building +# the wrap code. Normally, leave this empty. function(wrap_and_install_library interfaceHeader linkLibraries extraIncludeDirs extraMexFlags) - wrap_library_internal("${interfaceHeader}" "${otherLibraries}" "${extraIncludeDirs}" "${mexFlags}") + wrap_library_internal("${interfaceHeader}" "${linkLibraries}" "${extraIncludeDirs}" "${mexFlags}") install_wrapped_library_internal("${interfaceHeader}") endfunction() +# Internal function that wraps a library and compiles the wrapper function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs extraMexFlags) if(UNIX AND NOT APPLE) if(CMAKE_SIZEOF_VOID_P EQUAL 8) @@ -103,7 +112,8 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs ex # Add -shared or -static suffix to targets set(correctedOtherLibraries "") set(otherLibraryTargets "") - foreach(lib ${moduleName} ${otherLibraries}) + set(otherLibraryNontargets "") + foreach(lib ${moduleName} ${linkLibraries}) if(TARGET ${lib}) list(APPEND correctedOtherLibraries ${lib}) list(APPEND otherLibraryTargets ${lib}) @@ -115,8 +125,20 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs ex list(APPEND otherLibraryTargets ${lib}-static) else() list(APPEND correctedOtherLibraries ${lib}) + list(APPEND otherLibraryNontargets ${lib}) endif() endforeach() + + # Check libraries for conflicting versions built-in to MATLAB + set(dependentLibraries "") + if(NOT "${otherLibraryTargets}" STREQUAL "") + foreach(target ${otherLibraryTargets}) + get_target_property(dependentLibrariesOne ${target} INTERFACE_LINK_LIBRARIES) + list(APPEND dependentLibraries ${dependentLibrariesOne}) + endforeach() + endif() + list(APPEND dependentLibraries ${otherLibraryNontargets}) + check_conflicting_libraries_internal("${dependentLibraries}") # Set up generation of module source file file(MAKE_DIRECTORY "${generated_files_path}") @@ -174,6 +196,7 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs ex COMMAND cmake -E remove_directory ${compiled_mex_modules_root}) endfunction() +# Internal function that installs a wrap toolbox function(install_wrapped_library_internal interfaceHeader) get_filename_component(moduleName "${interfaceHeader}" NAME_WE) set(generated_files_path "${PROJECT_BINARY_DIR}/wrap/${moduleName}") @@ -205,53 +228,77 @@ function(install_wrapped_library_internal interfaceHeader) endif() endfunction() - -# Function to setup codegen and building of the wrap toolbox -# -# params: -# moduleName : the name of the module, interface file must be called moduleName.h -# mexFlags : Compilation flags to be passed to the mex compiler -# modulePath : relative path to module markup header file (called moduleName.h) -# otherLibraries : list of library targets this should depend on -# toolboxPath : the directory in which to generate/build wrappers -# wrap_header_path : path to the installed wrap header -function(wrap_library_generic moduleName mexFlags modulePath otherLibraries toolbox_path wrap_header_path) - - if(NOT "${CMAKE_PROJECT_NAME}" STREQUAL "GTSAM") - message("Your project uses wrap_library or wrap_library_generic - this is deprecated, please use the more user-friendly function wrap_and_install_library") - endif() - - # Append module name to link libraries to keep original behavior - list(APPEND otherLibraries ${moduleName}) - - # Set up arguments - set(interfaceHeader ${modulePath}/${moduleName}.h) - - # Call internal function - wrap_library_internal("${interfaceHeader}" "${otherLibraries}" "" "${mexFlags}") -endfunction(wrap_library_generic) - -# Function to setup codegen, building and installation of the wrap toolbox -# This wrap setup function assumes that the toolbox will be installed directly, -# with predictable matlab.h sourcing. Use this version when the toolbox will be used -# from the installed version, rather than in place. -# Assumes variable GTSAM_WRAP_HEADER_PATH has been set -# params: -# moduleName : the name of the module, interface file must be called moduleName.h -# mexFlags : Compilation flags to be passed to the mex compiler -# modulePath : relative path to module markup header file (called moduleName.h) -# otherLibraries : list of library targets this should depend on -function(wrap_library moduleName mexFlags modulePath otherLibraries) - # Toolbox generation path goes in build folder - set(toolbox_base_path ${PROJECT_BINARY_DIR}/wrap) - set(toolbox_path ${toolbox_base_path}/${moduleName}) - - # Call generic version of function - wrap_library_generic("${moduleName}" "${mexFlags}" "${modulePath}" "${otherLibraries}" "${toolbox_path}" "${GTSAM_WRAP_HEADER_PATH}") - - install_wrapped_library_internal("${modulePath}/${moduleName}.h") - -endfunction(wrap_library) +# Internal function to check for libraries installed with MATLAB that may conflict +# and prints a warning to move them if problems occur. +function(check_conflicting_libraries_internal libraries) + if(UNIX) + # Set path for matlab's built-in libraries + if(APPLE) + set(mxLibPath "${MATLAB_ROOT}/bin/maci64") + else() + if(CMAKE_CL_64) + set(mxLibPath "${MATLAB_ROOT}/bin/glnxa64") + else() + set(mxLibPath "${MATLAB_ROOT}/bin/glnx86") + endif() + endif() + + # List matlab's built-in libraries + file(GLOB matlabLibs RELATIVE "${mxLibPath}" "${mxLibPath}/lib*") + + # Convert to base names + set(matlabLibNames "") + foreach(lib ${matlabLibs}) + get_filename_component(libName "${lib}" NAME_WE) + list(APPEND matlabLibNames "${libName}") + endforeach() + + # Get names of link libraries + set(linkLibNames "") + foreach(lib ${libraries}) + string(FIND "${lib}" "/" slashPos) + if(NOT slashPos EQUAL -1) + # If the name is a path, just get the library name + get_filename_component(libName "${lib}" NAME_WE) + list(APPEND linkLibNames "${libName}") + else() + # It's not a path, so see if it looks like a filename + get_filename_component(ext "${lib}" EXT) + if(NOT "${ext}" STREQUAL "") + # It's a filename, so get the base name + get_filename_component(libName "${lib}" NAME_WE) + list(APPEND linkLibNames "${libName}") + else() + # It's not a filename so it must be a short name, add the "lib" prefix + list(APPEND linkLibNames "lib${lib}") + endif() + endif() + endforeach() + + # Remove duplicates + list(REMOVE_DUPLICATES linkLibNames) + + set(conflictingLibs "") + foreach(lib ${linkLibNames}) + list(FIND matlabLibNames "${lib}" libPos) + if(NOT libPos EQUAL -1) + if(NOT conflictingLibs STREQUAL "") + set(conflictingLibs "${conflictingLibs}, ") + endif() + set(conflictingLibs "${conflictingLibs}${lib}") + endif() + endforeach() + + if(NOT "${conflictingLibs}" STREQUAL "") + message(WARNING "GTSAM links to the libraries [ ${conflictingLibs} ] on your system, but " + "MATLAB is distributed with its own versions of these libraries which may conflict. " + "If you get strange errors or crashes with the GTSAM MATLAB wrapper, move these " + "libraries out of MATLAB's built-in library directory, which is ${mxLibPath} on " + "your system. MATLAB will usually still work with these libraries moved away, but " + "if not, you'll have to compile the static GTSAM MATLAB wrapper module.") + endif() + endif() +endfunction() # Helper function to install MATLAB scripts and handle multiple build types where the scripts # should be installed to all build type toolboxes diff --git a/cmake/GtsamTesting.cmake b/cmake/GtsamTesting.cmake index 25c2e25dd..19f487256 100644 --- a/cmake/GtsamTesting.cmake +++ b/cmake/GtsamTesting.cmake @@ -1,111 +1,204 @@ -# Build macros for using tests +# This file defines the two macros below for easily adding groups of unit tests and scripts, +# as well as sets up unit testing and defines several cache options used to control how +# tests and scripts are built and run. + + +############################################################################### +# Macro: +# +# gtsamAddTestsGlob(groupName globPatterns excludedFiles linkLibraries) +# +# Add a group of unit tests. A list of unit test .cpp files or glob patterns specifies the +# tests to create. Tests are assigned into a group name so they can easily by run +# independently with a make target. Running 'make check' builds and runs all tests. +# +# Usage example: +# gtsamAddTestsGlob(basic "test*.cpp" "testBroken.cpp" "gtsam;GeographicLib") +# +# Arguments: +# groupName: A name that will allow this group of tests to be run independently, e.g. +# 'basic' causes a 'check.basic' target to be created to run this test +# group. +# globPatterns: The list of files or glob patterns from which to create unit tests, with +# one test created for each cpp file. e.g. "test*.cpp", or +# "testA*.cpp;testB*.cpp;testOneThing.cpp". +# excludedFiles: A list of files or globs to exclude, e.g. "testC*.cpp;testBroken.cpp". +# Pass an empty string "" if nothing needs to be excluded. +# linkLibraries: The list of libraries to link to in addition to CppUnitLite. +macro(gtsamAddTestsGlob groupName globPatterns excludedFiles linkLibraries) + gtsamAddTestsGlob_impl("${groupName}" "${globPatterns}" "${excludedFiles}" "${linkLibraries}") +endmacro() + + +############################################################################### +# Macro: +# +# gtsamAddExamplesGlob(globPatterns excludedFiles linkLibraries) +# +# Add scripts that will serve as examples of how to use the library. A list of files or +# glob patterns is specified, and one executable will be created for each matching .cpp +# file. These executables will not be installed. They are build with 'make all' if +# GTSAM_BUILD_EXAMPLES_ALWAYS is enabled. They may also be built with 'make examples'. +# +# Usage example: +# gtsamAddExamplesGlob("*.cpp" "BrokenExample.cpp" "gtsam;GeographicLib") +# +# Arguments: +# globPatterns: The list of files or glob patterns from which to create unit tests, with +# one test created for each cpp file. e.g. "*.cpp", or +# "A*.cpp;B*.cpp;MyExample.cpp". +# excludedFiles: A list of files or globs to exclude, e.g. "C*.cpp;BrokenExample.cpp". Pass +# an empty string "" if nothing needs to be excluded. +# linkLibraries: The list of libraries to link to. +macro(gtsamAddExamplesGlob globPatterns excludedFiles linkLibraries) + gtsamAddExamplesGlob_impl("${globPatterns}" "${excludedFiles}" "${linkLibraries}") +endmacro() + + +# Implementation follows: + +# Build macros for using tests enable_testing() -# Enable make check (http://www.cmake.org/Wiki/CMakeEmulateMakeCheck) -add_custom_target(check COMMAND ${CMAKE_CTEST_COMMAND} -C $ --output-on-failure) -add_custom_target(timing) +option(GTSAM_BUILD_TESTS "Enable/Disable building of tests" ON) +option(GTSAM_BUILD_EXAMPLES_ALWAYS "Build examples with 'make all' (build with 'make examples' if not)" ON) # Add option for combining unit tests -if(MSVC) +if(MSVC OR XCODE_VERSION) option(GTSAM_SINGLE_TEST_EXE "Combine unit tests into single executable (faster compile)" ON) else() option(GTSAM_SINGLE_TEST_EXE "Combine unit tests into single executable (faster compile)" OFF) endif() +mark_as_advanced(GTSAM_SINGLE_TEST_EXE) -# Macro for adding categorized tests in a "tests" folder, with -# optional exclusion of tests and convenience library linking options -# -# By default, all tests are linked with CppUnitLite and boost -# Arguments: -# - subdir The name of the category for this test -# - local_libs A list of convenience libraries to use (if GTSAM_BUILD_CONVENIENCE_LIBRARIES is true) -# - full_libs The main library to link against if not using convenience libraries -# - excluded_tests A list of test files that should not be compiled - use for debugging -function(gtsam_add_subdir_tests subdir local_libs full_libs excluded_tests) - # Subdirectory target for tests - add_custom_target(check.${subdir} COMMAND ${CMAKE_CTEST_COMMAND} -C $ --output-on-failure) - set(is_test TRUE) +# Enable make check (http://www.cmake.org/Wiki/CMakeEmulateMakeCheck) +if(GTSAM_BUILD_TESTS) + add_custom_target(check COMMAND ${CMAKE_CTEST_COMMAND} -C $ --output-on-failure) +endif() - # Put check target in Visual Studio solution folder - file(RELATIVE_PATH relative_path "${PROJECT_SOURCE_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}") - set_property(TARGET check.${subdir} PROPERTY FOLDER "${relative_path}") - - # Link with CppUnitLite - pulled from gtsam installation - list(APPEND local_libs CppUnitLite) - list(APPEND full_libs CppUnitLite) +# Add examples target +add_custom_target(examples) - # Build grouped tests - gtsam_add_grouped_scripts("${subdir}" # Use subdirectory as group label - "tests/test*.cpp" check "Test" # Standard for all tests - "${local_libs}" - "${full_libs}" "${excluded_tests}" # Pass in linking and exclusion lists - ${is_test}) # Set all as tests -endfunction() +# Include obsolete macros - will be removed in the near future +include(GtsamTestingObsolete) -# Macro for adding categorized timing scripts in a "tests" folder, with -# optional exclusion of tests and convenience library linking options -# -# By default, all tests are linked with boost -# Arguments: -# - subdir The name of the category for this timing script -# - local_libs A list of convenience libraries to use (if GTSAM_BUILD_CONVENIENCE_LIBRARIES is true) -# - full_libs The main library to link against if not using convenience libraries -# - excluded_srcs A list of timing files that should not be compiled - use for debugging -macro(gtsam_add_subdir_timing subdir local_libs full_libs excluded_srcs) - # Subdirectory target for timing - does not actually execute the scripts - add_custom_target(timing.${subdir}) - set(is_test FALSE) - # Build grouped benchmarks - gtsam_add_grouped_scripts("${subdir}" # Use subdirectory as group label - "tests/time*.cpp" timing "Timing Benchmark" # Standard for all timing scripts - "${local_libs}" "${full_libs}" "${excluded_srcs}" # Pass in linking and exclusion lists - ${is_test}) # Treat as not a test +# Implementations of this file's macros: + +macro(gtsamAddTestsGlob_impl groupName globPatterns excludedFiles linkLibraries) + if(GTSAM_BUILD_TESTS) + # Add group target if it doesn't already exist + if(NOT TARGET check.${groupName}) + add_custom_target(check.${groupName} COMMAND ${CMAKE_CTEST_COMMAND} -C $ --output-on-failure) + endif() + + # Get all script files + file(GLOB script_files ${globPatterns}) + + # Remove excluded scripts from the list + if(NOT "${excludedFiles}" STREQUAL "") + file(GLOB excludedFilePaths ${excludedFiles}) + if("${excludedFilePaths}" STREQUAL "") + message(WARNING "The pattern '${excludedFiles}' for excluding tests from group ${groupName} did not match any files") + else() + list(REMOVE_ITEM script_files ${excludedFilePaths}) + endif() + endif() + + # Separate into source files and headers (allows for adding headers to show up in + # MSVC and Xcode projects). + set(script_srcs "") + set(script_headers "") + foreach(script_file IN ITEMS ${script_files}) + get_filename_component(script_ext ${script_file} EXT) + if(script_ext MATCHES "(h|H)") + list(APPEND script_headers ${script_file}) + else() + list(APPEND script_srcs ${script_file}) + endif() + endforeach() + + # Don't put test files in folders in MSVC and Xcode because they're already grouped + source_group("" FILES ${script_srcs} ${script_headers}) + + if(NOT GTSAM_SINGLE_TEST_EXE) + # Default for Makefiles - each test in its own executable + foreach(script_src IN ITEMS ${script_srcs}) + # Get test base name + get_filename_component(script_name ${script_src} NAME_WE) + + # Add executable + add_executable(${script_name} ${script_src} ${script_headers}) + target_link_libraries(${script_name} CppUnitLite ${linkLibraries}) + + # Add target dependencies + add_test(NAME ${script_name} COMMAND ${script_name}) + add_dependencies(check.${groupName} ${script_name}) + add_dependencies(check ${script_name}) + if(NOT MSVC AND NOT XCODE_VERSION) + add_custom_target(${script_name}.run ${EXECUTABLE_OUTPUT_PATH}${script_name}) + endif() + + # Add TOPSRCDIR + set_property(SOURCE ${script_src} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"") + + # Exclude from 'make all' and 'make install' + set_target_properties(${script_name} PROPERTIES EXCLUDE_FROM_ALL ON) + + # Configure target folder (for MSVC and Xcode) + set_property(TARGET ${script_name} PROPERTY FOLDER "Unit tests/${groupName}") + endforeach() + else() + # Default on MSVC and XCode - combine test group into a single exectuable + set(target_name check_${groupName}_program) + + # Add executable + add_executable(${target_name} ${script_srcs} ${script_headers}) + target_link_libraries(${target_name} CppUnitLite ${linkLibraries}) + + # Only have a main function in one script - use preprocessor + set(rest_script_srcs ${script_srcs}) + list(REMOVE_AT rest_script_srcs 0) + set_property(SOURCE ${rest_script_srcs} APPEND PROPERTY COMPILE_DEFINITIONS "main=inline no_main") + + # Add target dependencies + add_test(NAME ${target_name} COMMAND ${target_name}) + add_dependencies(check.${groupName} ${target_name}) + add_dependencies(check ${target_name}) + + # Add TOPSRCDIR + set_property(SOURCE ${script_srcs} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"") + + # Exclude from 'make all' and 'make install' + set_target_properties(${target_name} PROPERTIES EXCLUDE_FROM_ALL ON) + + # Configure target folder (for MSVC and Xcode) + set_property(TARGET ${script_name} PROPERTY FOLDER "Unit tests") + endif() + endif() endmacro() -# Macro for adding executables matching a pattern - builds one executable for -# each file matching the pattern. These exectuables are automatically linked -# with boost. -# Arguments: -# - pattern The glob pattern to match source files -# - local_libs A list of convenience libraries to use (if GTSAM_BUILD_CONVENIENCE_LIBRARIES is true) -# - full_libs The main library to link against if not using convenience libraries -# - excluded_srcs A list of timing files that should not be compiled - use for debugging -function(gtsam_add_executables pattern local_libs full_libs excluded_srcs) - set(is_test FALSE) - - if(NOT excluded_srcs) - set(excluded_srcs "") - endif() - - # Build executables - gtsam_add_grouped_scripts("" "${pattern}" "" "Executable" "${local_libs}" "${full_libs}" "${excluded_srcs}" ${is_test}) -endfunction() -# General-purpose script for adding tests with categories and linking options -macro(gtsam_add_grouped_scripts group pattern target_prefix pretty_prefix_name local_libs full_libs excluded_srcs is_test) +macro(gtsamAddExamplesGlob_impl globPatterns excludedFiles linkLibraries) # Get all script files - set(script_files "") - foreach(one_pattern ${pattern}) - file(GLOB one_script_files "${one_pattern}") - list(APPEND script_files "${one_script_files}") - endforeach() + file(GLOB script_files ${globPatterns}) # Remove excluded scripts from the list - set(exclusions "") # Need to copy out exclusion list for logic to work - foreach(one_exclusion ${excluded_srcs}) - file(GLOB one_exclusion_srcs "${one_exclusion}") - list(APPEND exclusions "${one_exclusion_srcs}") - endforeach() - if(exclusions) - list(REMOVE_ITEM script_files ${exclusions}) - endif(exclusions) + if(NOT "${excludedFiles}" STREQUAL "") + file(GLOB excludedFilePaths ${excludedFiles}) + if("${excludedFilePaths}" STREQUAL "") + message(WARNING "The script exclusion pattern '${excludedFiles}' did not match any files") + else() + list(REMOVE_ITEM script_files ${excludedFilePaths}) + endif() + endif() - # Separate into source files and headers + # Separate into source files and headers (allows for adding headers to show up in + # MSVC and Xcode projects). set(script_srcs "") set(script_headers "") - foreach(script_file ${script_files}) + foreach(script_file IN ITEMS ${script_files}) get_filename_component(script_ext ${script_file} EXT) if(script_ext MATCHES "(h|H)") list(APPEND script_headers ${script_file}) @@ -113,94 +206,34 @@ macro(gtsam_add_grouped_scripts group pattern target_prefix pretty_prefix_name l list(APPEND script_srcs ${script_file}) endif() endforeach() - - - # Add targets and dependencies for each script - if(NOT "${group}" STREQUAL "") - message(STATUS "Adding ${pretty_prefix_name}s in ${group}") - endif() - - # Create exe's for each script, unless we're in SINGLE_TEST_EXE mode - if(NOT is_test OR NOT GTSAM_SINGLE_TEST_EXE) - foreach(script_src ${script_srcs}) - get_filename_component(script_base ${script_src} NAME_WE) - if (script_base) # Check for null filenames and headers - set( script_bin ${script_base} ) - message(STATUS "Adding ${pretty_prefix_name} ${script_bin}") - add_executable(${script_bin} ${script_src} ${script_headers}) - if(NOT "${target_prefix}" STREQUAL "") - if(NOT "${group}" STREQUAL "") - add_dependencies(${target_prefix}.${group} ${script_bin}) - endif() - add_dependencies(${target_prefix} ${script_bin}) - endif() - - # Add TOPSRCDIR - set_property(SOURCE ${script_src} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"") - # Disable building during make all/install - if (GTSAM_DISABLE_TESTS_ON_INSTALL) - set_target_properties(${script_bin} PROPERTIES EXCLUDE_FROM_ALL ON) - endif() - - if (is_test) - add_test(NAME ${script_base} COMMAND ${script_bin}) - endif() - - # Linking and dependendencies - if (GTSAM_BUILD_CONVENIENCE_LIBRARIES) - target_link_libraries(${script_bin} ${local_libs} ${GTSAM_BOOST_LIBRARIES}) - else() - target_link_libraries(${script_bin} ${full_libs} ${GTSAM_BOOST_LIBRARIES}) - endif() - - # Add .run target - if(NOT MSVC) - add_custom_target(${script_bin}.run ${EXECUTABLE_OUTPUT_PATH}${script_bin} ${ARGN}) - endif() - - # Set up Visual Studio folders - file(RELATIVE_PATH relative_path "${PROJECT_SOURCE_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}") - set_property(TARGET ${script_bin} PROPERTY FOLDER "${relative_path}") - endif() - endforeach(script_src) - - if(MSVC) - source_group("" FILES ${script_srcs} ${script_headers}) + # Don't put test files in folders in MSVC and Xcode because they're already grouped + source_group("" FILES ${script_srcs} ${script_headers}) + + # Create executables + foreach(script_src IN ITEMS ${script_srcs}) + # Get script base name + get_filename_component(script_name ${script_src} NAME_WE) + + # Add executable + add_executable(${script_name} ${script_src} ${script_headers}) + target_link_libraries(${script_name} ${linkLibraries}) + + # Add target dependencies + add_dependencies(examples ${script_name}) + if(NOT MSVC AND NOT XCODE_VERSION) + add_custom_target(${script_name}.run ${EXECUTABLE_OUTPUT_PATH}${script_name}) endif() - else() - # Create single unit test exe from all test scripts - set(script_bin ${target_prefix}_${group}_prog) - add_executable(${script_bin} ${script_srcs} ${script_headers}) - if (GTSAM_BUILD_CONVENIENCE_LIBRARIES) - target_link_libraries(${script_bin} ${local_libs} ${Boost_LIBRARIES}) - else() - target_link_libraries(${script_bin} ${Boost_LIBRARIES} ${full_libs}) - endif() - - # Only have a main function in one script - set(rest_script_srcs ${script_srcs}) - list(REMOVE_AT rest_script_srcs 0) - set_property(SOURCE ${rest_script_srcs} APPEND PROPERTY COMPILE_DEFINITIONS "main=static no_main") - + # Add TOPSRCDIR - set_property(SOURCE ${script_srcs} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"") - - # Add test - add_dependencies(${target_prefix}.${group} ${script_bin}) - add_dependencies(${target_prefix} ${script_bin}) - add_test(NAME ${target_prefix}.${group} COMMAND ${script_bin}) - - # Disable building during make all/install - if (GTSAM_DISABLE_TESTS_ON_INSTALL) - set_target_properties(${script_bin} PROPERTIES EXCLUDE_FROM_ALL ON) + set_property(SOURCE ${script_src} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"") + + if(NOT GTSAM_BUILD_EXAMPLES_ALWAYS) + # Exclude from 'make all' and 'make install' + set_target_properties(${target_name} PROPERTIES EXCLUDE_FROM_ALL ON) endif() - - # Set up Visual Studio folders - if(MSVC) - file(RELATIVE_PATH relative_path "${PROJECT_SOURCE_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}") - set_property(TARGET ${script_bin} PROPERTY FOLDER "${relative_path}") - source_group("" FILES ${script_srcs} ${script_headers}) - endif() - endif() + + # Configure target folder (for MSVC and Xcode) + set_property(TARGET ${script_name} PROPERTY FOLDER "Examples") + endforeach() endmacro() diff --git a/cmake/GtsamTestingObsolete.cmake b/cmake/GtsamTestingObsolete.cmake new file mode 100644 index 000000000..f56d138e6 --- /dev/null +++ b/cmake/GtsamTestingObsolete.cmake @@ -0,0 +1,195 @@ + +# Macro for adding categorized tests in a "tests" folder, with +# optional exclusion of tests and convenience library linking options +# +# By default, all tests are linked with CppUnitLite and boost +# Arguments: +# - subdir The name of the category for this test +# - local_libs A list of convenience libraries to use (if GTSAM_BUILD_CONVENIENCE_LIBRARIES is true) +# - full_libs The main library to link against if not using convenience libraries +# - excluded_tests A list of test files that should not be compiled - use for debugging +function(gtsam_add_subdir_tests subdir local_libs full_libs excluded_tests) + # Subdirectory target for tests + add_custom_target(check.${subdir} COMMAND ${CMAKE_CTEST_COMMAND} -C $ --output-on-failure) + set(is_test TRUE) + + # Put check target in Visual Studio solution folder + file(RELATIVE_PATH relative_path "${PROJECT_SOURCE_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}") + set_property(TARGET check.${subdir} PROPERTY FOLDER "${relative_path}") + + # Link with CppUnitLite - pulled from gtsam installation + list(APPEND local_libs CppUnitLite) + list(APPEND full_libs CppUnitLite) + + # Build grouped tests + gtsam_add_grouped_scripts("${subdir}" # Use subdirectory as group label + "tests/test*.cpp" check "Test" # Standard for all tests + "${local_libs}" + "${full_libs}" "${excluded_tests}" # Pass in linking and exclusion lists + ${is_test}) # Set all as tests +endfunction() + +# Macro for adding categorized timing scripts in a "tests" folder, with +# optional exclusion of tests and convenience library linking options +# +# By default, all tests are linked with boost +# Arguments: +# - subdir The name of the category for this timing script +# - local_libs A list of convenience libraries to use (if GTSAM_BUILD_CONVENIENCE_LIBRARIES is true) +# - full_libs The main library to link against if not using convenience libraries +# - excluded_srcs A list of timing files that should not be compiled - use for debugging +macro(gtsam_add_subdir_timing subdir local_libs full_libs excluded_srcs) + # Subdirectory target for timing - does not actually execute the scripts + add_custom_target(timing.${subdir}) + set(is_test FALSE) + + # Build grouped benchmarks + gtsam_add_grouped_scripts("${subdir}" # Use subdirectory as group label + "tests/time*.cpp" timing "Timing Benchmark" # Standard for all timing scripts + "${local_libs}" "${full_libs}" "${excluded_srcs}" # Pass in linking and exclusion lists + ${is_test}) # Treat as not a test +endmacro() + +# Macro for adding executables matching a pattern - builds one executable for +# each file matching the pattern. These exectuables are automatically linked +# with boost. +# Arguments: +# - pattern The glob pattern to match source files +# - local_libs A list of convenience libraries to use (if GTSAM_BUILD_CONVENIENCE_LIBRARIES is true) +# - full_libs The main library to link against if not using convenience libraries +# - excluded_srcs A list of timing files that should not be compiled - use for debugging +function(gtsam_add_executables pattern local_libs full_libs excluded_srcs) + set(is_test FALSE) + + if(NOT excluded_srcs) + set(excluded_srcs "") + endif() + + # Build executables + gtsam_add_grouped_scripts("" "${pattern}" "" "Executable" "${local_libs}" "${full_libs}" "${excluded_srcs}" ${is_test}) +endfunction() + +# General-purpose script for adding tests with categories and linking options +macro(gtsam_add_grouped_scripts group pattern target_prefix pretty_prefix_name local_libs full_libs excluded_srcs is_test) + # Print warning about using this obsolete function + message(AUTHOR_WARNING "Warning: Please see GtsamTesting.cmake - obsolete cmake cmake macro for creating unit tests, examples, and scripts was called. This will be removed in the future. The new macros are much easier anyway!!") + + # Get all script files + set(script_files "") + foreach(one_pattern ${pattern}) + file(GLOB one_script_files "${one_pattern}") + list(APPEND script_files "${one_script_files}") + endforeach() + + # Remove excluded scripts from the list + set(exclusions "") # Need to copy out exclusion list for logic to work + foreach(one_exclusion ${excluded_srcs}) + file(GLOB one_exclusion_srcs "${one_exclusion}") + list(APPEND exclusions "${one_exclusion_srcs}") + endforeach() + if(exclusions) + list(REMOVE_ITEM script_files ${exclusions}) + endif(exclusions) + + # Separate into source files and headers + set(script_srcs "") + set(script_headers "") + foreach(script_file ${script_files}) + get_filename_component(script_ext ${script_file} EXT) + if(script_ext MATCHES "(h|H)") + list(APPEND script_headers ${script_file}) + else() + list(APPEND script_srcs ${script_file}) + endif() + endforeach() + + + # Add targets and dependencies for each script + if(NOT "${group}" STREQUAL "") + message(STATUS "Adding ${pretty_prefix_name}s in ${group}") + endif() + + # Create exe's for each script, unless we're in SINGLE_TEST_EXE mode + if(NOT is_test OR NOT GTSAM_SINGLE_TEST_EXE) + foreach(script_src ${script_srcs}) + get_filename_component(script_base ${script_src} NAME_WE) + if (script_base) # Check for null filenames and headers + set( script_bin ${script_base} ) + message(STATUS "Adding ${pretty_prefix_name} ${script_bin}") + add_executable(${script_bin} ${script_src} ${script_headers}) + if(NOT "${target_prefix}" STREQUAL "") + if(NOT "${group}" STREQUAL "") + add_dependencies(${target_prefix}.${group} ${script_bin}) + endif() + add_dependencies(${target_prefix} ${script_bin}) + endif() + + # Add TOPSRCDIR + set_property(SOURCE ${script_src} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"") + + # Disable building during make all/install + if (GTSAM_DISABLE_TESTS_ON_INSTALL) + set_target_properties(${script_bin} PROPERTIES EXCLUDE_FROM_ALL ON) + endif() + + if (is_test) + add_test(NAME ${script_base} COMMAND ${script_bin}) + endif() + + # Linking and dependendencies + if (GTSAM_BUILD_CONVENIENCE_LIBRARIES) + target_link_libraries(${script_bin} ${local_libs} ${GTSAM_BOOST_LIBRARIES}) + else() + target_link_libraries(${script_bin} ${full_libs} ${GTSAM_BOOST_LIBRARIES}) + endif() + + # Add .run target + if(NOT MSVC AND NOT XCODE_VERSION) + add_custom_target(${script_bin}.run ${EXECUTABLE_OUTPUT_PATH}${script_bin} ${ARGN}) + endif() + + # Set up Visual Studio folders + file(RELATIVE_PATH relative_path "${PROJECT_SOURCE_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}") + set_property(TARGET ${script_bin} PROPERTY FOLDER "${relative_path}") + endif() + endforeach(script_src) + + if(MSVC) + source_group("" FILES ${script_srcs} ${script_headers}) + endif() + else() + # Create single unit test exe from all test scripts + set(script_bin ${target_prefix}_${group}_prog) + add_executable(${script_bin} ${script_srcs} ${script_headers}) + if (GTSAM_BUILD_CONVENIENCE_LIBRARIES) + target_link_libraries(${script_bin} ${local_libs} ${Boost_LIBRARIES}) + else() + target_link_libraries(${script_bin} ${Boost_LIBRARIES} ${full_libs}) + endif() + + # Only have a main function in one script + set(rest_script_srcs ${script_srcs}) + list(REMOVE_AT rest_script_srcs 0) + set_property(SOURCE ${rest_script_srcs} APPEND PROPERTY COMPILE_DEFINITIONS "main=static no_main") + + # Add TOPSRCDIR + set_property(SOURCE ${script_srcs} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"") + + # Add test + add_dependencies(${target_prefix}.${group} ${script_bin}) + add_dependencies(${target_prefix} ${script_bin}) + add_test(NAME ${target_prefix}.${group} COMMAND ${script_bin}) + + # Disable building during make all/install + if (GTSAM_DISABLE_TESTS_ON_INSTALL) + set_target_properties(${script_bin} PROPERTIES EXCLUDE_FROM_ALL ON) + endif() + + # Set up Visual Studio folders + if(MSVC) + file(RELATIVE_PATH relative_path "${PROJECT_SOURCE_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}") + set_property(TARGET ${script_bin} PROPERTY FOLDER "${relative_path}") + source_group("" FILES ${script_srcs} ${script_headers}) + endif() + endif() +endmacro() diff --git a/cmake/README.html b/cmake/README.html new file mode 100644 index 000000000..8170cd489 --- /dev/null +++ b/cmake/README.html @@ -0,0 +1,92 @@ +

GTSAMCMakeTools

+

This is the collection of GTSAM CMake tools that may be useful in external projects. The way to use this collection is by first making a find_package call:

+
find_package(GTSAMCMakeTools)
+
+

which will add a directory containing the GTSAM CMake tools to the CMAKE_MODULE_PATH variable. After that, you may include the files you would like to use. These files and the functions they define are explained below.

+

GtsamBuildTypes

+
include(GtsamBuildTypes)
+
+

Including this file immediately sets up the following build types and a drop-down list in cmake-gui:

+
    +
  • Debug
    • +
  • Release
    • +
  • RelWithDebInfo
    • +
  • Profiling: All optimizations enabled and minimal debug symbols
    • +
  • Timing: Defines the symbol GTSAM_ENABLE_TIMING for using GTSAM timing instrumentation
    • +
+

It also configures several minor details, as follows:

+
    +
  • The compile flag -ftemplate-depth=1024 is set for newer versions of Clang to handle complex templates.
    • +
  • On Windows, executable and dll output paths are set to ${CMAKE_BINARY_DIR}/bin and import library output to ${CMAKE_BINARY_DIR}/bin.
    • +
+

It defines the following functions:

+
    +
  • gtsam_assign_source_folders( [files] ) Organizes files in the IDE into folders to reflect the actual directory structure of those files. Folders will be determined relative to the current source folder when this function is called.
    • +
  • gtsam_assign_all_source_folders() Calls gtsam_assign_source_folders on all cpp, c, and h files recursively in the current source folder.
    • +
+

GtsamTesting

+
include(GtsamTesting)
+
+

Defines two useful functions for creating CTest unit tests. Also immediately creates a check target that builds and runs all unit tests.

+
    +
  • +

    gtsamAddTestsGlob(groupName globPatterns excludedFiles linkLibraries) Add a group of unit tests. A list of unit test .cpp files or glob patterns specifies the tests to create. Tests are assigned into a group name so they can easily by run independently with a make target. Running 'make check' builds and runs all tests.

    +

    Usage example:

    +
    gtsamAddTestsGlob(basic "test*.cpp" "testBroken.cpp" "gtsam;GeographicLib")
+
    +

    Arguments:

    +
    groupName:     A name that will allow this group of tests to be run independently, e.g.
+               'basic' causes a 'check.basic' target to be created to run this test
+               group.
+globPatterns:  The list of files or glob patterns from which to create unit tests, with
+               one test created for each cpp file.  e.g. "test*.cpp", or
+               "testA*.cpp;testB*.cpp;testOneThing.cpp".
+excludedFiles: A list of files or globs to exclude, e.g. "testC*.cpp;testBroken.cpp".
+               Pass an empty string "" if nothing needs to be excluded.
+linkLibraries: The list of libraries to link to in addition to CppUnitLite.
+
    +
    • +
  • +

    gtsamAddExamplesGlob(globPatterns excludedFiles linkLibraries) Add scripts that will serve as examples of how to use the library. A list of files or glob patterns is specified, and one executable will be created for each matching .cpp file. These executables will not be installed. They are build with 'make all' if GTSAM_BUILD_EXAMPLES_ALWAYS is enabled. They may also be built with 'make examples'.

    +

    Usage example:

    +
    gtsamAddExamplesGlob("*.cpp" "BrokenExample.cpp" "gtsam;GeographicLib")
+
    +

    Arguments:

    +
    globPatterns:  The list of files or glob patterns from which to create unit tests, with
+               one test created for each cpp file.  e.g. "*.cpp", or
+               "A*.cpp;B*.cpp;MyExample.cpp".
+excludedFiles: A list of files or globs to exclude, e.g. "C*.cpp;BrokenExample.cpp".  Pass
+               an empty string "" if nothing needs to be excluded.
+linkLibraries: The list of libraries to link to.
+
    +
    • +
+

GtsamMatlabWrap

+
include(GtsamMatlabWrap)
+
+

Defines functions for generating MATLAB wrappers. Also immediately creates several CMake options for configuring the wrapper.

+
    +
  • +

    wrap_and_install_library(interfaceHeader linkLibraries extraIncludeDirs extraMexFlags) Generates wrap code and compiles the wrapper.

    +

    Usage example:

    +
    `wrap_and_install_library("lba.h" "" "" "")`
+
    +

    Arguments:

    +
    interfaceHeader:  The relative or absolute path to the wrapper interface
+                  definition file.
+linkLibraries:    Any *additional* libraries to link.  Your project library
+                  (e.g. `lba`), libraries it depends on, and any necessary
+                  MATLAB libraries will be linked automatically.  So normally,
+                  leave this empty.
+extraIncludeDirs: Any *additional* include paths required by dependent
+                  libraries that have not already been added by
+                  include_directories.  Again, normally, leave this empty.
+extraMexFlags:    Any *additional* flags to pass to the compiler when building
+                  the wrap code.  Normally, leave this empty.
+
    +
    • +
+

GtsamMakeConfigFile

+
include(GtsamMakeConfigFile)
+
+

Defines a function for generating a config file so your project may be found with the CMake find_package function. TODO: Write documentation.

\ No newline at end of file diff --git a/cmake/README.md b/cmake/README.md new file mode 100644 index 000000000..34d1ffb52 --- /dev/null +++ b/cmake/README.md @@ -0,0 +1,105 @@ +GTSAMCMakeTools +=============== + +This is the collection of GTSAM CMake tools that may be useful in external projects. The way to use this collection is by first making a find_package call: + + find_package(GTSAMCMakeTools) + +which will add a directory containing the GTSAM CMake tools to the CMAKE_MODULE_PATH variable. After that, you may include the files you would like to use. These files and the functions they define are explained below. + +GtsamBuildTypes +--------------- + + include(GtsamBuildTypes) + +Including this file immediately sets up the following build types and a drop-down list in cmake-gui: + +* `Debug` +* `Release` +* `RelWithDebInfo` +* `Profiling`: All optimizations enabled and minimal debug symbols +* `Timing`: Defines the symbol GTSAM_ENABLE_TIMING for using GTSAM timing instrumentation + +It also configures several minor details, as follows: + +* The compile flag `-ftemplate-depth=1024` is set for newer versions of Clang to handle complex templates. +* On Windows, executable and dll output paths are set to `${CMAKE_BINARY_DIR}/bin` and import library output to `${CMAKE_BINARY_DIR}/lib`. + +It defines the following functions: + +* `gtsam_assign_source_folders( [files] )` Organizes files in the IDE into folders to reflect the actual directory structure of those files. Folders will be determined relative to the current source folder when this function is called. +* `gtsam_assign_all_source_folders()` Calls `gtsam_assign_source_folders` on all cpp, c, and h files recursively in the current source folder. + +GtsamTesting +------------ + + include(GtsamTesting) + +Defines two useful functions for creating CTest unit tests. Also immediately creates a `check` target that builds and runs all unit tests. + +* `gtsamAddTestsGlob(groupName globPatterns excludedFiles linkLibraries)` Add a group of unit tests. A list of unit test .cpp files or glob patterns specifies the tests to create. Tests are assigned into a group name so they can easily by run independently with a make target. Running 'make check' builds and runs all tests. + + Usage example: + + gtsamAddTestsGlob(basic "test*.cpp" "testBroken.cpp" "gtsam;GeographicLib") + + Arguments: + + groupName: A name that will allow this group of tests to be run independently, e.g. + 'basic' causes a 'check.basic' target to be created to run this test + group. + globPatterns: The list of files or glob patterns from which to create unit tests, with + one test created for each cpp file. e.g. "test*.cpp", or + "testA*.cpp;testB*.cpp;testOneThing.cpp". + excludedFiles: A list of files or globs to exclude, e.g. "testC*.cpp;testBroken.cpp". + Pass an empty string "" if nothing needs to be excluded. + linkLibraries: The list of libraries to link to in addition to CppUnitLite. + +* `gtsamAddExamplesGlob(globPatterns excludedFiles linkLibraries)` Add scripts that will serve as examples of how to use the library. A list of files or glob patterns is specified, and one executable will be created for each matching .cpp file. These executables will not be installed. They are build with 'make all' if GTSAM_BUILD_EXAMPLES_ALWAYS is enabled. They may also be built with 'make examples'. + + Usage example: + + gtsamAddExamplesGlob("*.cpp" "BrokenExample.cpp" "gtsam;GeographicLib") + + Arguments: + + globPatterns: The list of files or glob patterns from which to create unit tests, with + one test created for each cpp file. e.g. "*.cpp", or + "A*.cpp;B*.cpp;MyExample.cpp". + excludedFiles: A list of files or globs to exclude, e.g. "C*.cpp;BrokenExample.cpp". Pass + an empty string "" if nothing needs to be excluded. + linkLibraries: The list of libraries to link to. + +GtsamMatlabWrap +--------------- + + include(GtsamMatlabWrap) + +Defines functions for generating MATLAB wrappers. Also immediately creates several CMake options for configuring the wrapper. + +* `wrap_and_install_library(interfaceHeader linkLibraries extraIncludeDirs extraMexFlags)` Generates wrap code and compiles the wrapper. + + Usage example: + + `wrap_and_install_library("lba.h" "" "" "")` + + Arguments: + + interfaceHeader: The relative or absolute path to the wrapper interface + definition file. + linkLibraries: Any *additional* libraries to link. Your project library + (e.g. `lba`), libraries it depends on, and any necessary + MATLAB libraries will be linked automatically. So normally, + leave this empty. + extraIncludeDirs: Any *additional* include paths required by dependent + libraries that have not already been added by + include_directories. Again, normally, leave this empty. + extraMexFlags: Any *additional* flags to pass to the compiler when building + the wrap code. Normally, leave this empty. + +GtsamMakeConfigFile +------------------- + + include(GtsamMakeConfigFile) + +Defines a function for generating a config file so your project may be found with the CMake `find_package` function. TODO: Write documentation. \ No newline at end of file diff --git a/cmake/example_project/CMakeLists.txt b/cmake/example_project/CMakeLists.txt index 8dc124b2f..548d56acd 100644 --- a/cmake/example_project/CMakeLists.txt +++ b/cmake/example_project/CMakeLists.txt @@ -1,173 +1,43 @@ # This file should be used as a template for creating new projects using the CMake tools # This project has the following features # - GTSAM linking -# - Boost linking # - Unit tests via CppUnitLite -# - Automatic detection of sources and headers in subfolders -# - Installation of library and headers -# - Matlab wrap interface with within-project building -# - Use of GTSAM cmake macros +# - Scripts +# - Automatic MATLAB wrapper generation ################################################################################### -# To create your own project, replace "myproject" with the actual name of your project +# To create your own project, replace "example" with the actual name of your project cmake_minimum_required(VERSION 2.6) -enable_testing() -project(myproject CXX C) +project(example CXX C) -# Add the cmake subfolder to the cmake module path - necessary to use macros -set(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH}" "${PROJECT_SOURCE_DIR}/cmake") +# Include GTSAM CMake tools +find_package(GTSAMCMakeTools) +include(GtsamBuildTypes) # Load build type flags and default to Debug mode +include(GtsamTesting) # Easy functions for creating unit tests and scripts +include(GtsamMatlabWrap) # Automatic MATLAB wrapper generation # Ensure that local folder is searched before library folders include_directories(BEFORE "${PROJECT_SOURCE_DIR}") -# Load build type flags and default to Debug mode -include(GtsamBuildTypes) - ################################################################################### -# Create a list of library dependencies -# These will be linked with executables -set(library_deps "") -set(linking_mode "static") - # Find GTSAM components find_package(GTSAM REQUIRED) # Uses installed package -list(APPEND library_deps gtsam-${linking_mode} gtsam_unstable-${linking_mode}) - -# Include ransac -find_package(ransac REQUIRED) # Uses installed package -list(APPEND library_deps ransac-${linking_mode}) - -# Boost - same requirement as gtsam -find_package(Boost 1.43 COMPONENTS - serialization - system - filesystem - thread - date_time - REQUIRED) -list(APPEND library_deps - ${Boost_SERIALIZATION_LIBRARY} - ${Boost_SYSTEM_LIBRARY} - ${Boost_FILESYSTEM_LIBRARY} - ${Boost_THREAD_LIBRARY} - ${Boost_DATE_TIME_LIBRARY}) - -include_directories(${Boost_INCLUDE_DIR} ${GTSAM_INCLUDE_DIR} ${ransac_INCLUDE_DIR}) +include_directories(${GTSAM_INCLUDE_DIR}) ################################################################################### -# List subdirs to process tests/sources -# Each of these will be scanned for new files -set (myproject_subdirs - "." # ensure root folder gets included - stuff - things - ) - -# loop through subdirs to install and build up source lists -set(myproject_lib_source "") -set(myproject_tests_source "") -set(myproject_scripts_source "") -foreach(subdir ${myproject_subdirs}) - # Installing headers - message(STATUS "Installing ${subdir}") - file(GLOB sub_myproject_headers "myproject/${subdir}/*.h") - install(FILES ${sub_myproject_headers} DESTINATION include/myproject/${subdir}) - - # add sources to main sources list - file(GLOB subdir_srcs "myproject/${subdir}/*.cpp") - list(APPEND myproject_lib_source ${subdir_srcs}) - - # add tests to main tests list - file(GLOB subdir_test_srcs "myproject/${subdir}/tests/*.cpp") - list(APPEND myproject_tests_source ${subdir_test_srcs}) - - # add scripts to main tests list - file(GLOB subdir_scripts_srcs "myproject/${subdir}/scripts/*.cpp") - list(APPEND myproject_scripts_source ${subdir_scripts_srcs}) -endforeach(subdir) - -set(myproject_version ${myproject_VERSION_MAJOR}.${myproject_VERSION_MINOR}.${myproject_VERSION_PATCH}) -set(myproject_soversion ${myproject_VERSION_MAJOR}) -message(STATUS "GTSAM Version: ${gtsam_version}") -message(STATUS "Install prefix: ${CMAKE_INSTALL_PREFIX}") - -# Build library (static and shared versions) -# Include installed versions -SET(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) -add_library(${PROJECT_NAME}-shared SHARED ${myproject_lib_source}) -set_target_properties(${PROJECT_NAME}-shared PROPERTIES - OUTPUT_NAME ${PROJECT_NAME} - CLEAN_DIRECT_OUTPUT 1) -install(TARGETS myproject-shared EXPORT myproject-exports LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin) -list(APPEND myproject_EXPORTED_TARGETS myproject-shared) - -add_library(${PROJECT_NAME}-static STATIC ${myproject_lib_source}) -set_target_properties(${PROJECT_NAME}-static PROPERTIES - OUTPUT_NAME ${PROJECT_NAME} - CLEAN_DIRECT_OUTPUT 1) -install(TARGETS myproject-static EXPORT myproject-exports ARCHIVE DESTINATION lib) -list(APPEND myproject_EXPORTED_TARGETS myproject-static) - -install(TARGETS ${PROJECT_NAME}-shared LIBRARY DESTINATION lib ) -install(TARGETS ${PROJECT_NAME}-static ARCHIVE DESTINATION lib ) - -# Disabled tests - subtract these from the test files -# Note the need for a full path -set(disabled_tests - "dummy" - #"${PROJECT_SOURCE_DIR}/myproject/geometry/tests/testCovarianceEllipse.cpp" -) -list(REMOVE_ITEM myproject_tests_source ${disabled_tests}) +# Build static library from common sources +set(CONVENIENCE_LIB_NAME ${PROJECT_NAME}) +add_library(${CONVENIENCE_LIB_NAME} STATIC example/PrintExamples.h example/PrintExamples.cpp) +target_link_libraries(${CONVENIENCE_LIB_NAME} gtsam) ################################################################################### -# Build tests -add_custom_target(check COMMAND ${CMAKE_CTEST_COMMAND}) -foreach(test_src_file ${myproject_tests_source}) - get_filename_component(test_base ${test_src_file} NAME_WE) - message(STATUS "Adding test ${test_src_file} with base name ${test_base}" ) - add_executable(${test_base} ${test_src_file}) - target_link_libraries(${test_base} ${PROJECT_NAME}-${linking_mode} ${library_deps} CppUnitLite) - add_test(${test_base} ${EXECUTABLE_OUTPUT_PATH}/${test_base}) - add_custom_target(${test_base}.run ${test_base} ${ARGN}) - add_dependencies(check ${test_base}) -endforeach(test_src_file) - -# Build scripts -foreach(script_src_file ${myproject_scripts_source}) - get_filename_component(script_base ${script_src_file} NAME_WE) - message(STATUS "Adding script ${script_src_file} with base name ${script_base}" ) - add_executable(${script_base} ${script_src_file}) - target_link_libraries(${script_base} ${PROJECT_NAME}-${linking_mode} ${library_deps} CppUnitLite) - add_custom_target(${script_base}.run ${script_base} ${ARGN}) -endforeach(script_src_file) +# Build tests (CMake tracks the dependecy to link with GTSAM through our project's static library) +gtsamAddTestsGlob("example" "tests/test*.cpp" "" "${CONVENIENCE_LIB_NAME}") ################################################################################### -# Matlab wrapping -include(GtsamMatlabWrap) -set(MEX_COMMAND "mex" CACHE STRING "Command to use for executing mex (if on path, 'mex' will work)") -set(GTSAM_BUILD_MEX_BINARY_FLAGS "" CACHE STRING "Extra flags for running Matlab MEX compilation") -set(MYPROJECT_TOOLBOX_DIR "../matlab/myproject" CACHE PATH "Install folder for matlab toolbox - defaults to inside project") -set(WRAP_HEADER_PATH "${GTSAM_DIR}/../../../include") -set(MYPROJECT_TOOLBOX_FLAGS - ${GTSAM_BUILD_MEX_BINARY_FLAGS} -I${PROJECT_SOURCE_DIR} -I${PROJECT_SOURCE_DIR}/myproject -I${Boost_INCLUDE_DIR} -I${MEX_INCLUDE_ROOT} -I${GTSAM_INCLUDE_DIR} -I${WRAP_HEADER_PATH} -Wl,-rpath,${CMAKE_BINARY_DIR}:${CMAKE_INSTALL_PREFIX}/lib) -set(MYPROJECT_LIBRARY_DEPS gtsam gtsam_unstable ransac myproject) -set(GTSAM_BUILD_MEX_BIN ON) - -# Function to setup codegen, building and installation of the wrap toolbox -# This wrap setup function assumes that the toolbox will be installed directly, -# with predictable matlab.h sourcing -# params: -# moduleName : the name of the module, interface file must be called moduleName.h -# mexFlags : Compilation flags to be passed to the mex compiler -# modulePath : relative path to module markup header file (called moduleName.h) -# otherLibraries : list of library targets this should depend on -# toolboxPath : the directory in which to generate/build wrappers -# wrap_header_path : path to the installed wrap header -wrap_library_generic(myproject "${MYPROJECT_TOOLBOX_FLAGS}" "" "${MYPROJECT_LIBRARY_DEPS}" "${MYPROJECT_TOOLBOX_DIR}" "${WRAP_HEADER_PATH}") +# Build scripts (CMake tracks the dependecy to link with GTSAM through our project's static library) +gtsamAddExamplesGlob("*.cpp" "" "${CONVENIENCE_LIB_NAME}") ################################################################################### -# Create Install config and export files -# This config file takes the place of FindXXX.cmake scripts -include(GtsamMakeConfigFile) -GtsamMakeConfigFile(myproject) -export(TARGETS ${myproject_EXPORTED_TARGETS} FILE myproject-exports.cmake) \ No newline at end of file +# Build MATLAB wrapper (CMake tracks the dependecy to link with GTSAM through our project's static library) +wrap_and_install_library("example.h" "${CONVENIENCE_LIB_NAME}" "" "") diff --git a/gtsam/inference/SymbolicMultifrontalSolver.cpp b/cmake/example_project/SayGoodbye.cpp similarity index 56% rename from gtsam/inference/SymbolicMultifrontalSolver.cpp rename to cmake/example_project/SayGoodbye.cpp index 7072954a9..be1165ef6 100644 --- a/gtsam/inference/SymbolicMultifrontalSolver.cpp +++ b/cmake/example_project/SayGoodbye.cpp @@ -10,16 +10,14 @@ * -------------------------------------------------------------------------- */ /** - * @file SymbolicMultifrontalSolver.cpp - * @author Richard Roberts - * @date Oct 22, 2010 + * @file SayGoodbye.cpp + * @brief Example script for example project + * @author Richard Roberts */ -#include -#include - -namespace gtsam { - -//template class GenericMultifrontalSolver > >; +#include +int main(int argc, char *argv[]) { + example::PrintExamples().sayGoodbye(); + return 0; } diff --git a/cmake/example_project/SayHello.cpp b/cmake/example_project/SayHello.cpp new file mode 100644 index 000000000..2da06ab32 --- /dev/null +++ b/cmake/example_project/SayHello.cpp @@ -0,0 +1,23 @@ +/* ---------------------------------------------------------------------------- + + * 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 SayHello.cpp + * @brief Example script for example project + * @author Richard Roberts + */ + +#include + +int main(int argc, char *argv[]) { + example::PrintExamples().sayHello(); + return 0; +} diff --git a/gtsam/inference/SymbolicSequentialSolver.cpp b/cmake/example_project/example.h similarity index 54% rename from gtsam/inference/SymbolicSequentialSolver.cpp rename to cmake/example_project/example.h index 937623886..47b9a0aa2 100644 --- a/gtsam/inference/SymbolicSequentialSolver.cpp +++ b/cmake/example_project/example.h @@ -10,17 +10,19 @@ * -------------------------------------------------------------------------- */ /** - * @file SymbolicSequentialSolver.cpp - * @author Richard Roberts - * @date Oct 21, 2010 + * @file example.h + * @brief Example wrapper interface file + * @author Richard Roberts */ -#include -#include +// This is an interface file for automatic MATLAB wrapper generation. See +// gtsam.h for full documentation and more examples. -namespace gtsam { +namespace example { -// An explicit instantiation to be compiled into the library -//template class GenericSequentialSolver; +class PrintExamples { + void sayHello() const; + void sayGoodbye() const; +}; } diff --git a/cmake/example_project/example/PrintExamples.cpp b/cmake/example_project/example/PrintExamples.cpp new file mode 100644 index 000000000..1e9f10713 --- /dev/null +++ b/cmake/example_project/example/PrintExamples.cpp @@ -0,0 +1,44 @@ +/* ---------------------------------------------------------------------------- + + * 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 print_examples.cpp + * @brief Example library file + * @author Richard Roberts + */ + +#include + +#include + +namespace example { + +void PrintExamples::sayHello() const { + std::cout << internal::getHelloString() << std::endl; +} + +void PrintExamples::sayGoodbye() const { + std::cout << internal::getGoodbyeString() << std::endl; +} + +namespace internal { + +std::string getHelloString() { + return "Hello!"; +} + +std::string getGoodbyeString() { + return "See you soon!"; +} + +} // namespace internal + +} // namespace example diff --git a/cmake/example_project/example/PrintExamples.h b/cmake/example_project/example/PrintExamples.h new file mode 100644 index 000000000..b151dfbc7 --- /dev/null +++ b/cmake/example_project/example/PrintExamples.h @@ -0,0 +1,41 @@ +/* ---------------------------------------------------------------------------- + + * 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 print_examples.h + * @brief Example library file + * @author Richard Roberts + */ + +#pragma once + +#include + +namespace example { + +class PrintExamples { +public: + /// Print a greeting + void sayHello() const; + + /// Print a farewell + void sayGoodbye() const; +}; + +namespace internal { + +std::string getHelloString(); + +std::string getGoodbyeString(); + +} // namespace internal + +} // namespace example diff --git a/cmake/example_project/tests/testExample.cpp b/cmake/example_project/tests/testExample.cpp new file mode 100644 index 000000000..c2a5a173b --- /dev/null +++ b/cmake/example_project/tests/testExample.cpp @@ -0,0 +1,43 @@ +/* ---------------------------------------------------------------------------- + + * 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 testExample.cpp + * @brief Unit tests for example + * @author Richard Roberts + */ + +#include + +#include + +#include + +using namespace gtsam; + +TEST(Example, HelloString) { + const std::string expectedString = "Hello!"; + EXPECT(assert_equal(expectedString, example::internal::getHelloString())); +} + +TEST(Example, GoodbyeString) { + const std::string expectedString = "See you soon!"; + EXPECT(assert_equal(expectedString, example::internal::getGoodbyeString())); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + + diff --git a/cmake/example_project_simple/CMakeLists.txt b/cmake/example_project_simple/CMakeLists.txt deleted file mode 100644 index e8bea909c..000000000 --- a/cmake/example_project_simple/CMakeLists.txt +++ /dev/null @@ -1,38 +0,0 @@ -# This file should be used as a template for creating new projects using the CMake tools -# This project has the following features -# - GTSAM linking -# - Unit tests via CppUnitLite -# - Scripts - -################################################################################### -# To create your own project, replace "myproject" with the actual name of your project -cmake_minimum_required(VERSION 2.6) -enable_testing() -project(myproject CXX C) - -# Add the cmake subfolder to the cmake module path - necessary to use macros -list(APPEND CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake") - -# Ensure that local folder is searched before library folders -include_directories(BEFORE "${PROJECT_SOURCE_DIR}") - -# Load build type flags and default to Debug mode -include(GtsamBuildTypes) - -################################################################################### -# Find GTSAM components -find_package(GTSAM REQUIRED) # Uses installed package -include_directories(${GTSAM_INCLUDE_DIR}) - -################################################################################### -# Build static library from common sources -add_library(${PROJECT_NAME} STATIC ${PROJECT_NAME}/MySourceFiles.cpp) -target_link_libraries(${PROJECT_NAME} gtsam-shared) - -################################################################################### -# Build tests (CMake tracks the dependecy to link with GTSAM through our project's static library) -gtsam_add_subdir_tests(${PROJECT_NAME} "${PROJECT_NAME}" "${PROJECT_NAME}" "") - -################################################################################### -# Build scripts (CMake tracks the dependecy to link with GTSAM through our project's static library) -gtsam_add_executables("${PROJECT_NAME}/myScripts.cpp" "${PROJECT_NAME}" "${PROJECT_NAME}" "") diff --git a/doc/.gitignore b/doc/.gitignore deleted file mode 100644 index a7cbee1ba..000000000 --- a/doc/.gitignore +++ /dev/null @@ -1 +0,0 @@ -/*.DS_Store diff --git a/doc/CodingGuidelines.docx b/doc/CodingGuidelines.docx new file mode 100644 index 000000000..d466b1ed4 Binary files /dev/null and b/doc/CodingGuidelines.docx differ diff --git a/doc/CodingGuidelines.lyx b/doc/CodingGuidelines.lyx new file mode 100644 index 000000000..6d0b6eb1e --- /dev/null +++ b/doc/CodingGuidelines.lyx @@ -0,0 +1,481 @@ +#LyX 2.0 created this file. For more info see http://www.lyx.org/ +\lyxformat 413 +\begin_document +\begin_header +\textclass article +\begin_preamble +\usepackage{color} + +\definecolor{mygreen}{rgb}{0,0.6,0} +\definecolor{mygray}{rgb}{0.5,0.5,0.5} +\definecolor{mymauve}{rgb}{0.58,0,0.82} + +\lstset{ % + backgroundcolor=\color{white}, % choose the background color; you must add \usepackage{color} or \usepackage{xcolor} + basicstyle=\footnotesize, % the size of the fonts that are used for the code + breakatwhitespace=false, % sets if automatic breaks should only happen at whitespace + breaklines=true, % sets automatic line breaking + captionpos=b, % sets the caption-position to bottom + commentstyle=\color{mygreen}, % comment style +% deletekeywords={...}, % if you want to delete keywords from the given language + escapeinside={\%*}{*)}, % if you want to add LaTeX within your code + extendedchars=true, % lets you use non-ASCII characters; for 8-bits encodings only, does not work with UTF-8 + frame=single, % adds a frame around the code + keepspaces=true, % keeps spaces in text, useful for keeping indentation of code (possibly needs columns=flexible) + keywordstyle=\color{blue}, % keyword style + language=C++, % the language of the code + morekeywords={*,...}, % if you want to add more keywords to the set + numbers=left, % where to put the line-numbers; possible values are (none, left, right) + numbersep=5pt, % how far the line-numbers are from the code + numberstyle=\tiny\color{mygray}, % the style that is used for the line-numbers + rulecolor=\color{black}, % if not set, the frame-color may be changed on line-breaks within not-black text (e.g. comments (green here)) + showspaces=false, % show spaces everywhere adding particular underscores; it overrides 'showstringspaces' + showstringspaces=false, % underline spaces within strings only + showtabs=false, % show tabs within strings adding particular underscores + stepnumber=2, % the step between two line-numbers. If it's 1, each line will be numbered + stringstyle=\color{mymauve}, % string literal style + tabsize=2, % sets default tabsize to 2 spaces + title=\lstname % show the filename of files included with \lstinputlisting; also try caption instead of title +} +\end_preamble +\use_default_options true +\maintain_unincluded_children false +\language english +\language_package default +\inputencoding auto +\fontencoding global +\font_roman lmodern +\font_sans lmss +\font_typewriter lmtt +\font_default_family default +\use_non_tex_fonts false +\font_sc false +\font_osf false +\font_sf_scale 100 +\font_tt_scale 100 + +\graphics default +\default_output_format default +\output_sync 0 +\bibtex_command default +\index_command default +\paperfontsize default +\spacing single +\use_hyperref false +\papersize default +\use_geometry false +\use_amsmath 1 +\use_esint 1 +\use_mhchem 1 +\use_mathdots 1 +\cite_engine basic +\use_bibtopic false +\use_indices false +\paperorientation portrait +\suppress_date false +\use_refstyle 1 +\index Index +\shortcut idx +\color #008000 +\end_index +\secnumdepth 3 +\tocdepth 3 +\paragraph_separation indent +\paragraph_indentation default +\quotes_language english +\papercolumns 1 +\papersides 1 +\paperpagestyle default +\tracking_changes false +\output_changes false +\html_math_output 0 +\html_css_as_file 0 +\html_be_strict false +\end_header + +\begin_body + +\begin_layout Section +Template Classes +\end_layout + +\begin_layout Standard +Templated classes are great for writing generic code for multiple types + (e.g. + the same elimination algorithm code for symbolic, discrete, and Gaussian + elimination) without the drawbacks of virtual inheritance (which include + rigid class interfaces, downcasting from returned base class pointers, + and additional runtime overhead). + Depending on how they're used, though, templates can result in very slow + compile times, large binary files, and hard-to-use code. + This section describes the +\begin_inset Quotes eld +\end_inset + +best practices +\begin_inset Quotes erd +\end_inset + + we have developed for gaining the benefits of templates without the drawbacks. +\end_layout + +\begin_layout Standard +If you need to write generic code or classes, here are several programming + patterns we have found to work very well: +\end_layout + +\begin_layout Subsection +The +\begin_inset Quotes eld +\end_inset + +Templated Base, Specialized Derived +\begin_inset Quotes erd +\end_inset + + Pattern +\end_layout + +\begin_layout Standard +This pattern is for when you have a generic class containing algorithm or + data structure code that will be specialized to several types. + The templated base class should never be used directly, instead only the + specializations should be used. + Some specialized types can be pre-compiled into the library, but the option + remains to specialize new types in external libraries or projects. +\end_layout + +\begin_layout Subsubsection +Basic Class Structure +\end_layout + +\begin_layout Standard +We'll use +\family typewriter +FactorGraph +\family default + as an example. + It is templated on the factor type stored in it and has several specializations. + The templated base class +\family typewriter +FactorGraph +\family default + is divided into a header file ( +\family typewriter +.h +\family default +) and an +\begin_inset Quotes eld +\end_inset + +instantiation +\begin_inset Quotes erd +\end_inset + + file ( +\family typewriter +-inst.h +\family default +). + The basic class structure is as follows. +\begin_inset listings +lstparams "basicstyle={\scriptsize\ttfamily},language={C++}" +inline false +status open + +\begin_layout Plain Layout + +// File FactorGraph.h +\end_layout + +\begin_layout Plain Layout + +\end_layout + +\begin_layout Plain Layout + +%* +\backslash +bfseries{ +\backslash +emph{ +\backslash +color{red}{// Include a minimal set of headers. + Do not include any '-inst.h' files (this is the key to fast compiles).}}}*) +\end_layout + +\begin_layout Plain Layout + +#include +\end_layout + +\begin_layout Plain Layout + +... +\end_layout + +\begin_layout Plain Layout + +\end_layout + +\begin_layout Plain Layout + +namespace gtsam { +\end_layout + +\begin_layout Plain Layout + + /** Class description */ +\end_layout + +\begin_layout Plain Layout + + template +\end_layout + +\begin_layout Plain Layout + + class FactorGraph +\end_layout + +\begin_layout Plain Layout + + { +\end_layout + +\begin_layout Plain Layout + + %* +\backslash +bfseries{ +\backslash +emph{ +\backslash +color{red}{// Make 'private' any typedefs that must be redefined in derived + classes. + E.g. + 'This' in the context of the derived class should refer to the derived + class. + These typedefs will be used only by the generic code in this base class.}}}*) +\end_layout + +\begin_layout Plain Layout + + private: +\end_layout + +\begin_layout Plain Layout + + typedef FactorGraph This; ///< Typedef for this class +\end_layout + +\begin_layout Plain Layout + + typedef boost::shared_ptr shared_ptr; ///< Shared pointer to + this +\end_layout + +\begin_layout Plain Layout + + +\end_layout + +\begin_layout Plain Layout + + %* +\backslash +bfseries{ +\backslash +emph{ +\backslash +color{red}{// Make 'public' the typedefs that will be valid in the derived + class.}}}*) +\end_layout + +\begin_layout Plain Layout + + public: +\end_layout + +\begin_layout Plain Layout + + typedef FACTOR FactorType; ///< Factor type stored in this graph +\end_layout + +\begin_layout Plain Layout + + typedef boost::shared_ptr sharedFactor; ///< Shared pointer + to a factor +\end_layout + +\begin_layout Plain Layout + + ... +\end_layout + +\begin_layout Plain Layout + +\end_layout + +\begin_layout Plain Layout + + %* +\backslash +bfseries{ +\backslash +emph{ +\backslash +color{red}{// Normally, data is 'protected' so the derived class can access + it.}}}*) +\end_layout + +\begin_layout Plain Layout + + protected: +\end_layout + +\begin_layout Plain Layout + + /** Collection of factors */ +\end_layout + +\begin_layout Plain Layout + + std::vector factors_; +\end_layout + +\begin_layout Plain Layout + +\end_layout + +\begin_layout Plain Layout + + %* +\backslash +bfseries{ +\backslash +emph{ +\backslash +color{red}{// Make 'protected' all constructors, named constructors, or + methods returning the base class type. + These are not public - the derived class will call them and properly convert + returned base classes to the derived class.}}}*) +\end_layout + +\begin_layout Plain Layout + + /// @name Standard Constructors +\end_layout + +\begin_layout Plain Layout + + /// @{ +\end_layout + +\begin_layout Plain Layout + +\end_layout + +\begin_layout Plain Layout + + /** Default constructor */ +\end_layout + +\begin_layout Plain Layout + + FactorGraphUnordered() {} +\end_layout + +\begin_layout Plain Layout + +\end_layout + +\begin_layout Plain Layout + + /** Named constructor from iterator over factors */ +\end_layout + +\begin_layout Plain Layout + + template +\end_layout + +\begin_layout Plain Layout + + static This FromIterator(ITERATOR firstFactor, ITERATOR lastFactor); +\end_layout + +\begin_layout Plain Layout + + /// @} +\end_layout + +\begin_layout Plain Layout + +\end_layout + +\begin_layout Plain Layout + + %* +\backslash +bfseries{ +\backslash +emph{ +\backslash +color{red}{// Make 'public' standard methods that will be available in the + derived class's API.}}}*) +\end_layout + +\begin_layout Plain Layout + + public: +\end_layout + +\begin_layout Plain Layout + + /// @name Adding Factors +\end_layout + +\begin_layout Plain Layout + + /// @{ +\end_layout + +\begin_layout Plain Layout + + /** ... + */ +\end_layout + +\begin_layout Plain Layout + + void reserve(size_t size); +\end_layout + +\begin_layout Plain Layout + + ... +\end_layout + +\begin_layout Plain Layout + + /// @} +\end_layout + +\begin_layout Plain Layout + + }; +\end_layout + +\begin_layout Plain Layout + +} +\end_layout + +\end_inset + + +\end_layout + +\end_body +\end_document diff --git a/doc/Doxyfile.in b/doc/Doxyfile.in index f12e12217..d5c969a8a 100644 --- a/doc/Doxyfile.in +++ b/doc/Doxyfile.in @@ -1554,7 +1554,7 @@ TAGFILES = # When a file name is specified after GENERATE_TAGFILE, doxygen will create # a tag file that is based on the input files it reads. -GENERATE_TAGFILE = +GENERATE_TAGFILE = html/gtsam.tag # If the ALLEXTERNALS tag is set to YES all external classes will be listed # in the class index. If set to NO only the inherited external classes diff --git a/doc/LieGroups.lyx b/doc/LieGroups.lyx index 4af039252..adf62314c 100644 --- a/doc/LieGroups.lyx +++ b/doc/LieGroups.lyx @@ -3037,10 +3037,10 @@ key "Murray94book" \color none \begin_inset Formula \[ -\exp\left(\left[\begin{array}{c} +\exp\left(\widehat{\left[\begin{array}{c} \omega\\ v -\end{array}\right]t\right)=\left[\begin{array}{cc} +\end{array}\right]}t\right)=\left[\begin{array}{cc} e^{\Skew{\omega}t} & (I-e^{\Skew{\omega}t})\left(\omega\times v\right)+\omega\omega^{T}vt\\ 0 & 1 \end{array}\right] diff --git a/doc/gtsam-coordinate-frames.lyx b/doc/gtsam-coordinate-frames.lyx new file mode 100644 index 000000000..33d0dd977 --- /dev/null +++ b/doc/gtsam-coordinate-frames.lyx @@ -0,0 +1,2527 @@ +#LyX 2.0 created this file. For more info see http://www.lyx.org/ +\lyxformat 413 +\begin_document +\begin_header +\textclass article +\use_default_options true +\maintain_unincluded_children false +\language english +\language_package default +\inputencoding auto +\fontencoding global +\font_roman times +\font_sans helvet +\font_typewriter lmtt +\font_default_family default +\use_non_tex_fonts false +\font_sc false +\font_osf false +\font_sf_scale 100 +\font_tt_scale 100 + +\graphics default +\default_output_format default +\output_sync 0 +\bibtex_command default +\index_command default +\paperfontsize default +\spacing single +\use_hyperref false +\papersize default +\use_geometry false +\use_amsmath 1 +\use_esint 1 +\use_mhchem 1 +\use_mathdots 1 +\cite_engine basic +\use_bibtopic false +\use_indices false +\paperorientation portrait +\suppress_date false +\use_refstyle 1 +\index Index +\shortcut idx +\color #008000 +\end_index +\secnumdepth 3 +\tocdepth 3 +\paragraph_separation indent +\paragraph_indentation default +\quotes_language english +\papercolumns 1 +\papersides 1 +\paperpagestyle default +\tracking_changes false +\output_changes false +\html_math_output 0 +\html_css_as_file 0 +\html_be_strict false +\end_header + +\begin_body + +\begin_layout Standard +\begin_inset FormulaMacro +\newcommand{\SE}[1]{\mathbb{SE}\left(#1\right)} +{\mathbb{SE}\left(#1\right)} +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset FormulaMacro +\newcommand{\se}[1]{\mathfrak{se}\left(#1\right)} +{\mathfrak{se}\left(#1\right)} +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset FormulaMacro +\newcommand{\R}[1]{\mathbb{R}^{#1}} +{\mathbb{R}^{#1}} +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset FormulaMacro +\newcommand{\norm}[1]{\left\Vert #1\right\Vert } +{\left\Vert #1\right\Vert } +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset FormulaMacro +\newcommand{\t}{\mathsf{T}} +{\mathsf{T}} +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset FormulaMacro +\newcommand{\lin}[1]{\overset{{\scriptscriptstyle \circ}}{#1}} +{\overset{{\scriptscriptstyle \circ}}{#1}} +\end_inset + + +\end_layout + +\begin_layout Section +Introduction +\end_layout + +\begin_layout Standard +This document describes the coordinate frame conventions in which GTSAM + inputs and represents states and uncertainties. + When specifying initial conditions, measurements and their uncertainties, + and interpreting estimated uncertainties and the results of geometry operations +, the coordinate frame convention comes into play. +\end_layout + +\begin_layout Standard +GTSAM as consistently as possible represents all states and uncertainties + in the body frame. + In cases where several frames are used simultaneously, a good rule of thumb + is that measurements and uncertainties will be represented in the +\begin_inset Quotes eld +\end_inset + +last +\begin_inset Quotes erd +\end_inset + + frame of the series. +\end_layout + +\begin_layout Section +Frame Conventions in Geometry, Lie Group, and Manifold Operations +\end_layout + +\begin_layout Standard +\begin_inset Float table +wide false +sideways false +status open + +\begin_layout Plain Layout +\align center + +\size footnotesize +\begin_inset Tabular + + + + + + + + +\begin_inset Text + +\begin_layout Plain Layout + +\series bold +\size footnotesize +Syntax +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\series bold +\size footnotesize +Input +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\series bold +\size footnotesize +Output +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\series bold +\size footnotesize +Identities +\end_layout + +\end_inset + + + + +\begin_inset Text + +\begin_layout Plain Layout + +\size footnotesize +\begin_inset space \quad{} +\end_inset + + +\series bold +Lie group operations +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\end_layout + +\end_inset + + + + +\begin_inset Text + +\begin_layout Plain Layout + +\size footnotesize +\begin_inset Formula $c=a.\mathbf{compose}\left(b\right)$ +\end_inset + + +\begin_inset Newline newline +\end_inset + + +\begin_inset Formula $c=a\mathbf{*}b$ +\end_inset + + +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\size footnotesize +\begin_inset Formula $a$ +\end_inset + + in +\begin_inset Formula $g$ +\end_inset + + +\begin_inset Newline newline +\end_inset + + +\begin_inset Formula $b$ +\end_inset + + in +\begin_inset Formula $a$ +\end_inset + + +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\size footnotesize +\begin_inset Formula $c=b$ +\end_inset + + in +\begin_inset Formula $g$ +\end_inset + + +\begin_inset Newline newline +\end_inset + + +\begin_inset Formula $c=ab$ +\end_inset + + +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\end_layout + +\end_inset + + + + +\begin_inset Text + +\begin_layout Plain Layout + +\size footnotesize +\begin_inset Formula $b=a.\mathbf{inverse}()$ +\end_inset + + +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\size footnotesize +\begin_inset Formula $a$ +\end_inset + + in +\begin_inset Formula $g$ +\end_inset + + +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\size footnotesize +\begin_inset Formula $b=g$ +\end_inset + + in +\begin_inset Formula $a$ +\end_inset + + +\begin_inset Newline newline +\end_inset + + +\begin_inset Formula $b=a^{-1}$ +\end_inset + + +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\size footnotesize +\begin_inset Formula $a.\mathbf{compose}($ +\end_inset + + +\begin_inset Newline newline +\end_inset + + +\begin_inset Formula $\quad a.\mathbf{inverse}())==\mathbf{I}$ +\end_inset + + +\end_layout + +\end_inset + + + + +\begin_inset Text + +\begin_layout Plain Layout + +\size footnotesize +\begin_inset Formula $c=a.\mathbf{between}\left(b\right)$ +\end_inset + + +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\size footnotesize +\begin_inset Formula $a$ +\end_inset + + in +\begin_inset Formula $g$ +\end_inset + + +\begin_inset Newline newline +\end_inset + + +\begin_inset Formula $b$ +\end_inset + + in +\begin_inset Formula $g$ +\end_inset + + +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\size footnotesize +\begin_inset Formula $c=b$ +\end_inset + + in +\begin_inset Formula $a$ +\end_inset + + +\begin_inset Newline newline +\end_inset + + +\begin_inset Formula $c=a^{-1}b$ +\end_inset + + +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\size footnotesize +\begin_inset Formula $a.\mathbf{inverse}().$ +\end_inset + + +\begin_inset Newline newline +\end_inset + + +\begin_inset Formula $\quad\mathbf{compose}(b)==c$ +\end_inset + + +\end_layout + +\end_inset + + + + +\begin_inset Text + +\begin_layout Plain Layout + +\size footnotesize +\begin_inset Formula $\delta=a.\mathbf{logmap}()$ +\end_inset + + +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\size footnotesize +\begin_inset Formula $a$ +\end_inset + + in +\begin_inset Formula $g$ +\end_inset + + +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\size footnotesize +\begin_inset Formula $\delta$ +\end_inset + + in +\begin_inset Formula $g$ +\end_inset + + +\begin_inset Newline newline +\end_inset + + +\begin_inset Formula $\hat{\delta}=\log a$ +\end_inset + + +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\size footnotesize +\begin_inset Formula $\mathrm{X}::\mathbf{Expmap}(\delta)==a$ +\end_inset + + +\end_layout + +\end_inset + + + + +\begin_inset Text + +\begin_layout Plain Layout + +\size footnotesize +\begin_inset Formula $a=\mathrm{X}::\mathbf{Expmap}(\delta)$ +\end_inset + + +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\size footnotesize +\begin_inset Formula $\delta$ +\end_inset + + in +\begin_inset Formula $g$ +\end_inset + + +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\size footnotesize +\begin_inset Formula $a$ +\end_inset + + in +\begin_inset Formula $g$ +\end_inset + + +\begin_inset Newline newline +\end_inset + + +\begin_inset Formula $a=\exp\hat{\delta}$ +\end_inset + + +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\size footnotesize +\begin_inset Formula $a.\mathbf{logmap}()==\delta$ +\end_inset + + +\end_layout + +\end_inset + + + + +\begin_inset Text + +\begin_layout Plain Layout + +\series bold +\size footnotesize +\begin_inset space \quad{} +\end_inset + +Lie group actions +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\end_layout + +\end_inset + + + + +\begin_inset Text + +\begin_layout Plain Layout + +\size footnotesize +\begin_inset Formula $q=a.\mathbf{transform\_to}\left(p\right)$ +\end_inset + + +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\size footnotesize +\begin_inset Formula $a$ +\end_inset + + in +\begin_inset Formula $g$ +\end_inset + + +\begin_inset Newline newline +\end_inset + + +\begin_inset Formula $p$ +\end_inset + + in +\begin_inset Formula $g$ +\end_inset + + +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\size footnotesize +\begin_inset Formula $q=p$ +\end_inset + + in +\begin_inset Formula $a$ +\end_inset + + +\begin_inset Newline newline +\end_inset + + +\begin_inset Formula $q=a^{-1}p$ +\end_inset + + +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\end_layout + +\end_inset + + + + +\begin_inset Text + +\begin_layout Plain Layout + +\size footnotesize +\begin_inset Formula $q=a.\mathbf{transform\_from}\left(p\right)$ +\end_inset + + +\begin_inset Newline newline +\end_inset + + +\begin_inset Formula $q=a\mathbf{*}p$ +\end_inset + + +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\size footnotesize +\begin_inset Formula $a$ +\end_inset + + in +\begin_inset Formula $g$ +\end_inset + + +\begin_inset Newline newline +\end_inset + + +\begin_inset Formula $p$ +\end_inset + + in +\begin_inset Formula $a$ +\end_inset + + +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\size footnotesize +\begin_inset Formula $q=p$ +\end_inset + + in +\begin_inset Formula $g$ +\end_inset + + +\begin_inset Newline newline +\end_inset + + +\begin_inset Formula $q=ap$ +\end_inset + + +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\end_layout + +\end_inset + + + + +\end_inset + + +\end_layout + +\begin_layout Plain Layout +\begin_inset Caption + +\begin_layout Plain Layout +\begin_inset CommandInset label +LatexCommand label +name "tab:Coordinate-frame-transformations" + +\end_inset + +Coordinate frame transformations performed by GTSAM geometry operations. + Here, +\begin_inset Formula $a$ +\end_inset + +, +\begin_inset Formula $b$ +\end_inset + +, +\begin_inset Formula $c$ +\end_inset + +, and +\begin_inset Formula $g$ +\end_inset + + are Lie group elements (Pose2, Pose3, Rot2, Rot3, Point2, Point3, +\emph on +etc +\emph default +). + +\begin_inset Formula $\delta$ +\end_inset + + is a set of Lie algebra coordinates (i.e. + linear update, linear delta, tangent space coordinates), and +\begin_inset Formula $\mathrm{X}$ +\end_inset + + is a Lie group type (e.g. + Pose2). + +\begin_inset Formula $p$ +\end_inset + + and +\begin_inset Formula $q$ +\end_inset + + are the objects of Lie group actions (Point2, Point3, +\emph on +etc +\emph default +). +\end_layout + +\end_inset + + +\end_layout + +\end_inset + +At the core of most coordinate frame usage in GTSAM are geometry and Lie + group operations. + We explain the geometry and Lie group operations in GTSAM in terms of the + coordinate frame transformations they perform, detailed in Table +\begin_inset space ~ +\end_inset + + +\begin_inset CommandInset ref +LatexCommand ref +reference "tab:Coordinate-frame-transformations" + +\end_inset + +. +\end_layout + +\begin_layout Standard +\begin_inset Float table +wide false +sideways false +status open + +\begin_layout Plain Layout +\align center + +\size footnotesize +\begin_inset Tabular + + + + + + + + +\begin_inset Text + +\begin_layout Plain Layout + +\series bold +\size footnotesize +Syntax +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\series bold +\size footnotesize +Input +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\series bold +\size footnotesize +Output +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\series bold +\size footnotesize +Identities +\end_layout + +\end_inset + + + + +\begin_inset Text + +\begin_layout Plain Layout + +\size footnotesize +\begin_inset Formula $\delta=a.\mathbf{localCoordinates}\left(b\right)$ +\end_inset + + +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\size footnotesize +\begin_inset Formula $a$ +\end_inset + + in +\begin_inset Formula $g$ +\end_inset + + +\begin_inset Newline newline +\end_inset + + +\begin_inset Formula $b$ +\end_inset + + in +\begin_inset Formula $g$ +\end_inset + + +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\size footnotesize +\begin_inset Formula $\delta$ +\end_inset + + in +\begin_inset Formula $a$ +\end_inset + + +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\size footnotesize +\begin_inset Formula $a.\mathbf{retract}\left(\delta\right)==b$ +\end_inset + + +\begin_inset Newline newline +\end_inset + + +\begin_inset Formula $\mathbf{I}.\mathbf{localCoordinates}($ +\end_inset + + +\begin_inset Newline newline +\end_inset + + +\begin_inset Formula $\quad a.\mathbf{between}\left(b\right))==\delta$ +\end_inset + + +\end_layout + +\end_inset + + + + +\begin_inset Text + +\begin_layout Plain Layout + +\size footnotesize +\begin_inset Formula $b=a.\mathbf{retract}\left(\delta\right)$ +\end_inset + + +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\size footnotesize +\begin_inset Formula $a$ +\end_inset + + in +\begin_inset Formula $g$ +\end_inset + + +\begin_inset Newline newline +\end_inset + + +\begin_inset Formula $\delta$ +\end_inset + + in +\begin_inset Formula $a$ +\end_inset + + +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\size footnotesize +\begin_inset Formula $b$ +\end_inset + + in +\begin_inset Formula $g$ +\end_inset + + +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\size footnotesize +\begin_inset Formula $a.\mathbf{compose}($ +\end_inset + + +\begin_inset Newline newline +\end_inset + + +\begin_inset Formula $\quad\mathbf{I}.\mathbf{retract}\left(\delta\right))==b$ +\end_inset + + +\end_layout + +\end_inset + + + + +\end_inset + + +\end_layout + +\begin_layout Plain Layout +\begin_inset Caption + +\begin_layout Plain Layout +\begin_inset CommandInset label +LatexCommand label +name "tab:Coordinate-frames-manifold" + +\end_inset + +Coordinate frames for manifold tangent space operations. + Here, +\begin_inset Formula $a$ +\end_inset + +, +\begin_inset Formula $b$ +\end_inset + +, and +\begin_inset Formula $g$ +\end_inset + + are manifold elements, +\begin_inset Formula $\delta$ +\end_inset + + is a tangent space element, and +\begin_inset Formula $\mathrm{X}$ +\end_inset + + is a Lie group type (e.g. + Pose2). + For the identities column, we assume the elements are also Lie group elements + with identity +\begin_inset Formula $\mathbf{I}$ +\end_inset + +. +\end_layout + +\end_inset + + +\end_layout + +\end_inset + +The manifold tangent space operations +\begin_inset Quotes eld +\end_inset + +retract +\begin_inset Quotes erd +\end_inset + + and +\begin_inset Quotes eld +\end_inset + +local coordinates +\begin_inset Quotes erd +\end_inset + + also work in the body frame for Lie group elements. + The tangent space coordinates given to +\begin_inset Quotes eld +\end_inset + +retract +\begin_inset Quotes erd +\end_inset + + should be in the body frame, not the global frame. + Similarly, the tangent space coordinates returned by +\begin_inset Quotes eld +\end_inset + +local coordinates +\begin_inset Quotes erd +\end_inset + + will be in the same body frame. + This is detailed in Table +\begin_inset space ~ +\end_inset + + +\begin_inset CommandInset ref +LatexCommand ref +reference "tab:Coordinate-frames-manifold" + +\end_inset + +. +\end_layout + +\begin_layout Section +Frame and Uncertainty Conventions For Built-in Factors +\end_layout + +\begin_layout Standard +\begin_inset Float table +wide false +sideways false +status open + +\begin_layout Plain Layout +\align center + +\size footnotesize +\begin_inset Tabular + + + + + + + + + +\begin_inset Text + +\begin_layout Plain Layout + +\size footnotesize +Name +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\size footnotesize +Residual +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\size footnotesize +Variables +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\size footnotesize +Measurement +\end_layout + +\begin_layout Plain Layout + +\size footnotesize +( +\begin_inset Formula $z$ +\end_inset + +) +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\size footnotesize +Measurement Uncertainty +\end_layout + +\end_inset + + + + +\begin_inset Text + +\begin_layout Plain Layout + +\series bold +\size footnotesize +PriorFactor +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\size footnotesize +\begin_inset Formula $z.\mathrm{localCoordinates}\left(x\right)$ +\end_inset + + +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\size footnotesize +\begin_inset Formula $x$ +\end_inset + + in +\begin_inset Formula $g$ +\end_inset + + +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\size footnotesize +Ideal +\begin_inset Formula $x$ +\end_inset + + in +\begin_inset Formula $g$ +\end_inset + + +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\size footnotesize +In +\begin_inset Formula $z$ +\end_inset + + / In +\begin_inset Formula $x$ +\end_inset + + +\end_layout + +\end_inset + + + + +\begin_inset Text + +\begin_layout Plain Layout + +\series bold +\size footnotesize +BetweenFactor +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\size footnotesize +\begin_inset Formula $z.\mathrm{localCoordinates}($ +\end_inset + + +\begin_inset Newline newline +\end_inset + + +\begin_inset Formula $\; x.\mathrm{between}\left(y\right))$ +\end_inset + + +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\size footnotesize +\begin_inset Formula $x$ +\end_inset + + in +\begin_inset Formula $g$ +\end_inset + + +\begin_inset Newline newline +\end_inset + + +\begin_inset Formula $y$ +\end_inset + + in +\begin_inset Formula $g$ +\end_inset + + +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\size footnotesize +Ideal +\begin_inset Formula $y$ +\end_inset + + in +\begin_inset Formula $x$ +\end_inset + + +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\size footnotesize +In +\begin_inset Formula $z$ +\end_inset + + / In +\begin_inset Formula $y$ +\end_inset + + +\end_layout + +\end_inset + + + + +\begin_inset Text + +\begin_layout Plain Layout + +\series bold +\size footnotesize +RangeFactor +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\size footnotesize +\begin_inset Formula $x.\mathrm{range}\left(y\right)-z$ +\end_inset + + +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\size footnotesize +\begin_inset Formula $x$ +\end_inset + + in +\begin_inset Formula $g$ +\end_inset + + +\begin_inset Newline newline +\end_inset + + +\begin_inset Formula $y$ +\end_inset + + in +\begin_inset Formula $g$ +\end_inset + + +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\size footnotesize +Euclidean distance +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\size footnotesize +In +\begin_inset Formula $z$ +\end_inset + + +\end_layout + +\end_inset + + + + +\begin_inset Text + +\begin_layout Plain Layout + +\series bold +\size footnotesize +BearingFactor +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\size footnotesize +\begin_inset Formula $z.\mathrm{localCoordinates}($ +\end_inset + + +\begin_inset Newline newline +\end_inset + + +\begin_inset Formula $\; x.\mathrm{bearing}\left(y\right))$ +\end_inset + + +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\size footnotesize +\begin_inset Formula $x$ +\end_inset + + in +\begin_inset Formula $g$ +\end_inset + + +\begin_inset Newline newline +\end_inset + + +\begin_inset Formula $y$ +\end_inset + + in +\begin_inset Formula $g$ +\end_inset + + +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\size footnotesize +Bearing of +\begin_inset Formula $y$ +\end_inset + + position in frame +\begin_inset Formula $x$ +\end_inset + + +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\size footnotesize +In +\begin_inset Formula $z$ +\end_inset + + +\end_layout + +\end_inset + + + + +\begin_inset Text + +\begin_layout Plain Layout + +\series bold +\size footnotesize +GenericProjection +\begin_inset Newline newline +\end_inset + + +\begin_inset space ~ +\end_inset + + +\begin_inset space ~ +\end_inset + +Factor +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\size footnotesize +\begin_inset Formula $K^{-1}\left(P\left(x^{-1}p\right)\right)-z$ +\end_inset + + +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\size footnotesize +\begin_inset Formula $x$ +\end_inset + + in +\begin_inset Formula $g$ +\end_inset + + +\begin_inset Newline newline +\end_inset + + +\begin_inset Formula $p$ +\end_inset + + in +\begin_inset Formula $g$ +\end_inset + + +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\size footnotesize +Perspective projection of +\begin_inset Formula $p$ +\end_inset + + in +\begin_inset Formula $x$ +\end_inset + +. +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\size footnotesize +In +\begin_inset Formula $z$ +\end_inset + + +\end_layout + +\end_inset + + + + +\begin_inset Text + +\begin_layout Plain Layout + +\series bold +\size footnotesize +GeneralSFM +\begin_inset Newline newline +\end_inset + + +\begin_inset space ~ +\end_inset + + +\begin_inset space ~ +\end_inset + +Factor +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\size footnotesize +\begin_inset Formula $K^{-1}\left(P\left(x^{-1}p\right)\right)-z$ +\end_inset + + +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\size footnotesize +\begin_inset Formula $x$ +\end_inset + + in +\begin_inset Formula $g$ +\end_inset + + +\begin_inset Newline newline +\end_inset + + +\begin_inset Formula $p$ +\end_inset + + in +\begin_inset Formula $g$ +\end_inset + + +\begin_inset Newline newline +\end_inset + +Parameters +\begin_inset Newline newline +\end_inset + + +\begin_inset space ~ +\end_inset + + +\begin_inset space ~ +\end_inset + +of +\begin_inset Formula $K$ +\end_inset + + +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\size footnotesize +Perspective projection of +\begin_inset Formula $p$ +\end_inset + + in +\begin_inset Formula $x$ +\end_inset + +. +\end_layout + +\end_inset + + +\begin_inset Text + +\begin_layout Plain Layout + +\size footnotesize +In +\begin_inset Formula $z$ +\end_inset + + +\end_layout + +\end_inset + + + + +\end_inset + + +\size default + +\begin_inset Caption + +\begin_layout Plain Layout +Measurement functions and coordinate frames of factors provided with GTSAM. + To simplify notation, +\begin_inset Formula $K$ +\end_inset + + is a camera calibration function converting pixels to normalized image + coordinates, and +\begin_inset Formula $P$ +\end_inset + + is the pinhole projection function. +\end_layout + +\end_inset + + +\end_layout + +\end_inset + +All built-in GTSAM factors follow a consistent coordinate frame convention + (though fundamentally how a measurement and its uncertainty are specified + depends on the measurement model described by a factor). + In all built-in GTSAM factors, the +\emph on +noise model +\emph default +, i.e. + the measurement uncertainty, should be specified in the coordinate frame + of the measurement itself. + This is part of a convention in GTSAM that tangent-space quantities (like + Gaussian noise models and update delta vectors) are always in the coordinate + frame of the element owning the tangent space. +\end_layout + +\begin_layout Subsection +PriorFactor +\end_layout + +\begin_layout Standard +A PriorFactor is a simple unary prior. + It encodes a direct measurement of the value of a variable +\begin_inset Formula $x$ +\end_inset + +, with the specified mean +\begin_inset Formula $z$ +\end_inset + + and uncertainty, such that +\begin_inset Formula $z.\mathrm{between}\left(x\right)$ +\end_inset + + is distributed according to the specified noise model. + From this definition and the definition of +\series bold +between +\series default + in Table +\begin_inset space ~ +\end_inset + + +\begin_inset CommandInset ref +LatexCommand ref +reference "tab:Coordinate-frame-transformations" + +\end_inset + +, the measurement itself should be specified in the frame with respect to + which +\begin_inset Formula $x$ +\end_inset + + is specified, while the uncertainty is specified in the coordinate frame + of the measurement, or equivalently, in frame +\begin_inset Formula $x$ +\end_inset + +. +\end_layout + +\begin_layout Subsection +BetweenFactor +\end_layout + +\begin_layout Standard +A BetweenFactor is a measurement on the relative transformation between + two variables. + A BetweenFactor on variables +\begin_inset Formula $x$ +\end_inset + + and +\begin_inset Formula $y$ +\end_inset + + with measurement +\begin_inset Formula $z$ +\end_inset + + implies that +\begin_inset Formula $z.\mathrm{between}\left(x.\mathrm{between}\left(y\right)\right)$ +\end_inset + + is distributed according to the specified noise model. + This definition, along with that of +\series bold +between +\series default + in Table +\begin_inset space ~ +\end_inset + + +\begin_inset CommandInset ref +LatexCommand ref +reference "tab:Coordinate-frame-transformations" + +\end_inset + +, implies that the measurement is in frame +\begin_inset Formula $x$ +\end_inset + +, i.e. + it measures +\begin_inset Formula $y$ +\end_inset + + in +\begin_inset Formula $x$ +\end_inset + +, and that the uncertainty is in the frame of the measurement, or equivalently, + in frame +\begin_inset Formula $y$ +\end_inset + +. +\end_layout + +\begin_layout Subsection +RangeFactor +\end_layout + +\begin_layout Standard +A RangeFactor measures the Euclidean distance either between two poses, + a pose and a point, or two points. + The range is a scalar, specified to be distributed according to the specified + noise model. +\end_layout + +\begin_layout Subsection +BearingFactor +\end_layout + +\begin_layout Standard +A BearingFactor measures the bearing (angle) of the +\emph on +position +\emph default + of a pose or point +\begin_inset Formula $y$ +\end_inset + + as observed from a pose +\begin_inset Formula $x$ +\end_inset + +. + The orientation of +\begin_inset Formula $x$ +\end_inset + + affects the measurement prediction. + Though, if +\begin_inset Formula $y$ +\end_inset + + is a pose, it's orientation does not matter. + The noise model specifies the distribution of the bearing, in radians. +\end_layout + +\begin_layout Subsection +GenericProjectionFactor +\end_layout + +\begin_layout Standard +A GenericProjectionFactor measures the pixel coordinates of a landmark +\begin_inset Formula $p$ +\end_inset + + projected into a camera +\begin_inset Formula $x$ +\end_inset + + with the calibration function +\begin_inset Formula $K$ +\end_inset + + that converts pixels to normalized image coordinates. + The measurement +\begin_inset Formula $z$ +\end_inset + + is specified in real pixel coordinates (thanks to the +\begin_inset Quotes eld +\end_inset + +uncalibration +\begin_inset Quotes erd +\end_inset + + function +\begin_inset Formula $K^{-1}$ +\end_inset + + used in the residual). + In a GenericProjectionFactor, the calibration is fixed. + On the other hand, GeneralSFMFactor allows the calibration parameters to + be optimized as variables. +\end_layout + +\begin_layout Subsection +GeneralSFMFactor +\end_layout + +\begin_layout Standard +A GeneralSFMFactor is the same as a GenericProjectionFactor except that + a GeneralSFMFactor also allows the parameters of the calibration function + +\begin_inset Formula $K$ +\end_inset + + to be optimized as variables, instead of having them fixed. + A GeneralSFMFactor measures the pixel coordinates of a landmark +\begin_inset Formula $p$ +\end_inset + + projected into a camera +\begin_inset Formula $x$ +\end_inset + + with the calibration function +\begin_inset Formula $K$ +\end_inset + + that converts pixels to normalized image coordinates. + The measurement +\begin_inset Formula $z$ +\end_inset + + is specified in real pixel coordinates (thanks to the +\begin_inset Quotes eld +\end_inset + +uncalibration +\begin_inset Quotes erd +\end_inset + + function +\begin_inset Formula $K^{-1}$ +\end_inset + + used in the residual). +\end_layout + +\begin_layout Standard +\begin_inset Note Note +status collapsed + +\begin_layout Section +Noise models of prior factors +\end_layout + +\begin_layout Plain Layout +The simplest way to describe noise models is by an example. + Let's take a prior factor on a 3D pose +\begin_inset Formula $x\in\SE 3$ +\end_inset + +, +\family typewriter +Pose3 +\family default + in GTSAM. + Let +\begin_inset Formula $z\in\SE 3$ +\end_inset + + be the expected pose, i.e. + the zero-error solution for the prior factor. + The +\emph on +unwhitened error +\emph default + (the error vector not accounting for the noise model) is +\begin_inset Formula +\[ +h\left(x\right)=\log\left(z^{-1}x\right)\text{,} +\] + +\end_inset + +where +\begin_inset Formula $\cdot^{-1}$ +\end_inset + + is the Lie group inverse and +\begin_inset Formula $\log\cdot$ +\end_inset + + is the logarithm map on +\begin_inset Formula $\SE 3$ +\end_inset + +. + The full factor error, including the noise model, is +\begin_inset Formula +\[ +e\left(x\right)=\norm{h\left(x\right)}_{\Sigma}^{2}=h\left(x\right)^{\t}\Sigma^{-1}h\left(x\right)\text{.} +\] + +\end_inset + +[ Skipping details of the derivation for now, for lack of time to get a + useful answer out quickly ] +\end_layout + +\begin_layout Plain Layout +The density induced by a noise model on the prior factor is Gaussian in + the tangent space about the linearization point. + Suppose that the pose is linearized at +\begin_inset Formula $\lin x\in\SE 3$ +\end_inset + +, which we assume is near to +\begin_inset Formula $z$ +\end_inset + +. + Let +\begin_inset Formula $\delta x\in\R 6$ +\end_inset + + be an update vector in local coordinates (a twist). + Then, the factor error in terms of the update vector +\begin_inset Formula $\delta x$ +\end_inset + + is +\begin_inset Formula +\[ +e\left(\delta x\right)=\norm{h\left(\lin x\exp\delta x\right)}_{\Sigma}^{2} +\] + +\end_inset + +We can see why the covariance +\begin_inset Formula $\Sigma$ +\end_inset + + is in the body frame of +\begin_inset Formula $x$ +\end_inset + + by looking at the linearized error function, +\begin_inset Formula +\begin{align*} +e\left(\delta x\right) & \approx\norm{\log\left(z^{-1}\lin x\exp\delta x\right)}_{\Sigma}^{2}\\ + & \approx\norm{\log\left(z^{-1}\lin x\right)+\delta x}_{\Sigma}^{2} +\end{align*} + +\end_inset + +Here we see that the update +\begin_inset Formula $\exp\delta x$ +\end_inset + + from the linear step +\begin_inset Formula $\delta x$ +\end_inset + + is applied in the body frame of +\begin_inset Formula $\lin x$ +\end_inset + +, because of the ordering +\begin_inset Formula $\lin x\exp\delta x$ +\end_inset + +. + Furthermore, +\begin_inset Formula $z^{-1}\lin x$ +\end_inset + + is a constant term, so we can also see that the covariance +\begin_inset Formula $\Sigma$ +\end_inset + + is actually applied to the linear update vector +\begin_inset Formula $\delta x$ +\end_inset + +. +\end_layout + +\begin_layout Plain Layout +This means that to draw random pose samples, we actually draw random samples + of +\begin_inset Formula $\delta x$ +\end_inset + + with zero mean and covariance +\begin_inset Formula $\Sigma$ +\end_inset + +, i.e. +\begin_inset Formula +\[ +\delta x\sim\mathcal{N}\left(0,\:\Sigma\right)\text{.} +\] + +\end_inset + + +\end_layout + +\begin_layout Section +Noise models of between factors +\end_layout + +\begin_layout Plain Layout +The noise model of a BetweenFactor is a bit more complicated. + The unwhitened error is +\begin_inset Formula +\[ +h\left(x_{1},x_{2}\right)=\log\left(z^{-1}x_{1}^{-1}x_{2}\right)\text{,} +\] + +\end_inset + +where +\begin_inset Formula $z$ +\end_inset + + is the expected relative pose between +\begin_inset Formula $x_{1}$ +\end_inset + + and +\begin_inset Formula $x_{2}$ +\end_inset + +, i.e. + the factor has zero error when +\begin_inset Formula $x_{1}z=x_{2}$ +\end_inset + +. + If we consider the density on the second pose +\begin_inset Formula $x_{2}$ +\end_inset + + induced by holding the first pose +\begin_inset Formula $x_{1}$ +\end_inset + + fixed, we can see that the covariance is applied to the linear update in + the body frame of the second pose +\begin_inset Formula $x_{2}$ +\end_inset + +, +\begin_inset Formula +\[ +e\left(\delta x_{2}\right)\approx\norm{\log\left(z^{-1}x_{1}^{-1}x_{2}\exp\delta x_{2}\right)}_{\Sigma}^{2}. +\] + +\end_inset + +If we hold the second pose fixed, the covariance is applied as follows (actually +, what frame is it in now??) +\begin_inset Formula +\begin{align*} +e\left(\delta x_{1}\right) & \approx\norm{\log\left(z^{-1}\left(x_{1}\exp\delta x_{1}\right)^{-1}x_{2}\right)}_{\Sigma}^{2}\\ + & =\norm{\log\left(z^{-1}\exp-\delta x_{1}x_{1}^{-1}x_{2}\right)}_{\Sigma}^{2} +\end{align*} + +\end_inset + + +\end_layout + +\end_inset + + +\end_layout + +\end_body +\end_document diff --git a/doc/gtsam-coordinate-frames.pdf b/doc/gtsam-coordinate-frames.pdf new file mode 100644 index 000000000..3613ef0ac Binary files /dev/null and b/doc/gtsam-coordinate-frames.pdf differ diff --git a/doc/images/.gitignore b/doc/images/.gitignore deleted file mode 100644 index a7cbee1ba..000000000 --- a/doc/images/.gitignore +++ /dev/null @@ -1 +0,0 @@ -/*.DS_Store diff --git a/doc/math.lyx b/doc/math.lyx index 8d56963cb..d96f1f4c8 100644 --- a/doc/math.lyx +++ b/doc/math.lyx @@ -1594,7 +1594,7 @@ First, the derivative \begin_inset Formula $D_{2}f$ \end_inset - with respect to in + with respect to \begin_inset Formula $p$ \end_inset @@ -1614,11 +1614,11 @@ For the derivative \begin_inset Formula $T$ \end_inset -, we want -\end_layout - -\begin_layout Proof +, we want to find the linear map +\begin_inset Formula $D_{1}f$ +\end_inset + such that \family roman \series medium \shape up @@ -1630,9 +1630,10 @@ For the derivative \uwave off \noun off \color none + \begin_inset Formula \[ -f(Te^{\hat{\xi}},p)=Te^{\hat{\xi}}p\approx Tp+D_{1}f(\xi) +Tp+D_{1}f(\xi)\approx f(Te^{\hat{\xi}},p)=Te^{\hat{\xi}}p \] \end_inset @@ -1661,7 +1662,12 @@ Te^{\hat{\xi}}p\approx T(I+\hat{\xi})p=Tp+T\hat{\xi}p \end_inset +and +\begin_inset Formula $D_{1}f(\xi)=T\hat{\xi}p$ +\end_inset +. + \begin_inset Note Note status collapsed @@ -1679,7 +1685,7 @@ T\hat{\xi}p=\left(T\hat{\xi}T^{-1}\right)Tp=\left(\Ad T\xihat\right)\left(Tp\rig \end_inset -Hence, we need to show that +Hence, to complete the proof, we need to show that \begin_inset Formula \begin{equation} \xihat p=H(p)\xi\label{eq:Hp} @@ -4173,9 +4179,9 @@ so . Hence, the final derivative of an action in its first argument is \begin_inset Formula -\[ -\deriv{\left(Rp\right)}{\omega}=RH(p)=-R\Skew p -\] +\begin{equation} +\deriv{\left(Rp\right)}{\omega}=RH(p)=-R\Skew p\label{eq:Rot3action} +\end{equation} \end_inset @@ -5027,6 +5033,909 @@ Re^{\Skew{\omega}} & t+R\left[v+\left(\omega\times v\right)/2\right]\\ \end_inset +\end_layout + +\begin_layout Section +The Sphere +\begin_inset Formula $S^{2}$ +\end_inset + + +\end_layout + +\begin_layout Subsection +Definitions +\end_layout + +\begin_layout Standard +The sphere +\begin_inset Formula $S^{2}$ +\end_inset + + is the set of all unit vectors in +\begin_inset Formula $\Rthree$ +\end_inset + +, i.e., all directions in three-space: +\begin_inset Formula +\[ +S^{2}=\{p\in\Rthree|\left\Vert p\right\Vert =1\} +\] + +\end_inset + +The tangent space +\begin_inset Formula $T_{p}S^{2}$ +\end_inset + + at a point +\begin_inset Formula $p$ +\end_inset + + consists of three-vectors +\begin_inset Formula $\xihat$ +\end_inset + + such that +\begin_inset Formula $\xihat$ +\end_inset + + is tangent to +\begin_inset Formula $S^{2}$ +\end_inset + + at +\begin_inset Formula $p$ +\end_inset + +, i.e., +\begin_inset Formula +\[ +T_{p}S^{2}\define\left\{ \xihat\in\Rthree|p^{T}\xihat=0\right\} +\] + +\end_inset + +While not a Lie group, we can define an exponential map, which is given + in Ma et. + al +\begin_inset CommandInset citation +LatexCommand cite +key "Ma01ijcv" + +\end_inset + +, as well as in this CVPR tutorial by Anuj Srivastava: +\begin_inset CommandInset href +LatexCommand href +name "http://stat.fsu.edu/~anuj/CVPR_Tutorial/Part2.pdf" + +\end_inset + +. + +\begin_inset Formula +\[ +\exp_{p}\xihat=\cos\left(\left\Vert \xihat\right\Vert \right)p+\sin\left(\left\Vert \xihat\right\Vert \right)\frac{\xihat}{\left\Vert \xihat\right\Vert } +\] + +\end_inset + +The latter also gives the inverse, i.e., get the tangent vector +\begin_inset Formula $z$ +\end_inset + + to go from +\begin_inset Formula $p$ +\end_inset + + to +\begin_inset Formula $q$ +\end_inset + +: +\begin_inset Formula +\[ +z=\log_{p}q=\frac{\theta}{\sin\theta}\left(q-p\cos\theta\right)p +\] + +\end_inset + +with +\begin_inset Formula $\theta=\cos^{-1}\left(p^{T}q\right)$ +\end_inset + +. +\end_layout + +\begin_layout Subsection +Local Coordinates +\end_layout + +\begin_layout Standard +We can find a basis +\begin_inset Formula $B_{p}$ +\end_inset + + for the tangent space +\begin_inset Formula $T_{p}S^{2}$ +\end_inset + +, with +\begin_inset Formula $B_{p}=\left[b_{1}|b_{2}\right]$ +\end_inset + + a +\begin_inset Formula $3\times2$ +\end_inset + + matrix, by either +\end_layout + +\begin_layout Enumerate +Decompose +\begin_inset Formula $p=QR$ +\end_inset + +, with +\begin_inset Formula $Q$ +\end_inset + + orthonormal and +\begin_inset Formula $R$ +\end_inset + + of the form +\begin_inset Formula $[1\,0\,0]^{T}$ +\end_inset + +, and hence +\begin_inset Formula $p=Q_{1}$ +\end_inset + +. + The basis +\begin_inset Formula $B_{p}=\left[Q_{2}|Q_{3}\right]$ +\end_inset + +, i.e., the last two columns of +\begin_inset Formula $Q$ +\end_inset + +. +\end_layout + +\begin_layout Enumerate +Form +\begin_inset Formula $b_{1}=p\times a$ +\end_inset + +, with +\begin_inset Formula $a$ +\end_inset + + (consistently) chosen to be non-parallel to +\begin_inset Formula $p$ +\end_inset + +, and +\begin_inset Formula $b_{2}=p\times b_{1}$ +\end_inset + +. + +\begin_inset Note Note +status collapsed + +\begin_layout Plain Layout +To choose +\begin_inset Formula $a$ +\end_inset + +, one way is to divide the sphere into regions, e.g., pick the axis +\begin_inset Formula $e_{i}$ +\end_inset + + such that +\begin_inset Formula $e_{i}^{T}p$ +\end_inset + + is smallest. + However, that leads to discontinuous boundaries. + Since +\begin_inset Formula $0\leq\left|e_{i}^{T}p\right|\leq1$ +\end_inset + + for all +\begin_inset Formula $p\in S^{2}$ +\end_inset + +, a better idea might be to use a mixture, e.g., +\begin_inset Formula +\[ +a=\frac{1}{2(x^{2}+y^{2}+z^{2})}\left[\begin{array}{c} +y^{2}+z^{2}\\ +x^{2}+z^{2}\\ +x^{2}+y^{2} +\end{array}\right] +\] + +\end_inset + + +\end_layout + +\end_inset + + +\end_layout + +\begin_layout Standard +Now we can write +\begin_inset Formula $\xihat=B_{p}\xi$ +\end_inset + + with +\begin_inset Formula $\xi\in R^{2}$ +\end_inset + + the 2D coordinate in the tangent plane basis +\begin_inset Formula $B_{p}$ +\end_inset + +. +\end_layout + +\begin_layout Subsection +Retraction +\end_layout + +\begin_layout Standard +The exponential map uses +\begin_inset Formula $\cos$ +\end_inset + + and +\begin_inset Formula $\sin$ +\end_inset + +, and is more than we need for optimization. + Suppose we have a point +\begin_inset Formula $p\in S^{2}$ +\end_inset + + and a 3-vector +\begin_inset Formula $\xihat\in T_{p}S^{2}$ +\end_inset + +, Absil +\begin_inset CommandInset citation +LatexCommand cite +key "Absil07book" + +\end_inset + + tells us we can simply add +\begin_inset Formula $\xihat$ +\end_inset + + to +\begin_inset Formula $p$ +\end_inset + + and renormalize to get a new point +\begin_inset Formula $q$ +\end_inset + + on the sphere. + This is what he calls a +\series bold +retraction +\family roman +\series medium +\shape up +\size normal +\emph off +\bar no +\strikeout off +\uuline off +\uwave off +\noun off +\color none + +\begin_inset Formula $\retract_{p}(\xihat)$ +\end_inset + +, +\family default +\series default +\shape default +\size default +\emph default +\bar default +\strikeout default +\uuline default +\uwave default +\noun default +\color inherit + +\begin_inset Formula +\[ +q=\retract_{p}(\xihat)=\frac{p+\xihat}{\left\Vert p+z\right\Vert }=\frac{p+\xihat}{\alpha} +\] + +\end_inset + +with +\begin_inset Formula $\alpha$ +\end_inset + + the norm of +\begin_inset Formula $p+\xihat$ +\end_inset + +. +\end_layout + +\begin_layout Standard +We can also define a retraction from local coordinates +\begin_inset Formula $\xi\in\Rtwo$ +\end_inset + +: +\begin_inset Formula +\[ +q=\retract_{p}(\xi)=\frac{p+B_{p}\xi}{\left\Vert p+B_{p}\xi\right\Vert } +\] + +\end_inset + + +\end_layout + +\begin_layout Subsubsection* +Inverse Retraction +\end_layout + +\begin_layout Standard +If +\begin_inset Formula $\xihat=B_{p}\xi$ +\end_inset + + with +\begin_inset Formula $\xi\in R^{2}$ +\end_inset + + the 2D coordinate in the tangent plane basis +\begin_inset Formula $B_{p}$ +\end_inset + +, we have +\begin_inset Formula +\[ +\xi=\frac{B_{p}^{T}q}{p^{T}q} +\] + +\end_inset + + +\end_layout + +\begin_layout Proof +We seek +\begin_inset Formula +\[ +\alpha q=p+B_{p}\xi +\] + +\end_inset + +If we multiply both sides with +\begin_inset Formula $B_{p}^{T}$ +\end_inset + + (project on the basis +\begin_inset Formula $B_{p}$ +\end_inset + +) we obtain +\begin_inset Formula +\[ +\alpha B_{p}^{T}q=B_{p}^{T}p+B_{p}^{T}B_{p}\xi +\] + +\end_inset + +and because +\begin_inset Formula $B_{p}^{T}p=0$ +\end_inset + + and +\begin_inset Formula $B_{p}^{T}B_{p}=I$ +\end_inset + + we trivially obtain +\begin_inset Formula $\xi$ +\end_inset + + as the scaled projection +\begin_inset Formula $B_{p}^{T}q$ +\end_inset + +: +\begin_inset Formula +\[ +\xi=\alpha B_{p}^{T}q +\] + +\end_inset + +To recover the scale factor +\begin_inset Formula $\alpha$ +\end_inset + + we multiply with +\begin_inset Formula $p^{T}$ +\end_inset + + on both sides, and we get +\begin_inset Formula +\[ +\alpha p^{T}q=p^{T}p+p^{T}B_{p}\xi +\] + +\end_inset + +Since +\begin_inset Formula $p^{T}p=1$ +\end_inset + + and +\begin_inset Formula $p^{T}B_{p}\xi=0$ +\end_inset + +, we then obtain +\begin_inset Formula $\alpha=1/(p^{T}q)$ +\end_inset + +, which completes the proof. +\end_layout + +\begin_layout Subsection +Rotation acting on a 3D Direction +\end_layout + +\begin_layout Standard +Rotating a point +\begin_inset Formula $p\in S^{2}$ +\end_inset + + on the sphere obviously yields another point +\begin_inset Formula $q=Rp\in S^{2}$ +\end_inset + +, as rotation preserves the norm. + The derivative of +\begin_inset Formula $f(R,p)$ +\end_inset + + with respect to +\begin_inset Formula $R$ +\end_inset + + can be found by equating +\begin_inset Formula +\[ +Rp+B_{Rp}\xi=R(I+\Skew{\omega})p=Rp+R\Skew{\omega}p +\] + +\end_inset + + +\begin_inset Formula +\[ +B_{Rp}\xi=-R\Skew p\omega +\] + +\end_inset + + +\begin_inset Formula +\[ +\xi=-B_{Rp}^{T}R\Skew p\omega +\] + +\end_inset + +whereas with respect to +\begin_inset Formula $p$ +\end_inset + + we have +\begin_inset Formula +\[ +Rp+B_{Rp}\xi_{q}=R(p+B_{p}\xi_{p}) +\] + +\end_inset + + +\begin_inset Formula +\[ +\xi_{q}=B_{Rp}^{T}RB_{p}\xi_{p} +\] + +\end_inset + + +\end_layout + +\begin_layout Standard +In other words, the Jacobian matrix is given by +\family roman +\series medium +\shape up +\size normal +\emph off +\bar no +\strikeout off +\uuline off +\uwave off +\noun off +\color none + +\begin_inset Formula +\[ +f'(R,p)=\left[\begin{array}{cc} +-B_{Rp}^{T}R\Skew p & B_{Rp}^{T}RB_{p}\end{array}\right] +\] + +\end_inset + + +\end_layout + +\begin_layout Subsection +Error between 3D Directions +\end_layout + +\begin_layout Standard +We would like to define a distance metric +\begin_inset Formula $e(p,q)$ +\end_inset + + between two directions +\begin_inset Formula $p,q\in S^{2}$ +\end_inset + +. + An obvious choice is +\begin_inset Formula +\[ +\theta=\cos^{-1}\left(p^{T}q\right) +\] + +\end_inset + +which is exactly the distance along the shortest path (geodesic) on the + sphere, i.e., this is the distance metric associated with the exponential. + The advantage is that it is defined everywhere, but it involves +\begin_inset Formula $\cos^{-1}$ +\end_inset + +. + The derivative with respect to a change in +\begin_inset Formula $q$ +\end_inset + +, via +\begin_inset Formula $\xi$ +\end_inset + +, is then +\begin_inset Formula +\[ +\frac{\partial\theta(p,q)}{\partial\xi}=\frac{\partial\cos^{-1}\left(p^{T}q\right)}{\partial\xi}=\frac{p^{T}B_{q}}{\sqrt{1-\left(p^{T}q\right)^{2}}} +\] + +\end_inset + +which is also undefined for +\begin_inset Formula $p=q$ +\end_inset + +. + +\end_layout + +\begin_layout Standard +A simpler metric is derived from the retraction but only holds when +\begin_inset Formula $q\approx p$ +\end_inset + +. + It simply projects +\begin_inset Formula $q$ +\end_inset + + onto the local coordinate basis +\begin_inset Formula $B_{p}$ +\end_inset + + defined by +\begin_inset Formula $p$ +\end_inset + +, and takes the norm: +\begin_inset Formula +\[ +\theta(p,q)=\left\Vert B_{p}^{T}q\right\Vert +\] + +\end_inset + +The derivative with respect to a change in +\begin_inset Formula $q$ +\end_inset + +, via +\begin_inset Formula $\xi$ +\end_inset + +, is then +\begin_inset Formula +\[ +\frac{\partial\theta(p,q)}{\partial\xi_{q}}=\frac{\partial}{\partial\xi_{q}}\sqrt{\left(B_{p}^{T}q\right)^{2}}=\frac{1}{\sqrt{\left(B_{p}^{T}q\right)^{2}}}\left(B_{p}^{T}q\right)B_{p}^{T}B_{q}=\frac{B_{p}^{T}q}{\theta(q;p)}B_{p}^{T}B_{q} +\] + +\end_inset + +Note that this again is undefined for +\begin_inset Formula $\theta=0$ +\end_inset + +. +\end_layout + +\begin_layout Standard +For use in a probabilistic factor, a signed, vector-valued error will not + have the discontinuity: +\begin_inset Formula +\[ +\theta(p,q)=B_{p}^{T}q +\] + +\end_inset + +Note this is the inverse retraction up to a scale. + The derivative with respect to a change in +\begin_inset Formula $q$ +\end_inset + +, via +\begin_inset Formula $\xi$ +\end_inset + +, is found by +\begin_inset Formula +\[ +\frac{\partial\theta(p,q)}{\partial\xi_{q}}=B_{p}^{T}\frac{\partial q}{\partial\xi_{q}}=B_{p}^{T}B_{q} +\] + +\end_inset + + +\end_layout + +\begin_layout Subsubsection* +Application +\end_layout + +\begin_layout Standard +We can use the above to find the unknown rotation between a camera and an + IMU. + If we measure the rotation between two frames as +\begin_inset Formula $c_{1}Zc_{2}$ +\end_inset + +, and the predicted rotation from the IMU is +\begin_inset Formula $i_{1}Ri_{2}$ +\end_inset + +, then we can predict +\begin_inset Formula +\[ +c_{1}Zc_{2}=iRc^{T}\cdot i_{1}Ri_{2}\cdot iRc +\] + +\end_inset + +and the axis of the incremental rotations will relate as +\begin_inset Formula +\[ +p=iRc\cdot z +\] + +\end_inset + +with +\begin_inset Formula $p$ +\end_inset + + the angular velocity axis in the IMU frame, and +\begin_inset Formula $z$ +\end_inset + + the measured axis of rotation between the two cameras. + Note this only makes sense if the rotation is non-zero. + So, given an initial estimate +\begin_inset Formula $R$ +\end_inset + + for the unknown rotation +\begin_inset Formula $iRc$ +\end_inset + + between IMU and camera, the derivative of the error is (using +\begin_inset CommandInset ref +LatexCommand ref +reference "eq:Rot3action" + +\end_inset + +) +\family roman +\series medium +\shape up +\size normal +\emph off +\bar no +\strikeout off +\uuline off +\uwave off +\noun off +\color none + +\begin_inset Formula +\[ +\frac{\partial\theta(Rz;p)}{\partial\omega}=B_{p}^{T}\left(Rz\right)B_{p}^{T}B_{Rz}\frac{\partial\left(Rz\right)}{\partial\omega}=B_{p}^{T}\left(Rz\right)B_{p}^{T}R\Skew z +\] + +\end_inset + +Here the +\begin_inset Formula $2\times3$ +\end_inset + + matrix +\begin_inset Formula $B_{Rz}^{T}\Skew z$ +\end_inset + + translates changes in +\begin_inset Formula $R$ +\end_inset + + to changes in +\begin_inset Formula $Rz$ +\end_inset + +, and the +\begin_inset Formula $1\times2$ +\end_inset + + matrix +\begin_inset Formula $B_{p}^{T}\left(Rz\right)$ +\end_inset + + describes the downstream effect on the error metric. +\end_layout + +\begin_layout Section +The Essential Matrix Manifold +\end_layout + +\begin_layout Standard +We parameterize essential matrices as a pair +\begin_inset Formula $(R,t)$ +\end_inset + +, where +\begin_inset Formula $R\in\SOthree$ +\end_inset + + and +\begin_inset Formula $t\in S^{2}$ +\end_inset + +, the unit sphere. + The epipolar matrix is then given by +\begin_inset Formula +\[ +E=\Skew tR +\] + +\end_inset + +and the epipolar error given two corresponding points +\begin_inset Formula $a$ +\end_inset + + and +\begin_inset Formula $b$ +\end_inset + + is +\begin_inset Formula +\[ +e(R,t;a,b)=a^{T}Eb +\] + +\end_inset + +We are of course interested in the derivative with respect to orientation + (using +\begin_inset CommandInset ref +LatexCommand ref +reference "eq:Rot3action" + +\end_inset + +) +\begin_inset Formula +\[ +\frac{\partial(a^{T}[t]_{\times}Rb)}{\partial\omega}=a^{T}[t]_{\times}\frac{\partial(Rb)}{\partial\omega}=-a^{T}[t]_{\times}R\Skew b=-a^{T}E[b]_{\times} +\] + +\end_inset + +and with respect to change in the direction +\begin_inset Formula $t$ +\end_inset + + +\begin_inset Formula +\[ +\frac{\partial e(a^{T}[t]_{\times}Rb)}{\partial\xi}=a^{T}\frac{\partial(B\xi\times Rb)}{\partial v}=-a^{T}[Rb]_{\times}B +\] + +\end_inset + +where we made use of the fact that the retraction can be written as +\begin_inset Formula $t+B\xi$ +\end_inset + +, with +\begin_inset Formula $B$ +\end_inset + + a local basis, and we made use of +\begin_inset CommandInset ref +LatexCommand eqref +reference "eq:Dcross1" + +\end_inset + +: +\family roman +\series medium +\shape up +\size normal +\emph off +\bar no +\strikeout off +\uuline off +\uwave off +\noun off +\color none + +\begin_inset Formula +\[ +\frac{\partial(a\times b)}{\partial a}=\Skew{-b} +\] + +\end_inset + + \end_layout \begin_layout Section diff --git a/doc/math.pdf b/doc/math.pdf index 9b1046f68..6589af06a 100644 Binary files a/doc/math.pdf and b/doc/math.pdf differ diff --git a/doc/stats.html b/doc/stats.html index 46650c1e0..9bb12956c 100644 --- a/doc/stats.html +++ b/doc/stats.html @@ -13712,17 +13712,17 @@ can just fix the size of their placeholders.

The output has been generated by gitinspector; the statistical analysis tool for git repositories.

-

The following historical commit information, by author, was found in the repository.

Author Commits Insertions Deletions % of changes
Abhijit Kundu136881130.10
Alex Cunningham7571077558942923.82
Alireza Fathi514160.00
Andrew Melim33180312210.37
Brian Law1971740.03
Can Erdogan387240.01
Chris Beall141556958291.38
Duy-Nguyen Ta101707842471.37
Eohan George2310.00
Frank Dellaert8761055788350322.84
John Rogers728140.01
Julian Straub1200.00
Kai Ni17720780198204.90
Kyel Ok250180.01
Luca Carlone1114891550.20
Manohar Paluri6130876380.45
Michael Kaess69907525581.41
Natesh Srinivasan4671000.02
Nick Barrash821789250.37
Paul Kippes1880.00
Rahul Sawhney883413090.26
Richard Roberts93916350711759833.96
Stephen Williams17128367131815.02
Summit Patel579840.02
Vadim Indelman162861910.36
Viorela Ila2715493230.23
Yong-Dian Jian1551270485092.56
Zsolt Kira76671760.10
bpeasle55752440.10
jdurand74141880.03
justinca74591600.07
 
-

Below are the number of rows from each author that have survived and are still intact in the current revision.

Author Rows % in comments
Abhijit Kundu55431.230.45
Alex Cunningham2577125.4120.89
Alireza Fathi633.330.00
Andrew Melim64950.850.53
Brian Law2821.430.02
Can Erdogan30.000.00
Chris Beall149335.501.21
Duy-Nguyen Ta236823.101.92
Eohan George10.000.00
Frank Dellaert1956031.8515.86
John Rogers911.110.01
Julian Straub20.000.00
Kai Ni348458.982.82
Kyel Ok5032.000.04
Luca Carlone104537.800.85
Manohar Paluri3555.070.29
Michael Kaess26731.840.22
Natesh Srinivasan1656.250.01
Nick Barrash126959.341.03
Rahul Sawhney1394.320.11
Richard Roberts4129927.2933.48
Stephen Williams1831928.4714.85
Summit Patel74100.000.06
Vadim Indelman254226.952.06
Viorela Ila1145.450.01
Yong-Dian Jian291019.902.36
Zsolt Kira59915.530.49
bpeasle32326.320.26
jdurand713476.870.11
justinca7143.660.06
 
+

The following historical commit information, by author, was found in the repository.

Author Commits Insertions Deletions % of changes
Abhijit Kundu136881130.08
Alex Cunningham8121095679261919.93
Alireza Fathi514160.00
Andrew Melim36185712320.30
Brian Law1971740.03
Can Erdogan387240.01
Chris Beall160591960431.18
Duy-Nguyen Ta127887253501.40
Eohan George2310.00
Frank Dellaert9811113958646319.50
Jing Dong23123412380.24
John Rogers728140.00
Julian Straub1200.00
Kai Ni17720780198204.00
Kyel Ok250180.01
Luca Carlone80819890271.70
Manohar Paluri6130876380.37
Michael Kaess69907525581.15
Natesh Srinivasan4671000.02
Nick Barrash821789250.31
Pablo Fernandez Alcantarilla86081530.08
Paul Kippes1880.00
Rahul Sawhney883413090.21
Richard Roberts132923682519162242.23
Stephen Williams17128367131814.10
Summit Patel579840.02
Vadim Indelman2736664020.40
Viorela Ila2715493230.18
Yong-Dian Jian1551270485092.09
Zsolt Kira2122687690.30
bpeasle55752440.08
djensen316430.01
jdurand74141880.02
justinca74591600.06
 
+

Below are the number of rows from each author that have survived and are still intact in the current revision.

Author Rows % in comments
Abhijit Kundu38030.530.31
Alex Cunningham2197426.5317.98
Alireza Fathi633.330.00
Andrew Melim58741.400.48
Brian Law2821.430.02
Can Erdogan30.000.00
Chris Beall141437.551.16
Duy-Nguyen Ta363835.132.98
Eohan George10.000.00
Frank Dellaert1608435.0713.16
Jing Dong634.760.05
John Rogers911.110.01
Julian Straub20.000.00
Kai Ni274458.672.24
Kyel Ok4833.330.04
Luca Carlone175426.171.43
Manohar Paluri2342.990.19
Michael Kaess8937.080.07
Nick Barrash89258.630.73
Pablo Fernandez Alcantarilla1797.260.15
Rahul Sawhney780.000.06
Richard Roberts5008127.7440.97
Stephen Williams1600029.2013.09
Summit Patel59100.000.05
Vadim Indelman293821.342.40
Viorela Ila944.440.01
Yong-Dian Jian247123.352.02
Zsolt Kira10.000.00
bpeasle32126.480.26
djensen3640.000.05
jdurand720130.000.02
justinca7044.290.06
 

The following history timeline has been gathered from the repository.

Author2009-082009-092009-102009-112009-122010-012010-022010-03
Alex Cunningham..
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
Alireza Fathi.
Brian Law.
Chris Beall.
 
.
 
.
Eohan George..
Frank Dellaert.
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
John Rogers..
Kai Ni..
 
 
 
 
 
 
 
 
 
 
Manohar Paluri.....
 
 
 
 
.
Michael Kaess.
 
 
 
 
 
 
 
Richard Roberts
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
..
 
 
 
 
 
 
 
 
..
Viorela Ila..
 
 
 
...
Yong-Dian Jian.
justinca...
Modified Rows:17798225711451248651451231259182334290
Author2010-042010-052010-062010-072010-082010-092010-102010-11
Alex Cunningham
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
.
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
Chris Beall..
 
 
Duy-Nguyen Ta..
Frank Dellaert
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
.
 
 
 
 
 
 
 
 
..
John Rogers.
Kai Ni
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
Manohar Paluri..
Michael Kaess
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
Rahul Sawhney.
Richard Roberts
 
 
 
 
 
 
.
 
 
.
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
Viorela Ila.
Yong-Dian Jian..
 
Modified Rows:247710368801392571683125221640054490
Author2010-122011-012011-022011-032011-042011-052011-062011-07
Alex Cunningham..
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
.
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
Chris Beall..
 
.
Duy-Nguyen Ta
 
 
 
 
 
 
 
.
Frank Dellaert..
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
John Rogers.
Julian Straub.
Kai Ni...
Richard Roberts
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
..
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
Yong-Dian Jian
 
.
 
 
 
 
Modified Rows:1219053003635649892055361259981
Author2011-082011-092011-102011-112011-122012-012012-022012-03
Alex Cunningham..
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
..
Andrew Melim....
Can Erdogan...
Chris Beall....
 
 
.
Duy-Nguyen Ta....
 
 
 
 
 
 
 
 
 
 
 
.
Frank Dellaert.
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
..
 
 
 
 
 
 
 
 
 
.
Manohar Paluri.
Michael Kaess.
Natesh Srinivasan..
Nick Barrash.
 
 
 
 
 
 
Paul Kippes.
Richard Roberts
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
Stephen Williams
 
 
 
 
 
 
 
 
..
 
..
Vadim Indelman..
Yong-Dian Jian
 
 
..
 
 
 
 
.
 
Modified Rows:7262731397841603612173196661751412637
Author2012-042012-052012-062012-072012-082012-092012-102012-11
Abhijit Kundu
 
Alex Cunningham
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
.
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
Andrew Melim..
 
 
 
 
Chris Beall..
 
 
 
 
 
 
..
Duy-Nguyen Ta
 
 
 
 
...
Frank Dellaert
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
.
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
.
Kyel Ok.
Richard Roberts
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
Stephen Williams..
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
.
 
 
 
Summit Patel.
Vadim Indelman...
Yong-Dian Jian..
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
..
bpeasle.
jdurand7.
Modified Rows:159903942736781196835415937453454705
-
Author2012-122013-012013-022013-032013-042013-052013-062013-07
Alex Cunningham...
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
Andrew Melim.
Chris Beall....
Duy-Nguyen Ta..
 
.
Frank Dellaert.
 
.
 
 
 
 
 
 
 
 
 
 
 
 
Kyel Ok.
Luca Carlone
 
Richard Roberts
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
.
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
..
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
Stephen Williams.
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
.
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
Vadim Indelman
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
.
 
 
 
.
Yong-Dian Jian..
Modified Rows:109918158550529720015625169777476
-
Author2013-082013-092013-12
Alex Cunningham.
Chris Beall.
Duy-Nguyen Ta.
Frank Dellaert
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
Luca Carlone
 
Natesh Srinivasan.
Richard Roberts
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
Stephen Williams
 
 
 
 
 
 
 
 
 
 
 
 
Vadim Indelman
 
Zsolt Kira
 
Modified Rows:2347024756
+
Author2012-122013-012013-022013-032013-042013-052013-062013-07
Alex Cunningham...
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
Andrew Melim.
Chris Beall....
Duy-Nguyen Ta..
 
.
Frank Dellaert.
 
.
 
 
 
 
Kyel Ok.
Luca Carlone.
Richard Roberts
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
.
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
.
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
Stephen Williams.
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
.
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
Vadim Indelman
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
.
 
 
 
.
Yong-Dian Jian..
Modified Rows:10991815855052972001562512850374012
+
Author2013-082013-092013-102013-112013-12
Alex Cunningham.
 
 
 
...
Andrew Melim.
Chris Beall...
Duy-Nguyen Ta.
 
 
 
.
Frank Dellaert.
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
Jing Dong
 
 
 
 
 
 
Luca Carlone.
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
.
Natesh Srinivasan.
Pablo Fernandez Alcantarilla.
Richard Roberts
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
Stephen Williams
 
Vadim Indelman.
Zsolt Kira.
 
 
 
 
 
 
 
 
 
.
djensen3.
Modified Rows:81545255427521490410480
-

The following files are suspiciously big (in order of severity).

gtsam/base/tests/testMatrix.cpp (1062 eloc)

gtsam/nonlinear/ISAM2.cpp (983 eloc)

gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp (969 eloc)

gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp (861 eloc)

tests/testGaussianISAM2.cpp (859 eloc)

gtsam/inference/BayesTree-inl.h (666 eloc)

wrap/tests/expected/geometry_wrapper.cpp (652 eloc)

gtsam/geometry/tests/testPose3.cpp (650 eloc)

tests/testSerializationSLAM.cpp (639 eloc)

gtsam/geometry/tests/testPose2.cpp (639 eloc)

wrap/Module.cpp (628 eloc)

gtsam/slam/dataset.cpp (624 eloc)

gtsam/base/Matrix.cpp (599 eloc)

gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp (598 eloc)

gtsam/linear/tests/testHessianFactor.cpp (576 eloc)

gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp (571 eloc)

gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp (569 eloc)

gtsam/nonlinear/NonlinearFactor.h (565 eloc)

gtsam/linear/NoiseModel.cpp (549 eloc)

gtsam/linear/NoiseModel.h (543 eloc)

gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h (523 eloc)

gtsam/discrete/DecisionTree-inl.h (520 eloc)

gtsam/navigation/CombinedImuFactor.h (517 eloc)

gtsam/linear/GaussianFactorGraph.cpp (506 eloc)

gtsam/base/numericalDerivative.h (486 eloc)

tests/smallExample.h (480 eloc)

gtsam/base/blockMatrices.h (477 eloc)

gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h (461 eloc)

gtsam/navigation/ImuFactor.h (454 eloc)

gtsam_unstable/gtsam_unstable.h (425 eloc)

gtsam/nonlinear/ISAM2.h (418 eloc)

gtsam/geometry/PinholeCamera.h (382 eloc)

gtsam_unstable/slam/SmartProjectionFactor.h (374 eloc)

gtsam.h (336 eloc)

gtsam_unstable/base/BTree.h (317 eloc)

-

The following repsonsibilties, by author, were found in the current revision of the repository (comments are exluded from the line count, if possible).

Abhijit Kundu is mostly responsible for

examples/UGM_chain.cpp (74 eloc)
examples/DiscreteBayesNet_FG.cpp (72 eloc)
gtsam/inference/tests/testBayesTree.cpp (45 eloc)
gtsam/discrete/tests/testDiscreteMarginals.cpp (36 eloc)
gtsam/discrete/DiscreteConditional.cpp (30 eloc)
gtsam/discrete/Assignment.h (28 eloc)
examples/UGM_small.cpp (21 eloc)
gtsam/discrete/DiscreteMarginals.h (18 eloc)
gtsam/discrete/Potentials.cpp (15 eloc)
gtsam/discrete/DiscreteSequentialSolver.cpp (15 eloc)

Alex Cunningham is mostly responsible for

wrap/tests/expected/geometry_wrapper.cpp (607 eloc)
tests/smallExample.h (441 eloc)
gtsam/base/tests/testMatrix.cpp (418 eloc)
wrap/tests/expected_namespaces/testNamespaces_wrapper.cpp (392 eloc)
wrap/tests/testWrap.cpp (356 eloc)
gtsam_unstable/linear/tests/testBayesTreeOperations.cpp (344 eloc)
tests/testNonlinearEquality.cpp (331 eloc)
gtsam_unstable/base/BTree.h (317 eloc)
gtsam/geometry/tests/testPose3.cpp (295 eloc)
gtsam_unstable/geometry/SimPolygon2D.cpp (286 eloc)

Alireza Fathi is mostly responsible for

gtsam/geometry/tests/testRot3Q.cpp (2 eloc)
gtsam/geometry/tests/testRot3M.cpp (2 eloc)

Andrew Melim is mostly responsible for

wrap/Method.cpp (51 eloc)
wrap/Class.cpp (47 eloc)
wrap/StaticMethod.cpp (42 eloc)
wrap/Module.cpp (33 eloc)
wrap/Deconstructor.cpp (31 eloc)
wrap/Deconstructor.h (29 eloc)
wrap/matlab.h (17 eloc)
wrap/Argument.cpp (13 eloc)
wrap/Constructor.cpp (9 eloc)
wrap/utilities.h (8 eloc)

Brian Law is mostly responsible for

gtsam/geometry/Pose3.cpp (6 eloc)
gtsam/geometry/Pose2.h (4 eloc)
gtsam/geometry/Point3.cpp (4 eloc)
gtsam/geometry/Point3.h (3 eloc)
gtsam/geometry/Cal3_S2.h (3 eloc)
gtsam/geometry/Rot3M.cpp (2 eloc)

Can Erdogan is mostly responsible for

gtsam/nonlinear/Values-inl.h (2 eloc)
gtsam/base/LieScalar.h (1 eloc)

Chris Beall is mostly responsible for

gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp (115 eloc)
gtsam_unstable/slam/InvDepthFactor3.h (80 eloc)
gtsam_unstable/geometry/tests/testTriangulation.cpp (73 eloc)
gtsam/slam/StereoFactor.h (70 eloc)
gtsam_unstable/slam/tests/testInvDepthFactor3.cpp (67 eloc)
gtsam_unstable/geometry/triangulation.cpp (59 eloc)
gtsam/geometry/Rot3M.cpp (46 eloc)
gtsam/geometry/Cal3_S2Stereo.h (39 eloc)
gtsam/geometry/StereoCamera.h (37 eloc)
gtsam/geometry/Pose3.cpp (35 eloc)

Duy-Nguyen Ta is mostly responsible for

gtsam_unstable/dynamics/Pendulum.h (169 eloc)
gtsam_unstable/dynamics/SimpleHelicopter.h (163 eloc)
gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp (138 eloc)
gtsam/base/numericalDerivative.h (135 eloc)
gtsam/base/tests/testNumericalDerivative.cpp (86 eloc)
gtsam/nonlinear/ISAM2-inl.h (84 eloc)
gtsam/geometry/tests/testPose3.cpp (81 eloc)
gtsam_unstable/gtsam_unstable.h (52 eloc)
gtsam_unstable/dynamics/VelocityConstraint3.h (52 eloc)
gtsam_unstable/dynamics/tests/testPendulumFactors.cpp (46 eloc)

Eohan George is mostly responsible for

gtsam/base/Testable.h (1 eloc)

Frank Dellaert is mostly responsible for

gtsam/slam/dataset.cpp (334 eloc)
gtsam/inference/tests/testSymbolicBayesTree.cpp (268 eloc)
gtsam/geometry/tests/testPose2.cpp (266 eloc)
gtsam/linear/tests/testGaussianFactorGraph.cpp (242 eloc)
gtsam_unstable/discrete/examples/schedulingQuals13.cpp (236 eloc)
gtsam/geometry/PinholeCamera.h (223 eloc)
gtsam/linear/tests/testKalmanFilter.cpp (219 eloc)
gtsam/geometry/tests/testRot3M.cpp (215 eloc)
gtsam/discrete/tests/testDiscreteBayesTree.cpp (210 eloc)
gtsam_unstable/examples/SmartRangeExample_plaza1.cpp (208 eloc)

John Rogers is mostly responsible for

gtsam/base/FastMap.h (5 eloc)
gtsam/base/numericalDerivative.h (2 eloc)
gtsam/inference/graph-inl.h (1 eloc)

Julian Straub is mostly responsible for

gtsam/nonlinear/NonlinearISAM.cpp (2 eloc)

Kai Ni is mostly responsible for

gtsam/linear/tests/testNoiseModel.cpp (113 eloc)
gtsam/slam/dataset.cpp (96 eloc)
gtsam/inference/graph-inl.h (74 eloc)
gtsam/slam/tests/testGeneralSFMFactor.cpp (62 eloc)
gtsam/geometry/Cal3DS2.cpp (56 eloc)
gtsam/geometry/Cal3DS2.h (49 eloc)
gtsam/base/tests/testMatrix.cpp (44 eloc)
gtsam/linear/tests/testGaussianJunctionTree.cpp (39 eloc)
gtsam/geometry/Cal3Bundler.cpp (39 eloc)
tests/simulated2DOriented.h (35 eloc)

Kyel Ok is mostly responsible for

gtsam_unstable/slam/Mechanization_bRn2.cpp (15 eloc)
gtsam_unstable/slam/AHRS.cpp (13 eloc)
gtsam_unstable/slam/AHRS.h (4 eloc)
gtsam_unstable/slam/Mechanization_bRn2.h (2 eloc)

Luca Carlone is mostly responsible for

gtsam_unstable/slam/SmartProjectionFactor.h (273 eloc)
gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp (166 eloc)
gtsam_unstable/slam/MultiProjectionFactor.h (137 eloc)
gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp (66 eloc)
gtsam/geometry/Rot3Q.cpp (4 eloc)
gtsam_unstable/slam/tests/testBetweenFactorEM.cpp (2 eloc)
gtsam/slam/tests/testBetweenFactor.cpp (2 eloc)

Manohar Paluri is mostly responsible for

gtsam/base/tests/testMatrix.cpp (247 eloc)
gtsam/inference/BayesTree-inl.h (25 eloc)
tests/testGaussianBayesNet.cpp (23 eloc)
gtsam/linear/GaussianBayesNet.cpp (9 eloc)
gtsam/geometry/tests/testCal3_S2.cpp (7 eloc)
tests/testNonlinearFactorGraph.cpp (6 eloc)
gtsam/linear/NoiseModel.cpp (4 eloc)
gtsam/geometry/tests/testPoint3.cpp (4 eloc)
gtsam/nonlinear/Values.h (2 eloc)
gtsam/linear/GaussianConditional.h (2 eloc)

Michael Kaess is mostly responsible for

gtsam/inference/tests/testBayesTree.cpp (45 eloc)
gtsam/inference/BayesTree-inl.h (22 eloc)
tests/testGaussianISAM.cpp (20 eloc)
gtsam/slam/PriorFactor.h (19 eloc)
gtsam/inference/ISAM-inl.h (19 eloc)
gtsam/inference/tests/testISAM.cpp (15 eloc)
gtsam/inference/ISAM.h (14 eloc)
gtsam/linear/GaussianISAM.cpp (10 eloc)
gtsam/linear/GaussianISAM.h (8 eloc)
gtsam/inference/BayesTree.h (4 eloc)

Natesh Srinivasan is mostly responsible for

gtsam/linear/GaussianBayesTree.cpp (5 eloc)
gtsam/linear/GaussianBayesTree.h (2 eloc)

Nick Barrash is mostly responsible for

gtsam/geometry/Rot2.h (51 eloc)
gtsam/geometry/Point3.h (47 eloc)
gtsam/inference/BayesNet.h (30 eloc)
gtsam/geometry/Pose3.h (30 eloc)
gtsam/geometry/Point2.h (29 eloc)
gtsam/geometry/StereoPoint2.h (23 eloc)
gtsam/geometry/Pose2.h (20 eloc)
gtsam/inference/Factor.h (19 eloc)
gtsam/geometry/Cal3DS2.h (19 eloc)
gtsam/inference/BayesTreeCliqueBase.h (16 eloc)

Rahul Sawhney is mostly responsible for

gtsam/inference/graph-inl.h (38 eloc)
gtsam/inference/graph.h (19 eloc)
gtsam/base/blockMatrices.h (14 eloc)
gtsam/inference/FactorGraph.h (9 eloc)
gtsam/nonlinear/NonlinearFactor.h (8 eloc)
gtsam/inference/ISAM-inl.h (5 eloc)
gtsam/linear/GaussianConditional.h (4 eloc)
gtsam/inference/ClusterTree.h (4 eloc)
gtsam/inference/ClusterTree-inl.h (4 eloc)
gtsam/linear/GaussianISAM.h (3 eloc)

Richard Roberts is mostly responsible for

gtsam/nonlinear/ISAM2.cpp (886 eloc)
tests/testGaussianISAM2.cpp (689 eloc)
gtsam/discrete/DecisionTree-inl.h (519 eloc)
gtsam/navigation/CombinedImuFactor.h (517 eloc)
gtsam/navigation/ImuFactor.h (454 eloc)
examples/SolverComparer.cpp (440 eloc)
gtsam/navigation/tests/testImuFactor.cpp (424 eloc)
gtsam/inference/BayesTree-inl.h (424 eloc)
gtsam/discrete/tests/testAlgebraicDecisionTree.cpp (419 eloc)
gtsam/linear/tests/testHessianFactor.cpp (366 eloc)

Stephen Williams is mostly responsible for

gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp (969 eloc)
gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp (856 eloc)
gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp (571 eloc)
gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp (569 eloc)
tests/testSerializationSLAM.cpp (546 eloc)
gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h (522 eloc)
gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp (453 eloc)
gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp (453 eloc)
gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp (450 eloc)
gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp (348 eloc)

Vadim Indelman is mostly responsible for

gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp (576 eloc)
gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h (461 eloc)
gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h (292 eloc)
gtsam_unstable/slam/BetweenFactorEM.h (213 eloc)
gtsam_unstable/slam/tests/testBetweenFactorEM.cpp (204 eloc)
gtsam/slam/dataset.cpp (87 eloc)
gtsam_unstable/gtsam_unstable.h (14 eloc)
gtsam/base/LieVector.h (5 eloc)
gtsam/slam/dataset.h (4 eloc)
gtsam/slam/ProjectionFactor.h (1 eloc)

Viorela Ila is mostly responsible for

gtsam/geometry/Pose3.cpp (2 eloc)
tests/testNonlinearFactor.cpp (1 eloc)
gtsam/slam/dataset.cpp (1 eloc)
gtsam/nonlinear/NonlinearFactorGraph.h (1 eloc)
gtsam/inference/tests/testBayesTree.cpp (1 eloc)

Yong-Dian Jian is mostly responsible for

gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h (160 eloc)
gtsam/linear/NoiseModel.cpp (158 eloc)
gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp (152 eloc)
gtsam/slam/tests/testGeneralSFMFactor.cpp (109 eloc)
gtsam/geometry/PinholeCamera.h (102 eloc)
gtsam/linear/NoiseModel.h (100 eloc)
gtsam/linear/SubgraphSolver.cpp (95 eloc)
gtsam/linear/SubgraphPreconditioner.cpp (86 eloc)
gtsam/slam/GeneralSFMFactor.h (83 eloc)
tests/testSubgraphSolver.cpp (69 eloc)

Zsolt Kira is mostly responsible for

gtsam_unstable/examples/SmartProjectionFactorExample_kitti.cpp (232 eloc)
gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp (181 eloc)
gtsam_unstable/slam/SmartProjectionFactor.h (93 eloc)

bpeasle is mostly responsible for

tests/testOccupancyGrid.cpp (238 eloc)

jdurand7 is mostly responsible for

gtsam/inference/BayesNet-inl.h (24 eloc)
gtsam/inference/tests/testSymbolicBayesNet.cpp (13 eloc)
gtsam/inference/BayesNet.h (7 eloc)

justinca is mostly responsible for

gtsam/base/Matrix.cpp (19 eloc)
gtsam/base/tests/testMatrix.cpp (17 eloc)
gtsam/base/Matrix.h (4 eloc)
+

The following files are suspiciously big (in order of severity).

gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp (1114 eloc)

gtsam/base/tests/testMatrix.cpp (1087 eloc)

gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp (1034 eloc)

gtsam/nonlinear/ISAM2.cpp (972 eloc)

tests/testGaussianISAM2.cpp (712 eloc)

wrap/tests/expected/geometry_wrapper.cpp (652 eloc)

gtsam/geometry/tests/testPose3.cpp (650 eloc)

tests/testSerializationSLAM.cpp (641 eloc)

gtsam/geometry/tests/testPose2.cpp (639 eloc)

gtsam/base/Matrix.cpp (638 eloc)

wrap/Module.cpp (637 eloc)

gtsam/slam/dataset.cpp (624 eloc)

gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp (623 eloc)

gtsam/linear/NoiseModel.cpp (617 eloc)

gtsam/linear/NoiseModel.h (604 eloc)

gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp (599 eloc)

gtsam/symbolic/tests/testSymbolicBayesTree.cpp (574 eloc)

gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp (544 eloc)

gtsam/nonlinear/NonlinearFactor.h (544 eloc)

gtsam/linear/JacobianFactor.cpp (539 eloc)

gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h (525 eloc)

gtsam/discrete/DecisionTree-inl.h (520 eloc)

gtsam/navigation/CombinedImuFactor.h (517 eloc)

gtsam_unstable/slam/CombinedImuFactor.h (511 eloc)

examples/SolverComparer.cpp (507 eloc)

gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp (502 eloc)

gtsam/base/numericalDerivative.h (486 eloc)

tests/smallExample.h (475 eloc)

gtsam_unstable/gtsam_unstable.h (465 eloc)

gtsam_unstable/slam/ImuFactor.h (462 eloc)

gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h (461 eloc)

gtsam/navigation/ImuFactor.h (454 eloc)

gtsam/inference/BayesTree-inst.h (426 eloc)

gtsam/nonlinear/ISAM2.h (401 eloc)

gtsam/geometry/PinholeCamera.h (382 eloc)

gtsam_unstable/base/BTree.h (322 eloc)

+

The following repsonsibilties, by author, were found in the current revision of the repository (comments are exluded from the line count, if possible).

Abhijit Kundu is mostly responsible for

examples/UGM_chain.cpp (74 eloc)
examples/DiscreteBayesNet_FG.cpp (72 eloc)
gtsam/discrete/tests/testDiscreteMarginals.cpp (34 eloc)
gtsam/discrete/Assignment.h (28 eloc)
gtsam/discrete/DiscreteConditional.cpp (21 eloc)
examples/UGM_small.cpp (21 eloc)
gtsam/discrete/DiscreteMarginals.h (12 eloc)
gtsam/inference/BayesTreeCliqueBase.h (1 eloc)
gtsam/inference/BayesTree.h (1 eloc)

Alex Cunningham is mostly responsible for

wrap/tests/expected/geometry_wrapper.cpp (607 eloc)
wrap/tests/expected_namespaces/testNamespaces_wrapper.cpp (392 eloc)
gtsam/base/tests/testMatrix.cpp (358 eloc)
wrap/tests/testWrap.cpp (355 eloc)
tests/smallExample.h (341 eloc)
gtsam_unstable/base/BTree.h (317 eloc)
tests/testNonlinearEquality.cpp (311 eloc)
gtsam/geometry/tests/testPose3.cpp (295 eloc)
wrap/Module.cpp (288 eloc)
gtsam_unstable/geometry/SimPolygon2D.cpp (286 eloc)

Alireza Fathi is mostly responsible for

gtsam/geometry/tests/testRot3Q.cpp (2 eloc)
gtsam/geometry/tests/testRot3M.cpp (2 eloc)

Andrew Melim is mostly responsible for

wrap/Method.cpp (51 eloc)
wrap/Class.cpp (47 eloc)
wrap/StaticMethod.cpp (42 eloc)
wrap/Module.cpp (34 eloc)
wrap/Deconstructor.cpp (31 eloc)
wrap/Deconstructor.h (29 eloc)
gtsam_unstable/base/tests/testDSFMap.cpp (20 eloc)
wrap/matlab.h (17 eloc)
wrap/Argument.cpp (13 eloc)
wrap/Constructor.cpp (9 eloc)

Brian Law is mostly responsible for

gtsam/geometry/Pose3.cpp (6 eloc)
gtsam/geometry/Pose2.h (4 eloc)
gtsam/geometry/Point3.cpp (4 eloc)
gtsam/geometry/Point3.h (3 eloc)
gtsam/geometry/Cal3_S2.h (3 eloc)
gtsam/geometry/Rot3M.cpp (2 eloc)

Can Erdogan is mostly responsible for

gtsam/nonlinear/Values-inl.h (2 eloc)
gtsam/base/LieScalar.h (1 eloc)

Chris Beall is mostly responsible for

gtsam_unstable/geometry/tests/testTriangulation.cpp (108 eloc)
gtsam_unstable/slam/InvDepthFactor3.h (80 eloc)
gtsam/slam/StereoFactor.h (70 eloc)
gtsam_unstable/slam/tests/testInvDepthFactor3.cpp (64 eloc)
gtsam/geometry/Rot3M.cpp (46 eloc)
gtsam/geometry/Cal3_S2Stereo.h (39 eloc)
gtsam/geometry/tests/testStereoPoint2.cpp (38 eloc)
gtsam/geometry/StereoCamera.h (37 eloc)
gtsam/geometry/Pose3.cpp (35 eloc)
gtsam/geometry/tests/testPose2.cpp (32 eloc)

Duy-Nguyen Ta is mostly responsible for

gtsam_unstable/discrete/tests/testLoopyBelief.cpp (209 eloc)
gtsam_unstable/dynamics/Pendulum.h (169 eloc)
gtsam_unstable/dynamics/SimpleHelicopter.h (161 eloc)
gtsam/base/numericalDerivative.h (135 eloc)
gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp (129 eloc)
gtsam/geometry/tests/testPose3.cpp (81 eloc)
gtsam/base/tests/testNumericalDerivative.cpp (71 eloc)
gtsam/nonlinear/ISAM2-inl.h (59 eloc)
gtsam_unstable/gtsam_unstable.h (52 eloc)
gtsam_unstable/dynamics/VelocityConstraint3.h (52 eloc)

Eohan George is mostly responsible for

gtsam/base/Testable.h (1 eloc)

Frank Dellaert is mostly responsible for

gtsam/geometry/tests/testPose2.cpp (240 eloc)
gtsam_unstable/discrete/examples/schedulingQuals13.cpp (234 eloc)
gtsam_unstable/examples/SmartRangeExample_plaza1.cpp (203 eloc)
gtsam/linear/tests/testKalmanFilter.cpp (202 eloc)
gtsam/geometry/PinholeCamera.h (199 eloc)
gtsam/linear/NoiseModel.cpp (193 eloc)
gtsam/geometry/tests/testRot3M.cpp (190 eloc)
gtsam_unstable/slam/AHRS.cpp (187 eloc)
gtsam_unstable/examples/SmartRangeExample_plaza2.cpp (157 eloc)
gtsam/linear/NoiseModel.h (154 eloc)

Jing Dong is mostly responsible for

gtsam/base/tests/testMatrix.cpp (34 eloc)
examples/elaboratePoint2KalmanFilter.cpp (7 eloc)
gtsam_unstable/slam/tests/timeInertialNavFactor_GlobalVelocity.cpp (6 eloc)
gtsam/linear/tests/timeGaussianFactor.cpp (3 eloc)
gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp (3 eloc)
gtsam/geometry/tests/timePose3.cpp (2 eloc)
gtsam/geometry/tests/timeStereoCamera.cpp (1 eloc)
gtsam/geometry/tests/timePose2.cpp (1 eloc)
gtsam/geometry/tests/timePinholeCamera.cpp (1 eloc)
gtsam/geometry/tests/timeCalibratedCamera.cpp (1 eloc)

John Rogers is mostly responsible for

gtsam/base/FastMap.h (5 eloc)
gtsam/base/numericalDerivative.h (2 eloc)
gtsam/inference/graph-inl.h (1 eloc)

Julian Straub is mostly responsible for

gtsam/nonlinear/NonlinearISAM.cpp (2 eloc)

Kai Ni is mostly responsible for

gtsam/slam/dataset.cpp (96 eloc)
gtsam/linear/tests/testNoiseModel.cpp (93 eloc)
gtsam/inference/graph-inl.h (74 eloc)
gtsam/slam/tests/testGeneralSFMFactor.cpp (62 eloc)
gtsam/geometry/Cal3DS2.cpp (56 eloc)
gtsam/geometry/Cal3DS2.h (49 eloc)
gtsam/geometry/Cal3Bundler.cpp (38 eloc)
tests/simulated2DOriented.h (35 eloc)
gtsam/base/Vector.cpp (35 eloc)
gtsam/geometry/Cal3Bundler.h (34 eloc)

Kyel Ok is mostly responsible for

gtsam_unstable/slam/Mechanization_bRn2.cpp (13 eloc)
gtsam_unstable/slam/AHRS.cpp (13 eloc)
gtsam_unstable/slam/AHRS.h (4 eloc)
gtsam_unstable/slam/Mechanization_bRn2.h (2 eloc)

Luca Carlone is mostly responsible for

gtsam/slam/dataset.cpp (262 eloc)
gtsam/slam/tests/testDataset.cpp (155 eloc)
gtsam_unstable/slam/MultiProjectionFactor.h (137 eloc)
gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp (88 eloc)
gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp (73 eloc)
gtsam/geometry/tests/testPinholeCamera.cpp (60 eloc)
gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp (59 eloc)
gtsam_unstable/geometry/triangulation.cpp (54 eloc)
gtsam_unstable/geometry/triangulation.h (41 eloc)
gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp (36 eloc)

Manohar Paluri is mostly responsible for

gtsam/base/tests/testMatrix.cpp (195 eloc)
gtsam/geometry/tests/testCal3_S2.cpp (7 eloc)
tests/testNonlinearFactorGraph.cpp (6 eloc)
gtsam/linear/GaussianBayesNet.cpp (4 eloc)
gtsam/geometry/tests/testPoint3.cpp (4 eloc)
gtsam/linear/NoiseModel.cpp (3 eloc)
gtsam/nonlinear/Values.h (2 eloc)
gtsam/inference/BayesTree.h (2 eloc)
gtsam/geometry/Point3.cpp (2 eloc)
gtsam/geometry/Cal3_S2.h (1 eloc)

Michael Kaess is mostly responsible for

gtsam/slam/PriorFactor.h (19 eloc)
tests/testGaussianISAM.cpp (16 eloc)
gtsam/symbolic/tests/testSymbolicISAM.cpp (14 eloc)
gtsam/base/Vector.cpp (3 eloc)
tests/smallExample.h (1 eloc)
gtsam/inference/BayesTree.h (1 eloc)
gtsam/base/Vector.h (1 eloc)
gtsam/base/Testable.h (1 eloc)

Nick Barrash is mostly responsible for

gtsam/geometry/Rot2.h (51 eloc)
gtsam/geometry/Point3.h (47 eloc)
gtsam/geometry/Pose3.h (30 eloc)
gtsam/geometry/Point2.h (29 eloc)
gtsam/geometry/StereoPoint2.h (23 eloc)
gtsam/geometry/Pose2.h (20 eloc)
gtsam/geometry/Cal3DS2.h (19 eloc)
gtsam/geometry/Cal3Bundler.h (15 eloc)
gtsam/inference/Factor.h (13 eloc)
gtsam/geometry/CalibratedCamera.h (12 eloc)

Pablo Fernandez Alcantarilla is mostly responsible for

gtsam/nonlinear/LevenbergMarquardtOptimizer.h (90 eloc)
gtsam/linear/VectorValues.cpp (23 eloc)
gtsam/linear/tests/testVectorValuesUnordered.cpp (20 eloc)
gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp (15 eloc)
gtsam/geometry/PinholeCamera.h (6 eloc)
gtsam/linear/GaussianFactorGraph.cpp (5 eloc)
gtsam/linear/HessianFactor.cpp (3 eloc)
gtsam/linear/VectorValues.h (2 eloc)
gtsam/linear/GaussianFactorGraph.h (1 eloc)
gtsam/linear/GaussianFactor.h (1 eloc)

Rahul Sawhney is mostly responsible for

gtsam/inference/graph-inl.h (38 eloc)
gtsam/inference/graph.h (19 eloc)
gtsam/nonlinear/NonlinearFactor.h (8 eloc)
gtsam/inference/FactorGraph.h (3 eloc)
gtsam/geometry/Rot2.h (2 eloc)
gtsam/geometry/Point3.h (2 eloc)
gtsam/geometry/Point2.h (2 eloc)
gtsam/base/Vector.cpp (1 eloc)
gtsam/base/Matrix.cpp (1 eloc)
gtsam/base/FastSet.h (1 eloc)

Richard Roberts is mostly responsible for

gtsam/nonlinear/ISAM2.cpp (906 eloc)
tests/testGaussianISAM2.cpp (594 eloc)
gtsam/symbolic/tests/testSymbolicBayesTree.cpp (573 eloc)
gtsam/discrete/DecisionTree-inl.h (519 eloc)
gtsam/navigation/CombinedImuFactor.h (517 eloc)
gtsam_unstable/slam/CombinedImuFactor.h (509 eloc)
examples/SolverComparer.cpp (488 eloc)
gtsam/linear/JacobianFactor.cpp (485 eloc)
gtsam_unstable/slam/ImuFactor.h (462 eloc)
gtsam/linear/tests/testJacobianFactorUnordered.cpp (459 eloc)

Stephen Williams is mostly responsible for

gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp (833 eloc)
gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp (773 eloc)
gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h (517 eloc)
gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp (496 eloc)
tests/testSerializationSLAM.cpp (485 eloc)
gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp (408 eloc)
gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp (399 eloc)
gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp (397 eloc)
gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp (390 eloc)
gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp (302 eloc)

Vadim Indelman is mostly responsible for

gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp (503 eloc)
gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h (459 eloc)
gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h (287 eloc)
gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h (262 eloc)
gtsam_unstable/slam/BetweenFactorEM.h (235 eloc)
gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp (225 eloc)
gtsam_unstable/slam/tests/testBetweenFactorEM.cpp (192 eloc)
gtsam/slam/dataset.cpp (86 eloc)
gtsam_unstable/gtsam_unstable.h (42 eloc)
gtsam/base/LieVector.h (5 eloc)

Viorela Ila is mostly responsible for

gtsam/geometry/Pose3.cpp (2 eloc)
tests/testNonlinearFactor.cpp (1 eloc)
gtsam/slam/dataset.cpp (1 eloc)
gtsam/nonlinear/NonlinearFactorGraph.h (1 eloc)

Yong-Dian Jian is mostly responsible for

gtsam/linear/NoiseModel.cpp (157 eloc)
gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp (152 eloc)
gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h (146 eloc)
gtsam/slam/tests/testGeneralSFMFactor.cpp (109 eloc)
gtsam/linear/NoiseModel.h (100 eloc)
gtsam/geometry/PinholeCamera.h (99 eloc)
gtsam/slam/GeneralSFMFactor.h (83 eloc)
gtsam/linear/SubgraphSolver.cpp (72 eloc)
gtsam/inference/graph-inl.h (67 eloc)
gtsam/linear/ConjugateGradientSolver.h (61 eloc)

Zsolt Kira is mostly responsible for

gtsam_unstable/geometry/triangulation.h (1 eloc)

bpeasle is mostly responsible for

tests/testOccupancyGrid.cpp (236 eloc)

djensen3 is mostly responsible for

gtsam/nonlinear/NonlinearFactorGraph.cpp (60 eloc)
gtsam/nonlinear/NonlinearFactorGraph.h (4 eloc)

justinca is mostly responsible for

gtsam/base/Matrix.cpp (19 eloc)
gtsam/base/tests/testMatrix.cpp (16 eloc)
gtsam/base/Matrix.h (4 eloc)

The following files were excluded from the statistics due to the specified exclusion patterns.

gtsam/3rdparty/Eigen/src/Core/Replicate.h

gtsam/3rdparty/Eigen/Eigen/src/Sparse/SparseDot.h

gtsam/3rdparty/Eigen/bench/btl/cmake/FindBlitz.cmake

gtsam/3rdparty/Eigen/bench/btl/generic_bench/timers/portable_perf_analyzer.hh

gtsam/3rdparty/Eigen/Eigen/src/Core/BooleanRedux.h

gtsam/3rdparty/Eigen/Eigen/src/misc/SparseSolve.h

gtsam/3rdparty/Eigen/test/sparselu.cpp

gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/ComplexSchur_MKL.h

gtsam/3rdparty/Eigen/Eigen/src/Geometry/Hyperplane.h

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_template_int_rightCols.cpp

gtsam/3rdparty/Eigen/src/Core/IO.h

gtsam/3rdparty/Eigen/Eigen/src/misc/Solve.h

gtsam/3rdparty/Eigen/unsupported/Eigen/src/Skyline/SkylineMatrix.h

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_cwiseProduct.cpp

gtsam/3rdparty/Eigen/doc/examples/tut_arithmetic_redux_basic.cpp

gtsam/3rdparty/Eigen/src/Core/DenseStorage.h

gtsam/3rdparty/Eigen/Eigen/src/Core/util/Constants.h

gtsam/3rdparty/Eigen/Eigen/src/OrderingMethods/Ordering.h

gtsam/3rdparty/Eigen/doc/snippets/Tutorial_AdvancedInitialization_Join.cpp

gtsam/3rdparty/Eigen/lapack/dlapy3.f

gtsam/3rdparty/Eigen/blas/testing/sblat2.f

gtsam/3rdparty/Eigen/src/Eigen2Support/Block.h

gtsam/3rdparty/Eigen/bench/btl/libs/eigen3/main_vecmat.cpp

gtsam/3rdparty/Eigen/bench/btl/libs/hand_vec/CMakeLists.txt

gtsam/3rdparty/Eigen/bench/btl/libs/f77/daat.f

gtsam/3rdparty/Eigen/doc/snippets/JacobiSVD_basic.cpp

gtsam/3rdparty/Eigen/Eigen/src/SparseCore/CMakeLists.txt

gtsam/3rdparty/Eigen/doc/InsideEigenExample.dox

gtsam/3rdparty/Eigen/Eigen/src/plugins/MatrixCwiseUnaryOps.h

gtsam/3rdparty/Eigen/unsupported/Eigen/src/NonLinearOptimization/fdjac1.h

gtsam/3rdparty/Eigen/doc/examples/Tutorial_BlockOperations_vector.cpp

gtsam/3rdparty/Eigen/unsupported/Eigen/src/Polynomials/PolynomialUtils.h

gtsam/3rdparty/Eigen/Eigen/src/Core/Fuzzy.h

gtsam/3rdparty/Eigen/test/schur_real.cpp

gtsam/3rdparty/Eigen/doc/snippets/HessenbergDecomposition_compute.cpp

gtsam/3rdparty/Eigen/test/eigen2/eigen2_sparse_product.cpp

gtsam/3rdparty/Eigen/doc/snippets/SelfAdjointEigenSolver_SelfAdjointEigenSolver.cpp

gtsam/3rdparty/Eigen/src/Core/CommaInitializer.h

gtsam/3rdparty/Eigen/Eigen/src/Sparse/AmbiVector.h

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_inverse.cpp

gtsam/3rdparty/Eigen/src/Sparse/DynamicSparseMatrix.h

gtsam/3rdparty/Eigen/bench/btl/libs/STL/main.cpp

gtsam/3rdparty/Eigen/src/Sparse/SparseBlock.h

gtsam/3rdparty/Eigen/Eigen/src/Core/util/BlasUtil.h

gtsam/3rdparty/Eigen/bench/BenchTimer.h

gtsam/3rdparty/Eigen/Eigen/src/Sparse/SparseDenseProduct.h

gtsam/3rdparty/Eigen/blas/level2_impl.h

gtsam/3rdparty/Eigen/doc/examples/Tutorial_PartialLU_solve.cpp

gtsam/3rdparty/Eigen/doc/C01_TutorialMatrixClass.dox

gtsam/3rdparty/Eigen/src/Core/util/Memory.h

gtsam/3rdparty/Eigen/bench/btl/libs/mtl4/.kdbgrc.main

gtsam/3rdparty/Eigen/doc/snippets/Cwise_plus_equal.cpp

gtsam/3rdparty/Eigen/unsupported/Eigen/src/SVD/TODOBdcsvd.txt

gtsam/3rdparty/Eigen/unsupported/test/NonLinearOptimization.cpp

gtsam/3rdparty/Eigen/Eigen/src/Core/Map.h

gtsam/3rdparty/Eigen/Eigen/src/Core/ForceAlignedAccess.h

gtsam/3rdparty/Eigen/bench/product_threshold.cpp

gtsam/3rdparty/Eigen/doc/examples/TutorialLinAlgSVDSolve.cpp

gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD_MKL.h

gtsam/3rdparty/Eigen/unsupported/Eigen/OpenGLSupport

gtsam/3rdparty/Eigen/lapack/zlarfb.f

gtsam/3rdparty/Eigen/bench/btl/libs/BLAS/blas.h

gtsam/3rdparty/Eigen/Eigen/src/QR/FullPivHouseholderQR.h

gtsam/3rdparty/Eigen/doc/snippets/Tutorial_solve_triangular.cpp

gtsam/3rdparty/Eigen/doc/examples/Tutorial_ReductionsVisitorsBroadcasting_broadcast_1nn.cpp

gtsam/3rdparty/Eigen/Eigen/src/Core/Product.h

gtsam/3rdparty/Eigen/bench/btl/actions/action_aat_product.hh

gtsam/3rdparty/Eigen/bench/btl/libs/gmm/CMakeLists.txt

gtsam/3rdparty/Eigen/doc/snippets/Matrix_resize_int_int.cpp

gtsam/3rdparty/Eigen/Eigen/src/SPQRSupport/SuiteSparseQRSupport.h

gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/Geometry/RotationBase.h

gtsam/3rdparty/Eigen/doc/snippets/FullPivLU_solve.cpp

gtsam/3rdparty/Eigen/Eigen/Cholesky

gtsam/3rdparty/Eigen/doc/examples/function_taking_eigenbase.cpp

gtsam/3rdparty/Eigen/bench/BenchUtil.h

gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseProduct.h

gtsam/3rdparty/Eigen/test/eigen2/product.h

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_bottomRightCorner_int_int.cpp

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_cwiseEqual.cpp

gtsam/3rdparty/Eigen/unsupported/doc/snippets/CMakeLists.txt

gtsam/3rdparty/Eigen/unsupported/Eigen/src/Skyline/SkylineStorage.h

gtsam/3rdparty/Eigen/doc/snippets/FullPivHouseholderQR_solve.cpp

gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h

gtsam/3rdparty/Eigen/test/testsuite.cmake

gtsam/3rdparty/Eigen/test/geo_orthomethods.cpp

gtsam/3rdparty/gtsam_eigen_includes.h.in

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_template_int_int_bottomRightCorner.cpp

gtsam/3rdparty/Eigen/Eigen/src/Sparse/SparseSparseProduct.h

gtsam/3rdparty/Eigen/test/exceptions.cpp

gtsam/3rdparty/Eigen/doc/examples/CMakeLists.txt

gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/MatrixBaseEigenvalues.h

gtsam/3rdparty/Eigen/unsupported/doc/Doxyfile.in

gtsam/3rdparty/Eigen/failtest/const_qualified_diagonal_method_retval.cpp

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_cwiseNotEqual.cpp

gtsam/3rdparty/Eigen/Eigen/src/Core/ReturnByValue.h

gtsam/3rdparty/Eigen/lapack/cholesky.cpp

gtsam/3rdparty/Eigen/bench/btl/libs/hand_vec/main.cpp

gtsam/3rdparty/Eigen/Eigen/Array

gtsam/3rdparty/Eigen/Eigen/src/Core/Assign_MKL.h

gtsam/3rdparty/Eigen/test/eigen2/eigen2_eigensolver.cpp

gtsam/3rdparty/Eigen/src/Core/products/SelfadjointRank2Update.h

gtsam/3rdparty/Eigen/doc/examples/Tutorial_ArrayClass_interop.cpp

gtsam/3rdparty/Eigen/test/array.cpp

gtsam/3rdparty/Eigen/blas/common.h

gtsam/3rdparty/Eigen/test/spqr_support.cpp

gtsam/3rdparty/Eigen/doc/examples/TutorialLinAlgInverseDeterminant.cpp

gtsam/3rdparty/Eigen/Eigen/src/Sparse/SparseCwiseBinaryOp.h

gtsam/3rdparty/Eigen/src/Eigen2Support/TriangularSolver.h

gtsam/3rdparty/Eigen/doc/examples/class_VectorBlock.cpp

gtsam/3rdparty/CCOLAMD/MATLAB/csymamdtestmex.m

gtsam/3rdparty/ccolamd/ccolamd.h

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_topRows_int.cpp

gtsam/3rdparty/Eigen/doc/snippets/Tridiagonalization_Tridiagonalization_MatrixType.cpp

gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointMatrixMatrix.h

gtsam/3rdparty/CCOLAMD/MATLAB/csymamdtestmex.c

gtsam/3rdparty/Eigen/unsupported/Eigen/src/LevenbergMarquardt/LevenbergMarquardt.h

gtsam/3rdparty/ccolamd/ccolamd.c

gtsam/3rdparty/Eigen/failtest/const_qualified_block_method_retval_1.cpp

gtsam/3rdparty/Eigen/src/Core/arch/Default/CMakeLists.txt

gtsam/3rdparty/Eigen/doc/TopicResizing.dox

gtsam/3rdparty/ccolamd/core_ufconfig_Makefile.mk

gtsam/3rdparty/Eigen/src/LU/arch/Makefile.am

gtsam/3rdparty/Eigen/src/Core/GenericPacketMath.h

gtsam/3rdparty/Eigen/Eigen/src/Core/Reverse.h

gtsam/3rdparty/Eigen/Eigen/src/CholmodSupport/CMakeLists.txt

gtsam/3rdparty/Eigen/unsupported/Eigen/src/FFT/ei_fftw_impl.h

gtsam/3rdparty/Eigen/doc/snippets/EigenSolver_compute.cpp

gtsam/3rdparty/Eigen/bench/btl/cmake/MacroOptionalAddSubdirectory.cmake

gtsam/3rdparty/Eigen/bench/btl/cmake/FindGMM.cmake

gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/Tridiagonalization.h

gtsam/3rdparty/Eigen/unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h

gtsam/3rdparty/Eigen/Eigen/src/Core/Dot.h

gtsam/3rdparty/CCOLAMD/Demo/my_ccolamd_example.out

gtsam/3rdparty/Eigen/src/Core/products/Parallelizer.h

gtsam/3rdparty/Eigen/bench/btl/libs/mtl4/mtl4_LU_solve_interface.hh

gtsam/3rdparty/Eigen/failtest/block_nonconst_ctor_on_const_xpr_1.cpp

gtsam/3rdparty/Eigen/src/Geometry/AlignedBox.h

gtsam/3rdparty/Eigen/unsupported/Eigen/ArpackSupport

gtsam/3rdparty/Eigen/Eigen/src/StlSupport/CMakeLists.txt

gtsam/3rdparty/Eigen/src/Core/arch/NEON/Complex.h

gtsam/3rdparty/Eigen/lapack/ilaslr.f

gtsam/3rdparty/Eigen/lapack/dlarft.f

gtsam/3rdparty/Eigen/doc/snippets/GeneralizedEigenSolver.cpp

gtsam/3rdparty/Eigen/src/QR/HouseholderQR.h

gtsam/3rdparty/Eigen/lapack/dlarfb.f

gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/GeneralizedEigenSolver.h

gtsam/3rdparty/Eigen/Eigen/src/StlSupport/details.h

gtsam/3rdparty/Eigen/src/Core/SolveTriangular.h

gtsam/3rdparty/Eigen/doc/I16_TemplateKeyword.dox

gtsam/3rdparty/Eigen/failtest/map_on_const_type_actually_const_1.cpp

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_template_int_int_topLeftCorner_int_int.cpp

gtsam/3rdparty/Eigen/test/umeyama.cpp

gtsam/3rdparty/Eigen/doc/examples/QuickStart_example2_fixed.cpp

gtsam/3rdparty/ccolamd/LGPL.txt

gtsam/3rdparty/Eigen/Eigen/src/LU/arch/Inverse_SSE.h

gtsam/3rdparty/Eigen/test/eigen2/eigen2_sparse_basic.cpp

gtsam/3rdparty/Eigen/src/plugins/Makefile.am

gtsam/3rdparty/Eigen/Eigen/CholmodSupport

gtsam/3rdparty/Eigen/blas/README.txt

gtsam/3rdparty/Eigen/bench/btl/libs/eigen2/main_vecmat.cpp

gtsam/3rdparty/Eigen/Eigen/src/Core/MathFunctions.h

gtsam/3rdparty/Eigen/src/plugins/CMakeLists.txt

gtsam/3rdparty/Eigen/unsupported/Eigen/src/LevenbergMarquardt/LMonestep.h

gtsam/3rdparty/Eigen/bench/benchmarkX.cpp

gtsam/3rdparty/Eigen/demos/mix_eigen_and_c/binary_library.cpp

gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/LeastSquares.h

gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_kernel_bmod.h

gtsam/3rdparty/Eigen/unsupported/README.txt

gtsam/3rdparty/Eigen/demos/opengl/trackball.h

gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/BiCGSTAB.h

gtsam/3rdparty/Eigen/bench/sparse_cholesky.cpp

gtsam/3rdparty/Eigen/src/Core/DenseBase.h

gtsam/3rdparty/Eigen/cmake/EigenConfigureTesting.cmake

gtsam/3rdparty/Eigen/unsupported/Eigen/KroneckerProduct

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_leftCols_int.cpp

gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/Meta.h

gtsam/3rdparty/Eigen/lapack/dladiv.f

gtsam/3rdparty/Eigen/bench/btl/libs/eigen3/main_matmat.cpp

gtsam/3rdparty/Eigen/unsupported/Eigen/src/NonLinearOptimization/qrsolv.h

gtsam/3rdparty/Eigen/src/Eigen2Support/MathFunctions.h

gtsam/3rdparty/Eigen/src/Core/Functors.h

gtsam/3rdparty/Eigen/test/cwiseop.cpp

gtsam/3rdparty/Eigen/test/jacobisvd.cpp

gtsam/3rdparty/Eigen/Eigen/src/QR/CMakeLists.txt

gtsam/3rdparty/Eigen/test/basicstuff.cpp

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_replicate_int_int.cpp

gtsam/3rdparty/Eigen/Eigen/src/Core/EigenBase.h

gtsam/3rdparty/Eigen/src/Core/Stride.h

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_random_int.cpp

gtsam/3rdparty/Eigen/unsupported/test/minres.cpp

gtsam/3rdparty/Eigen/demos/mandelbrot/CMakeLists.txt

gtsam/3rdparty/Eigen/blas/CMakeLists.txt

gtsam/3rdparty/Eigen/Eigen/src/StlSupport/StdVector.h

gtsam/3rdparty/Eigen/unsupported/doc/examples/MatrixExponential.cpp

gtsam/3rdparty/Eigen/bench/btl/actions/action_lu_decomp.hh

gtsam/3rdparty/Eigen/doc/snippets/PartialRedux_count.cpp

gtsam/3rdparty/Eigen/unsupported/Eigen/src/AutoDiff/AutoDiffJacobian.h

gtsam/3rdparty/Eigen/doc/snippets/SelfAdjointEigenSolver_compute_MatrixType2.cpp

gtsam/3rdparty/Eigen/unsupported/doc/Overview.dox

gtsam/3rdparty/Eigen/Eigen/src/Sparse/SparseMatrixBase.h

gtsam/3rdparty/Eigen/blas/testing/cblat3.dat

gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixLogarithm.h

gtsam/3rdparty/Eigen/doc/snippets/Cwise_log.cpp

gtsam/3rdparty/Eigen/blas/chpr.f

gtsam/3rdparty/Eigen/Eigen/src/Core/Random.h

gtsam/3rdparty/Eigen/src/Core/DiagonalMatrix.h

gtsam/3rdparty/Eigen/Eigen/src/OrderingMethods/Amd.h

gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseUtil.h

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_identity.cpp

gtsam/3rdparty/Eigen/Eigen/src/Core/CoreIterators.h

gtsam/3rdparty/Eigen/doc/TutorialArrayClass.dox

gtsam/3rdparty/Eigen/Eigen/Eigen2Support

gtsam/3rdparty/Eigen/test/lu.cpp

gtsam/3rdparty/Eigen/unsupported/test/matrix_power.cpp

gtsam/3rdparty/Eigen/lapack/slapy2.f

gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/Geometry/Rotation2D.h

gtsam/3rdparty/Eigen/failtest/map_nonconst_ctor_on_const_ptr_4.cpp

gtsam/3rdparty/Eigen/Eigen/src/MetisSupport/CMakeLists.txt

gtsam/3rdparty/Eigen/Eigen/src/Sparse/SparseBlock.h

gtsam/3rdparty/Eigen/test/packetmath.cpp

gtsam/3rdparty/Eigen/unsupported/Eigen/src/MoreVectorization/MathFunctions.h

gtsam/3rdparty/Eigen/Eigen/src/Core/arch/CMakeLists.txt

gtsam/3rdparty/ccolamd/ChangeLog

gtsam/3rdparty/Eigen/bench/btl/actions/action_cholesky.hh

gtsam/3rdparty/Eigen/test/eigen2support.cpp

gtsam/3rdparty/Eigen/test/umfpack_support.cpp

gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralBlockPanelKernel.h

gtsam/3rdparty/Eigen/doc/snippets/PartialRedux_prod.cpp

gtsam/3rdparty/Eigen/bench/btl/actions/action_hessenberg.hh

gtsam/3rdparty/Eigen/test/map.cpp

gtsam/3rdparty/Eigen/doc/examples/Tutorial_BlockOperations_colrow.cpp

gtsam/3rdparty/Eigen/bench/btl/actions/action_trisolve_matrix.hh

gtsam/3rdparty/Eigen/bench/btl/libs/f77/daxpy.f

gtsam/3rdparty/Eigen/doc/I01_TopicLazyEvaluation.dox

gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointMatrixVector_MKL.h

gtsam/3rdparty/CCOLAMD/MATLAB/luflops.m

gtsam/3rdparty/Eigen/Eigen/src/Core/util/XprHelper.h

gtsam/3rdparty/Eigen/lapack/ilazlc.f

gtsam/3rdparty/Eigen/src/Core/StableNorm.h

gtsam/3rdparty/Eigen/bench/btl/actions/action_axpby.hh

gtsam/3rdparty/ccolamd/ccolamd_global.c

gtsam/3rdparty/Eigen/test/eigen2/eigen2_lu.cpp

gtsam/3rdparty/Eigen/.hg_archival.txt

gtsam/3rdparty/Eigen/Eigen/src/Core/util/Macros.h

gtsam/3rdparty/Eigen/unsupported/Eigen/src/SparseExtra/Amd.h

gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/MathFunctions.h

gtsam/3rdparty/Eigen/bench/spbench/spbenchsolver.cpp

gtsam/3rdparty/CCOLAMD/Demo/ccolamd_l_example.out

gtsam/3rdparty/Eigen/unsupported/doc/CMakeLists.txt

gtsam/3rdparty/Eigen/Eigen/src/misc/CMakeLists.txt

gtsam/3rdparty/Eigen/blas/drotm.f

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_setZero.cpp

gtsam/3rdparty/Eigen/test/nullary.cpp

gtsam/3rdparty/Eigen/src/Core/Random.h

gtsam/3rdparty/Eigen/src/Eigen2Support/Memory.h

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_setIdentity.cpp

gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularSolverMatrix_MKL.h

gtsam/3rdparty/Eigen/doc/examples/Tutorial_BlockOperations_block_assignment.cpp

gtsam/3rdparty/Eigen/Eigen/src/Core/SelfCwiseBinaryOp.h

gtsam/3rdparty/CMakeLists.txt

gtsam/3rdparty/Eigen/doc/snippets/Vectorwise_reverse.cpp

gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/IterativeSolverBase.h

gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointMatrixMatrix_MKL.h

gtsam/3rdparty/Eigen/unsupported/Eigen/src/NumericalDiff/CMakeLists.txt

gtsam/3rdparty/CCOLAMD/Demo/ccolamd_l_example.c

gtsam/3rdparty/Eigen/Eigen/src/Core/util/DisableStupidWarnings.h

gtsam/3rdparty/Eigen/doc/TutorialGeometry.dox

gtsam/3rdparty/Eigen/blas/ssbmv.f

gtsam/3rdparty/Eigen/doc/snippets/RealQZ_compute.cpp

gtsam/3rdparty/Eigen/test/corners.cpp

gtsam/3rdparty/Eigen/cmake/FindScotch.cmake

gtsam/3rdparty/Eigen/doc/snippets/Matrix_setOnes_int.cpp

gtsam/3rdparty/Eigen/src/Core/arch/NEON/CMakeLists.txt

gtsam/3rdparty/Eigen/lapack/clacgv.f

gtsam/3rdparty/Eigen/Eigen/src/Core/Transpose.h

gtsam/3rdparty/Eigen/unsupported/test/mpreal/mpreal.h

gtsam/3rdparty/Eigen/src/Core/VectorwiseOp.h

gtsam/3rdparty/Eigen/src/Eigen2Support/Minor.h

gtsam/3rdparty/Eigen/bench/benchBlasGemm.cpp

gtsam/3rdparty/CCOLAMD/Source/ccolamd.c

gtsam/3rdparty/Eigen/Eigen/src/Core/MatrixBase.h

gtsam/3rdparty/Eigen/doc/TemplateKeyword.dox

gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/IncompleteCholesky.h

gtsam/3rdparty/Eigen/doc/snippets/Cwise_less_equal.cpp

gtsam/3rdparty/Eigen/unsupported/test/splines.cpp

gtsam/3rdparty/Eigen/unsupported/Eigen/src/SparseExtra/BlockOfDynamicSparseMatrix.h

gtsam/3rdparty/Eigen/bench/btl/libs/C_BLAS/main.cpp

gtsam/3rdparty/Eigen/doc/examples/Tutorial_ReductionsVisitorsBroadcasting_visitors.cpp

gtsam/3rdparty/Eigen/src/Core/products/TriangularSolverMatrix.h

gtsam/3rdparty/Eigen/unsupported/doc/examples/MatrixSinh.cpp

gtsam/3rdparty/Eigen/src/Core/Product.h

gtsam/3rdparty/Eigen/doc/I10_Assertions.dox

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_cwiseQuotient.cpp

gtsam/3rdparty/Eigen/cmake/EigenTesting.cmake

gtsam/3rdparty/Eigen/Eigenvalues

gtsam/3rdparty/Eigen/unsupported/Eigen/src/SparseExtra/MarketIO.h

gtsam/3rdparty/Eigen/blas/ctbmv.f

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_cwiseAbs2.cpp

gtsam/3rdparty/Eigen/Eigen/MetisSupport

gtsam/3rdparty/Eigen/blas/testing/dblat2.dat

gtsam/3rdparty/Eigen/src/Geometry/EulerAngles.h

gtsam/3rdparty/Eigen/src/plugins/MatrixCwiseUnaryOps.h

gtsam/3rdparty/Eigen/unsupported/Eigen/src/Eigenvalues/ArpackSelfAdjointEigenSolver.h

gtsam/3rdparty/Eigen/Eigen/SPQRSupport

gtsam/3rdparty/Eigen/src/Sparse/CoreIterators.h

gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointMatrixVector.h

gtsam/3rdparty/Eigen/unsupported/Eigen/src/NonLinearOptimization/covar.h

gtsam/3rdparty/Eigen/cmake/FindSPQR.cmake

gtsam/3rdparty/Eigen/doc/snippets/Cwise_exp.cpp

gtsam/3rdparty/Eigen/src/QR/FullPivHouseholderQR.h

gtsam/3rdparty/Eigen/unsupported/test/sparse_lu.cpp

gtsam/3rdparty/Eigen/bench/btl/actions/action_axpy.hh

gtsam/3rdparty/Eigen/Eigen/SparseCore

gtsam/3rdparty/Eigen/unsupported/Eigen/LevenbergMarquardt

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_template_int_topRows.cpp

gtsam/3rdparty/Eigen/test/pastix_support.cpp

gtsam/3rdparty/Eigen/bench/btl/actions/action_trisolve.hh

gtsam/3rdparty/Eigen/Eigen/src/SparseCore/CoreIterators.h

gtsam/3rdparty/Eigen/src/Core/Transpositions.h

gtsam/3rdparty/Eigen/Eigen/src/Core/Transpositions.h

gtsam/3rdparty/Eigen/Eigen/src/Geometry/Transform.h

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_random.cpp

gtsam/3rdparty/Eigen/src/Core/arch/NEON/Makefile.am

gtsam/3rdparty/Eigen/src/Eigenvalues/GeneralizedSelfAdjointEigenSolver.h

gtsam/3rdparty/Eigen/src/Core/GlobalFunctions.h

gtsam/3rdparty/Eigen/doc/eigendoxy.css

gtsam/3rdparty/Eigen/blas/testing/cblat2.dat

gtsam/3rdparty/Eigen/bench/btl/actions/action_trmm.hh

gtsam/3rdparty/Eigen/doc/TopicScalarTypes.dox

gtsam/3rdparty/Eigen/test/simplicial_cholesky.cpp

gtsam/3rdparty/Eigen/src/Core/arch/SSE/Makefile.am

gtsam/3rdparty/Eigen/doc/LinearLeastSquares.dox

gtsam/3rdparty/Eigen/src/Eigen2Support/Geometry/ParametrizedLine.h

gtsam/3rdparty/Eigen/demos/mix_eigen_and_c/binary_library.h

gtsam/3rdparty/Eigen/doc/ClassHierarchy.dox

gtsam/3rdparty/Eigen/Eigen/src/OrderingMethods/CMakeLists.txt

gtsam/3rdparty/Eigen/cmake/CMakeDetermineVSServicePack.cmake

gtsam/3rdparty/Eigen/Eigen/src/Core/Assign.h

gtsam/3rdparty/Eigen/unsupported/Eigen/AlignedVector3

gtsam/3rdparty/Eigen/demos/opengl/CMakeLists.txt

gtsam/3rdparty/Eigen/doc/snippets/Cwise_max.cpp

gtsam/3rdparty/Eigen/test/diagonal.cpp

gtsam/3rdparty/Eigen/Eigen/src/Geometry/EulerAngles.h

gtsam/3rdparty/Eigen/Eigen/LeastSquares

gtsam/3rdparty/Eigen/src/Sparse/SparseCwiseBinaryOp.h

gtsam/3rdparty/Eigen/cmake/FindEigen3.cmake

gtsam/3rdparty/Eigen/test/vectorwiseop.cpp

gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/ConstrainedConjGrad.h

gtsam/3rdparty/Eigen/doc/snippets/Cwise_acos.cpp

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_eigenvalues.cpp

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_template_int_int_topLeftCorner.cpp

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_replicate.cpp

gtsam/3rdparty/Eigen/failtest/block_nonconst_ctor_on_const_xpr_0.cpp

gtsam/3rdparty/Eigen/blas/drotmg.f

gtsam/3rdparty/Eigen/Eigen/src/Jacobi/CMakeLists.txt

gtsam/3rdparty/Eigen/doc/snippets/Map_simple.cpp

gtsam/3rdparty/Eigen/src/Core/Fuzzy.h

gtsam/3rdparty/Eigen/doc/TopicLinearAlgebraDecompositions.dox

gtsam/3rdparty/Eigen/unsupported/doc/examples/MatrixSquareRoot.cpp

gtsam/3rdparty/Eigen/cmake/FindPastix.cmake

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_isUnitary.cpp

gtsam/3rdparty/Eigen/lapack/slarfb.f

gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseBinaryOp.h

gtsam/3rdparty/Eigen/test/meta.cpp

gtsam/3rdparty/Eigen/bench/btl/generic_bench/timers/x86_timer.hh

gtsam/3rdparty/Eigen/Eigen2Support

gtsam/3rdparty/Eigen/test/qr_fullpivoting.cpp

gtsam/3rdparty/Eigen/blas/complexdots.f

gtsam/3rdparty/Eigen/doc/snippets/Tutorial_AdvancedInitialization_Zero.cpp

gtsam/3rdparty/Eigen/cmake/FindMPFR.cmake

gtsam/3rdparty/Eigen/test/product_notemporary.cpp

gtsam/3rdparty/Eigen/doc/snippets/Cwise_greater_equal.cpp

gtsam/3rdparty/CCOLAMD/MATLAB/csymamdmex.c

gtsam/3rdparty/Eigen/unsupported/Eigen/src/Splines/SplineFwd.h

gtsam/3rdparty/Eigen/test/sparse_permutations.cpp

gtsam/3rdparty/Eigen/doc/snippets/Cwise_sqrt.cpp

gtsam/3rdparty/Eigen/doc/snippets/Matrix_setOnes_int_int.cpp

gtsam/3rdparty/Eigen/doc/snippets/Cwise_sin.cpp

gtsam/3rdparty/Eigen/doc/examples/Tutorial_ArrayClass_mult.cpp

gtsam/3rdparty/Eigen/test/eigensolver_generic.cpp

gtsam/3rdparty/Eigen/doc/snippets/Matrix_resize_int_NoChange.cpp

gtsam/3rdparty/Eigen/src/Core/util/XprHelper.h

gtsam/3rdparty/Eigen/bench/btl/actions/action_matrix_vector_product.hh

gtsam/3rdparty/Eigen/Eigen/src/Core/util/CMakeLists.txt

gtsam/3rdparty/Eigen/src/misc/CMakeLists.txt

gtsam/3rdparty/Eigen/failtest/transpose_on_const_type_actually_const.cpp

gtsam/3rdparty/Eigen/blas/ctpsv.f

gtsam/3rdparty/Eigen/test/upperbidiagonalization.cpp

gtsam/3rdparty/Eigen/unsupported/Eigen/src/BVH/BVAlgorithms.h

gtsam/3rdparty/Eigen/demos/mix_eigen_and_c/README

gtsam/3rdparty/Eigen/Eigen/src/CholmodSupport/CholmodSupport.h

gtsam/3rdparty/Eigen/Eigen/src/Sparse/SparseCwiseUnaryOp.h

gtsam/3rdparty/Eigen/bench/btl/cmake/FindMKL.cmake

gtsam/3rdparty/Eigen/Eigen/src/Core/Ref.h

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_topRightCorner_int_int.cpp

gtsam/3rdparty/Eigen/bench/btl/libs/tvmet/tvmet_interface.hh

gtsam/3rdparty/Eigen/src/Eigenvalues/ComplexEigenSolver.h

gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseBlock.h

gtsam/3rdparty/Eigen/src/Core/DenseStorageBase.h

gtsam/3rdparty/Eigen/unsupported/Eigen/src/NonLinearOptimization/HybridNonLinearSolver.h

gtsam/3rdparty/Eigen/demos/CMakeLists.txt

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_isIdentity.cpp

gtsam/3rdparty/Eigen/test/product_trmm.cpp

gtsam/3rdparty/Eigen/blas/zhpr.f

gtsam/3rdparty/Eigen/Eigen/src/SVD/CMakeLists.txt

gtsam/3rdparty/Eigen/blas/srotmg.f

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_template_int_leftCols.cpp

gtsam/3rdparty/Eigen/Eigen/src/Sparse/CoreIterators.h

gtsam/3rdparty/Eigen/doc/TutorialSparse.dox

gtsam/3rdparty/Eigen/bench/btl/generic_bench/utils/utilities.h

gtsam/3rdparty/Eigen/Eigen/src/Core/NestByValue.h

gtsam/3rdparty/Eigen/Cholesky

gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseSparseProductWithPruning.h

gtsam/3rdparty/Eigen/unsupported/Eigen/SuperLUSupport

gtsam/3rdparty/Eigen/bench/btl/data/mean.cxx

gtsam/3rdparty/Eigen/unsupported/Eigen/src/SparseExtra/SparseLLT.h

gtsam/3rdparty/Eigen/Eigen/src/LU/PartialPivLU.h

gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/Complex.h

gtsam/3rdparty/Eigen/unsupported/Eigen/src/SparseExtra/UmfPackSupport.h

gtsam/3rdparty/Eigen/doc/snippets/SelfAdjointView_operatorNorm.cpp

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_eval.cpp

gtsam/3rdparty/Eigen/src/Sparse/Makefile.am

gtsam/3rdparty/Eigen/doc/snippets/Cwise_plus.cpp

gtsam/3rdparty/Eigen/blas/level2_cplx_impl.h

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_cwiseInverse.cpp

gtsam/3rdparty/Eigen/doc/snippets/Tutorial_solve_matrix_inverse.cpp

gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseTriangularView.h

gtsam/3rdparty/Eigen/bench/btl/libs/gmm/main.cpp

gtsam/3rdparty/Eigen/src/Core/Select.h

gtsam/3rdparty/Eigen/bench/bench_multi_compilers.sh

gtsam/3rdparty/Eigen/Eigen/src/Householder/HouseholderSequence.h

gtsam/3rdparty/Eigen/src/Core/util/ForwardDeclarations.h

gtsam/3rdparty/Eigen/src/Geometry/Transform.h

gtsam/3rdparty/Eigen/demos/opengl/gpuhelper.h

gtsam/3rdparty/Eigen/blas/level3_impl.h

gtsam/3rdparty/Eigen/test/vectorization_logic.cpp

gtsam/3rdparty/Eigen/bench/benchmark_suite

gtsam/3rdparty/Eigen/src/Core/ForceAlignedAccess.h

gtsam/3rdparty/Eigen/test/product_extra.cpp

gtsam/3rdparty/Eigen/Eigen/src/Core/CMakeLists.txt

gtsam/3rdparty/Eigen/doc/TopicAliasing.dox

gtsam/3rdparty/Eigen/test/block.cpp

gtsam/3rdparty/Eigen/doc/snippets/Cwise_slash_equal.cpp

gtsam/3rdparty/Eigen/blas/chpr2.f

gtsam/3rdparty/Eigen/Eigen/src/Core/products/CoeffBasedProduct.h

gtsam/3rdparty/Eigen/unsupported/test/kronecker_product.cpp

gtsam/3rdparty/Eigen/doc/UsingIntelMKL.dox

gtsam/3rdparty/Eigen/cmake/FindFFTW.cmake

gtsam/3rdparty/Eigen/src/plugins/CommonCwiseUnaryOps.h

gtsam/3rdparty/Eigen/src/Sparse/SparseView.h

gtsam/3rdparty/Eigen/bench/btl/data/smooth.cxx

gtsam/3rdparty/Eigen/unsupported/Eigen/src/FFT/ei_kissfft_impl.h

gtsam/3rdparty/Eigen/bench/btl/generic_bench/timers/mixed_perf_analyzer.hh

gtsam/3rdparty/Eigen/Eigen/src/SparseCholesky/SimplicialCholesky_impl.h

gtsam/3rdparty/Eigen/cmake/FindAdolc.cmake

gtsam/3rdparty/Eigen/unsupported/Eigen/src/LevenbergMarquardt/CopyrightMINPACK.txt

gtsam/3rdparty/Eigen/Eigen/src/Core/DenseCoeffsBase.h

gtsam/3rdparty/Eigen/doc/snippets/Cwise_boolean_and.cpp

gtsam/3rdparty/Eigen/blas/ctpmv.f

gtsam/3rdparty/CCOLAMD/MATLAB/Contents.m

gtsam/3rdparty/Eigen/src/Sparse/SparseMatrix.h

gtsam/3rdparty/Eigen/INSTALL

gtsam/3rdparty/Eigen/doc/snippets/SelfAdjointView_eigenvalues.cpp

gtsam/3rdparty/Eigen/src/Eigen2Support/CMakeLists.txt

gtsam/3rdparty/Eigen/test/sizeof.cpp

gtsam/3rdparty/Eigen/failtest/block_on_const_type_actually_const_0.cpp

gtsam/3rdparty/Eigen/bench/spbench/sp_solver.cpp

gtsam/3rdparty/Eigen/doc/snippets/CMakeLists.txt

gtsam/3rdparty/Eigen/doc/snippets/Cwise_tan.cpp

gtsam/3rdparty/Eigen/lapack/lapack_common.h

gtsam/3rdparty/Eigen/src/Eigenvalues/EigenSolver.h

gtsam/3rdparty/CCOLAMD/MATLAB/ccolamd_install.m

gtsam/3rdparty/Eigen/test/hessenberg.cpp

gtsam/3rdparty/CCOLAMD/MATLAB/ccolamd_demo.m

gtsam/3rdparty/Eigen/doc/snippets/tut_arithmetic_transpose_conjugate.cpp

gtsam/3rdparty/Eigen/blas/PackedSelfadjointProduct.h

gtsam/3rdparty/Eigen/LU

gtsam/3rdparty/Eigen/Makefile.am

gtsam/3rdparty/Eigen/bench/btl/data/go_mean

gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixVector.h

gtsam/3rdparty/Eigen/src/Core/arch/NEON/PacketMath.h

gtsam/3rdparty/Eigen/Eigen/src/SuperLUSupport/SuperLUSupport.h

gtsam/3rdparty/Eigen/Eigen/Eigenvalues

gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/GMRES.h

gtsam/3rdparty/Eigen/doc/C07_TutorialReductionsVisitorsBroadcasting.dox

gtsam/3rdparty/Eigen/test/eigen2/eigen2_alignedbox.cpp

gtsam/3rdparty/Eigen/doc/examples/TutorialLinAlgSelfAdjointEigenSolver.cpp

gtsam/3rdparty/Eigen/Eigen/src/Core/DiagonalMatrix.h

gtsam/3rdparty/Eigen/blas/level1_real_impl.h

gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR.h

gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseUnaryOp.h

gtsam/3rdparty/Eigen/Eigen/src/OrderingMethods/Eigen_Colamd.h

gtsam/3rdparty/Eigen/bench/btl/libs/f77/dmxm.f.mfr

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_part.cpp

gtsam/3rdparty/Eigen/doc/TutorialBlockOperations.dox

gtsam/3rdparty/Eigen/test/jacobi.cpp

gtsam/3rdparty/Eigen/bench/BenchSparseUtil.h

gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularMatrixVector.h

gtsam/3rdparty/Eigen/Eigen/src/Core/util/Meta.h

gtsam/3rdparty/Eigen/Eigen/src/plugins/ArrayCwiseUnaryOps.h

gtsam/3rdparty/Eigen/src/LU/Inverse.h

gtsam/3rdparty/Eigen/lapack/eigenvalues.cpp

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_ones_int.cpp

gtsam/3rdparty/Eigen/Eigen/src/PaStiXSupport/PaStiXSupport.h

gtsam/3rdparty/Eigen/src/Core/NumTraits.h

gtsam/3rdparty/Eigen/test/eigensolver_complex.cpp

gtsam/3rdparty/Eigen/src/Core/CwiseUnaryView.h

gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/SVD.h

gtsam/3rdparty/Eigen/bench/btl/libs/C/C_interface.hh

gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/Geometry/AngleAxis.h

gtsam/3rdparty/Eigen/test/eigen2/eigen2_map.cpp

gtsam/3rdparty/Eigen/scripts/check.in

gtsam/3rdparty/Eigen/test/prec_inverse_4x4.cpp

gtsam/3rdparty/Eigen/bench/btl/libs/f77/test_interface.hh

gtsam/3rdparty/Eigen/src/plugins/ArrayCwiseUnaryOps.h

gtsam/3rdparty/Eigen/src/Core/util/CMakeLists.txt

gtsam/3rdparty/Eigen/doc/snippets/EigenSolver_eigenvectors.cpp

gtsam/3rdparty/Eigen/bench/btl/libs/gmm/gmm_LU_solve_interface.hh

gtsam/3rdparty/Eigen/src/Eigenvalues/ComplexSchur.h

gtsam/3rdparty/Eigen/Eigen/IterativeLinearSolvers

gtsam/3rdparty/Eigen/src/Householder/HouseholderSequence.h

gtsam/3rdparty/Eigen/bench/btl/libs/gmm/gmm_interface.hh

gtsam/3rdparty/Eigen/test/unalignedassert.cpp

gtsam/3rdparty/Eigen/unsupported/Eigen/src/SVD/JacobiSVD.h

gtsam/3rdparty/Eigen/src/Eigenvalues/Tridiagonalization.h

gtsam/3rdparty/Eigen/src/Core/ArrayBase.h

gtsam/3rdparty/Eigen/doc/examples/TutorialLinAlgRankRevealing.cpp

gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseUnaryView.h

gtsam/3rdparty/Eigen/src/Eigen2Support/Geometry/All.h

gtsam/3rdparty/Eigen/src/Core/products/GeneralMatrixVector.h

gtsam/3rdparty/Eigen/lapack/ilaclr.f

gtsam/3rdparty/Eigen/Jacobi

gtsam/3rdparty/Eigen/doc/I02_HiPerformance.dox

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_ones.cpp

gtsam/3rdparty/Eigen/src/Geometry/Umeyama.h

gtsam/3rdparty/Eigen/doc/snippets/Tutorial_solve_reuse_decomposition.cpp

gtsam/3rdparty/Eigen/test/bandmatrix.cpp

gtsam/3rdparty/Eigen/Eigen/PardisoSupport

gtsam/3rdparty/Eigen/Eigen/src/Core/GeneralProduct.h

gtsam/3rdparty/Eigen/scripts/eigen_gen_credits.cpp

gtsam/3rdparty/Eigen/blas/testing/cblat1.f

gtsam/3rdparty/Eigen/doc/snippets/Jacobi_makeJacobi.cpp

gtsam/3rdparty/Eigen/src/Core/util/DisableMSVCWarnings.h

gtsam/3rdparty/Eigen/doc/special_examples/CMakeLists.txt

gtsam/3rdparty/Eigen/test/mapped_matrix.cpp

gtsam/3rdparty/Eigen/doc/examples/DenseBase_middleRows_int.cpp

gtsam/3rdparty/Eigen/blas/testing/cblat3.f

gtsam/3rdparty/Eigen/src/Eigen2Support/Geometry/RotationBase.h

gtsam/3rdparty/Eigen/blas/dsbmv.f

gtsam/3rdparty/Eigen/test/eigen2/eigen2_geometry_with_eigen2_prefix.cpp

gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_Structs.h

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_template_int_end.cpp

gtsam/3rdparty/Eigen/doc/examples/Tutorial_ReductionsVisitorsBroadcasting_reductions_bool.cpp

gtsam/3rdparty/Eigen/doc/examples/Tutorial_ArrayClass_addition.cpp

gtsam/3rdparty/Eigen/blas/chpmv.f

gtsam/3rdparty/Eigen/test/geo_parametrizedline.cpp

gtsam/3rdparty/Eigen/bench/spmv.cpp

gtsam/3rdparty/Eigen/src/Eigen2Support/Makefile.am

gtsam/3rdparty/Eigen/doc/D07_PassingByValue.dox

gtsam/3rdparty/Eigen/lapack/iladlr.f

gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixSquareRoot.h

gtsam/3rdparty/Eigen/doc/examples/tut_arithmetic_dot_cross.cpp

gtsam/3rdparty/Eigen/src/Makefile.am

gtsam/3rdparty/Eigen/test/eigen2/eigen2_sparse_vector.cpp

gtsam/3rdparty/Eigen/unsupported/Eigen/src/FFT/CMakeLists.txt

gtsam/3rdparty/Eigen/blas/dspmv.f

gtsam/3rdparty/Eigen/test/runtest.sh

gtsam/3rdparty/Eigen/doc/TutorialMapClass.dox

gtsam/3rdparty/Eigen/src/LU/PartialPivLU.h

gtsam/3rdparty/Eigen/doc/eigen_navtree_hacks.js

gtsam/3rdparty/Eigen/test/sparse_product.cpp

gtsam/3rdparty/Eigen/bench/benchmarkXcwise.cpp

gtsam/3rdparty/Eigen/test/sparseqr.cpp

gtsam/3rdparty/Eigen/doc/snippets/PartialRedux_norm.cpp

gtsam/3rdparty/Eigen/test/geo_transformations.cpp

gtsam/3rdparty/Eigen/Eigen/src/Sparse/DynamicSparseMatrix.h

gtsam/3rdparty/Eigen/doc/snippets/SelfAdjointEigenSolver_operatorSqrt.cpp

gtsam/3rdparty/Eigen/doc/snippets/class_FullPivLU.cpp

gtsam/3rdparty/Eigen/doc/Manual.dox

gtsam/3rdparty/Eigen/doc/snippets/Matrix_setZero_int.cpp

gtsam/3rdparty/Eigen/lapack/double.cpp

gtsam/3rdparty/Eigen/debug/msvc/eigen_autoexp_part.dat

gtsam/3rdparty/Eigen/doc/AsciiQuickReference.txt

gtsam/3rdparty/Eigen/Eigen/src/PardisoSupport/CMakeLists.txt

gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseDiagonalProduct.h

gtsam/3rdparty/Eigen/bench/btl/libs/blitz/CMakeLists.txt

gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/PacketMath.h

gtsam/3rdparty/Eigen/doc/snippets/Tutorial_AdvancedInitialization_Block.cpp

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_start_int.cpp

gtsam/3rdparty/Eigen/COPYING.README

gtsam/3rdparty/Eigen/bench/bench_reverse.cpp

gtsam/3rdparty/Eigen/src/Householder/Makefile.am

gtsam/3rdparty/Eigen/Eigen/src/Core/Block.h

gtsam/3rdparty/Eigen/blas/dspr2.f

gtsam/3rdparty/Eigen/test/triangular.cpp

gtsam/3rdparty/Eigen/test/permutationmatrices.cpp

gtsam/3rdparty/Eigen/src/Core/Visitor.h

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_template_int_int_bottomRightCorner_int_int.cpp

gtsam/3rdparty/Eigen/doc/snippets/ComplexSchur_matrixU.cpp

gtsam/3rdparty/Eigen/src/Core/MapBase.h

gtsam/3rdparty/Eigen/Eigen/src/SparseQR/CMakeLists.txt

gtsam/3rdparty/Eigen/blas/zhpr2.f

gtsam/3rdparty/Eigen/unsupported/Eigen/src/Skyline/SkylineUtil.h

gtsam/3rdparty/Eigen/unsupported/Eigen/src/NonLinearOptimization/r1mpyq.h

gtsam/3rdparty/Eigen/doc/snippets/SelfAdjointEigenSolver_SelfAdjointEigenSolver_MatrixType.cpp

gtsam/3rdparty/Eigen/src/Sparse/SparseVector.h

gtsam/3rdparty/Eigen/demos/opengl/README

gtsam/3rdparty/Eigen/src/Sparse/CMakeLists.txt

gtsam/3rdparty/Eigen/doc/snippets/Tridiagonalization_packedMatrix.cpp

gtsam/3rdparty/Eigen/doc/examples/Tutorial_simple_example_fixed_size.cpp

gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_column_bmod.h

gtsam/3rdparty/Eigen/bench/btl/actions/basic_actions.hh

gtsam/3rdparty/Eigen/src/Core/BooleanRedux.h

gtsam/3rdparty/Eigen/doc/StorageOrders.dox

gtsam/3rdparty/Eigen/Eigen/src/SPQRSupport/CMakeLists.txt

gtsam/3rdparty/Eigen/bench/btl/generic_bench/init/init_vector.hh

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_diagonal.cpp

gtsam/3rdparty/Eigen/test/product_trmv.cpp

gtsam/3rdparty/Eigen/doc/examples/function_taking_ref.cpp

gtsam/3rdparty/Eigen/doc/snippets/Matrix_setRandom_int.cpp

gtsam/3rdparty/Eigen/bench/btl/actions/action_symv.hh

gtsam/3rdparty/Eigen/doc/snippets/Map_placement_new.cpp

gtsam/3rdparty/Eigen/Eigen/src/MetisSupport/MetisSupport.h

gtsam/3rdparty/Eigen/src/Geometry/RotationBase.h

gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/EigenvaluesCommon.h

gtsam/3rdparty/Eigen/unsupported/Eigen/src/Polynomials/PolynomialSolver.h

gtsam/3rdparty/Eigen/src/Geometry/ParametrizedLine.h

gtsam/3rdparty/Eigen/blas/double.cpp

gtsam/3rdparty/Eigen/bench/btl/libs/eigen2/eigen2_interface.hh

gtsam/3rdparty/Eigen/.hgignore

gtsam/3rdparty/Eigen/bench/btl/actions/action_syr2.hh

gtsam/3rdparty/Eigen/bench/btl/libs/f77/data.f

gtsam/3rdparty/Eigen/blas/ztbmv.f

gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_relax_snode.h

gtsam/3rdparty/Eigen/bench/btl/libs/blitz/tiny_blitz_interface.hh

gtsam/3rdparty/Eigen/src/Core/arch/SSE/MathFunctions.h

gtsam/3rdparty/Eigen/bench/btl/libs/f77/smxv.f

gtsam/3rdparty/Eigen/doc/snippets/Tutorial_AdvancedInitialization_LinSpaced.cpp

gtsam/3rdparty/Eigen/unsupported/doc/examples/BVH_Example.cpp

gtsam/3rdparty/Eigen/test/special_numbers.cpp

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_zero.cpp

gtsam/3rdparty/Eigen/src/Cholesky/LDLT.h

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_bottomRows_int.cpp

gtsam/3rdparty/Eigen/scripts/release.in

gtsam/3rdparty/Eigen/cmake/FindGMP.cmake

gtsam/3rdparty/Eigen/unsupported/test/bdcsvd.cpp

gtsam/3rdparty/CCOLAMD/MATLAB/ccolamd_test.m

gtsam/3rdparty/Eigen/unsupported/test/mpreal/dlmalloc.c

gtsam/3rdparty/Eigen/Eigen/src/Core/util/ForwardDeclarations.h

gtsam/3rdparty/Eigen/unsupported/test/mpreal/dlmalloc.h

gtsam/3rdparty/Eigen/demos/opengl/camera.h

gtsam/3rdparty/Eigen/src/LU/FullPivLU.h

gtsam/3rdparty/Eigen/lapack/iladlc.f

gtsam/3rdparty/Eigen/doc/TopicVectorization.dox

gtsam/3rdparty/Eigen/Eigen/src/Core/ProductBase.h

gtsam/3rdparty/Eigen/Eigen/Geometry

gtsam/3rdparty/Eigen/doc/snippets/Cwise_times_equal.cpp

gtsam/3rdparty/Eigen/src/Sparse/SparseDiagonalProduct.h

gtsam/3rdparty/Eigen/Eigen/src/Geometry/RotationBase.h

gtsam/3rdparty/Eigen/src/Core/Array.h

gtsam/3rdparty/Eigen/blas/testing/CMakeLists.txt

gtsam/3rdparty/Eigen/src/Eigenvalues/EigenvaluesCommon.h

gtsam/3rdparty/ccolamd/Makefile.am

gtsam/3rdparty/Eigen/doc/snippets/Cwise_quotient.cpp

gtsam/3rdparty/Eigen/blas/level1_cplx_impl.h

gtsam/3rdparty/Eigen/doc/examples/TutorialLinAlgExComputeSolveError.cpp

gtsam/3rdparty/Eigen/src/Eigen2Support/CwiseOperators.h

gtsam/3rdparty/CCOLAMD/MATLAB/ccolamdmex.c

gtsam/3rdparty/Eigen/blas/stbsv.f

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_block_int_int_int_int.cpp

gtsam/3rdparty/Eigen/Eigen/src/Core/VectorwiseOp.h

gtsam/3rdparty/Eigen/unsupported/Eigen/src/Polynomials/Companion.h

gtsam/3rdparty/Eigen/test/product_mmtr.cpp

gtsam/3rdparty/Eigen/test/eigen2/eigen2_linearstructure.cpp

gtsam/3rdparty/Eigen/bench/sparse_lu.cpp

gtsam/3rdparty/Eigen/unsupported/Eigen/src/LevenbergMarquardt/CMakeLists.txt

gtsam/3rdparty/Eigen/Eigen/src/Core/Redux.h

gtsam/3rdparty/Makefile.am

gtsam/3rdparty/Eigen/doc/StlContainers.dox

gtsam/3rdparty/Eigen/test/product_small.cpp

gtsam/3rdparty/Eigen/doc/snippets/Tutorial_solve_multiple_rhs.cpp

gtsam/3rdparty/Eigen/blas/testing/dblat3.dat

gtsam/3rdparty/Eigen/test/linearstructure.cpp

gtsam/3rdparty/Eigen/unsupported/test/mpreal_support.cpp

gtsam/3rdparty/Eigen/unsupported/Eigen/MoreVectorization

gtsam/3rdparty/Eigen/src/Core/util/EnableMSVCWarnings.h

gtsam/3rdparty/Eigen/Eigen/src/Geometry/Umeyama.h

gtsam/3rdparty/Eigen/bench/btl/generic_bench/timers/portable_timer.hh

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_diagonal_template_int.cpp

gtsam/3rdparty/Eigen/doc/snippets/Cwise_inverse.cpp

gtsam/3rdparty/Eigen/test/stable_norm.cpp

gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseVector.h

gtsam/3rdparty/Eigen/failtest/CMakeLists.txt

gtsam/3rdparty/Eigen/src/Geometry/Scaling.h

gtsam/3rdparty/Eigen/src/StlSupport/StdVector.h

gtsam/3rdparty/Eigen/bench/btl/cmake/FindGOTO.cmake

gtsam/3rdparty/Eigen/cmake/FindEigen2.cmake

gtsam/3rdparty/Eigen/unsupported/Eigen/NumericalDiff

gtsam/3rdparty/Eigen/src/Geometry/OrthoMethods.h

gtsam/3rdparty/CCOLAMD/Demo/ccolamd_example.c

gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixVector_MKL.h

gtsam/3rdparty/Eigen/doc/snippets/Map_general_stride.cpp

gtsam/3rdparty/Eigen/bench/btl/generic_bench/timers/x86_perf_analyzer.hh

gtsam/3rdparty/Eigen/test/eigen2/eigen2_prec_inverse_4x4.cpp

gtsam/3rdparty/Eigen/Eigen/src/Core/products/Parallelizer.h

gtsam/3rdparty/Eigen/blas/BandTriangularSolver.h

gtsam/3rdparty/Eigen/doc/snippets/Cwise_product.cpp

gtsam/3rdparty/Eigen/COPYING.BSD

gtsam/3rdparty/Eigen/doc/snippets/Cwise_min.cpp

gtsam/3rdparty/Eigen/Eigen/src/Geometry/Translation.h

gtsam/3rdparty/Eigen/bench/btl/actions/action_partial_lu.hh

gtsam/3rdparty/Eigen/lapack/cladiv.f

gtsam/3rdparty/Eigen/bench/btl/generic_bench/utils/xy_file.hh

gtsam/3rdparty/Eigen/unsupported/test/CMakeLists.txt

gtsam/3rdparty/Eigen/src/Core/arch/Default/Settings.h

gtsam/3rdparty/Eigen/doc/snippets/DirectionWise_replicate.cpp

gtsam/3rdparty/Eigen/src/Geometry/Rotation2D.h

gtsam/3rdparty/Eigen/unsupported/Eigen/src/NonLinearOptimization/dogleg.h

gtsam/3rdparty/Eigen/bench/btl/libs/C_BLAS/C_BLAS_interface.hh

gtsam/3rdparty/Eigen/src/Eigen2Support/Macros.h

gtsam/3rdparty/Eigen/bench/btl/libs/BLAS/CMakeLists.txt

gtsam/3rdparty/Eigen/doc/HiPerformance.dox

gtsam/3rdparty/Eigen/bench/btl/COPYING

gtsam/3rdparty/Eigen/demos/opengl/quaternion_demo.cpp

gtsam/3rdparty/Eigen/Eigen/StdVector

gtsam/3rdparty/Eigen/lapack/complex_double.cpp

gtsam/3rdparty/Eigen/doc/snippets/SelfAdjointEigenSolver_eigenvectors.cpp

gtsam/3rdparty/Eigen/bench/btl/libs/f77/dmxv.f

gtsam/3rdparty/Eigen/blas/ztpmv.f

gtsam/3rdparty/Eigen/cmake/FindUmfpack.cmake

gtsam/3rdparty/Eigen/cmake/FindBLAS.cmake

gtsam/3rdparty/Eigen/test/adjoint.cpp

gtsam/3rdparty/Eigen/Eigen/src/SparseQR/SparseQR.h

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_array.cpp

gtsam/3rdparty/Eigen/bench/btl/data/mk_mean_script.sh

gtsam/3rdparty/Eigen/bench/sparse_dense_product.cpp

gtsam/3rdparty/Eigen/bench/btl/libs/eigen2/main_matmat.cpp

gtsam/3rdparty/Eigen/blas/testing/zblat2.dat

gtsam/3rdparty/Eigen/unsupported/test/mpreal.cpp

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_bottomLeftCorner_int_int.cpp

gtsam/3rdparty/Eigen/doc/examples/Tutorial_ReductionsVisitorsBroadcasting_maxnorm.cpp

gtsam/3rdparty/Eigen/doc/FixedSizeVectorizable.dox

gtsam/3rdparty/Eigen/doc/snippets/SelfAdjointEigenSolver_SelfAdjointEigenSolver_MatrixType2.cpp

gtsam/3rdparty/Eigen/doc/C02_TutorialMatrixArithmetic.dox

gtsam/3rdparty/Eigen/src/Eigen2Support/Meta.h

gtsam/3rdparty/Eigen/Eigen/src/Core/NoAlias.h

gtsam/3rdparty/Eigen/doc/snippets/Matrix_setIdentity_int_int.cpp

gtsam/3rdparty/Eigen/bench/spbench/spbench.dtd

gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointRank2Update.h

gtsam/3rdparty/Eigen/Eigen/src/Householder/Householder.h

gtsam/3rdparty/Eigen/test/product_selfadjoint.cpp

gtsam/3rdparty/Eigen/lapack/single.cpp

gtsam/3rdparty/Eigen/Eigen/src/Core/SolveTriangular.h

gtsam/3rdparty/Eigen/src/Core/Transpose.h

gtsam/3rdparty/Eigen/Eigen/src/Householder/BlockHouseholder.h

gtsam/3rdparty/Eigen/COPYING.LGPL

gtsam/3rdparty/Eigen/demos/opengl/icosphere.cpp

gtsam/3rdparty/Eigen/doc/examples/Tutorial_ReductionsVisitorsBroadcasting_broadcast_simple_rowwise.cpp

gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/CMakeLists.txt

gtsam/3rdparty/Eigen/doc/examples/Tutorial_ReductionsVisitorsBroadcasting_reductions_norm.cpp

gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrixTriangular_MKL.h

gtsam/3rdparty/Eigen/Eigen/src/SuperLUSupport/CMakeLists.txt

gtsam/3rdparty/Eigen/doc/snippets/Map_outer_stride.cpp

gtsam/3rdparty/Eigen/src/Eigenvalues/HessenbergDecomposition.h

gtsam/3rdparty/Eigen/test/dontalign.cpp

gtsam/3rdparty/CCOLAMD/Source/ccolamd_global.c

gtsam/3rdparty/Eigen/blas/GeneralRank1Update.h

gtsam/3rdparty/Eigen/unsupported/doc/examples/PolynomialUtils1.cpp

gtsam/3rdparty/Eigen/bench/btl/data/mk_gnuplot_script.sh

gtsam/3rdparty/Eigen/unsupported/doc/examples/MatrixSine.cpp

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_zero_int.cpp

gtsam/3rdparty/Eigen/Eigen/src/Core/CommaInitializer.h

gtsam/3rdparty/Eigen/Eigen/LU

gtsam/3rdparty/Eigen/Eigen/src/Geometry/Quaternion.h

gtsam/3rdparty/Eigen/doc/snippets/Matrix_setConstant_int_int.cpp

gtsam/3rdparty/Eigen/src/Core/products/SelfadjointProduct.h

gtsam/3rdparty/Eigen/doc/C08_TutorialGeometry.dox

gtsam/3rdparty/Eigen/blas/zhpmv.f

gtsam/3rdparty/Eigen/SVD

gtsam/3rdparty/Eigen/unsupported/Eigen/src/SparseExtra/CholmodSupportLegacy.h

gtsam/3rdparty/Eigen/src/Geometry/arch/Geometry_SSE.h

gtsam/3rdparty/Eigen/unsupported/test/sparse_ldlt.cpp

gtsam/3rdparty/Eigen/test/stdvector_overload.cpp

gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/Scaling.h

gtsam/3rdparty/Eigen/doc/snippets/TopicAliasing_block_correct.cpp

gtsam/3rdparty/Eigen/bench/btl/CMakeLists.txt

gtsam/3rdparty/Eigen/failtest/map_on_const_type_actually_const_0.cpp

gtsam/3rdparty/Eigen/test/eigen2/eigen2_parametrizedline.cpp

gtsam/3rdparty/Eigen/src/Eigen2Support/Lazy.h

gtsam/3rdparty/Eigen/unsupported/doc/eigendoxy_layout.xml.in

gtsam/3rdparty/Eigen/failtest/map_nonconst_ctor_on_const_ptr_1.cpp

gtsam/3rdparty/Eigen/src/Jacobi/Jacobi.h

gtsam/3rdparty/Eigen/doc/CMakeLists.txt

gtsam/3rdparty/Eigen/doc/snippets/DenseBase_LinSpaced_seq.cpp

gtsam/3rdparty/Eigen/blas/Rank2Update.h

gtsam/3rdparty/Eigen/test/dynalloc.cpp

gtsam/3rdparty/Eigen/Eigen/src/Sparse/SparseUtil.h

gtsam/3rdparty/Eigen/test/selfadjoint.cpp

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_template_int_bottomRows.cpp

gtsam/3rdparty/Eigen/doc/examples/class_CwiseUnaryOp.cpp

gtsam/3rdparty/Eigen/bench/btl/libs/STL/STL_interface.hh

gtsam/3rdparty/Eigen/bench/btl/libs/f77/f77_interface_base.hh

gtsam/3rdparty/Eigen/Eigen/src/SVD/UpperBidiagonalization.h

gtsam/3rdparty/Eigen/test/eigen2/eigen2_product_large.cpp

gtsam/3rdparty/Eigen/test/eigen2/eigen2_smallvectors.cpp

gtsam/3rdparty/UFconfig/xerbla/xerbla.c

gtsam/3rdparty/Eigen/bench/btl/libs/eigen3/main_adv.cpp

gtsam/3rdparty/UFconfig/xerbla/xerbla.f

gtsam/3rdparty/Eigen/cmake/FindMetis.cmake

gtsam/3rdparty/Eigen/bench/sparse_product.cpp

gtsam/3rdparty/Eigen/test/product_trsolve.cpp

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_select.cpp

gtsam/3rdparty/Eigen/doc/TopicAssertions.dox

gtsam/3rdparty/Eigen/Eigen/Dense

gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/Complex.h

gtsam/3rdparty/Eigen/debug/gdb/__init__.py

gtsam/3rdparty/Eigen/COPYING.MPL2

gtsam/3rdparty/Eigen/Dense

gtsam/3rdparty/Eigen/doc/snippets/Cwise_minus_equal.cpp

gtsam/3rdparty/Eigen/debug/gdb/printers.py

gtsam/3rdparty/Eigen/doc/snippets/Matrix_setConstant_int.cpp

gtsam/3rdparty/Eigen/doc/examples/class_FixedVectorBlock.cpp

gtsam/3rdparty/Eigen/doc/TopicMultithreading.dox

gtsam/3rdparty/Eigen/src/StlSupport/details.h

gtsam/3rdparty/Eigen/blas/testing/dblat1.f

gtsam/3rdparty/Eigen/Eigen/src/plugins/CommonCwiseUnaryOps.h

gtsam/3rdparty/Eigen/src/Core/arch/SSE/Complex.h

gtsam/3rdparty/Eigen/test/eigen2/eigen2_svd.cpp

gtsam/3rdparty/Eigen/test/eigen2/eigen2_nomalloc.cpp

gtsam/3rdparty/Eigen/doc/snippets/Cwise_boolean_or.cpp

gtsam/3rdparty/Eigen/bench/vdw_new.cpp

gtsam/3rdparty/Eigen/bench/btl/libs/eigen3/CMakeLists.txt

gtsam/3rdparty/Eigen/src/Geometry/Homogeneous.h

gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrix_MKL.h

gtsam/3rdparty/Eigen/doc/I13_FunctionsTakingEigenTypes.dox

gtsam/3rdparty/Eigen/test/visitor.cpp

gtsam/3rdparty/Eigen/doc/snippets/ComplexEigenSolver_eigenvalues.cpp

gtsam/3rdparty/Eigen/doc/snippets/TopicAliasing_mult1.cpp

gtsam/3rdparty/Eigen/src/Sparse/SparseTriangularView.h

gtsam/3rdparty/Eigen/Eigen/SparseQR

gtsam/3rdparty/Eigen/blas/testing/zblat2.f

gtsam/3rdparty/Eigen/test/sparse_vector.cpp

gtsam/3rdparty/Eigen/doc/D01_StlContainers.dox

gtsam/3rdparty/ccolamd/Lib_Makefile.mk

gtsam/3rdparty/Eigen/doc/examples/Tutorial_ArrayClass_cwise_other.cpp

gtsam/3rdparty/Eigen/Eigen/src/Core/Diagonal.h

gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixFunctionAtomic.h

gtsam/3rdparty/Eigen/Eigen/CMakeLists.txt

gtsam/3rdparty/Eigen/src/plugins/CommonCwiseBinaryOps.h

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_setRandom.cpp

gtsam/3rdparty/Eigen/blas/ctbsv.f

gtsam/3rdparty/Eigen/Eigen/src/Sparse/SparseTranspose.h

gtsam/3rdparty/Eigen/doc/snippets/TopicAliasing_mult2.cpp

gtsam/3rdparty/Eigen/test/resize.cpp

gtsam/3rdparty/Eigen/doc/CustomizingEigen.dox

gtsam/3rdparty/Eigen/src/Sparse/AmbiVector.h

gtsam/3rdparty/Eigen/Eigen/src/PardisoSupport/PardisoSupport.h

gtsam/3rdparty/Eigen/test/eigen2/eigen2_bug_132.cpp

gtsam/3rdparty/Eigen/Eigen/QtAlignedMalloc

gtsam/3rdparty/Eigen/bench/btl/libs/f77/smxm.f

gtsam/3rdparty/Eigen/unsupported/Eigen/Polynomials

gtsam/3rdparty/Eigen/lapack/dlamch.f

gtsam/3rdparty/Eigen/QR

gtsam/3rdparty/Eigen/src/QR/CMakeLists.txt

gtsam/3rdparty/Eigen/doc/PassingByValue.dox

gtsam/3rdparty/Eigen/doc/snippets/DirectionWise_replicate_int.cpp

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_segment_int_int.cpp

gtsam/3rdparty/Eigen/unsupported/test/FFTW.cpp

gtsam/3rdparty/Eigen/unsupported/Eigen/src/CMakeLists.txt

gtsam/3rdparty/Eigen/test/sparse_solver.h

gtsam/3rdparty/Eigen/unsupported/Eigen/src/NonLinearOptimization/r1updt.h

gtsam/3rdparty/Eigen/cmake/FindGoogleHash.cmake

gtsam/3rdparty/Eigen/doc/examples/Tutorial_ArrayClass_accessors.cpp

gtsam/3rdparty/Eigen/src/plugins/MatrixCwiseBinaryOps.h

gtsam/3rdparty/Eigen/doc/C04_TutorialBlockOperations.dox

gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseTranspose.h

gtsam/3rdparty/Eigen/src/Eigenvalues/Makefile.am

gtsam/3rdparty/UFconfig/README.txt

gtsam/3rdparty/Eigen/doc/snippets/tut_matrix_assignment_resizing.cpp

gtsam/3rdparty/Eigen/Eigen/src/Core/VectorBlock.h

gtsam/3rdparty/Eigen/doc/Overview.dox

gtsam/3rdparty/Eigen/test/geo_eulerangles.cpp

gtsam/3rdparty/Eigen/bench/btl/libs/eigen2/main_linear.cpp

gtsam/3rdparty/Eigen/Eigen/src/Core/StableNorm.h

gtsam/3rdparty/Eigen/Eigen/src/Sparse/SparseVector.h

gtsam/3rdparty/Eigen/src/StlSupport/StdList.h

gtsam/3rdparty/Eigen/doc/PreprocessorDirectives.dox

gtsam/3rdparty/Eigen/test/eigen2/eigen2_regression.cpp

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_row.cpp

gtsam/3rdparty/Eigen/test/CMakeLists.txt

gtsam/3rdparty/Eigen/bench/btl/libs/BLAS/c_interface_base.h

gtsam/3rdparty/Eigen/doc/C06_TutorialLinearAlgebra.dox

gtsam/3rdparty/Eigen/lapack/slarft.f

gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/CMakeLists.txt

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_fixedBlock_int_int.cpp

gtsam/3rdparty/Eigen/doc/examples/QuickStart_example2_dynamic.cpp

gtsam/3rdparty/Eigen/unsupported/Eigen/src/NumericalDiff/NumericalDiff.h

gtsam/3rdparty/Eigen/src/Core/util/DisableStupidWarnings.h

gtsam/3rdparty/Eigen/test/mapstride.cpp

gtsam/3rdparty/Eigen/Eigen/src/Core/BandMatrix.h

gtsam/3rdparty/Eigen/test/main.h

gtsam/3rdparty/Eigen/test/eigen2/eigen2_stdvector.cpp

gtsam/3rdparty/Eigen/src/Sparse/SparseCwiseUnaryOp.h

gtsam/3rdparty/Eigen/doc/snippets/Matrix_setRandom_int_int.cpp

gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_column_dfs.h

gtsam/3rdparty/Eigen/test/array_for_matrix.cpp

gtsam/3rdparty/Eigen/doc/snippets/HouseholderSequence_HouseholderSequence.cpp

gtsam/3rdparty/Eigen/lapack/dsecnd_NONE.f

gtsam/3rdparty/Eigen/src/plugins/ArrayCwiseBinaryOps.h

gtsam/3rdparty/Eigen/src/Geometry/Hyperplane.h

gtsam/3rdparty/Eigen/unsupported/Eigen/SparseExtra

gtsam/3rdparty/Eigen/doc/snippets/ComplexEigenSolver_eigenvectors.cpp

gtsam/3rdparty/Eigen/blas/dtbsv.f

gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixPower.h

gtsam/3rdparty/Eigen/doc/snippets/TopicStorageOrders_example.cpp

gtsam/3rdparty/Eigen/Eigen/src/SparseLU/CMakeLists.txt

gtsam/3rdparty/Eigen/unsupported/Eigen/src/Splines/Spline.h

gtsam/3rdparty/Eigen/bench/btl/libs/ublas/ublas_interface.hh

gtsam/3rdparty/Eigen/unsupported/Eigen/src/SparseExtra/SparseLDLTLegacy.h

gtsam/3rdparty/Eigen/src/Core/arch/AltiVec/Complex.h

gtsam/3rdparty/Eigen/lapack/slamch.f

gtsam/3rdparty/Eigen/bench/README.txt

gtsam/3rdparty/Eigen/src/Core/products/GeneralMatrixMatrix.h

gtsam/3rdparty/Eigen/doc/UnalignedArrayAssert.dox

gtsam/3rdparty/Eigen/bench/btl/cmake/FindEigen3.cmake

gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/ConjugateGradient.h

gtsam/3rdparty/Eigen/src/Core/products/TriangularSolverVector.h

gtsam/3rdparty/Eigen/Eigen/src/Sparse/SparseProduct.h

gtsam/3rdparty/Eigen/Eigen/PaStiXSupport

gtsam/3rdparty/Eigen/Eigen/src/Sparse/SparseDiagonalProduct.h

gtsam/3rdparty/Eigen/Eigen/src/LU/FullPivLU.h

gtsam/3rdparty/Eigen/src/Core/products/SelfadjointMatrixVector.h

gtsam/3rdparty/Eigen/src/Eigenvalues/RealSchur.h

gtsam/3rdparty/Eigen/test/sparse.h

gtsam/3rdparty/Eigen/src/Sparse/SparseRedux.h

gtsam/3rdparty/UFconfig/UFconfig.h

gtsam/3rdparty/Eigen/lapack/ilazlr.f

gtsam/3rdparty/Eigen/test/eigen2/eigen2_sizeof.cpp

gtsam/3rdparty/Eigen/unsupported/Eigen/src/Skyline/SkylineMatrixBase.h

gtsam/3rdparty/Eigen/unsupported/Eigen/src/SparseExtra/DynamicSparseMatrix.h

gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/IterationController.h

gtsam/3rdparty/Eigen/src/Core/Diagonal.h

gtsam/3rdparty/Eigen/src/Core/PermutationMatrix.h

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_prod.cpp

gtsam/3rdparty/Eigen/test/eigen2/main.h

gtsam/3rdparty/Eigen/Eigen/src/SparseCore/ConservativeSparseSparseProduct.h

gtsam/3rdparty/Eigen/unsupported/Eigen/src/SparseExtra/SparseLU.h

gtsam/3rdparty/Eigen/failtest/const_qualified_block_method_retval_0.cpp

gtsam/3rdparty/Eigen/test/product_large.cpp

gtsam/3rdparty/Eigen/Array

gtsam/3rdparty/Eigen/blas/testing/sblat2.dat

gtsam/3rdparty/Eigen/test/product_syrk.cpp

gtsam/3rdparty/Eigen/Eigen/SVD

gtsam/3rdparty/Eigen/doc/snippets/LLT_example.cpp

gtsam/3rdparty/Eigen/failtest/map_nonconst_ctor_on_const_ptr_2.cpp

gtsam/3rdparty/Eigen/blas/xerbla.cpp

gtsam/3rdparty/Eigen/test/geo_alignedbox.cpp

gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_gemm_kernel.h

gtsam/3rdparty/Eigen/Eigen/src/Geometry/arch/Geometry_SSE.h

gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/IncompleteLUT.h

gtsam/3rdparty/Eigen/demos/mandelbrot/mandelbrot.cpp

gtsam/3rdparty/Eigen/src/Core/arch/AltiVec/CMakeLists.txt

gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/TriangularSolver.h

gtsam/3rdparty/Eigen/blas/complex_double.cpp

gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/Geometry/Quaternion.h

gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularMatrixMatrix.h

gtsam/3rdparty/Eigen/demos/opengl/icosphere.h

gtsam/3rdparty/Eigen/src/Eigen2Support/Geometry/Rotation2D.h

gtsam/3rdparty/Eigen/test/superlu_support.cpp

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_random_int_int.cpp

gtsam/3rdparty/Eigen/unsupported/test/mpreal.h

gtsam/3rdparty/Eigen/Eigen/src/misc/Image.h

gtsam/3rdparty/CCOLAMD/Include/ccolamd.h

gtsam/3rdparty/Eigen/bench/btl/libs/f77/CMakeLists.txt

gtsam/3rdparty/Eigen/test/integer_types.cpp

gtsam/3rdparty/Eigen/doc/examples/tut_arithmetic_scalar_mul_div.cpp

gtsam/3rdparty/Eigen/src/SVD/JacobiSVD.h

gtsam/3rdparty/Eigen/Eigen/src/Geometry/ParametrizedLine.h

gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/CMakeLists.txt

gtsam/3rdparty/Eigen/doc/snippets/Tutorial_solve_singular.cpp

gtsam/3rdparty/Eigen/Eigen/src/Jacobi/Jacobi.h

gtsam/3rdparty/Eigen/doc/snippets/Cwise_equal_equal.cpp

gtsam/3rdparty/Eigen/bench/btl/libs/tvmet/CMakeLists.txt

gtsam/3rdparty/Eigen/src/Core/products/Makefile.am

gtsam/3rdparty/Eigen/test/eigensolver_selfadjoint.cpp

gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/StemFunction.h

gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/IncompleteLU.h

gtsam/3rdparty/Eigen/src/Eigen2Support/QR.h

gtsam/3rdparty/Eigen/bench/btl/actions/action_rot.hh

gtsam/3rdparty/Eigen/unsupported/test/gmres.cpp

gtsam/3rdparty/Eigen/src/Core/util/Makefile.am

gtsam/3rdparty/Eigen/doc/snippets/SelfAdjointEigenSolver_operatorInverseSqrt.cpp

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_cwiseSqrt.cpp

gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_pivotL.h

gtsam/3rdparty/Eigen/doc/examples/tut_matrix_resize.cpp

gtsam/3rdparty/Eigen/unsupported/Eigen/src/Skyline/CMakeLists.txt

gtsam/3rdparty/Eigen/Eigen/src/SparseCore/TriangularSolver.h

gtsam/3rdparty/Eigen/test/eigen2/eigen2_cwiseop.cpp

gtsam/3rdparty/Eigen/Eigen/src/PaStiXSupport/CMakeLists.txt

gtsam/3rdparty/Eigen/Eigen/src/Core/util/MKL_support.h

gtsam/3rdparty/Eigen/src/Geometry/CMakeLists.txt

gtsam/3rdparty/Eigen/src/Core/ArrayWrapper.h

gtsam/3rdparty/Eigen/src/Core/arch/AltiVec/PacketMath.h

gtsam/3rdparty/Eigen/Eigen/src/Sparse/MappedSparseMatrix.h

gtsam/3rdparty/Eigen/doc/snippets/EigenSolver_pseudoEigenvectors.cpp

gtsam/3rdparty/Eigen/blas/dtpsv.f

gtsam/3rdparty/Eigen/Eigen/src/Sparse/SparseMatrix.h

gtsam/3rdparty/Eigen/Eigen/src/SparseCore/AmbiVector.h

gtsam/3rdparty/Eigen/failtest/const_qualified_transpose_method_retval.cpp

gtsam/3rdparty/Eigen/test/sparse_basic.cpp

gtsam/3rdparty/Eigen/src/Core/Makefile.am

gtsam/3rdparty/Eigen/src/Core/PlainObjectBase.h

gtsam/3rdparty/Eigen/src/Cholesky/Makefile.am

gtsam/3rdparty/Eigen/bench/btl/libs/STL_algo/STL_algo_interface.hh

gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/LU.h

gtsam/3rdparty/Eigen/unsupported/Eigen/src/MoreVectorization/CMakeLists.txt

gtsam/3rdparty/Eigen/doc/snippets/HouseholderQR_solve.cpp

gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/QR.h

gtsam/3rdparty/Eigen/test/eigen2/eigen2_geometry.cpp

gtsam/3rdparty/Eigen/doc/snippets/Matrix_resize_int.cpp

gtsam/3rdparty/Eigen/src/Core/util/ReenableStupidWarnings.h

gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/Geometry/AlignedBox.h

gtsam/3rdparty/Eigen/lapack/clarft.f

gtsam/3rdparty/Eigen/Eigen/src/Core/products/CMakeLists.txt

gtsam/3rdparty/Eigen/demos/mandelbrot/mandelbrot.h

gtsam/3rdparty/Eigen/bench/spbench/test_sparseLU.cpp

gtsam/3rdparty/Eigen/src/Core/ProductBase.h

gtsam/3rdparty/Eigen/src/Core/CMakeLists.txt

gtsam/3rdparty/Eigen/test/eigen2/eigen2_product_small.cpp

gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/Geometry/Scaling.h

gtsam/3rdparty/Eigen/Geometry

gtsam/3rdparty/Eigen/doc/examples/Tutorial_BlockOperations_corner.cpp

gtsam/3rdparty/Eigen/StdVector

gtsam/3rdparty/Eigen/doc/snippets/FullPivLU_kernel.cpp

gtsam/3rdparty/Eigen/Eigen/src/plugins/CommonCwiseBinaryOps.h

gtsam/3rdparty/Eigen/lapack/lu.cpp

gtsam/3rdparty/Eigen/doc/eigendoxy_layout.xml.in

gtsam/3rdparty/Eigen/Eigen/src/Core/Stride.h

gtsam/3rdparty/Eigen/unsupported/Eigen/src/SparseExtra/CMakeLists.txt

gtsam/3rdparty/Eigen/test/eigen2/eigen2_qr.cpp

gtsam/3rdparty/Eigen/Eigen/src/Core/arch/Default/CMakeLists.txt

gtsam/3rdparty/Eigen/unsupported/test/matrix_square_root.cpp

gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseDenseProduct.h

gtsam/3rdparty/Eigen/LGPL

gtsam/3rdparty/Eigen/bench/btl/libs/C_BLAS/blas.h

gtsam/3rdparty/Eigen/src/Eigen2Support/Geometry/Hyperplane.h

gtsam/3rdparty/Eigen/Eigen/src/Core/PlainObjectBase.h

gtsam/3rdparty/Eigen/blas/PackedTriangularSolverVector.h

gtsam/3rdparty/Eigen/src/Core/CwiseBinaryOp.h

gtsam/3rdparty/Eigen/unsupported/doc/examples/MatrixLogarithm.cpp

gtsam/3rdparty/Eigen/doc/snippets/Tutorial_AdvancedInitialization_CommaTemporary.cpp

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_array_const.cpp

gtsam/3rdparty/Eigen/cmake/RegexUtils.cmake

gtsam/3rdparty/Eigen/test/eigen2/eigen2_visitor.cpp

gtsam/3rdparty/Eigen/unsupported/test/matrix_exponential.cpp

gtsam/3rdparty/Eigen/src/Eigen2Support/Geometry/CMakeLists.txt

gtsam/3rdparty/Eigen/doc/examples/TutorialLinAlgComputeTwice.cpp

gtsam/3rdparty/Eigen/unsupported/Eigen/src/KroneckerProduct/CMakeLists.txt

gtsam/3rdparty/Eigen/scripts/eigen_gen_docs

gtsam/3rdparty/Eigen/unsupported/Eigen/src/Skyline/SkylineProduct.h

gtsam/3rdparty/Eigen/bench/btl/generic_bench/timers/STL_perf_analyzer.hh

gtsam/3rdparty/Eigen/blas/testing/dblat2.f

gtsam/3rdparty/Eigen/src/QR/Makefile.am

gtsam/3rdparty/Eigen/doc/I09_Vectorization.dox

gtsam/3rdparty/Eigen/test/pardiso_support.cpp

gtsam/3rdparty/Eigen/doc/TutorialSparse_example_details.dox

gtsam/3rdparty/Eigen/src/Sparse/SparseUtil.h

gtsam/3rdparty/Eigen/src/Core/ReturnByValue.h

gtsam/3rdparty/Eigen/unsupported/Eigen/AdolcForward

gtsam/3rdparty/Eigen/src/Core/util/BlasUtil.h

gtsam/3rdparty/Eigen/blas/dtbmv.f

gtsam/3rdparty/Eigen/failtest/block_nonconst_ctor_on_const_xpr_2.cpp

gtsam/3rdparty/Eigen/src/Core/SelfAdjointView.h

gtsam/3rdparty/Eigen/test/sparseLM.cpp

gtsam/3rdparty/Eigen/bench/btl/libs/eigen2/CMakeLists.txt

gtsam/3rdparty/Eigen/Eigen/src/Core/IO.h

gtsam/3rdparty/Eigen/doc/examples/tut_matrix_coefficient_accessors.cpp

gtsam/3rdparty/Eigen/failtest/failtest_sanity_check.cpp

gtsam/3rdparty/Eigen/lapack/zladiv.f

gtsam/3rdparty/Eigen/bench/btl/libs/f77/sata.f

gtsam/3rdparty/Eigen/doc/snippets/ComplexEigenSolver_compute.cpp

gtsam/3rdparty/Eigen/Eigen/src/Core/arch/AltiVec/Complex.h

gtsam/3rdparty/Eigen/test/eigen2/eigen2_dynalloc.cpp

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_computeInverseWithCheck.cpp

gtsam/3rdparty/Eigen/StdList

gtsam/3rdparty/Eigen/unsupported/test/forward_adolc.cpp

gtsam/3rdparty/Eigen/unsupported/Eigen/src/LevenbergMarquardt/LMcovar.h

gtsam/3rdparty/Eigen/Eigen/src/Geometry/Rotation2D.h

gtsam/3rdparty/Eigen/test/eigen2/eigen2_determinant.cpp

gtsam/3rdparty/Eigen/src/Eigen2Support/Geometry/Quaternion.h

gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseNullaryOp.h

gtsam/3rdparty/Eigen/doc/snippets/RealSchur_compute.cpp

gtsam/3rdparty/Eigen/Eigen/src/Core/Array.h

gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LLT_MKL.h

gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_pruneL.h

gtsam/3rdparty/Eigen/Eigen/src/Geometry/AngleAxis.h

gtsam/3rdparty/Eigen/bench/btl/actions/action_atv_product.hh

gtsam/3rdparty/Eigen/bench/btl/generic_bench/timers/portable_perf_analyzer_old.hh

gtsam/3rdparty/Eigen/doc/special_examples/Tutorial_sparse_example.cpp

gtsam/3rdparty/Eigen/bench/benchEigenSolver.cpp

gtsam/3rdparty/ccolamd/UFconfig.h

gtsam/3rdparty/Eigen/doc/snippets/Tutorial_AdvancedInitialization_ThreeWays.cpp

gtsam/3rdparty/Eigen/test/determinant.cpp

gtsam/3rdparty/Eigen/bench/spbench/spbenchsolver.h

gtsam/3rdparty/Eigen/test/sparse_solvers.cpp

gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/DGMRES.h

gtsam/3rdparty/Eigen/scripts/buildtests.in

gtsam/3rdparty/Eigen/scripts/debug.in

gtsam/3rdparty/Eigen/doc/snippets/ComplexSchur_compute.cpp

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_identity_int_int.cpp

gtsam/3rdparty/Eigen/unsupported/Eigen/IterativeSolvers

gtsam/3rdparty/Eigen/blas/testing/sblat3.dat

gtsam/3rdparty/Eigen/Eigen/src/Sparse/CMakeLists.txt

gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseRedux.h

gtsam/3rdparty/Eigen/src/Core/SelfCwiseBinaryOp.h

gtsam/3rdparty/Eigen/lapack/slarf.f

gtsam/3rdparty/Eigen/bench/btl/libs/blitz/btl_blitz.cpp

gtsam/3rdparty/Eigen/bench/btl/libs/ublas/CMakeLists.txt

gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparsePermutation.h

gtsam/3rdparty/Eigen/lapack/second_NONE.f

gtsam/3rdparty/Eigen/Eigen/src/Sparse/SparseFuzzy.h

gtsam/3rdparty/Eigen/Eigen/src/Core/ArrayBase.h

gtsam/3rdparty/Eigen/Eigen/src/Core/Matrix.h

gtsam/3rdparty/Eigen/src/Core/products/SelfadjointMatrixMatrix.h

gtsam/3rdparty/Eigen/doc/I05_FixedSizeVectorizable.dox

gtsam/3rdparty/Eigen/Eigen/OrderingMethods

gtsam/3rdparty/Eigen/src/Core/NestByValue.h

gtsam/3rdparty/Eigen/Eigen/Core

gtsam/3rdparty/Eigen/bench/btl/libs/STL/CMakeLists.txt

gtsam/3rdparty/Eigen/test/cholesky.cpp

gtsam/3rdparty/Eigen/src/Eigen2Support/LU.h

gtsam/3rdparty/Eigen/Householder

gtsam/3rdparty/Eigen/doc/D03_WrongStackAlignment.dox

gtsam/3rdparty/Eigen/Eigen/src/LU/PartialPivLU_MKL.h

gtsam/3rdparty/ccolamd/UFconfig.c

gtsam/3rdparty/Eigen/cmake/FindGSL.cmake

gtsam/3rdparty/Eigen/bench/btl/libs/eigen2/main_adv.cpp

gtsam/3rdparty/Eigen/bench/bench_sum.cpp

gtsam/3rdparty/Eigen/src/Core/Dot.h

gtsam/3rdparty/Eigen/doc/snippets/tut_arithmetic_redux_minmax.cpp

gtsam/3rdparty/Eigen/test/sizeoverflow.cpp

gtsam/3rdparty/Eigen/unsupported/Eigen/src/SparseExtra/SimplicialCholesky.h

gtsam/3rdparty/Eigen/test/redux.cpp

gtsam/3rdparty/Eigen/unsupported/test/jacobisvd.cpp

gtsam/3rdparty/Eigen/doc/C03_TutorialArrayClass.dox

gtsam/3rdparty/Eigen/bench/btl/libs/eigen3/btl_tiny_eigen3.cpp

gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseSelfAdjointView.h

gtsam/3rdparty/Eigen/src/Core/products/TriangularMatrixMatrix.h

gtsam/3rdparty/Eigen/bench/btl/cmake/FindATLAS.cmake

gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrixTriangular.h

gtsam/3rdparty/Eigen/doc/snippets/RealSchur_RealSchur_MatrixType.cpp

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_cwiseMax.cpp

gtsam/3rdparty/Eigen/unsupported/Eigen/src/SVD/CMakeLists.txt

gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_Utils.h

gtsam/3rdparty/Eigen/test/denseLM.cpp

gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/CMakeLists.txt

gtsam/3rdparty/Eigen/Eigen/src/Core/arch/Default/Settings.h

gtsam/3rdparty/Eigen/test/householder.cpp

gtsam/3rdparty/Eigen/unsupported/Eigen/src/SparseExtra/Solve.h

gtsam/3rdparty/Eigen/doc/snippets/Tutorial_Map_using.cpp

gtsam/3rdparty/Eigen/doc/examples/DenseBase_middleCols_int.cpp

gtsam/3rdparty/Eigen/demos/mandelbrot/README

gtsam/3rdparty/Eigen/doc/QuickStartGuide.dox

gtsam/3rdparty/Eigen/test/first_aligned.cpp

gtsam/3rdparty/Eigen/src/Jacobi/Makefile.am

gtsam/3rdparty/Eigen/bench/btl/libs/blitz/blitz_interface.hh

gtsam/3rdparty/Eigen/failtest/map_nonconst_ctor_on_const_ptr_3.cpp

gtsam/3rdparty/Eigen/test/eigen2/eigen2_qtvector.cpp

gtsam/3rdparty/Eigen/bench/benchmark.cpp

gtsam/3rdparty/Eigen/bench/btl/libs/mtl4/main.cpp

gtsam/3rdparty/Eigen/test/stdlist.cpp

gtsam/3rdparty/Eigen/unsupported/test/polynomialsolver.cpp

gtsam/3rdparty/Eigen/blas/zhbmv.f

gtsam/3rdparty/Eigen/test/cholmod_support.cpp

gtsam/3rdparty/Eigen/src/Geometry/arch/Makefile.am

gtsam/3rdparty/Eigen/src/Core/Swap.h

gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseColEtree.h

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_ones_int_int.cpp

gtsam/3rdparty/Eigen/lapack/dlapy2.f

gtsam/3rdparty/Eigen/blas/testing/sblat1.f

gtsam/3rdparty/Eigen/cmake/FindSuperLU.cmake

gtsam/3rdparty/Eigen/test/eigen2/eigen2_first_aligned.cpp

gtsam/3rdparty/Eigen/blas/testing/sblat3.f

gtsam/3rdparty/Eigen/unsupported/test/BVH.cpp

gtsam/3rdparty/Eigen/bench/basicbenchmark.cpp

gtsam/3rdparty/Eigen/doc/snippets/Tutorial_commainit_01b.cpp

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_isOnes.cpp

gtsam/3rdparty/Eigen/Eigen/src/Core/ArrayWrapper.h

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_end_int.cpp

gtsam/3rdparty/Eigen/failtest/diagonal_on_const_type_actually_const.cpp

gtsam/3rdparty/Eigen/src/Geometry/Quaternion.h

gtsam/3rdparty/Eigen/bench/btl/libs/hand_vec/hand_vec_interface.hh

gtsam/3rdparty/Eigen/Eigen

gtsam/3rdparty/CCOLAMD/README.txt

gtsam/3rdparty/Eigen/doc/snippets/Tutorial_Map_rowmajor.cpp

gtsam/3rdparty/Eigen/bench/btl/libs/BLAS/blas_interface_impl.hh

gtsam/3rdparty/Eigen/Eigen/src/Core/SelfAdjointView.h

gtsam/3rdparty/Eigen/Eigen/src/Core/util/ReenableStupidWarnings.h

gtsam/3rdparty/Eigen/Eigen/Sparse

gtsam/3rdparty/Eigen/unsupported/Eigen/src/SparseExtra/CholmodSupport.h

gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h

gtsam/3rdparty/Eigen/blas/testing/cblat2.f

gtsam/3rdparty/Eigen/unsupported/Eigen/FFT

gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_panel_dfs.h

gtsam/3rdparty/Eigen/Eigen/src/Core/Select.h

gtsam/3rdparty/Eigen/doc/snippets/ColPivHouseholderQR_solve.cpp

gtsam/3rdparty/Eigen/unsupported/test/sparse_extra.cpp

gtsam/3rdparty/Eigen/src/Core/Redux.h

gtsam/3rdparty/Eigen/bench/bench_unrolling

gtsam/3rdparty/Eigen/bench/sparse_setter.cpp

gtsam/3rdparty/Eigen/bench/btl/cmake/FindGOTO2.cmake

gtsam/3rdparty/Eigen/unsupported/test/sparse_llt.cpp

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_diagonal_int.cpp

gtsam/3rdparty/Eigen/lapack/zlarfg.f

gtsam/3rdparty/Eigen/doc/snippets/tut_arithmetic_transpose_inplace.cpp

gtsam/3rdparty/Eigen/doc/QuickReference.dox

gtsam/3rdparty/Eigen/unsupported/Eigen/src/Splines/SplineFitting.h

gtsam/3rdparty/Eigen/Eigen/src/Geometry/Scaling.h

gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_Memory.h

gtsam/3rdparty/Eigen/doc/snippets/PartialRedux_minCoeff.cpp

gtsam/3rdparty/Eigen/doc/snippets/Tridiagonalization_diagonal.cpp

gtsam/3rdparty/Eigen/bench/btl/libs/BLAS/blas_interface.hh

gtsam/3rdparty/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h

gtsam/3rdparty/Eigen/src/Eigen2Support/Geometry/AngleAxis.h

gtsam/3rdparty/Eigen/blas/dtpmv.f

gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/Geometry/Translation.h

gtsam/3rdparty/Eigen/src/Core/Matrix.h

gtsam/3rdparty/Eigen/debug/msvc/eigen.natvis

gtsam/3rdparty/Eigen/unsupported/test/openglsupport.cpp

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_operatorNorm.cpp

gtsam/3rdparty/Eigen/src/Core/BandMatrix.h

gtsam/3rdparty/Eigen/unsupported/doc/examples/MatrixPower_optimal.cpp

gtsam/3rdparty/Eigen/failtest/diagonal_nonconst_ctor_on_const_xpr.cpp

gtsam/3rdparty/Eigen/doc/snippets/Tutorial_commainit_02.cpp

gtsam/3rdparty/Eigen/src/Core/TriangularMatrix.h

gtsam/3rdparty/Eigen/src/Jacobi/CMakeLists.txt

gtsam/3rdparty/Eigen/CMakeLists.txt

gtsam/3rdparty/Eigen/doc/I08_Resizing.dox

gtsam/3rdparty/Eigen/bench/bench_norm.cpp

gtsam/3rdparty/Eigen/src/Cholesky/CMakeLists.txt

gtsam/3rdparty/Eigen/Eigen/src/SparseCore/MappedSparseMatrix.h

gtsam/3rdparty/Eigen/unsupported/Eigen/UmfPackSupport

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_set.cpp

gtsam/3rdparty/Eigen/test/eigen2/eigen2_mixingtypes.cpp

gtsam/3rdparty/Eigen/bench/basicbenchmark.h

gtsam/3rdparty/UFconfig/xerbla/xerbla.h

gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/ComplexSchur.h

gtsam/3rdparty/Eigen/blas/complex_single.cpp

gtsam/3rdparty/Eigen/lapack/ilaclc.f

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_template_int_start.cpp

gtsam/3rdparty/Eigen/doc/snippets/HessenbergDecomposition_packedMatrix.cpp

gtsam/3rdparty/Eigen/blas/srotm.f

gtsam/3rdparty/Eigen/doc/snippets/DenseBase_LinSpaced.cpp

gtsam/3rdparty/Eigen/doc/snippets/HessenbergDecomposition_matrixH.cpp

gtsam/3rdparty/Eigen/Eigen/src/UmfPackSupport/CMakeLists.txt

gtsam/3rdparty/Eigen/doc/snippets/.krazy

gtsam/3rdparty/Eigen/test/inverse.cpp

gtsam/3rdparty/Eigen/doc/snippets/Tridiagonalization_decomposeInPlace.cpp

gtsam/3rdparty/Eigen/bench/btl/generic_bench/utils/size_log.hh

gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_panel_bmod.h

gtsam/3rdparty/Eigen/Eigen/src/Sparse/SparseTriangularView.h

gtsam/3rdparty/Eigen/test/geo_quaternion.cpp

gtsam/3rdparty/Eigen/CTestConfig.cmake

gtsam/3rdparty/Eigen/Eigen/src/Geometry/OrthoMethods.h

gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/Minor.h

gtsam/3rdparty/Eigen/doc/A05_PortingFrom2To3.dox

gtsam/3rdparty/Eigen/bench/btl/actions/action_lu_solve.hh

gtsam/3rdparty/Eigen/bench/benchGeometry.cpp

gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixFunction.h

gtsam/3rdparty/Eigen/bench/btl/actions/action_ata_product.hh

gtsam/3rdparty/Eigen/bench/geometry.cpp

gtsam/3rdparty/Eigen/bench/btl/libs/blitz/blitz_LU_solve_interface.hh

gtsam/3rdparty/Eigen/unsupported/Eigen/src/AutoDiff/CMakeLists.txt

gtsam/3rdparty/Eigen/src/plugins/BlockMethods.h

gtsam/3rdparty/CCOLAMD/Doc/lesser.txt

gtsam/3rdparty/Eigen/src/misc/Solve.h

gtsam/3rdparty/Eigen/unsupported/Eigen/src/BVH/KdBVH.h

gtsam/3rdparty/Eigen/src/Core/MatrixBase.h

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_isDiagonal.cpp

gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/VectorBlock.h

gtsam/3rdparty/CCOLAMD/MATLAB/csymamd.m

gtsam/3rdparty/Eigen/Eigen/src/Core/NumTraits.h

gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/MINRES.h

gtsam/3rdparty/Eigen/test/eigensolver_generalized_real.cpp

gtsam/3rdparty/Eigen/lapack/zlacgv.f

gtsam/3rdparty/Eigen/test/eigen2/eigen2_swap.cpp

gtsam/3rdparty/Eigen/eigen3.pc.in

gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/GeneralizedSelfAdjointEigenSolver.h

gtsam/3rdparty/CCOLAMD/Demo/ccolamd_example.out

gtsam/3rdparty/Eigen/bench/btl/libs/blitz/btl_tiny_blitz.cpp

gtsam/3rdparty/Eigen/doc/snippets/ComplexSchur_matrixT.cpp

gtsam/3rdparty/Eigen/doc/SparseLinearSystems.dox

gtsam/3rdparty/Eigen/doc/examples/QuickStart_example.cpp

gtsam/3rdparty/Eigen/test/swap.cpp

gtsam/3rdparty/Eigen/doc/snippets/AngleAxis_mimic_euler.cpp

gtsam/3rdparty/Eigen/bench/btl/generic_bench/static/bench_static.hh

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_computeInverseAndDetWithCheck.cpp

gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/CMakeLists.txt

gtsam/3rdparty/Eigen/bench/spbench/CMakeLists.txt

gtsam/3rdparty/Eigen/test/eigen2/CMakeLists.txt

gtsam/3rdparty/Eigen/Eigen/src/Core/Replicate.h

gtsam/3rdparty/Eigen/Eigen/src/QR/HouseholderQR_MKL.h

gtsam/3rdparty/Eigen/src/Core/products/CoeffBasedProduct.h

gtsam/3rdparty/Eigen/unsupported/Eigen/SVD

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_isZero.cpp

gtsam/3rdparty/Eigen/doc/C09_TutorialSparse.dox

gtsam/3rdparty/Eigen/unsupported/test/autodiff.cpp

gtsam/3rdparty/Eigen/doc/examples/class_FixedBlock.cpp

gtsam/3rdparty/Eigen/src/Geometry/AngleAxis.h

gtsam/3rdparty/Eigen/test/miscmatrices.cpp

gtsam/3rdparty/Eigen/test/eigen2/eigen2_unalignedassert.cpp

gtsam/3rdparty/Eigen/doc/snippets/LLT_solve.cpp

gtsam/3rdparty/Eigen/src/Eigen2Support/SVD.h

gtsam/3rdparty/Eigen/doc/snippets/Tutorial_commainit_01.cpp

gtsam/3rdparty/Eigen/doc/snippets/Cwise_greater.cpp

gtsam/3rdparty/Eigen/lapack/ilaslc.f

gtsam/3rdparty/Eigen/doc/examples/Tutorial_simple_example_dynamic_size.cpp

gtsam/3rdparty/Eigen/src/misc/Makefile.am

gtsam/3rdparty/Eigen/src/Core/EigenBase.h

gtsam/3rdparty/Eigen/doc/examples/tut_matrix_resize_fixed_size.cpp

gtsam/3rdparty/Eigen/bench/btl/cmake/FindACML.cmake

gtsam/3rdparty/Eigen/doc/eigendoxy_footer.html.in

gtsam/3rdparty/Eigen/bench/btl/libs/eigen3/eigen3_interface.hh

gtsam/3rdparty/Eigen/doc/TutorialReductionsVisitorsBroadcasting.dox

gtsam/3rdparty/Eigen/src/Core/CwiseNullaryOp.h

gtsam/3rdparty/Eigen/bench/btl/cmake/FindMTL4.cmake

gtsam/3rdparty/Eigen/src/Eigen2Support/Geometry/Translation.h

gtsam/3rdparty/Eigen/bench/btl/actions/action_ger.hh

gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularSolverVector.h

gtsam/3rdparty/Eigen/bench/btl/README

gtsam/3rdparty/Eigen/src/Core/arch/SSE/CMakeLists.txt

gtsam/3rdparty/Eigen/doc/examples/DenseBase_template_int_middleCols.cpp

gtsam/3rdparty/Eigen/Eigen/src/Core/arch/AltiVec/CMakeLists.txt

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_template_int_int_topRightCorner.cpp

gtsam/3rdparty/Eigen/src/Sparse/SparseTranspose.h

gtsam/3rdparty/Eigen/Eigen/src/Core/util/NonMPL2.h

gtsam/3rdparty/Eigen/src/Sparse/SparseDot.h

gtsam/3rdparty/Eigen/failtest/block_on_const_type_actually_const_1.cpp

gtsam/3rdparty/Eigen/unsupported/Eigen/src/KroneckerProduct/KroneckerTensorProduct.h

gtsam/3rdparty/Eigen/bench/btl/cmake/FindCBLAS.cmake

gtsam/3rdparty/Eigen/src/Geometry/Translation.h

gtsam/3rdparty/Eigen/doc/examples/TutorialLinAlgExSolveLDLT.cpp

gtsam/3rdparty/Eigen/bench/benchVecAdd.cpp

gtsam/3rdparty/Eigen/Eigen/src/Core/Functors.h

gtsam/3rdparty/Eigen/bench/btl/libs/C_BLAS/CMakeLists.txt

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_template_int_int_block_int_int_int_int.cpp

gtsam/3rdparty/Eigen/src/Core/MatrixStorage.h

gtsam/3rdparty/Eigen/src/StlSupport/StdDeque.h

gtsam/3rdparty/Eigen/doc/examples/Tutorial_ReductionsVisitorsBroadcasting_broadcast_simple.cpp

gtsam/3rdparty/Eigen/bench/btl/generic_bench/init/init_function.hh

gtsam/3rdparty/Eigen/unsupported/Eigen/src/NonLinearOptimization/lmpar.h

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_all.cpp

gtsam/3rdparty/Eigen/src/Core/arch/SSE/PacketMath.h

gtsam/3rdparty/Eigen/lapack/slapy3.f

gtsam/3rdparty/Eigen/unsupported/test/dgmres.cpp

gtsam/3rdparty/Eigen/blas/sspr.f

gtsam/3rdparty/Eigen/Eigen/src/UmfPackSupport/UmfPackSupport.h

gtsam/3rdparty/Eigen/doc/D09_StructHavingEigenMembers.dox

gtsam/3rdparty/Eigen/test/zerosized.cpp

gtsam/3rdparty/Eigen/doc/snippets/Cwise_asin.cpp

gtsam/3rdparty/Eigen/blas/single.cpp

gtsam/3rdparty/Eigen/doc/examples/TemplateKeyword_simple.cpp

gtsam/3rdparty/UFconfig/UFconfig.c

gtsam/3rdparty/Eigen/test/geo_hyperplane.cpp

gtsam/3rdparty/Eigen/doc/A10_Eigen2SupportModes.dox

gtsam/3rdparty/Eigen/src/Sparse/SparseFuzzy.h

gtsam/3rdparty/Eigen/bench/btl/actions/action_matrix_matrix_product.hh

gtsam/3rdparty/Eigen/Eigen/src/plugins/CMakeLists.txt

gtsam/3rdparty/Eigen/unsupported/test/matrix_functions.h

gtsam/3rdparty/Eigen/blas/PackedTriangularMatrixVector.h

gtsam/3rdparty/Eigen/test/conservative_resize.cpp

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_setOnes.cpp

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_isOrthogonal.cpp

gtsam/3rdparty/Eigen/Eigen/src/SparseCholesky/CMakeLists.txt

gtsam/3rdparty/Eigen/Eigen/src/Geometry/Homogeneous.h

gtsam/3rdparty/Eigen/test/unalignedcount.cpp

gtsam/3rdparty/Eigen/bench/btl/libs/mtl4/CMakeLists.txt

gtsam/3rdparty/Eigen/src/misc/Image.h

gtsam/3rdparty/Eigen/src/Core/DiagonalProduct.h

gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LLT.h

gtsam/3rdparty/Eigen/doc/TutorialMatrixArithmetic.dox

gtsam/3rdparty/Eigen/LeastSquares

gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/CMakeLists.txt

gtsam/3rdparty/Eigen/src/Core/arch/Makefile.am

gtsam/3rdparty/Eigen/src/misc/Kernel.h

gtsam/3rdparty/Eigen/test/eigen2/eigen2_inverse.cpp

gtsam/3rdparty/Eigen/src/Core/NoAlias.h

gtsam/3rdparty/Eigen/bench/btl/libs/mtl4/mtl4_interface.hh

gtsam/3rdparty/Eigen/Eigen/src/Householder/CMakeLists.txt

gtsam/3rdparty/Eigen/Eigen/src/LU/Determinant.h

gtsam/3rdparty/Eigen/lapack/CMakeLists.txt

gtsam/3rdparty/Eigen/demos/opengl/gpuhelper.cpp

gtsam/3rdparty/ccolamd/UFconfig.mk

gtsam/3rdparty/Eigen/test/qtvector.cpp

gtsam/3rdparty/Eigen/bench/btl/libs/eigen3/main_linear.cpp

gtsam/3rdparty/Eigen/src/Core/Block.h

gtsam/3rdparty/Eigen/Eigen/Jacobi

gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/CMakeLists.txt

gtsam/3rdparty/Eigen/doc/examples/tut_arithmetic_add_sub.cpp

gtsam/3rdparty/Eigen/demos/opengl/camera.cpp

gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/MathFunctions.h

gtsam/3rdparty/Eigen/doc/snippets/Cwise_abs.cpp

gtsam/3rdparty/Eigen/bench/btl/data/perlib_plot_settings.txt

gtsam/3rdparty/Eigen/blas/dspr.f

gtsam/3rdparty/Eigen/Eigen/src/SparseCholesky/SimplicialCholesky.h

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_cwiseAbs.cpp

gtsam/3rdparty/Eigen/test/product_symm.cpp

gtsam/3rdparty/Eigen/Eigen/QR

gtsam/3rdparty/Eigen/Eigen/src/Geometry/AlignedBox.h

gtsam/3rdparty/Eigen/bench/btl/generic_bench/timers/STL_timer.hh

gtsam/3rdparty/Eigen/src/Sparse/SparseMatrixBase.h

gtsam/3rdparty/Eigen/unsupported/doc/examples/PolynomialSolver1.cpp

gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularMatrixVector_MKL.h

gtsam/3rdparty/Eigen/doc/snippets/PartialRedux_squaredNorm.cpp

gtsam/3rdparty/Eigen/unsupported/Eigen/NonLinearOptimization

gtsam/3rdparty/Eigen/bench/btl/libs/f77/saat.f

gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/RealQZ.h

gtsam/3rdparty/Eigen/src/Geometry/arch/CMakeLists.txt

gtsam/3rdparty/Eigen/doc/snippets/EigenSolver_eigenvalues.cpp

gtsam/3rdparty/Eigen/doc/examples/.krazy

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_extract.cpp

gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/Geometry/ParametrizedLine.h

gtsam/3rdparty/Eigen/src/Core/DenseCoeffsBase.h

gtsam/3rdparty/Eigen/cmake/FindStandardMathLibrary.cmake

gtsam/3rdparty/Eigen/src/Core/Reverse.h

gtsam/3rdparty/Eigen/bench/btl/libs/f77/dmxm.f

gtsam/3rdparty/Eigen/Core

gtsam/3rdparty/Eigen/doc/I06_TopicEigenExpressionTemplates.dox

gtsam/3rdparty/Eigen/doc/examples/Tutorial_BlockOperations_print_block.cpp

gtsam/3rdparty/Eigen/Eigen/src/Core/DiagonalProduct.h

gtsam/3rdparty/Eigen/doc/examples/DenseBase_template_int_middleRows.cpp

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_col.cpp

gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularSolverMatrix.h

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_zero_int_int.cpp

gtsam/3rdparty/Eigen/blas/chbmv.f

gtsam/3rdparty/Eigen/test/eigen2/eigen2_meta.cpp

gtsam/3rdparty/Eigen/doc/snippets/DenseBase_setLinSpaced.cpp

gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/RealSchur.h

gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_heap_relax_snode.h

gtsam/3rdparty/Eigen/doc/examples/Tutorial_ArrayClass_interop_matrix.cpp

gtsam/3rdparty/Eigen/lapack/clarfg.f

gtsam/3rdparty/Eigen/test/geo_homogeneous.cpp

gtsam/3rdparty/Eigen/Eigen/src/Core/TriangularMatrix.h

gtsam/3rdparty/Eigen/blas/testing/runblastest.sh

gtsam/3rdparty/Eigen/doc/snippets/IOFormat.cpp

gtsam/3rdparty/Eigen/Eigen/src/Core/Swap.h

gtsam/3rdparty/Eigen/doc/snippets/Map_inner_stride.cpp

gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/BasicPreconditioners.h

gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h

gtsam/3rdparty/Eigen/bench/btl/generic_bench/static/intel_bench_fixed_size.hh

gtsam/3rdparty/Eigen/Eigen/SuperLUSupport

gtsam/3rdparty/Eigen/doc/snippets/Cwise_abs2.cpp

gtsam/3rdparty/Eigen/test/eigen2/eigen2_miscmatrices.cpp

gtsam/3rdparty/Eigen/unsupported/test/alignedvector3.cpp

gtsam/3rdparty/Eigen/doc/B01_Experimental.dox

gtsam/3rdparty/Eigen/doc/examples/Tutorial_ReductionsVisitorsBroadcasting_colwise.cpp

gtsam/3rdparty/Eigen/unsupported/test/NumericalDiff.cpp

gtsam/3rdparty/Eigen/CTestCustom.cmake.in

gtsam/3rdparty/Eigen/test/eigen2/eigen2_basicstuff.cpp

gtsam/3rdparty/Eigen/unsupported/Eigen/src/SparseExtra/RandomSetter.h

gtsam/3rdparty/Eigen/signature_of_eigen3_matrix_library

gtsam/3rdparty/Eigen/src/Core/util/Macros.h

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_rightCols_int.cpp

gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/Cwise.h

gtsam/3rdparty/Eigen/blas/ztpsv.f

gtsam/3rdparty/CCOLAMD/MATLAB/ccolamdtestmex.c

gtsam/3rdparty/Eigen/src/Householder/BlockHouseholder.h

gtsam/3rdparty/CCOLAMD/MATLAB/ccolamdtestmex.m

gtsam/3rdparty/Eigen/doc/eigendoxy_header.html.in

gtsam/3rdparty/Eigen/src/Eigen2Support/Geometry/AlignedBox.h

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_template_int_int_bottomLeftCorner.cpp

gtsam/3rdparty/Eigen/src/Core/util/Constants.h

gtsam/3rdparty/Eigen/failtest/map_nonconst_ctor_on_const_ptr_0.cpp

gtsam/3rdparty/Eigen/doc/snippets/Tridiagonalization_compute.cpp

gtsam/3rdparty/CCOLAMD/Doc/ChangeLog

gtsam/3rdparty/Eigen/unsupported/Eigen/src/Skyline/SkylineInplaceLU.h

gtsam/3rdparty/Eigen/bench/check_cache_queries.cpp

gtsam/3rdparty/Eigen/bench/btl/libs/eigen2/btl_tiny_eigen2.cpp

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_marked.cpp

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_adjoint.cpp

gtsam/3rdparty/Eigen/test/smallvectors.cpp

gtsam/3rdparty/CCOLAMD/MATLAB/ccolamd_make.m

gtsam/3rdparty/Eigen/unsupported/Eigen/src/LevenbergMarquardt/LMpar.h

gtsam/3rdparty/Eigen/test/conjugate_gradient.cpp

gtsam/3rdparty/Eigen/doc/TutorialLinearAlgebra.dox

gtsam/3rdparty/Eigen/doc/examples/tut_arithmetic_matrix_mul.cpp

gtsam/3rdparty/Eigen/doc/unsupported_modules.dox

gtsam/3rdparty/Eigen/doc/snippets/Cwise_not_equal.cpp

gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseCwiseBinaryOp.h

gtsam/3rdparty/Eigen/doc/TutorialAdvancedInitialization.dox

gtsam/3rdparty/Eigen/Eigen/src/Sparse/TriangularSolver.h

gtsam/3rdparty/Eigen/cmake/FindCholmod.cmake

gtsam/3rdparty/Eigen/test/stddeque.cpp

gtsam/3rdparty/Eigen/doc/examples/class_Block.cpp

gtsam/3rdparty/Eigen/unsupported/Eigen/src/NonLinearOptimization/chkder.h

gtsam/3rdparty/Eigen/bench/btl/generic_bench/bench_parameter.hh

gtsam/3rdparty/Eigen/src/Core/MathFunctions.h

gtsam/3rdparty/Eigen/lapack/slarfg.f

gtsam/3rdparty/Eigen/doc/examples/MatrixBase_cwise_const.cpp

gtsam/3rdparty/Eigen/src/Householder/CMakeLists.txt

gtsam/3rdparty/Eigen/bench/btl/generic_bench/bench.hh

gtsam/3rdparty/Eigen/src/Core/VectorBlock.h

gtsam/3rdparty/Eigen/bench/btl/generic_bench/static/static_size_generator.hh

gtsam/3rdparty/Eigen/Eigen/src/Core/util/Memory.h

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_transpose.cpp

gtsam/3rdparty/Eigen/blas/testing/zblat3.f

gtsam/3rdparty/Eigen/unsupported/CMakeLists.txt

gtsam/3rdparty/Eigen/src/Core/arch/AltiVec/Makefile.am

gtsam/3rdparty/Eigen/src/SVD/Makefile.am

gtsam/3rdparty/Eigen/src/Core/Flagged.h

gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h

gtsam/3rdparty/Eigen/src/LU/Makefile.am

gtsam/3rdparty/Eigen/blas/sspr2.f

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_template_int_segment.cpp

gtsam/3rdparty/Eigen/bench/quatmul.cpp

gtsam/3rdparty/Eigen/bench/btl/libs/f77/saxpy.f

gtsam/3rdparty/Eigen/scripts/CMakeLists.txt

gtsam/3rdparty/Eigen/src/Core/products/GeneralBlockPanelKernel.h

gtsam/3rdparty/Eigen/unsupported/Eigen/src/Polynomials/CMakeLists.txt

gtsam/3rdparty/Eigen/test/array_reverse.cpp

gtsam/3rdparty/Eigen/src/Core/Map.h

gtsam/3rdparty/Eigen/unsupported/Eigen/AutoDiff

gtsam/3rdparty/Eigen/blas/stpsv.f

gtsam/3rdparty/Eigen/Eigen/src/Core/DenseBase.h

gtsam/3rdparty/Eigen/doc/snippets/Cwise_pow.cpp

gtsam/3rdparty/Eigen/doc/snippets/TopicAliasing_mult3.cpp

gtsam/3rdparty/Eigen/Eigen/src/Sparse/SparseRedux.h

gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/Geometry/Transform.h

gtsam/3rdparty/Eigen/bench/btl/actions/action_matrix_matrix_product_bis.hh

gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/Geometry/All.h

gtsam/3rdparty/Eigen/Eigen/src/QR/HouseholderQR.h

gtsam/3rdparty/Eigen/bench/btl/generic_bench/utils/size_lin_log.hh

gtsam/3rdparty/Eigen/unsupported/test/FFT.cpp

gtsam/3rdparty/Eigen/cmake/language_support.cmake

gtsam/3rdparty/Eigen/Eigen/src/LU/CMakeLists.txt

gtsam/3rdparty/Eigen/Eigen/StdDeque

gtsam/3rdparty/Eigen/Eigen/src/misc/blas.h

gtsam/3rdparty/Eigen/unsupported/doc/examples/CMakeLists.txt

gtsam/3rdparty/Eigen/bench/btl/libs/C_BLAS/cblas.h

gtsam/3rdparty/Eigen/test/eigen2/eigen2_array.cpp

gtsam/3rdparty/Eigen/doc/SparseQuickReference.dox

gtsam/3rdparty/Eigen/doc/snippets/PartialPivLU_solve.cpp

gtsam/3rdparty/Eigen/unsupported/doc/examples/MatrixPower.cpp

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_colwise.cpp

gtsam/3rdparty/Eigen/src/LU/CMakeLists.txt

gtsam/3rdparty/Eigen/doc/examples/class_CwiseUnaryOp_ptrfun.cpp

gtsam/3rdparty/Eigen/src/Sparse/SparseProduct.h

gtsam/3rdparty/Eigen/src/Core/products/CMakeLists.txt

gtsam/3rdparty/Eigen/src/Core/util/Meta.h

gtsam/3rdparty/Eigen/unsupported/Eigen/src/Splines/CMakeLists.txt

gtsam/3rdparty/Eigen/blas/ztbsv.f

gtsam/3rdparty/Eigen/failtest/transpose_nonconst_ctor_on_const_xpr.cpp

gtsam/3rdparty/Eigen/Eigen/src/Geometry/CMakeLists.txt

gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/Lazy.h

gtsam/3rdparty/Eigen/doc/snippets/PartialRedux_sum.cpp

gtsam/3rdparty/Eigen/cmake/FindGLEW.cmake

gtsam/3rdparty/Eigen/src/LU/Determinant.h

gtsam/3rdparty/Eigen/src/SVD/UpperBidiagonalization.h

gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrix.h

gtsam/3rdparty/Eigen/unsupported/test/svd_common.h

gtsam/3rdparty/Eigen/src/Eigen2Support/Geometry/Makefile.am

gtsam/3rdparty/Eigen/bench/btl/data/mk_new_gnuplot.sh

gtsam/3rdparty/Eigen/test/commainitializer.cpp

gtsam/3rdparty/Eigen/doc/snippets/Matrix_resize_NoChange_int.cpp

gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseCwiseUnaryOp.h

gtsam/3rdparty/Eigen/doc/snippets/Tridiagonalization_householderCoefficients.cpp

gtsam/3rdparty/Eigen/doc/snippets/TopicAliasing_block.cpp

gtsam/3rdparty/Eigen/doc/snippets/HouseholderQR_householderQ.cpp

gtsam/3rdparty/Eigen/unsupported/Eigen/src/BVH/CMakeLists.txt

gtsam/3rdparty/Eigen/Eigen/src/misc/Kernel.h

gtsam/3rdparty/Eigen/unsupported/Eigen/MPRealSupport

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_topLeftCorner_int_int.cpp

gtsam/3rdparty/Eigen/doc/I07_TopicScalarTypes.dox

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_rowwise.cpp

gtsam/3rdparty/Eigen/doc/TopicEigenExpressionTemplates.dox

gtsam/3rdparty/Eigen/test/eigen2/eigen2_adjoint.cpp

gtsam/3rdparty/Eigen/test/metis_support.cpp

gtsam/3rdparty/Eigen/test/eigen2/testsuite.cmake

gtsam/3rdparty/Eigen/lapack/clarfb.f

gtsam/3rdparty/Eigen/test/mapstaticmethods.cpp

gtsam/3rdparty/Eigen/src/Householder/Householder.h

gtsam/3rdparty/Eigen/unsupported/Eigen/src/SparseExtra/MatrixMarketIterator.h

gtsam/3rdparty/Eigen/lapack/clarf.f

gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLUImpl.h

gtsam/3rdparty/Eigen/bench/btl/libs/f77/f77_interface.hh

gtsam/3rdparty/Eigen/scripts/relicense.py

gtsam/3rdparty/Eigen/src/Eigenvalues/CMakeLists.txt

gtsam/3rdparty/Eigen/cmake/EigenDetermineVSServicePack.cmake

gtsam/3rdparty/Eigen/src/Eigen2Support/Geometry/Scaling.h

gtsam/3rdparty/Eigen/unsupported/test/matrix_function.cpp

gtsam/3rdparty/Eigen/bench/btl/data/regularize.cxx

gtsam/3rdparty/Eigen/src/LU/arch/Inverse_SSE.h

gtsam/3rdparty/Eigen/bench/btl/libs/BLAS/main.cpp

gtsam/3rdparty/Eigen/test/eigen2/eigen2_cholesky.cpp

gtsam/3rdparty/Eigen/doc/snippets/Cwise_cos.cpp

gtsam/3rdparty/Eigen/unsupported/Eigen/MatrixFunctions

gtsam/3rdparty/Eigen/unsupported/Eigen/src/LevenbergMarquardt/LMqrsolv.h

gtsam/3rdparty/Eigen/doc/I15_StorageOrders.dox

gtsam/3rdparty/Eigen/cmake/EigenDetermineOSVersion.cmake

gtsam/3rdparty/Eigen/bench/eig33.cpp

gtsam/3rdparty/Eigen/lapack/sladiv.f

gtsam/3rdparty/Eigen/unsupported/test/levenberg_marquardt.cpp

gtsam/3rdparty/Eigen/Eigen/src/StlSupport/StdDeque.h

gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointProduct.h

gtsam/3rdparty/Eigen/bench/btl/libs/C/CMakeLists.txt

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_reverse.cpp

gtsam/3rdparty/Eigen/src/Geometry/Makefile.am

gtsam/3rdparty/Eigen/Eigen/src/SparseCore/CompressedStorage.h

gtsam/3rdparty/Eigen/src/Sparse/MappedSparseMatrix.h

gtsam/3rdparty/Eigen/test/eigen2/gsl_helper.h

gtsam/3rdparty/Eigen/doc/snippets/EigenSolver_EigenSolver_MatrixType.cpp

gtsam/3rdparty/Eigen/src/Core/products/TriangularMatrixVector.h

gtsam/3rdparty/Eigen/Eigen/src/Sparse/SparseSelfAdjointView.h

gtsam/3rdparty/Eigen/bench/basicbench.cxxlist

gtsam/3rdparty/Eigen/bench/btl/cmake/FindPackageHandleStandardArgs.cmake

gtsam/3rdparty/Eigen/src/Core/arch/Default/Makefile.am

gtsam/3rdparty/Eigen/unsupported/test/mpreal/mpreal.cpp

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_block_int_int.cpp

gtsam/3rdparty/Eigen/unsupported/Eigen/CholmodSupport

gtsam/3rdparty/Eigen/doc/I12_ClassHierarchy.dox

gtsam/3rdparty/Eigen/src/Sparse/SparseDenseProduct.h

gtsam/3rdparty/Eigen/test/stdvector.cpp

gtsam/3rdparty/Eigen/unsupported/doc/examples/MatrixFunction.cpp

gtsam/3rdparty/Eigen/Eigen/src/plugins/BlockMethods.h

gtsam/3rdparty/Eigen/unsupported/Eigen/src/SVD/doneInBDCSVD.txt

gtsam/3rdparty/Eigen/src/LU/arch/CMakeLists.txt

gtsam/3rdparty/Eigen/test/eigen2/eigen2_hyperplane.cpp

gtsam/3rdparty/Eigen/doc/snippets/Tutorial_solve_triangular_inplace.cpp

gtsam/3rdparty/Eigen/unsupported/bench/bench_svd.cpp

gtsam/3rdparty/Eigen/doc/TopicLazyEvaluation.dox

gtsam/3rdparty/Eigen/test/nesting_ops.cpp

gtsam/3rdparty/Eigen/src/StlSupport/CMakeLists.txt

gtsam/3rdparty/Eigen/test/diagonalmatrices.cpp

gtsam/3rdparty/Eigen/test/gsl_helper.h

gtsam/3rdparty/Eigen/bench/btl/libs/tvmet/main.cpp

gtsam/3rdparty/Eigen/Eigen/Eigen

gtsam/3rdparty/Eigen/demos/opengl/quaternion_demo.h

gtsam/3rdparty/Eigen/.hgeol

gtsam/3rdparty/Eigen/doc/TutorialMatrixClass.dox

gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_copy_to_ucol.h

gtsam/3rdparty/Eigen/doc/snippets/FullPivLU_image.cpp

gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseView.h

gtsam/3rdparty/Eigen/src/Eigen2Support/VectorBlock.h

gtsam/3rdparty/Eigen/demos/opengl/trackball.cpp

gtsam/3rdparty/Eigen/test/qr_colpivoting.cpp

gtsam/3rdparty/Eigen/bench/sparse_transpose.cpp

gtsam/3rdparty/Eigen/doc/snippets/Matrix_setZero_int_int.cpp

gtsam/3rdparty/Eigen/src/QR/ColPivHouseholderQR.h

gtsam/3rdparty/Eigen/bench/bench_gemm.cpp

gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR_MKL.h

gtsam/3rdparty/Eigen/unsupported/doc/examples/FFT.cpp

gtsam/3rdparty/Eigen/lapack/zlarf.f

gtsam/3rdparty/Eigen/src/Eigenvalues/MatrixBaseEigenvalues.h

gtsam/3rdparty/Eigen/Eigen/src/Core/arch/AltiVec/PacketMath.h

gtsam/3rdparty/CCOLAMD/Demo/my_ccolamd_l_example.out

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_cast.cpp

gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/EigenSolver.h

gtsam/3rdparty/ccolamd/ccolamd_Makefile.mk

gtsam/3rdparty/Eigen/doc/snippets/Jacobi_makeGivens.cpp

gtsam/3rdparty/Eigen/bench/btl/libs/STL_algo/CMakeLists.txt

gtsam/3rdparty/Eigen/unsupported/Eigen/src/NonLinearOptimization/CMakeLists.txt

gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/ComplexEigenSolver.h

gtsam/3rdparty/Eigen/test/eigen2/eigen2_sum.cpp

gtsam/3rdparty/Eigen/lapack/complex_single.cpp

gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/HessenbergDecomposition.h

gtsam/3rdparty/Eigen/doc/eigendoxy_tabs.css

gtsam/3rdparty/Eigen/bench/benchFFT.cpp

gtsam/3rdparty/Eigen/Eigen/src/Core/DenseStorage.h

gtsam/3rdparty/Eigen/src/Sparse/SparseSparseProduct.h

gtsam/3rdparty/Eigen/test/eigen2/sparse.h

gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseMatrixBase.h

gtsam/3rdparty/Eigen/Eigen/src/Core/GlobalFunctions.h

gtsam/3rdparty/Eigen/unsupported/Eigen/src/SparseExtra/SuperLUSupport.h

gtsam/3rdparty/Eigen/unsupported/Eigen/src/NonLinearOptimization/LevenbergMarquardt.h

gtsam/3rdparty/Eigen/test/eigen2/eigen2_commainitializer.cpp

gtsam/3rdparty/Eigen/test/nomalloc.cpp

gtsam/3rdparty/UFconfig/UFconfig.mk

gtsam/3rdparty/Eigen/test/array_replicate.cpp

gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/Geometry/Hyperplane.h

gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/Memory.h

gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU.h

gtsam/3rdparty/Eigen/blas/lsame.f

gtsam/3rdparty/Eigen/doc/D11_UnalignedArrayAssert.dox

gtsam/3rdparty/Eigen/bench/sparse_randomsetter.cpp

gtsam/3rdparty/Eigen/doc/C05_TutorialAdvancedInitialization.dox

gtsam/3rdparty/Eigen/doc/FunctionsTakingEigenTypes.dox

gtsam/3rdparty/Eigen/doc/snippets/Cwise_minus.cpp

gtsam/3rdparty/Eigen/lapack/dlarf.f

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_template_int_int_bottomLeftCorner_int_int.cpp

gtsam/3rdparty/Eigen/doc/snippets/Cwise_cube.cpp

gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/RealSchur_MKL.h

gtsam/3rdparty/Eigen/src/Cholesky/LLT.h

gtsam/3rdparty/Eigen/doc/snippets/TopicAliasing_cwise.cpp

gtsam/3rdparty/Eigen/Eigen/src/Core/PermutationMatrix.h

gtsam/3rdparty/Eigen/Eigen/src/Sparse/SparseView.h

gtsam/3rdparty/Eigen/src/Core/util/StaticAssert.h

gtsam/3rdparty/Eigen/Eigen/src/LU/arch/CMakeLists.txt

gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/Block.h

gtsam/3rdparty/Eigen/test/qr.cpp

gtsam/3rdparty/Eigen/unsupported/Eigen/src/NonLinearOptimization/rwupdt.h

gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/Macros.h

gtsam/3rdparty/Eigen/Eigen/Householder

gtsam/3rdparty/Eigen/doc/snippets/SelfAdjointEigenSolver_compute_MatrixType.cpp

gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/CwiseOperators.h

gtsam/3rdparty/Eigen/test/schur_complex.cpp

gtsam/3rdparty/Eigen/src/CMakeLists.txt

gtsam/3rdparty/Eigen/doc/WrongStackAlignment.dox

gtsam/3rdparty/Eigen/bench/btl/data/action_settings.txt

gtsam/3rdparty/Eigen/unsupported/Eigen/BVH

gtsam/3rdparty/Eigen/test/eigen2/eigen2_packetmath.cpp

gtsam/3rdparty/Eigen/bench/spbench/spbenchstyle.h

gtsam/3rdparty/Eigen/bench/benchCholesky.cpp

gtsam/3rdparty/Eigen/blas/level1_impl.h

gtsam/3rdparty/Eigen/doc/snippets/tut_arithmetic_transpose_aliasing.cpp

gtsam/3rdparty/Eigen/doc/tutorial.cpp

gtsam/3rdparty/Eigen/Eigen/src/Geometry/arch/CMakeLists.txt

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_template_int_int_topRightCorner_int_int.cpp

gtsam/3rdparty/Eigen/doc/C00_QuickStartGuide.dox

gtsam/3rdparty/Eigen/doc/snippets/SelfAdjointEigenSolver_eigenvalues.cpp

gtsam/3rdparty/Eigen/src/Sparse/TriangularSolver.h

gtsam/3rdparty/ccolamd/README.txt

gtsam/3rdparty/Eigen/doc/examples/Tutorial_ReductionsVisitorsBroadcasting_rowwise.cpp

gtsam/3rdparty/Eigen/unsupported/Eigen/src/AutoDiff/AutoDiffVector.h

gtsam/3rdparty/Eigen/Eigen/src/plugins/ArrayCwiseBinaryOps.h

gtsam/3rdparty/Eigen/Sparse

gtsam/3rdparty/Eigen/Eigen/src/LU/Inverse.h

gtsam/3rdparty/Eigen/doc/examples/TutorialLinAlgSetThreshold.cpp

gtsam/3rdparty/Eigen/test/bicgstab.cpp

gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/PacketMath.h

gtsam/3rdparty/Eigen/unsupported/Eigen/src/SVD/BDCSVD.h

gtsam/3rdparty/Eigen/bench/btl/data/CMakeLists.txt

gtsam/3rdparty/Eigen/blas/stpmv.f

gtsam/3rdparty/Eigen/blas/testing/dblat3.f

gtsam/3rdparty/Eigen/test/real_qz.cpp

gtsam/3rdparty/Eigen/src/Sparse/SparseSelfAdjointView.h

gtsam/3rdparty/Eigen/doc/examples/TemplateKeyword_flexible.cpp

gtsam/3rdparty/Eigen/src/Core/Assign.h

gtsam/3rdparty/Eigen/test/product.h

gtsam/3rdparty/Eigen/StdDeque

gtsam/3rdparty/Eigen/src/Eigen2Support/Cwise.h

gtsam/3rdparty/Eigen/test/eigen2/eigen2_sparse_solvers.cpp

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_noalias.cpp

gtsam/3rdparty/Eigen/src/Eigen2Support/Geometry/Transform.h

gtsam/3rdparty/Eigen/doc/snippets/compile_snippet.cpp.in

gtsam/3rdparty/Eigen/bench/benchmarkSlice.cpp

gtsam/3rdparty/Eigen/src/Core/CwiseUnaryOp.h

gtsam/3rdparty/Eigen/src/Sparse/CompressedStorage.h

gtsam/3rdparty/Eigen/bench/btl/libs/ublas/main.cpp

gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularMatrixMatrix_MKL.h

gtsam/3rdparty/Eigen/Eigen/SparseLU

gtsam/3rdparty/Eigen/test/mixingtypes.cpp

gtsam/3rdparty/Eigen/blas/stbmv.f

gtsam/3rdparty/Eigen/Eigen/src/CMakeLists.txt

gtsam/3rdparty/Eigen/bench/btl/data/smooth_all.sh

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_asDiagonal.cpp

gtsam/3rdparty/Eigen/doc/Doxyfile.in

gtsam/3rdparty/Eigen/src/SVD/CMakeLists.txt

gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseDot.h

gtsam/3rdparty/Eigen/doc/examples/class_CwiseBinaryOp.cpp

gtsam/3rdparty/Eigen/scripts/cdashtesting.cmake.in

gtsam/3rdparty/Eigen/src/Core/products/GeneralMatrixMatrixTriangular.h

gtsam/3rdparty/Eigen/test/eigen2/runtest.sh

gtsam/3rdparty/Eigen/test/ref.cpp

gtsam/3rdparty/Eigen/doc/I00_CustomizingEigen.dox

gtsam/3rdparty/Eigen/COPYING.GPL

gtsam/3rdparty/Eigen/Eigen/src/Core/Visitor.h

gtsam/3rdparty/Eigen/Eigen/src/Core/util/StaticAssert.h

gtsam/3rdparty/Eigen/doc/examples/TutorialLinAlgExSolveColPivHouseholderQR.cpp

gtsam/3rdparty/Eigen/unsupported/Eigen/Splines

gtsam/3rdparty/Eigen/Eigen/StdList

gtsam/3rdparty/Eigen/lapack/dlarfg.f

gtsam/3rdparty/Eigen/Eigen/UmfPackSupport

gtsam/3rdparty/CCOLAMD/MATLAB/ccolamd.m

gtsam/3rdparty/Eigen/Eigen/src/Core/Flagged.h

gtsam/3rdparty/Eigen/doc/special_examples/Tutorial_sparse_example_details.cpp

gtsam/3rdparty/Eigen/unsupported/Eigen/Skyline

gtsam/3rdparty/Eigen/Eigen/src/Sparse/CompressedStorage.h

gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseMatrix.h

gtsam/3rdparty/Eigen/Eigen/src/Core/GenericPacketMath.h

gtsam/3rdparty/Eigen/src/StlSupport/Makefile.am

gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseFuzzy.h

gtsam/3rdparty/Eigen/bench/btl/generic_bench/init/init_matrix.hh

gtsam/3rdparty/Eigen/COPYING.MINPACK

gtsam/3rdparty/Eigen/test/eigen2/eigen2_submatrices.cpp

gtsam/3rdparty/Eigen/doc/C10_TutorialMapClass.dox

gtsam/3rdparty/Eigen/doc/snippets/Cwise_square.cpp

gtsam/3rdparty/Eigen/Eigen/SparseCholesky

gtsam/3rdparty/Eigen/Eigen/src/plugins/MatrixCwiseBinaryOps.h

gtsam/3rdparty/Eigen/blas/testing/zblat1.f

gtsam/3rdparty/Eigen/.hgtags

gtsam/3rdparty/Eigen/test/eigen2/eigen2_triangular.cpp

gtsam/3rdparty/Eigen/doc/snippets/PartialRedux_maxCoeff.cpp

gtsam/3rdparty/Eigen/doc/StructHavingEigenMembers.dox

gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/Geometry/CMakeLists.txt

gtsam/3rdparty/Eigen/doc/I03_InsideEigenExample.dox

gtsam/3rdparty/Eigen/bench/btl/libs/STL_algo/main.cpp

gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_SupernodalMatrix.h

gtsam/3rdparty/Eigen/bench/btl/cmake/FindTvmet.cmake

gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h

gtsam/3rdparty/Eigen/bench/btl/generic_bench/btl.hh

gtsam/3rdparty/Eigen/unsupported/Eigen/CMakeLists.txt

gtsam/3rdparty/Eigen/bench/btl/libs/C/main.cpp

gtsam/3rdparty/Eigen/cmake/FindLAPACK.cmake

gtsam/3rdparty/Eigen/Eigen/src/Cholesky/CMakeLists.txt

gtsam/3rdparty/Eigen/unsupported/Eigen/src/SVD/SVDBase.h

gtsam/3rdparty/Eigen/src/Eigen2Support/LeastSquares.h

gtsam/3rdparty/Eigen/Eigen/src/StlSupport/StdList.h

gtsam/3rdparty/Eigen/unsupported/test/polynomialutils.cpp

gtsam/3rdparty/Eigen/demos/mix_eigen_and_c/example.c

gtsam/3rdparty/Eigen/doc/I11_Aliasing.dox

gtsam/3rdparty/Eigen/bench/quat_slerp.cpp

gtsam/3rdparty/Eigen/test/eigen2/eigen2_newstdvector.cpp

gtsam/3rdparty/Eigen/blas/sspmv.f

gtsam/3rdparty/Eigen/lapack/zlarft.f

gtsam/3rdparty/Eigen/bench/btl/data/gnuplot_common_settings.hh

gtsam/3rdparty/Eigen/bench/btl/libs/f77/main.cpp

gtsam/3rdparty/Eigen/bench/sparse_trisolver.cpp

gtsam/3rdparty/Eigen/src/Core/arch/CMakeLists.txt

gtsam/3rdparty/Eigen/blas/level2_real_impl.h

gtsam/3rdparty/Eigen/doc/snippets/Cwise_less.cpp

gtsam/3rdparty/Eigen/QtAlignedMalloc

gtsam/3rdparty/Eigen/doc/I14_PreprocessorDirectives.dox

gtsam/3rdparty/Eigen/blas/testing/zblat3.dat

gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/SelfAdjointEigenSolver_MKL.h

gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_cwiseMin.cpp

diff --git a/examples/.gitignore b/examples/.gitignore deleted file mode 100644 index a7cbee1ba..000000000 --- a/examples/.gitignore +++ /dev/null @@ -1 +0,0 @@ -/*.DS_Store diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index eae90e298..7251c2b6f 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -1,42 +1,8 @@ -if(NOT MSVC) - add_custom_target(examples) -endif() - -# Build example executables -FILE(GLOB example_srcs "*.cpp") - -set (excluded_examples #"") - "${CMAKE_CURRENT_SOURCE_DIR}/DiscreteBayesNet_FG.cpp" - "${CMAKE_CURRENT_SOURCE_DIR}/UGM_chain.cpp" - "${CMAKE_CURRENT_SOURCE_DIR}/UGM_small.cpp" - "${CMAKE_CURRENT_SOURCE_DIR}/elaboratePoint2KalmanFilter.cpp" +set (excluded_examples + DiscreteBayesNet_FG.cpp + UGM_chain.cpp + UGM_small.cpp + elaboratePoint2KalmanFilter.cpp ) -list(REMOVE_ITEM example_srcs ${excluded_examples}) - -foreach(example_src ${example_srcs} ) - get_filename_component(example_base ${example_src} NAME_WE) - set( example_bin ${example_base} ) - message(STATUS "Adding Example ${example_bin}") - if(NOT MSVC) - add_dependencies(examples ${example_bin}) - endif() - add_executable(${example_bin} ${example_src}) - - # Disable building during make all/install - if (GTSAM_DISABLE_EXAMPLES_ON_INSTALL) - set_target_properties(${example_bin} PROPERTIES EXCLUDE_FROM_ALL ON) - endif() - - target_link_libraries(${example_bin} ${gtsam-default} ${Boost_PROGRAM_OPTIONS_LIBRARY}) - if(NOT MSVC) - add_custom_target(${example_bin}.run ${EXECUTABLE_OUTPUT_PATH}${example_bin} ${ARGN}) - endif() - - # Set up Visual Studio folder - if(MSVC) - set_property(TARGET ${example_bin} PROPERTY FOLDER "Examples") - endif() - -endforeach(example_src) - +gtsamAddExamplesGlob("*.cpp" "${excluded_examples}" "gtsam;${Boost_PROGRAM_OPTIONS_LIBRARY}") diff --git a/examples/CameraResectioning.cpp b/examples/CameraResectioning.cpp index 37ea57022..2048a84c8 100644 --- a/examples/CameraResectioning.cpp +++ b/examples/CameraResectioning.cpp @@ -16,7 +16,7 @@ * @date Aug 23, 2011 */ -#include +#include #include #include #include diff --git a/examples/Data/.gitignore b/examples/Data/.gitignore deleted file mode 100644 index a7cbee1ba..000000000 --- a/examples/Data/.gitignore +++ /dev/null @@ -1 +0,0 @@ -/*.DS_Store diff --git a/examples/Data/dubrovnik-1-1-pre.txt b/examples/Data/dubrovnik-1-1-pre.txt new file mode 100644 index 000000000..612dc16d6 --- /dev/null +++ b/examples/Data/dubrovnik-1-1-pre.txt @@ -0,0 +1,17 @@ +1 1 1 + +0 0 -3.859900e+02 3.871200e+02 + +-1.6943983532198115e-02 +1.1171804676513932e-02 +2.4643508831711991e-03 +7.3030995682610689e-01 +-2.6490818471043420e-01 +-1.7127892627337182e+00 +1.4300319432711681e+03 +-7.5572758535864072e-08 +3.2377569465570913e-14 + +-1.2055995050700867e+01 +1.2838775976205760e+01 +-4.1099369264082803e+01 \ No newline at end of file diff --git a/examples/Data/dubrovnik-3-7-18-pre.txt b/examples/Data/dubrovnik-3-7-18-pre.txt new file mode 100644 index 000000000..cb9d6e6e0 --- /dev/null +++ b/examples/Data/dubrovnik-3-7-18-pre.txt @@ -0,0 +1,79 @@ +3 7 18 + +0 0 -3.859900e+02 3.871200e+02 +1 0 -3.844000e+01 4.921200e+02 +2 0 -6.679200e+02 1.231100e+02 +0 1 3.838800e+02 -1.529999e+01 +1 1 5.597500e+02 -1.061500e+02 +0 2 5.915500e+02 1.364400e+02 +1 2 8.638600e+02 -2.346997e+01 +2 2 4.947200e+02 1.125200e+02 +0 3 5.925000e+02 1.257500e+02 +1 3 8.610800e+02 -3.521997e+01 +2 3 4.985400e+02 1.015600e+02 +0 4 3.487200e+02 5.583800e+02 +1 4 7.760300e+02 4.835300e+02 +2 4 7.780029e+00 3.263500e+02 +0 5 1.401001e+01 9.642001e+01 +1 5 2.071300e+02 1.183600e+02 +1 6 5.431801e+02 2.948100e+02 +2 6 -5.841998e+01 1.108300e+02 + +-1.6943983532198115e-02 +1.1171804676513932e-02 +2.4643508831711991e-03 +7.3030995682610689e-01 +-2.6490818471043420e-01 +-1.7127892627337182e+00 +1.4300319432711681e+03 +-7.5572758535864072e-08 +3.2377569465570913e-14 + +1.5049725341485708e-02 +-1.8504564785154357e-01 +-2.9278402790141456e-01 +-1.0590476152349551e+00 +-3.6017862414345798e-02 +-1.5720340175803784e+00 +1.4321374541298685e+03 +-7.3171919892612292e-08 +3.1759419019880947e-14 + +-3.0793597986873011e-01 +3.2077907982952031e-01 +2.2253985096991455e-01 +8.5034483295909009e+00 +6.7499603629668741e+00 +-3.6383814384447088e+00 +1.5720470590375264e+03 +-1.5962623661947355e-08 +-1.6507904848058800e-14 + +-1.2055995050700867e+01 +1.2838775976205760e+01 +-4.1099369264082803e+01 + +6.4168905904672933e+00 +3.8897031177598462e-01 +-2.3586282709150449e+01 + +1.3051100355717297e+01 +3.8387587111611952e+00 +-2.9777932175344951e+01 + +1.3060946673472820e+01 +3.5910521225905803e+00 +-2.9759080795372942e+01 + +1.4265764475421857e+01 +2.4096216156436530e+01 +-5.4823971067225500e+01 + +-2.5292283211391348e-01 +2.2166082122808284e+00 +-2.1712127480255084e+01 + +7.6465738085189585e+00 +1.4185331909846619e+01 +-5.2070299568846060e+01 + diff --git a/examples/LocalizationExample.cpp b/examples/LocalizationExample.cpp index 3ce3e0760..a34a96c9b 100644 --- a/examples/LocalizationExample.cpp +++ b/examples/LocalizationExample.cpp @@ -27,7 +27,7 @@ #include // We will use simple integer Keys to refer to the robot poses. -#include +#include // As in OdometryExample.cpp, we use a BetweenFactor to model odometry measurements. #include @@ -120,15 +120,15 @@ int main(int argc, char** argv) { // For simplicity, we will use the same noise model for each odometry factor noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1)); // Create odometry (Between) factors between consecutive poses - graph.add(BetweenFactor(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise)); - graph.add(BetweenFactor(2, 3, Pose2(2.0, 0.0, 0.0), odometryNoise)); + graph.push_back(BetweenFactor(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise)); + graph.push_back(BetweenFactor(2, 3, Pose2(2.0, 0.0, 0.0), odometryNoise)); // 2b. Add "GPS-like" measurements // We will use our custom UnaryFactor for this. noiseModel::Diagonal::shared_ptr unaryNoise = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.1)); // 10cm std on x,y - graph.add(boost::make_shared(1, 0.0, 0.0, unaryNoise)); - graph.add(boost::make_shared(2, 2.0, 0.0, unaryNoise)); - graph.add(boost::make_shared(3, 4.0, 0.0, unaryNoise)); + graph.push_back(boost::make_shared(1, 0.0, 0.0, unaryNoise)); + graph.push_back(boost::make_shared(2, 2.0, 0.0, unaryNoise)); + graph.push_back(boost::make_shared(3, 4.0, 0.0, unaryNoise)); graph.print("\nFactor Graph:\n"); // print // 3. Create the data structure to hold the initialEstimate estimate to the solution diff --git a/examples/OdometryExample.cpp b/examples/OdometryExample.cpp index 70c6e6fb0..f2d4c6497 100644 --- a/examples/OdometryExample.cpp +++ b/examples/OdometryExample.cpp @@ -65,15 +65,15 @@ int main(int argc, char** argv) { // A prior factor consists of a mean and a noise model (covariance matrix) Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1)); - graph.add(PriorFactor(1, priorMean, priorNoise)); + graph.push_back(PriorFactor(1, priorMean, priorNoise)); // Add odometry factors Pose2 odometry(2.0, 0.0, 0.0); // For simplicity, we will use the same noise model for each odometry factor noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1)); // Create odometry (Between) factors between consecutive poses - graph.add(BetweenFactor(1, 2, odometry, odometryNoise)); - graph.add(BetweenFactor(2, 3, odometry, odometryNoise)); + graph.push_back(BetweenFactor(1, 2, odometry, odometryNoise)); + graph.push_back(BetweenFactor(2, 3, odometry, odometryNoise)); graph.print("\nFactor Graph:\n"); // print // Create the data structure to hold the initialEstimate estimate to the solution diff --git a/examples/PlanarSLAMExample.cpp b/examples/PlanarSLAMExample.cpp index ac8157be6..a1c75eab7 100644 --- a/examples/PlanarSLAMExample.cpp +++ b/examples/PlanarSLAMExample.cpp @@ -33,7 +33,7 @@ // Each variable in the system (poses and landmarks) must be identified with a unique key. // We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1). // Here we will use Symbols -#include +#include // In GTSAM, measurement functions are represented as 'factors'. Several common factors // have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems. @@ -81,13 +81,13 @@ int main(int argc, char** argv) { // Add a prior on pose x1 at the origin. A prior factor consists of a mean and a noise model (covariance matrix) Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta - graph.add(PriorFactor(x1, prior, priorNoise)); // add directly to graph + graph.push_back(PriorFactor(x1, prior, priorNoise)); // add directly to graph // Add two odometry factors Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case) noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta - graph.add(BetweenFactor(x1, x2, odometry, odometryNoise)); - graph.add(BetweenFactor(x2, x3, odometry, odometryNoise)); + graph.push_back(BetweenFactor(x1, x2, odometry, odometryNoise)); + graph.push_back(BetweenFactor(x2, x3, odometry, odometryNoise)); // Add Range-Bearing measurements to two different landmarks // create a noise model for the landmark measurements @@ -101,9 +101,9 @@ int main(int argc, char** argv) { range32 = 2.0; // Add Bearing-Range factors - graph.add(BearingRangeFactor(x1, l1, bearing11, range11, measurementNoise)); - graph.add(BearingRangeFactor(x2, l1, bearing21, range21, measurementNoise)); - graph.add(BearingRangeFactor(x3, l2, bearing32, range32, measurementNoise)); + graph.push_back(BearingRangeFactor(x1, l1, bearing11, range11, measurementNoise)); + graph.push_back(BearingRangeFactor(x2, l1, bearing21, range21, measurementNoise)); + graph.push_back(BearingRangeFactor(x3, l2, bearing32, range32, measurementNoise)); // Print graph.print("Factor Graph:\n"); diff --git a/examples/Pose2SLAMExample.cpp b/examples/Pose2SLAMExample.cpp index df924ff3e..e1a51a493 100644 --- a/examples/Pose2SLAMExample.cpp +++ b/examples/Pose2SLAMExample.cpp @@ -29,7 +29,7 @@ #include // We will use simple integer Keys to refer to the robot poses. -#include +#include // In GTSAM, measurement functions are represented as 'factors'. Several common factors // have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems. @@ -72,23 +72,23 @@ int main(int argc, char** argv) { // 2a. Add a prior on the first pose, setting it to the origin // A prior factor consists of a mean and a noise model (covariance matrix) noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1)); - graph.add(PriorFactor(1, Pose2(0, 0, 0), priorNoise)); + graph.push_back(PriorFactor(1, Pose2(0, 0, 0), priorNoise)); // For simplicity, we will use the same noise model for odometry and loop closures noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1)); // 2b. Add odometry factors // Create odometry (Between) factors between consecutive poses - graph.add(BetweenFactor(1, 2, Pose2(2, 0, 0 ), model)); - graph.add(BetweenFactor(2, 3, Pose2(2, 0, M_PI_2), model)); - graph.add(BetweenFactor(3, 4, Pose2(2, 0, M_PI_2), model)); - graph.add(BetweenFactor(4, 5, Pose2(2, 0, M_PI_2), model)); + graph.push_back(BetweenFactor(1, 2, Pose2(2, 0, 0 ), model)); + graph.push_back(BetweenFactor(2, 3, Pose2(2, 0, M_PI_2), model)); + graph.push_back(BetweenFactor(3, 4, Pose2(2, 0, M_PI_2), model)); + graph.push_back(BetweenFactor(4, 5, Pose2(2, 0, M_PI_2), model)); // 2c. Add the loop closure constraint // This factor encodes the fact that we have returned to the same pose. In real systems, // these constraints may be identified in many ways, such as appearance-based techniques // with camera images. We will use another Between Factor to enforce this constraint: - graph.add(BetweenFactor(5, 2, Pose2(2, 0, M_PI_2), model)); + graph.push_back(BetweenFactor(5, 2, Pose2(2, 0, M_PI_2), model)); graph.print("\nFactor Graph:\n"); // print // 3. Create the data structure to hold the initialEstimate estimate to the solution diff --git a/examples/Pose2SLAMExample_graph.cpp b/examples/Pose2SLAMExample_graph.cpp index d5e93a31b..449144fef 100644 --- a/examples/Pose2SLAMExample_graph.cpp +++ b/examples/Pose2SLAMExample_graph.cpp @@ -38,7 +38,7 @@ int main (int argc, char** argv) { // Add a Gaussian prior on first poses Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.01, 0.01, 0.01)); - graph->add(PriorFactor(0, priorMean, priorNoise)); + graph->push_back(PriorFactor(0, priorMean, priorNoise)); // Single Step Optimization using Levenberg-Marquardt Values result = LevenbergMarquardtOptimizer(*graph, *initial).optimize(); diff --git a/examples/Pose2SLAMExample_graphviz.cpp b/examples/Pose2SLAMExample_graphviz.cpp index 0b318e7b3..4b0dc6252 100644 --- a/examples/Pose2SLAMExample_graphviz.cpp +++ b/examples/Pose2SLAMExample_graphviz.cpp @@ -33,19 +33,19 @@ int main (int argc, char** argv) { // 2a. Add a prior on the first pose, setting it to the origin noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1)); - graph.add(PriorFactor(1, Pose2(0, 0, 0), priorNoise)); + graph.push_back(PriorFactor(1, Pose2(0, 0, 0), priorNoise)); // For simplicity, we will use the same noise model for odometry and loop closures noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1)); // 2b. Add odometry factors - graph.add(BetweenFactor(1, 2, Pose2(2, 0, 0 ), model)); - graph.add(BetweenFactor(2, 3, Pose2(2, 0, M_PI_2), model)); - graph.add(BetweenFactor(3, 4, Pose2(2, 0, M_PI_2), model)); - graph.add(BetweenFactor(4, 5, Pose2(2, 0, M_PI_2), model)); + graph.push_back(BetweenFactor(1, 2, Pose2(2, 0, 0 ), model)); + graph.push_back(BetweenFactor(2, 3, Pose2(2, 0, M_PI_2), model)); + graph.push_back(BetweenFactor(3, 4, Pose2(2, 0, M_PI_2), model)); + graph.push_back(BetweenFactor(4, 5, Pose2(2, 0, M_PI_2), model)); // 2c. Add the loop closure constraint - graph.add(BetweenFactor(5, 2, Pose2(2, 0, M_PI_2), model)); + graph.push_back(BetweenFactor(5, 2, Pose2(2, 0, M_PI_2), model)); // 3. Create the data structure to hold the initial estimate to the solution // For illustrative purposes, these have been deliberately set to incorrect values diff --git a/examples/Pose2SLAMwSPCG.cpp b/examples/Pose2SLAMwSPCG.cpp index 1c2952365..2b2f63064 100644 --- a/examples/Pose2SLAMwSPCG.cpp +++ b/examples/Pose2SLAMwSPCG.cpp @@ -33,7 +33,7 @@ // Each variable in the system (poses) must be identified with a unique key. // We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1). // Here we will use simple integer keys -#include +#include // In GTSAM, measurement functions are represented as 'factors'. Several common factors // have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems. @@ -69,16 +69,16 @@ int main(int argc, char** argv) { // A prior factor consists of a mean and a noise model (covariance matrix) Pose2 prior(0.0, 0.0, 0.0); // prior at origin noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1)); - graph.add(PriorFactor(1, prior, priorNoise)); + graph.push_back(PriorFactor(1, prior, priorNoise)); // 2b. Add odometry factors // For simplicity, we will use the same noise model for each odometry factor noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1)); // Create odometry (Between) factors between consecutive poses - graph.add(BetweenFactor(1, 2, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); - graph.add(BetweenFactor(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); - graph.add(BetweenFactor(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); - graph.add(BetweenFactor(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); + graph.push_back(BetweenFactor(1, 2, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); + graph.push_back(BetweenFactor(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); + graph.push_back(BetweenFactor(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); + graph.push_back(BetweenFactor(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); // 2c. Add the loop closure constraint // This factor encodes the fact that we have returned to the same pose. In real systems, @@ -86,7 +86,7 @@ int main(int argc, char** argv) { // with camera images. // We will use another Between Factor to enforce this constraint, with the distance set to zero, noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1)); - graph.add(BetweenFactor(5, 1, Pose2(0.0, 0.0, 0.0), model)); + graph.push_back(BetweenFactor(5, 1, Pose2(0.0, 0.0, 0.0), model)); graph.print("\nFactor Graph:\n"); // print @@ -104,7 +104,7 @@ int main(int argc, char** argv) { LevenbergMarquardtParams parameters; parameters.verbosity = NonlinearOptimizerParams::ERROR; parameters.verbosityLM = LevenbergMarquardtParams::LAMBDA; - parameters.linearSolverType = SuccessiveLinearizationParams::CONJUGATE_GRADIENT; + parameters.linearSolverType = NonlinearOptimizerParams::CONJUGATE_GRADIENT; { parameters.iterativeParams = boost::make_shared(); diff --git a/examples/RangeISAMExample_plaza2.cpp b/examples/RangeISAMExample_plaza2.cpp index 7c3c43272..29b02a271 100644 --- a/examples/RangeISAMExample_plaza2.cpp +++ b/examples/RangeISAMExample_plaza2.cpp @@ -22,7 +22,7 @@ // Each variable in the system (poses and landmarks) must be identified with a unique key. // We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1). // Here we will use Symbols -#include +#include // We want to use iSAM2 to solve the range-SLAM problem incrementally #include @@ -125,7 +125,7 @@ int main (int argc, char** argv) { Pose2 pose0 = Pose2(-34.2086489999201, 45.3007639991120, M_PI - 2.02108900000000); NonlinearFactorGraph newFactors; - newFactors.add(PriorFactor(0, pose0, priorNoise)); + newFactors.push_back(PriorFactor(0, pose0, priorNoise)); Values initial; initial.insert(0, pose0); @@ -158,7 +158,7 @@ int main (int argc, char** argv) { boost::tie(t, odometry) = timedOdometry; // add odometry factor - newFactors.add(BetweenFactor(i-1, i, odometry,NM::Diagonal::Sigmas(odoSigmas))); + newFactors.push_back(BetweenFactor(i-1, i, odometry,NM::Diagonal::Sigmas(odoSigmas))); // predict pose and add as initial estimate Pose2 predictedPose = lastPose.compose(odometry); @@ -169,7 +169,7 @@ int main (int argc, char** argv) { while (k < K && t >= boost::get<0>(triples[k])) { size_t j = boost::get<1>(triples[k]); double range = boost::get<2>(triples[k]); - newFactors.add(RangeFactor(i, symbol('L', j), range,rangeNoise)); + newFactors.push_back(RangeFactor(i, symbol('L', j), range,rangeNoise)); k = k + 1; countK = countK + 1; } diff --git a/examples/SFMExample.cpp b/examples/SFMExample.cpp index c810cfe49..9fa4bd026 100644 --- a/examples/SFMExample.cpp +++ b/examples/SFMExample.cpp @@ -30,7 +30,7 @@ // Each variable in the system (poses and landmarks) must be identified with a unique key. // We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1). // Here we will use Symbols -#include +#include // In GTSAM, measurement functions are represented as 'factors'. Several common factors // have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems. @@ -81,14 +81,14 @@ int main(int argc, char* argv[]) { // Add a prior on pose x1. This indirectly specifies where the origin is. noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1))); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw - graph.add(PriorFactor(Symbol('x', 0), poses[0], poseNoise)); // add directly to graph + graph.push_back(PriorFactor(Symbol('x', 0), poses[0], poseNoise)); // add directly to graph // Simulated measurements from each camera pose, adding them to the factor graph for (size_t i = 0; i < poses.size(); ++i) { for (size_t j = 0; j < points.size(); ++j) { SimpleCamera camera(poses[i], *K); Point2 measurement = camera.project(points[j]); - graph.add(GenericProjectionFactor(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K)); + graph.push_back(GenericProjectionFactor(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K)); } } @@ -96,7 +96,7 @@ int main(int argc, char* argv[]) { // Here we add a prior on the position of the first landmark. This fixes the scale by indicating the distance // between the first camera and the first landmark. All other landmark positions are interpreted using this scale. noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1); - graph.add(PriorFactor(Symbol('l', 0), points[0], pointNoise)); // add directly to graph + graph.push_back(PriorFactor(Symbol('l', 0), points[0], pointNoise)); // add directly to graph graph.print("Factor Graph:\n"); // Create the data structure to hold the initial estimate to the solution diff --git a/examples/SFMExample_bal.cpp b/examples/SFMExample_bal.cpp index 672dbc86b..9d453fe5f 100644 --- a/examples/SFMExample_bal.cpp +++ b/examples/SFMExample_bal.cpp @@ -16,7 +16,7 @@ */ // For an explanation of headers, see SFMExample.cpp -#include +#include #include #include #include @@ -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.add(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.add(PriorFactor(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1))); - graph.add(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; diff --git a/examples/SelfCalibrationExample.cpp b/examples/SelfCalibrationExample.cpp index 01a2300e3..c69afe3b7 100644 --- a/examples/SelfCalibrationExample.cpp +++ b/examples/SelfCalibrationExample.cpp @@ -27,7 +27,7 @@ #include // Inference and optimization -#include +#include #include #include #include @@ -54,7 +54,7 @@ int main(int argc, char* argv[]) { // Add a prior on pose x1. noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1))); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw - graph.add(PriorFactor(Symbol('x', 0), poses[0], poseNoise)); + graph.push_back(PriorFactor(Symbol('x', 0), poses[0], poseNoise)); // Simulated measurements from each camera pose, adding them to the factor graph Cal3_S2 K(50.0, 50.0, 0.0, 50.0, 50.0); @@ -65,17 +65,17 @@ int main(int argc, char* argv[]) { Point2 measurement = camera.project(points[j]); // The only real difference with the Visual SLAM example is that here we use a // different factor type, that also calculates the Jacobian with respect to calibration - graph.add(GeneralSFMFactor2(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), Symbol('K', 0))); + graph.push_back(GeneralSFMFactor2(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), Symbol('K', 0))); } } // Add a prior on the position of the first landmark. noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1); - graph.add(PriorFactor(Symbol('l', 0), points[0], pointNoise)); // add directly to graph + graph.push_back(PriorFactor(Symbol('l', 0), points[0], pointNoise)); // add directly to graph // Add a prior on the calibration. noiseModel::Diagonal::shared_ptr calNoise = noiseModel::Diagonal::Sigmas((Vector(5) << 500, 500, 0.1, 100, 100)); - graph.add(PriorFactor(Symbol('K', 0), K, calNoise)); + graph.push_back(PriorFactor(Symbol('K', 0), K, calNoise)); // Create the initial estimate to the solution // now including an estimate on the camera calibration parameters diff --git a/examples/SimpleRotation.cpp b/examples/SimpleRotation.cpp index 54c64e820..1f40d8c60 100644 --- a/examples/SimpleRotation.cpp +++ b/examples/SimpleRotation.cpp @@ -28,7 +28,7 @@ // Each variable in the system (poses) must be identified with a unique key. // We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1). // Here we will use symbols -#include +#include // In GTSAM, measurement functions are represented as 'factors'. Several common factors // have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems. @@ -90,7 +90,7 @@ int main() { * many more factors would be added. */ NonlinearFactorGraph graph; - graph.add(factor); + graph.push_back(factor); graph.print("full graph"); /** diff --git a/examples/SolverComparer.cpp b/examples/SolverComparer.cpp index 5d472a804..2f0c71a40 100644 --- a/examples/SolverComparer.cpp +++ b/examples/SolverComparer.cpp @@ -9,23 +9,28 @@ * -------------------------------------------------------------------------- */ /** -* @file timeIncremental.cpp +* @file SolverComparer.cpp * @brief Incremental and batch solving, timing, and accuracy comparisons * @author Richard Roberts +* @date August, 2013 */ #include +#include #include #include #include #include #include -#include +#include +#include +#include #include #include #include #include +#include #include #include #include @@ -33,6 +38,12 @@ #include #include +#ifdef GTSAM_USE_TBB +#include +#undef max // TBB seems to include windows.h and we don't want these macros +#undef min +#endif + using namespace std; using namespace gtsam; using namespace gtsam::symbol_shorthand; @@ -81,11 +92,14 @@ string inputFile; string datasetName; int firstStep; int lastStep; +int nThreads; int relinSkip; bool incremental; +bool dogleg; bool batch; bool compare; bool perturb; +bool stats; double perturbationNoise; string compareFile1, compareFile2; @@ -97,6 +111,7 @@ void runIncremental(); void runBatch(); void runCompare(); void runPerturb(); +void runStats(); /* ************************************************************************* */ int main(int argc, char *argv[]) { @@ -109,11 +124,14 @@ int main(int argc, char *argv[]) { ("dataset,d", po::value(&datasetName)->default_value(""), "Read a dataset file (if and only if --incremental is used)") ("first-step,f", po::value(&firstStep)->default_value(0), "First step to process from the dataset file") ("last-step,l", po::value(&lastStep)->default_value(-1), "Last step to process, or -1 to process until the end of the dataset") + ("threads", po::value(&nThreads)->default_value(-1), "Number of threads, or -1 to use all processors") ("relinSkip", po::value(&relinSkip)->default_value(10), "Fluid relinearization check every arg steps") ("incremental", "Run in incremental mode using ISAM2 (default)") + ("dogleg", "When in incremental mode, solve with Dogleg instead of Gauss-Newton in iSAM2") ("batch", "Run in batch mode, requires an initialization from --read-solution") ("compare", po::value >()->multitoken(), "Compare two solution files") ("perturb", po::value(&perturbationNoise), "Perturb a solution file with the specified noise") + ("stats", "Gather factorization statistics about the dataset, writes text-file histograms") ; po::variables_map vm; po::store(po::command_line_parser(argc, argv).options(desc).run(), vm); @@ -122,7 +140,10 @@ int main(int argc, char *argv[]) { batch = (vm.count("batch") > 0); compare = (vm.count("compare") > 0); perturb = (vm.count("perturb") > 0); - incremental = (vm.count("incremental") > 0 || (!batch && !compare && !perturb)); + stats = (vm.count("stats") > 0); + const int modesSpecified = int(batch) + int(compare) + int(perturb) + int(stats); + incremental = (vm.count("incremental") > 0 || modesSpecified == 0); + dogleg = (vm.count("dogleg") > 0); if(compare) { const vector& compareFiles = vm["compare"].as >(); if(compareFiles.size() != 2) { @@ -133,15 +154,15 @@ int main(int argc, char *argv[]) { compareFile2 = compareFiles[1]; } - if((batch && incremental) || (batch && compare) || (incremental && compare)) { - cout << "Only one of --incremental, --batch, and --compare may be specified\n" << desc << endl; + if(modesSpecified > 1) { + cout << "Only one of --incremental, --batch, --compare, --perturb, and --stats may be specified\n" << desc << endl; exit(1); } if((incremental || batch) && datasetName.empty()) { cout << "In incremental and batch modes, a dataset must be specified\n" << desc << endl; exit(1); } - if(!(incremental || batch) && !datasetName.empty()) { + if(!(incremental || batch || stats) && !datasetName.empty()) { cout << "A dataset may only be specified in incremental or batch modes\n" << desc << endl; exit(1); } @@ -153,6 +174,10 @@ int main(int argc, char *argv[]) { cout << "In perturb mode, specify input and output files\n" << desc << endl; exit(1); } + if(stats && (datasetName.empty() || inputFile.empty())) { + cout << "In stats mode, specify dataset and input file\n" << desc << endl; + exit(1); + } // Read input file if(!inputFile.empty()) @@ -178,6 +203,19 @@ int main(int argc, char *argv[]) { } } +#ifdef GTSAM_USE_TBB + std::auto_ptr init; + if(nThreads > 0) { + cout << "Using " << nThreads << " threads" << endl; + init.reset(new tbb::task_scheduler_init(nThreads)); + } else + cout << "Using threads for all processors" << endl; +#else + if(nThreads > 0) { + std::cout << "GTSAM is not compiled with TBB, so threading is disabled and the --threads option cannot be used." << endl; + exit(1); + } +#endif // Run mode if(incremental) @@ -188,6 +226,8 @@ int main(int argc, char *argv[]) { runCompare(); else if(perturb) runPerturb(); + else if(stats) + runStats(); return 0; } @@ -196,6 +236,8 @@ int main(int argc, char *argv[]) { void runIncremental() { ISAM2Params params; + if(dogleg) + params.optimizationParams = ISAM2DoglegParams(); params.relinearizeSkip = relinSkip; params.enablePartialRelinearizationCheck = true; ISAM2 isam2(params); @@ -420,7 +462,7 @@ void runBatch() gttic_(Create_optimizer); GaussNewtonParams params; - params.linearSolverType = SuccessiveLinearizationParams::MULTIFRONTAL_CHOLESKY; + params.linearSolverType = NonlinearOptimizerParams::MULTIFRONTAL_CHOLESKY; GaussNewtonOptimizer optimizer(measurements, initial, params); gttoc_(Create_optimizer); double lastError; @@ -504,15 +546,14 @@ void runPerturb() // Perturb values VectorValues noise; - Ordering ordering = *initial.orderingArbitrary(); BOOST_FOREACH(const Values::KeyValuePair& key_val, initial) { Vector noisev(key_val.value.dim()); for(Vector::Index i = 0; i < noisev.size(); ++i) noisev(i) = normal(rng); - noise.insert(ordering[key_val.key], noisev); + noise.insert(key_val.key, noisev); } - Values perturbed = initial.retract(noise, ordering); + Values perturbed = initial.retract(noise); // Write results try { @@ -527,3 +568,28 @@ void runPerturb() } +/* ************************************************************************* */ +void runStats() +{ + cout << "Gathering statistics..." << endl; + GaussianFactorGraph linear = *datasetMeasurements.linearize(initial); + GaussianJunctionTree jt(GaussianEliminationTree(linear, Ordering::COLAMD(linear))); + treeTraversal::ForestStatistics statistics = treeTraversal::GatherStatistics(jt); + + ofstream file; + + cout << "Writing SolverComparer_Stats_problemSizeHistogram.txt..." << endl; + file.open("SolverComparer_Stats_problemSizeHistogram.txt"); + treeTraversal::ForestStatistics::Write(file, statistics.problemSizeHistogram); + file.close(); + + cout << "Writing SolverComparer_Stats_numberOfChildrenHistogram.txt..." << endl; + file.open("SolverComparer_Stats_numberOfChildrenHistogram.txt"); + treeTraversal::ForestStatistics::Write(file, statistics.numberOfChildrenHistogram); + file.close(); + + cout << "Writing SolverComparer_Stats_problemSizeOfSecondLargestChildHistogram.txt..." << endl; + file.open("SolverComparer_Stats_problemSizeOfSecondLargestChildHistogram.txt"); + treeTraversal::ForestStatistics::Write(file, statistics.problemSizeOfSecondLargestChildHistogram); + file.close(); +} diff --git a/examples/TimeTBB.cpp b/examples/TimeTBB.cpp new file mode 100644 index 000000000..077331848 --- /dev/null +++ b/examples/TimeTBB.cpp @@ -0,0 +1,195 @@ +/* ---------------------------------------------------------------------------- + +* 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 TimeTBB.cpp +* @brief Measure task scheduling overhead in TBB +* @author Richard Roberts +* @date November 6, 2013 +*/ + +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; +using boost::assign::list_of; + +#ifdef GTSAM_USE_TBB + +#include +#undef max // TBB seems to include windows.h and we don't want these macros +#undef min + +static const DenseIndex numberOfProblems = 1000000; +static const DenseIndex problemSize = 4; + +typedef Eigen::Matrix FixedMatrix; + +/* ************************************************************************* */ +struct ResultWithThreads +{ + typedef map::value_type value_type; + map grainSizesWithoutAllocation; + map grainSizesWithAllocation; +}; + +typedef map Results; + +/* ************************************************************************* */ +struct WorkerWithoutAllocation +{ + vector& results; + + WorkerWithoutAllocation(vector& results) : results(results) {} + + void operator()(const tbb::blocked_range& r) const + { + for(size_t i = r.begin(); i != r.end(); ++i) + { + FixedMatrix m1 = FixedMatrix::Random(); + FixedMatrix m2 = FixedMatrix::Random(); + FixedMatrix prod = m1 * m2; + results[i] = prod.norm(); + } + } +}; + +/* ************************************************************************* */ +map testWithoutMemoryAllocation() +{ + // A function to do some matrix operations without allocating any memory + + // Now call it + vector results(numberOfProblems); + + const vector grainSizes = list_of(1)(10)(100)(1000); + map timingResults; + BOOST_FOREACH(size_t grainSize, grainSizes) + { + tbb::tick_count t0 = tbb::tick_count::now(); + tbb::parallel_for(tbb::blocked_range(0, numberOfProblems), WorkerWithoutAllocation(results)); + tbb::tick_count t1 = tbb::tick_count::now(); + cout << "Without memory allocation, grain size = " << grainSize << ", time = " << (t1 - t0).seconds() << endl; + timingResults[(int)grainSize] = (t1 - t0).seconds(); + } + + return timingResults; +} + +/* ************************************************************************* */ +struct WorkerWithAllocation +{ + vector& results; + + WorkerWithAllocation(vector& results) : results(results) {} + + void operator()(const tbb::blocked_range& r) const + { + tbb::cache_aligned_allocator allocator; + for(size_t i = r.begin(); i != r.end(); ++i) + { + double *m1data = allocator.allocate(problemSize * problemSize); + Eigen::Map m1(m1data, problemSize, problemSize); + double *m2data = allocator.allocate(problemSize * problemSize); + Eigen::Map m2(m2data, problemSize, problemSize); + double *proddata = allocator.allocate(problemSize * problemSize); + Eigen::Map prod(proddata, problemSize, problemSize); + + m1 = Eigen::Matrix4d::Random(problemSize, problemSize); + m2 = Eigen::Matrix4d::Random(problemSize, problemSize); + prod = m1 * m2; + results[i] = prod.norm(); + + allocator.deallocate(m1data, problemSize * problemSize); + allocator.deallocate(m2data, problemSize * problemSize); + allocator.deallocate(proddata, problemSize * problemSize); + } + } +}; + +/* ************************************************************************* */ +map testWithMemoryAllocation() +{ + // A function to do some matrix operations with allocating memory + + // Now call it + vector results(numberOfProblems); + + const vector grainSizes = list_of(1)(10)(100)(1000); + map timingResults; + BOOST_FOREACH(size_t grainSize, grainSizes) + { + tbb::tick_count t0 = tbb::tick_count::now(); + tbb::parallel_for(tbb::blocked_range(0, numberOfProblems), WorkerWithAllocation(results)); + tbb::tick_count t1 = tbb::tick_count::now(); + cout << "With memory allocation, grain size = " << grainSize << ", time = " << (t1 - t0).seconds() << endl; + timingResults[grainSize] = (t1 - t0).seconds(); + } + + return timingResults; +} + +/* ************************************************************************* */ +int main(int argc, char* argv[]) +{ + cout << "numberOfProblems = " << numberOfProblems << endl; + cout << "problemSize = " << problemSize << endl; + + const vector numThreads = list_of(1)(4)(8); + Results results; + + BOOST_FOREACH(size_t n, numThreads) + { + cout << "With " << n << " threads:" << endl; + tbb::task_scheduler_init init(n); + results[n].grainSizesWithoutAllocation = testWithoutMemoryAllocation(); + results[n].grainSizesWithAllocation = testWithMemoryAllocation(); + cout << endl; + } + + cout << "Summary of results:" << endl; + BOOST_FOREACH(const Results::value_type& threads_result, results) + { + const int threads = threads_result.first; + const ResultWithThreads& result = threads_result.second; + if(threads != 1) + { + BOOST_FOREACH(const ResultWithThreads::value_type& grainsize_time, result.grainSizesWithoutAllocation) + { + const int grainsize = grainsize_time.first; + const double speedup = results[1].grainSizesWithoutAllocation[grainsize] / grainsize_time.second; + cout << threads << " threads, without allocation, grain size = " << grainsize << ", speedup = " << speedup << endl; + } + BOOST_FOREACH(const ResultWithThreads::value_type& grainsize_time, result.grainSizesWithAllocation) + { + const int grainsize = grainsize_time.first; + const double speedup = results[1].grainSizesWithAllocation[grainsize] / grainsize_time.second; + cout << threads << " threads, with allocation, grain size = " << grainsize << ", speedup = " << speedup << endl; + } + } + } + + return 0; +} + +#else + +/* ************************************************************************* */ +int main(int argc, char* argv []) +{ + cout << "GTSAM is compiled without TBB, please compile with TBB to use this program." << endl; + return 0; +} + +#endif diff --git a/examples/VisualISAM2Example.cpp b/examples/VisualISAM2Example.cpp index cb8dab2fb..de4de455a 100644 --- a/examples/VisualISAM2Example.cpp +++ b/examples/VisualISAM2Example.cpp @@ -31,7 +31,7 @@ // Each variable in the system (poses and landmarks) must be identified with a unique key. // We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1). // Here we will use Symbols -#include +#include // We want to use iSAM2 to solve the structure-from-motion problem incrementally, so // include iSAM2 here @@ -90,7 +90,7 @@ int main(int argc, char* argv[]) { for (size_t j = 0; j < points.size(); ++j) { SimpleCamera camera(poses[i], *K); Point2 measurement = camera.project(points[j]); - graph.add(GenericProjectionFactor(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K)); + graph.push_back(GenericProjectionFactor(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K)); } // Add an initial guess for the current pose @@ -104,11 +104,11 @@ int main(int argc, char* argv[]) { if( i == 0) { // Add a prior on pose x0 noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3),Vector3::Constant(0.1))); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw - graph.add(PriorFactor(Symbol('x', 0), poses[0], poseNoise)); + graph.push_back(PriorFactor(Symbol('x', 0), poses[0], poseNoise)); // Add a prior on landmark l0 noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1); - graph.add(PriorFactor(Symbol('l', 0), points[0], pointNoise)); // add directly to graph + graph.push_back(PriorFactor(Symbol('l', 0), points[0], pointNoise)); // add directly to graph // Add initial guesses to all observed landmarks // Intentionally initialize the variables off from the ground truth diff --git a/examples/VisualISAMExample.cpp b/examples/VisualISAMExample.cpp index 6c4fe1d7e..b5645e214 100644 --- a/examples/VisualISAMExample.cpp +++ b/examples/VisualISAMExample.cpp @@ -31,7 +31,7 @@ // Each variable in the system (poses and landmarks) must be identified with a unique key. // We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1). // Here we will use Symbols -#include +#include // In GTSAM, measurement functions are represented as 'factors'. Several common factors // have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems. @@ -84,7 +84,7 @@ int main(int argc, char* argv[]) { for (size_t j = 0; j < points.size(); ++j) { SimpleCamera camera(poses[i], *K); Point2 measurement = camera.project(points[j]); - graph.add(GenericProjectionFactor(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K)); + graph.push_back(GenericProjectionFactor(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K)); } // Add an initial guess for the current pose @@ -98,11 +98,11 @@ int main(int argc, char* argv[]) { if( i == 0) { // Add a prior on pose x0 noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1))); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw - graph.add(PriorFactor(Symbol('x', 0), poses[0], poseNoise)); + graph.push_back(PriorFactor(Symbol('x', 0), poses[0], poseNoise)); // Add a prior on landmark l0 noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1); - graph.add(PriorFactor(Symbol('l', 0), points[0], pointNoise)); // add directly to graph + graph.push_back(PriorFactor(Symbol('l', 0), points[0], pointNoise)); // add directly to graph // Add initial guesses to all observed landmarks // Intentionally initialize the variables off from the ground truth diff --git a/examples/VisualSLAMExample.cpp b/examples/VisualSLAMExample.cpp deleted file mode 100644 index 8aed051eb..000000000 --- a/examples/VisualSLAMExample.cpp +++ /dev/null @@ -1,139 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 VisualSLAMExample.cpp - * @brief A visualSLAM example for the structure-from-motion problem on a simulated dataset - * @author Duy-Nguyen Ta - */ - -/** - * A structure-from-motion example with landmarks - * - The landmarks form a 10 meter cube - * - The robot rotates around the landmarks, always facing towards the cube - */ - -// As this is a full 3D problem, we will use Pose3 variables to represent the camera -// positions and Point3 variables (x, y, z) to represent the landmark coordinates. -// Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y). -// We will also need a camera object to hold calibration information and perform projections. -#include -#include -#include -#include - -// Each variable in the system (poses and landmarks) must be identified with a unique key. -// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1). -// Here we will use Symbols -#include - -// In GTSAM, measurement functions are represented as 'factors'. Several common factors -// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems. -// Here we will use Projection factors to model the camera's landmark observations. -// Also, we will initialize the robot at some location using a Prior factor. -#include -#include - -// When the factors are created, we will add them to a Factor Graph. As the factors we are using -// are nonlinear factors, we will need a Nonlinear Factor Graph. -#include - -// Finally, once all of the factors have been added to our factor graph, we will want to -// solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values. -// GTSAM includes several nonlinear optimizers to perform this step. Here we will use a -// trust-region method known as Powell's Degleg -#include - -// The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the -// nonlinear functions around an initial linearization point, then solve the linear system -// to update the linearization point. This happens repeatedly until the solver converges -// to a consistent set of variable values. This requires us to specify an initial guess -// for each variable, held in a Values container. -#include - -#include - -using namespace std; -using namespace gtsam; - -/* ************************************************************************* */ -int main(int argc, char* argv[]) { - - // Define the camera calibration parameters - Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)); - - // Define the camera observation noise model - noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v - - // Create the set of ground-truth landmarks - std::vector points; - points.push_back(gtsam::Point3(10.0,10.0,10.0)); - points.push_back(gtsam::Point3(-10.0,10.0,10.0)); - points.push_back(gtsam::Point3(-10.0,-10.0,10.0)); - points.push_back(gtsam::Point3(10.0,-10.0,10.0)); - points.push_back(gtsam::Point3(10.0,10.0,-10.0)); - points.push_back(gtsam::Point3(-10.0,10.0,-10.0)); - points.push_back(gtsam::Point3(-10.0,-10.0,-10.0)); - points.push_back(gtsam::Point3(10.0,-10.0,-10.0)); - - // Create the set of ground-truth poses - std::vector poses; - double radius = 30.0; - int i = 0; - double theta = 0.0; - gtsam::Point3 up(0,0,1); - gtsam::Point3 target(0,0,0); - for(; i < 8; ++i, theta += 2*M_PI/8) { - gtsam::Point3 position = Point3(radius*cos(theta), radius*sin(theta), 0.0); - gtsam::SimpleCamera camera = SimpleCamera::Lookat(position, target, up); - poses.push_back(camera.pose()); - } - - // Create a factor graph - NonlinearFactorGraph graph; - - // Add a prior on pose x1. This indirectly specifies where the origin is. - noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas(Vector_(6, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1)); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw - graph.add(PriorFactor(Symbol('x', 0), poses[0], poseNoise)); // add directly to graph - - // Simulated measurements from each camera pose, adding them to the factor graph - for (size_t i = 0; i < poses.size(); ++i) { - for (size_t j = 0; j < points.size(); ++j) { - SimpleCamera camera(poses[i], *K); - Point2 measurement = camera.project(points[j]); - graph.add(GenericProjectionFactor(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K)); - } - } - - // Because the structure-from-motion problem has a scale ambiguity, the problem is still under-constrained - // Here we add a prior on the position of the first landmark. This fixes the scale by indicating the distance - // between the first camera and the first landmark. All other landmark positions are interpreted using this scale. - noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1); - graph.add(PriorFactor(Symbol('l', 0), points[0], pointNoise)); // add directly to graph - graph.print("Factor Graph:\n"); - - // Create the data structure to hold the initialEstimate estimate to the solution - // Intentionally initialize the variables off from the ground truth - Values initialEstimate; - for (size_t i = 0; i < poses.size(); ++i) - initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)))); - for (size_t j = 0; j < points.size(); ++j) - initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15))); - initialEstimate.print("Initial Estimates:\n"); - - /* Optimize the graph and print results */ - Values result = DoglegOptimizer(graph, initialEstimate).optimize(); - result.print("Final results:\n"); - - return 0; -} -/* ************************************************************************* */ - diff --git a/examples/easyPoint2KalmanFilter.cpp b/examples/easyPoint2KalmanFilter.cpp index 6d75e9149..bb25440b1 100644 --- a/examples/easyPoint2KalmanFilter.cpp +++ b/examples/easyPoint2KalmanFilter.cpp @@ -22,7 +22,7 @@ */ #include -#include +#include #include #include #include @@ -37,12 +37,13 @@ int main() { // Create the Kalman Filter initialization point Point2 x_initial(0.0, 0.0); - SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); + SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.1)); + + // Create Key for initial pose + Symbol x0('x',0); // Create an ExtendedKalmanFilter object - ExtendedKalmanFilter ekf(x_initial, P_initial); - - + ExtendedKalmanFilter ekf(x0, x_initial, P_initial); // Now predict the state at t=1, i.e. argmax_{x1} P(x1) = P(x1|x0) P(x0) // In Kalman Filter notation, this is x_{t+1|t} and P_{t+1|t} @@ -56,12 +57,12 @@ int main() { // For the purposes of this example, let us assume we are using a constant-position model and // the controls are driving the point to the right at 1 m/s. Then, F = [1 0 ; 0 1], B = [1 0 ; 0 1] // and u = [1 ; 0]. Let us also assume that the process noise Q = [0.1 0 ; 0 0.1]. - Vector u = Vector_(2, 1.0, 0.0); - SharedDiagonal Q = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1), true); + Vector u = (Vector(2) << 1.0, 0.0); + SharedDiagonal Q = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.1), true); // This simple motion can be modeled with a BetweenFactor - // Create Keys - Symbol x0('x',0), x1('x',1); + // Create Key for next pose + Symbol x1('x',1); // Predict delta based on controls Point2 difference(1,0); // Create Factor @@ -82,7 +83,7 @@ int main() { // For the purposes of this example, let us assume we have something like a GPS that returns // the current position of the robot. Then H = [1 0 ; 0 1]. Let us also assume that the measurement noise // R = [0.25 0 ; 0 0.25]. - SharedDiagonal R = noiseModel::Diagonal::Sigmas(Vector_(2, 0.25, 0.25), true); + SharedDiagonal R = noiseModel::Diagonal::Sigmas((Vector(2) << 0.25, 0.25), true); // This simple measurement can be modeled with a PriorFactor Point2 z1(1.0, 0.0); diff --git a/examples/elaboratePoint2KalmanFilter.cpp b/examples/elaboratePoint2KalmanFilter.cpp index 3c18b220a..a0300adf9 100644 --- a/examples/elaboratePoint2KalmanFilter.cpp +++ b/examples/elaboratePoint2KalmanFilter.cpp @@ -22,9 +22,8 @@ #include #include -#include -#include -#include +//#include +#include #include #include #include @@ -59,7 +58,7 @@ int main() { // Initialize state x0 (2D point) at origin by adding a prior factor, i.e., Bayes net P(x0) // This is equivalent to x_0 and P_0 Point2 x_initial(0,0); - SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); + SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas((Vec(2) << 0.1, 0.1)); PriorFactor factor1(x0, x_initial, P_initial); // Linearize the factor and add it to the linear factor graph linearizationPoints.insert(x0, x_initial); @@ -90,7 +89,7 @@ int main() { ordering->insert(x1, 1); Point2 difference(1,0); - SharedDiagonal Q = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); + SharedDiagonal Q = noiseModel::Diagonal::Sigmas((Vec(2) << 0.1, 0.1)); BetweenFactor factor2(x0, x1, difference, Q); // Linearize the factor and add it to the linear factor graph linearizationPoints.insert(x1, x_initial); @@ -168,7 +167,7 @@ int main() { // = (x_{t} - z_{t}) * R^-1 * (x_{t} - z_{t})^T // This can be modeled using the PriorFactor, where the mean is z_{t} and the covariance is R. Point2 z1(1.0, 0.0); - SharedDiagonal R1 = noiseModel::Diagonal::Sigmas(Vector_(2, 0.25, 0.25)); + SharedDiagonal R1 = noiseModel::Diagonal::Sigmas((Vec(2) << 0.25, 0.25)); PriorFactor factor4(x1, z1, R1); // Linearize the factor and add it to the linear factor graph linearFactorGraph->push_back(factor4.linearize(linearizationPoints, *ordering)); @@ -220,7 +219,7 @@ int main() { // Create a nonlinear factor describing the motion model difference = Point2(1,0); - Q = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); + Q = noiseModel::Diagonal::Sigmas((Vec(2) <, 0.1, 0.1)); BetweenFactor factor6(x1, x2, difference, Q); // Linearize the factor and add it to the linear factor graph @@ -258,7 +257,7 @@ int main() { // And update using z2 ... Point2 z2(2.0, 0.0); - SharedDiagonal R2 = noiseModel::Diagonal::Sigmas(Vector_(2, 0.25, 0.25)); + SharedDiagonal R2 = noiseModel::Diagonal::Sigmas((Vec(2) << 0.25, 0.25)); PriorFactor factor8(x2, z2, R2); // Linearize the factor and add it to the linear factor graph @@ -309,7 +308,7 @@ int main() { // Create a nonlinear factor describing the motion model difference = Point2(1,0); - Q = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); + Q = noiseModel::Diagonal::Sigmas((Vec(2) << 0.1, 0.1)); BetweenFactor factor10(x2, x3, difference, Q); // Linearize the factor and add it to the linear factor graph @@ -347,7 +346,7 @@ int main() { // And update using z3 ... Point2 z3(3.0, 0.0); - SharedDiagonal R3 = noiseModel::Diagonal::Sigmas(Vector_(2, 0.25, 0.25)); + SharedDiagonal R3 = noiseModel::Diagonal::Sigmas((Vec(2) << 0.25, 0.25)); PriorFactor factor12(x3, z3, R3); // Linearize the factor and add it to the linear factor graph diff --git a/gtsam.h b/gtsam.h index d6da9b164..2d181d350 100644 --- a/gtsam.h +++ b/gtsam.h @@ -838,203 +838,48 @@ virtual class StereoCamera : gtsam::Value { }; //************************************************************************* -// inference +// Symbolic //************************************************************************* -#include -class Permutation { + +#include +virtual class SymbolicFactor { // Standard Constructors and Named Constructors - Permutation(); - Permutation(size_t nVars); - static gtsam::Permutation Identity(size_t nVars); - - // Testable - void print(string s) const; - bool equals(const gtsam::Permutation& rhs, double tol) const; - - // Standard interface - size_t at(size_t variable) const; - size_t size() const; - bool empty() const; - void resize(size_t newSize); - gtsam::Permutation* permute(const gtsam::Permutation& permutation) const; - gtsam::Permutation* inverse() const; - // FIXME: Cannot currently wrap std::vector - //static gtsam::Permutation PullToFront(const vector& toFront, size_t size, bool filterDuplicates); - //static gtsam::Permutation PushToBack(const vector& toBack, size_t size, bool filterDuplicates = false); -}; - -class IndexFactor { - // Standard Constructors and Named Constructors - IndexFactor(const gtsam::IndexFactor& f); - IndexFactor(const gtsam::IndexConditional& c); - IndexFactor(); - IndexFactor(size_t j); - IndexFactor(size_t j1, size_t j2); - IndexFactor(size_t j1, size_t j2, size_t j3); - IndexFactor(size_t j1, size_t j2, size_t j3, size_t j4); - IndexFactor(size_t j1, size_t j2, size_t j3, size_t j4, size_t j5); - IndexFactor(size_t j1, size_t j2, size_t j3, size_t j4, size_t j5, size_t j6); - // FIXME: Must wrap std::set for this to work - //IndexFactor(const std::set& js); - void permuteWithInverse(const gtsam::Permutation& inversePermutation); + SymbolicFactor(const gtsam::SymbolicFactor& f); + SymbolicFactor(); + SymbolicFactor(size_t j); + SymbolicFactor(size_t j1, size_t j2); + SymbolicFactor(size_t j1, size_t j2, size_t j3); + SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4); + SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4, size_t j5); + SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4, size_t j5, size_t j6); + static gtsam::SymbolicFactor FromKeys(const gtsam::KeyVector& js); // From Factor size_t size() const; void print(string s) const; - bool equals(const gtsam::IndexFactor& other, double tol) const; - // FIXME: Need to wrap std::vector - //std::vector& keys(); -}; - -class IndexConditional { - // Standard Constructors and Named Constructors - IndexConditional(); - IndexConditional(size_t key); - IndexConditional(size_t key, size_t parent); - IndexConditional(size_t key, size_t parent1, size_t parent2); - IndexConditional(size_t key, size_t parent1, size_t parent2, size_t parent3); - // FIXME: Must wrap std::vector for this to work - //IndexFactor(size_t key, const std::vector& parents); - //IndexConditional(const std::vector& keys, size_t nrFrontals); - //template static shared_ptr FromKeys(const KEYS& keys, size_t nrFrontals); - - // Testable - void print(string s) const; - bool equals(const gtsam::IndexConditional& other, double tol) const; - - // Standard interface - size_t nrFrontals() const; - size_t nrParents() const; - gtsam::IndexFactor* toFactor() const; - - //Advanced interface - void permuteWithInverse(const gtsam::Permutation& inversePermutation); -}; - -#include -template -virtual class BayesNet { - // Testable - void print(string s) const; - bool equals(const This& other, double tol) const; - - // Standard interface - size_t size() const; - void printStats(string s) const; - void saveGraph(string s) const; - CONDITIONAL* front() const; - CONDITIONAL* back() const; - void push_back(CONDITIONAL* conditional); - void push_front(CONDITIONAL* conditional); - void push_back(This& conditional); - void push_front(This& conditional); - void pop_front(); - void permuteWithInverse(const gtsam::Permutation& inversePermutation); -}; - -#include -template -virtual class BayesTree { - - //Constructors - BayesTree(); - - // Testable - void print(string s); - bool equals(const This& other, double tol) const; - - //Standard Interface - //size_t findParentClique(const gtsam::IndexVector& parents) const; - size_t size(); - size_t nrNodes() const; - void saveGraph(string s) const; - CLIQUE* root() const; - void clear(); - void deleteCachedShortcuts(); - void insert(const CLIQUE* subtree); - size_t numCachedSeparatorMarginals() const; - CLIQUE* clique(size_t j) const; -}; - -template -virtual class BayesTreeClique { - BayesTreeClique(); - BayesTreeClique(CONDITIONAL* conditional); -// BayesTreeClique(const std::pair& result) : Base(result) {} - - bool equals(const This& other, double tol) const; - void print(string s) const; - void printTree() const; // Default indent of "" - void printTree(string indent) const; - size_t numCachedSeparatorMarginals() const; - - CONDITIONAL* conditional() const; - bool isRoot() const; - size_t treeSize() const; -// const std::list& children() const { return children_; } -// derived_ptr parent() const { return parent_.lock(); } - - void permuteWithInverse(const gtsam::Permutation& inversePermutation); - - // FIXME: need wrapped versions graphs, BayesNet -// BayesNet shortcut(derived_ptr root, Eliminate function) const; -// FactorGraph marginal(derived_ptr root, Eliminate function) const; -// FactorGraph joint(derived_ptr C2, derived_ptr root, Eliminate function) const; - - void deleteCachedShortcuts(); -}; - -#include -typedef gtsam::BayesNet SymbolicBayesNetBase; - -virtual class SymbolicBayesNet : gtsam::SymbolicBayesNetBase { - // Standard Constructors and Named Constructors - SymbolicBayesNet(); - SymbolicBayesNet(const gtsam::SymbolicBayesNet& bn); - SymbolicBayesNet(const gtsam::IndexConditional* conditional); - - // Standard interface - //TODO:Throws parse error - //void push_back(const gtsam::SymbolicBayesNet bn); - //TODO:Throws parse error - //void push_front(const gtsam::SymbolicBayesNet bn); - - //Advanced Interface - void pop_front(); - void permuteWithInverse(const gtsam::Permutation& inversePermutation); -}; - -typedef gtsam::BayesTreeClique SymbolicBayesTreeClique; -typedef gtsam::BayesTree SymbolicBayesTreeBase; -virtual class SymbolicBayesTree : gtsam::SymbolicBayesTreeBase { - // Standard Constructors and Named Constructors - SymbolicBayesTree(); - SymbolicBayesTree(const gtsam::SymbolicBayesNet& bn); - SymbolicBayesTree(const gtsam::SymbolicBayesTree& other); - // FIXME: wrap needs to understand std::list - //SymbolicBayesTree(const gtsam::SymbolicBayesNet& bayesNet, std::list subtrees); - - // Standard interface - size_t findParentClique(const gtsam::IndexConditional& parents) const; - // TODO: There are many other BayesTree member functions which might be of use + bool equals(const gtsam::SymbolicFactor& other, double tol) const; + gtsam::KeyVector keys(); }; +#include class SymbolicFactorGraph { - // Standard Constructors and Named Constructors SymbolicFactorGraph(); SymbolicFactorGraph(const gtsam::SymbolicBayesNet& bayesNet); SymbolicFactorGraph(const gtsam::SymbolicBayesTree& bayesTree); // From FactorGraph - void push_back(gtsam::IndexFactor* factor); + void push_back(gtsam::SymbolicFactor* factor); void print(string s) const; bool equals(const gtsam::SymbolicFactorGraph& rhs, double tol) const; size_t size() const; - bool exists(size_t i) const; + bool exists(size_t idx) const; // Standard interface - // FIXME: Must wrap FastSet for this to work - //FastSet keys() const; + gtsam::KeySet keys() const; + void push_back(gtsam::SymbolicFactor* factor); + void push_back(const gtsam::SymbolicFactorGraph& graph); + void push_back(const gtsam::SymbolicBayesNet& bayesNet); + void push_back(const gtsam::SymbolicBayesTree& bayesTree); //Advanced Interface void push_factor(size_t key); @@ -1042,41 +887,115 @@ class SymbolicFactorGraph { void push_factor(size_t key1, size_t key2, size_t key3); void push_factor(size_t key1, size_t key2, size_t key3, size_t key4); - pair eliminateFrontals(size_t nFrontals) const; - pair eliminateOne(size_t j); + gtsam::SymbolicBayesNet* eliminateSequential(); + gtsam::SymbolicBayesNet* eliminateSequential(const gtsam::Ordering& ordering); + gtsam::SymbolicBayesTree* eliminateMultifrontal(); + gtsam::SymbolicBayesTree* eliminateMultifrontal(const gtsam::Ordering& ordering); + pair eliminatePartialSequential( + const gtsam::Ordering& ordering); + pair eliminatePartialSequential( + const gtsam::KeyVector& keys); + pair eliminatePartialMultifrontal( + const gtsam::Ordering& ordering); + pair eliminatePartialMultifrontal( + const gtsam::KeyVector& keys); + gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& variables); + gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& variables); + gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& variables, + const gtsam::Ordering& marginalizedVariableOrdering); + gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& variables, + const gtsam::Ordering& marginalizedVariableOrdering); + gtsam::SymbolicFactorGraph* marginal(const gtsam::KeyVector& variables); }; -#include -class SymbolicSequentialSolver { +#include +virtual class SymbolicConditional : gtsam::SymbolicFactor { // Standard Constructors and Named Constructors - SymbolicSequentialSolver(const gtsam::SymbolicFactorGraph& factorGraph); - SymbolicSequentialSolver(const gtsam::SymbolicFactorGraph* factorGraph, const gtsam::VariableIndex* variableIndex); + SymbolicConditional(); + SymbolicConditional(const gtsam::SymbolicConditional& other); + SymbolicConditional(size_t key); + SymbolicConditional(size_t key, size_t parent); + SymbolicConditional(size_t key, size_t parent1, size_t parent2); + SymbolicConditional(size_t key, size_t parent1, size_t parent2, size_t parent3); + static gtsam::SymbolicConditional FromKeys(const gtsam::KeyVector& keys, size_t nrFrontals); // Testable void print(string s) const; - bool equals(const gtsam::SymbolicSequentialSolver& rhs, double tol) const; + bool equals(const gtsam::SymbolicConditional& other, double tol) const; // Standard interface - gtsam::SymbolicBayesNet* eliminate() const; - gtsam::IndexFactor* marginalFactor(size_t j) const; + size_t nrFrontals() const; + size_t nrParents() const; }; -#include -class SymbolicMultifrontalSolver { - // Standard Constructors and Named Constructors - SymbolicMultifrontalSolver(const gtsam::SymbolicFactorGraph& factorGraph); - SymbolicMultifrontalSolver(const gtsam::SymbolicFactorGraph* factorGraph, const gtsam::VariableIndex* variableIndex); - +#include +class SymbolicBayesNet { + SymbolicBayesNet(); + SymbolicBayesNet(const gtsam::SymbolicBayesNet& other); // Testable void print(string s) const; - bool equals(const gtsam::SymbolicMultifrontalSolver& rhs, double tol) const; + bool equals(const gtsam::SymbolicBayesNet& other, double tol) const; // Standard interface - gtsam::SymbolicBayesTree* eliminate() const; - gtsam::IndexFactor* marginalFactor(size_t j) const; + size_t size() const; + void saveGraph(string s) const; + gtsam::SymbolicConditional* at(size_t idx) const; + gtsam::SymbolicConditional* front() const; + gtsam::SymbolicConditional* back() const; + void push_back(gtsam::SymbolicConditional* conditional); + void push_back(const gtsam::SymbolicBayesNet& bayesNet); }; -#include +#include +class SymbolicBayesTree { + + //Constructors + SymbolicBayesTree(); + SymbolicBayesTree(const gtsam::SymbolicBayesTree& other); + + // Testable + void print(string s); + bool equals(const gtsam::SymbolicBayesTree& other, double tol) const; + + //Standard Interface + //size_t findParentClique(const gtsam::IndexVector& parents) const; + size_t size(); + void saveGraph(string s) const; + void clear(); + void deleteCachedShortcuts(); + size_t numCachedSeparatorMarginals() const; + + gtsam::SymbolicConditional* marginalFactor(size_t key) const; + gtsam::SymbolicFactorGraph* joint(size_t key1, size_t key2) const; + gtsam::SymbolicBayesNet* jointBayesNet(size_t key1, size_t key2) const; +}; + +// class SymbolicBayesTreeClique { +// BayesTreeClique(); +// BayesTreeClique(CONDITIONAL* conditional); +// // BayesTreeClique(const std::pair& result) : Base(result) {} +// +// bool equals(const This& other, double tol) const; +// void print(string s) const; +// void printTree() const; // Default indent of "" +// void printTree(string indent) const; +// size_t numCachedSeparatorMarginals() const; +// +// CONDITIONAL* conditional() const; +// bool isRoot() const; +// size_t treeSize() const; +// // const std::list& children() const { return children_; } +// // derived_ptr parent() const { return parent_.lock(); } +// +// // FIXME: need wrapped versions graphs, BayesNet +// // BayesNet shortcut(derived_ptr root, Eliminate function) const; +// // FactorGraph marginal(derived_ptr root, Eliminate function) const; +// // FactorGraph joint(derived_ptr C2, derived_ptr root, Eliminate function) const; +// +// void deleteCachedShortcuts(); +// }; + +#include class VariableIndex { // Standard Constructors and Named Constructors VariableIndex(); @@ -1085,11 +1004,8 @@ class VariableIndex { //VariableIndex(const T& factorGraph, size_t nVariables); //VariableIndex(const T& factorGraph); VariableIndex(const gtsam::SymbolicFactorGraph& factorGraph); - VariableIndex(const gtsam::SymbolicFactorGraph& factorGraph, size_t nVariables); VariableIndex(const gtsam::GaussianFactorGraph& factorGraph); - VariableIndex(const gtsam::GaussianFactorGraph& factorGraph, size_t nVariables); VariableIndex(const gtsam::NonlinearFactorGraph& factorGraph); - VariableIndex(const gtsam::NonlinearFactorGraph& factorGraph, size_t nVariables); VariableIndex(const gtsam::VariableIndex& other); // Testable @@ -1100,7 +1016,6 @@ class VariableIndex { size_t size() const; size_t nFactors() const; size_t nEntries() const; - void permuteInPlace(const gtsam::Permutation& permutation); }; //************************************************************************* @@ -1237,16 +1152,14 @@ class Sampler { Vector sampleNewModel(gtsam::noiseModel::Diagonal* model); }; +#include class VectorValues { //Constructors VectorValues(); VectorValues(const gtsam::VectorValues& other); - VectorValues(size_t nVars, size_t varDim); //Named Constructors static gtsam::VectorValues Zero(const gtsam::VectorValues& model); - static gtsam::VectorValues Zero(size_t nVars, size_t varDim); - static gtsam::VectorValues SameStructure(const gtsam::VectorValues& other); //Standard Interface size_t size() const; @@ -1255,128 +1168,48 @@ class VectorValues { void print(string s) const; bool equals(const gtsam::VectorValues& expected, double tol) const; void insert(size_t j, Vector value); - Vector asVector() const; + Vector vector() const; Vector at(size_t j) const; + void update(const gtsam::VectorValues& values); //Advanced Interface - void resizeLike(const gtsam::VectorValues& other); - void resize(size_t nVars, size_t varDim); void setZero(); gtsam::VectorValues add(const gtsam::VectorValues& c) const; - gtsam::VectorValues scale(double a, const gtsam::VectorValues& c) const; + void addInPlace(const gtsam::VectorValues& c); + gtsam::VectorValues subtract(const gtsam::VectorValues& c) const; + gtsam::VectorValues scale(double a) const; + void scaleInPlace(double a); - //FIXME: Parse errors with vector() - //const Vector& vector() const; - //Vector& vector(); - bool hasSameStructure(const gtsam::VectorValues& other) const; - double dot(const gtsam::VectorValues& V) const; - double norm() const; - - // enabling serialization functionality - void serialize() const; -}; - -class GaussianConditional { - //Constructors - GaussianConditional(size_t key, Vector d, Matrix R, Vector sigmas); - GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, - Vector sigmas); - GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, - size_t name2, Matrix T, Vector sigmas); - - //Standard Interface - void print(string s) const; - bool equals(const gtsam::GaussianConditional &cg, double tol) const; - size_t dim() const; - - //Advanced Interface - Matrix information() const; - Matrix augmentedInformation() const; - gtsam::JacobianFactor* toFactor() const; - void solveInPlace(gtsam::VectorValues& x) const; - void solveTransposeInPlace(gtsam::VectorValues& gy) const; - void scaleFrontalsBySigma(gtsam::VectorValues& gy) const; + bool hasSameStructure(const gtsam::VectorValues& other) const; + double dot(const gtsam::VectorValues& V) const; + double norm() const; + double squaredNorm() const; // enabling serialization functionality void serialize() const; }; -class GaussianDensity { - //Constructors - GaussianDensity(size_t key, Vector d, Matrix R, Vector sigmas); - - //Standard Interface - void print(string s) const; - Vector mean() const; - Matrix information() const; - Matrix covariance() const; -}; - -typedef gtsam::BayesNet GaussianBayesNetBase; -virtual class GaussianBayesNet : gtsam::GaussianBayesNetBase { - //Constructors - GaussianBayesNet(); - GaussianBayesNet(const gtsam::GaussianConditional* conditional); -}; - -//Non-Class methods found in GaussianBayesNet.h -//FIXME: No MATLAB documentation for these functions! -//gtsam::GaussianBayesNet scalarGaussian(size_t key, double mu, double sigma); -//gtsam::GaussianBayesNet simpleGaussian(size_t key, const Vector& mu, double sigma); -//void push_front(gtsam::GaussianBayesNet& bn, size_t key, Vector d, Matrix R, size_t name1, Matrix S, Vector sigmas); -//void push_front(gtsam::GaussianBayesNet& bn, size_t key, Vector d, Matrix R, size_t name1, Matrix S, size_t name2, Matrix T, Vector sigmas); -//gtsam::VectorValues* allocateVectorValues(const gtsam::GaussianBayesNet& bn); -//gtsam::VectorValues optimize(const gtsam::GaussianBayesNet& bn); -//void optimizeInPlace(const gtsam::GaussianBayesNet& bn, gtsam::VectorValues& x); -//gtsam::VectorValues optimizeGradientSearch(const gtsam::GaussianBayesNet& bn); -//void optimizeGradientSearchInPlace(const gtsam::GaussianBayesNet& bn, gtsam::VectorValues& grad); -//gtsam::VectorValues backSubstitute(const gtsam::GaussianBayesNet& bn, const gtsam::VectorValues& gx); -//gtsam::VectorValues backSubstituteTranspose(const gtsam::GaussianBayesNet& bn, const gtsam::VectorValues& gx); -//pair matrix(const gtsam::GaussianBayesNet& bn); -double determinant(const gtsam::GaussianBayesNet& bayesNet); -//gtsam::VectorValues gradient(const gtsam::GaussianBayesNet& bayesNet, const gtsam::VectorValues& x0); -//void gradientAtZero(const gtsam::GaussianBayesNet& bayesNet, const gtsam::VectorValues& g); - -#include -typedef gtsam::BayesTreeClique GaussianBayesTreeClique; -typedef gtsam::BayesTree GaussianBayesTreeBase; -virtual class GaussianBayesTree : gtsam::GaussianBayesTreeBase { - // Standard Constructors and Named Constructors - GaussianBayesTree(); - GaussianBayesTree(const gtsam::GaussianBayesNet& bn); - GaussianBayesTree(const gtsam::GaussianBayesNet& other); - bool equals(const gtsam::GaussianBayesTree& other, double tol) const; - void print(string s); - size_t size() const; - size_t nrNodes() const; - bool empty() const; - gtsam::GaussianBayesTreeClique* root() const; - gtsam::GaussianBayesTreeClique* clique(size_t j) const; - size_t numCachedSeparatorMarginals() const; - void saveGraph(string s) const; -}; - -// namespace functions for GaussianBayesTree -gtsam::VectorValues optimize(const gtsam::GaussianBayesTree& bayesTree); -gtsam::VectorValues optimizeGradientSearch(const gtsam::GaussianBayesTree& bayesTree); -gtsam::VectorValues gradient(const gtsam::GaussianBayesTree& bayesTree, const gtsam::VectorValues& x0); -gtsam::VectorValues* allocateVectorValues(const gtsam::GaussianBayesTree& bt); -double determinant(const gtsam::GaussianBayesTree& bayesTree); - +#include virtual class GaussianFactor { void print(string s) const; bool equals(const gtsam::GaussianFactor& lf, double tol) const; double error(const gtsam::VectorValues& c) const; + gtsam::GaussianFactor* clone() const; gtsam::GaussianFactor* negate() const; Matrix augmentedInformation() const; Matrix information() const; + Matrix augmentedJacobian() const; + pair jacobian() const; size_t size() const; + bool empty() const; }; +#include virtual class JacobianFactor : gtsam::GaussianFactor { - //Constructors + //Constructors JacobianFactor(); + JacobianFactor(const gtsam::GaussianFactor& factor); JacobianFactor(Vector b_in); JacobianFactor(size_t i1, Matrix A1, Vector b, const gtsam::noiseModel::Diagonal* model); @@ -1384,7 +1217,7 @@ virtual class JacobianFactor : gtsam::GaussianFactor { const gtsam::noiseModel::Diagonal* model); JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, size_t i3, Matrix A3, Vector b, const gtsam::noiseModel::Diagonal* model); - JacobianFactor(const gtsam::GaussianFactor& factor); + JacobianFactor(const gtsam::GaussianFactorGraph& graph); //Testable void print(string s) const; @@ -1396,38 +1229,32 @@ virtual class JacobianFactor : gtsam::GaussianFactor { double error(const gtsam::VectorValues& c) const; //Standard Interface - bool empty() const; Matrix getA() const; Vector getb() const; size_t rows() const; size_t cols() const; - size_t numberOfRows() const; bool isConstrained() const; - pair matrix() const; - Matrix matrix_augmented() const; + pair jacobianUnweighted() const; + Matrix augmentedJacobianUnweighted() const; - gtsam::GaussianConditional* eliminateFirst(); - gtsam::GaussianConditional* eliminate(size_t nrFrontals); - gtsam::GaussianFactor* negate() const; void transposeMultiplyAdd(double alpha, const Vector& e, gtsam::VectorValues& x) const; gtsam::JacobianFactor whiten() const; - gtsam::GaussianConditional* eliminateFirst(); - gtsam::GaussianConditional* eliminate(size_t nFrontals); - gtsam::GaussianConditional* splitConditional(size_t nFrontals); + + pair eliminate(const gtsam::Ordering& keys) const; void setModel(bool anyConstrained, const Vector& sigmas); - void assertInvariants() const; - //gtsam::SharedDiagonal& get_model(); + gtsam::noiseModel::Diagonal* get_model() const; // enabling serialization functionality void serialize() const; }; +#include virtual class HessianFactor : gtsam::GaussianFactor { - //Constructors - HessianFactor(const gtsam::HessianFactor& gf); + //Constructors HessianFactor(); + HessianFactor(const gtsam::GaussianFactor& factor); HessianFactor(size_t j, Matrix G, Vector g, double f); HessianFactor(size_t j, Vector mu, Matrix Sigma); HessianFactor(size_t j1, size_t j2, Matrix G11, Matrix G12, Vector g1, Matrix G22, @@ -1435,8 +1262,7 @@ virtual class HessianFactor : gtsam::GaussianFactor { HessianFactor(size_t j1, size_t j2, size_t j3, Matrix G11, Matrix G12, Matrix G13, Vector g1, Matrix G22, Matrix G23, Vector g2, Matrix G33, Vector g3, double f); - HessianFactor(const gtsam::GaussianConditional& cg); - HessianFactor(const gtsam::GaussianFactor& factor); + HessianFactor(const gtsam::GaussianFactorGraph& factors); //Testable size_t size() const; @@ -1451,32 +1277,30 @@ virtual class HessianFactor : gtsam::GaussianFactor { double constantTerm() const; Vector linearTerm() const; - //Advanced Interface - void partialCholesky(size_t nrFrontals); - gtsam::GaussianConditional* splitEliminatedFactor(size_t nrFrontals); - void assertInvariants() const; - // enabling serialization functionality void serialize() const; }; +#include class GaussianFactorGraph { GaussianFactorGraph(); - GaussianFactorGraph(const gtsam::GaussianBayesNet& CBN); + GaussianFactorGraph(const gtsam::GaussianBayesNet& bayesNet); + GaussianFactorGraph(const gtsam::GaussianBayesTree& bayesTree); // From FactorGraph void print(string s) const; bool equals(const gtsam::GaussianFactorGraph& lfgraph, double tol) const; size_t size() const; gtsam::GaussianFactor* at(size_t idx) const; + gtsam::KeySet keys() const; bool exists(size_t idx) const; - // Inference - pair eliminateFrontals(size_t nFrontals) const; - pair eliminateOne(size_t j) const; - // Building the graph - void push_back(gtsam::GaussianFactor* factor); + void push_back(const gtsam::GaussianFactor* factor); + void push_back(const gtsam::GaussianConditional* factor); + void push_back(const gtsam::GaussianFactorGraph& graph); + void push_back(const gtsam::GaussianBayesNet& bayesNet); + void push_back(const gtsam::GaussianBayesTree& bayesTree); void add(const gtsam::GaussianFactor& factor); void add(Vector b); void add(size_t key1, Matrix A1, Vector b, const gtsam::noiseModel::Diagonal* model); @@ -1485,37 +1309,155 @@ class GaussianFactorGraph { void add(size_t key1, Matrix A1, size_t key2, Matrix A2, size_t key3, Matrix A3, Vector b, const gtsam::noiseModel::Diagonal* model); - //Permutations - void permuteWithInverse(const gtsam::Permutation& inversePermutation); - // error and probability double error(const gtsam::VectorValues& c) const; double probPrime(const gtsam::VectorValues& c) const; - // combining - static gtsam::GaussianFactorGraph combine2( - const gtsam::GaussianFactorGraph& lfg1, - const gtsam::GaussianFactorGraph& lfg2); - void combine(const gtsam::GaussianFactorGraph& lfg); + gtsam::GaussianFactorGraph clone() const; + gtsam::GaussianFactorGraph negate() const; + + // Optimizing and linear algebra + gtsam::VectorValues optimize() const; + gtsam::VectorValues optimize(const gtsam::Ordering& ordering) const; + gtsam::VectorValues optimizeGradientSearch() const; + gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const; + gtsam::VectorValues gradientAtZero() const; + + // Elimination and marginals + gtsam::GaussianBayesNet* eliminateSequential(); + gtsam::GaussianBayesNet* eliminateSequential(const gtsam::Ordering& ordering); + gtsam::GaussianBayesTree* eliminateMultifrontal(); + gtsam::GaussianBayesTree* eliminateMultifrontal(const gtsam::Ordering& ordering); + pair eliminatePartialSequential( + const gtsam::Ordering& ordering); + pair eliminatePartialSequential( + const gtsam::KeyVector& keys); + pair eliminatePartialMultifrontal( + const gtsam::Ordering& ordering); + pair eliminatePartialMultifrontal( + const gtsam::KeyVector& keys); + gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& variables); + gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& variables); + gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& variables, + const gtsam::Ordering& marginalizedVariableOrdering); + gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& variables, + const gtsam::Ordering& marginalizedVariableOrdering); + gtsam::GaussianFactorGraph* marginal(const gtsam::KeyVector& variables); // Conversion to matrices Matrix sparseJacobian_() const; Matrix augmentedJacobian() const; + Matrix augmentedJacobian(const gtsam::Ordering& ordering) const; pair jacobian() const; + pair jacobian(const gtsam::Ordering& ordering) const; Matrix augmentedHessian() const; + Matrix augmentedHessian(const gtsam::Ordering& ordering) const; pair hessian() const; + pair hessian(const gtsam::Ordering& ordering) const; // enabling serialization functionality void serialize() const; }; -//Non-Class functions in GaussianFactorGraph.h -/*void multiplyInPlace(const gtsam::GaussianFactorGraph& fg, const gtsam::VectorValues& x, gtsam::Errors& e); -gtsam::VectorValues gradient(const gtsam::GaussianFactorGraph& fg, const gtsam::VectorValues& x0); -void gradientAtZero(const gtsam::GaussianFactorGraph& fg, gtsam::VectorValues& g); -void residual(const gtsam::GaussianFactorGraph& fg, const gtsam::VectorValues& x, gtsam::VectorValues& r); -void multiply(const gtsam::GaussianFactorGraph& fg, const gtsam::VectorValues& x, gtsam::VectorValues& r); -void transposeMultiply(const gtsam::GaussianFactorGraph& fg, const gtsam::VectorValues& x, gtsam::VectorValues& r);*/ +#include +virtual class GaussianConditional : gtsam::GaussianFactor { + //Constructors + GaussianConditional(size_t key, Vector d, Matrix R, const gtsam::noiseModel::Diagonal* sigmas); + GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, + const gtsam::noiseModel::Diagonal* sigmas); + GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, + size_t name2, Matrix T, const gtsam::noiseModel::Diagonal* sigmas); + + //Constructors with no noise model + GaussianConditional(size_t key, Vector d, Matrix R); + GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S); + GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, + size_t name2, Matrix T); + + //Standard Interface + void print(string s) const; + bool equals(const gtsam::GaussianConditional &cg, double tol) const; + + //Advanced Interface + gtsam::VectorValues solve(const gtsam::VectorValues& parents) const; + gtsam::VectorValues solveOtherRHS(const gtsam::VectorValues& parents, const gtsam::VectorValues& rhs) const; + void solveTransposeInPlace(gtsam::VectorValues& gy) const; + void scaleFrontalsBySigma(gtsam::VectorValues& gy) const; + + // enabling serialization functionality + void serialize() const; +}; + +#include +virtual class GaussianDensity : gtsam::GaussianConditional { + //Constructors + GaussianDensity(size_t key, Vector d, Matrix R, const gtsam::noiseModel::Diagonal* sigmas); + + //Standard Interface + void print(string s) const; + bool equals(const gtsam::GaussianDensity &cg, double tol) const; + Vector mean() const; + Matrix covariance() const; +}; + +#include +virtual class GaussianBayesNet { + //Constructors + GaussianBayesNet(); + GaussianBayesNet(const gtsam::GaussianConditional* conditional); + + // Testable + void print(string s) const; + bool equals(const gtsam::GaussianBayesNet& other, double tol) const; + size_t size() const; + + // FactorGraph derived interface + size_t size() const; + gtsam::GaussianConditional* at(size_t idx) const; + gtsam::KeySet keys() const; + bool exists(size_t idx) const; + + gtsam::GaussianConditional* front() const; + gtsam::GaussianConditional* back() const; + void push_back(gtsam::GaussianConditional* conditional); + void push_back(const gtsam::GaussianBayesNet& bayesNet); + + gtsam::VectorValues optimize() const; + gtsam::VectorValues optimize(gtsam::VectorValues& solutionForMissing) const; + gtsam::VectorValues optimizeGradientSearch() const; + gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const; + gtsam::VectorValues gradientAtZero() const; + double error(const gtsam::VectorValues& x) const; + double determinant() const; + double logDeterminant() const; + gtsam::VectorValues backSubstitute(const gtsam::VectorValues& gx) const; + gtsam::VectorValues backSubstituteTranspose(const gtsam::VectorValues& gx) const; +}; + +#include +virtual class GaussianBayesTree { + // Standard Constructors and Named Constructors + GaussianBayesTree(); + GaussianBayesTree(const gtsam::GaussianBayesTree& other); + bool equals(const gtsam::GaussianBayesTree& other, double tol) const; + void print(string s); + size_t size() const; + bool empty() const; + size_t numCachedSeparatorMarginals() const; + void saveGraph(string s) const; + + gtsam::VectorValues optimize() const; + gtsam::VectorValues optimizeGradientSearch() const; + gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const; + gtsam::VectorValues gradientAtZero() const; + double error(const gtsam::VectorValues& x) const; + double determinant() const; + double logDeterminant() const; + Matrix marginalCovariance(size_t key) const; + gtsam::GaussianConditional* marginalFactor(size_t key) const; + gtsam::GaussianFactorGraph* joint(size_t key1, size_t key2) const; + gtsam::GaussianBayesNet* jointBayesNet(size_t key1, size_t key2) const; +}; class Errors { //Constructors @@ -1527,50 +1469,16 @@ class Errors { bool equals(const gtsam::Errors& expected, double tol) const; }; -//Non-Class functions for Errors -//double dot(const gtsam::Errors& A, const gtsam::Errors& b); - -virtual class GaussianISAM : gtsam::GaussianBayesTree { +class GaussianISAM { //Constructor GaussianISAM(); //Standard Interface + void update(const gtsam::GaussianFactorGraph& newFactors); void saveGraph(string s) const; - gtsam::GaussianFactor* marginalFactor(size_t j) const; - gtsam::GaussianBayesNet* marginalBayesNet(size_t key) const; - Matrix marginalCovariance(size_t key) const; - gtsam::GaussianBayesNet* jointBayesNet(size_t key1, size_t key2) const; void clear(); }; -#include -class GaussianSequentialSolver { - //Constructors - GaussianSequentialSolver(const gtsam::GaussianFactorGraph& graph, - bool useQR); - - //Standard Interface - void replaceFactors(const gtsam::GaussianFactorGraph* factorGraph); - gtsam::GaussianBayesNet* eliminate() const; - gtsam::VectorValues* optimize() const; - gtsam::GaussianFactor* marginalFactor(size_t j) const; - Matrix marginalCovariance(size_t j) const; -}; - -#include -class GaussianMultifrontalSolver { - //Constructors - GaussianMultifrontalSolver(const gtsam::GaussianFactorGraph& graph, - bool useQR); - - //Standard Interface - void replaceFactors(const gtsam::GaussianFactorGraph* factorGraph); - gtsam::GaussianBayesTree* eliminate() const; - gtsam::VectorValues* optimize() const; - gtsam::GaussianFactor* marginalFactor(size_t j) const; - Matrix marginalCovariance(size_t j) const; -}; - #include virtual class IterativeOptimizationParameters { string getKernel() const ; @@ -1588,15 +1496,15 @@ virtual class IterativeOptimizationParameters { #include virtual class ConjugateGradientParameters : gtsam::IterativeOptimizationParameters { ConjugateGradientParameters(); - size_t getMinIterations() const ; - size_t getMaxIterations() const ; - size_t getReset() const; + int getMinIterations() const ; + int getMaxIterations() const ; + int getReset() const; double getEpsilon_rel() const; double getEpsilon_abs() const; - void setMinIterations(size_t value); - void setMaxIterations(size_t value); - void setReset(size_t value); + void setMinIterations(int value); + void setMaxIterations(int value); + void setReset(int value); void setEpsilon_rel(double value); void setEpsilon_abs(double value); void print(); @@ -1609,8 +1517,8 @@ virtual class SubgraphSolverParameters : gtsam::ConjugateGradientParameters { }; class SubgraphSolver { - SubgraphSolver(const gtsam::GaussianFactorGraph &A, const gtsam::SubgraphSolverParameters ¶meters); - SubgraphSolver(const gtsam::GaussianFactorGraph &Ab1, const gtsam::GaussianFactorGraph &Ab2, const gtsam::SubgraphSolverParameters ¶meters); + SubgraphSolver(const gtsam::GaussianFactorGraph &A, const gtsam::SubgraphSolverParameters ¶meters, const gtsam::Ordering& ordering); + SubgraphSolver(const gtsam::GaussianFactorGraph &Ab1, const gtsam::GaussianFactorGraph &Ab2, const gtsam::SubgraphSolverParameters ¶meters, const gtsam::Ordering& ordering); gtsam::VectorValues optimize() const; }; @@ -1637,16 +1545,20 @@ class KalmanFilter { // nonlinear //************************************************************************* -#include +#include size_t symbol(char chr, size_t index); char symbolChr(size_t key); size_t symbolIndex(size_t key); // Default keyformatter -void printKeySet(const gtsam::KeySet& keys); -void printKeySet(const gtsam::KeySet& keys, string s); +void printKeyList (const gtsam::KeyList& keys); +void printKeyList (const gtsam::KeyList& keys, string s); +void printKeyVector(const gtsam::KeyVector& keys); +void printKeyVector(const gtsam::KeyVector& keys, string s); +void printKeySet (const gtsam::KeySet& keys); +void printKeySet (const gtsam::KeySet& keys, string s); -#include +#include class LabeledSymbol { LabeledSymbol(size_t full_key); LabeledSymbol(const gtsam::LabeledSymbol& key); @@ -1659,6 +1571,8 @@ class LabeledSymbol { gtsam::LabeledSymbol upper() const; gtsam::LabeledSymbol lower() const; + gtsam::LabeledSymbol newChr(unsigned char c) const; + gtsam::LabeledSymbol newLabel(unsigned char label) const; void print(string s) const; }; @@ -1668,10 +1582,11 @@ unsigned char mrsymbolChr(size_t key); unsigned char mrsymbolLabel(size_t key); size_t mrsymbolIndex(size_t key); -#include +#include class Ordering { // Standard Constructors and Named Constructors Ordering(); + Ordering(const gtsam::Ordering& other); // Testable void print(string s) const; @@ -1680,12 +1595,7 @@ class Ordering { // Standard interface size_t size() const; size_t at(size_t key) const; - size_t key(size_t index) const; - bool exists(size_t key) const; - void insert(size_t key, size_t order); void push_back(size_t key); - void permuteInPlace(const gtsam::Permutation& permutation); - void permuteInPlace(const gtsam::Permutation& selector, const gtsam::Permutation& permutation); // enabling serialization functionality void serialize() const; @@ -1703,18 +1613,17 @@ class NonlinearFactorGraph { void remove(size_t i); size_t nrFactors() const; gtsam::NonlinearFactor* at(size_t idx) const; - bool exists(size_t idx) const; void push_back(const gtsam::NonlinearFactorGraph& factors); + void push_back(gtsam::NonlinearFactor* factor); + void add(gtsam::NonlinearFactor* factor); + bool exists(size_t idx) const; // NonlinearFactorGraph double error(const gtsam::Values& values) const; double probPrime(const gtsam::Values& values) const; - void add(const gtsam::NonlinearFactor* factor); - gtsam::Ordering* orderingCOLAMD(const gtsam::Values& values) const; + gtsam::Ordering orderingCOLAMD() const; // Ordering* orderingCOLAMDConstrained(const gtsam::Values& c, const std::map& constraints) const; - gtsam::GaussianFactorGraph* linearize(const gtsam::Values& values, - const gtsam::Ordering& ordering) const; - gtsam::SymbolicFactorGraph* symbolic(const gtsam::Ordering& ordering) const; + gtsam::GaussianFactorGraph* linearize(const gtsam::Values& values) const; gtsam::NonlinearFactorGraph clone() const; // enabling serialization functionality @@ -1732,7 +1641,7 @@ virtual class NonlinearFactor { double error(const gtsam::Values& c) const; size_t dim() const; bool active(const gtsam::Values& c) const; - gtsam::GaussianFactor* linearize(const gtsam::Values& c, const gtsam::Ordering& ordering) const; + gtsam::GaussianFactor* linearize(const gtsam::Values& c) const; gtsam::NonlinearFactor* clone() const; // gtsam::NonlinearFactor* rekey(const gtsam::KeyVector& newKeys) const; //FIXME: Conversion from KeyVector to std::vector does not happen }; @@ -1768,19 +1677,17 @@ class Values { gtsam::Value at(size_t j) const; gtsam::KeyList keys() const; - gtsam::VectorValues zeroVectors(const gtsam::Ordering& ordering) const; - gtsam::Ordering* orderingArbitrary(size_t firstVar) const; + gtsam::VectorValues zeroVectors() const; - gtsam::Values retract(const gtsam::VectorValues& delta, const gtsam::Ordering& ordering) const; - gtsam::VectorValues localCoordinates(const gtsam::Values& cp, const gtsam::Ordering& ordering) const; - void localCoordinates(const gtsam::Values& cp, const gtsam::Ordering& ordering, gtsam::VectorValues& delta) const; + gtsam::Values retract(const gtsam::VectorValues& delta) const; + gtsam::VectorValues localCoordinates(const gtsam::Values& cp) const; // enabling serialization functionality void serialize() const; }; // Actually a FastList -#include +#include class KeyList { KeyList(); KeyList(const gtsam::KeyList& other); @@ -1801,10 +1708,11 @@ class KeyList { void pop_front(); void sort(); void remove(size_t key); + + void serialize() const; }; // Actually a FastSet -#include class KeySet { KeySet(); KeySet(const gtsam::KeySet& other); @@ -1822,12 +1730,14 @@ class KeySet { // structure specific methods void insert(size_t key); + void merge(gtsam::KeySet& other); bool erase(size_t key); // returns true if value was removed bool count(size_t key) const; // returns true if value exists + + void serialize() const; }; -// Actually a FastVector -#include +// Actually a vector class KeyVector { KeyVector(); KeyVector(const gtsam::KeyVector& other); @@ -1846,6 +1756,25 @@ class KeyVector { size_t front() const; size_t back() const; void push_back(size_t key) const; + + void serialize() const; +}; + +// Actually a FastMap +class KeyGroupMap { + KeyGroupMap(); + + // Note: no print function + + // common STL methods + size_t size() const; + bool empty() const; + void clear(); + + // structure specific methods + size_t at(size_t key) const; + int erase(size_t key); + bool insert2(size_t key, int val); }; #include @@ -1869,71 +1798,58 @@ class JointMarginal { #include virtual class LinearContainerFactor : gtsam::NonlinearFactor { - LinearContainerFactor(gtsam::GaussianFactor* factor, const gtsam::Ordering& ordering, - const gtsam::Values& linearizationPoint); - LinearContainerFactor(gtsam::GaussianFactor* factor, const gtsam::Values& linearizationPoint); - - LinearContainerFactor(gtsam::GaussianFactor* factor, const gtsam::Ordering& ordering); + LinearContainerFactor(gtsam::GaussianFactor* factor, const gtsam::Values& linearizationPoint); LinearContainerFactor(gtsam::GaussianFactor* factor); gtsam::GaussianFactor* factor() const; // const boost::optional& linearizationPoint() const; - gtsam::GaussianFactor* order(const gtsam::Ordering& ordering) const; - gtsam::GaussianFactor* negate(const gtsam::Ordering& ordering) const; - gtsam::NonlinearFactor* negate() const; - bool isJacobian() const; gtsam::JacobianFactor* toJacobian() const; gtsam::HessianFactor* toHessian() const; static gtsam::NonlinearFactorGraph convertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph, - const gtsam::Ordering& ordering, const gtsam::Values& linearizationPoint); + const gtsam::Values& linearizationPoint); - static gtsam::NonlinearFactorGraph convertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph, - const gtsam::Ordering& ordering); + static gtsam::NonlinearFactorGraph convertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph); // enabling serialization functionality void serializable() const; }; // \class LinearContainerFactor // Summarization functionality -#include - -// Uses partial QR approach by default -pair summarize( - const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values, - const gtsam::KeySet& saved_keys); - -gtsam::NonlinearFactorGraph summarizeAsNonlinearContainer( - const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values, - const gtsam::KeySet& saved_keys); +//#include +// +//// Uses partial QR approach by default +//gtsam::GaussianFactorGraph summarize( +// const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values, +// const gtsam::KeySet& saved_keys); +// +//gtsam::NonlinearFactorGraph summarizeAsNonlinearContainer( +// const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values, +// const gtsam::KeySet& saved_keys); //************************************************************************* // Nonlinear optimizers //************************************************************************* #include +#include virtual class NonlinearOptimizerParams { NonlinearOptimizerParams(); void print(string s) const; - size_t getMaxIterations() const; + int getMaxIterations() const; double getRelativeErrorTol() const; double getAbsoluteErrorTol() const; double getErrorTol() const; string getVerbosity() const; - void setMaxIterations(size_t value); + void setMaxIterations(int value); void setRelativeErrorTol(double value); void setAbsoluteErrorTol(double value); void setErrorTol(double value); void setVerbosity(string s); -}; - -#include -virtual class SuccessiveLinearizationParams : gtsam::NonlinearOptimizerParams { - SuccessiveLinearizationParams(); string getLinearSolverType() const; @@ -1947,13 +1863,17 @@ virtual class SuccessiveLinearizationParams : gtsam::NonlinearOptimizerParams { bool isCG() const; }; +bool checkConvergence(double relativeErrorTreshold, + double absoluteErrorTreshold, double errorThreshold, + double currentError, double newError); + #include -virtual class GaussNewtonParams : gtsam::SuccessiveLinearizationParams { +virtual class GaussNewtonParams : gtsam::NonlinearOptimizerParams { GaussNewtonParams(); }; #include -virtual class LevenbergMarquardtParams : gtsam::SuccessiveLinearizationParams { +virtual class LevenbergMarquardtParams : gtsam::NonlinearOptimizerParams { LevenbergMarquardtParams(); double getlambdaInitial() const; @@ -1968,7 +1888,7 @@ virtual class LevenbergMarquardtParams : gtsam::SuccessiveLinearizationParams { }; #include -virtual class DoglegParams : gtsam::SuccessiveLinearizationParams { +virtual class DoglegParams : gtsam::NonlinearOptimizerParams { DoglegParams(); double getDeltaInitial() const; @@ -2078,17 +1998,14 @@ class ISAM2Params { void setEnablePartialRelinearizationCheck(bool enablePartialRelinearizationCheck); }; -virtual class ISAM2Clique { +class ISAM2Clique { //Constructors - ISAM2Clique(const gtsam::GaussianConditional* conditional); + ISAM2Clique(); //Standard Interface Vector gradientContribution() const; - gtsam::ISAM2Clique* clone() const; void print(string s); - - void permuteWithInverse(const gtsam::Permutation& inversePermutation); }; class ISAM2Result { @@ -2102,8 +2019,7 @@ class ISAM2Result { size_t getCliques() const; }; -typedef gtsam::BayesTree ISAM2BayesTree; -virtual class ISAM2 : gtsam::ISAM2BayesTree { +class ISAM2 { ISAM2(); ISAM2(const gtsam::ISAM2Params& params); ISAM2(const gtsam::ISAM2& other); @@ -2116,6 +2032,7 @@ virtual class ISAM2 : gtsam::ISAM2BayesTree { gtsam::ISAM2Result update(); gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta); gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices); + gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, const gtsam::KeyGroupMap& constrainedKeys); // TODO: wrap the full version of update //void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, FastMap& constrainedKeys); //void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, FastMap& constrainedKeys, bool force_relinearize); @@ -2127,7 +2044,6 @@ virtual class ISAM2 : gtsam::ISAM2BayesTree { Matrix marginalCovariance(size_t key) const; gtsam::VectorValues getDelta() const; gtsam::NonlinearFactorGraph getFactorsUnsafe() const; - gtsam::Ordering getOrdering() const; gtsam::VariableIndex getVariableIndex() const; gtsam::ISAM2Params params() const; }; @@ -2145,13 +2061,10 @@ class NonlinearISAM { int reorderCounter() const; void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& initialValues); void reorder_relinearize(); - void addKey(size_t key); - void setOrdering(const gtsam::Ordering& new_ordering); // These might be expensive as instead of a reference the wrapper will make a copy gtsam::GaussianISAM bayesTree() const; gtsam::Values getLinearizationPoint() const; - gtsam::Ordering getOrdering() const; gtsam::NonlinearFactorGraph getFactorsUnsafe() const; }; @@ -2233,6 +2146,8 @@ template virtual class BearingRangeFactor : gtsam::NoiseModelFactor { BearingRangeFactor(size_t poseKey, size_t pointKey, const ROTATION& measuredBearing, double measuredRange, const gtsam::noiseModel::Base* noiseModel); + pair measured() const; + // enabling serialization functionality void serialize() const; }; @@ -2331,6 +2246,7 @@ pair load2D(string filename, gtsam::noiseModel::Diagonal* model, int maxID); pair load2D(string filename, gtsam::noiseModel::Diagonal* model); +pair load2D(string filename); pair load2D_robust(string filename, gtsam::noiseModel::Base* model); @@ -2453,6 +2369,7 @@ namespace utilities { gtsam::Values allPose3s(gtsam::Values& values); Matrix extractPose3(const gtsam::Values& values); void perturbPoint2(gtsam::Values& values, double sigma, int seed); + void perturbPose2 (gtsam::Values& values, double sigmaT, double sigmaR, int seed); void perturbPoint3(gtsam::Values& values, double sigma, int seed); void insertBackprojections(gtsam::Values& values, const gtsam::SimpleCamera& c, Vector J, Matrix Z, double depth); void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K); diff --git a/gtsam/3rdparty/CMakeLists.txt b/gtsam/3rdparty/CMakeLists.txt index f06f4f769..430ae9515 100644 --- a/gtsam/3rdparty/CMakeLists.txt +++ b/gtsam/3rdparty/CMakeLists.txt @@ -15,16 +15,39 @@ if(NOT GTSAM_USE_SYSTEM_EIGEN) install(FILES Eigen/Eigen/${filename} DESTINATION include/gtsam/3rdparty/Eigen/Eigen) endif() endforeach(eigen_dir) - + # Add to project source set(eigen_headers ${eigen_headers} PARENT_SCOPE) - + # install Eigen - only the headers in our 3rdparty directory - install(DIRECTORY Eigen/Eigen + install(DIRECTORY Eigen/Eigen DESTINATION include/gtsam/3rdparty/Eigen FILES_MATCHING PATTERN "*.h") endif() ############ NOTE: When updating GeographicLib be sure to disable building their examples -############ by commenting out their line add_subdirectory (examples). -add_subdirectory(GeographicLib) +############ and unit tests by commenting out their lines: +# add_subdirectory (examples) +# set (TOOLS CartConvert ConicProj GeodesicProj GeoConvert GeodSolve +# GeoidEval Gravity MagneticField Planimeter TransverseMercatorProj) +# add_subdirectory (tools) + +# Find GeographicLib using the find script distributed with it +include(GeographicLib/cmake/FindGeographicLib.cmake) + +# Set up the option to install GeographicLib +if(GEOGRAPHICLIB_FOUND) + set(install_geographiclib_default OFF) +else() + set(install_geographiclib_default ON) +endif() +option(GTSAM_INSTALL_GEOGRAPHICLIB "Build and install the 3rd-party library GeographicLib" ${install_geographiclib_default}) + +# Print warning if we'll overwrite GeographicLib +if(GEOGRAPHICLIB_FOUND AND GTSAM_INSTALL_GEOGRAPHICLIB) + message(WARNING "GeographicLib is installed on your system and GTSAM_INSTALL_GEOGRAPHICLIB is enabled. Installing gtsam will either overwrite the installed GeographicLib or install a second version that may conflict. You may want to disable GTSAM_INSTALL_GEOGRAPHICLIB if the installed version was not installed by GTSAM.") +endif() + +if(GTSAM_INSTALL_GEOGRAPHICLIB) + add_subdirectory(GeographicLib) +endif() diff --git a/gtsam/3rdparty/Eigen/CMakeLists.txt b/gtsam/3rdparty/Eigen/CMakeLists.txt index ad0269ea6..76a11b9d2 100644 --- a/gtsam/3rdparty/Eigen/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/CMakeLists.txt @@ -204,7 +204,7 @@ if(NOT MSVC) option(EIGEN_TEST_NEON "Enable/Disable Neon in tests/examples" OFF) if(EIGEN_TEST_NEON) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mfpu=neon -mcpu=cortex-a"8) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mfpu=neon -mcpu=cortex-a8") message(STATUS "Enabling NEON in tests/examples") endif() diff --git a/gtsam/3rdparty/Eigen/Eigen/Eigen2Support b/gtsam/3rdparty/Eigen/Eigen/Eigen2Support index 36156d29a..6aa009d20 100644 --- a/gtsam/3rdparty/Eigen/Eigen/Eigen2Support +++ b/gtsam/3rdparty/Eigen/Eigen/Eigen2Support @@ -14,12 +14,25 @@ #error Eigen2 support must be enabled by defining EIGEN2_SUPPORT before including any Eigen header #endif +#ifndef EIGEN_NO_EIGEN2_DEPRECATED_WARNING + +#if defined(__GNUC__) || defined(__INTEL_COMPILER) || defined(__clang__) +#warning "Eigen2 support is deprecated in Eigen 3.2.x and it will be removed in Eigen 3.3. (Define EIGEN_NO_EIGEN2_DEPRECATED_WARNING to disable this warning)" +#else +#pragma message ("Eigen2 support is deprecated in Eigen 3.2.x and it will be removed in Eigen 3.3. (Define EIGEN_NO_EIGEN2_DEPRECATED_WARNING to disable this warning)") +#endif + +#endif // EIGEN_NO_EIGEN2_DEPRECATED_WARNING + #include "src/Core/util/DisableStupidWarnings.h" /** \ingroup Support_modules * \defgroup Eigen2Support_Module Eigen2 support module - * This module provides a couple of deprecated functions improving the compatibility with Eigen2. * + * \warning Eigen2 support is deprecated in Eigen 3.2.x and it will be removed in Eigen 3.3. + * + * This module provides a couple of deprecated functions improving the compatibility with Eigen2. + * * To use it, define EIGEN2_SUPPORT before including any Eigen header * \code * #define EIGEN2_SUPPORT diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h b/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h index d19cb3968..d026418f8 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h @@ -16,7 +16,10 @@ namespace Eigen { namespace internal { -template struct LDLT_Traits; + template struct LDLT_Traits; + + // PositiveSemiDef means positive semi-definite and non-zero; same for NegativeSemiDef + enum SignMatrix { PositiveSemiDef, NegativeSemiDef, ZeroSign, Indefinite }; } /** \ingroup Cholesky_Module @@ -69,7 +72,12 @@ template class LDLT * The default constructor is useful in cases in which the user intends to * perform decompositions via LDLT::compute(const MatrixType&). */ - LDLT() : m_matrix(), m_transpositions(), m_isInitialized(false) {} + LDLT() + : m_matrix(), + m_transpositions(), + m_sign(internal::ZeroSign), + m_isInitialized(false) + {} /** \brief Default Constructor with memory preallocation * @@ -81,6 +89,7 @@ template class LDLT : m_matrix(size, size), m_transpositions(size), m_temporary(size), + m_sign(internal::ZeroSign), m_isInitialized(false) {} @@ -93,6 +102,7 @@ template class LDLT : m_matrix(matrix.rows(), matrix.cols()), m_transpositions(matrix.rows()), m_temporary(matrix.rows()), + m_sign(internal::ZeroSign), m_isInitialized(false) { compute(matrix); @@ -139,7 +149,7 @@ template class LDLT inline bool isPositive() const { eigen_assert(m_isInitialized && "LDLT is not initialized."); - return m_sign == 1; + return m_sign == internal::PositiveSemiDef || m_sign == internal::ZeroSign; } #ifdef EIGEN2_SUPPORT @@ -153,7 +163,7 @@ template class LDLT inline bool isNegative(void) const { eigen_assert(m_isInitialized && "LDLT is not initialized."); - return m_sign == -1; + return m_sign == internal::NegativeSemiDef || m_sign == internal::ZeroSign; } /** \returns a solution x of \f$ A x = b \f$ using the current decomposition of A. @@ -235,7 +245,7 @@ template class LDLT MatrixType m_matrix; TranspositionType m_transpositions; TmpMatrixType m_temporary; - int m_sign; + internal::SignMatrix m_sign; bool m_isInitialized; }; @@ -246,7 +256,7 @@ template struct ldlt_inplace; template<> struct ldlt_inplace { template - static bool unblocked(MatrixType& mat, TranspositionType& transpositions, Workspace& temp, int* sign=0) + static bool unblocked(MatrixType& mat, TranspositionType& transpositions, Workspace& temp, SignMatrix& sign) { using std::abs; typedef typename MatrixType::Scalar Scalar; @@ -258,8 +268,9 @@ template<> struct ldlt_inplace if (size <= 1) { transpositions.setIdentity(); - if(sign) - *sign = numext::real(mat.coeff(0,0))>0 ? 1:-1; + if (numext::real(mat.coeff(0,0)) > 0) sign = PositiveSemiDef; + else if (numext::real(mat.coeff(0,0)) < 0) sign = NegativeSemiDef; + else sign = ZeroSign; return true; } @@ -284,7 +295,6 @@ template<> struct ldlt_inplace if(biggest_in_corner < cutoff) { for(Index i = k; i < size; i++) transpositions.coeffRef(i) = i; - if(sign) *sign = 0; break; } @@ -325,15 +335,15 @@ template<> struct ldlt_inplace } if((rs>0) && (abs(mat.coeffRef(k,k)) > cutoff)) A21 /= mat.coeffRef(k,k); - - if(sign) - { - // LDLT is not guaranteed to work for indefinite matrices, but let's try to get the sign right - int newSign = numext::real(mat.diagonal().coeff(index_of_biggest_in_corner)) > 0; - if(k == 0) - *sign = newSign; - else if(*sign != newSign) - *sign = 0; + + RealScalar realAkk = numext::real(mat.coeffRef(k,k)); + if (sign == PositiveSemiDef) { + if (realAkk < 0) sign = Indefinite; + } else if (sign == NegativeSemiDef) { + if (realAkk > 0) sign = Indefinite; + } else if (sign == ZeroSign) { + if (realAkk > 0) sign = PositiveSemiDef; + else if (realAkk < 0) sign = NegativeSemiDef; } } @@ -399,7 +409,7 @@ template<> struct ldlt_inplace template<> struct ldlt_inplace { template - static EIGEN_STRONG_INLINE bool unblocked(MatrixType& mat, TranspositionType& transpositions, Workspace& temp, int* sign=0) + static EIGEN_STRONG_INLINE bool unblocked(MatrixType& mat, TranspositionType& transpositions, Workspace& temp, SignMatrix& sign) { Transpose matt(mat); return ldlt_inplace::unblocked(matt, transpositions, temp, sign); @@ -445,7 +455,7 @@ LDLT& LDLT::compute(const MatrixType& a) m_isInitialized = false; m_temporary.resize(size); - internal::ldlt_inplace::unblocked(m_matrix, m_transpositions, m_temporary, &m_sign); + internal::ldlt_inplace::unblocked(m_matrix, m_transpositions, m_temporary, m_sign); m_isInitialized = true; return *this; @@ -473,7 +483,7 @@ LDLT& LDLT::rankUpdate(const MatrixBase=0 ? 1 : -1; + m_sign = sigma>=0 ? internal::PositiveSemiDef : internal::NegativeSemiDef; m_isInitialized = true; } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/CholmodSupport/CholmodSupport.h b/gtsam/3rdparty/Eigen/Eigen/src/CholmodSupport/CholmodSupport.h index 783324b0b..c449960de 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/CholmodSupport/CholmodSupport.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/CholmodSupport/CholmodSupport.h @@ -58,10 +58,12 @@ cholmod_sparse viewAsCholmod(SparseMatrix<_Scalar,_Options,_Index>& mat) res.p = mat.outerIndexPtr(); res.i = mat.innerIndexPtr(); res.x = mat.valuePtr(); + res.z = 0; res.sorted = 1; if(mat.isCompressed()) { res.packed = 1; + res.nz = 0; } else { @@ -170,6 +172,7 @@ class CholmodBase : internal::noncopyable CholmodBase() : m_cholmodFactor(0), m_info(Success), m_isInitialized(false) { + m_shiftOffset[0] = m_shiftOffset[1] = RealScalar(0.0); cholmod_start(&m_cholmod); } @@ -241,7 +244,7 @@ class CholmodBase : internal::noncopyable return internal::sparse_solve_retval(*this, b.derived()); } - /** Performs a symbolic decomposition on the sparcity of \a matrix. + /** Performs a symbolic decomposition on the sparsity pattern of \a matrix. * * This function is particularly useful when solving for several problems having the same structure. * @@ -265,7 +268,7 @@ class CholmodBase : internal::noncopyable /** Performs a numeric decomposition of \a matrix * - * The given matrix must has the same sparcity than the matrix on which the symbolic decomposition has been performed. + * The given matrix must have the same sparsity pattern as the matrix on which the symbolic decomposition has been performed. * * \sa analyzePattern() */ @@ -302,7 +305,7 @@ class CholmodBase : internal::noncopyable { this->m_info = NumericalIssue; } - // TODO optimize this copy by swapping when possible (be carreful with alignment, etc.) + // TODO optimize this copy by swapping when possible (be careful with alignment, etc.) dest = Matrix::Map(reinterpret_cast(x_cd->x),b.rows(),b.cols()); cholmod_free_dense(&x_cd, &m_cholmod); } @@ -323,7 +326,7 @@ class CholmodBase : internal::noncopyable { this->m_info = NumericalIssue; } - // TODO optimize this copy by swapping when possible (be carreful with alignment, etc.) + // TODO optimize this copy by swapping when possible (be careful with alignment, etc.) dest = viewAsEigen(*x_cs); cholmod_free_sparse(&x_cs, &m_cholmod); } @@ -365,8 +368,8 @@ class CholmodBase : internal::noncopyable * * This class allows to solve for A.X = B sparse linear problems via a simplicial LL^T Cholesky factorization * using the Cholmod library. - * This simplicial variant is equivalent to Eigen's built-in SimplicialLLT class. Thefore, it has little practical interest. - * The sparse matrix A must be selfajoint and positive definite. The vectors or matrices + * This simplicial variant is equivalent to Eigen's built-in SimplicialLLT class. Therefore, it has little practical interest. + * The sparse matrix A must be selfadjoint and positive definite. The vectors or matrices * X and B can be either dense or sparse. * * \tparam _MatrixType the type of the sparse matrix A, it must be a SparseMatrix<> @@ -412,8 +415,8 @@ class CholmodSimplicialLLT : public CholmodBase<_MatrixType, _UpLo, CholmodSimpl * * This class allows to solve for A.X = B sparse linear problems via a simplicial LDL^T Cholesky factorization * using the Cholmod library. - * This simplicial variant is equivalent to Eigen's built-in SimplicialLDLT class. Thefore, it has little practical interest. - * The sparse matrix A must be selfajoint and positive definite. The vectors or matrices + * This simplicial variant is equivalent to Eigen's built-in SimplicialLDLT class. Therefore, it has little practical interest. + * The sparse matrix A must be selfadjoint and positive definite. The vectors or matrices * X and B can be either dense or sparse. * * \tparam _MatrixType the type of the sparse matrix A, it must be a SparseMatrix<> @@ -458,7 +461,7 @@ class CholmodSimplicialLDLT : public CholmodBase<_MatrixType, _UpLo, CholmodSimp * This class allows to solve for A.X = B sparse linear problems via a supernodal LL^T Cholesky factorization * using the Cholmod library. * This supernodal variant performs best on dense enough problems, e.g., 3D FEM, or very high order 2D FEM. - * The sparse matrix A must be selfajoint and positive definite. The vectors or matrices + * The sparse matrix A must be selfadjoint and positive definite. The vectors or matrices * X and B can be either dense or sparse. * * \tparam _MatrixType the type of the sparse matrix A, it must be a SparseMatrix<> @@ -501,7 +504,7 @@ class CholmodSupernodalLLT : public CholmodBase<_MatrixType, _UpLo, CholmodSuper * \brief A general Cholesky factorization and solver based on Cholmod * * This class allows to solve for A.X = B sparse linear problems via a LL^T or LDL^T Cholesky factorization - * using the Cholmod library. The sparse matrix A must be selfajoint and positive definite. The vectors or matrices + * using the Cholmod library. The sparse matrix A must be selfadjoint and positive definite. The vectors or matrices * X and B can be either dense or sparse. * * This variant permits to change the underlying Cholesky method at runtime. diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Array.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Array.h index 497efff66..0ab03eff0 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Array.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Array.h @@ -210,7 +210,7 @@ class Array : Base(other.derived().rows() * other.derived().cols(), other.derived().rows(), other.derived().cols()) { Base::_check_template_params(); - Base::resize(other.rows(), other.cols()); + Base::_resize_to_match(other); *this = other; } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/BooleanRedux.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/BooleanRedux.h index 6e37e031a..be9f48a8c 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/BooleanRedux.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/BooleanRedux.h @@ -29,9 +29,9 @@ struct all_unroller }; template -struct all_unroller +struct all_unroller { - static inline bool run(const Derived &mat) { return mat.coeff(0, 0); } + static inline bool run(const Derived &/*mat*/) { return true; } }; template @@ -55,9 +55,9 @@ struct any_unroller }; template -struct any_unroller +struct any_unroller { - static inline bool run(const Derived &mat) { return mat.coeff(0, 0); } + static inline bool run(const Derived & /*mat*/) { return false; } }; template diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/CommaInitializer.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/CommaInitializer.h index 57ffb6b9d..46e3a960a 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/CommaInitializer.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/CommaInitializer.h @@ -30,9 +30,9 @@ struct CommaInitializer : { typedef typename internal::dense_xpr_base >::type Base; EIGEN_DENSE_PUBLIC_INTERFACE(CommaInitializer) - typedef typename internal::conditional::ret, - XprType, const XprType&>::type ExpressionTypeNested; - typedef typename XprType::InnerIterator InnerIterator; + typedef typename internal::conditional::ret, + XprType, const XprType&>::type ExpressionTypeNested; + typedef typename XprType::InnerIterator InnerIterator; inline CommaInitializer(XprType& xpr, const Scalar& s) : m_xpr(xpr), m_row(0), m_col(1), m_currentBlockRows(1) @@ -108,67 +108,67 @@ struct CommaInitializer : */ inline XprType& finished() { return m_xpr; } - // The following implement the DenseBase interface - - inline Index rows() const { return m_xpr.rows(); } - inline Index cols() const { return m_xpr.cols(); } - inline Index outerStride() const { return m_xpr.outerStride(); } - inline Index innerStride() const { return m_xpr.innerStride(); } - - inline CoeffReturnType coeff(Index row, Index col) const - { - return m_xpr.coeff(row, col); - } - - inline CoeffReturnType coeff(Index index) const - { - return m_xpr.coeff(index); - } - - inline const Scalar& coeffRef(Index row, Index col) const - { - return m_xpr.const_cast_derived().coeffRef(row, col); - } - - inline const Scalar& coeffRef(Index index) const - { - return m_xpr.const_cast_derived().coeffRef(index); - } - - inline Scalar& coeffRef(Index row, Index col) - { - return m_xpr.const_cast_derived().coeffRef(row, col); - } - - inline Scalar& coeffRef(Index index) - { - return m_xpr.const_cast_derived().coeffRef(index); - } - - template - inline const PacketScalar packet(Index row, Index col) const - { - return m_xpr.template packet(row, col); - } - - template - inline void writePacket(Index row, Index col, const PacketScalar& x) - { - m_xpr.const_cast_derived().template writePacket(row, col, x); - } - - template - inline const PacketScalar packet(Index index) const - { - return m_xpr.template packet(index); - } - - template - inline void writePacket(Index index, const PacketScalar& x) - { - m_xpr.const_cast_derived().template writePacket(index, x); - } - + // The following implement the DenseBase interface + + inline Index rows() const { return m_xpr.rows(); } + inline Index cols() const { return m_xpr.cols(); } + inline Index outerStride() const { return m_xpr.outerStride(); } + inline Index innerStride() const { return m_xpr.innerStride(); } + + inline CoeffReturnType coeff(Index row, Index col) const + { + return m_xpr.coeff(row, col); + } + + inline CoeffReturnType coeff(Index index) const + { + return m_xpr.coeff(index); + } + + inline const Scalar& coeffRef(Index row, Index col) const + { + return m_xpr.const_cast_derived().coeffRef(row, col); + } + + inline const Scalar& coeffRef(Index index) const + { + return m_xpr.const_cast_derived().coeffRef(index); + } + + inline Scalar& coeffRef(Index row, Index col) + { + return m_xpr.const_cast_derived().coeffRef(row, col); + } + + inline Scalar& coeffRef(Index index) + { + return m_xpr.const_cast_derived().coeffRef(index); + } + + template + inline const PacketScalar packet(Index row, Index col) const + { + return m_xpr.template packet(row, col); + } + + template + inline void writePacket(Index row, Index col, const PacketScalar& x) + { + m_xpr.const_cast_derived().template writePacket(row, col, x); + } + + template + inline const PacketScalar packet(Index index) const + { + return m_xpr.template packet(index); + } + + template + inline void writePacket(Index index, const PacketScalar& x) + { + m_xpr.const_cast_derived().template writePacket(index, x); + } + const XprType& _expression() const { return m_xpr; } XprType& m_xpr; // target expression @@ -176,12 +176,12 @@ struct CommaInitializer : Index m_col; // current col id Index m_currentBlockRows; // current block height }; - -namespace internal { - template - struct traits > : traits - { - }; + +namespace internal { + template + struct traits > : traits + { + }; } /** \anchor MatrixBaseCommaInitRef diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/EigenBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/EigenBase.h index 2b8dd1b70..fadb45852 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/EigenBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/EigenBase.h @@ -126,36 +126,6 @@ Derived& DenseBase::operator-=(const EigenBase &other) return derived(); } -/** replaces \c *this by \c *this * \a other. - * - * \returns a reference to \c *this - */ -template -template -inline Derived& -MatrixBase::operator*=(const EigenBase &other) -{ - other.derived().applyThisOnTheRight(derived()); - return derived(); -} - -/** replaces \c *this by \c *this * \a other. It is equivalent to MatrixBase::operator*=(). - */ -template -template -inline void MatrixBase::applyOnTheRight(const EigenBase &other) -{ - other.derived().applyThisOnTheRight(derived()); -} - -/** replaces \c *this by \c *this * \a other. */ -template -template -inline void MatrixBase::applyOnTheLeft(const EigenBase &other) -{ - other.derived().applyThisOnTheLeft(derived()); -} - } // end namespace Eigen #endif // EIGEN_EIGENBASE_H diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/IO.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/IO.h index c8d5f6379..8d4bc59e9 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/IO.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/IO.h @@ -185,21 +185,22 @@ std::ostream & print_matrix(std::ostream & s, const Derived& _m, const IOFormat& explicit_precision = fmt.precision; } + std::streamsize old_precision = 0; + if(explicit_precision) old_precision = s.precision(explicit_precision); + bool align_cols = !(fmt.flags & DontAlignCols); if(align_cols) { // compute the largest width - for(Index j = 1; j < m.cols(); ++j) + for(Index j = 0; j < m.cols(); ++j) for(Index i = 0; i < m.rows(); ++i) { std::stringstream sstr; - if(explicit_precision) sstr.precision(explicit_precision); + sstr.copyfmt(s); sstr << m.coeff(i,j); width = std::max(width, Index(sstr.str().length())); } } - std::streamsize old_precision = 0; - if(explicit_precision) old_precision = s.precision(explicit_precision); s << fmt.matPrefix; for(Index i = 0; i < m.rows(); ++i) { diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Matrix.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Matrix.h index 0ba5d90cc..d7d0b5b9a 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Matrix.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Matrix.h @@ -304,7 +304,7 @@ class Matrix : Base(other.derived().rows() * other.derived().cols(), other.derived().rows(), other.derived().cols()) { Base::_check_template_params(); - Base::resize(other.rows(), other.cols()); + Base::_resize_to_match(other); // FIXME/CHECK: isn't *this = other.derived() more efficient. it allows to // go for pure _set() implementations, right? *this = other; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/MatrixBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/MatrixBase.h index 9193b6abb..344b38f2f 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/MatrixBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/MatrixBase.h @@ -510,6 +510,51 @@ template class MatrixBase {EIGEN_STATIC_ASSERT(std::ptrdiff_t(sizeof(typename OtherDerived::Scalar))==-1,YOU_CANNOT_MIX_ARRAYS_AND_MATRICES); return *this;} }; + +/*************************************************************************** +* Implementation of matrix base methods +***************************************************************************/ + +/** replaces \c *this by \c *this * \a other. + * + * \returns a reference to \c *this + * + * Example: \include MatrixBase_applyOnTheRight.cpp + * Output: \verbinclude MatrixBase_applyOnTheRight.out + */ +template +template +inline Derived& +MatrixBase::operator*=(const EigenBase &other) +{ + other.derived().applyThisOnTheRight(derived()); + return derived(); +} + +/** replaces \c *this by \c *this * \a other. It is equivalent to MatrixBase::operator*=(). + * + * Example: \include MatrixBase_applyOnTheRight.cpp + * Output: \verbinclude MatrixBase_applyOnTheRight.out + */ +template +template +inline void MatrixBase::applyOnTheRight(const EigenBase &other) +{ + other.derived().applyThisOnTheRight(derived()); +} + +/** replaces \c *this by \a other * \c *this. + * + * Example: \include MatrixBase_applyOnTheLeft.cpp + * Output: \verbinclude MatrixBase_applyOnTheLeft.out + */ +template +template +inline void MatrixBase::applyOnTheLeft(const EigenBase &other) +{ + other.derived().applyThisOnTheLeft(derived()); +} + } // end namespace Eigen #endif // EIGEN_MATRIXBASE_H diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/PermutationMatrix.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/PermutationMatrix.h index 4fc5dd318..1297b8413 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/PermutationMatrix.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/PermutationMatrix.h @@ -553,7 +553,8 @@ struct permut_matrix_product_retval template inline void evalTo(Dest& dst) const { const Index n = Side==OnTheLeft ? rows() : cols(); - + // FIXME we need an is_same for expression that is not sensitive to constness. For instance + // is_same_xpr, Block >::value should be true. if(is_same::value && extract_data(dst) == extract_data(m_matrix)) { // apply the permutation inplace diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/PlainObjectBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/PlainObjectBase.h index af0a479c7..dd34b59e5 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/PlainObjectBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/PlainObjectBase.h @@ -47,7 +47,10 @@ template<> struct check_rows_cols_for_overflow { } }; -template struct conservative_resize_like_impl; +template +struct conservative_resize_like_impl; template struct matrix_swap_impl; @@ -668,8 +671,10 @@ private: enum { ThisConstantIsPrivateInPlainObjectBase }; }; +namespace internal { + template -struct internal::conservative_resize_like_impl +struct conservative_resize_like_impl { typedef typename Derived::Index Index; static void run(DenseBase& _this, Index rows, Index cols) @@ -729,11 +734,14 @@ struct internal::conservative_resize_like_impl } }; -namespace internal { - +// Here, the specialization for vectors inherits from the general matrix case +// to allow calling .conservativeResize(rows,cols) on vectors. template struct conservative_resize_like_impl + : conservative_resize_like_impl { + using conservative_resize_like_impl::run; + typedef typename Derived::Index Index; static void run(DenseBase& _this, Index size) { diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Ref.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Ref.h index aba795bdb..00d9e6d2b 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Ref.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Ref.h @@ -94,7 +94,8 @@ struct traits > typedef _PlainObjectType PlainObjectType; typedef _StrideType StrideType; enum { - Options = _Options + Options = _Options, + Flags = traits >::Flags | NestByRefBit }; template struct match { @@ -111,7 +112,7 @@ struct traits > }; typedef typename internal::conditional::type type; }; - + }; template diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/StableNorm.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/StableNorm.h index c83e955ee..389d94275 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/StableNorm.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/StableNorm.h @@ -17,16 +17,29 @@ namespace internal { template inline void stable_norm_kernel(const ExpressionType& bl, Scalar& ssq, Scalar& scale, Scalar& invScale) { - Scalar max = bl.cwiseAbs().maxCoeff(); - if (max>scale) + using std::max; + Scalar maxCoeff = bl.cwiseAbs().maxCoeff(); + + if (maxCoeff>scale) { - ssq = ssq * numext::abs2(scale/max); - scale = max; - invScale = Scalar(1)/scale; + ssq = ssq * numext::abs2(scale/maxCoeff); + Scalar tmp = Scalar(1)/maxCoeff; + if(tmp > NumTraits::highest()) + { + invScale = NumTraits::highest(); + scale = Scalar(1)/invScale; + } + else + { + scale = maxCoeff; + invScale = tmp; + } } - // TODO if the max is much much smaller than the current scale, + + // TODO if the maxCoeff is much much smaller than the current scale, // then we can neglect this sub vector - ssq += (bl*invScale).squaredNorm(); + if(scale>Scalar(0)) // if scale==0, then bl is 0 + ssq += (bl*invScale).squaredNorm(); } template diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Transpose.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Transpose.h index f21b3aa65..22096ea2f 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Transpose.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Transpose.h @@ -284,7 +284,8 @@ struct inplace_transpose_selector { // non square matrix * Notice however that this method is only useful if you want to replace a matrix by its own transpose. * If you just need the transpose of a matrix, use transpose(). * - * \note if the matrix is not square, then \c *this must be a resizable matrix. + * \note if the matrix is not square, then \c *this must be a resizable matrix. + * This excludes (non-square) fixed-size matrices, block-expressions and maps. * * \sa transpose(), adjoint(), adjointInPlace() */ template @@ -315,6 +316,7 @@ inline void DenseBase::transposeInPlace() * If you just need the adjoint of a matrix, use adjoint(). * * \note if the matrix is not square, then \c *this must be a resizable matrix. + * This excludes (non-square) fixed-size matrices, block-expressions and maps. * * \sa transpose(), adjoint(), transposeInPlace() */ template diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/VectorwiseOp.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/VectorwiseOp.h index 511564875..d5ab03664 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/VectorwiseOp.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/VectorwiseOp.h @@ -50,7 +50,7 @@ struct traits > MaxColsAtCompileTime = Direction==Horizontal ? 1 : MatrixType::MaxColsAtCompileTime, Flags0 = (unsigned int)_MatrixTypeNested::Flags & HereditaryBits, Flags = (Flags0 & ~RowMajorBit) | (RowsAtCompileTime == 1 ? RowMajorBit : 0), - TraversalSize = Direction==Vertical ? RowsAtCompileTime : ColsAtCompileTime + TraversalSize = Direction==Vertical ? MatrixType::RowsAtCompileTime : MatrixType::ColsAtCompileTime }; #if EIGEN_GNUC_AT_LEAST(3,4) typedef typename MemberOp::template Cost CostOpType; @@ -58,7 +58,8 @@ struct traits > typedef typename MemberOp::template Cost CostOpType; #endif enum { - CoeffReadCost = TraversalSize * traits<_MatrixTypeNested>::CoeffReadCost + int(CostOpType::value) + CoeffReadCost = TraversalSize==Dynamic ? Dynamic + : TraversalSize * traits<_MatrixTypeNested>::CoeffReadCost + int(CostOpType::value) }; }; } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/MathFunctions.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/MathFunctions.h index 3376a984e..99cbd0d95 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/MathFunctions.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/MathFunctions.h @@ -442,8 +442,11 @@ Packet4f pcos(const Packet4f& _x) return _mm_xor_ps(y, sign_bit); } +#if EIGEN_FAST_MATH + // This is based on Quake3's fast inverse square root. // For detail see here: http://www.beyond3d.com/content/articles/8/ +// It lacks 1 (or 2 bits in some rare cases) of precision, and does not handle negative, +inf, or denormalized numbers correctly. template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet4f psqrt(const Packet4f& _x) { @@ -457,6 +460,14 @@ Packet4f psqrt(const Packet4f& _x) return pmul(_x,x); } +#else + +template<> EIGEN_STRONG_INLINE Packet4f psqrt(const Packet4f& x) { return _mm_sqrt_ps(x); } + +#endif + +template<> EIGEN_STRONG_INLINE Packet2d psqrt(const Packet2d& x) { return _mm_sqrt_pd(x); } + } // end namespace internal } // end namespace Eigen diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/PacketMath.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/PacketMath.h index e256f4bac..fc8ae50fe 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/PacketMath.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/PacketMath.h @@ -83,7 +83,8 @@ template<> struct packet_traits : default_packet_traits size=2, HasDiv = 1, - HasExp = 1 + HasExp = 1, + HasSqrt = 1 }; }; template<> struct packet_traits : default_packet_traits @@ -507,8 +508,8 @@ template<> EIGEN_STRONG_INLINE int predux_min(const Packet4i& a) // for GCC (eg., it does not like using std::min after the pstore !!) EIGEN_ALIGN16 int aux[4]; pstore(aux, a); - register int aux0 = aux[0] EIGEN_STRONG_INLINE int predux_max(const Packet4i& a) // for GCC (eg., it does not like using std::min after the pstore !!) EIGEN_ALIGN16 int aux[4]; pstore(aux, a); - register int aux0 = aux[0]>aux[1] ? aux[0] : aux[1]; - register int aux2 = aux[2]>aux[3] ? aux[2] : aux[3]; + int aux0 = aux[0]>aux[1] ? aux[0] : aux[1]; + int aux2 = aux[2]>aux[3] ? aux[2] : aux[3]; return aux0>aux2 ? aux0 : aux2; } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralBlockPanelKernel.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralBlockPanelKernel.h index 780fa74d3..bcdca5b0d 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralBlockPanelKernel.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralBlockPanelKernel.h @@ -1128,6 +1128,8 @@ EIGEN_DONT_INLINE void gemm_pack_lhs::size }; EIGEN_ASM_COMMENT("EIGEN PRODUCT PACK LHS"); + EIGEN_UNUSED_VARIABLE(stride) + EIGEN_UNUSED_VARIABLE(offset) eigen_assert(((!PanelMode) && stride==0 && offset==0) || (PanelMode && stride>=depth && offset<=stride)); eigen_assert( (StorageOrder==RowMajor) || ((Pack1%PacketSize)==0 && Pack1<=4*PacketSize) ); conj_if::IsComplex && Conjugate> cj; @@ -1215,6 +1217,8 @@ EIGEN_DONT_INLINE void gemm_pack_rhs=depth && offset<=stride)); conj_if::IsComplex && Conjugate> cj; Index packet_cols = (cols/nr) * nr; @@ -1266,6 +1270,8 @@ EIGEN_DONT_INLINE void gemm_pack_rhs=depth && offset<=stride)); conj_if::IsComplex && Conjugate> cj; Index packet_cols = (cols/nr) * nr; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixVector.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixVector.h index c1cb78498..09387703e 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixVector.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixVector.h @@ -52,11 +52,7 @@ EIGEN_DONT_INLINE static void run( Index rows, Index cols, const LhsScalar* lhs, Index lhsStride, const RhsScalar* rhs, Index rhsIncr, - ResScalar* res, Index - #ifdef EIGEN_INTERNAL_DEBUGGING - resIncr - #endif - , RhsScalar alpha); + ResScalar* res, Index resIncr, RhsScalar alpha); }; template @@ -64,12 +60,9 @@ EIGEN_DONT_INLINE void general_matrix_vector_product(&lhs0[i]), ptmp0, pload(&res[i]))); + pstore(&res[i], pcj.pmadd(pload(&lhs0[i]), ptmp0, pload(&res[i]))); else for (Index i = alignedStart;i(&lhs0[i]), ptmp0, pload(&res[i]))); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointMatrixVector.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointMatrixVector.h index c40e80f53..f698f67f9 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointMatrixVector.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointMatrixVector.h @@ -79,8 +79,8 @@ EIGEN_DONT_INLINE void selfadjoint_matrix_vector_product(t0); @@ -147,7 +147,7 @@ EIGEN_DONT_INLINE void selfadjoint_matrix_vector_productx || (EIGEN_WORLD_VERSION>=x && \ (EIGEN_MAJOR_VERSION>y || (EIGEN_MAJOR_VERSION>=y && \ @@ -238,7 +238,12 @@ #endif // Suppresses 'unused variable' warnings. -#define EIGEN_UNUSED_VARIABLE(var) (void)var; +namespace Eigen { + namespace internal { + template void ignore_unused_variable(const T&) {} + } +} +#define EIGEN_UNUSED_VARIABLE(var) Eigen::internal::ignore_unused_variable(var); #if !defined(EIGEN_ASM_COMMENT) #if (defined __GNUC__) && ( defined(__i386__) || defined(__x86_64__) ) diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Memory.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Memory.h index 451535a0c..cacbd02fd 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Memory.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Memory.h @@ -578,7 +578,7 @@ template class aligned_stack_memory_handler */ #ifdef EIGEN_ALLOCA - #ifdef __arm__ + #if defined(__arm__) || defined(_WIN32) #define EIGEN_ALIGNED_ALLOCA(SIZE) reinterpret_cast((reinterpret_cast(EIGEN_ALLOCA(SIZE+16)) & ~(size_t(15))) + 16) #else #define EIGEN_ALIGNED_ALLOCA EIGEN_ALLOCA @@ -634,7 +634,9 @@ template class aligned_stack_memory_handler /* memory allocated we can safely let the default implementation handle */ \ /* this particular case. */ \ static void *operator new(size_t size, void *ptr) { return ::operator new(size,ptr); } \ + static void *operator new[](size_t size, void* ptr) { return ::operator new[](size,ptr); } \ void operator delete(void * memory, void *ptr) throw() { return ::operator delete(memory,ptr); } \ + void operator delete[](void * memory, void *ptr) throw() { return ::operator delete[](memory,ptr); } \ /* nothrow-new (returns zero instead of std::bad_alloc) */ \ EIGEN_MAKE_ALIGNED_OPERATOR_NEW_NOTHROW(NeedsToAlign) \ void operator delete(void *ptr, const std::nothrow_t&) throw() { \ @@ -729,15 +731,6 @@ public: ::new( p ) T( value ); } - // Support for c++11 -#if (__cplusplus >= 201103L) - template - void construct(pointer p, Args&&... args) - { - ::new(p) T(std::forward(args)...); - } -#endif - void destroy( pointer p ) { p->~T(); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/SVD.h b/gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/SVD.h index 077d26d54..3d03d2288 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/SVD.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/SVD.h @@ -512,8 +512,7 @@ template template bool SVD::solve(const MatrixBase &b, ResultType* result) const { - const int rows = m_matU.rows(); - ei_assert(b.rows() == rows); + ei_assert(b.rows() == m_matU.rows()); Scalar maxVal = m_sigma.cwise().abs().maxCoeff(); for (int j=0; j conjugate() const; - /** \returns an interpolation for a constant motion between \a other and \c *this - * \a t in [0;1] - * see http://en.wikipedia.org/wiki/Slerp - */ template Quaternion slerp(const Scalar& t, const QuaternionBase& other) const; /** \returns \c true if \c *this is approximately equal to \a other, within the precision @@ -194,11 +190,11 @@ public: * \brief The quaternion class used to represent 3D orientations and rotations * * \tparam _Scalar the scalar type, i.e., the type of the coefficients - * \tparam _Options controls the memory alignement of the coeffecients. Can be \# AutoAlign or \# DontAlign. Default is AutoAlign. + * \tparam _Options controls the memory alignment of the coefficients. Can be \# AutoAlign or \# DontAlign. Default is AutoAlign. * * This class represents a quaternion \f$ w+xi+yj+zk \f$ that is a convenient representation of * orientations and rotations of objects in three dimensions. Compared to other representations - * like Euler angles or 3x3 matrices, quatertions offer the following advantages: + * like Euler angles or 3x3 matrices, quaternions offer the following advantages: * \li \b compact storage (4 scalars) * \li \b efficient to compose (28 flops), * \li \b stable spherical interpolation @@ -385,7 +381,7 @@ class Map, _Options > /** Constructs a Mapped Quaternion object from the pointer \a coeffs * - * The pointer \a coeffs must reference the four coeffecients of Quaternion in the following order: + * The pointer \a coeffs must reference the four coefficients of Quaternion in the following order: * \code *coeffs == {x, y, z, w} \endcode * * If the template parameter _Options is set to #Aligned, then the pointer coeffs must be aligned. */ @@ -399,16 +395,16 @@ class Map, _Options > }; /** \ingroup Geometry_Module - * Map an unaligned array of single precision scalar as a quaternion */ + * Map an unaligned array of single precision scalars as a quaternion */ typedef Map, 0> QuaternionMapf; /** \ingroup Geometry_Module - * Map an unaligned array of double precision scalar as a quaternion */ + * Map an unaligned array of double precision scalars as a quaternion */ typedef Map, 0> QuaternionMapd; /** \ingroup Geometry_Module - * Map a 16-bits aligned array of double precision scalars as a quaternion */ + * Map a 16-byte aligned array of single precision scalars as a quaternion */ typedef Map, Aligned> QuaternionMapAlignedf; /** \ingroup Geometry_Module - * Map a 16-bits aligned array of double precision scalars as a quaternion */ + * Map a 16-byte aligned array of double precision scalars as a quaternion */ typedef Map, Aligned> QuaternionMapAlignedd; /*************************************************************************** @@ -579,7 +575,7 @@ inline Derived& QuaternionBase::setFromTwoVectors(const MatrixBase accuraletly compute the rotation axis by computing the + // => accurately compute the rotation axis by computing the // intersection of the two planes. This is done by solving: // x^T v0 = 0 // x^T v1 = 0 @@ -677,8 +673,13 @@ QuaternionBase::angularDistance(const QuaternionBase& oth return static_cast(2 * acos(d)); } + + /** \returns the spherical linear interpolation between the two quaternions - * \c *this and \a other at the parameter \a t + * \c *this and \a other at the parameter \a t in [0;1]. + * + * This represents an interpolation for a constant motion between \c *this and \a other, + * see also http://en.wikipedia.org/wiki/Slerp. */ template template diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Transform.h b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Transform.h index 887e718d6..498fea41a 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Transform.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Transform.h @@ -530,9 +530,9 @@ public: inline Transform& operator=(const UniformScaling& t); inline Transform& operator*=(const UniformScaling& s) { return scale(s.factor()); } - inline Transform operator*(const UniformScaling& s) const + inline Transform operator*(const UniformScaling& s) const { - Transform res = *this; + Transform res = *this; res.scale(s.factor()); return res; } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR.h b/gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR.h index 8b01f8179..bec85810c 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR.h @@ -349,7 +349,7 @@ template class ColPivHouseholderQR return m_usePrescribedThreshold ? m_prescribedThreshold // this formula comes from experimenting (see "LU precision tuning" thread on the list) // and turns out to be identical to Higham's formula used already in LDLt. - : NumTraits::epsilon() * m_qr.diagonalSize(); + : NumTraits::epsilon() * RealScalar(m_qr.diagonalSize()); } /** \returns the number of nonzero pivots in the QR decomposition. diff --git a/gtsam/3rdparty/Eigen/Eigen/src/QR/FullPivHouseholderQR.h b/gtsam/3rdparty/Eigen/Eigen/src/QR/FullPivHouseholderQR.h index 0dd5ad347..6168e7abf 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/QR/FullPivHouseholderQR.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/QR/FullPivHouseholderQR.h @@ -63,9 +63,10 @@ template class FullPivHouseholderQR typedef typename MatrixType::Index Index; typedef internal::FullPivHouseholderQRMatrixQReturnType MatrixQReturnType; typedef typename internal::plain_diag_type::type HCoeffsType; - typedef Matrix IntRowVectorType; + typedef Matrix IntDiagSizeVectorType; typedef PermutationMatrix PermutationType; - typedef typename internal::plain_col_type::type IntColVectorType; typedef typename internal::plain_row_type::type RowVectorType; typedef typename internal::plain_col_type::type ColVectorType; @@ -93,10 +94,10 @@ template class FullPivHouseholderQR FullPivHouseholderQR(Index rows, Index cols) : m_qr(rows, cols), m_hCoeffs((std::min)(rows,cols)), - m_rows_transpositions(rows), - m_cols_transpositions(cols), + m_rows_transpositions((std::min)(rows,cols)), + m_cols_transpositions((std::min)(rows,cols)), m_cols_permutation(cols), - m_temp((std::min)(rows,cols)), + m_temp(cols), m_isInitialized(false), m_usePrescribedThreshold(false) {} @@ -115,10 +116,10 @@ template class FullPivHouseholderQR FullPivHouseholderQR(const MatrixType& matrix) : m_qr(matrix.rows(), matrix.cols()), m_hCoeffs((std::min)(matrix.rows(), matrix.cols())), - m_rows_transpositions(matrix.rows()), - m_cols_transpositions(matrix.cols()), + m_rows_transpositions((std::min)(matrix.rows(), matrix.cols())), + m_cols_transpositions((std::min)(matrix.rows(), matrix.cols())), m_cols_permutation(matrix.cols()), - m_temp((std::min)(matrix.rows(), matrix.cols())), + m_temp(matrix.cols()), m_isInitialized(false), m_usePrescribedThreshold(false) { @@ -126,11 +127,12 @@ template class FullPivHouseholderQR } /** This method finds a solution x to the equation Ax=b, where A is the matrix of which - * *this is the QR decomposition, if any exists. + * \c *this is the QR decomposition. * * \param b the right-hand-side of the equation to solve. * - * \returns a solution. + * \returns the exact or least-square solution if the rank is greater or equal to the number of columns of A, + * and an arbitrary solution otherwise. * * \note The case where b is a matrix is not yet implemented. Also, this * code is space inefficient. @@ -172,7 +174,7 @@ template class FullPivHouseholderQR } /** \returns a const reference to the vector of indices representing the rows transpositions */ - const IntColVectorType& rowsTranspositions() const + const IntDiagSizeVectorType& rowsTranspositions() const { eigen_assert(m_isInitialized && "FullPivHouseholderQR is not initialized."); return m_rows_transpositions; @@ -344,7 +346,7 @@ template class FullPivHouseholderQR return m_usePrescribedThreshold ? m_prescribedThreshold // this formula comes from experimenting (see "LU precision tuning" thread on the list) // and turns out to be identical to Higham's formula used already in LDLt. - : NumTraits::epsilon() * m_qr.diagonalSize(); + : NumTraits::epsilon() * RealScalar(m_qr.diagonalSize()); } /** \returns the number of nonzero pivots in the QR decomposition. @@ -368,8 +370,8 @@ template class FullPivHouseholderQR protected: MatrixType m_qr; HCoeffsType m_hCoeffs; - IntColVectorType m_rows_transpositions; - IntRowVectorType m_cols_transpositions; + IntDiagSizeVectorType m_rows_transpositions; + IntDiagSizeVectorType m_cols_transpositions; PermutationType m_cols_permutation; RowVectorType m_temp; bool m_isInitialized, m_usePrescribedThreshold; @@ -415,10 +417,10 @@ FullPivHouseholderQR& FullPivHouseholderQR::compute(cons m_temp.resize(cols); - m_precision = NumTraits::epsilon() * size; + m_precision = NumTraits::epsilon() * RealScalar(size); - m_rows_transpositions.resize(matrix.rows()); - m_cols_transpositions.resize(matrix.cols()); + m_rows_transpositions.resize(size); + m_cols_transpositions.resize(size); Index number_of_transpositions = 0; RealScalar biggest(0); @@ -516,17 +518,6 @@ struct solve_retval, Rhs> dec().hCoeffs().coeff(k), &temp.coeffRef(0)); } - if(!dec().isSurjective()) - { - // is c is in the image of R ? - RealScalar biggest_in_upper_part_of_c = c.topRows( dec().rank() ).cwiseAbs().maxCoeff(); - RealScalar biggest_in_lower_part_of_c = c.bottomRows(rows-dec().rank()).cwiseAbs().maxCoeff(); - // FIXME brain dead - const RealScalar m_precision = NumTraits::epsilon() * (std::min)(rows,cols); - // this internal:: prefix is needed by at least gcc 3.4 and ICC - if(!internal::isMuchSmallerThan(biggest_in_lower_part_of_c, biggest_in_upper_part_of_c, m_precision)) - return; - } dec().matrixQR() .topLeftCorner(dec().rank(), dec().rank()) .template triangularView() @@ -548,14 +539,14 @@ template struct FullPivHouseholderQRMatrixQReturnType { public: typedef typename MatrixType::Index Index; - typedef typename internal::plain_col_type::type IntColVectorType; + typedef typename FullPivHouseholderQR::IntDiagSizeVectorType IntDiagSizeVectorType; typedef typename internal::plain_diag_type::type HCoeffsType; typedef Matrix WorkVectorType; FullPivHouseholderQRMatrixQReturnType(const MatrixType& qr, const HCoeffsType& hCoeffs, - const IntColVectorType& rowsTranspositions) + const IntDiagSizeVectorType& rowsTranspositions) : m_qr(qr), m_hCoeffs(hCoeffs), m_rowsTranspositions(rowsTranspositions) @@ -595,7 +586,7 @@ public: protected: typename MatrixType::Nested m_qr; typename HCoeffsType::Nested m_hCoeffs; - typename IntColVectorType::Nested m_rowsTranspositions; + typename IntDiagSizeVectorType::Nested m_rowsTranspositions; }; } // end namespace internal diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SPQRSupport/SuiteSparseQRSupport.h b/gtsam/3rdparty/Eigen/Eigen/src/SPQRSupport/SuiteSparseQRSupport.h index aa41f434c..a2cc2a9e2 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SPQRSupport/SuiteSparseQRSupport.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SPQRSupport/SuiteSparseQRSupport.h @@ -64,7 +64,8 @@ class SPQR typedef PermutationMatrix PermutationType; public: SPQR() - : m_ordering(SPQR_ORDERING_DEFAULT), + : m_isInitialized(false), + m_ordering(SPQR_ORDERING_DEFAULT), m_allow_tol(SPQR_DEFAULT_TOL), m_tolerance (NumTraits::epsilon()) { @@ -72,7 +73,8 @@ class SPQR } SPQR(const _MatrixType& matrix) - : m_ordering(SPQR_ORDERING_DEFAULT), + : m_isInitialized(false), + m_ordering(SPQR_ORDERING_DEFAULT), m_allow_tol(SPQR_DEFAULT_TOL), m_tolerance (NumTraits::epsilon()) { @@ -82,16 +84,22 @@ class SPQR ~SPQR() { - // Calls SuiteSparseQR_free() + SPQR_free(); + cholmod_l_finish(&m_cc); + } + void SPQR_free() + { cholmod_l_free_sparse(&m_H, &m_cc); cholmod_l_free_sparse(&m_cR, &m_cc); cholmod_l_free_dense(&m_HTau, &m_cc); std::free(m_E); std::free(m_HPinv); - cholmod_l_finish(&m_cc); } + void compute(const _MatrixType& matrix) { + if(m_isInitialized) SPQR_free(); + MatrixType mat(matrix); cholmod_sparse A; A = viewAsCholmod(mat); @@ -139,7 +147,7 @@ class SPQR eigen_assert(b.cols()==1 && "This method is for vectors only"); //Compute Q^T * b - Dest y; + typename Dest::PlainObject y; y = matrixQ().transpose() * b; // Solves with the triangular matrix R Index rk = this->rank(); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h b/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h index 4786768ff..f44995cd3 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h @@ -380,7 +380,10 @@ struct svd_precondition_2x2_block_to_be_real z = abs(work_matrix.coeff(p,q)) / work_matrix.coeff(p,q); work_matrix.row(p) *= z; if(svd.computeU()) svd.m_matrixU.col(p) *= conj(z); - z = abs(work_matrix.coeff(q,q)) / work_matrix.coeff(q,q); + if(work_matrix.coeff(q,q)!=Scalar(0)) + z = abs(work_matrix.coeff(q,q)) / work_matrix.coeff(q,q); + else + z = Scalar(0); work_matrix.row(q) *= z; if(svd.computeU()) svd.m_matrixU.col(q) *= conj(z); } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/ConservativeSparseSparseProduct.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/ConservativeSparseSparseProduct.h index 4b13f08d4..5c320e2d2 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/ConservativeSparseSparseProduct.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/ConservativeSparseSparseProduct.h @@ -66,9 +66,9 @@ static void conservative_sparse_sparse_product_impl(const Lhs& lhs, const Rhs& r } // unordered insertion - for(int k=0; k1) std::sort(indices.data(),indices.data()+nnz); - for(int k=0; k RowMajorMatrix; - typedef SparseMatrix ColMajorMatrix; + typedef SparseMatrix RowMajorMatrix; + typedef SparseMatrix ColMajorMatrix; ColMajorMatrix resCol(lhs.rows(),rhs.cols()); internal::conservative_sparse_sparse_product_impl(lhs, rhs, resCol); // sort the non zeros: @@ -149,7 +149,7 @@ struct conservative_sparse_sparse_product_selector RowMajorMatrix; + typedef SparseMatrix RowMajorMatrix; RowMajorMatrix rhsRow = rhs; RowMajorMatrix resRow(lhs.rows(), rhs.cols()); internal::conservative_sparse_sparse_product_impl(rhsRow, lhs, resRow); @@ -162,7 +162,7 @@ struct conservative_sparse_sparse_product_selector RowMajorMatrix; + typedef SparseMatrix RowMajorMatrix; RowMajorMatrix lhsRow = lhs; RowMajorMatrix resRow(lhs.rows(), rhs.cols()); internal::conservative_sparse_sparse_product_impl(rhs, lhsRow, resRow); @@ -175,7 +175,7 @@ struct conservative_sparse_sparse_product_selector RowMajorMatrix; + typedef SparseMatrix RowMajorMatrix; RowMajorMatrix resRow(lhs.rows(), rhs.cols()); internal::conservative_sparse_sparse_product_impl(rhs, lhs, resRow); res = resRow; @@ -190,7 +190,7 @@ struct conservative_sparse_sparse_product_selector ColMajorMatrix; + typedef SparseMatrix ColMajorMatrix; ColMajorMatrix resCol(lhs.rows(), rhs.cols()); internal::conservative_sparse_sparse_product_impl(lhs, rhs, resCol); res = resCol; @@ -202,7 +202,7 @@ struct conservative_sparse_sparse_product_selector ColMajorMatrix; + typedef SparseMatrix ColMajorMatrix; ColMajorMatrix lhsCol = lhs; ColMajorMatrix resCol(lhs.rows(), rhs.cols()); internal::conservative_sparse_sparse_product_impl(lhsCol, rhs, resCol); @@ -215,7 +215,7 @@ struct conservative_sparse_sparse_product_selector ColMajorMatrix; + typedef SparseMatrix ColMajorMatrix; ColMajorMatrix rhsCol = rhs; ColMajorMatrix resCol(lhs.rows(), rhs.cols()); internal::conservative_sparse_sparse_product_impl(lhs, rhsCol, resCol); @@ -228,8 +228,8 @@ struct conservative_sparse_sparse_product_selector RowMajorMatrix; - typedef SparseMatrix ColMajorMatrix; + typedef SparseMatrix RowMajorMatrix; + typedef SparseMatrix ColMajorMatrix; RowMajorMatrix resRow(lhs.rows(),rhs.cols()); internal::conservative_sparse_sparse_product_impl(rhs, lhs, resRow); // sort the non zeros: diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/MappedSparseMatrix.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/MappedSparseMatrix.h index 93cd4832d..ab1a266a9 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/MappedSparseMatrix.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/MappedSparseMatrix.h @@ -50,6 +50,8 @@ class MappedSparseMatrix inline Index cols() const { return IsRowMajor ? m_innerSize : m_outerSize; } inline Index innerSize() const { return m_innerSize; } inline Index outerSize() const { return m_outerSize; } + + bool isCompressed() const { return true; } //---------------------------------------- // direct access interface diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseBlock.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseBlock.h index 0b3e193db..16a20a574 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseBlock.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseBlock.h @@ -66,6 +66,8 @@ public: typename XprType::Nested m_matrix; Index m_outerStart; const internal::variable_if_dynamic m_outerSize; + + EIGEN_INHERIT_ASSIGNMENT_OPERATORS(BlockImpl) }; @@ -391,6 +393,8 @@ public: protected: friend class InnerIterator; friend class ReverseInnerIterator; + + EIGEN_INHERIT_ASSIGNMENT_OPERATORS(BlockImpl) typename XprType::Nested m_matrix; const internal::variable_if_dynamic m_startRow; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseColEtree.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseColEtree.h index f89ca3814..f8745f461 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseColEtree.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseColEtree.h @@ -63,6 +63,7 @@ int coletree(const MatrixType& mat, IndexVector& parent, IndexVector& firstRowEl typedef typename MatrixType::Index Index; Index nc = mat.cols(); // Number of columns Index m = mat.rows(); + Index diagSize = (std::min)(nc,m); IndexVector root(nc); // root of subtree of etree root.setZero(); IndexVector pp(nc); // disjoint sets @@ -72,7 +73,7 @@ int coletree(const MatrixType& mat, IndexVector& parent, IndexVector& firstRowEl Index row,col; firstRowElt.resize(m); firstRowElt.setConstant(nc); - firstRowElt.segment(0, nc).setLinSpaced(nc, 0, nc-1); + firstRowElt.segment(0, diagSize).setLinSpaced(diagSize, 0, diagSize-1); bool found_diag; for (col = 0; col < nc; col++) { @@ -91,7 +92,7 @@ int coletree(const MatrixType& mat, IndexVector& parent, IndexVector& firstRowEl Index rset, cset, rroot; for (col = 0; col < nc; col++) { - found_diag = false; + found_diag = col>=m; pp(col) = col; cset = col; root(cset) = col; @@ -105,6 +106,7 @@ int coletree(const MatrixType& mat, IndexVector& parent, IndexVector& firstRowEl Index i = col; if(it) i = it.index(); if (i == col) found_diag = true; + row = firstRowElt(i); if (row >= col) continue; rset = internal::etree_find(row, pp); // Find the name of the set containing row diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseDenseProduct.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseDenseProduct.h index 30975c29c..54fd633a1 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseDenseProduct.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseDenseProduct.h @@ -125,7 +125,7 @@ class SparseDenseOuterProduct::InnerIterator : public _LhsNes inline Scalar value() const { return Base::value() * m_factor; } protected: - int m_outer; + Index m_outer; Scalar m_factor; }; @@ -155,7 +155,7 @@ struct sparse_time_dense_product_impl::Constant(outerSize(), 2)); } return insertUncompressed(row,col); } @@ -402,7 +402,7 @@ class SparseMatrix * \sa insertBack, insertBackByOuterInner */ inline void startVec(Index outer) { - eigen_assert(m_outerIndex[outer]==int(m_data.size()) && "You must call startVec for each inner vector sequentially"); + eigen_assert(m_outerIndex[outer]==Index(m_data.size()) && "You must call startVec for each inner vector sequentially"); eigen_assert(m_outerIndex[outer+1]==0 && "You must call startVec for each inner vector sequentially"); m_outerIndex[outer+1] = m_outerIndex[outer]; } @@ -480,7 +480,7 @@ class SparseMatrix if(m_innerNonZeros != 0) return; m_innerNonZeros = static_cast(std::malloc(m_outerSize * sizeof(Index))); - for (int i = 0; i < m_outerSize; i++) + for (Index i = 0; i < m_outerSize; i++) { m_innerNonZeros[i] = m_outerIndex[i+1] - m_outerIndex[i]; } @@ -752,8 +752,8 @@ class SparseMatrix else for (Index i=0; i trMat(mat.rows(),mat.cols()); - if(begin wi(trMat.outerSize()); wi.setZero(); for(InputIterator it(begin); it!=end; ++it) { @@ -1018,11 +1019,11 @@ void SparseMatrix::sumupDuplicates() { eigen_assert(!isCompressed()); // TODO, in practice we should be able to use m_innerNonZeros for that task - VectorXi wi(innerSize()); + Matrix wi(innerSize()); wi.fill(-1); Index count = 0; // for each inner-vector, wi[inner_index] will hold the position of first element into the index/value buffers - for(int j=0; j& SparseMatrix positions(dest.outerSize()); for (Index j=0; j class SparseMatrixBase : public EigenBase } else { - SparseMatrix trans = m; - s << static_cast >&>(trans); + SparseMatrix trans = m; + s << static_cast >&>(trans); } } return s; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparsePermutation.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparsePermutation.h index b897b7595..b85be93f6 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparsePermutation.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparsePermutation.h @@ -57,7 +57,7 @@ struct permut_sparsematrix_product_retval if(MoveOuter) { SparseMatrix tmp(m_matrix.rows(), m_matrix.cols()); - VectorXi sizes(m_matrix.outerSize()); + Matrix sizes(m_matrix.outerSize()); for(Index j=0; j tmp(m_matrix.rows(), m_matrix.cols()); - VectorXi sizes(tmp.outerSize()); + Matrix sizes(tmp.outerSize()); sizes.setZero(); PermutationMatrix perm; if((Side==OnTheLeft) ^ Transposed) diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseProduct.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseProduct.h index 70b6480ef..cf7663070 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseProduct.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseProduct.h @@ -16,6 +16,7 @@ template struct SparseSparseProductReturnType { typedef typename internal::traits::Scalar Scalar; + typedef typename internal::traits::Index Index; enum { LhsRowMajor = internal::traits::Flags & RowMajorBit, RhsRowMajor = internal::traits::Flags & RowMajorBit, @@ -24,11 +25,11 @@ struct SparseSparseProductReturnType }; typedef typename internal::conditional, + SparseMatrix, typename internal::nested::type>::type LhsNested; typedef typename internal::conditional, + SparseMatrix, typename internal::nested::type>::type RhsNested; typedef SparseSparseProduct Type; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseSparseProductWithPruning.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseSparseProductWithPruning.h index 70857c7b6..fcc18f5c9 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseSparseProductWithPruning.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseSparseProductWithPruning.h @@ -27,7 +27,7 @@ static void sparse_sparse_product_with_pruning_impl(const Lhs& lhs, const Rhs& r // make sure to call innerSize/outerSize since we fake the storage order. Index rows = lhs.innerSize(); Index cols = rhs.outerSize(); - //int size = lhs.outerSize(); + //Index size = lhs.outerSize(); eigen_assert(lhs.outerSize() == rhs.innerSize()); // allocate a temporary buffer @@ -100,7 +100,7 @@ struct sparse_sparse_product_with_pruning_selector SparseTemporaryType; + typedef SparseMatrix SparseTemporaryType; SparseTemporaryType _res(res.rows(), res.cols()); internal::sparse_sparse_product_with_pruning_impl(lhs, rhs, _res, tolerance); res = _res; @@ -126,10 +126,11 @@ struct sparse_sparse_product_with_pruning_selector ColMajorMatrix; - ColMajorMatrix colLhs(lhs); - ColMajorMatrix colRhs(rhs); - internal::sparse_sparse_product_with_pruning_impl(colLhs, colRhs, res, tolerance); + typedef SparseMatrix ColMajorMatrixLhs; + typedef SparseMatrix ColMajorMatrixRhs; + ColMajorMatrixLhs colLhs(lhs); + ColMajorMatrixRhs colRhs(rhs); + internal::sparse_sparse_product_with_pruning_impl(colLhs, colRhs, res, tolerance); // let's transpose the product to get a column x column product // typedef SparseMatrix SparseTemporaryType; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseUtil.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseUtil.h index 064a40707..05023858b 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseUtil.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseUtil.h @@ -143,7 +143,7 @@ template struct plain_matrix_type * * \sa SparseMatrix::setFromTriplets() */ -template +template::Index > class Triplet { public: diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU.h index dd9eab2c2..1d592f2c8 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU.h @@ -219,9 +219,9 @@ class SparseLU : public internal::SparseLUImpl - bool _solve(const MatrixBase &B, MatrixBase &_X) const + bool _solve(const MatrixBase &B, MatrixBase &X_base) const { - Dest& X(_X.derived()); + Dest& X(X_base.derived()); eigen_assert(m_factorizationIsOk && "The matrix should be factorized first"); EIGEN_STATIC_ASSERT((Dest::Flags&RowMajorBit)==0, THIS_METHOD_IS_ONLY_FOR_COLUMN_MAJOR_MATRICES); @@ -229,8 +229,10 @@ class SparseLU : public internal::SparseLUImplmatrixL().solveInPlace(X); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_Memory.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_Memory.h index a5158025c..1ffa7d54e 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_Memory.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_Memory.h @@ -70,23 +70,30 @@ Index SparseLUImpl::expand(VectorType& vec, Index& length, Index if(num_expansions == 0 || keep_prev) new_len = length ; // First time allocate requested else - new_len = Index(alpha * length); + new_len = (std::max)(length+1,Index(alpha * length)); VectorType old_vec; // Temporary vector to hold the previous values if (nbElts > 0 ) old_vec = vec.segment(0,nbElts); //Allocate or expand the current vector - try +#ifdef EIGEN_EXCEPTIONS + try +#endif { vec.resize(new_len); } +#ifdef EIGEN_EXCEPTIONS catch(std::bad_alloc& ) +#else + if(!vec.size()) +#endif { - if ( !num_expansions ) + if (!num_expansions) { // First time to allocate from LUMemInit() - throw; // Pass the exception to LUMemInit() which has a try... catch block + // Let LUMemInit() deals with it. + return -1; } if (keep_prev) { @@ -100,12 +107,18 @@ Index SparseLUImpl::expand(VectorType& vec, Index& length, Index do { alpha = (alpha + 1)/2; - new_len = Index(alpha * length); + new_len = (std::max)(length+1,Index(alpha * length)); +#ifdef EIGEN_EXCEPTIONS try +#endif { vec.resize(new_len); } +#ifdef EIGEN_EXCEPTIONS catch(std::bad_alloc& ) +#else + if (!vec.size()) +#endif { tries += 1; if ( tries > 10) return new_len; @@ -139,10 +152,9 @@ template Index SparseLUImpl::memInit(Index m, Index n, Index annz, Index lwork, Index fillratio, Index panel_size, GlobalLU_t& glu) { Index& num_expansions = glu.num_expansions; //No memory expansions so far - num_expansions = 0; - glu.nzumax = glu.nzlumax = (std::max)(fillratio * annz, m*n); // estimated number of nonzeros in U + num_expansions = 0; + glu.nzumax = glu.nzlumax = (std::min)(fillratio * annz / n, m) * n; // estimated number of nonzeros in U glu.nzlmax = (std::max)(Index(4), fillratio) * annz / 4; // estimated nnz in L factor - // Return the estimated size to the user if necessary Index tempSpace; tempSpace = (2*panel_size + 4 + LUNoMarker) * m * sizeof(Index) + (panel_size + 1) * m * sizeof(Scalar); @@ -166,14 +178,10 @@ Index SparseLUImpl::memInit(Index m, Index n, Index annz, Index lw // Reserve memory for L/U factors do { - try - { - expand(glu.lusup, glu.nzlumax, 0, 0, num_expansions); - expand(glu.ucol,glu.nzumax, 0, 0, num_expansions); - expand(glu.lsub,glu.nzlmax, 0, 0, num_expansions); - expand(glu.usub,glu.nzumax, 0, 1, num_expansions); - } - catch(std::bad_alloc& ) + if( (expand(glu.lusup, glu.nzlumax, 0, 0, num_expansions)<0) + || (expand(glu.ucol, glu.nzumax, 0, 0, num_expansions)<0) + || (expand (glu.lsub, glu.nzlmax, 0, 0, num_expansions)<0) + || (expand (glu.usub, glu.nzumax, 0, 1, num_expansions)<0) ) { //Reduce the estimated size and retry glu.nzlumax /= 2; @@ -181,10 +189,7 @@ Index SparseLUImpl::memInit(Index m, Index n, Index annz, Index lw glu.nzlmax /= 2; if (glu.nzlumax < annz ) return glu.nzlumax; } - } while (!glu.lusup.size() || !glu.ucol.size() || !glu.lsub.size() || !glu.usub.size()); - - ++num_expansions; return 0; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseQR/SparseQR.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseQR/SparseQR.h index 07c46e4b9..afda43bfc 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseQR/SparseQR.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseQR/SparseQR.h @@ -161,8 +161,9 @@ class SparseQR b = y; // Solve with the triangular matrix R + y.resize((std::max)(cols(),Index(y.rows())),y.cols()); y.topRows(rank) = this->matrixR().topLeftCorner(rank, rank).template triangularView().solve(b.topRows(rank)); - y.bottomRows(y.size()-rank).setZero(); + y.bottomRows(y.rows()-rank).setZero(); // Apply the column permutation if (m_perm_c.size()) dest.topRows(cols()) = colsPermutation() * y.topRows(cols()); @@ -246,7 +247,7 @@ class SparseQR Index m_nonzeropivots; // Number of non zero pivots found IndexVector m_etree; // Column elimination tree IndexVector m_firstRowElt; // First element in each row - bool m_isQSorted; // whether Q is sorted or not + bool m_isQSorted; // whether Q is sorted or not template friend struct SparseQR_QProduct; template friend struct SparseQRMatrixQReturnType; @@ -338,7 +339,7 @@ void SparseQR::factorize(const MatrixType& mat) Index nonzeroCol = 0; // Record the number of valid pivots // Left looking rank-revealing QR factorization: compute a column of R and Q at a time - for (Index col = 0; col < n; ++col) + for (Index col = 0; col < (std::min)(n,m); ++col) { mark.setConstant(-1); m_R.startVec(col); @@ -346,7 +347,7 @@ void SparseQR::factorize(const MatrixType& mat) mark(nonzeroCol) = col; Qidx(0) = nonzeroCol; nzcolR = 0; nzcolQ = 1; - found_diag = false; + found_diag = col>=m; tval.setZero(); // Symbolic factorization: find the nonzero locations of the column k of the factors R and Q, i.e., @@ -571,6 +572,7 @@ struct SparseQR_QProduct : ReturnByValue topRightCorner() const /** \returns an expression of a top-right corner of *this. * - * \tparam CRows number of rows in corner as specified at compile time - * \tparam CCols number of columns in corner as specified at compile time - * \param cRows number of rows in corner as specified at run time - * \param cCols number of columns in corner as specified at run time + * \tparam CRows number of rows in corner as specified at compile-time + * \tparam CCols number of columns in corner as specified at compile-time + * \param cRows number of rows in corner as specified at run-time + * \param cCols number of columns in corner as specified at run-time * - * This function is mainly useful for corners where the number of rows is specified at compile time - * and the number of columns is specified at run time, or vice versa. The compile-time and run-time + * This function is mainly useful for corners where the number of rows is specified at compile-time + * and the number of columns is specified at run-time, or vice versa. The compile-time and run-time * information should not contradict. In other words, \a cRows should equal \a CRows unless * \a CRows is \a Dynamic, and the same for the number of columns. * @@ -188,13 +188,13 @@ inline const Block topLeftCorner() const /** \returns an expression of a top-left corner of *this. * - * \tparam CRows number of rows in corner as specified at compile time - * \tparam CCols number of columns in corner as specified at compile time - * \param cRows number of rows in corner as specified at run time - * \param cCols number of columns in corner as specified at run time + * \tparam CRows number of rows in corner as specified at compile-time + * \tparam CCols number of columns in corner as specified at compile-time + * \param cRows number of rows in corner as specified at run-time + * \param cCols number of columns in corner as specified at run-time * - * This function is mainly useful for corners where the number of rows is specified at compile time - * and the number of columns is specified at run time, or vice versa. The compile-time and run-time + * This function is mainly useful for corners where the number of rows is specified at compile-time + * and the number of columns is specified at run-time, or vice versa. The compile-time and run-time * information should not contradict. In other words, \a cRows should equal \a CRows unless * \a CRows is \a Dynamic, and the same for the number of columns. * @@ -263,13 +263,13 @@ inline const Block bottomRightCorner() const /** \returns an expression of a bottom-right corner of *this. * - * \tparam CRows number of rows in corner as specified at compile time - * \tparam CCols number of columns in corner as specified at compile time - * \param cRows number of rows in corner as specified at run time - * \param cCols number of columns in corner as specified at run time + * \tparam CRows number of rows in corner as specified at compile-time + * \tparam CCols number of columns in corner as specified at compile-time + * \param cRows number of rows in corner as specified at run-time + * \param cCols number of columns in corner as specified at run-time * - * This function is mainly useful for corners where the number of rows is specified at compile time - * and the number of columns is specified at run time, or vice versa. The compile-time and run-time + * This function is mainly useful for corners where the number of rows is specified at compile-time + * and the number of columns is specified at run-time, or vice versa. The compile-time and run-time * information should not contradict. In other words, \a cRows should equal \a CRows unless * \a CRows is \a Dynamic, and the same for the number of columns. * @@ -338,13 +338,13 @@ inline const Block bottomLeftCorner() const /** \returns an expression of a bottom-left corner of *this. * - * \tparam CRows number of rows in corner as specified at compile time - * \tparam CCols number of columns in corner as specified at compile time - * \param cRows number of rows in corner as specified at run time - * \param cCols number of columns in corner as specified at run time + * \tparam CRows number of rows in corner as specified at compile-time + * \tparam CCols number of columns in corner as specified at compile-time + * \param cRows number of rows in corner as specified at run-time + * \param cCols number of columns in corner as specified at run-time * - * This function is mainly useful for corners where the number of rows is specified at compile time - * and the number of columns is specified at run time, or vice versa. The compile-time and run-time + * This function is mainly useful for corners where the number of rows is specified at compile-time + * and the number of columns is specified at run-time, or vice versa. The compile-time and run-time * information should not contradict. In other words, \a cRows should equal \a CRows unless * \a CRows is \a Dynamic, and the same for the number of columns. * @@ -390,7 +390,11 @@ inline ConstRowsBlockXpr topRows(Index n) const /** \returns a block consisting of the top rows of *this. * - * \tparam N the number of rows in the block + * \tparam N the number of rows in the block as specified at compile-time + * \param n the number of rows in the block as specified at run-time + * + * The compile-time and run-time information should not contradict. In other words, + * \a n should equal \a N unless \a N is \a Dynamic. * * Example: \include MatrixBase_template_int_topRows.cpp * Output: \verbinclude MatrixBase_template_int_topRows.out @@ -398,16 +402,16 @@ inline ConstRowsBlockXpr topRows(Index n) const * \sa class Block, block(Index,Index,Index,Index) */ template -inline typename NRowsBlockXpr::Type topRows() +inline typename NRowsBlockXpr::Type topRows(Index n = N) { - return typename NRowsBlockXpr::Type(derived(), 0, 0, N, cols()); + return typename NRowsBlockXpr::Type(derived(), 0, 0, n, cols()); } /** This is the const version of topRows().*/ template -inline typename ConstNRowsBlockXpr::Type topRows() const +inline typename ConstNRowsBlockXpr::Type topRows(Index n = N) const { - return typename ConstNRowsBlockXpr::Type(derived(), 0, 0, N, cols()); + return typename ConstNRowsBlockXpr::Type(derived(), 0, 0, n, cols()); } @@ -434,7 +438,11 @@ inline ConstRowsBlockXpr bottomRows(Index n) const /** \returns a block consisting of the bottom rows of *this. * - * \tparam N the number of rows in the block + * \tparam N the number of rows in the block as specified at compile-time + * \param n the number of rows in the block as specified at run-time + * + * The compile-time and run-time information should not contradict. In other words, + * \a n should equal \a N unless \a N is \a Dynamic. * * Example: \include MatrixBase_template_int_bottomRows.cpp * Output: \verbinclude MatrixBase_template_int_bottomRows.out @@ -442,16 +450,16 @@ inline ConstRowsBlockXpr bottomRows(Index n) const * \sa class Block, block(Index,Index,Index,Index) */ template -inline typename NRowsBlockXpr::Type bottomRows() +inline typename NRowsBlockXpr::Type bottomRows(Index n = N) { - return typename NRowsBlockXpr::Type(derived(), rows() - N, 0, N, cols()); + return typename NRowsBlockXpr::Type(derived(), rows() - n, 0, n, cols()); } /** This is the const version of bottomRows().*/ template -inline typename ConstNRowsBlockXpr::Type bottomRows() const +inline typename ConstNRowsBlockXpr::Type bottomRows(Index n = N) const { - return typename ConstNRowsBlockXpr::Type(derived(), rows() - N, 0, N, cols()); + return typename ConstNRowsBlockXpr::Type(derived(), rows() - n, 0, n, cols()); } @@ -459,28 +467,32 @@ inline typename ConstNRowsBlockXpr::Type bottomRows() const /** \returns a block consisting of a range of rows of *this. * * \param startRow the index of the first row in the block - * \param numRows the number of rows in the block + * \param n the number of rows in the block * * Example: \include DenseBase_middleRows_int.cpp * Output: \verbinclude DenseBase_middleRows_int.out * * \sa class Block, block(Index,Index,Index,Index) */ -inline RowsBlockXpr middleRows(Index startRow, Index numRows) +inline RowsBlockXpr middleRows(Index startRow, Index n) { - return RowsBlockXpr(derived(), startRow, 0, numRows, cols()); + return RowsBlockXpr(derived(), startRow, 0, n, cols()); } /** This is the const version of middleRows(Index,Index).*/ -inline ConstRowsBlockXpr middleRows(Index startRow, Index numRows) const +inline ConstRowsBlockXpr middleRows(Index startRow, Index n) const { - return ConstRowsBlockXpr(derived(), startRow, 0, numRows, cols()); + return ConstRowsBlockXpr(derived(), startRow, 0, n, cols()); } /** \returns a block consisting of a range of rows of *this. * - * \tparam N the number of rows in the block + * \tparam N the number of rows in the block as specified at compile-time * \param startRow the index of the first row in the block + * \param n the number of rows in the block as specified at run-time + * + * The compile-time and run-time information should not contradict. In other words, + * \a n should equal \a N unless \a N is \a Dynamic. * * Example: \include DenseBase_template_int_middleRows.cpp * Output: \verbinclude DenseBase_template_int_middleRows.out @@ -488,16 +500,16 @@ inline ConstRowsBlockXpr middleRows(Index startRow, Index numRows) const * \sa class Block, block(Index,Index,Index,Index) */ template -inline typename NRowsBlockXpr::Type middleRows(Index startRow) +inline typename NRowsBlockXpr::Type middleRows(Index startRow, Index n = N) { - return typename NRowsBlockXpr::Type(derived(), startRow, 0, N, cols()); + return typename NRowsBlockXpr::Type(derived(), startRow, 0, n, cols()); } /** This is the const version of middleRows().*/ template -inline typename ConstNRowsBlockXpr::Type middleRows(Index startRow) const +inline typename ConstNRowsBlockXpr::Type middleRows(Index startRow, Index n = N) const { - return typename ConstNRowsBlockXpr::Type(derived(), startRow, 0, N, cols()); + return typename ConstNRowsBlockXpr::Type(derived(), startRow, 0, n, cols()); } @@ -524,7 +536,11 @@ inline ConstColsBlockXpr leftCols(Index n) const /** \returns a block consisting of the left columns of *this. * - * \tparam N the number of columns in the block + * \tparam N the number of columns in the block as specified at compile-time + * \param n the number of columns in the block as specified at run-time + * + * The compile-time and run-time information should not contradict. In other words, + * \a n should equal \a N unless \a N is \a Dynamic. * * Example: \include MatrixBase_template_int_leftCols.cpp * Output: \verbinclude MatrixBase_template_int_leftCols.out @@ -532,16 +548,16 @@ inline ConstColsBlockXpr leftCols(Index n) const * \sa class Block, block(Index,Index,Index,Index) */ template -inline typename NColsBlockXpr::Type leftCols() +inline typename NColsBlockXpr::Type leftCols(Index n = N) { - return typename NColsBlockXpr::Type(derived(), 0, 0, rows(), N); + return typename NColsBlockXpr::Type(derived(), 0, 0, rows(), n); } /** This is the const version of leftCols().*/ template -inline typename ConstNColsBlockXpr::Type leftCols() const +inline typename ConstNColsBlockXpr::Type leftCols(Index n = N) const { - return typename ConstNColsBlockXpr::Type(derived(), 0, 0, rows(), N); + return typename ConstNColsBlockXpr::Type(derived(), 0, 0, rows(), n); } @@ -568,7 +584,11 @@ inline ConstColsBlockXpr rightCols(Index n) const /** \returns a block consisting of the right columns of *this. * - * \tparam N the number of columns in the block + * \tparam N the number of columns in the block as specified at compile-time + * \param n the number of columns in the block as specified at run-time + * + * The compile-time and run-time information should not contradict. In other words, + * \a n should equal \a N unless \a N is \a Dynamic. * * Example: \include MatrixBase_template_int_rightCols.cpp * Output: \verbinclude MatrixBase_template_int_rightCols.out @@ -576,16 +596,16 @@ inline ConstColsBlockXpr rightCols(Index n) const * \sa class Block, block(Index,Index,Index,Index) */ template -inline typename NColsBlockXpr::Type rightCols() +inline typename NColsBlockXpr::Type rightCols(Index n = N) { - return typename NColsBlockXpr::Type(derived(), 0, cols() - N, rows(), N); + return typename NColsBlockXpr::Type(derived(), 0, cols() - n, rows(), n); } /** This is the const version of rightCols().*/ template -inline typename ConstNColsBlockXpr::Type rightCols() const +inline typename ConstNColsBlockXpr::Type rightCols(Index n = N) const { - return typename ConstNColsBlockXpr::Type(derived(), 0, cols() - N, rows(), N); + return typename ConstNColsBlockXpr::Type(derived(), 0, cols() - n, rows(), n); } @@ -613,8 +633,12 @@ inline ConstColsBlockXpr middleCols(Index startCol, Index numCols) const /** \returns a block consisting of a range of columns of *this. * - * \tparam N the number of columns in the block + * \tparam N the number of columns in the block as specified at compile-time * \param startCol the index of the first column in the block + * \param n the number of columns in the block as specified at run-time + * + * The compile-time and run-time information should not contradict. In other words, + * \a n should equal \a N unless \a N is \a Dynamic. * * Example: \include DenseBase_template_int_middleCols.cpp * Output: \verbinclude DenseBase_template_int_middleCols.out @@ -622,16 +646,16 @@ inline ConstColsBlockXpr middleCols(Index startCol, Index numCols) const * \sa class Block, block(Index,Index,Index,Index) */ template -inline typename NColsBlockXpr::Type middleCols(Index startCol) +inline typename NColsBlockXpr::Type middleCols(Index startCol, Index n = N) { - return typename NColsBlockXpr::Type(derived(), 0, startCol, rows(), N); + return typename NColsBlockXpr::Type(derived(), 0, startCol, rows(), n); } /** This is the const version of middleCols().*/ template -inline typename ConstNColsBlockXpr::Type middleCols(Index startCol) const +inline typename ConstNColsBlockXpr::Type middleCols(Index startCol, Index n = N) const { - return typename ConstNColsBlockXpr::Type(derived(), 0, startCol, rows(), N); + return typename ConstNColsBlockXpr::Type(derived(), 0, startCol, rows(), n); } @@ -667,15 +691,15 @@ inline const Block block(Index startRow, In /** \returns an expression of a block in *this. * - * \tparam BlockRows number of rows in block as specified at compile time - * \tparam BlockCols number of columns in block as specified at compile time + * \tparam BlockRows number of rows in block as specified at compile-time + * \tparam BlockCols number of columns in block as specified at compile-time * \param startRow the first row in the block * \param startCol the first column in the block - * \param blockRows number of rows in block as specified at run time - * \param blockCols number of columns in block as specified at run time + * \param blockRows number of rows in block as specified at run-time + * \param blockCols number of columns in block as specified at run-time * - * This function is mainly useful for blocks where the number of rows is specified at compile time - * and the number of columns is specified at run time, or vice versa. The compile-time and run-time + * This function is mainly useful for blocks where the number of rows is specified at compile-time + * and the number of columns is specified at run-time, or vice versa. The compile-time and run-time * information should not contradict. In other words, \a blockRows should equal \a BlockRows unless * \a BlockRows is \a Dynamic, and the same for the number of columns. * @@ -738,7 +762,7 @@ inline ConstRowXpr row(Index i) const * \only_for_vectors * * \param start the first coefficient in the segment - * \param vecSize the number of coefficients in the segment + * \param n the number of coefficients in the segment * * Example: \include MatrixBase_segment_int_int.cpp * Output: \verbinclude MatrixBase_segment_int_int.out @@ -749,25 +773,25 @@ inline ConstRowXpr row(Index i) const * * \sa class Block, segment(Index) */ -inline SegmentReturnType segment(Index start, Index vecSize) +inline SegmentReturnType segment(Index start, Index n) { EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) - return SegmentReturnType(derived(), start, vecSize); + return SegmentReturnType(derived(), start, n); } /** This is the const version of segment(Index,Index).*/ -inline ConstSegmentReturnType segment(Index start, Index vecSize) const +inline ConstSegmentReturnType segment(Index start, Index n) const { EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) - return ConstSegmentReturnType(derived(), start, vecSize); + return ConstSegmentReturnType(derived(), start, n); } /** \returns a dynamic-size expression of the first coefficients of *this. * * \only_for_vectors * - * \param vecSize the number of coefficients in the block + * \param n the number of coefficients in the segment * * Example: \include MatrixBase_start_int.cpp * Output: \verbinclude MatrixBase_start_int.out @@ -778,25 +802,24 @@ inline ConstSegmentReturnType segment(Index start, Index vecSize) const * * \sa class Block, block(Index,Index) */ -inline SegmentReturnType head(Index vecSize) +inline SegmentReturnType head(Index n) { EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) - return SegmentReturnType(derived(), 0, vecSize); + return SegmentReturnType(derived(), 0, n); } /** This is the const version of head(Index).*/ -inline ConstSegmentReturnType - head(Index vecSize) const +inline ConstSegmentReturnType head(Index n) const { EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) - return ConstSegmentReturnType(derived(), 0, vecSize); + return ConstSegmentReturnType(derived(), 0, n); } /** \returns a dynamic-size expression of the last coefficients of *this. * * \only_for_vectors * - * \param vecSize the number of coefficients in the block + * \param n the number of coefficients in the segment * * Example: \include MatrixBase_end_int.cpp * Output: \verbinclude MatrixBase_end_int.out @@ -807,95 +830,106 @@ inline ConstSegmentReturnType * * \sa class Block, block(Index,Index) */ -inline SegmentReturnType tail(Index vecSize) +inline SegmentReturnType tail(Index n) { EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) - return SegmentReturnType(derived(), this->size() - vecSize, vecSize); + return SegmentReturnType(derived(), this->size() - n, n); } /** This is the const version of tail(Index).*/ -inline ConstSegmentReturnType tail(Index vecSize) const +inline ConstSegmentReturnType tail(Index n) const { EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) - return ConstSegmentReturnType(derived(), this->size() - vecSize, vecSize); + return ConstSegmentReturnType(derived(), this->size() - n, n); } /** \returns a fixed-size expression of a segment (i.e. a vector block) in \c *this * * \only_for_vectors * - * The template parameter \a Size is the number of coefficients in the block + * \tparam N the number of coefficients in the segment as specified at compile-time + * \param start the index of the first element in the segment + * \param n the number of coefficients in the segment as specified at compile-time * - * \param start the index of the first element of the sub-vector + * The compile-time and run-time information should not contradict. In other words, + * \a n should equal \a N unless \a N is \a Dynamic. * * Example: \include MatrixBase_template_int_segment.cpp * Output: \verbinclude MatrixBase_template_int_segment.out * * \sa class Block */ -template -inline typename FixedSegmentReturnType::Type segment(Index start) +template +inline typename FixedSegmentReturnType::Type segment(Index start, Index n = N) { EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) - return typename FixedSegmentReturnType::Type(derived(), start); + return typename FixedSegmentReturnType::Type(derived(), start, n); } /** This is the const version of segment(Index).*/ -template -inline typename ConstFixedSegmentReturnType::Type segment(Index start) const +template +inline typename ConstFixedSegmentReturnType::Type segment(Index start, Index n = N) const { EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) - return typename ConstFixedSegmentReturnType::Type(derived(), start); + return typename ConstFixedSegmentReturnType::Type(derived(), start, n); } /** \returns a fixed-size expression of the first coefficients of *this. * * \only_for_vectors * - * The template parameter \a Size is the number of coefficients in the block + * \tparam N the number of coefficients in the segment as specified at compile-time + * \param n the number of coefficients in the segment as specified at run-time + * + * The compile-time and run-time information should not contradict. In other words, + * \a n should equal \a N unless \a N is \a Dynamic. * * Example: \include MatrixBase_template_int_start.cpp * Output: \verbinclude MatrixBase_template_int_start.out * * \sa class Block */ -template -inline typename FixedSegmentReturnType::Type head() +template +inline typename FixedSegmentReturnType::Type head(Index n = N) { EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) - return typename FixedSegmentReturnType::Type(derived(), 0); + return typename FixedSegmentReturnType::Type(derived(), 0, n); } /** This is the const version of head().*/ -template -inline typename ConstFixedSegmentReturnType::Type head() const +template +inline typename ConstFixedSegmentReturnType::Type head(Index n = N) const { EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) - return typename ConstFixedSegmentReturnType::Type(derived(), 0); + return typename ConstFixedSegmentReturnType::Type(derived(), 0, n); } /** \returns a fixed-size expression of the last coefficients of *this. * * \only_for_vectors * - * The template parameter \a Size is the number of coefficients in the block + * \tparam N the number of coefficients in the segment as specified at compile-time + * \param n the number of coefficients in the segment as specified at run-time + * + * The compile-time and run-time information should not contradict. In other words, + * \a n should equal \a N unless \a N is \a Dynamic. * * Example: \include MatrixBase_template_int_end.cpp * Output: \verbinclude MatrixBase_template_int_end.out * * \sa class Block */ -template -inline typename FixedSegmentReturnType::Type tail() +template +inline typename FixedSegmentReturnType::Type tail(Index n = N) { EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) - return typename FixedSegmentReturnType::Type(derived(), size() - Size); + return typename FixedSegmentReturnType::Type(derived(), size() - n); } /** This is the const version of tail.*/ -template -inline typename ConstFixedSegmentReturnType::Type tail() const +template +inline typename ConstFixedSegmentReturnType::Type tail(Index n = N) const { EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) - return typename ConstFixedSegmentReturnType::Type(derived(), size() - Size); + return typename ConstFixedSegmentReturnType::Type(derived(), size() - n); } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/plugins/MatrixCwiseBinaryOps.h b/gtsam/3rdparty/Eigen/Eigen/src/plugins/MatrixCwiseBinaryOps.h index 3a737df7b..7f62149e0 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/plugins/MatrixCwiseBinaryOps.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/plugins/MatrixCwiseBinaryOps.h @@ -83,7 +83,7 @@ cwiseMin(const EIGEN_CURRENT_STORAGE_BASE_CLASS &other) const EIGEN_STRONG_INLINE const CwiseBinaryOp, const Derived, const ConstantReturnType> cwiseMin(const Scalar &other) const { - return cwiseMin(Derived::PlainObject::Constant(rows(), cols(), other)); + return cwiseMin(Derived::Constant(rows(), cols(), other)); } /** \returns an expression of the coefficient-wise max of *this and \a other @@ -107,7 +107,7 @@ cwiseMax(const EIGEN_CURRENT_STORAGE_BASE_CLASS &other) const EIGEN_STRONG_INLINE const CwiseBinaryOp, const Derived, const ConstantReturnType> cwiseMax(const Scalar &other) const { - return cwiseMax(Derived::PlainObject::Constant(rows(), cols(), other)); + return cwiseMax(Derived::Constant(rows(), cols(), other)); } diff --git a/gtsam/3rdparty/Eigen/blas/CMakeLists.txt b/gtsam/3rdparty/Eigen/blas/CMakeLists.txt index c35a2fdbe..a9bc05137 100644 --- a/gtsam/3rdparty/Eigen/blas/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/blas/CMakeLists.txt @@ -7,6 +7,9 @@ workaround_9220(Fortran EIGEN_Fortran_COMPILER_WORKS) if(EIGEN_Fortran_COMPILER_WORKS) enable_language(Fortran OPTIONAL) + if(NOT CMAKE_Fortran_COMPILER) + set(EIGEN_Fortran_COMPILER_WORKS OFF) + endif() endif() add_custom_target(blas) diff --git a/gtsam/3rdparty/Eigen/cmake/language_support.cmake b/gtsam/3rdparty/Eigen/cmake/language_support.cmake index 2ca303c92..d687b71f6 100644 --- a/gtsam/3rdparty/Eigen/cmake/language_support.cmake +++ b/gtsam/3rdparty/Eigen/cmake/language_support.cmake @@ -23,7 +23,7 @@ function(workaround_9220 language language_works) #message("DEBUG: language = ${language}") set(text "project(test NONE) - cmake_minimum_required(VERSION 2.6.0) + cmake_minimum_required(VERSION 2.8.0) set (CMAKE_Fortran_FLAGS \"${CMAKE_Fortran_FLAGS}\") set (CMAKE_EXE_LINKER_FLAGS \"${CMAKE_EXE_LINKER_FLAGS}\") enable_language(${language} OPTIONAL) diff --git a/gtsam/3rdparty/Eigen/doc/A05_PortingFrom2To3.dox b/gtsam/3rdparty/Eigen/doc/A05_PortingFrom2To3.dox index d885b4f6d..4d5f3ae1f 100644 --- a/gtsam/3rdparty/Eigen/doc/A05_PortingFrom2To3.dox +++ b/gtsam/3rdparty/Eigen/doc/A05_PortingFrom2To3.dox @@ -2,6 +2,8 @@ namespace Eigen { /** \page Eigen2ToEigen3 Porting from Eigen2 to Eigen3 +
Eigen2 support is deprecated in Eigen 3.2.x and it will be removed in Eigen 3.3.
+ This page lists the most important API changes between Eigen2 and Eigen3, and gives tips to help porting your application from Eigen2 to Eigen3. diff --git a/gtsam/3rdparty/Eigen/doc/A10_Eigen2SupportModes.dox b/gtsam/3rdparty/Eigen/doc/A10_Eigen2SupportModes.dox index abfdb4683..f3df91515 100644 --- a/gtsam/3rdparty/Eigen/doc/A10_Eigen2SupportModes.dox +++ b/gtsam/3rdparty/Eigen/doc/A10_Eigen2SupportModes.dox @@ -2,6 +2,8 @@ namespace Eigen { /** \page Eigen2SupportModes Eigen 2 support modes +
Eigen2 support is deprecated in Eigen 3.2.x and it will be removed in Eigen 3.3.
+ This page documents the Eigen2 support modes, a powerful tool to help migrating your project from Eigen 2 to Eigen 3. Don't miss our page on \ref Eigen2ToEigen3 "API changes" between Eigen 2 and Eigen 3. diff --git a/gtsam/3rdparty/Eigen/doc/AsciiQuickReference.txt b/gtsam/3rdparty/Eigen/doc/AsciiQuickReference.txt index 6b0a7cd6a..694d32484 100644 --- a/gtsam/3rdparty/Eigen/doc/AsciiQuickReference.txt +++ b/gtsam/3rdparty/Eigen/doc/AsciiQuickReference.txt @@ -16,8 +16,8 @@ double s; // Basic usage // Eigen // Matlab // comments x.size() // length(x) // vector size -C.rows() // size(C)(1) // number of rows -C.cols() // size(C)(2) // number of columns +C.rows() // size(C,1) // number of rows +C.cols() // size(C,2) // number of columns x(i) // x(i+1) // Matlab is 1-based C(i,j) // C(i+1,j+1) // @@ -51,20 +51,34 @@ v.setLinSpace(size,low,high) // v = linspace(low,high,size)' // Eigen // Matlab x.head(n) // x(1:n) x.head() // x(1:n) -x.tail(n) // N = rows(x); x(N - n: N) -x.tail() // N = rows(x); x(N - n: N) +x.tail(n) // x(end - n + 1: end) +x.tail() // x(end - n + 1: end) x.segment(i, n) // x(i+1 : i+n) x.segment(i) // x(i+1 : i+n) P.block(i, j, rows, cols) // P(i+1 : i+rows, j+1 : j+cols) P.block(i, j) // P(i+1 : i+rows, j+1 : j+cols) +P.row(i) // P(i+1, :) +P.col(j) // P(:, j+1) +P.leftCols() // P(:, 1:cols) +P.leftCols(cols) // P(:, 1:cols) +P.middleCols(j) // P(:, j+1:j+cols) +P.middleCols(j, cols) // P(:, j+1:j+cols) +P.rightCols() // P(:, end-cols+1:end) +P.rightCols(cols) // P(:, end-cols+1:end) +P.topRows() // P(1:rows, :) +P.topRows(rows) // P(1:rows, :) +P.middleRows(i) // P(:, i+1:i+rows) +P.middleRows(i, rows) // P(:, i+1:i+rows) +P.bottomRows() // P(:, end-rows+1:end) +P.bottomRows(rows) // P(:, end-rows+1:end) P.topLeftCorner(rows, cols) // P(1:rows, 1:cols) -P.topRightCorner(rows, cols) // [m n]=size(P); P(1:rows, n-cols+1:n) -P.bottomLeftCorner(rows, cols) // [m n]=size(P); P(m-rows+1:m, 1:cols) -P.bottomRightCorner(rows, cols) // [m n]=size(P); P(m-rows+1:m, n-cols+1:n) +P.topRightCorner(rows, cols) // P(1:rows, end-cols+1:end) +P.bottomLeftCorner(rows, cols) // P(end-rows+1:end, 1:cols) +P.bottomRightCorner(rows, cols) // P(end-rows+1:end, end-cols+1:end) P.topLeftCorner() // P(1:rows, 1:cols) -P.topRightCorner() // [m n]=size(P); P(1:rows, n-cols+1:n) -P.bottomLeftCorner() // [m n]=size(P); P(m-rows+1:m, 1:cols) -P.bottomRightCorner() // [m n]=size(P); P(m-rows+1:m, n-cols+1:n) +P.topRightCorner() // P(1:rows, end-cols+1:end) +P.bottomLeftCorner() // P(end-rows+1:end, 1:cols) +P.bottomRightCorner() // P(end-rows+1:end, end-cols+1:end) // Of particular note is Eigen's swap function which is highly optimized. // Eigen // Matlab @@ -125,10 +139,8 @@ int r, c; // Eigen // Matlab R.minCoeff() // min(R(:)) R.maxCoeff() // max(R(:)) -s = R.minCoeff(&r, &c) // [aa, bb] = min(R); [cc, dd] = min(aa); - // r = bb(dd); c = dd; s = cc -s = R.maxCoeff(&r, &c) // [aa, bb] = max(R); [cc, dd] = max(aa); - // row = bb(dd); col = dd; s = cc +s = R.minCoeff(&r, &c) // [s, i] = min(R(:)); [r, c] = ind2sub(size(R), i); +s = R.maxCoeff(&r, &c) // [s, i] = max(R(:)); [r, c] = ind2sub(size(R), i); R.sum() // sum(R(:)) R.colwise().sum() // sum(R) R.rowwise().sum() // sum(R, 2) or sum(R')' @@ -150,13 +162,25 @@ x.squaredNorm() // dot(x, x) Note the equivalence is not true for co x.dot(y) // dot(x, y) x.cross(y) // cross(x, y) Requires #include +//// Type conversion +// Eigen // Matlab +A.cast(); // double(A) +A.cast(); // single(A) +A.cast(); // int32(A) +// if the original type equals destination type, no work is done + +// Note that for most operations Eigen requires all operands to have the same type: +MatrixXf F = MatrixXf::Zero(3,3); +A += F; // illegal in Eigen. In Matlab A = A+F is allowed +A += F.cast(); // F converted to double and then added (generally, conversion happens on-the-fly) + // Eigen can map existing memory into Eigen matrices. float array[3]; -Map(array, 3).fill(10); -int data[4] = 1, 2, 3, 4; -Matrix2i mat2x2(data); -MatrixXi mat2x2 = Map(data); -MatrixXi mat2x2 = Map(data, 2, 2); +Vector3f::Map(array).fill(10); // create a temporary Map over array and sets entries to 10 +int data[4] = {1, 2, 3, 4}; +Matrix2i mat2x2(data); // copies data into mat2x2 +Matrix2i::Map(data) = 2*mat2x2; // overwrite elements of data with 2*mat2x2 +MatrixXi::Map(data, 2, 2) += mat2x2; // adds mat2x2 to elements of data (alternative syntax if size is not know at compile time) // Solve Ax = b. Result stored in x. Matlab: x = A \ b. x = A.ldlt().solve(b)); // A sym. p.s.d. #include diff --git a/gtsam/3rdparty/Eigen/doc/PreprocessorDirectives.dox b/gtsam/3rdparty/Eigen/doc/PreprocessorDirectives.dox index eedd5524a..981083e96 100644 --- a/gtsam/3rdparty/Eigen/doc/PreprocessorDirectives.dox +++ b/gtsam/3rdparty/Eigen/doc/PreprocessorDirectives.dox @@ -64,9 +64,9 @@ run time. However, these assertions do cost time and can thus be turned off. \c EIGEN_DONT_ALIGN is defined. - \b EIGEN_DONT_VECTORIZE - disables explicit vectorization when defined. Not defined by default, unless alignment is disabled by %Eigen's platform test or the user defining \c EIGEN_DONT_ALIGN. - - \b EIGEN_FAST_MATH - enables some optimizations which might affect the accuracy of the result. The only - optimization this currently includes is single precision sin() and cos() in the present of SSE - vectorization. Defined by default. + - \b EIGEN_FAST_MATH - enables some optimizations which might affect the accuracy of the result. This currently + enables the SSE vectorization of sin() and cos(), and speedups sqrt() for single precision. Defined to 1 by default. + Define it to 0 to disable. - \b EIGEN_UNROLLING_LIMIT - defines the size of a loop to enable meta unrolling. Set it to zero to disable unrolling. The size of a loop here is expressed in %Eigen's own notion of "number of FLOPS", it does not correspond to the number of iterations or the number of instructions. The default is value 100. diff --git a/gtsam/3rdparty/Eigen/doc/QuickReference.dox b/gtsam/3rdparty/Eigen/doc/QuickReference.dox index 5e0168649..a4be0f68a 100644 --- a/gtsam/3rdparty/Eigen/doc/QuickReference.dox +++ b/gtsam/3rdparty/Eigen/doc/QuickReference.dox @@ -405,19 +405,19 @@ array1 == array2 array1 != array2 array1 == scalar array1 != scalar array1.min(array2) array1.max(array2) array1.abs2() -array1.abs() std::abs(array1) -array1.sqrt() std::sqrt(array1) -array1.log() std::log(array1) -array1.exp() std::exp(array1) -array1.pow(exponent) std::pow(array1,exponent) +array1.abs() abs(array1) +array1.sqrt() sqrt(array1) +array1.log() log(array1) +array1.exp() exp(array1) +array1.pow(exponent) pow(array1,exponent) array1.square() array1.cube() array1.inverse() -array1.sin() std::sin(array1) -array1.cos() std::cos(array1) -array1.tan() std::tan(array1) -array1.asin() std::asin(array1) -array1.acos() std::acos(array1) +array1.sin() sin(array1) +array1.cos() cos(array1) +array1.tan() tan(array1) +array1.asin() asin(array1) +array1.acos() acos(array1) \endcode diff --git a/gtsam/3rdparty/Eigen/doc/TutorialGeometry.dox b/gtsam/3rdparty/Eigen/doc/TutorialGeometry.dox index 81aeec978..372a275de 100644 --- a/gtsam/3rdparty/Eigen/doc/TutorialGeometry.dox +++ b/gtsam/3rdparty/Eigen/doc/TutorialGeometry.dox @@ -127,7 +127,7 @@ VectorNf vec1, vec2; vec2 = t.linear() * vec1;\endcode Apply a \em general transformation \n to a \b normal \b vector -(explanations)\code +(explanations)\code VectorNf n1, n2; MatrixNf normalMatrix = t.linear().inverse().transpose(); n2 = (normalMatrix * n1).normalized();\endcode diff --git a/gtsam/3rdparty/Eigen/doc/TutorialSparse.dox b/gtsam/3rdparty/Eigen/doc/TutorialSparse.dox index dbfb4a9eb..835c59354 100644 --- a/gtsam/3rdparty/Eigen/doc/TutorialSparse.dox +++ b/gtsam/3rdparty/Eigen/doc/TutorialSparse.dox @@ -83,7 +83,7 @@ There is no notion of compressed/uncompressed mode for a SparseVector. \section TutorialSparseExample First example -Before describing each individual class, let's start with the following typical example: solving the Lapace equation \f$ \nabla u = 0 \f$ on a regular 2D grid using a finite difference scheme and Dirichlet boundary conditions. +Before describing each individual class, let's start with the following typical example: solving the Laplace equation \f$ \nabla u = 0 \f$ on a regular 2D grid using a finite difference scheme and Dirichlet boundary conditions. Such problem can be mathematically expressed as a linear problem of the form \f$ Ax=b \f$ where \f$ x \f$ is the vector of \c m unknowns (in our case, the values of the pixels), \f$ b \f$ is the right hand side vector resulting from the boundary conditions, and \f$ A \f$ is an \f$ m \times m \f$ matrix containing only a few non-zero elements resulting from the discretization of the Laplacian operator. diff --git a/gtsam/3rdparty/Eigen/doc/eigendoxy.css b/gtsam/3rdparty/Eigen/doc/eigendoxy.css index 60243d870..efaeb46dc 100644 --- a/gtsam/3rdparty/Eigen/doc/eigendoxy.css +++ b/gtsam/3rdparty/Eigen/doc/eigendoxy.css @@ -199,3 +199,13 @@ h3.version { td.width20em p.endtd { width: 20em; } + +.bigwarning { + font-size:2em; + font-weight:bold; + margin:1em; + padding:1em; + color:red; + border:solid; +} + diff --git a/gtsam/3rdparty/Eigen/doc/eigendoxy_footer.html.in b/gtsam/3rdparty/Eigen/doc/eigendoxy_footer.html.in index e62af8ed5..878244a19 100644 --- a/gtsam/3rdparty/Eigen/doc/eigendoxy_footer.html.in +++ b/gtsam/3rdparty/Eigen/doc/eigendoxy_footer.html.in @@ -17,8 +17,7 @@ $generatedby   - - ---> diff --git a/gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_applyOnTheLeft.cpp b/gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_applyOnTheLeft.cpp new file mode 100644 index 000000000..6398c873a --- /dev/null +++ b/gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_applyOnTheLeft.cpp @@ -0,0 +1,7 @@ +Matrix3f A = Matrix3f::Random(3,3), B; +B << 0,1,0, + 0,0,1, + 1,0,0; +cout << "At start, A = " << endl << A << endl; +A.applyOnTheLeft(B); +cout << "After applyOnTheLeft, A = " << endl << A << endl; diff --git a/gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_applyOnTheRight.cpp b/gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_applyOnTheRight.cpp new file mode 100644 index 000000000..e4b71b2d8 --- /dev/null +++ b/gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_applyOnTheRight.cpp @@ -0,0 +1,9 @@ +Matrix3f A = Matrix3f::Random(3,3), B; +B << 0,1,0, + 0,0,1, + 1,0,0; +cout << "At start, A = " << endl << A << endl; +A *= B; +cout << "After A *= B, A = " << endl << A << endl; +A.applyOnTheRight(B); // equivalent to A *= B +cout << "After applyOnTheRight, A = " << endl << A << endl; diff --git a/gtsam/3rdparty/Eigen/doc/special_examples/CMakeLists.txt b/gtsam/3rdparty/Eigen/doc/special_examples/CMakeLists.txt index 138a2f6ef..0c9b3c3ba 100644 --- a/gtsam/3rdparty/Eigen/doc/special_examples/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/doc/special_examples/CMakeLists.txt @@ -10,12 +10,12 @@ endif(NOT EIGEN_TEST_NOQT) if(QT4_FOUND) add_executable(Tutorial_sparse_example Tutorial_sparse_example.cpp Tutorial_sparse_example_details.cpp) target_link_libraries(Tutorial_sparse_example ${EIGEN_STANDARD_LIBRARIES_TO_LINK_TO} ${QT_QTCORE_LIBRARY} ${QT_QTGUI_LIBRARY}) - + add_custom_command( TARGET Tutorial_sparse_example POST_BUILD - COMMAND Tutorial_sparse_example - ARGS ${CMAKE_CURRENT_BINARY_DIR}/../html/Tutorial_sparse_example.jpeg + COMMAND Tutorial_sparse_example ARGS ${CMAKE_CURRENT_BINARY_DIR}/../html/Tutorial_sparse_example.jpeg ) + add_dependencies(all_examples Tutorial_sparse_example) endif(QT4_FOUND) diff --git a/gtsam/3rdparty/Eigen/doc/special_examples/Tutorial_sparse_example_details.cpp b/gtsam/3rdparty/Eigen/doc/special_examples/Tutorial_sparse_example_details.cpp index 8c3020b63..7d820b44a 100644 --- a/gtsam/3rdparty/Eigen/doc/special_examples/Tutorial_sparse_example_details.cpp +++ b/gtsam/3rdparty/Eigen/doc/special_examples/Tutorial_sparse_example_details.cpp @@ -11,8 +11,8 @@ void insertCoefficient(int id, int i, int j, double w, std::vector& coeffs, int n = boundary.size(); int id1 = i+j*n; - if(i==-1 || i==n) b(id) -= w * boundary(j); // constrained coeffcieint - else if(j==-1 || j==n) b(id) -= w * boundary(i); // constrained coeffcieint + if(i==-1 || i==n) b(id) -= w * boundary(j); // constrained coefficient + else if(j==-1 || j==n) b(id) -= w * boundary(i); // constrained coefficient else coeffs.push_back(T(id,id1,w)); // unknown coefficient } diff --git a/gtsam/3rdparty/Eigen/lapack/CMakeLists.txt b/gtsam/3rdparty/Eigen/lapack/CMakeLists.txt index 7e7444326..9883d4c72 100644 --- a/gtsam/3rdparty/Eigen/lapack/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/lapack/CMakeLists.txt @@ -7,6 +7,9 @@ workaround_9220(Fortran EIGEN_Fortran_COMPILER_WORKS) if(EIGEN_Fortran_COMPILER_WORKS) enable_language(Fortran OPTIONAL) + if(NOT CMAKE_Fortran_COMPILER) + set(EIGEN_Fortran_COMPILER_WORKS OFF) + endif() endif() add_custom_target(lapack) diff --git a/gtsam/3rdparty/Eigen/scripts/eigen_gen_docs b/gtsam/3rdparty/Eigen/scripts/eigen_gen_docs index 9b71cd8e0..0e6f9ada2 100644 --- a/gtsam/3rdparty/Eigen/scripts/eigen_gen_docs +++ b/gtsam/3rdparty/Eigen/scripts/eigen_gen_docs @@ -4,6 +4,7 @@ # You should call this script with USER set as you want, else some default # will be used USER=${USER:-'orzel'} +UPLOAD_DIR=dox #ulimit -v 1024000 @@ -14,10 +15,10 @@ mkdir build -p #step 2 : upload # (the '/' at the end of path is very important, see rsync documentation) -rsync -az --no-p --delete build/doc/html/ $USER@ssh.tuxfamily.org:eigen/eigen.tuxfamily.org-web/htdocs/dox-devel/ || { echo "upload failed"; exit 1; } +rsync -az --no-p --delete build/doc/html/ $USER@ssh.tuxfamily.org:eigen/eigen.tuxfamily.org-web/htdocs/$UPLOAD_DIR/ || { echo "upload failed"; exit 1; } #step 3 : fix the perm -ssh $USER@ssh.tuxfamily.org 'chmod -R g+w /home/eigen/eigen.tuxfamily.org-web/htdocs/dox-devel' || { echo "perm failed"; exit 1; } +ssh $USER@ssh.tuxfamily.org "chmod -R g+w /home/eigen/eigen.tuxfamily.org-web/htdocs/$UPLOAD_DIR" || { echo "perm failed"; exit 1; } echo "Uploaded successfully" diff --git a/gtsam/3rdparty/Eigen/test/array.cpp b/gtsam/3rdparty/Eigen/test/array.cpp index 8960e49f8..c607da631 100644 --- a/gtsam/3rdparty/Eigen/test/array.cpp +++ b/gtsam/3rdparty/Eigen/test/array.cpp @@ -167,21 +167,14 @@ template void array_real(const ArrayType& m) Scalar s1 = internal::random(); // these tests are mostly to check possible compilation issues. -// VERIFY_IS_APPROX(m1.sin(), std::sin(m1)); VERIFY_IS_APPROX(m1.sin(), sin(m1)); -// VERIFY_IS_APPROX(m1.cos(), std::cos(m1)); VERIFY_IS_APPROX(m1.cos(), cos(m1)); -// VERIFY_IS_APPROX(m1.asin(), std::asin(m1)); VERIFY_IS_APPROX(m1.asin(), asin(m1)); -// VERIFY_IS_APPROX(m1.acos(), std::acos(m1)); VERIFY_IS_APPROX(m1.acos(), acos(m1)); -// VERIFY_IS_APPROX(m1.tan(), std::tan(m1)); VERIFY_IS_APPROX(m1.tan(), tan(m1)); VERIFY_IS_APPROX(cos(m1+RealScalar(3)*m2), cos((m1+RealScalar(3)*m2).eval())); -// VERIFY_IS_APPROX(std::cos(m1+RealScalar(3)*m2), std::cos((m1+RealScalar(3)*m2).eval())); -// VERIFY_IS_APPROX(m1.abs().sqrt(), std::sqrt(std::abs(m1))); VERIFY_IS_APPROX(m1.abs().sqrt(), sqrt(abs(m1))); VERIFY_IS_APPROX(m1.abs(), sqrt(numext::abs2(m1))); @@ -190,9 +183,10 @@ template void array_real(const ArrayType& m) if(!NumTraits::IsComplex) VERIFY_IS_APPROX(numext::real(m1), m1); - VERIFY_IS_APPROX(m1.abs().log() , log(abs(m1))); + // shift argument of logarithm so that it is not zero + Scalar smallNumber = NumTraits::dummy_precision(); + VERIFY_IS_APPROX((m1.abs() + smallNumber).log() , log(abs(m1) + smallNumber)); -// VERIFY_IS_APPROX(m1.exp(), std::exp(m1)); VERIFY_IS_APPROX(m1.exp() * m2.exp(), exp(m1+m2)); VERIFY_IS_APPROX(m1.exp(), exp(m1)); VERIFY_IS_APPROX(m1.exp() / m2.exp(),(m1-m2).exp()); @@ -236,7 +230,6 @@ template void array_complex(const ArrayType& m) m2(i,j) = sqrt(m1(i,j)); VERIFY_IS_APPROX(m1.sqrt(), m2); -// VERIFY_IS_APPROX(m1.sqrt(), std::sqrt(m1)); VERIFY_IS_APPROX(m1.sqrt(), Eigen::sqrt(m1)); } diff --git a/gtsam/3rdparty/Eigen/test/array_for_matrix.cpp b/gtsam/3rdparty/Eigen/test/array_for_matrix.cpp index 1250c9ff5..9a50f99ab 100644 --- a/gtsam/3rdparty/Eigen/test/array_for_matrix.cpp +++ b/gtsam/3rdparty/Eigen/test/array_for_matrix.cpp @@ -163,10 +163,14 @@ template void cwise_min_max(const MatrixType& m) // min/max with scalar input VERIFY_IS_APPROX(MatrixType::Constant(rows,cols, minM1), m1.cwiseMin( minM1)); - VERIFY_IS_APPROX(m1, m1.cwiseMin( maxM1)); + VERIFY_IS_APPROX(m1, m1.cwiseMin(maxM1)); + VERIFY_IS_APPROX(-m1, (-m1).cwiseMin(-minM1)); + VERIFY_IS_APPROX(-m1.array(), ((-m1).array().min)( -minM1)); VERIFY_IS_APPROX(MatrixType::Constant(rows,cols, maxM1), m1.cwiseMax( maxM1)); - VERIFY_IS_APPROX(m1, m1.cwiseMax( minM1)); + VERIFY_IS_APPROX(m1, m1.cwiseMax(minM1)); + VERIFY_IS_APPROX(-m1, (-m1).cwiseMax(-maxM1)); + VERIFY_IS_APPROX(-m1.array(), ((-m1).array().max)(-maxM1)); VERIFY_IS_APPROX(MatrixType::Constant(rows,cols, minM1).array(), (m1.array().min)( minM1)); VERIFY_IS_APPROX(m1.array(), (m1.array().min)( maxM1)); @@ -202,6 +206,12 @@ template void resize(const MatrixTraits& t) VERIFY(a1.size()==cols); } +void regression_bug_654() +{ + ArrayXf a = RowVectorXf(3); + VectorXf v = Array(3); +} + void test_array_for_matrix() { for(int i = 0; i < g_repeat; i++) { @@ -239,4 +249,5 @@ void test_array_for_matrix() CALL_SUBTEST_5( resize(MatrixXf(internal::random(1,EIGEN_TEST_MAX_SIZE), internal::random(1,EIGEN_TEST_MAX_SIZE))) ); CALL_SUBTEST_6( resize(MatrixXi(internal::random(1,EIGEN_TEST_MAX_SIZE), internal::random(1,EIGEN_TEST_MAX_SIZE))) ); } + CALL_SUBTEST_6( regression_bug_654() ); } diff --git a/gtsam/3rdparty/Eigen/test/cholesky.cpp b/gtsam/3rdparty/Eigen/test/cholesky.cpp index 378525a83..b980dc572 100644 --- a/gtsam/3rdparty/Eigen/test/cholesky.cpp +++ b/gtsam/3rdparty/Eigen/test/cholesky.cpp @@ -263,8 +263,8 @@ template void cholesky_bug241(const MatrixType& m) // LDLT is not guaranteed to work for indefinite matrices, but happens to work fine if matrix is diagonal. // This test checks that LDLT reports correctly that matrix is indefinite. -// See http://forum.kde.org/viewtopic.php?f=74&t=106942 -template void cholesky_indefinite(const MatrixType& m) +// See http://forum.kde.org/viewtopic.php?f=74&t=106942 and bug 736 +template void cholesky_definiteness(const MatrixType& m) { eigen_assert(m.rows() == 2 && m.cols() == 2); MatrixType mat; @@ -280,6 +280,24 @@ template void cholesky_indefinite(const MatrixType& m) VERIFY(!ldlt.isNegative()); VERIFY(!ldlt.isPositive()); } + { + mat << 0, 0, 0, 0; + LDLT ldlt(mat); + VERIFY(ldlt.isNegative()); + VERIFY(ldlt.isPositive()); + } + { + mat << 0, 0, 0, 1; + LDLT ldlt(mat); + VERIFY(!ldlt.isNegative()); + VERIFY(ldlt.isPositive()); + } + { + mat << -1, 0, 0, 0; + LDLT ldlt(mat); + VERIFY(ldlt.isNegative()); + VERIFY(!ldlt.isPositive()); + } } template void cholesky_verify_assert() @@ -309,7 +327,7 @@ void test_cholesky() CALL_SUBTEST_1( cholesky(Matrix()) ); CALL_SUBTEST_3( cholesky(Matrix2d()) ); CALL_SUBTEST_3( cholesky_bug241(Matrix2d()) ); - CALL_SUBTEST_3( cholesky_indefinite(Matrix2d()) ); + CALL_SUBTEST_3( cholesky_definiteness(Matrix2d()) ); CALL_SUBTEST_4( cholesky(Matrix3f()) ); CALL_SUBTEST_5( cholesky(Matrix4d()) ); s = internal::random(1,EIGEN_TEST_MAX_SIZE); diff --git a/gtsam/3rdparty/Eigen/test/conservative_resize.cpp b/gtsam/3rdparty/Eigen/test/conservative_resize.cpp index 2d1ab3f03..498421b4c 100644 --- a/gtsam/3rdparty/Eigen/test/conservative_resize.cpp +++ b/gtsam/3rdparty/Eigen/test/conservative_resize.cpp @@ -60,34 +60,51 @@ void run_matrix_tests() template void run_vector_tests() { - typedef Matrix MatrixType; + typedef Matrix VectorType; - MatrixType m, n; + VectorType m, n; // boundary cases ... - m = n = MatrixType::Random(50); + m = n = VectorType::Random(50); m.conservativeResize(1); VERIFY_IS_APPROX(m, n.segment(0,1)); - m = n = MatrixType::Random(50); + m = n = VectorType::Random(50); m.conservativeResize(50); VERIFY_IS_APPROX(m, n.segment(0,50)); + + m = n = VectorType::Random(50); + m.conservativeResize(m.rows(),1); + VERIFY_IS_APPROX(m, n.segment(0,1)); + + m = n = VectorType::Random(50); + m.conservativeResize(m.rows(),50); + VERIFY_IS_APPROX(m, n.segment(0,50)); // random shrinking ... for (int i=0; i<50; ++i) { const int size = internal::random(1,50); - m = n = MatrixType::Random(50); + m = n = VectorType::Random(50); m.conservativeResize(size); VERIFY_IS_APPROX(m, n.segment(0,size)); + + m = n = VectorType::Random(50); + m.conservativeResize(m.rows(), size); + VERIFY_IS_APPROX(m, n.segment(0,size)); } // random growing with zeroing ... for (int i=0; i<50; ++i) { const int size = internal::random(50,100); - m = n = MatrixType::Random(50); - m.conservativeResizeLike(MatrixType::Zero(size)); + m = n = VectorType::Random(50); + m.conservativeResizeLike(VectorType::Zero(size)); + VERIFY_IS_APPROX(m.segment(0,50), n); + VERIFY( size<=50 || m.segment(50,size-50).sum() == Scalar(0) ); + + m = n = VectorType::Random(50); + m.conservativeResizeLike(Matrix::Zero(1,size)); VERIFY_IS_APPROX(m.segment(0,50), n); VERIFY( size<=50 || m.segment(50,size-50).sum() == Scalar(0) ); } diff --git a/gtsam/3rdparty/Eigen/test/geo_eulerangles.cpp b/gtsam/3rdparty/Eigen/test/geo_eulerangles.cpp index 5445cd81a..b4830bd41 100644 --- a/gtsam/3rdparty/Eigen/test/geo_eulerangles.cpp +++ b/gtsam/3rdparty/Eigen/test/geo_eulerangles.cpp @@ -12,36 +12,48 @@ #include #include -template void check_all_var(const Matrix& ea) + +template +void verify_euler(const Matrix& ea, int i, int j, int k) { typedef Matrix Matrix3; typedef Matrix Vector3; typedef AngleAxis AngleAxisx; using std::abs; + Matrix3 m(AngleAxisx(ea[0], Vector3::Unit(i)) * AngleAxisx(ea[1], Vector3::Unit(j)) * AngleAxisx(ea[2], Vector3::Unit(k))); + Vector3 eabis = m.eulerAngles(i, j, k); + Matrix3 mbis(AngleAxisx(eabis[0], Vector3::Unit(i)) * AngleAxisx(eabis[1], Vector3::Unit(j)) * AngleAxisx(eabis[2], Vector3::Unit(k))); + VERIFY_IS_APPROX(m, mbis); + /* If I==K, and ea[1]==0, then there no unique solution. */ + /* The remark apply in the case where I!=K, and |ea[1]| is close to pi/2. */ + if( (i!=k || ea[1]!=0) && (i==k || !internal::isApprox(abs(ea[1]),Scalar(M_PI/2),test_precision())) ) + VERIFY((ea-eabis).norm() <= test_precision()); - #define VERIFY_EULER(I,J,K, X,Y,Z) { \ - Matrix3 m(AngleAxisx(ea[0], Vector3::Unit##X()) * AngleAxisx(ea[1], Vector3::Unit##Y()) * AngleAxisx(ea[2], Vector3::Unit##Z())); \ - Vector3 eabis = m.eulerAngles(I,J,K); \ - Matrix3 mbis(AngleAxisx(eabis[0], Vector3::Unit##X()) * AngleAxisx(eabis[1], Vector3::Unit##Y()) * AngleAxisx(eabis[2], Vector3::Unit##Z())); \ - VERIFY_IS_APPROX(m, mbis); \ - /* If I==K, and ea[1]==0, then there no unique solution. */ \ - /* The remark apply in the case where I!=K, and |ea[1]| is close to pi/2. */ \ - if( (I!=K || ea[1]!=0) && (I==K || !internal::isApprox(abs(ea[1]),Scalar(M_PI/2),test_precision())) ) VERIFY((ea-eabis).norm() <= test_precision()); \ - } - VERIFY_EULER(0,1,2, X,Y,Z); - VERIFY_EULER(0,1,0, X,Y,X); - VERIFY_EULER(0,2,1, X,Z,Y); - VERIFY_EULER(0,2,0, X,Z,X); + // approx_or_less_than does not work for 0 + VERIFY(0 < eabis[0] || test_isMuchSmallerThan(eabis[0], Scalar(1))); + VERIFY_IS_APPROX_OR_LESS_THAN(eabis[0], Scalar(M_PI)); + VERIFY_IS_APPROX_OR_LESS_THAN(-Scalar(M_PI), eabis[1]); + VERIFY_IS_APPROX_OR_LESS_THAN(eabis[1], Scalar(M_PI)); + VERIFY_IS_APPROX_OR_LESS_THAN(-Scalar(M_PI), eabis[2]); + VERIFY_IS_APPROX_OR_LESS_THAN(eabis[2], Scalar(M_PI)); +} - VERIFY_EULER(1,2,0, Y,Z,X); - VERIFY_EULER(1,2,1, Y,Z,Y); - VERIFY_EULER(1,0,2, Y,X,Z); - VERIFY_EULER(1,0,1, Y,X,Y); +template void check_all_var(const Matrix& ea) +{ + verify_euler(ea, 0,1,2); + verify_euler(ea, 0,1,0); + verify_euler(ea, 0,2,1); + verify_euler(ea, 0,2,0); - VERIFY_EULER(2,0,1, Z,X,Y); - VERIFY_EULER(2,0,2, Z,X,Z); - VERIFY_EULER(2,1,0, Z,Y,X); - VERIFY_EULER(2,1,2, Z,Y,Z); + verify_euler(ea, 1,2,0); + verify_euler(ea, 1,2,1); + verify_euler(ea, 1,0,2); + verify_euler(ea, 1,0,1); + + verify_euler(ea, 2,0,1); + verify_euler(ea, 2,0,2); + verify_euler(ea, 2,1,0); + verify_euler(ea, 2,1,2); } template void eulerangles() @@ -63,7 +75,16 @@ template void eulerangles() ea = m.eulerAngles(0,1,0); check_all_var(ea); - ea = (Array3::Random() + Array3(1,1,0))*Scalar(M_PI)*Array3(0.5,0.5,1); + // Check with purely random Quaternion: + q1.coeffs() = Quaternionx::Coefficients::Random().normalized(); + m = q1; + ea = m.eulerAngles(0,1,2); + check_all_var(ea); + ea = m.eulerAngles(0,1,0); + check_all_var(ea); + + // Check with random angles in range [0:pi]x[-pi:pi]x[-pi:pi]. + ea = (Array3::Random() + Array3(1,0,0))*Scalar(M_PI)*Array3(0.5,1,1); check_all_var(ea); ea[2] = ea[0] = internal::random(0,Scalar(M_PI)); diff --git a/gtsam/3rdparty/Eigen/test/geo_transformations.cpp b/gtsam/3rdparty/Eigen/test/geo_transformations.cpp index 35ae67ebe..ee3030b5d 100644 --- a/gtsam/3rdparty/Eigen/test/geo_transformations.cpp +++ b/gtsam/3rdparty/Eigen/test/geo_transformations.cpp @@ -279,6 +279,13 @@ template void transformations() t1 = Eigen::Scaling(s0,s0,s0) * t1; VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); + t0 = t3; + t0.scale(s0); + t1 = t3 * Eigen::Scaling(s0); + VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); + t0.prescale(s0); + t1 = Eigen::Scaling(s0) * t1; + VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); t0.setIdentity(); t0.prerotate(q1).prescale(v0).pretranslate(v0); diff --git a/gtsam/3rdparty/Eigen/test/product_trmm.cpp b/gtsam/3rdparty/Eigen/test/product_trmm.cpp index 506a1aeb9..d715b9a36 100644 --- a/gtsam/3rdparty/Eigen/test/product_trmm.cpp +++ b/gtsam/3rdparty/Eigen/test/product_trmm.cpp @@ -51,6 +51,7 @@ void trmm(int rows=internal::random(1,EIGEN_TEST_MAX_SIZE), ge_xs_save = ge_xs; VERIFY_IS_APPROX( (ge_xs_save + s1*triTr.conjugate() * (s2*ge_left.adjoint())).eval(), ge_xs.noalias() += (s1*mat.adjoint()).template triangularView() * (s2*ge_left.adjoint()) ); + ge_sx.setRandom(); ge_sx_save = ge_sx; VERIFY_IS_APPROX( ge_sx_save - (ge_right.adjoint() * (-s1 * triTr).conjugate()).eval(), ge_sx.noalias() -= (ge_right.adjoint() * (-s1 * mat).adjoint().template triangularView()).eval()); diff --git a/gtsam/3rdparty/Eigen/test/qr_fullpivoting.cpp b/gtsam/3rdparty/Eigen/test/qr_fullpivoting.cpp index 15d7299d7..511f2473f 100644 --- a/gtsam/3rdparty/Eigen/test/qr_fullpivoting.cpp +++ b/gtsam/3rdparty/Eigen/test/qr_fullpivoting.cpp @@ -130,4 +130,8 @@ void test_qr_fullpivoting() // Test problem size constructors CALL_SUBTEST_7(FullPivHouseholderQR(10, 20)); + CALL_SUBTEST_7((FullPivHouseholderQR >(10,20))); + CALL_SUBTEST_7((FullPivHouseholderQR >(Matrix::Random()))); + CALL_SUBTEST_7((FullPivHouseholderQR >(20,10))); + CALL_SUBTEST_7((FullPivHouseholderQR >(Matrix::Random()))); } diff --git a/gtsam/3rdparty/Eigen/test/ref.cpp b/gtsam/3rdparty/Eigen/test/ref.cpp index 65b4f5a3e..f639d900b 100644 --- a/gtsam/3rdparty/Eigen/test/ref.cpp +++ b/gtsam/3rdparty/Eigen/test/ref.cpp @@ -14,9 +14,9 @@ static int nb_temporaries; -inline void on_temporary_creation(int size) { +inline void on_temporary_creation(int) { // here's a great place to set a breakpoint when debugging failures in this test! - if(size!=0) nb_temporaries++; + nb_temporaries++; } diff --git a/gtsam/3rdparty/Eigen/test/sparse.h b/gtsam/3rdparty/Eigen/test/sparse.h index a09c65e5f..e19a76316 100644 --- a/gtsam/3rdparty/Eigen/test/sparse.h +++ b/gtsam/3rdparty/Eigen/test/sparse.h @@ -154,16 +154,16 @@ initSparse(double density, sparseMat.finalize(); } -template void +template void initSparse(double density, Matrix& refVec, - SparseVector& sparseVec, + SparseVector& sparseVec, std::vector* zeroCoords = 0, std::vector* nonzeroCoords = 0) { sparseVec.reserve(int(refVec.size()*density)); sparseVec.setZero(); - for(int i=0; i(0,1) < density) ? internal::random() : Scalar(0); if (v!=Scalar(0)) @@ -178,10 +178,10 @@ initSparse(double density, } } -template void +template void initSparse(double density, Matrix& refVec, - SparseVector& sparseVec, + SparseVector& sparseVec, std::vector* zeroCoords = 0, std::vector* nonzeroCoords = 0) { diff --git a/gtsam/3rdparty/Eigen/test/sparse_product.cpp b/gtsam/3rdparty/Eigen/test/sparse_product.cpp index 664e33887..a2ea9d5b7 100644 --- a/gtsam/3rdparty/Eigen/test/sparse_product.cpp +++ b/gtsam/3rdparty/Eigen/test/sparse_product.cpp @@ -13,8 +13,9 @@ template struct test_outer { static void run(SparseMatrixType& m2, SparseMatrixType& m4, DenseMatrix& refMat2, DenseMatrix& refMat4) { - int c = internal::random(0,m2.cols()-1); - int c1 = internal::random(0,m2.cols()-1); + typedef typename SparseMatrixType::Index Index; + Index c = internal::random(0,m2.cols()-1); + Index c1 = internal::random(0,m2.cols()-1); VERIFY_IS_APPROX(m4=m2.col(c)*refMat2.col(c1).transpose(), refMat4=refMat2.col(c)*refMat2.col(c1).transpose()); VERIFY_IS_APPROX(m4=refMat2.col(c1)*m2.col(c).transpose(), refMat4=refMat2.col(c1)*refMat2.col(c).transpose()); } @@ -22,8 +23,9 @@ template struct test_outer struct test_outer { static void run(SparseMatrixType& m2, SparseMatrixType& m4, DenseMatrix& refMat2, DenseMatrix& refMat4) { - int r = internal::random(0,m2.rows()-1); - int c1 = internal::random(0,m2.cols()-1); + typedef typename SparseMatrixType::Index Index; + Index r = internal::random(0,m2.rows()-1); + Index c1 = internal::random(0,m2.cols()-1); VERIFY_IS_APPROX(m4=m2.row(r).transpose()*refMat2.col(c1).transpose(), refMat4=refMat2.row(r).transpose()*refMat2.col(c1).transpose()); VERIFY_IS_APPROX(m4=refMat2.col(c1)*m2.row(r), refMat4=refMat2.col(c1)*refMat2.row(r)); } @@ -37,9 +39,9 @@ template void sparse_product() { typedef typename SparseMatrixType::Index Index; Index n = 100; - const Index rows = internal::random(1,n); - const Index cols = internal::random(1,n); - const Index depth = internal::random(1,n); + const Index rows = internal::random(1,n); + const Index cols = internal::random(1,n); + const Index depth = internal::random(1,n); typedef typename SparseMatrixType::Scalar Scalar; enum { Flags = SparseMatrixType::Flags }; @@ -244,6 +246,7 @@ void test_sparse_product() CALL_SUBTEST_1( (sparse_product >()) ); CALL_SUBTEST_2( (sparse_product, ColMajor > >()) ); CALL_SUBTEST_2( (sparse_product, RowMajor > >()) ); + CALL_SUBTEST_3( (sparse_product >()) ); CALL_SUBTEST_4( (sparse_product_regression_test, Matrix >()) ); } } diff --git a/gtsam/3rdparty/Eigen/test/sparse_vector.cpp b/gtsam/3rdparty/Eigen/test/sparse_vector.cpp index ec5877b6a..0c9476803 100644 --- a/gtsam/3rdparty/Eigen/test/sparse_vector.cpp +++ b/gtsam/3rdparty/Eigen/test/sparse_vector.cpp @@ -9,14 +9,14 @@ #include "sparse.h" -template void sparse_vector(int rows, int cols) +template void sparse_vector(int rows, int cols) { double densityMat = (std::max)(8./(rows*cols), 0.01); double densityVec = (std::max)(8./float(rows), 0.1); typedef Matrix DenseMatrix; typedef Matrix DenseVector; - typedef SparseVector SparseVectorType; - typedef SparseMatrix SparseMatrixType; + typedef SparseVector SparseVectorType; + typedef SparseMatrix SparseMatrixType; Scalar eps = 1e-6; SparseMatrixType m1(rows,rows); @@ -101,9 +101,10 @@ template void sparse_vector(int rows, int cols) void test_sparse_vector() { for(int i = 0; i < g_repeat; i++) { - CALL_SUBTEST_1( sparse_vector(8, 8) ); - CALL_SUBTEST_2( sparse_vector >(16, 16) ); - CALL_SUBTEST_1( sparse_vector(299, 535) ); + CALL_SUBTEST_1(( sparse_vector(8, 8) )); + CALL_SUBTEST_2(( sparse_vector, int>(16, 16) )); + CALL_SUBTEST_1(( sparse_vector(299, 535) )); + CALL_SUBTEST_1(( sparse_vector(299, 535) )); } } diff --git a/gtsam/3rdparty/Eigen/test/stable_norm.cpp b/gtsam/3rdparty/Eigen/test/stable_norm.cpp index c09fc17b7..549f91fbf 100644 --- a/gtsam/3rdparty/Eigen/test/stable_norm.cpp +++ b/gtsam/3rdparty/Eigen/test/stable_norm.cpp @@ -55,8 +55,16 @@ template void stable_norm(const MatrixType& m) Index rows = m.rows(); Index cols = m.cols(); - Scalar big = internal::random() * ((std::numeric_limits::max)() * RealScalar(1e-4)); - Scalar small = internal::random() * ((std::numeric_limits::min)() * RealScalar(1e4)); + // get a non-zero random factor + Scalar factor = internal::random(); + while(numext::abs2(factor)(); + Scalar big = factor * ((std::numeric_limits::max)() * RealScalar(1e-4)); + + factor = internal::random(); + while(numext::abs2(factor)(); + Scalar small = factor * ((std::numeric_limits::min)() * RealScalar(1e4)); MatrixType vzero = MatrixType::Zero(rows, cols), vrand = MatrixType::Random(rows, cols), @@ -91,7 +99,7 @@ template void stable_norm(const MatrixType& m) VERIFY_IS_APPROX(vsmall.blueNorm(), sqrt(size)*abs(small)); VERIFY_IS_APPROX(vsmall.hypotNorm(), sqrt(size)*abs(small)); -// Test compilation of cwise() version + // Test compilation of cwise() version VERIFY_IS_APPROX(vrand.colwise().stableNorm(), vrand.colwise().norm()); VERIFY_IS_APPROX(vrand.colwise().blueNorm(), vrand.colwise().norm()); VERIFY_IS_APPROX(vrand.colwise().hypotNorm(), vrand.colwise().norm()); diff --git a/gtsam/3rdparty/Eigen/test/umeyama.cpp b/gtsam/3rdparty/Eigen/test/umeyama.cpp index 738d0af70..2e8092434 100644 --- a/gtsam/3rdparty/Eigen/test/umeyama.cpp +++ b/gtsam/3rdparty/Eigen/test/umeyama.cpp @@ -130,10 +130,11 @@ void run_fixed_size_test(int num_elements) // MUST be positive because in any other case det(cR_t) may become negative for // odd dimensions! - const Scalar c = abs(internal::random()); + // Also if c is to small compared to t.norm(), problem is ill-posed (cf. Bug 744) + const Scalar c = internal::random(0.5, 2.0); FixedMatrix R = randMatrixSpecialUnitary(dim); - FixedVector t = Scalar(50)*FixedVector::Random(dim,1); + FixedVector t = Scalar(32)*FixedVector::Random(dim,1); HomMatrix cR_t = HomMatrix::Identity(dim+1,dim+1); cR_t.block(0,0,dim,dim) = c*R; @@ -149,9 +150,9 @@ void run_fixed_size_test(int num_elements) HomMatrix cR_t_umeyama = umeyama(src_block, dst_block); - const Scalar error = ( cR_t_umeyama*src - dst ).array().square().sum(); + const Scalar error = ( cR_t_umeyama*src - dst ).squaredNorm(); - VERIFY(error < Scalar(10)*std::numeric_limits::epsilon()); + VERIFY(error < Scalar(16)*std::numeric_limits::epsilon()); } void test_umeyama() diff --git a/gtsam/3rdparty/Eigen/test/vectorwiseop.cpp b/gtsam/3rdparty/Eigen/test/vectorwiseop.cpp index 9d60b0388..6cd1acdda 100644 --- a/gtsam/3rdparty/Eigen/test/vectorwiseop.cpp +++ b/gtsam/3rdparty/Eigen/test/vectorwiseop.cpp @@ -101,6 +101,16 @@ template void vectorwiseop_array(const ArrayType& m) VERIFY_RAISES_ASSERT(m2.rowwise() /= rowvec.transpose()); VERIFY_RAISES_ASSERT(m1.rowwise() / rowvec.transpose()); + + m2 = m1; + // yes, there might be an aliasing issue there but ".rowwise() /=" + // is suppposed to evaluate " m2.colwise().sum()" into to temporary to avoid + // evaluating the reducions multiple times + if(ArrayType::RowsAtCompileTime>2 || ArrayType::RowsAtCompileTime==Dynamic) + { + m2.rowwise() /= m2.colwise().sum(); + VERIFY_IS_APPROX(m2, m1.rowwise() / m1.colwise().sum()); + } } template void vectorwiseop_matrix(const MatrixType& m) diff --git a/gtsam/3rdparty/Eigen/test/zerosized.cpp b/gtsam/3rdparty/Eigen/test/zerosized.cpp index 6905e584e..da7dd0481 100644 --- a/gtsam/3rdparty/Eigen/test/zerosized.cpp +++ b/gtsam/3rdparty/Eigen/test/zerosized.cpp @@ -9,12 +9,26 @@ #include "main.h" + +template void zeroReduction(const MatrixType& m) { + // Reductions that must hold for zero sized objects + VERIFY(m.all()); + VERIFY(!m.any()); + VERIFY(m.prod()==1); + VERIFY(m.sum()==0); + VERIFY(m.count()==0); + VERIFY(m.allFinite()); + VERIFY(!m.hasNaN()); +} + + template void zeroSizedMatrix() { MatrixType t1; - if (MatrixType::SizeAtCompileTime == Dynamic) + if (MatrixType::SizeAtCompileTime == Dynamic || MatrixType::SizeAtCompileTime == 0) { + zeroReduction(t1); if (MatrixType::RowsAtCompileTime == Dynamic) VERIFY(t1.rows() == 0); if (MatrixType::ColsAtCompileTime == Dynamic) @@ -22,9 +36,13 @@ template void zeroSizedMatrix() if (MatrixType::RowsAtCompileTime == Dynamic && MatrixType::ColsAtCompileTime == Dynamic) { + MatrixType t2(0, 0); VERIFY(t2.rows() == 0); VERIFY(t2.cols() == 0); + + zeroReduction(t2); + VERIFY(t1==t2); } } } @@ -33,11 +51,15 @@ template void zeroSizedVector() { VectorType t1; - if (VectorType::SizeAtCompileTime == Dynamic) + if (VectorType::SizeAtCompileTime == Dynamic || VectorType::SizeAtCompileTime==0) { + zeroReduction(t1); VERIFY(t1.size() == 0); VectorType t2(DenseIndex(0)); // DenseIndex disambiguates with 0-the-null-pointer (error with gcc 4.4 and MSVC8) VERIFY(t2.size() == 0); + zeroReduction(t2); + + VERIFY(t1==t2); } } @@ -51,9 +73,12 @@ void test_zerosized() zeroSizedMatrix >(); zeroSizedMatrix >(); zeroSizedMatrix >(); - + zeroSizedMatrix >(); + zeroSizedMatrix >(); + zeroSizedVector(); zeroSizedVector(); zeroSizedVector(); zeroSizedVector >(); + zeroSizedVector >(); } diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/OpenGLSupport b/gtsam/3rdparty/Eigen/unsupported/Eigen/OpenGLSupport index 4454bf820..c4090ab11 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/OpenGLSupport +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/OpenGLSupport @@ -11,7 +11,12 @@ #define EIGEN_OPENGL_MODULE #include -#include + +#if defined(__APPLE_CC__) + #include +#else + #include +#endif namespace Eigen { diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/ConstrainedConjGrad.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/ConstrainedConjGrad.h index 3f18beeeb..dc0093eb9 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/ConstrainedConjGrad.h +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/ConstrainedConjGrad.h @@ -58,7 +58,9 @@ void pseudo_inverse(const CMatrix &C, CINVMatrix &CINV) Scalar rho, rho_1, alpha; d.setZero(); - CINV.startFill(); // FIXME estimate the number of non-zeros + typedef Triplet T; + std::vector tripletList; + for (Index i = 0; i < rows; ++i) { d[i] = 1.0; @@ -84,11 +86,12 @@ void pseudo_inverse(const CMatrix &C, CINVMatrix &CINV) // FIXME add a generic "prune/filter" expression for both dense and sparse object to sparse for (Index j=0; j TmpVec; diff --git a/gtsam/3rdparty/Eigen/unsupported/test/FFTW.cpp b/gtsam/3rdparty/Eigen/unsupported/test/FFTW.cpp index a07bf274b..d3718e2d2 100644 --- a/gtsam/3rdparty/Eigen/unsupported/test/FFTW.cpp +++ b/gtsam/3rdparty/Eigen/unsupported/test/FFTW.cpp @@ -16,9 +16,6 @@ std::complex RandomCpx() { return std::complex( (T)(rand()/(T)RAND_MAX - . using namespace std; using namespace Eigen; -float norm(float x) {return x*x;} -double norm(double x) {return x*x;} -long double norm(long double x) {return x*x;} template < typename T> complex promote(complex x) { return complex(x.real(),x.imag()); } @@ -40,11 +37,11 @@ complex promote(long double x) { return complex( x); for (size_t k1=0;k1<(size_t)timebuf.size();++k1) { acc += promote( timebuf[k1] ) * exp( complex(0,k1*phinc) ); } - totalpower += norm(acc); + totalpower += numext::abs2(acc); complex x = promote(fftbuf[k0]); complex dif = acc - x; - difpower += norm(dif); - //cerr << k0 << "\t" << acc << "\t" << x << "\t" << sqrt(norm(dif)) << endl; + difpower += numext::abs2(dif); + //cerr << k0 << "\t" << acc << "\t" << x << "\t" << sqrt(numext::abs2(dif)) << endl; } cerr << "rmse:" << sqrt(difpower/totalpower) << endl; return sqrt(difpower/totalpower); @@ -57,8 +54,8 @@ complex promote(long double x) { return complex( x); long double difpower=0; size_t n = (min)( buf1.size(),buf2.size() ); for (size_t k=0;k 0.02. + + * GeodSolve.cgi allows ellipsoid to be set (and uses the -E option + for GeodSolve). + + * Set title in HTML versions of man pages for the utility programs. + + * Changes in cmake support: + + add _d to names of executables in debug mode of Visual Studio; + + add support for Android (cmake-only), thanks to Pullan Yu; + + check CPACK version numbers supplied on command line; + + configured version of project-config.cmake.in is + project-config.cmake (instead of geographiclib-config.cmake), to + prevent find_package incorrectly using this file; + + fix tests with multi-line output; + + this release includes a file, pom.xml, which is used by an + experimental build system (based on maven) at SRI. Changes between 1.34 (released 2013-12-11) and 1.33 versions: diff --git a/gtsam/3rdparty/GeographicLib/cmake/CMakeLists.txt b/gtsam/3rdparty/GeographicLib/cmake/CMakeLists.txt index 51e6c44b2..bbb5192cf 100644 --- a/gtsam/3rdparty/GeographicLib/cmake/CMakeLists.txt +++ b/gtsam/3rdparty/GeographicLib/cmake/CMakeLists.txt @@ -31,14 +31,17 @@ endif () # it to prevent the source and build paths appearing in the installed # config files set (PROJECT_INCLUDE_DIRS) -configure_file (project-config.cmake.in - ${PROJECT_NAME_LOWER}-config.cmake @ONLY) +configure_file (project-config.cmake.in project-config.cmake @ONLY) configure_file (project-config-version.cmake.in - ${PROJECT_NAME_LOWER}-config-version.cmake @ONLY) + project-config-version.cmake @ONLY) install (FILES - "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME_LOWER}-config.cmake" - "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME_LOWER}-config-version.cmake" - DESTINATION "${INSTALL_CMAKE_DIR}") + "${CMAKE_CURRENT_BINARY_DIR}/project-config.cmake" + DESTINATION "${INSTALL_CMAKE_DIR}" + RENAME "${PROJECT_NAME_LOWER}-config.cmake") +install (FILES + "${CMAKE_CURRENT_BINARY_DIR}/project-config-version.cmake" + DESTINATION "${INSTALL_CMAKE_DIR}" + RENAME "${PROJECT_NAME_LOWER}-config-version.cmake") # Make information about the cmake targets (the library and the tools) # available. install (EXPORT depends diff --git a/gtsam/3rdparty/GeographicLib/cmake/project-config.cmake.in b/gtsam/3rdparty/GeographicLib/cmake/project-config.cmake.in index a8b15a098..9bd50c24d 100644 --- a/gtsam/3rdparty/GeographicLib/cmake/project-config.cmake.in +++ b/gtsam/3rdparty/GeographicLib/cmake/project-config.cmake.in @@ -30,7 +30,7 @@ if (IS_ABSOLUTE "@PROJECT_ROOT_DIR@") set (_ROOT "@PROJECT_ROOT_DIR@") set (@PROJECT_NAME@_INCLUDE_DIRS "@PROJECT_INCLUDE_DIRS@") set (@PROJECT_NAME@_LIBRARY_DIRS "${_ROOT}/src") - set (@PROJECT_NAME@_BINARY_DIRS "${_ROOT}/src") + set (@PROJECT_NAME@_BINARY_DIRS "${_ROOT}/tools") else () # This is an installed package; figure out the paths relative to the # current directory. diff --git a/gtsam/3rdparty/GeographicLib/configure b/gtsam/3rdparty/GeographicLib/configure index cc81327f5..a9a70828a 100755 --- a/gtsam/3rdparty/GeographicLib/configure +++ b/gtsam/3rdparty/GeographicLib/configure @@ -1,6 +1,6 @@ #! /bin/sh # Guess values for system-dependent variables and create Makefiles. -# Generated by GNU Autoconf 2.69 for GeographicLib 1.34. +# Generated by GNU Autoconf 2.69 for GeographicLib 1.35. # # Report bugs to . # @@ -590,8 +590,8 @@ MAKEFLAGS= # Identity of this package. PACKAGE_NAME='GeographicLib' PACKAGE_TARNAME='geographiclib' -PACKAGE_VERSION='1.34' -PACKAGE_STRING='GeographicLib 1.34' +PACKAGE_VERSION='1.35' +PACKAGE_STRING='GeographicLib 1.35' PACKAGE_BUGREPORT='charles@karney.com' PACKAGE_URL='' @@ -1339,7 +1339,7 @@ if test "$ac_init_help" = "long"; then # Omit some internal or obsolete options to make the list less imposing. # This message is too long to be a string in the A/UX 3.1 sh. cat <<_ACEOF -\`configure' configures GeographicLib 1.34 to adapt to many kinds of systems. +\`configure' configures GeographicLib 1.35 to adapt to many kinds of systems. Usage: $0 [OPTION]... [VAR=VALUE]... @@ -1410,7 +1410,7 @@ fi if test -n "$ac_init_help"; then case $ac_init_help in - short | recursive ) echo "Configuration of GeographicLib 1.34:";; + short | recursive ) echo "Configuration of GeographicLib 1.35:";; esac cat <<\_ACEOF @@ -1519,7 +1519,7 @@ fi test -n "$ac_init_help" && exit $ac_status if $ac_init_version; then cat <<\_ACEOF -GeographicLib configure 1.34 +GeographicLib configure 1.35 generated by GNU Autoconf 2.69 Copyright (C) 2012 Free Software Foundation, Inc. @@ -2063,7 +2063,7 @@ cat >config.log <<_ACEOF This file contains any messages produced by compilers while running configure, to aid debugging if configure makes a mistake. -It was created by GeographicLib $as_me 1.34, which was +It was created by GeographicLib $as_me 1.35, which was generated by GNU Autoconf 2.69. Invocation command line was $ $0 $@ @@ -3001,7 +3001,7 @@ fi # Define the identity of the package. PACKAGE='geographiclib' - VERSION='1.34' + VERSION='1.35' cat >>confdefs.h <<_ACEOF @@ -3049,7 +3049,7 @@ am__tar='$${TAR-tar} chof - "$$tardir"' am__untar='$${TAR-tar} xf -' GEOGRAPHICLIB_VERSION_MAJOR=1 -GEOGRAPHICLIB_VERSION_MINOR=34 +GEOGRAPHICLIB_VERSION_MINOR=35 GEOGRAPHICLIB_VERSION_PATCH=0 cat >>confdefs.h <<_ACEOF @@ -3098,7 +3098,7 @@ ac_config_headers="$ac_config_headers include/GeographicLib/Config-ac.h" LT_CURRENT=11 -LT_REVISION=1 +LT_REVISION=2 LT_AGE=1 @@ -16672,7 +16672,7 @@ cat >>$CONFIG_STATUS <<\_ACEOF || ac_write_fail=1 # report actual input values of CONFIG_FILES etc. instead of their # values after options handling. ac_log=" -This file was extended by GeographicLib $as_me 1.34, which was +This file was extended by GeographicLib $as_me 1.35, which was generated by GNU Autoconf 2.69. Invocation command line was CONFIG_FILES = $CONFIG_FILES @@ -16738,7 +16738,7 @@ _ACEOF cat >>$CONFIG_STATUS <<_ACEOF || ac_write_fail=1 ac_cs_config="`$as_echo "$ac_configure_args" | sed 's/^ //; s/[\\""\`\$]/\\\\&/g'`" ac_cs_version="\\ -GeographicLib config.status 1.34 +GeographicLib config.status 1.35 configured by $0, generated by GNU Autoconf 2.69, with options \\"\$ac_cs_config\\" diff --git a/gtsam/3rdparty/GeographicLib/configure.ac b/gtsam/3rdparty/GeographicLib/configure.ac index e9fb7b6f2..c0b539c5c 100644 --- a/gtsam/3rdparty/GeographicLib/configure.ac +++ b/gtsam/3rdparty/GeographicLib/configure.ac @@ -1,7 +1,7 @@ dnl dnl Copyright (C) 2009, Francesco P. Lovergine -AC_INIT([GeographicLib],[1.34],[charles@karney.com]) +AC_INIT([GeographicLib],[1.35],[charles@karney.com]) AC_CANONICAL_SYSTEM AC_PREREQ(2.61) AC_CONFIG_SRCDIR(src/Geodesic.cpp) @@ -9,7 +9,7 @@ AC_CONFIG_MACRO_DIR(m4) AM_INIT_AUTOMAKE GEOGRAPHICLIB_VERSION_MAJOR=1 -GEOGRAPHICLIB_VERSION_MINOR=34 +GEOGRAPHICLIB_VERSION_MINOR=35 GEOGRAPHICLIB_VERSION_PATCH=0 AC_DEFINE_UNQUOTED([GEOGRAPHICLIB_VERSION_MAJOR], [$GEOGRAPHICLIB_VERSION_MAJOR],[major version number]) @@ -35,7 +35,7 @@ dnl Interfaces changed/added/removed: CURRENT++ REVISION=0 dnl Interfaces added: AGE++ dnl Interfaces removed: AGE=0 LT_CURRENT=11 -LT_REVISION=1 +LT_REVISION=2 LT_AGE=1 AC_SUBST(LT_CURRENT) AC_SUBST(LT_REVISION) diff --git a/gtsam/3rdparty/GeographicLib/doc/GeographicLib.dox b/gtsam/3rdparty/GeographicLib/doc/GeographicLib.dox index b0f980d19..7fd60e992 100644 --- a/gtsam/3rdparty/GeographicLib/doc/GeographicLib.dox +++ b/gtsam/3rdparty/GeographicLib/doc/GeographicLib.dox @@ -11,8 +11,8 @@ /** \mainpage GeographicLib library \author Charles F. F. Karney (charles@karney.com) -\version 1.34 -\date 2013-12-11 +\version 1.35 +\date 2014-03-13 \section abstract Abstract @@ -45,22 +45,22 @@ The main project page is at http://sourceforge.net/projects/geographiclib . The code is available for download at -- - GeographicLib-1.34.tar.gz -- - GeographicLib-1.34.zip +- + GeographicLib-1.35.tar.gz +- + GeographicLib-1.35.zip . as either a compressed tar file (tar.gz) or a zip file. (The two archives have identical contents, except that the zip file has DOS line endings.) Alternatively you can get the latest release using git \verbatim - git clone -b r1.34 git://git.code.sf.net/p/geographiclib/code geographiclib + git clone -b r1.35 git://git.code.sf.net/p/geographiclib/code geographiclib \endverbatim There are also binary installers for Windows available at -- - GeographicLib-1.34-win32.exe -- - GeographicLib-1.34-win64.exe +- + GeographicLib-1.35-win32.exe +- + GeographicLib-1.35-win64.exe . It is licensed under the MIT/X11 License; @@ -180,14 +180,14 @@ Back to \ref intro. Forward to \ref start. Up to \ref contents. (versions 4.0 and later) and under Windows with Visual Studio 2005, 2008, and 2010. Earlier versions were tested also under Darwin and Solaris. It should compile on a wide range of other systems. First download either - -GeographicLib-1.34.tar.gz or - -GeographicLib-1.34.zip (or - -GeographicLib-1.34-win32.exe or - -GeographicLib-1.34-win64.exe for binary installation under Windows). + +GeographicLib-1.35.tar.gz or + +GeographicLib-1.35.zip (or + +GeographicLib-1.35-win32.exe or + +GeographicLib-1.35-win64.exe for binary installation under Windows). Then pick one of the first five options below: - \ref cmake. This is the preferred installation method as it will work on the widest range of platforms. However it requires that you have @@ -262,10 +262,10 @@ g++ on Linux and with the Visual Studio IDE on Windows. Here are the steps to compile and install %GeographicLib: - Unpack the source, running one of \verbatim - tar xfpz GeographicLib-1.34.tar.gz - unzip -q GeographicLib-1.34.zip \endverbatim + tar xfpz GeographicLib-1.35.tar.gz + unzip -q GeographicLib-1.35.zip \endverbatim then enter the directory created with one of \verbatim - cd GeographicLib-1.34 \endverbatim + cd GeographicLib-1.35 \endverbatim - Create a separate build directory and enter it, for example, \verbatim mkdir BUILD cd BUILD \endverbatim @@ -273,8 +273,8 @@ Here are the steps to compile and install %GeographicLib: and MacOSX systems, the command is \verbatim cmake .. \endverbatim For Windows, the command is typically one of \verbatim - cmake -G "Visual Studio 10" -D CMAKE_INSTALL_PREFIX=C:/pkg-vc10/GeographicLib-1.34 .. - cmake -G "Visual Studio 9 2008" -D CMAKE_INSTALL_PREFIX=C:/pkg-vc9/GeographicLib-1.34 .. + cmake -G "Visual Studio 10" -D CMAKE_INSTALL_PREFIX=C:/pkg-vc10/GeographicLib-1.35 .. + cmake -G "Visual Studio 9 2008" -D CMAKE_INSTALL_PREFIX=C:/pkg-vc9/GeographicLib-1.35 .. \endverbatim The definitions of CMAKE_INSTALL_PREFIX are optional (see below). The settings given above are recommended to ensure that packages that use @@ -290,7 +290,7 @@ Here are the steps to compile and install %GeographicLib: convention. If it is on ON (the Linux default), the installation is to a common directory, e.g., /usr/local. If it is OFF (the Windows default), the installation directory contains the package - name, e.g., C:/pkg/GeographicLib-1.34. The installation + name, e.g., C:/pkg/GeographicLib-1.35. The installation directories for the documentation, cmake configuration, python and matlab interfaces all depend on the variable with deeper paths relative to CMAKE_INSTALL_PREFIX being used when it's ON: @@ -305,7 +305,7 @@ Here are the steps to compile and install %GeographicLib: For windows systems, it is recommended to use a prefix which includes the compiler version, as shown above (and also, possibly, whether this is a 64-bit build, e.g., cmake -G "Visual Studio - 10 Win64" -D CMAKE_INSTALL_PREFIX=C:/pkg-vc10-x64/GeographicLib-1.34 + 10 Win64" -D CMAKE_INSTALL_PREFIX=C:/pkg-vc10-x64/GeographicLib-1.35 ..). If you just want to try the library to see if it suits your needs, pick, for example, CMAKE_INSTALL_PREFIX=/tmp/geographic. @@ -394,9 +394,9 @@ Here are the steps to compile and install %GeographicLib: The method works on most Unix-like systems including Linux and Mac OS X. Here are the steps to compile and install %GeographicLib: - Unpack the source, running \verbatim - tar xfpz GeographicLib-1.34.tar.gz \endverbatim + tar xfpz GeographicLib-1.35.tar.gz \endverbatim then enter the directory created \verbatim - cd GeographicLib-1.34 \endverbatim + cd GeographicLib-1.35 \endverbatim - Create a separate build directory and enter it, for example, \verbatim mkdir BUILD cd BUILD \endverbatim @@ -425,9 +425,9 @@ and g++. This builds a static library and the examples. Here are the steps to compile and install %GeographicLib: - Unpack the source, running \verbatim - tar xfpz GeographicLib-1.34.tar.gz \endverbatim + tar xfpz GeographicLib-1.35.tar.gz \endverbatim then enter the directory created \verbatim - cd GeographicLib-1.34 \endverbatim + cd GeographicLib-1.35 \endverbatim - Edit \verbatim include/GeographicLib/Config.h \endverbatim If your C++ compiler does not recognize the long double type @@ -458,8 +458,8 @@ static library and the utilities. If you only have Visual Studio 2003, use cmake to create the necessary solution file, see \ref cmake. (cmake is needed to build the Matlab interface and to run the tests.) - Unpack the source, running \verbatim - unzip -q GeographicLib-1.34.zip \endverbatim -- Open GeographicLib-1.34/windows/GeographicLib-vc10.sln in Visual Studio + unzip -q GeographicLib-1.35.zip \endverbatim +- Open GeographicLib-1.35/windows/GeographicLib-vc10.sln in Visual Studio 2010 (for Visual Studio 2005 and 2008, replace -vc10 by -vc8 or -vc9). - Pick the build type (e.g., Release), and select "Build Solution". - The library and the compiled examples are in the windows/Release. @@ -486,14 +486,14 @@ be advisable to build it with the compiler you are using for your own code using either \ref cmake or \ref windows. Download and run - -GeographicLib-1.34-win32.exe or - -GeographicLib-1.34-win64.exe: + +GeographicLib-1.35-win32.exe or + +GeographicLib-1.35-win64.exe: - read the MIT/X11 License agreement, - select whether you want your PATH modified, - select the installation folder, by default - C:\\pkg-vc10\\GeographicLib-1.34 or C:\\pkg-vc10-x64\\GeographicLib-1.34, + C:\\pkg-vc10\\GeographicLib-1.35 or C:\\pkg-vc10-x64\\GeographicLib-1.35, - select the start menu folder, - and install. . @@ -501,7 +501,7 @@ GeographicLib-1.34-win64.exe: given in \ref cmake.) The start menu will now include links to the documentation for the library and for the utilities (and a link for uninstalling the library). If you ask for your PATH to be modified, it -will include C:/pkg-vc10/GeographicLib-1.34/bin where the utilities are +will include C:/pkg-vc10/GeographicLib-1.35/bin where the utilities are installed. The headers and library are installed in the include/GeographicLib and lib folders. With the 64-bit installer, the Matlab interface is installed in the matlab folder. Add this to your @@ -552,7 +552,7 @@ Check the code out of git with \verbatim Here the "master" branch is checked out. There are three branches in the git repository: - master: the main branch for code maintainence. Releases are - tagged on this branch as, e.g., v1.34. + tagged on this branch as, e.g., v1.35. - devel: the development branch; changes made here are merged into master. - release: the release branch created by unpacking the source @@ -562,7 +562,7 @@ the git repository: specifying a branch). This differs from the master branch in that some administrative files are excluded while some intermediate files are included (in order to aid building on as many platforms as - possible). Releases are tagged on this branch as, e.g., r1.34. + possible). Releases are tagged on this branch as, e.g., r1.35. . The autoconf configuration script and the formatted man pages are not checked into master branch of the repository. In order to create the @@ -578,8 +578,8 @@ In the case of cmake, you then run \verbatim which will copy the man pages from the build directory back into the source tree and package the resulting source tree for distribution as \verbatim - GeographicLib-1.34.tar.gz - GeographicLib-1.34.zip \endverbatim + GeographicLib-1.35.tar.gz + GeographicLib-1.35.zip \endverbatim Finally, \verbatim make package \endverbatim or building PACKAGE in Visual Studio will create a binary installer for @@ -605,7 +605,7 @@ With configure, run \verbatim make dist-gzip \endverbatim which will create the additional files and packages the results ready for distribution as \verbatim - geographiclib-1.34.tar.gz \endverbatim + geographiclib-1.35.tar.gz \endverbatim
Back to \ref intro. Forward to \ref start. Up to \ref contents. @@ -695,7 +695,7 @@ In order to use %GeographicLib from C++ code, you will need to If %GeographicLib is found, then the following cmake variables are set: - GeographicLib_FOUND = 1 - - GeographicLib_VERSION = 1.34 + - GeographicLib_VERSION = 1.35 - GeographicLib_INCLUDE_DIRS - GeographicLib_LIBRARIES = one of the following two: - GeographicLib_SHARED_LIBRARIES = %GeographicLib @@ -1120,9 +1120,9 @@ feature of %GeographicLib, but want your code still to work with older versions. In that case, you can test the values of the macros GEOGRAPHICLIB_VERSION_MAJOR, GEOGRAPHICLIB_VERSION_MINOR, and GEOGRAPHICLIB_VERSION_PATCH; these expand to numbers (and the last one -is usually 0); these macros appeared starting in version 1.34. There's +is usually 0); these macros appeared starting in version 1.31. There's also a macro GEOGRAPHICLIB_VERSION_STRING which expands to, e.g., -"1.34"; this macro has been defined since version 1.9. +"1.35"; this macro has been defined since version 1.9.
Back to \ref utilities. Forward to \ref other. Up to \ref contents. @@ -1274,7 +1274,7 @@ The matlab directory contains - Native Matlab implementations of the geodesic routines. To use these, start Matlab or Octave and run one of (for example) \verbatim addpath /usr/local/libexec/GeographicLib/matlab - addpath 'C:/pkg-vc10-x64/GeographicLib-1.34/libexec/GeographicLib/matlab' + addpath 'C:/pkg-vc10-x64/GeographicLib-1.35/libexec/GeographicLib/matlab' \endverbatim The available functions are: - geoddoc: briefly descibe the routines @@ -1345,9 +1345,9 @@ There are two ways of compiling the interface code: (1) using cmake and - Invoking the compiler from Matlab or Octave: Start Matlab or Octave and run, e.g., \code mex -setup - cd 'C:/pkg-vc10-x64/GeographicLib-1.34/matlab' + cd 'C:/pkg-vc10-x64/GeographicLib-1.35/matlab' help geographiclibinterface - geographiclibinterface('C:/pkg-vc10/GeographicLib-1.34'); + geographiclibinterface('C:/pkg-vc10/GeographicLib-1.35'); addpath(pwd); \endcode The first command allows you to select the compiler to use (which @@ -1356,7 +1356,7 @@ There are two ways of compiling the interface code: (1) using cmake and To use the interface routines for %GeographicLib, run one of (for example) \verbatim addpath /usr/local/libexec/GeographicLib/matlab - addpath 'C:/pkg-vc10-x64/GeographicLib-1.34/libexec/GeographicLib/matlab' + addpath 'C:/pkg-vc10-x64/GeographicLib-1.35/libexec/GeographicLib/matlab' \endverbatim in Octave or Matlab. The available functions are: - geodesicdirect: solve direct geodesic problem @@ -4048,7 +4048,7 @@ starting point of this geodesic is \f$\beta_1 = 87.48^\circ\f$, \f$\omega_1 = 0^\circ\f$, and \f$\alpha_1 = 90^\circ\f$. If the starting point is \f$\beta_1 = 90^\circ\f$, \f$\omega_1 \in -(0^\circ, 180^\circ)\f$, and \f$\alpha_1 = 0^\circ\f$, then the geodesic +(0^\circ, 180^\circ)\f$, and \f$\alpha_1 = 180^\circ\f$, then the geodesic encircles the ellipsoid in a "transpolar" sense. The geodesic oscillates east and west of the ellipse \f$x = 0\f$; on each oscillation it completes slightly more that a full circuit around the ellipsoid @@ -4067,7 +4067,7 @@ Fig. 4 Fig. 4: Example of a transpolar geodesic on a triaxial ellipsoid. The starting point of this geodesic is \f$\beta_1 = 90^\circ\f$, \f$\omega_1 = -39.9^\circ\f$, and \f$\alpha_1 = 0^\circ\f$. +39.9^\circ\f$, and \f$\alpha_1 = 180^\circ\f$.
). - If only one point is an umbilicial point, the azimuth at the non-umbilical point is found using the generalization of Clairaut's equation (given above) with \f$\gamma = 0\f$. - - If both points lie on the equator \f$\beta = 0\f$, then determine the - reduced length \f$m_{12}\f$ for the geodesic which is the shorter - path along the ellipse \f$z = 0\f$. If \f$m_{12} \ge 0\f$, then this - is the shortest path on the ellipsoid; otherwise proceed to the - general case (next). + - Treat the cases where the geodesic might follow a line of constant + \f$\beta\f$. There are two such cases: (a) the points lie on the + ellipse \f$z = 0\f$ on a general ellipsoid and (b) the points lie on + an ellipse whose major axis is the \f$x\f$ axis on a prolate ellipsoid + (\f$a = b > c\f$). Determine the reduced length \f$m_{12}\f$ for the + geodesic which is the shorter path along the ellipse. If \f$m_{12} + \ge 0\f$, then this is the shortest path on the ellipsoid; otherwise + proceed to the general case (next). - Swap the points, if necessary, so that the first point is the one closest to a pole. Estimate \f$\alpha_1\f$ (by some means) and solve the \e hybrid problem, i.e., determine the longitude \f$\omega_2\f$ @@ -4238,6 +4241,12 @@ The shortest path found by this method is unique unless: - The points are opposite umbilical points. In this case, \f$\alpha_1\f$ can take on any value and \f$\alpha_2\f$ needs to be adjusted to maintain the value of \f$\tan\alpha_1 / \tan\alpha_2\f$. + Note that \f$\alpha\f$ increases by \f$\pm 90^\circ\f$ as the + geodesic passes through an umbilical point, depending on whether the + geodesic is considered as passing to the right or left of the point. + Here \f$\alpha_2\f$ is the \e forward azimuth at the second umbilical + point, i.e., its azimuth immediately \e after passage through the + umbilical point. - \f$\beta_1 + \beta_2 = 0\f$ and \f$\cos\alpha_1\f$ and \f$\cos\alpha_2\f$ have opposite signs. In this case, there another shortest geodesic with azimuths \f$\pi - \alpha_1\f$ and @@ -4757,6 +4766,26 @@ been migrated to the archive subdirectory). All the releases are available as tags “rm.nn” in the the "release" branch of the git repository for %GeographicLib. + - Version 1.35 + (released 2014-03-13) + - Fix blunder in GeographicLib::UTMUPS::EncodeEPSG (found by Ben + Adler). + - Matlab wrapper routines geodesic{direct,inverse,line} switch to + "exact" routes if |f| > 0.02. + - GeodSolve.cgi allows ellipsoid to be set (and uses the -E option + for GeodSolve). + - Set title in HTML versions of man pages for the \ref utilities. + - Changes in cmake support: + - add _d to names of executables in debug mode of Visual Studio; + - add support for Android (cmake-only), thanks to Pullan Yu; + - check CPACK version numbers supplied on command line; + - configured version of project-config.cmake.in is + project-config.cmake (instead of geographiclib-config.cmake), to + prevent find_package incorrectly using this file; + - fix tests with multi-line output; + - this release includes a file, pom.xml, which is used by an + experimental build system (based on maven) at SRI. + - Version 1.34 (released 2013-12-11) - Many changes in cmake support: diff --git a/gtsam/3rdparty/GeographicLib/doc/NETGeographicLib.dox b/gtsam/3rdparty/GeographicLib/doc/NETGeographicLib.dox index 89b934a7d..fd8f5e057 100644 --- a/gtsam/3rdparty/GeographicLib/doc/NETGeographicLib.dox +++ b/gtsam/3rdparty/GeographicLib/doc/NETGeographicLib.dox @@ -11,8 +11,8 @@ /** \mainpage NETGeographicLib library \author Scott Heiman (mrmtdew2@outlook.com) -\version 1.34 -\date 2013-12-11 +\version 1.35 +\date 2014-03-13 \section abstract Abstract @@ -26,14 +26,14 @@ to the GeographicLib classes. GeographicLib and NETGeographicLib is an integrated product. The NETGeographic project in the GeographicLib-vc10.sln file located in -\/GeographicLib-1.34/windows will create the NETGeographicLib +\/GeographicLib-1.35/windows will create the NETGeographicLib DLL. The source code for NETGeographicLib is located in -\/GeographicLib-1.34/dotnet/NETGeographicLib. NETGeographicLib +\/GeographicLib-1.35/dotnet/NETGeographicLib. NETGeographicLib is not available for older versions of Microsoft Visual Studio. NETGeographicLib has been tested with C#, Managed C++, and Visual Basic. Sample code snippets can be found in -\/GeographicLib-1.34/dotnet/examples. +\/GeographicLib-1.35/dotnet/examples. \section differences Differences between NETGeographicLib and GeographicLib @@ -135,7 +135,7 @@ to any Visual Basic source that uses NETGeographicLib classes. A C# sample application is provided that demonstrates NETGeographicLib classes. The source code for the sample application is located in -\/GeographicLib-1.34/dotnet/Projections. The sample +\/GeographicLib-1.35/dotnet/Projections. The sample application creates a tabbed dialog. Each tab provides data entry fields that allow the user to exercise one or more NETGeographicLib classes. @@ -200,7 +200,7 @@ code using the installed library: \verbatim project (geodesictest) cmake_minimum_required (VERSION 2.8.7) # required for VS_DOTNET_REFERENCES -find_package (GeographicLib 1.34 REQUIRED COMPONENTS NETGeographicLib) +find_package (GeographicLib 1.35 REQUIRED COMPONENTS NETGeographicLib) add_executable (${PROJECT_NAME} example-Geodesic-small.cpp) set_target_properties (${PROJECT_NAME} PROPERTIES COMPILE_FLAGS "/clr") diff --git a/gtsam/3rdparty/GeographicLib/doc/scripts/GeographicLib/Geodesic.js b/gtsam/3rdparty/GeographicLib/doc/scripts/GeographicLib/Geodesic.js index 09d22593a..cf059126f 100644 --- a/gtsam/3rdparty/GeographicLib/doc/scripts/GeographicLib/Geodesic.js +++ b/gtsam/3rdparty/GeographicLib/doc/scripts/GeographicLib/Geodesic.js @@ -753,7 +753,7 @@ GeographicLib.GeodesicLine = {}; // Add the check for sig12 since zero length geodesics might yield // m12 < 0. Test case was // - // echo 20.001 0 20.001 0 | Geod -i + // echo 20.001 0 20.001 0 | GeodSolve -i // // In fact, we will have sig12 > pi/2 for meridional geodesic // which is not a shortest path. diff --git a/gtsam/3rdparty/GeographicLib/doc/scripts/geod-google-instructions.html b/gtsam/3rdparty/GeographicLib/doc/scripts/geod-google-instructions.html index 8d6f25c56..89c2e5605 100644 --- a/gtsam/3rdparty/GeographicLib/doc/scripts/geod-google-instructions.html +++ b/gtsam/3rdparty/GeographicLib/doc/scripts/geod-google-instructions.html @@ -101,8 +101,8 @@ In putting together this Google Maps demonstration, I started with the sample code - - geometry-headings.html. + + geometry-headings.

Charles Karney diff --git a/gtsam/3rdparty/GeographicLib/doc/scripts/geod-google.html b/gtsam/3rdparty/GeographicLib/doc/scripts/geod-google.html index 35576cf87..5a48d7e2e 100644 --- a/gtsam/3rdparty/GeographicLib/doc/scripts/geod-google.html +++ b/gtsam/3rdparty/GeographicLib/doc/scripts/geod-google.html @@ -24,13 +24,18 @@ Google Maps, WGS84 ellipsoid, GeographicLib" /> - + -
diff --git a/gtsam/3rdparty/GeographicLib/include/GeographicLib/Config.h b/gtsam/3rdparty/GeographicLib/include/GeographicLib/Config.h index 1e5e30b9b..2249dd127 100644 --- a/gtsam/3rdparty/GeographicLib/include/GeographicLib/Config.h +++ b/gtsam/3rdparty/GeographicLib/include/GeographicLib/Config.h @@ -1,8 +1,8 @@ // This will be overwritten by ./configure -#define GEOGRAPHICLIB_VERSION_STRING "1.34" +#define GEOGRAPHICLIB_VERSION_STRING "1.35" #define GEOGRAPHICLIB_VERSION_MAJOR 1 -#define GEOGRAPHICLIB_VERSION_MINOR 34 +#define GEOGRAPHICLIB_VERSION_MINOR 35 #define GEOGRAPHICLIB_VERSION_PATCH 0 // Undefine HAVE_LONG_DOUBLE if this type is unknown to the compiler diff --git a/gtsam/3rdparty/GeographicLib/include/GeographicLib/Constants.hpp b/gtsam/3rdparty/GeographicLib/include/GeographicLib/Constants.hpp index e37ec62ce..99baff31f 100644 --- a/gtsam/3rdparty/GeographicLib/include/GeographicLib/Constants.hpp +++ b/gtsam/3rdparty/GeographicLib/include/GeographicLib/Constants.hpp @@ -21,6 +21,17 @@ # elif defined(__GXX_EXPERIMENTAL_CXX0X__) # define STATIC_ASSERT static_assert # elif defined(_MSC_VER) && _MSC_VER >= 1600 +// For reference, here is a table of Visual Studio and _MSC_VER +// correspondences: +// +// _MSC_VER Visual Studio +// 1300 vc7 +// 1311 vc7.1 (2003) +// 1400 vc8 (2005) +// 1500 vc9 (2008) +// 1600 vc10 (2010) +// 1700 vc11 (2012) +// 1800 vc12 (2013) # define STATIC_ASSERT static_assert # else # define STATIC_ASSERT(cond,reason) \ diff --git a/gtsam/3rdparty/GeographicLib/include/GeographicLib/Geodesic.hpp b/gtsam/3rdparty/GeographicLib/include/GeographicLib/Geodesic.hpp index cd687acce..6cec279a2 100644 --- a/gtsam/3rdparty/GeographicLib/include/GeographicLib/Geodesic.hpp +++ b/gtsam/3rdparty/GeographicLib/include/GeographicLib/Geodesic.hpp @@ -162,8 +162,8 @@ namespace GeographicLib { * Example of use: * \include example-Geodesic.cpp * - * Geod is a command-line utility providing access - * to the functionality of Geodesic and GeodesicLine. + * GeodSolve is a command-line utility + * providing access to the functionality of Geodesic and GeodesicLine. **********************************************************************/ class GEOGRAPHICLIB_EXPORT Geodesic { diff --git a/gtsam/3rdparty/GeographicLib/include/GeographicLib/GeodesicExact.hpp b/gtsam/3rdparty/GeographicLib/include/GeographicLib/GeodesicExact.hpp index c0444f70e..e76daee4c 100644 --- a/gtsam/3rdparty/GeographicLib/include/GeographicLib/GeodesicExact.hpp +++ b/gtsam/3rdparty/GeographicLib/include/GeographicLib/GeodesicExact.hpp @@ -25,7 +25,7 @@ namespace GeographicLib { class GeodesicLineExact; /** - * \brief Exact %Geodesic calculations + * \brief Exact geodesic calculations * * The equations for geodesics on an ellipsoid can be expressed in terms of * incomplete elliptic integrals. The Geodesic class expands these integrals @@ -67,14 +67,14 @@ namespace GeographicLib { * about 8 decimal digits for \e b/\e a ∈ [1/4, 4]. * * See \ref geodellip for the formulation. See the documentation on the - * Geodesic class for additional information on the geodesics problems. + * Geodesic class for additional information on the geodesic problems. * * Example of use: * \include example-GeodesicExact.cpp * - * Geod is a command-line utility providing access - * to the functionality of GeodesicExact and GeodesicLineExact (via the -E - * option). + * GeodSolve is a command-line utility + * providing access to the functionality of GeodesicExact and + * GeodesicLineExact (via the -E option). **********************************************************************/ class GEOGRAPHICLIB_EXPORT GeodesicExact { diff --git a/gtsam/3rdparty/GeographicLib/include/GeographicLib/GeodesicLine.hpp b/gtsam/3rdparty/GeographicLib/include/GeographicLib/GeodesicLine.hpp index fd968e2cf..c901b3007 100644 --- a/gtsam/3rdparty/GeographicLib/include/GeographicLib/GeodesicLine.hpp +++ b/gtsam/3rdparty/GeographicLib/include/GeographicLib/GeodesicLine.hpp @@ -52,8 +52,8 @@ namespace GeographicLib { * Example of use: * \include example-GeodesicLine.cpp * - * Geod is a command-line utility providing access - * to the functionality of Geodesic and GeodesicLine. + * GeodSolve is a command-line utility + * providing access to the functionality of Geodesic and GeodesicLine. **********************************************************************/ class GEOGRAPHICLIB_EXPORT GeodesicLine { diff --git a/gtsam/3rdparty/GeographicLib/include/GeographicLib/GeodesicLineExact.hpp b/gtsam/3rdparty/GeographicLib/include/GeographicLib/GeodesicLineExact.hpp index ca7d0d211..a2e19f845 100644 --- a/gtsam/3rdparty/GeographicLib/include/GeographicLib/GeodesicLineExact.hpp +++ b/gtsam/3rdparty/GeographicLib/include/GeographicLib/GeodesicLineExact.hpp @@ -27,9 +27,9 @@ namespace GeographicLib { * Example of use: * \include example-GeodesicLineExact.cpp * - * Geod is a command-line utility providing access - * to the functionality of GeodesicExact and GeodesicLineExact (via the -E - * option). + * GeodSolve is a command-line utility + * providing access to the functionality of GeodesicExact and + * GeodesicLineExact (via the -E option). **********************************************************************/ class GEOGRAPHICLIB_EXPORT GeodesicLineExact { diff --git a/gtsam/3rdparty/GeographicLib/include/GeographicLib/Math.hpp b/gtsam/3rdparty/GeographicLib/include/GeographicLib/Math.hpp index 9971ebdf0..2ef649e96 100644 --- a/gtsam/3rdparty/GeographicLib/include/GeographicLib/Math.hpp +++ b/gtsam/3rdparty/GeographicLib/include/GeographicLib/Math.hpp @@ -18,8 +18,11 @@ * Are C++11 math functions available? **********************************************************************/ #if !defined(GEOGRAPHICLIB_CPLUSPLUS11_MATH) -# if defined(__GNUC__) && __GNUC__ == 4 && __GNUC_MINOR__ >= 8 \ - && __cplusplus >= 201103 +# if defined(__GNUC__) && __GNUC__ == 4 && __GNUC_MINOR__ >= 7 \ + && __cplusplus >= 201103 && !(defined(__ANDROID__) || defined(ANDROID)) +// The android toolchain uses g++ and supports C++11, but not, apparently, the +// new mathematical functions introduced with C++11. Android toolchains might +// define __ANDROID__ or ANDROID; so need to check both. # define GEOGRAPHICLIB_CPLUSPLUS11_MATH 1 # elif defined(_MSC_VER) && _MSC_VER >= 1800 # define GEOGRAPHICLIB_CPLUSPLUS11_MATH 1 diff --git a/gtsam/3rdparty/GeographicLib/include/GeographicLib/UTMUPS.hpp b/gtsam/3rdparty/GeographicLib/include/GeographicLib/UTMUPS.hpp index 512ba5c43..9fc19c66e 100644 --- a/gtsam/3rdparty/GeographicLib/include/GeographicLib/UTMUPS.hpp +++ b/gtsam/3rdparty/GeographicLib/include/GeographicLib/UTMUPS.hpp @@ -70,7 +70,7 @@ namespace GeographicLib { static const int epsg01N = 32601; // EPSG code for UTM 01N static const int epsg60N = 32660; // EPSG code for UTM 60N static const int epsgN = 32661; // EPSG code for UPS N - static const int epsg01S = 32701; // EPSG code for UTM 01N + static const int epsg01S = 32701; // EPSG code for UTM 01S static const int epsg60S = 32760; // EPSG code for UTM 60S static const int epsgS = 32761; // EPSG code for UPS S static real CentralMeridian(int zone) throw() diff --git a/gtsam/3rdparty/GeographicLib/legacy/C/geodesic.c b/gtsam/3rdparty/GeographicLib/legacy/C/geodesic.c index bd9fc960f..f46a6ff41 100644 --- a/gtsam/3rdparty/GeographicLib/legacy/C/geodesic.c +++ b/gtsam/3rdparty/GeographicLib/legacy/C/geodesic.c @@ -700,7 +700,7 @@ real geod_geninverse(const struct geod_geodesic* g, /* Add the check for sig12 since zero length geodesics might yield m12 < * 0. Test case was * - * echo 20.001 0 20.001 0 | Geod -i + * echo 20.001 0 20.001 0 | GeodSolve -i * * In fact, we will have sig12 > pi/2 for meridional geodesic which is * not a shortest path. */ diff --git a/gtsam/3rdparty/GeographicLib/legacy/Fortran/geodesic.for b/gtsam/3rdparty/GeographicLib/legacy/Fortran/geodesic.for index 6501363c1..aaef081c5 100644 --- a/gtsam/3rdparty/GeographicLib/legacy/Fortran/geodesic.for +++ b/gtsam/3rdparty/GeographicLib/legacy/Fortran/geodesic.for @@ -685,7 +685,7 @@ * Add the check for sig12 since zero length geodesics might yield m12 < * 0. Test case was * -* echo 20.001 0 20.001 0 | Geod -i +* echo 20.001 0 20.001 0 | GeodSolve -i * * In fact, we will have sig12 > pi/2 for meridional geodesic which is * not a shortest path. diff --git a/gtsam/3rdparty/GeographicLib/man/CMakeLists.txt b/gtsam/3rdparty/GeographicLib/man/CMakeLists.txt index d8af12998..ccdf213c3 100644 --- a/gtsam/3rdparty/GeographicLib/man/CMakeLists.txt +++ b/gtsam/3rdparty/GeographicLib/man/CMakeLists.txt @@ -34,7 +34,7 @@ foreach (TOOL ${TOOLS}) MAIN_DEPENDENCY ${TOOL}.pod) add_custom_command (OUTPUT ${TOOL}.1.html COMMAND - pod2html --noindex ${CMAKE_CURRENT_SOURCE_DIR}/${TOOL}.pod | + pod2html --title "'${TOOL}(1)'" --noindex ${CMAKE_CURRENT_SOURCE_DIR}/${TOOL}.pod | sed -e 's%%%' -e 's%\\\([^<>]*\\\)\(\\\(.\\\)\)%&%'g > ${TOOL}.1.html && cp ${TOOL}.1.html ../doc/html-stage/ diff --git a/gtsam/3rdparty/GeographicLib/man/CartConvert.1 b/gtsam/3rdparty/GeographicLib/man/CartConvert.1 index 02dfbfa5a..9a2f74660 100644 --- a/gtsam/3rdparty/GeographicLib/man/CartConvert.1 +++ b/gtsam/3rdparty/GeographicLib/man/CartConvert.1 @@ -124,7 +124,7 @@ .\" ======================================================================== .\" .IX Title "CARTCONVERT 1" -.TH CARTCONVERT 1 "2013-12-11" "GeographicLib 1.34" "GeographicLib Utilities" +.TH CARTCONVERT 1 "2014-03-13" "GeographicLib 1.35" "GeographicLib Utilities" .\" For nroff, turn off justification. Always turn off hyphenation; it makes .\" way too many mistakes in technical documents. .if n .ad l diff --git a/gtsam/3rdparty/GeographicLib/man/CartConvert.1.html b/gtsam/3rdparty/GeographicLib/man/CartConvert.1.html index 38dcd6af1..17ec77507 100644 --- a/gtsam/3rdparty/GeographicLib/man/CartConvert.1.html +++ b/gtsam/3rdparty/GeographicLib/man/CartConvert.1.html @@ -2,7 +2,7 @@ - +CartConvert(1) diff --git a/gtsam/3rdparty/GeographicLib/man/CartConvert.usage b/gtsam/3rdparty/GeographicLib/man/CartConvert.usage index f9f6f05ac..57374a78f 100644 --- a/gtsam/3rdparty/GeographicLib/man/CartConvert.usage +++ b/gtsam/3rdparty/GeographicLib/man/CartConvert.usage @@ -9,7 +9,7 @@ int usage(int retval, bool brief) { "For full documentation type:\n" " CartConvert --help\n" "or visit:\n" -" http://geographiclib.sf.net/1.34/CartConvert.1.html\n"; +" http://geographiclib.sf.net/1.35/CartConvert.1.html\n"; else ( retval ? std::cerr : std::cout ) << "Man page:\n" "NAME\n" diff --git a/gtsam/3rdparty/GeographicLib/man/ConicProj.1 b/gtsam/3rdparty/GeographicLib/man/ConicProj.1 index 85f65f9a6..997017d7f 100644 --- a/gtsam/3rdparty/GeographicLib/man/ConicProj.1 +++ b/gtsam/3rdparty/GeographicLib/man/ConicProj.1 @@ -124,7 +124,7 @@ .\" ======================================================================== .\" .IX Title "CONICPROJ 1" -.TH CONICPROJ 1 "2013-12-11" "GeographicLib 1.34" "GeographicLib Utilities" +.TH CONICPROJ 1 "2014-03-13" "GeographicLib 1.35" "GeographicLib Utilities" .\" For nroff, turn off justification. Always turn off hyphenation; it makes .\" way too many mistakes in technical documents. .if n .ad l diff --git a/gtsam/3rdparty/GeographicLib/man/ConicProj.1.html b/gtsam/3rdparty/GeographicLib/man/ConicProj.1.html index e4af0e6b5..9de3fd683 100644 --- a/gtsam/3rdparty/GeographicLib/man/ConicProj.1.html +++ b/gtsam/3rdparty/GeographicLib/man/ConicProj.1.html @@ -2,7 +2,7 @@ - +ConicProj(1) diff --git a/gtsam/3rdparty/GeographicLib/man/ConicProj.usage b/gtsam/3rdparty/GeographicLib/man/ConicProj.usage index 3fbcc4481..a84aa4fde 100644 --- a/gtsam/3rdparty/GeographicLib/man/ConicProj.usage +++ b/gtsam/3rdparty/GeographicLib/man/ConicProj.usage @@ -9,7 +9,7 @@ int usage(int retval, bool brief) { "For full documentation type:\n" " ConicProj --help\n" "or visit:\n" -" http://geographiclib.sf.net/1.34/ConicProj.1.html\n"; +" http://geographiclib.sf.net/1.35/ConicProj.1.html\n"; else ( retval ? std::cerr : std::cout ) << "Man page:\n" "NAME\n" diff --git a/gtsam/3rdparty/GeographicLib/man/GeoConvert.1 b/gtsam/3rdparty/GeographicLib/man/GeoConvert.1 index 0dfa11283..747b8cd3f 100644 --- a/gtsam/3rdparty/GeographicLib/man/GeoConvert.1 +++ b/gtsam/3rdparty/GeographicLib/man/GeoConvert.1 @@ -124,7 +124,7 @@ .\" ======================================================================== .\" .IX Title "GEOCONVERT 1" -.TH GEOCONVERT 1 "2013-12-11" "GeographicLib 1.34" "GeographicLib Utilities" +.TH GEOCONVERT 1 "2014-03-13" "GeographicLib 1.35" "GeographicLib Utilities" .\" For nroff, turn off justification. Always turn off hyphenation; it makes .\" way too many mistakes in technical documents. .if n .ad l diff --git a/gtsam/3rdparty/GeographicLib/man/GeoConvert.1.html b/gtsam/3rdparty/GeographicLib/man/GeoConvert.1.html index c46e2e7c0..5c117b49e 100644 --- a/gtsam/3rdparty/GeographicLib/man/GeoConvert.1.html +++ b/gtsam/3rdparty/GeographicLib/man/GeoConvert.1.html @@ -2,7 +2,7 @@ - +GeoConvert(1) diff --git a/gtsam/3rdparty/GeographicLib/man/GeoConvert.usage b/gtsam/3rdparty/GeographicLib/man/GeoConvert.usage index 7a0f8a852..19d78a3ef 100644 --- a/gtsam/3rdparty/GeographicLib/man/GeoConvert.usage +++ b/gtsam/3rdparty/GeographicLib/man/GeoConvert.usage @@ -9,7 +9,7 @@ int usage(int retval, bool brief) { "For full documentation type:\n" " GeoConvert --help\n" "or visit:\n" -" http://geographiclib.sf.net/1.34/GeoConvert.1.html\n"; +" http://geographiclib.sf.net/1.35/GeoConvert.1.html\n"; else ( retval ? std::cerr : std::cout ) << "Man page:\n" "NAME\n" diff --git a/gtsam/3rdparty/GeographicLib/man/GeodSolve.1 b/gtsam/3rdparty/GeographicLib/man/GeodSolve.1 index 06ca388e4..3bd025aa4 100644 --- a/gtsam/3rdparty/GeographicLib/man/GeodSolve.1 +++ b/gtsam/3rdparty/GeographicLib/man/GeodSolve.1 @@ -124,7 +124,7 @@ .\" ======================================================================== .\" .IX Title "GEODSOLVE 1" -.TH GEODSOLVE 1 "2013-12-11" "GeographicLib 1.34" "GeographicLib Utilities" +.TH GEODSOLVE 1 "2014-03-13" "GeographicLib 1.35" "GeographicLib Utilities" .\" For nroff, turn off justification. Always turn off hyphenation; it makes .\" way too many mistakes in technical documents. .if n .ad l diff --git a/gtsam/3rdparty/GeographicLib/man/GeodSolve.1.html b/gtsam/3rdparty/GeographicLib/man/GeodSolve.1.html index 262bccf3b..8af2af9e7 100644 --- a/gtsam/3rdparty/GeographicLib/man/GeodSolve.1.html +++ b/gtsam/3rdparty/GeographicLib/man/GeodSolve.1.html @@ -2,7 +2,7 @@ - +GeodSolve(1) diff --git a/gtsam/3rdparty/GeographicLib/man/GeodSolve.usage b/gtsam/3rdparty/GeographicLib/man/GeodSolve.usage index 48bc7261e..78ec73ec7 100644 --- a/gtsam/3rdparty/GeographicLib/man/GeodSolve.usage +++ b/gtsam/3rdparty/GeographicLib/man/GeodSolve.usage @@ -9,7 +9,7 @@ int usage(int retval, bool brief) { "For full documentation type:\n" " GeodSolve --help\n" "or visit:\n" -" http://geographiclib.sf.net/1.34/GeodSolve.1.html\n"; +" http://geographiclib.sf.net/1.35/GeodSolve.1.html\n"; else ( retval ? std::cerr : std::cout ) << "Man page:\n" "NAME\n" diff --git a/gtsam/3rdparty/GeographicLib/man/GeodesicProj.1 b/gtsam/3rdparty/GeographicLib/man/GeodesicProj.1 index a4cc6680e..9b87078a3 100644 --- a/gtsam/3rdparty/GeographicLib/man/GeodesicProj.1 +++ b/gtsam/3rdparty/GeographicLib/man/GeodesicProj.1 @@ -124,7 +124,7 @@ .\" ======================================================================== .\" .IX Title "GEODESICPROJ 1" -.TH GEODESICPROJ 1 "2013-12-11" "GeographicLib 1.34" "GeographicLib Utilities" +.TH GEODESICPROJ 1 "2014-03-13" "GeographicLib 1.35" "GeographicLib Utilities" .\" For nroff, turn off justification. Always turn off hyphenation; it makes .\" way too many mistakes in technical documents. .if n .ad l diff --git a/gtsam/3rdparty/GeographicLib/man/GeodesicProj.1.html b/gtsam/3rdparty/GeographicLib/man/GeodesicProj.1.html index c40ca5a4d..2b63cf7de 100644 --- a/gtsam/3rdparty/GeographicLib/man/GeodesicProj.1.html +++ b/gtsam/3rdparty/GeographicLib/man/GeodesicProj.1.html @@ -2,7 +2,7 @@ - +GeodesicProj(1) diff --git a/gtsam/3rdparty/GeographicLib/man/GeodesicProj.usage b/gtsam/3rdparty/GeographicLib/man/GeodesicProj.usage index 9a5ab2517..013fb7acf 100644 --- a/gtsam/3rdparty/GeographicLib/man/GeodesicProj.usage +++ b/gtsam/3rdparty/GeographicLib/man/GeodesicProj.usage @@ -9,7 +9,7 @@ int usage(int retval, bool brief) { "For full documentation type:\n" " GeodesicProj --help\n" "or visit:\n" -" http://geographiclib.sf.net/1.34/GeodesicProj.1.html\n"; +" http://geographiclib.sf.net/1.35/GeodesicProj.1.html\n"; else ( retval ? std::cerr : std::cout ) << "Man page:\n" "NAME\n" diff --git a/gtsam/3rdparty/GeographicLib/man/GeoidEval.1 b/gtsam/3rdparty/GeographicLib/man/GeoidEval.1 index 681a096f0..7c5086e1c 100644 --- a/gtsam/3rdparty/GeographicLib/man/GeoidEval.1 +++ b/gtsam/3rdparty/GeographicLib/man/GeoidEval.1 @@ -124,7 +124,7 @@ .\" ======================================================================== .\" .IX Title "GEOIDEVAL 1" -.TH GEOIDEVAL 1 "2013-12-11" "GeographicLib 1.34" "GeographicLib Utilities" +.TH GEOIDEVAL 1 "2014-03-13" "GeographicLib 1.35" "GeographicLib Utilities" .\" For nroff, turn off justification. Always turn off hyphenation; it makes .\" way too many mistakes in technical documents. .if n .ad l diff --git a/gtsam/3rdparty/GeographicLib/man/GeoidEval.1.html b/gtsam/3rdparty/GeographicLib/man/GeoidEval.1.html index 91f8f3610..ea131d8a7 100644 --- a/gtsam/3rdparty/GeographicLib/man/GeoidEval.1.html +++ b/gtsam/3rdparty/GeographicLib/man/GeoidEval.1.html @@ -2,7 +2,7 @@ - +GeoidEval(1) diff --git a/gtsam/3rdparty/GeographicLib/man/GeoidEval.usage b/gtsam/3rdparty/GeographicLib/man/GeoidEval.usage index 727811108..4fa144ca7 100644 --- a/gtsam/3rdparty/GeographicLib/man/GeoidEval.usage +++ b/gtsam/3rdparty/GeographicLib/man/GeoidEval.usage @@ -10,7 +10,7 @@ int usage(int retval, bool brief) { "For full documentation type:\n" " GeoidEval --help\n" "or visit:\n" -" http://geographiclib.sf.net/1.34/GeoidEval.1.html\n"; +" http://geographiclib.sf.net/1.35/GeoidEval.1.html\n"; else ( retval ? std::cerr : std::cout ) << "Man page:\n" "NAME\n" diff --git a/gtsam/3rdparty/GeographicLib/man/Gravity.1 b/gtsam/3rdparty/GeographicLib/man/Gravity.1 index 97e247b7f..96462b244 100644 --- a/gtsam/3rdparty/GeographicLib/man/Gravity.1 +++ b/gtsam/3rdparty/GeographicLib/man/Gravity.1 @@ -124,7 +124,7 @@ .\" ======================================================================== .\" .IX Title "GRAVITY 1" -.TH GRAVITY 1 "2013-12-11" "GeographicLib 1.34" "GeographicLib Utilities" +.TH GRAVITY 1 "2014-03-13" "GeographicLib 1.35" "GeographicLib Utilities" .\" For nroff, turn off justification. Always turn off hyphenation; it makes .\" way too many mistakes in technical documents. .if n .ad l diff --git a/gtsam/3rdparty/GeographicLib/man/Gravity.1.html b/gtsam/3rdparty/GeographicLib/man/Gravity.1.html index 3fbdba19b..cca8ad941 100644 --- a/gtsam/3rdparty/GeographicLib/man/Gravity.1.html +++ b/gtsam/3rdparty/GeographicLib/man/Gravity.1.html @@ -2,7 +2,7 @@ - +Gravity(1) diff --git a/gtsam/3rdparty/GeographicLib/man/Gravity.usage b/gtsam/3rdparty/GeographicLib/man/Gravity.usage index 20b522307..d60547f09 100644 --- a/gtsam/3rdparty/GeographicLib/man/Gravity.usage +++ b/gtsam/3rdparty/GeographicLib/man/Gravity.usage @@ -9,7 +9,7 @@ int usage(int retval, bool brief) { "For full documentation type:\n" " Gravity --help\n" "or visit:\n" -" http://geographiclib.sf.net/1.34/Gravity.1.html\n"; +" http://geographiclib.sf.net/1.35/Gravity.1.html\n"; else ( retval ? std::cerr : std::cout ) << "Man page:\n" "NAME\n" diff --git a/gtsam/3rdparty/GeographicLib/man/MagneticField.1 b/gtsam/3rdparty/GeographicLib/man/MagneticField.1 index 0659f2325..3d520cb6b 100644 --- a/gtsam/3rdparty/GeographicLib/man/MagneticField.1 +++ b/gtsam/3rdparty/GeographicLib/man/MagneticField.1 @@ -124,7 +124,7 @@ .\" ======================================================================== .\" .IX Title "MAGNETICFIELD 1" -.TH MAGNETICFIELD 1 "2013-12-11" "GeographicLib 1.34" "GeographicLib Utilities" +.TH MAGNETICFIELD 1 "2014-03-13" "GeographicLib 1.35" "GeographicLib Utilities" .\" For nroff, turn off justification. Always turn off hyphenation; it makes .\" way too many mistakes in technical documents. .if n .ad l diff --git a/gtsam/3rdparty/GeographicLib/man/MagneticField.1.html b/gtsam/3rdparty/GeographicLib/man/MagneticField.1.html index cac6794c8..dd1884602 100644 --- a/gtsam/3rdparty/GeographicLib/man/MagneticField.1.html +++ b/gtsam/3rdparty/GeographicLib/man/MagneticField.1.html @@ -2,7 +2,7 @@ - +MagneticField(1) diff --git a/gtsam/3rdparty/GeographicLib/man/MagneticField.usage b/gtsam/3rdparty/GeographicLib/man/MagneticField.usage index b6b89d0de..9f1ca8b9b 100644 --- a/gtsam/3rdparty/GeographicLib/man/MagneticField.usage +++ b/gtsam/3rdparty/GeographicLib/man/MagneticField.usage @@ -10,7 +10,7 @@ int usage(int retval, bool brief) { "For full documentation type:\n" " MagneticField --help\n" "or visit:\n" -" http://geographiclib.sf.net/1.34/MagneticField.1.html\n"; +" http://geographiclib.sf.net/1.35/MagneticField.1.html\n"; else ( retval ? std::cerr : std::cout ) << "Man page:\n" "NAME\n" diff --git a/gtsam/3rdparty/GeographicLib/man/Makefile.am b/gtsam/3rdparty/GeographicLib/man/Makefile.am index 8fae122f6..cad9e45ce 100644 --- a/gtsam/3rdparty/GeographicLib/man/Makefile.am +++ b/gtsam/3rdparty/GeographicLib/man/Makefile.am @@ -62,7 +62,7 @@ if HAVE_PODPROGS $(POD2MAN) $^ > $@ .pod.1.html: - pod2html --noindex $^ | $(PODFIX) > $@ + pod2html --noindex --tile "$*(1)" $^ | $(PODFIX) > $@ else diff --git a/gtsam/3rdparty/GeographicLib/man/Makefile.in b/gtsam/3rdparty/GeographicLib/man/Makefile.in index b434fd523..3399f753c 100644 --- a/gtsam/3rdparty/GeographicLib/man/Makefile.in +++ b/gtsam/3rdparty/GeographicLib/man/Makefile.in @@ -567,7 +567,7 @@ htmlman: $(HTMLMAN) @HAVE_PODPROGS_TRUE@ $(POD2MAN) $^ > $@ @HAVE_PODPROGS_TRUE@.pod.1.html: -@HAVE_PODPROGS_TRUE@ pod2html --noindex $^ | $(PODFIX) > $@ +@HAVE_PODPROGS_TRUE@ pod2html --noindex --tile "$*(1)" $^ | $(PODFIX) > $@ @HAVE_PODPROGS_FALSE@CartConvert.usage: @HAVE_PODPROGS_FALSE@ $(USAGECMD) diff --git a/gtsam/3rdparty/GeographicLib/man/Planimeter.1 b/gtsam/3rdparty/GeographicLib/man/Planimeter.1 index 28bbbf46f..07e1563cd 100644 --- a/gtsam/3rdparty/GeographicLib/man/Planimeter.1 +++ b/gtsam/3rdparty/GeographicLib/man/Planimeter.1 @@ -124,7 +124,7 @@ .\" ======================================================================== .\" .IX Title "PLANIMETER 1" -.TH PLANIMETER 1 "2013-12-11" "GeographicLib 1.34" "GeographicLib Utilities" +.TH PLANIMETER 1 "2014-03-13" "GeographicLib 1.35" "GeographicLib Utilities" .\" For nroff, turn off justification. Always turn off hyphenation; it makes .\" way too many mistakes in technical documents. .if n .ad l @@ -200,7 +200,7 @@ is allowed for \fIf\fR. (Also, if \fIf\fR > 1, the flattening is set to 1/\fIf\fR.) By default, the \s-1WGS84\s0 ellipsoid is used, \fIa\fR = 6378137 m, \&\fIf\fR = 1/298.257223563. If entering vertices as \s-1UTM/UPS\s0 or \s-1MGRS\s0 coordinates, use the default ellipsoid, since the conversion of these -coordinates to latitude and longitude uses the \s-1WGS84\s0 parameters. +coordinates to latitude and longitude always uses the \s-1WGS84\s0 parameters. .IP "\fB\-\-comment\-delimiter\fR" 4 .IX Item "--comment-delimiter" set the comment delimiter to \fIcommentdelim\fR (e.g., \*(L"#\*(R" or \*(L"//\*(R"). If diff --git a/gtsam/3rdparty/GeographicLib/man/Planimeter.1.html b/gtsam/3rdparty/GeographicLib/man/Planimeter.1.html index b86a5ecd7..7a6eb52c4 100644 --- a/gtsam/3rdparty/GeographicLib/man/Planimeter.1.html +++ b/gtsam/3rdparty/GeographicLib/man/Planimeter.1.html @@ -2,7 +2,7 @@ - +Planimeter(1) @@ -56,7 +56,7 @@
-e
-

specify the ellipsoid via a f; the equatorial radius is a and the flattening is f. Setting f = 0 results in a sphere. Specify f < 0 for a prolate ellipsoid. A simple fraction, e.g., 1/297, is allowed for f. (Also, if f > 1, the flattening is set to 1/f.) By default, the WGS84 ellipsoid is used, a = 6378137 m, f = 1/298.257223563. If entering vertices as UTM/UPS or MGRS coordinates, use the default ellipsoid, since the conversion of these coordinates to latitude and longitude uses the WGS84 parameters.

+

specify the ellipsoid via a f; the equatorial radius is a and the flattening is f. Setting f = 0 results in a sphere. Specify f < 0 for a prolate ellipsoid. A simple fraction, e.g., 1/297, is allowed for f. (Also, if f > 1, the flattening is set to 1/f.) By default, the WGS84 ellipsoid is used, a = 6378137 m, f = 1/298.257223563. If entering vertices as UTM/UPS or MGRS coordinates, use the default ellipsoid, since the conversion of these coordinates to latitude and longitude always uses the WGS84 parameters.

--comment-delimiter
diff --git a/gtsam/3rdparty/GeographicLib/man/Planimeter.pod b/gtsam/3rdparty/GeographicLib/man/Planimeter.pod index 828ddc100..d78afca8c 100644 --- a/gtsam/3rdparty/GeographicLib/man/Planimeter.pod +++ b/gtsam/3rdparty/GeographicLib/man/Planimeter.pod @@ -78,7 +78,7 @@ is allowed for I. (Also, if I E 1, the flattening is set to 1/I.) By default, the WGS84 ellipsoid is used, I = 6378137 m, I = 1/298.257223563. If entering vertices as UTM/UPS or MGRS coordinates, use the default ellipsoid, since the conversion of these -coordinates to latitude and longitude uses the WGS84 parameters. +coordinates to latitude and longitude always uses the WGS84 parameters. =item B<--comment-delimiter> diff --git a/gtsam/3rdparty/GeographicLib/man/Planimeter.usage b/gtsam/3rdparty/GeographicLib/man/Planimeter.usage index 3a45ad85d..b96b12f9f 100644 --- a/gtsam/3rdparty/GeographicLib/man/Planimeter.usage +++ b/gtsam/3rdparty/GeographicLib/man/Planimeter.usage @@ -9,7 +9,7 @@ int usage(int retval, bool brief) { "For full documentation type:\n" " Planimeter --help\n" "or visit:\n" -" http://geographiclib.sf.net/1.34/Planimeter.1.html\n"; +" http://geographiclib.sf.net/1.35/Planimeter.1.html\n"; else ( retval ? std::cerr : std::cout ) << "Man page:\n" "NAME\n" @@ -76,7 +76,7 @@ int usage(int retval, bool brief) { " default, the WGS84 ellipsoid is used, a = 6378137 m, f =\n" " 1/298.257223563. If entering vertices as UTM/UPS or MGRS\n" " coordinates, use the default ellipsoid, since the conversion of\n" -" these coordinates to latitude and longitude uses the WGS84\n" +" these coordinates to latitude and longitude always uses the WGS84\n" " parameters.\n" "\n" " --comment-delimiter\n" diff --git a/gtsam/3rdparty/GeographicLib/man/TransverseMercatorProj.1 b/gtsam/3rdparty/GeographicLib/man/TransverseMercatorProj.1 index 0b03ff6ca..3a5d3d115 100644 --- a/gtsam/3rdparty/GeographicLib/man/TransverseMercatorProj.1 +++ b/gtsam/3rdparty/GeographicLib/man/TransverseMercatorProj.1 @@ -124,7 +124,7 @@ .\" ======================================================================== .\" .IX Title "TRANSVERSEMERCATORPROJ 1" -.TH TRANSVERSEMERCATORPROJ 1 "2013-12-11" "GeographicLib 1.34" "GeographicLib Utilities" +.TH TRANSVERSEMERCATORPROJ 1 "2014-03-13" "GeographicLib 1.35" "GeographicLib Utilities" .\" For nroff, turn off justification. Always turn off hyphenation; it makes .\" way too many mistakes in technical documents. .if n .ad l diff --git a/gtsam/3rdparty/GeographicLib/man/TransverseMercatorProj.1.html b/gtsam/3rdparty/GeographicLib/man/TransverseMercatorProj.1.html index c43bf74da..8b28a7d87 100644 --- a/gtsam/3rdparty/GeographicLib/man/TransverseMercatorProj.1.html +++ b/gtsam/3rdparty/GeographicLib/man/TransverseMercatorProj.1.html @@ -2,7 +2,7 @@ - +TransverseMercatorProj(1) diff --git a/gtsam/3rdparty/GeographicLib/man/TransverseMercatorProj.usage b/gtsam/3rdparty/GeographicLib/man/TransverseMercatorProj.usage index 189744e8b..62d6045a7 100644 --- a/gtsam/3rdparty/GeographicLib/man/TransverseMercatorProj.usage +++ b/gtsam/3rdparty/GeographicLib/man/TransverseMercatorProj.usage @@ -9,7 +9,7 @@ int usage(int retval, bool brief) { "For full documentation type:\n" " TransverseMercatorProj --help\n" "or visit:\n" -" http://geographiclib.sf.net/1.34/TransverseMercatorProj.1.html\n"; +" http://geographiclib.sf.net/1.35/TransverseMercatorProj.1.html\n"; else ( retval ? std::cerr : std::cout ) << "Man page:\n" "NAME\n" diff --git a/gtsam/3rdparty/GeographicLib/matlab/geodesicdirect.cpp b/gtsam/3rdparty/GeographicLib/matlab/geodesicdirect.cpp index 770feecd0..06a35e607 100644 --- a/gtsam/3rdparty/GeographicLib/matlab/geodesicdirect.cpp +++ b/gtsam/3rdparty/GeographicLib/matlab/geodesicdirect.cpp @@ -2,7 +2,7 @@ * \file geodesicdirect.cpp * \brief Matlab mex file for geographic to UTM/UPS conversions * - * Copyright (c) Charles Karney (2010-2011) and licensed + * Copyright (c) Charles Karney (2010-2013) and licensed * under the MIT/X11 License. For more information, see * http://geographiclib.sourceforge.net/ **********************************************************************/ @@ -17,11 +17,51 @@ #include #include +#include #include using namespace std; using namespace GeographicLib; +template void +compute(double a, double f, mwSize m, const double* geodesic, + double* latlong, double* aux) { + const double* lat1 = geodesic; + const double* lon1 = geodesic + m; + const double* azi1 = geodesic + 2*m; + const double* s12 = geodesic + 3*m; + double* lat2 = latlong; + double* lon2 = latlong + m; + double* azi2 = latlong + 2*m; + double* a12 = NULL; + double* m12 = NULL; + double* M12 = NULL; + double* M21 = NULL; + double* S12 = NULL; + if (aux) { + a12 = aux; + m12 = aux + m; + M12 = aux + 2*m; + M21 = aux + 3*m; + S12 = aux + 4*m; + } + + const G g(a, f); + for (mwIndex i = 0; i < m; ++i) { + if (abs(lat1[i]) <= 90 && + lon1[i] >= -540 && lon1[i] < 540 && + azi1[i] >= -540 && azi1[i] < 540) { + if (aux) + a12[i] = g.Direct(lat1[i], lon1[i], azi1[i], s12[i], + lat2[i], lon2[i], azi2[i], + m12[i], M12[i], M21[i], S12[i]); + else + g.Direct(lat1[i], lon1[i], azi1[i], s12[i], + lat2[i], lon2[i], azi2[i]); + } + } +} + void mexFunction( int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[] ) { @@ -54,48 +94,22 @@ void mexFunction( int nlhs, mxArray* plhs[], mwSize m = mxGetM(prhs[0]); - double* lat1 = mxGetPr(prhs[0]); - double* lon1 = lat1 + m; - double* azi1 = lat1 + 2*m; - double* s12 = lat1 + 3*m; + const double* geodesic = mxGetPr(prhs[0]); - plhs[0] = mxCreateDoubleMatrix(m, 3, mxREAL); - double* lat2 = mxGetPr(plhs[0]); - std::fill(lat2, lat2 + 3*m, Math::NaN()); - double* lon2 = lat2 + m; - double* azi2 = lat2 + 2*m; - double* a12 = NULL; - double* m12 = NULL; - double* M12 = NULL; - double* M21 = NULL; - double* S12 = NULL; - bool aux = nlhs == 2; + double* latlong = mxGetPr(plhs[0] = mxCreateDoubleMatrix(m, 3, mxREAL)); + std::fill(latlong, latlong + 3*m, Math::NaN()); - if (aux) { - plhs[1] = mxCreateDoubleMatrix(m, 5, mxREAL); - a12 = mxGetPr(plhs[1]); - std::fill(a12, a12 + 5*m, Math::NaN()); - m12 = a12 + m; - M12 = a12 + 2*m; - M21 = a12 + 3*m; - S12 = a12 + 4*m; - } + double* aux = + nlhs == 2 ? mxGetPr(plhs[1] = mxCreateDoubleMatrix(m, 5, mxREAL)) : + NULL; + if (aux) + std::fill(aux, aux + 5*m, Math::NaN()); try { - const Geodesic g(a, f); - for (mwIndex i = 0; i < m; ++i) { - if (abs(lat1[i]) <= 90 && - lon1[i] >= -540 && lon1[i] < 540 && - azi1[i] >= -540 && azi1[i] < 540) { - if (aux) - a12[i] = g.Direct(lat1[i], lon1[i], azi1[i], s12[i], - lat2[i], lon2[i], azi2[i], m12[i], - M12[i], M21[i], S12[i]); - else - g.Direct(lat1[i], lon1[i], azi1[i], s12[i], - lat2[i], lon2[i], azi2[i]); - } - } + if (std::abs(f) <= 0.02) + compute(a, f, m, geodesic, latlong, aux); + else + compute(a, f, m, geodesic, latlong, aux); } catch (const std::exception& e) { mexErrMsgTxt(e.what()); diff --git a/gtsam/3rdparty/GeographicLib/matlab/geodesicinverse.cpp b/gtsam/3rdparty/GeographicLib/matlab/geodesicinverse.cpp index c3b1b2ace..f03877f18 100644 --- a/gtsam/3rdparty/GeographicLib/matlab/geodesicinverse.cpp +++ b/gtsam/3rdparty/GeographicLib/matlab/geodesicinverse.cpp @@ -2,7 +2,7 @@ * \file geodesicinverse.cpp * \brief Matlab mex file for geographic to UTM/UPS conversions * - * Copyright (c) Charles Karney (2010-2011) and licensed + * Copyright (c) Charles Karney (2010-2013) and licensed * under the MIT/X11 License. For more information, see * http://geographiclib.sourceforge.net/ **********************************************************************/ @@ -17,11 +17,50 @@ #include #include +#include #include using namespace std; using namespace GeographicLib; +template void +compute(double a, double f, mwSize m, const double* latlong, + double* geodesic, double* aux) { + const double* lat1 = latlong; + const double* lon1 = latlong + m; + const double* lat2 = latlong + 2*m; + const double* lon2 = latlong + 3*m; + double* azi1 = geodesic; + double* azi2 = geodesic + m; + double* s12 = geodesic + 2*m; + double* a12 = NULL; + double* m12 = NULL; + double* M12 = NULL; + double* M21 = NULL; + double* S12 = NULL; + if (aux) { + a12 = aux; + m12 = aux + m; + M12 = aux + 2*m; + M21 = aux + 3*m; + S12 = aux + 4*m; + } + + const G g(a, f); + for (mwIndex i = 0; i < m; ++i) { + if (abs(lat1[i]) <= 90 && lon1[i] >= -540 && lon1[i] < 540 && + abs(lat2[i]) <= 90 && lon2[i] >= -540 && lon2[i] < 540) { + if (aux) + a12[i] = g.Inverse(lat1[i], lon1[i], lat2[i], lon2[i], + s12[i], azi1[i], azi2[i], + m12[i], M12[i], M21[i], S12[i]); + else + g.Inverse(lat1[i], lon1[i], lat2[i], lon2[i], + s12[i], azi1[i], azi2[i]); + } + } +} + void mexFunction( int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[] ) { @@ -54,47 +93,22 @@ void mexFunction( int nlhs, mxArray* plhs[], mwSize m = mxGetM(prhs[0]); - double* lat1 = mxGetPr(prhs[0]); - double* lon1 = lat1 + m; - double* lat2 = lat1 + 2*m; - double* lon2 = lat1 + 3*m; + const double* latlong = mxGetPr(prhs[0]); - plhs[0] = mxCreateDoubleMatrix(m, 3, mxREAL); - double* azi1 = mxGetPr(plhs[0]); - std::fill(azi1, azi1 + 3*m, Math::NaN()); - double* azi2 = azi1 + m; - double* s12 = azi1 + 2*m; - double* a12 = NULL; - double* m12 = NULL; - double* M12 = NULL; - double* M21 = NULL; - double* S12 = NULL; - bool aux = nlhs == 2; + double* geodesic = mxGetPr(plhs[0] = mxCreateDoubleMatrix(m, 3, mxREAL)); + std::fill(geodesic, geodesic + 3*m, Math::NaN()); - if (aux) { - plhs[1] = mxCreateDoubleMatrix(m, 5, mxREAL); - a12 = mxGetPr(plhs[1]); - std::fill(a12, a12 + 5*m, Math::NaN()); - m12 = a12 + m; - M12 = a12 + 2*m; - M21 = a12 + 3*m; - S12 = a12 + 4*m; - } + double* aux = + nlhs == 2 ? mxGetPr(plhs[1] = mxCreateDoubleMatrix(m, 5, mxREAL)) : + NULL; + if (aux) + std::fill(aux, aux + 5*m, Math::NaN()); try { - const Geodesic g(a, f); - for (mwIndex i = 0; i < m; ++i) { - if (abs(lat1[i]) <= 90 && lon1[i] >= -540 && lon1[i] < 540 && - abs(lat2[i]) <= 90 && lon2[i] >= -540 && lon2[i] < 540) { - if (aux) - a12[i] = g.Inverse(lat1[i], lon1[i], lat2[i], lon2[i], - s12[i], azi1[i], azi2[i], - m12[i], M12[i], M21[i], S12[i]); - else - g.Inverse(lat1[i], lon1[i], lat2[i], lon2[i], - s12[i], azi1[i], azi2[i]); - } - } + if (std::abs(f) <= 0.02) + compute(a, f, m, latlong, geodesic, aux); + else + compute(a, f, m, latlong, geodesic, aux); } catch (const std::exception& e) { mexErrMsgTxt(e.what()); diff --git a/gtsam/3rdparty/GeographicLib/matlab/geodesicline.cpp b/gtsam/3rdparty/GeographicLib/matlab/geodesicline.cpp index 4de50e476..7382cc03a 100644 --- a/gtsam/3rdparty/GeographicLib/matlab/geodesicline.cpp +++ b/gtsam/3rdparty/GeographicLib/matlab/geodesicline.cpp @@ -2,7 +2,7 @@ * \file geodesicline.cpp * \brief Matlab mex file for geographic to UTM/UPS conversions * - * Copyright (c) Charles Karney (2010-2011) and licensed + * Copyright (c) Charles Karney (2010-2013) and licensed * under the MIT/X11 License. For more information, see * http://geographiclib.sourceforge.net/ **********************************************************************/ @@ -16,11 +16,40 @@ // -lGeographic geodesicline.cpp #include +#include #include using namespace std; using namespace GeographicLib; +template void +compute(double a, double f, double lat1, double lon1, double azi1, + mwSize m, const double* s12, double* latlong, double* aux) { + double* lat2 = latlong; + double* lon2 = latlong + m; + double* azi2 = latlong + 2*m; + double* a12 = NULL; + double* m12 = NULL; + double* M12 = NULL; + double* M21 = NULL; + double* S12 = NULL; + if (aux) { + a12 = aux; + m12 = aux + m; + M12 = aux + 2*m; + M21 = aux + 3*m; + S12 = aux + 4*m; + } + const G g(a, f); + const L l(g, lat1, lon1, azi1); + for (mwIndex i = 0; i < m; ++i) + if (aux) + a12[i] = l.Position(s12[i], lat2[i], lon2[i], azi2[i], + m12[i], M12[i], M21[i], S12[i]); + else + l.Position(s12[i], lat2[i], lon2[i], azi2[i]); +} + void mexFunction( int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[] ) { @@ -70,41 +99,24 @@ void mexFunction( int nlhs, mxArray* plhs[], double* s12 = mxGetPr(prhs[3]); - plhs[0] = mxCreateDoubleMatrix(m, 3, mxREAL); - double* lat2 = mxGetPr(plhs[0]); - double* lon2 = lat2 + m; - double* azi2 = lat2 + 2*m; - double* a12 = NULL; - double* m12 = NULL; - double* M12 = NULL; - double* M21 = NULL; - double* S12 = NULL; - bool aux = nlhs == 2; - - if (aux) { - plhs[1] = mxCreateDoubleMatrix(m, 5, mxREAL); - a12 = mxGetPr(plhs[1]); - m12 = a12 + m; - M12 = a12 + 2*m; - M21 = a12 + 3*m; - S12 = a12 + 4*m; - } + double* latlong = mxGetPr(plhs[0] = mxCreateDoubleMatrix(m, 3, mxREAL)); + double* aux = + nlhs == 2 ? mxGetPr(plhs[1] = mxCreateDoubleMatrix(m, 5, mxREAL)) : + NULL; try { - const Geodesic g(a, f); if (!(abs(lat1) <= 90)) throw GeographicErr("Invalid latitude"); if (!(lon1 >= -540 || lon1 < 540)) throw GeographicErr("Invalid longitude"); if (!(azi1 >= -540 || azi1 < 540)) throw GeographicErr("Invalid azimuth"); - const GeodesicLine l(g, lat1, lon1, azi1); - for (mwIndex i = 0; i < m; ++i) - if (aux) - a12[i] = l.Position(s12[i], lat2[i], lon2[i], azi2[i], - m12[i], M12[i], M21[i], S12[i]); - else - l.Position(s12[i], lat2[i], lon2[i], azi2[i]); + if (std::abs(f) <= 0.02) + compute + (a, f, lat1, lon1, azi1, m, s12, latlong, aux); + else + compute + (a, f, lat1, lon1, azi1, m, s12, latlong, aux); } catch (const std::exception& e) { mexErrMsgTxt(e.what()); diff --git a/gtsam/3rdparty/GeographicLib/pom.xml b/gtsam/3rdparty/GeographicLib/pom.xml new file mode 100644 index 000000000..2d40845a3 --- /dev/null +++ b/gtsam/3rdparty/GeographicLib/pom.xml @@ -0,0 +1,82 @@ + + 4.0.0 + + + com.sri.vt.majic + majic-parent + 0.1.9-SNAPSHOT + + + com.sri.vt + geographiclib + 1.35-SNAPSHOT + majic-cmake + GeographicLib + + + + os-windows + + + Windows + + + + ON + + + + os-linux + + + Linux + + + + OFF + + + + + + + artifactory-vt + SRI VT Repository + https://artifactory-vt.sarnoff.internal/artifactory/repo + + + + + + + com.sri.vt.majic + build-helper-maven-plugin + + + default-cmake-configure + + + ON + BOTH + OFF + OFF + ${build.netgeographiclib} + + + + + default-cmake-test + + + Release + + + + + + + + diff --git a/gtsam/3rdparty/GeographicLib/src/UTMUPS.cpp b/gtsam/3rdparty/GeographicLib/src/UTMUPS.cpp index 2abf43e11..88c151a6a 100644 --- a/gtsam/3rdparty/GeographicLib/src/UTMUPS.cpp +++ b/gtsam/3rdparty/GeographicLib/src/UTMUPS.cpp @@ -269,21 +269,19 @@ namespace GeographicLib { } void UTMUPS::DecodeEPSG(int epsg, int& zone, bool& northp) throw() { + northp = false; if (epsg >= epsg01N && epsg <= epsg60N) { - zone = epsg - epsg01N + 1; + zone = (epsg - epsg01N) + MINUTMZONE; northp = true; } else if (epsg == epsgN) { zone = UPS; northp = true; } else if (epsg >= epsg01S && epsg <= epsg60S) { - zone = epsg - epsg01S + 1; - northp = false; + zone = (epsg - epsg01S) + MINUTMZONE; } else if (epsg == epsgS) { zone = UPS; - northp = false; } else { zone = INVALID; - northp = false; } } @@ -292,7 +290,7 @@ namespace GeographicLib { if (zone == UPS) epsg = epsgS; else if (zone >= MINUTMZONE && zone <= MAXUTMZONE) - epsg = epsg + (zone - MINUTMZONE) + epsg01S; + epsg = (zone - MINUTMZONE) + epsg01S; if (epsg >= 0 && northp) epsg += epsgN - epsgS; return epsg; diff --git a/gtsam/3rdparty/GeographicLib/tools/CMakeLists.txt b/gtsam/3rdparty/GeographicLib/tools/CMakeLists.txt index 98233fe29..987505817 100644 --- a/gtsam/3rdparty/GeographicLib/tools/CMakeLists.txt +++ b/gtsam/3rdparty/GeographicLib/tools/CMakeLists.txt @@ -22,6 +22,12 @@ foreach (TOOL ${TOOLS}) endforeach () +if (MSVC OR CMAKE_CONFIGURATION_TYPES) + # Add _d suffix for your debug versions of the tools + set_target_properties (${TOOLS} PROPERTIES + DEBUG_POSTFIX ${CMAKE_DEBUG_POSTFIX}) +endif () + if (MSVC) get_target_property (_LIBTYPE ${PROJECT_LIBRARIES} TYPE) if (_LIBTYPE STREQUAL "SHARED_LIBRARY") @@ -62,21 +68,23 @@ enable_testing () # Here are the tests. They consists of calling the various tools with # --input-string and matching the output against regular expressions. -add_test (GeoConvert0 GeoConvert -p -3 -m --input-string "33.3 44.4") +add_test (NAME GeoConvert0 + COMMAND GeoConvert -p -3 -m --input-string "33.3 44.4") set_tests_properties (GeoConvert0 PROPERTIES PASS_REGULAR_EXPRESSION "38SMB4484") -add_test (GeoConvert1 GeoConvert -d --input-string "38smb") +add_test (NAME GeoConvert1 COMMAND GeoConvert -d --input-string "38smb") set_tests_properties (GeoConvert1 PROPERTIES PASS_REGULAR_EXPRESSION "32d59'14\\.1\"N 044d27'53\\.4\"E") -add_test (GeoConvert2 GeoConvert -p -2 --input-string "30d30'30\" 30.50833") +add_test (NAME GeoConvert2 + COMMAND GeoConvert -p -2 --input-string "30d30'30\" 30.50833") set_tests_properties (GeoConvert2 PROPERTIES PASS_REGULAR_EXPRESSION "30\\.508 30\\.508") -add_test (GeoConvert3 GeoConvert --junk) +add_test (NAME GeoConvert3 COMMAND GeoConvert --junk) set_tests_properties (GeoConvert3 PROPERTIES WILL_FAIL ON) -add_test (GeoConvert4 GeoConvert --input-string garbage) +add_test (NAME GeoConvert4 COMMAND GeoConvert --input-string garbage) set_tests_properties (GeoConvert4 PROPERTIES WILL_FAIL ON) # Check fix for DMS::Decode bug fixed on 2011-03-22 -add_test (GeoConvert5 GeoConvert --input-string "5d. 0") +add_test (NAME GeoConvert5 COMMAND GeoConvert --input-string "5d. 0") set_tests_properties (GeoConvert5 PROPERTIES WILL_FAIL ON) if (NOT (MSVC AND MSVC_VERSION MATCHES "1[78]..")) # Check fix for DMS::Decode double rounding bug fixed on 2012-11-15 @@ -84,48 +92,49 @@ if (NOT (MSVC AND MSVC_VERSION MATCHES "1[78]..")) # http://connect.microsoft.com/VisualStudio/feedback/details/776287 # OK to skip this test for these compilers because this is a question # of accuracy of the least significant bit. - add_test (GeoConvert6 GeoConvert -p 9 + add_test (NAME GeoConvert6 COMMAND GeoConvert -p 9 --input-string "0 179.99999999999998578") set_tests_properties (GeoConvert6 PROPERTIES PASS_REGULAR_EXPRESSION "179\\.9999999999999[7-9]") endif () -add_test (GeodSolve0 +add_test (NAME GeodSolve0 COMMAND GeodSolve -i -p 0 --input-string "40.6 -73.8 49d01'N 2d33'E") set_tests_properties (GeodSolve0 PROPERTIES PASS_REGULAR_EXPRESSION "53\\.47022 111\\.59367 5853226") -add_test (GeodSolve1 +add_test (NAME GeodSolve1 COMMAND GeodSolve -p 0 --input-string "40d38'23\"N 073d46'44\"W 53d30' 5850e3") set_tests_properties (GeodSolve1 PROPERTIES PASS_REGULAR_EXPRESSION "49\\.01467 2\\.56106 111\\.62947") # Check fix for antipodal prolate bug found 2010-09-04 -add_test (GeodSolve2 +add_test (NAME GeodSolve2 COMMAND GeodSolve -i -p 0 -e 6.4e6 -1/150 --input-string "0.07476 0 -0.07476 180") set_tests_properties (GeodSolve2 PROPERTIES PASS_REGULAR_EXPRESSION "90\\.00078 90\\.00078 20106193") # Another check for similar bug -add_test (GeodSolve3 +add_test (NAME GeodSolve3 COMMAND GeodSolve -i -p 0 -e 6.4e6 -1/150 --input-string "0.1 0 -0.1 180") set_tests_properties (GeodSolve3 PROPERTIES PASS_REGULAR_EXPRESSION "90\\.00105 90\\.00105 20106193") # Check fix for short line bug found 2010-05-21 -add_test (GeodSolve4 +add_test (NAME GeodSolve4 COMMAND GeodSolve -i --input-string "36.493349428792 0 36.49334942879201 .0000008") set_tests_properties (GeodSolve4 PROPERTIES PASS_REGULAR_EXPRESSION ".* .* 0\\.072") # Check fix for point2=pole bug found 2010-05-03 (but only with long double) -add_test (GeodSolve5 GeodSolve -p 0 --input-string "0.01777745589997 30 0 10e6") +add_test (NAME GeodSolve5 + COMMAND GeodSolve -p 0 --input-string "0.01777745589997 30 0 10e6") set_tests_properties (GeodSolve5 PROPERTIES PASS_REGULAR_EXPRESSION "90\\.00000 -150\\.00000 -180\\.00000;90\\.00000 30\\.00000 0\\.00000") # Check fix for volatile sbet12a bug found 2011-06-25 (gcc 4.4.4 x86 -O3) # Found again on 2012-03-27 with tdm-mingw32 (g++ 4.6.1). -add_test (GeodSolve6 GeodSolve -i --input-string +add_test (NAME GeodSolve6 COMMAND GeodSolve -i --input-string "88.202499451857 0 -88.202499451857 179.981022032992859592") -add_test (GeodSolve7 GeodSolve -i --input-string +add_test (NAME GeodSolve7 COMMAND GeodSolve -i --input-string "89.262080389218 0 -89.262080389218 179.992207982775375662") -add_test (GeodSolve8 GeodSolve -i --input-string +add_test (NAME GeodSolve8 COMMAND GeodSolve -i --input-string "89.333123580033 0 -89.333123580032997687 179.99295812360148422") set_tests_properties (GeodSolve6 PROPERTIES PASS_REGULAR_EXPRESSION ".* .* 20003898.214") @@ -135,39 +144,42 @@ set_tests_properties (GeodSolve8 PROPERTIES PASS_REGULAR_EXPRESSION ".* .* 20003926.881") # Check fix for volatile x bug found 2011-06-25 (gcc 4.4.4 x86 -O3) -add_test (GeodSolve9 GeodSolve -i --input-string +add_test (NAME GeodSolve9 COMMAND GeodSolve -i --input-string "56.320923501171 0 -56.320923501171 179.664747671772880215") set_tests_properties (GeodSolve9 PROPERTIES PASS_REGULAR_EXPRESSION ".* .* 19993558.287") # Check fix for adjust tol1_ bug found 2011-06-25 (Visual Studio 10 rel + debug) -add_test (GeodSolve10 GeodSolve -i --input-string +add_test (NAME GeodSolve10 COMMAND GeodSolve -i --input-string "52.784459512564 0 -52.784459512563990912 179.634407464943777557") set_tests_properties (GeodSolve10 PROPERTIES PASS_REGULAR_EXPRESSION ".* .* 19991596.095") # Check fix for bet2 = -bet1 bug found 2011-06-25 (Visual Studio 10 rel + debug) -add_test (GeodSolve11 GeodSolve -i --input-string +add_test (NAME GeodSolve11 COMMAND GeodSolve -i --input-string "48.522876735459 0 -48.52287673545898293 179.599720456223079643") set_tests_properties (GeodSolve11 PROPERTIES PASS_REGULAR_EXPRESSION ".* .* 19989144.774") # Check fix for inverse geodesics on extreme prolate/oblate ellipsoids # Reported 2012-08-29 Stefan Guenther ; fixed 2012-10-07 -add_test (GeodSolve12 +add_test (NAME GeodSolve12 COMMAND GeodSolve -i -e 89.8 -1.83 -p 0 --input-string "0 0 -10 160") -add_test (GeodSolve13 +add_test (NAME GeodSolve13 COMMAND GeodSolve -i -e 89.8 -1.83 -p 0 --input-string "0 0 -10 160" -E) set_tests_properties (GeodSolve12 GeodSolve13 PROPERTIES PASS_REGULAR_EXPRESSION "120\\.27.* 105\\.15.* 267") # Check fix for pole-encircling bug found 2011-03-16 -add_test (Planimeter0 Planimeter --input-string "89 0;89 90;89 180;89 270") -add_test (Planimeter1 +add_test (NAME Planimeter0 + COMMAND Planimeter --input-string "89 0;89 90;89 180;89 270") +add_test (NAME Planimeter1 COMMAND Planimeter -r --input-string "-89 0;-89 90;-89 180;-89 270") -add_test (Planimeter2 Planimeter --input-string "0 -1;-1 0;0 1;1 0") -add_test (Planimeter3 Planimeter --input-string "90 0; 0 0; 0 90") -add_test (Planimeter4 Planimeter -l --input-string "90 0; 0 0; 0 90") +add_test (NAME Planimeter2 + COMMAND Planimeter --input-string "0 -1;-1 0;0 1;1 0") +add_test (NAME Planimeter3 COMMAND Planimeter --input-string "90 0; 0 0; 0 90") +add_test (NAME Planimeter4 + COMMAND Planimeter -l --input-string "90 0; 0 0; 0 90") set_tests_properties (Planimeter0 PROPERTIES PASS_REGULAR_EXPRESSION "4 631819\\.8745[0-9]+ 2495230567[78]\\.[0-9]+") @@ -182,56 +194,65 @@ set_tests_properties (Planimeter3 set_tests_properties (Planimeter4 PROPERTIES PASS_REGULAR_EXPRESSION "3 20020719\\.[0-9]+") # Check fix for Planimeter pole crossing bug found 2011-06-24 -add_test (Planimeter5 Planimeter --input-string "89,0.1;89,90.1;89,-179.9") +add_test (NAME Planimeter5 + COMMAND Planimeter --input-string "89,0.1;89,90.1;89,-179.9") set_tests_properties (Planimeter5 PROPERTIES PASS_REGULAR_EXPRESSION "3 539297\\.[0-9]+ 1247615283[89]\\.[0-9]+") # Check fix for Planimeter lon12 rounding bug found 2012-12-03 -add_test (Planimeter6 Planimeter --input-string "9 -0.00000000000001;9 180;9 0") -add_test (Planimeter7 Planimeter --input-string "9 0.00000000000001;9 0;9 180") -add_test (Planimeter8 Planimeter --input-string "9 0.00000000000001;9 180;9 0") -add_test (Planimeter9 Planimeter --input-string "9 -0.00000000000001;9 0;9 180") +add_test (NAME Planimeter6 + COMMAND Planimeter --input-string "9 -0.00000000000001;9 180;9 0") +add_test (NAME Planimeter7 + COMMAND Planimeter --input-string "9 0.00000000000001;9 0;9 180") +add_test (NAME Planimeter8 + COMMAND Planimeter --input-string "9 0.00000000000001;9 180;9 0") +add_test (NAME Planimeter9 + COMMAND Planimeter --input-string "9 -0.00000000000001;9 0;9 180") set_tests_properties (Planimeter6 Planimeter7 Planimeter8 Planimeter9 PROPERTIES PASS_REGULAR_EXPRESSION "3 36026861\\.[0-9]+ -?0.0[0-9]+") # Check fix for AlbersEqualArea::Reverse bug found 2011-05-01 -add_test (ConicProj0 +add_test (NAME ConicProj0 COMMAND ConicProj -a 40d58 39d56 -l 77d45W -r --input-string "220e3 -52e3") set_tests_properties (ConicProj0 PROPERTIES PASS_REGULAR_EXPRESSION "39\\.95[0-9]+ -75\\.17[0-9]+ 1\\.67[0-9]+ 0\\.99[0-9]+") # Check fix for AlbersEqualArea prolate bug found 2012-05-15 -add_test (ConicProj1 +add_test (NAME ConicProj1 COMMAND ConicProj -a 0 0 -e 6.4e6 -0.5 -r --input-string "0 8605508") set_tests_properties (ConicProj1 PROPERTIES PASS_REGULAR_EXPRESSION "^85\\.00") # Check fix for LambertConformalConic::Forward bug found 2012-07-14 -add_test (ConicProj2 ConicProj -c -30 -30 --input-string "-30 0") +add_test (NAME ConicProj2 COMMAND ConicProj -c -30 -30 --input-string "-30 0") set_tests_properties (ConicProj2 PROPERTIES PASS_REGULAR_EXPRESSION "^-?0\\.0+ -?0\\.0+ -?0\\.0+ 1\\.0+") # Check fixes for LambertConformalConic::Reverse overflow bugs found 2012-07-14 -add_test (ConicProj3 ConicProj -r -c 0 0 --input-string "1113195 -1e10") +add_test (NAME ConicProj3 + COMMAND ConicProj -r -c 0 0 --input-string "1113195 -1e10") set_tests_properties (ConicProj3 PROPERTIES PASS_REGULAR_EXPRESSION "^-90\\.0+ 10\\.00[0-9]+ ") -add_test (ConicProj4 ConicProj -r -c 0 0 --input-string "1113195 inf") +add_test (NAME ConicProj4 + COMMAND ConicProj -r -c 0 0 --input-string "1113195 inf") set_tests_properties (ConicProj4 PROPERTIES PASS_REGULAR_EXPRESSION "^90\\.0+ 10\\.00[0-9]+ ") -add_test (ConicProj5 ConicProj -r -c 45 45 --input-string "0 -1e100") +add_test (NAME ConicProj5 + COMMAND ConicProj -r -c 45 45 --input-string "0 -1e100") set_tests_properties (ConicProj5 PROPERTIES PASS_REGULAR_EXPRESSION "^-90\\.0+ -?0\\.00[0-9]+ ") -add_test (ConicProj6 ConicProj -r -c 45 45 --input-string "0 -inf") +add_test (NAME ConicProj6 COMMAND ConicProj -r -c 45 45 --input-string "0 -inf") set_tests_properties (ConicProj6 PROPERTIES PASS_REGULAR_EXPRESSION "^-90\\.0+ -?0\\.00[0-9]+ ") -add_test (ConicProj7 ConicProj -r -c 90 90 --input-string "0 -1e150") +add_test (NAME ConicProj7 + COMMAND ConicProj -r -c 90 90 --input-string "0 -1e150") set_tests_properties (ConicProj7 PROPERTIES PASS_REGULAR_EXPRESSION "^-90\\.0+ -?0\\.00[0-9]+ ") -add_test (ConicProj8 ConicProj -r -c 90 90 --input-string "0 -inf") +add_test (NAME ConicProj8 COMMAND ConicProj -r -c 90 90 --input-string "0 -inf") set_tests_properties (ConicProj8 PROPERTIES PASS_REGULAR_EXPRESSION "^-90\\.0+ -?0\\.00[0-9]+ ") -add_test (CartConvert0 +add_test (NAME CartConvert0 COMMAND CartConvert -e 6.4e6 1/100 -r --input-string "10e3 0 1e3") -add_test (CartConvert1 +add_test (NAME CartConvert1 COMMAND CartConvert -e 6.4e6 -1/100 -r --input-string "1e3 0 10e3") set_tests_properties (CartConvert0 PROPERTIES PASS_REGULAR_EXPRESSION @@ -242,60 +263,60 @@ set_tests_properties (CartConvert1 # Test fix to bad meridian convergence at pole with # TransverseMercatorExact found 2013-06-26 -add_test (TransverseMercatorProj0 +add_test (NAME TransverseMercatorProj0 COMMAND TransverseMercatorProj -k 1 --input-string "90 75") set_tests_properties (TransverseMercatorProj0 PROPERTIES PASS_REGULAR_EXPRESSION "^0\\.0+ 10001965\\.7293[0-9]+ 75\\.0+ 1\\.0+") # Test fix to bad scale at pole with TransverseMercatorExact # found 2013-06-30 (quarter meridian = 10001965.7293127228128889202m) -add_test (TransverseMercatorProj1 +add_test (NAME TransverseMercatorProj1 COMMAND TransverseMercatorProj -k 1 -r --input-string "0 10001965.7293127228") set_tests_properties (TransverseMercatorProj1 PROPERTIES PASS_REGULAR_EXPRESSION "90\\.0+ 0\\.0+ 0\\.0+ (1\\.0+|0\\.9999+)") if (EXISTS ${GEOGRAPHICLIB_DATA}/geoids/egm96-5.pgm) # Check fix for single-cell cache bug found 2010-11-23 - add_test (GeoidEval0 GeoidEval -n egm96-5 --input-string "0d1 0d1;0d4 0d4") + add_test (NAME GeoidEval0 + COMMAND GeoidEval -n egm96-5 --input-string "0d1 0d1;0d4 0d4") set_tests_properties (GeoidEval0 - PROPERTIES PASS_REGULAR_EXPRESSION "^17\\.1[56].. -17\\.1[45]..") + PROPERTIES PASS_REGULAR_EXPRESSION "^17\\.1[56]..\n17\\.1[45]..") endif () if (EXISTS ${GEOGRAPHICLIB_DATA}/magnetic/wmm2010.wmm) # Test case from WMM2010_Report.pdf, Sec 1.5, pp 14-15: # t = 2012.5, lat = -80, lon = 240, h = 100e3 - add_test (MagneticField0 + add_test (NAME MagneticField0 COMMAND MagneticField -n wmm2010 -p 10 -r --input-string "2012.5 -80 240 100e3") - add_test (MagneticField1 + add_test (NAME MagneticField1 COMMAND MagneticField -n wmm2010 -p 10 -r -t 2012.5 --input-string "-80 240 100e3") - add_test (MagneticField2 + add_test (NAME MagneticField2 COMMAND MagneticField -n wmm2010 -p 10 -r -c 2012.5 -80 100e3 --input-string "240") set_tests_properties (MagneticField0 PROPERTIES PASS_REGULAR_EXPRESSION - " 5535\\.5249148687 14765\\.3703243050 -50625\\.9305478794 .* -.* 20\\.4904268023 1\\.0272592716 83\\.5313962281 ") + " 5535\\.5249148687 14765\\.3703243050 -50625\\.9305478794 .*\n.* 20\\.4904268023 1\\.0272592716 83\\.5313962281 ") set_tests_properties (MagneticField1 PROPERTIES PASS_REGULAR_EXPRESSION - " 5535\\.5249148687 14765\\.3703243050 -50625\\.9305478794 .* -.* 20\\.4904268023 1\\.0272592716 83\\.5313962281 ") + " 5535\\.5249148687 14765\\.3703243050 -50625\\.9305478794 .*\n.* 20\\.4904268023 1\\.0272592716 83\\.5313962281 ") set_tests_properties (MagneticField2 PROPERTIES PASS_REGULAR_EXPRESSION - " 5535\\.5249148687 14765\\.3703243050 -50625\\.9305478794 .* -.* 20\\.4904268023 1\\.0272592716 83\\.5313962281 ") + " 5535\\.5249148687 14765\\.3703243050 -50625\\.9305478794 .*\n.* 20\\.4904268023 1\\.0272592716 83\\.5313962281 ") endif () if (EXISTS ${GEOGRAPHICLIB_DATA}/gravity/egm2008.egm) # Verify no overflow at poles with high degree model - add_test (Gravity0 Gravity -n egm2008 -p 6 --input-string "90 110 0") + add_test (NAME Gravity0 + COMMAND Gravity -n egm2008 -p 6 --input-string "90 110 0") set_tests_properties (Gravity0 PROPERTIES PASS_REGULAR_EXPRESSION "-0\\.000146 0\\.000078 -9\\.832294") # Check fix for invR bug in GravityCircle found by Mathieu Peyrega on # 2013-04-09 - add_test (Gravity1 Gravity -n egm2008 -A -c -18 4000 --input-string "-86") + add_test (NAME Gravity1 + COMMAND Gravity -n egm2008 -A -c -18 4000 --input-string "-86") set_tests_properties (Gravity1 PROPERTIES PASS_REGULAR_EXPRESSION "-7\\.438 1\\.305 -1\\.563") - add_test (Gravity2 Gravity -n egm2008 -D -c -18 4000 --input-string "-86") + add_test (NAME Gravity2 + COMMAND Gravity -n egm2008 -D -c -18 4000 --input-string "-86") set_tests_properties (Gravity2 PROPERTIES PASS_REGULAR_EXPRESSION "7\\.404 -6\\.168 7\\.616") endif () diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index 56d2b304c..9fac3b00b 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -5,6 +5,7 @@ set (gtsam_subdirs base geometry inference + symbolic discrete linear nonlinear @@ -20,14 +21,11 @@ add_subdirectory(3rdparty) # build convenience library set (3rdparty_srcs + ${eigen_headers} # Set by 3rdparty/CMakeLists.txt ${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/CCOLAMD/Source/ccolamd.c ${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/CCOLAMD/Source/ccolamd_global.c ${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/UFconfig/UFconfig.c) gtsam_assign_source_folders("${3rdparty_srcs}") # Create MSVC structure -if (GTSAM_BUILD_CONVENIENCE_LIBRARIES) - message(STATUS "Building Convenience Library: ccolamd") - add_library(ccolamd STATIC ${3rdparty_srcs}) -endif() # To exclude a source from the library build (in any subfolder) # Add the full name to this list, as in the following example @@ -53,17 +51,12 @@ install(FILES ${gtsam_core_headers} DESTINATION include/gtsam) # assemble core libaries foreach(subdir ${gtsam_subdirs}) # Build convenience libraries - file(GLOB subdir_cpp_srcs "${subdir}/*.cpp") - file(GLOB subdir_headers "${subdir}/*.h") - list(REMOVE_ITEM subdir_cpp_srcs ${excluded_sources}) - list(REMOVE_ITEM subdir_headers ${excluded_headers}) - set(subdir_srcs ${subdir_cpp_srcs} ${subdir_headers}) # Include header files so they show up in Visual Studio + file(GLOB_RECURSE subdir_srcs "${subdir}/*.cpp" "${subdir}/*.h") # Include header files so they show up in Visual Studio + list(REMOVE_ITEM subdir_srcs ${excluded_sources}) + file(GLOB subdir_test_files "${subdir}/tests/*") + list(REMOVE_ITEM subdir_srcs ${subdir_test_files}) # Remove test files from sources compiled into library gtsam_assign_source_folders("${subdir_srcs}") # Create MSVC structure set(${subdir}_srcs ${subdir_srcs}) - if (GTSAM_BUILD_CONVENIENCE_LIBRARIES AND (subdir_cpp_srcs)) # Only build convenience library if sources exist - message(STATUS "Building Convenience Library: ${subdir}") - add_library(${subdir} STATIC ${subdir_srcs}) - endif() # Build local library and tests message(STATUS "Building ${subdir}") @@ -77,6 +70,7 @@ set(gtsam_srcs ${base_srcs} ${geometry_srcs} ${inference_srcs} + ${symbolic_srcs} ${discrete_srcs} ${linear_srcs} ${nonlinear_srcs} @@ -84,6 +78,13 @@ set(gtsam_srcs ${navigation_srcs} ${gtsam_core_headers} ) + +# Generate and install config and dllexport files +configure_file(config.h.in config.h) +set(library_name GTSAM) # For substitution in dllexport.h.in +configure_file("${PROJECT_SOURCE_DIR}/cmake/dllexport.h.in" "dllexport.h") +list(APPEND gtsam_srcs "${PROJECT_BINARY_DIR}/gtsam/config.h" "${PROJECT_BINARY_DIR}/gtsam/dllexport.h") +install(FILES "${PROJECT_BINARY_DIR}/gtsam/config.h" "${PROJECT_BINARY_DIR}/gtsam/dllexport.h" DESTINATION include/gtsam) # Versions set(gtsam_version ${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}) @@ -94,42 +95,40 @@ message(STATUS "Install prefix: ${CMAKE_INSTALL_PREFIX}") # build shared and static versions of the library if (GTSAM_BUILD_STATIC_LIBRARY) message(STATUS "Building GTSAM - static") - add_library(gtsam-static STATIC ${gtsam_srcs}) - target_link_libraries(gtsam-static ${GTSAM_BOOST_LIBRARIES}) - set_target_properties(gtsam-static PROPERTIES + add_library(gtsam STATIC ${gtsam_srcs}) + target_link_libraries(gtsam ${GTSAM_BOOST_LIBRARIES} ${GTSAM_ADDITIONAL_LIBRARIES}) + set_target_properties(gtsam PROPERTIES OUTPUT_NAME gtsam CLEAN_DIRECT_OUTPUT 1 VERSION ${gtsam_version} SOVERSION ${gtsam_soversion}) if(WIN32) # Add 'lib' prefix to static library to avoid filename collision with shared library - set_target_properties(gtsam-static PROPERTIES + set_target_properties(gtsam PROPERTIES PREFIX "lib" COMPILE_DEFINITIONS GTSAM_IMPORT_STATIC) endif() - install(TARGETS gtsam-static EXPORT GTSAM-exports ARCHIVE DESTINATION lib) - list(APPEND GTSAM_EXPORTED_TARGETS gtsam-static) + install(TARGETS gtsam EXPORT GTSAM-exports ARCHIVE DESTINATION lib) + list(APPEND GTSAM_EXPORTED_TARGETS gtsam) set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE) -endif () - -if (GTSAM_BUILD_SHARED_LIBRARY) +else() message(STATUS "Building GTSAM - shared") - add_library(gtsam-shared SHARED ${gtsam_srcs}) - target_link_libraries(gtsam-shared ${GTSAM_BOOST_LIBRARIES}) - set_target_properties(gtsam-shared PROPERTIES + add_library(gtsam SHARED ${gtsam_srcs}) + target_link_libraries(gtsam ${GTSAM_BOOST_LIBRARIES} ${GTSAM_ADDITIONAL_LIBRARIES}) + set_target_properties(gtsam PROPERTIES OUTPUT_NAME gtsam CLEAN_DIRECT_OUTPUT 1 VERSION ${gtsam_version} SOVERSION ${gtsam_soversion}) if(WIN32) - set_target_properties(gtsam-shared PROPERTIES + set_target_properties(gtsam PROPERTIES PREFIX "" DEFINE_SYMBOL GTSAM_EXPORTS RUNTIME_OUTPUT_DIRECTORY "${PROJECT_BINARY_DIR}/bin") endif() - install(TARGETS gtsam-shared EXPORT GTSAM-exports LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin) - list(APPEND GTSAM_EXPORTED_TARGETS gtsam-shared) + install(TARGETS gtsam EXPORT GTSAM-exports LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin) + list(APPEND GTSAM_EXPORTED_TARGETS gtsam) set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE) -endif(GTSAM_BUILD_SHARED_LIBRARY) +endif() # Set dataset paths set_property(SOURCE "${CMAKE_CURRENT_SOURCE_DIR}/slam/dataset.cpp" @@ -139,31 +138,23 @@ set_property(SOURCE "${CMAKE_CURRENT_SOURCE_DIR}/slam/dataset.cpp" # Special cases if(MSVC) - set_property(SOURCE "${CMAKE_CURRENT_SOURCE_DIR}/slam/serialization.cpp" + set_property(SOURCE + "${CMAKE_CURRENT_SOURCE_DIR}/slam/serialization.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/nonlinear/ISAM2.cpp" APPEND PROPERTY COMPILE_FLAGS "/bigobj") endif() - -# Generate and install config file -configure_file(config.h.in config.h) -install(FILES ${PROJECT_BINARY_DIR}/gtsam/config.h DESTINATION include/gtsam) # Create the matlab toolbox for the gtsam library if (GTSAM_INSTALL_MATLAB_TOOLBOX) # Set up codegen include(GtsamMatlabWrap) - # Choose include flags depending on build process - set(MEX_INCLUDE_ROOT ${GTSAM_SOURCE_ROOT_DIR}) - set(MEX_LIB_ROOT ${PROJECT_BINARY_DIR}) # FIXME: is this used? - set(GTSAM_LIB_DIR ${MEX_LIB_ROOT}/gtsam) # FIXME: is this used? - # Generate, build and install toolbox - set(mexFlags ${GTSAM_BUILD_MEX_BINARY_FLAGS} -I${MEX_INCLUDE_ROOT} -I${Boost_INCLUDE_DIR} -I${PROJECT_BINARY_DIR}) - if("${gtsam-default}" STREQUAL "gtsam-static") + set(mexFlags "${GTSAM_BUILD_MEX_BINARY_FLAGS}") + if(GTSAM_BUILD_STATIC_LIBRARY) list(APPEND mexFlags -DGTSAM_IMPORT_STATIC) endif() - # Macro to handle details of setting up targets - # FIXME: issue with dependency between wrap_gtsam and wrap_gtsam_build, only shows up on CMake 2.8.3 - wrap_library(gtsam "${mexFlags}" "../" "") + # Wrap + wrap_and_install_library(../gtsam.h "${GTSAM_ADDITIONAL_LIBRARIES}" "" "${mexFlags}") endif () diff --git a/gtsam/base/CMakeLists.txt b/gtsam/base/CMakeLists.txt index a1f50d61d..50145846e 100644 --- a/gtsam/base/CMakeLists.txt +++ b/gtsam/base/CMakeLists.txt @@ -2,24 +2,14 @@ file(GLOB base_headers "*.h") install(FILES ${base_headers} DESTINATION include/gtsam/base) -# Components to link tests in this subfolder against -set(base_local_libs - base -) - -# Files to exclude from compilation of tests and timing scripts -set(base_excluded_files -# "${CMAKE_CURRENT_SOURCE_DIR}/tests/testTypedDiscreteFactor.cpp" # Example of excluding a test -# "" # Add to this list, with full path, to exclude -) +file(GLOB base_headers_tree "treeTraversal/*.h") +install(FILES ${base_headers_tree} DESTINATION include/gtsam/base/treeTraversal) # Build tests -if (GTSAM_BUILD_TESTS) - gtsam_add_subdir_tests(base "${base_local_libs}" "${gtsam-default}" "${base_excluded_files}") -endif(GTSAM_BUILD_TESTS) +add_subdirectory(tests) # Build timing scripts if (GTSAM_BUILD_TIMING) - gtsam_add_subdir_timing(base "${base_local_libs}" "${gtsam-default}" "${base_excluded_files}") + gtsam_add_subdir_timing(base "gtsam" "gtsam" "${base_excluded_files}") endif(GTSAM_BUILD_TIMING) diff --git a/gtsam/base/ConcurrentMap.h b/gtsam/base/ConcurrentMap.h new file mode 100644 index 000000000..336ea7e05 --- /dev/null +++ b/gtsam/base/ConcurrentMap.h @@ -0,0 +1,116 @@ +/* ---------------------------------------------------------------------------- + + * 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 FastMap.h + * @brief A thin wrapper around std::map that uses boost's fast_pool_allocator. + * @author Richard Roberts + * @date Oct 17, 2010 + */ + +#pragma once + +#include + +// Change class depending on whether we are using TBB +#ifdef GTSAM_USE_TBB + +// Include TBB header +# include +# undef min // TBB seems to include Windows.h which defines these macros that cause problems +# undef max +# undef ERROR + +// Use TBB concurrent_unordered_map for ConcurrentMap +# define CONCURRENT_MAP_BASE tbb::concurrent_unordered_map + +#else + +// If we're not using TBB, use a FastMap for ConcurrentMap +# include +# define CONCURRENT_MAP_BASE gtsam::FastMap + +#endif + +#include +#include +#include + +#include + +namespace gtsam { + +/** + * FastMap is a thin wrapper around std::map that uses the boost + * fast_pool_allocator instead of the default STL allocator. This is just a + * convenience to avoid having lengthy types in the code. Through timing, + * we've seen that the fast_pool_allocator can lead to speedups of several + * percent. + * @addtogroup base + */ +template +class ConcurrentMap : public CONCURRENT_MAP_BASE { + +public: + + typedef CONCURRENT_MAP_BASE Base; + + /** Default constructor */ + ConcurrentMap() {} + + /** Constructor from a range, passes through to base class */ + template + ConcurrentMap(INPUTITERATOR first, INPUTITERATOR last) : Base(first, last) {} + + /** Copy constructor from another ConcurrentMap */ + ConcurrentMap(const ConcurrentMap& x) : Base(x) {} + + /** Copy constructor from the base map class */ + ConcurrentMap(const Base& x) : Base(x) {} + + /** Handy 'exists' function */ + bool exists(const KEY& e) const { return this->count(e); } + +#ifndef GTSAM_USE_TBB + // If we're not using TBB and this is actually a FastMap, we need to add these functions and hide + // the original erase functions. + void unsafe_erase(typename Base::iterator position) { ((Base*)this)->erase(position); } + typename Base::size_type unsafe_erase(const KEY& k) { return ((Base*)this)->erase(k); } + void unsafe_erase(typename Base::iterator first, typename Base::iterator last) { + return ((Base*)this)->erase(first, last); } +private: + void erase() {} +public: +#endif + +private: + /** Serialization function */ + friend class boost::serialization::access; + template + void save(Archive& ar, const unsigned int version) const + { + // Copy to an STL container and serialize that + FastVector > map(this->size()); + std::copy(this->begin(), this->end(), map.begin()); + ar & BOOST_SERIALIZATION_NVP(map); + } + template + void load(Archive& ar, const unsigned int version) + { + // Load into STL container and then fill our map + FastVector > map; + ar & BOOST_SERIALIZATION_NVP(map); + this->insert(map.begin(), map.end()); + } + BOOST_SERIALIZATION_SPLIT_MEMBER() +}; + +} diff --git a/gtsam/base/DSFVector.cpp b/gtsam/base/DSFVector.cpp index 2bf18916d..454dea45b 100644 --- a/gtsam/base/DSFVector.cpp +++ b/gtsam/base/DSFVector.cpp @@ -16,38 +16,71 @@ * @brief a faster implementation for DSF, which uses vector rather than btree. */ +#include #include #include -#include +#include using namespace std; namespace gtsam { /* ************************************************************************* */ -DSFVector::DSFVector (const size_t numNodes) { - v_ = boost::make_shared(numNodes); +DSFBase::DSFBase(const size_t numNodes) { + v_ = boost::make_shared < V > (numNodes); int index = 0; - keys_.reserve(numNodes); - for(V::iterator it = v_->begin(); it!=v_->end(); it++, index++) { + for (V::iterator it = v_->begin(); it != v_->end(); it++, index++) *it = index; - keys_.push_back(index); - } } /* ************************************************************************* */ -DSFVector::DSFVector(const boost::shared_ptr& v_in, const std::vector& keys) : keys_(keys) { +DSFBase::DSFBase(const boost::shared_ptr& v_in) { v_ = v_in; - BOOST_FOREACH(const size_t key, keys) - (*v_)[key] = key; + int index = 0; + for (V::iterator it = v_->begin(); it != v_->end(); it++, index++) + *it = index; } /* ************************************************************************* */ -bool DSFVector::isSingleton(const Label& label) const { +size_t DSFBase::find(size_t key) const { + // follow parent pointers until we reach set representative + size_t parent = (*v_)[key]; + if (parent != key) + parent = find(parent); // recursive call + (*v_)[key] = parent; // path compression + return parent; +} + +/* ************************************************************************* */ +void DSFBase::merge(const size_t& i1, const size_t& i2) { + (*v_)[find(i2)] = find(i1); +} + +/* ************************************************************************* */ +DSFVector::DSFVector(const size_t numNodes) : + DSFBase(numNodes) { + keys_.reserve(numNodes); + for (size_t index = 0; index < numNodes; index++) + keys_.push_back(index); +} + +/* ************************************************************************* */ +DSFVector::DSFVector(const std::vector& keys) : + DSFBase(1 + *std::max_element(keys.begin(), keys.end())), keys_(keys) { +} + +/* ************************************************************************* */ +DSFVector::DSFVector(const boost::shared_ptr& v_in, + const std::vector& keys) : + DSFBase(v_in), keys_(keys) { + assert(*(std::max_element(keys.begin(), keys.end()))size()); +} + +/* ************************************************************************* */ +bool DSFVector::isSingleton(const size_t& label) const { bool result = false; - V::const_iterator it = keys_.begin(); - for (; it != keys_.end(); ++it) { - if(findSet(*it) == label) { + BOOST_FOREACH(size_t key,keys_) { + if (find(key) == label) { if (!result) // find the first occurrence result = true; else @@ -58,40 +91,29 @@ bool DSFVector::isSingleton(const Label& label) const { } /* ************************************************************************* */ -std::set DSFVector::set(const Label& label) const { - std::set set; - V::const_iterator it = keys_.begin(); - for (; it != keys_.end(); it++) { - if (findSet(*it) == label) - set.insert(*it); - } +std::set DSFVector::set(const size_t& label) const { + std::set < size_t > set; + BOOST_FOREACH(size_t key,keys_) + if (find(key) == label) + set.insert(key); return set; } /* ************************************************************************* */ -std::map > DSFVector::sets() const { - std::map > sets; - V::const_iterator it = keys_.begin(); - for (; it != keys_.end(); it++) { - sets[findSet(*it)].insert(*it); - } +std::map > DSFVector::sets() const { + std::map > sets; + BOOST_FOREACH(size_t key,keys_) + sets[find(key)].insert(key); return sets; } /* ************************************************************************* */ -std::map > DSFVector::arrays() const { - std::map > arrays; - V::const_iterator it = keys_.begin(); - for (; it != keys_.end(); it++) { - arrays[findSet(*it)].push_back(*it); - } +std::map > DSFVector::arrays() const { + std::map > arrays; + BOOST_FOREACH(size_t key,keys_) + arrays[find(key)].push_back(key); return arrays; } -/* ************************************************************************* */ -void DSFVector::makeUnionInPlace(const size_t& i1, const size_t& i2) { - (*v_)[findSet(i2)] = findSet(i1); -} - } // namespace gtsam diff --git a/gtsam/base/DSFVector.h b/gtsam/base/DSFVector.h index dd22d5b83..3c91d68c5 100644 --- a/gtsam/base/DSFVector.h +++ b/gtsam/base/DSFVector.h @@ -18,64 +18,80 @@ #pragma once -#include -#include -#include -#include - #include +#include +#include +#include +#include namespace gtsam { - /** - * A fast implementation of disjoint set forests that uses vector as underly data structure. - * @addtogroup base - */ - class GTSAM_EXPORT DSFVector { +/** + * A fast implementation of disjoint set forests that uses vector as underly data structure. + * This is the absolute minimal DSF data structure, and only allows size_t keys + * Uses rank compression but not union by rank :-( + * @addtogroup base + */ +class GTSAM_EXPORT DSFBase { - public: - typedef std::vector V; ///< Vector of ints - typedef size_t Label; ///< Label type - typedef V::const_iterator const_iterator; ///< const iterator over V - typedef V::iterator iterator;///< iterator over V +public: + typedef std::vector V; ///< Vector of ints - private: - // TODO could use existing memory to improve the efficiency - boost::shared_ptr v_; - std::vector keys_; +private: + boost::shared_ptr v_;///< Stores parent pointers, representative iff v[i]==i - public: - /// constructor that allocate a new memory - DSFVector(const size_t numNodes); +public: + /// constructor that allocate new memory, allows for keys 0...numNodes-1 + DSFBase(const size_t numNodes); - /// constructor that uses the existing memory - DSFVector(const boost::shared_ptr& v_in, const std::vector& keys); + /// constructor that uses the existing memory + DSFBase(const boost::shared_ptr& v_in); - /// find the label of the set in which {key} lives - inline Label findSet(size_t key) const { - size_t parent = (*v_)[key]; - while (parent != key) { - key = parent; - parent = (*v_)[key]; - } - return parent; - } + /// find the label of the set in which {key} lives + size_t find(size_t key) const; - /// find whether there is one and only one occurrence for the given {label} - bool isSingleton(const Label& label) const; + /// Merge two sets + void merge(const size_t& i1, const size_t& i2); - /// get the nodes in the tree with the given label - std::set set(const Label& label) const; + /// @deprecated old name + inline size_t findSet(size_t key) const {return find(key);} - /// return all sets, i.e. a partition of all elements - std::map > sets() const; + /// @deprecated old name + inline void makeUnionInPlace(const size_t& i1, const size_t& i2) {return merge(i1,i2);} +}; - /// return all sets, i.e. a partition of all elements - std::map > arrays() const; +/** + * DSFVector additionaly keeps a vector of keys to support more expensive operations + * @addtogroup base + */ +class GTSAM_EXPORT DSFVector: public DSFBase { - /// the in-place version of makeUnion - void makeUnionInPlace(const size_t& i1, const size_t& i2); +private: + std::vector keys_; ///< stores keys to support more expensive operations - }; +public: + /// constructor that allocate new memory, uses sequential keys 0...numNodes-1 + DSFVector(const size_t numNodes); + + /// constructor that allocates memory, uses given keys + DSFVector(const std::vector& keys); + + /// constructor that uses the existing memory + DSFVector(const boost::shared_ptr& v_in, const std::vector& keys); + + // all operations below loop over all keys and hence are *at least* O(n) + + /// find whether there is one and only one occurrence for the given {label} + bool isSingleton(const size_t& label) const; + + /// get the nodes in the tree with the given label + std::set set(const size_t& label) const; + + /// return all sets, i.e. a partition of all elements + std::map > sets() const; + + /// return all sets, i.e. a partition of all elements + std::map > arrays() const; +}; } diff --git a/gtsam/base/DerivedValue.h b/gtsam/base/DerivedValue.h index 9c61667ff..49528a126 100644 --- a/gtsam/base/DerivedValue.h +++ b/gtsam/base/DerivedValue.h @@ -17,9 +17,26 @@ #pragma once #include -#include #include +////////////////// +// The following includes windows.h in some MSVC versions, so we undef min, max, and ERROR +#include + +#ifdef min +#undef min +#endif + +#ifdef max +#undef max +#endif + +#ifdef ERROR +#undef ERROR +#endif +////////////////// + + namespace gtsam { template diff --git a/gtsam/base/FastDefaultAllocator.h b/gtsam/base/FastDefaultAllocator.h new file mode 100644 index 000000000..156a87f55 --- /dev/null +++ b/gtsam/base/FastDefaultAllocator.h @@ -0,0 +1,88 @@ +/* ---------------------------------------------------------------------------- + + * 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 FastDefaultAllocator.h + * @brief An easy way to control which allocator is used for Fast* collections + * @author Richard Roberts + * @date Aug 15, 2013 + */ + +#pragma once + +#include + +#if !defined GTSAM_ALLOCATOR_BOOSTPOOL && !defined GTSAM_ALLOCATOR_TBB && !defined GTSAM_ALLOCATOR_STL +# ifdef GTSAM_USE_TBB +// Use TBB allocator by default if we have TBB, otherwise boost pool +# define GTSAM_ALLOCATOR_TBB +# else +# define GTSAM_ALLOCATOR_BOOSTPOOL +# endif +#endif + +#if defined GTSAM_ALLOCATOR_BOOSTPOOL +# include +#elif defined GTSAM_ALLOCATOR_TBB +# include +# undef min // TBB seems to include Windows.h which defines these macros that cause problems +# undef max +# undef ERROR +#elif defined GTSAM_ALLOCATOR_STL +# include +#endif + +namespace gtsam +{ + + namespace internal + { + /// Default allocator for list, map, and set types + template + struct FastDefaultAllocator + { +#if defined GTSAM_ALLOCATOR_BOOSTPOOL + typedef boost::fast_pool_allocator type; + static const bool isBoost = true; + static const bool isTBB = false; + static const bool isSTL = false; +#elif defined GTSAM_ALLOCATOR_TBB + typedef tbb::tbb_allocator type; + static const bool isBoost = false; + static const bool isTBB = true; + static const bool isSTL = false; +#elif defined GTSAM_ALLOCATOR_STL + typedef std::allocator type; + static const bool isBoost = false; + static const bool isTBB = false; + static const bool isSTL = true; +#endif + }; + + /// Default allocator for vector types (we never use boost pool for vectors) + template + struct FastDefaultVectorAllocator + { +#if defined GTSAM_ALLOCATOR_TBB + typedef tbb::tbb_allocator type; + static const bool isBoost = false; + static const bool isTBB = true; + static const bool isSTL = false; +#else + typedef std::allocator type; + static const bool isBoost = false; + static const bool isTBB = false; + static const bool isSTL = true; +#endif + }; + } + +} \ No newline at end of file diff --git a/gtsam/base/FastList.h b/gtsam/base/FastList.h index a03121024..4b5d1caf1 100644 --- a/gtsam/base/FastList.h +++ b/gtsam/base/FastList.h @@ -18,8 +18,9 @@ #pragma once +#include #include -#include +#include #include #include @@ -34,11 +35,11 @@ namespace gtsam { * @addtogroup base */ template -class FastList: public std::list > { +class FastList: public std::list::type> { public: - typedef std::list > Base; + typedef std::list::type> Base; /** Default constructor */ FastList() {} @@ -53,6 +54,7 @@ public: /** Copy constructor from the base list class */ FastList(const Base& x) : Base(x) {} +#ifdef GTSAM_ALLOCATOR_BOOSTPOOL /** Copy constructor from a standard STL container */ FastList(const std::list& x) { // This if statement works around a bug in boost pool allocator and/or @@ -61,6 +63,7 @@ public: if(x.size() > 0) Base::assign(x.begin(), x.end()); } +#endif /** Conversion to a standard STL container */ operator std::list() const { diff --git a/gtsam/base/FastMap.h b/gtsam/base/FastMap.h index 7cea45e88..0a76c08b0 100644 --- a/gtsam/base/FastMap.h +++ b/gtsam/base/FastMap.h @@ -18,8 +18,8 @@ #pragma once +#include #include -#include #include #include @@ -34,11 +34,13 @@ namespace gtsam { * @addtogroup base */ template -class FastMap : public std::map, boost::fast_pool_allocator > > { +class FastMap : public std::map, + typename internal::FastDefaultAllocator >::type> { public: - typedef std::map, boost::fast_pool_allocator > > Base; + typedef std::map, + typename internal::FastDefaultAllocator >::type > Base; /** Default constructor */ FastMap() {} @@ -58,6 +60,12 @@ public: return std::map(this->begin(), this->end()); } + /** Handy 'insert' function for Matlab wrapper */ + bool insert2(const KEY& key, const VALUE& val) { return Base::insert(std::make_pair(key, val)).second; } + + /** Handy 'exists' function */ + bool exists(const KEY& e) const { return this->find(e) != this->end(); } + private: /** Serialization function */ friend class boost::serialization::access; diff --git a/gtsam/base/FastSet.h b/gtsam/base/FastSet.h index be2864f8f..69e841e57 100644 --- a/gtsam/base/FastSet.h +++ b/gtsam/base/FastSet.h @@ -18,11 +18,12 @@ #pragma once +#include + #include #include #include #include -#include #include #include #include @@ -45,11 +46,11 @@ struct FastSetTestableHelper; * @addtogroup base */ template -class FastSet: public std::set, boost::fast_pool_allocator > { +class FastSet: public std::set, typename internal::FastDefaultAllocator::type> { public: - typedef std::set, boost::fast_pool_allocator > Base; + typedef std::set, typename internal::FastDefaultAllocator::type> Base; /** Default constructor */ FastSet() { @@ -77,6 +78,7 @@ public: Base(x) { } +#ifdef GTSAM_ALLOCATOR_BOOSTPOOL /** Copy constructor from a standard STL container */ FastSet(const std::set& x) { // This if statement works around a bug in boost pool allocator and/or @@ -85,18 +87,27 @@ public: if(x.size() > 0) Base::insert(x.begin(), x.end()); } +#endif /** Conversion to a standard STL container */ operator std::set() const { return std::set(this->begin(), this->end()); } + /** Handy 'exists' function */ + bool exists(const VALUE& e) const { return this->find(e) != this->end(); } + /** Print to implement Testable */ void print(const std::string& str = "") const { FastSetTestableHelper::print(*this, str); } /** Check for equality within tolerance to implement Testable */ bool equals(const FastSet& other, double tol = 1e-9) const { return FastSetTestableHelper::equals(*this, other, tol); } + /** insert another set: handy for MATLAB access */ + void merge(const FastSet& other) { + Base::insert(other.begin(),other.end()); + } + private: /** Serialization function */ friend class boost::serialization::access; diff --git a/gtsam/base/FastVector.h b/gtsam/base/FastVector.h index f725cacc0..be3eaa981 100644 --- a/gtsam/base/FastVector.h +++ b/gtsam/base/FastVector.h @@ -19,7 +19,6 @@ #pragma once #include -#include #include #include @@ -36,11 +35,11 @@ namespace gtsam { * @addtogroup base */ template -class FastVector: public std::vector > { +class FastVector: public std::vector::type> { public: - typedef std::vector > Base; + typedef std::vector::type> Base; /** Default constructor */ FastVector() {} @@ -61,9 +60,6 @@ public: Base::assign(first, last); } - /** Copy constructor from another FastVector */ - FastVector(const FastVector& x) : Base(x) {} - /** Copy constructor from a FastList */ FastVector(const FastList& x) { if(x.size() > 0) @@ -80,7 +76,9 @@ public: FastVector(const Base& x) : Base(x) {} /** Copy constructor from a standard STL container */ - FastVector(const std::vector& x) { + template + FastVector(const std::vector& x) + { // This if statement works around a bug in boost pool allocator and/or // STL vector where if the size is zero, the pool allocator will allocate // huge amounts of memory. diff --git a/gtsam/base/Group.h b/gtsam/base/Group.h index 55762a108..5b2574ee3 100644 --- a/gtsam/base/Group.h +++ b/gtsam/base/Group.h @@ -19,6 +19,8 @@ #pragma once +#include + namespace gtsam { /** diff --git a/gtsam/base/LieMatrix.cpp b/gtsam/base/LieMatrix.cpp index 332401689..69054fc1c 100644 --- a/gtsam/base/LieMatrix.cpp +++ b/gtsam/base/LieMatrix.cpp @@ -19,18 +19,9 @@ namespace gtsam { - /* ************************************************************************* */ - LieMatrix::LieMatrix(size_t m, size_t n, ...) - : Matrix(m,n) { - va_list ap; - va_start(ap, n); - for(size_t i = 0; i < m; ++i) { - for(size_t j = 0; j < n; ++j) { - double value = va_arg(ap, double); - (*this)(i,j) = value; - } - } - va_end(ap); - } +/* ************************************************************************* */ +void LieMatrix::print(const std::string& name) const { + gtsam::print(matrix(), name); +} -} \ No newline at end of file +} diff --git a/gtsam/base/LieMatrix.h b/gtsam/base/LieMatrix.h index 1eebd0514..8d43dd6ea 100644 --- a/gtsam/base/LieMatrix.h +++ b/gtsam/base/LieMatrix.h @@ -48,17 +48,12 @@ struct LieMatrix : public Matrix, public DerivedValue { LieMatrix(size_t m, size_t n, const double* const data) : Matrix(Eigen::Map(data, m, n)) {} - /** Specify arguments directly, as in Matrix_() - always force these to be doubles */ - GTSAM_EXPORT LieMatrix(size_t m, size_t n, ...); - /// @} /// @name Testable interface /// @{ /** print @param s optional string naming the object */ - inline void print(const std::string& name="") const { - gtsam::print(matrix(), name); - } + GTSAM_EXPORT void print(const std::string& name="") const; /** equality up to tolerance */ inline bool equals(const LieMatrix& expected, double tol=1e-5) const { @@ -151,9 +146,13 @@ struct LieMatrix : public Matrix, public DerivedValue { throw std::runtime_error("LieMatrix::Expmap(): Don't use this function"); return LieMatrix(v); } - /** Logmap around identity - just returns with default cast back */ + /** Logmap around identity */ static inline Vector Logmap(const LieMatrix& p) { - return Eigen::Map(&p(0,0), p.dim()); } + Vector result(p.size()); + Eigen::Map >( + result.data(), p.rows(), p.cols()) = p; + return result; + } /// @} diff --git a/gtsam/base/LieScalar.h b/gtsam/base/LieScalar.h index 052e5d086..21a2e10ad 100644 --- a/gtsam/base/LieScalar.h +++ b/gtsam/base/LieScalar.h @@ -58,7 +58,7 @@ namespace gtsam { LieScalar retract(const Vector& v) const { return LieScalar(value() + v(0)); } /** @return the local coordinates of another object */ - Vector localCoordinates(const LieScalar& t2) const { return Vector_(1,(t2.value() - value())); } + Vector localCoordinates(const LieScalar& t2) const { return (Vector(1) << (t2.value() - value())); } // Group requirements @@ -68,7 +68,11 @@ namespace gtsam { } /** compose with another object */ - LieScalar compose(const LieScalar& p) const { + LieScalar compose(const LieScalar& p, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const { + if(H1) *H1 = eye(1); + if(H2) *H2 = eye(1); return LieScalar(d_ + p.d_); } @@ -92,7 +96,17 @@ namespace gtsam { static LieScalar Expmap(const Vector& v) { return LieScalar(v(0)); } /** Logmap around identity - just returns with default cast back */ - static Vector Logmap(const LieScalar& p) { return Vector_(1,p.value()); } + static Vector Logmap(const LieScalar& p) { return (Vector(1) << p.value()); } + + /// Left-trivialized derivative of the exponential map + static Matrix dexpL(const Vector& v) { + return eye(1); + } + + /// Left-trivialized derivative inverse of the exponential map + static Matrix dexpInvL(const Vector& v) { + return eye(1); + } private: double d_; diff --git a/gtsam/base/LieVector.cpp b/gtsam/base/LieVector.cpp index 17aeff73a..f6cae28e2 100644 --- a/gtsam/base/LieVector.cpp +++ b/gtsam/base/LieVector.cpp @@ -24,21 +24,10 @@ namespace gtsam { /* ************************************************************************* */ LieVector::LieVector(size_t m, const double* const data) -: Vector(Vector_(m,data)) -{ -} - -/* ************************************************************************* */ -LieVector::LieVector(size_t m, ...) : Vector(m) { - va_list ap; - va_start(ap, m); - for( size_t i = 0 ; i < m ; i++) { - double value = va_arg(ap, double); - (*this)(i) = value; - } - va_end(ap); + for(size_t i = 0; i < m; i++) + (*this)(i) = data[i]; } /* ************************************************************************* */ diff --git a/gtsam/base/LieVector.h b/gtsam/base/LieVector.h index c978500dd..6989bbfd2 100644 --- a/gtsam/base/LieVector.h +++ b/gtsam/base/LieVector.h @@ -39,14 +39,11 @@ struct LieVector : public Vector, public DerivedValue { LieVector(const Eigen::Matrix& v) : Vector(v) {} /** wrap a double */ - LieVector(double d) : Vector(Vector_(1, d)) {} + LieVector(double d) : Vector((Vector(1) << d)) {} /** constructor with size and initial data, row order ! */ GTSAM_EXPORT LieVector(size_t m, const double* const data); - /** Specify arguments directly, as in Vector_() - always force these to be doubles */ - GTSAM_EXPORT LieVector(size_t m, ...); - /** get the underlying vector */ Vector vector() const { return static_cast(*this); diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index f0b82cf9c..d9ab7d71c 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -23,6 +23,7 @@ #include #include +#include #include #include @@ -35,38 +36,6 @@ using namespace std; namespace gtsam { -/* ************************************************************************* */ -Matrix Matrix_( size_t m, size_t n, const double* const data) { - MatrixRowMajor A(m,n); - copy(data, data+m*n, A.data()); - return Matrix(A); -} - -/* ************************************************************************* */ -Matrix Matrix_( size_t m, size_t n, const Vector& v) -{ - Matrix A(m,n); - // column-wise copy - for( size_t j = 0, k=0 ; j < n ; j++) - for( size_t i = 0; i < m ; i++,k++) - A(i,j) = v(k); - return A; -} - -/* ************************************************************************* */ -Matrix Matrix_(size_t m, size_t n, ...) { - Matrix A(m,n); - va_list ap; - va_start(ap, n); - for( size_t i = 0 ; i < m ; i++) - for( size_t j = 0 ; j < n ; j++) { - double value = va_arg(ap, double); - A(i,j) = value; - } - va_end(ap); - return A; -} - /* ************************************************************************* */ Matrix zeros( size_t m, size_t n ) { return Matrix::Zero(m,n); @@ -210,17 +179,6 @@ void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, SubVec x += alpha * A.transpose() * e; } -/* ************************************************************************* */ -Vector Vector_(const Matrix& A) -{ - size_t m = A.rows(), n = A.cols(); - Vector v(m*n); - for( size_t j = 0, k=0 ; j < n ; j++) - for( size_t i = 0; i < m ; i++,k++) - v(k) = A(i,j); - return v; -} - /* ************************************************************************* */ void print(const Matrix& A, const string &s, ostream& stream) { size_t m = A.rows(), n = A.cols(); @@ -526,7 +484,7 @@ Matrix stack(size_t nrMatrices, ...) /* ************************************************************************* */ Matrix stack(const std::vector& blocks) { if (blocks.size() == 1) return blocks.at(0); - int nrows = 0, ncols = blocks.at(0).cols(); + DenseIndex nrows = 0, ncols = blocks.at(0).cols(); BOOST_FOREACH(const Matrix& mat, blocks) { nrows += mat.rows(); if (ncols != mat.cols()) @@ -534,7 +492,7 @@ Matrix stack(const std::vector& blocks) { } Matrix result(nrows, ncols); - int cur_row = 0; + DenseIndex cur_row = 0; BOOST_FOREACH(const Matrix& mat, blocks) { result.middleRows(cur_row, mat.rows()) = mat; cur_row += mat.rows(); @@ -583,16 +541,16 @@ Matrix collect(size_t nrMatrices, ...) /* ************************************************************************* */ // row scaling, in-place void vector_scale_inplace(const Vector& v, Matrix& A, bool inf_mask) { - const int m = A.rows(); + const DenseIndex m = A.rows(); if (inf_mask) { // only scale the first v.size() rows of A to support augmented Matrix - for (size_t i=0; i 1; + const DenseIndex effectiveRows = transposeMatrix ? matrix.cols() : matrix.rows(); + + if(matrix.rows() > 0 && matrix.cols() > 0) + { + stringstream matrixPrinted; + if(transposeMatrix) + matrixPrinted << matrix.transpose(); + else + matrixPrinted << matrix; + const std::string matrixStr = matrixPrinted.str(); + boost::tokenizer > tok(matrixStr, boost::char_separator("\n")); + + DenseIndex row = 0; + BOOST_FOREACH(const std::string& line, tok) + { + assert(row < effectiveRows); + if(row > 0) + ss << padding; + ss << "[ " << line << " ]"; + if(row < effectiveRows - 1) + ss << "\n"; + ++ row; + } + } else { + ss << "Empty (" << matrix.rows() << "x" << matrix.cols() << ")"; + } + return ss.str(); +} + + } // namespace gtsam diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 32899e6f2..6386bd526 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -45,27 +45,6 @@ typedef Eigen::Matrix Matrix6; typedef Eigen::Block SubMatrix; typedef Eigen::Block ConstSubMatrix; -/** - * constructor with size and initial data, row order ! - */ -GTSAM_EXPORT Matrix Matrix_(size_t m, size_t n, const double* const data); - -/** - * constructor with size and vector data, column order !!! - */ -GTSAM_EXPORT Matrix Matrix_(size_t m, size_t n, const Vector& v); - -/** - * constructor from Vector yielding v.size()*1 vector - */ -inline Matrix Matrix_(const Vector& v) { return Matrix_(v.size(),1,v);} - -/** - * nice constructor, dangerous as number of arguments must be exactly right - * and you have to pass doubles !!! always use 0.0 never 0 -*/ -GTSAM_EXPORT Matrix Matrix_(size_t m, size_t n, ...); - // Matlab-like syntax /** @@ -196,11 +175,6 @@ inline MATRIX prod(const MATRIX& A, const MATRIX&B) { return result; } -/** - * convert to column vector, column order !!! - */ -GTSAM_EXPORT Vector Vector_(const Matrix& A); - /** * print a matrix */ @@ -353,7 +327,7 @@ GTSAM_EXPORT std::list > weighted_eliminate(Matrix& A, Vector& b, const Vector& sigmas); /** - * Householder tranformation, Householder vectors below diagonal + * Householder transformation, Householder vectors below diagonal * @param k number of columns to zero out below diagonal * @param A matrix * @param copy_vectors - true to copy Householder vectors below diagonal @@ -423,7 +397,8 @@ GTSAM_EXPORT Matrix collect(size_t nrMatrices, ...); * scales a matrix row or column by the values in a vector * Arguments (Matrix, Vector) scales the columns, * (Vector, Matrix) scales the rows - * @param inf_mask when true, will not scale with a NaN or inf value + * @param inf_mask when true, will not scale with a NaN or inf value. + * The inplace version also allows v.size() inline Matrix3 skewSymmetric(const Eigen::MatrixBase& w) { return skewSymmetric(w(0),w(1),w(2));} -/** Use SVD to calculate inverse square root of a matrix */ +/** Use Cholesky to calculate inverse square root of a matrix */ GTSAM_EXPORT Matrix inverse_square_root(const Matrix& A); /** Calculate the LL^t decomposition of a S.P.D matrix */ @@ -497,6 +472,8 @@ Eigen::Matrix Cayley(const Eigen::Matrix& A) { return (FMat::Identity() - A)*(FMat::Identity() + A).inverse(); } +std::string formatMatrixIndented(const std::string& label, const Matrix& matrix, bool makeVectorHorizontal = false); + } // namespace gtsam #include diff --git a/gtsam/base/SymmetricBlockMatrix.cpp b/gtsam/base/SymmetricBlockMatrix.cpp new file mode 100644 index 000000000..546b6a7f1 --- /dev/null +++ b/gtsam/base/SymmetricBlockMatrix.cpp @@ -0,0 +1,82 @@ +/* ---------------------------------------------------------------------------- + + * 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 SymmetricBlockMatrix.cpp + * @brief Access to matrices via blocks of pre-defined sizes. Used in GaussianFactor and GaussianConditional. + * @author Richard Roberts + * @date Sep 18, 2010 + */ + +#include +#include +#include +#include + +namespace gtsam { + +/* ************************************************************************* */ +SymmetricBlockMatrix SymmetricBlockMatrix::LikeActiveViewOf( + const SymmetricBlockMatrix& other) { + SymmetricBlockMatrix result; + result.variableColOffsets_.resize(other.nBlocks() + 1); + for (size_t i = 0; i < result.variableColOffsets_.size(); ++i) + result.variableColOffsets_[i] = other.variableColOffsets_[other.blockStart_ + + i] - other.variableColOffsets_[other.blockStart_]; + result.matrix_.resize(other.cols(), other.cols()); + result.assertInvariants(); + return result; +} + +/* ************************************************************************* */ +SymmetricBlockMatrix SymmetricBlockMatrix::LikeActiveViewOf( + const VerticalBlockMatrix& other) { + SymmetricBlockMatrix result; + result.variableColOffsets_.resize(other.nBlocks() + 1); + for (size_t i = 0; i < result.variableColOffsets_.size(); ++i) + result.variableColOffsets_[i] = other.variableColOffsets_[other.blockStart_ + + i] - other.variableColOffsets_[other.blockStart_]; + result.matrix_.resize(other.cols(), other.cols()); + result.assertInvariants(); + return result; +} + +/* ************************************************************************* */ +VerticalBlockMatrix SymmetricBlockMatrix::choleskyPartial( + DenseIndex nFrontals) { + // Do dense elimination + if (blockStart() != 0) + throw std::invalid_argument( + "Can only do Cholesky when the SymmetricBlockMatrix is not a restricted view, i.e. when blockStart == 0."); + if (!gtsam::choleskyPartial(matrix_, offset(nFrontals))) + throw CholeskyFailed(); + + // Split conditional + + // Create one big conditionals with many frontal variables. + gttic(Construct_conditional); + const size_t varDim = offset(nFrontals); + VerticalBlockMatrix Ab = VerticalBlockMatrix::LikeActiveViewOf(*this, varDim); + Ab.full() = matrix_.topRows(varDim); + Ab.full().triangularView().setZero(); + gttoc(Construct_conditional); + + gttic(Remaining_factor); + // Take lower-right block of Ab_ to get the remaining factor + blockStart() = nFrontals; + gttoc(Remaining_factor); + + return Ab; +} +/* ************************************************************************* */ + +} //\ namespace gtsam + diff --git a/gtsam/base/SymmetricBlockMatrix.h b/gtsam/base/SymmetricBlockMatrix.h new file mode 100644 index 000000000..45edbf6af --- /dev/null +++ b/gtsam/base/SymmetricBlockMatrix.h @@ -0,0 +1,254 @@ +/* ---------------------------------------------------------------------------- + +* 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 SymmetricBlockMatrix.h +* @brief Access to matrices via blocks of pre-defined sizes. Used in GaussianFactor and GaussianConditional. +* @author Richard Roberts +* @date Sep 18, 2010 +*/ +#pragma once + +#include +#include +#include + +namespace gtsam { + + // Forward declarations + class VerticalBlockMatrix; + + /** + * This class stores a dense matrix and allows it to be accessed as a collection of blocks. When + * constructed, the caller must provide the dimensions of the blocks. + * + * The block structure is symmetric, but the underlying matrix does not necessarily need to be. + * + * This class also has a parameter that can be changed after construction to change the apparent + * matrix view. firstBlock() determines the block that appears to have index 0 for all operations + * (except re-setting firstBlock()). + * + * @addtogroup base */ + class GTSAM_EXPORT SymmetricBlockMatrix + { + public: + typedef SymmetricBlockMatrix This; + typedef SymmetricBlockMatrixBlockExpr Block; + typedef SymmetricBlockMatrixBlockExpr constBlock; + + protected: + Matrix matrix_; ///< The full matrix + FastVector variableColOffsets_; ///< the starting columns of each block (0-based) + + DenseIndex blockStart_; ///< Changes apparent matrix view, see main class comment. + + public: + /// Construct from an empty matrix (asserts that the matrix is empty) + SymmetricBlockMatrix() : + blockStart_(0) + { + variableColOffsets_.push_back(0); + assertInvariants(); + } + + /// Construct from a container of the sizes of each block. + template + SymmetricBlockMatrix(const CONTAINER& dimensions, bool appendOneDimension = false) : + blockStart_(0) + { + fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension); + matrix_.resize(variableColOffsets_.back(), variableColOffsets_.back()); + assertInvariants(); + } + + /// Construct from iterator over the sizes of each vertical block. + template + SymmetricBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool appendOneDimension = false) : + blockStart_(0) + { + fillOffsets(firstBlockDim, lastBlockDim, appendOneDimension); + matrix_.resize(variableColOffsets_.back(), variableColOffsets_.back()); + assertInvariants(); + } + + /// Construct from a container of the sizes of each vertical block and a pre-prepared matrix. + template + SymmetricBlockMatrix(const CONTAINER& dimensions, const Matrix& matrix, bool appendOneDimension = false) : + blockStart_(0) + { + matrix_.resize(matrix.rows(), matrix.cols()); + matrix_.triangularView() = matrix.triangularView(); + fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension); + if(matrix_.rows() != matrix_.cols()) + throw std::invalid_argument("Requested to create a SymmetricBlockMatrix from a non-square matrix."); + if(variableColOffsets_.back() != matrix_.cols()) + throw std::invalid_argument("Requested to create a SymmetricBlockMatrix with dimensions that do not sum to the total size of the provided matrix."); + assertInvariants(); + } + + /// Copy the block structure, but do not copy the matrix data. If blockStart() has been + /// modified, this copies the structure of the corresponding matrix view. In the destination + /// SymmetricBlockMatrix, blockStart() will be 0. + static SymmetricBlockMatrix LikeActiveViewOf(const SymmetricBlockMatrix& other); + + /// Copy the block structure, but do not copy the matrix data. If blockStart() has been + /// modified, this copies the structure of the corresponding matrix view. In the destination + /// SymmetricBlockMatrix, blockStart() will be 0. + static SymmetricBlockMatrix LikeActiveViewOf(const VerticalBlockMatrix& other); + + /// Row size + DenseIndex rows() const { assertInvariants(); return variableColOffsets_.back() - variableColOffsets_[blockStart_]; } + + /// Column size + DenseIndex cols() const { return rows(); } + + /// Block count + DenseIndex nBlocks() const { assertInvariants(); return variableColOffsets_.size() - 1 - blockStart_; } + + /// Access the block with vertical block index \c i_block and horizontal block index \c j_block. + /// Note that the actual block accessed in the underlying matrix is relative to blockStart(). + Block operator()(DenseIndex i_block, DenseIndex j_block) { + return Block(*this, i_block, j_block); + } + + /// Access the block with vertical block index \c i_block and horizontal block index \c j_block. + /// Note that the actual block accessed in the underlying matrix is relative to blockStart(). + constBlock operator()(DenseIndex i_block, DenseIndex j_block) const { + return constBlock(*this, i_block, j_block); + } + + /// Access the range of blocks starting with vertical block index \c i_startBlock, ending with + /// vertical block index \c i_endBlock, starting with horizontal block index \c j_startBlock, + /// and ending with horizontal block index \c j_endBlock. End block indices are exclusive. Note + /// that the actual blocks accessed in the underlying matrix are relative to blockStart(). + Block range(DenseIndex i_startBlock, DenseIndex i_endBlock, DenseIndex j_startBlock, DenseIndex j_endBlock) { + assertInvariants(); + return Block(*this, i_startBlock, j_startBlock, i_endBlock - i_startBlock, j_endBlock - j_startBlock); + } + + /// Access the range of blocks starting with vertical block index \c i_startBlock, ending with + /// vertical block index \c i_endBlock, starting with horizontal block index \c j_startBlock, + /// and ending with horizontal block index \c j_endBlock. End block indices are exclusive. Note + /// that the actual blocks accessed in the underlying matrix are relative to blockStart(). + constBlock range(DenseIndex i_startBlock, DenseIndex i_endBlock, DenseIndex j_startBlock, DenseIndex j_endBlock) const { + assertInvariants(); + return constBlock(*this, i_startBlock, j_startBlock, i_endBlock - i_startBlock, j_endBlock - j_startBlock); + } + + /** Return the full matrix, *not* including any portions excluded by firstBlock(). */ + Block full() + { + return Block(*this, 0, nBlocks(), 0); + } + + /** Return the full matrix, *not* including any portions excluded by firstBlock(). */ + constBlock full() const + { + return constBlock(*this, 0, nBlocks(), 0); + } + + /** Access to full matrix, including any portions excluded by firstBlock() to other operations. */ + Eigen::SelfAdjointView matrix() const + { + return matrix_; + } + + /** Access to full matrix, including any portions excluded by firstBlock() to other operations. */ + Eigen::SelfAdjointView matrix() + { + return matrix_; + } + + /// Return the absolute offset in the underlying matrix of the start of the specified \c block. + DenseIndex offset(DenseIndex block) const + { + assertInvariants(); + DenseIndex actualBlock = block + blockStart_; + checkBlock(actualBlock); + return variableColOffsets_[actualBlock]; + } + + /// Retrieve or modify the first logical block, i.e. the block referenced by block index 0. + /// Blocks before it will be inaccessible, except by accessing the underlying matrix using + /// matrix(). + DenseIndex& blockStart() { return blockStart_; } + + /// Retrieve the first logical block, i.e. the block referenced by block index 0. Blocks before + /// it will be inaccessible, except by accessing the underlying matrix using matrix(). + DenseIndex blockStart() const { return blockStart_; } + + /// Do partial Cholesky in-place and return the eliminated block matrix, leaving the remaining + /// symmetric matrix in place. + VerticalBlockMatrix choleskyPartial(DenseIndex nFrontals); + + protected: + void assertInvariants() const + { + assert(matrix_.rows() == matrix_.cols()); + assert(matrix_.cols() == variableColOffsets_.back()); + assert(blockStart_ < (DenseIndex)variableColOffsets_.size()); + } + + void checkBlock(DenseIndex block) const + { + assert(matrix_.rows() == matrix_.cols()); + assert(matrix_.cols() == variableColOffsets_.back()); + assert(block >= 0); + assert(block < (DenseIndex)variableColOffsets_.size()-1); + assert(variableColOffsets_[block] < matrix_.cols() && variableColOffsets_[block+1] <= matrix_.cols()); + } + + DenseIndex offsetUnchecked(DenseIndex block) const + { + return variableColOffsets_[block + blockStart_]; + } + + template + void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool appendOneDimension) + { + variableColOffsets_.resize((lastBlockDim-firstBlockDim) + 1 + (appendOneDimension ? 1 : 0)); + variableColOffsets_[0] = 0; + DenseIndex j=0; + for(ITERATOR dim=firstBlockDim; dim!=lastBlockDim; ++dim) { + variableColOffsets_[j+1] = variableColOffsets_[j] + *dim; + ++ j; + } + if(appendOneDimension) + { + variableColOffsets_[j+1] = variableColOffsets_[j] + 1; + ++ j; + } + } + + friend class VerticalBlockMatrix; + template friend class SymmetricBlockMatrixBlockExpr; + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(matrix_); + ar & BOOST_SERIALIZATION_NVP(variableColOffsets_); + ar & BOOST_SERIALIZATION_NVP(blockStart_); + } + }; + + /* ************************************************************************* */ + class CholeskyFailed : public gtsam::ThreadsafeException + { + public: + CholeskyFailed() throw() {} + virtual ~CholeskyFailed() throw() {} + }; + +} + diff --git a/gtsam/base/SymmetricBlockMatrixBlockExpr.h b/gtsam/base/SymmetricBlockMatrixBlockExpr.h new file mode 100644 index 000000000..6a65b2748 --- /dev/null +++ b/gtsam/base/SymmetricBlockMatrixBlockExpr.h @@ -0,0 +1,337 @@ +/* ---------------------------------------------------------------------------- + +* 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 SymmetricBlockMatrixBlockExpr.h +* @brief Matrix expression for a block of a SymmetricBlockMatrix +* @author Richard Roberts +* @date Nov 20, 2013 +*/ +#pragma once + +#include + +namespace gtsam { template class SymmetricBlockMatrixBlockExpr; } +namespace gtsam { class SymmetricBlockMatrix; } + +// traits class for Eigen expressions +namespace Eigen +{ + namespace internal + { + template + struct traits > : + public traits::type> + { + }; + } +} + +namespace gtsam +{ + /// A matrix expression that references a single block of a SymmetricBlockMatrix. Depending on + /// the position of the block, this expression will behave either as a regular matrix block, a + /// transposed matrix block, or a symmetric matrix block. The only reason this class is templated + /// on SymmetricBlockMatrixType is to allow for both const and non-const references. + template + class SymmetricBlockMatrixBlockExpr : public Eigen::EigenBase > + { + protected: + SymmetricBlockMatrixType& xpr_; ///< The referenced SymmetricBlockMatrix + DenseIndex densei_; ///< The scalar indices of the referenced block + DenseIndex densej_; ///< The scalar indices of the referenced block + DenseIndex denseRows_; ///< The scalar size of the referenced block + DenseIndex denseCols_; ///< The scalar size of the referenced block + enum BlockType { Plain, SelfAdjoint, Transposed } blockType_; ///< The type of the referenced block, as determined by the block position + typedef SymmetricBlockMatrixBlockExpr This; + + public: + // Typedefs and constants used in Eigen + typedef typename const_selector::Scalar&, typename Eigen::internal::traits::Scalar>::type ScalarRef; + typedef typename Eigen::internal::traits::Scalar Scalar; + typedef typename Eigen::internal::traits::Index Index; + static const Index ColsAtCompileTime = Eigen::Dynamic; + static const Index RowsAtCompileTime = Eigen::Dynamic; + + typedef typename const_selector::type + DenseMatrixType; + + typedef Eigen::Map > OffDiagonal; + typedef Eigen::SelfAdjointView, Eigen::Upper> SelfAdjointView; + typedef Eigen::TriangularView, Eigen::Upper> TriangularView; + + protected: + mutable Eigen::Block myBlock_; + template friend class SymmetricBlockMatrixBlockExpr; + + public: + /// Create a SymmetricBlockMatrixBlockExpr from the specified block of a SymmetricBlockMatrix. + SymmetricBlockMatrixBlockExpr(SymmetricBlockMatrixType& blockMatrix, Index iBlock, Index jBlock) : + xpr_(blockMatrix), myBlock_(blockMatrix.matrix_.block(0, 0, 0, 0)) + { + initIndices(iBlock, jBlock); + } + + /// Create a SymmetricBlockMatrixBlockExpr from the specified range of blocks of a + /// SymmetricBlockMatrix. + SymmetricBlockMatrixBlockExpr(SymmetricBlockMatrixType& blockMatrix, + Index firstRowBlock, Index firstColBlock, Index rowBlocks, Index colBlocks) : + xpr_(blockMatrix), myBlock_(blockMatrix.matrix_.block(0, 0, 0, 0)) + { + initIndices(firstRowBlock, firstColBlock, rowBlocks, colBlocks); + } + + /// Create a SymmetricBlockMatrixBlockExpr from the specified range of blocks of a + /// SymmetricBlockMatrix. + SymmetricBlockMatrixBlockExpr(SymmetricBlockMatrixType& blockMatrix, Index firstBlock, Index blocks, char dummy) : + xpr_(blockMatrix), myBlock_(blockMatrix.matrix_.block(0, 0, 0, 0)) + { + initIndices(firstBlock, firstBlock, blocks, blocks); + } + + inline Index rows() const { return blockType_ != Transposed ? denseRows_ : denseCols_; } + inline Index cols() const { return blockType_ != Transposed ? denseCols_ : denseRows_; } + + inline BlockType blockType() const { return blockType_; } + + inline ScalarRef operator()(Index row, Index col) const + { + return coeffInternal(row, col); + } + + inline OffDiagonal knownOffDiagonal() const + { + typedef Eigen::Stride DynamicStride; + + // We can return a Map if we are either on an off-diagonal block, or a block of size 0 or 1 + assert(blockType_ != SelfAdjoint || (denseRows_ <= 1 && denseCols_ <= 1)); + if(blockType_ == Transposed) + { + // Swap the inner and outer stride to produce a transposed Map + Eigen::Block block = const_cast(*this).xpr_.matrix_.block(densei_, densej_, denseRows_, denseCols_); + return Eigen::Map(block.data(), block.cols(), block.rows(), + DynamicStride(block.innerStride(), block.outerStride())); + } + else + { + Eigen::Block block = const_cast(*this).xpr_.matrix_.block(densei_, densej_, denseRows_, denseCols_); + return Eigen::Map(block.data(), block.rows(), block.cols(), + DynamicStride(block.outerStride(), block.innerStride())); + } + } + + inline SelfAdjointView selfadjointView() const + { + assert(blockType_ == SelfAdjoint); + return myBlock_; + } + + inline TriangularView triangularView() const + { + assert(blockType_ == SelfAdjoint); + return myBlock_; + } + + template inline void evalTo(Dest& dst) const + { + // Just try to assign to the object using either a selfadjoint view or a block view + if(blockType_ == SelfAdjoint) + dst = selfadjointView(); + else if(blockType_ == Plain) + dst = myBlock_; + else + dst = myBlock_.transpose(); + } + + //template inline void evalTo(const Eigen::SelfAdjointView& rhs) const + //{ + // if(blockType_ == SelfAdjoint) + // rhs.nestedExpression().triangularView() = triangularView(); + // else + // throw std::invalid_argument("Cannot assign an off-diagonal block to a self-adjoint matrix"); + //} + + //template inline void evalTo(const Eigen::TriangularView& rhs) const + //{ + // if(blockType_ == SelfAdjoint) + // rhs.nestedExpression().triangularView() = triangularView(); + // else + // throw std::invalid_argument("Cannot assign an off-diagonal block to a self-adjoint matrix"); + //} + + template + This& operator=(const Eigen::MatrixBase& rhs) + { + // Just try to assign to the object using either a selfadjoint view or a block view + if(blockType_ == SelfAdjoint) + triangularView() = rhs.derived().template triangularView(); + else if(blockType_ == Plain) + myBlock_ = rhs.derived(); + else + myBlock_.transpose() = rhs.derived(); + return *this; + } + + template + This& operator=(const Eigen::SelfAdjointView& rhs) + { + if(blockType_ == SelfAdjoint) + triangularView() = rhs.nestedExpression().template triangularView(); + else + throw std::invalid_argument("Cannot assign a self-adjoint matrix to an off-diagonal block"); + return *this; + } + + template + This& operator=(const SymmetricBlockMatrixBlockExpr& other) + { + _doAssign(other); + return *this; + } + + This& operator=(const This& other) + { + // This version is required so GCC doesn't synthesize a default operator=. + _doAssign(other); + return *this; + } + + template + This& operator+=(const SymmetricBlockMatrixBlockExpr& other) + { + if(blockType_ == SelfAdjoint) + { + assert((BlockType)other.blockType() == SelfAdjoint); + triangularView() += other.triangularView().nestedExpression(); + } + else if(blockType_ == Plain) + { + assert((BlockType)other.blockType() == Plain || (BlockType)other.blockType() == Transposed); + if((BlockType)other.blockType() == Transposed) + myBlock_ += other.myBlock_.transpose(); + else + myBlock_ += other.myBlock_; + } + else + { + assert((BlockType)other.blockType() == Plain || (BlockType)other.blockType() == Transposed); + if((BlockType)other.blockType() == Transposed) + myBlock_.transpose() += other.myBlock_.transpose(); + else + myBlock_.transpose() += other.myBlock_; + } + return *this; + } + + private: + void initIndices(Index iBlock, Index jBlock, Index blockRows = 1, Index blockCols = 1) + { + if(iBlock == jBlock && blockRows == blockCols) + { + densei_ = xpr_.offset(iBlock); + densej_ = densei_; + if(blockRows > 0) + xpr_.checkBlock(iBlock + blockRows - 1); + denseRows_ = xpr_.offsetUnchecked(iBlock + blockRows) - densei_; + if(blockCols > 0) + xpr_.checkBlock(jBlock + blockCols - 1); + denseCols_ = xpr_.offsetUnchecked(jBlock + blockCols) - densej_; + blockType_ = SelfAdjoint; + } + else + { + if(jBlock > iBlock || (iBlock == jBlock && blockCols > blockRows)) + { + densei_ = xpr_.offset(iBlock); + densej_ = xpr_.offset(jBlock); + if(blockRows > 0) + xpr_.checkBlock(iBlock + blockRows - 1); + denseRows_ = xpr_.offsetUnchecked(iBlock + blockRows) - densei_; + if(blockCols > 0) + xpr_.checkBlock(jBlock + blockCols - 1); + denseCols_ = xpr_.offsetUnchecked(jBlock + blockCols) - densej_; + blockType_ = Plain; + } + else + { + densei_ = xpr_.offset(jBlock); + densej_ = xpr_.offset(iBlock); + if(blockCols > 0) + xpr_.checkBlock(jBlock + blockCols - 1); + denseRows_ = xpr_.offsetUnchecked(jBlock + blockCols) - densei_; + if(blockRows > 0) + xpr_.checkBlock(iBlock + blockRows - 1); + denseCols_ = xpr_.offsetUnchecked(iBlock + blockRows) - densej_; + blockType_ = Transposed; + } + + // Validate that the block does not cross below the diagonal (the indices have already been + // flipped above the diagonal for ranges starting below the diagonal). + if(densei_ + denseRows_ > densej_ + 1) + throw std::invalid_argument("Off-diagonal block ranges may not cross the diagonal"); + } + + new (&myBlock_) Eigen::Block(xpr_.matrix_.block(densei_, densej_, denseRows_, denseCols_)); + } + + template + inline ScalarType coeffInternal(Index row, Index col) const + { + // We leave index checking up to the Block class + if(blockType_ == Plain) + { + return myBlock_(row, col); + } + else if(blockType_ == SelfAdjoint) + { + if(row <= col) + return myBlock_(row, col); + else + return myBlock_.transpose()(row, col); + } + else + { + return myBlock_.transpose()(row, col); + } + } + + template + void _doAssign(const SymmetricBlockMatrixBlockExpr& other) + { + if(blockType_ == SelfAdjoint) + { + assert((BlockType)other.blockType() == SelfAdjoint); + triangularView() = other.triangularView().nestedExpression(); + } + else if(blockType_ == Plain) + { + assert((BlockType)other.blockType() == Plain || (BlockType)other.blockType() == Transposed); + if((BlockType)other.blockType() == Transposed) + myBlock_ = other.myBlock_.transpose(); + else + myBlock_ = other.myBlock_; + } + else + { + assert((BlockType)other.blockType() == Plain || (BlockType)other.blockType() == Transposed); + if((BlockType)other.blockType() == Transposed) + myBlock_.transpose() = other.myBlock_.transpose(); + else + myBlock_.transpose() = other.myBlock_; + } + } + + + }; + +} diff --git a/gtsam/base/TestableAssertions.h b/gtsam/base/TestableAssertions.h index 42001b6f4..04d3fc676 100644 --- a/gtsam/base/TestableAssertions.h +++ b/gtsam/base/TestableAssertions.h @@ -30,7 +30,7 @@ namespace gtsam { /** * Equals testing for basic types */ -inline bool assert_equal(const Index& expected, const Index& actual, double tol = 0.0) { +inline bool assert_equal(const Key& expected, const Key& actual, double tol = 0.0) { if(expected != actual) { std::cout << "Not equal:\nexpected: " << expected << "\nactual: " << actual << std::endl; return false; @@ -231,8 +231,6 @@ bool assert_container_equal(const std::vector >& expected, template bool assert_container_equal(const V& expected, const V& actual, double tol = 1e-9) { bool match = true; - if (expected.size() != actual.size()) - match = false; typename V::const_iterator itExp = expected.begin(), itAct = actual.begin(); @@ -243,6 +241,8 @@ bool assert_container_equal(const V& expected, const V& actual, double tol = 1e- break; } } + if(itExp != expected.end() || itAct != actual.end()) + match = false; } if(!match) { std::cout << "expected: " << std::endl; diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index 9bd0926ae..48ada018f 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -79,33 +79,6 @@ void odprintf(const char *format, ...) { //#endif } -/* ************************************************************************* */ -Vector Vector_( size_t m, const double* const data) { - Vector A(m); - copy(data, data+m, A.data()); - return A; -} - -/* ************************************************************************* */ -Vector Vector_(size_t m, ...) { - Vector v(m); - va_list ap; - va_start(ap, m); - for( size_t i = 0 ; i < m ; i++) { - double value = va_arg(ap, double); - v(i) = value; - } - va_end(ap); - return v; -} - -/* ************************************************************************* */ -Vector Vector_(const std::vector& d) { - Vector v(d.size()); - copy(d.begin(), d.end(), v.data()); - return v; -} - /* ************************************************************************* */ bool zero(const Vector& v) { bool result = true; @@ -387,7 +360,7 @@ double weightedPseudoinverse(const Vector& a, const Vector& weights, // Slow version with error checking pair weightedPseudoinverse(const Vector& a, const Vector& weights) { - int m = weights.size(); + DenseIndex m = weights.size(); if (a.size() != m) throw invalid_argument("a and weights have different sizes!"); Vector pseudo(m); diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index 2c6066042..b35afccdb 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -46,22 +46,6 @@ typedef Eigen::VectorBlock ConstSubVector; */ GTSAM_EXPORT void odprintf(const char *format, ...); -/** - * constructor with size and initial data, row order ! - */ -GTSAM_EXPORT Vector Vector_( size_t m, const double* const data); - -/** - * nice constructor, dangerous as number of arguments must be exactly right - * and you have to pass doubles !!! always use 0.0 never 0 - */ -GTSAM_EXPORT Vector Vector_(size_t m, ...); - -/** - * Create a numeric vector from an STL vector of doubles - */ -GTSAM_EXPORT Vector Vector_(const std::vector& data); - /** * Create vector initialized to a constant value * @param n is the size of the vector diff --git a/gtsam/base/VerticalBlockMatrix.cpp b/gtsam/base/VerticalBlockMatrix.cpp new file mode 100644 index 000000000..9f54b9c97 --- /dev/null +++ b/gtsam/base/VerticalBlockMatrix.cpp @@ -0,0 +1,52 @@ +/* ---------------------------------------------------------------------------- + + * 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 VerticalBlockMatrix.cpp + * @brief A matrix with column blocks of pre-defined sizes. Used in JacobianFactor and + * GaussianConditional. + * @author Richard Roberts + * @date Sep 18, 2010 */ + +#include +#include + +namespace gtsam { + +/* ************************************************************************* */ +VerticalBlockMatrix VerticalBlockMatrix::LikeActiveViewOf( + const VerticalBlockMatrix& other) { + VerticalBlockMatrix result; + result.variableColOffsets_.resize(other.nBlocks() + 1); + for (size_t i = 0; i < result.variableColOffsets_.size(); ++i) + result.variableColOffsets_[i] = other.variableColOffsets_[other.blockStart_ + + i] - other.variableColOffsets_[other.blockStart_]; + result.matrix_.resize(other.rows(), result.variableColOffsets_.back()); + result.rowEnd_ = other.rows(); + result.assertInvariants(); + return result; +} + +/* ************************************************************************* */ +VerticalBlockMatrix VerticalBlockMatrix::LikeActiveViewOf( + const SymmetricBlockMatrix& other, DenseIndex height) { + VerticalBlockMatrix result; + result.variableColOffsets_.resize(other.nBlocks() + 1); + for (size_t i = 0; i < result.variableColOffsets_.size(); ++i) + result.variableColOffsets_[i] = other.variableColOffsets_[other.blockStart_ + + i] - other.variableColOffsets_[other.blockStart_]; + result.matrix_.resize(height, result.variableColOffsets_.back()); + result.rowEnd_ = height; + result.assertInvariants(); + return result; +} + +} diff --git a/gtsam/base/VerticalBlockMatrix.h b/gtsam/base/VerticalBlockMatrix.h new file mode 100644 index 000000000..029f55c58 --- /dev/null +++ b/gtsam/base/VerticalBlockMatrix.h @@ -0,0 +1,235 @@ +/* ---------------------------------------------------------------------------- + + * 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 VerticalBlockMatrix.h + * @brief A matrix with column blocks of pre-defined sizes. Used in JacobianFactor and + * GaussianConditional. + * @author Richard Roberts + * @date Sep 18, 2010 */ +#pragma once + +#include +#include + +namespace gtsam { + + // Forward declarations + class SymmetricBlockMatrix; + + /** + * This class stores a dense matrix and allows it to be accessed as a collection of vertical + * blocks. The dimensions of the blocks are provided when constructing this class. + * + * This class also has three parameters that can be changed after construction that change the + * apparent view of the matrix without any reallocation or data copying. firstBlock() determines + * the block that has index 0 for all operations (except for re-setting firstBlock()). rowStart() + * determines the apparent first row of the matrix for all operations (except for setting + * rowStart() and rowEnd()). rowEnd() determines the apparent exclusive (one-past-the-last) last + * row for all operations. To include all rows, rowEnd() should be set to the number of rows in + * the matrix (i.e. one after the last true row index). + * + * @addtogroup base */ + class GTSAM_EXPORT VerticalBlockMatrix + { + public: + typedef VerticalBlockMatrix This; + typedef Eigen::Block Block; + typedef Eigen::Block constBlock; + + protected: + Matrix matrix_; ///< The full matrix + FastVector variableColOffsets_; ///< the starting columns of each block (0-based) + + DenseIndex rowStart_; ///< Changes apparent matrix view, see main class comment. + DenseIndex rowEnd_; ///< Changes apparent matrix view, see main class comment. + DenseIndex blockStart_; ///< Changes apparent matrix view, see main class comment. + + public: + + /** Construct an empty VerticalBlockMatrix */ + VerticalBlockMatrix() : + rowStart_(0), rowEnd_(0), blockStart_(0) + { + variableColOffsets_.push_back(0); + assertInvariants(); + } + + /** Construct from a container of the sizes of each vertical block. */ + template + VerticalBlockMatrix(const CONTAINER& dimensions, DenseIndex height, bool appendOneDimension = false) : + rowStart_(0), rowEnd_(height), blockStart_(0) + { + fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension); + matrix_.resize(height, variableColOffsets_.back()); + assertInvariants(); + } + + /** Construct from a container of the sizes of each vertical block and a pre-prepared matrix. */ + template + VerticalBlockMatrix(const CONTAINER& dimensions, const Matrix& matrix, bool appendOneDimension = false) : + matrix_(matrix), rowStart_(0), rowEnd_(matrix.rows()), blockStart_(0) + { + fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension); + if(variableColOffsets_.back() != matrix_.cols()) + throw std::invalid_argument("Requested to create a VerticalBlockMatrix with dimensions that do not sum to the total columns of the provided matrix."); + assertInvariants(); + } + + /** + * Construct from iterator over the sizes of each vertical block. */ + template + VerticalBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, DenseIndex height, bool appendOneDimension = false) : + rowStart_(0), rowEnd_(height), blockStart_(0) + { + fillOffsets(firstBlockDim, lastBlockDim, appendOneDimension); + matrix_.resize(height, variableColOffsets_.back()); + assertInvariants(); + } + + /** Copy the block structure and resize the underlying matrix, but do not copy the matrix data. + * If blockStart(), rowStart(), and/or rowEnd() have been modified, this copies the structure of + * the corresponding matrix view. In the destination VerticalBlockView, blockStart() and + * rowStart() will thus be 0, rowEnd() will be cols() of the source VerticalBlockView, and the + * underlying matrix will be the size of the view of the source matrix. */ + static VerticalBlockMatrix LikeActiveViewOf(const VerticalBlockMatrix& rhs); + + /** Copy the block structure, but do not copy the matrix data. If blockStart() has been + * modified, this copies the structure of the corresponding matrix view. In the destination + * VerticalBlockMatrix, blockStart() will be 0. */ + static VerticalBlockMatrix LikeActiveViewOf(const SymmetricBlockMatrix& rhs, DenseIndex height); + + /// Row size + DenseIndex rows() const { assertInvariants(); return rowEnd_ - rowStart_; } + + /// Column size + DenseIndex cols() const { assertInvariants(); return variableColOffsets_.back() - variableColOffsets_[blockStart_]; } + + /// Block count + DenseIndex nBlocks() const { assertInvariants(); return variableColOffsets_.size() - 1 - blockStart_; } + + /** Access a single block in the underlying matrix with read/write access */ + Block operator()(DenseIndex block) { return range(block, block+1); } + + /** Access a const block view */ + const constBlock operator()(DenseIndex block) const { return range(block, block+1); } + + /** access ranges of blocks at a time */ + Block range(DenseIndex startBlock, DenseIndex endBlock) { + assertInvariants(); + DenseIndex actualStartBlock = startBlock + blockStart_; + DenseIndex actualEndBlock = endBlock + blockStart_; + if(startBlock != 0 || endBlock != 0) { + checkBlock(actualStartBlock); + assert(actualEndBlock < (DenseIndex)variableColOffsets_.size()); + } + const DenseIndex startCol = variableColOffsets_[actualStartBlock]; + const DenseIndex rangeCols = variableColOffsets_[actualEndBlock] - startCol; + return matrix_.block(rowStart_, startCol, this->rows(), rangeCols); + } + + const constBlock range(DenseIndex startBlock, DenseIndex endBlock) const { + assertInvariants(); + DenseIndex actualStartBlock = startBlock + blockStart_; + DenseIndex actualEndBlock = endBlock + blockStart_; + if(startBlock != 0 || endBlock != 0) { + checkBlock(actualStartBlock); + assert(actualEndBlock < (DenseIndex)variableColOffsets_.size()); + } + const DenseIndex startCol = variableColOffsets_[actualStartBlock]; + const DenseIndex rangeCols = variableColOffsets_[actualEndBlock] - startCol; + return ((const Matrix&)matrix_).block(rowStart_, startCol, this->rows(), rangeCols); + } + + /** Return the full matrix, *not* including any portions excluded by rowStart(), rowEnd(), and firstBlock() */ + Block full() { return range(0, nBlocks()); } + + /** Return the full matrix, *not* including any portions excluded by rowStart(), rowEnd(), and firstBlock() */ + const constBlock full() const { return range(0, nBlocks()); } + + DenseIndex offset(DenseIndex block) const { + assertInvariants(); + DenseIndex actualBlock = block + blockStart_; + checkBlock(actualBlock); + return variableColOffsets_[actualBlock]; + } + + /** Get or set the apparent first row of the underlying matrix for all operations */ + DenseIndex& rowStart() { return rowStart_; } + + /** Get or set the apparent last row (exclusive, i.e. rows() == rowEnd() - rowStart()) of the underlying matrix for all operations */ + DenseIndex& rowEnd() { return rowEnd_; } + + /** Get or set the apparent first block for all operations */ + DenseIndex& firstBlock() { return blockStart_; } + + /** Get the apparent first row of the underlying matrix for all operations */ + DenseIndex rowStart() const { return rowStart_; } + + /** Get the apparent last row (exclusive, i.e. rows() == rowEnd() - rowStart()) of the underlying matrix for all operations */ + DenseIndex rowEnd() const { return rowEnd_; } + + /** Get the apparent first block for all operations */ + DenseIndex firstBlock() const { return blockStart_; } + + /** Access to full matrix (*including* any portions excluded by rowStart(), rowEnd(), and firstBlock()) */ + const Matrix& matrix() const { return matrix_; } + + /** Non-const access to full matrix (*including* any portions excluded by rowStart(), rowEnd(), and firstBlock()) */ + Matrix& matrix() { return matrix_; } + + protected: + void assertInvariants() const { + assert(matrix_.cols() == variableColOffsets_.back()); + assert(blockStart_ < (DenseIndex)variableColOffsets_.size()); + assert(rowStart_ <= matrix_.rows()); + assert(rowEnd_ <= matrix_.rows()); + assert(rowStart_ <= rowEnd_); + } + + void checkBlock(DenseIndex block) const { + assert(matrix_.cols() == variableColOffsets_.back()); + assert(block < (DenseIndex)variableColOffsets_.size() - 1); + assert(variableColOffsets_[block] < matrix_.cols() && variableColOffsets_[block+1] <= matrix_.cols()); + } + + template + void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool appendOneDimension) { + variableColOffsets_.resize((lastBlockDim-firstBlockDim) + 1 + (appendOneDimension ? 1 : 0)); + variableColOffsets_[0] = 0; + DenseIndex j=0; + for(ITERATOR dim=firstBlockDim; dim!=lastBlockDim; ++dim) { + variableColOffsets_[j+1] = variableColOffsets_[j] + *dim; + ++ j; + } + if(appendOneDimension) + { + variableColOffsets_[j+1] = variableColOffsets_[j] + 1; + ++ j; + } + } + + friend class SymmetricBlockMatrix; + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(matrix_); + ar & BOOST_SERIALIZATION_NVP(variableColOffsets_); + ar & BOOST_SERIALIZATION_NVP(rowStart_); + ar & BOOST_SERIALIZATION_NVP(rowEnd_); + ar & BOOST_SERIALIZATION_NVP(blockStart_); + } + }; + +} diff --git a/gtsam/base/blockMatrices.h b/gtsam/base/blockMatrices.h deleted file mode 100644 index 973d63247..000000000 --- a/gtsam/base/blockMatrices.h +++ /dev/null @@ -1,627 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 blockMatrices.h - * @brief Access to matrices via blocks of pre-defined sizes. Used in GaussianFactor and GaussianConditional. - * @author Richard Roberts - * @date Sep 18, 2010 - */ -#pragma once - -#include - -namespace gtsam { - -template class SymmetricBlockView; - -/** - * This class stores a *reference* to a matrix and allows it to be accessed as - * a collection of vertical blocks. It also provides for accessing individual - * columns from those blocks. When constructed or resized, the caller must - * provide the dimensions of the blocks, as well as an underlying matrix - * storage object. This class will resize the underlying matrix such that it - * is consistent with the given block dimensions. - * - * This class also has three parameters that can be changed after construction - * that change the apparent view of the matrix. firstBlock() determines the - * block that has index 0 for all operations (except for re-setting - * firstBlock()). rowStart() determines the apparent first row of the matrix - * for all operations (except for setting rowStart() and rowEnd()). rowEnd() - * determines the apparent *exclusive* last row for all operations. To include - * all rows, rowEnd() should be set to the number of rows in the matrix (i.e. - * one after the last true row index). - * - * @addtogroup base - */ -template -class VerticalBlockView { -public: - typedef MATRIX FullMatrix; - typedef Eigen::Block Block; - typedef Eigen::Block constBlock; - - // columns of blocks - typedef Eigen::VectorBlock Column; - typedef Eigen::VectorBlock constColumn; - -protected: - FullMatrix& matrix_; // the reference to the full matrix - std::vector variableColOffsets_; // the starting columns of each block (0-based) - - // Changes apparent matrix view, see main class comment. - size_t rowStart_; // 0 initially - size_t rowEnd_; // the number of row - 1, initially - size_t blockStart_; // 0 initially - -public: - /** Construct from an empty matrix (asserts that the matrix is empty) */ - VerticalBlockView(FullMatrix& matrix) : - matrix_(matrix), rowStart_(0), rowEnd_(matrix_.rows()), blockStart_(0) { - fillOffsets((size_t*)0, (size_t*)0); - assertInvariants(); - } - - /** - * Construct from a non-empty matrix and copy the block structure from - * another block view. - */ - template - VerticalBlockView(FullMatrix& matrix, const RHS& rhs) : - matrix_(matrix) { - if((size_t) matrix_.rows() != rhs.rows() || (size_t) matrix_.cols() != rhs.cols()) - throw std::invalid_argument( - "In VerticalBlockView<>(FullMatrix& matrix, const RHS& rhs), matrix and rhs must\n" - "already be of the same size. If not, construct the VerticalBlockView from an\n" - "empty matrix and then use copyStructureFrom(const RHS& rhs) to resize the matrix\n" - "and set up the block structure."); - copyStructureFrom(rhs); - assertInvariants(); - } - - /** Construct from iterators over the sizes of each vertical block */ - template - VerticalBlockView(FullMatrix& matrix, ITERATOR firstBlockDim, ITERATOR lastBlockDim) : - matrix_(matrix), rowStart_(0), rowEnd_(matrix_.rows()), blockStart_(0) { - fillOffsets(firstBlockDim, lastBlockDim); - assertInvariants(); - } - - /** - * Construct from a vector of the sizes of each vertical block, resize the - * matrix so that its height is matrixNewHeight and its width fits the given - * block dimensions. - */ - template - VerticalBlockView(FullMatrix& matrix, ITERATOR firstBlockDim, ITERATOR lastBlockDim, size_t matrixNewHeight) : - matrix_(matrix), rowStart_(0), rowEnd_(matrixNewHeight), blockStart_(0) { - fillOffsets(firstBlockDim, lastBlockDim); - matrix_.resize(matrixNewHeight, variableColOffsets_.back()); - assertInvariants(); - } - - /** Row size - */ - size_t rows() const { assertInvariants(); return rowEnd_ - rowStart_; } - - /** Column size - */ - size_t cols() const { assertInvariants(); return variableColOffsets_.back() - variableColOffsets_[blockStart_]; } - - - /** Block count - */ - size_t nBlocks() const { assertInvariants(); return variableColOffsets_.size() - 1 - blockStart_; } - - - /** Access a single block in the underlying matrix with read/write access */ - inline Block operator()(size_t block) { - return range(block, block+1); - } - - /** Access a const block view */ - inline const constBlock operator()(size_t block) const { - return range(block, block+1); - } - - /** access ranges of blocks at a time */ - inline Block range(size_t startBlock, size_t endBlock) { - assertInvariants(); - size_t actualStartBlock = startBlock + blockStart_; - size_t actualEndBlock = endBlock + blockStart_; - if(startBlock != 0 && endBlock != 0) - checkBlock(actualStartBlock); - assert(actualEndBlock < variableColOffsets_.size()); - const size_t& startCol = variableColOffsets_[actualStartBlock]; - const size_t& endCol = variableColOffsets_[actualEndBlock]; - return matrix_.block(rowStart_, startCol, rowEnd_-rowStart_, endCol-startCol); - } - - inline const constBlock range(size_t startBlock, size_t endBlock) const { - assertInvariants(); - size_t actualStartBlock = startBlock + blockStart_; - size_t actualEndBlock = endBlock + blockStart_; - if(startBlock != 0 && endBlock != 0) - checkBlock(actualStartBlock); - assert(actualEndBlock < variableColOffsets_.size()); - const size_t& startCol = variableColOffsets_[actualStartBlock]; - const size_t& endCol = variableColOffsets_[actualEndBlock]; - return ((const FullMatrix&)matrix_).block(rowStart_, startCol, rowEnd_-rowStart_, endCol-startCol); - } - - /** Return the full matrix, *not* including any portions excluded by rowStart(), rowEnd(), and firstBlock() */ - inline Block full() { - return range(0,nBlocks()); - } - - /** Return the full matrix, *not* including any portions excluded by rowStart(), rowEnd(), and firstBlock() */ - inline const constBlock full() const { - return range(0,nBlocks()); - } - - /** get a single column out of a block */ - Column column(size_t block, size_t columnOffset) { - assertInvariants(); - size_t actualBlock = block + blockStart_; - checkBlock(actualBlock); - assert(variableColOffsets_[actualBlock] + columnOffset < variableColOffsets_[actualBlock+1]); - return matrix_.col(variableColOffsets_[actualBlock] + columnOffset).segment(rowStart_, rowEnd_-rowStart_); - } - - /** get a single column out of a block */ - const constColumn column(size_t block, size_t columnOffset) const { - assertInvariants(); - size_t actualBlock = block + blockStart_; - checkBlock(actualBlock); - assert(variableColOffsets_[actualBlock] + columnOffset < (size_t) matrix_.cols()); - return ((const FullMatrix&)matrix_).col(variableColOffsets_[actualBlock] + columnOffset).segment(rowStart_, rowEnd_-rowStart_); - } - - size_t offset(size_t block) const { - assertInvariants(); - size_t actualBlock = block + blockStart_; - checkBlock(actualBlock); - return variableColOffsets_[actualBlock]; - } - - /** Get or set the apparent first row of the underlying matrix for all operations */ - size_t& rowStart() { return rowStart_; } - - /** Get or set the apparent last row (exclusive, i.e. rows() == rowEnd() - rowStart()) of the underlying matrix for all operations */ - size_t& rowEnd() { return rowEnd_; } - - /** Get or set the apparent first block for all operations */ - size_t& firstBlock() { return blockStart_; } - - /** Get the apparent first row of the underlying matrix for all operations */ - size_t rowStart() const { return rowStart_; } - - /** Get the apparent last row (exclusive, i.e. rows() == rowEnd() - rowStart()) of the underlying matrix for all operations */ - size_t rowEnd() const { return rowEnd_; } - - /** Get the apparent first block for all operations */ - size_t firstBlock() const { return blockStart_; } - - /** access to full matrix (*including* any portions excluded by rowStart(), rowEnd(), and firstBlock()) */ - const FullMatrix& fullMatrix() const { return matrix_; } - - /** - * Copy the block structure and resize the underlying matrix, but do not - * copy the matrix data. If blockStart(), rowStart(), and/or rowEnd() have - * been modified, this copies the structure of the corresponding matrix view. - * In the destination VerticalBlockView, blockStart() and rowStart() will - * thus be 0, rowEnd() will be cols() of the source VerticalBlockView, and - * the underlying matrix will be the size of the view of the source matrix. - */ - template - void copyStructureFrom(const RHS& rhs) { - if((size_t) matrix_.rows() != (size_t) rhs.rows() || (size_t) matrix_.cols() != (size_t) rhs.cols()) - matrix_.resize(rhs.rows(), rhs.cols()); - if(rhs.blockStart_ == 0) - variableColOffsets_ = rhs.variableColOffsets_; - else { - variableColOffsets_.resize(rhs.nBlocks() + 1); - variableColOffsets_[0] = 0; - size_t j=0; - assert(rhs.variableColOffsets_.begin()+rhs.blockStart_ < rhs.variableColOffsets_.end()-1); - for(std::vector::const_iterator off=rhs.variableColOffsets_.begin()+rhs.blockStart_; off!=rhs.variableColOffsets_.end()-1; ++off) { - variableColOffsets_[j+1] = variableColOffsets_[j] + (*(off+1) - *off); - ++ j; - } - } - rowStart_ = 0; - rowEnd_ = matrix_.rows(); - blockStart_ = 0; - assertInvariants(); - } - - /** Copy the block struture and matrix data, resizing the underlying matrix - * in the process. This can deal with assigning between different types of - * underlying matrices, as long as the matrices themselves are assignable. - * To avoid creating a temporary matrix this assumes no aliasing, i.e. that - * no part of the underlying matrices refer to the same memory! - * - * If blockStart(), rowStart(), and/or rowEnd() have been modified, this - * copies the structure of the corresponding matrix view. In the destination - * VerticalBlockView, blockStart() and rowStart() will thus be 0, rowEnd() - * will be cols() of the source VerticalBlockView, and the underlying matrix - * will be the size of the view of the source matrix. - */ - template - VerticalBlockView& assignNoalias(const RHS& rhs) { - copyStructureFrom(rhs); - matrix_.noalias() = rhs.full(); - return *this; - } - - /** Swap the contents of the underlying matrix and the block information with - * another VerticalBlockView. - */ - void swap(VerticalBlockView& other) { - matrix_.swap(other.matrix_); - variableColOffsets_.swap(other.variableColOffsets_); - std::swap(rowStart_, other.rowStart_); - std::swap(rowEnd_, other.rowEnd_); - std::swap(blockStart_, other.blockStart_); - assertInvariants(); - other.assertInvariants(); - } - -protected: - void assertInvariants() const { - assert((size_t) matrix_.cols() == variableColOffsets_.back()); - assert(blockStart_ < variableColOffsets_.size()); - assert(rowStart_ <= (size_t) matrix_.rows()); - assert(rowEnd_ <= (size_t) matrix_.rows()); - assert(rowStart_ <= rowEnd_); - } - - void checkBlock(size_t block) const { - assert((size_t) matrix_.cols() == variableColOffsets_.back()); - assert(block < variableColOffsets_.size()-1); - assert(variableColOffsets_[block] < (size_t) matrix_.cols() && variableColOffsets_[block+1] <= (size_t) matrix_.cols()); - } - - template - void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim) { - variableColOffsets_.resize((lastBlockDim-firstBlockDim)+1); - variableColOffsets_[0] = 0; - size_t j=0; - for(ITERATOR dim=firstBlockDim; dim!=lastBlockDim; ++dim) { - variableColOffsets_[j+1] = variableColOffsets_[j] + *dim; - ++ j; - } - } - - template friend class SymmetricBlockView; - template friend class VerticalBlockView; - -private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(matrix_); - ar & BOOST_SERIALIZATION_NVP(variableColOffsets_); - ar & BOOST_SERIALIZATION_NVP(rowStart_); - ar & BOOST_SERIALIZATION_NVP(rowEnd_); - ar & BOOST_SERIALIZATION_NVP(blockStart_); - } -}; - -/** - * This class stores a *reference* to a matrix and allows it to be accessed as - * a collection of blocks. It also provides for accessing individual - * columns from those blocks. When constructed or resized, the caller must - * provide the dimensions of the blocks, as well as an underlying matrix - * storage object. This class will resize the underlying matrix such that it - * is consistent with the given block dimensions. - * - * This class uses a symmetric block structure. The underlying matrix does not - * necessarily need to be symmetric. - * - * This class also has a parameter that can be changed after construction to - * change the apparent matrix view. firstBlock() determines the block that - * appears to have index 0 for all operations (except re-setting firstBlock()). - * - * @addtogroup base - */ -template -class SymmetricBlockView { -public: - typedef MATRIX FullMatrix; - typedef Eigen::Block Block; - typedef Eigen::Block constBlock; - typedef typename FullMatrix::ColXpr::SegmentReturnType Column; - typedef typename FullMatrix::ConstColXpr::ConstSegmentReturnType constColumn; - -private: - static FullMatrix matrixTemp_; // just for finding types - -protected: - FullMatrix& matrix_; // the reference to the full matrix - std::vector variableColOffsets_; // the starting columns of each block (0-based) - - // Changes apparent matrix view, see main class comment. - size_t blockStart_; // 0 initially - -public: - /** Construct from an empty matrix (asserts that the matrix is empty) */ - SymmetricBlockView(FullMatrix& matrix) : - matrix_(matrix), blockStart_(0) { - fillOffsets((size_t*)0, (size_t*)0); - assertInvariants(); - } - - /** Construct from iterators over the sizes of each block */ - template - SymmetricBlockView(FullMatrix& matrix, ITERATOR firstBlockDim, ITERATOR lastBlockDim) : - matrix_(matrix), blockStart_(0) { - fillOffsets(firstBlockDim, lastBlockDim); - assertInvariants(); - } - - /** - * Modify the size and structure of the underlying matrix and this block - * view. If 'preserve' is true, the underlying matrix data will be copied if - * the matrix size changes, otherwise the new data will be uninitialized. - */ - template - void resize(ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool preserve) { - blockStart_ = 0; - fillOffsets(firstBlockDim, lastBlockDim); - if (preserve) - matrix_.conservativeResize(variableColOffsets_.back(), variableColOffsets_.back()); - else - matrix_.resize(variableColOffsets_.back(), variableColOffsets_.back()); - } - - /** Row size - */ - size_t rows() const { assertInvariants(); return variableColOffsets_.back() - variableColOffsets_[blockStart_]; } - - /** Column size - */ - size_t cols() const { return rows(); } - - - /** Block count - */ - size_t nBlocks() const { assertInvariants(); return variableColOffsets_.size() - 1 - blockStart_; } - - - Block operator()(size_t i_block, size_t j_block) { - return range(i_block, i_block+1, j_block, j_block+1); - } - - constBlock operator()(size_t i_block, size_t j_block) const { - return range(i_block, i_block+1, j_block, j_block+1); - } - - Block range(size_t i_startBlock, size_t i_endBlock, size_t j_startBlock, size_t j_endBlock) { - assertInvariants(); - size_t i_actualStartBlock = i_startBlock + blockStart_; - size_t i_actualEndBlock = i_endBlock + blockStart_; - size_t j_actualStartBlock = j_startBlock + blockStart_; - size_t j_actualEndBlock = j_endBlock + blockStart_; - checkBlock(i_actualStartBlock); - checkBlock(j_actualStartBlock); - assert(i_actualEndBlock < variableColOffsets_.size()); - assert(j_actualEndBlock < variableColOffsets_.size()); - return matrix_.block( - variableColOffsets_[i_actualStartBlock], variableColOffsets_[j_actualStartBlock], - variableColOffsets_[i_actualEndBlock]-variableColOffsets_[i_actualStartBlock], - variableColOffsets_[j_actualEndBlock]-variableColOffsets_[j_actualStartBlock]); - } - - constBlock range(size_t i_startBlock, size_t i_endBlock, size_t j_startBlock, size_t j_endBlock) const { - assertInvariants(); - size_t i_actualStartBlock = i_startBlock + blockStart_; - size_t i_actualEndBlock = i_endBlock + blockStart_; - size_t j_actualStartBlock = j_startBlock + blockStart_; - size_t j_actualEndBlock = j_endBlock + blockStart_; - checkBlock(i_actualStartBlock); - checkBlock(j_actualStartBlock); - assert(i_actualEndBlock < variableColOffsets_.size()); - assert(j_actualEndBlock < variableColOffsets_.size()); - return ((const FullMatrix&)matrix_).block( - variableColOffsets_[i_actualStartBlock], variableColOffsets_[j_actualStartBlock], - variableColOffsets_[i_actualEndBlock]-variableColOffsets_[i_actualStartBlock], - variableColOffsets_[j_actualEndBlock]-variableColOffsets_[j_actualStartBlock]); - } - - Block full() { - return range(0,nBlocks(), 0,nBlocks()); - } - - constBlock full() const { - return range(0,nBlocks(), 0,nBlocks()); - } - - /** access to full matrix */ - const FullMatrix& fullMatrix() const { return matrix_; } - - Column column(size_t i_block, size_t j_block, size_t columnOffset) { - assertInvariants(); - size_t i_actualBlock = i_block + blockStart_; - size_t j_actualBlock = j_block + blockStart_; - checkBlock(i_actualBlock); - checkBlock(j_actualBlock); - assert(i_actualBlock < variableColOffsets_.size()); - assert(j_actualBlock < variableColOffsets_.size()); - assert(variableColOffsets_[j_actualBlock] + columnOffset < variableColOffsets_[j_actualBlock+1]); - - return matrix_.col( - variableColOffsets_[j_actualBlock] + columnOffset).segment( - variableColOffsets_[i_actualBlock], - variableColOffsets_[i_actualBlock+1]-variableColOffsets_[i_actualBlock]); - } - - constColumn column(size_t i_block, size_t j_block, size_t columnOffset) const { - assertInvariants(); - size_t i_actualBlock = i_block + blockStart_; - size_t j_actualBlock = j_block + blockStart_; - checkBlock(i_actualBlock); - checkBlock(j_actualBlock); - assert(i_actualBlock < variableColOffsets_.size()); - assert(j_actualBlock < variableColOffsets_.size()); - assert(variableColOffsets_[j_actualBlock] + columnOffset < variableColOffsets_[j_actualBlock+1]); - - return ((const FullMatrix&)matrix_).col( - variableColOffsets_[j_actualBlock] + columnOffset).segment( - variableColOffsets_[i_actualBlock], - variableColOffsets_[i_actualBlock+1]-variableColOffsets_[i_actualBlock]); -// assertInvariants(); -// size_t j_actualBlock = j_block + blockStart_; -// assert(variableColOffsets_[j_actualBlock] + columnOffset < variableColOffsets_[j_actualBlock+1]); -// constBlock blockMat(operator()(i_block, j_block)); -// return constColumn(blockMat, columnOffset); - } - - Column rangeColumn(size_t i_startBlock, size_t i_endBlock, size_t j_block, size_t columnOffset) { - assertInvariants(); - - size_t i_actualStartBlock = i_startBlock + blockStart_; - size_t i_actualEndBlock = i_endBlock + blockStart_; - size_t j_actualStartBlock = j_block + blockStart_; - checkBlock(i_actualStartBlock); - checkBlock(j_actualStartBlock); - assert(i_actualEndBlock < variableColOffsets_.size()); - assert(variableColOffsets_[j_actualStartBlock] + columnOffset < variableColOffsets_[j_actualStartBlock+1]); - - return matrix_.col( - variableColOffsets_[j_actualStartBlock] + columnOffset).segment( - variableColOffsets_[i_actualStartBlock], - variableColOffsets_[i_actualEndBlock]-variableColOffsets_[i_actualStartBlock]); - } - - constColumn rangeColumn(size_t i_startBlock, size_t i_endBlock, size_t j_block, size_t columnOffset) const { - assertInvariants(); - - size_t i_actualStartBlock = i_startBlock + blockStart_; - size_t i_actualEndBlock = i_endBlock + blockStart_; - size_t j_actualStartBlock = j_block + blockStart_; - checkBlock(i_actualStartBlock); - checkBlock(j_actualStartBlock); - assert(i_actualEndBlock < variableColOffsets_.size()); - assert(variableColOffsets_[j_actualStartBlock] + columnOffset < variableColOffsets_[j_actualStartBlock+1]); - - return ((const FullMatrix&)matrix_).col( - variableColOffsets_[j_actualStartBlock] + columnOffset).segment( - variableColOffsets_[i_actualStartBlock], - variableColOffsets_[i_actualEndBlock]-variableColOffsets_[i_actualStartBlock]); - } - - size_t offset(size_t block) const { - assertInvariants(); - size_t actualBlock = block + blockStart_; - checkBlock(actualBlock); - return variableColOffsets_[actualBlock]; - } - - size_t& blockStart() { return blockStart_; } - size_t blockStart() const { return blockStart_; } - - /** Copy the block structure and resize the underlying matrix, but do not - * copy the matrix data. If blockStart() has been modified, this copies the - * structure of the corresponding matrix view. In the destination - * SymmetricBlockView, startBlock() will thus be 0 and the underlying matrix - * will be the size of the view of the source matrix. - */ - template - void copyStructureFrom(const RHS& rhs) { - matrix_.resize(rhs.cols(), rhs.cols()); - if(rhs.blockStart_ == 0) - variableColOffsets_ = rhs.variableColOffsets_; - else { - variableColOffsets_.resize(rhs.nBlocks() + 1); - variableColOffsets_[0] = 0; - size_t j=0; - assert(rhs.variableColOffsets_.begin()+rhs.blockStart_ < rhs.variableColOffsets_.end()-1); - for(std::vector::const_iterator off=rhs.variableColOffsets_.begin()+rhs.blockStart_; off!=rhs.variableColOffsets_.end()-1; ++off) { - variableColOffsets_[j+1] = variableColOffsets_[j] + (*(off+1) - *off); - ++ j; - } - } - blockStart_ = 0; - assertInvariants(); - } - - /** Copy the block struture and matrix data, resizing the underlying matrix - * in the process. This can deal with assigning between different types of - * underlying matrices, as long as the matrices themselves are assignable. - * To avoid creating a temporary matrix this assumes no aliasing, i.e. that - * no part of the underlying matrices refer to the same memory! - * - * If blockStart() has been modified, this copies the structure of the - * corresponding matrix view. In the destination SymmetricBlockView, - * startBlock() will thus be 0 and the underlying matrix will be the size - * of the view of the source matrix. - */ - template - SymmetricBlockView& assignNoalias(const SymmetricBlockView& rhs) { - copyStructureFrom(rhs); - matrix_.noalias() = rhs.full(); - return *this; - } - - /** Swap the contents of the underlying matrix and the block information with - * another VerticalBlockView. - */ - void swap(SymmetricBlockView& other) { - matrix_.swap(other.matrix_); - variableColOffsets_.swap(other.variableColOffsets_); - std::swap(blockStart_, other.blockStart_); - assertInvariants(); - other.assertInvariants(); - } - -protected: - void assertInvariants() const { - assert(matrix_.rows() == matrix_.cols()); - assert((size_t) matrix_.cols() == variableColOffsets_.back()); - assert(blockStart_ < variableColOffsets_.size()); - } - - void checkBlock(size_t block) const { - assert(matrix_.rows() == matrix_.cols()); - assert((size_t) matrix_.cols() == variableColOffsets_.back()); - assert(block < variableColOffsets_.size()-1); - assert(variableColOffsets_[block] < (size_t) matrix_.cols() && variableColOffsets_[block+1] <= (size_t) matrix_.cols()); - } - - template - void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim) { - variableColOffsets_.resize((lastBlockDim-firstBlockDim)+1); - variableColOffsets_[0] = 0; - size_t j=0; - for(ITERATOR dim=firstBlockDim; dim!=lastBlockDim; ++dim) { - variableColOffsets_[j+1] = variableColOffsets_[j] + *dim; - ++ j; - } - } - - template friend class SymmetricBlockView; - template friend class VerticalBlockView; - -private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(matrix_); - ar & BOOST_SERIALIZATION_NVP(variableColOffsets_); - ar & BOOST_SERIALIZATION_NVP(blockStart_); - } -}; - - -} diff --git a/gtsam/base/cholesky.cpp b/gtsam/base/cholesky.cpp index f3c6b2b23..f6e2848f6 100644 --- a/gtsam/base/cholesky.cpp +++ b/gtsam/base/cholesky.cpp @@ -120,6 +120,8 @@ pair choleskyCareful(Matrix& ATA, int order) { /* ************************************************************************* */ bool choleskyPartial(Matrix& ABC, size_t nFrontal) { + gttic(choleskyPartial); + const bool debug = ISDEBUG("choleskyPartial"); assert(ABC.rows() == ABC.cols()); @@ -129,8 +131,17 @@ bool choleskyPartial(Matrix& ABC, size_t nFrontal) { // Compute Cholesky factorization of A, overwrites A. gttic(lld); - Eigen::LLT llt = ABC.block(0,0,nFrontal,nFrontal).selfadjointView().llt(); - ABC.block(0,0,nFrontal,nFrontal).triangularView() = llt.matrixU(); + Eigen::ComputationInfo lltResult; + if(nFrontal > 0) + { + Eigen::LLT llt = ABC.block(0, 0, nFrontal, nFrontal).selfadjointView().llt(); + ABC.block(0, 0, nFrontal, nFrontal).triangularView() = llt.matrixU(); + lltResult = llt.info(); + } + else + { + lltResult = Eigen::Success; + } gttoc(lld); if(debug) cout << "R:\n" << Eigen::MatrixXd(ABC.topLeftCorner(nFrontal,nFrontal).triangularView()) << endl; @@ -155,7 +166,7 @@ bool choleskyPartial(Matrix& ABC, size_t nFrontal) { // Check last diagonal element - Eigen does not check it bool ok; - if(llt.info() == Eigen::Success) { + if(lltResult == Eigen::Success) { if(nFrontal >= 2) { int exp2, exp1; (void)frexp(ABC(nFrontal-2, nFrontal-2), &exp2); diff --git a/gtsam/base/lieProxies.h b/gtsam/base/lieProxies.h index 302ac32ea..ce224496e 100644 --- a/gtsam/base/lieProxies.h +++ b/gtsam/base/lieProxies.h @@ -17,6 +17,8 @@ #pragma once +#include + /** * Global functions in a separate testing namespace * diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index 053a89da8..036f23f68 100644 --- a/gtsam/base/numericalDerivative.h +++ b/gtsam/base/numericalDerivative.h @@ -59,7 +59,7 @@ namespace gtsam { /** global functions for converting to a LieVector for use with numericalDerivative */ inline LieVector makeLieVector(const Vector& v) { return LieVector(v); } - inline LieVector makeLieVectorD(double d) { return LieVector(Vector_(1, d)); } + inline LieVector makeLieVectorD(double d) { return LieVector((Vector) (Vector(1) << d)); } /** * Numerically compute gradient of scalar function diff --git a/gtsam/base/serialization.h b/gtsam/base/serialization.h index a41182f47..89544cfd6 100644 --- a/gtsam/base/serialization.h +++ b/gtsam/base/serialization.h @@ -135,12 +135,12 @@ void deserializeBinary(const std::string& serialized, T& output, const std::stri } template -bool deserializeToBinaryFile(const T& input, const std::string& filename, const std::string& name="data") { +bool serializeToBinaryFile(const T& input, const std::string& filename, const std::string& name="data") { std::ofstream out_archive_stream(filename.c_str()); if (!out_archive_stream.is_open()) return false; boost::archive::binary_oarchive out_archive(out_archive_stream); - out_archive << boost::serialization::make_nvp(name.c_str(), input);; + out_archive << boost::serialization::make_nvp(name.c_str(), input); out_archive_stream.close(); return true; } diff --git a/gtsam/base/tests/CMakeLists.txt b/gtsam/base/tests/CMakeLists.txt new file mode 100644 index 000000000..8e99ef5ba --- /dev/null +++ b/gtsam/base/tests/CMakeLists.txt @@ -0,0 +1 @@ +gtsamAddTestsGlob(base "test*.cpp" "" "gtsam") diff --git a/gtsam/base/tests/testBlockMatrices.cpp b/gtsam/base/tests/testBlockMatrices.cpp deleted file mode 100644 index 3950d561a..000000000 --- a/gtsam/base/tests/testBlockMatrices.cpp +++ /dev/null @@ -1,133 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 testBlockMatrices - * @author Alex Cunningham - */ - -#include -#include -#include - -using namespace std; -using namespace gtsam; - -/* ************************************************************************* */ -TEST(testBlockMatrices, jacobian_factor1) { - typedef Matrix AbMatrix; - typedef VerticalBlockView BlockAb; - - AbMatrix matrix; // actual matrix - empty to start with - - // from JacobianFactor::Constructor - one variable - Matrix A1 = Matrix_(2,3, - 1., 2., 3., - 4., 5., 6.); - Vector b = Vector_(2, 7., 8.); - size_t dims[] = { A1.cols(), 1}; - - // build the structure - BlockAb Ab(matrix, dims, dims+2, b.size()); - - // add a matrix and get back out - Ab(0) = A1; - EXPECT(assert_equal(A1, Ab(0))); - - // add vector to the system - Ab.column(1, 0) = b; - EXPECT(assert_equal(A1, Ab(0))); - EXPECT(assert_equal(b, Ab.column(1,0))); - - // examine matrix contents - EXPECT_LONGS_EQUAL(2, Ab.nBlocks()); - Matrix expFull = Matrix_(2, 4, - 1., 2., 3., 7., - 4., 5., 6., 8.); - Matrix actFull = Ab.full(); - EXPECT(assert_equal(expFull, actFull)); -} - -/* ************************************************************************* */ -TEST(testBlockMatrices, jacobian_factor2) { - typedef Matrix AbMatrix; - typedef VerticalBlockView BlockAb; - - AbMatrix matrix; // actual matrix - empty to start with - - // from JacobianFactor::Constructor - two variables - Matrix A1 = Matrix_(2,3, - 1., 2., 3., - 4., 5., 6.); - Matrix A2 = Matrix_(2,1, - 10., - 11.); - Vector b = Vector_(2, 7., 8.); - size_t dims[] = { A1.cols(), A2.cols(), 1}; - - // build the structure - BlockAb Ab(matrix, dims, dims+3, b.size()); - - // add blocks - Ab(0) = A1; - Ab(1) = A2; - EXPECT(assert_equal(A1, Ab(0))); - EXPECT(assert_equal(A2, Ab(1))); - - // add vector to the system - Ab.column(2, 0) = b; - EXPECT(assert_equal(A1, Ab(0))); - EXPECT(assert_equal(A2, Ab(1))); - EXPECT(assert_equal(b, Ab.column(2,0))); - - // examine matrix contents - EXPECT_LONGS_EQUAL(3, Ab.nBlocks()); - Matrix expFull = Matrix_(2, 5, - 1., 2., 3., 10., 7., - 4., 5., 6., 11., 8.); - Matrix actFull = Ab.full(); - EXPECT(assert_equal(expFull, actFull)); -} - -/* ************************************************************************* */ -TEST(testBlockMatrices, hessian_factor1) { - typedef Matrix InfoMatrix; - typedef SymmetricBlockView BlockInfo; - - Matrix expected_full = Matrix_(3, 3, - 3.0, 5.0, -8.0, - 0.0, 6.0, -9.0, - 0.0, 0.0, 10.0); - - Matrix G = Matrix_(2,2, 3.0, 5.0, 0.0, 6.0); - Vector g = Vector_(2, -8.0, -9.0); - double f = 10.0; - - size_t dims[] = { G.rows(), 1 }; - InfoMatrix fullMatrix = zeros(G.rows() + 1, G.rows() + 1); - BlockInfo infoView(fullMatrix, dims, dims+2); - infoView(0,0) = G; - infoView.column(0,1,0) = g; - infoView(1,1)(0,0) = f; - - EXPECT_LONGS_EQUAL(0, infoView.blockStart()); - EXPECT_LONGS_EQUAL(2, infoView.nBlocks()); - EXPECT(assert_equal(InfoMatrix(expected_full), fullMatrix)); - EXPECT(assert_equal(InfoMatrix(G), infoView.range(0, 1, 0, 1))) - EXPECT_DOUBLES_EQUAL(f, infoView(1, 1)(0,0), 1e-10); - - EXPECT(assert_equal(g, Vector(infoView.rangeColumn(0, 1, 1, 0)))); - EXPECT(assert_equal(g, Vector(((const BlockInfo)infoView).rangeColumn(0, 1, 1, 0)))); -} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ diff --git a/gtsam/base/tests/testCholesky.cpp b/gtsam/base/tests/testCholesky.cpp index 93e8ce2a2..b30326633 100644 --- a/gtsam/base/tests/testCholesky.cpp +++ b/gtsam/base/tests/testCholesky.cpp @@ -27,7 +27,7 @@ TEST(cholesky, choleskyPartial) { // choleskyPartial should only use the upper triangle, so this represents a // symmetric matrix. - Matrix ABC = Matrix_(7,7, + Matrix ABC = (Matrix(7,7) << 4.0375, 3.4584, 3.5735, 2.4815, 2.1471, 2.7400, 2.2063, 0., 4.7267, 3.8423, 2.3624, 2.8091, 2.9579, 2.5914, 0., 0., 5.1600, 2.0797, 3.4690, 3.2419, 2.9992, @@ -55,7 +55,7 @@ TEST(cholesky, choleskyPartial) { /* ************************************************************************* */ TEST(cholesky, BadScalingCholesky) { - Matrix A = Matrix_(2,2, + Matrix A = (Matrix(2,2) << 1e-40, 0.0, 0.0, 1.0); @@ -70,7 +70,7 @@ TEST(cholesky, BadScalingCholesky) { /* ************************************************************************* */ TEST(cholesky, BadScalingSVD) { - Matrix A = Matrix_(2,2, + Matrix A = (Matrix(2,2) << 1.0, 0.0, 0.0, 1e-40); diff --git a/gtsam/base/tests/testDSFVector.cpp b/gtsam/base/tests/testDSFVector.cpp index cad2f6ded..d900e1779 100644 --- a/gtsam/base/tests/testDSFVector.cpp +++ b/gtsam/base/tests/testDSFVector.cpp @@ -16,155 +16,210 @@ * @brief unit tests for DSF */ -#include +#include + +#include + +#include #include #include #include #include using namespace boost::assign; -#include -#include +#include using namespace std; using namespace gtsam; /* ************************************************************************* */ -TEST(DSFVectorVector, findSet) { - DSFVector dsf(3); - CHECK(dsf.findSet(0) != dsf.findSet(2)); +TEST(DSFBase, find) { + DSFBase dsf(3); + EXPECT(dsf.find(0) != dsf.find(2)); } /* ************************************************************************* */ -TEST(DSFVectorVector, makeUnionInPlace) { - DSFVector dsf(3); - dsf.makeUnionInPlace(0,2); - CHECK(dsf.findSet(0) == dsf.findSet(2)); +TEST(DSFBase, merge) { + DSFBase dsf(3); + dsf.merge(0,2); + EXPECT(dsf.find(0) == dsf.find(2)); } /* ************************************************************************* */ -TEST(DSFVectorVector, makeUnionInPlace2) { - boost::shared_ptr v = boost::make_shared(5); +TEST(DSFBase, makeUnion2) { + DSFBase dsf(3); + dsf.merge(2,0); + EXPECT(dsf.find(0) == dsf.find(2)); +} + +/* ************************************************************************* */ +TEST(DSFBase, makeUnion3) { + DSFBase dsf(3); + dsf.merge(0,1); + dsf.merge(1,2); + EXPECT(dsf.find(0) == dsf.find(2)); +} + +/* ************************************************************************* */ +TEST(DSFBase, mergePairwiseMatches) { + + // Create some "matches" + typedef pair Match; + vector matches; + matches += Match(1,2), Match(2,3), Match(4,5), Match(4,6); + + // Merge matches + DSFBase dsf(7); // We allow for keys 0..6 + BOOST_FOREACH(const Match& m, matches) + dsf.merge(m.first,m.second); + + // Each point is now associated with a set, represented by one of its members + size_t rep1 = 1, rep2 = 4; + EXPECT_LONGS_EQUAL(rep1,dsf.find(1)); + EXPECT_LONGS_EQUAL(rep1,dsf.find(2)); + EXPECT_LONGS_EQUAL(rep1,dsf.find(3)); + EXPECT_LONGS_EQUAL(rep2,dsf.find(4)); + EXPECT_LONGS_EQUAL(rep2,dsf.find(5)); + EXPECT_LONGS_EQUAL(rep2,dsf.find(6)); +} + +/* ************************************************************************* */ +TEST(DSFVector, merge2) { + boost::shared_ptr v = boost::make_shared(5); std::vector keys; keys += 1, 3; DSFVector dsf(v, keys); - dsf.makeUnionInPlace(1,3); - CHECK(dsf.findSet(1) == dsf.findSet(3)); -} - -/* ************************************************************************* */ -TEST(DSFVector, makeUnion2) { - DSFVector dsf(3); - dsf.makeUnionInPlace(2,0); - CHECK(dsf.findSet(0) == dsf.findSet(2)); -} - -/* ************************************************************************* */ -TEST(DSFVector, makeUnion3) { - DSFVector dsf(3); - dsf.makeUnionInPlace(0,1); - dsf.makeUnionInPlace(1,2); - CHECK(dsf.findSet(0) == dsf.findSet(2)); + dsf.merge(1,3); + EXPECT(dsf.find(1) == dsf.find(3)); } /* ************************************************************************* */ TEST(DSFVector, sets) { DSFVector dsf(2); - dsf.makeUnionInPlace(0,1); + dsf.merge(0,1); map > sets = dsf.sets(); LONGS_EQUAL(1, sets.size()); set expected; expected += 0, 1; - CHECK(expected == sets[dsf.findSet(0)]); + EXPECT(expected == sets[dsf.find(0)]); } /* ************************************************************************* */ TEST(DSFVector, arrays) { DSFVector dsf(2); - dsf.makeUnionInPlace(0,1); + dsf.merge(0,1); map > arrays = dsf.arrays(); LONGS_EQUAL(1, arrays.size()); vector expected; expected += 0, 1; - CHECK(expected == arrays[dsf.findSet(0)]); + EXPECT(expected == arrays[dsf.find(0)]); } /* ************************************************************************* */ TEST(DSFVector, sets2) { DSFVector dsf(3); - dsf.makeUnionInPlace(0,1); - dsf.makeUnionInPlace(1,2); + dsf.merge(0,1); + dsf.merge(1,2); map > sets = dsf.sets(); LONGS_EQUAL(1, sets.size()); set expected; expected += 0, 1, 2; - CHECK(expected == sets[dsf.findSet(0)]); + EXPECT(expected == sets[dsf.find(0)]); } /* ************************************************************************* */ TEST(DSFVector, arrays2) { DSFVector dsf(3); - dsf.makeUnionInPlace(0,1); - dsf.makeUnionInPlace(1,2); + dsf.merge(0,1); + dsf.merge(1,2); map > arrays = dsf.arrays(); LONGS_EQUAL(1, arrays.size()); vector expected; expected += 0, 1, 2; - CHECK(expected == arrays[dsf.findSet(0)]); + EXPECT(expected == arrays[dsf.find(0)]); } /* ************************************************************************* */ TEST(DSFVector, sets3) { DSFVector dsf(3); - dsf.makeUnionInPlace(0,1); + dsf.merge(0,1); map > sets = dsf.sets(); LONGS_EQUAL(2, sets.size()); set expected; expected += 0, 1; - CHECK(expected == sets[dsf.findSet(0)]); + EXPECT(expected == sets[dsf.find(0)]); } /* ************************************************************************* */ TEST(DSFVector, arrays3) { DSFVector dsf(3); - dsf.makeUnionInPlace(0,1); + dsf.merge(0,1); map > arrays = dsf.arrays(); LONGS_EQUAL(2, arrays.size()); vector expected; expected += 0, 1; - CHECK(expected == arrays[dsf.findSet(0)]); + EXPECT(expected == arrays[dsf.find(0)]); } /* ************************************************************************* */ TEST(DSFVector, set) { DSFVector dsf(3); - dsf.makeUnionInPlace(0,1); + dsf.merge(0,1); set set = dsf.set(0); LONGS_EQUAL(2, set.size()); std::set expected; expected += 0, 1; - CHECK(expected == set); + EXPECT(expected == set); } /* ************************************************************************* */ TEST(DSFVector, set2) { DSFVector dsf(3); - dsf.makeUnionInPlace(0,1); - dsf.makeUnionInPlace(1,2); + dsf.merge(0,1); + dsf.merge(1,2); set set = dsf.set(0); LONGS_EQUAL(3, set.size()); std::set expected; expected += 0, 1, 2; - CHECK(expected == set); + EXPECT(expected == set); } /* ************************************************************************* */ TEST(DSFVector, isSingleton) { DSFVector dsf(3); - dsf.makeUnionInPlace(0,1); - CHECK(!dsf.isSingleton(0)); - CHECK(!dsf.isSingleton(1)); - CHECK( dsf.isSingleton(2)); + dsf.merge(0,1); + EXPECT(!dsf.isSingleton(0)); + EXPECT(!dsf.isSingleton(1)); + EXPECT( dsf.isSingleton(2)); } + +/* ************************************************************************* */ +TEST(DSFVector, mergePairwiseMatches) { + + // Create some measurements + vector keys; + keys += 1,2,3,4,5,6; + + // Create some "matches" + typedef pair Match; + vector matches; + matches += Match(1,2), Match(2,3), Match(4,5), Match(4,6); + + // Merge matches + DSFVector dsf(keys); + BOOST_FOREACH(const Match& m, matches) + dsf.merge(m.first,m.second); + + // Check that we have two connected components, 1,2,3 and 4,5,6 + map > sets = dsf.sets(); + LONGS_EQUAL(2, sets.size()); + set expected1; expected1 += 1,2,3; + set actual1 = sets[dsf.find(2)]; + EXPECT(expected1 == actual1); + set expected2; expected2 += 4,5,6; + set actual2 = sets[dsf.find(5)]; + EXPECT(expected2 == actual2); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ diff --git a/gtsam/base/tests/testLieMatrix.cpp b/gtsam/base/tests/testLieMatrix.cpp index 5ef867e85..ee8fe14d9 100644 --- a/gtsam/base/tests/testLieMatrix.cpp +++ b/gtsam/base/tests/testLieMatrix.cpp @@ -27,7 +27,7 @@ GTSAM_CONCEPT_LIE_INST(LieMatrix) /* ************************************************************************* */ TEST( LieMatrix, construction ) { - Matrix m = Matrix_(2,2, 1.0, 2.0, 3.0, 4.0); + Matrix m = (Matrix(2,2) << 1.0,2.0, 3.0,4.0); LieMatrix lie1(m), lie2(m); EXPECT(lie1.dim() == 4); @@ -37,30 +37,31 @@ TEST( LieMatrix, construction ) { /* ************************************************************************* */ TEST( LieMatrix, other_constructors ) { - Matrix init = Matrix_(2,2, 10.0, 20.0, 30.0, 40.0); + Matrix init = (Matrix(2,2) << 10.0,20.0, 30.0,40.0); LieMatrix exp(init); - LieMatrix a(2,2,10.0,20.0,30.0,40.0); double data[] = {10,30,20,40}; LieMatrix b(2,2,data); - EXPECT(assert_equal(exp, a)); EXPECT(assert_equal(exp, b)); - EXPECT(assert_equal(b, a)); } /* ************************************************************************* */ TEST(LieMatrix, retract) { - LieMatrix init(2,2, 1.0,2.0,3.0,4.0); - Vector update = Vector_(4, 3.0, 4.0, 6.0, 7.0); + LieMatrix init((Matrix(2,2) << 1.0,2.0,3.0,4.0)); + Vector update = (Vector(4) << 3.0, 4.0, 6.0, 7.0); - LieMatrix expected(2,2, 4.0, 6.0, 9.0, 11.0); + LieMatrix expected((Matrix(2,2) << 4.0, 6.0, 9.0, 11.0)); LieMatrix actual = init.retract(update); EXPECT(assert_equal(expected, actual)); - LieMatrix expectedUpdate = update; - LieMatrix actualUpdate = init.localCoordinates(actual); + Vector expectedUpdate = update; + Vector actualUpdate = init.localCoordinates(actual); EXPECT(assert_equal(expectedUpdate, actualUpdate)); + + Vector expectedLogmap = (Vector(4) << 1, 2, 3, 4); + Vector actualLogmap = LieMatrix::Logmap(LieMatrix((Matrix(2,2) << 1.0, 2.0, 3.0, 4.0))); + EXPECT(assert_equal(expectedLogmap, actualLogmap)); } /* ************************************************************************* */ diff --git a/gtsam/base/tests/testLieScalar.cpp b/gtsam/base/tests/testLieScalar.cpp index 11157a701..68655cc71 100644 --- a/gtsam/base/tests/testLieScalar.cpp +++ b/gtsam/base/tests/testLieScalar.cpp @@ -42,7 +42,7 @@ TEST( testLieScalar, construction ) { TEST( testLieScalar, localCoordinates ) { LieScalar lie1(1.), lie2(3.); - EXPECT(assert_equal(Vector_(1, 2.), lie1.localCoordinates(lie2))); + EXPECT(assert_equal((Vector)(Vector(1) << 2), lie1.localCoordinates(lie2))); } /* ************************************************************************* */ diff --git a/gtsam/base/tests/testLieVector.cpp b/gtsam/base/tests/testLieVector.cpp index 74be6628f..f66678c25 100644 --- a/gtsam/base/tests/testLieVector.cpp +++ b/gtsam/base/tests/testLieVector.cpp @@ -27,7 +27,7 @@ GTSAM_CONCEPT_LIE_INST(LieVector) /* ************************************************************************* */ TEST( testLieVector, construction ) { - Vector v = Vector_(3, 1.0, 2.0, 3.0); + Vector v = (Vector(3) << 1.0, 2.0, 3.0); LieVector lie1(v), lie2(v); EXPECT(lie1.dim() == 3); @@ -37,16 +37,11 @@ TEST( testLieVector, construction ) { /* ************************************************************************* */ TEST( testLieVector, other_constructors ) { - Vector init = Vector_(2, 10.0, 20.0); + Vector init = (Vector(2) << 10.0, 20.0); LieVector exp(init); - LieVector a(2,10.0,20.0); double data[] = {10,20}; LieVector b(2,data); - LieVector c(2.3), c_exp(LieVector(1, 2.3)); - EXPECT(assert_equal(exp, a)); EXPECT(assert_equal(exp, b)); - EXPECT(assert_equal(b, a)); - EXPECT(assert_equal(c_exp, c)); } /* ************************************************************************* */ diff --git a/gtsam/base/tests/testMatrix.cpp b/gtsam/base/tests/testMatrix.cpp index c5fae2585..9d84e5ab7 100644 --- a/gtsam/base/tests/testMatrix.cpp +++ b/gtsam/base/tests/testMatrix.cpp @@ -32,8 +32,7 @@ static const double tol = 1e-9; /* ************************************************************************* */ TEST( matrix, constructor_data ) { - double data[] = { -5, 3, 0, -5 }; - Matrix A = Matrix_(2, 2, data); + Matrix A = (Matrix(2, 2) << -5, 3, 0, -5); Matrix B(2, 2); B(0, 0) = -5; @@ -44,21 +43,10 @@ TEST( matrix, constructor_data ) EQUALITY(A,B); } -/* ************************************************************************* */ -TEST( matrix, constructor_vector ) -{ - double data[] = { -5, 3, 0, -5 }; - Matrix A = Matrix_(2, 2, data); - Vector v(4); - copy(data, data + 4, v.data()); - Matrix B = Matrix_(2, 2, v); // this one is column order ! - EQUALITY(A,trans(B)); -} - /* ************************************************************************* */ TEST( matrix, Matrix_ ) { - Matrix A = Matrix_(2, 2, -5.0, 3.0, 00.0, -5.0); + Matrix A = (Matrix(2, 2) << -5.0, 3.0, 0.0, -5.0); Matrix B(2, 2); B(0, 0) = -5; B(0, 1) = 3; @@ -69,10 +57,55 @@ TEST( matrix, Matrix_ ) } +namespace { + /* ************************************************************************* */ + template + Matrix testFcn1(const Eigen::DenseBase& in) + { + return in; + } + + /* ************************************************************************* */ + template + Matrix testFcn2(const Eigen::MatrixBase& in) + { + return in; + } +} + +/* ************************************************************************* */ +TEST( matrix, special_comma_initializer) +{ + Matrix expected(2,2); + expected(0,0) = 1; + expected(0,1) = 2; + expected(1,0) = 3; + expected(1,1) = 4; + + Matrix actual1 = (Matrix(2,2) << 1, 2, 3, 4); + Matrix actual2((Matrix(2,2) << 1, 2, 3, 4)); + + Matrix submat1 = (Matrix(1,2) << 3, 4); + Matrix actual3 = (Matrix(2,2) << 1, 2, submat1); + + Matrix submat2 = (Matrix(1,2) << 1, 2); + Matrix actual4 = (Matrix(2,2) << submat2, 3, 4); + + Matrix actual5 = testFcn1((Matrix(2,2) << 1, 2, 3, 4)); + Matrix actual6 = testFcn2((Matrix(2,2) << 1, 2, 3, 4)); + + EXPECT(assert_equal(expected, actual1)); + EXPECT(assert_equal(expected, actual2)); + EXPECT(assert_equal(expected, actual3)); + EXPECT(assert_equal(expected, actual4)); + EXPECT(assert_equal(expected, actual5)); + EXPECT(assert_equal(expected, actual6)); +} + /* ************************************************************************* */ TEST( matrix, col_major ) { - Matrix A = Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0); + Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0); const double * const a = &A(0, 0); EXPECT_DOUBLES_EQUAL(1, a[0], tol); EXPECT_DOUBLES_EQUAL(3, a[1], tol); @@ -83,8 +116,8 @@ TEST( matrix, col_major ) /* ************************************************************************* */ TEST( matrix, collect1 ) { - Matrix A = Matrix_(2, 2, -5.0, 3.0, 00.0, -5.0); - Matrix B = Matrix_(2, 3, -0.5, 2.1, 1.1, 3.4, 2.6, 7.1); + Matrix A = (Matrix(2, 2) << -5.0, 3.0, 00.0, -5.0); + Matrix B = (Matrix(2, 3) << -0.5, 2.1, 1.1, 3.4, 2.6, 7.1); Matrix AB = collect(2, &A, &B); Matrix C(2, 5); for (int i = 0; i < 2; i++) @@ -100,8 +133,8 @@ TEST( matrix, collect1 ) /* ************************************************************************* */ TEST( matrix, collect2 ) { - Matrix A = Matrix_(2, 2, -5.0, 3.0, 00.0, -5.0); - Matrix B = Matrix_(2, 3, -0.5, 2.1, 1.1, 3.4, 2.6, 7.1); + Matrix A = (Matrix(2, 2) << -5.0, 3.0, 00.0, -5.0); + Matrix B = (Matrix(2, 3) << -0.5, 2.1, 1.1, 3.4, 2.6, 7.1); vector matrices; matrices.push_back(&A); matrices.push_back(&B); @@ -127,7 +160,7 @@ TEST( matrix, collect3 ) matrices.push_back(&A); matrices.push_back(&B); Matrix AB = collect(matrices, 2, 3); - Matrix exp = Matrix_(2, 6, + Matrix exp = (Matrix(2, 6) << 1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 1.0, 0.0); @@ -137,8 +170,8 @@ TEST( matrix, collect3 ) /* ************************************************************************* */ TEST( matrix, stack ) { - Matrix A = Matrix_(2, 2, -5.0, 3.0, 00.0, -5.0); - Matrix B = Matrix_(3, 2, -0.5, 2.1, 1.1, 3.4, 2.6, 7.1); + Matrix A = (Matrix(2, 2) << -5.0, 3.0, 00.0, -5.0); + Matrix B = (Matrix(3, 2) << -0.5, 2.1, 1.1, 3.4, 2.6, 7.1); Matrix AB = stack(2, &A, &B); Matrix C(5, 2); for (int i = 0; i < 2; i++) @@ -160,19 +193,19 @@ TEST( matrix, stack ) /* ************************************************************************* */ TEST( matrix, column ) { - Matrix A = Matrix_(4, 7, -1., 0., 1., 0., 0., 0., -0.2, 0., -1., 0., 1., + Matrix A = (Matrix(4, 7) << -1., 0., 1., 0., 0., 0., -0.2, 0., -1., 0., 1., 0., 0., 0.3, 1., 0., 0., 0., -1., 0., 0.2, 0., 1., 0., 0., 0., -1., -0.1); Vector a1 = column(A, 0); - Vector exp1 = Vector_(4, -1., 0., 1., 0.); + Vector exp1 = (Vector(4) << -1., 0., 1., 0.); EXPECT(assert_equal(a1, exp1)); Vector a2 = column(A, 3); - Vector exp2 = Vector_(4, 0., 1., 0., 0.); + Vector exp2 = (Vector(4) << 0., 1., 0., 0.); EXPECT(assert_equal(a2, exp2)); Vector a3 = column(A, 6); - Vector exp3 = Vector_(4, -0.2, 0.3, 0.2, -0.1); + Vector exp3 = (Vector(4) << -0.2, 0.3, 0.2, -0.1); EXPECT(assert_equal(a3, exp3)); } @@ -185,7 +218,7 @@ TEST( matrix, insert_column ) insertColumn(big, col, j); - Matrix expected = Matrix_(5, 6, + Matrix expected = (Matrix(5, 6) << 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, @@ -208,7 +241,7 @@ TEST( matrix, insert_subcolumn ) Vector col2 = ones(1); insertColumn(big, col2, 4, 5); // check 2 - Matrix expected = Matrix_(5, 6, + Matrix expected = (Matrix(5, 6) << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, @@ -221,19 +254,19 @@ TEST( matrix, insert_subcolumn ) /* ************************************************************************* */ TEST( matrix, row ) { - Matrix A = Matrix_(4, 7, -1., 0., 1., 0., 0., 0., -0.2, 0., -1., 0., 1., + Matrix A = (Matrix(4, 7) << -1., 0., 1., 0., 0., 0., -0.2, 0., -1., 0., 1., 0., 0., 0.3, 1., 0., 0., 0., -1., 0., 0.2, 0., 1., 0., 0., 0., -1., -0.1); Vector a1 = row(A, 0); - Vector exp1 = Vector_(7, -1., 0., 1., 0., 0., 0., -0.2); + Vector exp1 = (Vector(7) << -1., 0., 1., 0., 0., 0., -0.2); EXPECT(assert_equal(a1, exp1)); Vector a2 = row(A, 2); - Vector exp2 = Vector_(7, 1., 0., 0., 0., -1., 0., 0.2); + Vector exp2 = (Vector(7) << 1., 0., 0., 0., -1., 0., 0.2); EXPECT(assert_equal(a2, exp2)); Vector a3 = row(A, 3); - Vector exp3 = Vector_(7, 0., 1., 0., 0., 0., -1., -0.1); + Vector exp3 = (Vector(7) << 0., 1., 0., 0., 0., -1., -0.1); EXPECT(assert_equal(a3, exp3)); } @@ -256,12 +289,12 @@ TEST( matrix, zeros ) /* ************************************************************************* */ TEST( matrix, insert_sub ) { - Matrix big = zeros(5, 6), small = Matrix_(2, 3, 1.0, 1.0, 1.0, 1.0, 1.0, + Matrix big = zeros(5, 6), small = (Matrix(2, 3) << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0); insertSub(big, small, 1, 2); - Matrix expected = Matrix_(5, 6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + Matrix expected = (Matrix(5, 6) << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); @@ -278,7 +311,7 @@ TEST( matrix, diagMatrices ) Matrix actual = diag(Hs); - Matrix expected = Matrix_(9, 9, + Matrix expected = (Matrix(9, 9) << 1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, @@ -294,7 +327,7 @@ TEST( matrix, diagMatrices ) /* ************************************************************************* */ TEST( matrix, stream_read ) { - Matrix expected = Matrix_(3,4, + Matrix expected = (Matrix(3,4) << 1.1, 2.3, 4.2, 7.6, -0.3, -8e-2, 5.1, 9.0, 1.2, 3.4, 4.5, 6.7); @@ -329,7 +362,7 @@ TEST( matrix, scale_columns ) A(2, 2) = 1.; A(2, 3) = 1.; - Vector v = Vector_(4, 2., 3., 4., 5.); + Vector v = (Vector(4) << 2., 3., 4., 5.); Matrix actual = vector_scale(A, v); @@ -367,7 +400,7 @@ TEST( matrix, scale_rows ) A(2, 2) = 1.; A(2, 3) = 1.; - Vector v = Vector_(3, 2., 3., 4.); + Vector v = (Vector(3) << 2., 3., 4.); Matrix actual = vector_scale(v, A); @@ -405,7 +438,7 @@ TEST( matrix, scale_rows_mask ) A(2, 2) = 1.; A(2, 3) = 1.; - Vector v = Vector_(3, 2., std::numeric_limits::infinity(), 4.); + Vector v = (Vector(3) << 2., std::numeric_limits::infinity(), 4.); Matrix actual = vector_scale(v, A, true); @@ -426,6 +459,22 @@ TEST( matrix, scale_rows_mask ) EXPECT(assert_equal(actual, expected)); } +/* ************************************************************************* */ +TEST( matrix, skewSymmetric ) +{ + double wx = 1, wy = 2, wz = 3; + Matrix3 actual = skewSymmetric(wx,wy,wz); + + Matrix expected(3,3); + expected << 0, -3, 2, + 3, 0, -1, + -2, 1, 0; + + EXPECT(assert_equal(actual, expected)); + +} + + /* ************************************************************************* */ TEST( matrix, equal ) { @@ -488,18 +537,18 @@ TEST( matrix, equal_nan ) /* ************************************************************************* */ TEST( matrix, addition ) { - Matrix A = Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0); - Matrix B = Matrix_(2, 2, 4.0, 3.0, 2.0, 1.0); - Matrix C = Matrix_(2, 2, 5.0, 5.0, 5.0, 5.0); + Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0); + Matrix B = (Matrix(2, 2) << 4.0, 3.0, 2.0, 1.0); + Matrix C = (Matrix(2, 2) << 5.0, 5.0, 5.0, 5.0); EQUALITY(A+B,C); } /* ************************************************************************* */ TEST( matrix, addition_in_place ) { - Matrix A = Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0); - Matrix B = Matrix_(2, 2, 4.0, 3.0, 2.0, 1.0); - Matrix C = Matrix_(2, 2, 5.0, 5.0, 5.0, 5.0); + Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0); + Matrix B = (Matrix(2, 2) << 4.0, 3.0, 2.0, 1.0); + Matrix C = (Matrix(2, 2) << 5.0, 5.0, 5.0, 5.0); A += B; EQUALITY(A,C); } @@ -507,18 +556,18 @@ TEST( matrix, addition_in_place ) /* ************************************************************************* */ TEST( matrix, subtraction ) { - Matrix A = Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0); - Matrix B = Matrix_(2, 2, 4.0, 3.0, 2.0, 1.0); - Matrix C = Matrix_(2, 2, -3.0, -1.0, 1.0, 3.0); + Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0); + Matrix B = (Matrix(2, 2) << 4.0, 3.0, 2.0, 1.0); + Matrix C = (Matrix(2, 2) << -3.0, -1.0, 1.0, 3.0); EQUALITY(A-B,C); } /* ************************************************************************* */ TEST( matrix, subtraction_in_place ) { - Matrix A = Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0); - Matrix B = Matrix_(2, 2, 4.0, 3.0, 2.0, 1.0); - Matrix C = Matrix_(2, 2, -3.0, -1.0, 1.0, 3.0); + Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0); + Matrix B = (Matrix(2, 2) << 4.0, 3.0, 2.0, 1.0); + Matrix C = (Matrix(2, 2) << -3.0, -1.0, 1.0, 3.0); A -= B; EQUALITY(A,C); } @@ -568,10 +617,10 @@ TEST( matrix, matrix_vector_multiplication ) { Vector result(2); - Matrix A = Matrix_(2, 3, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0); - Vector v = Vector_(3, 1., 2., 3.); - Vector Av = Vector_(2, 14., 32.); - Vector AtAv = Vector_(3, 142., 188., 234.); + Matrix A = (Matrix(2, 3) << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0); + Vector v = (Vector(3) << 1., 2., 3.); + Vector Av = (Vector(2) << 14., 32.); + Vector AtAv = (Vector(3) << 142., 188., 234.); EQUALITY(A*v,Av); EQUALITY(A^Av,AtAv); @@ -605,12 +654,12 @@ TEST( matrix, scalar_divide ) /* ************************************************************************* */ TEST( matrix, zero_below_diagonal ) { - Matrix A1 = Matrix_(3, 4, + Matrix A1 = (Matrix(3, 4) << 1.0, 2.0, 3.0, 4.0, 1.0, 2.0, 3.0, 4.0, 1.0, 2.0, 3.0, 4.0); - Matrix expected1 = Matrix_(3, 4, + Matrix expected1 = (Matrix(3, 4) << 1.0, 2.0, 3.0, 4.0, 0.0, 2.0, 3.0, 4.0, 0.0, 0.0, 3.0, 4.0); @@ -626,13 +675,13 @@ TEST( matrix, zero_below_diagonal ) { zeroBelowDiagonal(actual1c, 4); EXPECT(assert_equal(Matrix(expected1), actual1c, 1e-10)); - Matrix A2 = Matrix_(5, 3, + Matrix A2 = (Matrix(5, 3) << 1.0, 2.0, 3.0, 1.0, 2.0, 3.0, 1.0, 2.0, 3.0, 1.0, 2.0, 3.0, 1.0, 2.0, 3.0); - Matrix expected2 = Matrix_(5, 3, + Matrix expected2 = (Matrix(5, 3) << 1.0, 2.0, 3.0, 0.0, 2.0, 3.0, 0.0, 0.0, 3.0, @@ -647,7 +696,7 @@ TEST( matrix, zero_below_diagonal ) { zeroBelowDiagonal(actual2c); EXPECT(assert_equal(Matrix(expected2), actual2c, 1e-10)); - Matrix expected2_partial = Matrix_(5, 3, + Matrix expected2_partial = (Matrix(5, 3) << 1.0, 2.0, 3.0, 0.0, 2.0, 3.0, 0.0, 2.0, 3.0, @@ -690,14 +739,14 @@ TEST( matrix, inverse ) EXPECT(assert_equal(expected, Ainv, 1e-4)); // These two matrices failed before version 2003 because we called LU incorrectly - Matrix lMg(Matrix_(3, 3, 0.0, 1.0, -2.0, -1.0, 0.0, 1.0, 0.0, 0.0, 1.0)); - EXPECT(assert_equal(Matrix_(3,3, + Matrix lMg((Matrix(3, 3) << 0.0, 1.0, -2.0, -1.0, 0.0, 1.0, 0.0, 0.0, 1.0)); + EXPECT(assert_equal((Matrix(3, 3) << 0.0, -1.0, 1.0, 1.0, 0.0, 2.0, 0.0, 0.0, 1.0), inverse(lMg))); - Matrix gMl(Matrix_(3, 3, 0.0, -1.0, 1.0, 1.0, 0.0, 2.0, 0.0, 0.0, 1.0)); - EXPECT(assert_equal(Matrix_(3,3, + Matrix gMl((Matrix(3, 3) << 0.0, -1.0, 1.0, 1.0, 0.0, 2.0, 0.0, 0.0, 1.0)); + EXPECT(assert_equal((Matrix(3, 3) << 0.0, 1.0,-2.0, -1.0, 0.0, 1.0, 0.0, 0.0, 1.0), @@ -738,19 +787,19 @@ TEST( matrix, inverse2 ) TEST( matrix, backsubtitution ) { // TEST ONE 2x2 matrix U1*x=b1 - Vector expected1 = Vector_(2, 3.6250, -0.75); - Matrix U22 = Matrix_(2, 2, 2., 3., 0., 4.); + Vector expected1 = (Vector(2) << 3.6250, -0.75); + Matrix U22 = (Matrix(2, 2) << 2., 3., 0., 4.); Vector b1 = U22 * expected1; EXPECT( assert_equal(expected1 , backSubstituteUpper(U22, b1), 0.000001)); // TEST TWO 3x3 matrix U2*x=b2 - Vector expected2 = Vector_(3, 5.5, -8.5, 5.); - Matrix U33 = Matrix_(3, 3, 3., 5., 6., 0., 2., 3., 0., 0., 1.); + Vector expected2 = (Vector(3) << 5.5, -8.5, 5.); + Matrix U33 = (Matrix(3, 3) << 3., 5., 6., 0., 2., 3., 0., 0., 1.); Vector b2 = U33 * expected2; EXPECT( assert_equal(expected2 , backSubstituteUpper(U33, b2), 0.000001)); // TEST THREE Lower triangular 3x3 matrix L3*x=b3 - Vector expected3 = Vector_(3, 1., 1., 1.); + Vector expected3 = (Vector(3) << 1., 1., 1.); Matrix L3 = trans(U33); Vector b3 = L3 * expected3; EXPECT( assert_equal(expected3 , backSubstituteLower(L3, b3), 0.000001)); @@ -762,29 +811,28 @@ TEST( matrix, backsubtitution ) /* ************************************************************************* */ TEST( matrix, householder ) { - double data[] = { -5, 0, 5, 0, 0, 0, -1, - 00,-5, 0, 5, 0, 0, 1.5, - 10, 0, 0, 0,-10,0, 2, - 00, 10,0, 0, 0, -10, -1 }; - // check in-place householder, with v vectors below diagonal - double data1[] = { - 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, + + Matrix expected1 = (Matrix(4, 7) << 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, 0, 11.1803, 0, -2.2361, 0, -8.9443, -1.565, -0.618034, 0, 4.4721, 0, -4.4721, 0, 0, - 0, -0.618034, 0, 4.4721, 0, -4.4721, 0.894 }; - Matrix expected1 = Matrix_(4, 7, data1); - Matrix A1 = Matrix_(4, 7, data); + 0, -0.618034, 0, 4.4721, 0, -4.4721, 0.894); + Matrix A1 = (Matrix(4, 7) << -5, 0, 5, 0, 0, 0, -1, + 00,-5, 0, 5, 0, 0, 1.5, + 10, 0, 0, 0,-10,0, 2, + 00, 10,0, 0, 0, -10, -1 ); householder_(A1, 3); EXPECT(assert_equal(expected1, A1, 1e-3)); // in-place, with zeros below diagonal - double data2[] = { - 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, 0, 11.1803, + + Matrix expected = (Matrix(4, 7) << 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, 0, 11.1803, 0, -2.2361, 0, -8.9443, -1.565, 0, 0, 4.4721, 0, -4.4721, 0, 0, 0, - 0, 0, 4.4721, 0, -4.4721, 0.894 }; - Matrix expected = Matrix_(4, 7, data2); - Matrix A2 = Matrix_(4, 7, data); + 0, 0, 4.4721, 0, -4.4721, 0.894); + Matrix A2 = (Matrix(4, 7) << -5, 0, 5, 0, 0, 0, -1, + 00,-5, 0, 5, 0, 0, 1.5, + 10, 0, 0, 0,-10,0, 2, + 00, 10,0, 0, 0, -10, -1); householder(A2, 3); EXPECT(assert_equal(expected, A2, 1e-3)); } @@ -792,30 +840,28 @@ TEST( matrix, householder ) /* ************************************************************************* */ TEST( matrix, householder_colMajor ) { - double data[] = { - -5, 0, 5, 0, 0, 0, -1, - 00,-5, 0, 5, 0, 0, 1.5, - 10, 0, 0, 0,-10,0, 2, - 00, 10,0, 0, 0, -10, -1 }; - // check in-place householder, with v vectors below diagonal - double data1[] = { - 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, + + Matrix expected1((Matrix(4, 7) << 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, 0, 11.1803, 0, -2.2361, 0, -8.9443, -1.565, -0.618034, 0, 4.4721, 0, -4.4721, 0, 0, - 0, -0.618034, 0, 4.4721, 0, -4.4721, 0.894 }; - Matrix expected1(Matrix_(4, 7, data1)); - Matrix A1(Matrix_(4, 7, data)); + 0, -0.618034, 0, 4.4721, 0, -4.4721, 0.894)); + Matrix A1((Matrix(4, 7) << -5, 0, 5, 0, 0, 0, -1, + 00,-5, 0, 5, 0, 0, 1.5, + 10, 0, 0, 0,-10,0, 2, + 00, 10,0, 0, 0, -10, -1)); householder_(A1, 3); EXPECT(assert_equal(expected1, A1, 1e-3)); // in-place, with zeros below diagonal - double data2[] = { - 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, 0, 11.1803, + + Matrix expected((Matrix(4, 7) << 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, 0, 11.1803, 0, -2.2361, 0, -8.9443, -1.565, 0, 0, 4.4721, 0, -4.4721, 0, 0, 0, - 0, 0, 4.4721, 0, -4.4721, 0.894 }; - Matrix expected(Matrix_(4, 7, data2)); - Matrix A2(Matrix_(4, 7, data)); + 0, 0, 4.4721, 0, -4.4721, 0.894)); + Matrix A2((Matrix(4, 7) << -5, 0, 5, 0, 0, 0, -1, + 00,-5, 0, 5, 0, 0, 1.5, + 10, 0, 0, 0,-10,0, 2, + 00, 10,0, 0, 0, -10, -1)); householder(A2, 3); EXPECT(assert_equal(expected, A2, 1e-3)); } @@ -824,26 +870,26 @@ TEST( matrix, householder_colMajor ) TEST( matrix, eigen_QR ) { // use standard Eigen function to yield a non-in-place QR factorization - double data[] = { - -5, 0, 5, 0, 0, 0, -1, - 00,-5, 0, 5, 0, 0, 1.5, - 10, 0, 0, 0,-10,0, 2, - 00, 10,0, 0, 0, -10, -1 }; // in-place, with zeros below diagonal - double data2[] = { - 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, 0, 11.1803, + + Matrix expected((Matrix(4, 7) << 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, 0, 11.1803, 0, -2.2361, 0, -8.9443, -1.565, 0, 0, 4.4721, 0, -4.4721, 0, 0, 0, - 0, 0, 4.4721, 0, -4.4721, 0.894 }; - Matrix expected(Matrix_(4, 7, data2)); - Matrix A(Matrix_(4, 7, data)); + 0, 0, 4.4721, 0, -4.4721, 0.894)); + Matrix A((Matrix(4, 7) << -5, 0, 5, 0, 0, 0, -1, + 00,-5, 0, 5, 0, 0, 1.5, + 10, 0, 0, 0,-10,0, 2, + 00, 10,0, 0, 0, -10, -1)); Matrix actual = A.householderQr().matrixQR(); zeroBelowDiagonal(actual); EXPECT(assert_equal(expected, actual, 1e-3)); // use shiny new in place QR inside gtsam - A = Matrix(Matrix_(4, 7, data)); + A = Matrix((Matrix(4, 7) << -5, 0, 5, 0, 0, 0, -1, + 00,-5, 0, 5, 0, 0, 1.5, + 10, 0, 0, 0,-10,0, 2, + 00, 10,0, 0, 0, -10, -1)); inplace_QR(A); EXPECT(assert_equal(expected, A, 1e-3)); } @@ -854,19 +900,18 @@ TEST( matrix, eigen_QR ) /* ************************************************************************* */ TEST( matrix, qr ) { - double data[] = { -5, 0, 5, 0, 00, -5, 0, 5, 10, 0, 0, 0, 00, 10, 0, 0, 00, - 0, 0, -10, 10, 0, -10, 0 }; - Matrix A = Matrix_(6, 4, data); - double dataQ[] = { -0.3333, 0, 0.2981, 0, 0, -0.8944, 0000000, -0.4472, 0, + Matrix A = (Matrix(6, 4) << -5, 0, 5, 0, 00, -5, 0, 5, 10, 0, 0, 0, 00, 10, 0, 0, 00, + 0, 0, -10, 10, 0, -10, 0); + + + Matrix expectedQ = (Matrix(6, 6) << -0.3333, 0, 0.2981, 0, 0, -0.8944, 0000000, -0.4472, 0, 0.3651, -0.8165, 0, 00.6667, 0, 0.7454, 0, 0, 0, 0000000, 0.8944, 0, 0.1826, -0.4082, 0, 0000000, 0, 0, -0.9129, -0.4082, 0, 00.6667, - 0, -0.5963, 0, 0, -0.4472, }; - Matrix expectedQ = Matrix_(6, 6, dataQ); + 0, -0.5963, 0, 0, -0.4472); - double dataR[] = { 15, 0, -8.3333, 0, 00, 11.1803, 0, -2.2361, 00, 0, - 7.4536, 0, 00, 0, 0, 10.9545, 00, 0, 0, 0, 00, 0, 0, 0, }; - Matrix expectedR = Matrix_(6, 4, dataR); + Matrix expectedR = (Matrix(6, 4) << 15, 0, -8.3333, 0, 00, 11.1803, 0, -2.2361, 00, 0, + 7.4536, 0, 00, 0, 0, 10.9545, 00, 0, 0, 0, 00, 0, 0, 0); Matrix Q, R; boost::tie(Q, R) = qr(A); @@ -878,13 +923,11 @@ TEST( matrix, qr ) /* ************************************************************************* */ TEST( matrix, sub ) { - double data1[] = { -5, 0, 5, 0, 0, 0, 00, -5, 0, 5, 0, 0, 10, 0, 0, 0, -10, - 0, 00, 10, 0, 0, 0, -10 }; - Matrix A = Matrix_(4, 6, data1); + Matrix A = (Matrix(4, 6) << -5, 0, 5, 0, 0, 0, 00, -5, 0, 5, 0, 0, 10, 0, 0, 0, -10, + 0, 00, 10, 0, 0, 0, -10); Matrix actual = sub(A, 1, 3, 1, 5); - double data2[] = { -5, 0, 5, 0, 00, 0, 0, -10, }; - Matrix expected = Matrix_(2, 4, data2); + Matrix expected = (Matrix(2, 4) << -5, 0, 5, 0, 00, 0, 0, -10); EQUALITY(actual,expected); } @@ -892,15 +935,15 @@ TEST( matrix, sub ) /* ************************************************************************* */ TEST( matrix, trans ) { - Matrix A = Matrix_(2, 2, 1.0, 3.0, 2.0, 4.0); - Matrix B = Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0); + Matrix A = (Matrix(2, 2) << 1.0, 3.0, 2.0, 4.0); + Matrix B = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0); EQUALITY(trans(A),B); } /* ************************************************************************* */ TEST( matrix, col_major_access ) { - Matrix A = Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0); + Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0); const double* a = &A(0, 0); DOUBLES_EQUAL(2.0,a[2],1e-9); } @@ -909,16 +952,16 @@ TEST( matrix, col_major_access ) TEST( matrix, weighted_elimination ) { // create a matrix to eliminate - Matrix A = Matrix_(4, 6, -1., 0., 1., 0., 0., 0., 0., -1., 0., 1., 0., 0., + Matrix A = (Matrix(4, 6) << -1., 0., 1., 0., 0., 0., 0., -1., 0., 1., 0., 0., 1., 0., 0., 0., -1., 0., 0., 1., 0., 0., 0., -1.); - Vector b = Vector_(4, -0.2, 0.3, 0.2, -0.1); - Vector sigmas = Vector_(4, 0.2, 0.2, 0.1, 0.1); + Vector b = (Vector(4) << -0.2, 0.3, 0.2, -0.1); + Vector sigmas = (Vector(4) << 0.2, 0.2, 0.1, 0.1); // expected values - Matrix expectedR = Matrix_(4, 6, 1., 0., -0.2, 0., -0.8, 0., 0., 1., 0., + Matrix expectedR = (Matrix(4, 6) << 1., 0., -0.2, 0., -0.8, 0., 0., 1., 0., -0.2, 0., -0.8, 0., 0., 1., 0., -1., 0., 0., 0., 0., 1., 0., -1.); - Vector d = Vector_(4, 0.2, -0.14, 0.0, 0.2); - Vector newSigmas = Vector_(4, 0.0894427, 0.0894427, 0.223607, 0.223607); + Vector d = (Vector(4) << 0.2, -0.14, 0.0, 0.2); + Vector newSigmas = (Vector(4) << 0.0894427, 0.0894427, 0.223607, 0.223607); Vector r; double di, sigma; @@ -943,11 +986,11 @@ TEST( matrix, weighted_elimination ) /* ************************************************************************* */ TEST( matrix, inverse_square_root ) { - Matrix measurement_covariance = Matrix_(3, 3, 0.25, 0.0, 0.0, 0.0, 0.25, + Matrix measurement_covariance = (Matrix(3, 3) << 0.25, 0.0, 0.0, 0.0, 0.25, 0.0, 0.0, 0.0, 0.01); Matrix actual = inverse_square_root(measurement_covariance); - Matrix expected = Matrix_(3, 3, 2.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, + Matrix expected = (Matrix(3, 3) << 2.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 10.0); EQUALITY(expected,actual); @@ -959,14 +1002,14 @@ TEST( matrix, inverse_square_root ) // use the same inverse routing inside inverse_square_root() // as we use here to check it. - Matrix M = Matrix_(5, 5, + Matrix M = (Matrix(5, 5) << 0.0785892, 0.0137923, -0.0142219, -0.0171880, 0.0028726, 0.0137923, 0.0908911, 0.0020775, -0.0101952, 0.0175868, -0.0142219, 0.0020775, 0.0973051, 0.0054906, 0.0047064, -0.0171880,-0.0101952, 0.0054906, 0.0892453, -0.0059468, 0.0028726, 0.0175868, 0.0047064, -0.0059468, 0.0816517); - expected = Matrix_(5, 5, + expected = (Matrix(5, 5) << 3.567126953241796, 0.000000000000000, 0.000000000000000, 0.000000000000000, 0.000000000000000, -0.590030436566913, 3.362022286742925, 0.000000000000000, 0.000000000000000, 0.000000000000000, 0.618207860252376, -0.168166020746503, 3.253086082942785, 0.000000000000000, 0.000000000000000, @@ -981,13 +1024,13 @@ TEST( matrix, inverse_square_root ) // we are checking against was generated via chol(M)' on octave TEST( matrix, LLt ) { - Matrix M = Matrix_(5, 5, 0.0874197, -0.0030860, 0.0116969, 0.0081463, + Matrix M = (Matrix(5, 5) << 0.0874197, -0.0030860, 0.0116969, 0.0081463, 0.0048741, -0.0030860, 0.0872727, 0.0183073, 0.0125325, -0.0037363, 0.0116969, 0.0183073, 0.0966217, 0.0103894, -0.0021113, 0.0081463, 0.0125325, 0.0103894, 0.0747324, 0.0036415, 0.0048741, -0.0037363, -0.0021113, 0.0036415, 0.0909464); - Matrix expected = Matrix_(5, 5, + Matrix expected = (Matrix(5, 5) << 0.295668226226627, 0.000000000000000, 0.000000000000000, 0.000000000000000, 0.000000000000000, -0.010437374483502, 0.295235094820875, 0.000000000000000, 0.000000000000000, 0.000000000000000, 0.039560896175007, 0.063407813693827, 0.301721866387571, 0.000000000000000, 0.000000000000000, @@ -1000,8 +1043,8 @@ TEST( matrix, LLt ) /* ************************************************************************* */ TEST( matrix, multiplyAdd ) { - Matrix A = Matrix_(3, 4, 4., 0., 0., 1., 0., 4., 0., 2., 0., 0., 1., 3.); - Vector x = Vector_(4, 1., 2., 3., 4.), e = Vector_(3, 5., 6., 7.), + Matrix A = (Matrix(3, 4) << 4., 0., 0., 1., 0., 4., 0., 2., 0., 0., 1., 3.); + Vector x = (Vector(4) << 1., 2., 3., 4.), e = (Vector(3) << 5., 6., 7.), expected = e + A * x; multiplyAdd(1, A, x, e); @@ -1011,8 +1054,8 @@ TEST( matrix, multiplyAdd ) /* ************************************************************************* */ TEST( matrix, transposeMultiplyAdd ) { - Matrix A = Matrix_(3, 4, 4., 0., 0., 1., 0., 4., 0., 2., 0., 0., 1., 3.); - Vector x = Vector_(4, 1., 2., 3., 4.), e = Vector_(3, 5., 6., 7.), + Matrix A = (Matrix(3, 4) << 4., 0., 0., 1., 0., 4., 0., 2., 0., 0., 1., 3.); + Vector x = (Vector(4) << 1., 2., 3., 4.), e = (Vector(3) << 5., 6., 7.), expected = x + trans(A) * e; transposeMultiplyAdd(1, A, e, x); @@ -1022,31 +1065,31 @@ TEST( matrix, transposeMultiplyAdd ) /* ************************************************************************* */ TEST( matrix, linear_dependent ) { - Matrix A = Matrix_(2, 3, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0); - Matrix B = Matrix_(2, 3, -1.0, -2.0, -3.0, 8.0, 10.0, 12.0); + Matrix A = (Matrix(2, 3) << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0); + Matrix B = (Matrix(2, 3) << -1.0, -2.0, -3.0, 8.0, 10.0, 12.0); EXPECT(linear_dependent(A, B)); } /* ************************************************************************* */ TEST( matrix, linear_dependent2 ) { - Matrix A = Matrix_(2, 3, 0.0, 2.0, 3.0, 4.0, 5.0, 6.0); - Matrix B = Matrix_(2, 3, 0.0, -2.0, -3.0, 8.0, 10.0, 12.0); + Matrix A = (Matrix(2, 3) << 0.0, 2.0, 3.0, 4.0, 5.0, 6.0); + Matrix B = (Matrix(2, 3) << 0.0, -2.0, -3.0, 8.0, 10.0, 12.0); EXPECT(linear_dependent(A, B)); } /* ************************************************************************* */ TEST( matrix, linear_dependent3 ) { - Matrix A = Matrix_(2, 3, 0.0, 2.0, 3.0, 4.0, 5.0, 6.0); - Matrix B = Matrix_(2, 3, 0.0, -2.0, -3.0, 8.1, 10.0, 12.0); + Matrix A = (Matrix(2, 3) << 0.0, 2.0, 3.0, 4.0, 5.0, 6.0); + Matrix B = (Matrix(2, 3) << 0.0, -2.0, -3.0, 8.1, 10.0, 12.0); EXPECT(linear_independent(A, B)); } /* ************************************************************************* */ TEST( matrix, svd1 ) { - Vector v = Vector_(3, 2., 1., 0.); + Vector v = (Vector(3) << 2., 1., 0.); Matrix U1 = eye(4, 3), S1 = diag(v), V1 = eye(3, 3), A = (U1 * S1) * Matrix(trans(V1)); Matrix U, V; @@ -1059,7 +1102,7 @@ TEST( matrix, svd1 ) /* ************************************************************************* */ /// Sample A matrix for SVD -static Matrix sampleA = Matrix_(3, 2, 0.,-2., 0., 0., 3., 0.); +static Matrix sampleA = (Matrix(3, 2) << 0.,-2., 0., 0., 3., 0.); static Matrix sampleAt = trans(sampleA); /* ************************************************************************* */ @@ -1068,9 +1111,9 @@ TEST( matrix, svd2 ) Matrix U, V; Vector s; - Matrix expectedU = Matrix_(3, 2, 0.,-1.,0.,0.,1.,0.); - Vector expected_s = Vector_(2, 3.,2.); - Matrix expectedV = Matrix_(2, 2, 1.,0.,0.,1.); + Matrix expectedU = (Matrix(3, 2) << 0.,-1.,0.,0.,1.,0.); + Vector expected_s = (Vector(2) << 3.,2.); + Matrix expectedV = (Matrix(2, 2) << 1.,0.,0.,1.); svd(sampleA, U, s, V); @@ -1085,9 +1128,9 @@ TEST( matrix, svd3 ) Matrix U, V; Vector s; - Matrix expectedU = Matrix_(2, 2, -1.,0.,0.,-1.); - Vector expected_s = Vector_(2, 3.0,2.0); - Matrix expectedV = Matrix_(3, 2, 0.,1.,0.,0.,-1.,0.); + Matrix expectedU = (Matrix(2, 2) << -1.,0.,0.,-1.); + Vector expected_s = (Vector(2) << 3.0, 2.0); + Matrix expectedV = (Matrix(3, 2) << 0.,1.,0.,0.,-1.,0.); svd(sampleAt, U, s, V); Matrix S = diag(s); @@ -1106,19 +1149,19 @@ TEST( matrix, svd4 ) Matrix U, V; Vector s; - Matrix A = Matrix_(3,2, + Matrix A = (Matrix(3, 2) << 0.8147, 0.9134, 0.9058, 0.6324, 0.1270, 0.0975); - Matrix expectedU = Matrix_(3,2, + Matrix expectedU = (Matrix(3, 2) << 0.7397, 0.6724, 0.6659, -0.7370, 0.0970, -0.0689); - Vector expected_s = Vector_(2, 1.6455, 0.1910); + Vector expected_s = (Vector(2) << 1.6455, 0.1910); - Matrix expectedV = Matrix_(2,2, + Matrix expectedV = (Matrix(2, 2) << 0.7403, -0.6723, 0.6723, 0.7403); @@ -1134,7 +1177,7 @@ TEST( matrix, svd4 ) /* ************************************************************************* */ TEST( matrix, DLT ) { - Matrix A = Matrix_(8,9, + Matrix A = (Matrix(8, 9) << 0.21, -0.42, -10.71, 0.18, -0.36, -9.18, -0.61, 1.22, 31.11, 0.44, -0.66, -15.84, 0.34, -0.51, -12.24, -1.64, 2.46, 59.04, 0.69, -8.28, -12.19, -0.48, 5.76, 8.48, -1.89, 22.68, 33.39, @@ -1148,7 +1191,7 @@ TEST( matrix, DLT ) double error; Vector actual; boost::tie(rank,error,actual) = DLT(A); - Vector expected = Vector_(9, -0.0, 0.2357, 0.4714, -0.2357, 0.0, - 0.4714,-0.4714, 0.4714, 0.0); + Vector expected = (Vector(9) << -0.0, 0.2357, 0.4714, -0.2357, 0.0, - 0.4714,-0.4714, 0.4714, 0.0); EXPECT_LONGS_EQUAL(8,rank); EXPECT_DOUBLES_EQUAL(0,error,1e-8); EXPECT(assert_equal(expected, actual, 1e-4)); diff --git a/gtsam/base/tests/testNumericalDerivative.cpp b/gtsam/base/tests/testNumericalDerivative.cpp index 9d272d682..f7e4d3baa 100644 --- a/gtsam/base/tests/testNumericalDerivative.cpp +++ b/gtsam/base/tests/testNumericalDerivative.cpp @@ -28,10 +28,10 @@ double f(const LieVector& x) { } /* ************************************************************************* */ -TEST_UNSAFE(testNumericalDerivative, numericalHessian) { +TEST(testNumericalDerivative, numericalHessian) { LieVector center = ones(2); - Matrix expected = Matrix_(2,2, + Matrix expected = (Matrix(2,2) << -sin(center(0)), 0.0, 0.0, -cos(center(1))); @@ -47,10 +47,11 @@ double f2(const LieVector& x) { } /* ************************************************************************* */ -TEST_UNSAFE(testNumericalDerivative, numericalHessian2) { - LieVector center(2, 0.5, 1.0); +TEST(testNumericalDerivative, numericalHessian2) { + Vector v_center = (Vector(2) << 0.5, 1.0); + LieVector center(v_center); - Matrix expected = Matrix_(2,2, + Matrix expected = (Matrix(2,2) << -cos(center(1))*sin(center(0)), -sin(center(1))*cos(center(0)), -cos(center(0))*sin(center(1)), -sin(center(0))*cos(center(1))); @@ -66,18 +67,20 @@ double f3(const LieVector& x1, const LieVector& x2) { } /* ************************************************************************* */ -TEST_UNSAFE(testNumericalDerivative, numericalHessian211) { - LieVector center1(1, 1.0), center2(1, 5.0); +TEST(testNumericalDerivative, numericalHessian211) { + Vector v_center1 = (Vector(1) << 1.0); + Vector v_center2 = (Vector(1) << 5.0); + LieVector center1(v_center1), center2(v_center2); - Matrix expected11 = Matrix_(1,1,-sin(center1(0))*cos(center2(0))); + Matrix expected11 = (Matrix(1, 1) << -sin(center1(0))*cos(center2(0))); Matrix actual11 = numericalHessian211(f3, center1, center2); EXPECT(assert_equal(expected11, actual11, 1e-5)); - Matrix expected12 = Matrix_(1,1,-cos(center1(0))*sin(center2(0))); + Matrix expected12 = (Matrix(1, 1) <<-cos(center1(0))*sin(center2(0))); Matrix actual12 = numericalHessian212(f3, center1, center2); EXPECT(assert_equal(expected12, actual12, 1e-5)); - Matrix expected22 = Matrix_(1,1,-sin(center1(0))*cos(center2(0))); + Matrix expected22 = (Matrix(1, 1) <<-sin(center1(0))*cos(center2(0))); Matrix actual22 = numericalHessian222(f3, center1, center2); EXPECT(assert_equal(expected22, actual22, 1e-5)); } @@ -89,30 +92,34 @@ double f4(const LieVector& x, const LieVector& y, const LieVector& z) { } /* ************************************************************************* */ -TEST_UNSAFE(testNumericalDerivative, numericalHessian311) { - LieVector center1(1, 1.0), center2(1, 2.0), center3(1, 3.0); +TEST(testNumericalDerivative, numericalHessian311) { + Vector v_center1 = (Vector(1) << 1.0); + Vector v_center2 = (Vector(1) << 2.0); + Vector v_center3 = (Vector(1) << 3.0); + LieVector center1(v_center1), center2(v_center2), center3(v_center3); + double x = center1(0), y = center2(0), z = center3(0); - Matrix expected11 = Matrix_(1,1,-sin(x)*cos(y)*z*z); + Matrix expected11 = (Matrix(1, 1) << -sin(x)*cos(y)*z*z); Matrix actual11 = numericalHessian311(f4, center1, center2, center3); EXPECT(assert_equal(expected11, actual11, 1e-5)); - Matrix expected12 = Matrix_(1,1,-cos(x)*sin(y)*z*z); + Matrix expected12 = (Matrix(1, 1) << -cos(x)*sin(y)*z*z); Matrix actual12 = numericalHessian312(f4, center1, center2, center3); EXPECT(assert_equal(expected12, actual12, 1e-5)); - Matrix expected13 = Matrix_(1,1,cos(x)*cos(y)*2*z); + Matrix expected13 = (Matrix(1, 1) << cos(x)*cos(y)*2*z); Matrix actual13 = numericalHessian313(f4, center1, center2, center3); EXPECT(assert_equal(expected13, actual13, 1e-5)); - Matrix expected22 = Matrix_(1,1,-sin(x)*cos(y)*z*z); + Matrix expected22 = (Matrix(1, 1) << -sin(x)*cos(y)*z*z); Matrix actual22 = numericalHessian322(f4, center1, center2, center3); EXPECT(assert_equal(expected22, actual22, 1e-5)); - Matrix expected23 = Matrix_(1,1,-sin(x)*sin(y)*2*z); + Matrix expected23 = (Matrix(1, 1) << -sin(x)*sin(y)*2*z); Matrix actual23 = numericalHessian323(f4, center1, center2, center3); EXPECT(assert_equal(expected23, actual23, 1e-5)); - Matrix expected33 = Matrix_(1,1,sin(x)*cos(y)*2); + Matrix expected33 = (Matrix(1, 1) << sin(x)*cos(y)*2); Matrix actual33 = numericalHessian333(f4, center1, center2, center3); EXPECT(assert_equal(expected33, actual33, 1e-5)); } diff --git a/gtsam/base/tests/testSerializationBase.cpp b/gtsam/base/tests/testSerializationBase.cpp index 30b7cd2f3..9b28eacf6 100644 --- a/gtsam/base/tests/testSerializationBase.cpp +++ b/gtsam/base/tests/testSerializationBase.cpp @@ -30,9 +30,9 @@ using namespace std; using namespace gtsam; using namespace gtsam::serializationTestHelpers; -Vector v1 = Vector_(2, 1.0, 2.0); -Vector v2 = Vector_(2, 3.0, 4.0); -Vector v3 = Vector_(2, 5.0, 6.0); +Vector v1 = (Vector(2) << 1.0, 2.0); +Vector v2 = (Vector(2) << 3.0, 4.0); +Vector v3 = (Vector(2) << 5.0, 6.0); /* ************************************************************************* */ TEST (Serialization, FastList) { @@ -84,23 +84,23 @@ TEST (Serialization, FastVector) { /* ************************************************************************* */ TEST (Serialization, matrix_vector) { - EXPECT(equality(Vector_(4, 1.0, 2.0, 3.0, 4.0))); + EXPECT(equality((Vector(4) << 1.0, 2.0, 3.0, 4.0))); EXPECT(equality(Vector2(1.0, 2.0))); EXPECT(equality(Vector3(1.0, 2.0, 3.0))); EXPECT(equality((Vector6() << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0).finished())); - EXPECT(equality(Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0))); + EXPECT(equality((Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0))); - EXPECT(equalityXML(Vector_(4, 1.0, 2.0, 3.0, 4.0))); + EXPECT(equalityXML((Vector(4) << 1.0, 2.0, 3.0, 4.0))); EXPECT(equalityXML(Vector2(1.0, 2.0))); EXPECT(equalityXML(Vector3(1.0, 2.0, 3.0))); EXPECT(equalityXML((Vector6() << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0).finished())); - EXPECT(equalityXML(Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0))); + EXPECT(equalityXML((Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0))); - EXPECT(equalityBinary(Vector_(4, 1.0, 2.0, 3.0, 4.0))); + EXPECT(equalityBinary((Vector(4) << 1.0, 2.0, 3.0, 4.0))); EXPECT(equalityBinary(Vector2(1.0, 2.0))); EXPECT(equalityBinary(Vector3(1.0, 2.0, 3.0))); EXPECT(equalityBinary((Vector6() << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0).finished())); - EXPECT(equalityBinary(Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0))); + EXPECT(equalityBinary((Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0))); } /* ************************************************************************* */ diff --git a/gtsam/base/tests/testSymmetricBlockMatrix.cpp b/gtsam/base/tests/testSymmetricBlockMatrix.cpp new file mode 100644 index 000000000..ed9d43e5a --- /dev/null +++ b/gtsam/base/tests/testSymmetricBlockMatrix.cpp @@ -0,0 +1,188 @@ +/* ---------------------------------------------------------------------------- + +* 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 testSymmetricBlockMatrix.cpp +* @brief Unit tests for SymmetricBlockMatrix class +* @author Richard Roberts +**/ + +#include +#include +#include + +using namespace std; +using namespace gtsam; +using boost::assign::list_of; + +static SymmetricBlockMatrix testBlockMatrix( + list_of(3)(2)(1), + (Matrix(6, 6) << + 1, 2, 3, 4, 5, 6, + 2, 8, 9, 10, 11, 12, + 3, 9, 15, 16, 17, 18, + 4, 10, 16, 22, 23, 24, + 5, 11, 17, 23, 29, 30, + 6, 12, 18, 24, 30, 36)); + +/* ************************************************************************* */ +TEST(SymmetricBlockMatrix, ReadBlocks) +{ + // On the diagonal + Matrix expected1 = (Matrix(2, 2) << + 22, 23, + 23, 29); + Matrix actual1 = testBlockMatrix(1, 1); + // Test only writing the upper triangle for efficiency + Matrix actual1t = Matrix::Zero(2, 2); + actual1t.triangularView() = testBlockMatrix(1, 1).triangularView(); + EXPECT(assert_equal(expected1, actual1)); + EXPECT(assert_equal(Matrix(expected1.triangularView()), actual1t)); + + // Above the diagonal + Matrix expected2 = (Matrix(3, 2) << + 4, 5, + 10, 11, + 16, 17); + Matrix actual2 = testBlockMatrix(0, 1); + EXPECT(assert_equal(expected2, actual2)); + + // Below the diagonal + Matrix expected3 = (Matrix(2, 3) << + 4, 10, 16, + 5, 11, 17); + Matrix actual3 = testBlockMatrix(1, 0); + EXPECT(assert_equal(expected3, actual3)); +} + +/* ************************************************************************* */ +TEST(SymmetricBlockMatrix, WriteBlocks) +{ + // On the diagonal + Matrix expected1 = testBlockMatrix(1, 1); + SymmetricBlockMatrix bm1 = SymmetricBlockMatrix::LikeActiveViewOf(testBlockMatrix); + bm1(1, 1) = expected1.selfadjointView(); // Verified with debugger that this only writes the upper triangle + Matrix actual1 = bm1(1, 1); + EXPECT(assert_equal(expected1, actual1)); + + // On the diagonal + Matrix expected1p = testBlockMatrix(1, 1); + SymmetricBlockMatrix bm1p = SymmetricBlockMatrix::LikeActiveViewOf(testBlockMatrix); + bm1p(1, 1) = expected1p; // Verified with debugger that this only writes the upper triangle + Matrix actual1p = bm1p(1, 1); + EXPECT(assert_equal(expected1p, actual1p)); + + // Above the diagonal + Matrix expected2 = testBlockMatrix(0, 1); + SymmetricBlockMatrix bm2 = SymmetricBlockMatrix::LikeActiveViewOf(testBlockMatrix); + bm2(0, 1) = expected2; + Matrix actual2 = bm2(0, 1); + EXPECT(assert_equal(expected2, actual2)); + + // Below the diagonal + Matrix expected3 = testBlockMatrix(1, 0); + SymmetricBlockMatrix bm3 = SymmetricBlockMatrix::LikeActiveViewOf(testBlockMatrix); + bm3(1, 0) = expected3; + Matrix actual3 = bm3(1, 0); + EXPECT(assert_equal(expected3, actual3)); +} + +/* ************************************************************************* */ +TEST(SymmetricBlockMatrix, Ranges) +{ + // On the diagonal + Matrix expected1 = (Matrix(3, 3) << + 22, 23, 24, + 23, 29, 30, + 24, 30, 36); + Matrix actual1 = testBlockMatrix.range(1, 3, 1, 3).selfadjointView(); + Matrix actual1a = testBlockMatrix.range(1, 3, 1, 3); + EXPECT(assert_equal(expected1, actual1)); + EXPECT(assert_equal(expected1, actual1a)); + + // Above the diagonal + Matrix expected2 = (Matrix(3, 1) << + 24, + 30, + 36); + Matrix actual2 = testBlockMatrix.range(1, 3, 2, 3).knownOffDiagonal(); + Matrix actual2a = testBlockMatrix.range(1, 3, 2, 3); + EXPECT(assert_equal(expected2, actual2)); + EXPECT(assert_equal(expected2, actual2a)); + + // Below the diagonal + Matrix expected3 = (Matrix(3, 3) << + 4, 10, 16, + 5, 11, 17, + 6, 12, 18); + Matrix actual3 = testBlockMatrix.range(1, 3, 0, 1).knownOffDiagonal(); + Matrix actual3a = testBlockMatrix.range(1, 3, 0, 1); + EXPECT(assert_equal(expected3, actual3)); + EXPECT(assert_equal(expected3, actual3a)); +} + +/* ************************************************************************* */ +TEST(SymmetricBlockMatrix, expressions) +{ + SymmetricBlockMatrix expected1(list_of(2)(3)(1), (Matrix(6, 6) << + 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, + 0, 0, 4, 6, 8, 0, + 0, 0, 0, 9, 12, 0, + 0, 0, 0, 0, 16, 0, + 0, 0, 0, 0, 0, 0)); + + SymmetricBlockMatrix expected2(list_of(2)(3)(1), (Matrix(6, 6) << + 0, 0, 10, 15, 20, 0, + 0, 0, 12, 18, 24, 0, + 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0)); + + Matrix a = (Matrix(1, 3) << 2, 3, 4); + Matrix b = (Matrix(1, 2) << 5, 6); + + SymmetricBlockMatrix bm1(list_of(2)(3)(1)); + bm1.full().triangularView().setZero(); + bm1(1, 1).selfadjointView().rankUpdate(a.transpose()); + EXPECT(assert_equal(Matrix(expected1.full().selfadjointView()), bm1.full().selfadjointView())); + + SymmetricBlockMatrix bm2(list_of(2)(3)(1)); + bm2.full().triangularView().setZero(); + bm2(0, 1).knownOffDiagonal() += b.transpose() * a; + EXPECT(assert_equal(Matrix(expected2.full().selfadjointView()), bm2.full().selfadjointView())); + + SymmetricBlockMatrix bm3(list_of(2)(3)(1)); + bm3.full().triangularView().setZero(); + bm3(1, 0).knownOffDiagonal() += a.transpose() * b; + EXPECT(assert_equal(Matrix(expected2.full().selfadjointView()), bm3.full().selfadjointView())); + + SymmetricBlockMatrix bm4(list_of(2)(3)(1)); + bm4.full().triangularView().setZero(); + bm4(1, 1) += expected1(1, 1); + EXPECT(assert_equal(Matrix(expected1.full().selfadjointView()), bm4.full().selfadjointView())); + + SymmetricBlockMatrix bm5(list_of(2)(3)(1)); + bm5.full().triangularView().setZero(); + bm5(0, 1) += expected2(0, 1); + EXPECT(assert_equal(Matrix(expected2.full().selfadjointView()), bm5.full().selfadjointView())); + + SymmetricBlockMatrix bm6(list_of(2)(3)(1)); + bm6.full().triangularView().setZero(); + bm6(1, 0) += expected2(1, 0); + EXPECT(assert_equal(Matrix(expected2.full().selfadjointView()), bm6.full().selfadjointView())); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ + diff --git a/gtsam/base/tests/testTreeTraversal.cpp b/gtsam/base/tests/testTreeTraversal.cpp new file mode 100644 index 000000000..c9083f781 --- /dev/null +++ b/gtsam/base/tests/testTreeTraversal.cpp @@ -0,0 +1,163 @@ +/* ---------------------------------------------------------------------------- + + * 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 testTreeTraversal + * @brief Unit tests for tree traversal, cloning, and printing + * @author Richard Roberts + */ + +#include +#include +#include + +#include +#include +#include +#include +#include + +using boost::assign::operator+=; +using namespace std; +using namespace gtsam; + +struct TestNode { + typedef boost::shared_ptr shared_ptr; + int data; + vector children; + TestNode() : data(-1) {} + TestNode(int data) : data(data) {} +}; + +struct TestForest { + typedef TestNode Node; + typedef Node::shared_ptr sharedNode; + vector roots_; + const vector& roots() const { return roots_; } +}; + +TestForest makeTestForest() { + // 0 1 + // / \ + // 2 3 + // | + // 4 + TestForest forest; + forest.roots_.push_back(boost::make_shared(0)); + forest.roots_.push_back(boost::make_shared(1)); + forest.roots_[0]->children.push_back(boost::make_shared(2)); + forest.roots_[0]->children.push_back(boost::make_shared(3)); + forest.roots_[0]->children[1]->children.push_back(boost::make_shared(4)); + return forest; +} + +/* ************************************************************************* */ +struct PreOrderVisitor { + // This visitor stores the nodes visited so the visit order can be checked later. It also returns + // the current node index as the data and checks that the parent index properly matches the index + // referenced in the node. + std::list visited; + bool parentsMatched; + PreOrderVisitor() : parentsMatched(true) {} + int operator()(const TestNode::shared_ptr& node, int parentData) { + visited.push_back(node->data); + // Check parent index + const int expectedParentIndex = + node->data == 0 ? -1 : + node->data == 1 ? -1 : + node->data == 2 ? 0 : + node->data == 3 ? 0 : + node->data == 4 ? 3 : + node->data == 10 ? 0 : + (parentsMatched = false, -1); + if(expectedParentIndex != parentData) + parentsMatched = false; + return node->data; + } +}; + +/* ************************************************************************* */ +struct PostOrderVisitor { + // This visitor stores the nodes visited so the visit order can be checked later. + std::list visited; + void operator()(const TestNode::shared_ptr& node, int myData) { + visited.push_back(node->data); + } +}; + +/* ************************************************************************* */ +std::list getPreorder(const TestForest& forest) { + std::list result; + PreOrderVisitor preVisitor; + int rootData = -1; + treeTraversal::DepthFirstForest(forest, rootData, preVisitor); + result = preVisitor.visited; + return result; +} + +/* ************************************************************************* */ +TEST(treeTraversal, DepthFirst) +{ + // Get test forest + TestForest testForest = makeTestForest(); + + // Expected visit order + std::list preOrderExpected; + preOrderExpected += 0, 2, 3, 4, 1; + std::list postOrderExpected; + postOrderExpected += 2, 4, 3, 0, 1; + + // Actual visit order + PreOrderVisitor preVisitor; + PostOrderVisitor postVisitor; + int rootData = -1; + treeTraversal::DepthFirstForest(testForest, rootData, preVisitor, postVisitor); + + EXPECT(preVisitor.parentsMatched); + EXPECT(assert_container_equality(preOrderExpected, preVisitor.visited)); + EXPECT(assert_container_equality(postOrderExpected, postVisitor.visited)); +} + +/* ************************************************************************* */ +TEST(treeTraversal, CloneForest) +{ + // Get test forest + TestForest testForest1 = makeTestForest(); + TestForest testForest2; + testForest2.roots_ = treeTraversal::CloneForest(testForest1); + + // Check that the original and clone both are expected + std::list preOrder1Expected; + preOrder1Expected += 0, 2, 3, 4, 1; + std::list preOrder1Actual = getPreorder(testForest1); + std::list preOrder2Actual = getPreorder(testForest2); + EXPECT(assert_container_equality(preOrder1Expected, preOrder1Actual)); + EXPECT(assert_container_equality(preOrder1Expected, preOrder2Actual)); + + // Modify clone - should not modify original + testForest2.roots_[0]->children[1]->data = 10; + std::list preOrderModifiedExpected; + preOrderModifiedExpected += 0, 2, 10, 4, 1; + + // Check that original is the same and only the clone is modified + std::list preOrder1ModActual = getPreorder(testForest1); + std::list preOrder2ModActual = getPreorder(testForest2); + EXPECT(assert_container_equality(preOrder1Expected, preOrder1ModActual)); + EXPECT(assert_container_equality(preOrderModifiedExpected, preOrder2ModActual)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + diff --git a/gtsam/base/tests/testVector.cpp b/gtsam/base/tests/testVector.cpp index 8fd151dbf..ee0d94366 100644 --- a/gtsam/base/tests/testVector.cpp +++ b/gtsam/base/tests/testVector.cpp @@ -23,13 +23,48 @@ using namespace std; using namespace gtsam; +namespace { + /* ************************************************************************* */ + template + Vector testFcn1(const Eigen::DenseBase& in) + { + return in; + } + + /* ************************************************************************* */ + template + Vector testFcn2(const Eigen::MatrixBase& in) + { + return in; + } +} + /* ************************************************************************* */ -TEST( TestVector, Vector_variants ) +TEST( TestVector, special_comma_initializer) { - Vector a = Vector_(2,10.0,20.0); - double data[] = {10,20}; - Vector b = Vector_(2,data); - EXPECT(assert_equal(a, b)); + Vector expected(3); + expected(0) = 1; + expected(1) = 2; + expected(2) = 3; + + Vector actual1 = (Vector(3) << 1, 2, 3); + Vector actual2((Vector(3) << 1, 2, 3)); + + Vector subvec1 = (Vector(2) << 2, 3); + Vector actual4 = (Vector(3) << 1, subvec1); + + Vector subvec2 = (Vector(2) << 1, 2); + Vector actual5 = (Vector(3) << subvec2, 3); + + Vector actual6 = testFcn1((Vector(3) << 1, 2, 3)); + Vector actual7 = testFcn2((Vector(3) << 1, 2, 3)); + + EXPECT(assert_equal(expected, actual1)); + EXPECT(assert_equal(expected, actual2)); + EXPECT(assert_equal(expected, actual4)); + EXPECT(assert_equal(expected, actual5)); + EXPECT(assert_equal(expected, actual6)); + EXPECT(assert_equal(expected, actual7)); } /* ************************************************************************* */ @@ -107,7 +142,7 @@ TEST( TestVector, subInsert ) size_t i = 2; subInsert(big, small, i); - Vector expected = Vector_(6, 0.0, 0.0, 1.0, 1.0, 1.0, 0.0); + Vector expected = (Vector(6) << 0.0, 0.0, 1.0, 1.0, 1.0, 0.0); EXPECT(assert_equal(expected, big)); } @@ -199,19 +234,19 @@ TEST( TestVector, weightedPseudoinverse_constraint ) // verify EXPECT(assert_equal(expected,actual)); - EXPECT(isinf(precision)); + EXPECT(std::isinf(precision)); } /* ************************************************************************* */ TEST( TestVector, weightedPseudoinverse_nan ) { - Vector a = Vector_(4, 1., 0., 0., 0.); - Vector sigmas = Vector_(4, 0.1, 0.1, 0., 0.); + Vector a = (Vector(4) << 1., 0., 0., 0.); + Vector sigmas = (Vector(4) << 0.1, 0.1, 0., 0.); Vector weights = reciprocal(emul(sigmas,sigmas)); Vector pseudo; double precision; boost::tie(pseudo, precision) = weightedPseudoinverse(a, weights); - Vector expected = Vector_(4, 1., 0., 0.,0.); + Vector expected = (Vector(4) << 1., 0., 0.,0.); EXPECT(assert_equal(expected, pseudo)); DOUBLES_EQUAL(100, precision, 1e-5); } @@ -219,31 +254,31 @@ TEST( TestVector, weightedPseudoinverse_nan ) /* ************************************************************************* */ TEST( TestVector, ediv ) { - Vector a = Vector_(3,10.,20.,30.); - Vector b = Vector_(3,2.0,5.0,6.0); + Vector a = (Vector(3) << 10., 20., 30.); + Vector b = (Vector(3) << 2.0, 5.0, 6.0); Vector actual(ediv(a,b)); - Vector c = Vector_(3,5.0,4.0,5.0); + Vector c = (Vector(3) << 5.0, 4.0, 5.0); EXPECT(assert_equal(c,actual)); } /* ************************************************************************* */ TEST( TestVector, dot ) { - Vector a = Vector_(3,10.,20.,30.); - Vector b = Vector_(3,2.0,5.0,6.0); + Vector a = (Vector(3) << 10., 20., 30.); + Vector b = (Vector(3) << 2.0, 5.0, 6.0); DOUBLES_EQUAL(20+100+180,dot(a,b),1e-9); } /* ************************************************************************* */ TEST( TestVector, axpy ) { - Vector x = Vector_(3,10.,20.,30.); - Vector y0 = Vector_(3,2.0,5.0,6.0); + Vector x = (Vector(3) << 10., 20., 30.); + Vector y0 = (Vector(3) << 2.0, 5.0, 6.0); Vector y1 = y0, y2 = y0; axpy(0.1,x,y1); axpy(0.1,x,y2.head(3)); - Vector expected = Vector_(3,3.0,7.0,9.0); + Vector expected = (Vector(3) << 3.0, 7.0, 9.0); EXPECT(assert_equal(expected,y1)); EXPECT(assert_equal(expected,Vector(y2))); } @@ -251,8 +286,8 @@ TEST( TestVector, axpy ) /* ************************************************************************* */ TEST( TestVector, equals ) { - Vector v1 = Vector_(1, 0.0/std::numeric_limits::quiet_NaN()); //testing nan - Vector v2 = Vector_(1, 1.0); + Vector v1 = (Vector(1) << 0.0/std::numeric_limits::quiet_NaN()); //testing nan + Vector v2 = (Vector(1) << 1.0); double tol = 1.; EXPECT(!equal_with_abs_tol(v1, v2, tol)); } @@ -260,7 +295,7 @@ TEST( TestVector, equals ) /* ************************************************************************* */ TEST( TestVector, greater_than ) { - Vector v1 = Vector_(3, 1.0, 2.0, 3.0), + Vector v1 = (Vector(3) << 1.0, 2.0, 3.0), v2 = zero(3); EXPECT(greaterThanOrEqual(v1, v1)); // test basic greater than EXPECT(greaterThanOrEqual(v1, v2)); // test equals @@ -269,31 +304,31 @@ TEST( TestVector, greater_than ) /* ************************************************************************* */ TEST( TestVector, reciprocal ) { - Vector v = Vector_(3, 1.0, 2.0, 4.0); - EXPECT(assert_equal(Vector_(3, 1.0, 0.5, 0.25),reciprocal(v))); + Vector v = (Vector(3) << 1.0, 2.0, 4.0); + EXPECT(assert_equal((Vector(3) << 1.0, 0.5, 0.25),reciprocal(v))); } /* ************************************************************************* */ TEST( TestVector, linear_dependent ) { - Vector v1 = Vector_(3, 1.0, 2.0, 3.0); - Vector v2 = Vector_(3, -2.0, -4.0, -6.0); + Vector v1 = (Vector(3) << 1.0, 2.0, 3.0); + Vector v2 = (Vector(3) << -2.0, -4.0, -6.0); EXPECT(linear_dependent(v1, v2)); } /* ************************************************************************* */ TEST( TestVector, linear_dependent2 ) { - Vector v1 = Vector_(3, 0.0, 2.0, 0.0); - Vector v2 = Vector_(3, 0.0, -4.0, 0.0); + Vector v1 = (Vector(3) << 0.0, 2.0, 0.0); + Vector v2 = (Vector(3) << 0.0, -4.0, 0.0); EXPECT(linear_dependent(v1, v2)); } /* ************************************************************************* */ TEST( TestVector, linear_dependent3 ) { - Vector v1 = Vector_(3, 0.0, 2.0, 0.0); - Vector v2 = Vector_(3, 0.1, -4.1, 0.0); + Vector v1 = (Vector(3) << 0.0, 2.0, 0.0); + Vector v2 = (Vector(3) << 0.1, -4.1, 0.0); EXPECT(!linear_dependent(v1, v2)); } diff --git a/gtsam/base/tests/testVerticalBlockMatrix.cpp b/gtsam/base/tests/testVerticalBlockMatrix.cpp new file mode 100644 index 000000000..fad23fa7d --- /dev/null +++ b/gtsam/base/tests/testVerticalBlockMatrix.cpp @@ -0,0 +1,47 @@ +/* ---------------------------------------------------------------------------- + + * 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 testVerticalBlockMatrix.cpp + * @brief Unit tests for VerticalBlockMatrix class + * @author Frank Dellaert + * @date February 15, 2014 + **/ + +#include +#include +#include + +using namespace std; +using namespace gtsam; +using boost::assign::list_of; + +//***************************************************************************** +TEST(VerticalBlockMatrix, constructor) { + VerticalBlockMatrix actual(list_of(3)(2)(1), + (Matrix(6, 6) << 1, 2, 3, 4, 5, 6, // + 2, 8, 9, 10, 11, 12, // + 3, 9, 15, 16, 17, 18, // + 4, 10, 16, 22, 23, 24, // + 5, 11, 17, 23, 29, 30, // + 6, 12, 18, 24, 30, 36)); + EXPECT_LONGS_EQUAL(6,actual.rows()); + EXPECT_LONGS_EQUAL(6,actual.cols()); + EXPECT_LONGS_EQUAL(3,actual.nBlocks()); +} + +//***************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//***************************************************************************** + diff --git a/gtsam/base/tests/timeMatrix.cpp b/gtsam/base/tests/timeMatrix.cpp index 9ed397ec8..2cd0a8b19 100644 --- a/gtsam/base/tests/timeMatrix.cpp +++ b/gtsam/base/tests/timeMatrix.cpp @@ -189,7 +189,7 @@ double timeColumn(size_t reps) { */ double timeHouseholder(size_t reps) { // create a matrix - Matrix Abase = Matrix_(4, 7, + Matrix Abase = Mat(4, 7) << -5, 0, 5, 0, 0, 0, -1, 00, -5, 0, 5, 0, 0, 1.5, 10, 0, 0, 0,-10, 0, 2, diff --git a/gtsam/base/timing.cpp b/gtsam/base/timing.cpp index bd9a719b5..36f1c2f5f 100644 --- a/gtsam/base/timing.cpp +++ b/gtsam/base/timing.cpp @@ -29,11 +29,10 @@ #include namespace gtsam { - -/* ************************************************************************* */ namespace internal { -GTSAM_EXPORT boost::shared_ptr timingRoot(new TimingOutline("Total", getTicTocID("Total"))); +GTSAM_EXPORT boost::shared_ptr timingRoot( + new TimingOutline("Total", getTicTocID("Total"))); GTSAM_EXPORT boost::weak_ptr timingCurrent(timingRoot); /* ************************************************************************* */ @@ -45,14 +44,15 @@ void TimingOutline::add(size_t usecs, size_t usecsWall) { t_ += usecs; tWall_ += usecsWall; tIt_ += usecs; - t2_ += (double(usecs)/1000000.0)*(double(usecs)/1000000.0); - ++ n_; + double secs = (double(usecs) / 1000000.0); + t2_ += secs * secs; + ++n_; } /* ************************************************************************* */ TimingOutline::TimingOutline(const std::string& label, size_t myId) : - myId_(myId), t_(0), tWall_(0), t2_(0.0), tIt_(0), tMax_(0), tMin_(0), n_(0), myOrder_(0), lastChildOrder_(0), label_(label) -{ + myId_(myId), t_(0), tWall_(0), t2_(0.0), tIt_(0), tMax_(0), tMin_(0), n_(0), myOrder_( + 0), lastChildOrder_(0), label_(label) { #ifdef GTSAM_USING_NEW_BOOST_TIMERS timer_.stop(); #endif @@ -66,7 +66,7 @@ size_t TimingOutline::time() const { time += child.second->time(); hasChildren = true; } - if(hasChildren) + if (hasChildren) return time; else return t_; @@ -76,9 +76,9 @@ size_t TimingOutline::time() const { void TimingOutline::print(const std::string& outline) const { std::string formattedLabel = label_; boost::replace_all(formattedLabel, "_", " "); - std::cout << outline << "-" << formattedLabel << ": " << double(t_)/1000000.0 << " CPU (" << - n_ << " times, " << double(tWall_)/1000000.0 << " wall, " << double(time())/1000000.0 << " children, min: " - << double(tMin_)/1000000.0 << " max: " << double(tMax_)/1000000.0 << ")\n"; + std::cout << outline << "-" << formattedLabel << ": " << self() << " CPU (" + << n_ << " times, " << wall() << " wall, " << secs() << " children, min: " + << min() << " max: " << max() << ")\n"; // Order children typedef FastMap > ChildOrder; ChildOrder childOrder; @@ -94,39 +94,43 @@ void TimingOutline::print(const std::string& outline) const { std::cout.flush(); } -void TimingOutline::print2(const std::string& outline, const double parentTotal) const { +void TimingOutline::print2(const std::string& outline, + const double parentTotal) const { const int w1 = 24, w2 = 2, w3 = 6, w4 = 8, precision = 2; - const double selfTotal = double(t_)/(1000000.0), - selfMean = selfTotal/double(n_); - const double childTotal = double(time())/(1000000.0); + const double selfTotal = self(), selfMean = selfTotal / double(n_); + const double childTotal = secs(); // compute standard deviation - const double selfStd = sqrt(t2_/double(n_) - selfMean*selfMean); - const std::string label = outline + label_ + ": " ; + const double selfStd = sqrt(t2_ / double(n_) - selfMean * selfMean); + const std::string label = outline + label_ + ": "; - if ( n_ == 0 ) { - std::cout << label << std::fixed << std::setprecision(precision) << childTotal << " seconds" << std::endl; - } - else { - std::cout << std::setw(w1+outline.length()) << label ; - std::cout << std::setiosflags(std::ios::right) << std::setw(w2) << n_ << " (times), " - << std::setiosflags(std::ios::right) << std::fixed << std::setw(w3) << std::setprecision(precision) << selfMean << " (mean), " - << std::setiosflags(std::ios::right) << std::fixed << std::setw(w3) << std::setprecision(precision) << selfStd << " (std)," - << std::setiosflags(std::ios::right) << std::fixed << std::setw(w4) << std::setprecision(precision) << selfTotal << " (total),"; + if (n_ == 0) { + std::cout << label << std::fixed << std::setprecision(precision) + << childTotal << " seconds" << std::endl; + } else { + std::cout << std::setw(w1 + outline.length()) << label; + std::cout << std::setiosflags(std::ios::right) << std::setw(w2) << n_ + << " (times), " << std::setiosflags(std::ios::right) << std::fixed + << std::setw(w3) << std::setprecision(precision) << selfMean + << " (mean), " << std::setiosflags(std::ios::right) << std::fixed + << std::setw(w3) << std::setprecision(precision) << selfStd << " (std)," + << std::setiosflags(std::ios::right) << std::fixed << std::setw(w4) + << std::setprecision(precision) << selfTotal << " (total),"; - if ( parentTotal > 0.0 ) - std::cout << std::setiosflags(std::ios::right) << std::fixed << std::setw(w3) << std::setprecision(precision) << 100.0*selfTotal/parentTotal << " (%)"; + if (parentTotal > 0.0) + std::cout << std::setiosflags(std::ios::right) << std::fixed + << std::setw(w3) << std::setprecision(precision) + << 100.0 * selfTotal / parentTotal << " (%)"; std::cout << std::endl; } BOOST_FOREACH(const ChildMap::value_type& child, children_) { std::string childOutline(outline); - if ( n_ == 0 ) { + if (n_ == 0) { child.second->print2(childOutline, childTotal); - } - else { + } else { childOutline += " "; child.second->print2(childOutline, selfTotal); } @@ -134,13 +138,14 @@ void TimingOutline::print2(const std::string& outline, const double parentTotal) } /* ************************************************************************* */ -const boost::shared_ptr& TimingOutline::child(size_t child, const std::string& label, const boost::weak_ptr& thisPtr) { +const boost::shared_ptr& TimingOutline::child(size_t child, + const std::string& label, const boost::weak_ptr& thisPtr) { assert(thisPtr.lock().get() == this); boost::shared_ptr& result = children_[child]; - if(!result) { + if (!result) { // Create child if necessary result.reset(new TimingOutline(label, child)); - ++ this->lastChildOrder_; + ++this->lastChildOrder_; result->myOrder_ = this->lastChildOrder_; result->parent_ = thisPtr; } @@ -157,27 +162,48 @@ void TimingOutline::ticInternal() { timer_.restart(); *timerActive_ = true; #endif + +#ifdef GTSAM_USE_TBB + tbbTimer_ = tbb::tick_count::now(); +#endif } /* ************************************************************************* */ void TimingOutline::tocInternal() { #ifdef GTSAM_USING_NEW_BOOST_TIMERS + assert(!timer_.is_stopped()); timer_.stop(); - add((timer_.elapsed().user + timer_.elapsed().system) / 1000, timer_.elapsed().wall / 1000); + size_t cpuTime = (timer_.elapsed().user + timer_.elapsed().system) / 1000; +# ifndef GTSAM_USE_TBB + size_t wallTime = timer_.elapsed().wall / 1000; +# endif + #else + assert(timerActive_); double elapsed = timer_.elapsed(); - add(size_t(elapsed * 1000000.0), 0); + size_t cpuTime = size_t(elapsed * 1000000.0); *timerActive_ = false; +# ifndef GTSAM_USE_TBB + size_t wallTime = cpuTime; +# endif + #endif + +#ifdef GTSAM_USE_TBB + size_t wallTime = size_t( + (tbb::tick_count::now() - tbbTimer_).seconds() * 1e6); +#endif + + add(cpuTime, wallTime); } /* ************************************************************************* */ void TimingOutline::finishedIteration() { - if(tIt_ > tMax_) + if (tIt_ > tMax_) tMax_ = tIt_; - if(tMin_ == 0 || tIt_ < tMin_) + if (tMin_ == 0 || tIt_ < tMin_) tMin_ = tIt_; tIt_ = 0; BOOST_FOREACH(ChildMap::value_type& child, children_) { @@ -185,56 +211,59 @@ void TimingOutline::finishedIteration() { } } - /* ************************************************************************* */ - // Generate or retrieve a unique global ID number that will be used to look up tic_/toc statements - size_t getTicTocID(const char *descriptionC) { - const std::string description(descriptionC); - // Global (static) map from strings to ID numbers and current next ID number - static size_t nextId = 0; - static gtsam::FastMap idMap; +/* ************************************************************************* */ +// Generate or retrieve a unique global ID number that will be used to look up tic_/toc statements +size_t getTicTocID(const char *descriptionC) { + const std::string description(descriptionC); + // Global (static) map from strings to ID numbers and current next ID number + static size_t nextId = 0; + static gtsam::FastMap idMap; - // Retrieve or add this string - gtsam::FastMap::const_iterator it = idMap.find(description); - if(it == idMap.end()) { - it = idMap.insert(std::make_pair(description, nextId)).first; - ++ nextId; - } - - // Return ID - return it->second; - } - - /* ************************************************************************* */ - void ticInternal(size_t id, const char *labelC) { - const std::string label(labelC); - if(ISDEBUG("timing-verbose")) - std::cout << "gttic_(" << id << ", " << label << ")" << std::endl; - boost::shared_ptr node = timingCurrent.lock()->child(id, label, timingCurrent); - timingCurrent = node; - node->ticInternal(); - } - - /* ************************************************************************* */ - void tocInternal(size_t id, const char *label) { - if(ISDEBUG("timing-verbose")) - std::cout << "gttoc(" << id << ", " << label << ")" << std::endl; - boost::shared_ptr current(timingCurrent.lock()); - if(id != current->myId_) { - timingRoot->print(); - throw std::invalid_argument( - (boost::format("gtsam timing: Mismatched tic/toc: gttoc(\"%s\") called when last tic was \"%s\".") % - label % current->label_).str()); - } - if(!current->parent_.lock()) { - timingRoot->print(); - throw std::invalid_argument( - (boost::format("gtsam timing: Mismatched tic/toc: extra gttoc(\"%s\"), already at the root") % - label).str()); - } - current->tocInternal(); - timingCurrent = current->parent_; + // Retrieve or add this string + gtsam::FastMap::const_iterator it = idMap.find( + description); + if (it == idMap.end()) { + it = idMap.insert(std::make_pair(description, nextId)).first; + ++nextId; } + // Return ID + return it->second; } +/* ************************************************************************* */ +void ticInternal(size_t id, const char *labelC) { + const std::string label(labelC); + if (ISDEBUG("timing-verbose")) + std::cout << "gttic_(" << id << ", " << label << ")" << std::endl; + boost::shared_ptr node = // + timingCurrent.lock()->child(id, label, timingCurrent); + timingCurrent = node; + node->ticInternal(); } + +/* ************************************************************************* */ +void tocInternal(size_t id, const char *label) { + if (ISDEBUG("timing-verbose")) + std::cout << "gttoc(" << id << ", " << label << ")" << std::endl; + boost::shared_ptr current(timingCurrent.lock()); + if (id != current->myId_) { + timingRoot->print(); + throw std::invalid_argument( + (boost::format( + "gtsam timing: Mismatched tic/toc: gttoc(\"%s\") called when last tic was \"%s\".") + % label % current->label_).str()); + } + if (!current->parent_.lock()) { + timingRoot->print(); + throw std::invalid_argument( + (boost::format( + "gtsam timing: Mismatched tic/toc: extra gttoc(\"%s\"), already at the root") + % label).str()); + } + current->tocInternal(); + timingCurrent = current->parent_; +} + +} // namespace internal +} // namespace gtsam diff --git a/gtsam/base/timing.h b/gtsam/base/timing.h index 22392ce00..7a43ef884 100644 --- a/gtsam/base/timing.h +++ b/gtsam/base/timing.h @@ -1,147 +1,270 @@ -/* ---------------------------------------------------------------------------- - - * 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 timing.h - * @brief Timing utilities - * @author Richard Roberts, Michael Kaess - * @date Oct 5, 2010 - */ -#pragma once - -#include -#include -#include -#include -#include -#include - -// Automatically use the new Boost timers if version is recent enough. -#if BOOST_VERSION >= 104800 -#ifndef GTSAM_DISABLE_NEW_TIMERS -#define GTSAM_USING_NEW_BOOST_TIMERS -#endif -#endif - -#ifdef GTSAM_USING_NEW_BOOST_TIMERS -#include -#else -#include -#endif - -namespace gtsam { - - namespace internal { - GTSAM_EXPORT size_t getTicTocID(const char *description); - GTSAM_EXPORT void ticInternal(size_t id, const char *label); - GTSAM_EXPORT void tocInternal(size_t id, const char *label); - - class GTSAM_EXPORT TimingOutline { - protected: - size_t myId_; - size_t t_; - size_t tWall_; - double t2_ ; /* cache the \sum t_i^2 */ - size_t tIt_; - size_t tMax_; - size_t tMin_; - size_t n_; - size_t myOrder_; - size_t lastChildOrder_; - std::string label_; - boost::weak_ptr parent_; - typedef FastMap > ChildMap; - ChildMap children_; -#ifdef GTSAM_USING_NEW_BOOST_TIMERS - boost::timer::cpu_timer timer_; -#else - boost::timer timer_; - gtsam::ValueWithDefault timerActive_; -#endif - void add(size_t usecs, size_t usecsWall); - public: - TimingOutline(const std::string& label, size_t myId); - size_t time() const; - void print(const std::string& outline = "") const; - void print2(const std::string& outline = "", const double parentTotal = -1.0) const; - const boost::shared_ptr& child(size_t child, const std::string& label, const boost::weak_ptr& thisPtr); - void ticInternal(); - void tocInternal(); - void finishedIteration(); - - GTSAM_EXPORT friend void tocInternal(size_t id, const char *label); - }; // \TimingOutline - - class AutoTicToc { - private: - size_t id_; - const char *label_; - bool isSet_; - public: - AutoTicToc(size_t id, const char* label) : id_(id), label_(label), isSet_(true) { ticInternal(id_, label_); } - void stop() { tocInternal(id_, label_); isSet_ = false; } - ~AutoTicToc() { if(isSet_) stop(); } - }; - - GTSAM_EXTERN_EXPORT boost::shared_ptr timingRoot; - GTSAM_EXTERN_EXPORT boost::weak_ptr timingCurrent; - } - -// Tic and toc functions that are always active (whether or not ENABLE_TIMING is defined) -// There is a trick being used here to achieve near-zero runtime overhead, in that a -// static variable is created for each tic/toc statement storing an integer ID, but the -// integer ID is only looked up by string once when the static variable is initialized -// as the program starts. -#define gttic_(label) \ - static const size_t label##_id_tic = ::gtsam::internal::getTicTocID(#label); \ - ::gtsam::internal::AutoTicToc label##_obj = ::gtsam::internal::AutoTicToc(label##_id_tic, #label) -#define gttoc_(label) \ - label##_obj.stop() -#define longtic_(label) \ - static const size_t label##_id_tic = ::gtsam::internal::getTicTocID(#label); \ - ::gtsam::internal::ticInternal(label##_id_tic, #label) -#define longtoc_(label) \ - static const size_t label##_id_toc = ::gtsam::internal::getTicTocID(#label); \ - ::gtsam::internal::tocInternal(label##_id_toc, #label) -inline void tictoc_finishedIteration_() { - internal::timingRoot->finishedIteration(); } -inline void tictoc_print_() { - internal::timingRoot->print(); } -/* print mean and standard deviation */ -inline void tictoc_print2_() { - internal::timingRoot->print2(); } -#define tictoc_getNode(variable, label) \ - static const size_t label##_id_getnode = ::gtsam::internal::getTicTocID(#label); \ - const boost::shared_ptr variable = \ - internal::timingCurrent.lock()->child(label##_id_getnode, #label, internal::timingCurrent); -inline void tictoc_reset_() { - internal::timingRoot.reset(new internal::TimingOutline("Total", internal::getTicTocID("Total"))); - internal::timingCurrent = internal::timingRoot; } - -#ifdef ENABLE_TIMING -#define gttic(label) gttic_(label) -#define gttoc(label) gttoc_(label) -#define longtic(label) longtic_(label) -#define longtoc(label) longtoc_(label) -#define tictoc_finishedIteration tictoc_finishedIteration_ -#define tictoc_print tictoc_print_ -#define tictoc_reset tictoc_reset_ -#else -#define gttic(label) ((void)0) -#define gttoc(label) ((void)0) -#define longtic(label) ((void)0) -#define longtoc(label) ((void)0) -#define tictoc_finishedIteration() ((void)0) -#define tictoc_print() ((void)0) -#define tictoc_reset() ((void)0) -#endif - -} +/* ---------------------------------------------------------------------------- + + * 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 timing.h + * @brief Timing utilities + * @author Richard Roberts, Michael Kaess + * @date Oct 5, 2010 + */ +#pragma once + +#include +#include +#include +#include +#include +#include + +// This file contains the GTSAM timing instrumentation library, a low-overhead method for +// learning at a medium-fine level how much time various components of an algorithm take +// in CPU and wall time. +// +// The output of this instrumentation is a call-tree-like printout containing statistics +// about each instrumented code block. To print this output at any time, call +// tictoc_print() or tictoc_print_(). +// +// An overall point to be aware of is that there are two versions of each function - one +// ending in an underscore '_' and one without the trailing underscore. The underscore +// versions always are active, but the versions without an underscore are active only when +// GTSAM_ENABLE_TIMING is defined (automatically defined in our CMake Timing build type). +// GTSAM algorithms are all instrumented with the non-underscore versions, so generally +// you should use the underscore versions in your own code to leave out the GTSAM detail. +// +// gttic and gttoc start and stop a timed section, respectively. gttic creates a *scoped* +// object - when it goes out of scope gttoc is called automatically. Thus, you do not +// need to call gttoc if you are timing an entire function (see basic use examples below). +// However, you must be *aware* of this scoped nature - putting gttic inside of an if(...) +// block, for example, will only time code until the closing brace '}'. See advanced +// usage below if you need to avoid this. +// +// Multiple calls nest automatically - each gttic nests under the previous gttic called +// for which gttoc has not been called (or the previous gttic did not go out of scope). +// +// Basic usage examples are as follows: +// +// - Timing an entire function: +// void myFunction() { +// gttic_(myFunction); +// ........ +// } +// +// - Timing an entire function as well as its component parts: +// void myLongFunction() { +// gttic_(myLongFunction); +// gttic_(step1); // Will nest under the 'myLongFunction' label +// ........ +// gttoc_(step1); +// gttic_(step2); // Will nest under the 'myLongFunction' label +// ........ +// gttoc_(step2); +// ........ +// } +// +// - Timing functions calling/called by other functions: +// void oneStep() { +// gttic_(oneStep); // Will automatically nest under the gttic label of the calling function +// ....... +// } +// void algorithm() { +// gttic_(algorithm); +// oneStep(); // gttic's inside this function will automatically nest inside our 'algorithm' label +// twoStep(); // gttic's inside this function will automatically nest inside our 'algorithm' label +// } +// +// +// Advanced usage: +// +// - "Finishing iterations" - to get correct min/max times for each call, you must define +// in your code what constitutes an iteration. A single sum for the min/max times is +// accumulated within each iteration. If you don't care about min/max times, you don't +// need to worry about this. For example: +// void myOuterLoop() { +// while(true) { +// iterateMyAlgorithm(); +// tictoc_finishedIteration_(); +// tictoc_print_(); // Optional +// } +// } +// +// - Stopping timing a section in a different scope than it is started. Normally, a gttoc +// statement goes out of scope at end of C++ scope. However, you can use longtic and +// longtoc to start and stop timing with the specified label at any point, without regard +// too scope. Note that if you use these, it may become difficult to ensure that you +// have matching gttic/gttoc statments. You may want to consider reorganizing your timing +// outline to match the scope of your code. + +// Automatically use the new Boost timers if version is recent enough. +#if BOOST_VERSION >= 104800 +# ifndef GTSAM_DISABLE_NEW_TIMERS +# define GTSAM_USING_NEW_BOOST_TIMERS +# endif +#endif + +#ifdef GTSAM_USING_NEW_BOOST_TIMERS +# include +#else +# include +#endif + +#ifdef GTSAM_USE_TBB +# include +# undef min +# undef max +# undef ERROR +#endif + +namespace gtsam { + + namespace internal { + GTSAM_EXPORT size_t getTicTocID(const char *description); + GTSAM_EXPORT void ticInternal(size_t id, const char *label); + GTSAM_EXPORT void tocInternal(size_t id, const char *label); + + /** + * Timing Entry, arranged in a tree + */ + class GTSAM_EXPORT TimingOutline { + protected: + size_t myId_; + size_t t_; + size_t tWall_; + double t2_ ; ///< cache the \sum t_i^2 + size_t tIt_; + size_t tMax_; + size_t tMin_; + size_t n_; + size_t myOrder_; + size_t lastChildOrder_; + std::string label_; + + // Tree structure + boost::weak_ptr parent_; ///< parent pointer + typedef FastMap > ChildMap; + ChildMap children_; ///< subtrees + +#ifdef GTSAM_USING_NEW_BOOST_TIMERS + boost::timer::cpu_timer timer_; +#else + boost::timer timer_; + gtsam::ValueWithDefault timerActive_; +#endif +#ifdef GTSAM_USE_TBB + tbb::tick_count tbbTimer_; +#endif + void add(size_t usecs, size_t usecsWall); + + public: + /// Constructor + TimingOutline(const std::string& label, size_t myId); + size_t time() const; ///< time taken, including children + double secs() const { return double(time()) / 1000000.0;} ///< time taken, in seconds, including children + double self() const { return double(t_) / 1000000.0;} ///< self time only, in seconds + double wall() const { return double(tWall_) / 1000000.0;} ///< wall time, in seconds + double min() const { return double(tMin_) / 1000000.0;} ///< min time, in seconds + double max() const { return double(tMax_) / 1000000.0;} ///< max time, in seconds + double mean() const { return self() / double(n_); } ///< mean self time, in seconds + void print(const std::string& outline = "") const; + void print2(const std::string& outline = "", const double parentTotal = -1.0) const; + const boost::shared_ptr& + child(size_t child, const std::string& label, const boost::weak_ptr& thisPtr); + void ticInternal(); + void tocInternal(); + void finishedIteration(); + + GTSAM_EXPORT friend void tocInternal(size_t id, const char *label); + }; // \TimingOutline + + /** + * No documentation + */ + class AutoTicToc { + private: + size_t id_; + const char *label_; + bool isSet_; + public: + AutoTicToc(size_t id, const char* label) : id_(id), label_(label), isSet_(true) { ticInternal(id_, label_); } + void stop() { tocInternal(id_, label_); isSet_ = false; } + ~AutoTicToc() { if(isSet_) stop(); } + }; + + GTSAM_EXTERN_EXPORT boost::shared_ptr timingRoot; + GTSAM_EXTERN_EXPORT boost::weak_ptr timingCurrent; + } + +// Tic and toc functions that are always active (whether or not ENABLE_TIMING is defined) +// There is a trick being used here to achieve near-zero runtime overhead, in that a +// static variable is created for each tic/toc statement storing an integer ID, but the +// integer ID is only looked up by string once when the static variable is initialized +// as the program starts. + +// tic +#define gttic_(label) \ + static const size_t label##_id_tic = ::gtsam::internal::getTicTocID(#label); \ + ::gtsam::internal::AutoTicToc label##_obj = ::gtsam::internal::AutoTicToc(label##_id_tic, #label) + +// toc +#define gttoc_(label) \ + label##_obj.stop() + +// tic +#define longtic_(label) \ + static const size_t label##_id_tic = ::gtsam::internal::getTicTocID(#label); \ + ::gtsam::internal::ticInternal(label##_id_tic, #label) + +// toc +#define longtoc_(label) \ + static const size_t label##_id_toc = ::gtsam::internal::getTicTocID(#label); \ + ::gtsam::internal::tocInternal(label##_id_toc, #label) + +// indicate iteration is finished +inline void tictoc_finishedIteration_() { + ::gtsam::internal::timingRoot->finishedIteration(); } + +// print +inline void tictoc_print_() { + ::gtsam::internal::timingRoot->print(); } + +// print mean and standard deviation +inline void tictoc_print2_() { + ::gtsam::internal::timingRoot->print2(); } + +// get a node by label and assign it to variable +#define tictoc_getNode(variable, label) \ + static const size_t label##_id_getnode = ::gtsam::internal::getTicTocID(#label); \ + const boost::shared_ptr variable = \ + ::gtsam::internal::timingCurrent.lock()->child(label##_id_getnode, #label, ::gtsam::internal::timingCurrent); + +// reset +inline void tictoc_reset_() { + ::gtsam::internal::timingRoot.reset(new ::gtsam::internal::TimingOutline("Total", ::gtsam::internal::getTicTocID("Total"))); + ::gtsam::internal::timingCurrent = ::gtsam::internal::timingRoot; } + +#ifdef ENABLE_TIMING +#define gttic(label) gttic_(label) +#define gttoc(label) gttoc_(label) +#define longtic(label) longtic_(label) +#define longtoc(label) longtoc_(label) +#define tictoc_finishedIteration tictoc_finishedIteration_ +#define tictoc_print tictoc_print_ +#define tictoc_reset tictoc_reset_ +#else +#define gttic(label) ((void)0) +#define gttoc(label) ((void)0) +#define longtic(label) ((void)0) +#define longtoc(label) ((void)0) +#define tictoc_finishedIteration() ((void)0) +#define tictoc_print() ((void)0) +#define tictoc_reset() ((void)0) +#endif + +} diff --git a/gtsam/base/treeTraversal-inst.h b/gtsam/base/treeTraversal-inst.h new file mode 100644 index 000000000..3edd7d076 --- /dev/null +++ b/gtsam/base/treeTraversal-inst.h @@ -0,0 +1,224 @@ +/* ---------------------------------------------------------------------------- + +* 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 treeTraversal-inst.h +* @author Richard Roberts +* @date April 9, 2013 +*/ +#pragma once + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace gtsam { + + /** Internal functions used for traversing trees */ + namespace treeTraversal { + + /* ************************************************************************* */ + namespace { + // Internal node used in DFS preorder stack + template + struct TraversalNode { + bool expanded; + const boost::shared_ptr& treeNode; + DATA& parentData; + typename FastList::iterator dataPointer; + TraversalNode(const boost::shared_ptr& _treeNode, DATA& _parentData) : + expanded(false), treeNode(_treeNode), parentData(_parentData) {} + }; + + // Do nothing - default argument for post-visitor for tree traversal + struct no_op { + template + void operator()(const boost::shared_ptr& node, const DATA& data) {} + }; + + } + + /** Traverse a forest depth-first with pre-order and post-order visits. + * @param forest The forest of trees to traverse. The method \c forest.roots() should exist + * and return a collection of (shared) pointers to \c FOREST::Node. + * @param visitorPre \c visitorPre(node, parentData) will be called at every node, before + * visiting its children, and will be passed, by reference, the \c DATA object returned + * by the visit to its parent. Likewise, \c visitorPre should return the \c DATA object + * to pass to the children. The returned \c DATA object will be copy-constructed only + * upon returning to store internally, thus may be modified by visiting the children. + * Regarding efficiency, this copy-on-return is usually optimized out by the compiler. + * @param visitorPost \c visitorPost(node, data) will be called at every node, after visiting + * its children, and will be passed, by reference, the \c DATA object returned by the + * call to \c visitorPre (the \c DATA object may be modified by visiting the children). + * @param rootData The data to pass by reference to \c visitorPre when it is called on each + * root node. */ + template + void DepthFirstForest(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost) + { + // Typedefs + typedef typename FOREST::Node Node; + typedef boost::shared_ptr sharedNode; + + // Depth first traversal stack + typedef TraversalNode TraversalNode; + typedef FastList Stack; + Stack stack; + FastList dataList; // List to store node data as it is returned from the pre-order visitor + + // Add roots to stack (insert such that they are visited and processed in order + { + typename Stack::iterator insertLocation = stack.begin(); + BOOST_FOREACH(const sharedNode& root, forest.roots()) + stack.insert(insertLocation, TraversalNode(root, rootData)); + } + + // Traverse + while(!stack.empty()) + { + // Get next node + TraversalNode& node = stack.front(); + + if(node.expanded) { + // If already expanded, then the data stored in the node is no longer needed, so visit + // then delete it. + (void) visitorPost(node.treeNode, *node.dataPointer); + dataList.erase(node.dataPointer); + stack.pop_front(); + } else { + // If not already visited, visit the node and add its children (use reverse iterators so + // children are processed in the order they appear) + node.dataPointer = dataList.insert(dataList.end(), visitorPre(node.treeNode, node.parentData)); + typename Stack::iterator insertLocation = stack.begin(); + BOOST_FOREACH(const sharedNode& child, node.treeNode->children) + stack.insert(insertLocation, TraversalNode(child, *node.dataPointer)); + node.expanded = true; + } + } + assert(dataList.empty()); + } + + /** Traverse a forest depth-first, with a pre-order visit but no post-order visit. + * @param forest The forest of trees to traverse. The method \c forest.roots() should exist + * and return a collection of (shared) pointers to \c FOREST::Node. + * @param visitorPre \c visitorPre(node, parentData) will be called at every node, before + * visiting its children, and will be passed, by reference, the \c DATA object returned + * by the visit to its parent. Likewise, \c visitorPre should return the \c DATA object + * to pass to the children. The returned \c DATA object will be copy-constructed only + * upon returning to store internally, thus may be modified by visiting the children. + * Regarding efficiency, this copy-on-return is usually optimized out by the compiler. + * @param rootData The data to pass by reference to \c visitorPre when it is called on each + * root node. */ + template + void DepthFirstForest(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre) + { + no_op visitorPost; + DepthFirstForest(forest, rootData, visitorPre, visitorPost); + } + + /** Traverse a forest depth-first with pre-order and post-order visits. + * @param forest The forest of trees to traverse. The method \c forest.roots() should exist + * and return a collection of (shared) pointers to \c FOREST::Node. + * @param visitorPre \c visitorPre(node, parentData) will be called at every node, before + * visiting its children, and will be passed, by reference, the \c DATA object returned + * by the visit to its parent. Likewise, \c visitorPre should return the \c DATA object + * to pass to the children. The returned \c DATA object will be copy-constructed only + * upon returning to store internally, thus may be modified by visiting the children. + * Regarding efficiency, this copy-on-return is usually optimized out by the compiler. + * @param visitorPost \c visitorPost(node, data) will be called at every node, after visiting + * its children, and will be passed, by reference, the \c DATA object returned by the + * call to \c visitorPre (the \c DATA object may be modified by visiting the children). + * @param rootData The data to pass by reference to \c visitorPre when it is called on each + * root node. */ + template + void DepthFirstForestParallel(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, + int problemSizeThreshold = 10) + { +#ifdef GTSAM_USE_TBB + // Typedefs + typedef typename FOREST::Node Node; + typedef boost::shared_ptr sharedNode; + + tbb::task::spawn_root_and_wait(internal::CreateRootTask( + forest.roots(), rootData, visitorPre, visitorPost, problemSizeThreshold)); +#else + DepthFirstForest(forest, rootData, visitorPre, visitorPost); +#endif + } + + + /* ************************************************************************* */ + /** Traversal function for CloneForest */ + namespace { + template + boost::shared_ptr + CloneForestVisitorPre(const boost::shared_ptr& node, const boost::shared_ptr& parentPointer) + { + // Clone the current node and add it to its cloned parent + boost::shared_ptr clone = boost::make_shared(*node); + clone->children.clear(); + parentPointer->children.push_back(clone); + return clone; + } + } + + /** Clone a tree, copy-constructing new nodes (calling boost::make_shared) and setting up child + * pointers for a clone of the original tree. + * @param forest The forest of trees to clone. The method \c forest.roots() should exist and + * return a collection of shared pointers to \c FOREST::Node. + * @return The new collection of roots. */ + template + FastVector > CloneForest(const FOREST& forest) + { + typedef typename FOREST::Node Node; + boost::shared_ptr rootContainer = boost::make_shared(); + DepthFirstForest(forest, rootContainer, CloneForestVisitorPre); + return FastVector >(rootContainer->children.begin(), rootContainer->children.end()); + } + + + /* ************************************************************************* */ + /** Traversal function for PrintForest */ + namespace { + struct PrintForestVisitorPre + { + const KeyFormatter& formatter; + PrintForestVisitorPre(const KeyFormatter& formatter) : formatter(formatter) {} + template std::string operator()(const boost::shared_ptr& node, const std::string& parentString) + { + // Print the current node + node->print(parentString + "-", formatter); + // Increment the indentation + return parentString + "| "; + } + }; + } + + /** Print a tree, prefixing each line with \c str, and formatting keys using \c keyFormatter. + * To print each node, this function calls the \c print function of the tree nodes. */ + template + void PrintForest(const FOREST& forest, std::string str, const KeyFormatter& keyFormatter) { + PrintForestVisitorPre visitor(keyFormatter); + DepthFirstForest(forest, str, visitor); + } + } + +} diff --git a/gtsam/base/treeTraversal/parallelTraversalTasks.h b/gtsam/base/treeTraversal/parallelTraversalTasks.h new file mode 100644 index 000000000..7986f9534 --- /dev/null +++ b/gtsam/base/treeTraversal/parallelTraversalTasks.h @@ -0,0 +1,205 @@ +/* ---------------------------------------------------------------------------- + +* 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 parallelTraversalTasks.h +* @author Richard Roberts +* @date April 9, 2013 +*/ +#pragma once + +#include + +#include +#include +#include + +#ifdef GTSAM_USE_TBB +# include +# undef max // TBB seems to include windows.h and we don't want these macros +# undef min +# undef ERROR + +namespace gtsam { + + /** Internal functions used for traversing trees */ + namespace treeTraversal { + + namespace internal { + + /* ************************************************************************* */ + template + class PostOrderTask : public tbb::task + { + public: + const boost::shared_ptr& treeNode; + boost::shared_ptr myData; + VISITOR_POST& visitorPost; + + PostOrderTask(const boost::shared_ptr& treeNode, const boost::shared_ptr& myData, VISITOR_POST& visitorPost) : + treeNode(treeNode), myData(myData), visitorPost(visitorPost) {} + + tbb::task* execute() + { + // Run the post-order visitor + (void) visitorPost(treeNode, *myData); + + return NULL; + } + }; + + /* ************************************************************************* */ + template + class PreOrderTask : public tbb::task + { + public: + const boost::shared_ptr& treeNode; + boost::shared_ptr myData; + VISITOR_PRE& visitorPre; + VISITOR_POST& visitorPost; + int problemSizeThreshold; + bool makeNewTasks; + + bool isPostOrderPhase; + + PreOrderTask(const boost::shared_ptr& treeNode, const boost::shared_ptr& myData, + VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold, + bool makeNewTasks = true) : + treeNode(treeNode), myData(myData), visitorPre(visitorPre), visitorPost(visitorPost), + problemSizeThreshold(problemSizeThreshold), makeNewTasks(makeNewTasks), isPostOrderPhase(false) {} + + tbb::task* execute() + { + if(isPostOrderPhase) + { + // Run the post-order visitor since this task was recycled to run the post-order visitor + (void) visitorPost(treeNode, *myData); + return NULL; + } + else + { + if(makeNewTasks) + { + if(!treeNode->children.empty()) + { + // Allocate post-order task as a continuation + isPostOrderPhase = true; + recycle_as_continuation(); + //PostOrderTask& postOrderTask = + // *new(allocate_continuation()) PostOrderTask(treeNode, myData, visitorPost); + + bool overThreshold = (treeNode->problemSize() >= problemSizeThreshold); + + tbb::task* firstChild = 0; + tbb::task_list childTasks; + BOOST_FOREACH(const boost::shared_ptr& child, treeNode->children) + { + // Process child in a subtask. Important: Run visitorPre before calling + // allocate_child so that if visitorPre throws an exception, we will not have + // allocated an extra child, this causes a TBB error. + boost::shared_ptr childData = boost::allocate_shared(tbb::scalable_allocator(), visitorPre(child, *myData)); + //childTasks.push_back(*new(postOrderTask.allocate_child()) + // PreOrderTask(child, childData, visitorPre, visitorPost, + // problemSizeThreshold, overThreshold)); + tbb::task* childTask = new(allocate_child()) + PreOrderTask(child, childData, visitorPre, visitorPost, + problemSizeThreshold, overThreshold); + if(firstChild) + childTasks.push_back(*childTask); + else + firstChild = childTask; + } + + // If we have child tasks, start subtasks and wait for them to complete + //postOrderTask.set_ref_count((int) treeNode->children.size()); + set_ref_count((int)treeNode->children.size()); + spawn(childTasks); + return firstChild; + } + else + { + // Run the post-order visitor in this task if we have no children + (void) visitorPost(treeNode, *myData); + return NULL; + } + } + else + { + // Process this node and its children in this task + processNodeRecursively(treeNode, *myData); + return NULL; + } + } + } + + void processNodeRecursively(const boost::shared_ptr& node, DATA& myData) + { + BOOST_FOREACH(const boost::shared_ptr& child, node->children) + { + DATA childData = visitorPre(child, myData); + processNodeRecursively(child, childData); + } + + // Run the post-order visitor + (void) visitorPost(node, myData); + } + }; + + /* ************************************************************************* */ + template + class RootTask : public tbb::task + { + public: + const ROOTS& roots; + DATA& myData; + VISITOR_PRE& visitorPre; + VISITOR_POST& visitorPost; + int problemSizeThreshold; + RootTask(const ROOTS& roots, DATA& myData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, + int problemSizeThreshold) : + roots(roots), myData(myData), visitorPre(visitorPre), visitorPost(visitorPost), + problemSizeThreshold(problemSizeThreshold) {} + + tbb::task* execute() + { + typedef PreOrderTask PreOrderTask; + // Create data and tasks for our children + tbb::task_list tasks; + BOOST_FOREACH(const boost::shared_ptr& root, roots) + { + boost::shared_ptr rootData = boost::allocate_shared(tbb::scalable_allocator(), visitorPre(root, myData)); + tasks.push_back(*new(allocate_child()) + PreOrderTask(root, rootData, visitorPre, visitorPost, problemSizeThreshold)); + } + // Set TBB ref count + set_ref_count(1 + (int) roots.size()); + // Spawn tasks + spawn_and_wait_for_all(tasks); + // Return NULL + return NULL; + } + }; + + template + RootTask& + CreateRootTask(const ROOTS& roots, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold) + { + typedef RootTask RootTask; + return *new(tbb::task::allocate_root()) RootTask(roots, rootData, visitorPre, visitorPost, problemSizeThreshold); + } + + } + + } + +} + +#endif diff --git a/gtsam/base/treeTraversal/statistics.h b/gtsam/base/treeTraversal/statistics.h new file mode 100644 index 000000000..805c69758 --- /dev/null +++ b/gtsam/base/treeTraversal/statistics.h @@ -0,0 +1,95 @@ +/* ---------------------------------------------------------------------------- + +* 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 statistics.h +* @brief Tools for gathering statistics about a forest to aid tuning parallel traversal +* @author Richard Roberts +* @date April 9, 2013 +*/ +#pragma once + +#include +#include + +#include + +namespace gtsam { + + namespace treeTraversal { + + /* ************************************************************************* */ + /// Struct to store gathered statistics about a forest + struct ForestStatistics + { + typedef FastMap > Histogram; + Histogram problemSizeHistogram; + Histogram numberOfChildrenHistogram; + Histogram problemSizeOfSecondLargestChildHistogram; + + static void Write(std::ostream& outStream, const Histogram& histogram) + { + if (!histogram.empty()) + { + Histogram::const_iterator endIt = histogram.end(); + -- endIt; + const int largest = endIt->first; + for (int bin = 0; bin <= largest; ++bin) + { + Histogram::const_iterator item = histogram.find(bin); + const int count = (item == histogram.end() ? 0 : *item->second); + outStream << bin << " " << count << "\n"; + } + } + } + }; + + /* ************************************************************************* */ + namespace internal { + template + ForestStatistics* statisticsVisitor(const boost::shared_ptr& node, ForestStatistics* stats) + { + (*stats->problemSizeHistogram[node->problemSize()]) ++; + (*stats->numberOfChildrenHistogram[(int)node->children.size()]) ++; + if (node->children.size() > 1) + { + int largestProblemSize = 0; + int secondLargestProblemSize = 0; + BOOST_FOREACH(const boost::shared_ptr& child, node->children) + { + if (child->problemSize() > largestProblemSize) + { + secondLargestProblemSize = largestProblemSize; + largestProblemSize = child->problemSize(); + } + else if (child->problemSize() > secondLargestProblemSize) + { + secondLargestProblemSize = child->problemSize(); + } + } + (*stats->problemSizeOfSecondLargestChildHistogram[secondLargestProblemSize]) ++; + } + return stats; + } + } + + /* ************************************************************************* */ + template + ForestStatistics GatherStatistics(const FOREST& forest) + { + ForestStatistics stats; + ForestStatistics* statsPtr = &stats; + DepthFirstForest(forest, statsPtr, internal::statisticsVisitor); + return stats; + } + + } +} diff --git a/gtsam/base/types.cpp b/gtsam/base/types.cpp index ea4db72c8..3f86dc0c1 100644 --- a/gtsam/base/types.cpp +++ b/gtsam/base/types.cpp @@ -20,11 +20,17 @@ #include #include +#include namespace gtsam { - std::string _defaultIndexFormatter(Index j) { - return boost::lexical_cast(j); + /* ************************************************************************* */ + std::string _defaultKeyFormatter(Key key) { + const Symbol asSymbol(key); + if(asSymbol.chr() > 0) + return (std::string)asSymbol; + else + return boost::lexical_cast(key); } } \ No newline at end of file diff --git a/gtsam/base/types.h b/gtsam/base/types.h index 94fcb4b27..f50f21d43 100644 --- a/gtsam/base/types.h +++ b/gtsam/base/types.h @@ -20,26 +20,61 @@ #pragma once #include +#include #include - #include +#include #include +#include +#include + +#ifdef GTSAM_USE_TBB +#include +#include +#include +#endif + +#ifdef GTSAM_USE_EIGEN_MKL_OPENMP +#include +#endif + +#ifdef __clang__ +# define CLANG_DIAGNOSTIC_PUSH_IGNORE(diag) \ + _Pragma("clang diagnostic push") \ + _Pragma("clang diagnostic ignored \"" diag "\"") +#else +# define CLANG_DIAGNOSTIC_PUSH_IGNORE(diag) +#endif + +#ifdef __clang__ +# define CLANG_DIAGNOSTIC_POP() _Pragma("clang diagnostic pop") +#else +# define CLANG_DIAGNOSTIC_POP() +#endif namespace gtsam { - /// Integer variable index type - typedef size_t Index; + /// Integer nonlinear key type + typedef size_t Key; - /** A function to convert indices to strings, for example by translating back - * to a nonlinear key and then to a Symbol. */ - typedef boost::function IndexFormatter; + /// Typedef for a function to format a key, i.e. to convert it to a string + typedef boost::function KeyFormatter; - GTSAM_EXPORT std::string _defaultIndexFormatter(Index j); + // Helper function for DefaultKeyFormatter + GTSAM_EXPORT std::string _defaultKeyFormatter(Key key); - /** The default IndexFormatter outputs the index */ - static const IndexFormatter DefaultIndexFormatter = &_defaultIndexFormatter; + /// The default KeyFormatter, which is used if no KeyFormatter is passed to + /// a nonlinear 'print' function. Automatically detects plain integer keys + /// and Symbol keys. + static const KeyFormatter DefaultKeyFormatter = &_defaultKeyFormatter; + + /// The index type for Eigen objects + typedef ptrdiff_t DenseIndex; + + + /* ************************************************************************* */ /** * Helper class that uses templates to select between two types based on * whether TEST_TYPE is const or not. @@ -61,6 +96,7 @@ namespace gtsam { typedef AS_CONST type; }; + /* ************************************************************************* */ /** * Helper struct that encapsulates a value with a default, this is just used * as a member object so you don't have to specify defaults in the class @@ -79,44 +115,221 @@ namespace gtsam { /** Operator to access the value */ T& operator*() { return value; } + /** Operator to access the value */ + const T& operator*() const { return value; } + /** Implicit conversion allows use in if statements for bool type, etc. */ operator T() const { return value; } }; + /* ************************************************************************* */ + /** A helper class that behaves as a container with one element, and works with + * boost::range */ + template + class ListOfOneContainer { + T element_; + public: + typedef T value_type; + typedef const T* const_iterator; + typedef T* iterator; + ListOfOneContainer(const T& element) : element_(element) {} + const T* begin() const { return &element_; } + const T* end() const { return &element_ + 1; } + T* begin() { return &element_; } + T* end() { return &element_ + 1; } + size_t size() const { return 1; } + }; + + BOOST_CONCEPT_ASSERT((boost::RandomAccessRangeConcept >)); + + /** Factory function for ListOfOneContainer to enable ListOfOne(e) syntax. */ + template + ListOfOneContainer ListOfOne(const T& element) { + return ListOfOneContainer(element); + } + + /* ************************************************************************* */ + /// Base exception type that uses tbb_exception if GTSAM is compiled with TBB. + template + class ThreadsafeException : +#ifdef GTSAM_USE_TBB + public tbb::tbb_exception +#else + public std::exception +#endif + { +#ifdef GTSAM_USE_TBB + private: + typedef tbb::tbb_exception Base; + protected: + typedef std::basic_string, tbb::tbb_allocator > String; +#else + private: + typedef std::exception Base; + protected: + typedef std::string String; +#endif + + protected: + bool dynamic_; ///< Whether this object was moved + mutable boost::optional description_; ///< Optional description + + /// Default constructor is protected - may only be created from derived classes + ThreadsafeException() : dynamic_(false) {} + + /// Copy constructor is protected - may only be created from derived classes + ThreadsafeException(const ThreadsafeException& other) : Base(other), dynamic_(false) {} + + /// Construct with description string + ThreadsafeException(const std::string& description) : dynamic_(false), description_(String(description.begin(), description.end())) {} + + /// Default destructor doesn't have the throw() + virtual ~ThreadsafeException() throw () {} + + public: + // Implement functions for tbb_exception +#ifdef GTSAM_USE_TBB + virtual tbb::tbb_exception* move() throw () { + void* cloneMemory = scalable_malloc(sizeof(DERIVED)); + if (!cloneMemory) { + std::cerr << "Failed allocating memory to copy thrown exception, exiting now." << std::endl; + exit(-1); + } + DERIVED* clone = ::new(cloneMemory) DERIVED(static_cast(*this)); + clone->dynamic_ = true; + return clone; + } + + virtual void destroy() throw() { + if (dynamic_) { + DERIVED* derivedPtr = static_cast(this); + derivedPtr->~DERIVED(); + scalable_free(derivedPtr); + } + } + + virtual void throw_self() { + throw *static_cast(this); } + + virtual const char* name() const throw() { + return typeid(DERIVED).name(); } +#endif + + virtual const char* what() const throw() { + return description_ ? description_->c_str() : ""; } + }; + + /* ************************************************************************* */ + /// Threadsafe runtime error exception + class RuntimeErrorThreadsafe : public ThreadsafeException + { + public: + /// Construct with a string describing the exception + RuntimeErrorThreadsafe(const std::string& description) : ThreadsafeException(description) {} + }; + + /* ************************************************************************* */ + /// Threadsafe runtime error exception + class OutOfRangeThreadsafe : public ThreadsafeException + { + public: + /// Construct with a string describing the exception + OutOfRangeThreadsafe(const std::string& description) : ThreadsafeException(description) {} + }; + + /* ************************************************************************* */ + /// Threadsafe invalid argument exception + class InvalidArgumentThreadsafe : public ThreadsafeException + { + public: + /// Construct with a string describing the exception + InvalidArgumentThreadsafe(const std::string& description) : ThreadsafeException(description) {} + }; + + /* ************************************************************************* */ +#ifdef __clang__ +# pragma clang diagnostic push +# pragma clang diagnostic ignored "-Wunused-private-field" // Clang complains that previousOpenMPThreads is unused in the #else case below +#endif + + /// An object whose scope defines a block where TBB and OpenMP parallelism are mixed. In such a + /// block, we use default threads for TBB, and p/2 threads for OpenMP. If GTSAM is not compiled to + /// use both TBB and OpenMP, this has no effect. + class TbbOpenMPMixedScope + { + int previousOpenMPThreads; + + public: +#if defined GTSAM_USE_TBB && defined GTSAM_USE_EIGEN_MKL_OPENMP + TbbOpenMPMixedScope() : + previousOpenMPThreads(omp_get_num_threads()) + { + omp_set_num_threads(omp_get_num_procs() / 4); + } + + ~TbbOpenMPMixedScope() + { + omp_set_num_threads(previousOpenMPThreads); + } +#else + TbbOpenMPMixedScope() : previousOpenMPThreads(-1) {} + ~TbbOpenMPMixedScope() {} +#endif + }; + +#ifdef __clang__ +# pragma clang diagnostic pop +#endif + } +/* ************************************************************************* */ +/** An assertion that throws an exception if NDEBUG is not defined and +* evaluates to an empty statement otherwise. */ +#ifdef NDEBUG +#define assert_throw(CONDITION, EXCEPTION) ((void)0) +#else +#define assert_throw(CONDITION, EXCEPTION) \ + if (!(CONDITION)) { \ + throw (EXCEPTION); \ + } +#endif + #ifdef _MSC_VER // Define some common g++ functions and macros we use that MSVC does not have -#include -namespace std { - template inline int isfinite(T a) { - return (int)boost::math::isfinite(a); } - template inline int isnan(T a) { - return (int)boost::math::isnan(a); } - template inline int isinf(T a) { - return (int)boost::math::isinf(a); } -} - -#include -#ifndef M_PI -#define M_PI (boost::math::constants::pi()) -#endif -#ifndef M_PI_2 -#define M_PI_2 (boost::math::constants::pi() / 2.0) -#endif -#ifndef M_PI_4 -#define M_PI_4 (boost::math::constants::pi() / 4.0) -#endif - -#endif - -#ifdef min -#undef min -#endif - -#ifdef max -#undef max -#endif +#include +namespace std { + template inline int isfinite(T a) { + return (int)boost::math::isfinite(a); } + template inline int isnan(T a) { + return (int)boost::math::isnan(a); } + template inline int isinf(T a) { + return (int)boost::math::isinf(a); } +} +#include +#ifndef M_PI +#define M_PI (boost::math::constants::pi()) +#endif +#ifndef M_PI_2 +#define M_PI_2 (boost::math::constants::pi() / 2.0) +#endif +#ifndef M_PI_4 +#define M_PI_4 (boost::math::constants::pi() / 4.0) +#endif + +#endif + +#ifdef min +#undef min +#endif + +#ifdef max +#undef max +#endif + +#ifdef ERROR +#undef ERROR +#endif diff --git a/gtsam/config.h.in b/gtsam/config.h.in index a9e3ba789..64136511d 100644 --- a/gtsam/config.h.in +++ b/gtsam/config.h.in @@ -21,8 +21,8 @@ #define GTSAM_VERSION_MAJOR @GTSAM_VERSION_MAJOR@ #define GTSAM_VERSION_MINOR @GTSAM_VERSION_MINOR@ #define GTSAM_VERSION_PATCH @GTSAM_VERSION_PATCH@ -#define GTSAM_VERSION_STRING "@GTSAM_VERSION_MAJOR@.@GTSAM_VERSION_MINOR@.@GTSAM_VERSION_PATCH@" -#define GTSAM_VERSION_NUMERIC (10000 * @GTSAM_VERSION_MAJOR@ + 100 * @GTSAM_VERSION_MINOR@ + @GTSAM_VERSION_PATCH@) +#define GTSAM_VERSION_NUMERIC @GTSAM_VERSION_NUMERIC@ +#define GTSAM_VERSION_STRING "@GTSAM_VERSION_STRING@" // Paths to example datasets distributed with GTSAM #define GTSAM_SOURCE_TREE_DATASET_DIR "@CMAKE_SOURCE_DIR@/examples/Data" @@ -38,3 +38,23 @@ #ifndef GTSAM_USE_QUATERNIONS #cmakedefine GTSAM_ROT3_EXPMAP #endif + +// Whether we are using TBB (if TBB was found and GTSAM_WITH_TBB is enabled in CMake) +#cmakedefine GTSAM_USE_TBB + +// Whether Eigen will use MKL (if MKL was found and GTSAM_WITH_EIGEN_MKL is enabled in CMake) +#cmakedefine GTSAM_USE_EIGEN_MKL +#cmakedefine EIGEN_USE_MKL_ALL // This is also defined in gtsam_eigen_includes.h + +// Whether Eigen with MKL will use OpenMP (if OpenMP was found, Eigen uses MKL, and GTSAM_WITH_EIGEN_MKL_OPENMP is enabled in CMake) +#cmakedefine GTSAM_USE_EIGEN_MKL_OPENMP + +// The default allocator to use +#cmakedefine GTSAM_ALLOCATOR_BOOSTPOOL +#cmakedefine GTSAM_ALLOCATOR_TBB +#cmakedefine GTSAM_ALLOCATOR_STL + +// Option for not throwing the CheiralityException for points that are behind a camera +#cmakedefine GTSAM_THROW_CHEIRALITY_EXCEPTION + + diff --git a/gtsam/discrete/CMakeLists.txt b/gtsam/discrete/CMakeLists.txt index ac43359d2..3c5602080 100644 --- a/gtsam/discrete/CMakeLists.txt +++ b/gtsam/discrete/CMakeLists.txt @@ -4,21 +4,8 @@ file(GLOB discrete_headers "*.h") # FIXME: exclude headers install(FILES ${discrete_headers} DESTINATION include/gtsam/discrete) -# Set up library dependencies -set (discrete_local_libs - discrete - inference - base - ccolamd -) - -# Exclude tests that don't work -set (discrete_excluded_tests "") - # Add all tests -if (GTSAM_BUILD_TESTS) - gtsam_add_subdir_tests(discrete "${discrete_local_libs}" "${gtsam-default}" "${discrete_excluded_tests}") -endif() +add_subdirectory(tests) # Build timing scripts #if (GTSAM_BUILD_TIMING) diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h index a13dba513..9319a1541 100644 --- a/gtsam/discrete/DecisionTree-inl.h +++ b/gtsam/discrete/DecisionTree-inl.h @@ -11,7 +11,7 @@ /** * @file DecisionTree.h - * @brief Decision Tree for use in DiscreteFactors + * @brief Decision Tree for use in DiscreteFactors * @author Frank Dellaert * @author Can Erdogan * @date Jan 30, 2012 @@ -421,7 +421,7 @@ namespace gtsam { DecisionTree::DecisionTree(const std::vector& labelCs, const std::string& table) { - // Convert std::string to doubles + // Convert std::string to values of type Y std::vector ys; std::istringstream iss(table); copy(std::istream_iterator(iss), std::istream_iterator(), @@ -643,6 +643,7 @@ namespace gtsam { // where there is no more branch on "label": only the subtree under that // branch point corresponding to the value "index" is left instead. // The function below get all these smaller trees and "ops" them together. + // This implements marginalization in Darwiche09book, pg 330 template DecisionTree DecisionTree::combine(const L& label, size_t cardinality, const Binary& op) const { diff --git a/gtsam/discrete/DecisionTree.h b/gtsam/discrete/DecisionTree.h index d579ef5de..ecf03ad5d 100644 --- a/gtsam/discrete/DecisionTree.h +++ b/gtsam/discrete/DecisionTree.h @@ -11,7 +11,7 @@ /** * @file DecisionTree.h - * @brief Decision Tree for use in DiscreteFactors + * @brief Decision Tree for use in DiscreteFactors * @author Frank Dellaert * @author Can Erdogan * @date Jan 30, 2012 @@ -28,7 +28,7 @@ namespace gtsam { /** - * Algebraic Decision Trees + * Decision Tree * L = label for variables * Y = function range (any algebra), e.g., bool, int, double */ @@ -44,7 +44,7 @@ namespace gtsam { /** A label annotated with cardinality */ typedef std::pair LabelC; - /** DD's consist of Leaf and Choice nodes, both subclasses of Node */ + /** DTs consist of Leaf and Choice nodes, both subclasses of Node */ class Leaf; class Choice; @@ -60,16 +60,14 @@ namespace gtsam { // Constructor Node() { #ifdef DT_DEBUG_MEMORY - std::cout << ++nrNodes << " constructed " << id() << std::endl; std::cout.flush(); - + std::cout << ++nrNodes << " constructed " << id() << std::endl; std::cout.flush(); #endif } // Destructor virtual ~Node() { #ifdef DT_DEBUG_MEMORY - std::cout << --nrNodes << " destructed " << id() << std::endl; std::cout.flush(); - + std::cout << --nrNodes << " destructed " << id() << std::endl; std::cout.flush(); #endif } @@ -94,10 +92,10 @@ namespace gtsam { public: - /** A function is a shared pointer to the root of an ADD */ + /** A function is a shared pointer to the root of a DT */ typedef typename Node::Ptr NodePtr; - /* an AlgebraicDecisionTree just contains the root */ + /* a DecisionTree just contains the root */ NodePtr root_; protected: @@ -128,7 +126,7 @@ namespace gtsam { /** Allow Label+Cardinality for convenience */ DecisionTree(const LabelC& label, const Y& y1, const Y& y2); - /** Create from keys and string table */ + /** Create from keys and a corresponding vector of values */ DecisionTree(const std::vector& labelCs, const std::vector& ys); /** Create from keys and string table */ @@ -138,7 +136,7 @@ namespace gtsam { template DecisionTree(Iterator begin, Iterator end, const L& label); - /** Create DecisionTree from others others (binary version) */ + /** Create DecisionTree from two others */ DecisionTree(const L& label, // const DecisionTree& f0, const DecisionTree& f1); @@ -177,7 +175,8 @@ namespace gtsam { /** apply binary operation "op" to f and g */ DecisionTree apply(const DecisionTree& g, const Binary& op) const; - /** create a new function where value(label)==index */ + /** create a new function where value(label)==index + * It's like "restrict" in Darwiche09book pg329, 330? */ DecisionTree choose(const L& label, size_t index) const { NodePtr newRoot = root_->choose(label, index); return DecisionTree(newRoot); diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp index 1eb428669..9ed88bc3d 100644 --- a/gtsam/discrete/DecisionTreeFactor.cpp +++ b/gtsam/discrete/DecisionTreeFactor.cpp @@ -44,25 +44,30 @@ namespace gtsam { } /* ************************************************************************* */ - bool DecisionTreeFactor::equals(const This& other, double tol) const { - return IndexFactor::equals(other, tol) && Potentials::equals(other, tol); + bool DecisionTreeFactor::equals(const DiscreteFactor& other, double tol) const { + if(!dynamic_cast(&other)) { + return false; + } + else { + const DecisionTreeFactor& f(static_cast(other)); + return Potentials::equals(f, tol); + } } /* ************************************************************************* */ void DecisionTreeFactor::print(const string& s, - const IndexFormatter& formatter) const { + const KeyFormatter& formatter) const { cout << s; - IndexFactor::print("IndexFactor:",formatter); Potentials::print("Potentials:",formatter); } /* ************************************************************************* */ - DecisionTreeFactor DecisionTreeFactor::apply // - (const DecisionTreeFactor& f, ADT::Binary op) const { - map cs; // new cardinalities + DecisionTreeFactor DecisionTreeFactor::apply(const DecisionTreeFactor& f, + ADT::Binary op) const { + map cs; // new cardinalities // make unique key-cardinality map - BOOST_FOREACH(Index j, keys()) cs[j] = cardinality(j); - BOOST_FOREACH(Index j, f.keys()) cs[j] = f.cardinality(j); + BOOST_FOREACH(Key j, keys()) cs[j] = cardinality(j); + BOOST_FOREACH(Key j, f.keys()) cs[j] = f.cardinality(j); // Convert map into keys DiscreteKeys keys; BOOST_FOREACH(const DiscreteKey& key, cs) @@ -74,8 +79,8 @@ namespace gtsam { } /* ************************************************************************* */ - DecisionTreeFactor::shared_ptr DecisionTreeFactor::combine // - (size_t nrFrontals, ADT::Binary op) const { + DecisionTreeFactor::shared_ptr DecisionTreeFactor::combine(size_t nrFrontals, + ADT::Binary op) const { if (nrFrontals > size()) throw invalid_argument( (boost::format( @@ -86,14 +91,45 @@ namespace gtsam { size_t i; ADT result(*this); for (i = 0; i < nrFrontals; i++) { - Index j = keys()[i]; + Key j = keys()[i]; result = result.combine(j, cardinality(j), op); } // create new factor, note we start keys after nrFrontals DiscreteKeys dkeys; for (; i < keys().size(); i++) { - Index j = keys()[i]; + Key j = keys()[i]; + dkeys.push_back(DiscreteKey(j,cardinality(j))); + } + return boost::make_shared(dkeys, result); + } + + + /* ************************************************************************* */ + DecisionTreeFactor::shared_ptr DecisionTreeFactor::combine(const Ordering& frontalKeys, + ADT::Binary op) const { + + if (frontalKeys.size() > size()) throw invalid_argument( + (boost::format( + "DecisionTreeFactor::combine: invalid number of frontal keys %d, nr.keys=%d") + % frontalKeys.size() % size()).str()); + + // sum over nrFrontals keys + size_t i; + ADT result(*this); + for (i = 0; i < frontalKeys.size(); i++) { + Key j = frontalKeys[i]; + result = result.combine(j, cardinality(j), op); + } + + // create new factor, note we collect keys that are not in frontalKeys + // TODO: why do we need this??? result should contain correct keys!!! + DiscreteKeys dkeys; + for (i = 0; i < keys().size(); i++) { + Key j = keys()[i]; + // TODO: inefficient! + if (std::find(frontalKeys.begin(), frontalKeys.end(), j) != frontalKeys.end()) + continue; dkeys.push_back(DiscreteKey(j,cardinality(j))); } return boost::make_shared(dkeys, result); diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h index d072c89af..dc7466199 100644 --- a/gtsam/discrete/DecisionTreeFactor.h +++ b/gtsam/discrete/DecisionTreeFactor.h @@ -20,6 +20,7 @@ #include #include +#include #include #include @@ -41,7 +42,7 @@ namespace gtsam { // typedefs needed to play nice with gtsam typedef DecisionTreeFactor This; - typedef DiscreteConditional ConditionalType; + typedef DiscreteFactor Base; ///< Typedef to base class typedef boost::shared_ptr shared_ptr; public: @@ -69,11 +70,11 @@ namespace gtsam { /// @{ /// equality - bool equals(const DecisionTreeFactor& other, double tol = 1e-9) const; + bool equals(const DiscreteFactor& other, double tol = 1e-9) const; // print virtual void print(const std::string& s = "DecisionTreeFactor:\n", - const IndexFormatter& formatter = DefaultIndexFormatter) const; + const KeyFormatter& formatter = DefaultKeyFormatter) const; /// @} /// @name Standard Interface @@ -104,6 +105,11 @@ namespace gtsam { return combine(nrFrontals, ADT::Ring::add); } + /// Create new factor by summing all values with the same separator values + shared_ptr sum(const Ordering& keys) const { + return combine(keys, ADT::Ring::add); + } + /// Create new factor by maximizing over all values with the same separator values shared_ptr max(size_t nrFrontals) const { return combine(nrFrontals, ADT::Ring::max); @@ -129,27 +135,36 @@ namespace gtsam { shared_ptr combine(size_t nrFrontals, ADT::Binary op) const; /** - * @brief Permutes the keys in Potentials and DiscreteFactor - * - * This re-implements the permuteWithInverse() in both Potentials - * and DiscreteFactor by doing both of them together. + * Combine frontal variables in an Ordering using binary operator "op" + * @param nrFrontals nr. of frontal to combine variables in this factor + * @param op a binary operator that operates on AlgebraicDecisionDiagram potentials + * @return shared pointer to newly created DecisionTreeFactor */ + shared_ptr combine(const Ordering& keys, ADT::Binary op) const; - void permuteWithInverse(const Permutation& inversePermutation){ - DiscreteFactor::permuteWithInverse(inversePermutation); - Potentials::permuteWithInverse(inversePermutation); - } - - /** - * Apply a reduction, which is a remapping of variable indices. - */ - virtual void reduceWithInverse(const internal::Reduction& inverseReduction) { - DiscreteFactor::reduceWithInverse(inverseReduction); - Potentials::reduceWithInverse(inverseReduction); - } + +// /** +// * @brief Permutes the keys in Potentials and DiscreteFactor +// * +// * This re-implements the permuteWithInverse() in both Potentials +// * and DiscreteFactor by doing both of them together. +// */ +// +// void permuteWithInverse(const Permutation& inversePermutation){ +// DiscreteFactor::permuteWithInverse(inversePermutation); +// Potentials::permuteWithInverse(inversePermutation); +// } +// +// /** +// * Apply a reduction, which is a remapping of variable indices. +// */ +// virtual void reduceWithInverse(const internal::Reduction& inverseReduction) { +// DiscreteFactor::reduceWithInverse(inverseReduction); +// Potentials::reduceWithInverse(inverseReduction); +// } /// @} - }; +}; // DecisionTreeFactor }// namespace gtsam diff --git a/gtsam/discrete/DiscreteBayesNet.cpp b/gtsam/discrete/DiscreteBayesNet.cpp index 98a61f7bd..3b3939674 100644 --- a/gtsam/discrete/DiscreteBayesNet.cpp +++ b/gtsam/discrete/DiscreteBayesNet.cpp @@ -18,47 +18,53 @@ #include #include -#include +#include #include namespace gtsam { - // Explicitly instantiate so we don't have to include everywhere - template class BayesNet ; + // Instantiate base class + template class FactorGraph; /* ************************************************************************* */ - void add_front(DiscreteBayesNet& bayesNet, const Signature& s) { - bayesNet.push_front(boost::make_shared(s)); + bool DiscreteBayesNet::equals(const This& bn, double tol) const + { + return Base::equals(bn, tol); } /* ************************************************************************* */ - void add(DiscreteBayesNet& bayesNet, const Signature& s) { - bayesNet.push_back(boost::make_shared(s)); +// void DiscreteBayesNet::add_front(const Signature& s) { +// push_front(boost::make_shared(s)); +// } + + /* ************************************************************************* */ + void DiscreteBayesNet::add(const Signature& s) { + push_back(boost::make_shared(s)); } /* ************************************************************************* */ - double evaluate(const DiscreteBayesNet& bn, const DiscreteConditional::Values & values) { + double DiscreteBayesNet::evaluate(const DiscreteConditional::Values & values) const { // evaluate all conditionals and multiply double result = 1.0; - BOOST_FOREACH(DiscreteConditional::shared_ptr conditional, bn) + BOOST_FOREACH(DiscreteConditional::shared_ptr conditional, *this) result *= (*conditional)(values); return result; } /* ************************************************************************* */ - DiscreteFactor::sharedValues optimize(const DiscreteBayesNet& bn) { + DiscreteFactor::sharedValues DiscreteBayesNet::optimize() const { // solve each node in turn in topological sort order (parents first) DiscreteFactor::sharedValues result(new DiscreteFactor::Values()); - BOOST_REVERSE_FOREACH (DiscreteConditional::shared_ptr conditional, bn) + BOOST_REVERSE_FOREACH (DiscreteConditional::shared_ptr conditional, *this) conditional->solveInPlace(*result); return result; } /* ************************************************************************* */ - DiscreteFactor::sharedValues sample(const DiscreteBayesNet& bn) { + DiscreteFactor::sharedValues DiscreteBayesNet::sample() const { // sample each node in turn in topological sort order (parents first) DiscreteFactor::sharedValues result(new DiscreteFactor::Values()); - BOOST_REVERSE_FOREACH(DiscreteConditional::shared_ptr conditional, bn) + BOOST_REVERSE_FOREACH(DiscreteConditional::shared_ptr conditional, *this) conditional->sampleInPlace(*result); return result; } diff --git a/gtsam/discrete/DiscreteBayesNet.h b/gtsam/discrete/DiscreteBayesNet.h index 9c30d800a..8cb2db182 100644 --- a/gtsam/discrete/DiscreteBayesNet.h +++ b/gtsam/discrete/DiscreteBayesNet.h @@ -20,27 +20,80 @@ #include #include #include -#include +#include #include namespace gtsam { - typedef BayesNet DiscreteBayesNet; +/** A Bayes net made from linear-Discrete densities */ + class GTSAM_EXPORT DiscreteBayesNet: public FactorGraph + { + public: - /** Add a DiscreteCondtional */ - GTSAM_EXPORT void add(DiscreteBayesNet&, const Signature& s); + typedef FactorGraph Base; + typedef DiscreteBayesNet This; + typedef DiscreteConditional ConditionalType; + typedef boost::shared_ptr shared_ptr; + typedef boost::shared_ptr sharedConditional; - /** Add a DiscreteCondtional in front, when listing parents first*/ - GTSAM_EXPORT void add_front(DiscreteBayesNet&, const Signature& s); + /// @name Standard Constructors + /// @{ - //** evaluate for given Values */ - GTSAM_EXPORT double evaluate(const DiscreteBayesNet& bn, const DiscreteConditional::Values & values); + /** Construct empty factor graph */ + DiscreteBayesNet() {} - /** Optimize function for back-substitution. */ - GTSAM_EXPORT DiscreteFactor::sharedValues optimize(const DiscreteBayesNet& bn); + /** Construct from iterator over conditionals */ + template + DiscreteBayesNet(ITERATOR firstConditional, ITERATOR lastConditional) : Base(firstConditional, lastConditional) {} - /** Do ancestral sampling */ - GTSAM_EXPORT DiscreteFactor::sharedValues sample(const DiscreteBayesNet& bn); + /** Construct from container of factors (shared_ptr or plain objects) */ + template + explicit DiscreteBayesNet(const CONTAINER& conditionals) : Base(conditionals) {} + + /** Implicit copy/downcast constructor to override explicit template container constructor */ + template + DiscreteBayesNet(const FactorGraph& graph) : Base(graph) {} + + /// @} + + /// @name Testable + /// @{ + + /** Check equality */ + bool equals(const This& bn, double tol = 1e-9) const; + + /// @} + + /// @name Standard Interface + /// @{ + + /** Add a DiscreteCondtional */ + void add(const Signature& s); + +// /** Add a DiscreteCondtional in front, when listing parents first*/ +// GTSAM_EXPORT void add_front(const Signature& s); + + //** evaluate for given Values */ + double evaluate(const DiscreteConditional::Values & values) const; + + /** + * Solve the DiscreteBayesNet by back-substitution + */ + DiscreteFactor::sharedValues optimize() const; + + /** Do ancestral sampling */ + DiscreteFactor::sharedValues sample() const; + + ///@} + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + } + }; } // namespace diff --git a/gtsam/discrete/DiscreteBayesTree.cpp b/gtsam/discrete/DiscreteBayesTree.cpp new file mode 100644 index 000000000..c7a81aa60 --- /dev/null +++ b/gtsam/discrete/DiscreteBayesTree.cpp @@ -0,0 +1,43 @@ +/* ---------------------------------------------------------------------------- + + * 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 DiscreteBayesTree.cpp + * @brief Discrete Bayes Tree, the result of eliminating a DiscreteJunctionTree + * @brief DiscreteBayesTree + * @author Frank Dellaert + * @author Richard Roberts + */ + +#include +#include +#include +#include +#include + +namespace gtsam { + + // Instantiate base class + template class BayesTreeCliqueBase; + template class BayesTree; + + + /* ************************************************************************* */ + bool DiscreteBayesTree::equals(const This& other, double tol) const + { + return Base::equals(other, tol); + } + +} // \namespace gtsam + + + + diff --git a/gtsam/discrete/DiscreteBayesTree.h b/gtsam/discrete/DiscreteBayesTree.h new file mode 100644 index 000000000..0df6ab476 --- /dev/null +++ b/gtsam/discrete/DiscreteBayesTree.h @@ -0,0 +1,66 @@ +/* ---------------------------------------------------------------------------- + + * 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 DiscreteBayesTree.h + * @brief Discrete Bayes Tree, the result of eliminating a DiscreteJunctionTree + * @brief DiscreteBayesTree + * @author Frank Dellaert + * @author Richard Roberts + */ + +#pragma once + +#include +#include +#include +#include + +namespace gtsam { + + // Forward declarations + class DiscreteConditional; + class VectorValues; + + /* ************************************************************************* */ + /** A clique in a DiscreteBayesTree */ + class GTSAM_EXPORT DiscreteBayesTreeClique : + public BayesTreeCliqueBase + { + public: + typedef DiscreteBayesTreeClique This; + typedef BayesTreeCliqueBase Base; + typedef boost::shared_ptr shared_ptr; + typedef boost::weak_ptr weak_ptr; + DiscreteBayesTreeClique() {} + DiscreteBayesTreeClique(const boost::shared_ptr& conditional) : Base(conditional) {} + }; + + /* ************************************************************************* */ + /** A Bayes tree representing a Discrete density */ + class GTSAM_EXPORT DiscreteBayesTree : + public BayesTree + { + private: + typedef BayesTree Base; + + public: + typedef DiscreteBayesTree This; + typedef boost::shared_ptr shared_ptr; + + /** Default constructor, creates an empty Bayes tree */ + DiscreteBayesTree() {} + + /** Check equality */ + bool equals(const This& other, double tol = 1e-9) const; + }; + +} diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp index ebf8c0c04..f21a705ff 100644 --- a/gtsam/discrete/DiscreteConditional.cpp +++ b/gtsam/discrete/DiscreteConditional.cpp @@ -18,6 +18,7 @@ #include #include +#include #include #include @@ -34,174 +35,191 @@ using namespace std; namespace gtsam { - /* ******************************************************************************** */ - DiscreteConditional::DiscreteConditional(const size_t nrFrontals, - const DecisionTreeFactor& f) : - IndexConditional(f.keys(), nrFrontals), Potentials( - f / (*f.sum(nrFrontals))) { - } - - /* ******************************************************************************** */ - DiscreteConditional::DiscreteConditional(const DecisionTreeFactor& joint, - const DecisionTreeFactor& marginal) : - IndexConditional(joint.keys(), joint.size() - marginal.size()), Potentials( - ISDEBUG("DiscreteConditional::COUNT") ? joint : joint / marginal) { - if (ISDEBUG("DiscreteConditional::DiscreteConditional")) - cout << (firstFrontalKey()) << endl; //TODO Print all keys - } - - /* ******************************************************************************** */ - DiscreteConditional::DiscreteConditional(const Signature& signature) : - IndexConditional(signature.indices(), 1), Potentials( - signature.discreteKeysParentsFirst(), signature.cpt()) { - } - - /* ******************************************************************************** */ - void DiscreteConditional::print(const std::string& s, const IndexFormatter& formatter) const { - std::cout << s << std::endl; - IndexConditional::print(s, formatter); - Potentials::print(s); - } - - /* ******************************************************************************** */ - bool DiscreteConditional::equals(const DiscreteConditional& other, double tol) const { - return IndexConditional::equals(other, tol) - && Potentials::equals(other, tol); - } - - /* ******************************************************************************** */ - Potentials::ADT DiscreteConditional::choose( - const Values& parentsValues) const { - ADT pFS(*this); - BOOST_FOREACH(Index key, parents()) - try { - Index j = (key); - size_t value = parentsValues.at(j); - pFS = pFS.choose(j, value); - } catch (exception&) { - throw runtime_error( - "DiscreteConditional::choose: parent value missing"); - }; - return pFS; - } - - /* ******************************************************************************** */ - void DiscreteConditional::solveInPlace(Values& values) const { - // TODO: Abhijit asks: is this really the fastest way? He thinks it is. - - ADT pFS = choose(values); // P(F|S=parentsValues) - - // Initialize - Values mpe; - double maxP = 0; - - DiscreteKeys keys; - BOOST_FOREACH(Index idx, frontals()) { - DiscreteKey dk(idx,cardinality(idx)); - keys & dk; - } - // Get all Possible Configurations - vector allPosbValues = cartesianProduct(keys); - - // Find the MPE - BOOST_FOREACH(Values& frontalVals, allPosbValues) { - double pValueS = pFS(frontalVals); // P(F=value|S=parentsValues) - // Update MPE solution if better - if (pValueS > maxP) { - maxP = pValueS; - mpe = frontalVals; - } - } - - //set values (inPlace) to mpe - BOOST_FOREACH(Index j, frontals()) { - values[j] = mpe[j]; - } - } - - /* ******************************************************************************** */ - void DiscreteConditional::sampleInPlace(Values& values) const { - assert(nrFrontals() == 1); - Index j = (firstFrontalKey()); - size_t sampled = sample(values); // Sample variable - values[j] = sampled; // store result in partial solution - } - - /* ******************************************************************************** */ - size_t DiscreteConditional::solve(const Values& parentsValues) const { - - // TODO: is this really the fastest way? I think it is. - ADT pFS = choose(parentsValues); // P(F|S=parentsValues) - - // Then, find the max over all remaining - // TODO, only works for one key now, seems horribly slow this way - size_t mpe = 0; - Values frontals; - double maxP = 0; - assert(nrFrontals() == 1); - Index j = (firstFrontalKey()); - for (size_t value = 0; value < cardinality(j); value++) { - frontals[j] = value; - double pValueS = pFS(frontals); // P(F=value|S=parentsValues) - // Update MPE solution if better - if (pValueS > maxP) { - maxP = pValueS; - mpe = value; - } - } - return mpe; - } - - /* ******************************************************************************** */ - size_t DiscreteConditional::sample(const Values& parentsValues) const { - - using boost::uniform_real; - static boost::mt19937 gen(2); // random number generator - - bool debug = ISDEBUG("DiscreteConditional::sample"); - - // Get the correct conditional density - ADT pFS = choose(parentsValues); // P(F|S=parentsValues) - if (debug) GTSAM_PRINT(pFS); - - // get cumulative distribution function (cdf) - // TODO, only works for one key now, seems horribly slow this way - assert(nrFrontals() == 1); - Index j = (firstFrontalKey()); - size_t nj = cardinality(j); - vector cdf(nj); - Values frontals; - double sum = 0; - for (size_t value = 0; value < nj; value++) { - frontals[j] = value; - double pValueS = pFS(frontals); // P(F=value|S=parentsValues) - sum += pValueS; // accumulate - if (debug) cout << sum << " "; - if (pValueS == 1) { - if (debug) cout << "--> " << value << endl; - return value; // shortcut exit - } - cdf[value] = sum; - } - - // inspired by http://www.boost.org/doc/libs/1_46_1/doc/html/boost_random/tutorial.html - uniform_real<> dist(0, cdf.back()); - boost::variate_generator > die(gen, dist); - size_t sampled = lower_bound(cdf.begin(), cdf.end(), die()) - cdf.begin(); - if (debug) cout << "-> " << sampled << endl; - - return sampled; - - return 0; - } - - /* ******************************************************************************** */ - void DiscreteConditional::permuteWithInverse(const Permutation& inversePermutation){ - IndexConditional::permuteWithInverse(inversePermutation); - Potentials::permuteWithInverse(inversePermutation); - } - +// Instantiate base class +template class Conditional ; /* ******************************************************************************** */ +DiscreteConditional::DiscreteConditional(const size_t nrFrontals, + const DecisionTreeFactor& f) : + BaseFactor(f / (*f.sum(nrFrontals))), BaseConditional(nrFrontals) { +} -} // namespace +/* ******************************************************************************** */ +DiscreteConditional::DiscreteConditional(const DecisionTreeFactor& joint, + const DecisionTreeFactor& marginal, const boost::optional& orderedKeys) : + BaseFactor( + ISDEBUG("DiscreteConditional::COUNT") ? joint : joint / marginal), BaseConditional( + joint.size()-marginal.size()) { + if (ISDEBUG("DiscreteConditional::DiscreteConditional")) + cout << (firstFrontalKey()) << endl; //TODO Print all keys + if (orderedKeys) { + keys_.clear(); + keys_.insert(keys_.end(), orderedKeys->begin(), orderedKeys->end()); + } +} + +/* ******************************************************************************** */ +DiscreteConditional::DiscreteConditional(const Signature& signature) : + BaseFactor(signature.discreteKeysParentsFirst(), signature.cpt()), BaseConditional( + 1) { +} + +/* ******************************************************************************** */ +void DiscreteConditional::print(const std::string& s, + const KeyFormatter& formatter) const { + std::cout << s << std::endl; + Potentials::print(s); +} + +/* ******************************************************************************** */ +bool DiscreteConditional::equals(const DiscreteFactor& other, + double tol) const { + if (!dynamic_cast(&other)) + return false; + else { + const DecisionTreeFactor& f( + static_cast(other)); + return DecisionTreeFactor::equals(f, tol); + } +} + +/* ******************************************************************************** */ +Potentials::ADT DiscreteConditional::choose(const Values& parentsValues) const { + ADT pFS(*this); + Key j; size_t value; + BOOST_FOREACH(Key key, parents()) + try { + j = (key); + value = parentsValues.at(j); + pFS = pFS.choose(j, value); + } catch (exception&) { + cout << "Key: " << j << " Value: " << value << endl; + parentsValues.print("parentsValues: "); +// pFS.print("pFS: "); + throw runtime_error("DiscreteConditional::choose: parent value missing"); + }; + return pFS; +} + +/* ******************************************************************************** */ +void DiscreteConditional::solveInPlace(Values& values) const { + // TODO: Abhijit asks: is this really the fastest way? He thinks it is. + ADT pFS = choose(values); // P(F|S=parentsValues) + + // Initialize + Values mpe; + double maxP = 0; + + DiscreteKeys keys; + BOOST_FOREACH(Key idx, frontals()) { + DiscreteKey dk(idx, cardinality(idx)); + keys & dk; + } + // Get all Possible Configurations + vector allPosbValues = cartesianProduct(keys); + + // Find the MPE + BOOST_FOREACH(Values& frontalVals, allPosbValues) { + double pValueS = pFS(frontalVals); // P(F=value|S=parentsValues) + // Update MPE solution if better + if (pValueS > maxP) { + maxP = pValueS; + mpe = frontalVals; + } + } + + //set values (inPlace) to mpe + BOOST_FOREACH(Key j, frontals()) { + values[j] = mpe[j]; + } +} + +/* ******************************************************************************** */ +void DiscreteConditional::sampleInPlace(Values& values) const { + assert(nrFrontals() == 1); + Key j = (firstFrontalKey()); + size_t sampled = sample(values); // Sample variable + values[j] = sampled; // store result in partial solution +} + +/* ******************************************************************************** */ +size_t DiscreteConditional::solve(const Values& parentsValues) const { + + // TODO: is this really the fastest way? I think it is. + ADT pFS = choose(parentsValues); // P(F|S=parentsValues) + + // Then, find the max over all remaining + // TODO, only works for one key now, seems horribly slow this way + size_t mpe = 0; + Values frontals; + double maxP = 0; + assert(nrFrontals() == 1); + Key j = (firstFrontalKey()); + for (size_t value = 0; value < cardinality(j); value++) { + frontals[j] = value; + double pValueS = pFS(frontals); // P(F=value|S=parentsValues) + // Update MPE solution if better + if (pValueS > maxP) { + maxP = pValueS; + mpe = value; + } + } + return mpe; +} + +/* ******************************************************************************** */ +size_t DiscreteConditional::sample(const Values& parentsValues) const { + + using boost::uniform_real; + static boost::mt19937 gen(2); // random number generator + + bool debug = ISDEBUG("DiscreteConditional::sample"); + + // Get the correct conditional density + ADT pFS = choose(parentsValues); // P(F|S=parentsValues) + if (debug) + GTSAM_PRINT(pFS); + + // get cumulative distribution function (cdf) + // TODO, only works for one key now, seems horribly slow this way + assert(nrFrontals() == 1); + Key j = (firstFrontalKey()); + size_t nj = cardinality(j); + vector cdf(nj); + Values frontals; + double sum = 0; + for (size_t value = 0; value < nj; value++) { + frontals[j] = value; + double pValueS = pFS(frontals); // P(F=value|S=parentsValues) + sum += pValueS; // accumulate + if (debug) + cout << sum << " "; + if (pValueS == 1) { + if (debug) + cout << "--> " << value << endl; + return value; // shortcut exit + } + cdf[value] = sum; + } + + // inspired by http://www.boost.org/doc/libs/1_46_1/doc/html/boost_random/tutorial.html + uniform_real<> dist(0, cdf.back()); + boost::variate_generator > die(gen, dist); + size_t sampled = lower_bound(cdf.begin(), cdf.end(), die()) - cdf.begin(); + if (debug) + cout << "-> " << sampled << endl; + + return sampled; + + return 0; +} + +/* ******************************************************************************** */ +//void DiscreteConditional::permuteWithInverse( +// const Permutation& inversePermutation) { +// IndexConditionalOrdered::permuteWithInverse(inversePermutation); +// Potentials::permuteWithInverse(inversePermutation); +//} +/* ******************************************************************************** */ + +}// namespace diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h index a14fa9ae7..1ba97444f 100644 --- a/gtsam/discrete/DiscreteConditional.h +++ b/gtsam/discrete/DiscreteConditional.h @@ -20,134 +20,134 @@ #include #include -#include +#include #include #include namespace gtsam { - /** - * Discrete Conditional Density - * Derives from DecisionTreeFactor - */ - class GTSAM_EXPORT DiscreteConditional: public IndexConditional, public Potentials { +/** + * Discrete Conditional Density + * Derives from DecisionTreeFactor + */ +class GTSAM_EXPORT DiscreteConditional: public DecisionTreeFactor, + public Conditional { - public: - // typedefs needed to play nice with gtsam - typedef DiscreteFactor FactorType; - typedef boost::shared_ptr shared_ptr; - typedef IndexConditional Base; +public: + // typedefs needed to play nice with gtsam + typedef DiscreteConditional This; ///< Typedef to this class + typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class + typedef DecisionTreeFactor BaseFactor; ///< Typedef to our factor base class + typedef Conditional BaseConditional; ///< Typedef to our conditional base class - /** A map from keys to values */ - typedef Assignment Values; - typedef boost::shared_ptr sharedValues; + /** A map from keys to values.. + * TODO: Again, do we need this??? */ + typedef Assignment Values; + typedef boost::shared_ptr sharedValues; - /// @name Standard Constructors - /// @{ + /// @name Standard Constructors + /// @{ - /** default constructor needed for serialization */ - DiscreteConditional() { - } - - /** constructor from factor */ - DiscreteConditional(size_t nFrontals, const DecisionTreeFactor& f); - - /** Construct from signature */ - DiscreteConditional(const Signature& signature); - - /** construct P(X|Y)=P(X,Y)/P(Y) from P(X,Y) and P(Y) */ - DiscreteConditional(const DecisionTreeFactor& joint, - const DecisionTreeFactor& marginal); - - /** - * Combine several conditional into a single one. - * The conditionals must be given in increasing order, meaning that the parents - * of any conditional may not include a conditional coming before it. - * @param firstConditional Iterator to the first conditional to combine, must dereference to a shared_ptr. - * @param lastConditional Iterator to after the last conditional to combine, must dereference to a shared_ptr. - * */ - template - static shared_ptr Combine(ITERATOR firstConditional, ITERATOR lastConditional); - - /// @} - /// @name Testable - /// @{ - - /// GTSAM-style print - void print(const std::string& s = "Discrete Conditional: ", - const IndexFormatter& formatter = DefaultIndexFormatter) const; - - /// GTSAM-style equals - bool equals(const DiscreteConditional& other, double tol = 1e-9) const; - - /// @} - /// @name Standard Interface - /// @{ - - /// Evaluate, just look up in AlgebraicDecisonTree - virtual double operator()(const Values& values) const { - return Potentials::operator()(values); - } - - /** Convert to a factor */ - DecisionTreeFactor::shared_ptr toFactor() const { - return DecisionTreeFactor::shared_ptr(new DecisionTreeFactor(*this)); - } - - /** Restrict to given parent values, returns AlgebraicDecisionDiagram */ - ADT choose(const Assignment& parentsValues) const; - - /** - * solve a conditional - * @param parentsValues Known values of the parents - * @return MPE value of the child (1 frontal variable). - */ - size_t solve(const Values& parentsValues) const; - - /** - * sample - * @param parentsValues Known values of the parents - * @return sample from conditional - */ - size_t sample(const Values& parentsValues) const; - - /// @} - /// @name Advanced Interface - /// @{ - - /// solve a conditional, in place - void solveInPlace(Values& parentsValues) const; - - /// sample in place, stores result in partial solution - void sampleInPlace(Values& parentsValues) const; - - /** - * Permutes both IndexConditional and Potentials. - */ - void permuteWithInverse(const Permutation& inversePermutation); - - /// @} - - }; -// DiscreteConditional - - /* ************************************************************************* */ - template - DiscreteConditional::shared_ptr DiscreteConditional::Combine( - ITERATOR firstConditional, ITERATOR lastConditional) { - // TODO: check for being a clique - - // multiply all the potentials of the given conditionals - size_t nrFrontals = 0; - DecisionTreeFactor product; - for(ITERATOR it = firstConditional; it != lastConditional; ++it, ++nrFrontals) { - DiscreteConditional::shared_ptr c = *it; - DecisionTreeFactor::shared_ptr factor = c->toFactor(); - product = (*factor) * product; - } - // and then create a new multi-frontal conditional - return boost::make_shared(nrFrontals,product); + /** default constructor needed for serialization */ + DiscreteConditional() { } -}// gtsam + /** constructor from factor */ + DiscreteConditional(size_t nFrontals, const DecisionTreeFactor& f); + + /** Construct from signature */ + DiscreteConditional(const Signature& signature); + + /** construct P(X|Y)=P(X,Y)/P(Y) from P(X,Y) and P(Y) */ + DiscreteConditional(const DecisionTreeFactor& joint, + const DecisionTreeFactor& marginal, const boost::optional& orderedKeys = boost::none); + + /** + * Combine several conditional into a single one. + * The conditionals must be given in increasing order, meaning that the parents + * of any conditional may not include a conditional coming before it. + * @param firstConditional Iterator to the first conditional to combine, must dereference to a shared_ptr. + * @param lastConditional Iterator to after the last conditional to combine, must dereference to a shared_ptr. + * */ + template + static shared_ptr Combine(ITERATOR firstConditional, + ITERATOR lastConditional); + + /// @} + /// @name Testable + /// @{ + + /// GTSAM-style print + void print(const std::string& s = "Discrete Conditional: ", + const KeyFormatter& formatter = DefaultKeyFormatter) const; + + /// GTSAM-style equals + bool equals(const DiscreteFactor& other, double tol = 1e-9) const; + + /// @} + /// @name Standard Interface + /// @{ + + /// Evaluate, just look up in AlgebraicDecisonTree + virtual double operator()(const Values& values) const { + return Potentials::operator()(values); + } + + /** Convert to a factor */ + DecisionTreeFactor::shared_ptr toFactor() const { + return DecisionTreeFactor::shared_ptr(new DecisionTreeFactor(*this)); + } + + /** Restrict to given parent values, returns AlgebraicDecisionDiagram */ + ADT choose(const Assignment& parentsValues) const; + + /** + * solve a conditional + * @param parentsValues Known values of the parents + * @return MPE value of the child (1 frontal variable). + */ + size_t solve(const Values& parentsValues) const; + + /** + * sample + * @param parentsValues Known values of the parents + * @return sample from conditional + */ + size_t sample(const Values& parentsValues) const; + + /// @} + /// @name Advanced Interface + /// @{ + + /// solve a conditional, in place + void solveInPlace(Values& parentsValues) const; + + /// sample in place, stores result in partial solution + void sampleInPlace(Values& parentsValues) const; + + /// @} + +}; +// DiscreteConditional + +/* ************************************************************************* */ +template +DiscreteConditional::shared_ptr DiscreteConditional::Combine( + ITERATOR firstConditional, ITERATOR lastConditional) { + // TODO: check for being a clique + + // multiply all the potentials of the given conditionals + size_t nrFrontals = 0; + DecisionTreeFactor product; + for (ITERATOR it = firstConditional; it != lastConditional; + ++it, ++nrFrontals) { + DiscreteConditional::shared_ptr c = *it; + DecisionTreeFactor::shared_ptr factor = c->toFactor(); + product = (*factor) * product; + } + // and then create a new multi-frontal conditional + return boost::make_shared(nrFrontals, product); +} + +} // gtsam diff --git a/gtsam/discrete/DiscreteEliminationTree.cpp b/gtsam/discrete/DiscreteEliminationTree.cpp new file mode 100644 index 000000000..df59681c2 --- /dev/null +++ b/gtsam/discrete/DiscreteEliminationTree.cpp @@ -0,0 +1,44 @@ +/* ---------------------------------------------------------------------------- + + * 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 DiscreteEliminationTree.cpp + * @date Mar 29, 2013 + * @author Frank Dellaert + * @author Richard Roberts + */ + +#include +#include + +namespace gtsam { + + // Instantiate base class + template class EliminationTree; + + /* ************************************************************************* */ + DiscreteEliminationTree::DiscreteEliminationTree( + const DiscreteFactorGraph& factorGraph, const VariableIndex& structure, + const Ordering& order) : + Base(factorGraph, structure, order) {} + + /* ************************************************************************* */ + DiscreteEliminationTree::DiscreteEliminationTree( + const DiscreteFactorGraph& factorGraph, const Ordering& order) : + Base(factorGraph, order) {} + + /* ************************************************************************* */ + bool DiscreteEliminationTree::equals(const This& other, double tol) const + { + return Base::equals(other, tol); + } + +} diff --git a/gtsam/discrete/DiscreteEliminationTree.h b/gtsam/discrete/DiscreteEliminationTree.h new file mode 100644 index 000000000..1ba33006e --- /dev/null +++ b/gtsam/discrete/DiscreteEliminationTree.h @@ -0,0 +1,63 @@ +/* ---------------------------------------------------------------------------- + + * 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 DiscreteEliminationTree.h + * @date Mar 29, 2013 + * @author Frank Dellaert + * @author Richard Roberts + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + + class GTSAM_EXPORT DiscreteEliminationTree : + public EliminationTree + { + public: + typedef EliminationTree Base; ///< Base class + typedef DiscreteEliminationTree This; ///< This class + typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class + + /** + * Build the elimination tree of a factor graph using pre-computed column structure. + * @param factorGraph The factor graph for which to build the elimination tree + * @param structure The set of factors involving each variable. If this is not + * precomputed, you can call the Create(const FactorGraph&) + * named constructor instead. + * @return The elimination tree + */ + DiscreteEliminationTree(const DiscreteFactorGraph& factorGraph, + const VariableIndex& structure, const Ordering& order); + + /** Build the elimination tree of a factor graph. Note that this has to compute the column + * structure as a VariableIndex, so if you already have this precomputed, use the other + * constructor instead. + * @param factorGraph The factor graph for which to build the elimination tree + */ + DiscreteEliminationTree(const DiscreteFactorGraph& factorGraph, + const Ordering& order); + + /** Test whether the tree is equal to another */ + bool equals(const This& other, double tol = 1e-9) const; + + private: + + friend class ::EliminationTreeTester; + + }; + +} diff --git a/gtsam/discrete/DiscreteFactor.cpp b/gtsam/discrete/DiscreteFactor.cpp index 1d53f1afd..e9f57c30d 100644 --- a/gtsam/discrete/DiscreteFactor.cpp +++ b/gtsam/discrete/DiscreteFactor.cpp @@ -24,9 +24,5 @@ using namespace std; namespace gtsam { - /* ******************************************************************************** */ - DiscreteFactor::DiscreteFactor() { - } - /* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h index 6adb1278b..8351b310b 100644 --- a/gtsam/discrete/DiscreteFactor.h +++ b/gtsam/discrete/DiscreteFactor.h @@ -19,104 +19,86 @@ #pragma once #include -#include +#include namespace gtsam { - class DecisionTreeFactor; - class DiscreteConditional; +class DecisionTreeFactor; +class DiscreteConditional; - /** - * Base class for discrete probabilistic factors - * The most general one is the derived DecisionTreeFactor +/** + * Base class for discrete probabilistic factors + * The most general one is the derived DecisionTreeFactor + */ +class GTSAM_EXPORT DiscreteFactor: public Factor { + +public: + + // typedefs needed to play nice with gtsam + typedef DiscreteFactor This; ///< This class + typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class + typedef Factor Base; ///< Our base class + + /** A map from keys to values + * TODO: Do we need this? Should we just use gtsam::Values? + * We just need another special DiscreteValue to represent labels, + * However, all other Lie's operators are undefined in this class. + * The good thing is we can have a Hybrid graph of discrete/continuous variables + * together.. + * Another good thing is we don't need to have the special DiscreteKey which stores + * cardinality of a Discrete variable. It should be handled naturally in + * the new class DiscreteValue, as the varible's type (domain) */ - class GTSAM_EXPORT DiscreteFactor: public IndexFactor { + typedef Assignment Values; + typedef boost::shared_ptr sharedValues; - public: +public: - // typedefs needed to play nice with gtsam - typedef DiscreteFactor This; - typedef DiscreteConditional ConditionalType; - typedef boost::shared_ptr shared_ptr; + /// @name Standard Constructors + /// @{ - /** A map from keys to values */ - typedef Assignment Values; - typedef boost::shared_ptr sharedValues; + /** Default constructor creates empty factor */ + DiscreteFactor() {} - protected: + /** Construct from container of keys. This constructor is used internally from derived factor + * constructors, either from a container of keys or from a boost::assign::list_of. */ + template + DiscreteFactor(const CONTAINER& keys) : Base(keys) {} - /// Construct n-way factor - DiscreteFactor(const std::vector& js) : - IndexFactor(js) { - } + /// Virtual destructor + virtual ~DiscreteFactor() { + } - /// Construct unary factor - DiscreteFactor(Index j) : - IndexFactor(j) { - } + /// @} + /// @name Testable + /// @{ - /// Construct binary factor - DiscreteFactor(Index j1, Index j2) : - IndexFactor(j1, j2) { - } + // equals + virtual bool equals(const DiscreteFactor& lf, double tol = 1e-9) const = 0; - /// construct from container - template - DiscreteFactor(KeyIterator beginKey, KeyIterator endKey) : - IndexFactor(beginKey, endKey) { - } + // print + virtual void print(const std::string& s = "DiscreteFactor\n", + const KeyFormatter& formatter = DefaultKeyFormatter) const { + Factor::print(s, formatter); + } - public: + /** Test whether the factor is empty */ + virtual bool empty() const { return size() == 0; } - /// @name Standard Constructors - /// @{ + /// @} + /// @name Standard Interface + /// @{ - /// Default constructor for I/O - DiscreteFactor(); + /// Find value for given assignment of values to variables + virtual double operator()(const Values&) const = 0; - /// Virtual destructor - virtual ~DiscreteFactor() {} + /// Multiply in a DecisionTreeFactor and return the result as DecisionTreeFactor + virtual DecisionTreeFactor operator*(const DecisionTreeFactor&) const = 0; - /// @} - /// @name Testable - /// @{ + virtual DecisionTreeFactor toDecisionTreeFactor() const = 0; - // print - virtual void print(const std::string& s = "DiscreteFactor\n", - const IndexFormatter& formatter - =DefaultIndexFormatter) const { - IndexFactor::print(s,formatter); - } - - /// @} - /// @name Standard Interface - /// @{ - - /// Find value for given assignment of values to variables - virtual double operator()(const Values&) const = 0; - - /// Multiply in a DecisionTreeFactor and return the result as DecisionTreeFactor - virtual DecisionTreeFactor operator*(const DecisionTreeFactor&) const = 0; - - virtual DecisionTreeFactor toDecisionTreeFactor() const = 0; - - /** - * Permutes the factor, but for efficiency requires the permutation - * to already be inverted. - */ - virtual void permuteWithInverse(const Permutation& inversePermutation){ - IndexFactor::permuteWithInverse(inversePermutation); - } - - /** - * Apply a reduction, which is a remapping of variable indices. - */ - virtual void reduceWithInverse(const internal::Reduction& inverseReduction) { - IndexFactor::reduceWithInverse(inverseReduction); - } - - /// @} - }; + /// @} +}; // DiscreteFactor }// namespace gtsam diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp index 5c4d815fb..77be1d277 100644 --- a/gtsam/discrete/DiscreteFactorGraph.cpp +++ b/gtsam/discrete/DiscreteFactorGraph.cpp @@ -1,124 +1,134 @@ -/* ---------------------------------------------------------------------------- - - * 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 DiscreteFactorGraph.cpp - * @date Feb 14, 2011 - * @author Duy-Nguyen Ta - * @author Frank Dellaert - */ - -//#define ENABLE_TIMING -#include -#include -#include -#include - -namespace gtsam { - - // Explicitly instantiate so we don't have to include everywhere - template class FactorGraph ; - template class EliminationTree ; - - /* ************************************************************************* */ - DiscreteFactorGraph::DiscreteFactorGraph() { - } - - /* ************************************************************************* */ - DiscreteFactorGraph::DiscreteFactorGraph( - const BayesNet& bayesNet) : - FactorGraph(bayesNet) { - } - - /* ************************************************************************* */ - FastSet DiscreteFactorGraph::keys() const { - FastSet keys; - BOOST_FOREACH(const sharedFactor& factor, *this) - if (factor) keys.insert(factor->begin(), factor->end()); - return keys; - } - - /* ************************************************************************* */ - DecisionTreeFactor DiscreteFactorGraph::product() const { - DecisionTreeFactor result; - BOOST_FOREACH(const sharedFactor& factor, *this) - if (factor) result = (*factor) * result; - return result; - } - - /* ************************************************************************* */ - double DiscreteFactorGraph::operator()( - const DiscreteFactor::Values &values) const { - double product = 1.0; - BOOST_FOREACH( const sharedFactor& factor, factors_ ) - product *= (*factor)(values); - return product; - } - - /* ************************************************************************* */ - void DiscreteFactorGraph::print(const std::string& s, - const IndexFormatter& formatter) const { - std::cout << s << std::endl; - std::cout << "size: " << size() << std::endl; - for (size_t i = 0; i < factors_.size(); i++) { - std::stringstream ss; - ss << "factor " << i << ": "; - if (factors_[i] != NULL) factors_[i]->print(ss.str(), formatter); - } - } - - /* ************************************************************************* */ - void DiscreteFactorGraph::permuteWithInverse( - const Permutation& inversePermutation) { - BOOST_FOREACH(const sharedFactor& factor, factors_) { - if(factor) - factor->permuteWithInverse(inversePermutation); - } - } - - /* ************************************************************************* */ - void DiscreteFactorGraph::reduceWithInverse( - const internal::Reduction& inverseReduction) { - BOOST_FOREACH(const sharedFactor& factor, factors_) { - if(factor) - factor->reduceWithInverse(inverseReduction); - } - } - - /* ************************************************************************* */ - std::pair // - EliminateDiscrete(const FactorGraph& factors, size_t num) { - - // PRODUCT: multiply all factors - gttic(product); - DecisionTreeFactor product; - BOOST_FOREACH(const DiscreteFactor::shared_ptr& factor, factors) - product = (*factor) * product; - - gttoc(product); - - // sum out frontals, this is the factor on the separator - gttic(sum); - DecisionTreeFactor::shared_ptr sum = product.sum(num); - gttoc(sum); - - // now divide product/sum to get conditional - gttic(divide); - DiscreteConditional::shared_ptr cond(new DiscreteConditional(product, *sum)); - gttoc(divide); - - return std::make_pair(cond, sum); - } - - -/* ************************************************************************* */ -} // namespace - +/* ---------------------------------------------------------------------------- + + * 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 DiscreteFactorGraph.cpp + * @date Feb 14, 2011 + * @author Duy-Nguyen Ta + * @author Frank Dellaert + */ + +//#define ENABLE_TIMING +#include +#include +#include +#include +#include +#include +#include +#include + +namespace gtsam { + + // Instantiate base classes + template class FactorGraph; + template class EliminateableFactorGraph; + + /* ************************************************************************* */ + bool DiscreteFactorGraph::equals(const This& fg, double tol) const + { + return Base::equals(fg, tol); + } + + /* ************************************************************************* */ + FastSet DiscreteFactorGraph::keys() const { + FastSet keys; + BOOST_FOREACH(const sharedFactor& factor, *this) + if (factor) keys.insert(factor->begin(), factor->end()); + return keys; + } + + /* ************************************************************************* */ + DecisionTreeFactor DiscreteFactorGraph::product() const { + DecisionTreeFactor result; + BOOST_FOREACH(const sharedFactor& factor, *this) + if (factor) result = (*factor) * result; + return result; + } + + /* ************************************************************************* */ + double DiscreteFactorGraph::operator()( + const DiscreteFactor::Values &values) const { + double product = 1.0; + BOOST_FOREACH( const sharedFactor& factor, factors_ ) + product *= (*factor)(values); + return product; + } + + /* ************************************************************************* */ + void DiscreteFactorGraph::print(const std::string& s, + const KeyFormatter& formatter) const { + std::cout << s << std::endl; + std::cout << "size: " << size() << std::endl; + for (size_t i = 0; i < factors_.size(); i++) { + std::stringstream ss; + ss << "factor " << i << ": "; + if (factors_[i] != NULL) factors_[i]->print(ss.str(), formatter); + } + } + +// /* ************************************************************************* */ +// void DiscreteFactorGraph::permuteWithInverse( +// const Permutation& inversePermutation) { +// BOOST_FOREACH(const sharedFactor& factor, factors_) { +// if(factor) +// factor->permuteWithInverse(inversePermutation); +// } +// } +// +// /* ************************************************************************* */ +// void DiscreteFactorGraph::reduceWithInverse( +// const internal::Reduction& inverseReduction) { +// BOOST_FOREACH(const sharedFactor& factor, factors_) { +// if(factor) +// factor->reduceWithInverse(inverseReduction); +// } +// } + + /* ************************************************************************* */ + DiscreteFactor::sharedValues DiscreteFactorGraph::optimize() const + { + gttic(DiscreteFactorGraph_optimize); + return BaseEliminateable::eliminateSequential()->optimize(); + } + + /* ************************************************************************* */ + std::pair // + EliminateDiscrete(const DiscreteFactorGraph& factors, const Ordering& frontalKeys) { + + // PRODUCT: multiply all factors + gttic(product); + DecisionTreeFactor product; + BOOST_FOREACH(const DiscreteFactor::shared_ptr& factor, factors) + product = (*factor) * product; + gttoc(product); + + // sum out frontals, this is the factor on the separator + gttic(sum); + DecisionTreeFactor::shared_ptr sum = product.sum(frontalKeys); + gttoc(sum); + + // Ordering keys for the conditional so that frontalKeys are really in front + Ordering orderedKeys; + orderedKeys.insert(orderedKeys.end(), frontalKeys.begin(), frontalKeys.end()); + orderedKeys.insert(orderedKeys.end(), sum->keys().begin(), sum->keys().end()); + + // now divide product/sum to get conditional + gttic(divide); + DiscreteConditional::shared_ptr cond(new DiscreteConditional(product, *sum, orderedKeys)); + gttoc(divide); + + return std::make_pair(cond, sum); + } + +/* ************************************************************************* */ +} // namespace + diff --git a/gtsam/discrete/DiscreteFactorGraph.h b/gtsam/discrete/DiscreteFactorGraph.h index 7078662f7..27eb722d9 100644 --- a/gtsam/discrete/DiscreteFactorGraph.h +++ b/gtsam/discrete/DiscreteFactorGraph.h @@ -18,33 +18,85 @@ #pragma once +#include +#include +#include #include #include -#include #include #include namespace gtsam { -class DiscreteFactorGraph: public FactorGraph { +// Forward declarations +class DiscreteFactorGraph; +class DiscreteFactor; +class DiscreteConditional; +class DiscreteBayesNet; +class DiscreteEliminationTree; +class DiscreteBayesTree; +class DiscreteJunctionTree; + +/** Main elimination function for DiscreteFactorGraph */ +GTSAM_EXPORT std::pair, DecisionTreeFactor::shared_ptr> +EliminateDiscrete(const DiscreteFactorGraph& factors, const Ordering& keys); + +/* ************************************************************************* */ +template<> struct EliminationTraits +{ + typedef DiscreteFactor FactorType; ///< Type of factors in factor graph + typedef DiscreteFactorGraph FactorGraphType; ///< Type of the factor graph (e.g. DiscreteFactorGraph) + typedef DiscreteConditional ConditionalType; ///< Type of conditionals from elimination + typedef DiscreteBayesNet BayesNetType; ///< Type of Bayes net from sequential elimination + typedef DiscreteEliminationTree EliminationTreeType; ///< Type of elimination tree + typedef DiscreteBayesTree BayesTreeType; ///< Type of Bayes tree + typedef DiscreteJunctionTree JunctionTreeType; ///< Type of Junction tree + /// The default dense elimination function + static std::pair, boost::shared_ptr > + DefaultEliminate(const FactorGraphType& factors, const Ordering& keys) { + return EliminateDiscrete(factors, keys); } +}; + +/* ************************************************************************* */ +/** + * A Discrete Factor Graph is a factor graph where all factors are Discrete, i.e. + * Factor == DiscreteFactor + */ +class GTSAM_EXPORT DiscreteFactorGraph: public FactorGraph, +public EliminateableFactorGraph { public: + typedef DiscreteFactorGraph This; ///< Typedef to this class + typedef FactorGraph Base; ///< Typedef to base factor graph type + typedef EliminateableFactorGraph BaseEliminateable; ///< Typedef to base elimination class + typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class + /** A map from keys to values */ - typedef std::vector Indices; - typedef Assignment Values; + typedef std::vector Indices; + typedef Assignment Values; typedef boost::shared_ptr sharedValues; - /** Construct empty factor graph */ - GTSAM_EXPORT DiscreteFactorGraph(); + /** Default constructor */ + DiscreteFactorGraph() {} - /** Constructor from a factor graph of GaussianFactor or a derived type */ + /** Construct from iterator over factors */ + template + DiscreteFactorGraph(ITERATOR firstFactor, ITERATOR lastFactor) : Base(firstFactor, lastFactor) {} + + /** Construct from container of factors (shared_ptr or plain objects) */ + template + explicit DiscreteFactorGraph(const CONTAINER& factors) : Base(factors) {} + + /** Implicit copy/downcast constructor to override explicit template container constructor */ template - DiscreteFactorGraph(const FactorGraph& fg) { - push_back(fg); - } + DiscreteFactorGraph(const FactorGraph& graph) : Base(graph) {} - /** construct from a BayesNet */ - GTSAM_EXPORT DiscreteFactorGraph(const BayesNet& bayesNet); + /// @name Testable + /// @{ + + bool equals(const This& fg, double tol = 1e-9) const; + + /// @} template void add(const DiscreteKey& j, SOURCE table) { @@ -68,29 +120,31 @@ public: } /** Return the set of variables involved in the factors (set union) */ - GTSAM_EXPORT FastSet keys() const; + FastSet keys() const; /** return product of all factors as a single factor */ - GTSAM_EXPORT DecisionTreeFactor product() const; + DecisionTreeFactor product() const; /** Evaluates the factor graph given values, returns the joint probability of the factor graph given specific instantiation of values*/ - GTSAM_EXPORT double operator()(const DiscreteFactor::Values & values) const; + double operator()(const DiscreteFactor::Values & values) const; /// print - GTSAM_EXPORT void print(const std::string& s = "DiscreteFactorGraph", - const IndexFormatter& formatter =DefaultIndexFormatter) const; - - /** Permute the variables in the factors */ - GTSAM_EXPORT void permuteWithInverse(const Permutation& inversePermutation); - - /** Apply a reduction, which is a remapping of variable indices. */ - GTSAM_EXPORT void reduceWithInverse(const internal::Reduction& inverseReduction); + void print(const std::string& s = "DiscreteFactorGraph", + const KeyFormatter& formatter =DefaultKeyFormatter) const; + + /** Solve the factor graph by performing variable elimination in COLAMD order using + * the dense elimination function specified in \c function, + * followed by back-substitution resulting from elimination. Is equivalent + * to calling graph.eliminateSequential()->optimize(). */ + DiscreteFactor::sharedValues optimize() const; + + +// /** Permute the variables in the factors */ +// GTSAM_EXPORT void permuteWithInverse(const Permutation& inversePermutation); +// +// /** Apply a reduction, which is a remapping of variable indices. */ +// GTSAM_EXPORT void reduceWithInverse(const internal::Reduction& inverseReduction); }; // DiscreteFactorGraph -/** Main elimination function for DiscreteFactorGraph */ -GTSAM_EXPORT std::pair, DecisionTreeFactor::shared_ptr> -EliminateDiscrete(const FactorGraph& factors, - size_t nrFrontals = 1); - } // namespace gtsam diff --git a/gtsam/discrete/DiscreteJunctionTree.cpp b/gtsam/discrete/DiscreteJunctionTree.cpp new file mode 100644 index 000000000..8e6d0f4d8 --- /dev/null +++ b/gtsam/discrete/DiscreteJunctionTree.cpp @@ -0,0 +1,34 @@ +/* ---------------------------------------------------------------------------- + + * 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 DiscreteJunctionTree.cpp + * @date Mar 29, 2013 + * @author Frank Dellaert + * @author Richard Roberts + */ + +#include +#include +#include + +namespace gtsam { + + // Instantiate base classes + template class ClusterTree; + template class JunctionTree; + + /* ************************************************************************* */ + DiscreteJunctionTree::DiscreteJunctionTree( + const DiscreteEliminationTree& eliminationTree) : + Base(eliminationTree) {} + +} diff --git a/gtsam/discrete/DiscreteJunctionTree.h b/gtsam/discrete/DiscreteJunctionTree.h new file mode 100644 index 000000000..23924acfd --- /dev/null +++ b/gtsam/discrete/DiscreteJunctionTree.h @@ -0,0 +1,66 @@ +/* ---------------------------------------------------------------------------- + + * 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 DiscreteJunctionTree.h + * @date Mar 29, 2013 + * @author Frank Dellaert + * @author Richard Roberts + */ + +#include +#include +#include + +namespace gtsam { + + // Forward declarations + class DiscreteEliminationTree; + + /** + * A ClusterTree, i.e., a set of variable clusters with factors, arranged in a tree, with + * the additional property that it represents the clique tree associated with a Bayes net. + * + * In GTSAM a junction tree is an intermediate data structure in multifrontal + * variable elimination. Each node is a cluster of factors, along with a + * clique of variables that are eliminated all at once. In detail, every node k represents + * a clique (maximal fully connected subset) of an associated chordal graph, such as a + * chordal Bayes net resulting from elimination. + * + * The difference with the BayesTree is that a JunctionTree stores factors, whereas a + * BayesTree stores conditionals, that are the product of eliminating the factors in the + * corresponding JunctionTree cliques. + * + * The tree structure and elimination method are exactly analagous to the EliminationTree, + * except that in the JunctionTree, at each node multiple variables are eliminated at a time. + * + * \addtogroup Multifrontal + * \nosubgrouping + */ + class GTSAM_EXPORT DiscreteJunctionTree : + public JunctionTree { + public: + typedef JunctionTree Base; ///< Base class + typedef DiscreteJunctionTree This; ///< This class + typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class + + /** + * Build the elimination tree of a factor graph using pre-computed column structure. + * @param factorGraph The factor graph for which to build the elimination tree + * @param structure The set of factors involving each variable. If this is not + * precomputed, you can call the Create(const FactorGraph&) + * named constructor instead. + * @return The elimination tree + */ + DiscreteJunctionTree(const DiscreteEliminationTree& eliminationTree); + }; + +} diff --git a/gtsam/discrete/DiscreteKey.cpp b/gtsam/discrete/DiscreteKey.cpp index df9f5c3d9..828a93eec 100644 --- a/gtsam/discrete/DiscreteKey.cpp +++ b/gtsam/discrete/DiscreteKey.cpp @@ -32,15 +32,15 @@ namespace gtsam { } } - vector DiscreteKeys::indices() const { - vector < Index > js; + vector DiscreteKeys::indices() const { + vector < Key > js; BOOST_FOREACH(const DiscreteKey& key, *this) js.push_back(key.first); return js; } - map DiscreteKeys::cardinalities() const { - map cs; + map DiscreteKeys::cardinalities() const { + map cs; cs.insert(begin(),end()); // BOOST_FOREACH(const DiscreteKey& key, *this) // cs.insert(key); diff --git a/gtsam/discrete/DiscreteKey.h b/gtsam/discrete/DiscreteKey.h index 6451886c3..bc2bd862d 100644 --- a/gtsam/discrete/DiscreteKey.h +++ b/gtsam/discrete/DiscreteKey.h @@ -30,7 +30,7 @@ namespace gtsam { * Key type for discrete conditionals * Includes name and cardinality */ - typedef std::pair DiscreteKey; + typedef std::pair DiscreteKey; /// DiscreteKeys is a set of keys that can be assembled using the & operator struct DiscreteKeys: public std::vector { @@ -53,10 +53,10 @@ namespace gtsam { GTSAM_EXPORT DiscreteKeys(const std::vector& cs); /// Return a vector of indices - GTSAM_EXPORT std::vector indices() const; + GTSAM_EXPORT std::vector indices() const; /// Return a map from index to cardinality - GTSAM_EXPORT std::map cardinalities() const; + GTSAM_EXPORT std::map cardinalities() const; /// Add a key (non-const!) DiscreteKeys& operator&(const DiscreteKey& key) { diff --git a/gtsam/discrete/DiscreteMarginals.h b/gtsam/discrete/DiscreteMarginals.h index 411e4a4dd..2158973a9 100644 --- a/gtsam/discrete/DiscreteMarginals.h +++ b/gtsam/discrete/DiscreteMarginals.h @@ -21,7 +21,8 @@ #pragma once #include -#include +#include +#include namespace gtsam { @@ -32,7 +33,7 @@ namespace gtsam { protected: - BayesTree bayesTree_; + DiscreteBayesTree::shared_ptr bayesTree_; public: @@ -40,16 +41,14 @@ namespace gtsam { * @param graph The factor graph defining the full joint density on all variables. */ DiscreteMarginals(const DiscreteFactorGraph& graph) { - typedef JunctionTree DiscreteJT; - GenericMultifrontalSolver solver(graph); - bayesTree_ = *solver.eliminate(&EliminateDiscrete); + bayesTree_ = graph.eliminateMultifrontal(); } /** Compute the marginal of a single variable */ - DiscreteFactor::shared_ptr operator()(Index variable) const { + DiscreteFactor::shared_ptr operator()(Key variable) const { // Compute marginal DiscreteFactor::shared_ptr marginalFactor; - marginalFactor = bayesTree_.marginalFactor(variable, &EliminateDiscrete); + marginalFactor = bayesTree_->marginalFactor(variable, &EliminateDiscrete); return marginalFactor; } @@ -60,7 +59,7 @@ namespace gtsam { Vector marginalProbabilities(const DiscreteKey& key) const { // Compute marginal DiscreteFactor::shared_ptr marginalFactor; - marginalFactor = bayesTree_.marginalFactor(key.first, &EliminateDiscrete); + marginalFactor = bayesTree_->marginalFactor(key.first, &EliminateDiscrete); //Create result Vector vResult(key.second); diff --git a/gtsam/discrete/DiscreteSequentialSolver.cpp b/gtsam/discrete/DiscreteSequentialSolver.cpp deleted file mode 100644 index 6aeab58e3..000000000 --- a/gtsam/discrete/DiscreteSequentialSolver.cpp +++ /dev/null @@ -1,75 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 DiscreteSequentialSolver.cpp - * @date Feb 16, 2011 - * @author Duy-Nguyen Ta - * @author Frank Dellaert - */ - -//#define ENABLE_TIMING -#include -#include -#include - -namespace gtsam { - - template class GenericSequentialSolver ; - - /* ************************************************************************* */ - DiscreteFactor::sharedValues DiscreteSequentialSolver::optimize() const { - - static const bool debug = false; - - if (debug) this->factors_->print("DiscreteSequentialSolver, eliminating "); - if (debug) this->eliminationTree_->print( - "DiscreteSequentialSolver, elimination tree "); - - // Eliminate using the elimination tree - gttic(eliminate); - DiscreteBayesNet::shared_ptr bayesNet = eliminate(); - gttoc(eliminate); - - if (debug) bayesNet->print("DiscreteSequentialSolver, Bayes net "); - - // Allocate the solution vector if it is not already allocated - - // Back-substitute - gttic(optimize); - DiscreteFactor::sharedValues solution = gtsam::optimize(*bayesNet); - gttoc(optimize); - - if (debug) solution->print("DiscreteSequentialSolver, solution "); - - return solution; - } - - /* ************************************************************************* */ - Vector DiscreteSequentialSolver::marginalProbabilities( - const DiscreteKey& key) const { - // Compute marginal - DiscreteFactor::shared_ptr marginalFactor; - marginalFactor = Base::marginalFactor(key.first, &EliminateDiscrete); - - //Create result - Vector vResult(key.second); - for (size_t state = 0; state < key.second; ++state) { - DiscreteFactor::Values values; - values[key.first] = state; - vResult(state) = (*marginalFactor)(values); - } - return vResult; - } - -/* ************************************************************************* */ - -} diff --git a/gtsam/discrete/DiscreteSequentialSolver.h b/gtsam/discrete/DiscreteSequentialSolver.h deleted file mode 100644 index 36cf25545..000000000 --- a/gtsam/discrete/DiscreteSequentialSolver.h +++ /dev/null @@ -1,113 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 DiscreteSequentialSolver.h - * @date Feb 16, 2011 - * @author Duy-Nguyen Ta - * @author Frank Dellaert - */ - -#pragma once - -#include -#include -#include - -namespace gtsam { - // The base class provides all of the needed functionality - - class DiscreteSequentialSolver: public GenericSequentialSolver { - - protected: - typedef GenericSequentialSolver Base; - typedef boost::shared_ptr shared_ptr; - - public: - - /** - * The problem we are trying to solve (SUM or MPE). - */ - typedef enum { - BEL, // Belief updating (or conditional updating) - MPE, // Most-Probable-Explanation - MAP - // Maximum A Posteriori hypothesis - } ProblemType; - - /** - * Construct the solver for a factor graph. This builds the elimination - * tree, which already does some of the work of elimination. - */ - DiscreteSequentialSolver(const FactorGraph& factorGraph) : - Base(factorGraph) { - } - - /** - * Construct the solver with a shared pointer to a factor graph and to a - * VariableIndex. The solver will store these pointers, so this constructor - * is the fastest. - */ - DiscreteSequentialSolver( - const FactorGraph::shared_ptr& factorGraph, - const VariableIndex::shared_ptr& variableIndex) : - Base(factorGraph, variableIndex) { - } - - const EliminationTree& eliminationTree() const { - return *eliminationTree_; - } - - /** - * Eliminate the factor graph sequentially. Uses a column elimination tree - * to recursively eliminate. - */ - BayesNet::shared_ptr eliminate() const { - return Base::eliminate(&EliminateDiscrete); - } - - /** - * Compute the marginal joint over a set of variables, by integrating out - * all of the other variables. This function returns the result as a factor - * graph. - */ - DiscreteFactorGraph::shared_ptr jointFactorGraph( - const std::vector& js) const { - DiscreteFactorGraph::shared_ptr results(new DiscreteFactorGraph( - *Base::jointFactorGraph(js, &EliminateDiscrete))); - return results; - } - - /** - * Compute the marginal density over a variable, by integrating out - * all of the other variables. This function returns the result as a factor. - */ - DiscreteFactor::shared_ptr marginalFactor(Index j) const { - return Base::marginalFactor(j, &EliminateDiscrete); - } - - /** - * Compute the marginal density over a variable, by integrating out - * all of the other variables. This function returns the result as a - * Vector of the probability values. - */ - GTSAM_EXPORT Vector marginalProbabilities(const DiscreteKey& key) const; - - /** - * Compute the MPE solution of the DiscreteFactorGraph. This - * eliminates to create a BayesNet and then back-substitutes this BayesNet to - * obtain the solution. - */ - GTSAM_EXPORT DiscreteFactor::sharedValues optimize() const; - - }; - -} // gtsam diff --git a/gtsam/discrete/Potentials.cpp b/gtsam/discrete/Potentials.cpp index c8fb3a11d..11dddee66 100644 --- a/gtsam/discrete/Potentials.cpp +++ b/gtsam/discrete/Potentials.cpp @@ -24,8 +24,8 @@ using namespace std; namespace gtsam { // explicit instantiation - template class DecisionTree ; - template class AlgebraicDecisionTree ; + template class DecisionTree ; + template class AlgebraicDecisionTree ; /* ************************************************************************* */ double Potentials::safe_div(const double& a, const double& b) { @@ -52,46 +52,46 @@ namespace gtsam { /* ************************************************************************* */ void Potentials::print(const string& s, - const IndexFormatter& formatter) const { + const KeyFormatter& formatter) const { cout << s << "\n Cardinalities: "; BOOST_FOREACH(const DiscreteKey& key, cardinalities_) cout << formatter(key.first) << "=" << formatter(key.second) << " "; cout << endl; ADT::print(" "); } - - /* ************************************************************************* */ - template - void Potentials::remapIndices(const P& remapping) { - // Permute the _cardinalities (TODO: Inefficient Consider Improving) - DiscreteKeys keys; - map ordering; - - // Get the original keys from cardinalities_ - BOOST_FOREACH(const DiscreteKey& key, cardinalities_) - keys & key; - - // Perform Permutation - BOOST_FOREACH(DiscreteKey& key, keys) { - ordering[key.first] = remapping[key.first]; - key.first = ordering[key.first]; - } - - // Change *this - AlgebraicDecisionTree permuted((*this), ordering); - *this = permuted; - cardinalities_ = keys.cardinalities(); - } - - /* ************************************************************************* */ - void Potentials::permuteWithInverse(const Permutation& inversePermutation) { - remapIndices(inversePermutation); - } - - /* ************************************************************************* */ - void Potentials::reduceWithInverse(const internal::Reduction& inverseReduction) { - remapIndices(inverseReduction); - } +// +// /* ************************************************************************* */ +// template +// void Potentials::remapIndices(const P& remapping) { +// // Permute the _cardinalities (TODO: Inefficient Consider Improving) +// DiscreteKeys keys; +// map ordering; +// +// // Get the original keys from cardinalities_ +// BOOST_FOREACH(const DiscreteKey& key, cardinalities_) +// keys & key; +// +// // Perform Permutation +// BOOST_FOREACH(DiscreteKey& key, keys) { +// ordering[key.first] = remapping[key.first]; +// key.first = ordering[key.first]; +// } +// +// // Change *this +// AlgebraicDecisionTree permuted((*this), ordering); +// *this = permuted; +// cardinalities_ = keys.cardinalities(); +// } +// +// /* ************************************************************************* */ +// void Potentials::permuteWithInverse(const Permutation& inversePermutation) { +// remapIndices(inversePermutation); +// } +// +// /* ************************************************************************* */ +// void Potentials::reduceWithInverse(const internal::Reduction& inverseReduction) { +// remapIndices(inverseReduction); +// } /* ************************************************************************* */ diff --git a/gtsam/discrete/Potentials.h b/gtsam/discrete/Potentials.h index 33e266c91..51475e07d 100644 --- a/gtsam/discrete/Potentials.h +++ b/gtsam/discrete/Potentials.h @@ -19,7 +19,6 @@ #include #include -#include #include #include @@ -29,16 +28,16 @@ namespace gtsam { /** * A base class for both DiscreteFactor and DiscreteConditional */ - class Potentials: public AlgebraicDecisionTree { + class Potentials: public AlgebraicDecisionTree { public: - typedef AlgebraicDecisionTree ADT; + typedef AlgebraicDecisionTree ADT; protected: /// Cardinality for each key, used in combine - std::map cardinalities_; + std::map cardinalities_; /** Constructor from ColumnIndex, and ADT */ Potentials(const ADT& potentials) : @@ -48,9 +47,9 @@ namespace gtsam { // Safe division for probabilities GTSAM_EXPORT static double safe_div(const double& a, const double& b); - // Apply either a permutation or a reduction - template - void remapIndices(const P& remapping); +// // Apply either a permutation or a reduction +// template +// void remapIndices(const P& remapping); public: @@ -69,23 +68,23 @@ namespace gtsam { // Testable GTSAM_EXPORT bool equals(const Potentials& other, double tol = 1e-9) const; GTSAM_EXPORT void print(const std::string& s = "Potentials: ", - const IndexFormatter& formatter = DefaultIndexFormatter) const; + const KeyFormatter& formatter = DefaultKeyFormatter) const; - size_t cardinality(Index j) const { return cardinalities_.at(j);} + size_t cardinality(Key j) const { return cardinalities_.at(j);} - /** - * @brief Permutes the keys in Potentials - * - * This permutes the Indices and performs necessary re-ordering of ADD. - * This is virtual so that derived types e.g. DecisionTreeFactor can - * re-implement it. - */ - GTSAM_EXPORT virtual void permuteWithInverse(const Permutation& inversePermutation); - - /** - * Apply a reduction, which is a remapping of variable indices. - */ - GTSAM_EXPORT virtual void reduceWithInverse(const internal::Reduction& inverseReduction); +// /** +// * @brief Permutes the keys in Potentials +// * +// * This permutes the Indices and performs necessary re-ordering of ADD. +// * This is virtual so that derived types e.g. DecisionTreeFactor can +// * re-implement it. +// */ +// GTSAM_EXPORT virtual void permuteWithInverse(const Permutation& inversePermutation); +// +// /** +// * Apply a reduction, which is a remapping of variable indices. +// */ +// GTSAM_EXPORT virtual void reduceWithInverse(const internal::Reduction& inverseReduction); }; // Potentials diff --git a/gtsam/discrete/Signature.cpp b/gtsam/discrete/Signature.cpp index 006c851eb..9e63b957d 100644 --- a/gtsam/discrete/Signature.cpp +++ b/gtsam/discrete/Signature.cpp @@ -12,7 +12,7 @@ /** * @file Signature.cpp * @brief signatures for conditional densities - * @author Frank dellaert + * @author Frank Dellaert * @date Feb 27, 2011 */ @@ -131,8 +131,8 @@ namespace gtsam { return keys; } - vector Signature::indices() const { - vector js; + vector Signature::indices() const { + vector js; js.push_back(key_.first); BOOST_FOREACH(const DiscreteKey& key, parents_) js.push_back(key.first); diff --git a/gtsam/discrete/Signature.h b/gtsam/discrete/Signature.h index ddb3a28f8..6d49c7e4c 100644 --- a/gtsam/discrete/Signature.h +++ b/gtsam/discrete/Signature.h @@ -12,7 +12,7 @@ /** * @file Signature.h * @brief signatures for conditional densities - * @author Frank dellaert + * @author Frank Dellaert * @date Feb 27, 2011 */ @@ -90,7 +90,7 @@ namespace gtsam { DiscreteKeys discreteKeysParentsFirst() const; /** All key indices, with variable key first */ - std::vector indices() const; + std::vector indices() const; // the CPT as parsed, if successful const boost::optional
& table() const { diff --git a/gtsam/discrete/tests/CMakeLists.txt b/gtsam/discrete/tests/CMakeLists.txt new file mode 100644 index 000000000..e968fac91 --- /dev/null +++ b/gtsam/discrete/tests/CMakeLists.txt @@ -0,0 +1 @@ +gtsamAddTestsGlob(discrete "test*.cpp" "" "gtsam") diff --git a/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp b/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp index 61cedd720..f8bcb45c2 100644 --- a/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp +++ b/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp @@ -38,7 +38,7 @@ using namespace std; using namespace gtsam; /* ******************************************************************************** */ -typedef AlgebraicDecisionTree ADT; +typedef AlgebraicDecisionTree ADT; #define DISABLE_DOT @@ -172,7 +172,7 @@ TEST(ADT, joint) dot(joint, "Asia-ASTLBEX"); joint = apply(joint, pD, &mul); dot(joint, "Asia-ASTLBEXD"); - EXPECT_LONGS_EQUAL(346, muls); + EXPECT_LONGS_EQUAL(346, (long)muls); printCounts("Asia joint"); ADT pASTL = pA; @@ -223,7 +223,7 @@ TEST(ADT, inference) dot(joint, "Joint-Product-ASTLBEX"); joint = apply(joint, pD, &mul); dot(joint, "Joint-Product-ASTLBEXD"); - EXPECT_LONGS_EQUAL(370, muls); // different ordering + EXPECT_LONGS_EQUAL(370, (long)muls); // different ordering printCounts("Asia product"); ADT marginal = joint; @@ -235,7 +235,7 @@ TEST(ADT, inference) dot(marginal, "Joint-Sum-ADBLE"); marginal = marginal.combine(E, &add_); dot(marginal, "Joint-Sum-ADBL"); - EXPECT_LONGS_EQUAL(161, adds); + EXPECT_LONGS_EQUAL(161, (long)adds); printCounts("Asia sum"); } @@ -264,7 +264,7 @@ TEST(ADT, factor_graph) fg = apply(fg, pX, &mul); fg = apply(fg, pD, &mul); dot(fg, "FactorGraph"); - EXPECT_LONGS_EQUAL(158, muls); + EXPECT_LONGS_EQUAL(158, (long)muls); printCounts("Asia FG"); fg = fg.combine(X, &add_); @@ -369,7 +369,7 @@ TEST(ADT, equality_parser) TEST(ADT, constructor) { DiscreteKey v0(0,2), v1(1,3); - Assignment x00, x01, x02, x10, x11, x12; + Assignment x00, x01, x02, x10, x11, x12; x00[0] = 0, x00[1] = 0; x01[0] = 0, x01[1] = 1; x02[0] = 0, x02[1] = 2; @@ -399,7 +399,7 @@ TEST(ADT, constructor) BOOST_FOREACH(double& t, table) t = x++; ADT f3(z0 & z1 & z2 & z3, table); - Assignment assignment; + Assignment assignment; assignment[0] = 0; assignment[1] = 0; assignment[2] = 0; @@ -501,7 +501,7 @@ TEST(ADT, zero) ADT notb(B, 1, 0); ADT anotb = a * notb; // GTSAM_PRINT(anotb); - Assignment x00, x01, x10, x11; + Assignment x00, x01, x10, x11; x00[0] = 0, x00[1] = 0; x01[0] = 0, x01[1] = 1; x10[0] = 1, x10[1] = 0; diff --git a/gtsam/discrete/tests/testDiscreteBayesNet.cpp b/gtsam/discrete/tests/testDiscreteBayesNet.cpp index 076c1d7d5..f8a1b6309 100644 --- a/gtsam/discrete/tests/testDiscreteBayesNet.cpp +++ b/gtsam/discrete/tests/testDiscreteBayesNet.cpp @@ -17,13 +17,15 @@ */ #include -#include +#include #include #include #include #include +#include + using namespace boost::assign; #include @@ -40,38 +42,39 @@ TEST(DiscreteBayesNet, Asia) DiscreteKey A(0,2), S(4,2), T(3,2), L(6,2), B(7,2), E(5,2), X(2,2), D(1,2); // TODO: make a version that doesn't use the parser - add_front(asia, A % "99/1"); - add_front(asia, S % "50/50"); + asia.add(A % "99/1"); + asia.add(S % "50/50"); - add_front(asia, T | A = "99/1 95/5"); - add_front(asia, L | S = "99/1 90/10"); - add_front(asia, B | S = "70/30 40/60"); + asia.add(T | A = "99/1 95/5"); + asia.add(L | S = "99/1 90/10"); + asia.add(B | S = "70/30 40/60"); - add_front(asia, (E | T, L) = "F T T T"); + asia.add((E | T, L) = "F T T T"); - add_front(asia, X | E = "95/5 2/98"); - // next lines are same as add_front(asia, (D | E, B) = "9/1 2/8 3/7 1/9"); + asia.add(X | E = "95/5 2/98"); + // next lines are same as asia.add((D | E, B) = "9/1 2/8 3/7 1/9"); DiscreteConditional::shared_ptr actual = boost::make_shared((D | E, B) = "9/1 2/8 3/7 1/9"); - asia.push_front(actual); + asia.push_back(actual); // GTSAM_PRINT(asia); // Convert to factor graph DiscreteFactorGraph fg(asia); - // GTSAM_PRINT(fg); - LONGS_EQUAL(3,fg.front()->size()); +// GTSAM_PRINT(fg); + LONGS_EQUAL(3,fg.back()->size()); Potentials::ADT expected(B & D & E, "0.9 0.3 0.1 0.7 0.2 0.1 0.8 0.9"); CHECK(assert_equal(expected,(Potentials::ADT)*actual)); // Create solver and eliminate - DiscreteSequentialSolver solver(fg); - DiscreteBayesNet::shared_ptr chordal = solver.eliminate(); - // GTSAM_PRINT(*chordal); + Ordering ordering; + ordering += Key(0),Key(1),Key(2),Key(3),Key(4),Key(5),Key(6),Key(7); + DiscreteBayesNet::shared_ptr chordal = fg.eliminateSequential(ordering); +// GTSAM_PRINT(*chordal); DiscreteConditional expected2(B % "11/9"); CHECK(assert_equal(expected2,*chordal->back())); // solve - DiscreteFactor::sharedValues actualMPE = optimize(*chordal); + DiscreteFactor::sharedValues actualMPE = chordal->optimize(); DiscreteFactor::Values expectedMPE; insert(expectedMPE)(A.first, 0)(D.first, 0)(X.first, 0)(T.first, 0)(S.first, 0)(E.first, 0)(L.first, 0)(B.first, 0); @@ -83,10 +86,9 @@ TEST(DiscreteBayesNet, Asia) // fg.product().dot("fg"); // solve again, now with evidence - DiscreteSequentialSolver solver2(fg); - DiscreteBayesNet::shared_ptr chordal2 = solver2.eliminate(); + DiscreteBayesNet::shared_ptr chordal2 = fg.eliminateSequential(ordering); // GTSAM_PRINT(*chordal2); - DiscreteFactor::sharedValues actualMPE2 = optimize(*chordal2); + DiscreteFactor::sharedValues actualMPE2 = chordal2->optimize(); DiscreteFactor::Values expectedMPE2; insert(expectedMPE2)(A.first, 1)(D.first, 1)(X.first, 0)(T.first, 0)(S.first, 1)(E.first, 0)(L.first, 0)(B.first, 1); @@ -97,7 +99,7 @@ TEST(DiscreteBayesNet, Asia) SETDEBUG("DiscreteConditional::sample", false); insert(expectedSample)(A.first, 1)(D.first, 1)(X.first, 0)(T.first, 0)( S.first, 1)(E.first, 0)(L.first, 0)(B.first, 1); - DiscreteFactor::sharedValues actualSample = sample(*chordal2); + DiscreteFactor::sharedValues actualSample = chordal2->sample(); EXPECT(assert_equal(expectedSample, *actualSample)); } @@ -114,12 +116,12 @@ TEST_UNSAFE(DiscreteBayesNet, Sugar) // add(bn, D | E = "blah"); // try logic - add(bn, (E | T, L) = "OR"); - add(bn, (E | T, L) = "AND"); + bn.add((E | T, L) = "OR"); + bn.add((E | T, L) = "AND"); // // try multivalued - add(bn, C % "1/1/2"); - add(bn, C | S = "1/1/2 5/2/3"); + bn.add(C % "1/1/2"); + bn.add(C | S = "1/1/2 5/2/3"); } /* ************************************************************************* */ diff --git a/gtsam/discrete/tests/testDiscreteBayesTree.cpp b/gtsam/discrete/tests/testDiscreteBayesTree.cpp index e3a771940..c1acaf83d 100644 --- a/gtsam/discrete/tests/testDiscreteBayesTree.cpp +++ b/gtsam/discrete/tests/testDiscreteBayesTree.cpp @@ -1,261 +1,261 @@ -/* ---------------------------------------------------------------------------- - - * 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 testDiscreteBayesTree.cpp - * @date sept 15, 2012 - * @author Frank Dellaert - */ - -#include -#include -#include - -#include -using namespace boost::assign; - +///* ---------------------------------------------------------------------------- +// +// * 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 testDiscreteBayesTree.cpp +// * @date sept 15, 2012 +// * @author Frank Dellaert +// */ +// +//#include +//#include +//#include +// +//#include +//using namespace boost::assign; +// #include - -using namespace std; -using namespace gtsam; - -static bool debug = false; - -/** - * Custom clique class to debug shortcuts - */ -class Clique: public BayesTreeCliqueBase { - -protected: - -public: - - typedef BayesTreeCliqueBase Base; - typedef boost::shared_ptr shared_ptr; - - // Constructors - Clique() { - } - Clique(const DiscreteConditional::shared_ptr& conditional) : - Base(conditional) { - } - Clique( - const std::pair& result) : - Base(result) { - } - - /// print index signature only - void printSignature(const std::string& s = "Clique: ", - const IndexFormatter& indexFormatter = DefaultIndexFormatter) const { - ((IndexConditional::shared_ptr) conditional_)->print(s, indexFormatter); - } - - /// evaluate value of sub-tree - double evaluate(const DiscreteConditional::Values & values) { - double result = (*(this->conditional_))(values); - // evaluate all children and multiply into result - BOOST_FOREACH(boost::shared_ptr c, children_) - result *= c->evaluate(values); - return result; - } - -}; - -typedef BayesTree DiscreteBayesTree; - -/* ************************************************************************* */ -double evaluate(const DiscreteBayesTree& tree, - const DiscreteConditional::Values & values) { - return tree.root()->evaluate(values); -} - -/* ************************************************************************* */ - -TEST_UNSAFE( DiscreteBayesTree, thinTree ) { - - const int nrNodes = 15; - const size_t nrStates = 2; - - // define variables - vector key; - for (int i = 0; i < nrNodes; i++) { - DiscreteKey key_i(i, nrStates); - key.push_back(key_i); - } - - // create a thin-tree Bayesnet, a la Jean-Guillaume - DiscreteBayesNet bayesNet; - add_front(bayesNet, key[14] % "1/3"); - - add_front(bayesNet, key[13] | key[14] = "1/3 3/1"); - add_front(bayesNet, key[12] | key[14] = "3/1 3/1"); - - add_front(bayesNet, (key[11] | key[13], key[14]) = "1/4 2/3 3/2 4/1"); - add_front(bayesNet, (key[10] | key[13], key[14]) = "1/4 3/2 2/3 4/1"); - add_front(bayesNet, (key[9] | key[12], key[14]) = "4/1 2/3 F 1/4"); - add_front(bayesNet, (key[8] | key[12], key[14]) = "T 1/4 3/2 4/1"); - - add_front(bayesNet, (key[7] | key[11], key[13]) = "1/4 2/3 3/2 4/1"); - add_front(bayesNet, (key[6] | key[11], key[13]) = "1/4 3/2 2/3 4/1"); - add_front(bayesNet, (key[5] | key[10], key[13]) = "4/1 2/3 3/2 1/4"); - add_front(bayesNet, (key[4] | key[10], key[13]) = "2/3 1/4 3/2 4/1"); - - add_front(bayesNet, (key[3] | key[9], key[12]) = "1/4 2/3 3/2 4/1"); - add_front(bayesNet, (key[2] | key[9], key[12]) = "1/4 8/2 2/3 4/1"); - add_front(bayesNet, (key[1] | key[8], key[12]) = "4/1 2/3 3/2 1/4"); - add_front(bayesNet, (key[0] | key[8], key[12]) = "2/3 1/4 3/2 4/1"); - - if (debug) { - GTSAM_PRINT(bayesNet); - bayesNet.saveGraph("/tmp/discreteBayesNet.dot"); - } - - // create a BayesTree out of a Bayes net - DiscreteBayesTree bayesTree(bayesNet); - if (debug) { - GTSAM_PRINT(bayesTree); - bayesTree.saveGraph("/tmp/discreteBayesTree.dot"); - } - - // Check whether BN and BT give the same answer on all configurations - // Also calculate all some marginals - Vector marginals = zero(15); - double joint_12_14 = 0, joint_9_12_14 = 0, joint_8_12_14 = 0, joint_8_12 = 0, - joint82 = 0, joint12 = 0, joint24 = 0, joint45 = 0, joint46 = 0, - joint_4_11 = 0; - vector allPosbValues = cartesianProduct( - key[0] & key[1] & key[2] & key[3] & key[4] & key[5] & key[6] & key[7] - & key[8] & key[9] & key[10] & key[11] & key[12] & key[13] & key[14]); - for (size_t i = 0; i < allPosbValues.size(); ++i) { - DiscreteFactor::Values x = allPosbValues[i]; - double expected = evaluate(bayesNet, x); - double actual = evaluate(bayesTree, x); - DOUBLES_EQUAL(expected, actual, 1e-9); - // collect marginals - for (size_t i = 0; i < 15; i++) - if (x[i]) - marginals[i] += actual; - // calculate shortcut 8 and 0 - if (x[12] && x[14]) - joint_12_14 += actual; - if (x[9] && x[12] & x[14]) - joint_9_12_14 += actual; - if (x[8] && x[12] & x[14]) - joint_8_12_14 += actual; - if (x[8] && x[12]) - joint_8_12 += actual; - if (x[8] && x[2]) - joint82 += actual; - if (x[1] && x[2]) - joint12 += actual; - if (x[2] && x[4]) - joint24 += actual; - if (x[4] && x[5]) - joint45 += actual; - if (x[4] && x[6]) - joint46 += actual; - if (x[4] && x[11]) - joint_4_11 += actual; - } - DiscreteFactor::Values all1 = allPosbValues.back(); - - Clique::shared_ptr R = bayesTree.root(); - - // check separator marginal P(S0) - Clique::shared_ptr c = bayesTree[0]; - DiscreteFactorGraph separatorMarginal0 = c->separatorMarginal(R, - EliminateDiscrete); - EXPECT_DOUBLES_EQUAL(joint_8_12, separatorMarginal0(all1), 1e-9); - - // check separator marginal P(S9), should be P(14) - c = bayesTree[9]; - DiscreteFactorGraph separatorMarginal9 = c->separatorMarginal(R, - EliminateDiscrete); - EXPECT_DOUBLES_EQUAL(marginals[14], separatorMarginal9(all1), 1e-9); - - // check separator marginal of root, should be empty - c = bayesTree[11]; - DiscreteFactorGraph separatorMarginal11 = c->separatorMarginal(R, - EliminateDiscrete); - EXPECT_LONGS_EQUAL(0, separatorMarginal11.size()); - - // check shortcut P(S9||R) to root - c = bayesTree[9]; - DiscreteBayesNet shortcut = c->shortcut(R, EliminateDiscrete); - EXPECT_LONGS_EQUAL(0, shortcut.size()); - - // check shortcut P(S8||R) to root - c = bayesTree[8]; - shortcut = c->shortcut(R, EliminateDiscrete); - EXPECT_DOUBLES_EQUAL(joint_12_14/marginals[14], evaluate(shortcut,all1), - 1e-9); - - // check shortcut P(S2||R) to root - c = bayesTree[2]; - shortcut = c->shortcut(R, EliminateDiscrete); - EXPECT_DOUBLES_EQUAL(joint_9_12_14/marginals[14], evaluate(shortcut,all1), - 1e-9); - - // check shortcut P(S0||R) to root - c = bayesTree[0]; - shortcut = c->shortcut(R, EliminateDiscrete); - EXPECT_DOUBLES_EQUAL(joint_8_12_14/marginals[14], evaluate(shortcut,all1), - 1e-9); - - // calculate all shortcuts to root - DiscreteBayesTree::Nodes cliques = bayesTree.nodes(); - BOOST_FOREACH(Clique::shared_ptr c, cliques) { - DiscreteBayesNet shortcut = c->shortcut(R, EliminateDiscrete); - if (debug) { - c->printSignature(); - shortcut.print("shortcut:"); - } - } - - // Check all marginals - DiscreteFactor::shared_ptr marginalFactor; - for (size_t i = 0; i < 15; i++) { - marginalFactor = bayesTree.marginalFactor(i, EliminateDiscrete); - double actual = (*marginalFactor)(all1); - EXPECT_DOUBLES_EQUAL(marginals[i], actual, 1e-9); - } - - DiscreteBayesNet::shared_ptr actualJoint; - - // Check joint P(8,2) TODO: not disjoint ! -// actualJoint = bayesTree.jointBayesNet(8, 2, EliminateDiscrete); -// EXPECT_DOUBLES_EQUAL(joint82, evaluate(*actualJoint,all1), 1e-9); - - // Check joint P(1,2) TODO: not disjoint ! -// actualJoint = bayesTree.jointBayesNet(1, 2, EliminateDiscrete); -// EXPECT_DOUBLES_EQUAL(joint12, evaluate(*actualJoint,all1), 1e-9); - - // Check joint P(2,4) - actualJoint = bayesTree.jointBayesNet(2, 4, EliminateDiscrete); - EXPECT_DOUBLES_EQUAL(joint24, evaluate(*actualJoint,all1), 1e-9); - - // Check joint P(4,5) TODO: not disjoint ! -// actualJoint = bayesTree.jointBayesNet(4, 5, EliminateDiscrete); -// EXPECT_DOUBLES_EQUAL(joint46, evaluate(*actualJoint,all1), 1e-9); - - // Check joint P(4,6) TODO: not disjoint ! -// actualJoint = bayesTree.jointBayesNet(4, 6, EliminateDiscrete); -// EXPECT_DOUBLES_EQUAL(joint46, evaluate(*actualJoint,all1), 1e-9); - - // Check joint P(4,11) - actualJoint = bayesTree.jointBayesNet(4, 11, EliminateDiscrete); - EXPECT_DOUBLES_EQUAL(joint_4_11, evaluate(*actualJoint,all1), 1e-9); - -} +// +//using namespace std; +//using namespace gtsam; +// +//static bool debug = false; +// +///** +// * Custom clique class to debug shortcuts +// */ +////class Clique: public BayesTreeCliqueBaseOrdered { +//// +////protected: +//// +////public: +//// +//// typedef BayesTreeCliqueBaseOrdered Base; +//// typedef boost::shared_ptr shared_ptr; +//// +//// // Constructors +//// Clique() { +//// } +//// Clique(const DiscreteConditional::shared_ptr& conditional) : +//// Base(conditional) { +//// } +//// Clique( +//// const std::pair& result) : +//// Base(result) { +//// } +//// +//// /// print index signature only +//// void printSignature(const std::string& s = "Clique: ", +//// const KeyFormatter& indexFormatter = DefaultKeyFormatter) const { +//// ((IndexConditionalOrdered::shared_ptr) conditional_)->print(s, indexFormatter); +//// } +//// +//// /// evaluate value of sub-tree +//// double evaluate(const DiscreteConditional::Values & values) { +//// double result = (*(this->conditional_))(values); +//// // evaluate all children and multiply into result +//// BOOST_FOREACH(boost::shared_ptr c, children_) +//// result *= c->evaluate(values); +//// return result; +//// } +//// +////}; +// +////typedef BayesTreeOrdered DiscreteBayesTree; +//// +/////* ************************************************************************* */ +////double evaluate(const DiscreteBayesTree& tree, +//// const DiscreteConditional::Values & values) { +//// return tree.root()->evaluate(values); +////} +// +///* ************************************************************************* */ +// +//TEST_UNSAFE( DiscreteBayesTree, thinTree ) { +// +// const int nrNodes = 15; +// const size_t nrStates = 2; +// +// // define variables +// vector key; +// for (int i = 0; i < nrNodes; i++) { +// DiscreteKey key_i(i, nrStates); +// key.push_back(key_i); +// } +// +// // create a thin-tree Bayesnet, a la Jean-Guillaume +// DiscreteBayesNet bayesNet; +// bayesNet.add(key[14] % "1/3"); +// +// bayesNet.add(key[13] | key[14] = "1/3 3/1"); +// bayesNet.add(key[12] | key[14] = "3/1 3/1"); +// +// bayesNet.add((key[11] | key[13], key[14]) = "1/4 2/3 3/2 4/1"); +// bayesNet.add((key[10] | key[13], key[14]) = "1/4 3/2 2/3 4/1"); +// bayesNet.add((key[9] | key[12], key[14]) = "4/1 2/3 F 1/4"); +// bayesNet.add((key[8] | key[12], key[14]) = "T 1/4 3/2 4/1"); +// +// bayesNet.add((key[7] | key[11], key[13]) = "1/4 2/3 3/2 4/1"); +// bayesNet.add((key[6] | key[11], key[13]) = "1/4 3/2 2/3 4/1"); +// bayesNet.add((key[5] | key[10], key[13]) = "4/1 2/3 3/2 1/4"); +// bayesNet.add((key[4] | key[10], key[13]) = "2/3 1/4 3/2 4/1"); +// +// bayesNet.add((key[3] | key[9], key[12]) = "1/4 2/3 3/2 4/1"); +// bayesNet.add((key[2] | key[9], key[12]) = "1/4 8/2 2/3 4/1"); +// bayesNet.add((key[1] | key[8], key[12]) = "4/1 2/3 3/2 1/4"); +// bayesNet.add((key[0] | key[8], key[12]) = "2/3 1/4 3/2 4/1"); +// +//// if (debug) { +//// GTSAM_PRINT(bayesNet); +//// bayesNet.saveGraph("/tmp/discreteBayesNet.dot"); +//// } +// +// // create a BayesTree out of a Bayes net +// DiscreteBayesTree bayesTree(bayesNet); +// if (debug) { +// GTSAM_PRINT(bayesTree); +// bayesTree.saveGraph("/tmp/discreteBayesTree.dot"); +// } +// +// // Check whether BN and BT give the same answer on all configurations +// // Also calculate all some marginals +// Vector marginals = zero(15); +// double joint_12_14 = 0, joint_9_12_14 = 0, joint_8_12_14 = 0, joint_8_12 = 0, +// joint82 = 0, joint12 = 0, joint24 = 0, joint45 = 0, joint46 = 0, +// joint_4_11 = 0; +// vector allPosbValues = cartesianProduct( +// key[0] & key[1] & key[2] & key[3] & key[4] & key[5] & key[6] & key[7] +// & key[8] & key[9] & key[10] & key[11] & key[12] & key[13] & key[14]); +// for (size_t i = 0; i < allPosbValues.size(); ++i) { +// DiscreteFactor::Values x = allPosbValues[i]; +// double expected = evaluate(bayesNet, x); +// double actual = evaluate(bayesTree, x); +// DOUBLES_EQUAL(expected, actual, 1e-9); +// // collect marginals +// for (size_t i = 0; i < 15; i++) +// if (x[i]) +// marginals[i] += actual; +// // calculate shortcut 8 and 0 +// if (x[12] && x[14]) +// joint_12_14 += actual; +// if (x[9] && x[12] & x[14]) +// joint_9_12_14 += actual; +// if (x[8] && x[12] & x[14]) +// joint_8_12_14 += actual; +// if (x[8] && x[12]) +// joint_8_12 += actual; +// if (x[8] && x[2]) +// joint82 += actual; +// if (x[1] && x[2]) +// joint12 += actual; +// if (x[2] && x[4]) +// joint24 += actual; +// if (x[4] && x[5]) +// joint45 += actual; +// if (x[4] && x[6]) +// joint46 += actual; +// if (x[4] && x[11]) +// joint_4_11 += actual; +// } +// DiscreteFactor::Values all1 = allPosbValues.back(); +// +// Clique::shared_ptr R = bayesTree.root(); +// +// // check separator marginal P(S0) +// Clique::shared_ptr c = bayesTree[0]; +// DiscreteFactorGraph separatorMarginal0 = c->separatorMarginal(R, +// EliminateDiscrete); +// EXPECT_DOUBLES_EQUAL(joint_8_12, separatorMarginal0(all1), 1e-9); +// +// // check separator marginal P(S9), should be P(14) +// c = bayesTree[9]; +// DiscreteFactorGraph separatorMarginal9 = c->separatorMarginal(R, +// EliminateDiscrete); +// EXPECT_DOUBLES_EQUAL(marginals[14], separatorMarginal9(all1), 1e-9); +// +// // check separator marginal of root, should be empty +// c = bayesTree[11]; +// DiscreteFactorGraph separatorMarginal11 = c->separatorMarginal(R, +// EliminateDiscrete); +// EXPECT_LONGS_EQUAL(0, separatorMarginal11.size()); +// +// // check shortcut P(S9||R) to root +// c = bayesTree[9]; +// DiscreteBayesNet shortcut = c->shortcut(R, EliminateDiscrete); +// EXPECT_LONGS_EQUAL(0, shortcut.size()); +// +// // check shortcut P(S8||R) to root +// c = bayesTree[8]; +// shortcut = c->shortcut(R, EliminateDiscrete); +// EXPECT_DOUBLES_EQUAL(joint_12_14/marginals[14], evaluate(shortcut,all1), +// 1e-9); +// +// // check shortcut P(S2||R) to root +// c = bayesTree[2]; +// shortcut = c->shortcut(R, EliminateDiscrete); +// EXPECT_DOUBLES_EQUAL(joint_9_12_14/marginals[14], evaluate(shortcut,all1), +// 1e-9); +// +// // check shortcut P(S0||R) to root +// c = bayesTree[0]; +// shortcut = c->shortcut(R, EliminateDiscrete); +// EXPECT_DOUBLES_EQUAL(joint_8_12_14/marginals[14], evaluate(shortcut,all1), +// 1e-9); +// +// // calculate all shortcuts to root +// DiscreteBayesTree::Nodes cliques = bayesTree.nodes(); +// BOOST_FOREACH(Clique::shared_ptr c, cliques) { +// DiscreteBayesNet shortcut = c->shortcut(R, EliminateDiscrete); +// if (debug) { +// c->printSignature(); +// shortcut.print("shortcut:"); +// } +// } +// +// // Check all marginals +// DiscreteFactor::shared_ptr marginalFactor; +// for (size_t i = 0; i < 15; i++) { +// marginalFactor = bayesTree.marginalFactor(i, EliminateDiscrete); +// double actual = (*marginalFactor)(all1); +// EXPECT_DOUBLES_EQUAL(marginals[i], actual, 1e-9); +// } +// +// DiscreteBayesNet::shared_ptr actualJoint; +// +// // Check joint P(8,2) TODO: not disjoint ! +//// actualJoint = bayesTree.jointBayesNet(8, 2, EliminateDiscrete); +//// EXPECT_DOUBLES_EQUAL(joint82, evaluate(*actualJoint,all1), 1e-9); +// +// // Check joint P(1,2) TODO: not disjoint ! +//// actualJoint = bayesTree.jointBayesNet(1, 2, EliminateDiscrete); +//// EXPECT_DOUBLES_EQUAL(joint12, evaluate(*actualJoint,all1), 1e-9); +// +// // Check joint P(2,4) +// actualJoint = bayesTree.jointBayesNet(2, 4, EliminateDiscrete); +// EXPECT_DOUBLES_EQUAL(joint24, evaluate(*actualJoint,all1), 1e-9); +// +// // Check joint P(4,5) TODO: not disjoint ! +//// actualJoint = bayesTree.jointBayesNet(4, 5, EliminateDiscrete); +//// EXPECT_DOUBLES_EQUAL(joint46, evaluate(*actualJoint,all1), 1e-9); +// +// // Check joint P(4,6) TODO: not disjoint ! +//// actualJoint = bayesTree.jointBayesNet(4, 6, EliminateDiscrete); +//// EXPECT_DOUBLES_EQUAL(joint46, evaluate(*actualJoint,all1), 1e-9); +// +// // Check joint P(4,11) +// actualJoint = bayesTree.jointBayesNet(4, 11, EliminateDiscrete); +// EXPECT_DOUBLES_EQUAL(joint_4_11, evaluate(*actualJoint,all1), 1e-9); +// +//} /* ************************************************************************* */ int main() { diff --git a/gtsam/discrete/tests/testDiscreteFactorGraph.cpp b/gtsam/discrete/tests/testDiscreteFactorGraph.cpp index f1b29a757..052180f7a 100644 --- a/gtsam/discrete/tests/testDiscreteFactorGraph.cpp +++ b/gtsam/discrete/tests/testDiscreteFactorGraph.cpp @@ -17,8 +17,8 @@ #include #include -#include -#include +#include +#include #include @@ -58,9 +58,9 @@ TEST_UNSAFE( DiscreteFactorGraph, debugScheduler) { // result.first->print("BayesNet: "); // result.second->print("New factor: "); // - DiscreteSequentialSolver solver(graph); -// solver.print("solver:"); - EliminationTree eliminationTree = solver.eliminationTree(); + Ordering ordering; + ordering += Key(0),Key(1),Key(2),Key(3); + DiscreteEliminationTree eliminationTree(graph, ordering); // eliminationTree.print("Elimination tree: "); eliminationTree.eliminate(EliminateDiscrete); // solver.optimize(); @@ -127,10 +127,11 @@ TEST( DiscreteFactorGraph, test) // Test EliminateDiscrete // FIXME: apparently Eliminate returns a conditional rather than a net + Ordering frontalKeys; + frontalKeys += Key(0); DiscreteConditional::shared_ptr conditional; DecisionTreeFactor::shared_ptr newFactor; - boost::tie(conditional, newFactor) =// - EliminateDiscrete(graph, 1); + boost::tie(conditional, newFactor) = EliminateDiscrete(graph, frontalKeys); // Check Bayes net CHECK(conditional); @@ -139,34 +140,35 @@ TEST( DiscreteFactorGraph, test) // cout << signature << endl; DiscreteConditional expectedConditional(signature); EXPECT(assert_equal(expectedConditional, *conditional)); - add(expected, signature); + expected.add(signature); // Check Factor CHECK(newFactor); DecisionTreeFactor expectedFactor(B & A, "10 6 6 10"); EXPECT(assert_equal(expectedFactor, *newFactor)); - // Create solver - DiscreteSequentialSolver solver(graph); - // add conditionals to complete expected Bayes net - add(expected, B | A = "5/3 3/5"); - add(expected, A % "1/1"); + expected.add(B | A = "5/3 3/5"); + expected.add(A % "1/1"); // GTSAM_PRINT(expected); // Test elimination tree - const EliminationTree& etree = solver.eliminationTree(); - DiscreteBayesNet::shared_ptr actual = etree.eliminate(&EliminateDiscrete); + Ordering ordering; + ordering += Key(0), Key(1), Key(2); + DiscreteEliminationTree etree(graph, ordering); + DiscreteBayesNet::shared_ptr actual; + DiscreteFactorGraph::shared_ptr remainingGraph; + boost::tie(actual, remainingGraph) = etree.eliminate(&EliminateDiscrete); EXPECT(assert_equal(expected, *actual)); - // Test solver - DiscreteBayesNet::shared_ptr actual2 = solver.eliminate(); - EXPECT(assert_equal(expected, *actual2)); +// // Test solver +// DiscreteBayesNet::shared_ptr actual2 = solver.eliminate(); +// EXPECT(assert_equal(expected, *actual2)); // Test optimization DiscreteFactor::Values expectedValues; insert(expectedValues)(0, 0)(1, 0)(2, 0); - DiscreteFactor::sharedValues actualValues = solver.optimize(); + DiscreteFactor::sharedValues actualValues = graph.optimize(); EXPECT(assert_equal(expectedValues, *actualValues)); } @@ -183,8 +185,7 @@ TEST( DiscreteFactorGraph, testMPE) // graph.product().print(); // DiscreteSequentialSolver(graph).eliminate()->print(); - DiscreteFactor::sharedValues actualMPE = - DiscreteSequentialSolver(graph).optimize(); + DiscreteFactor::sharedValues actualMPE = graph.optimize(); DiscreteFactor::Values expectedMPE; insert(expectedMPE)(0, 0)(1, 1)(2, 1); @@ -213,24 +214,29 @@ TEST( DiscreteFactorGraph, testMPE_Darwiche09book_p244) insert(expectedMPE)(4, 0)(2, 0)(3, 1)(0, 1)(1, 1); // Use the solver machinery. - DiscreteBayesNet::shared_ptr chordal = - DiscreteSequentialSolver(graph).eliminate(); - DiscreteFactor::sharedValues actualMPE = optimize(*chordal); + DiscreteBayesNet::shared_ptr chordal = graph.eliminateSequential(); + DiscreteFactor::sharedValues actualMPE = chordal->optimize(); EXPECT(assert_equal(expectedMPE, *actualMPE)); // DiscreteConditional::shared_ptr root = chordal->back(); // EXPECT_DOUBLES_EQUAL(0.4, (*root)(*actualMPE), 1e-9); // Let us create the Bayes tree here, just for fun, because we don't use it now - typedef JunctionTree JT; - GenericMultifrontalSolver solver(graph); - BayesTree::shared_ptr bayesTree = solver.eliminate(&EliminateDiscrete); -// bayesTree->print("Bayes Tree"); +// typedef JunctionTreeOrdered JT; +// GenericMultifrontalSolver solver(graph); +// BayesTreeOrdered::shared_ptr bayesTree = solver.eliminate(&EliminateDiscrete); +//// bayesTree->print("Bayes Tree"); +// EXPECT_LONGS_EQUAL(2,bayesTree->size()); + + Ordering ordering; + ordering += Key(0),Key(1),Key(2),Key(3),Key(4); + DiscreteBayesTree::shared_ptr bayesTree = graph.eliminateMultifrontal(ordering); + // bayesTree->print("Bayes Tree"); EXPECT_LONGS_EQUAL(2,bayesTree->size()); #ifdef OLD // Create the elimination tree manually -VariableIndex structure(graph); -typedef EliminationTree ETree; +VariableIndexOrdered structure(graph); +typedef EliminationTreeOrdered ETree; ETree::shared_ptr eTree = ETree::Create(graph, structure); //eTree->print(">>>>>>>>>>> Elimination Tree <<<<<<<<<<<<<<<<<"); diff --git a/gtsam/discrete/tests/testDiscreteMarginals.cpp b/gtsam/discrete/tests/testDiscreteMarginals.cpp index 5ca26200f..c088bcb4e 100644 --- a/gtsam/discrete/tests/testDiscreteMarginals.cpp +++ b/gtsam/discrete/tests/testDiscreteMarginals.cpp @@ -18,7 +18,6 @@ */ #include -#include #include using namespace boost::assign; @@ -54,10 +53,10 @@ TEST_UNSAFE( DiscreteMarginals, UGM_small ) { EXPECT_DOUBLES_EQUAL( 0.359631, (*actualC)(values), 1e-6); Vector actualCvector = marginals.marginalProbabilities(Cathy); - EXPECT(assert_equal(Vector_(2, 0.359631, 0.640369), actualCvector, 1e-6)); + EXPECT(assert_equal((Vector(2) << 0.359631, 0.640369), actualCvector, 1e-6)); actualCvector = marginals.marginalProbabilities(Mark); - EXPECT(assert_equal(Vector_(2, 0.48628, 0.51372), actualCvector, 1e-6)); + EXPECT(assert_equal((Vector(2) << 0.48628, 0.51372), actualCvector, 1e-6)); } /* ************************************************************************* */ @@ -119,11 +118,9 @@ TEST_UNSAFE( DiscreteMarginals, truss ) { graph.add(key[0] & key[2] & key[4],"2 2 2 2 1 1 1 1"); graph.add(key[1] & key[3] & key[4],"1 1 1 1 2 2 2 2"); graph.add(key[2] & key[3] & key[4],"1 1 1 1 1 1 1 1"); - typedef JunctionTree JT; - GenericMultifrontalSolver solver(graph); - BayesTree::shared_ptr bayesTree = solver.eliminate(&EliminateDiscrete); + DiscreteBayesTree::shared_ptr bayesTree = graph.eliminateMultifrontal(); // bayesTree->print("Bayes Tree"); - typedef BayesTreeClique Clique; + typedef DiscreteBayesTreeClique Clique; Clique expected0(boost::make_shared((key[0] | key[2], key[4]) = "2/1 2/1 2/1 2/1")); Clique::shared_ptr actual0 = (*bayesTree)[0]; @@ -180,16 +177,16 @@ TEST_UNSAFE( DiscreteMarginals, truss2 ) { } // Check all marginals given by a sequential solver and Marginals - DiscreteSequentialSolver solver(graph); +// DiscreteSequentialSolver solver(graph); DiscreteMarginals marginals(graph); for (size_t j=0;j<5;j++) { double sum = T[j]+F[j]; T[j]/=sum; F[j]/=sum; - // solver - Vector actualV = solver.marginalProbabilities(key[j]); - EXPECT(assert_equal(Vector_(2,F[j],T[j]), actualV)); +// // solver +// Vector actualV = solver.marginalProbabilities(key[j]); +// EXPECT(assert_equal((Vector(2) << F[j], T[j]), actualV)); // Marginals vector table; diff --git a/gtsam/discrete/tests/testSignature.cpp b/gtsam/discrete/tests/testSignature.cpp index e32f1cf33..de47a00f3 100644 --- a/gtsam/discrete/tests/testSignature.cpp +++ b/gtsam/discrete/tests/testSignature.cpp @@ -32,15 +32,15 @@ DiscreteKey X(0,2), Y(1,3), Z(2,2); TEST(testSignature, simple_conditional) { Signature sig(X | Y = "1/1 2/3 1/4"); DiscreteKey actKey = sig.key(); - LONGS_EQUAL(X.first, actKey.first); + LONGS_EQUAL((long)X.first, (long)actKey.first); DiscreteKeys actKeys = sig.discreteKeysParentsFirst(); - LONGS_EQUAL(2, actKeys.size()); - LONGS_EQUAL(Y.first, actKeys.front().first); - LONGS_EQUAL(X.first, actKeys.back().first); + LONGS_EQUAL(2, (long)actKeys.size()); + LONGS_EQUAL((long)Y.first, (long)actKeys.front().first); + LONGS_EQUAL((long)X.first, (long)actKeys.back().first); vector actCpt = sig.cpt(); - EXPECT_LONGS_EQUAL(6, actCpt.size()); + EXPECT_LONGS_EQUAL(6, (long)actCpt.size()); } /* ************************************************************************* */ @@ -54,15 +54,15 @@ TEST(testSignature, simple_conditional_nonparser) { Signature sig(X | Y = table); DiscreteKey actKey = sig.key(); - EXPECT_LONGS_EQUAL(X.first, actKey.first); + EXPECT_LONGS_EQUAL((long)X.first, (long)actKey.first); DiscreteKeys actKeys = sig.discreteKeysParentsFirst(); - LONGS_EQUAL(2, actKeys.size()); - LONGS_EQUAL(Y.first, actKeys.front().first); - LONGS_EQUAL(X.first, actKeys.back().first); + LONGS_EQUAL(2, (long)actKeys.size()); + LONGS_EQUAL((long)Y.first, (long)actKeys.front().first); + LONGS_EQUAL((long)X.first, (long)actKeys.back().first); vector actCpt = sig.cpt(); - EXPECT_LONGS_EQUAL(6, actCpt.size()); + EXPECT_LONGS_EQUAL(6, (long)actCpt.size()); } /* ************************************************************************* */ diff --git a/gtsam/dllexport.h b/gtsam/dllexport.h deleted file mode 100644 index 0e327fc14..000000000 --- a/gtsam/dllexport.h +++ /dev/null @@ -1,36 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 dllexport.h - * @brief Symbols for exporting classes and methods from DLLs - * @author Richard Roberts - * @date Mar 9, 2013 - */ - -#ifdef _WIN32 -# ifdef GTSAM_EXPORTS -# define GTSAM_EXPORT __declspec(dllexport) -# define GTSAM_EXTERN_EXPORT __declspec(dllexport) extern -# else -# ifndef GTSAM_IMPORT_STATIC -# define GTSAM_EXPORT __declspec(dllimport) -# define GTSAM_EXTERN_EXPORT __declspec(dllimport) -# else /* GTSAM_IMPORT_STATIC */ -# define GTSAM_EXPORT -# define GTSAM_EXTERN_EXPORT extern -# endif /* GTSAM_IMPORT_STATIC */ -# endif /* GTSAM_EXPORTS */ -#else /* _WIN32 */ -# define GTSAM_EXPORT -# define GTSAM_EXTERN_EXPORT extern -#endif - diff --git a/gtsam/geometry/CMakeLists.txt b/gtsam/geometry/CMakeLists.txt index b659d8314..f72f965ea 100644 --- a/gtsam/geometry/CMakeLists.txt +++ b/gtsam/geometry/CMakeLists.txt @@ -2,19 +2,11 @@ file(GLOB geometry_headers "*.h") install(FILES ${geometry_headers} DESTINATION include/gtsam/geometry) -# Files to exclude from compilation of tests and timing scripts -set(geometry_excluded_files -# "${CMAKE_CURRENT_SOURCE_DIR}/tests/testTypedDiscreteFactor.cpp" # Example of excluding a test - "" # Add to this list, with full path, to exclude -) - # Build tests -if (GTSAM_BUILD_TESTS) - gtsam_add_subdir_tests(geometry "${gtsam-default}" "${gtsam-default}" "${geometry_excluded_files}") -endif(GTSAM_BUILD_TESTS) +add_subdirectory(tests) # Build timing scripts if (GTSAM_BUILD_TIMING) - gtsam_add_subdir_timing(geometry "${gtsam-default}" "${gtsam-default}" "${geometry_excluded_files}") + gtsam_add_subdir_timing(geometry "gtsam" "gtsam" "${geometry_excluded_files}") endif(GTSAM_BUILD_TIMING) diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp index 8b7771410..587d0ea63 100644 --- a/gtsam/geometry/Cal3Bundler.cpp +++ b/gtsam/geometry/Cal3Bundler.cpp @@ -78,17 +78,16 @@ Point2 Cal3Bundler::uncalibrate(const Point2& p, // // Derivatives make use of intermediate variables above if (Dcal) { double rx = r * x, ry = r * y; - Eigen::Matrix D; - D << u, f_ * rx, f_ * r * rx, v, f_ * ry, f_ * r * ry; - *Dcal = D; + Dcal->resize(2, 3); + *Dcal << u, f_ * rx, f_ * r * rx, v, f_ * ry, f_ * r * ry; } if (Dp) { const double a = 2. * (k1_ + 2. * k2_ * r); const double axx = a * x * x, axy = a * x * y, ayy = a * y * y; - Eigen::Matrix D; - D << g + axx, axy, axy, g + ayy; - *Dp = f_ * D; + Dp->resize(2,2); + *Dp << g + axx, axy, axy, g + ayy; + *Dp *= f_; } return Point2(u0_ + f_ * u, v0_ + f_ * v); @@ -152,7 +151,7 @@ Cal3Bundler Cal3Bundler::retract(const Vector& d) const { /* ************************************************************************* */ Vector Cal3Bundler::localCoordinates(const Cal3Bundler& T2) const { - return vector() - T2.vector(); + return T2.vector() - vector(); } } diff --git a/gtsam/geometry/Cal3DS2.cpp b/gtsam/geometry/Cal3DS2.cpp index 0d263b625..e6c97cb1a 100644 --- a/gtsam/geometry/Cal3DS2.cpp +++ b/gtsam/geometry/Cal3DS2.cpp @@ -28,13 +28,20 @@ Cal3DS2::Cal3DS2(const Vector &v): fx_(v[0]), fy_(v[1]), s_(v[2]), u0_(v[3]), v0_(v[4]), k1_(v[5]), k2_(v[6]), k3_(v[7]), k4_(v[8]){} /* ************************************************************************* */ -Matrix Cal3DS2::K() const { return (Matrix(3, 3) << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0); } +Matrix Cal3DS2::K() const { + return (Matrix(3, 3) << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0); +} /* ************************************************************************* */ -Vector Cal3DS2::vector() const { return (Vector(9) << fx_, fy_, s_, u0_, v0_, k1_, k2_, k3_, k4_); } +Vector Cal3DS2::vector() const { + return (Vector(9) << fx_, fy_, s_, u0_, v0_, k1_, k2_, k3_, k4_); +} /* ************************************************************************* */ -void Cal3DS2::print(const std::string& s) const { gtsam::print(K(), s + ".K"); gtsam::print(Vector(k()), s + ".k"); } +void Cal3DS2::print(const std::string& s) const { + gtsam::print(K(), s + ".K"); + gtsam::print(Vector(k()), s + ".k"); +} /* ************************************************************************* */ bool Cal3DS2::equals(const Cal3DS2& K, double tol) const { @@ -173,10 +180,14 @@ Matrix Cal3DS2::D2d_calibration(const Point2& p) const { } /* ************************************************************************* */ -Cal3DS2 Cal3DS2::retract(const Vector& d) const { return Cal3DS2(vector() + d); } +Cal3DS2 Cal3DS2::retract(const Vector& d) const { + return Cal3DS2(vector() + d); +} /* ************************************************************************* */ -Vector Cal3DS2::localCoordinates(const Cal3DS2& T2) const { return vector() - T2.vector(); } +Vector Cal3DS2::localCoordinates(const Cal3DS2& T2) const { + return T2.vector() - vector(); +} } /* ************************************************************************* */ diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h index 27e5d4f1e..ced05086b 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -30,7 +30,7 @@ namespace gtsam { */ class GTSAM_EXPORT Cal3DS2 : public DerivedValue { -private: +protected: double fx_, fy_, s_, u0_, v0_ ; // focal length, skew and principal point double k1_, k2_ ; // radial 2nd-order and 4th-order @@ -103,7 +103,7 @@ public: boost::optional Dcal = boost::none, boost::optional Dp = boost::none) const ; - /// Convert pixel coordinates to ideal coordinates + /// Conver a pixel coordinate to ideal coordinate Point2 calibrate(const Point2& p, const double tol=1e-5) const; /// Derivative of uncalibrate wrpt intrinsic coordinates diff --git a/gtsam/geometry/Cal3Unified.cpp b/gtsam/geometry/Cal3Unified.cpp new file mode 100644 index 000000000..808cb84a4 --- /dev/null +++ b/gtsam/geometry/Cal3Unified.cpp @@ -0,0 +1,141 @@ +/* ---------------------------------------------------------------------------- + + * 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 Cal3Unified.cpp + * @date Mar 8, 2014 + * @author Jing Dong + */ + +#include +#include +#include +#include + +#include + +namespace gtsam { + +/* ************************************************************************* */ +Cal3Unified::Cal3Unified(const Vector &v): + Base(v[0], v[1], v[2], v[3], v[4], v[5], v[6], v[7], v[8]), xi_(v[9]) {} + +/* ************************************************************************* */ +Vector Cal3Unified::vector() const { + return (Vector(10) << Base::vector(), xi_); +} + +/* ************************************************************************* */ +void Cal3Unified::print(const std::string& s) const { + Base::print(s); + gtsam::print((Vector)(Vector(1) << xi_), s + ".xi"); +} + +/* ************************************************************************* */ +bool Cal3Unified::equals(const Cal3Unified& K, double tol) const { + if (fabs(fx_ - K.fx_) > tol || fabs(fy_ - K.fy_) > tol || fabs(s_ - K.s_) > tol || + fabs(u0_ - K.u0_) > tol || fabs(v0_ - K.v0_) > tol || fabs(k1_ - K.k1_) > tol || + fabs(k2_ - K.k2_) > tol || fabs(k3_ - K.k3_) > tol || fabs(k4_ - K.k4_) > tol || + fabs(xi_ - K.xi_) > tol) + return false; + return true; +} + +/* ************************************************************************* */ +Point2 Cal3Unified::uncalibrate(const Point2& p, + boost::optional H1, + boost::optional H2) const { + + // this part of code is modified from Cal3DS2, + // since the second part of this model (after project to normalized plane) + // is same as Cal3DS2 + + // parameters + const double xi = xi_; + + // Part1: project 3D space to NPlane + const double xs = p.x(), ys = p.y(); // normalized points in 3D space + const double sqrt_nx = sqrt(xs * xs + ys * ys + 1.0); + const double xi_sqrt_nx = 1.0 / (1 + xi * sqrt_nx); + const double xi_sqrt_nx2 = xi_sqrt_nx * xi_sqrt_nx; + const double x = xs * xi_sqrt_nx, y = ys * xi_sqrt_nx; // points on NPlane + + // Part2: project NPlane point to pixel plane: use Cal3DS2 + Point2 m(x,y); + Matrix H1base, H2base; // jacobians from Base class + Point2 puncalib = Base::uncalibrate(m, H1base, H2base); + + // Inlined derivative for calibration + if (H1) { + // part1 + Matrix DU = (Matrix(2,1) << -xs * sqrt_nx * xi_sqrt_nx2, + -ys * sqrt_nx * xi_sqrt_nx2); + Matrix DDS2U = H2base * DU; + + *H1 = collect(2, &H1base, &DDS2U); + } + // Inlined derivative for points + if (H2) { + // part1 + const double denom = 1.0 * xi_sqrt_nx2 / sqrt_nx; + const double mid = -(xi * xs*ys) * denom; + Matrix DU = (Matrix(2, 2) << + (sqrt_nx + xi*(ys*ys + 1)) * denom, mid, + mid, (sqrt_nx + xi*(xs*xs + 1)) * denom); + + *H2 = H2base * DU; + } + + return puncalib; +} + +/* ************************************************************************* */ +Point2 Cal3Unified::calibrate(const Point2& pi, const double tol) const { + + // calibrate point to Nplane use base class::calibrate() + Point2 pnplane = Base::calibrate(pi, tol); + + // call nplane to space + return this->nPlaneToSpace(pnplane); +} +/* ************************************************************************* */ +Point2 Cal3Unified::nPlaneToSpace(const Point2& p) const { + + const double x = p.x(), y = p.y(); + const double xy2 = x * x + y * y; + const double sq_xy = (xi_ + sqrt(1 + (1 - xi_ * xi_) * xy2)) / (xy2 + 1); + + return Point2((sq_xy * x / (sq_xy - xi_)), (sq_xy * y / (sq_xy - xi_))); +} + +/* ************************************************************************* */ +Point2 Cal3Unified::spaceToNPlane(const Point2& p) const { + + const double x = p.x(), y = p.y(); + const double sq_xy = 1 + xi_ * sqrt(x * x + y * y + 1); + + return Point2((x / sq_xy), (y / sq_xy)); +} + +/* ************************************************************************* */ +Cal3Unified Cal3Unified::retract(const Vector& d) const { + return Cal3Unified(vector() + d); +} + +/* ************************************************************************* */ +Vector Cal3Unified::localCoordinates(const Cal3Unified& T2) const { + return T2.vector() - vector(); +} + +} +/* ************************************************************************* */ + + diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h new file mode 100644 index 000000000..a1d47332a --- /dev/null +++ b/gtsam/geometry/Cal3Unified.h @@ -0,0 +1,146 @@ +/* ---------------------------------------------------------------------------- + + * 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 Cal3Unified.h + * @brief Unified Calibration Model, see Mei07icra for details + * @date Mar 8, 2014 + * @author Jing Dong + */ + +/** + * @addtogroup geometry + */ + +#pragma once + +#include +#include + +namespace gtsam { + +/** + * @brief Calibration of a omni-directional camera with mirror + lens radial distortion + * @addtogroup geometry + * \nosubgrouping + */ +class GTSAM_EXPORT Cal3Unified : public Cal3DS2 { + + typedef Cal3Unified This; + typedef Cal3DS2 Base; + +private: + + double xi_; // mirror parameter + + // K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ] + // Pn = [ P.x / (1 + xi * \sqrt{P.x^2 + P.y^2 + 1}), P.y / (1 + xi * \sqrt{P.x^2 + P.y^2 + 1})] + // rr = Pn.x^2 + Pn.y^2 + // \hat{pn} = (1 + k1*rr + k2*rr^2 ) pn + [ 2*k3 pn.x pn.y + k4 (rr + 2 Pn.x^2) ; + // k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ] + // pi = K*pn + +public: + //Matrix K() const ; + //Eigen::Vector4d k() const { return Base::k(); } + Vector vector() const ; + + /// @name Standard Constructors + /// @{ + + /// Default Constructor with only unit focal length + Cal3Unified() : Base(), xi_(0) {} + + Cal3Unified(double fx, double fy, double s, double u0, double v0, + double k1, double k2, double k3 = 0.0, double k4 = 0.0, double xi = 0.0) : + Base(fx, fy, s, u0, v0, k1, k2, k3, k4), xi_(xi) {} + + /// @} + /// @name Advanced Constructors + /// @{ + + Cal3Unified(const Vector &v) ; + + /// @} + /// @name Testable + /// @{ + + /// print with optional string + void print(const std::string& s = "") const ; + + /// assert equality up to a tolerance + bool equals(const Cal3Unified& K, double tol = 10e-9) const; + + /// @} + /// @name Standard Interface + /// @{ + + /// mirror parameter + inline double xi() const { return xi_;} + + /** + * convert intrinsic coordinates xy to image coordinates uv + * @param p point in intrinsic coordinates + * @param Dcal optional 2*9 Jacobian wrpt Cal3DS2 parameters + * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates + * @return point in image coordinates + */ + Point2 uncalibrate(const Point2& p, + boost::optional Dcal = boost::none, + boost::optional Dp = boost::none) const ; + + /// Conver a pixel coordinate to ideal coordinate + Point2 calibrate(const Point2& p, const double tol=1e-5) const; + + /// Convert a 3D point to normalized unit plane + Point2 spaceToNPlane(const Point2& p) const; + + /// Convert a normalized unit plane point to 3D space + Point2 nPlaneToSpace(const Point2& p) const; + + /// @} + /// @name Manifold + /// @{ + + /// Given delta vector, update calibration + Cal3Unified retract(const Vector& d) const ; + + /// Given a different calibration, calculate update to obtain it + Vector localCoordinates(const Cal3Unified& T2) const ; + + /// Return dimensions of calibration manifold object + virtual size_t dim() const { return 10 ; } //TODO: make a final dimension variable (also, usually size_t in other classes e.g. Pose2) + + /// Return dimensions of calibration manifold object + static size_t Dim() { return 10; } //TODO: make a final dimension variable + +private: + + /// @} + /// @name Advanced Interface + /// @{ + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) + { + ar & boost::serialization::make_nvp("Cal3Unified", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(xi_); + } + + /// @} + +}; + +} + diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index a62266046..c134860bb 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -186,7 +186,7 @@ public: /// Unretraction for the calibration Vector localCoordinates(const Cal3_S2& T2) const { - return vector() - T2.vector(); + return T2.vector() - vector(); } /// @} diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index 5e0376e92..6e7b5a114 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -24,10 +24,11 @@ namespace gtsam { -class GTSAM_EXPORT CheiralityException: public std::runtime_error { +class GTSAM_EXPORT CheiralityException: public ThreadsafeException< + CheiralityException> { public: CheiralityException() : - std::runtime_error("Cheirality Exception") { + ThreadsafeException("Cheirality Exception") { } }; @@ -150,8 +151,9 @@ public: * @param Dpoint the optionally computed Jacobian with respect to the 3D point * @return the intrinsic coordinates of the projected point */ - Point2 project(const Point3& point, boost::optional Dpose = - boost::none, boost::optional Dpoint = boost::none) const; + Point2 project(const Point3& point, + boost::optional Dpose = boost::none, + boost::optional Dpoint = boost::none) const; /** * projects a 3-dimensional point in camera coordinates into the @@ -212,12 +214,12 @@ private: friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("CalibratedCamera", - boost::serialization::base_object(*this)); + ar + & boost::serialization::make_nvp("CalibratedCamera", + boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(pose_); } /// @} -}; -} + };} diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index 7de68c7a1..3c9568c3d 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -20,7 +20,7 @@ namespace gtsam { * but here we choose instead to parameterize it as a (Rot3,Unit3) pair. * We can then non-linearly optimize immediately on this 5-dimensional manifold. */ -class EssentialMatrix: public DerivedValue { +class GTSAM_EXPORT EssentialMatrix: public DerivedValue { private: @@ -150,10 +150,37 @@ public: /// @{ /// stream to stream - friend std::ostream& operator <<(std::ostream& os, const EssentialMatrix& E); + GTSAM_EXPORT friend std::ostream& operator <<(std::ostream& os, const EssentialMatrix& E); /// stream from stream - friend std::istream& operator >>(std::istream& is, EssentialMatrix& E); + GTSAM_EXPORT friend std::istream& operator >>(std::istream& is, EssentialMatrix& E); + + /// @} + +private: + + /// @name Advanced Interface + /// @{ + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("EssentialMatrix", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(aRb_); + ar & BOOST_SERIALIZATION_NVP(aTb_); + + ar & boost::serialization::make_nvp("E11", E_(0,0)); + ar & boost::serialization::make_nvp("E12", E_(0,1)); + ar & boost::serialization::make_nvp("E13", E_(0,2)); + ar & boost::serialization::make_nvp("E21", E_(1,0)); + ar & boost::serialization::make_nvp("E22", E_(1,1)); + ar & boost::serialization::make_nvp("E23", E_(1,2)); + ar & boost::serialization::make_nvp("E31", E_(2,0)); + ar & boost::serialization::make_nvp("E32", E_(2,1)); + ar & boost::serialization::make_nvp("E33", E_(2,2)); + } /// @} diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index c162171c0..19eb87b5f 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -271,6 +271,10 @@ public: */ inline static Point2 project_to_camera(const Point3& P, boost::optional Dpoint = boost::none) { +#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION + if (P.z() <= 0) + throw CheiralityException(); +#endif if (Dpoint) { double d = 1.0 / P.z(), d2 = d * d; *Dpoint = (Matrix(2, 3) << d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2); @@ -300,40 +304,25 @@ public: // Transform to camera coordinates and check cheirality const Point3 pc = pose_.transform_to(pw); -#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION - if (pc.z() <= 0) - throw CheiralityException(); -#endif - // Project to normalized image coordinates const Point2 pn = project_to_camera(pc); if (Dpose || Dpoint) { - // optimized version of derivatives, see CalibratedCamera.nb const double z = pc.z(), d = 1.0 / z; - const double u = pn.x(), v = pn.y(); - Matrix Dpn_pose(2, 6), Dpn_point(2, 3); - if (Dpose) { - double uv = u * v, uu = u * u, vv = v * v; - Dpn_pose << uv, -1 - uu, v, -d, 0, d * u, 1 + vv, -uv, -u, 0, -d, d * v; - } - if (Dpoint) { - const Matrix R(pose_.rotation().matrix()); - Dpn_point << // - R(0, 0) - u * R(0, 2), R(1, 0) - u * R(1, 2), R(2, 0) - u * R(2, 2), // - /**/R(0, 1) - v * R(0, 2), R(1, 1) - v * R(1, 2), R(2, 1) - v * R(2, 2); - Dpn_point *= d; - } // uncalibration - Matrix Dpi_pn; // 2*2 + Matrix Dpi_pn(2, 2); const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn); // chain the Jacobian matrices - if (Dpose) - *Dpose = Dpi_pn * Dpn_pose; - if (Dpoint) - *Dpoint = Dpi_pn * Dpn_point; + if (Dpose) { + Dpose->resize(2, 6); + calculateDpose(pn, d, Dpi_pn, *Dpose); + } + if (Dpoint) { + Dpoint->resize(2, 3); + calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint); + } return pi; } else return K_.uncalibrate(pn, Dcal); @@ -391,27 +380,29 @@ public: boost::optional Dcamera = boost::none, boost::optional Dpoint = boost::none) const { + const Point3 pc = pose_.transform_to(pw); + const Point2 pn = project_to_camera(pc); + if (!Dcamera && !Dpoint) { - const Point3 pc = pose_.transform_to(pw); - -#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION - if (pc.z() <= 0) - throw CheiralityException(); -#endif - const Point2 pn = project_to_camera(pc); return K_.uncalibrate(pn); - } + } else { + const double z = pc.z(), d = 1.0 / z; - Matrix Dpose, Dp, Dcal; - const Point2 pi = this->project(pw, Dpose, Dp, Dcal); - if (Dcamera) { - *Dcamera = Matrix(2, this->dim()); - Dcamera->leftCols(pose().dim()) = Dpose; // Jacobian wrt pose3 - Dcamera->rightCols(calibration().dim()) = Dcal; // Jacobian wrt calib + // uncalibration + Matrix Dcal, Dpi_pn(2, 2); + const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn); + + if (Dcamera) { + Dcamera->resize(2, this->dim()); + calculateDpose(pn, d, Dpi_pn, Dcamera->leftCols<6>()); + Dcamera->rightCols(K_.dim()) = Dcal; // Jacobian wrt calib + } + if (Dpoint) { + Dpoint->resize(2, 3); + calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint); + } + return pi; } - if (Dpoint) - *Dpoint = Dp; - return pi; } /// backproject a 2-dimensional point to a 3-dimensional point at given depth @@ -516,6 +507,50 @@ public: private: + /** + * Calculate Jacobian with respect to pose + * @param pn projection in normalized coordinates + * @param d disparity (inverse depth) + * @param Dpi_pn derivative of uncalibrate with respect to pn + * @param Dpose Output argument, can be matrix or block, assumed right size ! + * See http://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html + */ + template + static void calculateDpose(const Point2& pn, double d, const Matrix& Dpi_pn, + Eigen::MatrixBase const & Dpose) { + // optimized version of derivatives, see CalibratedCamera.nb + const double u = pn.x(), v = pn.y(); + double uv = u * v, uu = u * u, vv = v * v; + Eigen::Matrix Dpn_pose; + Dpn_pose << uv, -1 - uu, v, -d, 0, d * u, 1 + vv, -uv, -u, 0, -d, d * v; + assert(Dpose.rows()==2 && Dpose.cols()==6); + const_cast&>(Dpose) = // + Dpi_pn.block<2, 2>(0, 0) * Dpn_pose; + } + + /** + * Calculate Jacobian with respect to point + * @param pn projection in normalized coordinates + * @param d disparity (inverse depth) + * @param Dpi_pn derivative of uncalibrate with respect to pn + * @param Dpoint Output argument, can be matrix or block, assumed right size ! + * See http://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html + */ + template + static void calculateDpoint(const Point2& pn, double d, const Matrix& R, + const Matrix& Dpi_pn, Eigen::MatrixBase const & Dpoint) { + // optimized version of derivatives, see CalibratedCamera.nb + const double u = pn.x(), v = pn.y(); + Eigen::Matrix Dpn_point; + Dpn_point << // + R(0, 0) - u * R(0, 2), R(1, 0) - u * R(1, 2), R(2, 0) - u * R(2, 2), // + /**/R(0, 1) - v * R(0, 2), R(1, 1) - v * R(1, 2), R(2, 1) - v * R(2, 2); + Dpn_point *= d; + assert(Dpoint.rows()==2 && Dpoint.cols()==3); + const_cast&>(Dpoint) = // + Dpi_pn.block<2, 2>(0, 0) * Dpn_point; + } + /// @} /// @name Advanced Interface /// @{ diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index 0838650ea..dc9a1dac8 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -172,7 +172,7 @@ public: static inline Point2 Expmap(const Vector& v) { return Point2(v); } /// Log map around identity - just return the Point2 as a vector - static inline Vector Logmap(const Point2& dp) { return Vector_(2, dp.x(), dp.y()); } + static inline Vector Logmap(const Point2& dp) { return (Vector(2) << dp.x(), dp.y()); } /// @} /// @name Vector Space diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index adbe763b3..bfd2fcb9a 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -98,7 +98,7 @@ Vector Pose3::adjointTranspose(const Vector& xi, const Vector& y, /* ************************************************************************* */ Matrix6 Pose3::dExpInv_exp(const Vector& xi) { // Bernoulli numbers, from Wikipedia - static const Vector B = Vector_(9, 1.0, -1.0 / 2.0, 1. / 6., 0.0, -1.0 / 30.0, + static const Vector B = (Vector(9) << 1.0, -1.0 / 2.0, 1. / 6., 0.0, -1.0 / 30.0, 0.0, 1.0 / 42.0, 0.0, -1.0 / 30); static const int N = 5; // order of approximation Matrix res = I6; diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index 45012770f..d121beb12 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -170,7 +170,7 @@ namespace gtsam { ///Log map at identity - return the canonical coordinates of this rotation static inline Vector Logmap(const Rot2& r) { - return Vector_(1, r.theta()); + return (Vector(1) << r.theta()); } /// @} diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 12fec368d..37aa78a78 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -45,10 +45,10 @@ Rot3 Rot3::rodriguez(const Unit3& w, double theta) { } /* ************************************************************************* */ -Rot3 Rot3::Random(boost::random::mt19937 & rng) { +Rot3 Rot3::Random(boost::mt19937 & rng) { // TODO allow any engine without including all of boost :-( Unit3 w = Unit3::Random(rng); - boost::random::uniform_real_distribution randomAngle(-M_PI,M_PI); + boost::uniform_real randomAngle(-M_PI,M_PI); double angle = randomAngle(rng); return rodriguez(w,angle); } @@ -73,7 +73,7 @@ Point3 Rot3::operator*(const Point3& p) const { /* ************************************************************************* */ Unit3 Rot3::rotate(const Unit3& p, boost::optional HR, boost::optional Hp) const { - Unit3 q = rotate(p.point3(Hp)); + Unit3 q = Unit3(rotate(p.point3(Hp))); if (Hp) (*Hp) = q.basis().transpose() * matrix() * (*Hp); if (HR) @@ -84,7 +84,7 @@ Unit3 Rot3::rotate(const Unit3& p, /* ************************************************************************* */ Unit3 Rot3::unrotate(const Unit3& p, boost::optional HR, boost::optional Hp) const { - Unit3 q = unrotate(p.point3(Hp)); + Unit3 q = Unit3(unrotate(p.point3(Hp))); if (Hp) (*Hp) = q.basis().transpose() * matrix().transpose () * (*Hp); if (HR) @@ -101,10 +101,9 @@ Unit3 Rot3::operator*(const Unit3& p) const { // see doc/math.lyx, SO(3) section Point3 Rot3::unrotate(const Point3& p, boost::optional H1, boost::optional H2) const { - const Matrix Rt(transpose()); - Point3 q(Rt*p.vector()); // q = Rt*p + Point3 q(transpose()*p.vector()); // q = Rt*p if (H1) *H1 = skewSymmetric(q.x(), q.y(), q.z()); - if (H2) *H2 = Rt; + if (H2) *H2 = transpose(); return q; } @@ -175,6 +174,39 @@ Vector Rot3::quaternion() const { return v; } +/* ************************************************************************* */ +Matrix3 Rot3::rightJacobianExpMapSO3(const Vector3& x) { + // x is the axis-angle representation (exponential coordinates) for a rotation + double normx = norm_2(x); // rotation angle + Matrix3 Jr; + if (normx < 10e-8){ + Jr = Matrix3::Identity(); + } + else{ + const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ + Jr = Matrix3::Identity() - ((1-cos(normx))/(normx*normx)) * X + + ((normx-sin(normx))/(normx*normx*normx)) * X * X; // right Jacobian + } + return Jr; +} + +/* ************************************************************************* */ +Matrix3 Rot3::rightJacobianExpMapSO3inverse(const Vector3& x) { + // x is the axis-angle representation (exponential coordinates) for a rotation + double normx = norm_2(x); // rotation angle + Matrix3 Jrinv; + + if (normx < 10e-8){ + Jrinv = Matrix3::Identity(); + } + else{ + const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ + Jrinv = Matrix3::Identity() + + 0.5 * X + (1/(normx*normx) - (1+cos(normx))/(2*normx * sin(normx)) ) * X * X; + } + return Jrinv; +} + /* ************************************************************************* */ pair RQ(const Matrix3& A) { diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index cface1751..c8aeae51b 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -104,7 +104,7 @@ namespace gtsam { Rot3(const Quaternion& q); /// Random, generates a random axis, then random angle \in [-p,pi] - static Rot3 Random(boost::random::mt19937 & rng); + static Rot3 Random(boost::mt19937 & rng); /** Virtual destructor */ virtual ~Rot3() {} @@ -194,7 +194,7 @@ namespace gtsam { * @return incremental rotation matrix */ static Rot3 rodriguez(double wx, double wy, double wz) - { return rodriguez(Vector_(3,wx,wy,wz));} + { return rodriguez((Vector(3) << wx, wy, wz));} /// @} /// @name Testable @@ -304,6 +304,17 @@ namespace gtsam { /// Left-trivialized derivative inverse of the exponential map static Matrix3 dexpInvL(const Vector3& v); + /** + * Right Jacobian for Exponential map in SO(3) - equation (10.86) and following equations in + * G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. + */ + static Matrix3 rightJacobianExpMapSO3(const Vector3& x); + + /** Right Jacobian for Log map in SO(3) - equation (10.86) and following equations in + * G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. + */ + static Matrix3 rightJacobianExpMapSO3inverse(const Vector3& x); + /// @} /// @name Group Action on Point3 /// @{ diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index 43b29f0bf..88359e0f8 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -22,6 +22,7 @@ #include #include #include +#include #include using namespace std; @@ -43,10 +44,14 @@ Unit3 Unit3::FromPoint3(const Point3& point, boost::optional H) { } /* ************************************************************************* */ -Unit3 Unit3::Random(boost::random::mt19937 & rng) { +Unit3 Unit3::Random(boost::mt19937 & rng) { // TODO allow any engine without including all of boost :-( - boost::random::uniform_on_sphere randomDirection(3); - vector d = randomDirection(rng); + boost::uniform_on_sphere randomDirection(3); + // This variate_generator object is required for versions of boost somewhere + // around 1.46, instead of drawing directly using boost::uniform_on_sphere(rng). + boost::variate_generator > + generator(rng, randomDirection); + vector d = generator(); Unit3 result; result.p_ = Point3(d[0], d[1], d[2]); return result; diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index e6e7c8e95..39e2c9799 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -22,23 +22,12 @@ #include #include - -// (Cumbersome) forward declaration for random generator -namespace boost { -namespace random { -template -class mersenne_twister_engine; -typedef mersenne_twister_engine mt19937; -} -} +#include namespace gtsam { /// Represents a 3D point on a unit sphere. -class Unit3: public DerivedValue { +class GTSAM_EXPORT Unit3: public DerivedValue { private: @@ -56,7 +45,7 @@ public: } /// Construct from point - Unit3(const Point3& p) : + explicit Unit3(const Point3& p) : p_(p / p.norm()) { } @@ -71,7 +60,7 @@ public: boost::none); /// Random direction, using boost::uniform_on_sphere - static Unit3 Random(boost::random::mt19937 & rng); + static Unit3 Random(boost::mt19937 & rng); /// @} @@ -147,6 +136,24 @@ public: Vector localCoordinates(const Unit3& s) const; /// @} + +private: + + /// @name Advanced Interface + /// @{ + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("Unit3", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(p_); + ar & BOOST_SERIALIZATION_NVP(B_); + } + + /// @} + }; } // namespace gtsam diff --git a/gtsam/geometry/tests/CMakeLists.txt b/gtsam/geometry/tests/CMakeLists.txt new file mode 100644 index 000000000..b1499ee9d --- /dev/null +++ b/gtsam/geometry/tests/CMakeLists.txt @@ -0,0 +1 @@ +gtsamAddTestsGlob(geometry "test*.cpp" "" "gtsam") diff --git a/gtsam/geometry/tests/testCal3Bundler.cpp b/gtsam/geometry/tests/testCal3Bundler.cpp index 7cec17b34..6e62d4be0 100644 --- a/gtsam/geometry/tests/testCal3Bundler.cpp +++ b/gtsam/geometry/tests/testCal3Bundler.cpp @@ -82,6 +82,7 @@ TEST( Cal3Bundler, retract) d << 10, 1e-3, 1e-3; Cal3Bundler actual = K.retract(d); CHECK(assert_equal(expected,actual,1e-7)); + CHECK(assert_equal(d,K.localCoordinates(actual),1e-7)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testCal3DS2.cpp b/gtsam/geometry/tests/testCal3DS2.cpp index c73ae1182..fc457767e 100644 --- a/gtsam/geometry/tests/testCal3DS2.cpp +++ b/gtsam/geometry/tests/testCal3DS2.cpp @@ -36,7 +36,7 @@ TEST( Cal3DS2, uncalibrate) double g = 1+k[0]*r+k[1]*r*r ; double tx = 2*k[2]*p.x()*p.y() + k[3]*(r+2*p.x()*p.x()) ; double ty = k[2]*(r+2*p.y()*p.y()) + 2*k[3]*p.x()*p.y() ; - Vector v_hat = Vector_(3, g*p.x() + tx, g*p.y() + ty, 1.0) ; + Vector v_hat = (Vector(3) << g*p.x() + tx, g*p.y() + ty, 1.0) ; Vector v_i = K.K() * v_hat ; Point2 p_i(v_i(0)/v_i(2), v_i(1)/v_i(2)) ; Point2 q = K.uncalibrate(p); @@ -76,6 +76,18 @@ TEST( Cal3DS2, assert_equal) CHECK(assert_equal(K,K,1e-5)); } +/* ************************************************************************* */ +TEST( Cal3DS2, retract) +{ + Cal3DS2 expected(500 + 1, 100 + 2, 0.1 + 3, 320 + 4, 240 + 5, 1e-3 + 6, + 2.0 * 1e-3 + 7, 3.0 * 1e-3 + 8, 4.0 * 1e-3 + 9); + Vector d(9); + d << 1,2,3,4,5,6,7,8,9; + Cal3DS2 actual = K.retract(d); + CHECK(assert_equal(expected,actual,1e-7)); + CHECK(assert_equal(d,K.localCoordinates(actual),1e-7)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testCal3Unified.cpp b/gtsam/geometry/tests/testCal3Unified.cpp new file mode 100644 index 000000000..b260415f1 --- /dev/null +++ b/gtsam/geometry/tests/testCal3Unified.cpp @@ -0,0 +1,102 @@ +/* ---------------------------------------------------------------------------- + + * 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 testCal3Unify.cpp + * @brief Unit tests for transform derivatives + */ + +#include +#include +#include +#include + +using namespace gtsam; + +GTSAM_CONCEPT_TESTABLE_INST(Cal3Unified) +GTSAM_CONCEPT_MANIFOLD_INST(Cal3Unified) + +/* +ground truth from matlab, code : +X = [0.5 0.7 1]'; +V = [0.1, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0, 0, 100, 105, 320, 240]; +[P, J] = spaceToImgPlane(X, V); +matlab toolbox available at http://homepages.laas.fr/~cmei/index.php/Toolbox +*/ + +static Cal3Unified K(100, 105, 0.0, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0.1); +static Point2 p(0.5, 0.7); + +/* ************************************************************************* */ +TEST( Cal3Unified, uncalibrate) +{ + Point2 p_i(364.7791831734982, 305.6677211952602) ; + Point2 q = K.uncalibrate(p); + CHECK(assert_equal(q,p_i)); +} + +/* ************************************************************************* */ +TEST( Cal3Unified, spaceNplane) +{ + Point2 q = K.spaceToNPlane(p); + CHECK(assert_equal(Point2(0.441731600049497, 0.618424240069295), q)); + CHECK(assert_equal(p, K.nPlaneToSpace(q))); +} + +/* ************************************************************************* */ +TEST( Cal3Unified, calibrate) +{ + Point2 pi = K.uncalibrate(p); + Point2 pn_hat = K.calibrate(pi); + CHECK( p.equals(pn_hat, 1e-8)); +} + +Point2 uncalibrate_(const Cal3Unified& k, const Point2& pt) { return k.uncalibrate(pt); } + +/* ************************************************************************* */ +TEST( Cal3Unified, Duncalibrate1) +{ + Matrix computed; + K.uncalibrate(p, computed, boost::none); + Matrix numerical = numericalDerivative21(uncalibrate_, K, p, 1e-7); + CHECK(assert_equal(numerical,computed,1e-6)); +} + +/* ************************************************************************* */ +TEST( Cal3Unified, Duncalibrate2) +{ + Matrix computed; + K.uncalibrate(p, boost::none, computed); + Matrix numerical = numericalDerivative22(uncalibrate_, K, p, 1e-7); + CHECK(assert_equal(numerical,computed,1e-6)); +} + +/* ************************************************************************* */ +TEST( Cal3Unified, assert_equal) +{ + CHECK(assert_equal(K,K,1e-9)); +} + +/* ************************************************************************* */ +TEST( Cal3Unified, retract) +{ + Cal3Unified expected(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, + 1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10, 0.1 + 1); + Vector d(10); + d << 2, 3, 4, 5, 6, 7, 8, 9, 10, 1; + Cal3Unified actual = K.retract(d); + CHECK(assert_equal(expected,actual,1e-9)); + CHECK(assert_equal(d,K.localCoordinates(actual),1e-9)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testCal3_S2.cpp b/gtsam/geometry/tests/testCal3_S2.cpp index e1dba2272..d77a534d6 100644 --- a/gtsam/geometry/tests/testCal3_S2.cpp +++ b/gtsam/geometry/tests/testCal3_S2.cpp @@ -82,6 +82,17 @@ TEST( Cal3_S2, assert_equal) CHECK(assert_equal(K,K1,1e-9)); } +/* ************************************************************************* */ +TEST( Cal3_S2, retract) +{ + Cal3_S2 expected(500+1, 500+2, 0.1+3, 640 / 2+4, 480 / 2+5); + Vector d(5); + d << 1,2,3,4,5; + Cal3_S2 actual = K.retract(d); + CHECK(assert_equal(expected,actual,1e-7)); + CHECK(assert_equal(d,K.localCoordinates(actual),1e-7)); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index 207b75211..9ad30bc41 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -22,11 +22,11 @@ GTSAM_CONCEPT_MANIFOLD_INST(EssentialMatrix) // Create two cameras and corresponding essential matrix E Rot3 c1Rc2 = Rot3::yaw(M_PI_2); Point3 c1Tc2(0.1, 0, 0); -EssentialMatrix trueE(c1Rc2, c1Tc2); +EssentialMatrix trueE(c1Rc2, Unit3(c1Tc2)); //************************************************************************* TEST (EssentialMatrix, equality) { - EssentialMatrix actual(c1Rc2, c1Tc2), expected(c1Rc2, c1Tc2); + EssentialMatrix actual(c1Rc2, Unit3(c1Tc2)), expected(c1Rc2, Unit3(c1Tc2)); EXPECT(assert_equal(expected, actual)); } @@ -62,7 +62,7 @@ TEST (EssentialMatrix, retract0) { //************************************************************************* TEST (EssentialMatrix, retract1) { - EssentialMatrix expected(c1Rc2.retract((Vector(3) << 0.1, 0, 0)), c1Tc2); + EssentialMatrix expected(c1Rc2.retract((Vector(3) << 0.1, 0, 0)), Unit3(c1Tc2)); EssentialMatrix actual = trueE.retract((Vector(5) << 0.1, 0, 0, 0, 0)); EXPECT(assert_equal(expected, actual)); } @@ -84,7 +84,7 @@ TEST (EssentialMatrix, transform_to) { Rot3 aRb2 = Rot3::yaw(M_PI / 3.0) * Rot3::pitch(M_PI_4) * Rot3::roll(M_PI / 6.0); Point3 aTb2(19.2, 3.7, 5.9); - EssentialMatrix E(aRb2, aTb2); + EssentialMatrix E(aRb2, Unit3(aTb2)); //EssentialMatrix E(aRb, Unit3(aTb).retract((Vector(2) << 0.1, 0))); static Point3 P(0.2, 0.7, -2); Matrix actH1, actH2; @@ -109,7 +109,7 @@ TEST (EssentialMatrix, rotate) { // Let's compute the ground truth E in body frame: Rot3 b1Rb2 = bRc * c1Rc2 * cRb; Point3 b1Tb2 = bRc * c1Tc2; - EssentialMatrix bodyE(b1Rb2, b1Tb2); + EssentialMatrix bodyE(b1Rb2, Unit3(b1Tb2)); EXPECT(assert_equal(bodyE, bRc * trueE, 1e-8)); EXPECT(assert_equal(bodyE, trueE.rotate(bRc), 1e-8)); @@ -125,7 +125,7 @@ TEST (EssentialMatrix, rotate) { } // avoid exception Matrix expH1 = numericalDerivative21(rotate_, bodyE, cRb), // expH2 = numericalDerivative22(rotate_, bodyE, cRb); - EXPECT(assert_equal(expH1, actH1, 1e-8)); + EXPECT(assert_equal(expH1, actH1, 1e-7)); // Does not work yet EXPECT(assert_equal(expH2, actH2, 1e-8)); } @@ -144,17 +144,17 @@ TEST (EssentialMatrix, FromPose3_b) { Matrix actualH; Rot3 c1Rc2 = Rot3::ypr(0.1, -0.2, 0.3); Point3 c1Tc2(0.4, 0.5, 0.6); - EssentialMatrix trueE(c1Rc2, c1Tc2); + EssentialMatrix trueE(c1Rc2, Unit3(c1Tc2)); Pose3 pose(c1Rc2, c1Tc2); // Pose between two cameras EXPECT(assert_equal(trueE, EssentialMatrix::FromPose3(pose, actualH), 1e-8)); Matrix expectedH = numericalDerivative11( boost::bind(EssentialMatrix::FromPose3, _1, boost::none), pose); - EXPECT(assert_equal(expectedH, actualH, 1e-8)); + EXPECT(assert_equal(expectedH, actualH, 1e-5)); } //************************************************************************* TEST (EssentialMatrix, streaming) { - EssentialMatrix expected(c1Rc2, c1Tc2), actual; + EssentialMatrix expected(c1Rc2, Unit3(c1Tc2)), actual; stringstream ss; ss << expected; ss >> actual; diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index 594db159b..6ed49d0d9 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -169,38 +169,54 @@ static Point2 project3(const Pose3& pose, const Point3& point, const Cal3_S2& ca } /* ************************************************************************* */ -static Point2 projectInfinity3(const Pose3& pose, const Point2& point2D, const Cal3_S2& cal) { - Point3 point(point2D.x(), point2D.y(), 1.0); - return Camera(pose,cal).projectPointAtInfinity(point); -} - -/* ************************************************************************* */ -TEST( PinholeCamera, Dproject_point_pose) +TEST( PinholeCamera, Dproject) { Matrix Dpose, Dpoint, Dcal; Point2 result = camera.project(point1, Dpose, Dpoint, Dcal); Matrix numerical_pose = numericalDerivative31(project3, pose1, point1, K); Matrix numerical_point = numericalDerivative32(project3, pose1, point1, K); Matrix numerical_cal = numericalDerivative33(project3, pose1, point1, K); - CHECK(assert_equal(result, Point2(-100, 100) )); - CHECK(assert_equal(Dpose, numerical_pose, 1e-7)); - CHECK(assert_equal(Dpoint, numerical_point,1e-7)); - CHECK(assert_equal(Dcal, numerical_cal,1e-7)); + CHECK(assert_equal(Point2(-100, 100), result)); + CHECK(assert_equal(numerical_pose, Dpose, 1e-7)); + CHECK(assert_equal(numerical_point, Dpoint, 1e-7)); + CHECK(assert_equal(numerical_cal, Dcal, 1e-7)); } /* ************************************************************************* */ -TEST( PinholeCamera, Dproject_point_pose_Infinity) +//static Point2 projectInfinity3(const Pose3& pose, const Point2& point2D, const Cal3_S2& cal) { +// Point3 point(point2D.x(), point2D.y(), 1.0); +// return Camera(pose,cal).projectPointAtInfinity(point); +//} +// +//TEST( PinholeCamera, Dproject_Infinity) +//{ +// Matrix Dpose, Dpoint, Dcal; +// Point2 point2D(-0.08,-0.08); +// Point3 point3D(point1.x(), point1.y(), 1.0); +// Point2 result = camera.projectPointAtInfinity(point3D, Dpose, Dpoint, Dcal); +// Matrix numerical_pose = numericalDerivative31(projectInfinity3, pose1, point2D, K); +// Matrix numerical_point = numericalDerivative32(projectInfinity3, pose1, point2D, K); +// Matrix numerical_cal = numericalDerivative33(projectInfinity3, pose1, point2D, K); +// CHECK(assert_equal(numerical_pose, Dpose, 1e-7)); +// CHECK(assert_equal(numerical_point, Dpoint, 1e-7)); +// CHECK(assert_equal(numerical_cal, Dcal, 1e-7)); +//} +// +/* ************************************************************************* */ +static Point2 project4(const Camera& camera, const Point3& point) { + return camera.project2(point); +} + +/* ************************************************************************* */ +TEST( PinholeCamera, Dproject2) { - Matrix Dpose, Dpoint, Dcal; - Point2 point2D(-0.08,-0.08); - Point3 point3D(point1.x(), point1.y(), 1.0); - Point2 result = camera.projectPointAtInfinity(point3D, Dpose, Dpoint, Dcal); - Matrix numerical_pose = numericalDerivative31(projectInfinity3, pose1, point2D, K); - Matrix numerical_point = numericalDerivative32(projectInfinity3, pose1, point2D, K); - Matrix numerical_cal = numericalDerivative33(projectInfinity3, pose1, point2D, K); - CHECK(assert_equal(Dpose, numerical_pose, 1e-7)); - CHECK(assert_equal(Dpoint, numerical_point,1e-7)); - CHECK(assert_equal(Dcal, numerical_cal,1e-7)); + Matrix Dcamera, Dpoint; + Point2 result = camera.project2(point1, Dcamera, Dpoint); + Matrix numerical_camera = numericalDerivative21(project4, camera, point1); + Matrix numerical_point = numericalDerivative22(project4, camera, point1); + CHECK(assert_equal(result, Point2(-100, 100) )); + CHECK(assert_equal(numerical_camera, Dcamera, 1e-7)); + CHECK(assert_equal(numerical_point, Dpoint, 1e-7)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index 58c59e83d..65c610c25 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -41,7 +41,7 @@ TEST(Point3, Lie) { EXPECT(assert_equal(eye(3), H2)); EXPECT(assert_equal(Point3(5, 7, 9), p1.retract((Vector(3) << 4., 5., 6.)))); - EXPECT(assert_equal(Vector_(3, 3., 3., 3.), p1.localCoordinates(p2))); + EXPECT(assert_equal((Vector)(Vector(3) << 3.,3.,3.), p1.localCoordinates(p2))); } /* ************************************************************************* */ @@ -81,7 +81,7 @@ TEST( Point3, stream) { TEST (Point3, normalize) { Matrix actualH; Point3 point(1, -2, 3); // arbitrary point - Point3 expected(point / sqrt(14)); + Point3 expected(point / sqrt(14.0)); EXPECT(assert_equal(expected, point.normalize(actualH), 1e-8)); Matrix expectedH = numericalDerivative11( boost::bind(&Point3::normalize, _1, boost::none), point); diff --git a/gtsam/geometry/tests/testRot2.cpp b/gtsam/geometry/tests/testRot2.cpp index 3de661224..c67031a13 100644 --- a/gtsam/geometry/tests/testRot2.cpp +++ b/gtsam/geometry/tests/testRot2.cpp @@ -96,7 +96,7 @@ TEST(Rot2, logmap) { Rot2 rot0(Rot2::fromAngle(M_PI/2.0)); Rot2 rot(Rot2::fromAngle(M_PI)); - Vector expected = Vector_(1, M_PI/2.0); + Vector expected = (Vector(1) << M_PI/2.0); Vector actual = rot0.localCoordinates(rot); CHECK(assert_equal(expected, actual)); } diff --git a/gtsam/geometry/tests/testRot3M.cpp b/gtsam/geometry/tests/testRot3M.cpp index 7b80e8ee9..f0e60ba03 100644 --- a/gtsam/geometry/tests/testRot3M.cpp +++ b/gtsam/geometry/tests/testRot3M.cpp @@ -200,6 +200,37 @@ TEST(Rot3, log) CHECK_OMEGA_ZERO(x*2.*PI,y*2.*PI,z*2.*PI) } +Rot3 evaluateRotation(const Vector3 thetahat){ + return Rot3::Expmap(thetahat); +} + +Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta){ + return Rot3::Logmap( Rot3::Expmap(thetahat).compose( Rot3::Expmap(deltatheta) ) ); +} + +/* ************************************************************************* */ +TEST( Rot3, rightJacobianExpMapSO3 ) +{ + // Linearization point + Vector3 thetahat; thetahat << 0.1, 0, 0; + + Matrix expectedJacobian = numericalDerivative11(boost::bind(&evaluateRotation, _1), LieVector(thetahat)); + Matrix actualJacobian = Rot3::rightJacobianExpMapSO3(thetahat); + EXPECT(assert_equal(expectedJacobian, actualJacobian)); +} + +/* ************************************************************************* */ +TEST( Rot3, rightJacobianExpMapSO3inverse ) +{ + // Linearization point + Vector3 thetahat; thetahat << 0.1,0.1,0; ///< Current estimate of rotation rate bias + Vector3 deltatheta; deltatheta << 0, 0, 0; + + Matrix expectedJacobian = numericalDerivative11(boost::bind(&evaluateLogRotation, thetahat, _1), LieVector(deltatheta)); + Matrix actualJacobian = Rot3::rightJacobianExpMapSO3inverse(thetahat); + EXPECT(assert_equal(expectedJacobian, actualJacobian)); +} + /* ************************************************************************* */ TEST(Rot3, manifold_caley) { diff --git a/gtsam/geometry/tests/testSphere2.cpp b/gtsam/geometry/tests/testSphere2.cpp index 92f9a9758..eb45ea60f 100644 --- a/gtsam/geometry/tests/testSphere2.cpp +++ b/gtsam/geometry/tests/testSphere2.cpp @@ -26,6 +26,7 @@ #include #include #include +#include #include #include @@ -43,7 +44,7 @@ Point3 point3_(const Unit3& p) { TEST(Unit3, point3) { vector ps; ps += Point3(1, 0, 0), Point3(0, 1, 0), Point3(0, 0, 1), Point3(1, 1, 0) - / sqrt(2); + / sqrt(2.0); Matrix actualH, expectedH; BOOST_FOREACH(Point3 p,ps) { Unit3 s(p); @@ -227,13 +228,13 @@ inline static Vector randomVector(const Vector& minLimits, TEST(Unit3, localCoordinates_retract) { size_t numIterations = 10000; - Vector minSphereLimit = Vector_(3, -1.0, -1.0, -1.0), maxSphereLimit = - Vector_(3, 1.0, 1.0, 1.0); - Vector minXiLimit = Vector_(2, -1.0, -1.0), maxXiLimit = Vector_(2, 1.0, 1.0); + Vector minSphereLimit = (Vector(3) << -1.0, -1.0, -1.0), maxSphereLimit = + (Vector(3) << 1.0, 1.0, 1.0); + Vector minXiLimit = (Vector(2) << -1.0, -1.0), maxXiLimit = (Vector(2) << 1.0, 1.0); for (size_t i = 0; i < numIterations; i++) { // Sleep for the random number generator (TODO?: Better create all of them first). - sleep(0); + boost::this_thread::sleep(boost::posix_time::milliseconds(0)); // Create the two Unit3s. // NOTE: You can not create two totally random Unit3's because you cannot always compute @@ -257,14 +258,13 @@ TEST(Unit3, localCoordinates_retract) { TEST(Unit3, localCoordinates_retract_expmap) { size_t numIterations = 10000; - Vector minSphereLimit = Vector_(3, -1.0, -1.0, -1.0), maxSphereLimit = - Vector_(3, 1.0, 1.0, 1.0); - Vector minXiLimit = Vector_(2, -M_PI, -M_PI), maxXiLimit = Vector_(2, M_PI, - M_PI); + Vector minSphereLimit = (Vector(3) << -1.0, -1.0, -1.0), maxSphereLimit = + (Vector(3) << 1.0, 1.0, 1.0); + Vector minXiLimit = (Vector(2) << -M_PI, -M_PI), maxXiLimit = (Vector(2) << M_PI, M_PI); for (size_t i = 0; i < numIterations; i++) { // Sleep for the random number generator (TODO?: Better create all of them first). - sleep(0); + boost::this_thread::sleep(boost::posix_time::milliseconds(0)); // Create the two Unit3s. // Unlike the above case, we can use any two sphers. @@ -303,7 +303,7 @@ TEST(Unit3, localCoordinates_retract_expmap) { // EXPECT(assert_equal(expected,actual1)); // EXPECT(assert_equal(expected,actual2)); // -// Matrix expectedH1 = Matrix_(3,3, +// Matrix expectedH1 = (Matrix(3,3) << // 0.0,-1.0,-2.0, // 1.0, 0.0,-2.0, // 0.0, 0.0,-1.0 @@ -314,7 +314,7 @@ TEST(Unit3, localCoordinates_retract_expmap) { // // Assert H1 = -AdjointMap(between(p2,p1)) as in doc/math.lyx // EXPECT(assert_equal(-gT2.between(gT1).AdjointMap(),actualH1)); // -// Matrix expectedH2 = Matrix_(3,3, +// Matrix expectedH2 = (Matrix(3,3) << // 1.0, 0.0, 0.0, // 0.0, 1.0, 0.0, // 0.0, 0.0, 1.0 @@ -327,7 +327,7 @@ TEST(Unit3, localCoordinates_retract_expmap) { //******************************************************************************* TEST(Unit3, Random) { - boost::random::mt19937 rng(42); + boost::mt19937 rng(42); // Check that is deterministic given same random seed Point3 expected(-0.667578, 0.671447, 0.321713); Point3 actual = Unit3::Random(rng).point3(); diff --git a/gtsam/geometry/tests/timePose2.cpp b/gtsam/geometry/tests/timePose2.cpp index a8f2f7137..28044068c 100644 --- a/gtsam/geometry/tests/timePose2.cpp +++ b/gtsam/geometry/tests/timePose2.cpp @@ -138,7 +138,7 @@ int main() // create a random pose: double x = 4.0, y = 2.0, r = 0.3; - Vector v = Vector_(3,x,y,r); + Vector v = (Vector(3) << x, y, r); Pose2 X = Pose2(3,2,0.4), X2 = X.retract(v), X3(5,6,0.3); TEST(Expmap, Pose2::Expmap(v)); diff --git a/gtsam/geometry/tests/timeRot2.cpp b/gtsam/geometry/tests/timeRot2.cpp index 6d828ef9c..86dda2b8c 100644 --- a/gtsam/geometry/tests/timeRot2.cpp +++ b/gtsam/geometry/tests/timeRot2.cpp @@ -95,7 +95,7 @@ int main() // create a random direction: double norm=sqrt(16.0+4.0); double x=4.0/norm, y=2.0/norm; - Vector v = Vector_(2,x,y); + Vector v = (Vector(2) << x, y); Rot2 R = Rot2(0.4), R2 = R.retract(v), R3(0.6); TEST(Rot2_Expmap, Rot2::Expmap(v)); diff --git a/gtsam/geometry/tests/timeRot3.cpp b/gtsam/geometry/tests/timeRot3.cpp index 64feb1909..dd98465ed 100644 --- a/gtsam/geometry/tests/timeRot3.cpp +++ b/gtsam/geometry/tests/timeRot3.cpp @@ -39,7 +39,7 @@ int main() // create a random direction: double norm=sqrt(1.0+16.0+4.0); double x=1.0/norm, y=4.0/norm, z=2.0/norm; - Vector v = Vector_(3,x,y,z); + Vector v = (Vector(3) << x, y, z); Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2), R2 = R.retract(v); TEST("Rodriguez formula given axis angle", Rot3::rodriguez(v,0.001)) diff --git a/gtsam/inference/.cvsignore b/gtsam/inference/.cvsignore deleted file mode 100644 index 67155973c..000000000 --- a/gtsam/inference/.cvsignore +++ /dev/null @@ -1,43 +0,0 @@ -*.lo -testPoint2 -testSimulated2D -testCal3_S2 -testChordalBayesNet -testNonlinearFactorGraph -testLinearFactor -testConditionalGaussian -testPose3 -testFactorgraph -testSimulated3D -testFGConfig -testNonlinearFactor -testPoint3 -testMatrix -testVector -testRot3 -testLinearFactorGraph -testVSLAMFactor -config.h -html -config.h.in -stamp-h1 -Makefile -Makefile.in -libgtsam.la -simple_svd_test -simple_ublas_resize_test -simple_ublas_resize_test.cpp -test_svdcmp.cpp -.deps -.libs -resize_test -testConstrainedChordalBayesNet -testConstrainedLinearFactorGraph -testConstrainedNonlinearFactorGraph -testDeltaFunction -testUnconstrainedLinearFactorGraph -testUnconstrainedNonlinearFactorGraph -testEqualityFactor -.DS_Store -testSimpleCamera -testCalibratedCamera diff --git a/gtsam/inference/BayesNet-inl.h b/gtsam/inference/BayesNet-inl.h deleted file mode 100644 index f0e26c98b..000000000 --- a/gtsam/inference/BayesNet-inl.h +++ /dev/null @@ -1,192 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 BayesNet-inl.h - * @brief Bayes net template definitions - * @author Frank Dellaert - */ - -#pragma once - -#include - -#include -#include -#include - -#include // for += -using boost::assign::operator+=; - -#include -#include -#include - -namespace gtsam { - - /* ************************************************************************* */ - template - void BayesNet::print(const std::string& s, - const IndexFormatter& formatter) const { - std::cout << s; - BOOST_REVERSE_FOREACH(sharedConditional conditional, conditionals_) - conditional->print("Conditional", formatter); - } - - /* ************************************************************************* */ - template - bool BayesNet::equals(const BayesNet& cbn, double tol) const { - if (size() != cbn.size()) - return false; - return std::equal(conditionals_.begin(), conditionals_.end(), - cbn.conditionals_.begin(), equals_star(tol)); - } - - /* ************************************************************************* */ - template - void BayesNet::printStats(const std::string& s) const { - - const size_t n = conditionals_.size(); - size_t max_size = 0; - size_t total = 0; - BOOST_REVERSE_FOREACH(sharedConditional conditional, conditionals_) { - max_size = std::max(max_size, conditional->size()); - total += conditional->size(); - } - std::cout << s - << "maximum conditional size = " << max_size << std::endl - << "average conditional size = " << total / n << std::endl - << "density = " << 100.0 * total / (double) (n*(n+1)/2) << " %" << std::endl; - } - - /* ************************************************************************* */ - template - void BayesNet::saveGraph(const std::string &s, - const IndexFormatter& indexFormatter) const { - std::ofstream of(s.c_str()); - of << "digraph G{\n"; - - BOOST_REVERSE_FOREACH(typename CONDITIONAL::shared_ptr conditional, conditionals_) { - typename CONDITIONAL::Frontals frontals = conditional->frontals(); - Index me = frontals.front(); - typename CONDITIONAL::Parents parents = conditional->parents(); - BOOST_FOREACH(Index p, parents) - of << p << "->" << me << std::endl; - } - - of << "}"; - of.close(); - } - - /* ************************************************************************* */ - template - typename BayesNet::const_iterator BayesNet::find( - Index key) const { - for (const_iterator it = begin(); it != end(); ++it) - if (std::find((*it)->beginFrontals(), (*it)->endFrontals(), key) - != (*it)->endFrontals()) - return it; - return end(); - } - - /* ************************************************************************* */ - template - typename BayesNet::iterator BayesNet::find( - Index key) { - for (iterator it = begin(); it != end(); ++it) - if (std::find((*it)->beginFrontals(), (*it)->endFrontals(), key) - != (*it)->endFrontals()) - return it; - return end(); - } - - /* ************************************************************************* */ - template - void BayesNet::permuteWithInverse( - const Permutation& inversePermutation) { - BOOST_FOREACH(sharedConditional conditional, conditionals_) { - conditional->permuteWithInverse(inversePermutation); - } - } - - /* ************************************************************************* */ - template - void BayesNet::push_back(const BayesNet& bn) { - BOOST_FOREACH(sharedConditional conditional,bn.conditionals_) - push_back(conditional); - } - - /* ************************************************************************* */ - template - void BayesNet::push_front(const BayesNet& bn) { - BOOST_FOREACH(sharedConditional conditional,bn.conditionals_) - push_front(conditional); - } - - /* ************************************************************************* */ - template - void BayesNet::popLeaf(iterator conditional) { -#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS - BOOST_FOREACH(typename CONDITIONAL::shared_ptr checkConditional, conditionals_) { - BOOST_FOREACH(Index key, (*conditional)->frontals()) { - if(std::find(checkConditional->beginParents(), checkConditional->endParents(), key) != checkConditional->endParents()) - throw std::invalid_argument( - "Debug mode exception: in BayesNet::popLeaf, the requested conditional is not a leaf."); - } - } -#endif - conditionals_.erase(conditional); - } - - /* ************************************************************************* */ - template - FastList BayesNet::ordering() const { - FastList ord; - BOOST_FOREACH(sharedConditional conditional,conditionals_) - ord.insert(ord.begin(), conditional->beginFrontals(), - conditional->endFrontals()); - return ord; - } - -// /* ************************************************************************* */ -// template -// void BayesNet::saveGraph(const std::string &s) const { -// ofstream of(s.c_str()); -// of<< "digraph G{\n"; -// BOOST_FOREACH(const_sharedConditional conditional,conditionals_) { -// Index child = conditional->key(); -// BOOST_FOREACH(Index parent, conditional->parents()) { -// of << parent << "->" << child << endl; -// } -// } -// of<<"}"; -// of.close(); -// } -// - /* ************************************************************************* */ - - template - typename BayesNet::sharedConditional BayesNet::operator[]( - Index key) const { - BOOST_FOREACH(typename CONDITIONAL::shared_ptr conditional, conditionals_) { - typename CONDITIONAL::const_iterator it = std::find( - conditional->beginFrontals(), conditional->endFrontals(), key); - if (it != conditional->endFrontals()) { - return conditional; - } - } - throw(std::invalid_argument( - (boost::format("BayesNet::operator['%1%']: not found") % key).str())); - } - -/* ************************************************************************* */ - -} // namespace gtsam diff --git a/gtsam/inference/BayesNet-inst.h b/gtsam/inference/BayesNet-inst.h new file mode 100644 index 000000000..f5e69fcce --- /dev/null +++ b/gtsam/inference/BayesNet-inst.h @@ -0,0 +1,55 @@ +/* ---------------------------------------------------------------------------- + +* 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 BayesNet.h +* @brief Bayes network +* @author Frank Dellaert +* @author Richard Roberts +*/ + +#pragma once + +#include +#include + +#include +#include + +namespace gtsam { + + /* ************************************************************************* */ + template + void BayesNet::print(const std::string& s, const KeyFormatter& formatter) const + { + Base::print(s, formatter); + } + + /* ************************************************************************* */ + template + void BayesNet::saveGraph(const std::string &s, const KeyFormatter& keyFormatter) const + { + std::ofstream of(s.c_str()); + of << "digraph G{\n"; + + BOOST_REVERSE_FOREACH(const sharedConditional& conditional, *this) { + typename CONDITIONAL::Frontals frontals = conditional->frontals(); + Key me = frontals.front(); + typename CONDITIONAL::Parents parents = conditional->parents(); + BOOST_FOREACH(Key p, parents) + of << keyFormatter(p) << "->" << keyFormatter(me) << std::endl; + } + + of << "}"; + of.close(); + } + +} diff --git a/gtsam/inference/BayesNet.h b/gtsam/inference/BayesNet.h index ad39da301..1934c58ed 100644 --- a/gtsam/inference/BayesNet.h +++ b/gtsam/inference/BayesNet.h @@ -1,244 +1,72 @@ /* ---------------------------------------------------------------------------- - * 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) +* 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 +* See LICENSE for the license information - * -------------------------------------------------------------------------- */ +* -------------------------------------------------------------------------- */ /** - * @file BayesNet.h - * @brief Bayes network - * @author Frank Dellaert - */ - -// \callgraph +* @file BayesNet.h +* @brief Bayes network +* @author Frank Dellaert +* @author Richard Roberts +*/ #pragma once -#include #include -#include -#include -#include -#include -#include -#include +#include namespace gtsam { -/** - * A BayesNet is a list of conditionals, stored in elimination order, i.e. - * leaves first, parents last. GaussianBayesNet and SymbolicBayesNet are - * defined as typedefs of this class, using GaussianConditional and - * IndexConditional as the CONDITIONAL template argument. - * - * todo: Symbolic using Index is a misnomer. - * todo: how to handle Bayes nets with an optimize function? Currently using global functions. - * \nosubgrouping - */ -template -class BayesNet { - -public: - - typedef typename boost::shared_ptr > shared_ptr; - - /** We store shared pointers to Conditional densities */ - typedef typename CONDITIONAL::KeyType KeyType; - typedef typename boost::shared_ptr sharedConditional; - typedef typename boost::shared_ptr const_sharedConditional; - typedef typename std::list Conditionals; - - typedef typename Conditionals::iterator iterator; - typedef typename Conditionals::reverse_iterator reverse_iterator; - typedef typename Conditionals::const_iterator const_iterator; - typedef typename Conditionals::const_reverse_iterator const_reverse_iterator; - -protected: - /** - * Conditional densities are stored in reverse topological sort order (i.e., leaves first, - * parents last), which corresponds to the elimination ordering if so obtained, - * and is consistent with the column (block) ordering of an upper triangular matrix. - */ - Conditionals conditionals_; + * A BayesNet is a tree of conditionals, stored in elimination order. + * + * todo: how to handle Bayes nets with an optimize function? Currently using global functions. + * \nosubgrouping + */ + template + class BayesNet : public FactorGraph { -public: + private: - /// @name Standard Constructors - /// @{ + typedef FactorGraph Base; - /** Default constructor as an empty BayesNet */ - BayesNet() {}; + public: + typedef typename boost::shared_ptr sharedConditional; ///< A shared pointer to a conditional - /** convert from a derived type */ - template - BayesNet(const BayesNet& bn) { - conditionals_.assign(bn.begin(), bn.end()); - } + protected: + /// @name Standard Constructors + /// @{ - /** BayesNet with 1 conditional */ - explicit BayesNet(const sharedConditional& conditional) { push_back(conditional); } + /** Default constructor as an empty BayesNet */ + BayesNet() {}; - /// @} - /// @name Testable - /// @{ + /** Construct from iterator over conditionals */ + template + BayesNet(ITERATOR firstConditional, ITERATOR lastConditional) : Base(firstConditional, lastConditional) {} - /** print */ - void print(const std::string& s = "", - const IndexFormatter& formatter = DefaultIndexFormatter) const; + /// @} - /** check equality */ - bool equals(const BayesNet& other, double tol = 1e-9) const; + public: + /// @name Testable + /// @{ - /// @} - /// @name Standard Interface - /// @{ + /** print out graph */ + void print(const std::string& s = "BayesNet", + const KeyFormatter& formatter = DefaultKeyFormatter) const; - /** size is the number of nodes */ - size_t size() const { - return conditionals_.size(); - } + /// @} - /** @return true if there are no conditionals/nodes */ - bool empty() const { - return conditionals_.empty(); - } + /// @name Standard Interface + /// @{ + + void saveGraph(const std::string &s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + }; - /** print statistics */ - void printStats(const std::string& s = "") const; - - /** save dot graph */ - void saveGraph(const std::string &s, const IndexFormatter& indexFormatter = - DefaultIndexFormatter) const; - - /** return keys in reverse topological sort order, i.e., elimination order */ - FastList ordering() const; - - /** SLOW O(n) random access to Conditional by key */ - sharedConditional operator[](Index key) const; - - /** return last node in ordering */ - boost::shared_ptr front() const { return conditionals_.front(); } - - /** return last node in ordering */ - boost::shared_ptr back() const { return conditionals_.back(); } - - /** return iterators. FD: breaks encapsulation? */ - const_iterator begin() const {return conditionals_.begin();} ///& bn); - - /// push_front an entire Bayes net - void push_front(const BayesNet& bn); - - /** += syntax for push_back, e.g. bayesNet += c1, c2, c3 - * @param conditional The conditional to add to the back of the BayesNet - */ - boost::assign::list_inserter >, sharedConditional> - operator+=(const sharedConditional& conditional) { - return boost::assign::make_list_inserter(boost::assign_detail::call_push_back >(*this))(conditional); } - - /** - * pop_front: remove node at the bottom, used in marginalization - * For example P(ABC)=P(A|BC)P(B|C)P(C) becomes P(BC)=P(B|C)P(C) - */ - void pop_front() {conditionals_.pop_front();} - - /** Permute the variables in the BayesNet */ - void permuteWithInverse(const Permutation& inversePermutation); - - iterator begin() {return conditionals_.begin();} /// - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(conditionals_); - } - - /// @} -}; // BayesNet - -} // namespace gtsam - -#include +} \ No newline at end of file diff --git a/gtsam/inference/BayesTree-inl.h b/gtsam/inference/BayesTree-inl.h deleted file mode 100644 index d5592ed2a..000000000 --- a/gtsam/inference/BayesTree-inl.h +++ /dev/null @@ -1,804 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 BayesTree-inl.h - * @brief Bayes Tree is a tree of cliques of a Bayes Chain - * @author Frank Dellaert - * @author Michael Kaess - * @author Viorela Ila - * @author Richard Roberts - */ - -#pragma once - -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include -#include // for operator += -using boost::assign::operator+=; -#include - -namespace gtsam { - - /* ************************************************************************* */ - template - typename BayesTree::CliqueData - BayesTree::getCliqueData() const { - CliqueData data; - getCliqueData(data, root_); - return data; - } - - /* ************************************************************************* */ - template - void BayesTree::getCliqueData(CliqueData& data, sharedClique clique) const { - data.conditionalSizes.push_back((*clique)->nrFrontals()); - data.separatorSizes.push_back((*clique)->nrParents()); - BOOST_FOREACH(sharedClique c, clique->children_) { - getCliqueData(data, c); - } - } - - /* ************************************************************************* */ - template - size_t BayesTree::numCachedSeparatorMarginals() const { - return (root_) ? root_->numCachedSeparatorMarginals() : 0; - } - - /* ************************************************************************* */ - template - void BayesTree::saveGraph(const std::string &s, const IndexFormatter& indexFormatter) const { - if (!root_.get()) throw std::invalid_argument("the root of Bayes tree has not been initialized!"); - std::ofstream of(s.c_str()); - of<< "digraph G{\n"; - saveGraph(of, root_, indexFormatter); - of<<"}"; - of.close(); - } - - /* ************************************************************************* */ - template - void BayesTree::saveGraph(std::ostream &s, sharedClique clique, const IndexFormatter& indexFormatter, int parentnum) const { - static int num = 0; - bool first = true; - std::stringstream out; - out << num; - std::string parent = out.str(); - parent += "[label=\""; - - BOOST_FOREACH(Index index, clique->conditional_->frontals()) { - if(!first) parent += ","; first = false; - parent += indexFormatter(index); - } - - if( clique != root_){ - parent += " : "; - s << parentnum << "->" << num << "\n"; - } - - first = true; - BOOST_FOREACH(Index sep, clique->conditional_->parents()) { - if(!first) parent += ","; first = false; - parent += indexFormatter(sep); - } - parent += "\"];\n"; - s << parent; - parentnum = num; - - BOOST_FOREACH(sharedClique c, clique->children_) { - num++; - saveGraph(s, c, indexFormatter, parentnum); - } - } - - - /* ************************************************************************* */ - template - void BayesTree::CliqueStats::print(const std::string& s) const { - std::cout << s - <<"avg Conditional Size: " << avgConditionalSize << std::endl - << "max Conditional Size: " << maxConditionalSize << std::endl - << "avg Separator Size: " << avgSeparatorSize << std::endl - << "max Separator Size: " << maxSeparatorSize << std::endl; - } - - /* ************************************************************************* */ - template - typename BayesTree::CliqueStats - BayesTree::CliqueData::getStats() const { - CliqueStats stats; - - double sum = 0.0; - size_t max = 0; - BOOST_FOREACH(size_t s, conditionalSizes) { - sum += (double)s; - if(s > max) max = s; - } - stats.avgConditionalSize = sum / (double)conditionalSizes.size(); - stats.maxConditionalSize = max; - - sum = 0.0; - max = 1; - BOOST_FOREACH(size_t s, separatorSizes) { - sum += (double)s; - if(s > max) max = s; - } - stats.avgSeparatorSize = sum / (double)separatorSizes.size(); - stats.maxSeparatorSize = max; - - return stats; - } - - /* ************************************************************************* */ - template - void BayesTree::Cliques::print(const std::string& s, const IndexFormatter& indexFormatter) const { - std::cout << s << ":\n"; - BOOST_FOREACH(sharedClique clique, *this) - clique->printTree("", indexFormatter); - } - - /* ************************************************************************* */ - template - bool BayesTree::Cliques::equals(const Cliques& other, double tol) const { - return other == *this; - } - - /* ************************************************************************* */ - template - typename BayesTree::sharedClique - BayesTree::addClique(const sharedConditional& conditional, const sharedClique& parent_clique) { - sharedClique new_clique(new Clique(conditional)); - addClique(new_clique, parent_clique); - return new_clique; - } - - /* ************************************************************************* */ - template - void BayesTree::addClique(const sharedClique& clique, const sharedClique& parent_clique) { - nodes_.resize(std::max((*clique)->lastFrontalKey()+1, nodes_.size())); - BOOST_FOREACH(Index j, (*clique)->frontals()) - nodes_[j] = clique; - if (parent_clique != NULL) { - clique->parent_ = parent_clique; - parent_clique->children_.push_back(clique); - } else { - assert(!root_); - root_ = clique; - } - clique->assertInvariants(); - } - - /* ************************************************************************* */ - template - typename BayesTree::sharedClique BayesTree::addClique( - const sharedConditional& conditional, std::list& child_cliques) { - sharedClique new_clique(new Clique(conditional)); - nodes_.resize(std::max(conditional->lastFrontalKey()+1, nodes_.size())); - BOOST_FOREACH(Index j, conditional->frontals()) - nodes_[j] = new_clique; - new_clique->children_ = child_cliques; - BOOST_FOREACH(sharedClique& child, child_cliques) - child->parent_ = new_clique; - new_clique->assertInvariants(); - return new_clique; - } - - /* ************************************************************************* */ - template - void BayesTree::permuteWithInverse(const Permutation& inversePermutation) { - // recursively permute the cliques and internal conditionals - if (root_) - root_->permuteWithInverse(inversePermutation); - - // need to know what the largest key is to get the right number of cliques - Index maxIndex = *std::max_element(inversePermutation.begin(), inversePermutation.end()); - - // Update the nodes structure - typename BayesTree::Nodes newNodes(maxIndex+1); -// inversePermutation.applyToCollection(newNodes, nodes_); // Uses the forward, rather than inverse permutation - for(size_t i = 0; i < nodes_.size(); ++i) - newNodes[inversePermutation[i]] = nodes_[i]; - - nodes_ = newNodes; - } - - /* ************************************************************************* */ - template - inline void BayesTree::addToCliqueFront(BayesTree& bayesTree, const sharedConditional& conditional, const sharedClique& clique) { - static const bool debug = false; -#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS - // Debug check to make sure the conditional variable is ordered lower than - // its parents and that all of its parents are present either in this - // clique or its separator. - BOOST_FOREACH(Index parent, conditional->parents()) { - assert(parent > conditional->lastFrontalKey()); - const Clique& cliquer(*clique); - assert(find(cliquer->begin(), cliquer->end(), parent) != cliquer->end()); - } -#endif - if(debug) conditional->print("Adding conditional "); - if(debug) clique->print("To clique "); - Index j = conditional->lastFrontalKey(); - bayesTree.nodes_.resize(std::max(j+1, bayesTree.nodes_.size())); - bayesTree.nodes_[j] = clique; - FastVector newIndices((*clique)->size() + 1); - newIndices[0] = j; - std::copy((*clique)->begin(), (*clique)->end(), newIndices.begin()+1); - clique->conditional_ = CONDITIONAL::FromKeys(newIndices, (*clique)->nrFrontals() + 1); - if(debug) clique->print("Expanded clique is "); - clique->assertInvariants(); - } - - /* ************************************************************************* */ - template - void BayesTree::removeClique(sharedClique clique) { - - if (clique->isRoot()) - root_.reset(); - else { // detach clique from parent - sharedClique parent = clique->parent_.lock(); - typename FastList::iterator child = std::find(parent->children().begin(), parent->children().end(), clique); - assert(child != parent->children().end()); - parent->children().erase(child); - } - - // orphan my children - BOOST_FOREACH(sharedClique child, clique->children_) - child->parent_ = typename Clique::weak_ptr(); - - BOOST_FOREACH(Index j, clique->conditional()->frontals()) { - nodes_[j].reset(); - } - } - - /* ************************************************************************* */ - template - void BayesTree::recursiveTreeBuild(const boost::shared_ptr >& symbolic, - const std::vector >& conditionals, - const typename BayesTree::sharedClique& parent) { - - // Helper function to build a non-symbolic tree (e.g. Gaussian) using a - // symbolic tree, used in the BT(BN) constructor. - - // Build the current clique - FastList cliqueConditionals; - BOOST_FOREACH(Index j, symbolic->conditional()->frontals()) { - cliqueConditionals.push_back(conditionals[j]); } - typename BayesTree::sharedClique thisClique(new CLIQUE(CONDITIONAL::Combine(cliqueConditionals.begin(), cliqueConditionals.end()))); - - // Add the new clique with the current parent - this->addClique(thisClique, parent); - - // Build the children, whose parent is the new clique - BOOST_FOREACH(const BayesTree::sharedClique& child, symbolic->children()) { - this->recursiveTreeBuild(child, conditionals, thisClique); } - } - - /* ************************************************************************* */ - template - BayesTree::BayesTree(const BayesNet& bayesNet) { - // First generate symbolic BT to determine clique structure - BayesTree sbt(bayesNet); - - // Build index of variables to conditionals - std::vector > conditionals(sbt.root()->conditional()->frontals().back() + 1); - BOOST_FOREACH(const boost::shared_ptr& c, bayesNet) { - if(c->nrFrontals() != 1) - throw std::invalid_argument("BayesTree constructor from BayesNet only supports single frontal variable conditionals"); - if(c->firstFrontalKey() >= conditionals.size()) - throw std::invalid_argument("An inconsistent BayesNet was passed into the BayesTree constructor!"); - if(conditionals[c->firstFrontalKey()]) - throw std::invalid_argument("An inconsistent BayesNet with duplicate frontal variables was passed into the BayesTree constructor!"); - - conditionals[c->firstFrontalKey()] = c; - } - - // Build the new tree - this->recursiveTreeBuild(sbt.root(), conditionals, sharedClique()); - } - - /* ************************************************************************* */ - template<> - inline BayesTree::BayesTree(const BayesNet& bayesNet) { - BayesNet::const_reverse_iterator rit; - for ( rit=bayesNet.rbegin(); rit != bayesNet.rend(); ++rit ) - insert(*this, *rit); - } - - /* ************************************************************************* */ - template - BayesTree::BayesTree(const BayesNet& bayesNet, std::list > subtrees) { - if (bayesNet.size() == 0) - throw std::invalid_argument("BayesTree::insert: empty bayes net!"); - - // get the roots of child subtrees and merge their nodes_ - std::list childRoots; - typedef BayesTree Tree; - BOOST_FOREACH(const Tree& subtree, subtrees) { - nodes_.assign(subtree.nodes_.begin(), subtree.nodes_.end()); - childRoots.push_back(subtree.root()); - } - - // create a new clique and add all the conditionals to the clique - sharedClique new_clique; - typename BayesNet::sharedConditional conditional; - BOOST_REVERSE_FOREACH(conditional, bayesNet) { - if (!new_clique.get()) - new_clique = addClique(conditional,childRoots); - else - addToCliqueFront(*this, conditional, new_clique); - } - - root_ = new_clique; - } - - /* ************************************************************************* */ - template - BayesTree::BayesTree(const This& other) { - *this = other; - } - - /* ************************************************************************* */ - template - BayesTree& BayesTree::operator=(const This& other) { - this->clear(); - other.cloneTo(*this); - return *this; - } - - /* ************************************************************************* */ - template - void BayesTree::print(const std::string& s, const IndexFormatter& indexFormatter) const { - if (root_.use_count() == 0) { - std::cout << "WARNING: BayesTree.print encountered a forest..." << std::endl; - return; - } - std::cout << s << ": clique size == " << size() << ", node size == " << nodes_.size() << std::endl; - if (nodes_.empty()) return; - root_->printTree("", indexFormatter); - } - - /* ************************************************************************* */ - // binary predicate to test equality of a pair for use in equals - template - bool check_sharedCliques( - const typename BayesTree::sharedClique& v1, - const typename BayesTree::sharedClique& v2 - ) { - return (!v1 && !v2) || (v1 && v2 && v1->equals(*v2)); - } - - /* ************************************************************************* */ - template - bool BayesTree::equals(const BayesTree& other, - double tol) const { - return size()==other.size() && - std::equal(nodes_.begin(), nodes_.end(), other.nodes_.begin(), &check_sharedCliques); - } - - /* ************************************************************************* */ - template - template - inline Index BayesTree::findParentClique(const CONTAINER& parents) const { - typename CONTAINER::const_iterator lowestOrderedParent = min_element(parents.begin(), parents.end()); - assert(lowestOrderedParent != parents.end()); - return *lowestOrderedParent; - } - - /* ************************************************************************* */ - template - void BayesTree::insert(BayesTree& bayesTree, const sharedConditional& conditional) - { - static const bool debug = false; - - // get indices and parents - const typename CONDITIONAL::Parents& parents = conditional->parents(); - - if(debug) conditional->print("Adding conditional "); - - // if no parents, start a new root clique - if (parents.empty()) { - if(debug) std::cout << "No parents so making root" << std::endl; - bayesTree.root_ = bayesTree.addClique(conditional); - return; - } - - // otherwise, find the parent clique by using the index data structure - // to find the lowest-ordered parent - Index parentRepresentative = bayesTree.findParentClique(parents); - if(debug) std::cout << "First-eliminated parent is " << parentRepresentative << ", have " << bayesTree.nodes_.size() << " nodes." << std::endl; - sharedClique parent_clique = bayesTree[parentRepresentative]; - if(debug) parent_clique->print("Parent clique is "); - - // if the parents and parent clique have the same size, add to parent clique - if ((*parent_clique)->size() == size_t(parents.size())) { - if(debug) std::cout << "Adding to parent clique" << std::endl; - addToCliqueFront(bayesTree, conditional, parent_clique); - } else { - if(debug) std::cout << "Starting new clique" << std::endl; - // otherwise, start a new clique and add it to the tree - bayesTree.addClique(conditional,parent_clique); - } - } - - /* ************************************************************************* */ - //TODO: remove this function after removing TSAM.cpp - template - typename BayesTree::sharedClique BayesTree::insert( - const sharedConditional& clique, std::list& children, bool isRootClique) { - - if (clique->nrFrontals() == 0) - throw std::invalid_argument("BayesTree::insert: empty clique!"); - - // create a new clique and add all the conditionals to the clique - sharedClique new_clique = addClique(clique, children); - if (isRootClique) root_ = new_clique; - - return new_clique; - } - - /* ************************************************************************* */ - template - void BayesTree::fillNodesIndex(const sharedClique& subtree) { - // Add each frontal variable of this root node - BOOST_FOREACH(const Index& j, subtree->conditional()->frontals()) { nodes_[j] = subtree; } - // Fill index for each child - typedef typename BayesTree::sharedClique sharedClique; - BOOST_FOREACH(const sharedClique& child, subtree->children_) { - fillNodesIndex(child); } - } - - /* ************************************************************************* */ - template - void BayesTree::insert(const sharedClique& subtree) { - if(subtree) { - // Find the parent clique of the new subtree. By the running intersection - // property, those separator variables in the subtree that are ordered - // lower than the highest frontal variable of the subtree root will all - // appear in the separator of the subtree root. - if(subtree->conditional()->parents().empty()) { - assert(!root_); - root_ = subtree; - } else { - Index parentRepresentative = findParentClique(subtree->conditional()->parents()); - sharedClique parent = (*this)[parentRepresentative]; - parent->children_ += subtree; - subtree->parent_ = parent; // set new parent! - } - - // Now fill in the nodes index - if(nodes_.size() == 0 || - *std::max_element(subtree->conditional()->beginFrontals(), subtree->conditional()->endFrontals()) > (nodes_.size() - 1)) { - nodes_.resize(subtree->conditional()->lastFrontalKey() + 1); - } - fillNodesIndex(subtree); - } - } - - /* ************************************************************************* */ - // First finds clique marginal then marginalizes that - /* ************************************************************************* */ - template - typename CONDITIONAL::FactorType::shared_ptr BayesTree::marginalFactor( - Index j, Eliminate function) const - { - gttic(BayesTree_marginalFactor); - - // get clique containing Index j - sharedClique clique = (*this)[j]; - - // calculate or retrieve its marginal P(C) = P(F,S) -#ifdef OLD_SHORTCUT_MARGINALS - FactorGraph cliqueMarginal = clique->marginal(root_,function); -#else - FactorGraph cliqueMarginal = clique->marginal2(root_,function); -#endif - - // Reduce the variable indices to start at zero - gttic(Reduce); - const Permutation reduction = internal::createReducingPermutation(cliqueMarginal.keys()); - internal::Reduction inverseReduction = internal::Reduction::CreateAsInverse(reduction); - BOOST_FOREACH(const boost::shared_ptr& factor, cliqueMarginal) { - if(factor) factor->reduceWithInverse(inverseReduction); } - gttoc(Reduce); - - // now, marginalize out everything that is not variable j - GenericSequentialSolver solver(cliqueMarginal); - boost::shared_ptr result = solver.marginalFactor(inverseReduction[j], function); - - // Undo the reduction - gttic(Undo_Reduce); - result->permuteWithInverse(reduction); - BOOST_FOREACH(const boost::shared_ptr& factor, cliqueMarginal) { - if(factor) factor->permuteWithInverse(reduction); } - gttoc(Undo_Reduce); - return result; - } - - /* ************************************************************************* */ - template - typename BayesNet::shared_ptr BayesTree::marginalBayesNet( - Index j, Eliminate function) const - { - gttic(BayesTree_marginalBayesNet); - - // calculate marginal as a factor graph - FactorGraph fg; - fg.push_back(this->marginalFactor(j,function)); - - // Reduce the variable indices to start at zero - gttic(Reduce); - const Permutation reduction = internal::createReducingPermutation(fg.keys()); - internal::Reduction inverseReduction = internal::Reduction::CreateAsInverse(reduction); - BOOST_FOREACH(const boost::shared_ptr& factor, fg) { - if(factor) factor->reduceWithInverse(inverseReduction); } - gttoc(Reduce); - - // eliminate factor graph marginal to a Bayes net - boost::shared_ptr > bn = GenericSequentialSolver(fg).eliminate(function); - - // Undo the reduction - gttic(Undo_Reduce); - bn->permuteWithInverse(reduction); - BOOST_FOREACH(const boost::shared_ptr& factor, fg) { - if(factor) factor->permuteWithInverse(reduction); } - gttoc(Undo_Reduce); - return bn; - } - - /* ************************************************************************* */ - // Find two cliques, their joint, then marginalizes - /* ************************************************************************* */ - template - typename FactorGraph::shared_ptr - BayesTree::joint(Index j1, Index j2, Eliminate function) const { - gttic(BayesTree_joint); - - // get clique C1 and C2 - sharedClique C1 = (*this)[j1], C2 = (*this)[j2]; - - gttic(Lowest_common_ancestor); - // Find lowest common ancestor clique - sharedClique B; { - // Build two paths to the root - FastList path1, path2; { - sharedClique p = C1; - while(p) { - path1.push_front(p); - p = p->parent(); - } - } { - sharedClique p = C2; - while(p) { - path2.push_front(p); - p = p->parent(); - } - } - // Find the path intersection - B = this->root(); - typename FastList::const_iterator p1 = path1.begin(), p2 = path2.begin(); - while(p1 != path1.end() && p2 != path2.end() && *p1 == *p2) { - B = *p1; - ++p1; - ++p2; - } - } - gttoc(Lowest_common_ancestor); - - // Compute marginal on lowest common ancestor clique - gttic(LCA_marginal); - FactorGraph p_B = B->marginal2(this->root(), function); - gttoc(LCA_marginal); - - // Compute shortcuts of the requested cliques given the lowest common ancestor - gttic(Clique_shortcuts); - BayesNet p_C1_Bred = C1->shortcut(B, function); - BayesNet p_C2_Bred = C2->shortcut(B, function); - gttoc(Clique_shortcuts); - - // Factor the shortcuts to be conditioned on the full root - // Get the set of variables to eliminate, which is C1\B. - gttic(Full_root_factoring); - sharedConditional p_C1_B; { - std::vector C1_minus_B; { - FastSet C1_minus_B_set(C1->conditional()->beginParents(), C1->conditional()->endParents()); - BOOST_FOREACH(const Index j, *B->conditional()) { - C1_minus_B_set.erase(j); } - C1_minus_B.assign(C1_minus_B_set.begin(), C1_minus_B_set.end()); - } - // Factor into C1\B | B. - FactorGraph temp_remaining; - boost::tie(p_C1_B, temp_remaining) = FactorGraph(p_C1_Bred).eliminate(C1_minus_B, function); - } - sharedConditional p_C2_B; { - std::vector C2_minus_B; { - FastSet C2_minus_B_set(C2->conditional()->beginParents(), C2->conditional()->endParents()); - BOOST_FOREACH(const Index j, *B->conditional()) { - C2_minus_B_set.erase(j); } - C2_minus_B.assign(C2_minus_B_set.begin(), C2_minus_B_set.end()); - } - // Factor into C2\B | B. - FactorGraph temp_remaining; - boost::tie(p_C2_B, temp_remaining) = FactorGraph(p_C2_Bred).eliminate(C2_minus_B, function); - } - gttoc(Full_root_factoring); - - gttic(Variable_joint); - // Build joint on all involved variables - FactorGraph p_BC1C2; - p_BC1C2.push_back(p_B); - p_BC1C2.push_back(p_C1_B->toFactor()); - p_BC1C2.push_back(p_C2_B->toFactor()); - if(C1 != B) - p_BC1C2.push_back(C1->conditional()->toFactor()); - if(C2 != B) - p_BC1C2.push_back(C2->conditional()->toFactor()); - - // Reduce the variable indices to start at zero - gttic(Reduce); - const Permutation reduction = internal::createReducingPermutation(p_BC1C2.keys()); - internal::Reduction inverseReduction = internal::Reduction::CreateAsInverse(reduction); - BOOST_FOREACH(const boost::shared_ptr& factor, p_BC1C2) { - if(factor) factor->reduceWithInverse(inverseReduction); } - std::vector js; js.push_back(inverseReduction[j1]); js.push_back(inverseReduction[j2]); - gttoc(Reduce); - - // now, marginalize out everything that is not variable j - GenericSequentialSolver solver(p_BC1C2); - boost::shared_ptr > result = solver.jointFactorGraph(js, function); - - // Undo the reduction - gttic(Undo_Reduce); - BOOST_FOREACH(const boost::shared_ptr& factor, *result) { - if(factor) factor->permuteWithInverse(reduction); } - BOOST_FOREACH(const boost::shared_ptr& factor, p_BC1C2) { - if(factor) factor->permuteWithInverse(reduction); } - gttoc(Undo_Reduce); - return result; - - } - - /* ************************************************************************* */ - template - typename BayesNet::shared_ptr BayesTree::jointBayesNet( - Index j1, Index j2, Eliminate function) const { - - // eliminate factor graph marginal to a Bayes net - return GenericSequentialSolver ( - *this->joint(j1, j2, function)).eliminate(function); - } - - /* ************************************************************************* */ - template - void BayesTree::clear() { - // Remove all nodes and clear the root pointer - nodes_.clear(); - root_.reset(); - } - - /* ************************************************************************* */ - template - void BayesTree::removePath(sharedClique clique, - BayesNet& bn, typename BayesTree::Cliques& orphans) { - - // base case is NULL, if so we do nothing and return empties above - if (clique!=NULL) { - - // remove the clique from orphans in case it has been added earlier - orphans.remove(clique); - - // remove me - this->removeClique(clique); - - // remove path above me - this->removePath(typename Clique::shared_ptr(clique->parent_.lock()), bn, orphans); - - // add children to list of orphans (splice also removed them from clique->children_) - orphans.splice(orphans.begin(), clique->children_); - - bn.push_back(clique->conditional()); - - } - } - - /* ************************************************************************* */ - template - template - void BayesTree::removeTop(const CONTAINER& keys, - BayesNet& bn, typename BayesTree::Cliques& orphans) { - - // process each key of the new factor - BOOST_FOREACH(const Index& j, keys) { - - // get the clique - if(j < nodes_.size()) { - const sharedClique& clique(nodes_[j]); - if(clique) { - // remove path from clique to root - this->removePath(clique, bn, orphans); - } - } - } - - // Delete cachedShortcuts for each orphan subtree - //TODO: Consider Improving - BOOST_FOREACH(sharedClique& orphan, orphans) - orphan->deleteCachedShortcuts(); - } - - /* ************************************************************************* */ - template - typename BayesTree::Cliques BayesTree::removeSubtree( - const sharedClique& subtree) - { - // Result clique list - Cliques cliques; - cliques.push_back(subtree); - - // Remove the first clique from its parents - if(!subtree->isRoot()) - subtree->parent()->children().remove(subtree); - else - root_.reset(); - - // Add all subtree cliques and erase the children and parent of each - for(typename Cliques::iterator clique = cliques.begin(); clique != cliques.end(); ++clique) - { - // Add children - BOOST_FOREACH(const sharedClique& child, (*clique)->children()) { - cliques.push_back(child); } - - // Delete cached shortcuts - (*clique)->deleteCachedShortcutsNonRecursive(); - - // Remove this node from the nodes index - BOOST_FOREACH(Index j, (*clique)->conditional()->frontals()) { - nodes_[j].reset(); } - - // Erase the parent and children pointers - (*clique)->parent_.reset(); - (*clique)->children_.clear(); - } - - return cliques; - } - - /* ************************************************************************* */ - template - void BayesTree::cloneTo(This& newTree) const { - if(root()) - cloneTo(newTree, root(), sharedClique()); - } - - /* ************************************************************************* */ - template - void BayesTree::cloneTo( - This& newTree, const sharedClique& subtree, const sharedClique& parent) const { - sharedClique newClique(subtree->clone()); - newTree.addClique(newClique, parent); - BOOST_FOREACH(const sharedClique& childClique, subtree->children()) { - cloneTo(newTree, childClique, newClique); - } - } - - /* ************************************************************************* */ - -} // \namespace gtsam diff --git a/gtsam/inference/BayesTree-inst.h b/gtsam/inference/BayesTree-inst.h new file mode 100644 index 000000000..281648409 --- /dev/null +++ b/gtsam/inference/BayesTree-inst.h @@ -0,0 +1,521 @@ +/* ---------------------------------------------------------------------------- + + * 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 BayesTree-inl.h + * @brief Bayes Tree is a tree of cliques of a Bayes Chain + * @author Frank Dellaert + * @author Michael Kaess + * @author Viorela Ila + * @author Richard Roberts + */ + +#pragma once + +#include +#include +#include +#include + +#include +#include +#include +#include + +using boost::assign::cref_list_of; + +namespace gtsam { + + /* ************************************************************************* */ + template + BayesTreeCliqueData BayesTree::getCliqueData() const { + BayesTreeCliqueData data; + BOOST_FOREACH(const sharedClique& root, roots_) + getCliqueData(data, root); + return data; + } + + /* ************************************************************************* */ + template + void BayesTree::getCliqueData(BayesTreeCliqueData& data, sharedClique clique) const { + data.conditionalSizes.push_back(clique->conditional()->nrFrontals()); + data.separatorSizes.push_back(clique->conditional()->nrParents()); + BOOST_FOREACH(sharedClique c, clique->children) { + getCliqueData(data, c); + } + } + + /* ************************************************************************* */ + template + size_t BayesTree::numCachedSeparatorMarginals() const { + size_t count = 0; + BOOST_FOREACH(const sharedClique& root, roots_) + count += root->numCachedSeparatorMarginals(); + return count; + } + + /* ************************************************************************* */ + template + void BayesTree::saveGraph(const std::string &s, const KeyFormatter& keyFormatter) const { + if (roots_.empty()) throw std::invalid_argument("the root of Bayes tree has not been initialized!"); + std::ofstream of(s.c_str()); + of<< "digraph G{\n"; + BOOST_FOREACH(const sharedClique& root, roots_) + saveGraph(of, root, keyFormatter); + of<<"}"; + of.close(); + } + + /* ************************************************************************* */ + template + void BayesTree::saveGraph(std::ostream &s, sharedClique clique, const KeyFormatter& indexFormatter, int parentnum) const { + static int num = 0; + bool first = true; + std::stringstream out; + out << num; + std::string parent = out.str(); + parent += "[label=\""; + + BOOST_FOREACH(Key index, clique->conditional_->frontals()) { + if(!first) parent += ","; first = false; + parent += indexFormatter(index); + } + + if(clique->parent()){ + parent += " : "; + s << parentnum << "->" << num << "\n"; + } + + first = true; + BOOST_FOREACH(Key sep, clique->conditional_->parents()) { + if(!first) parent += ","; first = false; + parent += indexFormatter(sep); + } + parent += "\"];\n"; + s << parent; + parentnum = num; + + BOOST_FOREACH(sharedClique c, clique->children) { + num++; + saveGraph(s, c, indexFormatter, parentnum); + } + } + + /* ************************************************************************* */ + template + size_t BayesTree::size() const { + size_t size = 0; + BOOST_FOREACH(const sharedClique& clique, roots_) + size += clique->treeSize(); + return size; + } + + /* ************************************************************************* */ + template + void BayesTree::addClique(const sharedClique& clique, const sharedClique& parent_clique) { + BOOST_FOREACH(Key j, clique->conditional()->frontals()) + nodes_[j] = clique; + if (parent_clique != NULL) { + clique->parent_ = parent_clique; + parent_clique->children.push_back(clique); + } else { + roots_.push_back(clique); + } + } + + /* ************************************************************************* */ + // TODO: Clean up + namespace { + template + int _pushClique(FactorGraph& fg, const boost::shared_ptr& clique) { + fg.push_back(clique->conditional_); + return 0; + } + + template + struct _pushCliqueFunctor { + _pushCliqueFunctor(FactorGraph& graph_) : graph(graph_) {} + FactorGraph& graph; + int operator()(const boost::shared_ptr& clique, int dummy) { + graph.push_back(clique->conditional_); + return 0; + } + }; + } + + /* ************************************************************************* */ + template + void BayesTree::addFactorsToGraph(FactorGraph& graph) const + { + // Traverse the BayesTree and add all conditionals to this graph + int data = 0; // Unused + _pushCliqueFunctor functor(graph); + treeTraversal::DepthFirstForest(*this, data, functor); // FIXME: sort of works? +// treeTraversal::DepthFirstForest(*this, data, boost::bind(&_pushClique, boost::ref(graph), _1)); + } + + /* ************************************************************************* */ + template + BayesTree::BayesTree(const This& other) { + *this = other; + } + + /* ************************************************************************* */ + namespace { + template + boost::shared_ptr + BayesTreeCloneForestVisitorPre(const boost::shared_ptr& node, const boost::shared_ptr& parentPointer) + { + // Clone the current node and add it to its cloned parent + boost::shared_ptr clone = boost::make_shared(*node); + clone->children.clear(); + clone->parent_ = parentPointer; + parentPointer->children.push_back(clone); + return clone; + } + } + + /* ************************************************************************* */ + template + BayesTree& BayesTree::operator=(const This& other) { + this->clear(); + boost::shared_ptr rootContainer = boost::make_shared(); + treeTraversal::DepthFirstForest(other, rootContainer, BayesTreeCloneForestVisitorPre); + BOOST_FOREACH(const sharedClique& root, rootContainer->children) { + root->parent_ = typename Clique::weak_ptr(); // Reset the parent since it's set to the dummy clique + insertRoot(root); + } + return *this; + } + + /* ************************************************************************* */ + template + void BayesTree::print(const std::string& s, const KeyFormatter& keyFormatter) const { + std::cout << s << ": cliques: " << size() << ", variables: " << nodes_.size() << std::endl; + treeTraversal::PrintForest(*this, s, keyFormatter); + } + + /* ************************************************************************* */ + // binary predicate to test equality of a pair for use in equals + template + bool check_sharedCliques( + const std::pair::sharedClique>& v1, + const std::pair::sharedClique>& v2 + ) { + return v1.first == v2.first && + ((!v1.second && !v2.second) || (v1.second && v2.second && v1.second->equals(*v2.second))); + } + + /* ************************************************************************* */ + template + bool BayesTree::equals(const BayesTree& other, double tol) const { + return size()==other.size() && + std::equal(nodes_.begin(), nodes_.end(), other.nodes_.begin(), &check_sharedCliques); + } + + /* ************************************************************************* */ + template + template + Key BayesTree::findParentClique(const CONTAINER& parents) const { + typename CONTAINER::const_iterator lowestOrderedParent = min_element(parents.begin(), parents.end()); + assert(lowestOrderedParent != parents.end()); + return *lowestOrderedParent; + } + + /* ************************************************************************* */ + template + void BayesTree::fillNodesIndex(const sharedClique& subtree) { + // Add each frontal variable of this root node + BOOST_FOREACH(const Key& j, subtree->conditional()->frontals()) { + bool inserted = nodes_.insert(std::make_pair(j, subtree)).second; + assert(inserted); (void)inserted; + } + // Fill index for each child + typedef typename BayesTree::sharedClique sharedClique; + BOOST_FOREACH(const sharedClique& child, subtree->children) { + fillNodesIndex(child); } + } + + /* ************************************************************************* */ + template + void BayesTree::insertRoot(const sharedClique& subtree) { + roots_.push_back(subtree); // Add to roots + fillNodesIndex(subtree); // Populate nodes index + } + + /* ************************************************************************* */ + // First finds clique marginal then marginalizes that + /* ************************************************************************* */ + template + typename BayesTree::sharedConditional + BayesTree::marginalFactor(Key j, const Eliminate& function) const + { + gttic(BayesTree_marginalFactor); + + // get clique containing Key j + sharedClique clique = this->clique(j); + + // calculate or retrieve its marginal P(C) = P(F,S) + FactorGraphType cliqueMarginal = clique->marginal2(function); + + // Now, marginalize out everything that is not variable j + BayesNetType marginalBN = *cliqueMarginal.marginalMultifrontalBayesNet( + Ordering(cref_list_of<1,Key>(j)), boost::none, function); + + // The Bayes net should contain only one conditional for variable j, so return it + return marginalBN.front(); + } + + /* ************************************************************************* */ + // Find two cliques, their joint, then marginalizes + /* ************************************************************************* */ + template + typename BayesTree::sharedFactorGraph + BayesTree::joint(Key j1, Key j2, const Eliminate& function) const + { + gttic(BayesTree_joint); + return boost::make_shared(*jointBayesNet(j1, j2, function)); + } + + /* ************************************************************************* */ + template + typename BayesTree::sharedBayesNet + BayesTree::jointBayesNet(Key j1, Key j2, const Eliminate& function) const + { + gttic(BayesTree_jointBayesNet); + // get clique C1 and C2 + sharedClique C1 = (*this)[j1], C2 = (*this)[j2]; + + gttic(Lowest_common_ancestor); + // Find lowest common ancestor clique + sharedClique B; { + // Build two paths to the root + FastList path1, path2; { + sharedClique p = C1; + while(p) { + path1.push_front(p); + p = p->parent(); + } + } { + sharedClique p = C2; + while(p) { + path2.push_front(p); + p = p->parent(); + } + } + // Find the path intersection + typename FastList::const_iterator p1 = path1.begin(), p2 = path2.begin(); + if(*p1 == *p2) + B = *p1; + while(p1 != path1.end() && p2 != path2.end() && *p1 == *p2) { + B = *p1; + ++p1; + ++p2; + } + } + gttoc(Lowest_common_ancestor); + + // Build joint on all involved variables + FactorGraphType p_BC1C2; + + if(B) + { + // Compute marginal on lowest common ancestor clique + gttic(LCA_marginal); + FactorGraphType p_B = B->marginal2(function); + gttoc(LCA_marginal); + + // Compute shortcuts of the requested cliques given the lowest common ancestor + gttic(Clique_shortcuts); + BayesNetType p_C1_Bred = C1->shortcut(B, function); + BayesNetType p_C2_Bred = C2->shortcut(B, function); + gttoc(Clique_shortcuts); + + // Factor the shortcuts to be conditioned on the full root + // Get the set of variables to eliminate, which is C1\B. + gttic(Full_root_factoring); + boost::shared_ptr p_C1_B; { + FastVector C1_minus_B; { + FastSet C1_minus_B_set(C1->conditional()->beginParents(), C1->conditional()->endParents()); + BOOST_FOREACH(const Key j, *B->conditional()) { + C1_minus_B_set.erase(j); } + C1_minus_B.assign(C1_minus_B_set.begin(), C1_minus_B_set.end()); + } + // Factor into C1\B | B. + sharedFactorGraph temp_remaining; + boost::tie(p_C1_B, temp_remaining) = + FactorGraphType(p_C1_Bred).eliminatePartialMultifrontal(Ordering(C1_minus_B), function); + } + boost::shared_ptr p_C2_B; { + FastVector C2_minus_B; { + FastSet C2_minus_B_set(C2->conditional()->beginParents(), C2->conditional()->endParents()); + BOOST_FOREACH(const Key j, *B->conditional()) { + C2_minus_B_set.erase(j); } + C2_minus_B.assign(C2_minus_B_set.begin(), C2_minus_B_set.end()); + } + // Factor into C2\B | B. + sharedFactorGraph temp_remaining; + boost::tie(p_C2_B, temp_remaining) = + FactorGraphType(p_C2_Bred).eliminatePartialMultifrontal(Ordering(C2_minus_B), function); + } + gttoc(Full_root_factoring); + + gttic(Variable_joint); + p_BC1C2 += p_B; + p_BC1C2 += *p_C1_B; + p_BC1C2 += *p_C2_B; + if(C1 != B) + p_BC1C2 += C1->conditional(); + if(C2 != B) + p_BC1C2 += C2->conditional(); + gttoc(Variable_joint); + } + else + { + // The nodes have no common ancestor, they're in different trees, so they're joint is just the + // product of their marginals. + gttic(Disjoint_marginals); + p_BC1C2 += C1->marginal2(function); + p_BC1C2 += C2->marginal2(function); + gttoc(Disjoint_marginals); + } + + // now, marginalize out everything that is not variable j1 or j2 + return p_BC1C2.marginalMultifrontalBayesNet(Ordering(cref_list_of<2,Key>(j1)(j2)), boost::none, function); + } + + /* ************************************************************************* */ + template + void BayesTree::clear() { + // Remove all nodes and clear the root pointer + nodes_.clear(); + roots_.clear(); + } + + /* ************************************************************************* */ + template + void BayesTree::deleteCachedShortcuts() { + BOOST_FOREACH(const sharedClique& root, roots_) { + root->deleteCachedShortcuts(); + } + } + + /* ************************************************************************* */ + template + void BayesTree::removeClique(sharedClique clique) + { + if (clique->isRoot()) { + typename Roots::iterator root = std::find(roots_.begin(), roots_.end(), clique); + if(root != roots_.end()) + roots_.erase(root); + } else { // detach clique from parent + sharedClique parent = clique->parent_.lock(); + typename Roots::iterator child = std::find(parent->children.begin(), parent->children.end(), clique); + assert(child != parent->children.end()); + parent->children.erase(child); + } + + // orphan my children + BOOST_FOREACH(sharedClique child, clique->children) + child->parent_ = typename Clique::weak_ptr(); + + BOOST_FOREACH(Key j, clique->conditional()->frontals()) { + nodes_.unsafe_erase(j); + } + } + + /* ************************************************************************* */ + template + void BayesTree::removePath(sharedClique clique, BayesNetType& bn, Cliques& orphans) + { + // base case is NULL, if so we do nothing and return empties above + if (clique) { + + // remove the clique from orphans in case it has been added earlier + orphans.remove(clique); + + // remove me + this->removeClique(clique); + + // remove path above me + this->removePath(typename Clique::shared_ptr(clique->parent_.lock()), bn, orphans); + + // add children to list of orphans (splice also removed them from clique->children_) + orphans.insert(orphans.begin(), clique->children.begin(), clique->children.end()); + clique->children.clear(); + + bn.push_back(clique->conditional_); + + } + } + + /* ************************************************************************* */ + template + void BayesTree::removeTop(const FastVector& keys, BayesNetType& bn, Cliques& orphans) + { + // process each key of the new factor + BOOST_FOREACH(const Key& j, keys) + { + // get the clique + // TODO: Nodes will be searched again in removeClique + typename Nodes::const_iterator node = nodes_.find(j); + if(node != nodes_.end()) { + // remove path from clique to root + this->removePath(node->second, bn, orphans); + } + } + + // Delete cachedShortcuts for each orphan subtree + //TODO: Consider Improving + BOOST_FOREACH(sharedClique& orphan, orphans) + orphan->deleteCachedShortcuts(); + } + + /* ************************************************************************* */ + template + typename BayesTree::Cliques BayesTree::removeSubtree( + const sharedClique& subtree) + { + // Result clique list + Cliques cliques; + cliques.push_back(subtree); + + // Remove the first clique from its parents + if(!subtree->isRoot()) + subtree->parent()->children.erase(std::find( + subtree->parent()->children.begin(), subtree->parent()->children.end(), subtree)); + else + roots_.erase(std::find(roots_.begin(), roots_.end(), subtree)); + + // Add all subtree cliques and erase the children and parent of each + for(typename Cliques::iterator clique = cliques.begin(); clique != cliques.end(); ++clique) + { + // Add children + BOOST_FOREACH(const sharedClique& child, (*clique)->children) { + cliques.push_back(child); } + + // Delete cached shortcuts + (*clique)->deleteCachedShortcutsNonRecursive(); + + // Remove this node from the nodes index + BOOST_FOREACH(Key j, (*clique)->conditional()->frontals()) { + nodes_.unsafe_erase(j); } + + // Erase the parent and children pointers + (*clique)->parent_.reset(); + (*clique)->children.clear(); + } + + return cliques; + } + +} +/// namespace gtsam diff --git a/gtsam/inference/BayesTree.cpp b/gtsam/inference/BayesTree.cpp new file mode 100644 index 000000000..6d6a4a4d3 --- /dev/null +++ b/gtsam/inference/BayesTree.cpp @@ -0,0 +1,63 @@ +/* ---------------------------------------------------------------------------- + + * 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 BayesTree.cpp + * @brief Bayes Tree is a tree of cliques of a Bayes Chain + * @author Frank Dellaert + * @author Michael Kaess + * @author Viorela Ila + * @author Richard Roberts + */ + +#include + +#include +#include + +namespace gtsam { + +/* ************************************************************************* */ +void BayesTreeCliqueStats::print(const std::string& s) const { + std::cout << s + << "avg Conditional Size: " << avgConditionalSize << std::endl + << "max Conditional Size: " << maxConditionalSize << std::endl + << "avg Separator Size: " << avgSeparatorSize << std::endl + << "max Separator Size: " << maxSeparatorSize << std::endl; +} + +/* ************************************************************************* */ +BayesTreeCliqueStats BayesTreeCliqueData::getStats() const +{ + BayesTreeCliqueStats stats; + + double sum = 0.0; + size_t max = 0; + BOOST_FOREACH(size_t s, conditionalSizes) { + sum += (double)s; + if(s > max) max = s; + } + stats.avgConditionalSize = sum / (double)conditionalSizes.size(); + stats.maxConditionalSize = max; + + sum = 0.0; + max = 1; + BOOST_FOREACH(size_t s, separatorSizes) { + sum += (double)s; + if(s > max) max = s; + } + stats.avgSeparatorSize = sum / (double)separatorSizes.size(); + stats.maxSeparatorSize = max; + + return stats; +} + +} diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index 9e487d0a2..830ddd3ec 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -19,27 +19,37 @@ #pragma once -#include -#include -#include -#include -#include -#include -#include +#include -#include +#include #include -#include -#include -#include -#include -#include +#include +#include namespace gtsam { - // Forward declaration of BayesTreeClique which is defined below BayesTree in this file - template struct BayesTreeClique; + // Forward declarations + template class FactorGraph; + template class ClusterTree; + /* ************************************************************************* */ + /** clique statistics */ + struct GTSAM_EXPORT BayesTreeCliqueStats { + double avgConditionalSize; + std::size_t maxConditionalSize; + double avgSeparatorSize; + std::size_t maxSeparatorSize; + void print(const std::string& s = "") const ; + }; + + /** store all the sizes */ + struct GTSAM_EXPORT BayesTreeCliqueData { + FastVector conditionalSizes; + FastVector separatorSizes; + BayesTreeCliqueStats getStats() const; + }; + + /* ************************************************************************* */ /** * Bayes tree * @tparam CONDITIONAL The type of the conditional densities, i.e. the type of node in the underlying Bayes chain, @@ -50,59 +60,45 @@ namespace gtsam { * \addtogroup Multifrontal * \nosubgrouping */ - template > - class BayesTree { + template + class BayesTree + { + protected: + typedef BayesTree This; + typedef boost::shared_ptr shared_ptr; public: - - typedef BayesTree This; - typedef boost::shared_ptr > shared_ptr; - typedef boost::shared_ptr sharedConditional; - typedef boost::shared_ptr > sharedBayesNet; - typedef CONDITIONAL ConditionalType; - typedef typename CONDITIONAL::FactorType FactorType; - typedef typename FactorGraph::Eliminate Eliminate; - typedef CLIQUE Clique; ///< The clique type, normally BayesTreeClique + typedef boost::shared_ptr sharedClique; ///< Shared pointer to a clique + typedef Clique Node; ///< Synonym for Clique (TODO: remove) + typedef sharedClique sharedNode; ///< Synonym for sharedClique (TODO: remove) + typedef typename CLIQUE::ConditionalType ConditionalType; + typedef boost::shared_ptr sharedConditional; + typedef typename CLIQUE::BayesNetType BayesNetType; + typedef boost::shared_ptr sharedBayesNet; + typedef typename CLIQUE::FactorType FactorType; + typedef boost::shared_ptr sharedFactor; + typedef typename CLIQUE::FactorGraphType FactorGraphType; + typedef boost::shared_ptr sharedFactorGraph; + typedef typename FactorGraphType::Eliminate Eliminate; + typedef typename CLIQUE::EliminationTraitsType EliminationTraitsType; - // typedef for shared pointers to cliques - typedef boost::shared_ptr sharedClique; + /** A convenience class for a list of shared cliques */ + typedef FastList Cliques; - // A convenience class for a list of shared cliques - struct Cliques : public FastList { - void print(const std::string& s = "Cliques", - const IndexFormatter& indexFormatter = DefaultIndexFormatter) const; - bool equals(const Cliques& other, double tol = 1e-9) const; - }; - - /** clique statistics */ - struct CliqueStats { - double avgConditionalSize; - std::size_t maxConditionalSize; - double avgSeparatorSize; - std::size_t maxSeparatorSize; - void print(const std::string& s = "") const ; - }; - - /** store all the sizes */ - struct CliqueData { - std::vector conditionalSizes; - std::vector separatorSizes; - CliqueStats getStats() const; - }; - - /** Map from indices to Clique */ - typedef std::vector Nodes; + /** Map from keys to Clique */ + typedef ConcurrentMap Nodes; protected: /** Map from indices to Clique */ Nodes nodes_; - /** Root clique */ - sharedClique root_; - - public: + /** Root cliques */ + typedef FastVector Roots; + + /** Root cliques */ + Roots roots_; /// @name Standard Constructors /// @{ @@ -110,60 +106,31 @@ namespace gtsam { /** Create an empty Bayes Tree */ BayesTree() {} - /** Create a Bayes Tree from a Bayes Net (requires CONDITIONAL is IndexConditional *or* CONDITIONAL::Combine) */ - explicit BayesTree(const BayesNet& bayesNet); - /** Copy constructor */ BayesTree(const This& other); + /// @} + /** Assignment operator */ This& operator=(const This& other); - /// @} - /// @name Advanced Constructors - /// @{ - - /** - * Create a Bayes Tree from a Bayes Net and some subtrees. The Bayes net corresponds to the - * new root clique and the subtrees are connected to the root clique. - */ - BayesTree(const BayesNet& bayesNet, std::list > subtrees); - - /** Destructor */ - virtual ~BayesTree() {} - - /// @} /// @name Testable /// @{ /** check equality */ - bool equals(const BayesTree& other, double tol = 1e-9) const; + bool equals(const This& other, double tol = 1e-9) const; + public: /** print */ void print(const std::string& s = "", - const IndexFormatter& indexFormatter = DefaultIndexFormatter ) const; - + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; /// @} + /// @name Standard Interface /// @{ - /** - * Find parent clique of a conditional. It will look at all parents and - * return the one with the lowest index in the ordering. - */ - template - Index findParentClique(const CONTAINER& parents) const; - /** number of cliques */ - inline size_t size() const { - if(root_) - return root_->treeSize(); - else - return 0; - } - - /** number of nodes */ - inline size_t nrNodes() const { return nodes_.size(); } + size_t size() const; /** Check if there are any cliques in the tree */ inline bool empty() const { @@ -173,246 +140,149 @@ namespace gtsam { /** return nodes */ const Nodes& nodes() const { return nodes_; } - /** return root clique */ - const sharedClique& root() const { return root_; } + /** Access node by variable */ + const sharedNode operator[](Key j) const { return nodes_.at(j); } - /** find the clique that contains the variable with Index j */ - inline sharedClique operator[](Index j) const { - return nodes_.at(j); - } + /** return root cliques */ + const Roots& roots() const { return roots_; } - /** alternate syntax for matlab: find the clique that contains the variable with Index j */ - inline sharedClique clique(Index j) const { - return nodes_.at(j); + /** alternate syntax for matlab: find the clique that contains the variable with Key j */ + const sharedClique& clique(Key j) const { + typename Nodes::const_iterator c = nodes_.find(j); + if(c == nodes_.end()) + throw std::out_of_range("Requested the BayesTree clique for a key that is not in the BayesTree"); + else + return c->second; } /** Gather data on all cliques */ - CliqueData getCliqueData() const; + BayesTreeCliqueData getCliqueData() const; /** Collect number of cliques with cached separator marginals */ size_t numCachedSeparatorMarginals() const; - /** return marginal on any variable */ - typename FactorType::shared_ptr marginalFactor(Index j, Eliminate function) const; - - /** - * return marginal on any variable, as a Bayes Net - * NOTE: this function calls marginal, and then eliminates it into a Bayes Net - * This is more expensive than the above function - */ - typename BayesNet::shared_ptr marginalBayesNet(Index j, Eliminate function) const; + /** Return marginal on any variable. Note that this actually returns a conditional, for which a + * solution may be directly obtained by calling .solve() on the returned object. + * Alternatively, it may be directly used as its factor base class. For example, for Gaussian + * systems, this returns a GaussianConditional, which inherits from JacobianFactor and + * GaussianFactor. */ + sharedConditional marginalFactor(Key j, const Eliminate& function = EliminationTraitsType::DefaultEliminate) const; /** * return joint on two variables * Limitation: can only calculate joint if cliques are disjoint or one of them is root */ - typename FactorGraph::shared_ptr joint(Index j1, Index j2, Eliminate function) const; + sharedFactorGraph joint(Key j1, Key j2, const Eliminate& function = EliminationTraitsType::DefaultEliminate) const; /** * return joint on two variables as a BayesNet * Limitation: can only calculate joint if cliques are disjoint or one of them is root */ - typename BayesNet::shared_ptr jointBayesNet(Index j1, Index j2, Eliminate function) const; + sharedBayesNet jointBayesNet(Key j1, Key j2, const Eliminate& function = EliminationTraitsType::DefaultEliminate) const; /** * Read only with side effects */ /** saves the Tree to a text file in GraphViz format */ - void saveGraph(const std::string& s, const IndexFormatter& indexFormatter = DefaultIndexFormatter ) const; + void saveGraph(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; /// @} /// @name Advanced Interface /// @{ - - /** Access the root clique (non-const version) */ - sharedClique& root() { return root_; } - - /** Access the nodes (non-cost version) */ - Nodes& nodes() { return nodes_; } + + /** + * Find parent clique of a conditional. It will look at all parents and + * return the one with the lowest index in the ordering. + */ + template + Key findParentClique(const CONTAINER& parents) const; /** Remove all nodes */ void clear(); /** Clear all shortcut caches - use before timing on marginal calculation to avoid residual cache data */ - void deleteCachedShortcuts() { - root_->deleteCachedShortcuts(); - } - - /** Apply a permutation to the full tree - also updates the nodes structure */ - void permuteWithInverse(const Permutation& inversePermutation); + void deleteCachedShortcuts(); /** * Remove path from clique to root and return that path as factors * plus a list of orphaned subtree roots. Used in removeTop below. */ - void removePath(sharedClique clique, BayesNet& bn, Cliques& orphans); + void removePath(sharedClique clique, BayesNetType& bn, Cliques& orphans); /** * Given a list of indices, turn "contaminated" part of the tree back into a factor graph. * Factors and orphans are added to the in/out arguments. */ - template - void removeTop(const CONTAINER& indices, BayesNet& bn, Cliques& orphans); + void removeTop(const FastVector& keys, BayesNetType& bn, Cliques& orphans); /** * Remove the requested subtree. */ Cliques removeSubtree(const sharedClique& subtree); - /** - * Hang a new subtree off of the existing tree. This finds the appropriate - * parent clique for the subtree (which may be the root), and updates the - * nodes index with the new cliques in the subtree. None of the frontal - * variables in the subtree may appear in the separators of the existing - * BayesTree. - */ - void insert(const sharedClique& subtree); - - /** Insert a new conditional - * This function only applies for Symbolic case with IndexCondtional, - * We make it static so that it won't be compiled in GaussianConditional case. - * */ - static void insert(BayesTree& bayesTree, const sharedConditional& conditional); - - /** - * Insert a new clique corresponding to the given Bayes net. - * It is the caller's responsibility to decide whether the given Bayes net is a valid clique, - * i.e. all the variables (frontal and separator) are connected - */ - sharedClique insert(const sharedConditional& clique, - std::list& children, bool isRootClique = false); - - /** - * Create a clone of this object as a shared pointer - * Necessary for inheritance in matlab interface - */ - virtual shared_ptr clone() const { - return shared_ptr(new This(*this)); - } - - protected: - - /** private helper method for saving the Tree to a text file in GraphViz format */ - void saveGraph(std::ostream &s, sharedClique clique, const IndexFormatter& indexFormatter, - int parentnum = 0) const; - - /** Gather data on a single clique */ - void getCliqueData(CliqueData& stats, sharedClique clique) const; - - /** remove a clique: warning, can result in a forest */ - void removeClique(sharedClique clique); - - /** add a clique (top down) */ - sharedClique addClique(const sharedConditional& conditional, const sharedClique& parent_clique = sharedClique()); + /** Insert a new subtree with known parent clique. This function does not check that the + * specified parent is the correct parent. This function updates all of the internal data + * structures associated with adding a subtree, such as populating the nodes index. */ + void insertRoot(const sharedClique& subtree); /** add a clique (top down) */ void addClique(const sharedClique& clique, const sharedClique& parent_clique = sharedClique()); - /** add a clique (bottom up) */ - sharedClique addClique(const sharedConditional& conditional, std::list& child_cliques); + /** Add all cliques in this BayesTree to the specified factor graph */ + void addFactorsToGraph(FactorGraph& graph) const; - /** - * Add a conditional to the front of a clique, i.e. a conditional whose - * parents are already in the clique or its separators. This function does - * not check for this condition, it just updates the data structures. - */ - static void addToCliqueFront(BayesTree& bayesTree, - const sharedConditional& conditional, const sharedClique& clique); + protected: + + /** private helper method for saving the Tree to a text file in GraphViz format */ + void saveGraph(std::ostream &s, sharedClique clique, const KeyFormatter& keyFormatter, + int parentnum = 0) const; + + /** Gather data on a single clique */ + void getCliqueData(BayesTreeCliqueData& stats, sharedClique clique) const; + + /** remove a clique: warning, can result in a forest */ + void removeClique(sharedClique clique); /** Fill the nodes index for a subtree */ void fillNodesIndex(const sharedClique& subtree); - /** Helper function to build a non-symbolic tree (e.g. Gaussian) using a - * symbolic tree, used in the BT(BN) constructor. - */ - void recursiveTreeBuild(const boost::shared_ptr >& symbolic, - const std::vector >& conditionals, - const typename BayesTree::sharedClique& parent); + // Friend JunctionTree because it directly fills roots and nodes index. + template friend class ClusterTree; private: - - /** deep copy to another tree */ - void cloneTo(This& newTree) const; - - /** deep copy to another tree */ - void cloneTo(This& newTree, const sharedClique& subtree, const sharedClique& parent) const; - /** Serialization function */ friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { ar & BOOST_SERIALIZATION_NVP(nodes_); - ar & BOOST_SERIALIZATION_NVP(root_); + ar & BOOST_SERIALIZATION_NVP(roots_); } /// @} }; // BayesTree - /* ************************************************************************* */ - template - void _BayesTree_dim_adder( - std::vector& dims, - const typename BayesTree::sharedClique& clique) { - - if(clique) { - // Add dims from this clique - for(typename CONDITIONAL::const_iterator it = (*clique)->beginFrontals(); it != (*clique)->endFrontals(); ++it) - dims[*it] = (*clique)->dim(it); - - // Traverse children - typedef typename BayesTree::sharedClique sharedClique; - BOOST_FOREACH(const sharedClique& child, clique->children()) { - _BayesTree_dim_adder(dims, child); - } - } - } - - /* ************************************************************************* */ - template - boost::shared_ptr allocateVectorValues(const BayesTree& bt) { - std::vector dimensions(bt.nodes().size(), 0); - _BayesTree_dim_adder(dimensions, bt.root()); - return boost::shared_ptr(new VectorValues(dimensions)); - } - - - /* ************************************************************************* */ - /** - * A Clique in the tree is an incomplete Bayes net: the variables - * in the Bayes net are the frontal nodes, and the variables conditioned - * on are the separator. We also have pointers up and down the tree. - * - * Since our Conditional class already handles multiple frontal variables, - * this Clique contains exactly 1 conditional. - * - * This is the default clique type in a BayesTree, but some algorithms, like - * iSAM2 (see ISAM2Clique), use a different clique type in order to store - * extra data along with the clique. - */ - template - struct BayesTreeClique : public BayesTreeCliqueBase, CONDITIONAL> { + template + class BayesTreeOrphanWrapper : public CLIQUE::ConditionalType + { public: - typedef CONDITIONAL ConditionalType; - typedef BayesTreeClique This; - typedef BayesTreeCliqueBase Base; - typedef boost::shared_ptr shared_ptr; - typedef boost::weak_ptr weak_ptr; - BayesTreeClique() {} - BayesTreeClique(const typename ConditionalType::shared_ptr& conditional) : Base(conditional) {} - BayesTreeClique(const std::pair& result) : Base(result) {} + typedef CLIQUE CliqueType; + typedef typename CLIQUE::ConditionalType Base; - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + boost::shared_ptr clique; + + BayesTreeOrphanWrapper(const boost::shared_ptr& clique) : + clique(clique) + { + // Store parent keys in our base type factor so that eliminating those parent keys will pull + // this subtree into the elimination. + this->keys_.assign(clique->conditional()->beginParents(), clique->conditional()->endParents()); + } + + void print(const std::string& s="", const KeyFormatter& formatter = DefaultKeyFormatter) const { + clique->print(s + "stored clique", formatter); } }; } /// namespace gtsam - -#include -#include diff --git a/gtsam/inference/BayesTreeCliqueBase-inl.h b/gtsam/inference/BayesTreeCliqueBase-inl.h deleted file mode 100644 index e2ac492b7..000000000 --- a/gtsam/inference/BayesTreeCliqueBase-inl.h +++ /dev/null @@ -1,301 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 BayesTreeCliqueBase-inl.h - * @brief Base class for cliques of a BayesTree - * @author Richard Roberts and Frank Dellaert - */ - -#pragma once - -#include -#include - -namespace gtsam { - - /* ************************************************************************* */ - template - void BayesTreeCliqueBase::assertInvariants() const { - } - - /* ************************************************************************* */ - template - std::vector BayesTreeCliqueBase::separator_setminus_B( - derived_ptr B) const { - sharedConditional p_F_S = this->conditional(); - std::vector &indicesB = B->conditional()->keys(); - std::vector S_setminus_B; - std::set_difference(p_F_S->beginParents(), p_F_S->endParents(), // - indicesB.begin(), indicesB.end(), back_inserter(S_setminus_B)); - return S_setminus_B; - } - - /* ************************************************************************* */ - template - std::vector BayesTreeCliqueBase::shortcut_indices( - derived_ptr B, const FactorGraph& p_Cp_B) const - { - gttic(shortcut_indices); - std::set allKeys = p_Cp_B.keys(); - std::vector &indicesB = B->conditional()->keys(); - std::vector S_setminus_B = separator_setminus_B(B); // TODO, get as argument? - std::vector keep; - // keep = S\B intersect allKeys - std::set_intersection(S_setminus_B.begin(), S_setminus_B.end(), // - allKeys.begin(), allKeys.end(), back_inserter(keep)); - // keep += B intersect allKeys - std::set_intersection(indicesB.begin(), indicesB.end(), // - allKeys.begin(), allKeys.end(), back_inserter(keep)); - // BOOST_FOREACH(Index j, keep) std::cout << j << " "; std::cout << std::endl; - return keep; - } - - /* ************************************************************************* */ - template - BayesTreeCliqueBase::BayesTreeCliqueBase( - const sharedConditional& conditional) : - conditional_(conditional) { - assertInvariants(); - } - - /* ************************************************************************* */ - template - BayesTreeCliqueBase::BayesTreeCliqueBase( - const std::pair >& result) : - conditional_(result.first) { - assertInvariants(); - } - - /* ************************************************************************* */ - template - void BayesTreeCliqueBase::print(const std::string& s, - const IndexFormatter& indexFormatter) const { - conditional_->print(s, indexFormatter); - } - - /* ************************************************************************* */ - template - size_t BayesTreeCliqueBase::treeSize() const { - size_t size = 1; - BOOST_FOREACH(const derived_ptr& child, children_) - size += child->treeSize(); - return size; - } - - /* ************************************************************************* */ - template - size_t BayesTreeCliqueBase::numCachedSeparatorMarginals() const { - if (!cachedSeparatorMarginal_) - return 0; - - size_t subtree_count = 1; - BOOST_FOREACH(const derived_ptr& child, children_) - subtree_count += child->numCachedSeparatorMarginals(); - - return subtree_count; - } - - /* ************************************************************************* */ - template - void BayesTreeCliqueBase::printTree( - const std::string& indent, const IndexFormatter& indexFormatter) const { - asDerived(this)->print(indent, indexFormatter); - BOOST_FOREACH(const derived_ptr& child, children_) - child->printTree(indent + " ", indexFormatter); - } - - /* ************************************************************************* */ - template - void BayesTreeCliqueBase::permuteWithInverse( - const Permutation& inversePermutation) { - conditional_->permuteWithInverse(inversePermutation); - BOOST_FOREACH(const derived_ptr& child, children_) { - child->permuteWithInverse(inversePermutation); - } - assertInvariants(); - } - - /* ************************************************************************* */ - template - bool BayesTreeCliqueBase::reduceSeparatorWithInverse( - const internal::Reduction& inverseReduction) - { - bool changed = conditional_->reduceSeparatorWithInverse(inverseReduction); -#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS - if(!changed) { - BOOST_FOREACH(const derived_ptr& child, children_) { - assert(child->reduceSeparatorWithInverse(inverseReduction) == false); } - } -#endif - if(changed) { - BOOST_FOREACH(const derived_ptr& child, children_) { - (void) child->reduceSeparatorWithInverse(inverseReduction); } - } - assertInvariants(); - return changed; - } - - /* ************************************************************************* */ - // The shortcut density is a conditional P(S|R) of the separator of this - // clique on the root. We can compute it recursively from the parent shortcut - // P(Sp|R) as \int P(Fp|Sp) P(Sp|R), where Fp are the frontal nodes in p - /* ************************************************************************* */ - template - BayesNet BayesTreeCliqueBase::shortcut( - derived_ptr B, Eliminate function) const - { - gttic(BayesTreeCliqueBase_shortcut); - - // We only calculate the shortcut when this clique is not B - // and when the S\B is not empty - std::vector S_setminus_B = separator_setminus_B(B); - if (B.get() != this && !S_setminus_B.empty()) { - - // Obtain P(Cp||B) = P(Fp|Sp) * P(Sp||B) as a factor graph - derived_ptr parent(parent_.lock()); - gttoc(BayesTreeCliqueBase_shortcut); - FactorGraph p_Cp_B(parent->shortcut(B, function)); // P(Sp||B) - gttic(BayesTreeCliqueBase_shortcut); - p_Cp_B.push_back(parent->conditional()->toFactor()); // P(Fp|Sp) - - // Determine the variables we want to keepSet, S union B - std::vector keep = shortcut_indices(B, p_Cp_B); - - // Reduce the variable indices to start at zero - gttic(Reduce); - const Permutation reduction = internal::createReducingPermutation(p_Cp_B.keys()); - internal::Reduction inverseReduction = internal::Reduction::CreateAsInverse(reduction); - BOOST_FOREACH(const boost::shared_ptr& factor, p_Cp_B) { - if(factor) factor->reduceWithInverse(inverseReduction); } - inverseReduction.applyInverse(keep); - gttoc(Reduce); - - // Create solver that will marginalize for us - GenericSequentialSolver solver(p_Cp_B); - - // Finally, we only want to have S\B variables in the Bayes net, so - size_t nrFrontals = S_setminus_B.size(); - BayesNet result = *solver.conditionalBayesNet(keep, nrFrontals, function); - - // Undo the reduction - gttic(Undo_Reduce); - BOOST_FOREACH(const typename boost::shared_ptr& factor, p_Cp_B) { - if (factor) factor->permuteWithInverse(reduction); } - result.permuteWithInverse(reduction); - gttoc(Undo_Reduce); - - assertInvariants(); - - return result; - } else { - return BayesNet(); - } - } - - /* ************************************************************************* */ - // separator marginal, uses separator marginal of parent recursively - // P(C) = P(F|S) P(S) - /* ************************************************************************* */ - template - FactorGraph::FactorType> BayesTreeCliqueBase< - DERIVED, CONDITIONAL>::separatorMarginal(derived_ptr R, Eliminate function) const - { - gttic(BayesTreeCliqueBase_separatorMarginal); - // Check if the Separator marginal was already calculated - if (!cachedSeparatorMarginal_) { - gttic(BayesTreeCliqueBase_separatorMarginal_cachemiss); - // If this is the root, there is no separator - if (R.get() == this) { - // we are root, return empty - FactorGraph empty; - cachedSeparatorMarginal_ = empty; - } else { - // Obtain P(S) = \int P(Cp) = \int P(Fp|Sp) P(Sp) - // initialize P(Cp) with the parent separator marginal - derived_ptr parent(parent_.lock()); - gttoc(BayesTreeCliqueBase_separatorMarginal_cachemiss); // Flatten recursion in timing outline - gttoc(BayesTreeCliqueBase_separatorMarginal); - FactorGraph p_Cp(parent->separatorMarginal(R, function)); // P(Sp) - gttic(BayesTreeCliqueBase_separatorMarginal); - gttic(BayesTreeCliqueBase_separatorMarginal_cachemiss); - // now add the parent conditional - p_Cp.push_back(parent->conditional()->toFactor()); // P(Fp|Sp) - - // Reduce the variable indices to start at zero - gttic(Reduce); - const Permutation reduction = internal::createReducingPermutation(p_Cp.keys()); - internal::Reduction inverseReduction = internal::Reduction::CreateAsInverse(reduction); - BOOST_FOREACH(const boost::shared_ptr& factor, p_Cp) { - if(factor) factor->reduceWithInverse(inverseReduction); } - - // The variables we want to keepSet are exactly the ones in S - sharedConditional p_F_S = this->conditional(); - std::vector indicesS(p_F_S->beginParents(), p_F_S->endParents()); - inverseReduction.applyInverse(indicesS); - gttoc(Reduce); - - // Create solver that will marginalize for us - GenericSequentialSolver solver(p_Cp); - - cachedSeparatorMarginal_ = *(solver.jointBayesNet(indicesS, function)); - - // Undo the reduction - gttic(Undo_Reduce); - BOOST_FOREACH(const typename boost::shared_ptr& factor, p_Cp) { - if (factor) factor->permuteWithInverse(reduction); } - BOOST_FOREACH(const typename boost::shared_ptr& factor, *cachedSeparatorMarginal_) { - if (factor) factor->permuteWithInverse(reduction); } - gttoc(Undo_Reduce); - } - } else { - gttic(BayesTreeCliqueBase_separatorMarginal_cachehit); - } - - // return the shortcut P(S||B) - return *cachedSeparatorMarginal_; // return the cached version - } - - /* ************************************************************************* */ - // marginal2, uses separator marginal of parent recursively - // P(C) = P(F|S) P(S) - /* ************************************************************************* */ - template - FactorGraph::FactorType> BayesTreeCliqueBase< - DERIVED, CONDITIONAL>::marginal2(derived_ptr R, Eliminate function) const - { - gttic(BayesTreeCliqueBase_marginal2); - // initialize with separator marginal P(S) - FactorGraph p_C(this->separatorMarginal(R, function)); - // add the conditional P(F|S) - p_C.push_back(this->conditional()->toFactor()); - return p_C; - } - - /* ************************************************************************* */ - template - void BayesTreeCliqueBase::deleteCachedShortcuts() { - - // When a shortcut is requested, all of the shortcuts between it and the - // root are also generated. So, if this clique's cached shortcut is set, - // recursively call over all child cliques. Otherwise, it is unnecessary. - if (cachedSeparatorMarginal_) { - BOOST_FOREACH(derived_ptr& child, children_) { - child->deleteCachedShortcuts(); - } - - //Delete CachedShortcut for this clique - cachedSeparatorMarginal_ = boost::none; - } - - } - -} diff --git a/gtsam/inference/BayesTreeCliqueBase-inst.h b/gtsam/inference/BayesTreeCliqueBase-inst.h new file mode 100644 index 000000000..274886c21 --- /dev/null +++ b/gtsam/inference/BayesTreeCliqueBase-inst.h @@ -0,0 +1,217 @@ +/* ---------------------------------------------------------------------------- + + * 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 BayesTreeCliqueBase-inst.h + * @brief Base class for cliques of a BayesTree + * @author Richard Roberts and Frank Dellaert + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + + /* ************************************************************************* */ + template + void BayesTreeCliqueBase::setEliminationResult( + const typename FactorGraphType::EliminationResult& eliminationResult) + { + conditional_ = eliminationResult.first; + } + + /* ************************************************************************* */ + template + bool BayesTreeCliqueBase::equals( + const DERIVED& other, double tol) const + { + return (!conditional_ && !other.conditional()) + || conditional_->equals(*other.conditional(), tol); + } + + /* ************************************************************************* */ + template + FastVector + BayesTreeCliqueBase::separator_setminus_B(const derived_ptr& B) const + { + FastSet p_F_S_parents(this->conditional()->beginParents(), this->conditional()->endParents()); + FastSet indicesB(B->conditional()->begin(), B->conditional()->end()); + FastVector S_setminus_B; + std::set_difference(p_F_S_parents.begin(), p_F_S_parents.end(), + indicesB.begin(), indicesB.end(), back_inserter(S_setminus_B)); + return S_setminus_B; + } + + /* ************************************************************************* */ + template + FastVector BayesTreeCliqueBase::shortcut_indices( + const derived_ptr& B, const FactorGraphType& p_Cp_B) const + { + gttic(shortcut_indices); + FastSet allKeys = p_Cp_B.keys(); + FastSet indicesB(B->conditional()->begin(), B->conditional()->end()); + FastVector S_setminus_B = separator_setminus_B(B); + FastVector keep; + // keep = S\B intersect allKeys (S_setminus_B is already sorted) + std::set_intersection(S_setminus_B.begin(), S_setminus_B.end(), // + allKeys.begin(), allKeys.end(), back_inserter(keep)); + // keep += B intersect allKeys + std::set_intersection(indicesB.begin(), indicesB.end(), // + allKeys.begin(), allKeys.end(), back_inserter(keep)); + return keep; + } + + /* ************************************************************************* */ + template + void BayesTreeCliqueBase::print( + const std::string& s, const KeyFormatter& keyFormatter) const + { + conditional_->print(s, keyFormatter); + } + + /* ************************************************************************* */ + template + size_t BayesTreeCliqueBase::treeSize() const { + size_t size = 1; + BOOST_FOREACH(const derived_ptr& child, children) + size += child->treeSize(); + return size; + } + + /* ************************************************************************* */ + template + size_t BayesTreeCliqueBase::numCachedSeparatorMarginals() const + { + if (!cachedSeparatorMarginal_) + return 0; + + size_t subtree_count = 1; + BOOST_FOREACH(const derived_ptr& child, children) + subtree_count += child->numCachedSeparatorMarginals(); + + return subtree_count; + } + + /* ************************************************************************* */ + // The shortcut density is a conditional P(S|R) of the separator of this + // clique on the root. We can compute it recursively from the parent shortcut + // P(Sp|R) as \int P(Fp|Sp) P(Sp|R), where Fp are the frontal nodes in p + /* ************************************************************************* */ + template + typename BayesTreeCliqueBase::BayesNetType + BayesTreeCliqueBase::shortcut(const derived_ptr& B, Eliminate function) const + { + gttic(BayesTreeCliqueBase_shortcut); + // We only calculate the shortcut when this clique is not B + // and when the S\B is not empty + FastVector S_setminus_B = separator_setminus_B(B); + if (!parent_.expired() /*(if we're not the root)*/ && !S_setminus_B.empty()) + { + // Obtain P(Cp||B) = P(Fp|Sp) * P(Sp||B) as a factor graph + derived_ptr parent(parent_.lock()); + gttoc(BayesTreeCliqueBase_shortcut); + FactorGraphType p_Cp_B(parent->shortcut(B, function)); // P(Sp||B) + gttic(BayesTreeCliqueBase_shortcut); + p_Cp_B += parent->conditional_; // P(Fp|Sp) + + // Determine the variables we want to keepSet, S union B + FastVector keep = shortcut_indices(B, p_Cp_B); + + // Marginalize out everything except S union B + boost::shared_ptr p_S_B = p_Cp_B.marginal(keep, function); + return *p_S_B->eliminatePartialSequential(S_setminus_B, function).first; + } + else + { + return BayesNetType(); + } + } + + /* ************************************************************************* */ + // separator marginal, uses separator marginal of parent recursively + // P(C) = P(F|S) P(S) + /* ************************************************************************* */ + template + typename BayesTreeCliqueBase::FactorGraphType + BayesTreeCliqueBase::separatorMarginal(Eliminate function) const + { + gttic(BayesTreeCliqueBase_separatorMarginal); + // Check if the Separator marginal was already calculated + if (!cachedSeparatorMarginal_) + { + gttic(BayesTreeCliqueBase_separatorMarginal_cachemiss); + // If this is the root, there is no separator + if (parent_.expired() /*(if we're the root)*/) + { + // we are root, return empty + FactorGraphType empty; + cachedSeparatorMarginal_ = empty; + } + else + { + // Obtain P(S) = \int P(Cp) = \int P(Fp|Sp) P(Sp) + // initialize P(Cp) with the parent separator marginal + derived_ptr parent(parent_.lock()); + gttoc(BayesTreeCliqueBase_separatorMarginal_cachemiss); // Flatten recursion in timing outline + gttoc(BayesTreeCliqueBase_separatorMarginal); + FactorGraphType p_Cp(parent->separatorMarginal(function)); // P(Sp) + gttic(BayesTreeCliqueBase_separatorMarginal); + gttic(BayesTreeCliqueBase_separatorMarginal_cachemiss); + // now add the parent conditional + p_Cp += parent->conditional_; // P(Fp|Sp) + + // The variables we want to keepSet are exactly the ones in S + FastVector indicesS(this->conditional()->beginParents(), this->conditional()->endParents()); + cachedSeparatorMarginal_ = *p_Cp.marginalMultifrontalBayesNet(Ordering(indicesS), boost::none, function); + } + } + + // return the shortcut P(S||B) + return *cachedSeparatorMarginal_; // return the cached version + } + + /* ************************************************************************* */ + // marginal2, uses separator marginal of parent recursively + // P(C) = P(F|S) P(S) + /* ************************************************************************* */ + template + typename BayesTreeCliqueBase::FactorGraphType + BayesTreeCliqueBase::marginal2(Eliminate function) const + { + gttic(BayesTreeCliqueBase_marginal2); + // initialize with separator marginal P(S) + FactorGraphType p_C = this->separatorMarginal(function); + // add the conditional P(F|S) + p_C += boost::shared_ptr(this->conditional_); + return p_C; + } + + /* ************************************************************************* */ + template + void BayesTreeCliqueBase::deleteCachedShortcuts() { + + // When a shortcut is requested, all of the shortcuts between it and the + // root are also generated. So, if this clique's cached shortcut is set, + // recursively call over all child cliques. Otherwise, it is unnecessary. + if (cachedSeparatorMarginal_) { + BOOST_FOREACH(derived_ptr& child, children) { + child->deleteCachedShortcuts(); + } + + //Delete CachedShortcut for this clique + cachedSeparatorMarginal_ = boost::none; + } + + } + +} diff --git a/gtsam/inference/BayesTreeCliqueBase.h b/gtsam/inference/BayesTreeCliqueBase.h index 5e06aea1c..beb222178 100644 --- a/gtsam/inference/BayesTreeCliqueBase.h +++ b/gtsam/inference/BayesTreeCliqueBase.h @@ -17,49 +17,48 @@ #pragma once -#include -#include -#include - -#include -#include -#include +#include +#include +#include namespace gtsam { - template class BayesTree; -} -namespace gtsam { + // Forward declarations + template class BayesTree; + template struct EliminationTraits; /** - * This is the base class for BayesTree cliques. The default and standard - * derived type is BayesTreeClique, but some algorithms, like iSAM2, use a - * different clique type in order to store extra data along with the clique. + * This is the base class for BayesTree cliques. The default and standard derived type is + * BayesTreeClique, but some algorithms, like iSAM2, use a different clique type in order to store + * extra data along with the clique. * - * This class is templated on the derived class (i.e. the curiously recursive - * template pattern). The advantage of this over using virtual classes is - * that it avoids the need for casting to get the derived type. This is - * possible because all cliques in a BayesTree are the same type - if they - * were not then we'd need a virtual class. + * This class is templated on the derived class (i.e. the curiously recursive template pattern). + * The advantage of this over using virtual classes is that it avoids the need for casting to get + * the derived type. This is possible because all cliques in a BayesTree are the same type - if + * they were not then we'd need a virtual class. * * @tparam DERIVED The derived clique type. * @tparam CONDITIONAL The conditional type. - * \nosubgrouping - */ - template - struct BayesTreeCliqueBase { - - public: - typedef BayesTreeCliqueBase This; + * \nosubgrouping */ + template + class BayesTreeCliqueBase + { + private: + typedef BayesTreeCliqueBase This; typedef DERIVED DerivedType; - typedef CONDITIONAL ConditionalType; - typedef boost::shared_ptr sharedConditional; + typedef EliminationTraits EliminationTraitsType; typedef boost::shared_ptr shared_ptr; typedef boost::weak_ptr weak_ptr; typedef boost::shared_ptr derived_ptr; typedef boost::weak_ptr derived_weak_ptr; - typedef typename ConditionalType::FactorType FactorType; - typedef typename FactorGraph::Eliminate Eliminate; + + public: + typedef FACTORGRAPH FactorGraphType; + typedef typename EliminationTraitsType::BayesNetType BayesNetType; + typedef typename BayesNetType::ConditionalType ConditionalType; + typedef boost::shared_ptr sharedConditional; + typedef typename FactorGraphType::FactorType FactorType; + typedef typename FactorGraphType::Eliminate Eliminate; protected: @@ -67,56 +66,45 @@ namespace gtsam { /// @{ /** Default constructor */ - BayesTreeCliqueBase() {} + BayesTreeCliqueBase() : problemSize_(1) {} /** Construct from a conditional, leaving parent and child pointers uninitialized */ - BayesTreeCliqueBase(const sharedConditional& conditional); - - /** Construct from an elimination result, which is a pair */ - BayesTreeCliqueBase( - const std::pair >& result); + BayesTreeCliqueBase(const sharedConditional& conditional) : conditional_(conditional), problemSize_(1) {} /// @} /// This stores the Cached separator margnal P(S) - mutable boost::optional > cachedSeparatorMarginal_; + mutable boost::optional cachedSeparatorMarginal_; public: sharedConditional conditional_; derived_weak_ptr parent_; - FastList children_; + FastVector children; + int problemSize_; + + /// Fill the elimination result produced during elimination. Here this just stores the + /// conditional and ignores the remaining factor, but this is overridden in ISAM2Clique + /// to also cache the remaining factor. + void setEliminationResult(const typename FactorGraphType::EliminationResult& eliminationResult); /// @name Testable /// @{ /** check equality */ - bool equals(const This& other, double tol = 1e-9) const { - return (!conditional_ && !other.conditional()) - || conditional_->equals(*other.conditional(), tol); - } + bool equals(const DERIVED& other, double tol = 1e-9) const; /** print this node */ - void print(const std::string& s = "", const IndexFormatter& indexFormatter = - DefaultIndexFormatter) const; - - /** print this node and entire subtree below it */ - void printTree(const std::string& indent = "", - const IndexFormatter& indexFormatter = DefaultIndexFormatter) const; + void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; /// @} /// @name Standard Interface /// @{ /** Access the conditional */ - const sharedConditional& conditional() const { - return conditional_; - } + const sharedConditional& conditional() const { return conditional_; } /** is this the root of a Bayes tree ? */ - inline bool isRoot() const { - return parent_.expired(); - } + inline bool isRoot() const { return parent_.expired(); } /** The size of subtree rooted at this clique, i.e., nr of Cliques */ size_t treeSize() const; @@ -124,75 +112,24 @@ namespace gtsam { /** Collect number of cliques with cached separator marginals */ size_t numCachedSeparatorMarginals() const; - /** The arrow operator accesses the conditional */ - const ConditionalType* operator->() const { - return conditional_.get(); - } - - /** return the const reference of children */ - const std::list& children() const { - return children_; - } - /** return a shared_ptr to the parent clique */ - derived_ptr parent() const { - return parent_.lock(); - } + derived_ptr parent() const { return parent_.lock(); } + + /** Problem size (used for parallel traversal) */ + int problemSize() const { return problemSize_; } /// @} /// @name Advanced Interface /// @{ - /** The arrow operator accesses the conditional */ - ConditionalType* operator->() { - return conditional_.get(); - } - - /** return the reference of children non-const version*/ - FastList& children() { - return children_; - } - - /** Construct shared_ptr from a conditional, leaving parent and child pointers uninitialized */ - static derived_ptr Create(const sharedConditional& conditional) { - return boost::make_shared(conditional); - } - - /** Construct shared_ptr from a FactorGraph::EliminationResult. In this class - * the conditional part is kept and the factor part is ignored, but in derived clique - * types, such as ISAM2Clique, the factor part is kept as a cached factor. - * @param result An elimination result, which is a pair - */ - static derived_ptr Create(const std::pair >& result) { - return boost::make_shared(result); - } - - /** Returns a new clique containing a copy of the conditional but without - * the parent and child clique pointers. - */ - derived_ptr clone() const { - return Create(sharedConditional(new ConditionalType(*conditional_))); - } - - /** Permute the variables in the whole subtree rooted at this clique */ - void permuteWithInverse(const Permutation& inversePermutation); - - /** Permute variables when they only appear in the separators. In this - * case the running intersection property will be used to prevent always - * traversing the whole tree. Returns whether any separator variables in - * this subtree were reordered. - */ - bool reduceSeparatorWithInverse(const internal::Reduction& inverseReduction); - /** return the conditional P(S|Root) on the separator given the root */ - BayesNet shortcut(derived_ptr root, Eliminate function) const; + BayesNetType shortcut(const derived_ptr& root, Eliminate function = EliminationTraitsType::DefaultEliminate) const; /** return the marginal P(S) on the separator */ - FactorGraph separatorMarginal(derived_ptr root, Eliminate function) const; + FactorGraphType separatorMarginal(Eliminate function = EliminationTraitsType::DefaultEliminate) const; /** return the marginal P(C) of the clique, using marginal caching */ - FactorGraph marginal2(derived_ptr root, Eliminate function) const; + FactorGraphType marginal2(Eliminate function = EliminationTraitsType::DefaultEliminate) const; /** * This deletes the cached shortcuts of all cliques (subtree) below this clique. @@ -200,74 +137,37 @@ namespace gtsam { */ void deleteCachedShortcuts(); - const boost::optional >& cachedSeparatorMarginal() const { + const boost::optional& cachedSeparatorMarginal() const { return cachedSeparatorMarginal_; } - friend class BayesTree ; + friend class BayesTree; protected: - /// assert invariants that have to hold in a clique - void assertInvariants() const; - /// Calculate set \f$ S \setminus B \f$ for shortcut calculations - std::vector separator_setminus_B(derived_ptr B) const; + FastVector separator_setminus_B(const derived_ptr& B) const; - /// Calculate set \f$ S_p \cap B \f$ for shortcut calculations - std::vector parent_separator_intersection_B(derived_ptr B) const; - - /** - * Determine variable indices to keep in recursive separator shortcut calculation - * The factor graph p_Cp_B has keys from the parent clique Cp and from B. - * But we only keep the variables not in S union B. - */ - std::vector shortcut_indices(derived_ptr B, - const FactorGraph& p_Cp_B) const; + /** Determine variable indices to keep in recursive separator shortcut calculation The factor + * graph p_Cp_B has keys from the parent clique Cp and from B. But we only keep the variables + * not in S union B. */ + FastVector shortcut_indices(const derived_ptr& B, const FactorGraphType& p_Cp_B) const; /** Non-recursive delete cached shortcuts and marginals - internal only. */ void deleteCachedShortcutsNonRecursive() { cachedSeparatorMarginal_ = boost::none; } private: - /** - * Cliques cannot be copied except by the clone() method, which does not - * copy the parent and child pointers. - */ - BayesTreeCliqueBase(const This& other) { - assert(false); - } - - /** Cliques cannot be copied except by the clone() method, which does not - * copy the parent and child pointers. - */ - This& operator=(const This& other) { - assert(false); - return *this; - } - /** Serialization function */ friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { ar & BOOST_SERIALIZATION_NVP(conditional_); ar & BOOST_SERIALIZATION_NVP(parent_); - ar & BOOST_SERIALIZATION_NVP(children_); + ar & BOOST_SERIALIZATION_NVP(children); } /// @} }; - // \struct Clique - - template - const DERIVED* asDerived( - const BayesTreeCliqueBase* base) { - return static_cast(base); - } - - template - DERIVED* asDerived(BayesTreeCliqueBase* base) { - return static_cast(base); - } } diff --git a/gtsam/inference/CMakeLists.txt b/gtsam/inference/CMakeLists.txt index f88abf463..d5c37d976 100644 --- a/gtsam/inference/CMakeLists.txt +++ b/gtsam/inference/CMakeLists.txt @@ -2,27 +2,11 @@ file(GLOB inference_headers "*.h") install(FILES ${inference_headers} DESTINATION include/gtsam/inference) -# Components to link tests in this subfolder against -set(inference_local_libs - inference - geometry - base - ccolamd -) - -# Files to exclude from compilation of tests and timing scripts -set(inference_excluded_files -# "${CMAKE_CURRENT_SOURCE_DIR}/tests/testTypedDiscreteFactor.cpp" # Example of excluding a test - "" # Add to this list, with full path, to exclude -) - # Build tests -if (GTSAM_BUILD_TESTS) - gtsam_add_subdir_tests(inference "${inference_local_libs}" "${gtsam-default}" "${inference_excluded_files}") -endif(GTSAM_BUILD_TESTS) +add_subdirectory(tests) # Build timing scripts if (GTSAM_BUILD_TIMING) - gtsam_add_subdir_timing(inference "${inference_local_libs}" "${gtsam-default}" "${inference_excluded_files}") + gtsam_add_subdir_timing(inference "gtsam" "gtsam" "${inference_excluded_files}") endif(GTSAM_BUILD_TIMING) diff --git a/gtsam/inference/ClusterTree-inl.h b/gtsam/inference/ClusterTree-inl.h deleted file mode 100644 index 5be1383c4..000000000 --- a/gtsam/inference/ClusterTree-inl.h +++ /dev/null @@ -1,112 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 ClusterTree-inl.h - * @date July 13, 2010 - * @author Kai Ni - * @author Frank Dellaert - * @brief: Collects factorgraph fragments defined on variable clusters, arranged in a tree - */ - -#pragma once - -#include -#include - -#include - -namespace gtsam { - - /* ************************************************************************* * - * Cluster - * ************************************************************************* */ - template - template - ClusterTree::Cluster::Cluster(const FG& fg, Index key, Iterator firstSeparator, Iterator lastSeparator) : - FG(fg), frontal(1, key), separator(firstSeparator, lastSeparator) {} - - /* ************************************************************************* */ - template - template - ClusterTree::Cluster::Cluster( - const FG& fg, FRONTALIT firstFrontal, FRONTALIT lastFrontal, SEPARATORIT firstSeparator, SEPARATORIT lastSeparator) : - FG(fg), frontal(firstFrontal, lastFrontal), separator(firstSeparator, lastSeparator) {} - - /* ************************************************************************* */ - template - template - ClusterTree::Cluster::Cluster( - FRONTALIT firstFrontal, FRONTALIT lastFrontal, SEPARATORIT firstSeparator, SEPARATORIT lastSeparator) : - frontal(firstFrontal, lastFrontal), separator(firstSeparator, lastSeparator) {} - - /* ************************************************************************* */ - template - void ClusterTree::Cluster::addChild(typename ClusterTree::Cluster::shared_ptr child) { - children_.push_back(child); - } - - /* ************************************************************************* */ - template - bool ClusterTree::Cluster::equals(const Cluster& other) const { - if (frontal != other.frontal) return false; - if (separator != other.separator) return false; - if (children_.size() != other.children_.size()) return false; - - typename std::list::const_iterator it1 = children_.begin(); - typename std::list::const_iterator it2 = other.children_.begin(); - for (; it1 != children_.end(); it1++, it2++) - if (!(*it1)->equals(**it2)) return false; - - return true; - } - - /* ************************************************************************* */ - template - void ClusterTree::Cluster::print(const std::string& indent, - const IndexFormatter& formatter) const { - std::cout << indent; - BOOST_FOREACH(const Index key, frontal) - std::cout << formatter(key) << " "; - std::cout << ": "; - BOOST_FOREACH(const Index key, separator) - std::cout << key << " "; - std::cout << std::endl; - } - - /* ************************************************************************* */ - template - void ClusterTree::Cluster::printTree(const std::string& indent, - const IndexFormatter& formatter) const { - print(indent, formatter); - BOOST_FOREACH(const shared_ptr& child, children_) - child->printTree(indent + " ", formatter); - } - - /* ************************************************************************* * - * ClusterTree - * ************************************************************************* */ - template - void ClusterTree::print(const std::string& str, - const IndexFormatter& formatter) const { - std::cout << str << std::endl; - if (root_) root_->printTree("", formatter); - } - - /* ************************************************************************* */ - template - bool ClusterTree::equals(const ClusterTree& other, double tol) const { - if (!root_ && !other.root_) return true; - if (!root_ || !other.root_) return false; - return root_->equals(*other.root_); - } - -} //namespace gtsam diff --git a/gtsam/inference/ClusterTree-inst.h b/gtsam/inference/ClusterTree-inst.h new file mode 100644 index 000000000..13130bf2e --- /dev/null +++ b/gtsam/inference/ClusterTree-inst.h @@ -0,0 +1,186 @@ +/** + * @file ClusterTree-inst.h + * @date Oct 8, 2013 + * @author Kai Ni + * @author Richard Roberts + * @author Frank Dellaert + * @brief Collects factorgraph fragments defined on variable clusters, arranged in a tree + */ + +#include +#include +#include +#include +#include + +#include +#include + +namespace gtsam +{ + namespace + { + /* ************************************************************************* */ + // Elimination traversal data - stores a pointer to the parent data and collects the factors + // resulting from elimination of the children. Also sets up BayesTree cliques with parent and + // child pointers. + template + struct EliminationData { + EliminationData* const parentData; + size_t myIndexInParent; + FastVector childFactors; + boost::shared_ptr bayesTreeNode; + EliminationData(EliminationData* _parentData, size_t nChildren) : + parentData(_parentData), + bayesTreeNode(boost::make_shared()) + { + if(parentData) { + myIndexInParent = parentData->childFactors.size(); + parentData->childFactors.push_back(typename CLUSTERTREE::sharedFactor()); + } else { + myIndexInParent = 0; + } + // Set up BayesTree parent and child pointers + if(parentData) { + if(parentData->parentData) // If our parent is not the dummy node + bayesTreeNode->parent_ = parentData->bayesTreeNode; + parentData->bayesTreeNode->children.push_back(bayesTreeNode); + } + } + }; + + /* ************************************************************************* */ + // Elimination pre-order visitor - just creates the EliminationData structure for the visited + // node. + template + EliminationData eliminationPreOrderVisitor( + const typename CLUSTERTREE::sharedNode& node, EliminationData& parentData) + { + EliminationData myData(&parentData, node->children.size()); + myData.bayesTreeNode->problemSize_ = node->problemSize(); + return myData; + } + + /* ************************************************************************* */ + // Elimination post-order visitor - combine the child factors with our own factors, add the + // resulting conditional to the BayesTree, and add the remaining factor to the parent. + template + struct EliminationPostOrderVisitor + { + const typename CLUSTERTREE::Eliminate& eliminationFunction; + typename CLUSTERTREE::BayesTreeType::Nodes& nodesIndex; + EliminationPostOrderVisitor(const typename CLUSTERTREE::Eliminate& eliminationFunction, + typename CLUSTERTREE::BayesTreeType::Nodes& nodesIndex) : + eliminationFunction(eliminationFunction), nodesIndex(nodesIndex) {} + void operator()(const typename CLUSTERTREE::sharedNode& node, EliminationData& myData) + { + // Typedefs + typedef typename CLUSTERTREE::sharedFactor sharedFactor; + typedef typename CLUSTERTREE::FactorType FactorType; + typedef typename CLUSTERTREE::FactorGraphType FactorGraphType; + typedef typename CLUSTERTREE::ConditionalType ConditionalType; + typedef typename CLUSTERTREE::BayesTreeType::Node BTNode; + + // Gather factors + FactorGraphType gatheredFactors; + gatheredFactors.reserve(node->factors.size() + node->children.size()); + gatheredFactors += node->factors; + gatheredFactors += myData.childFactors; + + // Check for Bayes tree orphan subtrees, and add them to our children + BOOST_FOREACH(const sharedFactor& f, node->factors) + { + if(const BayesTreeOrphanWrapper* asSubtree = dynamic_cast*>(f.get())) + { + myData.bayesTreeNode->children.push_back(asSubtree->clique); + asSubtree->clique->parent_ = myData.bayesTreeNode; + } + } + + // Do dense elimination step + std::pair, boost::shared_ptr > eliminationResult = + eliminationFunction(gatheredFactors, Ordering(node->keys)); + + // Store conditional in BayesTree clique, and in the case of ISAM2Clique also store the remaining factor + myData.bayesTreeNode->setEliminationResult(eliminationResult); + + // Fill nodes index - we do this here instead of calling insertRoot at the end to avoid + // putting orphan subtrees in the index - they'll already be in the index of the ISAM2 + // object they're added to. + BOOST_FOREACH(const Key& j, myData.bayesTreeNode->conditional()->frontals()) + nodesIndex.insert(std::make_pair(j, myData.bayesTreeNode)); + + // Store remaining factor in parent's gathered factors + if(!eliminationResult.second->empty()) + myData.parentData->childFactors[myData.myIndexInParent] = eliminationResult.second; + } + }; + } + + /* ************************************************************************* */ + template + void ClusterTree::Cluster::print( + const std::string& s, const KeyFormatter& keyFormatter) const + { + std::cout << s; + BOOST_FOREACH(Key j, keys) + std::cout << j << " "; + std::cout << "problemSize = " << problemSize_ << std::endl; + } + + /* ************************************************************************* */ + template + void ClusterTree::print( + const std::string& s, const KeyFormatter& keyFormatter) const + { + treeTraversal::PrintForest(*this, s, keyFormatter); + } + + /* ************************************************************************* */ + template + ClusterTree& ClusterTree::operator=(const This& other) + { + // Start by duplicating the tree. + roots_ = treeTraversal::CloneForest(other); + + // Assign the remaining factors - these are pointers to factors in the original factor graph and + // we do not clone them. + remainingFactors_ = other.remainingFactors_; + + return *this; + } + + /* ************************************************************************* */ + template + std::pair, boost::shared_ptr > + ClusterTree::eliminate(const Eliminate& function) const + { + gttic(ClusterTree_eliminate); + // Do elimination (depth-first traversal). The rootsContainer stores a 'dummy' BayesTree node + // that contains all of the roots as its children. rootsContainer also stores the remaining + // uneliminated factors passed up from the roots. + boost::shared_ptr result = boost::make_shared(); + EliminationData rootsContainer(0, roots_.size()); + EliminationPostOrderVisitor visitorPost(function, result->nodes_); + { + TbbOpenMPMixedScope threadLimiter; // Limits OpenMP threads since we're mixing TBB and OpenMP + treeTraversal::DepthFirstForestParallel(*this, rootsContainer, + eliminationPreOrderVisitor, visitorPost, 10); + } + + // Create BayesTree from roots stored in the dummy BayesTree node. + result->roots_.insert(result->roots_.end(), rootsContainer.bayesTreeNode->children.begin(), rootsContainer.bayesTreeNode->children.end()); + + // Add remaining factors that were not involved with eliminated variables + boost::shared_ptr allRemainingFactors = boost::make_shared(); + allRemainingFactors->reserve(remainingFactors_.size() + rootsContainer.childFactors.size()); + allRemainingFactors->push_back(remainingFactors_.begin(), remainingFactors_.end()); + BOOST_FOREACH(const sharedFactor& factor, rootsContainer.childFactors) + if(factor) + allRemainingFactors->push_back(factor); + + // Return result + return std::make_pair(result, allRemainingFactors); + } + +} diff --git a/gtsam/inference/ClusterTree.h b/gtsam/inference/ClusterTree.h index b88929878..5a412a79e 100644 --- a/gtsam/inference/ClusterTree.h +++ b/gtsam/inference/ClusterTree.h @@ -1,35 +1,20 @@ -/* ---------------------------------------------------------------------------- - - * 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 ClusterTree.h - * @date July 13, 2010 + * @date Oct 8, 2013 * @author Kai Ni + * @author Richard Roberts * @author Frank Dellaert - * @brief: Collects factorgraph fragments defined on variable clusters, arranged in a tree + * @brief Collects factorgraph fragments defined on variable clusters, arranged in a tree */ #pragma once -#include -#include -#include +#include +#include +#include -#include -#include -#include - -#include - -namespace gtsam { +namespace gtsam +{ /** * A cluster-tree is associated with a factor graph and is defined as in Koller-Friedman: @@ -37,105 +22,104 @@ namespace gtsam { * each factor \f$ f_i \f$ is associated with a single cluster and \f$ scope(f_i) \sub C_k \f$. * \nosubgrouping */ - template - class ClusterTree { + template + class ClusterTree + { public: - // Access to factor types - typedef typename FG::KeyType KeyType; + typedef GRAPH FactorGraphType; ///< The factor graph type + typedef typename GRAPH::FactorType FactorType; ///< The type of factors + typedef ClusterTree This; ///< This class + typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class + typedef boost::shared_ptr sharedFactor; ///< Shared pointer to a factor + typedef BAYESTREE BayesTreeType; ///< The BayesTree type produced by elimination + typedef typename BayesTreeType::ConditionalType ConditionalType; ///< The type of conditionals + typedef boost::shared_ptr sharedConditional; ///< Shared pointer to a conditional + typedef typename FactorGraphType::Eliminate Eliminate; ///< Typedef for an eliminate subroutine + + struct Cluster { + typedef FastVector Keys; + typedef FastVector Factors; + typedef FastVector > Children; + + Keys keys; ///< Frontal keys of this node + Factors factors; ///< Factors associated with this node + Children children; ///< sub-trees + int problemSize_; + + int problemSize() const { return problemSize_; } + + /** print this node */ + void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + }; + + typedef boost::shared_ptr sharedCluster; ///< Shared pointer to Cluster + typedef Cluster Node; ///< Define Node=Cluster for compatibility with tree traversal functions + typedef sharedCluster sharedNode; ///< Define Node=Cluster for compatibility with tree traversal functions + + /** concept check */ + GTSAM_CONCEPT_TESTABLE_TYPE(FactorType); protected: + FastVector roots_; + FastVector remainingFactors_; - // the class for subgraphs that also include the pointers to the parents and two children - class Cluster : public FG { - public: - typedef typename boost::shared_ptr shared_ptr; - typedef typename boost::weak_ptr weak_ptr; + /// @name Standard Constructors + /// @{ - const std::vector frontal; // the frontal variables - const std::vector separator; // the separator variables + /** Copy constructor - makes a deep copy of the tree structure, but only pointers to factors are + * copied, factors are not cloned. */ + ClusterTree(const This& other) { *this = other; } - protected: + /// @} - weak_ptr parent_; // the parent cluster - std::list children_; // the child clusters - const typename FG::sharedFactor eliminated_; // the eliminated factor to pass on to the parent + public: + /// @name Testable + /// @{ - public: + /** Print the cluster tree */ + void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; - /// Construct empty clique - Cluster() {} + /// @} - /** Create a node with a single frontal variable */ - template - Cluster(const FG& fg, Index key, Iterator firstSeparator, Iterator lastSeparator); + /// @name Standard Interface + /// @{ - /** Create a node with several frontal variables */ - template - Cluster(const FG& fg, FRONTALIT firstFrontal, FRONTALIT lastFrontal, SEPARATORIT firstSeparator, SEPARATORIT lastSeparator); + /** Eliminate the factors to a Bayes tree and remaining factor graph + * @param function The function to use to eliminate, see the namespace functions + * in GaussianFactorGraph.h + * @return The Bayes tree and factor graph resulting from elimination + */ + std::pair, boost::shared_ptr > + eliminate(const Eliminate& function) const; - /** Create a node with several frontal variables */ - template - Cluster(FRONTALIT firstFrontal, FRONTALIT lastFrontal, SEPARATORIT firstSeparator, SEPARATORIT lastSeparator); - - /// print - void print(const std::string& indent, const IndexFormatter& formatter = DefaultIndexFormatter) const; - - /// print the enire tree - void printTree(const std::string& indent, const IndexFormatter& formatter = DefaultIndexFormatter) const; - - /// check equality - bool equals(const Cluster& other) const; - - /// get a reference to the children - const std::list& children() const { return children_; } - - /// add a child - void addChild(shared_ptr child); - - /// get or set the parent - weak_ptr& parent() { return parent_; } - - }; + /// @} /// @name Advanced Interface /// @{ - /// typedef for shared pointers to clusters - typedef typename Cluster::shared_ptr sharedCluster; + /** Return the set of roots (one for a tree, multiple for a forest) */ + const FastVector& roots() const { return roots_; } - /// Root cluster - sharedCluster root_; - - public: + /** Return the remaining factors that are not pulled into elimination */ + const FastVector& remainingFactors() const { return remainingFactors_; } /// @} - /// @name Standard Constructors - /// @{ - /// constructor of empty tree + protected: + /// @name Details + + /// Assignment operator - makes a deep copy of the tree structure, but only pointers to factors + /// are copied, factors are not cloned. + This& operator=(const This& other); + + /// Default constructor to be used in derived classes ClusterTree() {} /// @} - /// @name Standard Interface - /// @{ - /// return the root cluster - sharedCluster root() const { return root_; } + }; - /// @} - /// @name Testable - /// @{ - /// print the object - void print(const std::string& str="", const IndexFormatter& formatter = DefaultIndexFormatter) const; +} - /** check equality */ - bool equals(const ClusterTree& other, double tol = 1e-9) const; - /// @} - - }; // ClusterTree - -} // namespace gtsam - -#include diff --git a/gtsam/inference/Conditional-inst.h b/gtsam/inference/Conditional-inst.h new file mode 100644 index 000000000..a22f290d9 --- /dev/null +++ b/gtsam/inference/Conditional-inst.h @@ -0,0 +1,48 @@ +/* ---------------------------------------------------------------------------- + + * 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 Conditional.h + * @brief Base class for conditional densities + * @author Frank Dellaert + */ + +// \callgraph +#pragma once + +#include +#include + +#include + +namespace gtsam { + + /* ************************************************************************* */ + template + void Conditional::print(const std::string& s, const KeyFormatter& formatter) const { + std::cout << s << " P("; + BOOST_FOREACH(Key key, frontals()) + std::cout << " " << formatter(key); + if (nrParents() > 0) + std::cout << " |"; + BOOST_FOREACH(Key parent, parents()) + std::cout << " " << formatter(parent); + std::cout << ")" << std::endl; + } + + /* ************************************************************************* */ + template + bool Conditional::equals(const This& c, double tol) const + { + return nrFrontals_ == c.nrFrontals_; + } + +} diff --git a/gtsam/inference/Conditional.h b/gtsam/inference/Conditional.h index 0e3e98361..bdfec9cd5 100644 --- a/gtsam/inference/Conditional.h +++ b/gtsam/inference/Conditional.h @@ -18,16 +18,15 @@ // \callgraph #pragma once -#include - #include -#include -#include +#include namespace gtsam { /** + * TODO: Update comments. The following comments are out of date!!! + * * Base class for conditional densities, templated on KEY type. This class * provides storage for the keys involved in a conditional, and iterators and * access to the frontal and separator keys. @@ -37,211 +36,112 @@ namespace gtsam { * IndexConditional and GaussianConditional for examples. * \nosubgrouping */ - template - class Conditional: public gtsam::Factor { - - private: - - /** Create keys by adding key in front */ - template - static std::vector MakeKeys(KEY key, ITERATOR firstParent, - ITERATOR lastParent) { - std::vector keys((lastParent - firstParent) + 1); - std::copy(firstParent, lastParent, keys.begin() + 1); - keys[0] = key; - return keys; - } - + template + class Conditional + { protected: - - /** The first nFrontal variables are frontal and the rest are parents. */ + /** The first nrFrontal variables are frontal and the rest are parents. */ size_t nrFrontals_; - // Calls the base class assertInvariants, which checks for unique keys - void assertInvariants() const { - Factor::assertInvariants(); - } + private: + /// Typedef to this class + typedef Conditional This; public: - - typedef KEY KeyType; - typedef Conditional This; - typedef Factor Base; - - /** - * Typedef to the factor type that produces this conditional and that this - * conditional can be converted to using a factor constructor. Derived - * classes must redefine this. - */ - typedef gtsam::Factor FactorType; - - /** A shared_ptr to this class. Derived classes must redefine this. */ - typedef boost::shared_ptr shared_ptr; - - /** Iterator over keys */ - typedef typename FactorType::iterator iterator; - - /** Const iterator over keys */ - typedef typename FactorType::const_iterator const_iterator; - /** View of the frontal keys (call frontals()) */ - typedef boost::iterator_range Frontals; + typedef boost::iterator_range Frontals; /** View of the separator keys (call parents()) */ - typedef boost::iterator_range Parents; + typedef boost::iterator_range Parents; + protected: /// @name Standard Constructors /// @{ /** Empty Constructor to make serialization possible */ - Conditional() : - nrFrontals_(0) { - assertInvariants(); - } + Conditional() : nrFrontals_(0) {} - /** No parents */ - Conditional(KeyType key) : - FactorType(key), nrFrontals_(1) { - assertInvariants(); - } - - /** Single parent */ - Conditional(KeyType key, KeyType parent) : - FactorType(key, parent), nrFrontals_(1) { - assertInvariants(); - } - - /** Two parents */ - Conditional(KeyType key, KeyType parent1, KeyType parent2) : - FactorType(key, parent1, parent2), nrFrontals_(1) { - assertInvariants(); - } - - /** Three parents */ - Conditional(KeyType key, KeyType parent1, KeyType parent2, KeyType parent3) : - FactorType(key, parent1, parent2, parent3), nrFrontals_(1) { - assertInvariants(); - } - - /// @} - /// @name Advanced Constructors - /// @{ - - /** Constructor from a frontal variable and a vector of parents */ - Conditional(KeyType key, const std::vector& parents) : - FactorType(MakeKeys(key, parents.begin(), parents.end())), nrFrontals_( - 1) { - assertInvariants(); - } - - /** Constructor from keys and nr of frontal variables */ - Conditional(const std::vector& keys, size_t nrFrontals) : - FactorType(keys), nrFrontals_(nrFrontals) { - assertInvariants(); - } - - /** Constructor from keys and nr of frontal variables */ - Conditional(const std::list& keys, size_t nrFrontals) : - FactorType(keys.begin(),keys.end()), nrFrontals_(nrFrontals) { - assertInvariants(); - } + /** Constructor */ + Conditional(size_t nrFrontals) : nrFrontals_(nrFrontals) {} /// @} /// @name Testable /// @{ /** print with optional formatter */ - void print(const std::string& s = "Conditional", - const IndexFormatter& formatter = DefaultIndexFormatter) const; + void print(const std::string& s = "Conditional", const KeyFormatter& formatter = DefaultKeyFormatter) const; /** check equality */ - template - bool equals(const DERIVED& c, double tol = 1e-9) const { - return nrFrontals_ == c.nrFrontals_ && FactorType::equals(c, tol); - } + bool equals(const This& c, double tol = 1e-9) const; /// @} + + public: /// @name Standard Interface /// @{ /** return the number of frontals */ - size_t nrFrontals() const { - return nrFrontals_; - } + size_t nrFrontals() const { return nrFrontals_; } /** return the number of parents */ - size_t nrParents() const { - return FactorType::size() - nrFrontals_; - } + size_t nrParents() const { return asFactor().size() - nrFrontals_; } - /** Special accessor when there is only one frontal variable. */ - KeyType firstFrontalKey() const { - assert(nrFrontals_>0); - return FactorType::front(); - } - KeyType lastFrontalKey() const { - assert(nrFrontals_>0); - return *(endFrontals() - 1); + /** Convenience function to get the first frontal key */ + Key firstFrontalKey() const { + if(nrFrontals_ > 0) + return asFactor().front(); + else + throw std::invalid_argument("Requested Conditional::firstFrontalKey from a conditional with zero frontal keys"); } /** return a view of the frontal keys */ - Frontals frontals() const { - return boost::make_iterator_range(beginFrontals(), endFrontals()); - } + Frontals frontals() const { return boost::make_iterator_range(beginFrontals(), endFrontals()); } /** return a view of the parent keys */ - Parents parents() const { - return boost::make_iterator_range(beginParents(), endParents()); - } + Parents parents() const { return boost::make_iterator_range(beginParents(), endParents()); } - /** Iterators over frontal and parent variables. */ - const_iterator beginFrontals() const { - return FactorType::begin(); - } /// frontals() { - return boost::make_iterator_range(beginFrontals(), endFrontals()); - } + /** Mutable iterator pointing to first frontal key. */ + typename FACTOR::iterator beginFrontals() { return asFactor().begin(); } - ///TODO: comment - boost::iterator_range parents() { - return boost::make_iterator_range(beginParents(), endParents()); - } + /** Mutable iterator pointing past the last frontal key. */ + typename FACTOR::iterator endFrontals() { return asFactor().begin() + nrFrontals_; } + + /** Mutable iterator pointing to the first parent key. */ + typename FACTOR::iterator beginParents() { return asFactor().begin() + nrFrontals_; } + + /** Mutable iterator pointing past the last parent key. */ + typename FACTOR::iterator endParents() { return asFactor().end(); } private: + // Cast to factor type (non-const) (casts down to derived conditional type, then up to factor type) + FACTOR& asFactor() { return static_cast(static_cast(*this)); } + + // Cast to derived type (const) (casts down to derived conditional type, then up to factor type) + const FACTOR& asFactor() const { return static_cast(static_cast(*this)); } + /** Serialization function */ friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(nrFrontals_); } @@ -249,18 +149,4 @@ namespace gtsam { }; - /* ************************************************************************* */ - template - void Conditional::print(const std::string& s, - const IndexFormatter& formatter) const { - std::cout << s << " P("; - BOOST_FOREACH(KeyType key, frontals()) - std::cout << " " << formatter(key); - if (nrParents() > 0) - std::cout << " |"; - BOOST_FOREACH(KeyType parent, parents()) - std::cout << " " << formatter(parent); - std::cout << ")" << std::endl; - } - } // gtsam diff --git a/gtsam/inference/EliminateableFactorGraph-inst.h b/gtsam/inference/EliminateableFactorGraph-inst.h new file mode 100644 index 000000000..b64ebe259 --- /dev/null +++ b/gtsam/inference/EliminateableFactorGraph-inst.h @@ -0,0 +1,312 @@ +/* ---------------------------------------------------------------------------- + + * 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 EliminateableFactorGraph.h + * @brief Variable elimination algorithms for factor graphs + * @author Richard Roberts + * @date Apr 21, 2013 + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + + /* ************************************************************************* */ + template + boost::shared_ptr::BayesNetType> + EliminateableFactorGraph::eliminateSequential( + OptionalOrdering ordering, const Eliminate& function, OptionalVariableIndex variableIndex) const + { + if(ordering && variableIndex) { + gttic(eliminateSequential); + // Do elimination + EliminationTreeType etree(asDerived(), *variableIndex, *ordering); + boost::shared_ptr bayesNet; + boost::shared_ptr factorGraph; + boost::tie(bayesNet,factorGraph) = etree.eliminate(function); + // If any factors are remaining, the ordering was incomplete + if(!factorGraph->empty()) + throw InconsistentEliminationRequested(); + // Return the Bayes net + return bayesNet; + } + else if(!variableIndex) { + // If no VariableIndex provided, compute one and call this function again IMPORTANT: we check + // for no variable index first so that it's always computed if we need to call COLAMD because + // no Ordering is provided. + return eliminateSequential(ordering, function, VariableIndex(asDerived())); + } + else /*if(!ordering)*/ { + // If no Ordering provided, compute one and call this function again. We are guaranteed to + // have a VariableIndex already here because we computed one if needed in the previous 'else' + // block. + return eliminateSequential(Ordering::COLAMD(*variableIndex), function); + } + } + + /* ************************************************************************* */ + template + boost::shared_ptr::BayesTreeType> + EliminateableFactorGraph::eliminateMultifrontal( + OptionalOrdering ordering, const Eliminate& function, OptionalVariableIndex variableIndex) const + { + if(ordering && variableIndex) { + gttic(eliminateMultifrontal); + // Do elimination with given ordering + EliminationTreeType etree(asDerived(), *variableIndex, *ordering); + JunctionTreeType junctionTree(etree); + boost::shared_ptr bayesTree; + boost::shared_ptr factorGraph; + boost::tie(bayesTree,factorGraph) = junctionTree.eliminate(function); + // If any factors are remaining, the ordering was incomplete + if(!factorGraph->empty()) + throw InconsistentEliminationRequested(); + // Return the Bayes tree + return bayesTree; + } + else if(!variableIndex) { + // If no VariableIndex provided, compute one and call this function again IMPORTANT: we check + // for no variable index first so that it's always computed if we need to call COLAMD because + // no Ordering is provided. + return eliminateMultifrontal(ordering, function, VariableIndex(asDerived())); + } + else /*if(!ordering)*/ { + // If no Ordering provided, compute one and call this function again. We are guaranteed to + // have a VariableIndex already here because we computed one if needed in the previous 'else' + // block. + return eliminateMultifrontal(Ordering::COLAMD(*variableIndex), function); + } + } + + /* ************************************************************************* */ + template + std::pair::BayesNetType>, boost::shared_ptr > + EliminateableFactorGraph::eliminatePartialSequential( + const Ordering& ordering, const Eliminate& function, OptionalVariableIndex variableIndex) const + { + if(variableIndex) { + gttic(eliminatePartialSequential); + // Do elimination + EliminationTreeType etree(asDerived(), *variableIndex, ordering); + return etree.eliminate(function); + } else { + // If no variable index is provided, compute one and call this function again + return eliminatePartialSequential(ordering, function, VariableIndex(asDerived())); + } + } + + /* ************************************************************************* */ + template + std::pair::BayesNetType>, boost::shared_ptr > + EliminateableFactorGraph::eliminatePartialSequential( + const std::vector& variables, const Eliminate& function, OptionalVariableIndex variableIndex) const + { + if(variableIndex) { + gttic(eliminatePartialSequential); + // Compute full ordering + 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()); + return eliminatePartialSequential(ordering, function, variableIndex); + } else { + // If no variable index is provided, compute one and call this function again + return eliminatePartialSequential(variables, function, VariableIndex(asDerived())); + } + } + + /* ************************************************************************* */ + template + std::pair::BayesTreeType>, boost::shared_ptr > + EliminateableFactorGraph::eliminatePartialMultifrontal( + const Ordering& ordering, const Eliminate& function, OptionalVariableIndex variableIndex) const + { + if(variableIndex) { + gttic(eliminatePartialMultifrontal); + // Do elimination + EliminationTreeType etree(asDerived(), *variableIndex, ordering); + JunctionTreeType junctionTree(etree); + return junctionTree.eliminate(function); + } else { + // If no variable index is provided, compute one and call this function again + return eliminatePartialMultifrontal(ordering, function, VariableIndex(asDerived())); + } + } + + /* ************************************************************************* */ + template + std::pair::BayesTreeType>, boost::shared_ptr > + EliminateableFactorGraph::eliminatePartialMultifrontal( + const std::vector& variables, const Eliminate& function, OptionalVariableIndex variableIndex) const + { + if(variableIndex) { + gttic(eliminatePartialMultifrontal); + // Compute full ordering + 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()); + return eliminatePartialMultifrontal(ordering, function, variableIndex); + } else { + // If no variable index is provided, compute one and call this function again + return eliminatePartialMultifrontal(variables, function, VariableIndex(asDerived())); + } + } + + /* ************************************************************************* */ + template + boost::shared_ptr::BayesNetType> + EliminateableFactorGraph::marginalMultifrontalBayesNet( + boost::variant&> variables, + OptionalOrdering marginalizedVariableOrdering, + const Eliminate& function, OptionalVariableIndex variableIndex) const + { + if(variableIndex) + { + if(marginalizedVariableOrdering) + { + gttic(marginalMultifrontalBayesNet); + // An ordering was provided for the marginalized variables, so we can first eliminate them + // in the order requested. + boost::shared_ptr bayesTree; + boost::shared_ptr factorGraph; + boost::tie(bayesTree,factorGraph) = + eliminatePartialMultifrontal(*marginalizedVariableOrdering, function, *variableIndex); + + if(const Ordering* varsAsOrdering = boost::get(&variables)) + { + // An ordering was also provided for the unmarginalized variables, so we can also + // eliminate them in the order requested. + return factorGraph->eliminateSequential(*varsAsOrdering, function); + } + else + { + // No ordering was provided for the unmarginalized variables, so order them with COLAMD. + return factorGraph->eliminateSequential(boost::none, function); + } + } + else + { + // No ordering was provided for the marginalized variables, so order them using constrained + // COLAMD. + bool unmarginalizedAreOrdered = (boost::get(&variables) != 0); + const std::vector* variablesOrOrdering = + unmarginalizedAreOrdered ? + boost::get(&variables) : boost::get&>(&variables); + + Ordering totalOrdering = + Ordering::COLAMDConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered); + + // Split up ordering + const size_t nVars = variablesOrOrdering->size(); + Ordering marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - nVars); + Ordering marginalVarsOrdering(totalOrdering.end() - nVars, totalOrdering.end()); + + // Call this function again with the computed orderings + return marginalMultifrontalBayesNet(marginalVarsOrdering, marginalizationOrdering, function, *variableIndex); + } + } else { + // If no variable index is provided, compute one and call this function again + VariableIndex index(asDerived()); + return marginalMultifrontalBayesNet(variables, marginalizedVariableOrdering, function, index); + } + } + + /* ************************************************************************* */ + template + boost::shared_ptr::BayesTreeType> + EliminateableFactorGraph::marginalMultifrontalBayesTree( + boost::variant&> variables, + OptionalOrdering marginalizedVariableOrdering, + const Eliminate& function, OptionalVariableIndex variableIndex) const + { + if(variableIndex) + { + if(marginalizedVariableOrdering) + { + gttic(marginalMultifrontalBayesTree); + // An ordering was provided for the marginalized variables, so we can first eliminate them + // in the order requested. + boost::shared_ptr bayesTree; + boost::shared_ptr factorGraph; + boost::tie(bayesTree,factorGraph) = + eliminatePartialMultifrontal(*marginalizedVariableOrdering, function, *variableIndex); + + if(const Ordering* varsAsOrdering = boost::get(&variables)) + { + // An ordering was also provided for the unmarginalized variables, so we can also + // eliminate them in the order requested. + return factorGraph->eliminateMultifrontal(*varsAsOrdering, function); + } + else + { + // No ordering was provided for the unmarginalized variables, so order them with COLAMD. + return factorGraph->eliminateMultifrontal(boost::none, function); + } + } + else + { + // No ordering was provided for the marginalized variables, so order them using constrained + // COLAMD. + bool unmarginalizedAreOrdered = (boost::get(&variables) != 0); + const std::vector* variablesOrOrdering = + unmarginalizedAreOrdered ? + boost::get(&variables) : boost::get&>(&variables); + + Ordering totalOrdering = + Ordering::COLAMDConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered); + + // Split up ordering + const size_t nVars = variablesOrOrdering->size(); + Ordering marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - nVars); + Ordering marginalVarsOrdering(totalOrdering.end() - nVars, totalOrdering.end()); + + // Call this function again with the computed orderings + return marginalMultifrontalBayesTree(marginalVarsOrdering, marginalizationOrdering, function, *variableIndex); + } + } else { + // If no variable index is provided, compute one and call this function again + return marginalMultifrontalBayesTree(variables, marginalizedVariableOrdering, function, VariableIndex(asDerived())); + } + } + + /* ************************************************************************* */ + template + boost::shared_ptr + EliminateableFactorGraph::marginal( + const std::vector& variables, + const Eliminate& function, OptionalVariableIndex variableIndex) const + { + if(variableIndex) + { + // Compute a total ordering for all variables + Ordering totalOrdering = Ordering::COLAMDConstrainedLast(*variableIndex, variables); + + // Split out the part for the marginalized variables + Ordering marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - variables.size()); + + // Eliminate and return the remaining factor graph + return eliminatePartialMultifrontal(marginalizationOrdering, function, *variableIndex).second; + } + else + { + // If no variable index is provided, compute one and call this function again + return marginal(variables, function, VariableIndex(asDerived())); + } + } + + +} diff --git a/gtsam/inference/EliminateableFactorGraph.h b/gtsam/inference/EliminateableFactorGraph.h new file mode 100644 index 000000000..717f49167 --- /dev/null +++ b/gtsam/inference/EliminateableFactorGraph.h @@ -0,0 +1,236 @@ +/* ---------------------------------------------------------------------------- + + * 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 EliminateableFactorGraph.h + * @brief Variable elimination algorithms for factor graphs + * @author Richard Roberts + * @date Apr 21, 2013 + */ + +#pragma once + +#include +#include +#include +#include + +#include +#include + +namespace gtsam { + + /// Traits class for eliminateable factor graphs, specifies the types that result from + /// elimination, etc. This must be defined for each factor graph that inherits from + /// EliminateableFactorGraph. + template + struct EliminationTraits + { + // Template for deriving: + // typedef MyFactor FactorType; ///< Type of factors in factor graph (e.g. GaussianFactor) + // typedef MyFactorGraphType FactorGraphType; ///< Type of the factor graph (e.g. GaussianFactorGraph) + // typedef MyConditional ConditionalType; ///< Type of conditionals from elimination (e.g. GaussianConditional) + // typedef MyBayesNet BayesNetType; ///< Type of Bayes net from sequential elimination (e.g. GaussianBayesNet) + // typedef MyEliminationTree EliminationTreeType; ///< Type of elimination tree (e.g. GaussianEliminationTree) + // typedef MyBayesTree BayesTreeType; ///< Type of Bayes tree (e.g. GaussianBayesTree) + // typedef MyJunctionTree JunctionTreeType; ///< Type of Junction tree (e.g. GaussianJunctionTree) + // static pair, shared_ptr + // DefaultEliminate( + // const MyFactorGraph& factors, const Ordering& keys); ///< The default dense elimination function + }; + + + /** EliminateableFactorGraph is a base class for factor graphs that contains elimination + * algorithms. Any factor graph holding eliminateable factors can derive from this class to + * expose functions for computing marginals, conditional marginals, doing multifrontal and + * sequential elimination, etc. */ + template + class EliminateableFactorGraph + { + private: + typedef EliminateableFactorGraph This; ///< Typedef to this class. + typedef FACTORGRAPH FactorGraphType; ///< Typedef to factor graph type + // Base factor type stored in this graph (private because derived classes will get this from + // their FactorGraph base class) + typedef typename EliminationTraits::FactorType _FactorType; + + public: + /// Typedef to the specific EliminationTraits for this graph + typedef EliminationTraits EliminationTraitsType; + + /// Conditional type stored in the Bayes net produced by elimination + typedef typename EliminationTraitsType::ConditionalType ConditionalType; + + /// Bayes net type produced by sequential elimination + typedef typename EliminationTraitsType::BayesNetType BayesNetType; + + /// Elimination tree type that can do sequential elimination of this graph + typedef typename EliminationTraitsType::EliminationTreeType EliminationTreeType; + + /// Bayes tree type produced by multifrontal elimination + typedef typename EliminationTraitsType::BayesTreeType BayesTreeType; + + /// Junction tree type that can do multifrontal elimination of this graph + typedef typename EliminationTraitsType::JunctionTreeType JunctionTreeType; + + /// The pair of conditional and remaining factor produced by a single dense elimination step on + /// a subgraph. + typedef std::pair, boost::shared_ptr<_FactorType> > EliminationResult; + + /// The function type that does a single dense elimination step on a subgraph. + typedef boost::function Eliminate; + + /// Typedef for an optional ordering as an argument to elimination functions + typedef boost::optional OptionalOrdering; + + /// Typedef for an optional variable index as an argument to elimination functions + typedef boost::optional OptionalVariableIndex; + + /** Do sequential elimination of all variables to produce a Bayes net. If an ordering is not + * provided, the ordering provided by COLAMD will be used. + * + * Example - Full Cholesky elimination in COLAMD order: + * \code + * boost::shared_ptr result = graph.eliminateSequential(EliminateCholesky); + * \endcode + * + * Example - Full QR elimination in specified order: + * \code + * boost::shared_ptr result = graph.eliminateSequential(EliminateQR, myOrdering); + * \endcode + * + * Example - Reusing an existing VariableIndex to improve performance, and using COLAMD ordering: + * \code + * VariableIndex varIndex(graph); // Build variable index + * Data data = otherFunctionUsingVariableIndex(graph, varIndex); // Other code that uses variable index + * boost::shared_ptr result = graph.eliminateSequential(EliminateQR, boost::none, varIndex); + * \endcode + * */ + boost::shared_ptr eliminateSequential( + OptionalOrdering ordering = boost::none, + const Eliminate& function = EliminationTraitsType::DefaultEliminate, + OptionalVariableIndex variableIndex = 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. + * + * Example - Full Cholesky elimination in COLAMD order: + * \code + * boost::shared_ptr result = graph.eliminateMultifrontal(EliminateCholesky); + * \endcode + * + * Example - Full QR elimination in specified order: + * \code + * boost::shared_ptr result = graph.eliminateMultifrontal(EliminateQR, myOrdering); + * \endcode + * + * Example - Reusing an existing VariableIndex to improve performance, and using COLAMD ordering: + * \code + * VariableIndex varIndex(graph); // Build variable index + * Data data = otherFunctionUsingVariableIndex(graph, varIndex); // Other code that uses variable index + * boost::shared_ptr result = graph.eliminateMultifrontal(EliminateQR, boost::none, varIndex); + * \endcode + * */ + boost::shared_ptr eliminateMultifrontal( + OptionalOrdering ordering = boost::none, + const Eliminate& function = EliminationTraitsType::DefaultEliminate, + OptionalVariableIndex variableIndex = boost::none) const; + + /** Do sequential elimination of some variables, in \c ordering provided, to produce a Bayes net + * and a remaining factor graph. This computes the factorization \f$ p(X) = p(A|B) p(B) \f$, + * where \f$ A = \f$ \c variables, \f$ X \f$ is all the variables in the factor graph, and \f$ + * B = X\backslash A \f$. */ + std::pair, boost::shared_ptr > + eliminatePartialSequential( + const Ordering& ordering, + const Eliminate& function = EliminationTraitsType::DefaultEliminate, + OptionalVariableIndex variableIndex = boost::none) const; + + /** Do sequential elimination of the given \c variables in an ordering computed by COLAMD to + * produce a Bayes net and a remaining factor graph. This computes the factorization \f$ p(X) + * = p(A|B) p(B) \f$, where \f$ A = \f$ \c variables, \f$ X \f$ is all the variables in the + * factor graph, and \f$ B = X\backslash A \f$. */ + std::pair, boost::shared_ptr > + eliminatePartialSequential( + const std::vector& variables, + const Eliminate& function = EliminationTraitsType::DefaultEliminate, + OptionalVariableIndex variableIndex = boost::none) const; + + /** Do multifrontal elimination of some variables, in \c ordering provided, to produce a Bayes + * tree and a remaining factor graph. This computes the factorization \f$ p(X) = p(A|B) p(B) + * \f$, where \f$ A = \f$ \c variables, \f$ X \f$ is all the variables in the factor graph, and + * \f$ B = X\backslash A \f$. */ + std::pair, boost::shared_ptr > + eliminatePartialMultifrontal( + const Ordering& ordering, + const Eliminate& function = EliminationTraitsType::DefaultEliminate, + OptionalVariableIndex variableIndex = boost::none) const; + + /** Do multifrontal elimination of the given \c variables in an ordering computed by COLAMD to + * produce a Bayes net and a remaining factor graph. This computes the factorization \f$ p(X) + * = p(A|B) p(B) \f$, where \f$ A = \f$ \c variables, \f$ X \f$ is all the variables in the + * factor graph, and \f$ B = X\backslash A \f$. */ + std::pair, boost::shared_ptr > + eliminatePartialMultifrontal( + const std::vector& variables, + const Eliminate& function = EliminationTraitsType::DefaultEliminate, + OptionalVariableIndex variableIndex = boost::none) const; + + /** Compute the marginal of the requested variables and return the result as a Bayes net. + * @param variables Determines the variables whose marginal to compute, if provided as an + * Ordering they will be ordered in the returned BayesNet as specified, and if provided + * as a vector they will be ordered using constrained COLAMD. + * @param marginalizedVariableOrdering Optional ordering for the variables being marginalized + * out, i.e. all variables not in \c variables. If this is boost::none, the ordering + * will be computed with COLAMD. + * @param function Optional dense elimination function, if not provided the default will be + * used. + * @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not + * provided one will be computed. */ + boost::shared_ptr marginalMultifrontalBayesNet( + boost::variant&> variables, + OptionalOrdering marginalizedVariableOrdering = boost::none, + const Eliminate& function = EliminationTraitsType::DefaultEliminate, + OptionalVariableIndex variableIndex = boost::none) const; + + /** Compute the marginal of the requested variables and return the result as a Bayes tree. + * @param variables Determines the variables whose marginal to compute, if provided as an + * Ordering they will be ordered in the returned BayesNet as specified, and if provided + * as a vector they will be ordered using constrained COLAMD. + * @param marginalizedVariableOrdering Optional ordering for the variables being marginalized + * out, i.e. all variables not in \c variables. If this is boost::none, the ordering + * will be computed with COLAMD. + * @param function Optional dense elimination function, if not provided the default will be + * used. + * @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not + * provided one will be computed. */ + boost::shared_ptr marginalMultifrontalBayesTree( + boost::variant&> variables, + OptionalOrdering marginalizedVariableOrdering = boost::none, + const Eliminate& function = EliminationTraitsType::DefaultEliminate, + OptionalVariableIndex variableIndex = boost::none) const; + + /** Compute the marginal factor graph of the requested variables. */ + boost::shared_ptr marginal( + const std::vector& variables, + const Eliminate& function = EliminationTraitsType::DefaultEliminate, + OptionalVariableIndex variableIndex = boost::none) const; + + private: + + // Access the derived factor graph class + const FactorGraphType& asDerived() const { return static_cast(*this); } + + // Access the derived factor graph class + FactorGraphType& asDerived() { return static_cast(*this); } + }; + +} diff --git a/gtsam/inference/EliminationTree-inl.h b/gtsam/inference/EliminationTree-inl.h deleted file mode 100644 index d85e2e651..000000000 --- a/gtsam/inference/EliminationTree-inl.h +++ /dev/null @@ -1,234 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 EliminationTree-inl.h - * @author Frank Dellaert - * @date Oct 13, 2010 - */ -#pragma once - -#include -#include -#include -#include -#include -#include - -#include -#include - -#include -#include -#include - -namespace gtsam { - -/* ************************************************************************* */ -template -typename EliminationTree::sharedFactor EliminationTree::eliminate_( - Eliminate function, Conditionals& conditionals) const { - - static const bool debug = false; - - if(debug) std::cout << "ETree: eliminating " << this->key_ << std::endl; - - if(this->key_ < conditionals.size()) { // If it is requested to eliminate the current variable - // Create the list of factors to be eliminated, initially empty, and reserve space - FactorGraph factors; - factors.reserve(this->factors_.size() + this->subTrees_.size()); - - // Add all factors associated with the current node - factors.push_back(this->factors_.begin(), this->factors_.end()); - - // for all subtrees, eliminate into Bayes net and a separator factor, added to [factors] - BOOST_FOREACH(const shared_ptr& child, subTrees_) - factors.push_back(child->eliminate_(function, conditionals)); // TODO: spawn thread - // TODO: wait for completion of all threads - - // Combine all factors (from this node and from subtrees) into a joint factor - typename FactorGraph::EliminationResult - eliminated(function(factors, 1)); - conditionals[this->key_] = eliminated.first; - - if(debug) std::cout << "Eliminated " << this->key_ << " to get:\n"; - if(debug) eliminated.first->print("Conditional: "); - if(debug) eliminated.second->print("Factor: "); - - return eliminated.second; - } else { - // Eliminate each child but discard the result. - BOOST_FOREACH(const shared_ptr& child, subTrees_) { - (void)child->eliminate_(function, conditionals); - } - return sharedFactor(); // Return a NULL factor - } -} - -/* ************************************************************************* */ -template -std::vector EliminationTree::ComputeParents(const VariableIndex& structure) { - - // Number of factors and variables - const size_t m = structure.nFactors(); - const size_t n = structure.size(); - - static const Index none = std::numeric_limits::max(); - - // Allocate result parent vector and vector of last factor columns - std::vector parents(n, none); - std::vector prevCol(m, none); - - // for column j \in 1 to n do - for (Index j = 0; j < n; j++) { - // for row i \in Struct[A*j] do - BOOST_FOREACH(const size_t i, structure[j]) { - if (prevCol[i] != none) { - Index k = prevCol[i]; - // find root r of the current tree that contains k - Index r = k; - while (parents[r] != none) - r = parents[r]; - if (r != j) parents[r] = j; - } - prevCol[i] = j; - } - } - - return parents; -} - -/* ************************************************************************* */ -template -template -typename EliminationTree::shared_ptr EliminationTree::Create( - const FactorGraph& factorGraph, - const VariableIndex& structure) { - - static const bool debug = false; - gttic(ET_Create1); - - gttic(ET_ComputeParents); - // Compute the tree structure - std::vector parents(ComputeParents(structure)); - gttoc(ET_ComputeParents); - - // Number of variables - const size_t n = structure.size(); - - static const Index none = std::numeric_limits::max(); - - // Create tree structure - gttic(assemble_tree); - std::vector trees(n); - for (Index k = 1; k <= n; k++) { - Index j = n - k; // Start at the last variable and loop down to 0 - trees[j].reset(new EliminationTree(j)); // Create a new node on this variable - if (parents[j] != none) // If this node has a parent, add it to the parent's children - trees[parents[j]]->add(trees[j]); - else if(!structure[j].empty() && j != n - 1) // If a node other than the last has no parents, this is a forest - throw DisconnectedGraphException(); - } - gttoc(assemble_tree); - - // Hang factors in right places - gttic(hang_factors); - BOOST_FOREACH(const typename boost::shared_ptr& factor, factorGraph) { - if(factor && factor->size() > 0) { - Index j = *std::min_element(factor->begin(), factor->end()); - if(j < structure.size()) - trees[j]->add(factor); - } - } - gttoc(hang_factors); - - if(debug) - trees.back()->print("ETree: "); - - // Check that this is not null - assert(trees.back().get()); - return trees.back(); -} - -/* ************************************************************************* */ -template -template -typename EliminationTree::shared_ptr -EliminationTree::Create(const FactorGraph& factorGraph) { - - gttic(ET_Create2); - // Build variable index - const VariableIndex variableIndex(factorGraph); - - // Build elimination tree - return Create(factorGraph, variableIndex); -} - -/* ************************************************************************* */ -template -void EliminationTree::print(const std::string& name, - const IndexFormatter& formatter) const { - std::cout << name << " (" << formatter(key_) << ")" << std::endl; - BOOST_FOREACH(const sharedFactor& factor, factors_) { - factor->print(name + " ", formatter); } - BOOST_FOREACH(const shared_ptr& child, subTrees_) { - child->print(name + " ", formatter); } -} - -/* ************************************************************************* */ -template -bool EliminationTree::equals(const EliminationTree& expected, double tol) const { - if(this->key_ == expected.key_ && this->factors_ == expected.factors_ - && this->subTrees_.size() == expected.subTrees_.size()) { - typename SubTrees::const_iterator this_subtree = this->subTrees_.begin(); - typename SubTrees::const_iterator expected_subtree = expected.subTrees_.begin(); - while(this_subtree != this->subTrees_.end()) - if( ! (*(this_subtree++))->equals(**(expected_subtree++), tol)) - return false; - return true; - } else - return false; -} - -/* ************************************************************************* */ -template -typename EliminationTree::BayesNet::shared_ptr - EliminationTree::eliminatePartial(typename EliminationTree::Eliminate function, size_t nrToEliminate) const { - - // call recursive routine - gttic(ET_recursive_eliminate); - if(nrToEliminate > this->key_ + 1) - throw std::invalid_argument("Requested that EliminationTree::eliminatePartial eliminate more variables than exist"); - Conditionals conditionals(nrToEliminate); // reserve a vector of conditional shared pointers - (void)eliminate_(function, conditionals); // modify in place - gttoc(ET_recursive_eliminate); - - // Add conditionals to BayesNet - gttic(assemble_BayesNet); - typename BayesNet::shared_ptr bayesNet(new BayesNet); - BOOST_FOREACH(const typename BayesNet::sharedConditional& conditional, conditionals) { - if(conditional) - bayesNet->push_back(conditional); - } - gttoc(assemble_BayesNet); - - return bayesNet; -} - -/* ************************************************************************* */ -template -typename EliminationTree::BayesNet::shared_ptr -EliminationTree::eliminate(Eliminate function) const { - size_t nrConditionals = this->key_ + 1; // root key has highest index - return eliminatePartial(function, nrConditionals); -} - -} diff --git a/gtsam/inference/EliminationTree-inst.h b/gtsam/inference/EliminationTree-inst.h new file mode 100644 index 000000000..70b0dd393 --- /dev/null +++ b/gtsam/inference/EliminationTree-inst.h @@ -0,0 +1,290 @@ +/* ---------------------------------------------------------------------------- + +* 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 EliminationTree-inl.h +* @author Frank Dellaert +* @author Richard Roberts +* @date Oct 13, 2010 +*/ +#pragma once + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace gtsam { + + /* ************************************************************************* */ + template + typename EliminationTree::sharedFactor + EliminationTree::Node::eliminate( + const boost::shared_ptr& output, + const Eliminate& function, const FastVector& childrenResults) const + { + // This function eliminates one node (Node::eliminate) - see below eliminate for the whole tree. + + assert(childrenResults.size() == children.size()); + + // Gather factors + FactorGraphType gatheredFactors; + gatheredFactors.reserve(factors.size() + children.size()); + gatheredFactors.push_back(factors.begin(), factors.end()); + gatheredFactors.push_back(childrenResults.begin(), childrenResults.end()); + + // Do dense elimination step + FastVector keyAsVector(1); keyAsVector[0] = key; + std::pair, boost::shared_ptr > eliminationResult = + function(gatheredFactors, Ordering(keyAsVector)); + + // Add conditional to BayesNet + output->push_back(eliminationResult.first); + + // Return result + return eliminationResult.second; + } + + /* ************************************************************************* */ + template + void EliminationTree::Node::print( + const std::string& str, const KeyFormatter& keyFormatter) const + { + std::cout << str << "(" << keyFormatter(key) << ")\n"; + BOOST_FOREACH(const sharedFactor& factor, factors) { + if(factor) + factor->print(str); + else + std::cout << str << "null factor\n"; + } + } + + + /* ************************************************************************* */ + template + EliminationTree::EliminationTree(const FactorGraphType& graph, + const VariableIndex& structure, const Ordering& order) + { + gttic(EliminationTree_Contructor); + + // Number of factors and variables - NOTE in the case of partial elimination, n here may + // be fewer variables than are actually present in the graph. + const size_t m = graph.size(); + const size_t n = order.size(); + + static const size_t none = std::numeric_limits::max(); + + // Allocate result parent vector and vector of last factor columns + FastVector nodes(n); + FastVector parents(n, none); + FastVector prevCol(m, none); + FastVector factorUsed(m, false); + + try { + // for column j \in 1 to n do + for (size_t j = 0; j < n; j++) + { + // Retrieve the factors involving this variable and create the current node + const VariableIndex::Factors& factors = structure[order[j]]; + nodes[j] = boost::make_shared(); + nodes[j]->key = order[j]; + + // for row i \in Struct[A*j] do + BOOST_FOREACH(const size_t i, factors) { + // If we already hit a variable in this factor, make the subtree containing the previous + // variable in this factor a child of the current node. This means that the variables + // eliminated earlier in the factor depend on the later variables in the factor. If we + // haven't yet hit a variable in this factor, we add the factor to the current node. + // TODO: Store root shortcuts instead of parents. + if (prevCol[i] != none) { + size_t k = prevCol[i]; + // Find root r of the current tree that contains k. Use raw pointers in computing the + // parents to avoid changing the reference counts while traversing up the tree. + size_t r = k; + while (parents[r] != none) + r = parents[r]; + // If the root of the subtree involving this node is actually the current node, + // TODO: what does this mean? forest? + if (r != j) { + // Now that we found the root, hook up parent and child pointers in the nodes. + parents[r] = j; + nodes[j]->children.push_back(nodes[r]); + } + } else { + // Add the current factor to the current node since we are at the first variable in this + // factor. + nodes[j]->factors.push_back(graph[i]); + factorUsed[i] = true; + } + prevCol[i] = j; + } + } + } catch(std::invalid_argument& e) { + // If this is thrown from structure[order[j]] above, it means that it was requested to + // eliminate a variable not present in the graph, so throw a more informative error message. + (void)e; // Prevent unused variable warning + throw std::invalid_argument("EliminationTree: given ordering contains variables that are not involved in the factor graph"); + } catch(...) { + throw; + } + + // Find roots + assert(parents.empty() || parents.back() == none); // We expect the last-eliminated node to be a root no matter what + for(size_t j = 0; j < n; ++j) + if(parents[j] == none) + roots_.push_back(nodes[j]); + + // Gather remaining factors (exclude null factors) + for(size_t i = 0; i < m; ++i) + if(!factorUsed[i] && graph[i]) + remainingFactors_.push_back(graph[i]); + } + + /* ************************************************************************* */ + template + EliminationTree::EliminationTree( + const FactorGraphType& factorGraph, const Ordering& order) + { + gttic(ET_Create2); + // Build variable index first + const VariableIndex variableIndex(factorGraph); + This temp(factorGraph, variableIndex, order); + this->swap(temp); // Swap in the tree, and temp will be deleted + } + + /* ************************************************************************* */ + template + EliminationTree& + EliminationTree::operator=(const EliminationTree& other) + { + // Start by duplicating the tree. + roots_ = treeTraversal::CloneForest(other); + + // Assign the remaining factors - these are pointers to factors in the original factor graph and + // we do not clone them. + remainingFactors_ = other.remainingFactors_; + + return *this; + } + + /* ************************************************************************* */ + template + std::pair, boost::shared_ptr > + EliminationTree::eliminate(Eliminate function) const + { + gttic(EliminationTree_eliminate); + // Allocate result + boost::shared_ptr result = boost::make_shared(); + + // Run tree elimination algorithm + FastVector remainingFactors = inference::EliminateTree(result, *this, function); + + // Add remaining factors that were not involved with eliminated variables + boost::shared_ptr allRemainingFactors = boost::make_shared(); + allRemainingFactors->push_back(remainingFactors_.begin(), remainingFactors_.end()); + allRemainingFactors->push_back(remainingFactors.begin(), remainingFactors.end()); + + // Return result + return std::make_pair(result, allRemainingFactors); + } + + /* ************************************************************************* */ + template + void EliminationTree::print(const std::string& name, const KeyFormatter& formatter) const + { + treeTraversal::PrintForest(*this, name, formatter); + } + + /* ************************************************************************* */ + template + bool EliminationTree::equals(const This& expected, double tol) const + { + // Depth-first-traversal stacks + std::stack > stack1, stack2; + + // Add roots in sorted order + { + FastMap keys; + BOOST_FOREACH(const sharedNode& root, this->roots_) { keys.insert(std::make_pair(root->key, root)); } + typedef typename FastMap::value_type Key_Node; + BOOST_FOREACH(const Key_Node& key_node, keys) { stack1.push(key_node.second); } + } + { + FastMap keys; + BOOST_FOREACH(const sharedNode& root, expected.roots_) { keys.insert(std::make_pair(root->key, root)); } + typedef typename FastMap::value_type Key_Node; + BOOST_FOREACH(const Key_Node& key_node, keys) { stack2.push(key_node.second); } + } + + // Traverse, adding children in sorted order + while(!stack1.empty() && !stack2.empty()) { + // Pop nodes + sharedNode node1 = stack1.top(); + stack1.pop(); + sharedNode node2 = stack2.top(); + stack2.pop(); + + // Compare nodes + if(node1->key != node2->key) + return false; + if(node1->factors.size() != node2->factors.size()) { + return false; + } else { + for(typename Node::Factors::const_iterator it1 = node1->factors.begin(), it2 = node2->factors.begin(); + it1 != node1->factors.end(); ++it1, ++it2) // Only check it1 == end because we already returned false for different counts + { + if(*it1 && *it2) { + if(!(*it1)->equals(**it2, tol)) + return false; + } else if((*it1 && !*it2) || (*it2 && !*it1)) { + return false; + } + } + } + + // Add children in sorted order + { + FastMap keys; + BOOST_FOREACH(const sharedNode& node, node1->children) { keys.insert(std::make_pair(node->key, node)); } + typedef typename FastMap::value_type Key_Node; + BOOST_FOREACH(const Key_Node& key_node, keys) { stack1.push(key_node.second); } + } + { + FastMap keys; + BOOST_FOREACH(const sharedNode& node, node2->children) { keys.insert(std::make_pair(node->key, node)); } + typedef typename FastMap::value_type Key_Node; + BOOST_FOREACH(const Key_Node& key_node, keys) { stack2.push(key_node.second); } + } + } + + // If either stack is not empty, the number of nodes differed + if(!stack1.empty() || !stack2.empty()) + return false; + + return true; + } + + /* ************************************************************************* */ + template + void EliminationTree::swap(This& other) { + roots_.swap(other.roots_); + remainingFactors_.swap(other.remainingFactors_); + } + + +} diff --git a/gtsam/inference/EliminationTree.h b/gtsam/inference/EliminationTree.h index 9be5f5b6a..fcea1284e 100644 --- a/gtsam/inference/EliminationTree.h +++ b/gtsam/inference/EliminationTree.h @@ -1,187 +1,168 @@ /* ---------------------------------------------------------------------------- - * 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) +* 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 +* See LICENSE for the license information - * -------------------------------------------------------------------------- */ +* -------------------------------------------------------------------------- */ /** - * @file EliminationTree.h - * @author Frank Dellaert - * @date Oct 13, 2010 - */ +* @file EliminationTree.h +* @author Frank Dellaert +* @author Richard Roberts +* @date Oct 13, 2010 +*/ #pragma once #include +#include -#include #include -#include -#include +#include class EliminationTreeTester; // for unit tests, see testEliminationTree -namespace gtsam { template class BayesNet; } namespace gtsam { -/** - * An elimination tree is a data structure used intermediately during - * elimination. In future versions it will be used to save work between - * multiple eliminations. - * - * When a variable is eliminated, a new factor is created by combining that - * variable's neighboring factors. The new combined factor involves the combined - * factors' involved variables. When the lowest-ordered one of those variables - * is eliminated, it consumes that combined factor. In the elimination tree, - * that lowest-ordered variable is the parent of the variable that was eliminated to - * produce the combined factor. This yields a tree in general, and not a chain - * because of the implicit sparse structure of the resulting Bayes net. - * - * This structure is examined even more closely in a JunctionTree, which - * additionally identifies cliques in the chordal Bayes net. - * \nosubgrouping - */ -template -class EliminationTree { - -public: - - typedef EliminationTree This; ///< This class - typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class - typedef typename boost::shared_ptr sharedFactor; ///< Shared pointer to a factor - typedef gtsam::BayesNet BayesNet; ///< The BayesNet corresponding to FACTOR - typedef FACTOR Factor; - typedef typename FACTOR::KeyType KeyType; - - /** Typedef for an eliminate subroutine */ - typedef typename FactorGraph::Eliminate Eliminate; - -private: - - /** concept check */ - GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR) - - typedef FastList Factors; - typedef FastList SubTrees; - typedef std::vector Conditionals; - - Index key_; ///< index associated with root // FIXME: doesn't this require that "Index" is the type of keys in the generic factor? - Factors factors_; ///< factors associated with root - SubTrees subTrees_; ///< sub-trees - -public: - - /// @name Standard Constructors - /// @{ + class VariableIndex; + class Ordering; /** - * Named constructor to build the elimination tree of a factor graph using - * pre-computed column structure. - * @param factorGraph The factor graph for which to build the elimination tree - * @param structure The set of factors involving each variable. If this is not - * precomputed, you can call the Create(const FactorGraph&) - * named constructor instead. - * @return The elimination tree - */ - template - static shared_ptr Create(const FactorGraph& factorGraph, const VariableIndex& structure); + * An elimination tree is a data structure used intermediately during + * elimination. In future versions it will be used to save work between + * multiple eliminations. + * + * When a variable is eliminated, a new factor is created by combining that + * variable's neighboring factors. The new combined factor involves the combined + * factors' involved variables. When the lowest-ordered one of those variables + * is eliminated, it consumes that combined factor. In the elimination tree, + * that lowest-ordered variable is the parent of the variable that was eliminated to + * produce the combined factor. This yields a tree in general, and not a chain + * because of the implicit sparse structure of the resulting Bayes net. + * + * This structure is examined even more closely in a JunctionTree, which + * additionally identifies cliques in the chordal Bayes net. + * \nosubgrouping + */ + template + class EliminationTree + { + protected: + typedef EliminationTree This; ///< This class + typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class - /** Named constructor to build the elimination tree of a factor graph. Note - * that this has to compute the column structure as a VariableIndex, so if you - * already have this precomputed, use the Create(const FactorGraph&, const VariableIndex&) - * named constructor instead. - * @param factorGraph The factor graph for which to build the elimination tree - */ - template - static shared_ptr Create(const FactorGraph& factorGraph); + public: + typedef GRAPH FactorGraphType; ///< The factor graph type + typedef typename GRAPH::FactorType FactorType; ///< The type of factors + typedef typename boost::shared_ptr sharedFactor; ///< Shared pointer to a factor + typedef BAYESNET BayesNetType; ///< The BayesNet corresponding to FACTOR + typedef typename BayesNetType::ConditionalType ConditionalType; ///< The type of conditionals + typedef typename boost::shared_ptr sharedConditional; ///< Shared pointer to a conditional + typedef typename GRAPH::Eliminate Eliminate; - /// @} - /// @name Standard Interface - /// @{ + struct Node { + typedef FastVector Factors; + typedef FastVector > Children; - /** Eliminate the factors to a Bayes Net - * @param function The function to use to eliminate, see the namespace functions - * in GaussianFactorGraph.h - * @return The BayesNet resulting from elimination - */ - typename BayesNet::shared_ptr eliminate(Eliminate function) const; + Key key; ///< key associated with root + Factors factors; ///< factors associated with root + Children children; ///< sub-trees - /** Eliminate the factors to a Bayes Net and return the remaining factor - * @param function The function to use to eliminate, see the namespace functions - * in GaussianFactorGraph.h - * @return The BayesNet resulting from elimination and the remaining factor - */ - typename BayesNet::shared_ptr eliminatePartial(Eliminate function, size_t nrToEliminate) const; + sharedFactor eliminate(const boost::shared_ptr& output, + const Eliminate& function, const FastVector& childrenFactors) const; - /// @} - /// @name Testable - /// @{ + void print(const std::string& str, const KeyFormatter& keyFormatter) const; + }; - /** Print the tree to cout */ - void print(const std::string& name = "EliminationTree: ", - const IndexFormatter& formatter = DefaultIndexFormatter) const; + typedef boost::shared_ptr sharedNode; ///< Shared pointer to Node - /** Test whether the tree is equal to another */ - bool equals(const EliminationTree& other, double tol = 1e-9) const; + protected: + /** concept check */ + GTSAM_CONCEPT_TESTABLE_TYPE(FactorType); - /// @} + FastVector roots_; + FastVector remainingFactors_; -private: - - /** default constructor, private, as you should use Create below */ - EliminationTree(Index key = 0) : key_(key) {} + /// @name Standard Constructors + /// @{ - /** - * Static internal function to build a vector of parent pointers using the - * algorithm of Gilbert et al., 2001, BIT. - */ - static std::vector ComputeParents(const VariableIndex& structure); + /** + * Build the elimination tree of a factor graph using pre-computed column structure. + * @param factorGraph The factor graph for which to build the elimination tree + * @param structure The set of factors involving each variable. If this is not + * precomputed, you can call the Create(const FactorGraph&) + * named constructor instead. + * @return The elimination tree + */ + EliminationTree(const FactorGraphType& factorGraph, + const VariableIndex& structure, const Ordering& order); - /** add a factor, for Create use only */ - void add(const sharedFactor& factor) { factors_.push_back(factor); } + /** Build the elimination tree of a factor graph. Note that this has to compute the column + * structure as a VariableIndex, so if you already have this precomputed, use the other + * constructor instead. + * @param factorGraph The factor graph for which to build the elimination tree + */ + EliminationTree(const FactorGraphType& factorGraph, const Ordering& order); - /** add a subtree, for Create use only */ - void add(const shared_ptr& child) { subTrees_.push_back(child); } + /** Copy constructor - makes a deep copy of the tree structure, but only pointers to factors are + * copied, factors are not cloned. */ + EliminationTree(const This& other) { *this = other; } - /** - * Recursive routine that eliminates the factors arranged in an elimination tree - * @param Conditionals is a vector of shared pointers that will be modified in place - */ - sharedFactor eliminate_(Eliminate function, Conditionals& conditionals) const; + /** Assignment operator - makes a deep copy of the tree structure, but only pointers to factors + * are copied, factors are not cloned. */ + This& operator=(const This& other); - /// Allow access to constructor and add methods for testing purposes - friend class ::EliminationTreeTester; + /// @} + + public: + /// @name Standard Interface + /// @{ -}; + /** Eliminate the factors to a Bayes net and remaining factor graph + * @param function The function to use to eliminate, see the namespace functions + * in GaussianFactorGraph.h + * @return The Bayes net and factor graph resulting from elimination + */ + std::pair, boost::shared_ptr > + eliminate(Eliminate function) const; + /// @} + /// @name Testable + /// @{ -/** - * An exception thrown when attempting to eliminate a disconnected factor - * graph, which is not currently possible in gtsam. If you need to work with - * disconnected graphs, a workaround is to create zero-information factors to - * bridge the disconnects. To do this, create any factor type (e.g. - * BetweenFactor or RangeFactor) with the noise model - * sharedPrecision(dim, 0.0), where \c dim is the appropriate - * dimensionality for that factor. - */ -struct DisconnectedGraphException : public std::exception { - DisconnectedGraphException() {} - virtual ~DisconnectedGraphException() throw() {} + /** Print the tree to cout */ + void print(const std::string& name = "EliminationTree: ", + const KeyFormatter& formatter = DefaultKeyFormatter) const; - /// Returns the string "Attempting to eliminate a disconnected graph - this is not currently possible in gtsam." - virtual const char* what() const throw() { - return - "Attempting to eliminate a disconnected graph - this is not currently possible in\n" - "GTSAM. You may add \"empty\" BetweenFactor's to join disconnected graphs, these\n" - "will affect the symbolic structure and solving complexity of the graph but not\n" - "the solution. To do this, create BetweenFactor's with zero-precision noise\n" - "models, i.e. noiseModel::Isotropic::Precision(n, 0.0);\n"; } -}; + protected: + /** Test whether the tree is equal to another */ + bool equals(const This& other, double tol = 1e-9) const; + + /// @} + + public: + /// @name Advanced Interface + /// @{ + + /** Return the set of roots (one for a tree, multiple for a forest) */ + const FastVector& roots() const { return roots_; } + + /** Return the remaining factors that are not pulled into elimination */ + const FastVector& remainingFactors() const { return remainingFactors_; } + + /** Swap the data of this tree with another one, this operation is very fast. */ + void swap(This& other); + + protected: + /// Protected default constructor + EliminationTree() {} + + private: + /// Allow access to constructor and add methods for testing purposes + friend class ::EliminationTreeTester; + }; } - -#include diff --git a/gtsam/inference/Factor-inl.h b/gtsam/inference/Factor-inl.h deleted file mode 100644 index 7b22e083e..000000000 --- a/gtsam/inference/Factor-inl.h +++ /dev/null @@ -1,108 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 Factor-inl.h - * @author Richard Roberts - * @date Sep 1, 2010 - */ - -#pragma once - -#include -#include -#include -#include -#include - -namespace gtsam { - - /* ************************************************************************* */ - template - Factor::Factor(const Factor& f) : - keys_(f.keys_) { - assertInvariants(); - } - - /* ************************************************************************* */ - template - Factor::Factor(const ConditionalType& c) : - keys_(c.keys()) { -// assert(c.nrFrontals() == 1); - assertInvariants(); - } - - /* ************************************************************************* */ - template - void Factor::assertInvariants() const { -#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS - // Check that keys are all unique - std::multiset nonunique(keys_.begin(), keys_.end()); - std::set unique(keys_.begin(), keys_.end()); - bool correct = (nonunique.size() == unique.size()) - && std::equal(nonunique.begin(), nonunique.end(), unique.begin()); - if (!correct) throw std::logic_error( - "Factor::assertInvariants: Factor contains duplicate keys"); -#endif - } - - /* ************************************************************************* */ - template - void Factor::print(const std::string& s, - const IndexFormatter& formatter) const { - printKeys(s,formatter); - } - - /* ************************************************************************* */ - template - void Factor::printKeys(const std::string& s, const IndexFormatter& formatter) const { - std::cout << s << " "; - BOOST_FOREACH(KEY key, keys_) std::cout << " " << formatter(key); - std::cout << std::endl; - } - - /* ************************************************************************* */ - template - bool Factor::equals(const This& other, double tol) const { - return keys_ == other.keys_; - } - - /* ************************************************************************* */ -#ifdef TRACK_ELIMINATE - template - template - typename COND::shared_ptr Factor::eliminateFirst() { - assert(!keys_.empty()); - assertInvariants(); - KEY eliminated = keys_.front(); - keys_.erase(keys_.begin()); - assertInvariants(); - return typename COND::shared_ptr(new COND(eliminated, keys_)); - } - - /* ************************************************************************* */ - template - template - typename BayesNet::shared_ptr Factor::eliminate(size_t nrFrontals) { - assert(keys_.size() >= nrFrontals); - assertInvariants(); - typename BayesNet::shared_ptr fragment(new BayesNet ()); - const_iterator it = this->begin(); - for (KEY n = 0; n < nrFrontals; ++n, ++it) - fragment->push_back(COND::FromRange(it, const_iterator(this->end()), 1)); - if (nrFrontals > 0) keys_.assign(fragment->back()->beginParents(), - fragment->back()->endParents()); - assertInvariants(); - return fragment; - } -#endif - -} diff --git a/gtsam/inference/Factor.cpp b/gtsam/inference/Factor.cpp new file mode 100644 index 000000000..707193a52 --- /dev/null +++ b/gtsam/inference/Factor.cpp @@ -0,0 +1,47 @@ +/* ---------------------------------------------------------------------------- + + * 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 Factor.cpp + * @brief The base class for all factors + * @author Kai Ni + * @author Frank Dellaert + * @author Richard Roberts + */ + +// \callgraph + +#include +#include + +#include + +namespace gtsam { + + /* ************************************************************************* */ + void Factor::print(const std::string& s, const KeyFormatter& formatter) const + { + return this->printKeys(s, formatter); + } + + /* ************************************************************************* */ + void Factor::printKeys(const std::string& s, const KeyFormatter& formatter) const { + std::cout << s << " "; + BOOST_FOREACH(Key key, keys_) std::cout << " " << formatter(key); + std::cout << std::endl; + } + + /* ************************************************************************* */ + bool Factor::equals(const This& other, double tol) const { + return keys_ == other.keys_; + } + +} \ No newline at end of file diff --git a/gtsam/inference/Factor.h b/gtsam/inference/Factor.h index b8ff34788..d66f6b8ac 100644 --- a/gtsam/inference/Factor.h +++ b/gtsam/inference/Factor.h @@ -21,212 +21,151 @@ #pragma once -#include -#include #include -#include -#include -#include -#include -#include + +#include +#include +#include namespace gtsam { -template class Conditional; - -/** - * This is the base class for all factor types. It is templated on a KEY type, - * which will be the type used to label variables. Key types currently in use - * in gtsam are Index with symbolic (IndexFactor, SymbolicFactorGraph) and - * Gaussian factors (GaussianFactor, JacobianFactor, HessianFactor, GaussianFactorGraph), - * and Key with nonlinear factors (NonlinearFactor, NonlinearFactorGraph). - * though currently only IndexFactor and IndexConditional derive from this - * class, using Index keys. This class does not store any data other than its - * keys. Derived classes store data such as matrices and probability tables. - * - * Note that derived classes *must* redefine the ConditionalType and shared_ptr - * typedefs to refer to the associated conditional and shared_ptr types of the - * derived class. See IndexFactor, JacobianFactor, etc. for examples. - * - * This class is \b not virtual for performance reasons - derived symbolic classes, - * IndexFactor and IndexConditional, need to be created and destroyed quickly - * during symbolic elimination. GaussianFactor and NonlinearFactor are virtual. - * \nosubgrouping - */ -template -class Factor { - -public: - - typedef KEY KeyType; ///< The KEY template parameter - typedef Factor This; ///< This class - /** - * Typedef to the conditional type obtained by eliminating this factor, - * derived classes must redefine this. + * This is the base class for all factor types. It is templated on a KEY type, + * which will be the type used to label variables. Key types currently in use + * in gtsam are Index with symbolic (IndexFactor, SymbolicFactorGraph) and + * Gaussian factors (GaussianFactor, JacobianFactor, HessianFactor, GaussianFactorGraph), + * and Key with nonlinear factors (NonlinearFactor, NonlinearFactorGraph). + * though currently only IndexFactor and IndexConditional derive from this + * class, using Index keys. This class does not store any data other than its + * keys. Derived classes store data such as matrices and probability tables. + * + * Note that derived classes *must* redefine the ConditionalType and shared_ptr + * typedefs to refer to the associated conditional and shared_ptr types of the + * derived class. See IndexFactor, JacobianFactor, etc. for examples. + * + * This class is \b not virtual for performance reasons - derived symbolic classes, + * IndexFactor and IndexConditional, need to be created and destroyed quickly + * during symbolic elimination. GaussianFactor and NonlinearFactor are virtual. + * \nosubgrouping */ - typedef Conditional ConditionalType; + class GTSAM_EXPORT Factor + { - /// A shared_ptr to this class, derived classes must redefine this. - typedef boost::shared_ptr shared_ptr; + private: + // These typedefs are private because they must be overridden in derived classes. + typedef Factor This; ///< This class + typedef boost::shared_ptr shared_ptr; ///< A shared_ptr to this class. - /// Iterator over keys - typedef typename std::vector::iterator iterator; + public: + /// Iterator over keys + typedef FastVector::iterator iterator; - /// Const iterator over keys - typedef typename std::vector::const_iterator const_iterator; + /// Const iterator over keys + typedef FastVector::const_iterator const_iterator; -protected: + protected: - /// The keys involved in this factor - std::vector keys_; + /// The keys involved in this factor + FastVector keys_; -public: + /// @name Standard Constructors + /// @{ - /// @name Standard Constructors - /// @{ + /** Default constructor for I/O */ + Factor() {} - /** Copy constructor */ - Factor(const This& f); + /** Construct factor from container of keys. This constructor is used internally from derived factor + * constructors, either from a container of keys or from a boost::assign::list_of. */ + template + explicit Factor(const CONTAINER& keys) : keys_(keys.begin(), keys.end()) {} - /** Construct from conditional, calls ConditionalType::toFactor() */ - Factor(const ConditionalType& c); + /** Construct factor from iterator keys. This constructor may be used internally from derived + * factor constructors, although our code currently does not use this. */ + template + Factor(ITERATOR first, ITERATOR last) : keys_(first, last) {} - /** Default constructor for I/O */ - Factor() {} + /** Construct factor from container of keys. This is called internally from derived factor static + * factor methods, as a workaround for not being able to call the protected constructors above. */ + template + static Factor FromKeys(const CONTAINER& keys) { + return Factor(keys.begin(), keys.end()); } - /** Construct unary factor */ - Factor(KeyType key) : keys_(1) { - keys_[0] = key; assertInvariants(); } + /** Construct factor from iterator keys. This is called internally from derived factor static + * factor methods, as a workaround for not being able to call the protected constructors above. */ + template + static Factor FromIterators(ITERATOR first, ITERATOR last) { + return Factor(first, last); } - /** Construct binary factor */ - Factor(KeyType key1, KeyType key2) : keys_(2) { - keys_[0] = key1; keys_[1] = key2; assertInvariants(); } + /// @} - /** Construct ternary factor */ - Factor(KeyType key1, KeyType key2, KeyType key3) : keys_(3) { - keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; assertInvariants(); } + public: + /// @name Standard Interface + /// @{ - /** Construct 4-way factor */ - Factor(KeyType key1, KeyType key2, KeyType key3, KeyType key4) : keys_(4) { - keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; keys_[3] = key4; assertInvariants(); } + /// First key + Key front() const { return keys_.front(); } - /** Construct 5-way factor */ - Factor(KeyType key1, KeyType key2, KeyType key3, KeyType key4, KeyType key5) : keys_(5) { - keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; keys_[3] = key4; keys_[4] = key5; assertInvariants(); } + /// Last key + Key back() const { return keys_.back(); } - /** Construct 6-way factor */ - Factor(KeyType key1, KeyType key2, KeyType key3, KeyType key4, KeyType key5, KeyType key6) : keys_(6) { - keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; keys_[3] = key4; keys_[4] = key5; keys_[5] = key6; assertInvariants(); } + /// find + const_iterator find(Key key) const { return std::find(begin(), end(), key); } - /// @} - /// @name Advanced Constructors - /// @{ + /// Access the factor's involved variable keys + const FastVector& keys() const { return keys_; } - /** Construct n-way factor */ - Factor(const std::set& keys) { - BOOST_FOREACH(const KeyType& key, keys) keys_.push_back(key); - assertInvariants(); - } + /** Iterator at beginning of involved variable keys */ + const_iterator begin() const { return keys_.begin(); } - /** Construct n-way factor */ - Factor(const std::vector& keys) : keys_(keys) { - assertInvariants(); - } + /** Iterator at end of involved variable keys */ + const_iterator end() const { return keys_.end(); } - /** Constructor from a collection of keys */ - template Factor(KEYITERATOR beginKey, KEYITERATOR endKey) : - keys_(beginKey, endKey) { assertInvariants(); } + /** + * @return the number of variables involved in this factor + */ + size_t size() const { return keys_.size(); } - /// @} + /// @} -#ifdef TRACK_ELIMINATE - /** - * eliminate the first variable involved in this factor - * @return a conditional on the eliminated variable - */ - template - typename CONDITIONAL::shared_ptr eliminateFirst(); - /** - * eliminate the first nrFrontals frontal variables. - */ - template - typename BayesNet::shared_ptr eliminate(size_t nrFrontals = 1); -#endif + /// @name Testable + /// @{ - /// @name Standard Interface - /// @{ + /// print + void print(const std::string& s = "Factor", const KeyFormatter& formatter = DefaultKeyFormatter) const; - /// First key - KeyType front() const { return keys_.front(); } + /// print only keys + void printKeys(const std::string& s = "Factor", const KeyFormatter& formatter = DefaultKeyFormatter) const; - /// Last key - KeyType back() const { return keys_.back(); } + protected: + /// check equality + bool equals(const This& other, double tol = 1e-9) const; - /// find - const_iterator find(KeyType key) const { return std::find(begin(), end(), key); } + /// @} - /// Access the factor's involved variable keys - const std::vector& keys() const { return keys_; } + public: + /// @name Advanced Interface + /// @{ - /** iterators */ - const_iterator begin() const { return keys_.begin(); } ///TODO: comment - const_iterator end() const { return keys_.end(); } ///TODO: comment + /** @return keys involved in this factor */ + FastVector& keys() { return keys_; } - /** - * @return the number of variables involved in this factor - */ - size_t size() const { return keys_.size(); } + /** Iterator at beginning of involved variable keys */ + iterator begin() { return keys_.begin(); } - /// @} - /// @name Testable - /// @{ + /** Iterator at end of involved variable keys */ + iterator end() { return keys_.end(); } - /// print - void print(const std::string& s = "Factor", - const IndexFormatter& formatter = DefaultIndexFormatter) const; + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(keys_); + } - /// print only keys - void printKeys(const std::string& s = "Factor", - const IndexFormatter& formatter = DefaultIndexFormatter) const; + /// @} - /// check equality - bool equals(const This& other, double tol = 1e-9) const; - - /// @} - /// @name Advanced Interface - /// @{ - - /** - * @return keys involved in this factor - */ - std::vector& keys() { return keys_; } - - /** mutable iterators */ - iterator begin() { return keys_.begin(); } ///TODO: comment - iterator end() { return keys_.end(); } ///TODO: comment - -protected: - friend class JacobianFactor; - friend class HessianFactor; - - /// Internal consistency check that is run frequently when in debug mode. - /// If NDEBUG is defined, this is empty and optimized out. - void assertInvariants() const; - -private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(keys_); - } - - /// @} - -}; + }; } - -#include diff --git a/gtsam/inference/FactorGraph-inl.h b/gtsam/inference/FactorGraph-inl.h deleted file mode 100644 index 60fc2e767..000000000 --- a/gtsam/inference/FactorGraph-inl.h +++ /dev/null @@ -1,288 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 FactorGraph-inl.h - * This is a template definition file, include it where needed (only!) - * so that the appropriate code is generated and link errors avoided. - * @brief Factor Graph Base Class - * @author Carlos Nieto - * @author Frank Dellaert - * @author Alireza Fathi - * @author Michael Kaess - */ - -#pragma once - -#include -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include - -namespace gtsam { - - /* ************************************************************************* */ - template - template - FactorGraph::FactorGraph(const BayesNet& bayesNet) { - factors_.reserve(bayesNet.size()); - BOOST_FOREACH(const typename CONDITIONAL::shared_ptr& cond, bayesNet) { - this->push_back(cond->toFactor()); - } - } - - /* ************************************************************************* */ - template - void FactorGraph::print(const std::string& s, - const IndexFormatter& formatter) const { - std::cout << s << std::endl; - std::cout << "size: " << size() << std::endl; - for (size_t i = 0; i < factors_.size(); i++) { - std::stringstream ss; - ss << "factor " << i << ": "; - if (factors_[i] != NULL) factors_[i]->print(ss.str(), formatter); - } - } - - /* ************************************************************************* */ - template - bool FactorGraph::equals(const This& fg, double tol) const { - /** check whether the two factor graphs have the same number of factors_ */ - if (factors_.size() != fg.size()) return false; - - /** check whether the factors_ are the same */ - for (size_t i = 0; i < factors_.size(); i++) { - // TODO: Doesn't this force order of factor insertion? - sharedFactor f1 = factors_[i], f2 = fg.factors_[i]; - if (f1 == NULL && f2 == NULL) continue; - if (f1 == NULL || f2 == NULL) return false; - if (!f1->equals(*f2, tol)) return false; - } - return true; - } - - /* ************************************************************************* */ - template - size_t FactorGraph::nrFactors() const { - size_t size_ = 0; - BOOST_FOREACH(const sharedFactor& factor, factors_) - if (factor) size_++; - return size_; - } - - /* ************************************************************************* */ - template - std::set FactorGraph::keys() const { - std::set allKeys; - BOOST_FOREACH(const sharedFactor& factor, factors_) - if (factor) { - BOOST_FOREACH(Index j, factor->keys()) - allKeys.insert(j); - } - return allKeys; - } - - /* ************************************************************************* */ - template - std::pair::sharedConditional, FactorGraph > - FactorGraph::eliminateFrontals(size_t nFrontals, const Eliminate& eliminate) const - { - // Build variable index - VariableIndex variableIndex(*this); - - // Find first variable - Index firstIndex = 0; - while(firstIndex < variableIndex.size() && variableIndex[firstIndex].empty()) - ++ firstIndex; - - // Check that number of variables is in bounds - if(firstIndex + nFrontals > variableIndex.size()) - throw std::invalid_argument("Requested to eliminate more frontal variables than exist in the factor graph."); - - // Get set of involved factors - FastSet involvedFactorIs; - for(Index j = firstIndex; j < firstIndex + nFrontals; ++j) { - BOOST_FOREACH(size_t i, variableIndex[j]) { - involvedFactorIs.insert(i); - } - } - - // Separate factors into involved and remaining - FactorGraph involvedFactors; - FactorGraph remainingFactors; - FastSet::const_iterator involvedFactorIsIt = involvedFactorIs.begin(); - for(size_t i = 0; i < this->size(); ++i) { - if(involvedFactorIsIt != involvedFactorIs.end() && *involvedFactorIsIt == i) { - // If the current factor is involved, add it to involved and increment involved iterator - involvedFactors.push_back((*this)[i]); - ++ involvedFactorIsIt; - } else { - // If not involved, add to remaining - remainingFactors.push_back((*this)[i]); - } - } - - // Do dense elimination on the involved factors - typename FactorGraph::EliminationResult eliminationResult = - eliminate(involvedFactors, nFrontals); - - // Add the remaining factor back into the factor graph - remainingFactors.push_back(eliminationResult.second); - - // Return the eliminated factor and remaining factor graph - return std::make_pair(eliminationResult.first, remainingFactors); - } - - /* ************************************************************************* */ - template - std::pair::sharedConditional, FactorGraph > - FactorGraph::eliminate(const std::vector& variables, const Eliminate& eliminateFcn, - boost::optional variableIndex_) const - { - const VariableIndex& variableIndex = - variableIndex_ ? *variableIndex_ : VariableIndex(*this); - - // First find the involved factors - FactorGraph involvedFactors; - Index highestInvolvedVariable = 0; // Largest index of the variables in the involved factors - - // First get the indices of the involved factors, but uniquely in a set - FastSet involvedFactorIndices; - BOOST_FOREACH(Index variable, variables) { - involvedFactorIndices.insert(variableIndex[variable].begin(), variableIndex[variable].end()); } - - // Add the factors themselves to involvedFactors and update largest index - involvedFactors.reserve(involvedFactorIndices.size()); - BOOST_FOREACH(size_t factorIndex, involvedFactorIndices) { - const sharedFactor factor = this->at(factorIndex); - involvedFactors.push_back(factor); // Add involved factor - highestInvolvedVariable = std::max( // Updated largest index - highestInvolvedVariable, - *std::max_element(factor->begin(), factor->end())); - } - - sharedConditional conditional; - sharedFactor remainingFactor; - if(involvedFactors.size() > 0) { - // Now permute the variables to be eliminated to the front of the ordering - Permutation toFront = Permutation::PullToFront(variables, highestInvolvedVariable+1); - Permutation toFrontInverse = *toFront.inverse(); - BOOST_FOREACH(const sharedFactor& factor, involvedFactors) { - factor->permuteWithInverse(toFrontInverse); } - - // Eliminate into conditional and remaining factor - EliminationResult eliminated = eliminateFcn(involvedFactors, variables.size()); - conditional = eliminated.first; - remainingFactor = eliminated.second; - - // Undo the permutation - BOOST_FOREACH(const sharedFactor& factor, involvedFactors) { - factor->permuteWithInverse(toFront); } - conditional->permuteWithInverse(toFront); - remainingFactor->permuteWithInverse(toFront); - } else { - // Eliminate 0 variables - EliminationResult eliminated = eliminateFcn(involvedFactors, 0); - conditional = eliminated.first; - remainingFactor = eliminated.second; - } - - // Build the remaining graph, without the removed factors - FactorGraph remainingGraph; - remainingGraph.reserve(this->size() - involvedFactors.size() + 1); - FastSet::const_iterator involvedFactorIndexIt = involvedFactorIndices.begin(); - for(size_t i = 0; i < this->size(); ++i) { - if(involvedFactorIndexIt != involvedFactorIndices.end() && *involvedFactorIndexIt == i) - ++ involvedFactorIndexIt; - else - remainingGraph.push_back(this->at(i)); - } - - // Add the remaining factor if it is not empty. - if(remainingFactor->size() != 0) - remainingGraph.push_back(remainingFactor); - - return std::make_pair(conditional, remainingGraph); - - } - /* ************************************************************************* */ - template - void FactorGraph::replace(size_t index, sharedFactor factor) { - if (index >= factors_.size()) throw std::invalid_argument(boost::str( - boost::format("Factor graph does not contain a factor with index %d.") - % index)); - // Replace the factor - factors_[index] = factor; - } - - /* ************************************************************************* */ - template - FACTORGRAPH combine(const FACTORGRAPH& fg1, const FACTORGRAPH& fg2) { - // create new linear factor graph equal to the first one - FACTORGRAPH fg = fg1; - - // add the second factors_ in the graph - fg.push_back(fg2); - - return fg; - } - - /* ************************************************************************* */ - template - typename DERIVEDFACTOR::shared_ptr Combine(const FactorGraph& factors, - const FastMap >& variableSlots) { - - typedef const std::pair > KeySlotPair; - // Local functional for getting keys out of key-value pairs - struct Local { static KEY FirstOf(const KeySlotPair& pr) { return pr.first; } }; - - return typename DERIVEDFACTOR::shared_ptr(new DERIVEDFACTOR( - boost::make_transform_iterator(variableSlots.begin(), &Local::FirstOf), - boost::make_transform_iterator(variableSlots.end(), &Local::FirstOf))); - } - - /* ************************************************************************* */ - // Recursive function to add factors in cliques to vector of factors_io - template - void _FactorGraph_BayesTree_adder( - std::vector >& factors_io, - const typename BayesTree::sharedClique& clique) { - - if(clique) { - // Add factor from this clique - factors_io.push_back((*clique)->toFactor()); - - // Traverse children - typedef typename BayesTree::sharedClique sharedClique; - BOOST_FOREACH(const sharedClique& child, clique->children()) - _FactorGraph_BayesTree_adder(factors_io, child); - } - } - - /* ************************************************************************* */ - template - template - FactorGraph::FactorGraph(const BayesTree& bayesTree) { - factors_.reserve(bayesTree.size()); - _FactorGraph_BayesTree_adder(factors_, bayesTree.root()); - } - - /* ************************************************************************* */ -} // namespace gtsam diff --git a/gtsam/inference/FactorGraph-inst.h b/gtsam/inference/FactorGraph-inst.h new file mode 100644 index 000000000..8c19d4ff4 --- /dev/null +++ b/gtsam/inference/FactorGraph-inst.h @@ -0,0 +1,84 @@ +/* ---------------------------------------------------------------------------- + + * 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 FactorGraph-inl.h + * @brief Factor Graph Base Class + * @author Carlos Nieto + * @author Frank Dellaert + * @author Alireza Fathi + * @author Michael Kaess + * @author Richard Roberts + */ + +#pragma once + +#include + +#include +#include + +#include +#include + +namespace gtsam { + + /* ************************************************************************* */ + template + void FactorGraph::print(const std::string& s, const KeyFormatter& formatter) const { + std::cout << s << std::endl; + std::cout << "size: " << size() << std::endl; + for (size_t i = 0; i < factors_.size(); i++) { + std::stringstream ss; + ss << "factor " << i << ": "; + if (factors_[i]) + factors_[i]->print(ss.str(), formatter); + } + } + + /* ************************************************************************* */ + template + bool FactorGraph::equals(const This& fg, double tol) const { + /** check whether the two factor graphs have the same number of factors_ */ + if (factors_.size() != fg.size()) return false; + + /** check whether the factors_ are the same */ + for (size_t i = 0; i < factors_.size(); i++) { + // TODO: Doesn't this force order of factor insertion? + sharedFactor f1 = factors_[i], f2 = fg.factors_[i]; + if (f1 == NULL && f2 == NULL) continue; + if (f1 == NULL || f2 == NULL) return false; + if (!f1->equals(*f2, tol)) return false; + } + return true; + } + + /* ************************************************************************* */ + template + size_t FactorGraph::nrFactors() const { + size_t size_ = 0; + BOOST_FOREACH(const sharedFactor& factor, factors_) + if (factor) size_++; + return size_; + } + + /* ************************************************************************* */ + template + FastSet FactorGraph::keys() const { + FastSet allKeys; + BOOST_FOREACH(const sharedFactor& factor, factors_) + if (factor) + allKeys.insert(factor->begin(), factor->end()); + return allKeys; + } + + /* ************************************************************************* */ +} // namespace gtsam diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index 0d3b10e31..043c4caf6 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -15,25 +15,61 @@ * @author Carlos Nieto * @author Christian Potthast * @author Michael Kaess + * @author Richard Roberts */ // \callgraph #pragma once -#include -#include -#include - #include -#include -#include +#include +#include +#include +#include +#include + +#include +#include +#include namespace gtsam { -// Forward declarations -template class BayesTree; -class VariableIndex; + // Forward declarations + template class BayesTree; + + /** Helper */ + template + class CRefCallPushBack + { + C& obj; + public: + CRefCallPushBack(C& obj) : obj(obj) {} + template + void operator()(const A& a) { obj.push_back(a); } + }; + + /** Helper */ + template + class RefCallPushBack + { + C& obj; + public: + RefCallPushBack(C& obj) : obj(obj) {} + template + void operator()(A& a) { obj.push_back(a); } + }; + + /** Helper */ + template + class CRefCallAddCopy + { + C& obj; + public: + CRefCallAddCopy(C& obj) : obj(obj) {} + template + void operator()(const A& a) { obj.addCopy(a); } + }; /** * A factor graph is a bipartite graph with factor nodes connected to variable nodes. @@ -44,95 +80,166 @@ class VariableIndex; class FactorGraph { public: - typedef FACTOR FactorType; ///< factor type - typedef typename FACTOR::KeyType KeyType; ///< type of Keys we use to index variables with typedef boost::shared_ptr sharedFactor; ///< Shared pointer to a factor - typedef boost::shared_ptr sharedConditional; ///< Shared pointer to a conditional + typedef sharedFactor value_type; + typedef typename FastVector::iterator iterator; + typedef typename FastVector::const_iterator const_iterator; + private: typedef FactorGraph This; ///< Typedef for this class typedef boost::shared_ptr shared_ptr; ///< Shared pointer for this class - typedef typename std::vector::iterator iterator; - typedef typename std::vector::const_iterator const_iterator; - - /** typedef for elimination result */ - typedef std::pair EliminationResult; - - /** typedef for an eliminate subroutine */ - typedef boost::function Eliminate; protected: - /** concept check, makes sure FACTOR defines print and equals */ GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR) /** Collection of factors */ - std::vector factors_; + FastVector factors_; - public: - - /// @name Standard Constructor + /// @name Standard Constructors /// @{ /** Default constructor */ FactorGraph() {} + /** Constructor from iterator over factors (shared_ptr or plain objects) */ + template + FactorGraph(ITERATOR firstFactor, ITERATOR lastFactor) { push_back(firstFactor, lastFactor); } + + /** Construct from container of factors (shared_ptr or plain objects) */ + template + explicit FactorGraph(const CONTAINER& factors) { push_back(factors); } + /// @} /// @name Advanced Constructors /// @{ + + // TODO: are these needed? - /** - * @brief Constructor from a Bayes net - * @param bayesNet the Bayes net to convert, type CONDITIONAL must yield compatible factor - * @return a factor graph with all the conditionals, as factors - */ - template - FactorGraph(const BayesNet& bayesNet); + ///** + // * @brief Constructor from a Bayes net + // * @param bayesNet the Bayes net to convert, type CONDITIONAL must yield compatible factor + // * @return a factor graph with all the conditionals, as factors + // */ + //template + //FactorGraph(const BayesNet& bayesNet); - /** convert from Bayes tree */ - template - FactorGraph(const BayesTree& bayesTree); + ///** convert from Bayes tree */ + //template + //FactorGraph(const BayesTree& bayesTree); - /** convert from a derived type */ - template - FactorGraph(const FactorGraph& factors) { - factors_.assign(factors.begin(), factors.end()); - } + ///** convert from a derived type */ + //template + //FactorGraph(const FactorGraph& factors) { + // factors_.assign(factors.begin(), factors.end()); + //} /// @} + + public: /// @name Adding Factors /// @{ /** * Reserve space for the specified number of factors if you know in - * advance how many there will be (works like std::vector::reserve). + * advance how many there will be (works like FastVector::reserve). */ void reserve(size_t size) { factors_.reserve(size); } - /** Add a factor */ + // TODO: are these needed? + + /** Add a factor directly using a shared_ptr */ template - void push_back(const boost::shared_ptr& factor) { - factors_.push_back(boost::shared_ptr(factor)); - } + typename boost::enable_if >::type + push_back(boost::shared_ptr factor) { + factors_.push_back(boost::shared_ptr(factor)); } - /** push back many factors */ - void push_back(const This& factors) { - factors_.insert(end(), factors.begin(), factors.end()); - } + /** Add a factor directly using a shared_ptr */ + void push_back(const sharedFactor& factor) { + factors_.push_back(factor); } - /** push back many factors with an iterator */ + /** push back many factors with an iterator over shared_ptr (factors are not copied) */ template - void push_back(ITERATOR firstFactor, ITERATOR lastFactor) { - factors_.insert(end(), firstFactor, lastFactor); + typename boost::enable_if >::type + push_back(ITERATOR firstFactor, ITERATOR lastFactor) { + factors_.insert(end(), firstFactor, lastFactor); } + + /** push back many factors as shared_ptr's in a container (factors are not copied) */ + template + typename boost::enable_if >::type + push_back(const CONTAINER& container) { + push_back(container.begin(), container.end()); } - /** - * @brief Add a vector of derived factors - * @param factors to add - */ - template - void push_back(const std::vector >& factors) { - factors_.insert(end(), factors.begin(), factors.end()); + /** push back a BayesTree as a collection of factors. NOTE: This should be hidden in derived + * classes in favor of a type-specialized version that calls this templated function. */ + template + typename boost::enable_if >::type + push_back(const BayesTree& bayesTree) { + bayesTree.addFactorsToGraph(*this); + } + + /** Add a factor by value, will be copy-constructed (use push_back with a shared_ptr to avoid + * the copy). */ + template + typename boost::enable_if >::type + push_back(const DERIVEDFACTOR& factor) { + factors_.push_back(boost::make_shared(factor)); + } + + /** push back many factors with an iterator over plain factors (factors are copied) */ + template + typename boost::enable_if >::type + push_back(ITERATOR firstFactor, ITERATOR lastFactor) { + for (ITERATOR f = firstFactor; f != lastFactor; ++f) + push_back(*f); + } + + /** push back many factors as non-pointer objects in a container (factors are copied) */ + template + typename boost::enable_if >::type + push_back(const CONTAINER& container) { + push_back(container.begin(), container.end()); + } + + /** Add a factor directly using a shared_ptr */ + template + typename boost::enable_if, + boost::assign::list_inserter > >::type + operator+=(boost::shared_ptr factor) { + return boost::assign::make_list_inserter(RefCallPushBack(*this))(factor); + } + + /** Add a factor directly using a shared_ptr */ + boost::assign::list_inserter > + operator+=(const sharedFactor& factor) { + return boost::assign::make_list_inserter(CRefCallPushBack(*this))(factor); + } + + /** Add a factor or container of factors, including STL collections, BayesTrees, etc. */ + template + boost::assign::list_inserter > + operator+=(const FACTOR_OR_CONTAINER& factorOrContainer) { + return boost::assign::make_list_inserter(CRefCallPushBack(*this))(factorOrContainer); + } + + /** Add a factor directly using a shared_ptr */ + template + typename boost::enable_if >::type + add(boost::shared_ptr factor) { + push_back(factor); + } + + /** Add a factor directly using a shared_ptr */ + void add(const sharedFactor& factor) { + push_back(factor); + } + + /** Add a factor or container of factors, including STL collections, BayesTrees, etc. */ + template + void add(const FACTOR_OR_CONTAINER& factorOrContainer) { + push_back(factorOrContainer); } /// @} @@ -141,37 +248,47 @@ class VariableIndex; /** print out graph */ void print(const std::string& s = "FactorGraph", - const IndexFormatter& formatter = DefaultIndexFormatter) const; + const KeyFormatter& formatter = DefaultKeyFormatter) const; + protected: /** Check equality */ bool equals(const This& fg, double tol = 1e-9) const; - /// @} + + public: /// @name Standard Interface /// @{ - /** return the number of factors and NULLS */ - size_t size() const { return factors_.size();} + /** return the number of factors (including any null factors set by remove() ). */ + size_t size() const { return factors_.size(); } - /** Simple check for an empty graph - faster than comparing size() to zero */ + /** Check if the graph is empty (null factors set by remove() will cause this to return false). */ bool empty() const { return factors_.empty(); } - /** const cast to the underlying vector of factors */ - operator const std::vector&() const { return factors_; } + /** Get a specific factor by index (this checks array bounds and may throw an exception, as + * opposed to operator[] which does not). + */ + const sharedFactor at(size_t i) const { return factors_.at(i); } - /** Get a specific factor by index */ - const sharedFactor at(size_t i) const { assert(i > eliminateFrontals(size_t nFrontals, const Eliminate& eliminate) const; - - /** Factor the factor graph into a conditional and a remaining factor graph. - * Given the factor graph \f$ f(X) \f$, and \c variables to factorize out - * \f$ V \f$, this function factorizes into \f$ f(X) = f(V;Y)f(Y) \f$, where - * \f$ Y := X\V \f$ are the remaining variables. If \f$ f(X) = p(X) \f$ is - * a probability density or likelihood, the factorization produces a - * conditional probability density and a marginal \f$ p(X) = p(V|Y)p(Y) \f$. - * - * For efficiency, this function treats the variables to eliminate - * \c variables as fully-connected, so produces a dense (fully-connected) - * conditional on all of the variables in \c variables, instead of a sparse - * BayesNet. If the variables are not fully-connected, it is more efficient - * to sequentially factorize multiple times. - */ - std::pair > eliminate( - const std::vector& variables, const Eliminate& eliminateFcn, - boost::optional variableIndex = boost::none) const; - - /** Eliminate a single variable, by calling FactorGraph::eliminate. */ - std::pair > eliminateOne( - KeyType variable, const Eliminate& eliminateFcn, - boost::optional variableIndex = boost::none) const { - std::vector variables(1, variable); - return eliminate(variables, eliminateFcn, variableIndex); - } - /// @} /// @name Modifying Factor Graphs (imperative, discouraged) /// @{ @@ -222,24 +307,36 @@ class VariableIndex; /** non-const STL-style end() */ iterator end() { return factors_.end(); } - /** resize the factor graph. TODO: effects? */ + /** Directly resize the number of factors in the graph. If the new size is less than the + * original, factors at the end will be removed. If the new size is larger than the original, + * null factors will be appended. + */ void resize(size_t size) { factors_.resize(size); } /** delete factor without re-arranging indexes by inserting a NULL pointer */ - inline void remove(size_t i) { factors_[i].reset();} + void remove(size_t i) { factors_[i].reset();} /** replace a factor by index */ - void replace(size_t index, sharedFactor factor); + void replace(size_t index, sharedFactor factor) { at(index) = factor; } + + /** Erase factor and rearrange other factors to take up the empty space */ + void erase(iterator item) { factors_.erase(item); } + + /** Erase factors and rearrange other factors to take up the empty space */ + void erase(iterator first, iterator last) { factors_.erase(first, last); } /// @} /// @name Advanced Interface /// @{ - /** return the number valid factors */ + /** return the number of non-null factors */ size_t nrFactors() const; /** Potentially very slow function to return all keys involved */ - std::set keys() const; + FastSet keys() const; + + /** MATLAB interface utility: Checks whether a factor index idx exists in the graph and is a live pointer */ + inline bool exists(size_t idx) const { return idx < size() && at(idx); } private: @@ -254,20 +351,4 @@ class VariableIndex; }; // FactorGraph - /** Create a combined joint factor (new style for EliminationTree). */ - template - typename DERIVEDFACTOR::shared_ptr Combine(const FactorGraph& factors, - const FastMap >& variableSlots); - - /** - * static function that combines two factor graphs - * @param fg1 Linear factor graph - * @param fg2 Linear factor graph - * @return a new combined factor graph - */ - template - FACTORGRAPH combine(const FACTORGRAPH& fg1, const FACTORGRAPH& fg2); - } // namespace gtsam - -#include diff --git a/gtsam/inference/GenericMultifrontalSolver-inl.h b/gtsam/inference/GenericMultifrontalSolver-inl.h deleted file mode 100644 index ff41d0627..000000000 --- a/gtsam/inference/GenericMultifrontalSolver-inl.h +++ /dev/null @@ -1,107 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 GenericMultifrontalSolver-inl.h - * @author Richard Roberts - * @date Oct 21, 2010 - */ - -#pragma once - -#include -#include -#include - -namespace gtsam { - - /* ************************************************************************* */ - template - GenericMultifrontalSolver::GenericMultifrontalSolver( - const FactorGraph& graph) : - structure_(new VariableIndex(graph)), junctionTree_( - new JT(graph, *structure_)) { - } - - /* ************************************************************************* */ - template - GenericMultifrontalSolver::GenericMultifrontalSolver( - const sharedGraph& graph, - const VariableIndex::shared_ptr& variableIndex) : - structure_(variableIndex), junctionTree_(new JT(*graph, *structure_)) { - } - - /* ************************************************************************* */ - template - void GenericMultifrontalSolver::print(const std::string& s) const { - this->structure_->print(s + " structure:\n"); - this->junctionTree_->print(s + " jtree:"); - } - - /* ************************************************************************* */ - template - bool GenericMultifrontalSolver::equals( - const GenericMultifrontalSolver& expected, double tol) const { - if (!this->structure_->equals(*expected.structure_, tol)) return false; - if (!this->junctionTree_->equals(*expected.junctionTree_, tol)) return false; - return true; - } - - /* ************************************************************************* */ - template - void GenericMultifrontalSolver::replaceFactors(const sharedGraph& graph) { - junctionTree_.reset(new JT(*graph, *structure_)); - } - - /* ************************************************************************* */ - template - typename BayesTree::shared_ptr - GenericMultifrontalSolver::eliminate(Eliminate function) const { - - // eliminate junction tree, returns pointer to root - typename BayesTree::sharedClique - root = junctionTree_->eliminate(function); - - // create an empty Bayes tree and insert root clique - typename BayesTree::shared_ptr - bayesTree(new BayesTree); - bayesTree->insert(root); - - // return the Bayes tree - return bayesTree; - } - - /* ************************************************************************* */ - template - typename FactorGraph::shared_ptr GenericMultifrontalSolver::jointFactorGraph( - const std::vector& js, Eliminate function) const { - - // FIXME: joint for arbitrary sets of variables not present - // TODO: develop and implement theory for shortcuts of more than two variables - - if (js.size() != 2) throw std::domain_error( - "*MultifrontalSolver::joint(js) currently can only compute joint marginals\n" - "for exactly two variables. You can call marginal to compute the\n" - "marginal for one variable. *SequentialSolver::joint(js) can compute the\n" - "joint marginal over any number of variables, so use that if necessary.\n"); - - return eliminate(function)->joint(js[0], js[1], function); - } - - /* ************************************************************************* */ - template - typename boost::shared_ptr GenericMultifrontalSolver::marginalFactor( - Index j, Eliminate function) const { - return eliminate(function)->marginalFactor(j, function); - } - -} - diff --git a/gtsam/inference/GenericMultifrontalSolver.h b/gtsam/inference/GenericMultifrontalSolver.h deleted file mode 100644 index c06214122..000000000 --- a/gtsam/inference/GenericMultifrontalSolver.h +++ /dev/null @@ -1,124 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 GenericMultifrontalSolver.h - * @author Richard Roberts - * @date Oct 21, 2010 - */ - -#pragma once - -#include - -#include -#include - -namespace gtsam { - - /** - * A Generic Multifrontal Solver class - * - * A solver is given a factor graph at construction, and implements - * a strategy to solve it (in this case, eliminate into a Bayes tree). - * This generic one will create a Bayes tree when eliminate() is called. - * - * Takes two template arguments: - * FACTOR the factor type, e.g., GaussianFactor, DiscreteFactor - * JUNCTIONTREE annoyingly, you also have to supply a compatible JT type - * i.e., one templated on a factor graph with the same factors - * TODO: figure why this is so and possibly fix it - * \nosubgrouping - */ - template - class GenericMultifrontalSolver { - - protected: - - /// Column structure of the factor graph - VariableIndex::shared_ptr structure_; - - /// Junction tree that performs elimination. - typename JUNCTIONTREE::shared_ptr junctionTree_; - - public: - - typedef typename FactorGraph::shared_ptr sharedGraph; - typedef typename FactorGraph::Eliminate Eliminate; - - /// @name Standard Constructors - /// @{ - - /** - * Construct the solver for a factor graph. This builds the junction - * tree, which does the symbolic elimination, identifies the cliques, - * and distributes all the factors to the right cliques. - */ - GenericMultifrontalSolver(const FactorGraph& factorGraph); - - /** - * Construct the solver with a shared pointer to a factor graph and to a - * VariableIndex. The solver will store these pointers, so this constructor - * is the fastest. - */ - GenericMultifrontalSolver(const sharedGraph& factorGraph, - const VariableIndex::shared_ptr& variableIndex); - - /// @} - /// @name Testable - /// @{ - - /** Print to cout */ - void print(const std::string& name = "GenericMultifrontalSolver: ") const; - - /** Test whether is equal to another */ - bool equals(const GenericMultifrontalSolver& other, double tol = 1e-9) const; - - /// @} - /// @name Standard Interface - /// @{ - - /** - * Replace the factor graph with a new one having the same structure. The - * This function can be used if the numerical part of the factors changes, - * such as during relinearization or adjusting of noise models. - */ - void replaceFactors(const sharedGraph& factorGraph); - - /** - * Eliminate the factor graph sequentially. Uses a column elimination tree - * to recursively eliminate. - */ - typename BayesTree::shared_ptr - eliminate(Eliminate function) const; - - /** - * Compute the marginal joint over a set of variables, by integrating out - * all of the other variables. This function returns the result as a factor - * graph. - */ - typename FactorGraph::shared_ptr jointFactorGraph( - const std::vector& js, Eliminate function) const; - - /** - * Compute the marginal density over a variable, by integrating out - * all of the other variables. This function returns the result as a factor. - */ - typename boost::shared_ptr marginalFactor(Index j, - Eliminate function) const; - - /// @} - - }; - -} // gtsam - -#include diff --git a/gtsam/inference/GenericSequentialSolver-inl.h b/gtsam/inference/GenericSequentialSolver-inl.h deleted file mode 100644 index 9dfe64fdc..000000000 --- a/gtsam/inference/GenericSequentialSolver-inl.h +++ /dev/null @@ -1,203 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 GenericSequentialSolver-inl.h - * @brief Implementation for generic sequential solver - * @author Richard Roberts - * @date Oct 21, 2010 - */ - -#pragma once - -#include -#include -#include -#include -#include - -#include - -namespace gtsam { - - /* ************************************************************************* */ - template - GenericSequentialSolver::GenericSequentialSolver(const FactorGraph& factorGraph) { - gttic(GenericSequentialSolver_constructor1); - assert(factorGraph.size()); - factors_.reset(new FactorGraph(factorGraph)); - structure_.reset(new VariableIndex(factorGraph)); - eliminationTree_ = EliminationTree::Create(*factors_, *structure_); - } - - /* ************************************************************************* */ - template - GenericSequentialSolver::GenericSequentialSolver( - const sharedFactorGraph& factorGraph, - const boost::shared_ptr& variableIndex) - { - gttic(GenericSequentialSolver_constructor2); - factors_ = factorGraph; - structure_ = variableIndex; - eliminationTree_ = EliminationTree::Create(*factors_, *structure_); - } - - /* ************************************************************************* */ - template - void GenericSequentialSolver::print(const std::string& s) const { - this->factors_->print(s + " factors:"); - this->structure_->print(s + " structure:\n"); - this->eliminationTree_->print(s + " etree:"); - } - - /* ************************************************************************* */ - template - bool GenericSequentialSolver::equals( - const GenericSequentialSolver& expected, double tol) const { - if (!this->factors_->equals(*expected.factors_, tol)) - return false; - if (!this->structure_->equals(*expected.structure_, tol)) - return false; - if (!this->eliminationTree_->equals(*expected.eliminationTree_, tol)) - return false; - return true; - } - - /* ************************************************************************* */ - template - void GenericSequentialSolver::replaceFactors( - const sharedFactorGraph& factorGraph) { - // Reset this shared pointer first to deallocate if possible - for big - // problems there may not be enough memory to store two copies. - eliminationTree_.reset(); - factors_ = factorGraph; - eliminationTree_ = EliminationTree::Create(*factors_, *structure_); - } - - /* ************************************************************************* */ - template - typename GenericSequentialSolver::sharedBayesNet // - GenericSequentialSolver::eliminate(Eliminate function) const { - return eliminationTree_->eliminate(function); - } - - /* ************************************************************************* */ - template - typename GenericSequentialSolver::sharedBayesNet // - GenericSequentialSolver::eliminate(const Permutation& permutation, - Eliminate function, boost::optional nrToEliminate) const - { - gttic(GenericSequentialSolver_eliminate); - // Create inverse permutation - Permutation::shared_ptr permutationInverse(permutation.inverse()); - - // Permute the factors - NOTE that this permutes the original factors, not - // copies. Other parts of the code may hold shared_ptr's to these factors so - // we must undo the permutation before returning. - BOOST_FOREACH(const typename boost::shared_ptr& factor, *factors_) - if (factor) - factor->permuteWithInverse(*permutationInverse); - - // Eliminate using elimination tree provided - typename EliminationTree::shared_ptr etree = EliminationTree::Create(*factors_); - sharedBayesNet bayesNet; - if(nrToEliminate) - bayesNet = etree->eliminatePartial(function, *nrToEliminate); - else - bayesNet = etree->eliminate(function); - - // Undo the permutation on the original factors and on the structure. - BOOST_FOREACH(const typename boost::shared_ptr& factor, *factors_) - if (factor) - factor->permuteWithInverse(permutation); - - // Undo the permutation on the conditionals - BOOST_FOREACH(const boost::shared_ptr& c, *bayesNet) - c->permuteWithInverse(permutation); - - return bayesNet; - } - - /* ************************************************************************* */ - template - typename GenericSequentialSolver::sharedBayesNet // - GenericSequentialSolver::conditionalBayesNet( - const std::vector& js, size_t nrFrontals, - Eliminate function) const - { - gttic(GenericSequentialSolver_conditionalBayesNet); - // Compute a COLAMD permutation with the marginal variables constrained to the end. - // TODO in case of nrFrontals, the order of js has to be respected here ! - Permutation::shared_ptr permutation( - inference::PermutationCOLAMD(*structure_, js, true)); - - // Eliminate only variables J \cup F from P(J,F,S) to get P(F|S) - size_t nrVariables = structure_->size(); - size_t nrMarginalized = nrVariables - js.size(); - size_t nrToEliminate = nrMarginalized + nrFrontals; - sharedBayesNet bayesNet = eliminate(*permutation, function, nrToEliminate); - // Get rid of conditionals on variables that we want to marginalize out - for (size_t i = 0; i < nrMarginalized; i++) - bayesNet->pop_front(); - - return bayesNet; - } - - /* ************************************************************************* */ - template - typename GenericSequentialSolver::sharedBayesNet // - GenericSequentialSolver::jointBayesNet(const std::vector& js, - Eliminate function) const - { - gttic(GenericSequentialSolver_jointBayesNet); - // Compute a COLAMD permutation with the marginal variables constrained to the end. - Permutation::shared_ptr permutation( - inference::PermutationCOLAMD(*structure_, js)); - - // Eliminate all variables - sharedBayesNet bayesNet = eliminate(*permutation, function); - - // Get rid of conditionals on variables that we want to marginalize out - size_t nrMarginalized = bayesNet->size() - js.size(); - for (size_t i = 0; i < nrMarginalized; i++) - bayesNet->pop_front(); - - return bayesNet; - } - - /* ************************************************************************* */ - template - typename FactorGraph::shared_ptr // - GenericSequentialSolver::jointFactorGraph( - const std::vector& js, Eliminate function) const - { - gttic(GenericSequentialSolver_jointFactorGraph); - // Eliminate all variables - typename BayesNet::shared_ptr bayesNet = jointBayesNet(js, function); - - return boost::make_shared >(*bayesNet); - } - - /* ************************************************************************* */ - template - typename boost::shared_ptr // - GenericSequentialSolver::marginalFactor(Index j, Eliminate function) const { - gttic(GenericSequentialSolver_marginalFactor); - // Create a container for the one variable index - std::vector js(1); - js[0] = j; - - // Call joint and return the only factor in the factor graph it returns - // TODO: just call jointBayesNet and grab last conditional, then toFactor.... - return (*this->jointFactorGraph(js, function))[0]; - } - -} // namespace gtsam diff --git a/gtsam/inference/GenericSequentialSolver.h b/gtsam/inference/GenericSequentialSolver.h deleted file mode 100644 index 250ff0d73..000000000 --- a/gtsam/inference/GenericSequentialSolver.h +++ /dev/null @@ -1,179 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 GenericSequentialSolver.h - * @brief generic sequential elimination - * @author Richard Roberts - * @date Oct 21, 2010 - */ - -#pragma once - -#include -#include - -#include -#include - -#include -#include - -namespace gtsam { - class VariableIndex; - class Permutation; -} -namespace gtsam { - template class EliminationTree; -} -namespace gtsam { - template class FactorGraph; -} -namespace gtsam { - template class BayesNet; -} - -namespace gtsam { - - /** - * This solver implements sequential variable elimination for factor graphs. - * Underlying this is a column elimination tree, see Gilbert 2001 BIT. - * - * The elimination ordering is "baked in" to the variable indices at this - * stage, i.e. elimination proceeds in order from '0'. - * - * This is not the most efficient algorithm we provide, most efficient is the - * MultifrontalSolver, which examines and uses the clique structure. - * However, sequential variable elimination is easier to understand so this is a good - * starting point to learn about these algorithms and our implementation. - * Additionally, the first step of MFQR is symbolic sequential elimination. - * \nosubgrouping - */ - template - class GenericSequentialSolver { - - protected: - - typedef boost::shared_ptr > sharedFactorGraph; - typedef typename FACTOR::ConditionalType Conditional; - typedef typename boost::shared_ptr sharedConditional; - typedef typename boost::shared_ptr > sharedBayesNet; - typedef std::pair, boost::shared_ptr > EliminationResult; - typedef boost::function< - EliminationResult(const FactorGraph&, size_t)> Eliminate; - - /** Store the original factors for computing marginals - * TODO Frank says: really? Marginals should be computed from result. - */ - sharedFactorGraph factors_; - - /** Store column structure of the factor graph. Why? */ - boost::shared_ptr structure_; - - /** Elimination tree that performs elimination */ - boost::shared_ptr > eliminationTree_; - - /** concept checks */ - GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR) - // GTSAM_CONCEPT_TESTABLE_TYPE(EliminationTree) - - /** - * Eliminate in a different order, given a permutation - */ - sharedBayesNet eliminate(const Permutation& permutation, Eliminate function, - boost::optional nrToEliminate = boost::none ///< If given a number of variables to eliminate, will only eliminate that many - ) const; - - public: - - /// @name Standard Constructors - /// @{ - - /** - * Construct the solver for a factor graph. This builds the elimination - * tree, which already does some of the work of elimination. - */ - GenericSequentialSolver(const FactorGraph& factorGraph); - - /** - * Construct the solver with a shared pointer to a factor graph and to a - * VariableIndex. The solver will store these pointers, so this constructor - * is the fastest. - */ - GenericSequentialSolver(const sharedFactorGraph& factorGraph, - const boost::shared_ptr& variableIndex); - - /// @} - /// @name Testable - /// @{ - - /** Print to cout */ - void print(const std::string& name = "GenericSequentialSolver: ") const; - - /** Test whether is equal to another */ - bool equals(const GenericSequentialSolver& other, double tol = 1e-9) const; - - /// @} - /// @name Standard Interface - /// @{ - - /** - * Replace the factor graph with a new one having the same structure. The - * This function can be used if the numerical part of the factors changes, - * such as during relinearization or adjusting of noise models. - */ - void replaceFactors(const sharedFactorGraph& factorGraph); - - /** - * Eliminate the factor graph sequentially. Uses a column elimination tree - * to recursively eliminate. - */ - sharedBayesNet eliminate(Eliminate function) const; - - /** - * Compute a conditional density P(F|S) while marginalizing out variables J - * P(F|S) is obtained by P(J,F,S)=P(J|F,S)P(F|S)P(S) and dropping P(S) - * Returns the result as a Bayes net. - */ - sharedBayesNet - conditionalBayesNet(const std::vector& js, size_t nrFrontals, - Eliminate function) const; - - /** - * Compute the marginal joint over a set of variables, by integrating out - * all of the other variables. Returns the result as a Bayes net - */ - sharedBayesNet - jointBayesNet(const std::vector& js, Eliminate function) const; - - /** - * Compute the marginal joint over a set of variables, by integrating out - * all of the other variables. Returns the result as a factor graph. - */ - typename FactorGraph::shared_ptr - jointFactorGraph(const std::vector& js, Eliminate function) const; - - /** - * Compute the marginal Gaussian density over a variable, by integrating out - * all of the other variables. This function returns the result as a factor. - */ - typename boost::shared_ptr - marginalFactor(Index j, Eliminate function) const; - - /// @} - - } - ; -// GenericSequentialSolver - -}// namespace gtsam - -#include diff --git a/gtsam/inference/ISAM-inl.h b/gtsam/inference/ISAM-inl.h deleted file mode 100644 index 4b5333f33..000000000 --- a/gtsam/inference/ISAM-inl.h +++ /dev/null @@ -1,74 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 ISAM-inl.h - * @brief Incremental update functionality (iSAM) for BayesTree. - * @author Michael Kaess - */ - -#pragma once - -#include -#include // for operator += - -#include -#include -#include - -namespace gtsam { - - /* ************************************************************************* */ - template - ISAM::ISAM() : BayesTree() {} - - /* ************************************************************************* */ - template - template void ISAM::update_internal( - const FG& newFactors, Cliques& orphans, typename FG::Eliminate function) { - - // Remove the contaminated part of the Bayes tree - // Throw exception if disconnected - BayesNet bn; - if (!this->empty()) { - this->removeTop(newFactors.keys(), bn, orphans); - if (bn.empty()) - throw std::runtime_error( - "ISAM::update_internal(): no variables in common between existing Bayes tree and incoming factors!"); - } - FG factors(bn); - - // add the factors themselves - factors.push_back(newFactors); - - // eliminate into a Bayes net - GenericMultifrontalSolver > solver(factors); - boost::shared_ptr > bayesTree; - bayesTree = solver.eliminate(function); - this->root_ = bayesTree->root(); - this->nodes_ = bayesTree->nodes(); - - // add orphans to the bottom of the new tree - BOOST_FOREACH(sharedClique orphan, orphans) - this->insert(orphan); - } - - /* ************************************************************************* */ - template - template - void ISAM::update(const FG& newFactors, - typename FG::Eliminate function) { - Cliques orphans; - this->update_internal(newFactors, orphans, function); - } - -} -/// namespace gtsam diff --git a/gtsam/inference/ISAM-inst.h b/gtsam/inference/ISAM-inst.h new file mode 100644 index 000000000..645499b2c --- /dev/null +++ b/gtsam/inference/ISAM-inst.h @@ -0,0 +1,64 @@ +/* ---------------------------------------------------------------------------- + + * 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 ISAM-inl.h + * @brief Incremental update functionality (iSAM) for BayesTree. + * @author Michael Kaess + */ + +#pragma once + +#include +#include + +namespace gtsam { + + /* ************************************************************************* */ + template + void ISAM::update_internal(const FactorGraphType& newFactors, Cliques& orphans, const Eliminate& function) + { + // Remove the contaminated part of the Bayes tree + BayesNetType bn; + if (!this->empty()) { + const FastSet newFactorKeys = newFactors.keys(); + this->removeTop(std::vector(newFactorKeys.begin(), newFactorKeys.end()), bn, orphans); + } + + // Add the removed top and the new factors + FactorGraphType factors; + factors += bn; + factors += newFactors; + + // Add the orphaned subtrees + BOOST_FOREACH(const sharedClique& orphan, orphans) + factors += boost::make_shared >(orphan); + + // eliminate into a Bayes net + const VariableIndex varIndex(factors); + const FastSet newFactorKeys = newFactors.keys(); + const Ordering constrainedOrdering = + 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()); + } + + /* ************************************************************************* */ + template + void ISAM::update(const FactorGraphType& newFactors, const Eliminate& function) + { + Cliques orphans; + this->update_internal(newFactors, orphans, function); + } + +} +/// namespace gtsam diff --git a/gtsam/inference/ISAM.h b/gtsam/inference/ISAM.h index e227ea57c..36edb423c 100644 --- a/gtsam/inference/ISAM.h +++ b/gtsam/inference/ISAM.h @@ -18,7 +18,7 @@ // \callgraph #pragma once -#include +#include namespace gtsam { @@ -27,12 +27,22 @@ namespace gtsam { * Given a set of new factors, it re-eliminates the invalidated part of the tree. * \nosubgrouping */ - template - class ISAM: public BayesTree { + template + class ISAM: public BAYESTREE + { + public: + + typedef BAYESTREE Base; + typedef typename Base::BayesNetType BayesNetType; + typedef typename Base::FactorGraphType FactorGraphType; + typedef typename Base::Clique Clique; + typedef typename Base::sharedClique sharedClique; + typedef typename Base::Cliques Cliques; private: - typedef BayesTree Base; + typedef typename Base::Eliminate Eliminate; + typedef typename Base::EliminationTraitsType EliminationTraitsType; public: @@ -40,12 +50,10 @@ namespace gtsam { /// @{ /** Create an empty Bayes Tree */ - ISAM(); + ISAM() {} /** Copy constructor */ - ISAM(const Base& bayesTree) : - Base(bayesTree) { - } + ISAM(const Base& bayesTree) : Base(bayesTree) {} /// @} /// @name Advanced Interface Interface @@ -56,21 +64,14 @@ namespace gtsam { * @param newFactors is a factor graph that contains the new factors * @param function an elimination routine */ - template - void update(const FG& newFactors, typename FG::Eliminate function); - - typedef typename BayesTree::sharedClique sharedClique; ///::Cliques Cliques; /// - void update_internal(const FG& newFactors, Cliques& orphans, - typename FG::Eliminate function); + void update_internal(const FactorGraphType& newFactors, Cliques& orphans, + const Eliminate& function = EliminationTraitsType::DefaultEliminate); /// @} }; }/// namespace gtsam - -#include diff --git a/gtsam/inference/IndexConditional.cpp b/gtsam/inference/IndexConditional.cpp deleted file mode 100644 index 20e978afe..000000000 --- a/gtsam/inference/IndexConditional.cpp +++ /dev/null @@ -1,65 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 IndexConditional.cpp - * @author Richard Roberts - * @date Oct 17, 2010 - */ - -#include -#include -#ifdef __GNUC__ -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wunused-variable" -#endif -#include -#ifdef __GNUC__ -#pragma GCC diagnostic pop -#endif - -namespace gtsam { - - using namespace std; - using namespace boost::lambda; - - /* ************************************************************************* */ - void IndexConditional::assertInvariants() const { - // Checks for uniqueness of keys - Base::assertInvariants(); - } - - /* ************************************************************************* */ - bool IndexConditional::reduceSeparatorWithInverse(const internal::Reduction& inverseReduction) { -#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS - BOOST_FOREACH(KeyType key, frontals()) { assert(inverseReduction.find(key) == inverseReduction.end()); } -#endif - bool parentChanged = false; - BOOST_FOREACH(KeyType& parent, parents()) { - internal::Reduction::const_iterator it = inverseReduction.find(parent); - if(it != inverseReduction.end()) { - parentChanged = true; - parent = it->second; - } - } - assertInvariants(); - return parentChanged; - } - - /* ************************************************************************* */ - void IndexConditional::permuteWithInverse(const Permutation& inversePermutation) { - BOOST_FOREACH(Index& key, keys()) - key = inversePermutation[key]; - assertInvariants(); - } - /* ************************************************************************* */ - -} diff --git a/gtsam/inference/IndexConditional.h b/gtsam/inference/IndexConditional.h deleted file mode 100644 index a4068f6c4..000000000 --- a/gtsam/inference/IndexConditional.h +++ /dev/null @@ -1,135 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 IndexConditional.h - * @author Richard Roberts - * @date Oct 17, 2010 - */ - -#pragma once - -#include -#include -#include -#include - -namespace gtsam { - - /** - * IndexConditional serves two purposes. It is the base class for - * GaussianConditional, and also functions as a symbolic conditional with - * Index keys, produced by symbolic elimination of IndexFactor. - * - * It derives from Conditional with a key type of Index, which is an - * unsigned integer. - * \nosubgrouping - */ - class IndexConditional : public Conditional { - - protected: - - // Checks that frontal indices are sorted and lower than parent indices - GTSAM_EXPORT void assertInvariants() const; - - public: - - typedef IndexConditional This; - typedef Conditional Base; - typedef IndexFactor FactorType; - typedef boost::shared_ptr shared_ptr; - - /// @name Standard Constructors - /// @{ - - /** Empty Constructor to make serialization possible */ - IndexConditional() { assertInvariants(); } - - /** No parents */ - IndexConditional(Index j) : Base(j) { assertInvariants(); } - - /** Single parent */ - IndexConditional(Index j, Index parent) : Base(j, parent) { assertInvariants(); } - - /** Two parents */ - IndexConditional(Index j, Index parent1, Index parent2) : Base(j, parent1, parent2) { assertInvariants(); } - - /** Three parents */ - IndexConditional(Index j, Index parent1, Index parent2, Index parent3) : Base(j, parent1, parent2, parent3) { assertInvariants(); } - - /// @} - /// @name Advanced Constructors - /// @{ - - /** Constructor from a frontal variable and a vector of parents */ - IndexConditional(Index j, const std::vector& parents) : Base(j, parents) { - assertInvariants(); - } - - /** Constructor from keys and nr of frontal variables */ - IndexConditional(const std::vector& keys, size_t nrFrontals) : - Base(keys, nrFrontals) { - assertInvariants(); - } - - /** Constructor from keys and nr of frontal variables */ - IndexConditional(const std::list& keys, size_t nrFrontals) : - Base(keys, nrFrontals) { - assertInvariants(); - } - - /// @} - /// @name Standard Interface - /// @{ - - /** Named constructor directly returning a shared pointer */ - template - static shared_ptr FromKeys(const KEYS& keys, size_t nrFrontals) { - shared_ptr conditional(new IndexConditional()); - conditional->keys_.assign(keys.begin(), keys.end()); - conditional->nrFrontals_ = nrFrontals; - return conditional; - } - - /** Convert to a factor */ - IndexFactor::shared_ptr toFactor() const { - return IndexFactor::shared_ptr(new IndexFactor(*this)); - } - - /// @} - /// @name Advanced Interface - /// @{ - - /** Permute the variables when only separator variables need to be permuted. - * Returns true if any reordered variables appeared in the separator and - * false if not. - */ - GTSAM_EXPORT bool reduceSeparatorWithInverse(const internal::Reduction& inverseReduction); - - /** - * Permutes the Conditional, but for efficiency requires the permutation - * to already be inverted. - */ - GTSAM_EXPORT void permuteWithInverse(const Permutation& inversePermutation); - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - } - - /// @} - - }; - -} diff --git a/gtsam/inference/IndexFactor.cpp b/gtsam/inference/IndexFactor.cpp deleted file mode 100644 index 77b84074b..000000000 --- a/gtsam/inference/IndexFactor.cpp +++ /dev/null @@ -1,71 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 IndexFactor.cpp - * @author Richard Roberts - * @date Oct 17, 2010 - */ - -#include -#include -#include - -using namespace std; - -namespace gtsam { - - /* ************************************************************************* */ - void IndexFactor::assertInvariants() const { - Base::assertInvariants(); - } - - /* ************************************************************************* */ - IndexFactor::IndexFactor(const IndexConditional& c) : - Base(c) { - assertInvariants(); - } - - /* ************************************************************************* */ -#ifdef TRACK_ELIMINATE - boost::shared_ptr IndexFactor::eliminateFirst() { - boost::shared_ptr result(Base::eliminateFirst< - IndexConditional>()); - assertInvariants(); - return result; - } - - /* ************************************************************************* */ - boost::shared_ptr > IndexFactor::eliminate( - size_t nrFrontals) { - boost::shared_ptr > result(Base::eliminate< - IndexConditional>(nrFrontals)); - assertInvariants(); - return result; - } -#endif - - /* ************************************************************************* */ - void IndexFactor::permuteWithInverse(const Permutation& inversePermutation) { - BOOST_FOREACH(Index& key, keys()) - key = inversePermutation[key]; - assertInvariants(); - } - - /* ************************************************************************* */ - void IndexFactor::reduceWithInverse(const internal::Reduction& inverseReduction) { - BOOST_FOREACH(Index& key, keys()) - key = inverseReduction[key]; - assertInvariants(); - } - - /* ************************************************************************* */ -} // gtsam diff --git a/gtsam/inference/IndexFactor.h b/gtsam/inference/IndexFactor.h deleted file mode 100644 index d18c70086..000000000 --- a/gtsam/inference/IndexFactor.h +++ /dev/null @@ -1,179 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 IndexFactor.h - * @author Richard Roberts - * @date Oct 17, 2010 - */ - -#pragma once - -#include -#include - -namespace gtsam { - - // Forward declaration of IndexConditional - class IndexConditional; - - /** - * IndexFactor serves two purposes. It is the base class for all linear - * factors (GaussianFactor, JacobianFactor, HessianFactor), and also - * functions as a symbolic factor with Index keys, used to do symbolic - * elimination by JunctionTree. - * - * It derives from Factor with a key type of Index, an unsigned integer. - * \nosubgrouping - */ - class IndexFactor: public Factor { - - protected: - - /// @name Advanced Interface - /// @{ - - /// Internal function for checking class invariants (unique keys for this factor) - GTSAM_EXPORT void assertInvariants() const; - - /// @} - - public: - - typedef IndexFactor This; - typedef Factor Base; - - /** Elimination produces an IndexConditional */ - typedef IndexConditional ConditionalType; - - /** Overriding the shared_ptr typedef */ - typedef boost::shared_ptr shared_ptr; - - /// @name Standard Interface - /// @{ - - /** Copy constructor */ - IndexFactor(const This& f) : - Base(f) { - assertInvariants(); - } - - /** Construct from derived type */ - GTSAM_EXPORT IndexFactor(const IndexConditional& c); - - /** Default constructor for I/O */ - IndexFactor() { - assertInvariants(); - } - - /** Construct unary factor */ - IndexFactor(Index j) : - Base(j) { - assertInvariants(); - } - - /** Construct binary factor */ - IndexFactor(Index j1, Index j2) : - Base(j1, j2) { - assertInvariants(); - } - - /** Construct ternary factor */ - IndexFactor(Index j1, Index j2, Index j3) : - Base(j1, j2, j3) { - assertInvariants(); - } - - /** Construct 4-way factor */ - IndexFactor(Index j1, Index j2, Index j3, Index j4) : - Base(j1, j2, j3, j4) { - assertInvariants(); - } - - /** Construct 5-way factor */ - IndexFactor(Index j1, Index j2, Index j3, Index j4, Index j5) : - Base(j1, j2, j3, j4, j5) { - assertInvariants(); - } - - /** Construct 6-way factor */ - IndexFactor(Index j1, Index j2, Index j3, Index j4, Index j5, Index j6) : - Base(j1, j2, j3, j4, j5, j6) { - assertInvariants(); - } - - /// @} - /// @name Advanced Constructors - /// @{ - - /** Construct n-way factor */ - IndexFactor(const std::set& js) : - Base(js) { - assertInvariants(); - } - - /** Construct n-way factor */ - IndexFactor(const std::vector& js) : - Base(js) { - assertInvariants(); - } - - /** Constructor from a collection of keys */ - template IndexFactor(KeyIterator beginKey, - KeyIterator endKey) : - Base(beginKey, endKey) { - assertInvariants(); - } - - /// @} - -#ifdef TRACK_ELIMINATE - /** - * eliminate the first variable involved in this factor - * @return a conditional on the eliminated variable - */ - GTSAM_EXPORT boost::shared_ptr eliminateFirst(); - - /** eliminate the first nrFrontals frontal variables.*/ - GTSAM_EXPORT boost::shared_ptr > eliminate(size_t nrFrontals = - 1); -#endif - - /// @name Advanced Interface - /// @{ - - /** - * Permutes the factor, but for efficiency requires the permutation - * to already be inverted. - */ - GTSAM_EXPORT void permuteWithInverse(const Permutation& inversePermutation); - - /** - * Apply a reduction, which is a remapping of variable indices. - */ - GTSAM_EXPORT void reduceWithInverse(const internal::Reduction& inverseReduction); - - virtual ~IndexFactor() { - } - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - } - - /// @} - - }; // IndexFactor - -} diff --git a/gtsam/inference/JunctionTree-inl.h b/gtsam/inference/JunctionTree-inl.h deleted file mode 100644 index 1978bde38..000000000 --- a/gtsam/inference/JunctionTree-inl.h +++ /dev/null @@ -1,205 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 JunctionTree-inl.h - * @date Feb 4, 2010 - * @author Kai Ni - * @author Frank Dellaert - * @brief The junction tree, template bodies - */ - -#pragma once - -#include -#include -#include -#include - -#include - -namespace gtsam { - - /* ************************************************************************* */ - template - void JunctionTree::construct(const FG& fg, const VariableIndex& variableIndex) { - gttic(JT_symbolic_ET); - const typename EliminationTree::shared_ptr symETree = - EliminationTree::Create(fg, variableIndex); - assert(symETree.get()); - gttoc(JT_symbolic_ET); - gttic(JT_symbolic_eliminate); - SymbolicBayesNet::shared_ptr sbn = symETree->eliminate(&EliminateSymbolic); - assert(sbn.get()); - gttoc(JT_symbolic_eliminate); - gttic(symbolic_BayesTree); - SymbolicBayesTree sbt(*sbn); - gttoc(symbolic_BayesTree); - - // distribute factors - gttic(distributeFactors); - this->root_ = distributeFactors(fg, sbt.root()); - gttoc(distributeFactors); - } - - /* ************************************************************************* */ - template - JunctionTree::JunctionTree(const FG& fg) { - gttic(VariableIndex); - VariableIndex varIndex(fg); - gttoc(VariableIndex); - construct(fg, varIndex); - } - - /* ************************************************************************* */ - template - JunctionTree::JunctionTree(const FG& fg, const VariableIndex& variableIndex) { - construct(fg, variableIndex); - } - - /* ************************************************************************* */ - template - typename JunctionTree::sharedClique JunctionTree::distributeFactors( - const FG& fg, const SymbolicBayesTree::sharedClique& bayesClique) { - - // Build "target" index. This is an index for each variable of the factors - // that involve this variable as their *lowest-ordered* variable. For each - // factor, it is the lowest-ordered variable of that factor that pulls the - // factor into elimination, after which all of the information in the - // factor is contained in the eliminated factors that are passed up the - // tree as elimination continues. - - // Two stages - first build an array of the lowest-ordered variable in each - // factor and find the last variable to be eliminated. - std::vector lowestOrdered(fg.size(), std::numeric_limits::max()); - Index maxVar = 0; - for(size_t i=0; ibegin(), fg[i]->end()); - if(min != fg[i]->end()) { - lowestOrdered[i] = *min; - maxVar = std::max(maxVar, *min); - } - } - - // Now add each factor to the list corresponding to its lowest-ordered - // variable. - std::vector > targets(maxVar+1); - for(size_t i=0; i::max()) - targets[lowestOrdered[i]].push_back(i); - - // Now call the recursive distributeFactors - return distributeFactors(fg, targets, bayesClique); - } - - /* ************************************************************************* */ - template - typename JunctionTree::sharedClique JunctionTree::distributeFactors(const FG& fg, - const std::vector >& targets, - const SymbolicBayesTree::sharedClique& bayesClique) { - - if(bayesClique) { - // create a new clique in the junction tree - sharedClique clique(new Clique((*bayesClique)->beginFrontals(), (*bayesClique)->endFrontals(), - (*bayesClique)->beginParents(), (*bayesClique)->endParents())); - - // count the factors for this cluster to pre-allocate space - { - size_t nFactors = 0; - BOOST_FOREACH(const Index frontal, clique->frontal) { - // There may be less variables in "targets" than there really are if - // some of the highest-numbered variables do not pull in any factors. - if(frontal < targets.size()) - nFactors += targets[frontal].size(); } - clique->reserve(nFactors); - } - // add the factors to this cluster - BOOST_FOREACH(const Index frontal, clique->frontal) { - if(frontal < targets.size()) { - BOOST_FOREACH(const size_t factorI, targets[frontal]) { - clique->push_back(fg[factorI]); } } } - - // recursively call the children - BOOST_FOREACH(const typename SymbolicBayesTree::sharedClique bayesChild, bayesClique->children()) { - sharedClique child = distributeFactors(fg, targets, bayesChild); - clique->addChild(child); - child->parent() = clique; - } - return clique; - } else - return sharedClique(); - } - - /* ************************************************************************* */ - template - std::pair::BTClique::shared_ptr, - typename FG::sharedFactor> JunctionTree::eliminateOneClique( - typename FG::Eliminate function, - const boost::shared_ptr& current) const { - - FG fg; // factor graph will be assembled from local factors and marginalized children - fg.reserve(current->size() + current->children().size()); - fg.push_back(*current); // add the local factors - - // receive the factors from the child and its clique point - std::list children; - BOOST_FOREACH(const boost::shared_ptr& child, current->children()) { - std::pair tree_factor( - eliminateOneClique(function, child)); - children.push_back(tree_factor.first); - fg.push_back(tree_factor.second); - } - - // eliminate the combined factors - // warning: fg is being eliminated in-place and will contain marginal afterwards - - // Now that we know which factors and variables, and where variables - // come from and go to, create and eliminate the new joint factor. - gttic(CombineAndEliminate); - typename FG::EliminationResult eliminated(function(fg, - current->frontal.size())); - gttoc(CombineAndEliminate); - -#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS - assert(std::equal(eliminated.second->begin(), eliminated.second->end(), current->separator.begin())); -#endif - - gttic(Update_tree); - // create a new clique corresponding the combined factors - typename BTClique::shared_ptr new_clique(BTClique::Create(eliminated)); - new_clique->children_ = children; - - BOOST_FOREACH(typename BTClique::shared_ptr& childRoot, children) { - childRoot->parent_ = new_clique; - } - gttoc(Update_tree); - - return std::make_pair(new_clique, eliminated.second); - } - - /* ************************************************************************* */ - template - typename BTCLIQUE::shared_ptr JunctionTree::eliminate( - typename FG::Eliminate function) const { - if (this->root()) { - gttic(JT_eliminate); - std::pair ret = - this->eliminateOneClique(function, this->root()); - if (ret.second->size() != 0) throw std::runtime_error( - "JuntionTree::eliminate: elimination failed because of factors left over!"); - gttoc(JT_eliminate); - return ret.first; - } else - return typename BTClique::shared_ptr(); - } - -} //namespace gtsam diff --git a/gtsam/inference/JunctionTree-inst.h b/gtsam/inference/JunctionTree-inst.h new file mode 100644 index 000000000..9d96c5b9c --- /dev/null +++ b/gtsam/inference/JunctionTree-inst.h @@ -0,0 +1,148 @@ +/* ---------------------------------------------------------------------------- + + * 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 JunctionTree-inst.h + * @date Feb 4, 2010 + * @author Kai Ni + * @author Frank Dellaert + * @author Richard Roberts + * @brief The junction tree, template bodies + */ + +#pragma once + +#include +#include +#include +#include + +namespace gtsam { + + namespace { + /* ************************************************************************* */ + template + struct ConstructorTraversalData { + ConstructorTraversalData* const parentData; + typename JunctionTree::sharedNode myJTNode; + FastVector childSymbolicConditionals; + FastVector childSymbolicFactors; + ConstructorTraversalData(ConstructorTraversalData* _parentData) : parentData(_parentData) {} + }; + + /* ************************************************************************* */ + // Pre-order visitor function + template + ConstructorTraversalData ConstructorTraversalVisitorPre( + const boost::shared_ptr& node, + ConstructorTraversalData& parentData) + { + // On the pre-order pass, before children have been visited, we just set up a traversal data + // structure with its own JT node, and create a child pointer in its parent. + ConstructorTraversalData myData = ConstructorTraversalData(&parentData); + myData.myJTNode = boost::make_shared::Node>(); + myData.myJTNode->keys.push_back(node->key); + myData.myJTNode->factors.insert(myData.myJTNode->factors.begin(), node->factors.begin(), node->factors.end()); + parentData.myJTNode->children.push_back(myData.myJTNode); + return myData; + } + + /* ************************************************************************* */ + // Post-order visitor function + template + void ConstructorTraversalVisitorPostAlg2( + const boost::shared_ptr& ETreeNode, + const ConstructorTraversalData& myData) + { + // In this post-order visitor, we combine the symbolic elimination results from the + // elimination tree children and symbolically eliminate the current elimination tree node. We + // then check whether each of our elimination tree child nodes should be merged with us. The + // check for this is that our number of symbolic elimination parents is exactly 1 less than + // our child's symbolic elimination parents - this condition indicates that eliminating the + // current node did not introduce any parents beyond those already in the child. + + // Do symbolic elimination for this node + class : public FactorGraph {} symbolicFactors; + symbolicFactors.reserve(ETreeNode->factors.size() + myData.childSymbolicFactors.size()); + // Add ETree node factors + symbolicFactors += ETreeNode->factors; + // Add symbolic factors passed up from children + symbolicFactors += myData.childSymbolicFactors; + + Ordering keyAsOrdering; keyAsOrdering.push_back(ETreeNode->key); + std::pair symbolicElimResult = + internal::EliminateSymbolic(symbolicFactors, keyAsOrdering); + + // Store symbolic elimination results in the parent + myData.parentData->childSymbolicConditionals.push_back(symbolicElimResult.first); + myData.parentData->childSymbolicFactors.push_back(symbolicElimResult.second); + + // Merge our children if they are in our clique - if our conditional has exactly one fewer + // parent than our child's conditional. + size_t myNrFrontals = 1; + const size_t myNrParents = symbolicElimResult.first->nrParents(); + size_t nrMergedChildren = 0; + assert(myData.myJTNode->children.size() == myData.childSymbolicConditionals.size()); + // Loop over children + int combinedProblemSize = (int) (symbolicElimResult.first->size() * symbolicFactors.size()); + for(size_t child = 0; child < myData.childSymbolicConditionals.size(); ++child) { + // Check if we should merge the child + if(myNrParents + myNrFrontals == myData.childSymbolicConditionals[child]->nrParents()) { + // Get a reference to the child, adjusting the index to account for children previously + // merged and removed from the child list. + const typename JunctionTree::Node& childToMerge = + *myData.myJTNode->children[child - nrMergedChildren]; + // Merge keys, factors, and children. + myData.myJTNode->keys.insert(myData.myJTNode->keys.begin(), childToMerge.keys.begin(), childToMerge.keys.end()); + myData.myJTNode->factors.insert(myData.myJTNode->factors.end(), childToMerge.factors.begin(), childToMerge.factors.end()); + myData.myJTNode->children.insert(myData.myJTNode->children.end(), childToMerge.children.begin(), childToMerge.children.end()); + // Increment problem size + combinedProblemSize = std::max(combinedProblemSize, childToMerge.problemSize_); + // Increment number of frontal variables + myNrFrontals += childToMerge.keys.size(); + // Remove child from list. + myData.myJTNode->children.erase(myData.myJTNode->children.begin() + (child - nrMergedChildren)); + // Increment number of merged children + ++ nrMergedChildren; + } + } + myData.myJTNode->problemSize_ = combinedProblemSize; + } + } + + /* ************************************************************************* */ + template + template + JunctionTree::JunctionTree(const EliminationTree& eliminationTree) + { + gttic(JunctionTree_FromEliminationTree); + // Here we rely on the BayesNet having been produced by this elimination tree, such that the + // conditionals are arranged in DFS post-order. We traverse the elimination tree, and inspect + // the symbolic conditional corresponding to each node. The elimination tree node is added to + // the same clique with its parent if it has exactly one more Bayes net conditional parent than + // does its elimination tree parent. + + // Traverse the elimination tree, doing symbolic elimination and merging nodes as we go. Gather + // the created junction tree roots in a dummy Node. + typedef typename EliminationTree::Node ETreeNode; + ConstructorTraversalData rootData(0); + rootData.myJTNode = boost::make_shared(); // Make a dummy node to gather the junction tree roots + treeTraversal::DepthFirstForest(eliminationTree, rootData, + ConstructorTraversalVisitorPre, ConstructorTraversalVisitorPostAlg2); + + // Assign roots from the dummy node + Base::roots_ = rootData.myJTNode->children; + + // Transfer remaining factors from elimination tree + Base::remainingFactors_ = eliminationTree.remainingFactors(); + } + +} //namespace gtsam diff --git a/gtsam/inference/JunctionTree.h b/gtsam/inference/JunctionTree.h index c8ecc0576..cdf38b97c 100644 --- a/gtsam/inference/JunctionTree.h +++ b/gtsam/inference/JunctionTree.h @@ -14,24 +14,23 @@ * @date Feb 4, 2010 * @author Kai Ni * @author Frank Dellaert - * @brief: The junction tree + * @author Richard Roberts + * @brief The junction tree */ #pragma once -#include -#include -#include -#include #include -#include -#include namespace gtsam { + // Forward declarations + template class EliminationTree; + /** - * A ClusterTree, i.e., a set of variable clusters with factors, arranged in a tree, with - * the additional property that it represents the clique tree associated with a Bayes net. + * A JunctionTree is a ClusterTree, i.e., a set of variable clusters with factors, arranged + * in a tree, with the additional property that it represents the clique tree associated + * with a Bayes net. * * In GTSAM a junction tree is an intermediate data structure in multifrontal * variable elimination. Each node is a cluster of factors, along with a @@ -46,90 +45,38 @@ namespace gtsam { * The tree structure and elimination method are exactly analagous to the EliminationTree, * except that in the JunctionTree, at each node multiple variables are eliminated at a time. * - * * \addtogroup Multifrontal * \nosubgrouping */ - template::Clique> - class JunctionTree: public ClusterTree { + template + class JunctionTree : public ClusterTree { public: - /// In a junction tree each cluster is associated with a clique - typedef typename ClusterTree::Cluster Clique; - typedef typename Clique::shared_ptr sharedClique; ///< Shared pointer to a clique + typedef JunctionTree This; ///< This class + typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class + typedef ClusterTree Base; ///< Our base class - /// The BayesTree type produced by elimination - typedef BTCLIQUE BTClique; - - /// Shared pointer to this class - typedef boost::shared_ptr > shared_ptr; - - /// We will frequently refer to a symbolic Bayes tree, used to find the clique structure - typedef gtsam::BayesTree SymbolicBayesTree; - - private: - - /// @name Advanced Interface - /// @{ - - /// distribute the factors along the cluster tree - sharedClique distributeFactors(const FG& fg, - const SymbolicBayesTree::sharedClique& clique); - - /// distribute the factors along the cluster tree - sharedClique distributeFactors(const FG& fg, const std::vector >& targets, - const SymbolicBayesTree::sharedClique& clique); - - /// recursive elimination function - std::pair - eliminateOneClique(typename FG::Eliminate function, - const boost::shared_ptr& clique) const; - - /// internal constructor - void construct(const FG& fg, const VariableIndex& variableIndex); - - /// @} - - public: + protected: /// @name Standard Constructors /// @{ - /** Default constructor */ + /** Build the junction tree from an elimination tree. */ + template + static This FromEliminationTree(const ETREE& eliminationTree) { return This(eliminationTree); } + + /** Build the junction tree from an elimination tree. */ + template + JunctionTree(const EliminationTree& eliminationTree); + + /// @} + + private: + + // Private default constructor (used in static construction methods) JunctionTree() {} - /** Named constructor to build the junction tree of a factor graph. Note - * that this has to compute the column structure as a VariableIndex, so if you - * already have this precomputed, use the JunctionTree(const FG&, const VariableIndex&) - * constructor instead. - * @param factorGraph The factor graph for which to build the elimination tree - */ - JunctionTree(const FG& factorGraph); + }; - /** Construct from a factor graph and pre-computed variable index. - * @param fg The factor graph for which to build the junction tree - * @param structure The set of factors involving each variable. If this is not - * precomputed, you can call the JunctionTree(const FG&) - * constructor instead. - */ - JunctionTree(const FG& fg, const VariableIndex& variableIndex); - - /// @} - /// @name Standard Interface - /// @{ - - /** Eliminate the factors in the subgraphs to produce a BayesTree. - * @param function The function used to eliminate, see the namespace functions - * in GaussianFactorGraph.h - * @return The BayesTree resulting from elimination - */ - typename BTClique::shared_ptr eliminate(typename FG::Eliminate function) const; - - /// @} - - }; // JunctionTree - -} // namespace gtsam - -#include +} diff --git a/gtsam/nonlinear/Key.cpp b/gtsam/inference/Key.cpp similarity index 55% rename from gtsam/nonlinear/Key.cpp rename to gtsam/inference/Key.cpp index c4b21e681..0b9be2f1c 100644 --- a/gtsam/nonlinear/Key.cpp +++ b/gtsam/inference/Key.cpp @@ -22,44 +22,53 @@ #include #include -#include -#include +#include +#include namespace gtsam { /* ************************************************************************* */ -std::string _defaultKeyFormatter(Key key) { - const Symbol asSymbol(key); - if(asSymbol.chr() > 0) - return (std::string)asSymbol; - else - return boost::lexical_cast(key); - } - -/* ************************************************************************* */ -std::string _multirobotKeyFormatter(gtsam::Key key) { +std::string _multirobotKeyFormatter(Key key) { const LabeledSymbol asLabeledSymbol(key); - if(asLabeledSymbol.chr() > 0 && asLabeledSymbol.label() > 0) - return (std::string)asLabeledSymbol; + if (asLabeledSymbol.chr() > 0 && asLabeledSymbol.label() > 0) + return (std::string) asLabeledSymbol; - const gtsam::Symbol asSymbol(key); + const Symbol asSymbol(key); if (asLabeledSymbol.chr() > 0) - return (std::string)asSymbol; + return (std::string) asSymbol; else return boost::lexical_cast(key); } /* ************************************************************************* */ -void printKeySet(const gtsam::KeySet& keys, const std::string& s, const KeyFormatter& keyFormatter) { +template +static void print(const CONTAINER& keys, const std::string& s, + const KeyFormatter& keyFormatter) { std::cout << s << " "; if (keys.empty()) std::cout << "(none)" << std::endl; else { - BOOST_FOREACH(const gtsam::Key& key, keys) + BOOST_FOREACH(const Key& key, keys) std::cout << keyFormatter(key) << " "; std::cout << std::endl; } } + +/* ************************************************************************* */ +void printKeyList(const KeyList& keys, const std::string& s, + const KeyFormatter& keyFormatter) { + print(keys, s, keyFormatter); +} +/* ************************************************************************* */ +void printKeyVector(const KeyVector& keys, const std::string& s, + const KeyFormatter& keyFormatter) { + print(keys, s, keyFormatter); +} +/* ************************************************************************* */ +void printKeySet(const KeySet& keys, const std::string& s, + const KeyFormatter& keyFormatter) { + print(keys, s, keyFormatter); +} /* ************************************************************************* */ } // \namespace gtsam diff --git a/gtsam/nonlinear/Key.h b/gtsam/inference/Key.h similarity index 74% rename from gtsam/nonlinear/Key.h rename to gtsam/inference/Key.h index edeea396f..e2be79dc7 100644 --- a/gtsam/nonlinear/Key.h +++ b/gtsam/inference/Key.h @@ -24,22 +24,10 @@ #include #include #include +#include namespace gtsam { - /// Integer nonlinear key type - typedef size_t Key; - - /// Typedef for a function to format a key, i.e. to convert it to a string - typedef boost::function KeyFormatter; - - // Helper function for DefaultKeyFormatter - GTSAM_EXPORT std::string _defaultKeyFormatter(Key key); - - /// The default KeyFormatter, which is used if no KeyFormatter is passed to - /// a nonlinear 'print' function. Automatically detects plain integer keys - /// and Symbol keys. - static const KeyFormatter DefaultKeyFormatter = &_defaultKeyFormatter; // Helper function for Multi-robot Key Formatter GTSAM_EXPORT std::string _multirobotKeyFormatter(gtsam::Key key); @@ -56,6 +44,15 @@ namespace gtsam { typedef FastList KeyList; typedef FastVector KeyVector; typedef FastSet KeySet; + typedef FastMap KeyGroupMap; + + /// Utility function to print sets of keys with optional prefix + GTSAM_EXPORT void printKeyList(const KeyList& keys, const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter); + + /// Utility function to print sets of keys with optional prefix + GTSAM_EXPORT void printKeyVector(const KeyVector& keys, const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter); /// Utility function to print sets of keys with optional prefix GTSAM_EXPORT void printKeySet(const KeySet& keys, const std::string& s = "", diff --git a/gtsam/nonlinear/LabeledSymbol.cpp b/gtsam/inference/LabeledSymbol.cpp similarity index 99% rename from gtsam/nonlinear/LabeledSymbol.cpp rename to gtsam/inference/LabeledSymbol.cpp index 97272e873..7e573c13f 100644 --- a/gtsam/nonlinear/LabeledSymbol.cpp +++ b/gtsam/inference/LabeledSymbol.cpp @@ -26,7 +26,7 @@ #include -#include +#include namespace gtsam { diff --git a/gtsam/nonlinear/LabeledSymbol.h b/gtsam/inference/LabeledSymbol.h similarity index 99% rename from gtsam/nonlinear/LabeledSymbol.h rename to gtsam/inference/LabeledSymbol.h index eaa7f373f..4b125988c 100644 --- a/gtsam/nonlinear/LabeledSymbol.h +++ b/gtsam/inference/LabeledSymbol.h @@ -19,7 +19,7 @@ #pragma once -#include +#include namespace gtsam { diff --git a/gtsam/inference/Ordering.cpp b/gtsam/inference/Ordering.cpp new file mode 100644 index 000000000..4f4b14bb5 --- /dev/null +++ b/gtsam/inference/Ordering.cpp @@ -0,0 +1,232 @@ +/* ---------------------------------------------------------------------------- + + * 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 Ordering.cpp + * @author Richard Roberts + * @date Sep 2, 2010 + */ + +#include +#include + +#include + +#include +#include + +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; + } + + /* ************************************************************************* */ + 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; + } + + 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()); + } + + // ccolamd_report(stats); + + 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); + + 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); + } + + /* ************************************************************************* */ + 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); + } + + /* ************************************************************************* */ + 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) + cout << "\n"; + cout.flush(); + } + + /* ************************************************************************* */ + bool Ordering::equals(const Ordering& other, double tol) const + { + return (*this) == other; + } + +} diff --git a/gtsam/inference/Ordering.h b/gtsam/inference/Ordering.h new file mode 100644 index 000000000..ea2ad7eda --- /dev/null +++ b/gtsam/inference/Ordering.h @@ -0,0 +1,161 @@ +/* ---------------------------------------------------------------------------- + + * 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 Ordering.h + * @author Richard Roberts + * @date Sep 2, 2010 + */ + +#pragma once + +#include +#include + +#include +#include +#include + +namespace gtsam { + class Ordering : public std::vector { + protected: + typedef std::vector Base; + + public: + 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); + + /// @} + + /// @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); + } + }; +} + diff --git a/gtsam/inference/Permutation.cpp b/gtsam/inference/Permutation.cpp deleted file mode 100644 index 94528f64f..000000000 --- a/gtsam/inference/Permutation.cpp +++ /dev/null @@ -1,226 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 Permutation.cpp - * @author Richard Roberts - * @date Oct 9, 2010 - */ - -#include - -#include -#include -#include -#include - -using namespace std; - -namespace gtsam { - -/* ************************************************************************* */ -Permutation Permutation::Identity(Index nVars) { - Permutation ret(nVars); - for(Index i=0; i& toFront, size_t size, bool filterDuplicates) { - - Permutation ret(size); - - // Mask of which variables have been pulled, used to reorder - vector pulled(size, false); - - // Put the pulled variables at the front of the permutation and set up the - // pulled flags. - size_t toFrontUniqueSize = 0; - for(Index j=0; j& js) const { - BOOST_FOREACH(Index& j, js) { - j = this->find(j)->second; - } - } - - /* ************************************************************************* */ - Permutation Reduction::inverse() const { - Index maxIndex = 0; - BOOST_FOREACH(const value_type& target_source, *this) { - if(target_source.second > maxIndex) - maxIndex = target_source.second; - } - Permutation result(maxIndex + 1); - BOOST_FOREACH(const value_type& target_source, *this) { - result[target_source.second] = target_source.first; - } - return result; - } - - /* ************************************************************************* */ - const Index& Reduction::operator[](const Index& j) { - iterator it = this->find(j); - if(it == this->end()) - return j; - else - return it->second; - } - - /* ************************************************************************* */ - const Index& Reduction::operator[](const Index& j) const { - const_iterator it = this->find(j); - if(it == this->end()) - return j; - else - return it->second; - } - - /* ************************************************************************* */ - void Reduction::print(const std::string& s) const { - cout << s << " reduction:" << endl; - BOOST_FOREACH(const value_type& p, *this) - cout << " " << p.first << " : " << p.second << endl; - } - - /* ************************************************************************* */ - bool Reduction::equals(const Reduction& other, double tol) const { - return (const Base&)(*this) == (const Base&)other; - } - - /* ************************************************************************* */ - Permutation createReducingPermutation(const std::set& indices) { - Permutation p(indices.size()); - Index newJ = 0; - BOOST_FOREACH(Index j, indices) { - p[newJ] = j; - ++ newJ; - } - return p; - } -} // \namespace internal - -/* ************************************************************************* */ -} // \namespace gtsam diff --git a/gtsam/inference/Permutation.h b/gtsam/inference/Permutation.h deleted file mode 100644 index 213055781..000000000 --- a/gtsam/inference/Permutation.h +++ /dev/null @@ -1,215 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 Permutation.h - * @author Richard Roberts - * @date Sep 12, 2010 - */ - -#pragma once - -#include -#include -#include -#include -#include - -#include -#include - -namespace gtsam { - - /** - * A permutation reorders variables, for example to reduce fill-in during - * elimination. To save computation, the permutation can be applied to - * the necessary data structures only once, then multiple computations - * performed on the permuted problem. For example, in an iterative - * non-linear setting, a permutation can be created from the symbolic graph - * structure and applied to the ordering of nonlinear variables once, so - * that linearized factor graphs are already correctly ordered and need - * not be permuted. - * - * Consistent with their mathematical definition, permutations are functions - * that accept the destination index and return the source index. For example, - * to permute data structure A into a new data structure B using permutation P, - * the mapping is B[i] = A[P[i]]. This is the behavior implemented by - * B = A.permute(P). - * - * For some data structures in GTSAM, for efficiency it is necessary to have - * the inverse of the desired permutation. In this case, the data structure - * will implement permuteWithInverse(P) instead of permute(P). You may already - * have the inverse permutation by construction, or may instead compute it with - * Pinv = P.inverse(). - * - * Permutations can be composed and inverted - * in order to create new indexing for a structure. - * \nosubgrouping - */ -class GTSAM_EXPORT Permutation { -protected: - std::vector rangeIndices_; - -public: - typedef boost::shared_ptr shared_ptr; - typedef std::vector::const_iterator const_iterator; - typedef std::vector::iterator iterator; - - /// @name Standard Constructors - /// @{ - - /** - * Create an empty permutation. This cannot do anything, but you can later - * assign to it. - */ - Permutation() {} - - /** - * Create an uninitialized permutation. You must assign all values using the - * square bracket [] operator or they will be undefined! - */ - Permutation(Index nVars) : rangeIndices_(nVars) {} - - /// @} - /// @name Testable - /// @{ - - /** Print */ - void print(const std::string& str = "Permutation: ") const; - - /** Check equality */ - bool equals(const Permutation& rhs, double tol=0.0) const { return rangeIndices_ == rhs.rangeIndices_; } - - - /// @} - /// @name Standard Interface - /// @{ - - /** - * Return the source index of the supplied destination index. - */ - Index operator[](Index variable) const { check(variable); return rangeIndices_[variable]; } - - /** - * Return the source index of the supplied destination index. This version allows modification. - */ - Index& operator[](Index variable) { check(variable); return rangeIndices_[variable]; } - - /** - * Return the source index of the supplied destination index. Synonym for operator[](Index). - */ - Index at(Index variable) const { return operator[](variable); } - - /** - * Return the source index of the supplied destination index. This version allows modification. Synonym for operator[](Index). - */ - Index& at(Index variable) { return operator[](variable); } - - /** - * The number of variables in the range of this permutation, i.e. the - * destination space. - */ - Index size() const { return rangeIndices_.size(); } - - /** Whether the permutation contains any entries */ - bool empty() const { return rangeIndices_.empty(); } - - /** - * Resize the permutation. You must fill in the new entries if new new size - * is larger than the old one. If the new size is smaller, entries from the - * end are discarded. - */ - void resize(size_t newSize) { rangeIndices_.resize(newSize); } - - /** - * Return an identity permutation. - */ - static Permutation Identity(Index nVars); - - /** - * Create a permutation that pulls the given variables to the front while - * pushing the rest to the back. - */ - static Permutation PullToFront(const std::vector& toFront, size_t size, bool filterDuplicates = false); - - /** - * Create a permutation that pushes the given variables to the back while - * pulling the rest to the front. - */ - static Permutation PushToBack(const std::vector& toBack, size_t size, bool filterDuplicates = false); - - - /** - * Permute the permutation, p1.permute(p2)[i] is equivalent to p1[p2[i]]. - */ - Permutation::shared_ptr permute(const Permutation& permutation) const; - - /** - * Return the inverse permutation. This is only possible if this is a non- - * reducing permutation, that is, (*this)[i] < this->size() for all - * i < size(). If NDEBUG is not defined, this conditional will be checked. - */ - Permutation::shared_ptr inverse() const; - - const_iterator begin() const { return rangeIndices_.begin(); } ///< Iterate through the indices - const_iterator end() const { return rangeIndices_.end(); } ///< Iterate through the indices - - /** Apply the permutation to a collection, which must have operator[] defined. - * Note that permutable gtsam data structures typically have their own - * permute function to apply a permutation. Permutation::applyToCollection is - * a generic function, e.g. for STL classes. - * @param input The input collection. - * @param output The preallocated output collection, which is assigned output[i] = input[permutation[i]] - */ - template - void applyToCollection(OUTPUT_COLLECTION& output, const INPUT_COLLECTION& input) const { - for(size_t i = 0; i < output.size(); ++i) output[i] = input[(*this)[i]]; } - - - /// @} - /// @name Advanced Interface - /// @{ - - iterator begin() { return rangeIndices_.begin(); } ///< Iterate through the indices - iterator end() { return rangeIndices_.end(); } ///< Iterate through the indices - -protected: - void check(Index variable) const { assert(variable < rangeIndices_.size()); } - - /// @} -}; - - -namespace internal { - /** - * An internal class used for storing and applying a permutation from a map - */ - class Reduction : public gtsam::FastMap { - public: - typedef gtsam::FastMap Base; - - GTSAM_EXPORT static Reduction CreateAsInverse(const Permutation& p); - GTSAM_EXPORT static Reduction CreateFromPartialPermutation(const Permutation& selector, const Permutation& p); - GTSAM_EXPORT void applyInverse(std::vector& js) const; - GTSAM_EXPORT Permutation inverse() const; - GTSAM_EXPORT const Index& operator[](const Index& j); - GTSAM_EXPORT const Index& operator[](const Index& j) const; - - GTSAM_EXPORT void print(const std::string& s="") const; - GTSAM_EXPORT bool equals(const Reduction& other, double tol = 1e-9) const; - }; - - /** - * Reduce the variable indices so that those in the set are mapped to start at zero - */ - GTSAM_EXPORT Permutation createReducingPermutation(const std::set& indices); -} // \namespace internal -} // \namespace gtsam diff --git a/gtsam/nonlinear/Symbol.cpp b/gtsam/inference/Symbol.cpp similarity index 98% rename from gtsam/nonlinear/Symbol.cpp rename to gtsam/inference/Symbol.cpp index d2d756626..37a6d0897 100644 --- a/gtsam/nonlinear/Symbol.cpp +++ b/gtsam/inference/Symbol.cpp @@ -16,7 +16,7 @@ * @author: Richard Roberts */ -#include +#include #include #include diff --git a/gtsam/inference/Symbol.h b/gtsam/inference/Symbol.h new file mode 100644 index 000000000..6963905e0 --- /dev/null +++ b/gtsam/inference/Symbol.h @@ -0,0 +1,165 @@ +/* ---------------------------------------------------------------------------- + + * 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 Symbol.h + * @date Jan 12, 2010 + * @author: Frank Dellaert + * @author: Richard Roberts + */ + +#pragma once + +#include + +#include + +namespace gtsam { + +/** + * Character and index key used in VectorValues, GaussianFactorGraph, + * GaussianFactor, etc. These keys are generated at runtime from TypedSymbol + * keys when linearizing a nonlinear factor graph. This key is not type + * safe, so cannot be used with any Nonlinear* classes. + */ +class GTSAM_EXPORT Symbol { +protected: + unsigned char c_; + size_t j_; + +public: + + /** Default constructor */ + Symbol() : + c_(0), j_(0) { + } + + /** Copy constructor */ + Symbol(const Symbol& key) : + c_(key.c_), j_(key.j_) { + } + + /** Constructor */ + Symbol(unsigned char c, size_t j) : + c_(c), j_(j) { + } + + /** Constructor that decodes an integer Key */ + Symbol(Key key); + + /** return Key (integer) representation */ + Key key() const; + + /** Cast to integer */ + operator Key() const { return key(); } + + /// Print + void print(const std::string& s = "") const; + + /// Check equality + bool equals(const Symbol& expected, double tol = 0.0) const; + + /** Retrieve key character */ + unsigned char chr() const { + return c_; + } + + /** Retrieve key index */ + size_t index() const { + return j_; + } + + /** Create a string from the key */ + operator std::string() const; + + /** Comparison for use in maps */ + bool operator<(const Symbol& comp) const { + return c_ < comp.c_ || (comp.c_ == c_ && j_ < comp.j_); + } + + /** Comparison for use in maps */ + bool operator==(const Symbol& comp) const { + return comp.c_ == c_ && comp.j_ == j_; + } + + /** Comparison for use in maps */ + bool operator==(Key comp) const { + return comp == (Key)(*this); + } + + /** Comparison for use in maps */ + bool operator!=(const Symbol& comp) const { + return comp.c_ != c_ || comp.j_ != j_; + } + + /** Comparison for use in maps */ + bool operator!=(Key comp) const { + return comp != (Key)(*this); + } + + /** Return a filter function that returns true when evaluated on a Key whose + * character (when converted to a Symbol) matches \c c. Use this with the + * Values::filter() function to retrieve all key-value pairs with the + * requested character. + */ + static boost::function ChrTest(unsigned char c); + +private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(c_); + ar & BOOST_SERIALIZATION_NVP(j_); + } +}; + +/** Create a symbol key from a character and index, i.e. x5. */ +inline Key symbol(unsigned char c, size_t j) { return (Key)Symbol(c,j); } + +/** Return the character portion of a symbol key. */ +inline unsigned char symbolChr(Key key) { return Symbol(key).chr(); } + +/** Return the index portion of a symbol key. */ +inline size_t symbolIndex(Key key) { return Symbol(key).index(); } + +namespace symbol_shorthand { +inline Key A(size_t j) { return Symbol('a', j); } +inline Key B(size_t j) { return Symbol('b', j); } +inline Key C(size_t j) { return Symbol('c', j); } +inline Key D(size_t j) { return Symbol('d', j); } +inline Key E(size_t j) { return Symbol('e', j); } +inline Key F(size_t j) { return Symbol('f', j); } +inline Key G(size_t j) { return Symbol('g', j); } +inline Key H(size_t j) { return Symbol('h', j); } +inline Key I(size_t j) { return Symbol('i', j); } +inline Key J(size_t j) { return Symbol('j', j); } +inline Key K(size_t j) { return Symbol('k', j); } +inline Key L(size_t j) { return Symbol('l', j); } +inline Key M(size_t j) { return Symbol('m', j); } +inline Key N(size_t j) { return Symbol('n', j); } +inline Key O(size_t j) { return Symbol('o', j); } +inline Key P(size_t j) { return Symbol('p', j); } +inline Key Q(size_t j) { return Symbol('q', j); } +inline Key R(size_t j) { return Symbol('r', j); } +inline Key S(size_t j) { return Symbol('s', j); } +inline Key T(size_t j) { return Symbol('t', j); } +inline Key U(size_t j) { return Symbol('u', j); } +inline Key V(size_t j) { return Symbol('v', j); } +inline Key W(size_t j) { return Symbol('w', j); } +inline Key X(size_t j) { return Symbol('x', j); } +inline Key Y(size_t j) { return Symbol('y', j); } +inline Key Z(size_t j) { return Symbol('z', j); } +} + +} // namespace gtsam + diff --git a/gtsam/inference/SymbolicFactorGraph.cpp b/gtsam/inference/SymbolicFactorGraph.cpp deleted file mode 100644 index 7f71ab54a..000000000 --- a/gtsam/inference/SymbolicFactorGraph.cpp +++ /dev/null @@ -1,132 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 SymbolicFactorGraph.cpp - * @date Oct 29, 2009 - * @author Frank Dellaert - */ - -#include -#include -#include -#include - -#include - -namespace gtsam { - - using namespace std; - - /* ************************************************************************* */ - SymbolicFactorGraph::SymbolicFactorGraph(const SymbolicBayesNet& bayesNet) : - FactorGraph(bayesNet) {} - - /* ************************************************************************* */ - SymbolicFactorGraph::SymbolicFactorGraph(const SymbolicBayesTree& bayesTree) : - FactorGraph(bayesTree) {} - - /* ************************************************************************* */ - void SymbolicFactorGraph::push_factor(Index key) { - push_back(boost::make_shared(key)); - } - - /** Push back binary factor */ - void SymbolicFactorGraph::push_factor(Index key1, Index key2) { - push_back(boost::make_shared(key1,key2)); - } - - /** Push back ternary factor */ - void SymbolicFactorGraph::push_factor(Index key1, Index key2, Index key3) { - push_back(boost::make_shared(key1,key2,key3)); - } - - /** Push back 4-way factor */ - void SymbolicFactorGraph::push_factor(Index key1, Index key2, Index key3, Index key4) { - push_back(boost::make_shared(key1,key2,key3,key4)); - } - - /* ************************************************************************* */ - FastSet - SymbolicFactorGraph::keys() const { - FastSet keys; - BOOST_FOREACH(const sharedFactor& factor, *this) { - if(factor) keys.insert(factor->begin(), factor->end()); } - return keys; - } - - /* ************************************************************************* */ - std::pair - SymbolicFactorGraph::eliminateFrontals(size_t nFrontals) const - { - return FactorGraph::eliminateFrontals(nFrontals, EliminateSymbolic); - } - - /* ************************************************************************* */ - std::pair - SymbolicFactorGraph::eliminate(const std::vector& variables) const - { - return FactorGraph::eliminate(variables, EliminateSymbolic); - } - - /* ************************************************************************* */ - std::pair - SymbolicFactorGraph::eliminateOne(Index variable) const - { - return FactorGraph::eliminateOne(variable, EliminateSymbolic); - } - - /* ************************************************************************* */ - void SymbolicFactorGraph::permuteWithInverse( - const Permutation& inversePermutation) { - BOOST_FOREACH(const sharedFactor& factor, factors_) { - if(factor) - factor->permuteWithInverse(inversePermutation); - } - } - - /* ************************************************************************* */ - void SymbolicFactorGraph::reduceWithInverse( - const internal::Reduction& inverseReduction) { - BOOST_FOREACH(const sharedFactor& factor, factors_) { - if(factor) - factor->reduceWithInverse(inverseReduction); - } - } - - /* ************************************************************************* */ - IndexFactor::shared_ptr CombineSymbolic( - const FactorGraph& factors, const FastMap >& variableSlots) { - IndexFactor::shared_ptr combined(Combine (factors, variableSlots)); -// combined->assertInvariants(); - return combined; - } - - /* ************************************************************************* */ - pair // - EliminateSymbolic(const FactorGraph& factors, size_t nrFrontals) { - - FastSet keys; - BOOST_FOREACH(const IndexFactor::shared_ptr& factor, factors) - BOOST_FOREACH(Index var, *factor) - keys.insert(var); - - if (keys.size() < nrFrontals) throw invalid_argument( - "EliminateSymbolic requested to eliminate more variables than exist in graph."); - - vector newKeys(keys.begin(), keys.end()); - return make_pair(boost::make_shared(newKeys, nrFrontals), - boost::make_shared(newKeys.begin() + nrFrontals, newKeys.end())); - } - - /* ************************************************************************* */ -} diff --git a/gtsam/inference/SymbolicFactorGraph.h b/gtsam/inference/SymbolicFactorGraph.h deleted file mode 100644 index 31e2b2b88..000000000 --- a/gtsam/inference/SymbolicFactorGraph.h +++ /dev/null @@ -1,151 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 SymbolicFactorGraph.h - * @date Oct 29, 2009 - * @author Frank Dellaert - */ - -#pragma once - -#include -#include -#include - -namespace gtsam { template class EliminationTree; } -namespace gtsam { template class BayesNet; } -namespace gtsam { template class BayesTree; } -namespace gtsam { class IndexConditional; } - -namespace gtsam { - - typedef EliminationTree SymbolicEliminationTree; - typedef BayesNet SymbolicBayesNet; - typedef BayesTree SymbolicBayesTree; - - /** Symbolic IndexFactor Graph - * \nosubgrouping - */ - class SymbolicFactorGraph: public FactorGraph { - - public: - - /// @name Standard Constructors - /// @{ - - /** Construct empty factor graph */ - SymbolicFactorGraph() { - } - - /** Construct from a BayesNet */ - GTSAM_EXPORT SymbolicFactorGraph(const SymbolicBayesNet& bayesNet); - - /** Construct from a BayesTree */ - GTSAM_EXPORT SymbolicFactorGraph(const SymbolicBayesTree& bayesTree); - - /** - * Construct from a factor graph of any type - */ - template - SymbolicFactorGraph(const FactorGraph& fg); - - /** Eliminate the first \c n frontal variables, returning the resulting - * conditional and remaining factor graph - this is very inefficient for - * eliminating all variables, to do that use EliminationTree or - * JunctionTree. Note that this version simply calls - * FactorGraph::eliminateFrontals with EliminateSymbolic - * as the eliminate function argument. - */ - GTSAM_EXPORT std::pair eliminateFrontals(size_t nFrontals) const; - - /** Factor the factor graph into a conditional and a remaining factor graph. - * Given the factor graph \f$ f(X) \f$, and \c variables to factorize out - * \f$ V \f$, this function factorizes into \f$ f(X) = f(V;Y)f(Y) \f$, where - * \f$ Y := X\V \f$ are the remaining variables. If \f$ f(X) = p(X) \f$ is - * a probability density or likelihood, the factorization produces a - * conditional probability density and a marginal \f$ p(X) = p(V|Y)p(Y) \f$. - * - * For efficiency, this function treats the variables to eliminate - * \c variables as fully-connected, so produces a dense (fully-connected) - * conditional on all of the variables in \c variables, instead of a sparse - * BayesNet. If the variables are not fully-connected, it is more efficient - * to sequentially factorize multiple times. - * Note that this version simply calls - * FactorGraph::eliminate with EliminateSymbolic as the eliminate - * function argument. - */ - GTSAM_EXPORT std::pair eliminate(const std::vector& variables) const; - - /** Eliminate a single variable, by calling SymbolicFactorGraph::eliminate. */ - GTSAM_EXPORT std::pair eliminateOne(Index variable) const; - - /// @} - /// @name Standard Interface - /// @{ - - /** - * Return the set of variables involved in the factors (computes a set - * union). - */ - GTSAM_EXPORT FastSet keys() const; - - /// @} - /// @name Advanced Interface - /// @{ - - /** Push back unary factor */ - GTSAM_EXPORT void push_factor(Index key); - - /** Push back binary factor */ - GTSAM_EXPORT void push_factor(Index key1, Index key2); - - /** Push back ternary factor */ - GTSAM_EXPORT void push_factor(Index key1, Index key2, Index key3); - - /** Push back 4-way factor */ - GTSAM_EXPORT void push_factor(Index key1, Index key2, Index key3, Index key4); - - /** Permute the variables in the factors */ - GTSAM_EXPORT void permuteWithInverse(const Permutation& inversePermutation); - - /** Apply a reduction, which is a remapping of variable indices. */ - GTSAM_EXPORT void reduceWithInverse(const internal::Reduction& inverseReduction); - - }; - - /** Create a combined joint factor (new style for EliminationTree). */ - GTSAM_EXPORT IndexFactor::shared_ptr CombineSymbolic(const FactorGraph& factors, - const FastMap >& variableSlots); - - /** - * CombineAndEliminate provides symbolic elimination. - * Combine and eliminate can also be called separately, but for this and - * derived classes calling them separately generally does extra work. - */ - GTSAM_EXPORT std::pair, boost::shared_ptr > - EliminateSymbolic(const FactorGraph&, size_t nrFrontals = 1); - - /// @} - - /** Template function implementation */ - template - SymbolicFactorGraph::SymbolicFactorGraph(const FactorGraph& fg) { - for (size_t i = 0; i < fg.size(); i++) { - if (fg[i]) { - IndexFactor::shared_ptr factor(new IndexFactor(*fg[i])); - push_back(factor); - } else - push_back(IndexFactor::shared_ptr()); - } - } - -} // namespace gtsam diff --git a/gtsam/inference/SymbolicMultifrontalSolver.h b/gtsam/inference/SymbolicMultifrontalSolver.h deleted file mode 100644 index 802b10d72..000000000 --- a/gtsam/inference/SymbolicMultifrontalSolver.h +++ /dev/null @@ -1,71 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 SymbolicMultifrontalSolver.h - * @author Richard Roberts - * @date Oct 22, 2010 - */ - -#pragma once - -#include -#include - -namespace gtsam { - - class SymbolicMultifrontalSolver : GenericMultifrontalSolver > > { - - protected: - typedef GenericMultifrontalSolver > > Base; - - public: - /** - * Construct the solver for a factor graph. This builds the junction - * tree, which does the symbolic elimination, identifies the cliques, - * and distributes all the factors to the right cliques. - */ - SymbolicMultifrontalSolver(const SymbolicFactorGraph& factorGraph) : Base(factorGraph) {}; - - /** - * Construct the solver with a shared pointer to a factor graph and to a - * VariableIndex. The solver will store these pointers, so this constructor - * is the fastest. - */ - SymbolicMultifrontalSolver(const SymbolicFactorGraph::shared_ptr& factorGraph, - const VariableIndex::shared_ptr& variableIndex) : Base(factorGraph, variableIndex) {}; - - /** Print to cout */ - void print(const std::string& name = "SymbolicMultifrontalSolver: ") const { Base::print(name); }; - - /** Test whether is equal to another */ - bool equals(const SymbolicMultifrontalSolver& other, double tol = 1e-9) const { return Base::equals(other, tol); }; - - /** - * Eliminate the factor graph sequentially. Uses a column elimination tree - * to recursively eliminate. - */ - SymbolicBayesTree::shared_ptr eliminate() const { return Base::eliminate(&EliminateSymbolic); }; - - /** - * Compute the marginal joint over a set of variables, by integrating out - * all of the other variables. Returns the result as a factor graph. - */ - SymbolicFactorGraph::shared_ptr jointFactorGraph(const std::vector& js) const { return Base::jointFactorGraph(js, &EliminateSymbolic); }; - - /** - * Compute the marginal Gaussian density over a variable, by integrating out - * all of the other variables. This function returns the result as a factor. - */ - IndexFactor::shared_ptr marginalFactor(Index j) const { return Base::marginalFactor(j, &EliminateSymbolic); }; - }; - -} diff --git a/gtsam/inference/SymbolicSequentialSolver.h b/gtsam/inference/SymbolicSequentialSolver.h deleted file mode 100644 index 5bff88430..000000000 --- a/gtsam/inference/SymbolicSequentialSolver.h +++ /dev/null @@ -1,88 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 SymbolicSequentialSolver.h - * @author Richard Roberts - * @date Oct 21, 2010 - */ -#pragma once - -#include -#include - -namespace gtsam { - - class SymbolicSequentialSolver : GenericSequentialSolver { - - protected: - typedef GenericSequentialSolver Base; - - public: - /** - * Construct the solver for a factor graph. This builds the junction - * tree, which does the symbolic elimination, identifies the cliques, - * and distributes all the factors to the right cliques. - */ - SymbolicSequentialSolver(const SymbolicFactorGraph& factorGraph) : Base(factorGraph) {}; - - /** - * Construct the solver with a shared pointer to a factor graph and to a - * VariableIndex. The solver will store these pointers, so this constructor - * is the fastest. - */ - SymbolicSequentialSolver(const SymbolicFactorGraph::shared_ptr& factorGraph, - const VariableIndex::shared_ptr& variableIndex) : Base(factorGraph, variableIndex) {}; - - /** Print to cout */ - void print(const std::string& name = "SymbolicSequentialSolver: ") const { Base::print(name); }; - - /** Test whether is equal to another */ - bool equals(const SymbolicSequentialSolver& other, double tol = 1e-9) const { return Base::equals(other, tol); }; - - /** - * Eliminate the factor graph sequentially. Uses a column elimination tree - * to recursively eliminate. - */ - SymbolicBayesNet::shared_ptr eliminate() const - { return Base::eliminate(&EliminateSymbolic); }; - - /** - * Compute a conditional density P(F|S) while marginalizing out variables J - * P(F|S) is obtained by P(J,F,S)=P(J|F,S)P(F|S)P(S) and dropping P(S) - * Returns the result as a Bayes net. - */ - SymbolicBayesNet::shared_ptr conditionalBayesNet(const std::vector& js, size_t nrFrontals) const - { return Base::conditionalBayesNet(js, nrFrontals, &EliminateSymbolic); }; - - /** - * Compute the marginal joint over a set of variables, by integrating out - * all of the other variables. Returns the result as a Bayes net. - */ - SymbolicBayesNet::shared_ptr jointBayesNet(const std::vector& js) const - { return Base::jointBayesNet(js, &EliminateSymbolic); }; - - /** - * Compute the marginal joint over a set of variables, by integrating out - * all of the other variables. Returns the result as a factor graph. - */ - SymbolicFactorGraph::shared_ptr jointFactorGraph(const std::vector& js) const - { return Base::jointFactorGraph(js, &EliminateSymbolic); }; - - /** - * Compute the marginal Gaussian density over a variable, by integrating out - * all of the other variables. This function returns the result as a factor. - */ - IndexFactor::shared_ptr marginalFactor(Index j) const { return Base::marginalFactor(j, &EliminateSymbolic); }; - }; - -} - diff --git a/gtsam/inference/VariableIndex-inl.h b/gtsam/inference/VariableIndex-inl.h new file mode 100644 index 000000000..a88ea5d57 --- /dev/null +++ b/gtsam/inference/VariableIndex-inl.h @@ -0,0 +1,98 @@ +/* ---------------------------------------------------------------------------- + + * 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 VariableIndex-inl.h + * @author Richard Roberts + * @date March 26, 2013 + */ + +#pragma once + +#include + +namespace gtsam { + +/* ************************************************************************* */ +template +void VariableIndex::augment(const FG& factors, boost::optional&> newFactorIndices) +{ + gttic(VariableIndex_augment); + + // Augment index for each factor + for(size_t i = 0; i < factors.size(); ++i) + { + if(factors[i]) + { + const size_t globalI = + newFactorIndices ? + (*newFactorIndices)[i] : + nFactors_; + BOOST_FOREACH(const Key key, *factors[i]) + { + index_[key].push_back(globalI); + ++ nEntries_; + } + } + + // Increment factor count even if factors are null, to keep indices consistent + if(newFactorIndices) + { + if((*newFactorIndices)[i] >= nFactors_) + nFactors_ = (*newFactorIndices)[i] + 1; + } + else + { + ++ nFactors_; + } + } +} + +/* ************************************************************************* */ +template +void VariableIndex::remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG& factors) +{ + gttic(VariableIndex_remove); + + // NOTE: We intentionally do not decrement nFactors_ because the factor + // indices need to remain consistent. Removing factors from a factor graph + // does not shift the indices of other factors. Also, we keep nFactors_ + // one greater than the highest-numbered factor referenced in a VariableIndex. + ITERATOR factorIndex = firstFactor; + size_t i = 0; + for( ; factorIndex != lastFactor; ++factorIndex, ++i) { + if(i >= factors.size()) + throw std::invalid_argument("Internal error, requested inconsistent number of factor indices and factors in VariableIndex::remove"); + if(factors[i]) { + BOOST_FOREACH(Key j, *factors[i]) { + Factors& factorEntries = internalAt(j); + Factors::iterator entry = std::find(factorEntries.begin(), factorEntries.end(), *factorIndex); + if(entry == factorEntries.end()) + throw std::invalid_argument("Internal error, indices and factors passed into VariableIndex::remove are not consistent with the existing variable index"); + factorEntries.erase(entry); + -- nEntries_; + } + } + } +} + +/* ************************************************************************* */ +template +void VariableIndex::removeUnusedVariables(ITERATOR firstKey, ITERATOR lastKey) { + for(ITERATOR key = firstKey; key != lastKey; ++key) { + KeyMap::iterator entry = index_.find(*key); + if(!entry->second.empty()) + throw std::invalid_argument("Asking to remove variables from the variable index that are not unused"); + index_.erase(entry); + } +} + +} diff --git a/gtsam/inference/VariableIndex.cpp b/gtsam/inference/VariableIndex.cpp index 91036e82a..e67c0e248 100644 --- a/gtsam/inference/VariableIndex.cpp +++ b/gtsam/inference/VariableIndex.cpp @@ -12,13 +12,12 @@ /** * @file VariableIndex.cpp * @author Richard Roberts - * @date Oct 22, 2010 + * @date March 26, 2013 */ #include #include -#include namespace gtsam { @@ -26,79 +25,34 @@ using namespace std; /* ************************************************************************* */ bool VariableIndex::equals(const VariableIndex& other, double tol) const { - if(this->nEntries_ == other.nEntries_ && this->nFactors_ == other.nFactors_) { - for(size_t var=0; var < max(this->index_.size(), other.index_.size()); ++var) - if(var >= this->index_.size() || var >= other.index_.size()) { - if(!((var >= this->index_.size() && other.index_[var].empty()) || - (var >= other.index_.size() && this->index_[var].empty()))) - return false; - } else if(this->index_[var] != other.index_[var]) - return false; - } else - return false; - return true; + return this->nEntries_ == other.nEntries_ && this->nFactors_ == other.nFactors_ + && this->index_ == other.index_; } /* ************************************************************************* */ -void VariableIndex::print(const string& str) const { +void VariableIndex::print(const string& str, const KeyFormatter& keyFormatter) const { cout << str; cout << "nEntries = " << nEntries() << ", nFactors = " << nFactors() << "\n"; - for(Index var = 0; var < size(); ++var) { - cout << "var " << var << ":"; - BOOST_FOREACH(const size_t factor, index_[var]) + BOOST_FOREACH(KeyMap::value_type key_factors, index_) { + cout << "var " << keyFormatter(key_factors.first) << ":"; + BOOST_FOREACH(const size_t factor, key_factors.second) cout << " " << factor; cout << "\n"; } - cout << flush; + cout.flush(); } /* ************************************************************************* */ void VariableIndex::outputMetisFormat(ostream& os) const { os << size() << " " << nFactors() << "\n"; // run over variables, which will be hyper-edges. - BOOST_FOREACH(const Factors& variable, index_) { + BOOST_FOREACH(KeyMap::value_type key_factors, index_) { // every variable is a hyper-edge covering its factors - BOOST_FOREACH(const size_t factor, variable) + BOOST_FOREACH(const size_t factor, key_factors.second) os << (factor+1) << " "; // base 1 os << "\n"; } os << flush; } -/* ************************************************************************* */ -void VariableIndex::permuteInPlace(const Permutation& permutation) { - // Create new index and move references to data into it in permuted order - vector newIndex(this->size()); - for(Index i = 0; i < newIndex.size(); ++i) - newIndex[i].swap(this->index_[permutation[i]]); - - // Move reference to entire index into the VariableIndex - index_.swap(newIndex); -} - -/* ************************************************************************* */ -void VariableIndex::permuteInPlace(const Permutation& selector, const Permutation& permutation) { - if(selector.size() != permutation.size()) - throw invalid_argument("VariableIndex::permuteInPlace (partial permutation version) called with selector and permutation of different sizes."); - // Create new index the size of the permuted entries - vector newIndex(selector.size()); - // Permute the affected entries into the new index - for(size_t dstSlot = 0; dstSlot < selector.size(); ++dstSlot) - newIndex[dstSlot].swap(this->index_[selector[permutation[dstSlot]]]); - // Put the affected entries back in the new order - for(size_t slot = 0; slot < selector.size(); ++slot) - this->index_[selector[slot]].swap(newIndex[slot]); -} - -/* ************************************************************************* */ -void VariableIndex::removeUnusedAtEnd(size_t nToRemove) { -#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS - for(size_t i = this->size() - nToRemove; i < this->size(); ++i) - if(!(*this)[i].empty()) - throw std::invalid_argument("Attempting to remove non-empty variables with VariableIndex::removeUnusedAtEnd()"); -#endif - - index_.resize(this->size() - nToRemove); -} - } diff --git a/gtsam/inference/VariableIndex.h b/gtsam/inference/VariableIndex.h index 570369e0a..3985221d3 100644 --- a/gtsam/inference/VariableIndex.h +++ b/gtsam/inference/VariableIndex.h @@ -12,7 +12,7 @@ /** * @file VariableIndex.h * @author Richard Roberts - * @date Sep 12, 2010 + * @date March 26, 2013 */ #pragma once @@ -23,13 +23,13 @@ #include #include -#include +#include +#include #include +#include namespace gtsam { - class Permutation; - /** * The VariableIndex class computes and stores the block column structure of a * factor graph. The factor graph stores a collection of factors, each of @@ -45,13 +45,19 @@ public: typedef boost::shared_ptr shared_ptr; typedef FastList Factors; typedef Factors::iterator Factor_iterator; - typedef Factors::const_iterator Factor_const_iterator; + typedef Factors::const_iterator Factor_const_iterator; protected: - std::vector index_; + typedef FastMap KeyMap; + KeyMap index_; size_t nFactors_; // Number of factors in the original factor graph. size_t nEntries_; // Sum of involved variable counts of each factor. +public: + typedef KeyMap::const_iterator const_iterator; + typedef KeyMap::const_iterator iterator; + typedef KeyMap::value_type value_type; + public: /// @name Standard Constructors @@ -60,18 +66,12 @@ public: /** Default constructor, creates an empty VariableIndex */ VariableIndex() : nFactors_(0), nEntries_(0) {} - /** - * Create a VariableIndex that computes and stores the block column structure - * of a factor graph. This constructor is used when the number of variables - * is known beforehand. - */ - template VariableIndex(const FG& factorGraph, Index nVariables); - /** * Create a VariableIndex that computes and stores the block column structure * of a factor graph. */ - template VariableIndex(const FG& factorGraph); + template + VariableIndex(const FG& factorGraph) : nFactors_(0), nEntries_(0) { augment(factorGraph); } /// @} /// @name Standard Interface @@ -81,7 +81,7 @@ public: * The number of variable entries. This is one greater than the variable * with the highest index. */ - Index size() const { return index_.size(); } + Key size() const { return index_.size(); } /** The number of factors in the original factor graph */ size_t nFactors() const { return nFactors_; } @@ -90,7 +90,13 @@ public: size_t nEntries() const { return nEntries_; } /** Access a list of factors by variable */ - const Factors& operator[](Index variable) const { checkVar(variable); return index_[variable]; } + const Factors& operator[](Key variable) const { + KeyMap::const_iterator item = index_.find(variable); + if(item == index_.end()) + throw std::invalid_argument("Requested non-existent variable from VariableIndex"); + else + return item->second; + } /// @} /// @name Testable @@ -100,7 +106,8 @@ public: bool equals(const VariableIndex& other, double tol=0.0) const; /** Print the variable index (for unit tests and debugging). */ - void print(const std::string& str = "VariableIndex: ") const; + void print(const std::string& str = "VariableIndex: ", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; /** * Output dual hypergraph to Metis file format for use with hmetis @@ -117,176 +124,57 @@ public: * Augment the variable index with new factors. This can be used when * solving problems incrementally. */ - template void augment(const FG& factors); + template + void augment(const FG& factors, boost::optional&> newFactorIndices = boost::none); /** - * Remove entries corresponding to the specified factors. - * NOTE: We intentionally do not decrement nFactors_ because the factor - * indices need to remain consistent. Removing factors from a factor graph - * does not shift the indices of other factors. Also, we keep nFactors_ - * one greater than the highest-numbered factor referenced in a VariableIndex. + * Remove entries corresponding to the specified factors. NOTE: We intentionally do not decrement + * nFactors_ because the factor indices need to remain consistent. Removing factors from a factor + * graph does not shift the indices of other factors. Also, we keep nFactors_ one greater than + * the highest-numbered factor referenced in a VariableIndex. * * @param indices The indices of the factors to remove, which must match \c factors - * @param factors The factors being removed, which must symbolically correspond - * exactly to the factors with the specified \c indices that were added. + * @param factors The factors being removed, which must symbolically correspond exactly to the + * factors with the specified \c indices that were added. */ - template void remove(const CONTAINER& indices, const FG& factors); + template + void remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG& factors); - /// Permute the variables in the VariableIndex according to the given permutation - void permuteInPlace(const Permutation& permutation); + /** Remove unused empty variables (in debug mode verifies they are empty). */ + template + void removeUnusedVariables(ITERATOR firstKey, ITERATOR lastKey); - /// Permute the variables in the VariableIndex according to the given partial permutation - void permuteInPlace(const Permutation& selector, const Permutation& permutation); + /** Iterator to the first variable entry */ + const_iterator begin() const { return index_.begin(); } - /** Remove unused empty variables at the end of the ordering (in debug mode - * verifies they are empty). - * @param nToRemove The number of unused variables at the end to remove - */ - void removeUnusedAtEnd(size_t nToRemove); + /** Iterator to the first variable entry */ + const_iterator end() const { return index_.end(); } + + /** Find the iterator for the requested variable entry */ + const_iterator find(Key key) const { return index_.find(key); } protected: - Factor_iterator factorsBegin(Index variable) { checkVar(variable); return index_[variable].begin(); } - Factor_iterator factorsEnd(Index variable) { checkVar(variable); return index_[variable].end(); } + Factor_iterator factorsBegin(Key variable) { return internalAt(variable).begin(); } + Factor_iterator factorsEnd(Key variable) { return internalAt(variable).end(); } - Factor_const_iterator factorsBegin(Index variable) const { checkVar(variable); return index_[variable].begin(); } - Factor_const_iterator factorsEnd(Index variable) const { checkVar(variable); return index_[variable].end(); } + Factor_const_iterator factorsBegin(Key variable) const { return internalAt(variable).begin(); } + Factor_const_iterator factorsEnd(Key variable) const { return internalAt(variable).end(); } - /// Internal constructor to allocate a VariableIndex of the requested size - VariableIndex(size_t nVars) : index_(nVars), nFactors_(0), nEntries_(0) {} + /// Internal version of 'at' that asserts existence + const Factors& internalAt(Key variable) const { + const KeyMap::const_iterator item = index_.find(variable); + assert(item != index_.end()); + return item->second; } - /// Internal check of the validity of a variable - void checkVar(Index variable) const { assert(variable < index_.size()); } - - /// Internal function to populate the variable index from a factor graph - template void fill(const FG& factorGraph); + /// Internal version of 'at' that asserts existence + Factors& internalAt(Key variable) { + const KeyMap::iterator item = index_.find(variable); + assert(item != index_.end()); + return item->second; } /// @} - -private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(index_); - ar & BOOST_SERIALIZATION_NVP(nFactors_); - ar & BOOST_SERIALIZATION_NVP(nEntries_); - } }; -/* ************************************************************************* */ -template -void VariableIndex::fill(const FG& factorGraph) { - gttic(VariableIndex_fill); - // Build index mapping from variable id to factor index - for(size_t fi=0; fikeys()) { - if(key < index_.size()) { - index_[key].push_back(fi); - ++ nEntries_; - } - } - } - ++ nFactors_; // Increment factor count even if factors are null, to keep indices consistent - } } -/* ************************************************************************* */ -template -VariableIndex::VariableIndex(const FG& factorGraph) : - nFactors_(0), nEntries_(0) -{ - gttic(VariableIndex_constructor); - // If the factor graph is empty, return an empty index because inside this - // if block we assume at least one factor. - if(factorGraph.size() > 0) { - gttic(VariableIndex_constructor_find_highest); - // Find highest-numbered variable - Index maxVar = 0; - BOOST_FOREACH(const typename FG::sharedFactor& factor, factorGraph) { - if(factor) { - BOOST_FOREACH(const Index key, factor->keys()) { - if(key > maxVar) - maxVar = key; - } - } - } - gttoc(VariableIndex_constructor_find_highest); - - // Allocate array - gttic(VariableIndex_constructor_allocate); - index_.resize(maxVar+1); - gttoc(VariableIndex_constructor_allocate); - - fill(factorGraph); - } -} - -/* ************************************************************************* */ -template -VariableIndex::VariableIndex(const FG& factorGraph, Index nVariables) : - nFactors_(0), nEntries_(0) -{ - gttic(VariableIndex_constructor_allocate); - index_.resize(nVariables); - gttoc(VariableIndex_constructor_allocate); - fill(factorGraph); -} - -/* ************************************************************************* */ -template -void VariableIndex::augment(const FG& factors) { - gttic(VariableIndex_augment); - // If the factor graph is empty, return an empty index because inside this - // if block we assume at least one factor. - if(factors.size() > 0) { - // Find highest-numbered variable - Index maxVar = 0; - BOOST_FOREACH(const typename FG::sharedFactor& factor, factors) { - if(factor) { - BOOST_FOREACH(const Index key, factor->keys()) { - if(key > maxVar) - maxVar = key; - } - } - } - - // Allocate index - index_.resize(std::max(index_.size(), maxVar+1)); - - // Augment index mapping from variable id to factor index - size_t orignFactors = nFactors_; - for(size_t fi=0; fikeys()) { - index_[key].push_back(orignFactors + fi); - ++ nEntries_; - } - } - ++ nFactors_; // Increment factor count even if factors are null, to keep indices consistent - } - } -} - -/* ************************************************************************* */ -template -void VariableIndex::remove(const CONTAINER& indices, const FG& factors) { - gttic(VariableIndex_remove); - // NOTE: We intentionally do not decrement nFactors_ because the factor - // indices need to remain consistent. Removing factors from a factor graph - // does not shift the indices of other factors. Also, we keep nFactors_ - // one greater than the highest-numbered factor referenced in a VariableIndex. - for(size_t fi=0; fikeys().size(); ++ji) { - Factors& factorEntries = index_[factors[fi]->keys()[ji]]; - Factors::iterator entry = std::find(factorEntries.begin(), factorEntries.end(), indices[fi]); - if(entry == factorEntries.end()) - throw std::invalid_argument("Internal error, indices and factors passed into VariableIndex::remove are not consistent with the existing variable index"); - factorEntries.erase(entry); - -- nEntries_; - } - } -} - -} +#include diff --git a/gtsam/inference/VariableSlots.cpp b/gtsam/inference/VariableSlots.cpp index a06aad6cb..783071ae2 100644 --- a/gtsam/inference/VariableSlots.cpp +++ b/gtsam/inference/VariableSlots.cpp @@ -24,6 +24,8 @@ using namespace std; namespace gtsam { +const size_t VariableSlots::Empty = numeric_limits::max(); + /** print */ void VariableSlots::print(const std::string& str) const { if(this->empty()) @@ -37,7 +39,7 @@ void VariableSlots::print(const std::string& str) const { for(size_t i=0; ibegin()->second.size(); ++i) { cout << " \t"; BOOST_FOREACH(const value_type& slot, *this) { - if(slot.second[i] == numeric_limits::max()) + if(slot.second[i] == Empty) cout << "x" << "\t"; else cout << slot.second[i] << "\t"; diff --git a/gtsam/inference/VariableSlots.h b/gtsam/inference/VariableSlots.h index 28367a647..9a16ca788 100644 --- a/gtsam/inference/VariableSlots.h +++ b/gtsam/inference/VariableSlots.h @@ -11,15 +11,17 @@ /** * @file VariableSlots.h - * @brief VariableSlots describes the structure of a combined factor in terms of where each block comes from in the source factors. + * @brief VariableSlots describes the structure of a combined factor in terms of where each block + * comes from in the source factors. * @author Richard Roberts - * @date Oct 4, 2010 - */ + * @date Oct 4, 2010 */ #pragma once #include #include +#include +#include #include @@ -27,35 +29,33 @@ #include #include -#include namespace gtsam { -/** - * A combined factor is assembled as one block of rows for each component - * factor. In each row-block (factor), some of the column-blocks (variables) - * may be empty since factors involving different sets of variables are - * interleaved. - * - * VariableSlots describes the 2D block structure of the combined factor. It - * is essentially a map >. The Index is the real - * variable index of the combined factor slot. The vector tells, for - * each row-block (factor), which column-block (variable slot) from the - * component factor appears in this block of the combined factor. - * - * As an example, if the combined factor contains variables 1, 3, and 5, then - * "variableSlots[3][2] == 0" indicates that column-block 1 (corresponding to - * variable index 3), row-block 2 (also meaning component factor 2), comes from - * column-block 0 of component factor 2. - * - * \nosubgrouping - */ +/** A combined factor is assembled as one block of rows for each component +* factor. In each row-block (factor), some of the column-blocks (variables) +* may be empty since factors involving different sets of variables are +* interleaved. +* +* VariableSlots describes the 2D block structure of the combined factor. It +* is a map >. The Key is the real +* variable index of the combined factor slot. The vector tells, for +* each row-block (factor), which column-block (variable slot) from the +* component factor appears in this block of the combined factor. +* +* As an example, if the combined factor contains variables 1, 3, and 5, then +* "variableSlots[3][2] == 0" indicates that column-block 1 (corresponding to +* variable index 3), row-block 2 (also meaning component factor 2), comes from +* column-block 0 of component factor 2. +* +* \nosubgrouping */ -class VariableSlots : public FastMap > { +class VariableSlots : public FastMap > { public: - typedef FastMap > Base; + typedef FastMap > Base; + GTSAM_EXPORT static const size_t Empty; /// @name Standard Constructors /// @{ @@ -69,6 +69,7 @@ public: VariableSlots(const FG& factorGraph); /// @} + /// @name Testable /// @{ @@ -79,12 +80,13 @@ public: GTSAM_EXPORT bool equals(const VariableSlots& rhs, double tol = 0.0) const; /// @} - }; /* ************************************************************************* */ template -VariableSlots::VariableSlots(const FG& factorGraph) { +VariableSlots::VariableSlots(const FG& factorGraph) +{ + gttic(VariableSlots_constructor); static const bool debug = false; // Compute a mapping (called variableSlots) *from* each involved @@ -96,8 +98,8 @@ VariableSlots::VariableSlots(const FG& factorGraph) { size_t jointFactorPos = 0; BOOST_FOREACH(const typename FG::sharedFactor& factor, factorGraph) { assert(factor); - Index factorVarSlot = 0; - BOOST_FOREACH(const Index involvedVariable, *factor) { + size_t factorVarSlot = 0; + BOOST_FOREACH(const Key involvedVariable, *factor) { // Set the slot in this factor for this variable. If the // variable was not already discovered, create an array for it // that we'll fill with the slot indices for each factor that @@ -105,9 +107,9 @@ VariableSlots::VariableSlots(const FG& factorGraph) { // the array entry for each factor that will indicate the factor // does not involve the variable. iterator thisVarSlots; bool inserted; - boost::tie(thisVarSlots, inserted) = this->insert(make_pair(involvedVariable, std::vector())); + boost::tie(thisVarSlots, inserted) = this->insert(std::make_pair(involvedVariable, FastVector())); if(inserted) - thisVarSlots->second.resize(factorGraph.size(), std::numeric_limits::max()); + thisVarSlots->second.resize(factorGraph.size(), Empty); thisVarSlots->second[jointFactorPos] = factorVarSlot; if(debug) std::cout << " var " << involvedVariable << " rowblock " << jointFactorPos << " comes from factor's slot " << factorVarSlot << std::endl; ++ factorVarSlot; diff --git a/gtsam/inference/inference-inl.h b/gtsam/inference/inference-inl.h deleted file mode 100644 index 5c5cedbc3..000000000 --- a/gtsam/inference/inference-inl.h +++ /dev/null @@ -1,88 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 inference-inl.h - * @brief - * @author Richard Roberts - * @date Mar 3, 2012 - */ - -#pragma once - -#include - -// Only for Eclipse parser, inference-inl.h (this file) is included at the bottom of inference.h -#include - -#include - -namespace gtsam { - -namespace inference { - -/* ************************************************************************* */ -template -Permutation::shared_ptr PermutationCOLAMD( - const VariableIndex& variableIndex, const CONSTRAINED& constrainLast, bool forceOrder) { - gttic(PermutationCOLAMD_constrained); - - size_t n = variableIndex.size(); - std::vector cmember(n, 0); - - // If at least some variables are not constrained to be last, constrain the - // ones that should be constrained. - if(constrainLast.size() < n) { - BOOST_FOREACH(Index var, constrainLast) { - assert(var < n); - cmember[var] = 1; - } - } - - Permutation::shared_ptr permutation = PermutationCOLAMD_(variableIndex, cmember); - if (forceOrder) { - Index j=n; - BOOST_REVERSE_FOREACH(Index c, constrainLast) - permutation->operator[](--j) = c; - } - return permutation; -} - -/* ************************************************************************* */ -template -Permutation::shared_ptr PermutationCOLAMDGrouped( - const VariableIndex& variableIndex, const CONSTRAINED_MAP& constraints) { - gttic(PermutationCOLAMD_grouped); - size_t n = variableIndex.size(); - std::vector cmember(n, 0); - - typedef typename CONSTRAINED_MAP::value_type constraint_pair; - BOOST_FOREACH(const constraint_pair& p, constraints) { - assert(p.first < n); - // FIXME: check that no groups are skipped - cmember[p.first] = p.second; - } - - return PermutationCOLAMD_(variableIndex, cmember); -} - -/* ************************************************************************* */ -inline Permutation::shared_ptr PermutationCOLAMD(const VariableIndex& variableIndex) { - gttic(PermutationCOLAMD_unconstrained); - size_t n = variableIndex.size(); - std::vector cmember(n, 0); - return PermutationCOLAMD_(variableIndex, cmember); -} - -} // namespace inference -} // namespace gtsam - - diff --git a/gtsam/inference/inference-inst.h b/gtsam/inference/inference-inst.h new file mode 100644 index 000000000..434b705f2 --- /dev/null +++ b/gtsam/inference/inference-inst.h @@ -0,0 +1,93 @@ +/* ---------------------------------------------------------------------------- + +* 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 inference-inst.h +* @brief Contains *generic* inference algorithms that convert between templated +* graphical models, i.e., factor graphs, Bayes nets, and Bayes trees +* @author Frank Dellaert +* @author Richard Roberts +*/ + +#pragma once + +#include +#include +#include + +#include +#include + +namespace gtsam { + namespace inference { + + namespace { + /* ************************************************************************* */ + template + struct EliminationData { + EliminationData* const parentData; + FastVector childFactors; + EliminationData(EliminationData* _parentData, size_t nChildren) : + parentData(_parentData) { childFactors.reserve(nChildren); } + }; + + /* ************************************************************************* */ + template + EliminationData eliminationPreOrderVisitor( + const typename TREE::sharedNode& node, EliminationData& parentData) + { + // This function is called before visiting the children. Here, we create this node's data, + // which includes a pointer to the parent data and space for the factors of the children. + return EliminationData(&parentData, node->children.size()); + } + + /* ************************************************************************* */ + template + struct EliminationPostOrderVisitor + { + RESULT& result; + const typename TREE::Eliminate& eliminationFunction; + EliminationPostOrderVisitor(RESULT& result, const typename TREE::Eliminate& eliminationFunction) : + result(result), eliminationFunction(eliminationFunction) {} + void operator()(const typename TREE::sharedNode& node, EliminationData& myData) + { + // Call eliminate on the node and add the result to the parent's gathered factors + typename TREE::sharedFactor childFactor = node->eliminate(result, eliminationFunction, myData.childFactors); + if(!childFactor->empty()) + myData.parentData->childFactors.push_back(childFactor); + } + }; + } + + /* ************************************************************************* */ + /** Eliminate an elimination tree or a Bayes tree (used internally). Requires + * TREE::BayesNetType, TREE::FactorGraphType, TREE::sharedConditional, TREE::sharedFactor, + * TREE::Node, TREE::sharedNode, TREE::Node::factors, TREE::Node::children. */ + template + FastVector + EliminateTree(RESULT& result, const TREE& tree, const typename TREE::Eliminate& function) + { + // Do elimination using a depth-first traversal. During the pre-order visit (see + // eliminationPreOrderVisitor), we store a pointer to the parent data (where we'll put the + // remaining factor) and reserve a vector of factors to store the children elimination + // results. During the post-order visit (see eliminationPostOrderVisitor), we call dense + // elimination (using the gathered child factors) and store the result in the parent's + // gathered factors. + EliminationData rootData(0, tree.roots().size()); + EliminationPostOrderVisitor visitorPost(result, function); + treeTraversal::DepthFirstForest(tree, rootData, eliminationPreOrderVisitor, visitorPost); + + // Return remaining factors + return rootData.childFactors; + } + + } +} diff --git a/gtsam/inference/inference.cpp b/gtsam/inference/inference.cpp deleted file mode 100644 index 2704e8c26..000000000 --- a/gtsam/inference/inference.cpp +++ /dev/null @@ -1,106 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 inference.cpp - * @brief inference definitions - * @author Frank Dellaert - * @author Richard Roberts - */ - -#include -#include - -#include -#include -#include -#include - -#include - -using namespace std; - -namespace gtsam { -namespace inference { - -/* ************************************************************************* */ -Permutation::shared_ptr PermutationCOLAMD_(const VariableIndex& variableIndex, std::vector& cmember) { - gttic(PermutationCOLAMD_internal); - - 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!) - int 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 */ - - static const bool debug = false; - - p[0] = 0; - int count = 0; - for(Index var = 0; var < variableIndex.size(); ++var) { - const VariableIndex::Factors& column(variableIndex[var]); - 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 - if(debug) cout << "A[" << count-1 << "] = " << factorIndex << endl; - } - p[var+1] = count; // column j (base 1) goes from A[j-1] to A[j]-1 - } - - assert((size_t)count == variableIndex.nEntries()); - - if(debug) - for(size_t i=0; ioperator[](j) = j; - // else - permutation->operator[](j) = p[j]; - if(debug) cout << "COLAMD: " << j << "->" << p[j] << endl; - } - if(debug) cout << "COLAMD: p[" << nVars << "] = " << p[nVars] << endl; - gttoc(Create_permutation); - - return permutation; -} - -/* ************************************************************************* */ -} // \namespace inference -} // \namespace gtsam diff --git a/gtsam/inference/inference.h b/gtsam/inference/inference.h deleted file mode 100644 index 099887ab6..000000000 --- a/gtsam/inference/inference.h +++ /dev/null @@ -1,82 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 inference.h - * @brief Contains *generic* inference algorithms that convert between templated - * graphical models, i.e., factor graphs, Bayes nets, and Bayes trees - * @author Frank Dellaert - * @author Richard Roberts - */ - -#pragma once - -#include -#include - -#include -#include - -#include - -namespace gtsam { - - namespace inference { - - /** - * Compute a permutation (variable ordering) using colamd - */ - GTSAM_EXPORT Permutation::shared_ptr PermutationCOLAMD( - const VariableIndex& variableIndex); - - /** - * Compute a permutation (variable ordering) using constrained colamd to move - * a set of variables to the end of the ordering - * @param variableIndex is the variable index lookup from a graph - * @param constrainlast is a vector of keys that should be constrained - * @tparam constrainLast is a std::vector (or similar structure) - * @param forceOrder if true, will not allow re-ordering of constrained variables - */ - template - Permutation::shared_ptr PermutationCOLAMD( - const VariableIndex& variableIndex, const CONSTRAINED& constrainLast, bool forceOrder=false); - - /** - * Compute a permutation of variable ordering using constrained colamd to - * move variables to the end in groups (0 = unconstrained, higher numbers at - * the end). - * @param variableIndex is the variable index lookup from a graph - * @param constraintMap is a map from variable index -> group number for constrained variables - * @tparam CONSTRAINED_MAP is an associative structure (like std::map), from size_t->int - */ - template - Permutation::shared_ptr PermutationCOLAMDGrouped( - const VariableIndex& variableIndex, const CONSTRAINED_MAP& constraints); - - /** - * Compute a CCOLAMD permutation using the constraint groups in cmember. - * The format for cmember is a part of ccolamd. - * - * @param variableIndex is the variable structure from a graph - * @param cmember is the constraint group list for each variable, where - * 0 is the default, unconstrained group, and higher numbers move further to - * the back of the list - * - * AGC: does cmember change? - */ - GTSAM_EXPORT Permutation::shared_ptr PermutationCOLAMD_( - const VariableIndex& variableIndex, std::vector& cmember); - - } // \namespace inference - -} // \namespace gtsam - -#include diff --git a/gtsam/inference/inferenceExceptions.h b/gtsam/inference/inferenceExceptions.h new file mode 100644 index 000000000..edd0e0aa5 --- /dev/null +++ b/gtsam/inference/inferenceExceptions.h @@ -0,0 +1,42 @@ +/* ---------------------------------------------------------------------------- + + * 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 inferenceExceptions.h + * @brief Exceptions that may be thrown by inference algorithms + * @author Richard Roberts + * @date Apr 25, 2013 + */ +#pragma once + +#include +#include +#include + +namespace gtsam { + + /** An inference algorithm was called with inconsistent arguments. The factor graph, ordering, or + * variable index were inconsistent with each other, or a full elimination routine was called + * with an ordering that does not include all of the variables. */ + class InconsistentEliminationRequested : public std::exception { + public: + InconsistentEliminationRequested() throw() {} + virtual ~InconsistentEliminationRequested() throw() {} + virtual const char* what() const throw() { + return + "An inference algorithm was called with inconsistent arguments. The\n" + "factor graph, ordering, or variable index were inconsistent with each\n" + "other, or a full elimination routine was called with an ordering that\n" + "does not include all of the variables."; + } + }; + +} diff --git a/gtsam/inference/tests/CMakeLists.txt b/gtsam/inference/tests/CMakeLists.txt new file mode 100644 index 000000000..aaa01d775 --- /dev/null +++ b/gtsam/inference/tests/CMakeLists.txt @@ -0,0 +1 @@ +gtsamAddTestsGlob(inference "test*.cpp" "" "gtsam") diff --git a/gtsam/inference/tests/testBayesTree.cpp b/gtsam/inference/tests/testBayesTree.cpp deleted file mode 100644 index 4943dedb4..000000000 --- a/gtsam/inference/tests/testBayesTree.cpp +++ /dev/null @@ -1,561 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 testBayesTree.cpp - * @brief Unit tests for Bayes Tree - * @author Frank Dellaert - * @author Michael Kaess - * @author Viorela Ila - */ - -#include // for operator += -#include -#include -#include -using namespace boost::assign; - -#include -#include - -#include -#include -#include -#include - -using namespace std; -using namespace gtsam; - -///* ************************************************************************* */ -//// SLAM example from RSS sqrtSAM paper -static const Index _x3_=0, _x2_=1; -//static const Index _x1_=2, _l2_=3, _l1_=4; // unused -//IndexConditional::shared_ptr -// x3(new IndexConditional(_x3_)), -// x2(new IndexConditional(_x2_,_x3_)), -// x1(new IndexConditional(_x1_,_x2_,_x3_)), -// l1(new IndexConditional(_l1_,_x1_,_x2_)), -// l2(new IndexConditional(_l2_,_x1_,_x3_)); -// -//// Bayes Tree for sqrtSAM example -//SymbolicBayesTree createSlamSymbolicBayesTree(){ -// // Create using insert -//// Ordering slamOrdering; slamOrdering += _x3_, _x2_, _x1_, _l2_, _l1_; -// SymbolicBayesTree bayesTree_slam; -// bayesTree_slam.insert(x3); -// bayesTree_slam.insert(x2); -// bayesTree_slam.insert(x1); -// bayesTree_slam.insert(l2); -// bayesTree_slam.insert(l1); -// return bayesTree_slam; -//} - -/* ************************************************************************* */ -// Conditionals for ASIA example from the tutorial with A and D evidence -static const Index _X_=0, _T_=1, _S_=2, _E_=3, _L_=4, _B_=5; -static IndexConditional::shared_ptr - B(new IndexConditional(_B_)), - L(new IndexConditional(_L_, _B_)), - E(new IndexConditional(_E_, _L_, _B_)), - S(new IndexConditional(_S_, _L_, _B_)), - T(new IndexConditional(_T_, _E_, _L_)), - X(new IndexConditional(_X_, _E_)); - -// Cliques -static IndexConditional::shared_ptr - ELB(IndexConditional::FromKeys(cref_list_of<3>(_E_)(_L_)(_B_), 3)); - -// Bayes Tree for Asia example -static SymbolicBayesTree createAsiaSymbolicBayesTree() { - SymbolicBayesTree bayesTree; -// Ordering asiaOrdering; asiaOrdering += _X_, _T_, _S_, _E_, _L_, _B_; - SymbolicBayesTree::insert(bayesTree, B); - SymbolicBayesTree::insert(bayesTree, L); - SymbolicBayesTree::insert(bayesTree, E); - SymbolicBayesTree::insert(bayesTree, S); - SymbolicBayesTree::insert(bayesTree, T); - SymbolicBayesTree::insert(bayesTree, X); - return bayesTree; -} - -/* ************************************************************************* */ -TEST( BayesTree, constructor ) -{ - // Create using insert - SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree(); - - // Check Size - LONGS_EQUAL(4,bayesTree.size()); - EXPECT(!bayesTree.empty()); - - // Check root - boost::shared_ptr actual_root = bayesTree.root()->conditional(); - CHECK(assert_equal(*ELB,*actual_root)); - - // Create from symbolic Bayes chain in which we want to discover cliques - BayesNet ASIA; - ASIA.push_back(X); - ASIA.push_back(T); - ASIA.push_back(S); - ASIA.push_back(E); - ASIA.push_back(L); - ASIA.push_back(B); - SymbolicBayesTree bayesTree2(ASIA); - - // Check whether the same - CHECK(assert_equal(bayesTree,bayesTree2)); - - // CHECK findParentClique, should *not depend on order of parents* -// Ordering ordering; ordering += _X_, _T_, _S_, _E_, _L_, _B_; -// IndexTable index(ordering); - - list parents1; parents1 += _E_, _L_; - CHECK(assert_equal(_E_, bayesTree.findParentClique(parents1))); - - list parents2; parents2 += _L_, _E_; - CHECK(assert_equal(_E_, bayesTree.findParentClique(parents2))); - - list parents3; parents3 += _L_, _B_; - CHECK(assert_equal(_L_, bayesTree.findParentClique(parents3))); -} - -/* ************************************************************************* */ -TEST(BayesTree, clear) -{ -// SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree(); -// bayesTree.clear(); -// -// SymbolicBayesTree expected; -// -// // Check whether cleared BayesTree is equal to a new BayesTree -// CHECK(assert_equal(expected, bayesTree)); -} - -/* ************************************************************************* * -Bayes Tree for testing conversion to a forest of orphans needed for incremental. - A,B - C|A E|B - D|C F|E - */ -/* ************************************************************************* */ -TEST( BayesTree, removePath ) -{ - const Index _A_=5, _B_=4, _C_=3, _D_=2, _E_=1, _F_=0; - IndexConditional::shared_ptr - A(new IndexConditional(_A_)), - B(new IndexConditional(_B_, _A_)), - C(new IndexConditional(_C_, _A_)), - D(new IndexConditional(_D_, _C_)), - E(new IndexConditional(_E_, _B_)), - F(new IndexConditional(_F_, _E_)); - SymbolicBayesTree bayesTree; - EXPECT(bayesTree.empty()); -// Ordering ord; ord += _A_,_B_,_C_,_D_,_E_,_F_; - SymbolicBayesTree::insert(bayesTree, A); - SymbolicBayesTree::insert(bayesTree, B); - SymbolicBayesTree::insert(bayesTree, C); - SymbolicBayesTree::insert(bayesTree, D); - SymbolicBayesTree::insert(bayesTree, E); - SymbolicBayesTree::insert(bayesTree, F); - - // remove C, expected outcome: factor graph with ABC, - // Bayes Tree now contains two orphan trees: D|C and E|B,F|E - SymbolicFactorGraph expected; - expected.push_factor(_B_,_A_); -// expected.push_factor(_A_); - expected.push_factor(_C_,_A_); - SymbolicBayesTree::Cliques expectedOrphans; - expectedOrphans += bayesTree[_D_], bayesTree[_E_]; - - BayesNet bn; - SymbolicBayesTree::Cliques orphans; - bayesTree.removePath(bayesTree[_C_], bn, orphans); - SymbolicFactorGraph factors(bn); - CHECK(assert_equal((SymbolicFactorGraph)expected, factors)); - CHECK(assert_equal(expectedOrphans, orphans)); - - // remove E: factor graph with EB; E|B removed from second orphan tree - SymbolicFactorGraph expected2; - expected2.push_factor(_E_,_B_); - SymbolicBayesTree::Cliques expectedOrphans2; - expectedOrphans2 += bayesTree[_F_]; - - BayesNet bn2; - SymbolicBayesTree::Cliques orphans2; - bayesTree.removePath(bayesTree[_E_], bn2, orphans2); - SymbolicFactorGraph factors2(bn2); - CHECK(assert_equal((SymbolicFactorGraph)expected2, factors2)); - CHECK(assert_equal(expectedOrphans2, orphans2)); -} - -/* ************************************************************************* */ -TEST( BayesTree, removePath2 ) -{ - SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree(); - - // Call remove-path with clique B - BayesNet bn; - SymbolicBayesTree::Cliques orphans; - bayesTree.removePath(bayesTree[_B_], bn, orphans); - SymbolicFactorGraph factors(bn); - - // Check expected outcome - SymbolicFactorGraph expected; - expected.push_factor(_E_,_L_,_B_); -// expected.push_factor(_L_,_B_); -// expected.push_factor(_B_); - CHECK(assert_equal(expected, factors)); - SymbolicBayesTree::Cliques expectedOrphans; - expectedOrphans += bayesTree[_S_], bayesTree[_T_], bayesTree[_X_]; - CHECK(assert_equal(expectedOrphans, orphans)); -} - -/* ************************************************************************* */ -TEST( BayesTree, removePath3 ) -{ - SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree(); - - // Call remove-path with clique S - BayesNet bn; - SymbolicBayesTree::Cliques orphans; - bayesTree.removePath(bayesTree[_S_], bn, orphans); - SymbolicFactorGraph factors(bn); - - // Check expected outcome - SymbolicFactorGraph expected; - expected.push_factor(_E_,_L_,_B_); -// expected.push_factor(_L_,_B_); -// expected.push_factor(_B_); - expected.push_factor(_S_,_L_,_B_); - CHECK(assert_equal(expected, factors)); - SymbolicBayesTree::Cliques expectedOrphans; - expectedOrphans += bayesTree[_T_], bayesTree[_X_]; - CHECK(assert_equal(expectedOrphans, orphans)); -} - -void getAllCliques(const SymbolicBayesTree::sharedClique& subtree, SymbolicBayesTree::Cliques& cliques) { - // Check if subtree exists - if (subtree) { - cliques.push_back(subtree); - // Recursive call over all child cliques - BOOST_FOREACH(SymbolicBayesTree::sharedClique& childClique, subtree->children()) { - getAllCliques(childClique,cliques); - } - } -} - -/* ************************************************************************* */ -TEST( BayesTree, shortcutCheck ) -{ - const Index _A_=6, _B_=5, _C_=4, _D_=3, _E_=2, _F_=1, _G_=0; - IndexConditional::shared_ptr - A(new IndexConditional(_A_)), - B(new IndexConditional(_B_, _A_)), - C(new IndexConditional(_C_, _A_)), - D(new IndexConditional(_D_, _C_)), - E(new IndexConditional(_E_, _B_)), - F(new IndexConditional(_F_, _E_)), - G(new IndexConditional(_G_, _F_)); - SymbolicBayesTree bayesTree; -// Ordering ord; ord += _A_,_B_,_C_,_D_,_E_,_F_; - SymbolicBayesTree::insert(bayesTree, A); - SymbolicBayesTree::insert(bayesTree, B); - SymbolicBayesTree::insert(bayesTree, C); - SymbolicBayesTree::insert(bayesTree, D); - SymbolicBayesTree::insert(bayesTree, E); - SymbolicBayesTree::insert(bayesTree, F); - SymbolicBayesTree::insert(bayesTree, G); - - //bayesTree.print("BayesTree"); - //bayesTree.saveGraph("BT1.dot"); - - SymbolicBayesTree::sharedClique rootClique= bayesTree.root(); - //rootClique->printTree(); - SymbolicBayesTree::Cliques allCliques; - getAllCliques(rootClique,allCliques); - - BayesNet bn; - BOOST_FOREACH(SymbolicBayesTree::sharedClique& clique, allCliques) { - //clique->print("Clique#"); - bn = clique->shortcut(rootClique, &EliminateSymbolic); - //bn.print("Shortcut:\n"); - //cout << endl; - } - - // Check if all the cached shortcuts are cleared - rootClique->deleteCachedShortcuts(); - BOOST_FOREACH(SymbolicBayesTree::sharedClique& clique, allCliques) { - bool notCleared = clique->cachedSeparatorMarginal(); - CHECK( notCleared == false); - } - EXPECT_LONGS_EQUAL(0, rootClique->numCachedSeparatorMarginals()); - -// BOOST_FOREACH(SymbolicBayesTree::sharedClique& clique, allCliques) { -// clique->print("Clique#"); -// if(clique->cachedShortcut()){ -// bn = clique->cachedShortcut().get(); -// bn.print("Shortcut:\n"); -// } -// else -// cout << "Not Initialized" << endl; -// cout << endl; -// } -} - -/* ************************************************************************* */ -TEST( BayesTree, removeTop ) -{ - SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree(); - - // create a new factor to be inserted - boost::shared_ptr newFactor(new IndexFactor(_S_,_B_)); - - // Remove the contaminated part of the Bayes tree - BayesNet bn; - SymbolicBayesTree::Cliques orphans; - list keys; keys += _B_,_S_; - bayesTree.removeTop(keys, bn, orphans); - SymbolicFactorGraph factors(bn); - - // Check expected outcome - SymbolicFactorGraph expected; - expected.push_factor(_E_,_L_,_B_); -// expected.push_factor(_L_,_B_); -// expected.push_factor(_B_); - expected.push_factor(_S_,_L_,_B_); - CHECK(assert_equal(expected, factors)); - SymbolicBayesTree::Cliques expectedOrphans; - expectedOrphans += bayesTree[_T_], bayesTree[_X_]; - CHECK(assert_equal(expectedOrphans, orphans)); - - // Try removeTop again with a factor that should not change a thing - boost::shared_ptr newFactor2(new IndexFactor(_B_)); - BayesNet bn2; - SymbolicBayesTree::Cliques orphans2; - keys.clear(); keys += _B_; - bayesTree.removeTop(keys, bn2, orphans2); - SymbolicFactorGraph factors2(bn2); - SymbolicFactorGraph expected2; - CHECK(assert_equal(expected2, factors2)); - SymbolicBayesTree::Cliques expectedOrphans2; - CHECK(assert_equal(expectedOrphans2, orphans2)); -} - -/* ************************************************************************* */ -TEST( BayesTree, removeTop2 ) -{ - SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree(); - - // create two factors to be inserted - SymbolicFactorGraph newFactors; - newFactors.push_factor(_B_); - newFactors.push_factor(_S_); - - // Remove the contaminated part of the Bayes tree - BayesNet bn; - SymbolicBayesTree::Cliques orphans; - list keys; keys += _B_,_S_; - bayesTree.removeTop(keys, bn, orphans); - SymbolicFactorGraph factors(bn); - - // Check expected outcome - SymbolicFactorGraph expected; - expected.push_factor(_E_,_L_,_B_); -// expected.push_factor(_L_,_B_); -// expected.push_factor(_B_); - expected.push_factor(_S_,_L_,_B_); - CHECK(assert_equal(expected, factors)); - SymbolicBayesTree::Cliques expectedOrphans; - expectedOrphans += bayesTree[_T_], bayesTree[_X_]; - CHECK(assert_equal(expectedOrphans, orphans)); -} - -/* ************************************************************************* */ -TEST( BayesTree, removeTop3 ) -{ - const Index _x4_=5, _l5_=6; - // simple test case that failed after COLAMD was fixed/activated - IndexConditional::shared_ptr - X(new IndexConditional(_l5_)), - A(new IndexConditional(_x4_, _l5_)), - B(new IndexConditional(_x2_, _x4_)), - C(new IndexConditional(_x3_, _x2_)); - -// Ordering newOrdering; -// newOrdering += _x3_, _x2_, _x1_, _l2_, _l1_, _x4_, _l5_; - SymbolicBayesTree bayesTree; - SymbolicBayesTree::insert(bayesTree, X); - SymbolicBayesTree::insert(bayesTree, A); - SymbolicBayesTree::insert(bayesTree, B); - SymbolicBayesTree::insert(bayesTree, C); - - // remove all - list keys; - keys += _l5_, _x2_, _x3_, _x4_; - BayesNet bn; - SymbolicBayesTree::Cliques orphans; - bayesTree.removeTop(keys, bn, orphans); - SymbolicFactorGraph factors(bn); - - CHECK(orphans.size() == 0); -} - -/* ************************************************************************* */ -TEST( BayesTree, permute ) -{ - // creates a permutation and ensures that the nodes listing is updated - - // initial keys - more than just 6 variables - for a system with 9 variables - const Index _A0_=8, _B0_=7, _C0_=6, _D0_=5, _E0_=4, _F0_=0; - - // reduced keys - back to just 6 variables - const Index _A_=5, _B_=4, _C_=3, _D_=2, _E_=1, _F_=0; - - // Create and verify the permutation - std::set indices; indices += _A0_, _B0_, _C0_, _D0_, _E0_, _F0_; - Permutation actReducingPermutation = gtsam::internal::createReducingPermutation(indices); - Permutation expReducingPermutation(6); - expReducingPermutation[_A_] = _A0_; - expReducingPermutation[_B_] = _B0_; - expReducingPermutation[_C_] = _C0_; - expReducingPermutation[_D_] = _D0_; - expReducingPermutation[_E_] = _E0_; - expReducingPermutation[_F_] = _F0_; - EXPECT(assert_equal(expReducingPermutation, actReducingPermutation)); - - // Invert the permutation - gtsam::internal::Reduction inv_reduction = gtsam::internal::Reduction::CreateAsInverse(expReducingPermutation); - - // Build a bayes tree around reduced keys as if just eliminated from subset of factors/variables - IndexConditional::shared_ptr - A(new IndexConditional(_A_)), - B(new IndexConditional(_B_, _A_)), - C(new IndexConditional(_C_, _A_)), - D(new IndexConditional(_D_, _C_)), - E(new IndexConditional(_E_, _B_)), - F(new IndexConditional(_F_, _E_)); - SymbolicBayesTree bayesTreeReduced; - SymbolicBayesTree::insert(bayesTreeReduced, A); - SymbolicBayesTree::insert(bayesTreeReduced, B); - SymbolicBayesTree::insert(bayesTreeReduced, C); - SymbolicBayesTree::insert(bayesTreeReduced, D); - SymbolicBayesTree::insert(bayesTreeReduced, E); - SymbolicBayesTree::insert(bayesTreeReduced, F); - -// bayesTreeReduced.print("Reduced bayes tree"); -// P( 4 5) -// P( 3 | 5) -// P( 2 | 3) -// P( 1 | 4) -// P( 0 | 1) - - // Apply the permutation - should add placeholders for variables not present in nodes - SymbolicBayesTree actBayesTree = *bayesTreeReduced.clone(); - actBayesTree.permuteWithInverse(expReducingPermutation); - -// actBayesTree.print("Full bayes tree"); -// P( 7 8) -// P( 6 | 8) -// P( 5 | 6) -// P( 4 | 7) -// P( 0 | 4) - - // check keys in cliques - std::vector expRootIndices; expRootIndices += _B0_, _A0_; - IndexConditional::shared_ptr - expRoot(new IndexConditional(expRootIndices, 2)), // root - A0(new IndexConditional(_A0_)), - B0(new IndexConditional(_B0_, _A0_)), - C0(new IndexConditional(_C0_, _A0_)), // leaf level 1 - D0(new IndexConditional(_D0_, _C0_)), // leaf level 2 - E0(new IndexConditional(_E0_, _B0_)), // leaf level 2 - F0(new IndexConditional(_F0_, _E0_)); // leaf level 3 - - CHECK(actBayesTree.root()); - EXPECT(assert_equal(*expRoot, *actBayesTree.root()->conditional())); - EXPECT(assert_equal(*C0, *actBayesTree.root()->children().front()->conditional())); - EXPECT(assert_equal(*D0, *actBayesTree.root()->children().front()->children().front()->conditional())); - EXPECT(assert_equal(*E0, *actBayesTree.root()->children().back()->conditional())); - EXPECT(assert_equal(*F0, *actBayesTree.root()->children().back()->children().front()->conditional())); - - // check nodes structure - LONGS_EQUAL(9, actBayesTree.nodes().size()); - - SymbolicBayesTree expFullTree; - SymbolicBayesTree::insert(expFullTree, A0); - SymbolicBayesTree::insert(expFullTree, B0); - SymbolicBayesTree::insert(expFullTree, C0); - SymbolicBayesTree::insert(expFullTree, D0); - SymbolicBayesTree::insert(expFullTree, E0); - SymbolicBayesTree::insert(expFullTree, F0); - - EXPECT(assert_equal(expFullTree, actBayesTree)); -} - -///* ************************************************************************* */ -///** -// * x2 - x3 - x4 - x5 -// * | / \ | -// * x1 / \ x6 -// */ -//TEST( BayesTree, insert ) -//{ -// // construct bayes tree by split the graph along the separator x3 - x4 -// const Index _x1_=0, _x2_=1, _x6_=2, _x5_=3, _x3_=4, _x4_=5; -// SymbolicFactorGraph fg1, fg2, fg3; -// fg1.push_factor(_x3_, _x4_); -// fg2.push_factor(_x1_, _x2_); -// fg2.push_factor(_x2_, _x3_); -// fg2.push_factor(_x1_, _x3_); -// fg3.push_factor(_x5_, _x4_); -// fg3.push_factor(_x6_, _x5_); -// fg3.push_factor(_x6_, _x4_); -// -//// Ordering ordering1; ordering1 += _x3_, _x4_; -//// Ordering ordering2; ordering2 += _x1_, _x2_; -//// Ordering ordering3; ordering3 += _x6_, _x5_; -// -// BayesNet bn1, bn2, bn3; -// bn1 = *SymbolicSequentialSolver::EliminateUntil(fg1, _x4_+1); -// bn2 = *SymbolicSequentialSolver::EliminateUntil(fg2, _x2_+1); -// bn3 = *SymbolicSequentialSolver::EliminateUntil(fg3, _x5_+1); -// -// // insert child cliques -// SymbolicBayesTree actual; -// list children; -// SymbolicBayesTree::sharedClique r1 = actual.insert(bn2, children); -// SymbolicBayesTree::sharedClique r2 = actual.insert(bn3, children); -// -// // insert root clique -// children.push_back(r1); -// children.push_back(r2); -// actual.insert(bn1, children, true); -// -// // traditional way -// SymbolicFactorGraph fg; -// fg.push_factor(_x3_, _x4_); -// fg.push_factor(_x1_, _x2_); -// fg.push_factor(_x2_, _x3_); -// fg.push_factor(_x1_, _x3_); -// fg.push_factor(_x5_, _x4_); -// fg.push_factor(_x6_, _x5_); -// fg.push_factor(_x6_, _x4_); -// -//// Ordering ordering; ordering += _x1_, _x2_, _x6_, _x5_, _x3_, _x4_; -// BayesNet bn(*SymbolicSequentialSolver(fg).eliminate()); -// SymbolicBayesTree expected(bn); -// CHECK(assert_equal(expected, actual)); -// -//} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ diff --git a/gtsam/inference/tests/testClusterTree.cpp b/gtsam/inference/tests/testClusterTree.cpp deleted file mode 100644 index 0464aa546..000000000 --- a/gtsam/inference/tests/testClusterTree.cpp +++ /dev/null @@ -1,43 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 testClusterTree.cpp - * @brief Unit tests for Bayes Tree - * @author Kai Ni - * @author Frank Dellaert - */ - -#include // for operator += -using namespace boost::assign; - -#include - -#include -#include - -using namespace gtsam; - -// explicit instantiation and typedef -namespace gtsam { template class ClusterTree; } -typedef ClusterTree SymbolicClusterTree; - -/* ************************************************************************* */ -TEST(ClusterTree, constructor) { - SymbolicClusterTree tree; -} - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ diff --git a/gtsam/inference/tests/testConditional.cpp b/gtsam/inference/tests/testConditional.cpp deleted file mode 100644 index 5ee695bb5..000000000 --- a/gtsam/inference/tests/testConditional.cpp +++ /dev/null @@ -1,106 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 testConditional.cpp - * @brief Unit tests for IndexConditional class - * @author Frank Dellaert - */ - -#include // for operator += -#include // for operator += -using namespace boost::assign; - -#include -#include -#include - -using namespace std; -using namespace gtsam; - -/* ************************************************************************* */ -TEST( IndexConditional, empty ) -{ - IndexConditional c0; - LONGS_EQUAL(0,c0.nrFrontals()) - LONGS_EQUAL(0,c0.nrParents()) -} - -/* ************************************************************************* */ -TEST( IndexConditional, noParents ) -{ - IndexConditional c0(0); - LONGS_EQUAL(1,c0.nrFrontals()) - LONGS_EQUAL(0,c0.nrParents()) -} - -/* ************************************************************************* */ -TEST( IndexConditional, oneParents ) -{ - IndexConditional c0(0,1); - LONGS_EQUAL(1,c0.nrFrontals()) - LONGS_EQUAL(1,c0.nrParents()) -} - -/* ************************************************************************* */ -TEST( IndexConditional, twoParents ) -{ - IndexConditional c0(0,1,2); - LONGS_EQUAL(1,c0.nrFrontals()) - LONGS_EQUAL(2,c0.nrParents()) -} - -/* ************************************************************************* */ -TEST( IndexConditional, threeParents ) -{ - IndexConditional c0(0,1,2,3); - LONGS_EQUAL(1,c0.nrFrontals()) - LONGS_EQUAL(3,c0.nrParents()) -} - -/* ************************************************************************* */ -TEST( IndexConditional, fourParents ) -{ - vector parents; - parents += 1,2,3,4; - IndexConditional c0(0,parents); - LONGS_EQUAL(1,c0.nrFrontals()) - LONGS_EQUAL(4,c0.nrParents()) -} - -/* ************************************************************************* */ -TEST( IndexConditional, FromRange ) -{ - vector keys; - keys += 1,2,3,4,5; - IndexConditional::shared_ptr c0(new IndexConditional(keys,2)); - LONGS_EQUAL(2,c0->nrFrontals()) - LONGS_EQUAL(3,c0->nrParents()) -} - -/* ************************************************************************* */ -TEST( IndexConditional, equals ) -{ - IndexConditional c0(0, 1, 2), c1(0, 1, 2), c2(1, 2, 3), c3(3,4); - CHECK(c0.equals(c1)); - CHECK(c1.equals(c0)); - CHECK(!c0.equals(c2)); - CHECK(!c2.equals(c0)); - CHECK(!c0.equals(c3)); - CHECK(!c3.equals(c0)); -} - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ diff --git a/gtsam/inference/tests/testEliminationTree.cpp b/gtsam/inference/tests/testEliminationTree.cpp deleted file mode 100644 index 00679083a..000000000 --- a/gtsam/inference/tests/testEliminationTree.cpp +++ /dev/null @@ -1,119 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 testEliminationTree.cpp - * @brief - * @author Richard Roberts - * @date Oct 14, 2010 - */ - -#include -#include - -#include -#include - -using namespace gtsam; -using namespace std; - -class EliminationTreeTester { -public: - // build hardcoded tree - static SymbolicEliminationTree::shared_ptr buildHardcodedTree(const SymbolicFactorGraph& fg) { - - SymbolicEliminationTree::shared_ptr leaf0(new SymbolicEliminationTree); - leaf0->add(fg[0]); - leaf0->add(fg[1]); - - SymbolicEliminationTree::shared_ptr node1(new SymbolicEliminationTree(1)); - node1->add(fg[2]); - node1->add(leaf0); - - SymbolicEliminationTree::shared_ptr node2(new SymbolicEliminationTree(2)); - node2->add(fg[3]); - node2->add(node1); - - SymbolicEliminationTree::shared_ptr leaf3(new SymbolicEliminationTree(3)); - leaf3->add(fg[4]); - - SymbolicEliminationTree::shared_ptr etree(new SymbolicEliminationTree(4)); - etree->add(leaf3); - etree->add(node2); - - return etree; - } -}; - -TEST(EliminationTree, Create) -{ - // create example factor graph - SymbolicFactorGraph fg; - fg.push_factor(0, 1); - fg.push_factor(0, 2); - fg.push_factor(1, 4); - fg.push_factor(2, 4); - fg.push_factor(3, 4); - - SymbolicEliminationTree::shared_ptr expected = EliminationTreeTester::buildHardcodedTree(fg); - - // Build from factor graph - SymbolicEliminationTree::shared_ptr actual = SymbolicEliminationTree::Create(fg); - - CHECK(assert_equal(*expected,*actual)); -} - -/* ************************************************************************* */ -// Test to drive elimination tree development -// graph: f(0,1) f(0,2) f(1,4) f(2,4) f(3,4) -/* ************************************************************************* */ - -TEST(EliminationTree, eliminate ) -{ - // create expected Chordal bayes Net - SymbolicBayesNet expected; - expected.push_front(boost::make_shared(4)); - expected.push_front(boost::make_shared(3,4)); - expected.push_front(boost::make_shared(2,4)); - expected.push_front(boost::make_shared(1,2,4)); - expected.push_front(boost::make_shared(0,1,2)); - - // Create factor graph - SymbolicFactorGraph fg; - fg.push_factor(0, 1); - fg.push_factor(0, 2); - fg.push_factor(1, 4); - fg.push_factor(2, 4); - fg.push_factor(3, 4); - - // eliminate - SymbolicBayesNet actual = *SymbolicSequentialSolver(fg).eliminate(); - - CHECK(assert_equal(expected,actual)); -} - -/* ************************************************************************* */ -TEST(EliminationTree, disconnected_graph) { - SymbolicFactorGraph fg; - fg.push_factor(0, 1); - fg.push_factor(0, 2); - fg.push_factor(1, 2); - fg.push_factor(3, 4); - - CHECK_EXCEPTION(SymbolicEliminationTree::Create(fg), DisconnectedGraphException); -} - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ diff --git a/gtsam/inference/tests/testFactorGraph.cpp b/gtsam/inference/tests/testFactorGraph.cpp deleted file mode 100644 index 43402d2cb..000000000 --- a/gtsam/inference/tests/testFactorGraph.cpp +++ /dev/null @@ -1,65 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 testFactorgraph.cpp - * @brief Unit tests for IndexFactor Graphs - * @author Christian Potthast - **/ - -/*STL/C++*/ -#include -#include -#include -#include -#include // for operator += -#include -using namespace boost::assign; - -#include - -#include -#include - -using namespace std; -using namespace gtsam; - -/* ************************************************************************* */ -TEST(FactorGraph, eliminateFrontals) { - - SymbolicFactorGraph sfgOrig; - sfgOrig.push_factor(0,1); - sfgOrig.push_factor(0,2); - sfgOrig.push_factor(1,3); - sfgOrig.push_factor(1,4); - sfgOrig.push_factor(2,3); - sfgOrig.push_factor(4,5); - - IndexConditional::shared_ptr actualCond; - SymbolicFactorGraph actualSfg; - boost::tie(actualCond, actualSfg) = sfgOrig.eliminateFrontals(2); - - vector condIndices; - condIndices += 0,1,2,3,4; - IndexConditional expectedCond(condIndices, 2); - - SymbolicFactorGraph expectedSfg; - expectedSfg.push_factor(2,3); - expectedSfg.push_factor(4,5); - expectedSfg.push_factor(2,3,4); - - EXPECT(assert_equal(expectedSfg, actualSfg)); - EXPECT(assert_equal(expectedCond, *actualCond)); -} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ diff --git a/gtsam/inference/tests/testInference.cpp b/gtsam/inference/tests/testInference.cpp deleted file mode 100644 index b25703cb6..000000000 --- a/gtsam/inference/tests/testInference.cpp +++ /dev/null @@ -1,118 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 testInference.cpp - * @author Richard Roberts - * @author Alex Cunningham - * @date Dec 6, 2010 - */ - -#include - -#include -#include -#include - -using namespace gtsam; - -/* ************************************************************************* */ -TEST(inference, UnobservedVariables) { - SymbolicFactorGraph sfg; - - // Create a factor graph that skips some variables - sfg.push_factor(0,1); - sfg.push_factor(1,3); - sfg.push_factor(3,5); - - VariableIndex variableIndex(sfg); - - // Computes a permutation with known variables first, skipped variables last - // Actual 0 1 3 5 2 4 - Permutation::shared_ptr actual(inference::PermutationCOLAMD(variableIndex)); - Permutation expected(6); - expected[0] = 0; - expected[1] = 1; - expected[2] = 3; - expected[3] = 5; - expected[4] = 2; - expected[5] = 4; - EXPECT(assert_equal(expected, *actual)); -} - -/* ************************************************************************* */ -TEST(inference, 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); - - VariableIndex variableIndex(sfg); - - // unconstrained version - Permutation::shared_ptr actUnconstrained(inference::PermutationCOLAMD(variableIndex)); - Permutation expUnconstrained = Permutation::Identity(6); - EXPECT(assert_equal(expUnconstrained, *actUnconstrained)); - - // constrained version - push one set to the end - std::vector constrainLast; - constrainLast.push_back(2); - constrainLast.push_back(4); - Permutation::shared_ptr actConstrained(inference::PermutationCOLAMD(variableIndex, constrainLast)); - Permutation expConstrained(6); - expConstrained[0] = 0; - expConstrained[1] = 1; - expConstrained[2] = 5; - expConstrained[3] = 3; - expConstrained[4] = 4; - expConstrained[5] = 2; - EXPECT(assert_equal(expConstrained, *actConstrained)); -} - -/* ************************************************************************* */ -TEST(inference, 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); - - VariableIndex variableIndex(sfg); - - // constrained version - push one set to the end - FastMap constraints; - constraints[2] = 1; - constraints[4] = 1; - constraints[5] = 2; - - Permutation::shared_ptr actConstrained(inference::PermutationCOLAMDGrouped(variableIndex, constraints)); - Permutation expConstrained(6); - expConstrained[0] = 0; - expConstrained[1] = 1; - expConstrained[2] = 3; - expConstrained[3] = 2; - expConstrained[4] = 4; - expConstrained[5] = 5; - EXPECT(assert_equal(expConstrained, *actConstrained)); -} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ diff --git a/gtsam/inference/tests/testJunctionTree.cpp b/gtsam/inference/tests/testJunctionTree.cpp deleted file mode 100644 index ba7175e24..000000000 --- a/gtsam/inference/tests/testJunctionTree.cpp +++ /dev/null @@ -1,99 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 testJunctionTree.cpp - * @brief Unit tests for Junction Tree - * @author Kai Ni - * @author Frank Dellaert - */ - -#include // for operator += -#include // for operator += -#include // for operator += -using namespace boost::assign; - -#include -#include - -#include -#include -#include -#include -#include -#include - -using namespace gtsam; -using namespace std; - -typedef JunctionTree SymbolicJunctionTree; - -/* ************************************************************************* * - * x1 - x2 - x3 - x4 - * x3 x4 - * x2 x1 : x3 - ****************************************************************************/ -TEST( JunctionTree, constructor ) -{ - const Index x2=0, x1=1, x3=2, x4=3; - SymbolicFactorGraph fg; - fg.push_factor(x2,x1); - fg.push_factor(x2,x3); - fg.push_factor(x3,x4); - - SymbolicJunctionTree actual(fg); - - vector frontal1; frontal1 += x3, x4; - vector frontal2; frontal2 += x2, x1; - vector sep1; - vector sep2; sep2 += x3; - CHECK(assert_equal(frontal1, actual.root()->frontal)); - CHECK(assert_equal(sep1, actual.root()->separator)); - LONGS_EQUAL(1, actual.root()->size()); - CHECK(assert_equal(frontal2, actual.root()->children().front()->frontal)); - CHECK(assert_equal(sep2, actual.root()->children().front()->separator)); - LONGS_EQUAL(2, actual.root()->children().front()->size()); - CHECK(assert_equal(*fg[2], *(*actual.root())[0])); - CHECK(assert_equal(*fg[0], *(*actual.root()->children().front())[0])); - CHECK(assert_equal(*fg[1], *(*actual.root()->children().front())[1])); -} - -/* ************************************************************************* * - * x1 - x2 - x3 - x4 - * x3 x4 - * x2 x1 : x3 - ****************************************************************************/ -TEST( JunctionTree, eliminate) -{ - const Index x2=0, x1=1, x3=2, x4=3; - SymbolicFactorGraph fg; - fg.push_factor(x2,x1); - fg.push_factor(x2,x3); - fg.push_factor(x3,x4); - - SymbolicJunctionTree jt(fg); - SymbolicBayesTree::sharedClique actual = jt.eliminate(&EliminateSymbolic); - - BayesNet bn(*SymbolicSequentialSolver(fg).eliminate()); - SymbolicBayesTree expected(bn); - -// cout << "BT from JT:\n"; -// actual->printTree(""); - - CHECK(assert_equal(*expected.root(), *actual)); -} - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ diff --git a/gtsam/nonlinear/tests/testKey.cpp b/gtsam/inference/tests/testKey.cpp similarity index 71% rename from gtsam/nonlinear/tests/testKey.cpp rename to gtsam/inference/tests/testKey.cpp index ee9734af5..5b57096cb 100644 --- a/gtsam/nonlinear/tests/testKey.cpp +++ b/gtsam/inference/tests/testKey.cpp @@ -20,7 +20,7 @@ using namespace boost::assign; #include #include #include -#include +#include using namespace std; using namespace gtsam; @@ -36,28 +36,33 @@ TEST(Key, KeySymbolConversion) { EXPECT(assert_equal(original, actual)) } +/* ************************************************************************* */ +template +Key KeyTestValue(); + +template<> +Key KeyTestValue<8>() +{ + return 0x6100000000000005; +}; + +template<> +Key KeyTestValue<4>() +{ + return 0x61000005; +}; + /* ************************************************************************* */ TEST(Key, KeySymbolEncoding) { // Test encoding of Symbol <-> size_t <-> string + Symbol symbol(0x61, 5); + Key key = KeyTestValue(); + string str = "a5"; - if(sizeof(Key) == 8) { - Symbol symbol(0x61, 5); - Key key = 0x6100000000000005; - string str = "a5"; - - EXPECT_LONGS_EQUAL(key, (Key)symbol); - EXPECT(assert_equal(str, DefaultKeyFormatter(symbol))); - EXPECT(assert_equal(symbol, Symbol(key))); - } else if(sizeof(Key) == 4) { - Symbol symbol(0x61, 5); - Key key = 0x61000005; - string str = "a5"; - - EXPECT_LONGS_EQUAL(key, (Key)symbol); - EXPECT(assert_equal(str, DefaultKeyFormatter(symbol))); - EXPECT(assert_equal(symbol, Symbol(key))); - } + EXPECT_LONGS_EQUAL((long)key, (long)(Key)symbol); + EXPECT(assert_equal(str, DefaultKeyFormatter(symbol))); + EXPECT(assert_equal(symbol, Symbol(key))); } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/tests/testLabeledSymbol.cpp b/gtsam/inference/tests/testLabeledSymbol.cpp similarity index 83% rename from gtsam/nonlinear/tests/testLabeledSymbol.cpp rename to gtsam/inference/tests/testLabeledSymbol.cpp index 107c4502e..07727c8dc 100644 --- a/gtsam/nonlinear/tests/testLabeledSymbol.cpp +++ b/gtsam/inference/tests/testLabeledSymbol.cpp @@ -21,7 +21,7 @@ using namespace boost::assign; #include #include -#include +#include using namespace std; using namespace gtsam; @@ -49,7 +49,10 @@ TEST( testLabeledSymbol, KeyLabeledSymbolEncoding ) { Key key = 0x7841000000000005; string str = "xA5"; - EXPECT_LONGS_EQUAL(key, (Key)symbol); + EXPECT_LONGS_EQUAL((long)key, (long)(Key)symbol); + EXPECT_LONGS_EQUAL(5, symbol.index()); + EXPECT_LONGS_EQUAL(0x78, symbol.chr()); + EXPECT_LONGS_EQUAL(0x41, symbol.label()); EXPECT(assert_equal(str, MultiRobotKeyFormatter(symbol))); EXPECT(assert_equal(symbol, LabeledSymbol(key))); } else if(sizeof(Key) == 4) { @@ -57,7 +60,10 @@ TEST( testLabeledSymbol, KeyLabeledSymbolEncoding ) { Key key = 0x78410005; string str = "xA5"; - EXPECT_LONGS_EQUAL(key, (Key)symbol); + EXPECT_LONGS_EQUAL((long)key, (long)(Key) symbol); + EXPECT_LONGS_EQUAL(5, symbol.index()); + EXPECT_LONGS_EQUAL(0x78, symbol.chr()); + EXPECT_LONGS_EQUAL(0x41, symbol.label()); EXPECT(assert_equal(str, MultiRobotKeyFormatter(symbol))); EXPECT(assert_equal(symbol, LabeledSymbol(key))); } diff --git a/gtsam/inference/tests/testOrdering.cpp b/gtsam/inference/tests/testOrdering.cpp new file mode 100644 index 000000000..3bf6f7ca0 --- /dev/null +++ b/gtsam/inference/tests/testOrdering.cpp @@ -0,0 +1,82 @@ +/* ---------------------------------------------------------------------------- + + * 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 testOrdering + * @author Alex Cunningham + */ + +#include +#include +#include +#include +#include + +#include + +using namespace std; +using namespace gtsam; +using namespace boost::assign; + +/* ************************************************************************* */ +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); + + // unconstrained version + 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 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, list_of(2)(4)); + Ordering expConstrained2 = Ordering(list_of(2)(4)(0)(1)(3)(5)); + EXPECT(assert_equal(expConstrained2, actConstrained2)); +} + +/* ************************************************************************* */ +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); + + // constrained version - push one set to the end + FastMap constraints; + constraints[2] = 1; + constraints[4] = 1; + constraints[5] = 2; + + Ordering actConstrained = Ordering::COLAMDConstrained(sfg, constraints); + Ordering expConstrained = list_of(0)(1)(3)(2)(4)(5); + EXPECT(assert_equal(expConstrained, actConstrained)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ diff --git a/gtsam/inference/tests/testPermutation.cpp b/gtsam/inference/tests/testPermutation.cpp deleted file mode 100644 index 5aa83422f..000000000 --- a/gtsam/inference/tests/testPermutation.cpp +++ /dev/null @@ -1,134 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 testPermutation.cpp - * @date Sep 22, 2011 - * @author richard - */ - -#include - -#include -#include - -#include - -using namespace gtsam; -using namespace std; - -/* ************************************************************************* */ -TEST(Permutation, Identity) { - Permutation expected(5); - expected[0] = 0; - expected[1] = 1; - expected[2] = 2; - expected[3] = 3; - expected[4] = 4; - - Permutation actual = Permutation::Identity(5); - - EXPECT(assert_equal(expected, actual)); -} - -/* ************************************************************************* */ -TEST(Permutation, PullToFront) { - Permutation expected(5); - expected[0] = 4; - expected[1] = 0; - expected[2] = 2; - expected[3] = 1; - expected[4] = 3; - - std::vector toFront; - toFront.push_back(4); - toFront.push_back(0); - toFront.push_back(2); - Permutation actual = Permutation::PullToFront(toFront, 5); - - EXPECT(assert_equal(expected, actual)); -} - -/* ************************************************************************* */ -TEST(Permutation, PushToBack) { - Permutation expected(5); - expected[0] = 1; - expected[1] = 3; - expected[2] = 4; - expected[3] = 0; - expected[4] = 2; - - std::vector toBack; - toBack.push_back(4); - toBack.push_back(0); - toBack.push_back(2); - Permutation actual = Permutation::PushToBack(toBack, 5); - - EXPECT(assert_equal(expected, actual)); -} - -/* ************************************************************************* */ -TEST(Permutation, compose) { - Permutation p1(5); - p1[0] = 1; - p1[1] = 2; - p1[2] = 3; - p1[3] = 4; - p1[4] = 0; - - Permutation p2(5); - p2[0] = 1; - p2[1] = 2; - p2[2] = 4; - p2[3] = 3; - p2[4] = 0; - - Permutation expected(5); - expected[0] = 2; - expected[1] = 3; - expected[2] = 0; - expected[3] = 4; - expected[4] = 1; - - Permutation actual = *p1.permute(p2); - - EXPECT(assert_equal(expected, actual)); - LONGS_EQUAL(p1[p2[0]], actual[0]); - LONGS_EQUAL(p1[p2[1]], actual[1]); - LONGS_EQUAL(p1[p2[2]], actual[2]); - LONGS_EQUAL(p1[p2[3]], actual[3]); - LONGS_EQUAL(p1[p2[4]], actual[4]); -} - -/* ************************************************************************* */ -TEST(Reduction, CreateFromPartialPermutation) { - Permutation selector(3); - selector[0] = 2; - selector[1] = 4; - selector[2] = 6; - Permutation p(3); - p[0] = 2; - p[1] = 0; - p[2] = 1; - - internal::Reduction expected; - expected.insert(make_pair(2,6)); - expected.insert(make_pair(4,2)); - expected.insert(make_pair(6,4)); - - internal::Reduction actual = internal::Reduction::CreateFromPartialPermutation(selector, p); - EXPECT(assert_equal(expected, actual)); -} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } - - diff --git a/gtsam/inference/tests/testSerializationInference.cpp b/gtsam/inference/tests/testSerializationInference.cpp deleted file mode 100644 index b2b300440..000000000 --- a/gtsam/inference/tests/testSerializationInference.cpp +++ /dev/null @@ -1,88 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 testSerializationInference.cpp - * @brief - * @author Richard Roberts - * @date Feb 7, 2012 - */ - -#include -#include -#include - -#include -#include - -using namespace std; -using namespace gtsam; -using namespace gtsam::serializationTestHelpers; - -/* ************************************************************************* */ -TEST (Serialization, symbolic_graph) { - // construct expected symbolic graph - SymbolicFactorGraph sfg; - sfg.push_factor(0); - sfg.push_factor(0,1); - sfg.push_factor(0,2); - sfg.push_factor(2,1); - - EXPECT(equalsObj(sfg)); - EXPECT(equalsXML(sfg)); - EXPECT(equalsBinary(sfg)); -} - -/* ************************************************************************* */ -TEST (Serialization, symbolic_bn) { - IndexConditional::shared_ptr x2(new IndexConditional(1, 2, 0)); - IndexConditional::shared_ptr l1(new IndexConditional(2, 0)); - IndexConditional::shared_ptr x1(new IndexConditional(0)); - - SymbolicBayesNet sbn; - sbn.push_back(x2); - sbn.push_back(l1); - sbn.push_back(x1); - - EXPECT(equalsObj(sbn)); - EXPECT(equalsXML(sbn)); - EXPECT(equalsBinary(sbn)); -} - -/* ************************************************************************* */ -TEST (Serialization, symbolic_bayes_tree ) { - typedef BayesTree SymbolicBayesTree; - static const Index _X_=0, _T_=1, _S_=2, _E_=3, _L_=4, _B_=5; - IndexConditional::shared_ptr - B(new IndexConditional(_B_)), - L(new IndexConditional(_L_, _B_)), - E(new IndexConditional(_E_, _L_, _B_)), - S(new IndexConditional(_S_, _L_, _B_)), - T(new IndexConditional(_T_, _E_, _L_)), - X(new IndexConditional(_X_, _E_)); - - // Bayes Tree for Asia example - SymbolicBayesTree bayesTree; - SymbolicBayesTree::insert(bayesTree, B); - SymbolicBayesTree::insert(bayesTree, L); - SymbolicBayesTree::insert(bayesTree, E); - SymbolicBayesTree::insert(bayesTree, S); - SymbolicBayesTree::insert(bayesTree, T); - SymbolicBayesTree::insert(bayesTree, X); - - EXPECT(equalsObj(bayesTree)); - EXPECT(equalsXML(bayesTree)); - EXPECT(equalsBinary(bayesTree)); -} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ diff --git a/gtsam/inference/tests/testSymbolicBayesNet.cpp b/gtsam/inference/tests/testSymbolicBayesNet.cpp deleted file mode 100644 index 44def4c24..000000000 --- a/gtsam/inference/tests/testSymbolicBayesNet.cpp +++ /dev/null @@ -1,210 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 testSymbolicBayesNet.cpp - * @brief Unit tests for a symbolic Bayes chain - * @author Frank Dellaert - */ - -#include // for 'insert()' -#include // for operator += -using namespace boost::assign; - -#include - -#include -#include -#include - -using namespace std; -using namespace gtsam; - -static const Index _L_ = 0; -static const Index _A_ = 1; -static const Index _B_ = 2; -static const Index _C_ = 3; -static const Index _D_ = 4; -static const Index _E_ = 5; - -static IndexConditional::shared_ptr - B(new IndexConditional(_B_)), - L(new IndexConditional(_L_, _B_)); - -/* ************************************************************************* */ -TEST( SymbolicBayesNet, equals ) -{ - BayesNet f1; - f1.push_back(B); - f1.push_back(L); - BayesNet f2; - f2.push_back(L); - f2.push_back(B); - CHECK(f1.equals(f1)); - CHECK(!f1.equals(f2)); -} - -/* ************************************************************************* */ -TEST( SymbolicBayesNet, pop_front ) -{ - IndexConditional::shared_ptr - A(new IndexConditional(_A_,_B_,_C_)), - B(new IndexConditional(_B_,_C_)), - C(new IndexConditional(_C_)); - - // Expected after pop_front - BayesNet expected; - expected.push_back(B); - expected.push_back(C); - - // Actual - BayesNet actual; - actual.push_back(A); - actual.push_back(B); - actual.push_back(C); - actual.pop_front(); - - CHECK(assert_equal(expected,actual)); -} - -/* ************************************************************************* */ -TEST( SymbolicBayesNet, combine ) -{ - IndexConditional::shared_ptr - A(new IndexConditional(_A_,_B_,_C_)), - B(new IndexConditional(_B_,_C_)), - C(new IndexConditional(_C_)); - - // p(A|BC) - BayesNet p_ABC; - p_ABC.push_back(A); - - // P(BC)=P(B|C)P(C) - BayesNet p_BC; - p_BC.push_back(B); - p_BC.push_back(C); - - // P(ABC) = P(A|BC) P(BC) - p_ABC.push_back(p_BC); - - BayesNet expected; - expected.push_back(A); - expected.push_back(B); - expected.push_back(C); - - CHECK(assert_equal(expected,p_ABC)); -} - -/* ************************************************************************* */ -TEST(SymbolicBayesNet, find) { - SymbolicBayesNet bn; - bn += IndexConditional::shared_ptr(new IndexConditional(_A_, _B_)); - std::vector keys; - keys.push_back(_B_); - keys.push_back(_C_); - keys.push_back(_D_); - bn += IndexConditional::shared_ptr(new IndexConditional(keys,2)); - bn += IndexConditional::shared_ptr(new IndexConditional(_D_)); - - SymbolicBayesNet::iterator expected = bn.begin(); ++ expected; - SymbolicBayesNet::iterator actual = bn.find(_C_); - EXPECT(assert_equal(**expected, **actual)); -} - -/* ************************************************************************* */ -TEST_UNSAFE(SymbolicBayesNet, popLeaf) { - IndexConditional::shared_ptr - A(new IndexConditional(_A_,_E_)), - B(new IndexConditional(_B_,_E_)), - C(new IndexConditional(_C_,_D_)), - D(new IndexConditional(_D_,_E_)), - E(new IndexConditional(_E_)); - - // BayesNet after popping A - SymbolicBayesNet expected1; - expected1 += B, C, D, E; - - // BayesNet after popping C - SymbolicBayesNet expected2; - expected2 += A, B, D, E; - - // BayesNet after popping C and D - SymbolicBayesNet expected3; - expected3 += A, B, E; - - // BayesNet after popping C and A - SymbolicBayesNet expected4; - expected4 += B, D, E; - - - // BayesNet after popping A - SymbolicBayesNet actual1; - actual1 += A, B, C, D, E; - actual1.popLeaf(actual1.find(_A_)); - - // BayesNet after popping C - SymbolicBayesNet actual2; - actual2 += A, B, C, D, E; - actual2.popLeaf(actual2.find(_C_)); - - // BayesNet after popping C and D - SymbolicBayesNet actual3; - actual3 += A, B, C, D, E; - actual3.popLeaf(actual3.find(_C_)); - actual3.popLeaf(actual3.find(_D_)); - - // BayesNet after popping C and A - SymbolicBayesNet actual4; - actual4 += A, B, C, D, E; - actual4.popLeaf(actual4.find(_C_)); - actual4.popLeaf(actual4.find(_A_)); - - EXPECT(assert_equal(expected1, actual1)); - EXPECT(assert_equal(expected2, actual2)); - EXPECT(assert_equal(expected3, actual3)); - EXPECT(assert_equal(expected4, actual4)); - - // Try to remove a non-leaf node (this test is not working in non-debug mode) -//#undef NDEBUG_SAVED -//#ifdef NDEBUG -//#define NDEBUG_SAVED -//#endif -// -//#undef NDEBUG -// SymbolicBayesNet actual5; -// actual5 += A, B, C, D, E; -// CHECK_EXCEPTION(actual5.popLeaf(actual5.find(_D_)), std::invalid_argument); -// -//#ifdef NDEBUG_SAVED -//#define NDEBUG -//#endif -} - -/* ************************************************************************* */ -TEST(SymbolicBayesNet, saveGraph) { - SymbolicBayesNet bn; - bn += IndexConditional::shared_ptr(new IndexConditional(_A_, _B_)); - std::vector keys; - keys.push_back(_B_); - keys.push_back(_C_); - keys.push_back(_D_); - bn += IndexConditional::shared_ptr(new IndexConditional(keys,2)); - bn += IndexConditional::shared_ptr(new IndexConditional(_D_)); - - bn.saveGraph("SymbolicBayesNet.dot"); -} - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ diff --git a/gtsam/inference/tests/testSymbolicBayesTree.cpp b/gtsam/inference/tests/testSymbolicBayesTree.cpp deleted file mode 100644 index 68693817b..000000000 --- a/gtsam/inference/tests/testSymbolicBayesTree.cpp +++ /dev/null @@ -1,323 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 testSymbolicBayesTree.cpp - * @date sept 15, 2012 - * @author Frank Dellaert - */ - -#include -#include -#include - -#include -#include -using namespace boost::assign; - -#include - -using namespace std; -using namespace gtsam; - -static bool debug = false; - -/* ************************************************************************* */ - -TEST_UNSAFE( SymbolicBayesTree, thinTree ) { - - // create a thin-tree Bayesnet, a la Jean-Guillaume - SymbolicBayesNet bayesNet; - bayesNet.push_front(boost::make_shared(14)); - - bayesNet.push_front(boost::make_shared(13, 14)); - bayesNet.push_front(boost::make_shared(12, 14)); - - bayesNet.push_front(boost::make_shared(11, 13, 14)); - bayesNet.push_front(boost::make_shared(10, 13, 14)); - bayesNet.push_front(boost::make_shared(9, 12, 14)); - bayesNet.push_front(boost::make_shared(8, 12, 14)); - - bayesNet.push_front(boost::make_shared(7, 11, 13)); - bayesNet.push_front(boost::make_shared(6, 11, 13)); - bayesNet.push_front(boost::make_shared(5, 10, 13)); - bayesNet.push_front(boost::make_shared(4, 10, 13)); - - bayesNet.push_front(boost::make_shared(3, 9, 12)); - bayesNet.push_front(boost::make_shared(2, 9, 12)); - bayesNet.push_front(boost::make_shared(1, 8, 12)); - bayesNet.push_front(boost::make_shared(0, 8, 12)); - - if (debug) { - GTSAM_PRINT(bayesNet); - bayesNet.saveGraph("/tmp/symbolicBayesNet.dot"); - } - - // create a BayesTree out of a Bayes net - SymbolicBayesTree bayesTree(bayesNet); - if (debug) { - GTSAM_PRINT(bayesTree); - bayesTree.saveGraph("/tmp/symbolicBayesTree.dot"); - } - - SymbolicBayesTree::Clique::shared_ptr R = bayesTree.root(); - - { - // check shortcut P(S9||R) to root - SymbolicBayesTree::Clique::shared_ptr c = bayesTree[9]; - SymbolicBayesNet shortcut = c->shortcut(R, EliminateSymbolic); - SymbolicBayesNet expected; - EXPECT(assert_equal(expected, shortcut)); - } - - { - // check shortcut P(S8||R) to root - SymbolicBayesTree::Clique::shared_ptr c = bayesTree[8]; - SymbolicBayesNet shortcut = c->shortcut(R, EliminateSymbolic); - SymbolicBayesNet expected; - expected.push_front(boost::make_shared(12, 14)); - EXPECT(assert_equal(expected, shortcut)); - } - - { - // check shortcut P(S4||R) to root - SymbolicBayesTree::Clique::shared_ptr c = bayesTree[4]; - SymbolicBayesNet shortcut = c->shortcut(R, EliminateSymbolic); - SymbolicBayesNet expected; - expected.push_front(boost::make_shared(10, 13, 14)); - EXPECT(assert_equal(expected, shortcut)); - } - - { - // check shortcut P(S2||R) to root - SymbolicBayesTree::Clique::shared_ptr c = bayesTree[2]; - SymbolicBayesNet shortcut = c->shortcut(R, EliminateSymbolic); - SymbolicBayesNet expected; - expected.push_front(boost::make_shared(12, 14)); - expected.push_front(boost::make_shared(9, 12, 14)); - EXPECT(assert_equal(expected, shortcut)); - } - - { - // check shortcut P(S0||R) to root - SymbolicBayesTree::Clique::shared_ptr c = bayesTree[0]; - SymbolicBayesNet shortcut = c->shortcut(R, EliminateSymbolic); - SymbolicBayesNet expected; - expected.push_front(boost::make_shared(12, 14)); - expected.push_front(boost::make_shared(8, 12, 14)); - EXPECT(assert_equal(expected, shortcut)); - } - - SymbolicBayesNet::shared_ptr actualJoint; - - // Check joint P(8,2) - if (false) { // TODO, not disjoint - actualJoint = bayesTree.jointBayesNet(8, 2, EliminateSymbolic); - SymbolicBayesNet expected; - expected.push_front(boost::make_shared(8)); - expected.push_front(boost::make_shared(2, 8)); - EXPECT(assert_equal(expected, *actualJoint)); - } - - // Check joint P(1,2) - if (false) { // TODO, not disjoint - actualJoint = bayesTree.jointBayesNet(1, 2, EliminateSymbolic); - SymbolicBayesNet expected; - expected.push_front(boost::make_shared(2)); - expected.push_front(boost::make_shared(1, 2)); - EXPECT(assert_equal(expected, *actualJoint)); - } - - // Check joint P(2,6) - if (true) { - actualJoint = bayesTree.jointBayesNet(2, 6, EliminateSymbolic); - SymbolicBayesNet expected; - expected.push_front(boost::make_shared(6)); - expected.push_front(boost::make_shared(2, 6)); - EXPECT(assert_equal(expected, *actualJoint)); - } - - // Check joint P(4,6) - if (false) { // TODO, not disjoint - actualJoint = bayesTree.jointBayesNet(4, 6, EliminateSymbolic); - SymbolicBayesNet expected; - expected.push_front(boost::make_shared(6)); - expected.push_front(boost::make_shared(4, 6)); - EXPECT(assert_equal(expected, *actualJoint)); - } -} - -/* ************************************************************************* * - Bayes tree for smoother with "natural" ordering: - C1 5 6 - C2 4 : 5 - C3 3 : 4 - C4 2 : 3 - C5 1 : 2 - C6 0 : 1 - **************************************************************************** */ - -TEST_UNSAFE( SymbolicBayesTree, linear_smoother_shortcuts ) { - // Create smoother with 7 nodes - SymbolicFactorGraph smoother; - smoother.push_factor(0); - smoother.push_factor(0, 1); - smoother.push_factor(1, 2); - smoother.push_factor(2, 3); - smoother.push_factor(3, 4); - smoother.push_factor(4, 5); - smoother.push_factor(5, 6); - - BayesNet bayesNet = - *SymbolicSequentialSolver(smoother).eliminate(); - - if (debug) { - GTSAM_PRINT(bayesNet); - bayesNet.saveGraph("/tmp/symbolicBayesNet.dot"); - } - - // create a BayesTree out of a Bayes net - SymbolicBayesTree bayesTree(bayesNet); - if (debug) { - GTSAM_PRINT(bayesTree); - bayesTree.saveGraph("/tmp/symbolicBayesTree.dot"); - } - - SymbolicBayesTree::Clique::shared_ptr R = bayesTree.root(); - - { - // check shortcut P(S2||R) to root - SymbolicBayesTree::Clique::shared_ptr c = bayesTree[4]; // 4 is frontal in C2 - SymbolicBayesNet shortcut = c->shortcut(R, EliminateSymbolic); - SymbolicBayesNet expected; - EXPECT(assert_equal(expected, shortcut)); - } - - { - // check shortcut P(S3||R) to root - SymbolicBayesTree::Clique::shared_ptr c = bayesTree[3]; // 3 is frontal in C3 - SymbolicBayesNet shortcut = c->shortcut(R, EliminateSymbolic); - SymbolicBayesNet expected; - expected.push_front(boost::make_shared(4, 5)); - EXPECT(assert_equal(expected, shortcut)); - } - - { - // check shortcut P(S4||R) to root - SymbolicBayesTree::Clique::shared_ptr c = bayesTree[2]; // 2 is frontal in C4 - SymbolicBayesNet shortcut = c->shortcut(R, EliminateSymbolic); - SymbolicBayesNet expected; - expected.push_front(boost::make_shared(3, 5)); - EXPECT(assert_equal(expected, shortcut)); - } -} - -/* ************************************************************************* */ -// from testSymbolicJunctionTree, which failed at one point -TEST(SymbolicBayesTree, complicatedMarginal) { - - // Create the conditionals to go in the BayesTree - list L; - L = list_of(1)(2)(5); - IndexConditional::shared_ptr R_1_2(new IndexConditional(L, 2)); - L = list_of(3)(4)(6); - IndexConditional::shared_ptr R_3_4(new IndexConditional(L, 2)); - L = list_of(5)(6)(7)(8); - IndexConditional::shared_ptr R_5_6(new IndexConditional(L, 2)); - L = list_of(7)(8)(11); - IndexConditional::shared_ptr R_7_8(new IndexConditional(L, 2)); - L = list_of(9)(10)(11)(12); - IndexConditional::shared_ptr R_9_10(new IndexConditional(L, 2)); - L = list_of(11)(12); - IndexConditional::shared_ptr R_11_12(new IndexConditional(L, 2)); - - // Symbolic Bayes Tree - typedef SymbolicBayesTree::Clique Clique; - typedef SymbolicBayesTree::sharedClique sharedClique; - - // Create Bayes Tree - SymbolicBayesTree bt; - bt.insert(sharedClique(new Clique(R_11_12))); - bt.insert(sharedClique(new Clique(R_9_10))); - bt.insert(sharedClique(new Clique(R_7_8))); - bt.insert(sharedClique(new Clique(R_5_6))); - bt.insert(sharedClique(new Clique(R_3_4))); - bt.insert(sharedClique(new Clique(R_1_2))); - if (debug) { - GTSAM_PRINT(bt); - bt.saveGraph("/tmp/symbolicBayesTree.dot"); - } - - SymbolicBayesTree::Clique::shared_ptr R = bt.root(); - SymbolicBayesNet empty; - - // Shortcut on 9 - { - SymbolicBayesTree::Clique::shared_ptr c = bt[9]; - SymbolicBayesNet shortcut = c->shortcut(R, EliminateSymbolic); - EXPECT(assert_equal(empty, shortcut)); - } - - // Shortcut on 7 - { - SymbolicBayesTree::Clique::shared_ptr c = bt[7]; - SymbolicBayesNet shortcut = c->shortcut(R, EliminateSymbolic); - EXPECT(assert_equal(empty, shortcut)); - } - - // Shortcut on 5 - { - SymbolicBayesTree::Clique::shared_ptr c = bt[5]; - SymbolicBayesNet shortcut = c->shortcut(R, EliminateSymbolic); - SymbolicBayesNet expected; - expected.push_front(boost::make_shared(8, 11)); - expected.push_front(boost::make_shared(7, 8, 11)); - EXPECT(assert_equal(expected, shortcut)); - } - - // Shortcut on 3 - { - SymbolicBayesTree::Clique::shared_ptr c = bt[3]; - SymbolicBayesNet shortcut = c->shortcut(R, EliminateSymbolic); - SymbolicBayesNet expected; - expected.push_front(boost::make_shared(6, 11)); - EXPECT(assert_equal(expected, shortcut)); - } - - // Shortcut on 1 - { - SymbolicBayesTree::Clique::shared_ptr c = bt[1]; - SymbolicBayesNet shortcut = c->shortcut(R, EliminateSymbolic); - SymbolicBayesNet expected; - expected.push_front(boost::make_shared(5, 11)); - EXPECT(assert_equal(expected, shortcut)); - } - - // Marginal on 5 - { - IndexFactor::shared_ptr actual = bt.marginalFactor(5, EliminateSymbolic); - EXPECT(assert_equal(IndexFactor(5), *actual, 1e-1)); - } - - // Shortcut on 6 - { - IndexFactor::shared_ptr actual = bt.marginalFactor(6, EliminateSymbolic); - EXPECT(assert_equal(IndexFactor(6), *actual, 1e-1)); - } - -} -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ - diff --git a/gtsam/inference/tests/testSymbolicFactorGraph.cpp b/gtsam/inference/tests/testSymbolicFactorGraph.cpp deleted file mode 100644 index 35c50eb9e..000000000 --- a/gtsam/inference/tests/testSymbolicFactorGraph.cpp +++ /dev/null @@ -1,107 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 testSymbolicFactorGraph.cpp - * @brief Unit tests for a symbolic IndexFactor Graph - * @author Frank Dellaert - */ - -#include // for operator += -using namespace boost::assign; - -#include - -#include -#include -#include -#include -#include - -using namespace std; -using namespace gtsam; - -static const Index vx2 = 0; -static const Index vx1 = 1; -static const Index vl1 = 2; - -///* ************************************************************************* */ -//TEST( SymbolicFactorGraph, EliminateOne ) -//{ -// // create a test graph -// SymbolicFactorGraph fg; -// fg.push_factor(vx2, vx1); -// -// SymbolicSequentialSolver::EliminateUntil(fg, vx2+1); -// SymbolicFactorGraph expected; -// expected.push_back(boost::shared_ptr()); -// expected.push_factor(vx1); -// -// CHECK(assert_equal(expected, fg)); -//} - -/* ************************************************************************* */ -TEST( SymbolicFactorGraph, constructFromBayesNet ) -{ - // create expected factor graph - SymbolicFactorGraph expected; - expected.push_factor(vx2, vx1, vl1); - expected.push_factor(vx1, vl1); - expected.push_factor(vx1); - - // create Bayes Net - IndexConditional::shared_ptr x2(new IndexConditional(vx2, vx1, vl1)); - IndexConditional::shared_ptr l1(new IndexConditional(vx1, vl1)); - IndexConditional::shared_ptr x1(new IndexConditional(vx1)); - - BayesNet bayesNet; - bayesNet.push_back(x2); - bayesNet.push_back(l1); - bayesNet.push_back(x1); - - // create actual factor graph from a Bayes Net - SymbolicFactorGraph actual(bayesNet); - - CHECK(assert_equal((SymbolicFactorGraph)expected,actual)); -} - -/* ************************************************************************* */ -TEST( SymbolicFactorGraph, push_back ) -{ - // Create two factor graphs and expected combined graph - SymbolicFactorGraph fg1, fg2, expected; - - fg1.push_factor(vx1); - fg1.push_factor(vx2, vx1); - - fg2.push_factor(vx1, vl1); - fg2.push_factor(vx2, vl1); - - expected.push_factor(vx1); - expected.push_factor(vx2, vx1); - expected.push_factor(vx1, vl1); - expected.push_factor(vx2, vl1); - - // combine - SymbolicFactorGraph actual = combine(fg1, fg2); - CHECK(assert_equal(expected, actual)); - - // combine using push_back - fg1.push_back(fg2); - CHECK(assert_equal(expected, fg1)); -} - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ diff --git a/gtsam/inference/tests/testSymbolicSequentialSolver.cpp b/gtsam/inference/tests/testSymbolicSequentialSolver.cpp deleted file mode 100644 index c95bbdab5..000000000 --- a/gtsam/inference/tests/testSymbolicSequentialSolver.cpp +++ /dev/null @@ -1,130 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 testSymbolicSequentialSolver.cpp - * @brief Unit tests for a symbolic sequential solver routines - * @author Frank Dellaert - * @date Sept 16, 2012 - */ - -#include - -#include - -#include // for operator += -using namespace boost::assign; - -using namespace std; -using namespace gtsam; - -/* ************************************************************************* */ - -TEST( SymbolicSequentialSolver, SymbolicSequentialSolver ) { - // create factor graph - SymbolicFactorGraph g; - g.push_factor(2, 1, 0); - g.push_factor(2, 0); - g.push_factor(2); - // test solver is Testable - SymbolicSequentialSolver solver(g); -// GTSAM_PRINT(solver); - EXPECT(assert_equal(solver,solver)); -} - -/* ************************************************************************* */ - -TEST( SymbolicSequentialSolver, inference ) { - // Create factor graph - SymbolicFactorGraph fg; - fg.push_factor(0, 1); - fg.push_factor(0, 2); - fg.push_factor(1, 4); - fg.push_factor(2, 4); - fg.push_factor(3, 4); - - // eliminate - SymbolicSequentialSolver solver(fg); - SymbolicBayesNet::shared_ptr actual = solver.eliminate(); - SymbolicBayesNet expected; - expected.push_front(boost::make_shared(4)); - expected.push_front(boost::make_shared(3, 4)); - expected.push_front(boost::make_shared(2, 4)); - expected.push_front(boost::make_shared(1, 2, 4)); - expected.push_front(boost::make_shared(0, 1, 2)); - EXPECT(assert_equal(expected,*actual)); - - { - // jointBayesNet - vector js; - js.push_back(0); - js.push_back(4); - js.push_back(3); - SymbolicBayesNet::shared_ptr actualBN = solver.jointBayesNet(js); - SymbolicBayesNet expectedBN; - expectedBN.push_front(boost::make_shared(3)); - expectedBN.push_front(boost::make_shared(4, 3)); - expectedBN.push_front(boost::make_shared(0, 4)); - EXPECT( assert_equal(expectedBN,*actualBN)); - - // jointFactorGraph - SymbolicFactorGraph::shared_ptr actualFG = solver.jointFactorGraph(js); - SymbolicFactorGraph expectedFG; - expectedFG.push_factor(0, 4); - expectedFG.push_factor(4, 3); - expectedFG.push_factor(3); - EXPECT( assert_equal(expectedFG,(SymbolicFactorGraph)(*actualFG))); - } - - { - // jointBayesNet - vector js; - js.push_back(0); - js.push_back(2); - js.push_back(3); - SymbolicBayesNet::shared_ptr actualBN = solver.jointBayesNet(js); - SymbolicBayesNet expectedBN; - expectedBN.push_front(boost::make_shared(2)); - expectedBN.push_front(boost::make_shared(3, 2)); - expectedBN.push_front(boost::make_shared(0, 3, 2)); - EXPECT( assert_equal(expectedBN,*actualBN)); - - // jointFactorGraph - SymbolicFactorGraph::shared_ptr actualFG = solver.jointFactorGraph(js); - SymbolicFactorGraph expectedFG; - expectedFG.push_factor(0, 3, 2); - expectedFG.push_factor(3, 2); - expectedFG.push_factor(2); - EXPECT( assert_equal(expectedFG,(SymbolicFactorGraph)(*actualFG))); - } - - { - // conditionalBayesNet - vector js; - js.push_back(0); - js.push_back(2); - js.push_back(3); - size_t nrFrontals = 2; - SymbolicBayesNet::shared_ptr actualBN = // - solver.conditionalBayesNet(js, nrFrontals); - SymbolicBayesNet expectedBN; - expectedBN.push_front(boost::make_shared(2, 3)); - expectedBN.push_front(boost::make_shared(0, 2, 3)); - EXPECT( assert_equal(expectedBN,*actualBN)); - } -} - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ diff --git a/gtsam/inference/tests/testVariableSlots.cpp b/gtsam/inference/tests/testVariableSlots.cpp index 1dd422908..f61b49bdd 100644 --- a/gtsam/inference/tests/testVariableSlots.cpp +++ b/gtsam/inference/tests/testVariableSlots.cpp @@ -20,7 +20,7 @@ #include #include -#include +#include #include diff --git a/gtsam/linear/CMakeLists.txt b/gtsam/linear/CMakeLists.txt index 744151a8b..29eb60b93 100644 --- a/gtsam/linear/CMakeLists.txt +++ b/gtsam/linear/CMakeLists.txt @@ -2,32 +2,10 @@ file(GLOB linear_headers "*.h") install(FILES ${linear_headers} DESTINATION include/gtsam/linear) -# Components to link tests in this subfolder against -set(linear_local_libs - linear - inference - geometry - base - ccolamd -) - -# Files to exclude from compilation of tests and timing scripts -set(linear_excluded_files -# "${CMAKE_CURRENT_SOURCE_DIR}/tests/testTypedDiscreteFactor.cpp" # Example of excluding a test -# "" # Add to this list, with full path, to exclude -) - # Build tests -if (GTSAM_BUILD_TESTS) - gtsam_add_subdir_tests(linear "${linear_local_libs}" "${gtsam-default}" "${linear_excluded_files}") -endif(GTSAM_BUILD_TESTS) - -if(MSVC) - set_property(SOURCE "${CMAKE_CURRENT_SOURCE_DIR}/tests/testSerializationLinear.cpp" - APPEND PROPERTY COMPILE_FLAGS "/bigobj") -endif() +add_subdirectory(tests) # Build timing scripts if (GTSAM_BUILD_TIMING) - gtsam_add_subdir_timing(linear "${linear_local_libs}" "${gtsam-default}" "${linear_excluded_files}") + gtsam_add_subdir_timing(linear "gtsam" "gtsam" "${linear_excluded_files}") endif(GTSAM_BUILD_TIMING) diff --git a/gtsam/linear/Errors.cpp b/gtsam/linear/Errors.cpp index 1aae91869..c0740f756 100644 --- a/gtsam/linear/Errors.cpp +++ b/gtsam/linear/Errors.cpp @@ -18,7 +18,9 @@ */ #include +#include #include +#include using namespace std; @@ -28,11 +30,9 @@ namespace gtsam { Errors::Errors(){} /* ************************************************************************* */ -Errors::Errors(const VectorValues &V) { - this->resize(V.size()) ; - int i = 0 ; - BOOST_FOREACH( Vector &e, *this) { - e = V[i++]; +Errors::Errors(const VectorValues& V) { + BOOST_FOREACH(const Vector& e, V | boost::adaptors::map_values) { + push_back(e); } } diff --git a/gtsam/linear/Errors.h b/gtsam/linear/Errors.h index f3ccffb14..13da360a5 100644 --- a/gtsam/linear/Errors.h +++ b/gtsam/linear/Errors.h @@ -19,19 +19,25 @@ #pragma once -#include +#include +#include + +#include namespace gtsam { - + + // Forward declarations + class VectorValues; + /** vector of errors */ - class Errors : public std::list { + class Errors : public FastList { public: GTSAM_EXPORT Errors() ; - /** break V into pieces according to its start indices */ - GTSAM_EXPORT Errors(const VectorValues &V) ; + /** break V into pieces according to its start indices */ + GTSAM_EXPORT Errors(const VectorValues&V); /** print */ GTSAM_EXPORT void print(const std::string& s = "Errors") const; @@ -51,13 +57,13 @@ namespace gtsam { }; // Errors /** - * dot product - */ + * dot product + */ GTSAM_EXPORT double dot(const Errors& a, const Errors& b); /** - * BLAS level 2 style - */ + * BLAS level 2 style + */ template <> GTSAM_EXPORT void axpy(double alpha, const Errors& x, Errors& y); diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp index 8e1520235..b3b8b9a41 100644 --- a/gtsam/linear/GaussianBayesNet.cpp +++ b/gtsam/linear/GaussianBayesNet.cpp @@ -17,239 +17,167 @@ #include #include -#include -#include +#include +#include #include -#include -#include - -#include using namespace std; using namespace gtsam; -// trick from some reading group #define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL) #define REVERSE_FOREACH_PAIR( KEY, VAL, COL) BOOST_REVERSE_FOREACH (boost::tie(KEY,VAL),COL) namespace gtsam { -// Explicitly instantiate so we don't have to include everywhere -template class BayesNet; + // Instantiate base class + template class FactorGraph; -/* ************************************************************************* */ -GaussianBayesNet scalarGaussian(Index key, double mu, double sigma) { - GaussianBayesNet bn; - GaussianConditional::shared_ptr - conditional(new GaussianConditional(key, Vector_(1,mu)/sigma, eye(1)/sigma, ones(1))); - bn.push_back(conditional); - return bn; -} - -/* ************************************************************************* */ -GaussianBayesNet simpleGaussian(Index key, const Vector& mu, double sigma) { - GaussianBayesNet bn; - size_t n = mu.size(); - GaussianConditional::shared_ptr - conditional(new GaussianConditional(key, mu/sigma, eye(n)/sigma, ones(n))); - bn.push_back(conditional); - return bn; -} - -/* ************************************************************************* */ -void push_front(GaussianBayesNet& bn, Index key, Vector d, Matrix R, - Index name1, Matrix S, Vector sigmas) { - GaussianConditional::shared_ptr cg(new GaussianConditional(key, d, R, name1, S, sigmas)); - bn.push_front(cg); -} - -/* ************************************************************************* */ -void push_front(GaussianBayesNet& bn, Index key, Vector d, Matrix R, - Index name1, Matrix S, Index name2, Matrix T, Vector sigmas) { - GaussianConditional::shared_ptr cg(new GaussianConditional(key, d, R, name1, S, name2, T, sigmas)); - bn.push_front(cg); -} - -/* ************************************************************************* */ -boost::shared_ptr allocateVectorValues(const GaussianBayesNet& bn) { - vector dimensions(bn.size()); - Index var = 0; - BOOST_FOREACH(const boost::shared_ptr conditional, bn) { - dimensions[var++] = conditional->dim(); - } - return boost::shared_ptr(new VectorValues(dimensions)); -} - -/* ************************************************************************* */ -VectorValues optimize(const GaussianBayesNet& bn) { - VectorValues x = *allocateVectorValues(bn); - optimizeInPlace(bn, x); - return x; -} - -/* ************************************************************************* */ -// (R*x)./sigmas = y by solving x=inv(R)*(y.*sigmas) -void optimizeInPlace(const GaussianBayesNet& bn, VectorValues& x) { - /** solve each node in turn in topological sort order (parents first)*/ - BOOST_REVERSE_FOREACH(const boost::shared_ptr cg, bn) { - // i^th part of R*x=y, x=inv(R)*y - // (Rii*xi + R_i*x(i+1:))./si = yi <-> xi = inv(Rii)*(yi.*si - R_i*x(i+1:)) - cg->solveInPlace(x); - } -} - -/* ************************************************************************* */ -VectorValues backSubstitute(const GaussianBayesNet& bn, const VectorValues& input) { - VectorValues output = input; - BOOST_REVERSE_FOREACH(const boost::shared_ptr cg, bn) { - const Index key = *(cg->beginFrontals()); - Vector xS = internal::extractVectorValuesSlices(output, cg->beginParents(), cg->endParents()); - xS = input[key] - cg->get_S() * xS; - output[key] = cg->get_R().triangularView().solve(xS); + /* ************************************************************************* */ + bool GaussianBayesNet::equals(const This& bn, double tol) const + { + return Base::equals(bn, tol); } - BOOST_FOREACH(const boost::shared_ptr cg, bn) { - cg->scaleFrontalsBySigma(output); + /* ************************************************************************* */ + VectorValues GaussianBayesNet::optimize() const + { + VectorValues soln; // no missing variables -> just create an empty vector + return optimize(soln); } - return output; -} - - -/* ************************************************************************* */ -// gy=inv(L)*gx by solving L*gy=gx. -// gy=inv(R'*inv(Sigma))*gx -// gz'*R'=gx', gy = gz.*sigmas -VectorValues backSubstituteTranspose(const GaussianBayesNet& bn, - const VectorValues& gx) { - - // Initialize gy from gx - // TODO: used to insert zeros if gx did not have an entry for a variable in bn - VectorValues gy = gx; - - // we loop from first-eliminated to last-eliminated - // i^th part of L*gy=gx is done block-column by block-column of L - BOOST_FOREACH(const boost::shared_ptr cg, bn) - cg->solveTransposeInPlace(gy); - - // Scale gy - BOOST_FOREACH(GaussianConditional::shared_ptr cg, bn) - cg->scaleFrontalsBySigma(gy); - - return gy; -} - -/* ************************************************************************* */ -VectorValues optimizeGradientSearch(const GaussianBayesNet& Rd) { - gttic(Allocate_VectorValues); - VectorValues grad = *allocateVectorValues(Rd); - gttoc(Allocate_VectorValues); - - optimizeGradientSearchInPlace(Rd, grad); - - return grad; -} - -/* ************************************************************************* */ -void optimizeGradientSearchInPlace(const GaussianBayesNet& Rd, VectorValues& grad) { - gttic(Compute_Gradient); - // Compute gradient (call gradientAtZero function, which is defined for various linear systems) - gradientAtZero(Rd, grad); - double gradientSqNorm = grad.dot(grad); - gttoc(Compute_Gradient); - - gttic(Compute_Rg); - // Compute R * g - FactorGraph Rd_jfg(Rd); - Errors Rg = Rd_jfg * grad; - gttoc(Compute_Rg); - - gttic(Compute_minimizing_step_size); - // Compute minimizing step size - double step = -gradientSqNorm / dot(Rg, Rg); - gttoc(Compute_minimizing_step_size); - - gttic(Compute_point); - // Compute steepest descent point - scal(step, grad); - gttoc(Compute_point); -} - -/* ************************************************************************* */ -pair matrix(const GaussianBayesNet& bn) { - - // add the dimensions of all variables to get matrix dimension - // and at the same time create a mapping from keys to indices - size_t N=0; map mapping; - BOOST_FOREACH(GaussianConditional::shared_ptr cg,bn) { - GaussianConditional::const_iterator it = cg->beginFrontals(); - for (; it != cg->endFrontals(); ++it) { - mapping.insert(make_pair(*it,N)); - N += cg->dim(it); + /* ************************************************************************* */ + VectorValues GaussianBayesNet::optimize( + const VectorValues& solutionForMissing) const { + VectorValues soln(solutionForMissing); // possibly empty + // (R*x)./sigmas = y by solving x=inv(R)*(y.*sigmas) + /** solve each node in turn in topological sort order (parents first)*/ + BOOST_REVERSE_FOREACH(const sharedConditional& cg, *this) { + // i^th part of R*x=y, x=inv(R)*y + // (Rii*xi + R_i*x(i+1:))./si = yi <-> xi = inv(Rii)*(yi.*si - R_i*x(i+1:)) + soln.insert(cg->solve(soln)); } + return soln; } - // create matrix and copy in values - Matrix R = zeros(N,N); - Vector d(N); - Index key; size_t I; - FOREACH_PAIR(key,I,mapping) { - // find corresponding conditional - boost::shared_ptr cg = bn[key]; - - // get sigmas - Vector sigmas = cg->get_sigmas(); - - // get RHS and copy to d - GaussianConditional::const_d_type d_ = cg->get_d(); - const size_t n = d_.size(); - for (size_t i=0;iget_R(); - for (size_t i=0;ibeginParents(); - for (; keyS!=cg->endParents(); keyS++) { - Matrix S = cg->get_S(keyS); // get S matrix - const size_t m = S.rows(), n = S.cols(); // find S size - const size_t J = mapping[*keyS]; // find column index - for (size_t i=0;i cg, bayesNet){ - logDet += cg->get_R().diagonal().unaryExpr(ptr_fun(log)).sum(); + /* ************************************************************************* */ + VectorValues GaussianBayesNet::optimizeGradientSearch() const + { + gttic(GaussianBayesTree_optimizeGradientSearch); + return GaussianFactorGraph(*this).optimizeGradientSearch(); } - return exp(logDet); -} + /* ************************************************************************* */ + VectorValues GaussianBayesNet::gradient(const VectorValues& x0) const { + return GaussianFactorGraph(*this).gradient(x0); + } -/* ************************************************************************* */ -VectorValues gradient(const GaussianBayesNet& bayesNet, const VectorValues& x0) { - return gradient(GaussianFactorGraph(bayesNet), x0); -} + /* ************************************************************************* */ + VectorValues GaussianBayesNet::gradientAtZero() const { + return GaussianFactorGraph(*this).gradientAtZero(); + } -/* ************************************************************************* */ -void gradientAtZero(const GaussianBayesNet& bayesNet, VectorValues& g) { - gradientAtZero(GaussianFactorGraph(bayesNet), g); -} + /* ************************************************************************* */ + double GaussianBayesNet::error(const VectorValues& x) const { + return GaussianFactorGraph(*this).error(x); + } -/* ************************************************************************* */ + /* ************************************************************************* */ + VectorValues GaussianBayesNet::backSubstitute(const VectorValues& rhs) const + { + VectorValues result; + BOOST_REVERSE_FOREACH(const sharedConditional& cg, *this) { + result.insert(cg->solveOtherRHS(result, rhs)); + } + return result; + } + + + /* ************************************************************************* */ + // gy=inv(L)*gx by solving L*gy=gx. + // gy=inv(R'*inv(Sigma))*gx + // gz'*R'=gx', gy = gz.*sigmas + VectorValues GaussianBayesNet::backSubstituteTranspose(const VectorValues& gx) const + { + // Initialize gy from gx + // TODO: used to insert zeros if gx did not have an entry for a variable in bn + VectorValues gy = gx; + + // we loop from first-eliminated to last-eliminated + // i^th part of L*gy=gx is done block-column by block-column of L + BOOST_FOREACH(const sharedConditional& cg, *this) + cg->solveTransposeInPlace(gy); + + return gy; + } + + ///* ************************************************************************* */ + //VectorValues GaussianBayesNet::optimizeGradientSearch() const + //{ + // gttic(Compute_Gradient); + // // Compute gradient (call gradientAtZero function, which is defined for various linear systems) + // VectorValues grad = gradientAtZero(); + // double gradientSqNorm = grad.dot(grad); + // gttoc(Compute_Gradient); + + // gttic(Compute_Rg); + // // Compute R * g + // Errors Rg = GaussianFactorGraph(*this) * grad; + // gttoc(Compute_Rg); + + // gttic(Compute_minimizing_step_size); + // // Compute minimizing step size + // double step = -gradientSqNorm / dot(Rg, Rg); + // gttoc(Compute_minimizing_step_size); + + // gttic(Compute_point); + // // Compute steepest descent point + // scal(step, grad); + // gttoc(Compute_point); + + // return grad; + //} + + /* ************************************************************************* */ + pair GaussianBayesNet::matrix() const + { + return GaussianFactorGraph(*this).jacobian(); + } + + ///* ************************************************************************* */ + //VectorValues GaussianBayesNet::gradient(const VectorValues& x0) const + //{ + // return GaussianFactorGraph(*this).gradient(x0); + //} + + ///* ************************************************************************* */ + //VectorValues GaussianBayesNet::gradientAtZero() const + //{ + // return GaussianFactorGraph(*this).gradientAtZero(); + //} + + /* ************************************************************************* */ + double GaussianBayesNet::determinant() const + { + return exp(logDeterminant()); + } + + /* ************************************************************************* */ + double GaussianBayesNet::logDeterminant() const + { + double logDet = 0.0; + BOOST_FOREACH(const sharedConditional& cg, *this) { + if(cg->get_model()) { + Vector diag = cg->get_R().diagonal(); + cg->get_model()->whitenInPlace(diag); + logDet += diag.unaryExpr(ptr_fun(log)).sum(); + } else { + logDet += cg->get_R().diagonal().unaryExpr(ptr_fun(log)).sum(); + } + } + return logDet; + } + + /* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h index 3052e3926..69a70a5e4 100644 --- a/gtsam/linear/GaussianBayesNet.h +++ b/gtsam/linear/GaussianBayesNet.h @@ -20,142 +20,155 @@ #pragma once -#include #include -#include +#include +#include namespace gtsam { /** A Bayes net made from linear-Gaussian densities */ - typedef BayesNet GaussianBayesNet; + class GTSAM_EXPORT GaussianBayesNet: public FactorGraph + { + public: - /** Create a scalar Gaussian */ - GTSAM_EXPORT GaussianBayesNet scalarGaussian(Index key, double mu=0.0, double sigma=1.0); + typedef FactorGraph Base; + typedef GaussianBayesNet This; + typedef GaussianConditional ConditionalType; + typedef boost::shared_ptr shared_ptr; + typedef boost::shared_ptr sharedConditional; - /** Create a simple Gaussian on a single multivariate variable */ - GTSAM_EXPORT GaussianBayesNet simpleGaussian(Index key, const Vector& mu, double sigma=1.0); + /// @name Standard Constructors + /// @{ - /** - * Add a conditional node with one parent - * |Rx+Sy-d| - */ - GTSAM_EXPORT void push_front(GaussianBayesNet& bn, Index key, Vector d, Matrix R, - Index name1, Matrix S, Vector sigmas); + /** Construct empty factor graph */ + GaussianBayesNet() {} - /** - * Add a conditional node with two parents - * |Rx+Sy+Tz-d| - */ - GTSAM_EXPORT void push_front(GaussianBayesNet& bn, Index key, Vector d, Matrix R, - Index name1, Matrix S, Index name2, Matrix T, Vector sigmas); + /** Construct from iterator over conditionals */ + template + GaussianBayesNet(ITERATOR firstConditional, ITERATOR lastConditional) : Base(firstConditional, lastConditional) {} - /** - * Allocate a VectorValues for the variables in a BayesNet - */ - GTSAM_EXPORT boost::shared_ptr allocateVectorValues(const GaussianBayesNet& bn); + /** Construct from container of factors (shared_ptr or plain objects) */ + template + explicit GaussianBayesNet(const CONTAINER& conditionals) : Base(conditionals) {} - /** - * Solve the GaussianBayesNet, i.e. return \f$ x = R^{-1}*d \f$, computed by - * back-substitution. - */ - GTSAM_EXPORT VectorValues optimize(const GaussianBayesNet& bn); + /** Implicit copy/downcast constructor to override explicit template container constructor */ + template + GaussianBayesNet(const FactorGraph& graph) : Base(graph) {} - /** - * Solve the GaussianBayesNet, i.e. return \f$ x = R^{-1}*d \f$, computed by - * back-substitution, writes the solution \f$ x \f$ into a pre-allocated - * VectorValues. You can use allocateVectorValues(const GaussianBayesNet&) - * allocate it. See also optimize(const GaussianBayesNet&), which does not - * require pre-allocation. - */ - GTSAM_EXPORT void optimizeInPlace(const GaussianBayesNet& bn, VectorValues& x); + /// @} - /** - * Optimize along the gradient direction, with a closed-form computation to - * perform the line search. The gradient is computed about \f$ \delta x=0 \f$. - * - * This function returns \f$ \delta x \f$ that minimizes a reparametrized - * problem. The error function of a GaussianBayesNet is - * - * \f[ f(\delta x) = \frac{1}{2} |R \delta x - d|^2 = \frac{1}{2}d^T d - d^T R \delta x + \frac{1}{2} \delta x^T R^T R \delta x \f] - * - * with gradient and Hessian - * - * \f[ g(\delta x) = R^T(R\delta x - d), \qquad G(\delta x) = R^T R. \f] - * - * This function performs the line search in the direction of the - * gradient evaluated at \f$ g = g(\delta x = 0) \f$ with step size - * \f$ \alpha \f$ that minimizes \f$ f(\delta x = \alpha g) \f$: - * - * \f[ f(\alpha) = \frac{1}{2} d^T d + g^T \delta x + \frac{1}{2} \alpha^2 g^T G g \f] - * - * Optimizing by setting the derivative to zero yields - * \f$ \hat \alpha = (-g^T g) / (g^T G g) \f$. For efficiency, this function - * evaluates the denominator without computing the Hessian \f$ G \f$, returning - * - * \f[ \delta x = \hat\alpha g = \frac{-g^T g}{(R g)^T(R g)} \f] - * - * @param bn The GaussianBayesNet on which to perform this computation - * @return The resulting \f$ \delta x \f$ as described above - */ - GTSAM_EXPORT VectorValues optimizeGradientSearch(const GaussianBayesNet& bn); + /// @name Testable + /// @{ - /** In-place version of optimizeGradientSearch(const GaussianBayesNet&) requiring pre-allocated VectorValues \c grad - * - * @param bn The GaussianBayesNet on which to perform this computation - * @param [out] grad The resulting \f$ \delta x \f$ as described in optimizeGradientSearch(const GaussianBayesNet&) - * */ - GTSAM_EXPORT void optimizeGradientSearchInPlace(const GaussianBayesNet& bn, VectorValues& grad); + /** Check equality */ + bool equals(const This& bn, double tol = 1e-9) const; - /** - * Backsubstitute - * gy=inv(R*inv(Sigma))*gx - */ - GTSAM_EXPORT VectorValues backSubstitute(const GaussianBayesNet& bn, const VectorValues& gx); + /// @} - /** - * Transpose Backsubstitute - * gy=inv(L)*gx by solving L*gy=gx. - * gy=inv(R'*inv(Sigma))*gx - * gz'*R'=gx', gy = gz.*sigmas - */ - GTSAM_EXPORT VectorValues backSubstituteTranspose(const GaussianBayesNet& bn, const VectorValues& gx); + /// @name Standard Interface + /// @{ - /** - * Return (dense) upper-triangular matrix representation - */ - GTSAM_EXPORT std::pair matrix(const GaussianBayesNet&); + /// Solve the GaussianBayesNet, i.e. return \f$ x = R^{-1}*d \f$, by back-substitution + VectorValues optimize() const; - /** - * Computes the determinant of a GassianBayesNet - * A GaussianBayesNet is an upper triangular matrix and for an upper triangular matrix - * determinant is the product of the diagonal elements. Instead of actually multiplying - * we add the logarithms of the diagonal elements and take the exponent at the end - * because this is more numerically stable. - * @param bayesNet The input GaussianBayesNet - * @return The determinant - */ - GTSAM_EXPORT double determinant(const GaussianBayesNet& bayesNet); + /// Version of optimize for incomplete BayesNet, needs solution for missing variables + VectorValues optimize(const VectorValues& solutionForMissing) const; - /** - * Compute the gradient of the energy function, - * \f$ \nabla_{x=x_0} \left\Vert \Sigma^{-1} R x - d \right\Vert^2 \f$, - * centered around \f$ x = x_0 \f$. - * The gradient is \f$ R^T(Rx-d) \f$. - * @param bayesNet The Gaussian Bayes net $(R,d)$ - * @param x0 The center about which to compute the gradient - * @return The gradient as a VectorValues - */ - GTSAM_EXPORT VectorValues gradient(const GaussianBayesNet& bayesNet, const VectorValues& x0); + ///@} - /** - * Compute the gradient of the energy function, - * \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} R x - d \right\Vert^2 \f$, - * centered around zero. - * The gradient about zero is \f$ -R^T d \f$. See also gradient(const GaussianBayesNet&, const VectorValues&). - * @param bayesNet The Gaussian Bayes net $(R,d)$ - * @param [output] g A VectorValues to store the gradient, which must be preallocated, see allocateVectorValues - * @return The gradient as a VectorValues - */ - GTSAM_EXPORT void gradientAtZero(const GaussianBayesNet& bayesNet, VectorValues& g); + ///@name Linear Algebra + ///@{ + + /** + * Return (dense) upper-triangular matrix representation + */ + std::pair matrix() const; + + /** + * Optimize along the gradient direction, with a closed-form computation to perform the line + * search. The gradient is computed about \f$ \delta x=0 \f$. + * + * This function returns \f$ \delta x \f$ that minimizes a reparametrized problem. The error + * function of a GaussianBayesNet is + * + * \f[ f(\delta x) = \frac{1}{2} |R \delta x - d|^2 = \frac{1}{2}d^T d - d^T R \delta x + + * \frac{1}{2} \delta x^T R^T R \delta x \f] + * + * with gradient and Hessian + * + * \f[ g(\delta x) = R^T(R\delta x - d), \qquad G(\delta x) = R^T R. \f] + * + * This function performs the line search in the direction of the gradient evaluated at \f$ g = + * g(\delta x = 0) \f$ with step size \f$ \alpha \f$ that minimizes \f$ f(\delta x = \alpha g) + * \f$: + * + * \f[ f(\alpha) = \frac{1}{2} d^T d + g^T \delta x + \frac{1}{2} \alpha^2 g^T G g \f] + * + * Optimizing by setting the derivative to zero yields \f$ \hat \alpha = (-g^T g) / (g^T G g) + * \f$. For efficiency, this function evaluates the denominator without computing the Hessian + * \f$ G \f$, returning + * + * \f[ \delta x = \hat\alpha g = \frac{-g^T g}{(R g)^T(R g)} \f] */ + VectorValues optimizeGradientSearch() const; + + /** Compute the gradient of the energy function, \f$ \nabla_{x=x_0} \left\Vert \Sigma^{-1} R x - + * d \right\Vert^2 \f$, centered around \f$ x = x_0 \f$. The gradient is \f$ R^T(Rx-d) \f$. + * + * @param x0 The center about which to compute the gradient + * @return The gradient as a VectorValues */ + VectorValues gradient(const VectorValues& x0) const; + + /** Compute the gradient of the energy function, \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} R x - d + * \right\Vert^2 \f$, centered around zero. The gradient about zero is \f$ -R^T d \f$. See also + * gradient(const GaussianBayesNet&, const VectorValues&). + * + * @param [output] g A VectorValues to store the gradient, which must be preallocated, see + * allocateVectorValues */ + VectorValues gradientAtZero() const; + + /** Mahalanobis norm error. */ + double error(const VectorValues& x) const; + + /** + * Computes the determinant of a GassianBayesNet. A GaussianBayesNet is an upper triangular + * matrix and for an upper triangular matrix determinant is the product of the diagonal + * elements. Instead of actually multiplying we add the logarithms of the diagonal elements and + * take the exponent at the end because this is more numerically stable. + * @param bayesNet The input GaussianBayesNet + * @return The determinant */ + double determinant() const; + + /** + * Computes the log of the determinant of a GassianBayesNet. A GaussianBayesNet is an upper + * triangular matrix and for an upper triangular matrix determinant is the product of the + * diagonal elements. + * @param bayesNet The input GaussianBayesNet + * @return The determinant */ + double logDeterminant() const; + + /** + * Backsubstitute with a different RHS vector than the one stored in this BayesNet. + * gy=inv(R*inv(Sigma))*gx + */ + VectorValues backSubstitute(const VectorValues& gx) const; + + /** + * Transpose backsubstitute with a different RHS vector than the one stored in this BayesNet. + * gy=inv(L)*gx by solving L*gy=gx. + * gy=inv(R'*inv(Sigma))*gx + * gz'*R'=gx', gy = gz.*sigmas + */ + VectorValues backSubstituteTranspose(const VectorValues& gx) const; + + /// @} + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + } + }; } /// namespace gtsam diff --git a/gtsam/linear/GaussianBayesTree.cpp b/gtsam/linear/GaussianBayesTree.cpp index 17d54635c..0ceecdfd2 100644 --- a/gtsam/linear/GaussianBayesTree.cpp +++ b/gtsam/linear/GaussianBayesTree.cpp @@ -17,82 +17,90 @@ * @author Richard Roberts */ +#include +#include +#include +#include #include -#include +#include +#include namespace gtsam { -/* ************************************************************************* */ -VectorValues optimize(const GaussianBayesTree& bayesTree) { - VectorValues result = *allocateVectorValues(bayesTree); - optimizeInPlace(bayesTree, result); - return result; -} + // Instantiate base class + template class BayesTreeCliqueBase; + template class BayesTree; -/* ************************************************************************* */ -void optimizeInPlace(const GaussianBayesTree& bayesTree, VectorValues& result) { - internal::optimizeInPlace(bayesTree.root(), result); -} + /* ************************************************************************* */ + namespace internal + { + /* ************************************************************************* */ + double logDeterminant(const GaussianBayesTreeClique::shared_ptr& clique, double& parentSum) + { + parentSum += clique->conditional()->get_R().diagonal().unaryExpr(std::ptr_fun(log)).sum(); + assert(false); + return 0; + } + } -/* ************************************************************************* */ -VectorValues optimizeGradientSearch(const GaussianBayesTree& bayesTree) { - gttic(Allocate_VectorValues); - VectorValues grad = *allocateVectorValues(bayesTree); - gttoc(Allocate_VectorValues); + /* ************************************************************************* */ + bool GaussianBayesTree::equals(const This& other, double tol) const + { + return Base::equals(other, tol); + } - optimizeGradientSearchInPlace(bayesTree, grad); + /* ************************************************************************* */ + VectorValues GaussianBayesTree::optimize() const + { + return internal::linearAlgorithms::optimizeBayesTree(*this); + } - return grad; -} + /* ************************************************************************* */ + VectorValues GaussianBayesTree::optimizeGradientSearch() const + { + gttic(GaussianBayesTree_optimizeGradientSearch); + return GaussianFactorGraph(*this).optimizeGradientSearch(); + } -/* ************************************************************************* */ -void optimizeGradientSearchInPlace(const GaussianBayesTree& bayesTree, VectorValues& grad) { - gttic(Compute_Gradient); - // Compute gradient (call gradientAtZero function, which is defined for various linear systems) - gradientAtZero(bayesTree, grad); - double gradientSqNorm = grad.dot(grad); - gttoc(Compute_Gradient); + /* ************************************************************************* */ + VectorValues GaussianBayesTree::gradient(const VectorValues& x0) const { + return GaussianFactorGraph(*this).gradient(x0); + } - gttic(Compute_Rg); - // Compute R * g - FactorGraph Rd_jfg(bayesTree); - Errors Rg = Rd_jfg * grad; - gttoc(Compute_Rg); + /* ************************************************************************* */ + VectorValues GaussianBayesTree::gradientAtZero() const { + return GaussianFactorGraph(*this).gradientAtZero(); + } - gttic(Compute_minimizing_step_size); - // Compute minimizing step size - double step = -gradientSqNorm / dot(Rg, Rg); - gttoc(Compute_minimizing_step_size); + /* ************************************************************************* */ + double GaussianBayesTree::error(const VectorValues& x) const { + return GaussianFactorGraph(*this).error(x); + } - gttic(Compute_point); - // Compute steepest descent point - scal(step, grad); - gttoc(Compute_point); -} + /* ************************************************************************* */ + double GaussianBayesTree::logDeterminant() const + { + if(this->roots_.empty()) { + return 0.0; + } else { + double sum = 0.0; + treeTraversal::DepthFirstForest(*this, sum, internal::logDeterminant); + return sum; + } + } -/* ************************************************************************* */ -VectorValues gradient(const GaussianBayesTree& bayesTree, const VectorValues& x0) { - return gradient(FactorGraph(bayesTree), x0); -} + /* ************************************************************************* */ + double GaussianBayesTree::determinant() const + { + return exp(logDeterminant()); + } -/* ************************************************************************* */ -void gradientAtZero(const GaussianBayesTree& bayesTree, VectorValues& g) { - gradientAtZero(FactorGraph(bayesTree), g); -} + /* ************************************************************************* */ + Matrix GaussianBayesTree::marginalCovariance(Key key) const + { + return marginalFactor(key)->information().inverse(); + } -/* ************************************************************************* */ -double determinant(const GaussianBayesTree& bayesTree) { - return exp(logDeterminant(bayesTree)); -} - -/* ************************************************************************* */ -double logDeterminant(const GaussianBayesTree& bayesTree) { - if (!bayesTree.root()) - return 0.0; - - return internal::logDeterminant(bayesTree.root()); -} -/* ************************************************************************* */ } // \namespace gtsam diff --git a/gtsam/linear/GaussianBayesTree.h b/gtsam/linear/GaussianBayesTree.h index 63411b610..5185154b4 100644 --- a/gtsam/linear/GaussianBayesTree.h +++ b/gtsam/linear/GaussianBayesTree.h @@ -19,106 +19,113 @@ #pragma once +#include +#include #include -#include -#include +#include namespace gtsam { -/// A Bayes Tree representing a Gaussian density -GTSAM_EXPORT typedef BayesTree GaussianBayesTree; + // Forward declarations + class GaussianConditional; + class VectorValues; -/// optimize the BayesTree, starting from the root -GTSAM_EXPORT VectorValues optimize(const GaussianBayesTree& bayesTree); + /* ************************************************************************* */ + /** A clique in a GaussianBayesTree */ + class GTSAM_EXPORT GaussianBayesTreeClique : + public BayesTreeCliqueBase + { + public: + typedef GaussianBayesTreeClique This; + typedef BayesTreeCliqueBase Base; + typedef boost::shared_ptr shared_ptr; + typedef boost::weak_ptr weak_ptr; + GaussianBayesTreeClique() {} + GaussianBayesTreeClique(const boost::shared_ptr& conditional) : Base(conditional) {} + }; -/// recursively optimize this conditional and all subtrees -GTSAM_EXPORT void optimizeInPlace(const GaussianBayesTree& bayesTree, VectorValues& result); + /* ************************************************************************* */ + /** A Bayes tree representing a Gaussian density */ + class GTSAM_EXPORT GaussianBayesTree : + public BayesTree + { + private: + typedef BayesTree Base; -namespace internal { -template -void optimizeInPlace(const typename BAYESTREE::sharedClique& clique, VectorValues& result); -} + public: + typedef GaussianBayesTree This; + typedef boost::shared_ptr shared_ptr; -/** - * Optimize along the gradient direction, with a closed-form computation to - * perform the line search. The gradient is computed about \f$ \delta x=0 \f$. - * - * This function returns \f$ \delta x \f$ that minimizes a reparametrized - * problem. The error function of a GaussianBayesNet is - * - * \f[ f(\delta x) = \frac{1}{2} |R \delta x - d|^2 = \frac{1}{2}d^T d - d^T R \delta x + \frac{1}{2} \delta x^T R^T R \delta x \f] - * - * with gradient and Hessian - * - * \f[ g(\delta x) = R^T(R\delta x - d), \qquad G(\delta x) = R^T R. \f] - * - * This function performs the line search in the direction of the - * gradient evaluated at \f$ g = g(\delta x = 0) \f$ with step size - * \f$ \alpha \f$ that minimizes \f$ f(\delta x = \alpha g) \f$: - * - * \f[ f(\alpha) = \frac{1}{2} d^T d + g^T \delta x + \frac{1}{2} \alpha^2 g^T G g \f] - * - * Optimizing by setting the derivative to zero yields - * \f$ \hat \alpha = (-g^T g) / (g^T G g) \f$. For efficiency, this function - * evaluates the denominator without computing the Hessian \f$ G \f$, returning - * - * \f[ \delta x = \hat\alpha g = \frac{-g^T g}{(R g)^T(R g)} \f] - */ -GTSAM_EXPORT VectorValues optimizeGradientSearch(const GaussianBayesTree& bayesTree); + /** Default constructor, creates an empty Bayes tree */ + GaussianBayesTree() {} -/** In-place version of optimizeGradientSearch requiring pre-allocated VectorValues \c x */ -GTSAM_EXPORT void optimizeGradientSearchInPlace(const GaussianBayesTree& bayesTree, VectorValues& grad); + /** Check equality */ + bool equals(const This& other, double tol = 1e-9) const; -/** - * Compute the gradient of the energy function, - * \f$ \nabla_{x=x_0} \left\Vert \Sigma^{-1} R x - d \right\Vert^2 \f$, - * centered around \f$ x = x_0 \f$. - * The gradient is \f$ R^T(Rx-d) \f$. - * @param bayesTree The Gaussian Bayes Tree $(R,d)$ - * @param x0 The center about which to compute the gradient - * @return The gradient as a VectorValues - */ -GTSAM_EXPORT VectorValues gradient(const GaussianBayesTree& bayesTree, const VectorValues& x0); + /** Recursively optimize the BayesTree to produce a vector solution. */ + VectorValues optimize() const; -/** - * Compute the gradient of the energy function, - * \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} R x - d \right\Vert^2 \f$, - * centered around zero. - * The gradient about zero is \f$ -R^T d \f$. See also gradient(const GaussianBayesNet&, const VectorValues&). - * @param bayesTree The Gaussian Bayes Tree $(R,d)$ - * @param [output] g A VectorValues to store the gradient, which must be preallocated, see allocateVectorValues - * @return The gradient as a VectorValues - */ -GTSAM_EXPORT void gradientAtZero(const GaussianBayesTree& bayesTree, VectorValues& g); + /** + * Optimize along the gradient direction, with a closed-form computation to perform the line + * search. The gradient is computed about \f$ \delta x=0 \f$. + * + * This function returns \f$ \delta x \f$ that minimizes a reparametrized problem. The error + * function of a GaussianBayesNet is + * + * \f[ f(\delta x) = \frac{1}{2} |R \delta x - d|^2 = \frac{1}{2}d^T d - d^T R \delta x + + * \frac{1}{2} \delta x^T R^T R \delta x \f] + * + * with gradient and Hessian + * + * \f[ g(\delta x) = R^T(R\delta x - d), \qquad G(\delta x) = R^T R. \f] + * + * This function performs the line search in the direction of the gradient evaluated at \f$ g = + * g(\delta x = 0) \f$ with step size \f$ \alpha \f$ that minimizes \f$ f(\delta x = \alpha g) + * \f$: + * + * \f[ f(\alpha) = \frac{1}{2} d^T d + g^T \delta x + \frac{1}{2} \alpha^2 g^T G g \f] + * + * Optimizing by setting the derivative to zero yields \f$ \hat \alpha = (-g^T g) / (g^T G g) + * \f$. For efficiency, this function evaluates the denominator without computing the Hessian + * \f$ G \f$, returning + * + * \f[ \delta x = \hat\alpha g = \frac{-g^T g}{(R g)^T(R g)} \f] */ + VectorValues optimizeGradientSearch() const; -/** - * Computes the determinant of a GassianBayesTree - * A GassianBayesTree is an upper triangular matrix and for an upper triangular matrix - * determinant is the product of the diagonal elements. Instead of actually multiplying - * we add the logarithms of the diagonal elements and take the exponent at the end - * because this is more numerically stable. - * @param bayesTree The input GassianBayesTree - * @return The determinant - */ -GTSAM_EXPORT double determinant(const GaussianBayesTree& bayesTree); + /** Compute the gradient of the energy function, \f$ \nabla_{x=x_0} \left\Vert \Sigma^{-1} R x - + * d \right\Vert^2 \f$, centered around \f$ x = x_0 \f$. The gradient is \f$ R^T(Rx-d) \f$. + * + * @param x0 The center about which to compute the gradient + * @return The gradient as a VectorValues */ + VectorValues gradient(const VectorValues& x0) const; -/** - * Computes the log determinant of a GassianBayesTree - * A GassianBayesTree is an upper triangular matrix and for an upper triangular matrix - * determinant is the product of the diagonal elements. Instead of actually multiplying - * we add the logarithms of the diagonal elements because this is more numerically stable. - * @param bayesTree The input GassianBayesTree - * @return The log determinant - */ -GTSAM_EXPORT double logDeterminant(const GaussianBayesTree& bayesTree); + /** Compute the gradient of the energy function, \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} R x - d + * \right\Vert^2 \f$, centered around zero. The gradient about zero is \f$ -R^T d \f$. See also + * gradient(const GaussianBayesNet&, const VectorValues&). + * + * @return A VectorValues storing the gradient. */ + VectorValues gradientAtZero() const; + /** Mahalanobis norm error. */ + double error(const VectorValues& x) const; -namespace internal { -template -double logDeterminant(const typename BAYESTREE::sharedClique& clique); -} + /** Computes the determinant of a GassianBayesTree, as if the Bayes tree is reorganized into a + * matrix. A GassianBayesTree is equivalent to an upper triangular matrix, and for an upper + * triangular matrix determinant is the product of the diagonal elements. Instead of actually + * multiplying we add the logarithms of the diagonal elements and take the exponent at the end + * because this is more numerically stable. */ + double determinant() const; + + /** Computes the determinant of a GassianBayesTree, as if the Bayes tree is reorganized into a + * matrix. A GassianBayesTree is equivalent to an upper triangular matrix, and for an upper + * triangular matrix determinant is the product of the diagonal elements. Instead of actually + * multiplying we add the logarithms of the diagonal elements and take the exponent at the end + * because this is more numerically stable. */ + double logDeterminant() const; + + /** Return the marginal on the requested variable as a covariance matrix. See also + * marginalFactor(). */ + Matrix marginalCovariance(Key key) const; + }; } - -#include - diff --git a/gtsam/linear/GaussianConditional-inl.h b/gtsam/linear/GaussianConditional-inl.h new file mode 100644 index 000000000..45d7ecc3b --- /dev/null +++ b/gtsam/linear/GaussianConditional-inl.h @@ -0,0 +1,36 @@ +/* ---------------------------------------------------------------------------- + + * 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 GaussianConditional-inl.h + * @brief Conditional Gaussian Base class + * @author Christian Potthast + */ + +// \callgraph + +#pragma once + +namespace gtsam { + + /* ************************************************************************* */ + template + GaussianConditional::GaussianConditional(const TERMS& terms, + size_t nrFrontals, const Vector& d, const SharedDiagonal& sigmas) : + BaseFactor(terms, d, sigmas), BaseConditional(nrFrontals) {} + + /* ************************************************************************* */ + template + GaussianConditional::GaussianConditional( + const KEYS& keys, size_t nrFrontals, const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& sigmas) : + BaseFactor(keys, augmentedMatrix, sigmas), BaseConditional(nrFrontals) {} + +} // gtsam diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index 16b04af14..a5c651a44 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -16,6 +16,7 @@ */ #include +#include #include #ifdef __GNUC__ #pragma GCC diagnostic push @@ -29,210 +30,182 @@ #include #include -#include -#include +#include using namespace std; namespace gtsam { -/* ************************************************************************* */ -GaussianConditional::GaussianConditional() : rsd_(matrix_) {} + /* ************************************************************************* */ + GaussianConditional::GaussianConditional( + Key key, const Vector& d, const Matrix& R, const SharedDiagonal& sigmas) : + BaseFactor(key, R, d, sigmas), BaseConditional(1) {} -/* ************************************************************************* */ -GaussianConditional::GaussianConditional(Index key) : IndexConditional(key), rsd_(matrix_) {} + /* ************************************************************************* */ + GaussianConditional::GaussianConditional( + Key key, const Vector& d, const Matrix& R, + Key name1, const Matrix& S, const SharedDiagonal& sigmas) : + BaseFactor(key, R, name1, S, d, sigmas), BaseConditional(1) {} -/* ************************************************************************* */ -GaussianConditional::GaussianConditional(Index key,const Vector& d, const Matrix& R, const Vector& sigmas) : - IndexConditional(key), rsd_(matrix_), sigmas_(sigmas) { - assert(R.rows() <= R.cols()); - size_t dims[] = { R.cols(), 1 }; - rsd_.copyStructureFrom(rsd_type(matrix_, dims, dims+2, d.size())); - rsd_(0) = R.triangularView(); - get_d_() = d; -} + /* ************************************************************************* */ + GaussianConditional::GaussianConditional( + Key key, const Vector& d, const Matrix& R, + Key name1, const Matrix& S, Key name2, const Matrix& T, const SharedDiagonal& sigmas) : + BaseFactor(key, R, name1, S, name2, T, d, sigmas), BaseConditional(1) {} -/* ************************************************************************* */ -GaussianConditional::GaussianConditional(Index key, const Vector& d, const Matrix& R, - Index name1, const Matrix& S, const Vector& sigmas) : - IndexConditional(key,name1), rsd_(matrix_), sigmas_(sigmas) { - assert(R.rows() <= R.cols()); - size_t dims[] = { R.cols(), S.cols(), 1 }; - rsd_.copyStructureFrom(rsd_type(matrix_, dims, dims+3, d.size())); - rsd_(0) = R.triangularView(); - rsd_(1) = S; - get_d_() = d; -} + /* ************************************************************************* */ + void GaussianConditional::print(const string &s, const KeyFormatter& formatter) const + { + cout << s << " Conditional density "; + for(const_iterator it = beginFrontals(); it != endFrontals(); ++it) { + cout << (boost::format("[%1%]")%(formatter(*it))).str() << " "; + } + cout << endl; + cout << formatMatrixIndented(" R = ", get_R()) << endl; + for(const_iterator it = beginParents() ; it != endParents() ; ++it ) { + cout << formatMatrixIndented((boost::format(" S[%1%] = ")%(formatter(*it))).str(), getA(it)) + << endl; + } + cout << formatMatrixIndented(" d = ", getb(), true) << "\n"; + if(model_) + model_->print(" Noise model: "); + else + cout << " No noise model" << endl; + } -/* ************************************************************************* */ -GaussianConditional::GaussianConditional(Index key, const Vector& d, const Matrix& R, - Index name1, const Matrix& S, Index name2, const Matrix& T, const Vector& sigmas) : - IndexConditional(key,name1,name2), rsd_(matrix_), sigmas_(sigmas) { - assert(R.rows() <= R.cols()); - size_t dims[] = { R.cols(), S.cols(), T.cols(), 1 }; - rsd_.copyStructureFrom(rsd_type(matrix_, dims, dims+4, d.size())); - rsd_(0) = R.triangularView(); - rsd_(1) = S; - rsd_(2) = T; - get_d_() = d; -} - -/* ************************************************************************* */ - GaussianConditional::GaussianConditional(Index key, const Vector& d, - const Matrix& R, const list >& parents, const Vector& sigmas) : - IndexConditional(key, GetKeys(parents.size(), parents.begin(), parents.end())), rsd_(matrix_), sigmas_(sigmas) { - assert(R.rows() <= R.cols()); - size_t* dims = (size_t*)alloca(sizeof(size_t)*(1+parents.size()+1)); // FIXME: alloca is bad, just ask Google. - dims[0] = R.cols(); - size_t j=1; - std::list >::const_iterator parent=parents.begin(); - for(; parent!=parents.end(); ++parent,++j) - dims[j] = parent->second.cols(); - dims[j] = 1; - rsd_.copyStructureFrom(rsd_type(matrix_, dims, dims+1+parents.size()+1, d.size())); - rsd_(0) = R.triangularView(); - j = 1; - for(std::list >::const_iterator parent=parents.begin(); parent!=parents.end(); ++parent) { - rsd_(j).noalias() = parent->second; - ++ j; - } - get_d_() = d; -} - -/* ************************************************************************* */ -GaussianConditional::GaussianConditional(const std::list >& terms, - const size_t nrFrontals, const Vector& d, const Vector& sigmas) : - IndexConditional(GetKeys(terms.size(), terms.begin(), terms.end()), nrFrontals), - rsd_(matrix_), sigmas_(sigmas) { - size_t* dims = (size_t*)alloca(sizeof(size_t)*(terms.size()+1)); // FIXME: alloca is bad, just ask Google. - size_t j=0; - typedef pair Index_Matrix; - BOOST_FOREACH(const Index_Matrix& term, terms) { - dims[j] = term.second.cols(); - ++ j; - } - dims[j] = 1; - rsd_.copyStructureFrom(rsd_type(matrix_, dims, dims+terms.size()+1, d.size())); - j=0; - BOOST_FOREACH(const Index_Matrix& term, terms) { - rsd_(j) = term.second; - ++ j; - } - get_d_() = d; -} - -/* ************************************************************************* */ -GaussianConditional::GaussianConditional(const GaussianConditional& rhs) : - rsd_(matrix_) { - *this = rhs; -} - -/* ************************************************************************* */ -GaussianConditional& GaussianConditional::operator=(const GaussianConditional& rhs) { - if(this != &rhs) { - this->Base::operator=(rhs); // Copy keys - rsd_.assignNoalias(rhs.rsd_); // Copy matrix and block configuration - sigmas_ = rhs.sigmas_; // Copy sigmas - } - return *this; -} - -/* ************************************************************************* */ -void GaussianConditional::print(const string &s, const IndexFormatter& formatter) const -{ - cout << s << ": density on "; - for(const_iterator it = beginFrontals(); it != endFrontals(); ++it) { - cout << (boost::format("[%1%]")%(formatter(*it))).str() << " "; - } - cout << endl; - gtsam::print(Matrix(get_R()),"R"); - for(const_iterator it = beginParents() ; it != endParents() ; ++it ) { - gtsam::print(Matrix(get_S(it)), (boost::format("A[%1%]")%(formatter(*it))).str()); - } - gtsam::print(Vector(get_d()),"d"); - gtsam::print(sigmas_,"sigmas"); -} - -/* ************************************************************************* */ -bool GaussianConditional::equals(const GaussianConditional &c, double tol) const { - // check if the size of the parents_ map is the same - if (parents().size() != c.parents().size()) - return false; - - // check if R_ and d_ are linear independent - for (size_t i=0; i rows1; rows1.push_back(Vector(get_R().row(i))); - list rows2; rows2.push_back(Vector(c.get_R().row(i))); - - // check if the matrices are the same - // iterate over the parents_ map - for (const_iterator it = beginParents(); it != endParents(); ++it) { - const_iterator it2 = c.beginParents() + (it-beginParents()); - if(*it != *(it2)) + /* ************************************************************************* */ + bool GaussianConditional::equals(const GaussianFactor& f, double tol) const + { + if (const GaussianConditional* c = dynamic_cast(&f)) + { + // check if the size of the parents_ map is the same + if (parents().size() != c->parents().size()) return false; - rows1.push_back(row(get_S(it), i)); - rows2.push_back(row(c.get_S(it2), i)); + + // check if R_ and d_ are linear independent + for (DenseIndex i = 0; i < Ab_.rows(); i++) { + list rows1; rows1.push_back(Vector(get_R().row(i))); + list rows2; rows2.push_back(Vector(c->get_R().row(i))); + + // check if the matrices are the same + // iterate over the parents_ map + for (const_iterator it = beginParents(); it != endParents(); ++it) { + const_iterator it2 = c->beginParents() + (it - beginParents()); + if (*it != *(it2)) + return false; + rows1.push_back(row(getA(it), i)); + rows2.push_back(row(c->getA(it2), i)); + } + + Vector row1 = concatVectors(rows1); + Vector row2 = concatVectors(rows2); + if (!linear_dependent(row1, row2, tol)) + return false; + } + + // check if sigmas are equal + if ((model_ && !c->model_) || (!model_ && c->model_) + || (model_ && c->model_ && !model_->equals(*c->model_, tol))) + return false; + + return true; + } + else + { + return false; + } + } + + /* ************************************************************************* */ + VectorValues GaussianConditional::solve(const VectorValues& x) const + { + // Concatenate all vector values that correspond to parent variables + Vector xS = x.vector(FastVector(beginParents(), endParents())); + + // Update right-hand-side + xS = getb() - get_S() * xS; + + // Solve matrix + Vector soln = get_R().triangularView().solve(xS); + + // Check for indeterminant solution + if(soln.hasNaN()) throw IndeterminantLinearSystemException(keys().front()); + + // TODO, do we not have to scale by sigmas here? Copy/paste bug + + // Insert solution into a VectorValues + VectorValues result; + DenseIndex vectorPosition = 0; + for(const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) { + result.insert(*frontal, soln.segment(vectorPosition, getDim(frontal))); + vectorPosition += getDim(frontal); } - Vector row1 = concatVectors(rows1); - Vector row2 = concatVectors(rows2); - if (!linear_dependent(row1, row2, tol)) - return false; + return result; } - // check if sigmas are equal - if (!(equal_with_abs_tol(sigmas_, c.sigmas_, tol))) return false; + /* ************************************************************************* */ + VectorValues GaussianConditional::solveOtherRHS( + const VectorValues& parents, const VectorValues& rhs) const + { + // Concatenate all vector values that correspond to parent variables + Vector xS = parents.vector(FastVector(beginParents(), endParents())); - return true; -} + // Instead of updating getb(), update the right-hand-side from the given rhs + const Vector rhsR = rhs.vector(FastVector(beginFrontals(), endFrontals())); + xS = rhsR - get_S() * xS; -/* ************************************************************************* */ -JacobianFactor::shared_ptr GaussianConditional::toFactor() const { - return JacobianFactor::shared_ptr(new JacobianFactor(*this)); -} + // Solve Matrix + Vector soln = get_R().triangularView().solve(xS); -/* ************************************************************************* */ -void GaussianConditional::solveInPlace(VectorValues& x) const { - static const bool debug = false; - if(debug) this->print("Solving conditional in place"); - Vector xS = internal::extractVectorValuesSlices(x, this->beginParents(), this->endParents()); - xS = this->get_d() - this->get_S() * xS; - Vector soln = this->get_R().triangularView().solve(xS); + // Scale by sigmas + if(model_) + soln.array() *= model_->sigmas().array(); - // Check for indeterminant solution - if(soln.unaryExpr(!boost::lambda::bind(ptr_fun(isfinite), boost::lambda::_1)).any()) - throw IndeterminantLinearSystemException(this->keys().front()); + // Insert solution into a VectorValues + VectorValues result; + DenseIndex vectorPosition = 0; + for(const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) { + result.insert(*frontal, soln.segment(vectorPosition, getDim(frontal))); + vectorPosition += getDim(frontal); + } - if(debug) { - gtsam::print(Matrix(this->get_R()), "Calling backSubstituteUpper on "); - gtsam::print(soln, "full back-substitution solution: "); + return result; } - internal::writeVectorValuesSlices(soln, x, this->beginFrontals(), this->endFrontals()); -} -/* ************************************************************************* */ -void GaussianConditional::solveTransposeInPlace(VectorValues& gy) const { - Vector frontalVec = internal::extractVectorValuesSlices(gy, beginFrontals(), endFrontals()); - frontalVec = gtsam::backSubstituteUpper(frontalVec,Matrix(get_R())); + /* ************************************************************************* */ + void GaussianConditional::solveTransposeInPlace(VectorValues& gy) const + { + Vector frontalVec = gy.vector(FastVector(beginFrontals(), endFrontals())); + frontalVec = gtsam::backSubstituteUpper(frontalVec, Matrix(get_R())); - // Check for indeterminant solution - if(frontalVec.unaryExpr(!boost::lambda::bind(ptr_fun(isfinite), boost::lambda::_1)).any()) - throw IndeterminantLinearSystemException(this->keys().front()); + // Check for indeterminant solution + if (frontalVec.hasNaN()) throw IndeterminantLinearSystemException(this->keys().front()); - GaussianConditional::const_iterator it; - for (it = beginParents(); it!= endParents(); it++) { - const Index i = *it; - transposeMultiplyAdd(-1.0,get_S(it),frontalVec,gy[i]); + for (const_iterator it = beginParents(); it!= endParents(); it++) + gtsam::transposeMultiplyAdd(-1.0, Matrix(getA(it)), frontalVec, gy[*it]); + + // Scale by sigmas + if(model_) + frontalVec.array() *= model_->sigmas().array(); + + // Write frontal solution into a VectorValues + DenseIndex vectorPosition = 0; + for(const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) { + gy[*frontal] = frontalVec.segment(vectorPosition, getDim(frontal)); + vectorPosition += getDim(frontal); + } } - internal::writeVectorValuesSlices(frontalVec, gy, beginFrontals(), endFrontals()); -} -/* ************************************************************************* */ -void GaussianConditional::scaleFrontalsBySigma(VectorValues& gy) const { - Vector frontalVec = internal::extractVectorValuesSlices(gy, beginFrontals(), endFrontals()); - frontalVec = emul(frontalVec, get_sigmas()); - internal::writeVectorValuesSlices(frontalVec, gy, beginFrontals(), endFrontals()); -} + /* ************************************************************************* */ + void GaussianConditional::scaleFrontalsBySigma(VectorValues& gy) const + { + DenseIndex vectorPosition = 0; + for(const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) { + gy[*frontal].array() *= model_->sigmas().segment(vectorPosition, getDim(frontal)).array(); + vectorPosition += getDim(frontal); + } + } } diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index f20d10ef9..a36de0dc2 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -22,279 +22,126 @@ #include #include -#include -#include +#include +#include #include -// Forward declaration to friend unit tests -class JacobianFactoreliminate2Test; -class GaussianConditionalconstructorTest; -class GaussianFactorGrapheliminationTest; -class GaussianJunctionTreecomplicatedMarginalTest; -class GaussianConditionalinformationTest; -class GaussianConditionalisGaussianFactorTest; - namespace gtsam { -// Forward declarations -class GaussianFactor; -class JacobianFactor; - -/** - * A conditional Gaussian functions as the node in a Bayes network - * It has a set of parents y,z, etc. and implements a probability density on x. - * The negative log-probability is given by \f$ \frac{1}{2} |Rx - (d - Sy - Tz - ...)|^2 \f$ - */ -class GTSAM_EXPORT GaussianConditional : public IndexConditional { - -public: - typedef GaussianFactor FactorType; - typedef boost::shared_ptr shared_ptr; - - /** Store the conditional matrix as upper-triangular column-major */ - typedef Matrix RdMatrix; - typedef VerticalBlockView rsd_type; - - typedef rsd_type::Block r_type; - typedef rsd_type::constBlock const_r_type; - typedef rsd_type::Column d_type; - typedef rsd_type::constColumn const_d_type; - -protected: - - /** Store the conditional as one big upper-triangular wide matrix, arranged - * as \f$ [ R S1 S2 ... d ] \f$. Access these blocks using a VerticalBlockView. - * */ - RdMatrix matrix_; - rsd_type rsd_; - - /** vector of standard deviations */ - Vector sigmas_; - - /** typedef to base class */ - typedef IndexConditional Base; - -public: - - /** default constructor needed for serialization */ - GaussianConditional(); - - /** constructor */ - explicit GaussianConditional(Index key); - - /** constructor with no parents - * |Rx-d| - */ - GaussianConditional(Index key, const Vector& d, const Matrix& R, const Vector& sigmas); - - /** constructor with only one parent - * |Rx+Sy-d| - */ - GaussianConditional(Index key, const Vector& d, const Matrix& R, - Index name1, const Matrix& S, const Vector& sigmas); - - /** constructor with two parents - * |Rx+Sy+Tz-d| - */ - GaussianConditional(Index key, const Vector& d, const Matrix& R, - Index name1, const Matrix& S, Index name2, const Matrix& T, const Vector& sigmas); - /** - * constructor with number of arbitrary parents (only used in unit tests, - * std::list is not efficient) - * \f$ |Rx+sum(Ai*xi)-d| \f$ - */ - GaussianConditional(Index key, const Vector& d, - const Matrix& R, const std::list >& parents, const Vector& sigmas); + * A conditional Gaussian functions as the node in a Bayes network + * It has a set of parents y,z, etc. and implements a probability density on x. + * The negative log-probability is given by \f$ \frac{1}{2} |Rx - (d - Sy - Tz - ...)|^2 \f$ + */ + class GTSAM_EXPORT GaussianConditional : + public JacobianFactor, + public Conditional + { + public: + typedef GaussianConditional This; ///< Typedef to this class + typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class + typedef JacobianFactor BaseFactor; ///< Typedef to our factor base class + typedef Conditional BaseConditional; ///< Typedef to our conditional base class - /** - * Constructor with arbitrary number of frontals and parents (only used in unit tests, - * std::list is not efficient) - */ - GaussianConditional(const std::list >& terms, - size_t nrFrontals, const Vector& d, const Vector& sigmas); + /** default constructor needed for serialization */ + GaussianConditional() {} - /** - * Constructor when matrices are already stored in a combined matrix, allows - * for multiple frontal variables. - */ - template - GaussianConditional(ITERATOR firstKey, ITERATOR lastKey, size_t nrFrontals, - const VerticalBlockView& matrices, const Vector& sigmas); + /** constructor with no parents |Rx-d| */ + GaussianConditional(Key key, const Vector& d, const Matrix& R, + const SharedDiagonal& sigmas = SharedDiagonal()); - /** Copy constructor */ - GaussianConditional(const GaussianConditional& rhs); + /** constructor with only one parent |Rx+Sy-d| */ + GaussianConditional(Key key, const Vector& d, const Matrix& R, + Key name1, const Matrix& S, const SharedDiagonal& sigmas = SharedDiagonal()); - /** Combine several GaussianConditional into a single dense GC. The - * conditionals enumerated by \c first and \c last must be in increasing - * order, meaning that the parents of any conditional may not include a - * conditional coming before it. - * @param firstConditional Iterator to the first conditional to combine, must dereference to a shared_ptr. - * @param lastConditional Iterator to after the last conditional to combine, must dereference to a shared_ptr. */ - template - static shared_ptr Combine(ITERATOR firstConditional, ITERATOR lastConditional); + /** constructor with two parents |Rx+Sy+Tz-d| */ + GaussianConditional(Key key, const Vector& d, const Matrix& R, + Key name1, const Matrix& S, Key name2, const Matrix& T, + const SharedDiagonal& sigmas = SharedDiagonal()); - /** Assignment operator */ - GaussianConditional& operator=(const GaussianConditional& rhs); + /** Constructor with arbitrary number of frontals and parents. + * @tparam TERMS A container whose value type is std::pair, specifying the + * collection of keys and matrices making up the conditional. */ + template + GaussianConditional(const TERMS& terms, + size_t nrFrontals, const Vector& d, + const SharedDiagonal& sigmas = SharedDiagonal()); - /** print */ - void print(const std::string& = "GaussianConditional", - const IndexFormatter& formatter = DefaultIndexFormatter) const; + /** Constructor with arbitrary number keys, and where the augmented matrix is given all together + * instead of in block terms. Note that only the active view of the provided augmented matrix + * is used, and that the matrix data is copied into a newly-allocated matrix in the constructed + * factor. */ + template + GaussianConditional( + const KEYS& keys, size_t nrFrontals, const VerticalBlockMatrix& augmentedMatrix, + const SharedDiagonal& sigmas = SharedDiagonal()); - /** equals function */ - bool equals(const GaussianConditional &cg, double tol = 1e-9) const; + /** Combine several GaussianConditional into a single dense GC. The conditionals enumerated by + * \c first and \c last must be in increasing order, meaning that the parents of any + * conditional may not include a conditional coming before it. + * @param firstConditional Iterator to the first conditional to combine, must dereference to a + * shared_ptr. + * @param lastConditional Iterator to after the last conditional to combine, must dereference + * to a shared_ptr. */ + template + static shared_ptr Combine(ITERATOR firstConditional, ITERATOR lastConditional); - /** dimension of multivariate variable (same as rows()) */ - size_t dim() const { return rsd_.rows(); } + /** print */ + void print(const std::string& = "GaussianConditional", + const KeyFormatter& formatter = DefaultKeyFormatter) const; - /** dimension of multivariate variable (same as dim()) */ - size_t rows() const { return dim(); } + /** equals function */ + bool equals(const GaussianFactor&cg, double tol = 1e-9) const; - /** Compute the augmented information matrix as - * \f$ [ R S d ]^T [ R S d ] \f$ - */ - Matrix augmentedInformation() const { - return rsd_.full().transpose() * rsd_.full().transpose(); - } + /** Return a view of the upper-triangular R block of the conditional */ + constABlock get_R() const { return Ab_.range(0, nrFrontals()); } - /** Compute the information matrix */ - Matrix information() const { - return get_R().transpose() * get_R(); - } + /** Get a view of the parent blocks. */ + constABlock get_S() const { return Ab_.range(nrFrontals(), size()); } - /** Return a view of the upper-triangular R block of the conditional */ - rsd_type::constBlock get_R() const { return rsd_.range(0, nrFrontals()); } + /** Get a view of the S matrix for the variable pointed to by the given key iterator */ + constABlock get_S(const_iterator variable) const { return BaseFactor::getA(variable); } - /** Return a view of the r.h.s. d vector */ - const_d_type get_d() const { return rsd_.column(nrFrontals()+nrParents(), 0); } + /** Get a view of the r.h.s. vector d */ + const constBVector get_d() const { return BaseFactor::getb(); } - /** get the dimension of a variable */ - size_t dim(const_iterator variable) const { return rsd_(variable - this->begin()).cols(); } + /** + * Solves a conditional Gaussian and writes the solution into the entries of + * \c x for each frontal variable of the conditional. The parents are + * assumed to have already been solved in and their values are read from \c x. + * This function works for multiple frontal variables. + * + * Given the Gaussian conditional with log likelihood \f$ |R x_f - (d - S x_s)|^2, + * where \f$ f \f$ are the frontal variables and \f$ s \f$ are the separator + * variables of this conditional, this solve function computes + * \f$ x_f = R^{-1} (d - S x_s) \f$ using back-substitution. + * + * @param parents VectorValues containing solved parents \f$ x_s \f$. + */ + VectorValues solve(const VectorValues& parents) const; - /** Get a view of the parent block corresponding to the variable pointed to by the given key iterator */ - rsd_type::constBlock get_S(const_iterator variable) const { return rsd_(variable - this->begin()); } - /** Get a view of the parent block corresponding to the variable pointed to by the given key iterator (non-const version) */ - rsd_type::constBlock get_S() const { return rsd_.range(nrFrontals(), size()); } - /** Get the Vector of sigmas */ - const Vector& get_sigmas() const {return sigmas_;} + VectorValues solveOtherRHS(const VectorValues& parents, const VectorValues& rhs) const; -protected: + /** Performs transpose backsubstition in place on values */ + void solveTransposeInPlace(VectorValues& gy) const; - const RdMatrix& matrix() const { return matrix_; } - const rsd_type& rsd() const { return rsd_; } + /** Scale the values in \c gy according to the sigmas for the frontal variables in this + * conditional. */ + void scaleFrontalsBySigma(VectorValues& gy) const; +// __declspec(deprecated) void scaleFrontalsBySigma(VectorValues& gy) const; // FIXME: depreciated flag doesn't appear to exist? -public: - /** - * Copy to a Factor (this creates a JacobianFactor and returns it as its - * base class GaussianFactor. - */ - boost::shared_ptr toFactor() const; + private: - /** - * Solves a conditional Gaussian and writes the solution into the entries of - * \c x for each frontal variable of the conditional. The parents are - * assumed to have already been solved in and their values are read from \c x. - * This function works for multiple frontal variables. - * - * Given the Gaussian conditional with log likelihood \f$ |R x_f - (d - S x_s)|^2, - * where \f$ f \f$ are the frontal variables and \f$ s \f$ are the separator - * variables of this conditional, this solve function computes - * \f$ x_f = R^{-1} (d - S x_s) \f$ using back-substitution. - * - * @param x VectorValues structure with solved parents \f$ x_s \f$, and into which the - * solution \f$ x_f \f$ will be written. - */ - void solveInPlace(VectorValues& x) const; - - // functions for transpose backsubstitution - - /** - * Performs backsubstition in place on values - */ - void solveTransposeInPlace(VectorValues& gy) const; - void scaleFrontalsBySigma(VectorValues& gy) const; - -protected: - rsd_type::Column get_d_() { return rsd_.column(nrFrontals()+nrParents(), 0); } - rsd_type::Block get_R_() { return rsd_.range(0, nrFrontals()); } - rsd_type::Block get_S_(iterator variable) { return rsd_(variable - this->begin()); } - -private: - - // Friends - friend class JacobianFactor; - friend class ::JacobianFactoreliminate2Test; - friend class ::GaussianConditionalconstructorTest; - friend class ::GaussianFactorGrapheliminationTest; - friend class ::GaussianJunctionTreecomplicatedMarginalTest; - friend class ::GaussianConditionalinformationTest; - friend class ::GaussianConditionalisGaussianFactorTest; - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(IndexConditional); - ar & BOOST_SERIALIZATION_NVP(matrix_); - ar & BOOST_SERIALIZATION_NVP(rsd_); - ar & BOOST_SERIALIZATION_NVP(sigmas_); - } -}; // GaussianConditional - -/* ************************************************************************* */ -template -GaussianConditional::GaussianConditional(ITERATOR firstKey, ITERATOR lastKey, - size_t nrFrontals, const VerticalBlockView& matrices, - const Vector& sigmas) : - IndexConditional(std::vector(firstKey, lastKey), nrFrontals), rsd_( - matrix_), sigmas_(sigmas) { - rsd_.assignNoalias(matrices); -} - -/* ************************************************************************* */ -template -GaussianConditional::shared_ptr GaussianConditional::Combine(ITERATOR firstConditional, ITERATOR lastConditional) { - - // TODO: check for being a clique - - // Get dimensions from first conditional - std::vector dims; dims.reserve((*firstConditional)->size() + 1); - for(const_iterator j = (*firstConditional)->begin(); j != (*firstConditional)->end(); ++j) - dims.push_back((*firstConditional)->dim(j)); - dims.push_back(1); - - // We assume the conditionals form clique, so the first n variables will be - // frontal variables in the new conditional. - size_t nFrontals = 0; - size_t nRows = 0; - for(ITERATOR c = firstConditional; c != lastConditional; ++c) { - nRows += dims[nFrontals]; - ++ nFrontals; - } - - // Allocate combined conditional, has same keys as firstConditional - Matrix tempCombined; - VerticalBlockView tempBlockView(tempCombined, dims.begin(), dims.end(), 0); - GaussianConditional::shared_ptr combinedConditional(new GaussianConditional((*firstConditional)->begin(), (*firstConditional)->end(), nFrontals, tempBlockView, zero(nRows))); - - // Resize to correct number of rows - combinedConditional->matrix_.resize(nRows, combinedConditional->matrix_.cols()); - combinedConditional->rsd_.rowEnd() = combinedConditional->matrix_.rows(); - - // Copy matrix and sigmas - const size_t totalDims = combinedConditional->matrix_.cols(); - size_t currentSlot = 0; - for(ITERATOR c = firstConditional; c != lastConditional; ++c) { - const size_t startRow = combinedConditional->rsd_.offset(currentSlot); // Start row is same as start column - combinedConditional->rsd_.range(0, currentSlot).block(startRow, 0, dims[currentSlot], combinedConditional->rsd_.offset(currentSlot)).operator=( - Matrix::Zero(dims[currentSlot], combinedConditional->rsd_.offset(currentSlot))); - combinedConditional->rsd_.range(currentSlot, dims.size()).block(startRow, 0, dims[currentSlot], totalDims - startRow).operator=( - (*c)->matrix_); - combinedConditional->sigmas_.segment(startRow, dims[currentSlot]) = (*c)->sigmas_; - ++ currentSlot; - } - - return combinedConditional; -} + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(BaseFactor); + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(BaseConditional); + } + }; // GaussianConditional } // gtsam + +#include + diff --git a/gtsam/linear/GaussianDensity.cpp b/gtsam/linear/GaussianDensity.cpp index a8f2fc895..3e35d5e65 100644 --- a/gtsam/linear/GaussianDensity.cpp +++ b/gtsam/linear/GaussianDensity.cpp @@ -23,31 +23,33 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ - void GaussianDensity::print(const string &s, const IndexFormatter& formatter) const + GaussianDensity GaussianDensity::FromMeanAndStddev(Key key, const Vector& mean, const double& sigma) + { + return GaussianDensity(key, mean / sigma, Matrix::Identity(mean.size(), mean.size()) / sigma); + } + + /* ************************************************************************* */ + void GaussianDensity::print(const string &s, const KeyFormatter& formatter) const { cout << s << ": density on "; for(const_iterator it = beginFrontals(); it != endFrontals(); ++it) cout << (boost::format("[%1%]")%(formatter(*it))).str() << " "; cout << endl; - gtsam::print(Matrix(get_R()),"R"); - gtsam::print(Vector(get_d()),"d"); - gtsam::print(sigmas_,"sigmas"); + gtsam::print(Matrix(get_R()), "R: "); + gtsam::print(Vector(get_d()), "d: "); + if(model_) + model_->print("Noise model: "); } /* ************************************************************************* */ Vector GaussianDensity::mean() const { - // Solve for mean - VectorValues x; - Index k = firstFrontalKey(); - // a VectorValues that only has a value for k: cannot be printed if k<>0 - x.insert(k, Vector(sigmas_.size())); - solveInPlace(x); - return x[k]; + VectorValues soln = this->solve(VectorValues()); + return soln[firstFrontalKey()]; } /* ************************************************************************* */ Matrix GaussianDensity::covariance() const { - return inverse(information()); + return information().inverse(); } } // gtsam diff --git a/gtsam/linear/GaussianDensity.h b/gtsam/linear/GaussianDensity.h index e7bc51f74..4b4fc1665 100644 --- a/gtsam/linear/GaussianDensity.h +++ b/gtsam/linear/GaussianDensity.h @@ -24,13 +24,13 @@ namespace gtsam { /** - * A Gaussian density. - * - * It is implemented as a GaussianConditional without parents. - * The negative log-probability is given by \f$ |Rx - d|^2 \f$ - * with \f$ \Lambda = \Sigma^{-1} = R^T R \f$ and \f$ \mu = R^{-1} d \f$ - */ - class GTSAM_EXPORT GaussianDensity: public GaussianConditional { + * A Gaussian density. + * + * It is implemented as a GaussianConditional without parents. + * The negative log-probability is given by \f$ |Rx - d|^2 \f$ + * with \f$ \Lambda = \Sigma^{-1} = R^T R \f$ and \f$ \mu = R^{-1} d \f$ + */ + class GTSAM_EXPORT GaussianDensity : public GaussianConditional { public: @@ -38,24 +38,26 @@ namespace gtsam { /// default constructor needed for serialization GaussianDensity() : - GaussianConditional() { + GaussianConditional() { } /// Copy constructor from GaussianConditional GaussianDensity(const GaussianConditional& conditional) : - GaussianConditional(conditional) { - assert(conditional.nrParents() == 0); + GaussianConditional(conditional) { + if(conditional.nrParents() != 0) + throw std::invalid_argument("GaussianDensity can only be created from a conditional with no parents"); } /// constructor using d, R - GaussianDensity(Index key, const Vector& d, const Matrix& R, - const Vector& sigmas) : - GaussianConditional(key, d, R, sigmas) { - } + GaussianDensity(Key key, const Vector& d, const Matrix& R, const SharedDiagonal& noiseModel = SharedDiagonal()) : + GaussianConditional(key, d, R, noiseModel) {} + + /// Construct using a mean and variance + static GaussianDensity FromMeanAndStddev(Key key, const Vector& mean, const double& sigma); /// print void print(const std::string& = "GaussianDensity", - const IndexFormatter& formatter =DefaultIndexFormatter) const; + const KeyFormatter& formatter = DefaultKeyFormatter) const; /// Mean \f$ \mu = R^{-1} d \f$ Vector mean() const; @@ -64,6 +66,6 @@ namespace gtsam { Matrix covariance() const; }; -// GaussianDensity + // GaussianDensity }// gtsam diff --git a/gtsam/linear/GaussianEliminationTree.cpp b/gtsam/linear/GaussianEliminationTree.cpp new file mode 100644 index 000000000..0daaf0707 --- /dev/null +++ b/gtsam/linear/GaussianEliminationTree.cpp @@ -0,0 +1,44 @@ +/* ---------------------------------------------------------------------------- + + * 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 GaussianEliminationTree.cpp + * @date Mar 29, 2013 + * @author Frank Dellaert + * @author Richard Roberts + */ + +#include +#include + +namespace gtsam { + + // Instantiate base class + template class EliminationTree; + + /* ************************************************************************* */ + GaussianEliminationTree::GaussianEliminationTree( + const GaussianFactorGraph& factorGraph, const VariableIndex& structure, + const Ordering& order) : + Base(factorGraph, structure, order) {} + + /* ************************************************************************* */ + GaussianEliminationTree::GaussianEliminationTree( + const GaussianFactorGraph& factorGraph, const Ordering& order) : + Base(factorGraph, order) {} + + /* ************************************************************************* */ + bool GaussianEliminationTree::equals(const This& other, double tol) const + { + return Base::equals(other, tol); + } + +} diff --git a/gtsam/linear/GaussianEliminationTree.h b/gtsam/linear/GaussianEliminationTree.h new file mode 100644 index 000000000..1a4ba7868 --- /dev/null +++ b/gtsam/linear/GaussianEliminationTree.h @@ -0,0 +1,63 @@ +/* ---------------------------------------------------------------------------- + + * 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 GaussianEliminationTree.h + * @date Mar 29, 2013 + * @author Frank Dellaert + * @author Richard Roberts + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + + class GTSAM_EXPORT GaussianEliminationTree : + public EliminationTree + { + public: + typedef EliminationTree Base; ///< Base class + typedef GaussianEliminationTree This; ///< This class + typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class + + /** + * Build the elimination tree of a factor graph using pre-computed column structure. + * @param factorGraph The factor graph for which to build the elimination tree + * @param structure The set of factors involving each variable. If this is not + * precomputed, you can call the Create(const FactorGraph&) + * named constructor instead. + * @return The elimination tree + */ + GaussianEliminationTree(const GaussianFactorGraph& factorGraph, + const VariableIndex& structure, const Ordering& order); + + /** Build the elimination tree of a factor graph. Note that this has to compute the column + * structure as a VariableIndex, so if you already have this precomputed, use the other + * constructor instead. + * @param factorGraph The factor graph for which to build the elimination tree + */ + GaussianEliminationTree(const GaussianFactorGraph& factorGraph, + const Ordering& order); + + /** Test whether the tree is equal to another */ + bool equals(const This& other, double tol = 1e-9) const; + + private: + + friend class ::EliminationTreeTester; + + }; + +} diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index 3363880ef..543822ce0 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -21,77 +21,65 @@ #pragma once #include -#include - -#include - -#include -#include +#include namespace gtsam { // Forward declarations class VectorValues; - class Permutation; - class GaussianConditional; - template class BayesNet; /** - * An abstract virtual base class for JacobianFactor and HessianFactor. - * A GaussianFactor has a quadratic error function. - * GaussianFactor is non-mutable (all methods const!). - * The factor value is exp(-0.5*||Ax-b||^2) - */ - class GTSAM_EXPORT GaussianFactor: public IndexFactor { - - protected: - - /** Copy constructor */ - GaussianFactor(const This& f) : IndexFactor(f) {} - - /** Construct from derived type */ - GaussianFactor(const GaussianConditional& c); - - /** Constructor from a collection of keys */ - template GaussianFactor(KeyIterator beginKey, KeyIterator endKey) : - Base(beginKey, endKey) {} - - /** Default constructor for I/O */ - GaussianFactor() {} - - /** Construct unary factor */ - GaussianFactor(Index j) : IndexFactor(j) {} - - /** Construct binary factor */ - GaussianFactor(Index j1, Index j2) : IndexFactor(j1, j2) {} - - /** Construct ternary factor */ - GaussianFactor(Index j1, Index j2, Index j3) : IndexFactor(j1, j2, j3) {} - - /** Construct 4-way factor */ - GaussianFactor(Index j1, Index j2, Index j3, Index j4) : IndexFactor(j1, j2, j3, j4) {} - - /** Construct n-way factor */ - GaussianFactor(const std::set& js) : IndexFactor(js) {} - - /** Construct n-way factor */ - GaussianFactor(const std::vector& js) : IndexFactor(js) {} - + * An abstract virtual base class for JacobianFactor and HessianFactor. A GaussianFactor has a + * quadratic error function. GaussianFactor is non-mutable (all methods const!). The factor value + * is exp(-0.5*||Ax-b||^2) */ + class GTSAM_EXPORT GaussianFactor : public Factor + { public: + typedef GaussianFactor This; ///< This class + typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class + typedef Factor Base; ///< Our base class - typedef GaussianConditional ConditionalType; - typedef boost::shared_ptr shared_ptr; + /** Default constructor creates empty factor */ + GaussianFactor() {} + + /** Construct from container of keys. This constructor is used internally from derived factor + * constructors, either from a container of keys or from a boost::assign::list_of. */ + template + GaussianFactor(const CONTAINER& keys) : Base(keys) {} + + /** Destructor */ + virtual ~GaussianFactor() {} // Implementing Testable interface virtual void print(const std::string& s = "", - const IndexFormatter& formatter = DefaultIndexFormatter) const = 0; + const KeyFormatter& formatter = DefaultKeyFormatter) const = 0; + /** Equals for testable */ virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const = 0; + /** Print for testable */ virtual double error(const VectorValues& c) const = 0; /** 0.5*(A*x-b)'*D*(A*x-b) */ /** Return the dimension of the variable pointed to by the given key iterator */ - virtual size_t getDim(const_iterator variable) const = 0; + virtual DenseIndex getDim(const_iterator variable) const = 0; + + /** + * Return a dense \f$ [ \;A\;b\; ] \in \mathbb{R}^{m \times n+1} \f$ + * Jacobian matrix, augmented with b with the noise models baked + * into A and b. The negative log-likelihood is + * \f$ \frac{1}{2} \Vert Ax-b \Vert^2 \f$. See also + * GaussianFactorGraph::jacobian and GaussianFactorGraph::sparseJacobian. + */ + virtual Matrix augmentedJacobian() const = 0; + + /** + * Return the dense Jacobian \f$ A \f$ and right-hand-side \f$ b \f$, + * with the noise models baked into A and b. The negative log-likelihood + * is \f$ \frac{1}{2} \Vert Ax-b \Vert^2 \f$. See also + * GaussianFactorGraph::augmentedJacobian and + * GaussianFactorGraph::sparseJacobian. + */ + virtual std::pair jacobian() const = 0; /** Return the augmented information matrix represented by this GaussianFactor. * The augmented information matrix contains the information matrix with an @@ -108,24 +96,20 @@ namespace gtsam { */ virtual Matrix information() const = 0; + /// Return the diagonal of the Hessian for this factor + virtual VectorValues hessianDiagonal() const = 0; + + /// Return the diagonal of the Hessian for this factor (raw memory version) + virtual void hessianDiagonal(double* d) const = 0; + + /// Return the block diagonal of the Hessian for this factor + virtual std::map hessianBlockDiagonal() const = 0; + /** Clone a factor (make a deep copy) */ virtual GaussianFactor::shared_ptr clone() const = 0; - /** - * Permutes the GaussianFactor, but for efficiency requires the permutation - * to already be inverted. This acts just as a change-of-name for each - * variable. The order of the variables within the factor is not changed. - */ - virtual void permuteWithInverse(const Permutation& inversePermutation) { - IndexFactor::permuteWithInverse(inversePermutation); - } - - /** - * Apply a reduction, which is a remapping of variable indices. - */ - virtual void reduceWithInverse(const internal::Reduction& inverseReduction) { - IndexFactor::reduceWithInverse(inverseReduction); - } + /** Test whether the factor is empty */ + virtual bool empty() const = 0; /** * Construct the corresponding anti-factor to negate information @@ -134,23 +118,26 @@ namespace gtsam { */ virtual GaussianFactor::shared_ptr negate() const = 0; + /// y += alpha * A'*A*x + virtual void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const = 0; + + /// y += alpha * A'*A*x + virtual void multiplyHessianAdd(double alpha, const double* x, double* y, std::vector keys) const = 0; + + /// y += alpha * A'*A*x + virtual void multiplyHessianAdd(double alpha, const double* x, double* y) const = 0; + + /// A'*b for Jacobian, eta for Hessian + virtual VectorValues gradientAtZero() const = 0; + private: /** Serialization function */ friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(IndexFactor); + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); } }; // GaussianFactor - - /** make keys from list, vector, or map of matrices */ - template - static std::vector GetKeys(size_t n, ITERATOR begin, ITERATOR end) { - std::vector keys; - keys.reserve(n); - for (ITERATOR it=begin;it!=end;++it) keys.push_back(it->first); - return keys; - } - + } // namespace gtsam diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index b04287288..b56270a55 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -18,15 +18,14 @@ * @author Frank Dellaert */ -#include -#include -#include -#include -#include -#include #include -#include -#include +#include +#include +#include +#include +#include +#include +#include #include #include #include @@ -36,85 +35,99 @@ using namespace gtsam; namespace gtsam { + // Instantiate base classes + template class FactorGraph; + template class EliminateableFactorGraph; + /* ************************************************************************* */ - GaussianFactorGraph::GaussianFactorGraph(const GaussianBayesNet& CBN) : Base(CBN) {} + bool GaussianFactorGraph::equals(const This& fg, double tol) const + { + return Base::equals(fg, tol); + } /* ************************************************************************* */ GaussianFactorGraph::Keys GaussianFactorGraph::keys() const { - FastSet keys; + FastSet keys; BOOST_FOREACH(const sharedFactor& factor, *this) - if (factor) keys.insert(factor->begin(), factor->end()); + if (factor) + keys.insert(factor->begin(), factor->end()); return keys; } /* ************************************************************************* */ - void GaussianFactorGraph::permuteWithInverse( - const Permutation& inversePermutation) { - BOOST_FOREACH(const sharedFactor& factor, factors_) { - if(factor) - factor->permuteWithInverse(inversePermutation); - } - } - - /* ************************************************************************* */ - void GaussianFactorGraph::reduceWithInverse( - const internal::Reduction& inverseReduction) { - BOOST_FOREACH(const sharedFactor& factor, factors_) { - if(factor) - factor->reduceWithInverse(inverseReduction); + vector GaussianFactorGraph::getkeydim() const { + // First find dimensions of each variable + vector dims; + BOOST_FOREACH(const sharedFactor& factor, *this) { + for (GaussianFactor::const_iterator pos = factor->begin(); + pos != factor->end(); ++pos) { + if (dims.size() <= *pos) + dims.resize(*pos + 1, 0); + dims[*pos] = factor->getDim(pos); + } } - } - - /* ************************************************************************* */ - void GaussianFactorGraph::combine(const GaussianFactorGraph &lfg) { - for (const_iterator factor = lfg.factors_.begin(); factor - != lfg.factors_.end(); factor++) { - push_back(*factor); + // Find accumulated dimensions for variables + vector dims_accumulated; + dims_accumulated.resize(dims.size()+1,0); + dims_accumulated[0]=0; + for (int i=1; iclone()); + else + result.push_back(sharedFactor()); // Passes on null factors so indices remain valid } - return fg; + return result; } /* ************************************************************************* */ - std::vector > GaussianFactorGraph::sparseJacobian() const { + GaussianFactorGraph GaussianFactorGraph::negate() const { + GaussianFactorGraph result; + BOOST_FOREACH(const sharedFactor& f, *this) { + if (f) + result.push_back(f->negate()); + else + result.push_back(sharedFactor()); // Passes on null factors so indices remain valid + } + return result; + } + + /* ************************************************************************* */ + vector > GaussianFactorGraph::sparseJacobian() const { // First find dimensions of each variable - FastVector dims; + vector dims; BOOST_FOREACH(const sharedFactor& factor, *this) { - for(GaussianFactor::const_iterator pos = factor->begin(); pos != factor->end(); ++pos) { - if(dims.size() <= *pos) + for (GaussianFactor::const_iterator pos = factor->begin(); + pos != factor->end(); ++pos) { + if (dims.size() <= *pos) dims.resize(*pos + 1, 0); dims[*pos] = factor->getDim(pos); } } // Compute first scalar column of each variable - vector columnIndices(dims.size()+1, 0); - for(size_t j=1; j columnIndices(dims.size() + 1, 0); + for (size_t j = 1; j < dims.size() + 1; ++j) + columnIndices[j] = columnIndices[j - 1] + dims[j - 1]; // Iterate over all factors, adding sparse scalar entries typedef boost::tuple triplet; - FastVector entries; + vector entries; size_t row = 0; BOOST_FOREACH(const sharedFactor& factor, *this) { // Convert to JacobianFactor if necessary JacobianFactor::shared_ptr jacobianFactor( boost::dynamic_pointer_cast(factor)); if (!jacobianFactor) { - HessianFactor::shared_ptr hessian(boost::dynamic_pointer_cast(factor)); + HessianFactor::shared_ptr hessian( + boost::dynamic_pointer_cast(factor)); if (hessian) jacobianFactor.reset(new JacobianFactor(*hessian)); else @@ -125,22 +138,23 @@ namespace gtsam { // Whiten the factor and add entries for it // iterate over all variables in the factor const JacobianFactor whitened(jacobianFactor->whiten()); - for(JacobianFactor::const_iterator pos=whitened.begin(); pos 1e-12) entries.push_back( - boost::make_tuple(row+i, column_start+j, s)); + double s = whitenedA(i, j); + if (std::abs(s) > 1e-12) + entries.push_back(boost::make_tuple(row + i, column_start + j, s)); } } JacobianFactor::constBVector whitenedb(whitened.getb()); size_t bcolumn = columnIndices.back(); for (size_t i = 0; i < (size_t) whitenedb.size(); i++) - entries.push_back(boost::make_tuple(row+i, bcolumn, whitenedb(i))); + entries.push_back(boost::make_tuple(row + i, bcolumn, whitenedb(i))); // Increment row index row += jacobianFactor->rows(); @@ -153,7 +167,7 @@ namespace gtsam { // call sparseJacobian typedef boost::tuple triplet; - std::vector result = sparseJacobian(); + vector result = sparseJacobian(); // translate to base 1 matrix size_t nzmax = result.size(); @@ -168,168 +182,26 @@ namespace gtsam { } /* ************************************************************************* */ - Matrix GaussianFactorGraph::augmentedJacobian() const { - // Convert to Jacobians - FactorGraph jfg; - jfg.reserve(this->size()); - BOOST_FOREACH(const sharedFactor& factor, *this) { - if(boost::shared_ptr jf = - boost::dynamic_pointer_cast(factor)) - jfg.push_back(jf); - else - jfg.push_back(boost::make_shared(*factor)); - } + Matrix GaussianFactorGraph::augmentedJacobian( + boost::optional optionalOrdering) const { // combine all factors - JacobianFactor combined(*CombineJacobians(jfg, VariableSlots(*this))); - return combined.matrix_augmented(); + JacobianFactor combined(*this, optionalOrdering); + return combined.augmentedJacobian(); } /* ************************************************************************* */ - std::pair GaussianFactorGraph::jacobian() const { - Matrix augmented = augmentedJacobian(); - return make_pair( - augmented.leftCols(augmented.cols()-1), - augmented.col(augmented.cols()-1)); + pair GaussianFactorGraph::jacobian( + boost::optional optionalOrdering) const { + Matrix augmented = augmentedJacobian(optionalOrdering); + return make_pair(augmented.leftCols(augmented.cols() - 1), + augmented.col(augmented.cols() - 1)); } /* ************************************************************************* */ - // Helper functions for Combine - static boost::tuple, size_t, size_t> countDims(const FactorGraph& factors, const VariableSlots& variableSlots) { -#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS - vector varDims(variableSlots.size(), numeric_limits::max()); -#else - vector varDims(variableSlots.size()); -#endif - size_t m = 0; - size_t n = 0; - { - Index jointVarpos = 0; - BOOST_FOREACH(const VariableSlots::value_type& slots, variableSlots) { - - assert(slots.second.size() == factors.size()); - - Index sourceFactorI = 0; - BOOST_FOREACH(const Index sourceVarpos, slots.second) { - if(sourceVarpos < numeric_limits::max()) { - const JacobianFactor& sourceFactor = *factors[sourceFactorI]; - size_t vardim = sourceFactor.getDim(sourceFactor.begin() + sourceVarpos); -#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS -if(varDims[jointVarpos] == numeric_limits::max()) { - varDims[jointVarpos] = vardim; - n += vardim; -} else - assert(varDims[jointVarpos] == vardim); -#else - varDims[jointVarpos] = vardim; -n += vardim; -break; -#endif - } - ++ sourceFactorI; - } - ++ jointVarpos; - } - BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, factors) { - m += factor->rows(); - } - } - return boost::make_tuple(varDims, m, n); - } - - /* ************************************************************************* */ - JacobianFactor::shared_ptr CombineJacobians( - const FactorGraph& factors, - const VariableSlots& variableSlots) { - - const bool debug = ISDEBUG("CombineJacobians"); - if (debug) factors.print("Combining factors: "); - if (debug) variableSlots.print(); - - if (debug) cout << "Determine dimensions" << endl; - gttic(countDims); - vector varDims; - size_t m, n; - boost::tie(varDims, m, n) = countDims(factors, variableSlots); - if (debug) { - cout << "Dims: " << m << " x " << n << "\n"; - BOOST_FOREACH(const size_t dim, varDims) cout << dim << " "; - cout << endl; - } - gttoc(countDims); - - if (debug) cout << "Allocate new factor" << endl; - gttic(allocate); - JacobianFactor::shared_ptr combined(new JacobianFactor()); - combined->allocate(variableSlots, varDims, m); - Vector sigmas(m); - gttoc(allocate); - - if (debug) cout << "Copy blocks" << endl; - gttic(copy_blocks); - // Loop over slots in combined factor - Index combinedSlot = 0; - BOOST_FOREACH(const VariableSlots::value_type& varslot, variableSlots) { - JacobianFactor::ABlock destSlot(combined->getA(combined->begin()+combinedSlot)); - // Loop over source factors - size_t nextRow = 0; - for(size_t factorI = 0; factorI < factors.size(); ++factorI) { - // Slot in source factor - const Index sourceSlot = varslot.second[factorI]; - const size_t sourceRows = factors[factorI]->rows(); - JacobianFactor::ABlock::RowsBlockXpr destBlock(destSlot.middleRows(nextRow, sourceRows)); - // Copy if exists in source factor, otherwise set zero - if(sourceSlot != numeric_limits::max()) - destBlock = factors[factorI]->getA(factors[factorI]->begin()+sourceSlot); - else - destBlock.setZero(); - nextRow += sourceRows; - } - ++combinedSlot; - } - gttoc(copy_blocks); - - if (debug) cout << "Copy rhs (b) and sigma" << endl; - gttic(copy_vectors); - bool anyConstrained = false; - // Loop over source factors - size_t nextRow = 0; - for(size_t factorI = 0; factorI < factors.size(); ++factorI) { - const size_t sourceRows = factors[factorI]->rows(); - combined->getb().segment(nextRow, sourceRows) = factors[factorI]->getb(); - sigmas.segment(nextRow, sourceRows) = factors[factorI]->get_model()->sigmas(); - if (factors[factorI]->isConstrained()) anyConstrained = true; - nextRow += sourceRows; - } - gttoc(copy_vectors); - - if (debug) cout << "Create noise model from sigmas" << endl; - gttic(noise_model); - combined->setModel(anyConstrained, sigmas); - gttoc(noise_model); - - if (debug) cout << "Assert Invariants" << endl; - combined->assertInvariants(); - - return combined; - } - - /* ************************************************************************* */ - GaussianFactorGraph::EliminationResult EliminateJacobians(const FactorGraph< - JacobianFactor>& factors, size_t nrFrontals) { - gttic(Combine); - JacobianFactor::shared_ptr jointFactor = - CombineJacobians(factors, VariableSlots(factors)); - gttoc(Combine); - gttic(eliminate); - GaussianConditional::shared_ptr gbn = jointFactor->eliminate(nrFrontals); - gttoc(eliminate); - return make_pair(gbn, jointFactor); - } - - /* ************************************************************************* */ - Matrix GaussianFactorGraph::augmentedHessian() const { + Matrix GaussianFactorGraph::augmentedHessian( + boost::optional optionalOrdering) const { // combine all factors and get upper-triangular part of Hessian - HessianFactor combined(*this); + HessianFactor combined(*this, Scatter(*this, optionalOrdering)); Matrix result = combined.info(); // Fill in lower-triangular part of Hessian result.triangularView() = result.transpose(); @@ -337,253 +209,232 @@ break; } /* ************************************************************************* */ - std::pair GaussianFactorGraph::hessian() const { - Matrix augmented = augmentedHessian(); + pair GaussianFactorGraph::hessian( + boost::optional optionalOrdering) const { + Matrix augmented = augmentedHessian(optionalOrdering); return make_pair( - augmented.topLeftCorner(augmented.rows()-1, augmented.rows()-1), - augmented.col(augmented.rows()-1).head(augmented.rows()-1)); + augmented.topLeftCorner(augmented.rows() - 1, augmented.rows() - 1), + augmented.col(augmented.rows() - 1).head(augmented.rows() - 1)); } /* ************************************************************************* */ - GaussianFactorGraph::EliminationResult EliminateCholesky(const FactorGraph< - GaussianFactor>& factors, size_t nrFrontals) { - - const bool debug = ISDEBUG("EliminateCholesky"); - - // Form Ab' * Ab - gttic(combine); - HessianFactor::shared_ptr combinedFactor(new HessianFactor(factors)); - gttoc(combine); - - // Do Cholesky, note that after this, the lower triangle still contains - // some untouched non-zeros that should be zero. We zero them while - // extracting submatrices next. - gttic(partial_Cholesky); - combinedFactor->partialCholesky(nrFrontals); - - gttoc(partial_Cholesky); - - // Extract conditional and fill in details of the remaining factor - gttic(split); - GaussianConditional::shared_ptr conditional = - combinedFactor->splitEliminatedFactor(nrFrontals); - if (debug) { - conditional->print("Extracted conditional: "); - combinedFactor->print("Eliminated factor (L piece): "); - } - gttoc(split); - - combinedFactor->assertInvariants(); - return make_pair(conditional, combinedFactor); - } - - /* ************************************************************************* */ - static FactorGraph convertToJacobians(const FactorGraph< - GaussianFactor>& factors) { - - typedef JacobianFactor J; - typedef HessianFactor H; - - const bool debug = ISDEBUG("convertToJacobians"); - - FactorGraph jacobians; - jacobians.reserve(factors.size()); - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) - if (factor) { - J::shared_ptr jacobian(boost::dynamic_pointer_cast(factor)); - if (jacobian) { - jacobians.push_back(jacobian); - if (debug) jacobian->print("Existing JacobianFactor: "); - } else { - H::shared_ptr hessian(boost::dynamic_pointer_cast(factor)); - if (!hessian) throw std::invalid_argument( - "convertToJacobians: factor is neither a JacobianFactor nor a HessianFactor."); - J::shared_ptr converted(new J(*hessian)); - if (debug) { - cout << "Converted HessianFactor to JacobianFactor:\n"; - hessian->print("HessianFactor: "); - converted->print("JacobianFactor: "); - if (!assert_equal(*hessian, HessianFactor(*converted), 1e-3)) throw runtime_error( - "convertToJacobians: Conversion between Jacobian and Hessian incorrect"); - } - jacobians.push_back(converted); + VectorValues GaussianFactorGraph::hessianDiagonal() const { + VectorValues d; + BOOST_FOREACH(const sharedFactor& factor, *this) { + if(factor){ + VectorValues di = factor->hessianDiagonal(); + d.addInPlace_(di); } } - return jacobians; + return d; } /* ************************************************************************* */ - GaussianFactorGraph::EliminationResult EliminateQR(const FactorGraph< - GaussianFactor>& factors, size_t nrFrontals) { - - const bool debug = ISDEBUG("EliminateQR"); - - // Convert all factors to the appropriate type and call the type-specific EliminateGaussian. - if (debug) cout << "Using QR" << endl; - - gttic(convert_to_Jacobian); - FactorGraph jacobians = convertToJacobians(factors); - gttoc(convert_to_Jacobian); - - gttic(Jacobian_EliminateGaussian); - GaussianConditional::shared_ptr conditional; - GaussianFactor::shared_ptr factor; - boost::tie(conditional, factor) = EliminateJacobians(jacobians, nrFrontals); - gttoc(Jacobian_EliminateGaussian); - - return make_pair(conditional, factor); - } // \EliminateQR - - /* ************************************************************************* */ - bool hasConstraints(const FactorGraph& factors) { - typedef JacobianFactor J; - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) { - J::shared_ptr jacobian(boost::dynamic_pointer_cast(factor)); - if (jacobian && jacobian->get_model()->isConstrained()) { - return true; + map GaussianFactorGraph::hessianBlockDiagonal() const { + map blocks; + BOOST_FOREACH(const sharedFactor& factor, *this) { + map BD = factor->hessianBlockDiagonal(); + map::const_iterator it = BD.begin(); + for(;it!=BD.end();it++) { + Key j = it->first; // variable key for this block + const Matrix& Bj = it->second; + if (blocks.count(j)) + blocks[j] += Bj; + else + blocks.insert(make_pair(j,Bj)); } } - return false; + return blocks; } /* ************************************************************************* */ - GaussianFactorGraph::EliminationResult EliminatePreferCholesky( - const FactorGraph& factors, size_t nrFrontals) { - - // If any JacobianFactors have constrained noise models, we have to convert - // all factors to JacobianFactors. Otherwise, we can convert all factors - // to HessianFactors. This is because QR can handle constrained noise - // models but Cholesky cannot. - if (hasConstraints(factors)) - return EliminateQR(factors, nrFrontals); - else { - GaussianFactorGraph::EliminationResult ret; - gttic(EliminateCholesky); - ret = EliminateCholesky(factors, nrFrontals); - gttoc(EliminateCholesky); - return ret; - } - - } // \EliminatePreferCholesky - - - - /* ************************************************************************* */ - static JacobianFactor::shared_ptr convertToJacobianFactorPtr(const GaussianFactor::shared_ptr &gf) { - JacobianFactor::shared_ptr result = boost::dynamic_pointer_cast(gf); - if( !result ) { - result = boost::make_shared(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky) - } - return result; + VectorValues GaussianFactorGraph::optimize(OptionalOrdering ordering, const Eliminate& function) const + { + gttic(GaussianFactorGraph_optimize); + return BaseEliminateable::eliminateMultifrontal(ordering, function)->optimize(); } /* ************************************************************************* */ - Errors operator*(const GaussianFactorGraph& fg, const VectorValues& x) { - Errors e; - BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { + namespace { + JacobianFactor::shared_ptr convertToJacobianFactorPtr(const GaussianFactor::shared_ptr &gf) { + JacobianFactor::shared_ptr result = boost::dynamic_pointer_cast(gf); + if( !result ) + // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky) + result = boost::make_shared(*gf); + return result; + } + } + + /* ************************************************************************* */ + VectorValues GaussianFactorGraph::gradient(const VectorValues& x0) const + { + VectorValues g = VectorValues::Zero(x0); + BOOST_FOREACH(const sharedFactor& Ai_G, *this) { JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); - e.push_back((*Ai)*x); + Vector e = Ai->error_vector(x0); + Ai->transposeMultiplyAdd(1.0, e, g); + } + return g; + } + + /* ************************************************************************* */ + VectorValues GaussianFactorGraph::gradientAtZero() const { + // Zero-out the gradient + VectorValues g; + BOOST_FOREACH(const sharedFactor& factor, *this) { + VectorValues gi = factor->gradientAtZero(); + g.addInPlace_(gi); + } + return g; + } + + /* ************************************************************************* */ + VectorValues GaussianFactorGraph::optimizeGradientSearch() const + { + gttic(GaussianFactorGraph_optimizeGradientSearch); + + gttic(Compute_Gradient); + // Compute gradient (call gradientAtZero function, which is defined for various linear systems) + VectorValues grad = gradientAtZero(); + double gradientSqNorm = grad.dot(grad); + gttoc(Compute_Gradient); + + gttic(Compute_Rg); + // Compute R * g + Errors Rg = *this * grad; + gttoc(Compute_Rg); + + gttic(Compute_minimizing_step_size); + // Compute minimizing step size + double step = -gradientSqNorm / dot(Rg, Rg); + gttoc(Compute_minimizing_step_size); + + gttic(Compute_point); + // Compute steepest descent point + grad *= step; + gttoc(Compute_point); + + return grad; + } + + /* ************************************************************************* */ + Errors GaussianFactorGraph::operator*(const VectorValues& x) const { + Errors e; + BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, *this) { + JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); + e.push_back((*Ai) * x); } return e; } /* ************************************************************************* */ - void multiplyInPlace(const GaussianFactorGraph& fg, const VectorValues& x, Errors& e) { - multiplyInPlace(fg,x,e.begin()); + void GaussianFactorGraph::multiplyHessianAdd(double alpha, + const VectorValues& x, VectorValues& y) const { + BOOST_FOREACH(const GaussianFactor::shared_ptr& f, *this) + f->multiplyHessianAdd(alpha, x, y); } /* ************************************************************************* */ - void multiplyInPlace(const GaussianFactorGraph& fg, const VectorValues& x, const Errors::iterator& e) { + void GaussianFactorGraph::multiplyHessianAdd(double alpha, + const double* x, double* y) const { + vector FactorKeys = getkeydim(); + BOOST_FOREACH(const GaussianFactor::shared_ptr& f, *this) + f->multiplyHessianAdd(alpha, x, y, FactorKeys); + + } + + /* ************************************************************************* */ + void GaussianFactorGraph::multiplyInPlace(const VectorValues& x, Errors& e) const { + multiplyInPlace(x, e.begin()); + } + + /* ************************************************************************* */ + void GaussianFactorGraph::multiplyInPlace(const VectorValues& x, const Errors::iterator& e) const { Errors::iterator ei = e; - BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { + BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, *this) { JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); *ei = (*Ai)*x; ei++; } } + /* ************************************************************************* */ + bool hasConstraints(const GaussianFactorGraph& factors) { + typedef JacobianFactor J; + BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) { + J::shared_ptr jacobian(boost::dynamic_pointer_cast(factor)); + if (jacobian && jacobian->get_model() && jacobian->get_model()->isConstrained()) { + return true; + } + } + return false; + } + /* ************************************************************************* */ // x += alpha*A'*e - void transposeMultiplyAdd(const GaussianFactorGraph& fg, double alpha, const Errors& e, VectorValues& x) { - // For each factor add the gradient contribution +void GaussianFactorGraph::transposeMultiplyAdd(double alpha, const Errors& e, + VectorValues& x) const { + // For each factor add the gradient contribution + Errors::const_iterator ei = e.begin(); + BOOST_FOREACH(const sharedFactor& Ai_G, *this) { + JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); + Ai->transposeMultiplyAdd(alpha, *(ei++), x); + } +} + + ///* ************************************************************************* */ + //void residual(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r) { + // Key i = 0 ; + // BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { + // JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); + // r[i] = Ai->getb(); + // i++; + // } + // VectorValues Ax = VectorValues::SameStructure(r); + // multiply(fg,x,Ax); + // axpy(-1.0,Ax,r); + //} + + ///* ************************************************************************* */ + //void multiply(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r) { + // r.setZero(); + // Key i = 0; + // BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { + // JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); + // Vector &y = r[i]; + // for(JacobianFactor::const_iterator j = Ai->begin(); j != Ai->end(); ++j) { + // y += Ai->getA(j) * x[*j]; + // } + // ++i; + // } + //} + + /* ************************************************************************* */ + VectorValues GaussianFactorGraph::transposeMultiply(const Errors& e) const + { + VectorValues x; Errors::const_iterator ei = e.begin(); - BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { - JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); - Ai->transposeMultiplyAdd(alpha,*(ei++),x); - } - } - - /* ************************************************************************* */ - VectorValues gradient(const GaussianFactorGraph& fg, const VectorValues& x0) { - // It is crucial for performance to make a zero-valued clone of x - VectorValues g = VectorValues::Zero(x0); - Errors e; - BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { - JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); - e.push_back(Ai->error_vector(x0)); - } - transposeMultiplyAdd(fg, 1.0, e, g); - return g; - } - - /* ************************************************************************* */ - void gradientAtZero(const GaussianFactorGraph& fg, VectorValues& g) { - // Zero-out the gradient - g.setZero(); - Errors e; - BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { - JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); - e.push_back(-Ai->getb()); - } - transposeMultiplyAdd(fg, 1.0, e, g); - } - - /* ************************************************************************* */ - void residual(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r) { - Index i = 0 ; - BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { - JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); - r[i] = Ai->getb(); - i++; - } - VectorValues Ax = VectorValues::SameStructure(r); - multiply(fg,x,Ax); - axpy(-1.0,Ax,r); - } - - /* ************************************************************************* */ - void multiply(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r) { - r.setZero(); - Index i = 0; - BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { - JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); - Vector &y = r[i]; - for(JacobianFactor::const_iterator j = Ai->begin(); j != Ai->end(); ++j) { - y += Ai->getA(j) * x[*j]; - } - ++i; - } - } - - /* ************************************************************************* */ - void transposeMultiply(const GaussianFactorGraph& fg, const VectorValues &r, VectorValues &x) { - x.setZero(); - Index i = 0; - BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { + BOOST_FOREACH(const sharedFactor& Ai_G, *this) { JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); for(JacobianFactor::const_iterator j = Ai->begin(); j != Ai->end(); ++j) { - x[*j] += Ai->getA(j).transpose() * r[i]; + // Create the value as a zero vector if it does not exist. + pair xi = x.tryInsert(*j, Vector()); + if(xi.second) + xi.first->second = Vector::Zero(Ai->getDim(j)); + xi.first->second += Ai->getA(j).transpose() * *ei; } - ++i; + ++ ei; } + return x; } /* ************************************************************************* */ - boost::shared_ptr gaussianErrors_(const GaussianFactorGraph& fg, const VectorValues& x) { - boost::shared_ptr e(new Errors); - BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { + Errors GaussianFactorGraph::gaussianErrors(const VectorValues& x) const + { + Errors e; + BOOST_FOREACH(const sharedFactor& Ai_G, *this) { JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); - e->push_back(Ai->error_vector(x)); + e.push_back(Ai->error_vector(x)); } return e; } diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 03fd04215..50442ac6b 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -21,148 +21,132 @@ #pragma once -#include - -#include -#include -#include +#include +#include +#include #include -#include +#include +#include // Included here instead of fw-declared so we can use Errors::iterator namespace gtsam { - // Forward declaration to use as default argument, documented declaration below. - GTSAM_EXPORT FactorGraph::EliminationResult - EliminateQR(const FactorGraph& factors, size_t nrFrontals); + // Forward declarations + class GaussianFactorGraph; + class GaussianFactor; + class GaussianConditional; + class GaussianBayesNet; + class GaussianEliminationTree; + class GaussianBayesTree; + class GaussianJunctionTree; + /* ************************************************************************* */ + template<> struct EliminationTraits + { + typedef GaussianFactor FactorType; ///< Type of factors in factor graph + typedef GaussianFactorGraph FactorGraphType; ///< Type of the factor graph (e.g. GaussianFactorGraph) + typedef GaussianConditional ConditionalType; ///< Type of conditionals from elimination + typedef GaussianBayesNet BayesNetType; ///< Type of Bayes net from sequential elimination + typedef GaussianEliminationTree EliminationTreeType; ///< Type of elimination tree + typedef GaussianBayesTree BayesTreeType; ///< Type of Bayes tree + typedef GaussianJunctionTree JunctionTreeType; ///< Type of Junction tree + /// The default dense elimination function + static std::pair, boost::shared_ptr > + DefaultEliminate(const FactorGraphType& factors, const Ordering& keys) { + return EliminatePreferCholesky(factors, keys); } + }; + + /* ************************************************************************* */ /** * A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e. * Factor == GaussianFactor * VectorValues = A values structure of vectors * Most of the time, linear factor graphs arise by linearizing a non-linear factor graph. */ - class GaussianFactorGraph : public FactorGraph { + class GTSAM_EXPORT GaussianFactorGraph : + public FactorGraph, + public EliminateableFactorGraph + { public: - typedef boost::shared_ptr shared_ptr; - typedef FactorGraph Base; + typedef GaussianFactorGraph This; ///< Typedef to this class + typedef FactorGraph Base; ///< Typedef to base factor graph type + typedef EliminateableFactorGraph BaseEliminateable; ///< Typedef to base elimination class + typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class - /** - * Default constructor - */ + /** Default constructor */ GaussianFactorGraph() {} - /** - * Constructor that receives a Chordal Bayes Net and returns a GaussianFactorGraph - */ - GTSAM_EXPORT GaussianFactorGraph(const GaussianBayesNet& CBN); + /** Construct from iterator over factors */ + template + GaussianFactorGraph(ITERATOR firstFactor, ITERATOR lastFactor) : Base(firstFactor, lastFactor) {} - /** - * Constructor that receives a BayesTree and returns a GaussianFactorGraph - */ - template - GaussianFactorGraph(const BayesTree& gbt) : Base(gbt) {} + /** Construct from container of factors (shared_ptr or plain objects) */ + template + explicit GaussianFactorGraph(const CONTAINER& factors) : Base(factors) {} - /** Constructor from a factor graph of GaussianFactor or a derived type */ + /** Implicit copy/downcast constructor to override explicit template container constructor */ template - GaussianFactorGraph(const FactorGraph& fg) { - push_back(fg); - } + GaussianFactorGraph(const FactorGraph& graph) : Base(graph) {} + + /** Virtual destructor */ + virtual ~GaussianFactorGraph() {} + + /// @name Testable + /// @{ + + bool equals(const This& fg, double tol = 1e-9) const; + + /// @} /** Add a factor by value - makes a copy */ - void add(const GaussianFactor& factor) { - factors_.push_back(factor.clone()); - } + void add(const GaussianFactor& factor) { push_back(factor.clone()); } /** Add a factor by pointer - stores pointer without copying the factor */ - void add(const sharedFactor& factor) { - factors_.push_back(factor); - } + void add(const sharedFactor& factor) { push_back(factor); } /** Add a null factor */ void add(const Vector& b) { - add(JacobianFactor(b)); - } + add(JacobianFactor(b)); } /** Add a unary factor */ - void add(Index key1, const Matrix& A1, - const Vector& b, const SharedDiagonal& model) { - add(JacobianFactor(key1,A1,b,model)); - } + void add(Key key1, const Matrix& A1, + const Vector& b, const SharedDiagonal& model = SharedDiagonal()) { + add(JacobianFactor(key1,A1,b,model)); } /** Add a binary factor */ - void add(Index key1, const Matrix& A1, - Index key2, const Matrix& A2, - const Vector& b, const SharedDiagonal& model) { - add(JacobianFactor(key1,A1,key2,A2,b,model)); - } + void add(Key key1, const Matrix& A1, + Key key2, const Matrix& A2, + const Vector& b, const SharedDiagonal& model = SharedDiagonal()) { + add(JacobianFactor(key1,A1,key2,A2,b,model)); } /** Add a ternary factor */ - void add(Index key1, const Matrix& A1, - Index key2, const Matrix& A2, - Index key3, const Matrix& A3, - const Vector& b, const SharedDiagonal& model) { - add(JacobianFactor(key1,A1,key2,A2,key3,A3,b,model)); - } + void add(Key key1, const Matrix& A1, + Key key2, const Matrix& A2, + Key key3, const Matrix& A3, + const Vector& b, const SharedDiagonal& model = SharedDiagonal()) { + add(JacobianFactor(key1,A1,key2,A2,key3,A3,b,model)); } /** Add an n-ary factor */ - void add(const std::vector > &terms, - const Vector &b, const SharedDiagonal& model) { - add(JacobianFactor(terms,b,model)); - } + template + void add(const TERMS& terms, const Vector &b, const SharedDiagonal& model = SharedDiagonal()) { + add(JacobianFactor(terms,b,model)); } /** * Return the set of variables involved in the factors (computes a set * union). */ - typedef FastSet Keys; - GTSAM_EXPORT Keys keys() const; + typedef FastSet Keys; + Keys keys() const; - - /** Eliminate the first \c n frontal variables, returning the resulting - * conditional and remaining factor graph - this is very inefficient for - * eliminating all variables, to do that use EliminationTree or - * JunctionTree. Note that this version simply calls - * FactorGraph::eliminateFrontals with EliminateQR as the - * eliminate function argument. - */ - std::pair eliminateFrontals(size_t nFrontals, const Eliminate& function = EliminateQR) const { - return Base::eliminateFrontals(nFrontals, function); } - - /** Factor the factor graph into a conditional and a remaining factor graph. - * Given the factor graph \f$ f(X) \f$, and \c variables to factorize out - * \f$ V \f$, this function factorizes into \f$ f(X) = f(V;Y)f(Y) \f$, where - * \f$ Y := X\V \f$ are the remaining variables. If \f$ f(X) = p(X) \f$ is - * a probability density or likelihood, the factorization produces a - * conditional probability density and a marginal \f$ p(X) = p(V|Y)p(Y) \f$. - * - * For efficiency, this function treats the variables to eliminate - * \c variables as fully-connected, so produces a dense (fully-connected) - * conditional on all of the variables in \c variables, instead of a sparse - * BayesNet. If the variables are not fully-connected, it is more efficient - * to sequentially factorize multiple times. - * Note that this version simply calls - * FactorGraph::eliminate with EliminateQR as the eliminate - * function argument. - */ - std::pair eliminate(const std::vector& variables, const Eliminate& function = EliminateQR) const { - return Base::eliminate(variables, function); } - - /** Eliminate a single variable, by calling GaussianFactorGraph::eliminate. */ - std::pair eliminateOne(Index variable, const Eliminate& function = EliminateQR) const { - return Base::eliminateOne(variable, function); } - - /** Permute the variables in the factors */ - GTSAM_EXPORT void permuteWithInverse(const Permutation& inversePermutation); - - /** Apply a reduction, which is a remapping of variable indices. */ - GTSAM_EXPORT void reduceWithInverse(const internal::Reduction& inverseReduction); + std::vector getkeydim() const; /** unnormalized error */ double error(const VectorValues& x) const { double total_error = 0.; - BOOST_FOREACH(const sharedFactor& factor, *this) - total_error += factor->error(x); + BOOST_FOREACH(const sharedFactor& factor, *this){ + if(factor) + total_error += factor->error(x); + } return total_error; } @@ -172,33 +156,36 @@ namespace gtsam { } /** - * Static function that combines two factor graphs. - * @param lfg1 Linear factor graph - * @param lfg2 Linear factor graph - * @return a new combined factor graph + * Clone() performs a deep-copy of the graph, including all of the factors. + * Cloning preserves null factors so indices for the original graph are still + * valid for the cloned graph. */ - GTSAM_EXPORT static GaussianFactorGraph combine2(const GaussianFactorGraph& lfg1, - const GaussianFactorGraph& lfg2); + GaussianFactorGraph clone() const; /** - * combine two factor graphs - * @param *lfg Linear factor graph + * Returns the negation of all factors in this graph - corresponds to antifactors. + * Will convert all factors to HessianFactors due to negation of information. + * Cloning preserves null factors so indices for the original graph are still + * valid for the cloned graph. */ - GTSAM_EXPORT void combine(const GaussianFactorGraph &lfg); + GaussianFactorGraph negate() const; + + ///@name Linear Algebra + ///@{ /** * Return vector of i, j, and s to generate an m-by-n sparse Jacobian matrix, * where i(k) and j(k) are the base 0 row and column indices, s(k) a double. * The standard deviations are baked into A and b */ - GTSAM_EXPORT std::vector > sparseJacobian() const; + std::vector > sparseJacobian() const; /** * Matrix version of sparseJacobian: generates a 3*m matrix with [i,j,s] entries * such that S(i(k),j(k)) = s(k), which can be given to MATLAB's sparse. * The standard deviations are baked into A and b */ - GTSAM_EXPORT Matrix sparseJacobian_() const; + Matrix sparseJacobian_() const; /** * Return a dense \f$ [ \;A\;b\; ] \in \mathbb{R}^{m \times n+1} \f$ @@ -207,7 +194,7 @@ namespace gtsam { * \f$ \frac{1}{2} \Vert Ax-b \Vert^2 \f$. See also * GaussianFactorGraph::jacobian and GaussianFactorGraph::sparseJacobian. */ - GTSAM_EXPORT Matrix augmentedJacobian() const; + Matrix augmentedJacobian(boost::optional optionalOrdering = boost::none) const; /** * Return the dense Jacobian \f$ A \f$ and right-hand-side \f$ b \f$, @@ -216,7 +203,7 @@ namespace gtsam { * GaussianFactorGraph::augmentedJacobian and * GaussianFactorGraph::sparseJacobian. */ - GTSAM_EXPORT std::pair jacobian() const; + std::pair jacobian(boost::optional optionalOrdering = boost::none) const; /** * Return a dense \f$ \Lambda \in \mathbb{R}^{n+1 \times n+1} \f$ Hessian @@ -229,7 +216,7 @@ namespace gtsam { and the negative log-likelihood is \f$ \frac{1}{2} x^T \Lambda x + \eta^T x + c \f$. */ - GTSAM_EXPORT Matrix augmentedHessian() const; + Matrix augmentedHessian(boost::optional optionalOrdering = boost::none) const; /** * Return the dense Hessian \f$ \Lambda \f$ and information vector @@ -237,7 +224,94 @@ namespace gtsam { * is \frac{1}{2} x^T \Lambda x + \eta^T x + c. See also * GaussianFactorGraph::augmentedHessian. */ - GTSAM_EXPORT std::pair hessian() const; + std::pair hessian(boost::optional optionalOrdering = boost::none) const; + + /** Return only the diagonal of the Hessian A'*A, as a VectorValues */ + virtual VectorValues hessianDiagonal() const; + + /** Return the block diagonal of the Hessian for this factor */ + virtual std::map hessianBlockDiagonal() const; + + /** Solve the factor graph by performing multifrontal variable elimination in COLAMD order using + * the dense elimination function specified in \c function (default EliminatePreferCholesky), + * followed by back-substitution in the Bayes tree resulting from elimination. Is equivalent + * to calling graph.eliminateMultifrontal()->optimize(). */ + VectorValues optimize(OptionalOrdering ordering = boost::none, + const Eliminate& function = EliminationTraitsType::DefaultEliminate) const; + + /** + * Compute the gradient of the energy function, + * \f$ \nabla_{x=x_0} \left\Vert \Sigma^{-1} A x - b \right\Vert^2 \f$, + * centered around \f$ x = x_0 \f$. + * The gradient is \f$ A^T(Ax-b) \f$. + * @param fg The Jacobian factor graph $(A,b)$ + * @param x0 The center about which to compute the gradient + * @return The gradient as a VectorValues + */ + VectorValues gradient(const VectorValues& x0) const; + + /** + * Compute the gradient of the energy function, \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} A x - b + * \right\Vert^2 \f$, centered around zero. The gradient is \f$ A^T(Ax-b) \f$. + * @param fg The Jacobian factor graph $(A,b)$ + * @param [output] g A VectorValues to store the gradient, which must be preallocated, + * see allocateVectorValues + * @return The gradient as a VectorValues */ + VectorValues gradientAtZero() const; + + /** Optimize along the gradient direction, with a closed-form computation to perform the line + * search. The gradient is computed about \f$ \delta x=0 \f$. + * + * This function returns \f$ \delta x \f$ that minimizes a reparametrized problem. The error + * function of a GaussianBayesNet is + * + * \f[ f(\delta x) = \frac{1}{2} |R \delta x - d|^2 = \frac{1}{2}d^T d - d^T R \delta x + + * \frac{1}{2} \delta x^T R^T R \delta x \f] + * + * with gradient and Hessian + * + * \f[ g(\delta x) = R^T(R\delta x - d), \qquad G(\delta x) = R^T R. \f] + * + * This function performs the line search in the direction of the gradient evaluated at \f$ g = + * g(\delta x = 0) \f$ with step size \f$ \alpha \f$ that minimizes \f$ f(\delta x = \alpha g) + * \f$: + * + * \f[ f(\alpha) = \frac{1}{2} d^T d + g^T \delta x + \frac{1}{2} \alpha^2 g^T G g \f] + * + * Optimizing by setting the derivative to zero yields \f$ \hat \alpha = (-g^T g) / (g^T G g) + * \f$. For efficiency, this function evaluates the denominator without computing the Hessian + * \f$ G \f$, returning + * + * \f[ \delta x = \hat\alpha g = \frac{-g^T g}{(R g)^T(R g)} \f] */ + VectorValues optimizeGradientSearch() const; + + /** x = A'*e */ + VectorValues transposeMultiply(const Errors& e) const; + + /** x += alpha*A'*e */ + void transposeMultiplyAdd(double alpha, const Errors& e, VectorValues& x) const; + + /** return A*x-b */ + Errors gaussianErrors(const VectorValues& x) const; + + ///** return A*x */ + Errors operator*(const VectorValues& x) const; + + ///** y += alpha*A'A*x */ + void multiplyHessianAdd(double alpha, const VectorValues& x, + VectorValues& y) const; + + ///** y += alpha*A'A*x */ + void multiplyHessianAdd(double alpha, const double* x, + double* y) const; + + ///** In-place version e <- A*x that overwrites e. */ + void multiplyInPlace(const VectorValues& x, Errors& e) const; + + /** In-place version e <- A*x that takes an iterator. */ + void multiplyInPlace(const VectorValues& x, const Errors::iterator& e) const; + + /// @} private: /** Serialization function */ @@ -249,140 +323,16 @@ namespace gtsam { }; - /** - * Combine and eliminate several factors. - * \addtogroup LinearSolving - */ - GTSAM_EXPORT JacobianFactor::shared_ptr CombineJacobians( - const FactorGraph& factors, - const VariableSlots& variableSlots); - /** * Evaluates whether linear factors have any constrained noise models * @return true if any factor is constrained. */ - GTSAM_EXPORT bool hasConstraints(const FactorGraph& factors); - - /** - * Densely combine and partially eliminate JacobianFactors to produce a - * single conditional with nrFrontals frontal variables and a remaining - * factor. - * Variables are eliminated in the natural order of the variable indices of in the factors. - * @param factors Factors to combine and eliminate - * @param nrFrontals Number of frontal variables to eliminate. - * @return The conditional and remaining factor - - * \addtogroup LinearSolving - */ - GTSAM_EXPORT GaussianFactorGraph::EliminationResult EliminateJacobians(const FactorGraph< - JacobianFactor>& factors, size_t nrFrontals = 1); - - /** - * Densely partially eliminate with QR factorization. HessianFactors are - * first factored with Cholesky decomposition to produce JacobianFactors, - * by calling the conversion constructor JacobianFactor(const HessianFactor&). - * Variables are eliminated in the natural order of the variable indices of in - * the factors. - * @param factors Factors to combine and eliminate - * @param nrFrontals Number of frontal variables to eliminate. - * @return The conditional and remaining factor - - * \addtogroup LinearSolving - */ - GTSAM_EXPORT GaussianFactorGraph::EliminationResult EliminateQR(const FactorGraph< - GaussianFactor>& factors, size_t nrFrontals = 1); - - /** - * Densely partially eliminate with Cholesky factorization. JacobianFactors - * are left-multiplied with their transpose to form the Hessian using the - * conversion constructor HessianFactor(const JacobianFactor&). - * - * If any factors contain constrained noise models (any sigmas equal to - * zero), QR factorization will be performed instead, because our current - * implementation cannot handle constrained noise models in Cholesky - * factorization. EliminateCholesky(), on the other hand, will fail if any - * factors contain constrained noise models. - * - * Variables are eliminated in the natural order of the variable indices of in - * the factors. - * @param factors Factors to combine and eliminate - * @param nrFrontals Number of frontal variables to eliminate. - * @return The conditional and remaining factor - - * \addtogroup LinearSolving - */ - GTSAM_EXPORT GaussianFactorGraph::EliminationResult EliminatePreferCholesky(const FactorGraph< - GaussianFactor>& factors, size_t nrFrontals = 1); - - /** - * Densely partially eliminate with Cholesky factorization. JacobianFactors - * are left-multiplied with their transpose to form the Hessian using the - * conversion constructor HessianFactor(const JacobianFactor&). - * - * If any factors contain constrained noise models, this function will fail - * because our current implementation cannot handle constrained noise models - * in Cholesky factorization. The function EliminatePreferCholesky() - * automatically does QR instead when this is the case. - * - * Variables are eliminated in the natural order of the variable indices of in - * the factors. - * @param factors Factors to combine and eliminate - * @param nrFrontals Number of frontal variables to eliminate. - * @return The conditional and remaining factor - - * \addtogroup LinearSolving - */ - GTSAM_EXPORT GaussianFactorGraph::EliminationResult EliminateCholesky(const FactorGraph< - GaussianFactor>& factors, size_t nrFrontals = 1); + GTSAM_EXPORT bool hasConstraints(const GaussianFactorGraph& factors); /****** Linear Algebra Opeations ******/ - /** return A*x */ - GTSAM_EXPORT Errors operator*(const GaussianFactorGraph& fg, const VectorValues& x); - - /** In-place version e <- A*x that overwrites e. */ - GTSAM_EXPORT void multiplyInPlace(const GaussianFactorGraph& fg, const VectorValues& x, Errors& e); - - /** In-place version e <- A*x that takes an iterator. */ - GTSAM_EXPORT void multiplyInPlace(const GaussianFactorGraph& fg, const VectorValues& x, const Errors::iterator& e); - - /** x += alpha*A'*e */ - GTSAM_EXPORT void transposeMultiplyAdd(const GaussianFactorGraph& fg, double alpha, const Errors& e, VectorValues& x); - - /** - * Compute the gradient of the energy function, - * \f$ \nabla_{x=x_0} \left\Vert \Sigma^{-1} A x - b \right\Vert^2 \f$, - * centered around \f$ x = x_0 \f$. - * The gradient is \f$ A^T(Ax-b) \f$. - * @param fg The Jacobian factor graph $(A,b)$ - * @param x0 The center about which to compute the gradient - * @return The gradient as a VectorValues - */ - GTSAM_EXPORT VectorValues gradient(const GaussianFactorGraph& fg, const VectorValues& x0); - - /** - * Compute the gradient of the energy function, - * \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} A x - b \right\Vert^2 \f$, - * centered around zero. - * The gradient is \f$ A^T(Ax-b) \f$. - * @param fg The Jacobian factor graph $(A,b)$ - * @param [output] g A VectorValues to store the gradient, which must be preallocated, see allocateVectorValues - * @return The gradient as a VectorValues - */ - GTSAM_EXPORT void gradientAtZero(const GaussianFactorGraph& fg, VectorValues& g); - - /* matrix-vector operations */ - GTSAM_EXPORT void residual(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r); - GTSAM_EXPORT void multiply(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r); - GTSAM_EXPORT void transposeMultiply(const GaussianFactorGraph& fg, const VectorValues &r, VectorValues &x); - - /** shared pointer version - * \todo Make this a member function - affects SubgraphPreconditioner */ - GTSAM_EXPORT boost::shared_ptr gaussianErrors_(const GaussianFactorGraph& fg, const VectorValues& x); - - /** return A*x-b - * \todo Make this a member function - affects SubgraphPreconditioner */ - inline Errors gaussianErrors(const GaussianFactorGraph& fg, const VectorValues& x) { - return *gaussianErrors_(fg, x); } + ///* matrix-vector operations */ + //GTSAM_EXPORT void residual(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r); + //GTSAM_EXPORT void multiply(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r); } // namespace gtsam diff --git a/gtsam/linear/GaussianISAM.cpp b/gtsam/linear/GaussianISAM.cpp index a358e5cab..ddbc5c1b3 100644 --- a/gtsam/linear/GaussianISAM.cpp +++ b/gtsam/linear/GaussianISAM.cpp @@ -10,58 +10,25 @@ * -------------------------------------------------------------------------- */ /** - * @file GaussianISAM.cpp - * @brief Linear ISAM only - * @author Michael Kaess + * @file SymbolicISAM.cpp + * @date July 29, 2013 + * @author Frank Dellaert + * @author Richard Roberts */ #include -#include - -#include - -using namespace std; -using namespace gtsam; +#include namespace gtsam { -// Explicitly instantiate so we don't have to include everywhere -template class ISAM; + // Instantiate base class + template class ISAM; + + /* ************************************************************************* */ + GaussianISAM::GaussianISAM() {} + + /* ************************************************************************* */ + GaussianISAM::GaussianISAM(const GaussianBayesTree& bayesTree) : + Base(bayesTree) {} -/* ************************************************************************* */ -GaussianFactor::shared_ptr GaussianISAM::marginalFactor(Index j) const { - return Super::marginalFactor(j, &EliminateQR); } - -/* ************************************************************************* */ -BayesNet::shared_ptr GaussianISAM::marginalBayesNet(Index j) const { - return Super::marginalBayesNet(j, &EliminateQR); -} - -/* ************************************************************************* */ -Matrix GaussianISAM::marginalCovariance(Index j) const { - GaussianConditional::shared_ptr conditional = marginalBayesNet(j)->front(); - return conditional->information().inverse(); -} - -/* ************************************************************************* */ - BayesNet::shared_ptr GaussianISAM::jointBayesNet( - Index key1, Index key2) const { - return Super::jointBayesNet(key1, key2, &EliminateQR); -} - -/* ************************************************************************* */ -VectorValues optimize(const GaussianISAM& isam) { - VectorValues result(isam.dims_); - // Call optimize for BayesTree - optimizeInPlace((const BayesTree&)isam, result); - return result; -} - -/* ************************************************************************* */ -BayesNet GaussianISAM::shortcut(sharedClique clique, sharedClique root) { - return clique->shortcut(root,&EliminateQR); -} -/* ************************************************************************* */ - -} // \namespace gtsam diff --git a/gtsam/linear/GaussianISAM.h b/gtsam/linear/GaussianISAM.h index fcee326e4..24cf2a95d 100644 --- a/gtsam/linear/GaussianISAM.h +++ b/gtsam/linear/GaussianISAM.h @@ -10,93 +10,37 @@ * -------------------------------------------------------------------------- */ /** - * @file GaussianISAM.h - * @brief Linear ISAM only - * @author Michael Kaess + * @file SymbolicISAM.h + * @date July 29, 2013 + * @author Frank Dellaert + * @author Richard Roberts */ -// \callgraph - #pragma once -#include - +#include #include -#include -#include namespace gtsam { -class GaussianISAM : public ISAM { + class GTSAM_EXPORT GaussianISAM : public ISAM + { + public: + typedef ISAM Base; + typedef GaussianISAM This; + typedef boost::shared_ptr shared_ptr; - typedef ISAM Super; - std::vector dims_; + /// @name Standard Constructors + /// @{ -public: + /** Create an empty Bayes Tree */ + GaussianISAM(); - typedef std::vector Dims; + /** Copy constructor */ + GaussianISAM(const GaussianBayesTree& bayesTree); - /** Create an empty Bayes Tree */ - GaussianISAM() : Super() {} + /// @} - /** Create a Bayes Tree from a Bayes Net */ -// GaussianISAM(const GaussianBayesNet& bayesNet) : Super(bayesNet) {} - GaussianISAM(const BayesTree& bayesTree) : Super(bayesTree) { - GaussianJunctionTree::countDims(bayesTree, dims_); - } + }; - /** Override update_internal to also keep track of variable dimensions. */ - template - void update_internal(const FACTORGRAPH& newFactors, Cliques& orphans) { - Super::update_internal(newFactors, orphans, &EliminateQR); // TODO: why does this force QR? - update_dimensions(newFactors); - } - - template - void update(const FACTORGRAPH& newFactors) { - Cliques orphans; - this->update_internal(newFactors, orphans); - } - - template - inline void update_dimensions(const FACTORGRAPH& newFactors) { - BOOST_FOREACH(const typename FACTORGRAPH::sharedFactor& factor, newFactors) { - for(typename FACTORGRAPH::FactorType::const_iterator key = factor->begin(); key != factor->end(); ++key) { - if(*key >= dims_.size()) - dims_.resize(*key + 1); - if(dims_[*key] == 0) - dims_[*key] = factor->getDim(key); - else - assert(dims_[*key] == factor->getDim(key)); - } - } - } - - void clear() { - Super::clear(); - dims_.clear(); - } - - // access - const Dims& dims() const { return dims_; } ///< Const access to dimensions structure - Dims& dims() { return dims_; } ///< non-const access to dimensions structure (advanced interface) - - GTSAM_EXPORT friend VectorValues optimize(const GaussianISAM&); - - /** return marginal on any variable as a factor, Bayes net, or mean/cov */ - GTSAM_EXPORT GaussianFactor::shared_ptr marginalFactor(Index j) const; - GTSAM_EXPORT BayesNet::shared_ptr marginalBayesNet(Index key) const; - GTSAM_EXPORT Matrix marginalCovariance(Index key) const; - - /** return joint between two variables, as a Bayes net */ - GTSAM_EXPORT BayesNet::shared_ptr jointBayesNet(Index key1, Index key2) const; - - /** return the conditional P(S|Root) on the separator given the root */ - GTSAM_EXPORT static BayesNet shortcut(sharedClique clique, sharedClique root); - -}; // \class GaussianISAM - -// optimize the BayesTree, starting from the root -GTSAM_EXPORT VectorValues optimize(const GaussianISAM& isam); - -} // \namespace gtsam +} diff --git a/gtsam/linear/GaussianJunctionTree.cpp b/gtsam/linear/GaussianJunctionTree.cpp index 381c35e3f..982c17bd3 100644 --- a/gtsam/linear/GaussianJunctionTree.cpp +++ b/gtsam/linear/GaussianJunctionTree.cpp @@ -11,49 +11,24 @@ /** * @file GaussianJunctionTree.cpp - * @date Jul 12, 2010 - * @author Kai Ni + * @date Mar 29, 2013 * @author Frank Dellaert - * @brief: the Gaussian junction tree + * @author Richard Roberts */ -#include -#include +#include #include -#include - -#include - -#include +#include namespace gtsam { - // explicit template instantiation - template class JunctionTree; - template class ClusterTree; - - using namespace std; + // Instantiate base classes + template class ClusterTree; + template class JunctionTree; /* ************************************************************************* */ - VectorValues GaussianJunctionTree::optimize(Eliminate function) const { - gttic(GJT_eliminate); - // eliminate from leaves to the root - BTClique::shared_ptr rootClique(this->eliminate(function)); - gttoc(GJT_eliminate); + GaussianJunctionTree::GaussianJunctionTree( + const GaussianEliminationTree& eliminationTree) : + Base(eliminationTree) {} - // Allocate solution vector and copy RHS - gttic(allocate_VectorValues); - vector dims(rootClique->conditional()->back()+1, 0); - countDims(rootClique, dims); - VectorValues result(dims); - gttoc(allocate_VectorValues); - - // back-substitution - gttic(backsubstitute); - internal::optimizeInPlace(rootClique, result); - gttoc(backsubstitute); - return result; - } - - /* ************************************************************************* */ -} //namespace gtsam +} diff --git a/gtsam/linear/GaussianJunctionTree.h b/gtsam/linear/GaussianJunctionTree.h index ba67e6002..f944d417b 100644 --- a/gtsam/linear/GaussianJunctionTree.h +++ b/gtsam/linear/GaussianJunctionTree.h @@ -11,78 +11,56 @@ /** * @file GaussianJunctionTree.h - * @date Jul 12, 2010 - * @author Kai Ni + * @date Mar 29, 2013 * @author Frank Dellaert - * @brief: the Gaussian junction tree + * @author Richard Roberts */ -#pragma once - -#include -#include -#include -#include #include +#include +#include namespace gtsam { + // Forward declarations + class GaussianEliminationTree; + /** - * A JunctionTree where all the factors are of type GaussianFactor. + * A ClusterTree, i.e., a set of variable clusters with factors, arranged in a tree, with + * the additional property that it represents the clique tree associated with a Bayes net. * - * In GTSAM, typically, a GaussianJunctionTree is created directly from a GaussianFactorGraph, - * after which you call optimize() to solve for the mean, or JunctionTree::eliminate() to - * create a BayesTree. In both cases, you need to provide a basic - * GaussianFactorGraph::Eliminate function that will be used to + * In GTSAM a junction tree is an intermediate data structure in multifrontal + * variable elimination. Each node is a cluster of factors, along with a + * clique of variables that are eliminated all at once. In detail, every node k represents + * a clique (maximal fully connected subset) of an associated chordal graph, such as a + * chordal Bayes net resulting from elimination. + * + * The difference with the BayesTree is that a JunctionTree stores factors, whereas a + * BayesTree stores conditionals, that are the product of eliminating the factors in the + * corresponding JunctionTree cliques. + * + * The tree structure and elimination method are exactly analagous to the EliminationTree, + * except that in the JunctionTree, at each node multiple variables are eliminated at a time. * * \addtogroup Multifrontal + * \nosubgrouping */ - class GaussianJunctionTree: public JunctionTree { - + class GTSAM_EXPORT GaussianJunctionTree : + public JunctionTree { public: - typedef boost::shared_ptr shared_ptr; - typedef JunctionTree Base; - typedef Base::sharedClique sharedClique; - typedef GaussianFactorGraph::Eliminate Eliminate; - - public : - - /** Default constructor */ - GaussianJunctionTree() : Base() {} - - /** Constructor from a factor graph. Builds a VariableIndex. */ - GaussianJunctionTree(const GaussianFactorGraph& fg) : Base(fg) {} - - /** Construct from a factor graph and a pre-computed variable index. */ - GaussianJunctionTree(const GaussianFactorGraph& fg, const VariableIndex& variableIndex) - : Base(fg, variableIndex) {} - + typedef JunctionTree Base; ///< Base class + typedef GaussianJunctionTree This; ///< This class + typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class + /** - * optimize the linear graph - */ - GTSAM_EXPORT VectorValues optimize(Eliminate function) const; + * Build the elimination tree of a factor graph using pre-computed column structure. + * @param factorGraph The factor graph for which to build the elimination tree + * @param structure The set of factors involving each variable. If this is not + * precomputed, you can call the Create(const FactorGraph&) + * named constructor instead. + * @return The elimination tree + */ + GaussianJunctionTree(const GaussianEliminationTree& eliminationTree); + }; - // convenient function to return dimensions of all variables in the BayesTree - template - static void countDims(const BayesTree& bayesTree, DIM_CONTAINER& dims) { - dims = DIM_CONTAINER(bayesTree.root()->conditional()->back()+1, 0); - countDims(bayesTree.root(), dims); - } - - private: - template - static void countDims(const boost::shared_ptr& clique, DIM_CONTAINER& dims) { - GaussianConditional::const_iterator it = clique->conditional()->beginFrontals(); - for (; it != clique->conditional()->endFrontals(); ++it) { - assert(dims.at(*it) == 0); - dims.at(*it) = clique->conditional()->dim(it); - } - - BOOST_FOREACH(const typename CLIQUE::shared_ptr& child, clique->children()) { - countDims(child, dims); - } - } - - }; // GaussianJunctionTree - -} // namespace gtsam +} diff --git a/gtsam/linear/GaussianMultifrontalSolver.cpp b/gtsam/linear/GaussianMultifrontalSolver.cpp deleted file mode 100644 index af4fe90ec..000000000 --- a/gtsam/linear/GaussianMultifrontalSolver.cpp +++ /dev/null @@ -1,93 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 GaussianMultifrontalSolver.cpp - * @author Richard Roberts - * @date Oct 21, 2010 - */ - -#include - -namespace gtsam { - -/* ************************************************************************* */ -GaussianMultifrontalSolver::GaussianMultifrontalSolver(const FactorGraph& factorGraph, bool useQR) : - Base(factorGraph), useQR_(useQR) {} - -/* ************************************************************************* */ -GaussianMultifrontalSolver::GaussianMultifrontalSolver(const FactorGraph::shared_ptr& factorGraph, - const VariableIndex::shared_ptr& variableIndex, bool useQR) : - Base(factorGraph, variableIndex), useQR_(useQR) {} - -/* ************************************************************************* */ -GaussianMultifrontalSolver::shared_ptr -GaussianMultifrontalSolver::Create(const FactorGraph::shared_ptr& factorGraph, - const VariableIndex::shared_ptr& variableIndex, bool useQR) { - return shared_ptr(new GaussianMultifrontalSolver(factorGraph, variableIndex, useQR)); -} - -/* ************************************************************************* */ -void GaussianMultifrontalSolver::replaceFactors(const FactorGraph::shared_ptr& factorGraph) { - Base::replaceFactors(factorGraph); -} - -/* ************************************************************************* */ -GaussianBayesTree::shared_ptr GaussianMultifrontalSolver::eliminate() const { - if (useQR_) - return Base::eliminate(&EliminateQR); - else - return Base::eliminate(&EliminatePreferCholesky); -} - -/* ************************************************************************* */ -VectorValues::shared_ptr GaussianMultifrontalSolver::optimize() const { - gttic(optimize); - VectorValues::shared_ptr values; - if (useQR_) - values = VectorValues::shared_ptr(new VectorValues(junctionTree_->optimize(&EliminateQR))); - else - values= VectorValues::shared_ptr(new VectorValues(junctionTree_->optimize(&EliminatePreferCholesky))); - gttoc(optimize); - return values; -} - -/* ************************************************************************* */ -GaussianFactorGraph::shared_ptr GaussianMultifrontalSolver::jointFactorGraph(const std::vector& js) const { - if (useQR_) - return GaussianFactorGraph::shared_ptr(new GaussianFactorGraph(*Base::jointFactorGraph(js,&EliminateQR))); - else - return GaussianFactorGraph::shared_ptr(new GaussianFactorGraph(*Base::jointFactorGraph(js,&EliminatePreferCholesky))); -} - -/* ************************************************************************* */ -GaussianFactor::shared_ptr GaussianMultifrontalSolver::marginalFactor(Index j) const { - if (useQR_) - return Base::marginalFactor(j,&EliminateQR); - else - return Base::marginalFactor(j,&EliminatePreferCholesky); -} - -/* ************************************************************************* */ -Matrix GaussianMultifrontalSolver::marginalCovariance(Index j) const { - FactorGraph fg; - GaussianConditional::shared_ptr conditional; - if (useQR_) { - fg.push_back(Base::marginalFactor(j,&EliminateQR)); - conditional = EliminateQR(fg,1).first; - } else { - fg.push_back(Base::marginalFactor(j,&EliminatePreferCholesky)); - conditional = EliminatePreferCholesky(fg,1).first; - } - return conditional->information().inverse(); -} - -} diff --git a/gtsam/linear/GaussianMultifrontalSolver.h b/gtsam/linear/GaussianMultifrontalSolver.h deleted file mode 100644 index 97f3de67c..000000000 --- a/gtsam/linear/GaussianMultifrontalSolver.h +++ /dev/null @@ -1,132 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 GaussianMultifrontalSolver.h - * @author Richard Roberts - * @date Oct 21, 2010 - */ - -#pragma once - -#include -#include -#include -#include - -#include - -namespace gtsam { - -/** - * This solver uses multifrontal elimination to solve a GaussianFactorGraph, - * i.e. a sparse linear system. Underlying this is a junction tree, which is - * eliminated into a Bayes tree. - * - * The elimination ordering is "baked in" to the variable indices at this - * stage, i.e. elimination proceeds in order from '0'. A fill-reducing - * ordering is computed symbolically from the NonlinearFactorGraph, on the - * nonlinear side of gtsam. (It is actually also possible to permute an - * existing GaussianFactorGraph into a COLAMD ordering instead, this is done - * when computing marginals). - * - * The JunctionTree recursively produces a BayesTree, - * on which this class calls optimize(...) to perform back-substitution. - */ -class GaussianMultifrontalSolver : GenericMultifrontalSolver { - -protected: - - typedef GenericMultifrontalSolver Base; - typedef boost::shared_ptr shared_ptr; - - /** flag to determine whether to use Cholesky or QR */ - bool useQR_; - -public: - - /** - * Construct the solver for a factor graph. This builds the junction - * tree, which already does some of the work of elimination. - */ - GTSAM_EXPORT GaussianMultifrontalSolver(const FactorGraph& factorGraph, bool useQR = false); - - /** - * Construct the solver with a shared pointer to a factor graph and to a - * VariableIndex. The solver will store these pointers, so this constructor - * is the fastest. - */ - GTSAM_EXPORT GaussianMultifrontalSolver(const FactorGraph::shared_ptr& factorGraph, - const VariableIndex::shared_ptr& variableIndex, bool useQR = false); - - /** - * Named constructor to return a shared_ptr. This builds the elimination - * tree, which already does some of the symbolic work of elimination. - */ - GTSAM_EXPORT static shared_ptr Create(const FactorGraph::shared_ptr& factorGraph, - const VariableIndex::shared_ptr& variableIndex, bool useQR = false); - - /** - * Return a new solver that solves the given factor graph, which must have - * the *same structure* as the one this solver solves. For some solvers this - * is more efficient than constructing the solver from scratch. This can be - * used in cases where the numerical values of the linear problem change, - * e.g. during iterative nonlinear optimization. - */ - GTSAM_EXPORT void replaceFactors(const FactorGraph::shared_ptr& factorGraph); - - /** - * Eliminate the factor graph sequentially. Uses a column elimination tree - * to recursively eliminate. - */ - GTSAM_EXPORT GaussianBayesTree::shared_ptr eliminate() const; - - /** - * Compute the least-squares solution of the GaussianFactorGraph. This - * eliminates to create a BayesNet and then back-substitutes this BayesNet to - * obtain the solution. - */ - GTSAM_EXPORT VectorValues::shared_ptr optimize() const; - - /** - * Compute the marginal joint over a set of variables, by integrating out - * all of the other variables. This function returns the result as a factor - * graph. - * - * NOTE: This function is limited to computing a joint on 2 variables - */ - GTSAM_EXPORT GaussianFactorGraph::shared_ptr jointFactorGraph(const std::vector& js) const; - - /** - * Compute the marginal Gaussian density over a variable, by integrating out - * all of the other variables. This function returns the result as an upper- - * triangular R factor and right-hand-side, i.e. a GaussianConditional with - * R*x = d. To get a mean and covariance matrix, use marginalStandard(...) - */ - GTSAM_EXPORT GaussianFactor::shared_ptr marginalFactor(Index j) const; - - /** - * Compute the marginal Gaussian density over a variable, by integrating out - * all of the other variables. This function returns the result as a mean - * vector and covariance matrix. Compared to marginalCanonical, which - * returns a GaussianConditional, this function back-substitutes the R factor - * to obtain the mean, then computes \f$ \Sigma = (R^T * R)^{-1} \f$. - */ - GTSAM_EXPORT Matrix marginalCovariance(Index j) const; - - /** @return true if using QR */ - inline bool usingQR() const { return useQR_; } - -}; - -} - - diff --git a/gtsam/linear/GaussianSequentialSolver.cpp b/gtsam/linear/GaussianSequentialSolver.cpp deleted file mode 100644 index 75a8c3871..000000000 --- a/gtsam/linear/GaussianSequentialSolver.cpp +++ /dev/null @@ -1,120 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 GaussianSequentialSolver.cpp - * @author Richard Roberts - * @date Oct 19, 2010 - */ - -#include -#include - -namespace gtsam { - -/* ************************************************************************* */ -GaussianSequentialSolver::GaussianSequentialSolver( - const FactorGraph& factorGraph, bool useQR) : - Base(factorGraph), useQR_(useQR) { -} - -/* ************************************************************************* */ -GaussianSequentialSolver::GaussianSequentialSolver( - const FactorGraph::shared_ptr& factorGraph, - const VariableIndex::shared_ptr& variableIndex, bool useQR) : - Base(factorGraph, variableIndex), useQR_(useQR) { -} - -/* ************************************************************************* */ -GaussianSequentialSolver::shared_ptr GaussianSequentialSolver::Create( - const FactorGraph::shared_ptr& factorGraph, - const VariableIndex::shared_ptr& variableIndex, bool useQR) { - return shared_ptr( - new GaussianSequentialSolver(factorGraph, variableIndex, useQR)); -} - -/* ************************************************************************* */ -void GaussianSequentialSolver::replaceFactors( - const FactorGraph::shared_ptr& factorGraph) { - Base::replaceFactors(factorGraph); -} - -/* ************************************************************************* */ -GaussianBayesNet::shared_ptr GaussianSequentialSolver::eliminate() const { - if (useQR_) - return Base::eliminate(&EliminateQR); - else - return Base::eliminate(&EliminatePreferCholesky); -} - -/* ************************************************************************* */ -VectorValues::shared_ptr GaussianSequentialSolver::optimize() const { - - static const bool debug = false; - - if(debug) this->factors_->print("GaussianSequentialSolver, eliminating "); - if(debug) this->eliminationTree_->print("GaussianSequentialSolver, elimination tree "); - - gttic(eliminate); - // Eliminate using the elimination tree - GaussianBayesNet::shared_ptr bayesNet(this->eliminate()); - gttoc(eliminate); - - if(debug) bayesNet->print("GaussianSequentialSolver, Bayes net "); - - // Allocate the solution vector if it is not already allocated -// VectorValues::shared_ptr solution = allocateVectorValues(*bayesNet); - - gttic(optimize); - // Back-substitute - VectorValues::shared_ptr solution( - new VectorValues(gtsam::optimize(*bayesNet))); - gttoc(optimize); - - if(debug) solution->print("GaussianSequentialSolver, solution "); - - return solution; -} - -/* ************************************************************************* */ -GaussianFactor::shared_ptr GaussianSequentialSolver::marginalFactor(Index j) const { - if (useQR_) - return Base::marginalFactor(j,&EliminateQR); - else - return Base::marginalFactor(j,&EliminatePreferCholesky); -} - -/* ************************************************************************* */ -Matrix GaussianSequentialSolver::marginalCovariance(Index j) const { - FactorGraph fg; - GaussianConditional::shared_ptr conditional; - if (useQR_) { - fg.push_back(Base::marginalFactor(j, &EliminateQR)); - conditional = EliminateQR(fg, 1).first; - } else { - fg.push_back(Base::marginalFactor(j, &EliminatePreferCholesky)); - conditional = EliminatePreferCholesky(fg, 1).first; - } - return conditional->information().inverse(); -} - -/* ************************************************************************* */ -GaussianFactorGraph::shared_ptr -GaussianSequentialSolver::jointFactorGraph(const std::vector& js) const { - if (useQR_) - return GaussianFactorGraph::shared_ptr(new GaussianFactorGraph( - *Base::jointFactorGraph(js, &EliminateQR))); - else - return GaussianFactorGraph::shared_ptr(new GaussianFactorGraph( - *Base::jointFactorGraph(js, &EliminatePreferCholesky))); -} - -} /// namespace gtsam diff --git a/gtsam/linear/GaussianSequentialSolver.h b/gtsam/linear/GaussianSequentialSolver.h deleted file mode 100644 index c3c54154e..000000000 --- a/gtsam/linear/GaussianSequentialSolver.h +++ /dev/null @@ -1,133 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 GaussianSequentialSolver.h - * @brief Solves a GaussianFactorGraph (i.e. a sparse linear system) using sequential variable elimination. - * @author Richard Roberts - * @date Oct 19, 2010 - */ - -#pragma once - -#include -#include -#include - -#include - -namespace gtsam { - -/** This solver uses sequential variable elimination to solve a - * GaussianFactorGraph, i.e. a sparse linear system. Underlying this is a - * column elimination tree (inference/EliminationTree), see Gilbert 2001 BIT. - * - * The elimination ordering is "baked in" to the variable indices at this - * stage, i.e. elimination proceeds in order from '0'. A fill-reducing - * ordering is computed symbolically from the NonlinearFactorGraph, on the - * nonlinear side of gtsam. (To be precise, it is possible to permute an - * existing GaussianFactorGraph into a COLAMD ordering instead, this is done - * when computing marginals). - * - * This is not the most efficient algorithm we provide, most efficient is the - * MultifrontalSolver, which performs Multi-frontal QR factorization. However, - * sequential variable elimination is easier to understand so this is a good - * starting point to learn about these algorithms and our implementation. - * Additionally, the first step of MFQR is symbolic sequential elimination. - * - * The EliminationTree recursively produces a BayesNet, - * typedef'ed in linear/GaussianBayesNet, on which this class calls - * optimize(...) to perform back-substitution. - */ -class GaussianSequentialSolver : GenericSequentialSolver { - -protected: - - typedef GenericSequentialSolver Base; - typedef boost::shared_ptr shared_ptr; - - /** flag to determine whether to use Cholesky or QR */ - bool useQR_; - -public: - - /** - * Construct the solver for a factor graph. This builds the elimination - * tree, which already does some of the work of elimination. - */ - GTSAM_EXPORT GaussianSequentialSolver(const FactorGraph& factorGraph, bool useQR = false); - - /** - * Construct the solver with a shared pointer to a factor graph and to a - * VariableIndex. The solver will store these pointers, so this constructor - * is the fastest. - */ - GTSAM_EXPORT GaussianSequentialSolver(const FactorGraph::shared_ptr& factorGraph, - const VariableIndex::shared_ptr& variableIndex, bool useQR = false); - - /** - * Named constructor to return a shared_ptr. This builds the elimination - * tree, which already does some of the symbolic work of elimination. - */ - GTSAM_EXPORT static shared_ptr Create(const FactorGraph::shared_ptr& factorGraph, - const VariableIndex::shared_ptr& variableIndex, bool useQR = false); - - /** - * Return a new solver that solves the given factor graph, which must have - * the *same structure* as the one this solver solves. For some solvers this - * is more efficient than constructing the solver from scratch. This can be - * used in cases where the numerical values of the linear problem change, - * e.g. during iterative nonlinear optimization. - */ - GTSAM_EXPORT void replaceFactors(const FactorGraph::shared_ptr& factorGraph); - - /** - * Eliminate the factor graph sequentially. Uses a column elimination tree - * to recursively eliminate. - */ - GTSAM_EXPORT GaussianBayesNet::shared_ptr eliminate() const; - - /** - * Compute the least-squares solution of the GaussianFactorGraph. This - * eliminates to create a BayesNet and then back-substitutes this BayesNet to - * obtain the solution. - */ - GTSAM_EXPORT VectorValues::shared_ptr optimize() const; - - /** - * Compute the marginal Gaussian density over a variable, by integrating out - * all of the other variables. This function returns the result as an upper- - * triangular R factor and right-hand-side, i.e. a GaussianConditional with - * R*x = d. To get a mean and covariance matrix, use marginalStandard(...) - */ - GTSAM_EXPORT GaussianFactor::shared_ptr marginalFactor(Index j) const; - - /** - * Compute the marginal Gaussian density over a variable, by integrating out - * all of the other variables. This function returns the result as a mean - * vector and covariance matrix. Compared to marginalCanonical, which - * returns a GaussianConditional, this function back-substitutes the R factor - * to obtain the mean, then computes \f$ \Sigma = (R^T * R)^{-1} \f$. - */ - GTSAM_EXPORT Matrix marginalCovariance(Index j) const; - - /** - * Compute the marginal joint over a set of variables, by integrating out - * all of the other variables. This function returns the result as an upper- - * triangular R factor and right-hand-side, i.e. a GaussianBayesNet with - * R*x = d. To get a mean and covariance matrix, use jointStandard(...) - */ - GTSAM_EXPORT GaussianFactorGraph::shared_ptr jointFactorGraph(const std::vector& js) const; - -}; - -} - diff --git a/gtsam/linear/HessianFactor-inl.h b/gtsam/linear/HessianFactor-inl.h new file mode 100644 index 000000000..4a21ab0ff --- /dev/null +++ b/gtsam/linear/HessianFactor-inl.h @@ -0,0 +1,41 @@ +/* ---------------------------------------------------------------------------- + + * 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 HessianFactor-inl.h + * @brief Contains the HessianFactor class, a general quadratic factor + * @author Richard Roberts + * @date Dec 8, 2010 + */ + +#pragma once + +namespace gtsam { + + /* ************************************************************************* */ + template + HessianFactor::HessianFactor(const KEYS& keys, const SymmetricBlockMatrix& augmentedInformation) : + GaussianFactor(keys), info_(augmentedInformation) + { + // Check number of variables + if((DenseIndex)Base::keys_.size() != augmentedInformation.nBlocks() - 1) + throw std::invalid_argument( + "Error in HessianFactor constructor input. Number of provided keys plus\n" + "one for the information vector must equal the number of provided matrix blocks."); + + // Check RHS dimension + if(augmentedInformation(0, augmentedInformation.nBlocks() - 1).cols() != 1) + throw std::invalid_argument( + "Error in HessianFactor constructor input. The last provided matrix block\n" + "must be the information vector, but the last provided block had more than one column."); + } + +} diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 5baee8712..e62af9a05 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -15,7 +15,17 @@ * @date Dec 8, 2010 */ -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include #include @@ -29,20 +39,17 @@ #ifdef __GNUC__ #pragma GCC diagnostic pop #endif +#include +#include +#include +#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include using namespace std; +using namespace boost::assign; +namespace br { using namespace boost::range; using namespace boost::adaptors; } namespace gtsam { @@ -54,132 +61,128 @@ string SlotEntry::toString() const { } /* ************************************************************************* */ -Scatter::Scatter(const FactorGraph& gfg) { - // First do the set union. - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, gfg) { - if(factor) { - for(GaussianFactor::const_iterator variable = factor->begin(); variable != factor->end(); ++variable) { - this->insert(make_pair(*variable, SlotEntry(0, factor->getDim(variable)))); - } - } - } - - // Next fill in the slot indices (we can only get these after doing the set - // union. - size_t slot = 0; - BOOST_FOREACH(value_type& var_slot, *this) { - var_slot.second.slot = (slot ++); - } +Scatter::Scatter(const GaussianFactorGraph& gfg, + boost::optional ordering) { + gttic(Scatter_Constructor); + static const size_t none = std::numeric_limits::max(); + + // First do the set union. + BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, gfg) { + if (factor) { + for (GaussianFactor::const_iterator variable = factor->begin(); + variable != factor->end(); ++variable) { + // TODO: Fix this hack to cope with zero-row Jacobians that come from BayesTreeOrphanWrappers + const JacobianFactor* asJacobian = + dynamic_cast(factor.get()); + if (!asJacobian || asJacobian->cols() > 1) + this->insert( + make_pair(*variable, SlotEntry(none, factor->getDim(variable)))); + } + } + } + + // If we have an ordering, pre-fill the ordered variables first + size_t slot = 0; + if (ordering) { + BOOST_FOREACH(Key key, *ordering) { + const_iterator entry = find(key); + if (entry == end()) + throw std::invalid_argument( + "The ordering provided to the HessianFactor Scatter constructor\n" + "contained extra variables that did not appear in the factors to combine."); + at(key).slot = (slot++); + } + } + + // Next fill in the slot indices (we can only get these after doing the set + // union. + BOOST_FOREACH(value_type& var_slot, *this) { + if (var_slot.second.slot == none) + var_slot.second.slot = (slot++); + } } /* ************************************************************************* */ -void HessianFactor::assertInvariants() const { - GaussianFactor::assertInvariants(); // The base class checks for unique keys +HessianFactor::HessianFactor() : + info_(cref_list_of<1>(1)) +{ + linearTerm().setZero(); + constantTerm() = 0.0; } /* ************************************************************************* */ -HessianFactor::HessianFactor(const HessianFactor& gf) : - GaussianFactor(gf), info_(matrix_) { - info_.assignNoalias(gf.info_); - assertInvariants(); -} - -/* ************************************************************************* */ -HessianFactor::HessianFactor() : info_(matrix_) { - // The empty HessianFactor has only a constant error term of zero - FastVector dims; - dims.push_back(1); - info_.resize(dims.begin(), dims.end(), false); - info_(0,0)(0,0) = 0.0; - assertInvariants(); -} - -/* ************************************************************************* */ -HessianFactor::HessianFactor(Index j, const Matrix& G, const Vector& g, double f) : - GaussianFactor(j), info_(matrix_) { - if(G.rows() != G.cols() || G.rows() != g.size()) - throw invalid_argument("Inconsistent matrix and/or vector dimensions in HessianFactor constructor"); - size_t dims[] = { G.rows(), 1 }; - InfoMatrix fullMatrix(G.rows() + 1, G.rows() + 1); - BlockInfo infoMatrix(fullMatrix, dims, dims+2); - infoMatrix(0,0) = G; - infoMatrix.column(0,1,0) = g; - infoMatrix(1,1)(0,0) = f; - infoMatrix.swap(info_); - assertInvariants(); +HessianFactor::HessianFactor(Key j, const Matrix& G, const Vector& g, double f) : + GaussianFactor(cref_list_of<1>(j)), info_(cref_list_of<2>(G.cols())(1)) +{ + if(G.rows() != G.cols() || G.rows() != g.size()) throw invalid_argument( + "Attempting to construct HessianFactor with inconsistent matrix and/or vector dimensions"); + info_(0,0) = G; + info_(0,1) = g; + info_(1,1)(0,0) = f; } /* ************************************************************************* */ // error is 0.5*(x-mu)'*inv(Sigma)*(x-mu) = 0.5*(x'*G*x - 2*x'*G*mu + mu'*G*mu) // where G = inv(Sigma), g = G*mu, f = mu'*G*mu = mu'*g -HessianFactor::HessianFactor(Index j, const Vector& mu, const Matrix& Sigma) : - GaussianFactor(j), info_(matrix_) { +HessianFactor::HessianFactor(Key j, const Vector& mu, const Matrix& Sigma) : + GaussianFactor(cref_list_of<1>(j)), + info_(cref_list_of<2> (Sigma.cols()) (1) ) +{ if (Sigma.rows() != Sigma.cols() || Sigma.rows() != mu.size()) throw invalid_argument( - "Inconsistent matrix and/or vector dimensions in HessianFactor constructor"); - Matrix G = inverse(Sigma); - Vector g = G * mu; - double f = dot(mu, g); - size_t dims[] = { G.rows(), 1 }; - InfoMatrix fullMatrix(G.rows() + 1, G.rows() + 1); - BlockInfo infoMatrix(fullMatrix, dims, dims + 2); - infoMatrix(0, 0) = G; - infoMatrix.column(0, 1, 0) = g; - infoMatrix(1, 1)(0, 0) = f; - infoMatrix.swap(info_); - assertInvariants(); + "Attempting to construct HessianFactor with inconsistent matrix and/or vector dimensions"); + info_(0,0) = Sigma.inverse(); // G + info_(0,1) = info_(0,0).selfadjointView() * mu; // g + info_(1,1)(0,0) = mu.dot(info_(0,1).knownOffDiagonal().col(0)); // f } /* ************************************************************************* */ -HessianFactor::HessianFactor(Index j1, Index j2, +HessianFactor::HessianFactor(Key j1, Key j2, const Matrix& G11, const Matrix& G12, const Vector& g1, const Matrix& G22, const Vector& g2, double f) : - GaussianFactor(j1, j2), info_(matrix_) { - if(G11.rows() != G11.cols() || G11.rows() != G12.rows() || G11.rows() != g1.size() || - G22.cols() != G12.cols() || G22.cols() != g2.size()) - throw invalid_argument("Inconsistent matrix and/or vector dimensions in HessianFactor constructor"); - size_t dims[] = { G11.rows(), G22.rows(), 1 }; - InfoMatrix fullMatrix(G11.rows() + G22.rows() + 1, G11.rows() + G22.rows() + 1); - BlockInfo infoMatrix(fullMatrix, dims, dims+3); - infoMatrix(0,0) = G11; - infoMatrix(0,1) = G12; - infoMatrix.column(0,2,0) = g1; - infoMatrix(1,1) = G22; - infoMatrix.column(1,2,0) = g2; - infoMatrix(2,2)(0,0) = f; - infoMatrix.swap(info_); - assertInvariants(); + GaussianFactor(cref_list_of<2>(j1)(j2)), + info_(cref_list_of<3> (G11.cols()) (G22.cols()) (1) ) +{ + info_(0,0) = G11; + info_(0,1) = G12; + info_(0,2) = g1; + info_(1,1) = G22; + info_(1,2) = g2; + info_(2,2)(0,0) = f; } /* ************************************************************************* */ -HessianFactor::HessianFactor(Index j1, Index j2, Index j3, +HessianFactor::HessianFactor(Key j1, Key j2, Key j3, const Matrix& G11, const Matrix& G12, const Matrix& G13, const Vector& g1, const Matrix& G22, const Matrix& G23, const Vector& g2, const Matrix& G33, const Vector& g3, double f) : - GaussianFactor(j1, j2, j3), info_(matrix_) { + GaussianFactor(cref_list_of<3>(j1)(j2)(j3)), + info_(cref_list_of<4> (G11.cols()) (G22.cols()) (G33.cols()) (1) ) +{ if(G11.rows() != G11.cols() || G11.rows() != G12.rows() || G11.rows() != G13.rows() || G11.rows() != g1.size() || G22.cols() != G12.cols() || G33.cols() != G13.cols() || G22.cols() != g2.size() || G33.cols() != g3.size()) throw invalid_argument("Inconsistent matrix and/or vector dimensions in HessianFactor constructor"); - size_t dims[] = { G11.rows(), G22.rows(), G33.rows(), 1 }; - InfoMatrix fullMatrix(G11.rows() + G22.rows() + G33.rows() + 1, G11.rows() + G22.rows() + G33.rows() + 1); - BlockInfo infoMatrix(fullMatrix, dims, dims+4); - infoMatrix(0,0) = G11; - infoMatrix(0,1) = G12; - infoMatrix(0,2) = G13; - infoMatrix.column(0,3,0) = g1; - infoMatrix(1,1) = G22; - infoMatrix(1,2) = G23; - infoMatrix.column(1,3,0) = g2; - infoMatrix(2,2) = G33; - infoMatrix.column(2,3,0) = g3; - infoMatrix(3,3)(0,0) = f; - infoMatrix.swap(info_); - assertInvariants(); + info_(0,0) = G11; + info_(0,1) = G12; + info_(0,2) = G13; + info_(0,3) = g1; + info_(1,1) = G22; + info_(1,2) = G23; + info_(1,3) = g2; + info_(2,2) = G33; + info_(2,3) = g3; + info_(3,3)(0,0) = f; } /* ************************************************************************* */ -HessianFactor::HessianFactor(const std::vector& js, const std::vector& Gs, - const std::vector& gs, double f) : GaussianFactor(js), info_(matrix_) { +namespace { +DenseIndex _getSizeHF(const Vector& m) { return m.size(); } +} +/* ************************************************************************* */ +HessianFactor::HessianFactor(const std::vector& js, const std::vector& Gs, + const std::vector& gs, double f) : + GaussianFactor(js), info_(gs | br::transformed(&_getSizeHF), true) +{ // Get the number of variables size_t variable_count = js.size(); @@ -191,7 +194,7 @@ HessianFactor::HessianFactor(const std::vector& js, const std::vector& js, const std::vector(&gf)) { - const JacobianFactor& jf(static_cast(gf)); - if(jf.model_->isConstrained()) - throw invalid_argument("Cannot construct HessianFactor from JacobianFactor with constrained noise model"); - else { - const Vector& precisions = jf.model_->precisions(); - info_.copyStructureFrom(jf.Ab_); - BlockInfo::constBlock A = jf.Ab_.full(); - matrix_.noalias() = A.transpose() * precisions.asDiagonal() * A; - } - } else if(dynamic_cast(&gf)) { - const HessianFactor& hf(static_cast(gf)); - info_.assignNoalias(hf.info_); - } else - throw std::invalid_argument("In HessianFactor(const GaussianFactor& gf), gf is neither a JacobianFactor nor a HessianFactor"); - assertInvariants(); -} - -/* ************************************************************************* */ -HessianFactor::HessianFactor(const FactorGraph& factors) : info_(matrix_) +namespace { +void _FromJacobianHelper(const JacobianFactor& jf, SymmetricBlockMatrix& info) { - Scatter scatter(factors); + gttic(HessianFactor_fromJacobian); + const SharedDiagonal& jfModel = jf.get_model(); + if(jfModel) + { + if(jf.get_model()->isConstrained()) + throw invalid_argument("Cannot construct HessianFactor from JacobianFactor with constrained noise model"); + info.full().triangularView() = jf.matrixObject().full().transpose() * + (jfModel->invsigmas().array() * jfModel->invsigmas().array()).matrix().asDiagonal() * + jf.matrixObject().full(); + } else { + info.full().triangularView() = jf.matrixObject().full().transpose() * jf.matrixObject().full(); + } +} +} - // Pull out keys and dimensions - gttic(keys); - vector dimensions(scatter.size() + 1); - BOOST_FOREACH(const Scatter::value_type& var_slot, scatter) { - dimensions[var_slot.second.slot] = var_slot.second.dimension; - } - // This is for the r.h.s. vector - dimensions.back() = 1; - gttoc(keys); +/* ************************************************************************* */ +HessianFactor::HessianFactor(const JacobianFactor& jf) : + GaussianFactor(jf), info_(SymmetricBlockMatrix::LikeActiveViewOf(jf.matrixObject())) +{ + _FromJacobianHelper(jf, info_); +} - const bool debug = ISDEBUG("EliminateCholesky"); - // Form Ab' * Ab +/* ************************************************************************* */ +HessianFactor::HessianFactor(const GaussianFactor& gf) : + GaussianFactor(gf) +{ + // Copy the matrix data depending on what type of factor we're copying from + if(const JacobianFactor* jf = dynamic_cast(&gf)) + { + info_ = SymmetricBlockMatrix::LikeActiveViewOf(jf->matrixObject()); + _FromJacobianHelper(*jf, info_); + } + else if(const HessianFactor* hf = dynamic_cast(&gf)) + { + info_ = hf->info_; + } + else + { + throw std::invalid_argument("In HessianFactor(const GaussianFactor& gf), gf is neither a JacobianFactor nor a HessianFactor"); + } +} + +/* ************************************************************************* */ +HessianFactor::HessianFactor(const GaussianFactorGraph& factors, + boost::optional scatter) +{ + gttic(HessianFactor_MergeConstructor); + boost::optional computedScatter; + if(!scatter) { + computedScatter = Scatter(factors); + scatter = computedScatter; + } + + // Allocate and copy keys gttic(allocate); - info_.resize(dimensions.begin(), dimensions.end(), false); - // Fill in keys - keys_.resize(scatter.size()); - std::transform(scatter.begin(), scatter.end(), keys_.begin(), boost::bind(&Scatter::value_type::first, ::_1)); + // Allocate with dimensions for each variable plus 1 at the end for the information vector + keys_.resize(scatter->size()); + FastVector dims(scatter->size() + 1); + BOOST_FOREACH(const Scatter::value_type& key_slotentry, *scatter) { + keys_[key_slotentry.second.slot] = key_slotentry.first; + dims[key_slotentry.second.slot] = key_slotentry.second.dimension; + } + dims.back() = 1; + info_ = SymmetricBlockMatrix(dims); + info_.full().triangularView().setZero(); gttoc(allocate); - gttic(zero); - matrix_.noalias() = Matrix::Zero(matrix_.rows(),matrix_.cols()); - gttoc(zero); + + // Form A' * A gttic(update); - if (debug) cout << "Combining " << factors.size() << " factors" << endl; BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) { if(factor) { - if(shared_ptr hessian = boost::dynamic_pointer_cast(factor)) - updateATA(*hessian, scatter); - else if(JacobianFactor::shared_ptr jacobianFactor = boost::dynamic_pointer_cast(factor)) - updateATA(*jacobianFactor, scatter); + if(const HessianFactor* hessian = dynamic_cast(factor.get())) + updateATA(*hessian, *scatter); + else if(const JacobianFactor* jacobian = dynamic_cast(factor.get())) + updateATA(*jacobian, *scatter); else throw invalid_argument("GaussianFactor is neither Hessian nor Jacobian"); } } gttoc(update); - - if (debug) gtsam::print(matrix_, "Ab' * Ab: "); - - assertInvariants(); } /* ************************************************************************* */ -HessianFactor& HessianFactor::operator=(const HessianFactor& rhs) { - this->Base::operator=(rhs); // Copy keys - info_.assignNoalias(rhs.info_); // Copy matrix and block structure - return *this; -} - -/* ************************************************************************* */ -void HessianFactor::print(const std::string& s, const IndexFormatter& formatter) const { +void HessianFactor::print(const std::string& s, const KeyFormatter& formatter) const { cout << s << "\n"; cout << " keys: "; for(const_iterator key=this->begin(); key!=this->end(); ++key) cout << formatter(*key) << "(" << this->getDim(key) << ") "; cout << "\n"; - gtsam::print(Matrix(info_.range(0,info_.nBlocks(), 0,info_.nBlocks()).selfadjointView()), "Ab^T * Ab: "); + gtsam::print(Matrix(info_.full().selfadjointView()), "Augmented information matrix: "); } /* ************************************************************************* */ @@ -335,22 +324,79 @@ bool HessianFactor::equals(const GaussianFactor& lf, double tol) const { if(!dynamic_cast(&lf)) return false; else { - Matrix thisMatrix = this->info_.full().selfadjointView(); + if(!Factor::equals(lf, tol)) + return false; + Matrix thisMatrix = this->info_.full().selfadjointView(); thisMatrix(thisMatrix.rows()-1, thisMatrix.cols()-1) = 0.0; - Matrix rhsMatrix = static_cast(lf).info_.full().selfadjointView(); + Matrix rhsMatrix = static_cast(lf).info_.full().selfadjointView(); rhsMatrix(rhsMatrix.rows()-1, rhsMatrix.cols()-1) = 0.0; return equal_with_abs_tol(thisMatrix, rhsMatrix, tol); } } /* ************************************************************************* */ -Matrix HessianFactor::augmentedInformation() const { - return info_.full().selfadjointView(); +Matrix HessianFactor::augmentedInformation() const +{ + return info_.full().selfadjointView(); } /* ************************************************************************* */ -Matrix HessianFactor::information() const { - return info_.range(0, this->size(), 0, this->size()).selfadjointView(); +Matrix HessianFactor::information() const +{ + return info_.range(0, this->size(), 0, this->size()).selfadjointView(); +} + +/* ************************************************************************* */ +VectorValues HessianFactor::hessianDiagonal() const { + VectorValues d; + // Loop over all variables + for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) { + // Get the diagonal block, and insert its diagonal + Matrix B = info_(j, j).selfadjointView(); + d.insert(keys_[j],B.diagonal()); + } + return d; +} + +/* ************************************************************************* */ +// TODO: currently assumes all variables of the same size 9 and keys arranged from 0 to n +void HessianFactor::hessianDiagonal(double* d) const { + + // Use eigen magic to access raw memory + typedef Eigen::Matrix DVector; + typedef Eigen::Map DMap; + + // Loop over all variables in the factor + for (DenseIndex pos = 0; pos < (DenseIndex)size(); ++pos) { + Key j = keys_[pos]; + // Get the diagonal block, and insert its diagonal + const Matrix& B = info_(pos, pos).selfadjointView(); + DMap(d + 9 * j) += B.diagonal(); + } +} + +/* ************************************************************************* */ +map HessianFactor::hessianBlockDiagonal() const { + map blocks; + // Loop over all variables + for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) { + // Get the diagonal block, and insert it + Matrix B = info_(j, j).selfadjointView(); + blocks.insert(make_pair(keys_[j],B)); + } + return blocks; +} + +/* ************************************************************************* */ +Matrix HessianFactor::augmentedJacobian() const +{ + return JacobianFactor(*this).augmentedJacobian(); +} + +/* ************************************************************************* */ +std::pair HessianFactor::jacobian() const +{ + return JacobianFactor(*this).jacobian(); } /* ************************************************************************* */ @@ -362,58 +408,35 @@ double HessianFactor::error(const VectorValues& c) const { // NOTE may not be as efficient const Vector x = c.vector(this->keys()); xtg = x.dot(linearTerm()); - xGx = x.transpose() * info_.range(0, this->size(), 0, this->size()).selfadjointView() * x; + xGx = x.transpose() * info_.range(0, this->size(), 0, this->size()).selfadjointView() * x; return 0.5 * (f - 2.0 * xtg + xGx); } /* ************************************************************************* */ -void HessianFactor::updateATA(const HessianFactor& update, const Scatter& scatter) { - - // This function updates 'combined' with the information in 'update'. - // 'scatter' maps variables in the update factor to slots in the combined - // factor. - - const bool debug = ISDEBUG("updateATA"); +void HessianFactor::updateATA(const HessianFactor& update, const Scatter& scatter) +{ + gttic(updateATA); + // This function updates 'combined' with the information in 'update'. 'scatter' maps variables in + // the update factor to slots in the combined factor. // First build an array of slots gttic(slots); - size_t* slots = (size_t*)alloca(sizeof(size_t)*update.size()); // FIXME: alloca is bad, just ask Google. - size_t slot = 0; - BOOST_FOREACH(Index j, update) { - slots[slot] = scatter.find(j)->second.slot; + FastVector slots(update.size()); + DenseIndex slot = 0; + BOOST_FOREACH(Key j, update) { + slots[slot] = scatter.at(j).slot; ++ slot; } gttoc(slots); - if(debug) { - this->print("Updating this: "); - update.print("with (Hessian): "); - } - // Apply updates to the upper triangle gttic(update); - for(size_t j2=0; j2info_.nBlocks()-1 : slots[j2]; - for(size_t j1=0; j1<=j2; ++j1) { - size_t slot1 = (j1 == update.size()) ? this->info_.nBlocks()-1 : slots[j1]; - if(slot2 > slot1) { - if(debug) - cout << "Updating (" << slot1 << "," << slot2 << ") from (" << j1 << "," << j2 << ")" << endl; - matrix_.block(info_.offset(slot1), info_.offset(slot2), info_(slot1,slot2).rows(), info_(slot1,slot2).cols()).noalias() += - update.matrix_.block(update.info_.offset(j1), update.info_.offset(j2), update.info_(j1,j2).rows(), update.info_(j1,j2).cols()); - } else if(slot1 > slot2) { - if(debug) - cout << "Updating (" << slot2 << "," << slot1 << ") from (" << j1 << "," << j2 << ")" << endl; - matrix_.block(info_.offset(slot2), info_.offset(slot1), info_(slot2,slot1).rows(), info_(slot2,slot1).cols()).noalias() += - update.matrix_.block(update.info_.offset(j1), update.info_.offset(j2), update.info_(j1,j2).rows(), update.info_(j1,j2).cols()).transpose(); - } else { - if(debug) - cout << "Updating (" << slot1 << "," << slot2 << ") from (" << j1 << "," << j2 << ")" << endl; - matrix_.block(info_.offset(slot1), info_.offset(slot2), info_(slot1,slot2).rows(), info_(slot1,slot2).cols()).triangularView() += - update.matrix_.block(update.info_.offset(j1), update.info_.offset(j2), update.info_(j1,j2).rows(), update.info_(j1,j2).cols()); - } - if(debug) cout << "Updating block " << slot1 << "," << slot2 << " from block " << j1 << "," << j2 << "\n"; - if(debug) this->print(); + size_t nrInfoBlocks = this->info_.nBlocks(); + for(DenseIndex j2=0; j2second.slot; - ++ slot; - } - gttoc(slots); + if(update.rows() > 0) + { + // First build an array of slots + gttic(slots); + FastVector slots(update.size()); + DenseIndex slot = 0; + BOOST_FOREACH(Key j, update) { + slots[slot] = scatter.at(j).slot; + ++ slot; + } + gttoc(slots); - gttic(form_ATA); - if(update.model_->isConstrained()) - throw invalid_argument("Cannot update HessianFactor from JacobianFactor with constrained noise model"); + gttic(whiten); + // Whiten the factor if it has a noise model + boost::optional _whitenedFactor; + const JacobianFactor* whitenedFactor; + if(update.get_model()) + { + if(update.get_model()->isConstrained()) + throw invalid_argument("Cannot update HessianFactor from JacobianFactor with constrained noise model"); - if(debug) { - this->print("Updating this: "); - update.print("with (Jacobian): "); - } + _whitenedFactor = update.whiten(); + whitenedFactor = &(*_whitenedFactor); + } + else + { + whitenedFactor = &update; + } + gttoc(whiten); - typedef Eigen::Block BlockUpdateMatrix; - BlockUpdateMatrix updateA(update.matrix_.block( - update.Ab_.rowStart(),update.Ab_.offset(0), update.Ab_.full().rows(), update.Ab_.full().cols())); - if (debug) cout << "updateA: \n" << updateA << endl; + const VerticalBlockMatrix& updateBlocks = whitenedFactor->matrixObject(); - Matrix updateInform; - if(boost::dynamic_pointer_cast(update.model_)) { - updateInform.noalias() = updateA.transpose() * updateA; - } else { - noiseModel::Diagonal::shared_ptr diagonal(boost::dynamic_pointer_cast(update.model_)); - if(diagonal) { - const Vector& precisions = diagonal->precisions(); - updateInform.noalias() = updateA.transpose() * precisions.asDiagonal() * updateA; - } else - throw invalid_argument("In HessianFactor::updateATA, JacobianFactor noise model is neither Unit nor Diagonal"); - } - if (debug) cout << "updateInform: \n" << updateInform << endl; - gttoc(form_ATA); - - // Apply updates to the upper triangle - gttic(update); - for(size_t j2=0; j2info_.nBlocks()-1 : slots[j2]; - for(size_t j1=0; j1<=j2; ++j1) { - size_t slot1 = (j1 == update.size()) ? this->info_.nBlocks()-1 : slots[j1]; - size_t off0 = update.Ab_.offset(0); - if(slot2 > slot1) { - if(debug) - cout << "Updating (" << slot1 << "," << slot2 << ") from (" << j1 << "," << j2 << ")" << endl; - matrix_.block(info_.offset(slot1), info_.offset(slot2), info_(slot1,slot2).rows(), info_(slot1,slot2).cols()).noalias() += - updateInform.block(update.Ab_.offset(j1)-off0, update.Ab_.offset(j2)-off0, update.Ab_(j1).cols(), update.Ab_(j2).cols()); - } else if(slot1 > slot2) { - if(debug) - cout << "Updating (" << slot2 << "," << slot1 << ") from (" << j1 << "," << j2 << ")" << endl; - matrix_.block(info_.offset(slot2), info_.offset(slot1), info_(slot2,slot1).rows(), info_(slot2,slot1).cols()).noalias() += - updateInform.block(update.Ab_.offset(j1)-off0, update.Ab_.offset(j2)-off0, update.Ab_(j1).cols(), update.Ab_(j2).cols()).transpose(); - } else { - if(debug) - cout << "Updating (" << slot1 << "," << slot2 << ") from (" << j1 << "," << j2 << ")" << endl; - matrix_.block(info_.offset(slot1), info_.offset(slot2), info_(slot1,slot2).rows(), info_(slot1,slot2).cols()).triangularView() += - updateInform.block(update.Ab_.offset(j1)-off0, update.Ab_.offset(j2)-off0, update.Ab_(j1).cols(), update.Ab_(j2).cols()); + // Apply updates to the upper triangle + gttic(update); + DenseIndex nrInfoBlocks = this->info_.nBlocks(), nrUpdateBlocks = updateBlocks.nBlocks(); + for(DenseIndex j2 = 0; j2 < nrUpdateBlocks; ++j2) { // Horizontal block of Hessian + DenseIndex slot2 = (j2 == (DenseIndex)update.size()) ? nrInfoBlocks-1 : slots[j2]; + assert(slot2 >= 0 && slot2 <= nrInfoBlocks); + for(DenseIndex j1 = 0; j1 <= j2; ++j1) { // Vertical block of Hessian + DenseIndex slot1 = (j1 == (DenseIndex)update.size()) ? nrInfoBlocks-1 : slots[j1]; + assert(slot1 >= 0 && slot1 < nrInfoBlocks); + if(slot1 != slot2) + info_(slot1, slot2).knownOffDiagonal() += updateBlocks(j1).transpose() * updateBlocks(j2); + else + info_(slot1, slot2).selfadjointView().rankUpdate(updateBlocks(j1).transpose()); } - if(debug) cout << "Updating block " << slot1 << "," << slot2 << " from block " << j1 << "," << j2 << "\n"; - if(debug) this->print(); } + gttoc(update); } - gttoc(update); } /* ************************************************************************* */ -void HessianFactor::partialCholesky(size_t nrFrontals) { - if(!choleskyPartial(matrix_, info_.offset(nrFrontals))) - throw IndeterminantLinearSystemException(this->keys().front()); +GaussianFactor::shared_ptr HessianFactor::negate() const +{ + shared_ptr result = boost::make_shared(*this); + result->info_.full().triangularView() = -result->info_.full().triangularView().nestedExpression(); // Negate the information matrix of the result + return result; } /* ************************************************************************* */ -GaussianConditional::shared_ptr HessianFactor::splitEliminatedFactor(size_t nrFrontals) { +void HessianFactor::multiplyHessianAdd(double alpha, const VectorValues& x, + VectorValues& yvalues) const { - static const bool debug = false; + // Create a vector of temporary y values, corresponding to rows i + vector y; + y.reserve(size()); + for (const_iterator it = begin(); it != end(); it++) + y.push_back(zero(getDim(it))); - // Extract conditionals - gttic(extract_conditionals); - GaussianConditional::shared_ptr conditional(new GaussianConditional()); - typedef VerticalBlockView BlockAb; - BlockAb Ab(matrix_, info_); - - size_t varDim = info_.offset(nrFrontals); - Ab.rowEnd() = Ab.rowStart() + varDim; - - // Create one big conditionals with many frontal variables. - gttic(construct_cond); - Vector sigmas = Vector::Ones(varDim); - conditional = boost::make_shared(keys_.begin(), keys_.end(), nrFrontals, Ab, sigmas); - gttoc(construct_cond); - if(debug) conditional->print("Extracted conditional: "); - - gttoc(extract_conditionals); - - // Take lower-right block of Ab_ to get the new factor - gttic(remaining_factor); - info_.blockStart() = nrFrontals; - // Assign the keys - vector remainingKeys(keys_.size() - nrFrontals); - remainingKeys.assign(keys_.begin() + nrFrontals, keys_.end()); - keys_.swap(remainingKeys); - gttoc(remaining_factor); - - return conditional; -} - -/* ************************************************************************* */ -GaussianFactor::shared_ptr HessianFactor::negate() const { - // Copy Hessian Blocks from Hessian factor and invert - std::vector js; - std::vector Gs; - std::vector gs; - double f; - js.insert(js.end(), begin(), end()); - for(size_t i = 0; i < js.size(); ++i){ - for(size_t j = i; j < js.size(); ++j){ - Gs.push_back( -info(begin()+i, begin()+j) ); - } - gs.push_back( -linearTerm(begin()+i) ); + // Accessing the VectorValues one by one is expensive + // So we will loop over columns to access x only once per column + // And fill the above temporary y values, to be added into yvalues after + for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) { + // xj is the input vector + Vector xj = x.at(keys_[j]); + DenseIndex i = 0; + for (; i < j; ++i) + y[i] += info_(i, j).knownOffDiagonal() * xj; + // blocks on the diagonal are only half + y[i] += info_(j, j).selfadjointView() * xj; + // for below diagonal, we take transpose block from upper triangular part + for (i = j + 1; i < (DenseIndex)size(); ++i) + y[i] += info_(i, j).knownOffDiagonal() * xj; } - f = -constantTerm(); - // Create the Anti-Hessian Factor from the negated blocks - return HessianFactor::shared_ptr(new HessianFactor(js, Gs, gs, f)); + // copy to yvalues + for(DenseIndex i = 0; i < (DenseIndex)size(); ++i) { + bool didNotExist; + VectorValues::iterator it; + boost::tie(it, didNotExist) = yvalues.tryInsert(keys_[i], Vector()); + if (didNotExist) + it->second = alpha * y[i]; // init + else + it->second += alpha * y[i]; // add + } +} + +/* ************************************************************************* */ +void HessianFactor::multiplyHessianAdd(double alpha, const double* x, + double* yvalues, vector offsets) const { + + // Use eigen magic to access raw memory + typedef Eigen::Matrix DVector; + typedef Eigen::Map DMap; + typedef Eigen::Map ConstDMap; + + // Create a vector of temporary y values, corresponding to rows i + vector y; + y.reserve(size()); + for (const_iterator it = begin(); it != end(); it++) + y.push_back(zero(getDim(it))); + + // Accessing the VectorValues one by one is expensive + // So we will loop over columns to access x only once per column + // And fill the above temporary y values, to be added into yvalues after + for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { + DenseIndex i = 0; + for (; i < j; ++i) + y[i] += info_(i, j).knownOffDiagonal() + * ConstDMap(x + offsets[keys_[j]], + offsets[keys_[j] + 1] - offsets[keys_[j]]); + // blocks on the diagonal are only half + y[i] += info_(j, j).selfadjointView() + * ConstDMap(x + offsets[keys_[j]], + offsets[keys_[j] + 1] - offsets[keys_[j]]); + // for below diagonal, we take transpose block from upper triangular part + for (i = j + 1; i < (DenseIndex) size(); ++i) + y[i] += info_(i, j).knownOffDiagonal() + * ConstDMap(x + offsets[keys_[j]], + offsets[keys_[j] + 1] - offsets[keys_[j]]); + } + + // copy to yvalues + for (DenseIndex i = 0; i < (DenseIndex) size(); ++i) + DMap(yvalues + offsets[keys_[i]], offsets[keys_[i] + 1] - offsets[keys_[i]]) += + alpha * y[i]; +} + + +/* ************************************************************************* */ +VectorValues HessianFactor::gradientAtZero() const { + VectorValues g; + size_t n = size(); + for (size_t j = 0; j < n; ++j) + g.insert(keys_[j], -info_(j,n).knownOffDiagonal()); + return g; +} + +/* ************************************************************************* */ +std::pair, boost::shared_ptr > +EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys) +{ + gttic(EliminateCholesky); + + // Build joint factor + HessianFactor::shared_ptr jointFactor; + try { + jointFactor = boost::make_shared(factors, Scatter(factors, keys)); + } catch(std::invalid_argument&) { + throw InvalidDenseElimination( + "EliminateCholesky was called with a request to eliminate variables that are not\n" + "involved in the provided factors."); + } + + // Do dense elimination + GaussianConditional::shared_ptr conditional; + try { + VerticalBlockMatrix Ab = jointFactor->info_.choleskyPartial(keys.size()); + conditional = boost::make_shared(jointFactor->keys(), keys.size(), Ab); + // Erase the eliminated keys in the remaining factor + jointFactor->keys_.erase(jointFactor->begin(), jointFactor->begin() + keys.size()); + } catch(CholeskyFailed&) { + throw IndeterminantLinearSystemException(keys.front()); + } + + // Return result + return make_pair(conditional, jointFactor); +} + +/* ************************************************************************* */ +std::pair, boost::shared_ptr > +EliminatePreferCholesky(const GaussianFactorGraph& factors, const Ordering& keys) +{ + gttic(EliminatePreferCholesky); + + // If any JacobianFactors have constrained noise models, we have to convert + // all factors to JacobianFactors. Otherwise, we can convert all factors + // to HessianFactors. This is because QR can handle constrained noise + // models but Cholesky cannot. + if (hasConstraints(factors)) + return EliminateQR(factors, keys); + else + return EliminateCholesky(factors, keys); } } // gtsam diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index 9f44a0964..faf2ccd33 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -1,377 +1,440 @@ -/* ---------------------------------------------------------------------------- - - * 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 HessianFactor.h - * @brief Contains the HessianFactor class, a general quadratic factor - * @author Richard Roberts - * @date Dec 8, 2010 - */ - -#pragma once - -#include -#include -#include - -// Forward declarations for friend unit tests -class HessianFactorConversionConstructorTest; -class HessianFactorConstructor1Test; -class HessianFactorConstructor1bTest; -class HessianFactorcombineTest; - - -namespace gtsam { - - // Forward declarations - class JacobianFactor; - class GaussianConditional; - template class BayesNet; - - // Definition of Scatter, which is an intermediate data structure used when - // building a HessianFactor incrementally, to get the keys in the right - // order. - // The "scatter" is a map from global variable indices to slot indices in the - // union of involved variables. We also include the dimensionality of the - // variable. - struct GTSAM_EXPORT SlotEntry { - size_t slot; - size_t dimension; - SlotEntry(size_t _slot, size_t _dimension) - : slot(_slot), dimension(_dimension) {} - std::string toString() const; - }; - class Scatter : public FastMap { - public: - Scatter() {} - Scatter(const FactorGraph& gfg); - }; - - /** - * @brief A Gaussian factor using the canonical parameters (information form) - * - * HessianFactor implements a general quadratic factor of the form - * \f[ E(x) = 0.5 x^T G x - x^T g + 0.5 f \f] - * that stores the matrix \f$ G \f$, the vector \f$ g \f$, and the constant term \f$ f \f$. - * - * When \f$ G \f$ is positive semidefinite, this factor represents a Gaussian, - * in which case \f$ G \f$ is the information matrix \f$ \Lambda \f$, - * \f$ g \f$ is the information vector \f$ \eta \f$, and \f$ f \f$ is the residual - * sum-square-error at the mean, when \f$ x = \mu \f$. - * - * Indeed, the negative log-likelihood of a Gaussian is (up to a constant) - * @f$ E(x) = 0.5(x-\mu)^T P^{-1} (x-\mu) @f$ - * with @f$ \mu @f$ the mean and @f$ P @f$ the covariance matrix. Expanding the product we get - * @f[ - * E(x) = 0.5 x^T P^{-1} x - x^T P^{-1} \mu + 0.5 \mu^T P^{-1} \mu - * @f] - * We define the Information matrix (or Hessian) @f$ \Lambda = P^{-1} @f$ - * and the information vector @f$ \eta = P^{-1} \mu = \Lambda \mu @f$ - * to arrive at the canonical form of the Gaussian: - * @f[ - * E(x) = 0.5 x^T \Lambda x - x^T \eta + 0.5 \mu^T \Lambda \mu - * @f] - * - * This factor is one of the factors that can be in a GaussianFactorGraph. - * It may be returned from NonlinearFactor::linearize(), but is also - * used internally to store the Hessian during Cholesky elimination. - * - * This can represent a quadratic factor with characteristics that cannot be - * represented using a JacobianFactor (which has the form - * \f$ E(x) = \Vert Ax - b \Vert^2 \f$ and stores the Jacobian \f$ A \f$ - * and error vector \f$ b \f$, i.e. is a sum-of-squares factor). For example, - * a HessianFactor need not be positive semidefinite, it can be indefinite or - * even negative semidefinite. - * - * If a HessianFactor is indefinite or negative semi-definite, then in order - * for solving the linear system to be possible, - * the Hessian of the full system must be positive definite (i.e. when all - * small Hessians are combined, the result must be positive definite). If - * this is not the case, an error will occur during elimination. - * - * This class stores G, g, and f as an augmented matrix HessianFactor::matrix_. - * The upper-left n x n blocks of HessianFactor::matrix_ store the upper-right - * triangle of G, the upper-right-most column of length n of HessianFactor::matrix_ - * stores g, and the lower-right entry of HessianFactor::matrix_ stores f, i.e. - * \code - HessianFactor::matrix_ = [ G11 G12 G13 ... g1 - 0 G22 G23 ... g2 - 0 0 G33 ... g3 - : : : : - 0 0 0 ... f ] - \endcode - Blocks can be accessed as follows: - \code - G11 = info(begin(), begin()); - G12 = info(begin(), begin()+1); - G23 = info(begin()+1, begin()+2); - g2 = linearTerm(begin()+1); - f = constantTerm(); - ....... - \endcode - */ - class GTSAM_EXPORT HessianFactor : public GaussianFactor { - protected: - typedef Matrix InfoMatrix; ///< The full augmented Hessian - typedef SymmetricBlockView BlockInfo; ///< A blockwise view of the Hessian - typedef GaussianFactor Base; ///< Typedef to base class - - InfoMatrix matrix_; ///< The full augmented information matrix, s.t. the quadratic error is 0.5*[x -1]'*H*[x -1] - BlockInfo info_; ///< The block view of the full information matrix. - - public: - - typedef boost::shared_ptr shared_ptr; ///< A shared_ptr to this - typedef BlockInfo::Block Block; ///< A block from the Hessian matrix - typedef BlockInfo::constBlock constBlock; ///< A block from the Hessian matrix (const version) - typedef BlockInfo::Column Column; ///< A column containing the linear term h - typedef BlockInfo::constColumn constColumn; ///< A column containing the linear term h (const version) - - /** Copy constructor */ - HessianFactor(const HessianFactor& gf); - - /** default constructor for I/O */ - HessianFactor(); - - /** Construct a unary factor. G is the quadratic term (Hessian matrix), g - * the linear term (a vector), and f the constant term. The quadratic - * error is: - * 0.5*(f - 2*x'*g + x'*G*x) - */ - HessianFactor(Index j, const Matrix& G, const Vector& g, double f); - - /** Construct a unary factor, given a mean and covariance matrix. - * error is 0.5*(x-mu)'*inv(Sigma)*(x-mu) - */ - HessianFactor(Index j, const Vector& mu, const Matrix& Sigma); - - /** Construct a binary factor. Gxx are the upper-triangle blocks of the - * quadratic term (the Hessian matrix), gx the pieces of the linear vector - * term, and f the constant term. - * JacobianFactor error is \f[ 0.5* (Ax-b)' M (Ax-b) = 0.5*x'A'MAx - x'A'Mb + 0.5*b'Mb \f] - * HessianFactor error is \f[ 0.5*(x'Gx - 2x'g + f) = 0.5*x'Gx - x'*g + 0.5*f \f] - * So, with \f$ A = [A1 A2] \f$ and \f$ G=A*'M*A = [A1';A2']*M*[A1 A2] \f$ we have - \code - n1*n1 G11 = A1'*M*A1 - n1*n2 G12 = A1'*M*A2 - n2*n2 G22 = A2'*M*A2 - n1*1 g1 = A1'*M*b - n2*1 g2 = A2'*M*b - 1*1 f = b'*M*b - \endcode - */ - HessianFactor(Index j1, Index j2, - const Matrix& G11, const Matrix& G12, const Vector& g1, - const Matrix& G22, const Vector& g2, double f); - - /** Construct a ternary factor. Gxx are the upper-triangle blocks of the - * quadratic term (the Hessian matrix), gx the pieces of the linear vector - * term, and f the constant term. - */ - HessianFactor(Index j1, Index j2, Index j3, - const Matrix& G11, const Matrix& G12, const Matrix& G13, const Vector& g1, - const Matrix& G22, const Matrix& G23, const Vector& g2, - const Matrix& G33, const Vector& g3, double f); - - /** Construct an n-way factor. Gs contains the upper-triangle blocks of the - * quadratic term (the Hessian matrix) provided in row-order, gs the pieces - * of the linear vector term, and f the constant term. - */ - HessianFactor(const std::vector& js, const std::vector& Gs, - const std::vector& gs, double f); - - /** Construct from Conditional Gaussian */ - explicit HessianFactor(const GaussianConditional& cg); - - /** Convert from a JacobianFactor (computes A^T * A) or HessianFactor */ - explicit HessianFactor(const GaussianFactor& factor); - - /** Combine a set of factors into a single dense HessianFactor */ - HessianFactor(const FactorGraph& factors); - - /** Destructor */ - virtual ~HessianFactor() {} - - /** Aassignment operator */ - HessianFactor& operator=(const HessianFactor& rhs); - - /** Clone this JacobianFactor */ - virtual GaussianFactor::shared_ptr clone() const { - return boost::static_pointer_cast( - shared_ptr(new HessianFactor(*this))); - } - - /** Print the factor for debugging and testing (implementing Testable) */ - virtual void print(const std::string& s = "", - const IndexFormatter& formatter = DefaultIndexFormatter) const; - - /** Compare to another factor for testing (implementing Testable) */ - virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const; - - /** Evaluate the factor error f(x), see above. */ - virtual double error(const VectorValues& c) const; /** 0.5*[x -1]'*H*[x -1] (also see constructor documentation) */ - - /** Return the dimension of the variable pointed to by the given key iterator - * todo: Remove this in favor of keeping track of dimensions with variables? - * @param variable An iterator pointing to the slot in this factor. You can - * use, for example, begin() + 2 to get the 3rd variable in this factor. - */ - virtual size_t getDim(const_iterator variable) const { return info_(variable-this->begin(), 0).rows(); } - - /** Return the number of columns and rows of the Hessian matrix */ - size_t rows() const { return info_.rows(); } - - /** - * Construct the corresponding anti-factor to negate information - * stored stored in this factor. - * @return a HessianFactor with negated Hessian matrices - */ - virtual GaussianFactor::shared_ptr negate() const; - - /** Return a view of the block at (j1,j2) of the upper-triangular part of the - * information matrix \f$ H \f$, no data is copied. See HessianFactor class documentation - * above to explain that only the upper-triangular part of the information matrix is stored - * and returned by this function. - * @param j1 Which block row to get, as an iterator pointing to the slot in this factor. You can - * use, for example, begin() + 2 to get the 3rd variable in this factor. - * @param j2 Which block column to get, as an iterator pointing to the slot in this factor. You can - * use, for example, begin() + 2 to get the 3rd variable in this factor. - * @return A view of the requested block, not a copy. - */ - constBlock info(const_iterator j1, const_iterator j2) const { return info_(j1-begin(), j2-begin()); } - - /** Return a view of the block at (j1,j2) of the upper-triangular part of the - * information matrix \f$ H \f$, no data is copied. See HessianFactor class documentation - * above to explain that only the upper-triangular part of the information matrix is stored - * and returned by this function. - * @param j1 Which block row to get, as an iterator pointing to the slot in this factor. You can - * use, for example, begin() + 2 to get the 3rd variable in this factor. - * @param j2 Which block column to get, as an iterator pointing to the slot in this factor. You can - * use, for example, begin() + 2 to get the 3rd variable in this factor. - * @return A view of the requested block, not a copy. - */ - Block info(iterator j1, iterator j2) { return info_(j1-begin(), j2-begin()); } - - /** Return the upper-triangular part of the full *augmented* information matrix, - * as described above. See HessianFactor class documentation above to explain that only the - * upper-triangular part of the information matrix is stored and returned by this function. - */ - constBlock info() const { return info_.full(); } - - /** Return the upper-triangular part of the full *augmented* information matrix, - * as described above. See HessianFactor class documentation above to explain that only the - * upper-triangular part of the information matrix is stored and returned by this function. - */ - Block info() { return info_.full(); } - - /** Return the constant term \f$ f \f$ as described above - * @return The constant term \f$ f \f$ - */ - double constantTerm() const { return info_(this->size(), this->size())(0,0); } - - /** Return the constant term \f$ f \f$ as described above - * @return The constant term \f$ f \f$ - */ - double& constantTerm() { return info_(this->size(), this->size())(0,0); } - - /** Return the part of linear term \f$ g \f$ as described above corresponding to the requested variable. - * @param j Which block row to get, as an iterator pointing to the slot in this factor. You can - * use, for example, begin() + 2 to get the 3rd variable in this factor. - * @return The linear term \f$ g \f$ */ - constColumn linearTerm(const_iterator j) const { return info_.column(j-begin(), size(), 0); } - - /** Return the part of linear term \f$ g \f$ as described above corresponding to the requested variable. - * @param j Which block row to get, as an iterator pointing to the slot in this factor. You can - * use, for example, begin() + 2 to get the 3rd variable in this factor. - * @return The linear term \f$ g \f$ */ - Column linearTerm(iterator j) { return info_.column(j-begin(), size(), 0); } - - /** Return the complete linear term \f$ g \f$ as described above. - * @return The linear term \f$ g \f$ */ - constColumn linearTerm() const { return info_.rangeColumn(0, this->size(), this->size(), 0); } - - /** Return the complete linear term \f$ g \f$ as described above. - * @return The linear term \f$ g \f$ */ - Column linearTerm() { return info_.rangeColumn(0, this->size(), this->size(), 0); } - - /** Return the augmented information matrix represented by this GaussianFactor. - * The augmented information matrix contains the information matrix with an - * additional column holding the information vector, and an additional row - * holding the transpose of the information vector. The lower-right entry - * contains the constant error term (when \f$ \delta x = 0 \f$). The - * augmented information matrix is described in more detail in HessianFactor, - * which in fact stores an augmented information matrix. - * - * For HessianFactor, this is the same as info() except that this function - * returns a complete symmetric matrix whereas info() returns a matrix where - * only the upper triangle is valid, but should be interpreted as symmetric. - * This is because info() returns only a reference to the internal - * representation of the augmented information matrix, which stores only the - * upper triangle. - */ - virtual Matrix augmentedInformation() const; - - /** Return the non-augmented information matrix represented by this - * GaussianFactor. - */ - virtual Matrix information() const; - - // Friend unit test classes - friend class ::HessianFactorConversionConstructorTest; - friend class ::HessianFactorConstructor1Test; - friend class ::HessianFactorConstructor1bTest; - friend class ::HessianFactorcombineTest; - - // Friend JacobianFactor for conversion - friend class JacobianFactor; - - // used in eliminateCholesky: - - /** - * Do Cholesky. Note that after this, the lower triangle still contains - * some untouched non-zeros that should be zero. We zero them while - * extracting submatrices in splitEliminatedFactor. Frank says :-( - */ - void partialCholesky(size_t nrFrontals); - - /** split partially eliminated factor */ - boost::shared_ptr splitEliminatedFactor(size_t nrFrontals); - - /** Update the factor by adding the information from the JacobianFactor - * (used internally during elimination). - * @param update The JacobianFactor containing the new information to add - * @param scatter A mapping from variable index to slot index in this HessianFactor - */ - void updateATA(const JacobianFactor& update, const Scatter& scatter); - - /** Update the factor by adding the information from the HessianFactor - * (used internally during elimination). - * @param update The HessianFactor containing the new information to add - * @param scatter A mapping from variable index to slot index in this HessianFactor - */ - void updateATA(const HessianFactor& update, const Scatter& scatter); - - /** assert invariants */ - void assertInvariants() const; - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(GaussianFactor); - ar & BOOST_SERIALIZATION_NVP(info_); - ar & BOOST_SERIALIZATION_NVP(matrix_); - } - }; - -} - +/* ---------------------------------------------------------------------------- + + * 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 HessianFactor.h + * @brief Contains the HessianFactor class, a general quadratic factor + * @author Richard Roberts + * @date Dec 8, 2010 + */ + +#pragma once + +#include +#include +#include +#include + +#include + +namespace gtsam { + + // Forward declarations + class Ordering; + class JacobianFactor; + class HessianFactor; + class GaussianConditional; + class GaussianBayesNet; + class GaussianFactorGraph; + + GTSAM_EXPORT std::pair, boost::shared_ptr > + EliminatePreferCholesky(const GaussianFactorGraph& factors, const Ordering& keys); + + GTSAM_EXPORT std::pair, boost::shared_ptr > + EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys); + + /** + * One SlotEntry stores the slot index for a variable, as well its dimension. + */ + struct GTSAM_EXPORT SlotEntry { + size_t slot, dimension; + SlotEntry(size_t _slot, size_t _dimension) + : slot(_slot), dimension(_dimension) {} + std::string toString() const; + }; + + /** + * Scatter is an intermediate data structure used when building a HessianFactor + * incrementally, to get the keys in the right order. The "scatter" is a map from + * global variable indices to slot indices in the union of involved variables. + * We also include the dimensionality of the variable. + */ + class Scatter: public FastMap { + public: + Scatter() { + } + Scatter(const GaussianFactorGraph& gfg, + boost::optional ordering = boost::none); + }; + + /** + * @brief A Gaussian factor using the canonical parameters (information form) + * + * HessianFactor implements a general quadratic factor of the form + * \f[ E(x) = 0.5 x^T G x - x^T g + 0.5 f \f] + * that stores the matrix \f$ G \f$, the vector \f$ g \f$, and the constant term \f$ f \f$. + * + * When \f$ G \f$ is positive semidefinite, this factor represents a Gaussian, + * in which case \f$ G \f$ is the information matrix \f$ \Lambda \f$, + * \f$ g \f$ is the information vector \f$ \eta \f$, and \f$ f \f$ is the residual + * sum-square-error at the mean, when \f$ x = \mu \f$. + * + * Indeed, the negative log-likelihood of a Gaussian is (up to a constant) + * @f$ E(x) = 0.5(x-\mu)^T P^{-1} (x-\mu) @f$ + * with @f$ \mu @f$ the mean and @f$ P @f$ the covariance matrix. Expanding the product we get + * @f[ + * E(x) = 0.5 x^T P^{-1} x - x^T P^{-1} \mu + 0.5 \mu^T P^{-1} \mu + * @f] + * We define the Information matrix (or Hessian) @f$ \Lambda = P^{-1} @f$ + * and the information vector @f$ \eta = P^{-1} \mu = \Lambda \mu @f$ + * to arrive at the canonical form of the Gaussian: + * @f[ + * E(x) = 0.5 x^T \Lambda x - x^T \eta + 0.5 \mu^T \Lambda \mu + * @f] + * + * This factor is one of the factors that can be in a GaussianFactorGraph. + * It may be returned from NonlinearFactor::linearize(), but is also + * used internally to store the Hessian during Cholesky elimination. + * + * This can represent a quadratic factor with characteristics that cannot be + * represented using a JacobianFactor (which has the form + * \f$ E(x) = \Vert Ax - b \Vert^2 \f$ and stores the Jacobian \f$ A \f$ + * and error vector \f$ b \f$, i.e. is a sum-of-squares factor). For example, + * a HessianFactor need not be positive semidefinite, it can be indefinite or + * even negative semidefinite. + * + * If a HessianFactor is indefinite or negative semi-definite, then in order + * for solving the linear system to be possible, + * the Hessian of the full system must be positive definite (i.e. when all + * small Hessians are combined, the result must be positive definite). If + * this is not the case, an error will occur during elimination. + * + * This class stores G, g, and f as an augmented matrix HessianFactor::matrix_. + * The upper-left n x n blocks of HessianFactor::matrix_ store the upper-right + * triangle of G, the upper-right-most column of length n of HessianFactor::matrix_ + * stores g, and the lower-right entry of HessianFactor::matrix_ stores f, i.e. + * \code + HessianFactor::matrix_ = [ G11 G12 G13 ... g1 + 0 G22 G23 ... g2 + 0 0 G33 ... g3 + : : : : + 0 0 0 ... f ] + \endcode + Blocks can be accessed as follows: + \code + G11 = info(begin(), begin()); + G12 = info(begin(), begin()+1); + G23 = info(begin()+1, begin()+2); + g2 = linearTerm(begin()+1); + f = constantTerm(); + ....... + \endcode + */ + class GTSAM_EXPORT HessianFactor : public GaussianFactor { + protected: + + SymmetricBlockMatrix info_; ///< The full augmented information matrix, s.t. the quadratic error is 0.5*[x -1]'*H*[x -1] + + public: + + typedef GaussianFactor Base; ///< Typedef to base class + typedef HessianFactor This; ///< Typedef to this class + typedef boost::shared_ptr shared_ptr; ///< A shared_ptr to this class + typedef SymmetricBlockMatrix::Block Block; ///< A block from the Hessian matrix + typedef SymmetricBlockMatrix::constBlock constBlock; ///< A block from the Hessian matrix (const version) + + + /** default constructor for I/O */ + HessianFactor(); + + /** Construct a unary factor. G is the quadratic term (Hessian matrix), g + * the linear term (a vector), and f the constant term. The quadratic + * error is: + * 0.5*(f - 2*x'*g + x'*G*x) + */ + HessianFactor(Key j, const Matrix& G, const Vector& g, double f); + + /** Construct a unary factor, given a mean and covariance matrix. + * error is 0.5*(x-mu)'*inv(Sigma)*(x-mu) + */ + HessianFactor(Key j, const Vector& mu, const Matrix& Sigma); + + /** Construct a binary factor. Gxx are the upper-triangle blocks of the + * quadratic term (the Hessian matrix), gx the pieces of the linear vector + * term, and f the constant term. + * JacobianFactor error is \f[ 0.5* (Ax-b)' M (Ax-b) = 0.5*x'A'MAx - x'A'Mb + 0.5*b'Mb \f] + * HessianFactor error is \f[ 0.5*(x'Gx - 2x'g + f) = 0.5*x'Gx - x'*g + 0.5*f \f] + * So, with \f$ A = [A1 A2] \f$ and \f$ G=A*'M*A = [A1';A2']*M*[A1 A2] \f$ we have + \code + n1*n1 G11 = A1'*M*A1 + n1*n2 G12 = A1'*M*A2 + n2*n2 G22 = A2'*M*A2 + n1*1 g1 = A1'*M*b + n2*1 g2 = A2'*M*b + 1*1 f = b'*M*b + \endcode + */ + HessianFactor(Key j1, Key j2, + const Matrix& G11, const Matrix& G12, const Vector& g1, + const Matrix& G22, const Vector& g2, double f); + + /** Construct a ternary factor. Gxx are the upper-triangle blocks of the + * quadratic term (the Hessian matrix), gx the pieces of the linear vector + * term, and f the constant term. + */ + HessianFactor(Key j1, Key j2, Key j3, + const Matrix& G11, const Matrix& G12, const Matrix& G13, const Vector& g1, + const Matrix& G22, const Matrix& G23, const Vector& g2, + const Matrix& G33, const Vector& g3, double f); + + /** Construct an n-way factor. Gs contains the upper-triangle blocks of the + * quadratic term (the Hessian matrix) provided in row-order, gs the pieces + * of the linear vector term, and f the constant term. + */ + HessianFactor(const std::vector& js, const std::vector& Gs, + const std::vector& gs, double f); + + /** Constructor with an arbitrary number of keys and with the augmented information matrix + * specified as a block matrix. */ + template + HessianFactor(const KEYS& keys, const SymmetricBlockMatrix& augmentedInformation); + + /** Construct from a JacobianFactor (or from a GaussianConditional since it derives from it) */ + explicit HessianFactor(const JacobianFactor& cg); + + /** Attempt to construct from any GaussianFactor - currently supports JacobianFactor, + * HessianFactor, GaussianConditional, or any derived classes. */ + explicit HessianFactor(const GaussianFactor& factor); + + /** Combine a set of factors into a single dense HessianFactor */ + explicit HessianFactor(const GaussianFactorGraph& factors, + boost::optional scatter = boost::none); + + /** Destructor */ + virtual ~HessianFactor() {} + + /** Clone this HessianFactor */ + virtual GaussianFactor::shared_ptr clone() const { + return boost::make_shared(*this); } + + /** Print the factor for debugging and testing (implementing Testable) */ + virtual void print(const std::string& s = "", + const KeyFormatter& formatter = DefaultKeyFormatter) const; + + /** Compare to another factor for testing (implementing Testable) */ + virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const; + + /** Evaluate the factor error f(x), see above. */ + virtual double error(const VectorValues& c) const; /** 0.5*[x -1]'*H*[x -1] (also see constructor documentation) */ + + /** Return the dimension of the variable pointed to by the given key iterator + * todo: Remove this in favor of keeping track of dimensions with variables? + * @param variable An iterator pointing to the slot in this factor. You can + * use, for example, begin() + 2 to get the 3rd variable in this factor. + */ + virtual DenseIndex getDim(const_iterator variable) const { return info_(variable-this->begin(), 0).rows(); } + + /** Return the number of columns and rows of the Hessian matrix, including the information vector. */ + size_t rows() const { return info_.rows(); } + + /** + * Construct the corresponding anti-factor to negate information + * stored stored in this factor. + * @return a HessianFactor with negated Hessian matrices + */ + virtual GaussianFactor::shared_ptr negate() const; + + /** Check if the factor is empty. TODO: How should this be defined? */ + virtual bool empty() const { return size() == 0 /*|| rows() == 0*/; } + + /** Return a view of the block at (j1,j2) of the upper-triangular part of the + * information matrix \f$ H \f$, no data is copied. See HessianFactor class documentation + * above to explain that only the upper-triangular part of the information matrix is stored + * and returned by this function. + * @param j1 Which block row to get, as an iterator pointing to the slot in this factor. You can + * use, for example, begin() + 2 to get the 3rd variable in this factor. + * @param j2 Which block column to get, as an iterator pointing to the slot in this factor. You can + * use, for example, begin() + 2 to get the 3rd variable in this factor. + * @return A view of the requested block, not a copy. + */ + constBlock info(const_iterator j1, const_iterator j2) const { return info_(j1-begin(), j2-begin()); } + + /** Return a view of the block at (j1,j2) of the upper-triangular part of the + * information matrix \f$ H \f$, no data is copied. See HessianFactor class documentation + * above to explain that only the upper-triangular part of the information matrix is stored + * and returned by this function. + * @param j1 Which block row to get, as an iterator pointing to the slot in this factor. You can + * use, for example, begin() + 2 to get the 3rd variable in this factor. + * @param j2 Which block column to get, as an iterator pointing to the slot in this factor. You can + * use, for example, begin() + 2 to get the 3rd variable in this factor. + * @return A view of the requested block, not a copy. + */ + Block info(iterator j1, iterator j2) { return info_(j1-begin(), j2-begin()); } + + /** Return the upper-triangular part of the full *augmented* information matrix, + * as described above. See HessianFactor class documentation above to explain that only the + * upper-triangular part of the information matrix is stored and returned by this function. + */ + SymmetricBlockMatrix::constBlock info() const { return info_.full(); } + + /** Return the upper-triangular part of the full *augmented* information matrix, + * as described above. See HessianFactor class documentation above to explain that only the + * upper-triangular part of the information matrix is stored and returned by this function. + */ + SymmetricBlockMatrix::Block info() { return info_.full(); } + + /** Return the constant term \f$ f \f$ as described above + * @return The constant term \f$ f \f$ + */ + double constantTerm() const { return info_(this->size(), this->size())(0,0); } + + /** Return the constant term \f$ f \f$ as described above + * @return The constant term \f$ f \f$ + */ + double& constantTerm() { return info_(this->size(), this->size())(0,0); } + + /** Return the part of linear term \f$ g \f$ as described above corresponding to the requested variable. + * @param j Which block row to get, as an iterator pointing to the slot in this factor. You can + * use, for example, begin() + 2 to get the 3rd variable in this factor. + * @return The linear term \f$ g \f$ */ + constBlock::OffDiagonal::ColXpr linearTerm(const_iterator j) const { + return info_(j-begin(), size()).knownOffDiagonal().col(0); } + + /** Return the part of linear term \f$ g \f$ as described above corresponding to the requested variable. + * @param j Which block row to get, as an iterator pointing to the slot in this factor. You can + * use, for example, begin() + 2 to get the 3rd variable in this factor. + * @return The linear term \f$ g \f$ */ + Block::OffDiagonal::ColXpr linearTerm(iterator j) { + return info_(j-begin(), size()).knownOffDiagonal().col(0); } + + /** Return the complete linear term \f$ g \f$ as described above. + * @return The linear term \f$ g \f$ */ + constBlock::OffDiagonal::ColXpr linearTerm() const { + return info_.range(0, this->size(), this->size(), this->size() + 1).knownOffDiagonal().col(0); } + + /** Return the complete linear term \f$ g \f$ as described above. + * @return The linear term \f$ g \f$ */ + Block::OffDiagonal::ColXpr linearTerm() { + return info_.range(0, this->size(), this->size(), this->size() + 1).knownOffDiagonal().col(0); } + + /** Return the augmented information matrix represented by this GaussianFactor. + * The augmented information matrix contains the information matrix with an + * additional column holding the information vector, and an additional row + * holding the transpose of the information vector. The lower-right entry + * contains the constant error term (when \f$ \delta x = 0 \f$). The + * augmented information matrix is described in more detail in HessianFactor, + * which in fact stores an augmented information matrix. + * + * For HessianFactor, this is the same as info() except that this function + * returns a complete symmetric matrix whereas info() returns a matrix where + * only the upper triangle is valid, but should be interpreted as symmetric. + * This is because info() returns only a reference to the internal + * representation of the augmented information matrix, which stores only the + * upper triangle. + */ + virtual Matrix augmentedInformation() const; + + /** Return the non-augmented information matrix represented by this + * GaussianFactor. + */ + virtual Matrix information() const; + + /// Return the diagonal of the Hessian for this factor + virtual VectorValues hessianDiagonal() const; + + /* ************************************************************************* */ + virtual void hessianDiagonal(double* d) const; + + /// Return the block diagonal of the Hessian for this factor + virtual std::map hessianBlockDiagonal() const; + + /** + * Return (dense) matrix associated with factor + * @param ordering of variables needed for matrix column order + * @param set weight to true to bake in the weights + */ + virtual std::pair jacobian() const; + + /** + * Return (dense) matrix associated with factor + * The returned system is an augmented matrix: [A b] + * @param set weight to use whitening to bake in weights + */ + virtual Matrix augmentedJacobian() const; + + /** Return the full augmented Hessian matrix of this factor as a SymmetricBlockMatrix object. */ + const SymmetricBlockMatrix& matrixObject() const { return info_; } + + /** Update the factor by adding the information from the JacobianFactor + * (used internally during elimination). + * @param update The JacobianFactor containing the new information to add + * @param scatter A mapping from variable index to slot index in this HessianFactor + */ + void updateATA(const JacobianFactor& update, const Scatter& scatter); + + /** Update the factor by adding the information from the HessianFactor + * (used internally during elimination). + * @param update The HessianFactor containing the new information to add + * @param scatter A mapping from variable index to slot index in this HessianFactor + */ + void updateATA(const HessianFactor& update, const Scatter& scatter); + + /** y += alpha * A'*A*x */ + void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const; + + void multiplyHessianAdd(double alpha, const double* x, double* y, std::vector keys) const; + + void multiplyHessianAdd(double alpha, const double* x, double* y) const {}; + + /// eta for Hessian + VectorValues gradientAtZero() const; + + /** + * Densely partially eliminate with Cholesky factorization. JacobianFactors are + * left-multiplied with their transpose to form the Hessian using the conversion constructor + * HessianFactor(const JacobianFactor&). + * + * If any factors contain constrained noise models, this function will fail because our current + * implementation cannot handle constrained noise models in Cholesky factorization. The + * function EliminatePreferCholesky() automatically does QR instead when this is the case. + * + * Variables are eliminated in the order specified in \c keys. + * + * @param factors Factors to combine and eliminate + * @param keys The variables to eliminate and their elimination ordering + * @return The conditional and remaining factor + * + * \addtogroup LinearSolving */ + friend GTSAM_EXPORT std::pair, boost::shared_ptr > + EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys); + + /** + * Densely partially eliminate with Cholesky factorization. JacobianFactors are + * left-multiplied with their transpose to form the Hessian using the conversion constructor + * HessianFactor(const JacobianFactor&). + * + * This function will fall back on QR factorization for any cliques containing JacobianFactor's + * with constrained noise models. + * + * Variables are eliminated in the order specified in \c keys. + * + * @param factors Factors to combine and eliminate + * @param keys The variables to eliminate and their elimination ordering + * @return The conditional and remaining factor + * + * \addtogroup LinearSolving */ + friend GTSAM_EXPORT std::pair, boost::shared_ptr > + EliminatePreferCholesky(const GaussianFactorGraph& factors, const Ordering& keys); + + private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(GaussianFactor); + ar & BOOST_SERIALIZATION_NVP(info_); + } + }; + +} + +#include diff --git a/gtsam/linear/IterativeSolver.h b/gtsam/linear/IterativeSolver.h index 38515952e..988293a4f 100644 --- a/gtsam/linear/IterativeSolver.h +++ b/gtsam/linear/IterativeSolver.h @@ -11,12 +11,17 @@ #pragma once -#include -#include +#include + #include +#include namespace gtsam { + // Forward declarations + class VectorValues; + class GaussianFactorGraph; + /** * parameters for iterative linear solvers */ @@ -73,7 +78,7 @@ namespace gtsam { virtual VectorValues optimize (const VectorValues &initial) = 0; /* update interface to the nonlinear optimizer */ - virtual void replaceFactors(const GaussianFactorGraph::shared_ptr &factorGraph, const double lambda) {} + virtual void replaceFactors(const boost::shared_ptr &factorGraph, const double lambda) {} }; } diff --git a/gtsam/linear/JacobianFactor-inl.h b/gtsam/linear/JacobianFactor-inl.h new file mode 100644 index 000000000..7b0741ed5 --- /dev/null +++ b/gtsam/linear/JacobianFactor-inl.h @@ -0,0 +1,115 @@ +/* ---------------------------------------------------------------------------- + + * 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 JacobianFactor.h + * @author Richard Roberts + * @author Christian Potthast + * @author Frank Dellaert + * @date Dec 8, 2010 + */ +#pragma once + +#include +#include +#include +#include +#include + +namespace gtsam { + + /* ************************************************************************* */ + template + JacobianFactor::JacobianFactor(const TERMS&terms, const Vector &b, const SharedDiagonal& model) + { + fillTerms(terms, b, model); + } + + /* ************************************************************************* */ + template + JacobianFactor::JacobianFactor( + const KEYS& keys, const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& model) : + Base(keys), Ab_(augmentedMatrix) + { + // Check noise model dimension + if(model && (DenseIndex)model->dim() != augmentedMatrix.rows()) + throw InvalidNoiseModel(augmentedMatrix.rows(), model->dim()); + + // Check number of variables + if((DenseIndex)Base::keys_.size() != augmentedMatrix.nBlocks() - 1) + throw std::invalid_argument( + "Error in JacobianFactor constructor input. Number of provided keys plus\n" + "one for the RHS vector must equal the number of provided matrix blocks."); + + // Check RHS dimension + if(augmentedMatrix(augmentedMatrix.nBlocks() - 1).cols() != 1) + throw std::invalid_argument( + "Error in JacobianFactor constructor input. The last provided matrix block\n" + "must be the RHS vector, but the last provided block had more than one column."); + + // Take noise model + model_ = model; + } + + /* ************************************************************************* */ + namespace internal { + static inline DenseIndex getColsJF(const std::pair& p) { + return p.second.cols(); + } + } + + /* ************************************************************************* */ + template + void JacobianFactor::fillTerms(const TERMS& terms, const Vector& b, const SharedDiagonal& noiseModel) + { + using boost::adaptors::transformed; + namespace br = boost::range; + + // Check noise model dimension + if(noiseModel && (DenseIndex)noiseModel->dim() != b.size()) + throw InvalidNoiseModel(b.size(), noiseModel->dim()); + + // Resize base class key vector + Base::keys_.resize(terms.size()); + + // Construct block matrix - this uses the boost::range 'transformed' construct to apply + // internal::getColsJF (defined just above here in this file) to each term. This + // transforms the list of terms into a list of variable dimensions, by extracting the + // number of columns in each matrix. This is done to avoid separately allocating an + // array of dimensions before constructing the VerticalBlockMatrix. + Ab_ = VerticalBlockMatrix(terms | transformed(&internal::getColsJF), b.size(), true); + + // Check and add terms + DenseIndex i = 0; // For block index + for(typename TERMS::const_iterator termIt = terms.begin(); termIt != terms.end(); ++termIt) { + const std::pair& term = *termIt; + + // Check block rows + if(term.second.rows() != Ab_.rows()) + throw InvalidMatrixBlock(Ab_.rows(), term.second.rows()); + + // Assign key and matrix + Base::keys_[i] = term.first; + Ab_(i) = term.second; + + // Increment block index + ++ i; + } + + // Assign RHS vector + getb() = b; + + // Assign noise model + model_ = noiseModel; + } + +} // gtsam + diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 5de446d25..c080f75cb 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -12,6 +12,8 @@ /** * @file JacobianFactor.cpp * @author Richard Roberts + * @author Christian Potthast + * @author Frank Dellaert * @date Dec 8, 2010 */ @@ -20,16 +22,20 @@ #include #include #include +#include #include +#include #include #include #include #include #include +#include #include #include #include +#include #ifdef __GNUC__ #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wunused-variable" @@ -38,491 +44,682 @@ #ifdef __GNUC__ #pragma GCC diagnostic pop #endif +#include +#include +#include #include #include #include using namespace std; +using namespace boost::assign; namespace gtsam { - /* ************************************************************************* */ - void JacobianFactor::assertInvariants() const { -#ifndef NDEBUG - GaussianFactor::assertInvariants(); // The base class checks for unique keys - assert((size() == 0 && Ab_.rows() == 0 && Ab_.nBlocks() == 0) || size()+1 == Ab_.nBlocks()); +/* ************************************************************************* */ +JacobianFactor::JacobianFactor() : + Ab_(cref_list_of<1>(1), 0) { + getb().setZero(); +} + +/* ************************************************************************* */ +JacobianFactor::JacobianFactor(const GaussianFactor& gf) { + // Copy the matrix data depending on what type of factor we're copying from + if (const JacobianFactor* rhs = dynamic_cast(&gf)) + *this = JacobianFactor(*rhs); + else if (const HessianFactor* rhs = dynamic_cast(&gf)) + *this = JacobianFactor(*rhs); + else + throw std::invalid_argument( + "In JacobianFactor(const GaussianFactor& rhs), rhs is neither a JacobianFactor nor a HessianFactor"); +} + +/* ************************************************************************* */ +JacobianFactor::JacobianFactor(const Vector& b_in) : + Ab_(cref_list_of<1>(1), b_in.size()) { + getb() = b_in; +} + +/* ************************************************************************* */ +JacobianFactor::JacobianFactor(Key i1, const Matrix& A1, const Vector& b, + const SharedDiagonal& model) { + fillTerms(cref_list_of<1>(make_pair(i1, A1)), b, model); +} + +/* ************************************************************************* */ +JacobianFactor::JacobianFactor(const Key i1, const Matrix& A1, Key i2, + const Matrix& A2, const Vector& b, const SharedDiagonal& model) { + fillTerms(cref_list_of<2>(make_pair(i1, A1))(make_pair(i2, A2)), b, model); +} + +/* ************************************************************************* */ +JacobianFactor::JacobianFactor(const Key i1, const Matrix& A1, Key i2, + const Matrix& A2, Key i3, const Matrix& A3, const Vector& b, + const SharedDiagonal& model) { + fillTerms( + cref_list_of<3>(make_pair(i1, A1))(make_pair(i2, A2))(make_pair(i3, A3)), + b, model); +} + +/* ************************************************************************* */ +JacobianFactor::JacobianFactor(const HessianFactor& factor) : + Base(factor), Ab_( + VerticalBlockMatrix::LikeActiveViewOf(factor.matrixObject(), + factor.rows())) { + // Copy Hessian into our matrix and then do in-place Cholesky + Ab_.full() = factor.matrixObject().full(); + + // Do Cholesky to get a Jacobian + size_t maxrank; + bool success; + boost::tie(maxrank, success) = choleskyCareful(Ab_.matrix()); + + // Check for indefinite system + if (!success) + throw IndeterminantLinearSystemException(factor.keys().front()); + + // Zero out lower triangle + Ab_.matrix().topRows(maxrank).triangularView() = + Matrix::Zero(maxrank, Ab_.matrix().cols()); + // FIXME: replace with triangular system + Ab_.rowEnd() = maxrank; + model_ = SharedDiagonal(); // should be same as Unit::Create(maxrank); +} + +/* ************************************************************************* */ +// Helper functions for combine constructor +namespace { +boost::tuple, DenseIndex, DenseIndex> _countDims( + const FastVector& factors, + const FastVector& variableSlots) { + gttic(countDims); +#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS + FastVector varDims(variableSlots.size(), numeric_limits::max()); +#else + FastVector varDims(variableSlots.size(), + numeric_limits::max()); #endif - } + DenseIndex m = 0; + DenseIndex n = 0; + for (size_t jointVarpos = 0; jointVarpos < variableSlots.size(); + ++jointVarpos) { + const VariableSlots::const_iterator& slots = variableSlots[jointVarpos]; - /* ************************************************************************* */ - JacobianFactor::JacobianFactor(const JacobianFactor& gf) : - GaussianFactor(gf), model_(gf.model_), Ab_(matrix_) { - Ab_.assignNoalias(gf.Ab_); - assertInvariants(); - } + assert(slots->second.size() == factors.size()); - /* ************************************************************************* */ - JacobianFactor::JacobianFactor(const GaussianFactor& gf) : Ab_(matrix_) { - // Copy the matrix data depending on what type of factor we're copying from - if(const JacobianFactor* rhs = dynamic_cast(&gf)) - *this = JacobianFactor(*rhs); - else if(const HessianFactor* rhs = dynamic_cast(&gf)) - *this = JacobianFactor(*rhs); - else - throw std::invalid_argument("In JacobianFactor(const GaussianFactor& rhs), rhs is neither a JacobianFactor nor a HessianFactor"); - assertInvariants(); - } + bool foundVariable = false; + for (size_t sourceFactorI = 0; sourceFactorI < slots->second.size(); + ++sourceFactorI) { + const size_t sourceVarpos = slots->second[sourceFactorI]; + if (sourceVarpos != VariableSlots::Empty) { + const JacobianFactor& sourceFactor = *factors[sourceFactorI]; + if (sourceFactor.cols() > 1) { + foundVariable = true; + DenseIndex vardim = sourceFactor.getDim( + sourceFactor.begin() + sourceVarpos); - /* ************************************************************************* */ - JacobianFactor::JacobianFactor() : Ab_(matrix_) { assertInvariants(); } +#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS + if(varDims[jointVarpos] == numeric_limits::max()) { + varDims[jointVarpos] = vardim; + n += vardim; + } else { + if(!(varDims[jointVarpos] == vardim)) { + cout << "Factor " << sourceFactorI << " variable " << DefaultKeyFormatter(sourceFactor.keys()[sourceVarpos]) << + " has different dimensionality of " << vardim << " instead of " << varDims[jointVarpos] << endl; + exit(1); + } + } +#else - /* ************************************************************************* */ - JacobianFactor::JacobianFactor(const Vector& b_in) : Ab_(matrix_) { - size_t dims[] = { 1 }; - Ab_.copyStructureFrom(BlockAb(matrix_, dims, dims+1, b_in.size())); - getb() = b_in; - model_ = noiseModel::Unit::Create(this->rows()); - assertInvariants(); - } - - /* ************************************************************************* */ - JacobianFactor::JacobianFactor(Index i1, const Matrix& A1, - const Vector& b, const SharedDiagonal& model) : - GaussianFactor(i1), model_(model), Ab_(matrix_) { - - if(model->dim() != (size_t) b.size()) - throw InvalidNoiseModel(b.size(), model->dim()); - - size_t dims[] = { A1.cols(), 1}; - Ab_.copyStructureFrom(BlockAb(matrix_, dims, dims+2, b.size())); - Ab_(0) = A1; - getb() = b; - assertInvariants(); - } - - /* ************************************************************************* */ - JacobianFactor::JacobianFactor(Index i1, const Matrix& A1, Index i2, const Matrix& A2, - const Vector& b, const SharedDiagonal& model) : - GaussianFactor(i1,i2), model_(model), Ab_(matrix_) { - - if(model->dim() != (size_t) b.size()) - throw InvalidNoiseModel(b.size(), model->dim()); - - size_t dims[] = { A1.cols(), A2.cols(), 1}; - Ab_.copyStructureFrom(BlockAb(matrix_, dims, dims+3, b.size())); - Ab_(0) = A1; - Ab_(1) = A2; - getb() = b; - assertInvariants(); - } - - /* ************************************************************************* */ - JacobianFactor::JacobianFactor(Index i1, const Matrix& A1, Index i2, const Matrix& A2, - Index i3, const Matrix& A3, const Vector& b, const SharedDiagonal& model) : - GaussianFactor(i1,i2,i3), model_(model), Ab_(matrix_) { - - if(model->dim() != (size_t) b.size()) - throw InvalidNoiseModel(b.size(), model->dim()); - - size_t dims[] = { A1.cols(), A2.cols(), A3.cols(), 1}; - Ab_.copyStructureFrom(BlockAb(matrix_, dims, dims+4, b.size())); - Ab_(0) = A1; - Ab_(1) = A2; - Ab_(2) = A3; - getb() = b; - assertInvariants(); - } - - /* ************************************************************************* */ - JacobianFactor::JacobianFactor(const std::vector > &terms, - const Vector &b, const SharedDiagonal& model) : - GaussianFactor(GetKeys(terms.size(), terms.begin(), terms.end())), - model_(model), Ab_(matrix_) - { - // get number of measurements and variables involved in this factor - size_t m = b.size(), n = terms.size(); - - if(model->dim() != (size_t) m) - throw InvalidNoiseModel(b.size(), model->dim()); - - // Get the dimensions of each variable and copy to "dims" array, add 1 for RHS - size_t* dims = (size_t*)alloca(sizeof(size_t)*(n+1)); // FIXME: alloca is bad, just ask Google. - for(size_t j=0; j > &terms, - const Vector &b, const SharedDiagonal& model) : - GaussianFactor(GetKeys(terms.size(), terms.begin(), terms.end())), - model_(model), Ab_(matrix_) - { - - if(model->dim() != (size_t) b.size()) - throw InvalidNoiseModel(b.size(), model->dim()); - - size_t* dims=(size_t*)alloca(sizeof(size_t)*(terms.size()+1)); // FIXME: alloca is bad, just ask Google. - size_t j=0; - std::list >::const_iterator term=terms.begin(); - for(; term!=terms.end(); ++term,++j) - dims[j] = term->second.cols(); - dims[j] = 1; - Ab_.copyStructureFrom(BlockAb(matrix_, dims, dims+terms.size()+1, b.size())); - j = 0; - for(term=terms.begin(); term!=terms.end(); ++term,++j) - Ab_(j) = term->second; - getb() = b; - assertInvariants(); - } - - /* ************************************************************************* */ - JacobianFactor::JacobianFactor(const GaussianConditional& cg) : - GaussianFactor(cg), - model_(noiseModel::Diagonal::Sigmas(cg.get_sigmas(), true)), - Ab_(matrix_) { - Ab_.assignNoalias(cg.rsd_); - assertInvariants(); - } - - /* ************************************************************************* */ - JacobianFactor::JacobianFactor(const HessianFactor& factor) : Ab_(matrix_) { - keys_ = factor.keys_; - Ab_.assignNoalias(factor.info_); - - // Do Cholesky to get a Jacobian - size_t maxrank; - bool success; - boost::tie(maxrank, success) = choleskyCareful(matrix_); - - // Check for indefinite system - if(!success) - throw IndeterminantLinearSystemException(factor.keys().front()); - - // Zero out lower triangle - matrix_.topRows(maxrank).triangularView() = - Matrix::Zero(maxrank, matrix_.cols()); - // FIXME: replace with triangular system - Ab_.rowEnd() = maxrank; - model_ = noiseModel::Unit::Create(maxrank); - - assertInvariants(); - } - - /* ************************************************************************* */ - JacobianFactor::JacobianFactor(const GaussianFactorGraph& gfg) : Ab_(matrix_) { - // Cast or convert to Jacobians - FactorGraph jacobians; - BOOST_FOREACH(const GaussianFactorGraph::sharedFactor& factor, gfg) { - if(factor) { - if(JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast(factor)) - jacobians.push_back(jf); - else - jacobians.push_back(boost::make_shared(*factor)); + varDims[jointVarpos] = vardim; + n += vardim; + break; +#endif + } } } - *this = *CombineJacobians(jacobians, VariableSlots(jacobians)); + if (!foundVariable) + throw std::invalid_argument( + "Unable to determine dimensionality for all variables"); } - /* ************************************************************************* */ - JacobianFactor& JacobianFactor::operator=(const JacobianFactor& rhs) { - this->Base::operator=(rhs); // Copy keys - model_ = rhs.model_; // Copy noise model - Ab_.assignNoalias(rhs.Ab_); // Copy matrix and block structure - assertInvariants(); - return *this; + BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, factors) { + m += factor->rows(); } - /* ************************************************************************* */ - void JacobianFactor::print(const string& s, const IndexFormatter& formatter) const { +#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS + BOOST_FOREACH(DenseIndex d, varDims) { + assert(d != numeric_limits::max()); + } +#endif + + return boost::make_tuple(varDims, m, n); +} + +/* ************************************************************************* */ +FastVector _convertOrCastToJacobians( + const GaussianFactorGraph& factors) { + gttic(Convert_to_Jacobians); + FastVector jacobians; + jacobians.reserve(factors.size()); + BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) { + if (factor) { + if (JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast< + JacobianFactor>(factor)) + jacobians.push_back(jf); + else + jacobians.push_back(boost::make_shared(*factor)); + } + } + return jacobians; +} +} + +/* ************************************************************************* */ +JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph, + boost::optional ordering, + boost::optional variableSlots) { + gttic(JacobianFactor_combine_constructor); + + // Compute VariableSlots if one was not provided + boost::optional computedVariableSlots; + if (!variableSlots) { + computedVariableSlots = VariableSlots(graph); + variableSlots = computedVariableSlots; // Binds reference, does not copy VariableSlots + } + + // Cast or convert to Jacobians + FastVector jacobians = _convertOrCastToJacobians( + graph); + + gttic(Order_slots); + // Order variable slots - we maintain the vector of ordered slots, as well as keep a list + // 'unorderedSlots' of any variables discovered that are not in the ordering. Those will then + // be added after all of the ordered variables. + FastVector orderedSlots; + orderedSlots.reserve(variableSlots->size()); + if (ordering) { + // If an ordering is provided, arrange the slots first that ordering + FastList unorderedSlots; + size_t nOrderingSlotsUsed = 0; + orderedSlots.resize(ordering->size()); + FastMap inverseOrdering = ordering->invert(); + for (VariableSlots::const_iterator item = variableSlots->begin(); + item != variableSlots->end(); ++item) { + FastMap::const_iterator orderingPosition = + inverseOrdering.find(item->first); + if (orderingPosition == inverseOrdering.end()) { + unorderedSlots.push_back(item); + } else { + orderedSlots[orderingPosition->second] = item; + ++nOrderingSlotsUsed; + } + } + if (nOrderingSlotsUsed != ordering->size()) + throw std::invalid_argument( + "The ordering provided to the JacobianFactor combine constructor\n" + "contained extra variables that did not appear in the factors to combine."); + // Add the remaining slots + BOOST_FOREACH(VariableSlots::const_iterator item, unorderedSlots) { + orderedSlots.push_back(item); + } + } else { + // If no ordering is provided, arrange the slots as they were, which will be sorted + // numerically since VariableSlots uses a map sorting on Key. + for (VariableSlots::const_iterator item = variableSlots->begin(); + item != variableSlots->end(); ++item) + orderedSlots.push_back(item); + } + gttoc(Order_slots); + + // Count dimensions + FastVector varDims; + DenseIndex m, n; + boost::tie(varDims, m, n) = _countDims(jacobians, orderedSlots); + + // Allocate matrix and copy keys in order + gttic(allocate); + Ab_ = VerticalBlockMatrix(varDims, m, true); // Allocate augmented matrix + Base::keys_.resize(orderedSlots.size()); + boost::range::copy( + // Get variable keys + orderedSlots | boost::adaptors::indirected | boost::adaptors::map_keys, + Base::keys_.begin()); + gttoc(allocate); + + // Loop over slots in combined factor and copy blocks from source factors + gttic(copy_blocks); + size_t combinedSlot = 0; + BOOST_FOREACH(VariableSlots::const_iterator varslot, orderedSlots) { + JacobianFactor::ABlock destSlot(this->getA(this->begin() + combinedSlot)); + // Loop over source jacobians + DenseIndex nextRow = 0; + for (size_t factorI = 0; factorI < jacobians.size(); ++factorI) { + // Slot in source factor + const size_t sourceSlot = varslot->second[factorI]; + const DenseIndex sourceRows = jacobians[factorI]->rows(); + if (sourceRows > 0) { + JacobianFactor::ABlock::RowsBlockXpr destBlock( + destSlot.middleRows(nextRow, sourceRows)); + // Copy if exists in source factor, otherwise set zero + if (sourceSlot != VariableSlots::Empty) + destBlock = jacobians[factorI]->getA( + jacobians[factorI]->begin() + sourceSlot); + else + destBlock.setZero(); + nextRow += sourceRows; + } + } + ++combinedSlot; + } + gttoc(copy_blocks); + + // Copy the RHS vectors and sigmas + gttic(copy_vectors); + bool anyConstrained = false; + boost::optional sigmas; + // Loop over source jacobians + DenseIndex nextRow = 0; + for (size_t factorI = 0; factorI < jacobians.size(); ++factorI) { + const DenseIndex sourceRows = jacobians[factorI]->rows(); + if (sourceRows > 0) { + this->getb().segment(nextRow, sourceRows) = jacobians[factorI]->getb(); + if (jacobians[factorI]->get_model()) { + // If the factor has a noise model and we haven't yet allocated sigmas, allocate it. + if (!sigmas) + sigmas = Vector::Constant(m, 1.0); + sigmas->segment(nextRow, sourceRows) = + jacobians[factorI]->get_model()->sigmas(); + if (jacobians[factorI]->isConstrained()) + anyConstrained = true; + } + nextRow += sourceRows; + } + } + gttoc(copy_vectors); + + if (sigmas) + this->setModel(anyConstrained, *sigmas); +} + +/* ************************************************************************* */ +void JacobianFactor::print(const string& s, + const KeyFormatter& formatter) const { + if (!s.empty()) cout << s << "\n"; - if (empty()) { - cout << " empty, keys: "; - BOOST_FOREACH(const Index& key, keys()) { cout << formatter(key) << " "; } - cout << endl; - } else { - for(const_iterator key=begin(); key!=end(); ++key) - cout << boost::format("A[%1%]=\n")%formatter(*key) << getA(key) << endl; - cout << "b=" << getb() << endl; - model_->print("model"); - } + for (const_iterator key = begin(); key != end(); ++key) { + cout + << formatMatrixIndented( + (boost::format(" A[%1%] = ") % formatter(*key)).str(), getA(key)) + << endl; } + cout << formatMatrixIndented(" b = ", getb(), true) << "\n"; + if (model_) + model_->print(" Noise model: "); + else + cout << " No noise model" << endl; +} - /* ************************************************************************* */ - // Check if two linear factors are equal - bool JacobianFactor::equals(const GaussianFactor& f_, double tol) const { - if(!dynamic_cast(&f_)) +/* ************************************************************************* */ +// Check if two linear factors are equal +bool JacobianFactor::equals(const GaussianFactor& f_, double tol) const { + if (!dynamic_cast(&f_)) + return false; + else { + const JacobianFactor& f(static_cast(f_)); + + // Check keys + if (keys() != f.keys()) return false; - else { - const JacobianFactor& f(static_cast(f_)); - if (empty()) return (f.empty()); - if(keys()!=f.keys() /*|| !model_->equals(lf->model_, tol)*/) + + // Check noise model + if ((model_ && !f.model_) || (!model_ && f.model_)) + return false; + if (model_ && f.model_ && !model_->equals(*f.model_, tol)) + return false; + + // Check matrix sizes + if (!(Ab_.rows() == f.Ab_.rows() && Ab_.cols() == f.Ab_.cols())) + return false; + + // Check matrix contents + constABlock Ab1(Ab_.range(0, Ab_.nBlocks())); + constABlock Ab2(f.Ab_.range(0, f.Ab_.nBlocks())); + for (size_t row = 0; row < (size_t) Ab1.rows(); ++row) + if (!equal_with_abs_tol(Ab1.row(row), Ab2.row(row), tol) + && !equal_with_abs_tol(-Ab1.row(row), Ab2.row(row), tol)) return false; - if (!(Ab_.rows() == f.Ab_.rows() && Ab_.cols() == f.Ab_.cols())) - return false; - - constABlock Ab1(Ab_.range(0, Ab_.nBlocks())); - constABlock Ab2(f.Ab_.range(0, f.Ab_.nBlocks())); - for(size_t row=0; row< (size_t) Ab1.rows(); ++row) - if(!equal_with_abs_tol(Ab1.row(row), Ab2.row(row), tol) && - !equal_with_abs_tol(-Ab1.row(row), Ab2.row(row), tol)) - return false; - - return true; - } + return true; } +} - /* ************************************************************************* */ - Vector JacobianFactor::unweighted_error(const VectorValues& c) const { - Vector e = -getb(); - if (empty()) return e; - for(size_t pos=0; poswhiten(-getb()); +/* ************************************************************************* */ +Vector JacobianFactor::error_vector(const VectorValues& c) const { + if (model_) return model_->whiten(unweighted_error(c)); - } + else + return unweighted_error(c); +} - /* ************************************************************************* */ - double JacobianFactor::error(const VectorValues& c) const { - if (empty()) return 0; - Vector weighted = error_vector(c); - return 0.5 * weighted.dot(weighted); - } +/* ************************************************************************* */ +double JacobianFactor::error(const VectorValues& c) const { + if (empty()) + return 0; + Vector weighted = error_vector(c); + return 0.5 * weighted.dot(weighted); +} - /* ************************************************************************* */ - Matrix JacobianFactor::augmentedInformation() const { +/* ************************************************************************* */ +Matrix JacobianFactor::augmentedInformation() const { + if (model_) { Matrix AbWhitened = Ab_.full(); model_->WhitenInPlace(AbWhitened); return AbWhitened.transpose() * AbWhitened; + } else { + return Ab_.full().transpose() * Ab_.full(); } +} - /* ************************************************************************* */ - Matrix JacobianFactor::information() const { +/* ************************************************************************* */ +Matrix JacobianFactor::information() const { + if (model_) { Matrix AWhitened = this->getA(); model_->WhitenInPlace(AWhitened); return AWhitened.transpose() * AWhitened; + } else { + return this->getA().transpose() * this->getA(); } +} - /* ************************************************************************* */ - Vector JacobianFactor::operator*(const VectorValues& x) const { - Vector Ax = zero(Ab_.rows()); - if (empty()) return Ax; - - // Just iterate over all A matrices and multiply in correct config part - for(size_t pos=0; poswhiten(Ax); - } - - /* ************************************************************************* */ - void JacobianFactor::transposeMultiplyAdd(double alpha, const Vector& e, - VectorValues& x) const { - Vector E = alpha * model_->whiten(e); - // Just iterate over all A matrices and insert Ai^e into VectorValues - for(size_t pos=0; pos JacobianFactor::matrix(bool weight) const { - Matrix A(Ab_.range(0, size())); - Vector b(getb()); - // divide in sigma so error is indeed 0.5*|Ax-b| - if (weight) model_->WhitenSystem(A,b); - return make_pair(A, b); - } - - /* ************************************************************************* */ - Matrix JacobianFactor::matrix_augmented(bool weight) const { - if (weight) { Matrix Ab(Ab_.range(0,Ab_.nBlocks())); model_->WhitenInPlace(Ab); return Ab; } - else return Ab_.range(0, Ab_.nBlocks()); - } - - /* ************************************************************************* */ - std::vector > - JacobianFactor::sparse(const std::vector& columnIndices) const { - - std::vector > entries; - - // iterate over all variables in the factor - for(const_iterator var=begin(); varWhiten(getA(var))); - // find first column index for this key - size_t column_start = columnIndices[*var]; - for (size_t i = 0; i < (size_t) whitenedA.rows(); i++) - for (size_t j = 0; j < (size_t) whitenedA.cols(); j++) { - double s = whitenedA(i,j); - if (std::abs(s) > 1e-12) entries.push_back( - boost::make_tuple(i, column_start + j, s)); - } +/* ************************************************************************* */ +VectorValues JacobianFactor::hessianDiagonal() const { + VectorValues d; + for (size_t pos = 0; pos < size(); ++pos) { + Key j = keys_[pos]; + size_t nj = Ab_(pos).cols(); + Vector dj(nj); + for (size_t k = 0; k < nj; ++k) { + Vector column_k = Ab_(pos).col(k); + if (model_) + column_k = model_->whiten(column_k); + dj(k) = dot(column_k, column_k); } + d.insert(j, dj); + } + return d; +} - Vector whitenedb(model_->whiten(getb())); - size_t bcolumn = columnIndices.back(); - for (size_t i = 0; i < (size_t) whitenedb.size(); i++) - entries.push_back(boost::make_tuple(i, bcolumn, whitenedb(i))); +/* ************************************************************************* */ +// TODO: currently assumes all variables of the same size 9 and keys arranged from 0 to n +void JacobianFactor::hessianDiagonal(double* d) const { - // return the result - return entries; + // Use eigen magic to access raw memory + typedef Eigen::Matrix DVector; + typedef Eigen::Map DMap; + + // Loop over all variables in the factor + for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { + // Get the diagonal block, and insert its diagonal + DVector dj; + for (size_t k = 0; k < 9; ++k) + dj(k) = Ab_(j).col(k).squaredNorm(); + + DMap(d + 9 * j) += dj; + } +} + +/* ************************************************************************* */ +map JacobianFactor::hessianBlockDiagonal() const { + map blocks; + for (size_t pos = 0; pos < size(); ++pos) { + Key j = keys_[pos]; + Matrix Aj = Ab_(pos); + if (model_) + Aj = model_->Whiten(Aj); + blocks.insert(make_pair(j, Aj.transpose() * Aj)); + } + return blocks; +} + +/* ************************************************************************* */ +Vector JacobianFactor::operator*(const VectorValues& x) const { + Vector Ax = zero(Ab_.rows()); + if (empty()) + return Ax; + + // Just iterate over all A matrices and multiply in correct config part + for (size_t pos = 0; pos < size(); ++pos) + Ax += Ab_(pos) * x[keys_[pos]]; + + return model_ ? model_->whiten(Ax) : Ax; +} + +/* ************************************************************************* */ +void JacobianFactor::transposeMultiplyAdd(double alpha, const Vector& e, + VectorValues& x) const { + Vector E = alpha * (model_ ? model_->whiten(e) : e); + // Just iterate over all A matrices and insert Ai^e into VectorValues + for (size_t pos = 0; pos < size(); ++pos) { + Key j = keys_[pos]; + // Create the value as a zero vector if it does not exist. + pair xi = x.tryInsert(j, Vector()); + if (xi.second) + xi.first->second = Vector::Zero(getDim(begin() + pos)); + gtsam::transposeMultiplyAdd(Ab_(pos), E, xi.first->second); + + } +} + +/* ************************************************************************* */ +void JacobianFactor::multiplyHessianAdd(double alpha, const VectorValues& x, + VectorValues& y) const { + Vector Ax = (*this) * x; + transposeMultiplyAdd(alpha, Ax, y); +} + +void JacobianFactor::multiplyHessianAdd(double alpha, const double* x, + double* y, std::vector keys) const { + + // Use eigen magic to access raw memory + typedef Eigen::Matrix DVector; + typedef Eigen::Map DMap; + typedef Eigen::Map ConstDMap; + + if (empty()) + return; + Vector Ax = zero(Ab_.rows()); + + // Just iterate over all A matrices and multiply in correct config part + for (size_t pos = 0; pos < size(); ++pos) + Ax += Ab_(pos) + * ConstDMap(x + keys[keys_[pos]], + keys[keys_[pos] + 1] - keys[keys_[pos]]); + + // Deal with noise properly, need to Double* whiten as we are dividing by variance + if (model_) { + model_->whitenInPlace(Ax); + model_->whitenInPlace(Ax); } - /* ************************************************************************* */ - JacobianFactor JacobianFactor::whiten() const { - JacobianFactor result(*this); - result.model_->WhitenInPlace(result.matrix_); - result.model_ = noiseModel::Unit::Create(result.model_->dim()); - return result; - } + // multiply with alpha + Ax *= alpha; - /* ************************************************************************* */ - GaussianFactor::shared_ptr JacobianFactor::negate() const { - HessianFactor hessian(*this); - return hessian.negate(); - } - - /* ************************************************************************* */ - GaussianConditional::shared_ptr JacobianFactor::eliminateFirst() { - return this->eliminate(1); - } - - /* ************************************************************************* */ - GaussianConditional::shared_ptr JacobianFactor::splitConditional(size_t nrFrontals) { - assert(Ab_.rowStart() == 0 && Ab_.rowEnd() == (size_t) matrix_.rows() && Ab_.firstBlock() == 0); - assert(size() >= nrFrontals); - assertInvariants(); - - const bool debug = ISDEBUG("JacobianFactor::splitConditional"); - - if(debug) cout << "Eliminating " << nrFrontals << " frontal variables" << endl; - if(debug) this->print("Splitting JacobianFactor: "); - - size_t frontalDim = Ab_.range(0,nrFrontals).cols(); - - // Check for singular factor - if(model_->dim() < frontalDim) - throw IndeterminantLinearSystemException(this->keys().front()); - - // Extract conditional - gttic(cond_Rd); - - // Restrict the matrix to be in the first nrFrontals variables - Ab_.rowEnd() = Ab_.rowStart() + frontalDim; - const Eigen::VectorBlock sigmas = model_->sigmas().segment(Ab_.rowStart(), Ab_.rowEnd()-Ab_.rowStart()); - GaussianConditional::shared_ptr conditional(new GaussianConditional(begin(), end(), nrFrontals, Ab_, sigmas)); - if(debug) conditional->print("Extracted conditional: "); - Ab_.rowStart() += frontalDim; - Ab_.firstBlock() += nrFrontals; - gttoc(cond_Rd); - - if(debug) conditional->print("Extracted conditional: "); - - gttic(remaining_factor); - // Take lower-right block of Ab to get the new factor - Ab_.rowEnd() = model_->dim(); - keys_.erase(begin(), begin() + nrFrontals); - // Set sigmas with the right model - if (model_->isConstrained()) - model_ = noiseModel::Constrained::MixedSigmas(sub(model_->sigmas(), frontalDim, model_->dim())); - else - model_ = noiseModel::Diagonal::Sigmas(sub(model_->sigmas(), frontalDim, model_->dim())); - if(debug) this->print("Eliminated factor: "); - assert(Ab_.rows() <= Ab_.cols()-1); - gttoc(remaining_factor); - - if(debug) print("Eliminated factor: "); - - assertInvariants(); - - return conditional; - } - - /* ************************************************************************* */ - GaussianConditional::shared_ptr JacobianFactor::eliminate(size_t nrFrontals) { - - assert(Ab_.rowStart() == 0 && Ab_.rowEnd() == (size_t) matrix_.rows() && Ab_.firstBlock() == 0); - assert(size() >= nrFrontals); - assertInvariants(); - - const bool debug = ISDEBUG("JacobianFactor::eliminate"); - - if(debug) cout << "Eliminating " << nrFrontals << " frontal variables" << endl; - if(debug) this->print("Eliminating JacobianFactor: "); - if(debug) gtsam::print(matrix_, "Augmented Ab: "); - - size_t frontalDim = Ab_.range(0,nrFrontals).cols(); - - if(debug) cout << "frontalDim = " << frontalDim << endl; - - // Use in-place QR dense Ab appropriate to NoiseModel - gttic(QR); - SharedDiagonal noiseModel = model_->QR(matrix_); - gttoc(QR); - - // Zero the lower-left triangle. todo: not all of these entries actually - // need to be zeroed if we are careful to start copying rows after the last - // structural zero. - if(matrix_.rows() > 0) - for(size_t j=0; j<(size_t) matrix_.cols(); ++j) - for(size_t i=j+1; idim(); ++i) - matrix_(i,j) = 0.0; - - if(debug) gtsam::print(matrix_, "QR result: "); - if(debug) noiseModel->print("QR result noise model: "); - - // Start of next part - model_ = noiseModel; - return splitConditional(nrFrontals); - } - - /* ************************************************************************* */ - void JacobianFactor::allocate(const VariableSlots& variableSlots, vector< - size_t>& varDims, size_t m) { - keys_.resize(variableSlots.size()); - std::transform(variableSlots.begin(), variableSlots.end(), begin(), - boost::bind(&VariableSlots::const_iterator::value_type::first, _1)); - varDims.push_back(1); - Ab_.copyStructureFrom(BlockAb(matrix_, varDims.begin(), varDims.end(), m)); - } - - /* ************************************************************************* */ - void JacobianFactor::setModel(bool anyConstrained, const Vector& sigmas) { - if((size_t) sigmas.size() != this->rows()) - throw InvalidNoiseModel(this->rows(), sigmas.size()); - if (anyConstrained) - model_ = noiseModel::Constrained::MixedSigmas(sigmas); - else - model_ = noiseModel::Diagonal::Sigmas(sigmas); - } - - /* ************************************************************************* */ - const char* JacobianFactor::InvalidNoiseModel::what() const throw() { - if(description_.empty()) - description_ = (boost::format( - "A JacobianFactor was attempted to be constructed or modified to use a\n" - "noise model of incompatible dimension. The JacobianFactor has\n" - "dimensionality (i.e. length of error vector) %d but the provided noise\n" - "model has dimensionality %d.") % factorDims % noiseModelDims).str(); - return description_.c_str(); - } + // Again iterate over all A matrices and insert Ai^e into y + for (size_t pos = 0; pos < size(); ++pos) + DMap(y + keys[keys_[pos]], keys[keys_[pos] + 1] - keys[keys_[pos]]) += Ab_( + pos).transpose() * Ax; + +} + +/* ************************************************************************* */ +VectorValues JacobianFactor::gradientAtZero() const { + VectorValues g; + Vector b = getb(); + // Gradient is really -A'*b / sigma^2 + // transposeMultiplyAdd will divide by sigma once, so we need one more + Vector b_sigma = model_ ? model_->whiten(b) : b; + this->transposeMultiplyAdd(-1.0, b_sigma, g); // g -= A'*b/sigma^2 + return g; +} + +/* ************************************************************************* */ +pair JacobianFactor::jacobian() const { + pair result = jacobianUnweighted(); + // divide in sigma so error is indeed 0.5*|Ax-b| + if (model_) + model_->WhitenSystem(result.first, result.second); + return result; +} + +/* ************************************************************************* */ +pair JacobianFactor::jacobianUnweighted() const { + Matrix A(Ab_.range(0, size())); + Vector b(getb()); + return make_pair(A, b); +} + +/* ************************************************************************* */ +Matrix JacobianFactor::augmentedJacobian() const { + Matrix Ab = augmentedJacobianUnweighted(); + if (model_) + model_->WhitenInPlace(Ab); + return Ab; +} + +/* ************************************************************************* */ +Matrix JacobianFactor::augmentedJacobianUnweighted() const { + return Ab_.range(0, Ab_.nBlocks()); +} + +/* ************************************************************************* */ +JacobianFactor JacobianFactor::whiten() const { + JacobianFactor result(*this); + if (model_) { + result.model_->WhitenInPlace(result.Ab_.full()); + result.model_ = SharedDiagonal(); + } + return result; +} + +/* ************************************************************************* */ +GaussianFactor::shared_ptr JacobianFactor::negate() const { + HessianFactor hessian(*this); + return hessian.negate(); +} + +/* ************************************************************************* */ +std::pair, + boost::shared_ptr > JacobianFactor::eliminate( + const Ordering& keys) { + GaussianFactorGraph graph; + graph.add(*this); + return EliminateQR(graph, keys); +} + +/* ************************************************************************* */ +void JacobianFactor::setModel(bool anyConstrained, const Vector& sigmas) { + if ((size_t) sigmas.size() != this->rows()) + throw InvalidNoiseModel(this->rows(), sigmas.size()); + if (anyConstrained) + model_ = noiseModel::Constrained::MixedSigmas(sigmas); + else + model_ = noiseModel::Diagonal::Sigmas(sigmas); +} + +/* ************************************************************************* */ +std::pair, + boost::shared_ptr > EliminateQR( + const GaussianFactorGraph& factors, const Ordering& keys) { + gttic(EliminateQR); + // Combine and sort variable blocks in elimination order + JacobianFactor::shared_ptr jointFactor; + try { + jointFactor = boost::make_shared(factors, keys); + } catch (std::invalid_argument&) { + throw InvalidDenseElimination( + "EliminateQR was called with a request to eliminate variables that are not\n" + "involved in the provided factors."); + } + + // Do dense elimination + SharedDiagonal noiseModel; + if (jointFactor->model_) + jointFactor->model_ = jointFactor->model_->QR(jointFactor->Ab_.matrix()); + else + inplace_QR(jointFactor->Ab_.matrix()); + + // Zero below the diagonal + jointFactor->Ab_.matrix().triangularView().setZero(); + + // Split elimination result into conditional and remaining factor + GaussianConditional::shared_ptr conditional = jointFactor->splitConditional( + keys.size()); + + return make_pair(conditional, jointFactor); +} + +/* ************************************************************************* */ +GaussianConditional::shared_ptr JacobianFactor::splitConditional( + size_t nrFrontals) { + gttic(JacobianFactor_splitConditional); + + if (nrFrontals > size()) + throw std::invalid_argument( + "Requesting to split more variables than exist using JacobianFactor::splitConditional"); + + DenseIndex frontalDim = Ab_.range(0, nrFrontals).cols(); + + // Restrict the matrix to be in the first nrFrontals variables and create the conditional + gttic(cond_Rd); + const DenseIndex originalRowEnd = Ab_.rowEnd(); + Ab_.rowEnd() = Ab_.rowStart() + frontalDim; + SharedDiagonal conditionalNoiseModel; + if (model_) { + if ((DenseIndex) model_->dim() < frontalDim) + throw IndeterminantLinearSystemException(this->keys().front()); + conditionalNoiseModel = noiseModel::Diagonal::Sigmas( + model_->sigmas().segment(Ab_.rowStart(), Ab_.rows())); + } + GaussianConditional::shared_ptr conditional = boost::make_shared< + GaussianConditional>(Base::keys_, nrFrontals, Ab_, conditionalNoiseModel); + const DenseIndex maxRemainingRows = std::min(Ab_.cols() - 1, originalRowEnd) + - Ab_.rowStart() - frontalDim; + const DenseIndex remainingRows = + model_ ? + std::min(model_->sigmas().size() - frontalDim, maxRemainingRows) : + maxRemainingRows; + Ab_.rowStart() += frontalDim; + Ab_.rowEnd() = Ab_.rowStart() + remainingRows; + Ab_.firstBlock() += nrFrontals; + gttoc(cond_Rd); + + // Take lower-right block of Ab to get the new factor + gttic(remaining_factor); + keys_.erase(begin(), begin() + nrFrontals); + // Set sigmas with the right model + if (model_) { + if (model_->isConstrained()) + model_ = noiseModel::Constrained::MixedSigmas( + model_->sigmas().tail(remainingRows)); + else + model_ = noiseModel::Diagonal::Sigmas( + model_->sigmas().tail(remainingRows)); + assert(model_->dim() == (size_t)Ab_.rows()); + } + gttoc(remaining_factor); + + return conditional; +} } diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 01de4953a..5a567c2c7 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -12,32 +12,33 @@ /** * @file JacobianFactor.h * @author Richard Roberts + * @author Christian Potthast + * @author Frank Dellaert * @date Dec 8, 2010 */ #pragma once #include -#include -#include #include -#include -#include +#include #include -#include - -// Forward declarations of friend unit tests -class JacobianFactorCombine2Test; -class JacobianFactoreliminateFrontalsTest; -class JacobianFactorconstructor2Test; +#include namespace gtsam { // Forward declarations class HessianFactor; class VariableSlots; - template class BayesNet; class GaussianFactorGraph; + class GaussianConditional; + class HessianFactor; + class VectorValues; + class Ordering; + class JacobianFactor; + + GTSAM_EXPORT std::pair, boost::shared_ptr > + EliminateQR(const GaussianFactorGraph& factors, const Ordering& keys); /** * A Gaussian factor in the squared-error form. @@ -78,80 +79,88 @@ namespace gtsam { * and the negative log-likelihood represented by this factor would be * \f[ E(x) = \frac{1}{2} (A_1 x_{j1} + A_2 x_{j2} - b)^T \Sigma^{-1} (A_1 x_{j1} + A_2 x_{j2} - b) . \f] */ - class GTSAM_EXPORT JacobianFactor : public GaussianFactor { - protected: - typedef Matrix AbMatrix; - typedef VerticalBlockView BlockAb; + class GTSAM_EXPORT JacobianFactor : public GaussianFactor + { + public: + typedef JacobianFactor This; ///< Typedef to this class + typedef GaussianFactor Base; ///< Typedef to base class + typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class + protected: + VerticalBlockMatrix Ab_; // the block view of the full matrix noiseModel::Diagonal::shared_ptr model_; // Gaussian noise model with diagonal covariance matrix - AbMatrix matrix_; // the full matrix corresponding to the factor - BlockAb Ab_; // the block view of the full matrix - typedef GaussianFactor Base; // typedef to base public: - typedef boost::shared_ptr shared_ptr; - typedef BlockAb::Block ABlock; - typedef BlockAb::constBlock constABlock; - typedef BlockAb::Column BVector; - typedef BlockAb::constColumn constBVector; + typedef VerticalBlockMatrix::Block ABlock; + typedef VerticalBlockMatrix::constBlock constABlock; + typedef ABlock::ColXpr BVector; + typedef constABlock::ConstColXpr constBVector; - /** Copy constructor */ - JacobianFactor(const JacobianFactor& gf); /** Convert from other GaussianFactor */ - JacobianFactor(const GaussianFactor& gf); + explicit JacobianFactor(const GaussianFactor& gf); + + /** Copy constructor */ + JacobianFactor(const JacobianFactor& jf) : Base(jf), Ab_(jf.Ab_), model_(jf.model_) {} + + /** Conversion from HessianFactor (does Cholesky to obtain Jacobian matrix) */ + explicit JacobianFactor(const HessianFactor& hf); /** default constructor for I/O */ JacobianFactor(); /** Construct Null factor */ - JacobianFactor(const Vector& b_in); + explicit JacobianFactor(const Vector& b_in); /** Construct unary factor */ - JacobianFactor(Index i1, const Matrix& A1, - const Vector& b, const SharedDiagonal& model); + JacobianFactor(Key i1, const Matrix& A1, + const Vector& b, const SharedDiagonal& model = SharedDiagonal()); /** Construct binary factor */ - JacobianFactor(Index i1, const Matrix& A1, - Index i2, const Matrix& A2, - const Vector& b, const SharedDiagonal& model); + JacobianFactor(Key i1, const Matrix& A1, + Key i2, const Matrix& A2, + const Vector& b, const SharedDiagonal& model = SharedDiagonal()); /** Construct ternary factor */ - JacobianFactor(Index i1, const Matrix& A1, Index i2, - const Matrix& A2, Index i3, const Matrix& A3, - const Vector& b, const SharedDiagonal& model); + JacobianFactor(Key i1, const Matrix& A1, Key i2, + const Matrix& A2, Key i3, const Matrix& A3, + const Vector& b, const SharedDiagonal& model = SharedDiagonal()); - /** Construct an n-ary factor */ - JacobianFactor(const std::vector > &terms, - const Vector &b, const SharedDiagonal& model); + /** Construct an n-ary factor + * @tparam TERMS A container whose value type is std::pair, specifying the + * collection of keys and matrices making up the factor. */ + template + JacobianFactor(const TERMS& terms, const Vector& b, const SharedDiagonal& model = SharedDiagonal()); - JacobianFactor(const std::list > &terms, - const Vector &b, const SharedDiagonal& model); + /** Constructor with arbitrary number keys, and where the augmented matrix is given all together + * instead of in block terms. Note that only the active view of the provided augmented matrix + * is used, and that the matrix data is copied into a newly-allocated matrix in the constructed + * factor. */ + template + JacobianFactor( + const KEYS& keys, const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& sigmas = SharedDiagonal()); - /** Construct from Conditional Gaussian */ - JacobianFactor(const GaussianConditional& cg); - - /** Convert from a HessianFactor (does Cholesky) */ - JacobianFactor(const HessianFactor& factor); - - /** Build a dense joint factor from all the factors in a factor graph. */ - JacobianFactor(const GaussianFactorGraph& gfg); + /** + * Build a dense joint factor from all the factors in a factor graph. If a VariableSlots + * structure computed for \c graph is already available, providing it will reduce the amount of + * computation performed. */ + explicit JacobianFactor( + const GaussianFactorGraph& graph, + boost::optional ordering = boost::none, + boost::optional variableSlots = boost::none); /** Virtual destructor */ virtual ~JacobianFactor() {} - /** Aassignment operator */ - JacobianFactor& operator=(const JacobianFactor& rhs); - /** Clone this JacobianFactor */ virtual GaussianFactor::shared_ptr clone() const { return boost::static_pointer_cast( - shared_ptr(new JacobianFactor(*this))); + boost::make_shared(*this)); } // Implementing Testable interface virtual void print(const std::string& s = "", - const IndexFormatter& formatter = DefaultIndexFormatter) const; + const KeyFormatter& formatter = DefaultKeyFormatter) const; virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const; Vector unweighted_error(const VectorValues& c) const; /** (A*x-b) */ @@ -167,11 +176,46 @@ namespace gtsam { * which in fact stores an augmented information matrix. */ virtual Matrix augmentedInformation() const; - - /** Return the non-augmented information matrix represented by this - * GaussianFactor. - */ - virtual Matrix information() const; + + /** Return the non-augmented information matrix represented by this + * GaussianFactor. + */ + virtual Matrix information() const; + + /// Return the diagonal of the Hessian for this factor + virtual VectorValues hessianDiagonal() const; + + /* ************************************************************************* */ + virtual void hessianDiagonal(double* d) const; + + /// Return the block diagonal of the Hessian for this factor + virtual std::map hessianBlockDiagonal() const; + + /** + * @brief Returns (dense) A,b pair associated with factor, bakes in the weights + */ + virtual std::pair jacobian() const; + + /** + * @brief Returns (dense) A,b pair associated with factor, does not bake in weights + */ + std::pair jacobianUnweighted() const; + + /** Return (dense) matrix associated with factor. The returned system is an augmented matrix: + * [A b] + * weights are baked in */ + virtual Matrix augmentedJacobian() const; + + /** Return (dense) matrix associated with factor. The returned system is an augmented matrix: + * [A b] + * weights are not baked in */ + Matrix augmentedJacobianUnweighted() const; + + /** Return the full augmented Jacobian matrix of this factor as a VerticalBlockMatrix object. */ + const VerticalBlockMatrix& matrixObject() const { return Ab_; } + + /** Mutable access to the full augmented Jacobian matrix of this factor as a VerticalBlockMatrix object. */ + VerticalBlockMatrix& matrixObject() { return Ab_; } /** * Construct the corresponding anti-factor to negate information @@ -180,30 +224,22 @@ namespace gtsam { */ virtual GaussianFactor::shared_ptr negate() const; - /** Check if the factor contains no information, i.e. zero rows. This does - * not necessarily mean that the factor involves no variables (to check for - * involving no variables use keys().empty()). - */ - bool empty() const { return Ab_.rows() == 0;} + /** Check if the factor is empty. TODO: How should this be defined? */ + virtual bool empty() const { return size() == 0 /*|| rows() == 0*/; } /** is noise model constrained ? */ - bool isConstrained() const { return model_->isConstrained();} + bool isConstrained() const { return model_->isConstrained(); } /** Return the dimension of the variable pointed to by the given key iterator * todo: Remove this in favor of keeping track of dimensions with variables? */ - virtual size_t getDim(const_iterator variable) const { return Ab_(variable - begin()).cols(); } + virtual DenseIndex getDim(const_iterator variable) const { return Ab_(variable - begin()).cols(); } /** * return the number of rows in the corresponding linear system */ size_t rows() const { return Ab_.rows(); } - /** - * return the number of rows in the corresponding linear system - */ - size_t numberOfRows() const { return rows(); } - /** * return the number of columns in the corresponding linear system */ @@ -215,17 +251,17 @@ namespace gtsam { /** get a copy of model (non-const version) */ SharedDiagonal& get_model() { return model_; } - /** Get a view of the r.h.s. vector b */ - const constBVector getb() const { return Ab_.column(size(), 0); } + /** Get a view of the r.h.s. vector b, not weighted by noise */ + const constBVector getb() const { return Ab_(size()).col(0); } /** Get a view of the A matrix for the variable pointed to by the given key iterator */ constABlock getA(const_iterator variable) const { return Ab_(variable - begin()); } - /** Get a view of the A matrix */ + /** Get a view of the A matrix, not weighted by noise */ constABlock getA() const { return Ab_.range(0, size()); } /** Get a view of the r.h.s. vector b (non-const version) */ - BVector getb() { return Ab_.column(size(), 0); } + BVector getb() { return Ab_(size()).col(0); } /** Get a view of the A matrix for the variable pointed to by the given key iterator (non-const version) */ ABlock getA(iterator variable) { return Ab_(variable - begin()); } @@ -236,100 +272,71 @@ namespace gtsam { /** Return A*x */ Vector operator*(const VectorValues& x) const; - /** x += A'*e */ + /** x += A'*e. If x is initially missing any values, they are created and assumed to start as + * zero vectors. */ void transposeMultiplyAdd(double alpha, const Vector& e, VectorValues& x) const; - /** - * Return (dense) matrix associated with factor - * @param ordering of variables needed for matrix column order - * @param set weight to true to bake in the weights - */ - std::pair matrix(bool weight = true) const; + /** y += alpha * A'*A*x */ + void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const; - /** - * Return (dense) matrix associated with factor - * The returned system is an augmented matrix: [A b] - * @param set weight to use whitening to bake in weights - */ - Matrix matrix_augmented(bool weight = true) const; + void multiplyHessianAdd(double alpha, const double* x, double* y, std::vector keys) const; - /** - * Return vector of i, j, and s to generate an m-by-n sparse matrix - * such that S(i(k),j(k)) = s(k), which can be given to MATLAB's sparse. - * As above, the standard deviations are baked into A and b - * @param columnIndices First column index for each variable. - */ - std::vector > - sparse(const std::vector& columnIndices) const; + void multiplyHessianAdd(double alpha, const double* x, double* y) const {}; - /** - * Return a whitened version of the factor, i.e. with unit diagonal noise - * model. */ + /// A'*b for Jacobian + VectorValues gradientAtZero() const; + + /** Return a whitened version of the factor, i.e. with unit diagonal noise model. */ JacobianFactor whiten() const; - /** Eliminate the first variable, modifying the factor in place to contain the remaining marginal. */ - boost::shared_ptr eliminateFirst(); + /** Eliminate the requested variables. */ + std::pair, boost::shared_ptr > + eliminate(const Ordering& keys); - /** Eliminate the requested number of frontal variables, modifying the factor in place to contain the remaining marginal. */ - boost::shared_ptr eliminate(size_t nrFrontals = 1); + /** set noiseModel correctly */ + void setModel(bool anyConstrained, const Vector& sigmas); + + /** + * Densely partially eliminate with QR factorization, this is usually provided as an argument to + * one of the factor graph elimination functions (see EliminateableFactorGraph). HessianFactors + * are first factored with Cholesky decomposition to produce JacobianFactors, by calling the + * conversion constructor JacobianFactor(const HessianFactor&). Variables are eliminated in the + * order specified in \c keys. + * @param factors Factors to combine and eliminate + * @param keys The variables to eliminate in the order as specified here in \c keys + * @return The conditional and remaining factor + * + * \addtogroup LinearSolving */ + friend GTSAM_EXPORT std::pair, boost::shared_ptr > + EliminateQR(const GaussianFactorGraph& factors, const Ordering& keys); /** * splits a pre-factorized factor into a conditional, and changes the current * factor to be the remaining component. Performs same operation as eliminate(), * but without running QR. */ - boost::shared_ptr splitConditional(size_t nrFrontals = 1); + boost::shared_ptr splitConditional(size_t nrFrontals); - // following methods all used in CombineJacobians: - // Many imperative, perhaps all need to be combined in constructor - - /** allocate space */ - void allocate(const VariableSlots& variableSlots, - std::vector& varDims, size_t m); - - /** set noiseModel correctly */ - void setModel(bool anyConstrained, const Vector& sigmas); - - /** Assert invariants after elimination */ - void assertInvariants() const; - - /** An exception indicating that the noise model dimension passed into the - * JacobianFactor has a different dimensionality than the factor. */ - class InvalidNoiseModel : public std::exception { - public: - const size_t factorDims; ///< The dimensionality of the factor - const size_t noiseModelDims; ///< The dimensionality of the noise model - - InvalidNoiseModel(size_t factorDims, size_t noiseModelDims) : - factorDims(factorDims), noiseModelDims(noiseModelDims) {} - virtual ~InvalidNoiseModel() throw() {} - - virtual const char* what() const throw(); - - private: - mutable std::string description_; - }; + protected: + /// Internal function to fill blocks and set dimensions + template + void fillTerms(const TERMS& terms, const Vector& b, const SharedDiagonal& noiseModel); + private: - // Friend HessianFactor to facilitate conversion constructors - friend class HessianFactor; - - // Friend unit tests (see also forward declarations above) - friend class ::JacobianFactorCombine2Test; - friend class ::JacobianFactoreliminateFrontalsTest; - friend class ::JacobianFactorconstructor2Test; - /** Serialization function */ friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(GaussianFactor); + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(Ab_); ar & BOOST_SERIALIZATION_NVP(model_); - ar & BOOST_SERIALIZATION_NVP(matrix_); } }; // JacobianFactor } // gtsam +#include + + diff --git a/gtsam/linear/KalmanFilter.cpp b/gtsam/linear/KalmanFilter.cpp index 4a4730e60..68e6a00f1 100644 --- a/gtsam/linear/KalmanFilter.cpp +++ b/gtsam/linear/KalmanFilter.cpp @@ -20,155 +20,139 @@ * @author Frank Dellaert */ -#include -#include #include +#include +#include #include -#include #include #include +#include + +using namespace boost::assign; +using namespace std; namespace gtsam { - using namespace std; +/* ************************************************************************* */ +// Auxiliary function to solve factor graph and return pointer to root conditional +KalmanFilter::State // +KalmanFilter::solve(const GaussianFactorGraph& factorGraph) const { - /// Auxiliary function to solve factor graph and return pointer to root conditional - KalmanFilter::State solve(const GaussianFactorGraph& factorGraph, bool useQR) - { - // Start indices at zero - Index nVars = 0; - internal::Reduction remapping; - BOOST_FOREACH(const GaussianFactorGraph::sharedFactor& factor, factorGraph) - BOOST_FOREACH(Index j, *factor) - if(remapping.insert(make_pair(j, nVars)).second) - ++ nVars; - Permutation inverseRemapping = remapping.inverse(); - GaussianFactorGraph factorGraphOrdered(factorGraph); // NOTE this shares the factors with the original!! - factorGraphOrdered.reduceWithInverse(remapping); + // Eliminate the graph using the provided Eliminate function + Ordering ordering(factorGraph.keys()); + GaussianBayesNet::shared_ptr bayesNet = // + factorGraph.eliminateSequential(ordering, function_); - // Solve the factor graph - GaussianSequentialSolver solver(factorGraphOrdered, useQR); - GaussianBayesNet::shared_ptr bayesNet = solver.eliminate(); + // As this is a filter, all we need is the posterior P(x_t). + // This is the last GaussianConditional in the resulting BayesNet + GaussianConditional::shared_ptr posterior = *(--bayesNet->end()); + return boost::make_shared(*posterior); +} - // As this is a filter, all we need is the posterior P(x_t), - // so we just keep the root of the Bayes net - GaussianConditional::shared_ptr conditional = bayesNet->back(); +/* ************************************************************************* */ +// Auxiliary function to create a small graph for predict or update and solve +KalmanFilter::State // +KalmanFilter::fuse(const State& p, GaussianFactor::shared_ptr newFactor) const { - // Undo the remapping - factorGraphOrdered.permuteWithInverse(inverseRemapping); - conditional->permuteWithInverse(inverseRemapping); + // Create a factor graph + GaussianFactorGraph factorGraph; + factorGraph += p, newFactor; - // TODO: awful ! A copy constructor followed by ANOTHER copy constructor in make_shared? - return boost::make_shared(*conditional); - } + // Eliminate graph in order x0, x1, to get Bayes net P(x0|x1)P(x1) + return solve(factorGraph); +} - /* ************************************************************************* */ - KalmanFilter::State fuse(const KalmanFilter::State& p, - GaussianFactor* newFactor, bool useQR) { +/* ************************************************************************* */ +KalmanFilter::State KalmanFilter::init(const Vector& x0, + const SharedDiagonal& P0) const { - // Create a factor graph - GaussianFactorGraph factorGraph; + // Create a factor graph f(x0), eliminate it into P(x0) + GaussianFactorGraph factorGraph; + factorGraph += JacobianFactor(0, I_, x0, P0); // |x-x0|^2_diagSigma + return solve(factorGraph); +} - // push back previous solution and new factor - factorGraph.push_back(p->toFactor()); - factorGraph.push_back(GaussianFactor::shared_ptr(newFactor)); +/* ************************************************************************* */ +KalmanFilter::State KalmanFilter::init(const Vector& x, const Matrix& P0) const { - // Eliminate graph in order x0, x1, to get Bayes net P(x0|x1)P(x1) - return solve(factorGraph, useQR); - } + // Create a factor graph f(x0), eliminate it into P(x0) + GaussianFactorGraph factorGraph; + factorGraph += HessianFactor(0, x, P0); // 0.5*(x-x0)'*inv(Sigma)*(x-x0) + return solve(factorGraph); +} - /* ************************************************************************* */ - KalmanFilter::State KalmanFilter::init(const Vector& x0, - const SharedDiagonal& P0) { +/* ************************************************************************* */ +void KalmanFilter::print(const string& s) const { + cout << "KalmanFilter " << s << ", dim = " << n_ << endl; +} - // Create a factor graph f(x0), eliminate it into P(x0) - GaussianFactorGraph factorGraph; - factorGraph.add(0, I_, x0, P0); // |x-x0|^2_diagSigma - return solve(factorGraph, useQR()); - } +/* ************************************************************************* */ +KalmanFilter::State KalmanFilter::predict(const State& p, const Matrix& F, + const Matrix& B, const Vector& u, const SharedDiagonal& model) const { - /* ************************************************************************* */ - KalmanFilter::State KalmanFilter::init(const Vector& x, const Matrix& P0) { + // The factor related to the motion model is defined as + // f2(x_{t},x_{t+1}) = (F*x_{t} + B*u - x_{t+1}) * Q^-1 * (F*x_{t} + B*u - x_{t+1})^T + Key k = step(p); + return fuse(p, + boost::make_shared(k, -F, k + 1, I_, B * u, model)); +} - // Create a factor graph f(x0), eliminate it into P(x0) - GaussianFactorGraph factorGraph; - // 0.5*(x-x0)'*inv(Sigma)*(x-x0) - HessianFactor::shared_ptr factor(new HessianFactor(0, x, P0)); - factorGraph.push_back(factor); - return solve(factorGraph, useQR()); - } - - /* ************************************************************************* */ - void KalmanFilter::print(const string& s) const { - cout << "KalmanFilter " << s << ", dim = " << n_ << endl; - } - - /* ************************************************************************* */ - KalmanFilter::State KalmanFilter::predict(const State& p, const Matrix& F, - const Matrix& B, const Vector& u, const SharedDiagonal& model) { - - // The factor related to the motion model is defined as - // f2(x_{t},x_{t+1}) = (F*x_{t} + B*u - x_{t+1}) * Q^-1 * (F*x_{t} + B*u - x_{t+1})^T - Index k = step(p); - return fuse(p, new JacobianFactor(k, -F, k + 1, I_, B * u, model), useQR()); - } - - /* ************************************************************************* */ - KalmanFilter::State KalmanFilter::predictQ(const State& p, const Matrix& F, - const Matrix& B, const Vector& u, const Matrix& Q) { +/* ************************************************************************* */ +KalmanFilter::State KalmanFilter::predictQ(const State& p, const Matrix& F, + const Matrix& B, const Vector& u, const Matrix& Q) const { #ifndef NDEBUG - int n = F.cols(); - assert(F.rows() == n); - assert(B.rows() == n); - assert(B.cols() == u.size()); - assert(Q.rows() == n); - assert(Q.cols() == n); + DenseIndex n = F.cols(); + assert(F.rows() == n); + assert(B.rows() == n); + assert(B.cols() == u.size()); + assert(Q.rows() == n); + assert(Q.cols() == n); #endif - // The factor related to the motion model is defined as - // f2(x_{t},x_{t+1}) = (F*x_{t} + B*u - x_{t+1}) * Q^-1 * (F*x_{t} + B*u - x_{t+1})^T - // See documentation in HessianFactor, we have A1 = -F, A2 = I_, b = B*u: - // TODO: starts to seem more elaborate than straight-up KF equations? - Matrix M = inverse(Q), Ft = trans(F); - Matrix G12 = -Ft * M, G11 = -G12 * F, G22 = M; - Vector b = B * u, g2 = M * b, g1 = -Ft * g2; - double f = dot(b, g2); - Index k = step(p); - return fuse(p, new HessianFactor(k, k + 1, G11, G12, g1, G22, g2, f), - useQR()); - } + // The factor related to the motion model is defined as + // f2(x_{t},x_{t+1}) = (F*x_{t} + B*u - x_{t+1}) * Q^-1 * (F*x_{t} + B*u - x_{t+1})^T + // See documentation in HessianFactor, we have A1 = -F, A2 = I_, b = B*u: + // TODO: starts to seem more elaborate than straight-up KF equations? + Matrix M = inverse(Q), Ft = trans(F); + Matrix G12 = -Ft * M, G11 = -G12 * F, G22 = M; + Vector b = B * u, g2 = M * b, g1 = -Ft * g2; + double f = dot(b, g2); + Key k = step(p); + return fuse(p, + boost::make_shared(k, k + 1, G11, G12, g1, G22, g2, f)); +} - /* ************************************************************************* */ - KalmanFilter::State KalmanFilter::predict2(const State& p, const Matrix& A0, - const Matrix& A1, const Vector& b, const SharedDiagonal& model) { - // Nhe factor related to the motion model is defined as - // f2(x_{t},x_{t+1}) = |A0*x_{t} + A1*x_{t+1} - b|^2 - Index k = step(p); - return fuse(p, new JacobianFactor(k, A0, k + 1, A1, b, model), useQR()); - } +/* ************************************************************************* */ +KalmanFilter::State KalmanFilter::predict2(const State& p, const Matrix& A0, + const Matrix& A1, const Vector& b, const SharedDiagonal& model) const { + // Nhe factor related to the motion model is defined as + // f2(x_{t},x_{t+1}) = |A0*x_{t} + A1*x_{t+1} - b|^2 + Key k = step(p); + return fuse(p, boost::make_shared(k, A0, k + 1, A1, b, model)); +} - /* ************************************************************************* */ - KalmanFilter::State KalmanFilter::update(const State& p, const Matrix& H, - const Vector& z, const SharedDiagonal& model) { - // The factor related to the measurements would be defined as - // f2 = (h(x_{t}) - z_{t}) * R^-1 * (h(x_{t}) - z_{t})^T - // = (x_{t} - z_{t}) * R^-1 * (x_{t} - z_{t})^T - Index k = step(p); - return fuse(p, new JacobianFactor(k, H, z, model), useQR()); - } +/* ************************************************************************* */ +KalmanFilter::State KalmanFilter::update(const State& p, const Matrix& H, + const Vector& z, const SharedDiagonal& model) const { + // The factor related to the measurements would be defined as + // f2 = (h(x_{t}) - z_{t}) * R^-1 * (h(x_{t}) - z_{t})^T + // = (x_{t} - z_{t}) * R^-1 * (x_{t} - z_{t})^T + Key k = step(p); + return fuse(p, boost::make_shared(k, H, z, model)); +} - /* ************************************************************************* */ - KalmanFilter::State KalmanFilter::updateQ(const State& p, const Matrix& H, const Vector& z, - const Matrix& Q) { - Index k = step(p); - Matrix M = inverse(Q), Ht = trans(H); - Matrix G = Ht * M * H; - Vector g = Ht * M * z; - double f = dot(z, M * z); - return fuse(p, new HessianFactor(k, G, g, f), useQR()); - } +/* ************************************************************************* */ +KalmanFilter::State KalmanFilter::updateQ(const State& p, const Matrix& H, + const Vector& z, const Matrix& Q) const { + Key k = step(p); + Matrix M = inverse(Q), Ht = trans(H); + Matrix G = Ht * M * H; + Vector g = Ht * M * z; + double f = dot(z, M * z); + return fuse(p, boost::make_shared(k, G, g, f)); +} /* ************************************************************************* */ diff --git a/gtsam/linear/KalmanFilter.h b/gtsam/linear/KalmanFilter.h index 0089bd34d..63271401c 100644 --- a/gtsam/linear/KalmanFilter.h +++ b/gtsam/linear/KalmanFilter.h @@ -20,6 +20,7 @@ #pragma once #include +#include #include #ifndef KALMANFILTER_DEFAULT_FACTORIZATION @@ -28,122 +29,122 @@ namespace gtsam { +/** + * Kalman Filter class + * + * Knows how to maintain a Gaussian density under linear-Gaussian motion and + * measurement models. It uses the square-root information form, as usual in GTSAM. + * + * The filter is functional, in that it does not have state: you call init() to create + * an initial state, then predict() and update() that create new states out of an old state. + */ +class GTSAM_EXPORT KalmanFilter { + +public: + /** - * Kalman Filter class - * - * Knows how to maintain a Gaussian density under linear-Gaussian motion and - * measurement models. It uses the square-root information form, as usual in GTSAM. - * - * The filter is functional, in that it does not have state: you call init() to create - * an initial state, then predict() and update() that create new states out of old. + * This Kalman filter is a Square-root Information filter + * The type below allows you to specify the factorization variant. */ - class GTSAM_EXPORT KalmanFilter { - - public: - - /** - * This Kalman filter is a Square-root Information filter - * The type below allows you to specify the factorization variant. - */ - enum Factorization { - QR, CHOLESKY - }; - - /** - * The Kalman filter state is simply a GaussianDensity - */ - typedef GaussianDensity::shared_ptr State; - - private: - - const size_t n_; /** dimensionality of state */ - const Matrix I_; /** identity matrix of size n*n */ - const Factorization method_; /** algorithm */ - - bool useQR() const { - return method_ == QR; - } - - public: - - // private constructor - KalmanFilter(size_t n, Factorization method = - KALMANFILTER_DEFAULT_FACTORIZATION) : - n_(n), I_(eye(n_, n_)), method_(method) { - } - - /** - * Create initial state, i.e., prior density at time k=0 - * In Kalman Filter notation, this are is x_{0|0} and P_{0|0} - * @param x0 estimate at time 0 - * @param P0 covariance at time 0, given as a diagonal Gaussian 'model' - */ - State init(const Vector& x0, const SharedDiagonal& P0); - - /// version of init with a full covariance matrix - State init(const Vector& x0, const Matrix& P0); - - /// print - void print(const std::string& s = "") const; - - /** Return step index k, starts at 0, incremented at each predict. */ - static Index step(const State& p) { - return p->firstFrontalKey(); - } - - /** - * Predict the state P(x_{t+1}|Z^t) - * In Kalman Filter notation, this is x_{t+1|t} and P_{t+1|t} - * After the call, that is the density that can be queried. - * Details and parameters: - * In a linear Kalman Filter, the motion model is f(x_{t}) = F*x_{t} + B*u_{t} + w - * where F is the state transition model/matrix, B is the control input model, - * and w is zero-mean, Gaussian white noise with covariance Q. - */ - State predict(const State& p, const Matrix& F, const Matrix& B, - const Vector& u, const SharedDiagonal& modelQ); - - /* - * Version of predict with full covariance - * Q is normally derived as G*w*G^T where w models uncertainty of some - * physical property, such as velocity or acceleration, and G is derived from physics. - * This version allows more realistic models than a diagonal covariance matrix. - */ - State predictQ(const State& p, const Matrix& F, const Matrix& B, - const Vector& u, const Matrix& Q); - - /** - * Predict the state P(x_{t+1}|Z^t) - * In Kalman Filter notation, this is x_{t+1|t} and P_{t+1|t} - * After the call, that is the density that can be queried. - * Details and parameters: - * This version of predict takes GaussianFactor motion model [A0 A1 b] - * with an optional noise model. - */ - State predict2(const State& p, const Matrix& A0, const Matrix& A1, - const Vector& b, const SharedDiagonal& model); - - /** - * Update Kalman filter with a measurement - * For the Kalman Filter, the measurement function, h(x_{t}) = z_{t} - * will be of the form h(x_{t}) = H*x_{t} + v - * where H is the observation model/matrix, and v is zero-mean, - * Gaussian white noise with covariance R. - * Currently, R is restricted to diagonal Gaussians (model parameter) - */ - State update(const State& p, const Matrix& H, const Vector& z, - const SharedDiagonal& model); - - /* - * Version of update with full covariance - * Q is normally derived as G*w*G^T where w models uncertainty of some - * physical property, such as velocity or acceleration, and G is derived from physics. - * This version allows more realistic models than a diagonal covariance matrix. - */ - State updateQ(const State& p, const Matrix& H, const Vector& z, - const Matrix& Q); + enum Factorization { + QR, CHOLESKY }; + /** + * The Kalman filter state is simply a GaussianDensity + */ + typedef GaussianDensity::shared_ptr State; + +private: + + const size_t n_; /** dimensionality of state */ + const Matrix I_; /** identity matrix of size n*n */ + const GaussianFactorGraph::Eliminate function_; /** algorithm */ + + State solve(const GaussianFactorGraph& factorGraph) const; + State fuse(const State& p, GaussianFactor::shared_ptr newFactor) const; + +public: + + // Constructor + KalmanFilter(size_t n, Factorization method = + KALMANFILTER_DEFAULT_FACTORIZATION) : + n_(n), I_(eye(n_, n_)), function_( + method == QR ? GaussianFactorGraph::Eliminate(EliminateQR) : + GaussianFactorGraph::Eliminate(EliminateCholesky)) { + } + + /** + * Create initial state, i.e., prior density at time k=0 + * In Kalman Filter notation, these are x_{0|0} and P_{0|0} + * @param x0 estimate at time 0 + * @param P0 covariance at time 0, given as a diagonal Gaussian 'model' + */ + State init(const Vector& x0, const SharedDiagonal& P0) const; + + /// version of init with a full covariance matrix + State init(const Vector& x0, const Matrix& P0) const; + + /// print + void print(const std::string& s = "") const; + + /** Return step index k, starts at 0, incremented at each predict. */ + static Key step(const State& p) { + return p->firstFrontalKey(); + } + + /** + * Predict the state P(x_{t+1}|Z^t) + * In Kalman Filter notation, this is x_{t+1|t} and P_{t+1|t} + * Details and parameters: + * In a linear Kalman Filter, the motion model is f(x_{t}) = F*x_{t} + B*u_{t} + w + * where F is the state transition model/matrix, B is the control input model, + * and w is zero-mean, Gaussian white noise with covariance Q. + */ + State predict(const State& p, const Matrix& F, const Matrix& B, + const Vector& u, const SharedDiagonal& modelQ) const; + + /* + * Version of predict with full covariance + * Q is normally derived as G*w*G^T where w models uncertainty of some + * physical property, such as velocity or acceleration, and G is derived from physics. + * This version allows more realistic models than a diagonal covariance matrix. + */ + State predictQ(const State& p, const Matrix& F, const Matrix& B, + const Vector& u, const Matrix& Q) const; + + /** + * Predict the state P(x_{t+1}|Z^t) + * In Kalman Filter notation, this is x_{t+1|t} and P_{t+1|t} + * After the call, that is the density that can be queried. + * Details and parameters: + * This version of predict takes GaussianFactor motion model [A0 A1 b] + * with an optional noise model. + */ + State predict2(const State& p, const Matrix& A0, const Matrix& A1, + const Vector& b, const SharedDiagonal& model) const; + + /** + * Update Kalman filter with a measurement + * For the Kalman Filter, the measurement function, h(x_{t}) = z_{t} + * will be of the form h(x_{t}) = H*x_{t} + v + * where H is the observation model/matrix, and v is zero-mean, + * Gaussian white noise with covariance R. + * In this version, R is restricted to diagonal Gaussians (model parameter) + */ + State update(const State& p, const Matrix& H, const Vector& z, + const SharedDiagonal& model) const; + + /* + * Version of update with full covariance + * Q is normally derived as G*w*G^T where w models uncertainty of some + * physical property, such as velocity or acceleration, and G is derived from physics. + * This version allows more realistic models than a diagonal covariance matrix. + */ + State updateQ(const State& p, const Matrix& H, const Vector& z, + const Matrix& Q) const; +}; + } // \namespace gtsam /* ************************************************************************* */ diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 16b0a5bc1..641b47640 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -106,16 +106,23 @@ void Gaussian::WhitenInPlace(Matrix& H) const { H = thisR() * H; } +/* ************************************************************************* */ +void Gaussian::WhitenInPlace(Eigen::Block H) const { + H = thisR() * H; +} + /* ************************************************************************* */ // General QR, see also special version in Constrained SharedDiagonal Gaussian::QR(Matrix& Ab) const { + gttic(Gaussian_noise_model_QR); + static const bool debug = false; // get size(A) and maxRank // TODO: really no rank problems ? - size_t m = Ab.rows(), n = Ab.cols()-1; - size_t maxRank = min(m,n); + // size_t m = Ab.rows(), n = Ab.cols()-1; + // size_t maxRank = min(m,n); // pre-whiten everything (cheaply if possible) WhitenInPlace(Ab); @@ -127,9 +134,9 @@ SharedDiagonal Gaussian::QR(Matrix& Ab) const { // hand-coded householder implementation // TODO: necessary to isolate last column? -// householder(Ab, maxRank); + // householder(Ab, maxRank); - return Unit::Create(maxRank); + return SharedDiagonal(); } void Gaussian::WhitenSystem(vector& A, Vector& b) const { @@ -159,9 +166,11 @@ void Gaussian::WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const // Diagonal /* ************************************************************************* */ Diagonal::Diagonal() : - Gaussian(1), sigmas_(ones(1)), invsigmas_(ones(1)), precisions_(ones(1)) { + Gaussian(1)//, sigmas_(ones(1)), invsigmas_(ones(1)), precisions_(ones(1)) +{ } +/* ************************************************************************* */ Diagonal::Diagonal(const Vector& sigmas) : Gaussian(sigmas.size()), sigmas_(sigmas), invsigmas_(reciprocal(sigmas)), precisions_( emul(invsigmas_, invsigmas_)) { @@ -171,7 +180,7 @@ Diagonal::Diagonal(const Vector& sigmas) : Diagonal::shared_ptr Diagonal::Variances(const Vector& variances, bool smart) { if (smart) { // check whether all the same entry - int j, n = variances.size(); + DenseIndex j, n = variances.size(); for (j = 1; j < n; j++) if (variances(j) != variances(0)) goto full; return Isotropic::Variance(n, variances(0), true); @@ -215,8 +224,38 @@ void Diagonal::WhitenInPlace(Matrix& H) const { vector_scale_inplace(invsigmas(), H); } +/* ************************************************************************* */ +void Diagonal::WhitenInPlace(Eigen::Block H) const { + H = invsigmas().asDiagonal() * H; +} + /* ************************************************************************* */ // Constrained +/* ************************************************************************* */ + +/* ************************************************************************* */ +Constrained::Constrained(const Vector& sigmas) + : Diagonal(sigmas), mu_(repeat(sigmas.size(), 1000.0)) { + for (int i=0; i= b.size() + Vector c = a; + for( DenseIndex i = 0; i < b.size(); i++ ) { const double& ai = a(i), &bi = b(i); - c(i) = (bi==0.0) ? ai : ai/bi; // NOTE: not ediv_() + if (bi!=0) c(i) = ai/bi; } return c; } @@ -257,19 +296,20 @@ Vector Constrained::whiten(const Vector& v) const { double Constrained::distance(const Vector& v) const { Vector w = Diagonal::whiten(v); // get noisemodel for constrained elements // TODO Find a better way of doing these checks - for (size_t i=0; i H) const { + // selective scaling + // Scale row i of H by sigmas[i], basically multiplying H with diag(sigmas) + // Set inf_mask flag is true so that if invsigmas[i] is inf, i.e. sigmas[i] = 0, + // indicating a hard constraint, we leave H's row i in place. + const Vector& _invsigmas = invsigmas(); + for(DenseIndex row = 0; row < _invsigmas.size(); ++row) + if(isfinite(_invsigmas(row))) + H.row(row) *= _invsigmas(row); +} + +/* ************************************************************************* */ +Constrained::shared_ptr Constrained::unit(size_t augmentedDim) const { + Vector sigmas = ones(dim()+augmentedDim); for (size_t i=0; isigmas_(i) == 0.0) sigmas(i) = 0.0; - return MixedSigmas(mu_, sigmas); + Vector augmentedMu = zero(dim()+augmentedDim); + subInsert(augmentedMu, mu_, 0); + return MixedSigmas(augmentedMu, sigmas); } /* ************************************************************************* */ @@ -308,6 +370,8 @@ SharedDiagonal Constrained::QR(Matrix& Ab) const { list Rd; Vector pseudo(m); // allocate storage for pseudo-inverse + Vector invsigmas = reciprocal(sigmas_); + Vector weights = emul(invsigmas,invsigmas); // calculate weights once // We loop over all columns, because the columns that can be eliminated // are not necessarily contiguous. For each one, estimate the corresponding @@ -319,7 +383,7 @@ SharedDiagonal Constrained::QR(Matrix& Ab) const { // Calculate weighted pseudo-inverse and corresponding precision gttic(constrained_QR_weightedPseudoinverse); - double precision = weightedPseudoinverse(a, precisions_, pseudo); + double precision = weightedPseudoinverse(a, weights, pseudo); gttoc(constrained_QR_weightedPseudoinverse); // If precision is zero, no information on this column @@ -416,6 +480,11 @@ void Isotropic::WhitenInPlace(Matrix& H) const { H *= invsigma_; } +/* ************************************************************************* */ +void Isotropic::WhitenInPlace(Eigen::Block H) const { + H *= invsigma_; +} + /* ************************************************************************* */ // Unit /* ************************************************************************* */ @@ -595,6 +664,36 @@ Huber::shared_ptr Huber::Create(const double c, const ReweightScheme reweight) { return shared_ptr(new Huber(c, reweight)); } +/* ************************************************************************* */ +// Cauchy +/* ************************************************************************* */ + +Cauchy::Cauchy(const double k, const ReweightScheme reweight) + : Base(reweight), k_(k) { + if ( k_ <= 0 ) { + cout << "mEstimator Cauchy takes only positive double in constructor. forced to 1.0" << endl; + k_ = 1.0; + } +} + +double Cauchy::weight(const double &error) const { + return k_*k_ / (k_*k_ + error*error); +} + +void Cauchy::print(const std::string &s="") const { + cout << s << "cauchy (" << k_ << ")" << endl; +} + +bool Cauchy::equals(const Base &expected, const double tol) const { + const Cauchy* p = dynamic_cast(&expected); + if (p == NULL) return false; + return fabs(k_ - p->k_) < tol; +} + +Cauchy::shared_ptr Cauchy::Create(const double c, const ReweightScheme reweight) { + return shared_ptr(new Cauchy(c, reweight)); +} + /* ************************************************************************* */ // Tukey /* ************************************************************************* */ @@ -625,6 +724,32 @@ Tukey::shared_ptr Tukey::Create(const double c, const ReweightScheme reweight) { return shared_ptr(new Tukey(c, reweight)); } +/* ************************************************************************* */ +// Welsh +/* ************************************************************************* */ +Welsh::Welsh(const double c, const ReweightScheme reweight) + : Base(reweight), c_(c) { +} + +double Welsh::weight(const double &error) const { + double xc2 = (error/c_)*(error/c_); + return std::exp(-xc2); +} + +void Welsh::print(const std::string &s="") const { + std::cout << s << ": Welsh (" << c_ << ")" << std::endl; +} + +bool Welsh::equals(const Base &expected, const double tol) const { + const Welsh* p = dynamic_cast(&expected); + if (p == NULL) return false; + return fabs(c_ - p->c_) < tol; +} + +Welsh::shared_ptr Welsh::Create(const double c, const ReweightScheme reweight) { + return shared_ptr(new Welsh(c, reweight)); +} + } // namespace mEstimator /* ************************************************************************* */ @@ -672,7 +797,6 @@ Robust::shared_ptr Robust::Create( return shared_ptr(new Robust(robust,noise)); } - /* ************************************************************************* */ } diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 37e5f0090..a87f426fa 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -93,6 +93,16 @@ namespace gtsam { v = unwhiten(v); } + /** in-place whiten, override if can be done more efficiently */ + virtual void whitenInPlace(Eigen::Block& v) const { + v = whiten(v); + } + + /** in-place unwhiten, override if can be done more efficiently */ + virtual void unwhitenInPlace(Eigen::Block& v) const { + v = unwhiten(v); + } + private: /** Serialization function */ friend class boost::serialization::access; @@ -186,6 +196,11 @@ namespace gtsam { */ virtual void WhitenInPlace(Matrix& H) const; + /** + * In-place version + */ + virtual void WhitenInPlace(Eigen::Block H) const; + /** * Whiten a system, in place as well */ @@ -243,10 +258,10 @@ namespace gtsam { Vector sigmas_, invsigmas_, precisions_; protected: - /** protected constructor takes sigmas */ + /** protected constructor - no initializations */ Diagonal(); - /** constructor to allow for disabling initializaion of invsigmas */ + /** constructor to allow for disabling initialization of invsigmas */ Diagonal(const Vector& sigmas); public: @@ -257,7 +272,7 @@ namespace gtsam { /** * A diagonal noise model created by specifying a Vector of sigmas, i.e. - * standard devations, the diagonal of the square root covariance matrix. + * standard deviations, the diagonal of the square root covariance matrix. */ static shared_ptr Sigmas(const Vector& sigmas, bool smart = true); @@ -282,6 +297,7 @@ namespace gtsam { virtual Vector unwhiten(const Vector& v) const; virtual Matrix Whiten(const Matrix& H) const; virtual void WhitenInPlace(Matrix& H) const; + virtual void WhitenInPlace(Eigen::Block H) const; /** * Return standard deviations (sqrt of diagonal) @@ -337,17 +353,22 @@ namespace gtsam { protected: // Sigmas are contained in the base class + Vector mu_; ///< Penalty function weight - needs to be large enough to dominate soft constraints - // Penalty function parameters - Vector mu_; + /** + * protected constructor takes sigmas. + * prevents any inf values + * from appearing in invsigmas or precisions. + * mu set to large default value (1000.0) + */ + Constrained(const Vector& sigmas = zero(1)); - /** protected constructor takes sigmas */ - Constrained(const Vector& sigmas = zero(1)) : - Diagonal(sigmas), mu_(repeat(sigmas.size(), 1000.0)) {} - - // allows for specifying mu - Constrained(const Vector& mu, const Vector& sigmas) : - Diagonal(sigmas), mu_(mu) {} + /** + * Constructor that prevents any inf values + * from appearing in invsigmas or precisions. + * Allows for specifying mu. + */ + Constrained(const Vector& mu, const Vector& sigmas); public: @@ -435,6 +456,7 @@ namespace gtsam { /// with a non-zero sigma. Other rows remain untouched. virtual Matrix Whiten(const Matrix& H) const; virtual void WhitenInPlace(Matrix& H) const; + virtual void WhitenInPlace(Eigen::Block H) const; /** * Apply QR factorization to the system [A b], taking into account constraints @@ -450,8 +472,9 @@ namespace gtsam { /** * Returns a Unit version of a constrained noisemodel in which * constrained sigmas remain constrained and the rest are unit scaled + * Now support augmented part from the Lagrange multiplier. */ - shared_ptr unit() const; + shared_ptr unit(size_t augmentedDim = 0) const; private: /** Serialization function */ @@ -513,6 +536,7 @@ namespace gtsam { virtual Vector unwhiten(const Vector& v) const; virtual Matrix Whiten(const Matrix& H) const; virtual void WhitenInPlace(Matrix& H) const; + virtual void WhitenInPlace(Eigen::Block H) const; /** * Return standard deviation @@ -560,6 +584,11 @@ namespace gtsam { virtual Vector unwhiten(const Vector& v) const { return v; } virtual Matrix Whiten(const Matrix& H) const { return H; } virtual void WhitenInPlace(Matrix& H) const {} + virtual void WhitenInPlace(Eigen::Block H) const {} + virtual void whitenInPlace(Vector& v) const {} + virtual void unwhitenInPlace(Vector& v) const {} + virtual void whitenInPlace(Eigen::Block& v) const {} + virtual void unwhitenInPlace(Eigen::Block& v) const {} private: /** Serialization function */ @@ -693,6 +722,35 @@ namespace gtsam { } }; + /// Cauchy implements the "Cauchy" robust error model (Lee2013IROS). Contributed by: + /// Dipl.-Inform. Jan Oberl�nder (M.Sc.), FZI Research Center for + /// Information Technology, Karlsruhe, Germany. + /// oberlaender@fzi.de + /// Thanks Jan! + class GTSAM_EXPORT Cauchy : public Base { + public: + typedef boost::shared_ptr shared_ptr; + + virtual ~Cauchy() {} + Cauchy(const double k = 0.1, const ReweightScheme reweight = Block); + virtual double weight(const double &error) const ; + virtual void print(const std::string &s) const ; + virtual bool equals(const Base& expected, const double tol=1e-8) const ; + static shared_ptr Create(const double k, const ReweightScheme reweight = Block) ; + + protected: + double k_; + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar & BOOST_SERIALIZATION_NVP(k_); + } + }; + /// Tukey implements the "Tukey" robust error model (Zhang97ivc) class GTSAM_EXPORT Tukey : public Base { public: @@ -718,6 +776,31 @@ namespace gtsam { } }; + /// Welsh implements the "Welsh" robust error model (Zhang97ivc) + class GTSAM_EXPORT Welsh : public Base { + public: + typedef boost::shared_ptr shared_ptr; + + Welsh(const double c = 2.9846, const ReweightScheme reweight = Block); + virtual ~Welsh() {} + virtual double weight(const double &error) const ; + virtual void print(const std::string &s) const ; + virtual bool equals(const Base& expected, const double tol=1e-8) const ; + static shared_ptr Create(const double k, const ReweightScheme reweight = Block) ; + + protected: + double c_; + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar & BOOST_SERIALIZATION_NVP(c_); + } + }; + } ///\namespace mEstimator /// Base class for robust error models @@ -790,6 +873,7 @@ namespace gtsam { typedef noiseModel::Base::shared_ptr SharedNoiseModel; typedef noiseModel::Gaussian::shared_ptr SharedGaussian; typedef noiseModel::Diagonal::shared_ptr SharedDiagonal; + typedef noiseModel::Constrained::shared_ptr SharedConstrained; } // namespace gtsam diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp index c12838fbd..44cb8c146 100644 --- a/gtsam/linear/SubgraphPreconditioner.cpp +++ b/gtsam/linear/SubgraphPreconditioner.cpp @@ -17,103 +17,104 @@ #include #include +#include #include using namespace std; namespace gtsam { - - /* ************************************************************************* */ - static GaussianFactorGraph::shared_ptr convertToJacobianFactors(const GaussianFactorGraph &gfg) { - GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph()); - BOOST_FOREACH(const GaussianFactor::shared_ptr &gf, gfg) { + + /* ************************************************************************* */ + static GaussianFactorGraph::shared_ptr convertToJacobianFactors(const GaussianFactorGraph &gfg) { + GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph()); + BOOST_FOREACH(const GaussianFactor::shared_ptr &gf, gfg) { JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast(gf); if( !jf ) { jf = boost::make_shared(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky) } - result->push_back(jf); - } - return result; + result->push_back(jf); + } + return result; } /* ************************************************************************* */ SubgraphPreconditioner::SubgraphPreconditioner(const sharedFG& Ab2, const sharedBayesNet& Rc1, const sharedValues& xbar) : - Ab2_(convertToJacobianFactors(*Ab2)), Rc1_(Rc1), xbar_(xbar), b2bar_(new Errors(-gaussianErrors(*Ab2_,*xbar))) { + Ab2_(convertToJacobianFactors(*Ab2)), Rc1_(Rc1), xbar_(xbar), b2bar_(new Errors(-Ab2_->gaussianErrors(*xbar))) { } /* ************************************************************************* */ // x = xbar + inv(R1)*y VectorValues SubgraphPreconditioner::x(const VectorValues& y) const { - return *xbar_ + gtsam::backSubstitute(*Rc1_, y); + return *xbar_ + Rc1_->backSubstitute(y); } /* ************************************************************************* */ - double error(const SubgraphPreconditioner& sp, const VectorValues& y) { + double SubgraphPreconditioner::error(const VectorValues& y) const { Errors e(y); - VectorValues x = sp.x(y); - Errors e2 = gaussianErrors(*sp.Ab2(),x); + VectorValues x = this->x(y); + Errors e2 = Ab2()->gaussianErrors(x); return 0.5 * (dot(e, e) + dot(e2,e2)); } /* ************************************************************************* */ // gradient is y + inv(R1')*A2'*(A2*inv(R1)*y-b2bar), - VectorValues gradient(const SubgraphPreconditioner& sp, const VectorValues& y) { - VectorValues x = gtsam::backSubstitute(*sp.Rc1(), y); /* inv(R1)*y */ - Errors e = (*sp.Ab2()*x - *sp.b2bar()); /* (A2*inv(R1)*y-b2bar) */ + VectorValues SubgraphPreconditioner::gradient(const VectorValues& y) const { + VectorValues x = Rc1()->backSubstitute(y); /* inv(R1)*y */ + Errors e = (*Ab2()*x - *b2bar()); /* (A2*inv(R1)*y-b2bar) */ VectorValues v = VectorValues::Zero(x); - transposeMultiplyAdd(*sp.Ab2(), 1.0, e, v); /* A2'*(A2*inv(R1)*y-b2bar) */ - return y + gtsam::backSubstituteTranspose(*sp.Rc1(), v); + Ab2()->transposeMultiplyAdd(1.0, e, v); /* A2'*(A2*inv(R1)*y-b2bar) */ + return y + Rc1()->backSubstituteTranspose(v); } /* ************************************************************************* */ // Apply operator A, A*y = [I;A2*inv(R1)]*y = [y; A2*inv(R1)*y] - Errors operator*(const SubgraphPreconditioner& sp, const VectorValues& y) { + Errors SubgraphPreconditioner::operator*(const VectorValues& y) const { Errors e(y); - VectorValues x = gtsam::backSubstitute(*sp.Rc1(), y); /* x=inv(R1)*y */ - Errors e2 = *sp.Ab2() * x; /* A2*x */ + VectorValues x = Rc1()->backSubstitute(y); /* x=inv(R1)*y */ + Errors e2 = *Ab2() * x; /* A2*x */ e.splice(e.end(), e2); return e; } /* ************************************************************************* */ // In-place version that overwrites e - void multiplyInPlace(const SubgraphPreconditioner& sp, const VectorValues& y, Errors& e) { + void SubgraphPreconditioner::multiplyInPlace(const VectorValues& y, Errors& e) const { Errors::iterator ei = e.begin(); - for ( Index i = 0 ; i < y.size() ; ++i, ++ei ) { + for ( Key i = 0 ; i < y.size() ; ++i, ++ei ) { *ei = y[i]; } // Add A2 contribution - VectorValues x = gtsam::backSubstitute(*sp.Rc1(), y); // x=inv(R1)*y - gtsam::multiplyInPlace(*sp.Ab2(), x, ei); // use iterator version + VectorValues x = Rc1()->backSubstitute(y); // x=inv(R1)*y + Ab2()->multiplyInPlace(x, ei); // use iterator version } /* ************************************************************************* */ // Apply operator A', A'*e = [I inv(R1')*A2']*e = e1 + inv(R1')*A2'*e2 - VectorValues operator^(const SubgraphPreconditioner& sp, const Errors& e) { + VectorValues SubgraphPreconditioner::operator^(const Errors& e) const { Errors::const_iterator it = e.begin(); - VectorValues y = sp.zero(); - for ( Index i = 0 ; i < y.size() ; ++i, ++it ) + VectorValues y = zero(); + for ( Key i = 0 ; i < y.size() ; ++i, ++it ) y[i] = *it ; - sp.transposeMultiplyAdd2(1.0,it,e.end(),y); + transposeMultiplyAdd2(1.0,it,e.end(),y); return y; } /* ************************************************************************* */ // y += alpha*A'*e - void transposeMultiplyAdd - (const SubgraphPreconditioner& sp, double alpha, const Errors& e, VectorValues& y) { + void SubgraphPreconditioner::transposeMultiplyAdd + (double alpha, const Errors& e, VectorValues& y) const { Errors::const_iterator it = e.begin(); - for ( Index i = 0 ; i < y.size() ; ++i, ++it ) { + for ( Key i = 0 ; i < y.size() ; ++i, ++it ) { const Vector& ei = *it; axpy(alpha, ei, y[i]); } - sp.transposeMultiplyAdd2(alpha, it, e.end(), y); + transposeMultiplyAdd2(alpha, it, e.end(), y); } /* ************************************************************************* */ @@ -127,8 +128,8 @@ namespace gtsam { while (it != end) e2.push_back(*(it++)); VectorValues x = VectorValues::Zero(y); // x = 0 - gtsam::transposeMultiplyAdd(*Ab2_,1.0,e2,x); // x += A2'*e2 - axpy(alpha, gtsam::backSubstituteTranspose(*Rc1_, x), y); // y += alpha*inv(R1')*x + Ab2_->transposeMultiplyAdd(1.0,e2,x); // x += A2'*e2 + axpy(alpha, Rc1_->backSubstituteTranspose(x), y); // y += alpha*inv(R1')*x } /* ************************************************************************* */ diff --git a/gtsam/linear/SubgraphPreconditioner.h b/gtsam/linear/SubgraphPreconditioner.h index 4396a59dc..d9d7524b6 100644 --- a/gtsam/linear/SubgraphPreconditioner.h +++ b/gtsam/linear/SubgraphPreconditioner.h @@ -17,12 +17,19 @@ #pragma once -#include -#include -#include +#include +#include +#include + +#include namespace gtsam { + // Forward declarations + class GaussianBayesNet; + class GaussianFactorGraph; + class VectorValues; + /** * Subgraph conditioner class, as explained in the RSS 2010 submission. * Starting with a graph A*x=b, we split it in two systems A1*x=b1 and A2*x=b2 @@ -57,6 +64,9 @@ namespace gtsam { */ SubgraphPreconditioner(const sharedFG& Ab2, const sharedBayesNet& Rc1, const sharedValues& xbar); + /** print the object */ + void print(const std::string& s = "SubgraphPreconditioner") const; + /** Access Ab2 */ const sharedFG& Ab2() const { return Ab2_; } @@ -89,29 +99,26 @@ namespace gtsam { void transposeMultiplyAdd2(double alpha, Errors::const_iterator begin, Errors::const_iterator end, VectorValues& y) const; - /** print the object */ - void print(const std::string& s = "SubgraphPreconditioner") const; - }; + /* error, given y */ + double error(const VectorValues& y) const; - /* error, given y */ - GTSAM_EXPORT double error(const SubgraphPreconditioner& sp, const VectorValues& y); + /** gradient = y + inv(R1')*A2'*(A2*inv(R1)*y-b2bar) */ + VectorValues gradient(const VectorValues& y) const; - /** gradient = y + inv(R1')*A2'*(A2*inv(R1)*y-b2bar) */ - GTSAM_EXPORT VectorValues gradient(const SubgraphPreconditioner& sp, const VectorValues& y); + /** Apply operator A */ + Errors operator*(const VectorValues& y) const; - /** Apply operator A */ - GTSAM_EXPORT Errors operator*(const SubgraphPreconditioner& sp, const VectorValues& y); - - /** Apply operator A in place: needs e allocated already */ - GTSAM_EXPORT void multiplyInPlace(const SubgraphPreconditioner& sp, const VectorValues& y, Errors& e); + /** Apply operator A in place: needs e allocated already */ + void multiplyInPlace(const VectorValues& y, Errors& e) const; /** Apply operator A' */ - GTSAM_EXPORT VectorValues operator^(const SubgraphPreconditioner& sp, const Errors& e); + VectorValues operator^(const Errors& e) const; - /** - * Add A'*e to y - * y += alpha*A'*[e1;e2] = [alpha*e1; alpha*inv(R1')*A2'*e2] - */ - GTSAM_EXPORT void transposeMultiplyAdd(const SubgraphPreconditioner& sp, double alpha, const Errors& e, VectorValues& y); + /** + * Add A'*e to y + * y += alpha*A'*[e1;e2] = [alpha*e1; alpha*inv(R1')*A2'*e2] + */ + void transposeMultiplyAdd(double alpha, const Errors& e, VectorValues& y) const; + }; } // namespace gtsam diff --git a/gtsam/linear/SubgraphSolver.cpp b/gtsam/linear/SubgraphSolver.cpp index 519192144..034fdeeaf 100644 --- a/gtsam/linear/SubgraphSolver.cpp +++ b/gtsam/linear/SubgraphSolver.cpp @@ -12,13 +12,11 @@ #include #include #include -#include #include #include #include #include #include -#include #include #include #include @@ -29,60 +27,64 @@ using namespace std; namespace gtsam { /**************************************************************************************************/ -SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &gfg, const Parameters ¶meters) - : parameters_(parameters) +SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &gfg, const Parameters ¶meters, const Ordering& ordering) + : parameters_(parameters), ordering_(ordering) { initialize(gfg); } /**************************************************************************************************/ -SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &jfg, const Parameters ¶meters) - : parameters_(parameters) +SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &jfg, const Parameters ¶meters, const Ordering& ordering) + : parameters_(parameters), ordering_(ordering) { initialize(*jfg); } /**************************************************************************************************/ -SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFactorGraph &Ab2, const Parameters ¶meters) - : parameters_(parameters) { +SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFactorGraph &Ab2, const Parameters ¶meters, const Ordering& ordering) + : parameters_(parameters), ordering_(ordering) { - GaussianBayesNet::shared_ptr Rc1 = EliminationTree::Create(Ab1)->eliminate(&EliminateQR); + GaussianBayesNet::shared_ptr Rc1 = Ab1.eliminateSequential(ordering_, EliminateQR); initialize(Rc1, boost::make_shared(Ab2)); } /**************************************************************************************************/ SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &Ab1, - const GaussianFactorGraph::shared_ptr &Ab2, const Parameters ¶meters) - : parameters_(parameters) { + const GaussianFactorGraph::shared_ptr &Ab2, const Parameters ¶meters, const Ordering& ordering) + : parameters_(parameters), ordering_(ordering) { - GaussianBayesNet::shared_ptr Rc1 = EliminationTree::Create(*Ab1)->eliminate(&EliminateQR); + GaussianBayesNet::shared_ptr Rc1 = Ab1->eliminateSequential(ordering_, EliminateQR); initialize(Rc1, Ab2); } /**************************************************************************************************/ SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph &Ab2, - const Parameters ¶meters) : parameters_(parameters) + const Parameters ¶meters, const Ordering& ordering) : parameters_(parameters), ordering_(ordering) { initialize(Rc1, boost::make_shared(Ab2)); } /**************************************************************************************************/ SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, - const GaussianFactorGraph::shared_ptr &Ab2, const Parameters ¶meters) : parameters_(parameters) + const GaussianFactorGraph::shared_ptr &Ab2, const Parameters ¶meters, const Ordering& ordering) : +parameters_(parameters), ordering_(ordering) { initialize(Rc1, Ab2); } +/**************************************************************************************************/ VectorValues SubgraphSolver::optimize() { VectorValues ybar = conjugateGradients(*pc_, pc_->zero(), parameters_); return pc_->x(ybar); } +/**************************************************************************************************/ VectorValues SubgraphSolver::optimize(const VectorValues &initial) { // the initial is ignored in this case ... return optimize(); } +/**************************************************************************************************/ void SubgraphSolver::initialize(const GaussianFactorGraph &jfg) { GaussianFactorGraph::shared_ptr Ab1 = boost::make_shared(), @@ -92,17 +94,19 @@ void SubgraphSolver::initialize(const GaussianFactorGraph &jfg) if (parameters_.verbosity()) cout << "Split A into (A1) " << Ab1->size() << " and (A2) " << Ab2->size() << " factors" << endl; - GaussianBayesNet::shared_ptr Rc1 = EliminationTree::Create(*Ab1)->eliminate(&EliminateQR); - VectorValues::shared_ptr xbar(new VectorValues(gtsam::optimize(*Rc1))); + GaussianBayesNet::shared_ptr Rc1 = Ab1->eliminateSequential(ordering_, EliminateQR); + VectorValues::shared_ptr xbar = boost::make_shared(Rc1->optimize()); pc_ = boost::make_shared(Ab2, Rc1, xbar); } +/**************************************************************************************************/ void SubgraphSolver::initialize(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph::shared_ptr &Ab2) { - VectorValues::shared_ptr xbar(new VectorValues(gtsam::optimize(*Rc1))); + VectorValues::shared_ptr xbar = boost::make_shared(Rc1->optimize()); pc_ = boost::make_shared(Ab2, Rc1, xbar); } +/**************************************************************************************************/ boost::tuple SubgraphSolver::splitGraph(const GaussianFactorGraph &jfg) { @@ -125,7 +129,7 @@ SubgraphSolver::splitGraph(const GaussianFactorGraph &jfg) { /* check whether this factor should be augmented to the "tree" graph */ if ( gf->keys().size() == 1 ) augment = true; else { - const Index u = gf->keys()[0], v = gf->keys()[1], + const Key u = gf->keys()[0], v = gf->keys()[1], u_root = D.findSet(u), v_root = D.findSet(v); if ( u_root != v_root ) { t++; augment = true ; diff --git a/gtsam/linear/SubgraphSolver.h b/gtsam/linear/SubgraphSolver.h index 1f071fcdd..f6557a2c2 100644 --- a/gtsam/linear/SubgraphSolver.h +++ b/gtsam/linear/SubgraphSolver.h @@ -11,14 +11,19 @@ #pragma once + #include -#include -#include #include -#include +#include + +#include namespace gtsam { + // Forward declarations + class GaussianFactorGraph; + class GaussianBayesNet; + class GTSAM_EXPORT SubgraphSolverParameters : public ConjugateGradientParameters { public: typedef ConjugateGradientParameters Base; @@ -40,7 +45,7 @@ public: * To use it in nonlinear optimization, please see the following example * * LevenbergMarquardtParams parameters; - * parameters.linearSolverType = SuccessiveLinearizationParams::CONJUGATE_GRADIENT; + * parameters.linearSolverType = NonlinearOptimizerParams::CONJUGATE_GRADIENT; * parameters.iterativeParams = boost::make_shared(); * LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parameters); * Values result = optimizer.optimize(); @@ -55,20 +60,21 @@ public: protected: Parameters parameters_; + Ordering ordering_; SubgraphPreconditioner::shared_ptr pc_; ///< preconditioner object public: /* Given a gaussian factor graph, split it into a spanning tree (A1) + others (A2) for SPCG */ - SubgraphSolver(const GaussianFactorGraph &A, const Parameters ¶meters); - SubgraphSolver(const GaussianFactorGraph::shared_ptr &A, const Parameters ¶meters); + SubgraphSolver(const GaussianFactorGraph &A, const Parameters ¶meters, const Ordering& ordering); + SubgraphSolver(const boost::shared_ptr &A, const Parameters ¶meters, const Ordering& ordering); /* The user specify the subgraph part and the constraint part, may throw exception if A1 is underdetermined */ - SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFactorGraph &Ab2, const Parameters ¶meters); - SubgraphSolver(const GaussianFactorGraph::shared_ptr &Ab1, const GaussianFactorGraph::shared_ptr &Ab2, const Parameters ¶meters); + SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFactorGraph &Ab2, const Parameters ¶meters, const Ordering& ordering); + SubgraphSolver(const boost::shared_ptr &Ab1, const boost::shared_ptr &Ab2, const Parameters ¶meters, const Ordering& ordering); /* The same as above, but the A1 is solved before */ - SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph &Ab2, const Parameters ¶meters); - SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph::shared_ptr &Ab2, const Parameters ¶meters); + SubgraphSolver(const boost::shared_ptr &Rc1, const GaussianFactorGraph &Ab2, const Parameters ¶meters, const Ordering& ordering); + SubgraphSolver(const boost::shared_ptr &Rc1, const boost::shared_ptr &Ab2, const Parameters ¶meters, const Ordering& ordering); virtual ~SubgraphSolver() {} virtual VectorValues optimize () ; @@ -77,12 +83,10 @@ public: protected: void initialize(const GaussianFactorGraph &jfg); - void initialize(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph::shared_ptr &Ab2); + void initialize(const boost::shared_ptr &Rc1, const boost::shared_ptr &Ab2); - boost::tuple + boost::tuple, boost::shared_ptr > splitGraph(const GaussianFactorGraph &gfg) ; }; } // namespace gtsam - - diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index 3c6c7133f..664fcf3b7 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -16,222 +16,332 @@ * @author Alex Cunningham */ -#include -#include #include -#include +#include +#include +#include +#include +#include +#include using namespace std; namespace gtsam { -/* ************************************************************************* */ -VectorValues VectorValues::Zero(const VectorValues& x) { - VectorValues result; - result.values_.resize(x.size()); - for(size_t j=0; j VectorValues::dims() const { - std::vector result(this->size()); - for(Index j = 0; j < this->size(); ++j) - result[j] = this->dim(j); - return result; -} - -/* ************************************************************************* */ -void VectorValues::insert(Index j, const Vector& value) { - // Make sure j does not already exist - if(exists(j)) - throw invalid_argument("VectorValues: requested variable index to insert already exists."); - - // If this adds variables at the end, insert zero-length entries up to j - if(j >= size()) - values_.resize(j+1); - - // Assign value - values_[j] = value; -} - -/* ************************************************************************* */ -void VectorValues::print(const std::string& str, const IndexFormatter& formatter) const { - std::cout << str << ": " << size() << " elements\n"; - for (Index var = 0; var < size(); ++var) - std::cout << " " << formatter(var) << ": \n" << (*this)[var] << "\n"; - std::cout.flush(); -} - -/* ************************************************************************* */ -bool VectorValues::equals(const VectorValues& x, double tol) const { - if(this->size() != x.size()) - return false; - for(Index j=0; j < size(); ++j) - if(!equal_with_abs_tol(values_[j], x.values_[j], tol)) - return false; - return true; -} - -/* ************************************************************************* */ -void VectorValues::resize(Index nVars, size_t varDim) { - values_.resize(nVars); - for(Index j = 0; j < nVars; ++j) - values_[j] = Vector(varDim); -} - -/* ************************************************************************* */ -void VectorValues::resizeLike(const VectorValues& other) { - values_.resize(other.size()); - for(Index j = 0; j < other.size(); ++j) - values_[j].resize(other.values_[j].size()); -} - -/* ************************************************************************* */ -VectorValues VectorValues::SameStructure(const VectorValues& other) { - VectorValues ret; - ret.resizeLike(other); - return ret; -} - -/* ************************************************************************* */ -VectorValues VectorValues::Zero(Index nVars, size_t varDim) { - VectorValues ret(nVars, varDim); - ret.setZero(); - return ret; -} - -/* ************************************************************************* */ -void VectorValues::setZero() { - BOOST_FOREACH(Vector& v, *this) { - v.setZero(); + /* ************************************************************************* */ + VectorValues::VectorValues(const VectorValues& first, const VectorValues& second) + { + // Merge using predicate for comparing first of pair + merge(first.begin(), first.end(), second.begin(), second.end(), inserter(values_, values_.end()), + boost::bind(&less::operator(), less(), boost::bind(&KeyValuePair::first, _1), boost::bind(&KeyValuePair::first, _2))); + if(size() != first.size() + second.size()) + throw invalid_argument("Requested to merge two VectorValues that have one or more variables in common."); } -} -/* ************************************************************************* */ -const Vector VectorValues::asVector() const { - return internal::extractVectorValuesSlices(*this, - boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(this->size()), true); -} + /* ************************************************************************* */ + VectorValues::VectorValues(const Vector& x, const Dims& dims) { + typedef pair Pair; + size_t j = 0; + BOOST_FOREACH(const Pair& v, dims) { + Key key; + size_t n; + boost::tie(key, n) = v; + values_.insert(make_pair(key, sub(x, j, j + n))); + j += n; + } + } -/* ************************************************************************* */ -const Vector VectorValues::vector(const std::vector& indices) const { - return internal::extractVectorValuesSlices(*this, indices.begin(), indices.end()); -} + /* ************************************************************************* */ + VectorValues VectorValues::Zero(const VectorValues& other) + { + VectorValues result; + BOOST_FOREACH(const KeyValuePair& v, other) + result.values_.insert(make_pair(v.first, Vector::Zero(v.second.size()))); + return result; + } -/* ************************************************************************* */ -bool VectorValues::hasSameStructure(const VectorValues& other) const { - if(this->size() != other.size()) - return false; - for(size_t j = 0; j < size(); ++j) - // Directly accessing maps instead of using VV::dim in case some values are empty - if(this->values_[j].rows() != other.values_[j].rows()) + /* ************************************************************************* */ + void VectorValues::update(const VectorValues& values) + { + iterator hint = begin(); + BOOST_FOREACH(const KeyValuePair& key_value, values) + { + // Use this trick to find the value using a hint, since we are inserting from another sorted map + size_t oldSize = values_.size(); + hint = values_.insert(hint, key_value); + if(values_.size() > oldSize) { + values_.unsafe_erase(hint); + throw out_of_range("Requested to update a VectorValues with another VectorValues that contains keys not present in the first."); + } else { + hint->second = key_value.second; + } + } + } + + /* ************************************************************************* */ + void VectorValues::insert(const VectorValues& values) + { + size_t originalSize = size(); + values_.insert(values.begin(), values.end()); + if(size() != originalSize + values.size()) + throw invalid_argument("Requested to insert a VectorValues into another VectorValues that already contains one or more of its keys."); + } + + /* ************************************************************************* */ + void VectorValues::setZero() + { + BOOST_FOREACH(Vector& v, values_ | map_values) + v.setZero(); + } + + /* ************************************************************************* */ + void VectorValues::print(const string& str, const KeyFormatter& formatter) const { + cout << str << ": " << size() << " elements\n"; + BOOST_FOREACH(const value_type& key_value, *this) + cout << " " << formatter(key_value.first) << ": " << key_value.second.transpose() << "\n"; + cout.flush(); + } + + /* ************************************************************************* */ + bool VectorValues::equals(const VectorValues& x, double tol) const { + if(this->size() != x.size()) return false; - return true; -} + typedef boost::tuple ValuePair; + BOOST_FOREACH(const ValuePair& values, boost::combine(*this, x)) { + if(values.get<0>().first != values.get<1>().first || + !equal_with_abs_tol(values.get<0>().second, values.get<1>().second, tol)) + return false; + } + return true; + } -/* ************************************************************************* */ -void VectorValues::permuteInPlace(const Permutation& permutation) { - // Allocate new values - Values newValues(this->size()); + /* ************************************************************************* */ + Vector VectorValues::vector() const + { + // Count dimensions + DenseIndex totalDim = 0; + BOOST_FOREACH(const Vector& v, *this | map_values) + totalDim += v.size(); - // Swap values from this VectorValues to the permuted VectorValues - for(size_t i = 0; i < this->size(); ++i) - newValues[i].swap(this->at(permutation[i])); + // Copy vectors + Vector result(totalDim); + DenseIndex pos = 0; + BOOST_FOREACH(const Vector& v, *this | map_values) { + result.segment(pos, v.size()) = v; + pos += v.size(); + } - // Swap the values into this VectorValues - values_.swap(newValues); -} + return result; + } -/* ************************************************************************* */ -void VectorValues::permuteInPlace(const Permutation& selector, const Permutation& permutation) { - if(selector.size() != permutation.size()) - throw invalid_argument("VariableIndex::permuteInPlace (partial permutation version) called with selector and permutation of different sizes."); - // Create new index the size of the permuted entries - Values reorderedEntries(selector.size()); - // Permute the affected entries into the new index - for(size_t dstSlot = 0; dstSlot < selector.size(); ++dstSlot) - reorderedEntries[dstSlot].swap(values_[selector[permutation[dstSlot]]]); - // Put the affected entries back in the new order - for(size_t slot = 0; slot < selector.size(); ++slot) - values_[selector[slot]].swap(reorderedEntries[slot]); -} + /* ************************************************************************* */ + Vector VectorValues::vector(const FastVector& keys) const + { + // Count dimensions and collect pointers to avoid double lookups + DenseIndex totalDim = 0; + FastVector items(keys.size()); + for(size_t i = 0; i < keys.size(); ++i) { + items[i] = &at(keys[i]); + totalDim += items[i]->size(); + } -/* ************************************************************************* */ -void VectorValues::swap(VectorValues& other) { - this->values_.swap(other.values_); -} + // Copy vectors + Vector result(totalDim); + DenseIndex pos = 0; + BOOST_FOREACH(const Vector *v, items) { + result.segment(pos, v->size()) = *v; + pos += v->size(); + } -/* ************************************************************************* */ -double VectorValues::dot(const VectorValues& v) const { - double result = 0.0; - if(this->size() != v.size()) - throw invalid_argument("VectorValues::dot called with different vector sizes"); - for(Index j = 0; j < this->size(); ++j) - // Directly accessing maps instead of using VV::dim in case some values are empty - if(this->values_[j].size() == v.values_[j].size()) - result += this->values_[j].dot(v.values_[j]); - else - throw invalid_argument("VectorValues::dot called with different vector sizes"); - return result; -} + return result; + } -/* ************************************************************************* */ -double VectorValues::norm() const { - return std::sqrt(this->squaredNorm()); -} + /* ************************************************************************* */ + Vector VectorValues::vector(const Dims& keys) const + { + // Count dimensions + DenseIndex totalDim = 0; + BOOST_FOREACH(size_t dim, keys | map_values) + totalDim += dim; + Vector result(totalDim); + size_t j = 0; + BOOST_FOREACH(const Dims::value_type& it, keys) { + result.segment(j,it.second) = at(it.first); + j += it.second; + } + return result; + } -/* ************************************************************************* */ -double VectorValues::squaredNorm() const { - double sumSquares = 0.0; - for(Index j = 0; j < this->size(); ++j) - // Directly accessing maps instead of using VV::dim in case some values are empty - sumSquares += this->values_[j].squaredNorm(); - return sumSquares; -} + /* ************************************************************************* */ + void VectorValues::swap(VectorValues& other) { + this->values_.swap(other.values_); + } -/* ************************************************************************* */ -VectorValues VectorValues::operator+(const VectorValues& c) const { - VectorValues result = SameStructure(*this); - if(this->size() != c.size()) - throw invalid_argument("VectorValues::operator+ called with different vector sizes"); - for(Index j = 0; j < this->size(); ++j) - // Directly accessing maps instead of using VV::dim in case some values are empty - if(this->values_[j].size() == c.values_[j].size()) - result.values_[j] = this->values_[j] + c.values_[j]; - else - throw invalid_argument("VectorValues::operator- called with different vector sizes"); - return result; -} + /* ************************************************************************* */ + namespace internal + { + bool structureCompareOp(const boost::tuple& vv) + { + return vv.get<0>().first == vv.get<1>().first + && vv.get<0>().second.size() == vv.get<1>().second.size(); + } + } -/* ************************************************************************* */ -VectorValues VectorValues::operator-(const VectorValues& c) const { - VectorValues result = SameStructure(*this); - if(this->size() != c.size()) - throw invalid_argument("VectorValues::operator- called with different vector sizes"); - for(Index j = 0; j < this->size(); ++j) - // Directly accessing maps instead of using VV::dim in case some values are empty - if(this->values_[j].size() == c.values_[j].size()) - result.values_[j] = this->values_[j] - c.values_[j]; - else - throw invalid_argument("VectorValues::operator- called with different vector sizes"); - return result; -} + /* ************************************************************************* */ + bool VectorValues::hasSameStructure(const VectorValues other) const + { + return accumulate(combine(*this, other) + | transformed(internal::structureCompareOp), true, logical_and()); + } -/* ************************************************************************* */ -void VectorValues::operator+=(const VectorValues& c) { - if(this->size() != c.size()) - throw invalid_argument("VectorValues::operator+= called with different vector sizes"); - for(Index j = 0; j < this->size(); ++j) - // Directly accessing maps instead of using VV::dim in case some values are empty - if(this->values_[j].size() == c.values_[j].size()) - this->values_[j] += c.values_[j]; - else + /* ************************************************************************* */ + double VectorValues::dot(const VectorValues& v) const + { + if(this->size() != v.size()) + throw invalid_argument("VectorValues::dot called with a VectorValues of different structure"); + double result = 0.0; + typedef boost::tuple ValuePair; + using boost::adaptors::map_values; + BOOST_FOREACH(const ValuePair& values, boost::combine(*this, v)) { + assert_throw(values.get<0>().first == values.get<1>().first, + invalid_argument("VectorValues::dot called with a VectorValues of different structure")); + assert_throw(values.get<0>().second.size() == values.get<1>().second.size(), + invalid_argument("VectorValues::dot called with a VectorValues of different structure")); + result += values.get<0>().second.dot(values.get<1>().second); + } + return result; + } + + /* ************************************************************************* */ + double VectorValues::norm() const { + return std::sqrt(this->squaredNorm()); + } + + /* ************************************************************************* */ + double VectorValues::squaredNorm() const { + double sumSquares = 0.0; + using boost::adaptors::map_values; + BOOST_FOREACH(const Vector& v, *this | map_values) + sumSquares += v.squaredNorm(); + return sumSquares; + } + + /* ************************************************************************* */ + VectorValues VectorValues::operator+(const VectorValues& c) const + { + if(this->size() != c.size()) + throw invalid_argument("VectorValues::operator+ called with different vector sizes"); + assert_throw(hasSameStructure(c), + invalid_argument("VectorValues::operator+ called with different vector sizes")); + + VectorValues result; + // The result.end() hint here should result in constant-time inserts + for(const_iterator j1 = begin(), j2 = c.begin(); j1 != end(); ++j1, ++j2) + result.values_.insert(result.end(), make_pair(j1->first, j1->second + j2->second)); + + return result; + } + + /* ************************************************************************* */ + VectorValues VectorValues::add(const VectorValues& c) const + { + return *this + c; + } + + /* ************************************************************************* */ + VectorValues& VectorValues::operator+=(const VectorValues& c) + { + if(this->size() != c.size()) throw invalid_argument("VectorValues::operator+= called with different vector sizes"); -} + assert_throw(hasSameStructure(c), + invalid_argument("VectorValues::operator+= called with different vector sizes")); -/* ************************************************************************* */ + iterator j1 = begin(); + const_iterator j2 = c.begin(); + // The result.end() hint here should result in constant-time inserts + for(; j1 != end(); ++j1, ++j2) + j1->second += j2->second; + + return *this; + } + + /* ************************************************************************* */ + VectorValues& VectorValues::addInPlace(const VectorValues& c) + { + return *this += c; + } + + /* ************************************************************************* */ + VectorValues& VectorValues::addInPlace_(const VectorValues& c) + { + for(const_iterator j2 = c.begin(); j2 != c.end(); ++j2) { + pair xi = tryInsert(j2->first, Vector()); + if(xi.second) + xi.first->second = j2->second; + else + xi.first->second += j2->second; + } + return *this; + } + + /* ************************************************************************* */ + VectorValues VectorValues::operator-(const VectorValues& c) const + { + if(this->size() != c.size()) + throw invalid_argument("VectorValues::operator- called with different vector sizes"); + assert_throw(hasSameStructure(c), + invalid_argument("VectorValues::operator- called with different vector sizes")); + + VectorValues result; + // The result.end() hint here should result in constant-time inserts + for(const_iterator j1 = begin(), j2 = c.begin(); j1 != end(); ++j1, ++j2) + result.values_.insert(result.end(), make_pair(j1->first, j1->second - j2->second)); + + return result; + } + + /* ************************************************************************* */ + VectorValues VectorValues::subtract(const VectorValues& c) const + { + return *this - c; + } + + /* ************************************************************************* */ + VectorValues operator*(const double a, const VectorValues &v) + { + VectorValues result; + BOOST_FOREACH(const VectorValues::KeyValuePair& key_v, v) + result.values_.insert(result.values_.end(), make_pair(key_v.first, a * key_v.second)); + return result; + } + + /* ************************************************************************* */ + VectorValues VectorValues::scale(const double a) const + { + return a * *this; + } + + /* ************************************************************************* */ + VectorValues& VectorValues::operator*=(double alpha) + { + BOOST_FOREACH(Vector& v, *this | map_values) + v *= alpha; + return *this; + } + + /* ************************************************************************* */ + VectorValues& VectorValues::scaleInPlace(double alpha) + { + return *this *= alpha; + } + + /* ************************************************************************* */ } // \namespace gtsam diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index af13231fb..502d9314b 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -18,20 +18,15 @@ #pragma once #include +#include +#include #include +#include -#include -#include -#include #include -#include -#include namespace gtsam { - // Forward declarations - class Permutation; - /** * This class represents a collection of vector-valued variables associated * each with a unique integer index. It is typically used to store the variables @@ -42,9 +37,9 @@ namespace gtsam { * or creating this class in unit tests and examples where speed is not important, * you can use a simple interface: * - The default constructor VectorValues() to create this class - * - insert(Index, const Vector&) to add vector variables - * - operator[](Index) for read and write access to stored variables - * - \ref exists (Index) to check if a variable is present + * - insert(Key, const Vector&) to add vector variables + * - operator[](Key) for read and write access to stored variables + * - \ref exists (Key) to check if a variable is present * - Other facilities like iterators, size(), dim(), etc. * * Indices can be non-consecutive and inserted out-of-order, but you should not @@ -54,15 +49,15 @@ namespace gtsam { * Example: * \code VectorValues values; - values.insert(3, Vector_(3, 1.0, 2.0, 3.0)); - values.insert(4, Vector_(2, 4.0, 5.0)); - values.insert(0, Vector_(4, 6.0, 7.0, 8.0, 9.0)); + values.insert(3, (Vector(3) << 1.0, 2.0, 3.0)); + values.insert(4, (Vector(2) << 4.0, 5.0)); + values.insert(0, (Vector(4) << 6.0, 7.0, 8.0, 9.0)); // Prints [ 3.0 4.0 ] gtsam::print(values[1]); // Prints [ 8.0 9.0 ] - values[1] = Vector_(2, 8.0, 9.0); + values[1] = (Vector(2) << 8.0, 9.0); gtsam::print(values[1]); \endcode * @@ -78,7 +73,7 @@ namespace gtsam { * - Allocate space ahead of time using a pre-allocating constructor * (\ref AdvancedConstructors "Advanced Constructors"), Zero(), * SameStructure(), resize(), or append(). Do not use - * insert(Index, const Vector&), which always has to re-allocate the + * insert(Key, const Vector&), which always has to re-allocate the * internal vector. * - The vector() function permits access to the underlying Vector, for * doing mathematical or other operations that require all values. @@ -93,15 +88,19 @@ namespace gtsam { */ class GTSAM_EXPORT VectorValues { protected: - typedef std::vector Values; ///< Typedef for the collection of Vectors making up a VectorValues + typedef VectorValues This; + typedef ConcurrentMap Values; ///< Typedef for the collection of Vectors making up a VectorValues Values values_; ///< Collection of Vectors making up this VectorValues public: typedef Values::iterator iterator; ///< Iterator over vector values typedef Values::const_iterator const_iterator; ///< Const iterator over vector values - typedef Values::reverse_iterator reverse_iterator; ///< Reverse iterator over vector values - typedef Values::const_reverse_iterator const_reverse_iterator; ///< Const reverse iterator over vector values - typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class + //typedef Values::reverse_iterator reverse_iterator; ///< Reverse iterator over vector values + //typedef Values::const_reverse_iterator const_reverse_iterator; ///< Const reverse iterator over vector values + typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class + typedef Values::value_type value_type; ///< Typedef to pair, a key-value pair + typedef value_type KeyValuePair; ///< Typedef to pair, a key-value pair + typedef std::map Dims; /// @name Standard Constructors /// @{ @@ -111,192 +110,163 @@ namespace gtsam { */ VectorValues() {} - /** Named constructor to create a VectorValues of the same structure of the - * specified one, but filled with zeros. - * @return - */ - static VectorValues Zero(const VectorValues& model); + /** Merge two VectorValues into one, this is more efficient than inserting elements one by one. */ + VectorValues(const VectorValues& first, const VectorValues& second); + + /** Create from another container holding pair. */ + template + explicit VectorValues(const CONTAINER& c) : values_(c.begin(), c.end()) {} + + /** Implicit copy constructor to specialize the explicit constructor from any container. */ + VectorValues(const VectorValues& c) : values_(c.values_) {} + + /** Create from a pair of iterators over pair. */ + template + VectorValues(ITERATOR first, ITERATOR last) : values_(first, last) {} + + /** Constructor from Vector. */ + VectorValues(const Vector& c, const Dims& dims); + + /** Create a VectorValues with the same structure as \c other, but filled with zeros. */ + static VectorValues Zero(const VectorValues& other); /// @} /// @name Standard Interface /// @{ - /** Number of variables stored, always 1 more than the highest variable index, - * even if some variables with lower indices are not present. */ - Index size() const { return values_.size(); } + /** Number of variables stored. */ + Key size() const { return values_.size(); } /** Return the dimension of variable \c j. */ - size_t dim(Index j) const { checkExists(j); return (*this)[j].rows(); } + size_t dim(Key j) const { return at(j).rows(); } - /** Return the dimension of each vector in this container */ - std::vector dims() const; + /** Check whether a variable with key \c j exists. */ + bool exists(Key j) const { return find(j) != end(); } - /** Check whether a variable with index \c j exists. */ - bool exists(Index j) const { return j < size() && values_[j].rows() > 0; } + /** Read/write access to the vector value with key \c j, throws std::out_of_range if \c j does not exist, identical to operator[](Key). */ + Vector& at(Key j) { + iterator item = find(j); + if(item == end()) + throw std::out_of_range( + "Requested variable '" + DefaultKeyFormatter(j) + "' is not in this VectorValues."); + else + return item->second; + } - /** Read/write access to the vector value with index \c j, throws std::out_of_range if \c j does not exist, identical to operator[](Index). */ - Vector& at(Index j) { checkExists(j); return values_[j]; } + /** Access the vector value with key \c j (const version), throws std::out_of_range if \c j does not exist, identical to operator[](Key). */ + const Vector& at(Key j) const { + const_iterator item = find(j); + if(item == end()) + throw std::out_of_range( + "Requested variable '" + DefaultKeyFormatter(j) + "' is not in this VectorValues."); + else + return item->second; + } - /** Access the vector value with index \c j (const version), throws std::out_of_range if \c j does not exist, identical to operator[](Index). */ - const Vector& at(Index j) const { checkExists(j); return values_[j]; } + /** Read/write access to the vector value with key \c j, throws std::out_of_range if \c j does + * not exist, identical to at(Key). */ + Vector& operator[](Key j) { return at(j); } - /** Read/write access to the vector value with index \c j, throws std::out_of_range if \c j does not exist, identical to at(Index). */ - Vector& operator[](Index j) { return at(j); } + /** Access the vector value with key \c j (const version), throws std::out_of_range if \c j does + * not exist, identical to at(Key). */ + const Vector& operator[](Key j) const { return at(j); } - /** Access the vector value with index \c j (const version), throws std::out_of_range if \c j does not exist, identical to at(Index). */ - const Vector& operator[](Index j) const { return at(j); } + /** For all key/value pairs in \c values, replace values with corresponding keys in this class + * with those in \c values. Throws std::out_of_range if any keys in \c values are not present + * in this class. */ + void update(const VectorValues& values); - /** Insert a vector \c value with index \c j. - * Causes reallocation, but can insert values in any order. - * Throws an invalid_argument exception if the index \c j is already used. + /** Insert a vector \c value with key \c j. Throws an invalid_argument exception if the key \c + * j is already used. * @param value The vector to be inserted. - * @param j The index with which the value will be associated. - */ - void insert(Index j, const Vector& value); + * @param j The index with which the value will be associated. */ + iterator insert(Key j, const Vector& value) { + return insert(std::make_pair(j, value)); // Note only passing a reference to the Vector + } + + /** Insert a vector \c value with key \c j. Throws an invalid_argument exception if the key \c + * j is already used. + * @param value The vector to be inserted. + * @param j The index with which the value will be associated. */ + iterator insert(const std::pair& key_value) { + // Note that here we accept a pair with a reference to the Vector, but the Vector is copied as + // it is inserted into the values_ map. + std::pair result = values_.insert(key_value); + if(!result.second) + throw std::invalid_argument( + "Requested to insert variable '" + DefaultKeyFormatter(key_value.first) + + "' already in this VectorValues."); + return result.first; + } + + /** Insert all values from \c values. Throws an invalid_argument exception if any keys to be + * inserted are already used. */ + void insert(const VectorValues& values); + + /** insert that mimics the STL map insert - if the value already exists, the map is not modified + * and an iterator to the existing value is returned, along with 'false'. If the value did not + * exist, it is inserted and an iterator pointing to the new element, along with 'true', is + * returned. */ + std::pair tryInsert(Key j, const Vector& value) { + return values_.insert(std::make_pair(j, value)); } + + /** Erase the vector with the given key, or throw std::out_of_range if it does not exist */ + void erase(Key var) { + if(values_.unsafe_erase(var) == 0) + throw std::invalid_argument("Requested variable '" + DefaultKeyFormatter(var) + "', is not in this VectorValues."); + } + + /** Set all values to zero vectors. */ + void setZero(); iterator begin() { return values_.begin(); } ///< Iterator over variables const_iterator begin() const { return values_.begin(); } ///< Iterator over variables - iterator end() { return values_.end(); } ///< Iterator over variables + iterator end() { return values_.end(); } ///< Iterator over variables const_iterator end() const { return values_.end(); } ///< Iterator over variables - reverse_iterator rbegin() { return values_.rbegin(); } ///< Reverse iterator over variables - const_reverse_iterator rbegin() const { return values_.rbegin(); } ///< Reverse iterator over variables - reverse_iterator rend() { return values_.rend(); } ///< Reverse iterator over variables - const_reverse_iterator rend() const { return values_.rend(); } ///< Reverse iterator over variables + //reverse_iterator rbegin() { return values_.rbegin(); } ///< Reverse iterator over variables + //const_reverse_iterator rbegin() const { return values_.rbegin(); } ///< Reverse iterator over variables + //reverse_iterator rend() { return values_.rend(); } ///< Reverse iterator over variables + //const_reverse_iterator rend() const { return values_.rend(); } ///< Reverse iterator over variables + + /** Return the iterator corresponding to the requested key, or end() if no variable is present with this key. */ + iterator find(Key j) { return values_.find(j); } + + /** Return the iterator corresponding to the requested key, or end() if no variable is present with this key. */ + const_iterator find(Key j) const { return values_.find(j); } /** print required by Testable for unit testing */ void print(const std::string& str = "VectorValues: ", - const IndexFormatter& formatter = DefaultIndexFormatter) const; + const KeyFormatter& formatter = DefaultKeyFormatter) const; /** equals required by Testable for unit testing */ bool equals(const VectorValues& x, double tol = 1e-9) const; /// @{ - /// \anchor AdvancedConstructors - /// @name Advanced Constructors - /// @} - - /** Construct from a container of variable dimensions (in variable order), without initializing any values. */ - template - explicit VectorValues(const CONTAINER& dimensions) { this->append(dimensions); } - - /** Construct from a container of variable dimensions (in variable order), with initial values. */ - template - explicit VectorValues(const Vector& d, const CONTAINER& dimensions) { - this->append(d, dimensions); - } - - /** Construct to hold nVars vectors of varDim dimension each. */ - VectorValues(Index nVars, size_t varDim) { this->resize(nVars, varDim); } - - /** Named constructor to create a VectorValues that matches the structure of - * the specified VectorValues, but do not initialize the new values. */ - static VectorValues SameStructure(const VectorValues& other); - - /** Named constructor to create a VectorValues from a container of variable - * dimensions that is filled with zeros. - * @param dimensions A container of the dimension of each variable to create. - */ - template - static VectorValues Zero(const CONTAINER& dimensions); - - /** Named constructor to create a VectorValues filled with zeros that has - * \c nVars variables, each of dimension \c varDim - * @param nVars The number of variables to create - * @param varDim The dimension of each variable - * @return The new VectorValues - */ - static VectorValues Zero(Index nVars, size_t varDim); - - /// @} /// @name Advanced Interface /// @{ - /** Resize this VectorValues to have identical structure to other, leaving - * this VectorValues with uninitialized values. - * @param other The VectorValues whose structure to copy - */ - void resizeLike(const VectorValues& other); - - /** Resize the VectorValues to hold \c nVars variables, each of dimension - * \c varDim. Any individual vectors that do not change size will keep - * their values, but any new or resized vectors will be uninitialized. - * @param nVars The number of variables to create - * @param varDim The dimension of each variable - */ - void resize(Index nVars, size_t varDim); - - /** Resize the VectorValues to contain variables of the dimensions stored - * in \c dimensions. Any individual vectors that do not change size will keep - * their values, but any new or resized vectors will be uninitialized. - * @param dimensions A container of the dimension of each variable to create. - */ - template - void resize(const CONTAINER& dimensions); - - /** Append to the VectorValues to additionally contain variables of the - * dimensions stored in \c dimensions. The new variables are uninitialized, - * but this function is used to pre-allocate space for performance. This - * function preserves the original data, so all previously-existing variables - * are left unchanged. - * @param dimensions A container of the dimension of each variable to create. - */ - template - void append(const CONTAINER& dimensions); - - /** Append to the VectorValues to additionally contain variables of the - * dimensions stored in \c dimensions. Initial values for the new variables - * are extracted from the input vector, holding values for all components - * in the same order specified in the dimensions container. - * This function preserves the original data, so all previously-existing - * variables are left unchanged. - * @param d A vector holding values for all variables, which order - * specified in the below container - * @param dimensions A container of the dimension of each variable to create. - */ - template - void append(const Vector& d, const CONTAINER& dimensions); - - /** Removes the last subvector from the VectorValues */ - void pop_back() { values_.pop_back(); }; - - /** Set all entries to zero, does not modify the size. */ - void setZero(); - /** Retrieve the entire solution as a single vector */ - const Vector asVector() const; + Vector vector() const; - /** Access a vector that is a subset of relevant indices */ - const Vector vector(const std::vector& indices) const; + /** Access a vector that is a subset of relevant keys. */ + Vector vector(const FastVector& keys) const; - /** Check whether this VectorValues has the same structure, meaning has the - * same number of variables and that all variables are of the same dimension, - * as another VectorValues - * @param other The other VectorValues with which to compare structure - * @return \c true if the structure is the same, \c false if not. - */ - bool hasSameStructure(const VectorValues& other) const; + /** Access a vector that is a subset of relevant keys, dims version. */ + Vector vector(const Dims& dims) const; - /** - * Permute the variables in the VariableIndex according to the given partial permutation - */ - void permuteInPlace(const Permutation& selector, const Permutation& permutation); - - /** - * Permute the entries of this VectorValues in place - */ - void permuteInPlace(const Permutation& permutation); - - /** - * Swap the data in this VectorValues with another. - */ + /** Swap the data in this VectorValues with another. */ void swap(VectorValues& other); + /** Check if this VectorValues has the same structure (keys and dimensions) as another */ + bool hasSameStructure(const VectorValues other) const; + /// @} /// @name Linear algebra operations /// @{ /** Dot product with another VectorValues, interpreting both as vectors of - * their concatenated values. */ + * their concatenated values. Both VectorValues must have the + * same structure (checked when NDEBUG is not defined). */ double dot(const VectorValues& v) const; /** Vector L2 norm */ @@ -305,23 +275,44 @@ namespace gtsam { /** Squared vector L2 norm */ double squaredNorm() const; - /** - * + operator does element-wise addition. Both VectorValues must have the - * same structure (checked when NDEBUG is not defined). - */ + /** Element-wise addition, synonym for add(). Both VectorValues must have the same structure + * (checked when NDEBUG is not defined). */ VectorValues operator+(const VectorValues& c) const; - /** - * + operator does element-wise subtraction. Both VectorValues must have the - * same structure (checked when NDEBUG is not defined). - */ + /** Element-wise addition, synonym for operator+(). Both VectorValues must have the same + * structure (checked when NDEBUG is not defined). */ + VectorValues add(const VectorValues& c) const; + + /** Element-wise addition in-place, synonym for operator+=(). Both VectorValues must have the + * same structure (checked when NDEBUG is not defined). */ + VectorValues& operator+=(const VectorValues& c); + + /** Element-wise addition in-place, synonym for operator+=(). Both VectorValues must have the + * same structure (checked when NDEBUG is not defined). */ + VectorValues& addInPlace(const VectorValues& c); + + /** Element-wise addition in-place, but allows for empty slots in *this. Slower */ + VectorValues& addInPlace_(const VectorValues& c); + + /** Element-wise subtraction, synonym for subtract(). Both VectorValues must have the same + * structure (checked when NDEBUG is not defined). */ VectorValues operator-(const VectorValues& c) const; - /** - * += operator does element-wise addition. Both VectorValues must have the - * same structure (checked when NDEBUG is not defined). - */ - void operator+=(const VectorValues& c); + /** Element-wise subtraction, synonym for operator-(). Both VectorValues must have the same + * structure (checked when NDEBUG is not defined). */ + VectorValues subtract(const VectorValues& c) const; + + /** Element-wise scaling by a constant. */ + friend GTSAM_EXPORT VectorValues operator*(const double a, const VectorValues &v); + + /** Element-wise scaling by a constant. */ + VectorValues scale(const double a) const; + + /** Element-wise scaling by a constant in-place. */ + VectorValues& operator*=(double alpha); + + /** Element-wise scaling by a constant in-place. */ + VectorValues& scaleInPlace(double alpha); /// @} @@ -329,75 +320,57 @@ namespace gtsam { /// @name Matlab syntactic sugar for linear algebra operations /// @{ - inline VectorValues add(const VectorValues& c) const { return *this + c; } - inline VectorValues scale(const double a, const VectorValues& c) const { return a * (*this); } + //inline VectorValues scale(const double a, const VectorValues& c) const { return a * (*this); } /// @} - private: - // Throw an exception if j does not exist - void checkExists(Index j) const { - if(!exists(j)) { - const std::string msg = - (boost::format("VectorValues: requested variable index j=%1% is not in this VectorValues.") % j).str(); - throw std::out_of_range(msg); - } - } - - public: - /** * scale a vector by a scalar */ - friend VectorValues operator*(const double a, const VectorValues &v) { - VectorValues result = VectorValues::SameStructure(v); - for(Index j = 0; j < v.size(); ++j) - result.values_[j] = a * v.values_[j]; - return result; - } + //friend VectorValues operator*(const double a, const VectorValues &v) { + // VectorValues result = VectorValues::SameStructure(v); + // for(Key j = 0; j < v.size(); ++j) + // result.values_[j] = a * v.values_[j]; + // return result; + //} - /// TODO: linear algebra interface seems to have been added for SPCG. - friend void scal(double alpha, VectorValues& x) { - for(Index j = 0; j < x.size(); ++j) - x.values_[j] *= alpha; - } - /// TODO: linear algebra interface seems to have been added for SPCG. - friend void axpy(double alpha, const VectorValues& x, VectorValues& y) { - if(x.size() != y.size()) - throw std::invalid_argument("axpy(VectorValues) called with different vector sizes"); - for(Index j = 0; j < x.size(); ++j) - if(x.values_[j].size() == y.values_[j].size()) - y.values_[j] += alpha * x.values_[j]; - else - throw std::invalid_argument("axpy(VectorValues) called with different vector sizes"); - } - /// TODO: linear algebra interface seems to have been added for SPCG. - friend void sqrt(VectorValues &x) { - for(Index j = 0; j < x.size(); ++j) - x.values_[j] = x.values_[j].cwiseSqrt(); - } + //// TODO: linear algebra interface seems to have been added for SPCG. + //friend void axpy(double alpha, const VectorValues& x, VectorValues& y) { + // if(x.size() != y.size()) + // throw std::invalid_argument("axpy(VectorValues) called with different vector sizes"); + // for(Key j = 0; j < x.size(); ++j) + // if(x.values_[j].size() == y.values_[j].size()) + // y.values_[j] += alpha * x.values_[j]; + // else + // throw std::invalid_argument("axpy(VectorValues) called with different vector sizes"); + //} + //// TODO: linear algebra interface seems to have been added for SPCG. + //friend void sqrt(VectorValues &x) { + // for(Key j = 0; j < x.size(); ++j) + // x.values_[j] = x.values_[j].cwiseSqrt(); + //} - /// TODO: linear algebra interface seems to have been added for SPCG. - friend void ediv(const VectorValues& numerator, const VectorValues& denominator, VectorValues &result) { - if(numerator.size() != denominator.size() || numerator.size() != result.size()) - throw std::invalid_argument("ediv(VectorValues) called with different vector sizes"); - for(Index j = 0; j < numerator.size(); ++j) - if(numerator.values_[j].size() == denominator.values_[j].size() && numerator.values_[j].size() == result.values_[j].size()) - result.values_[j] = numerator.values_[j].cwiseQuotient(denominator.values_[j]); - else - throw std::invalid_argument("ediv(VectorValues) called with different vector sizes"); - } + //// TODO: linear algebra interface seems to have been added for SPCG. + //friend void ediv(const VectorValues& numerator, const VectorValues& denominator, VectorValues &result) { + // if(numerator.size() != denominator.size() || numerator.size() != result.size()) + // throw std::invalid_argument("ediv(VectorValues) called with different vector sizes"); + // for(Key j = 0; j < numerator.size(); ++j) + // if(numerator.values_[j].size() == denominator.values_[j].size() && numerator.values_[j].size() == result.values_[j].size()) + // result.values_[j] = numerator.values_[j].cwiseQuotient(denominator.values_[j]); + // else + // throw std::invalid_argument("ediv(VectorValues) called with different vector sizes"); + //} - /// TODO: linear algebra interface seems to have been added for SPCG. - friend void edivInPlace(VectorValues& x, const VectorValues& y) { - if(x.size() != y.size()) - throw std::invalid_argument("edivInPlace(VectorValues) called with different vector sizes"); - for(Index j = 0; j < x.size(); ++j) - if(x.values_[j].size() == y.values_[j].size()) - x.values_[j].array() /= y.values_[j].array(); - else - throw std::invalid_argument("edivInPlace(VectorValues) called with different vector sizes"); - } + //// TODO: linear algebra interface seems to have been added for SPCG. + //friend void edivInPlace(VectorValues& x, const VectorValues& y) { + // if(x.size() != y.size()) + // throw std::invalid_argument("edivInPlace(VectorValues) called with different vector sizes"); + // for(Key j = 0; j < x.size(); ++j) + // if(x.values_[j].size() == y.values_[j].size()) + // x.values_[j].array() /= y.values_[j].array(); + // else + // throw std::invalid_argument("edivInPlace(VectorValues) called with different vector sizes"); + //} private: /** Serialization function */ @@ -408,93 +381,4 @@ namespace gtsam { } }; // VectorValues definition - // Implementations of template and inline functions - - /* ************************************************************************* */ - template - void VectorValues::resize(const CONTAINER& dimensions) { - values_.clear(); - append(dimensions); - } - - /* ************************************************************************* */ - template - void VectorValues::append(const CONTAINER& dimensions) { - size_t i = size(); - values_.resize(size() + dimensions.size()); - BOOST_FOREACH(size_t dim, dimensions) { - values_[i] = Vector(dim); - ++ i; - } - } - - /* ************************************************************************* */ - template - void VectorValues::append(const Vector& d, const CONTAINER& dimensions) { - size_t i = size(); - size_t idx = 0; - values_.resize(size() + dimensions.size()); - BOOST_FOREACH(size_t dim, dimensions) { - values_[i] = sub(d, idx, idx+dim); - ++ i; - idx += dim; - } - } - - /* ************************************************************************* */ - template - VectorValues VectorValues::Zero(const CONTAINER& dimensions) { - VectorValues ret; - ret.values_.resize(dimensions.size()); - size_t i = 0; - BOOST_FOREACH(size_t dim, dimensions) { - ret.values_[i] = Vector::Zero(dim); - ++ i; - } - return ret; - } - - namespace internal { - /* ************************************************************************* */ - // Helper function, extracts vectors with variable indices - // in the first and last iterators, and concatenates them in that order into the - // output. - template - const Vector extractVectorValuesSlices(const VectorValues& values, ITERATOR first, ITERATOR last, bool allowNonexistant = false) { - // Find total dimensionality - size_t dim = 0; - for(ITERATOR j = first; j != last; ++j) - // If allowNonexistant is true, skip nonexistent indices (otherwise dim will throw an error on nonexistent) - if(!allowNonexistant || values.exists(*j)) - dim += values.dim(*j); - - // Copy vectors - Vector ret(dim); - size_t varStart = 0; - for(ITERATOR j = first; j != last; ++j) { - // If allowNonexistant is true, skip nonexistent indices (otherwise dim will throw an error on nonexistent) - if(!allowNonexistant || values.exists(*j)) { - ret.segment(varStart, values.dim(*j)) = values[*j]; - varStart += values.dim(*j); - } - } - return ret; - } - - /* ************************************************************************* */ - // Helper function, writes to the variables in values - // with indices iterated over by first and last, interpreting vector as the - // concatenated vectors to write. - template - void writeVectorValuesSlices(const VECTOR& vector, VectorValues& values, ITERATOR first, ITERATOR last) { - // Copy vectors - size_t varStart = 0; - for(ITERATOR j = first; j != last; ++j) { - values[*j] = vector.segment(varStart, values[*j].rows()); - varStart += values[*j].rows(); - } - assert(varStart == vector.rows()); - } - } - } // \namespace gtsam diff --git a/gtsam/linear/iterative-inl.h b/gtsam/linear/iterative-inl.h index 47997b44c..e61b67f73 100644 --- a/gtsam/linear/iterative-inl.h +++ b/gtsam/linear/iterative-inl.h @@ -45,11 +45,11 @@ namespace gtsam { // Start with g0 = A'*(A*x0-b), d0 = - g0 // i.e., first step is in direction of negative gradient - g = gradient(Ab,x); + g = Ab.gradient(x); d = g; // instead of negating gradient, alpha will be negated // init gamma and calculate threshold - gamma = dot(g,g) ; + gamma = dot(g,g); threshold = std::max(parameters_.epsilon_abs(), parameters_.epsilon() * parameters_.epsilon() * gamma); // Allocate and calculate A*d for first iteration @@ -86,9 +86,9 @@ namespace gtsam { double alpha = takeOptimalStep(x); // update gradient (or re-calculate at reset time) - if (k % parameters_.reset() == 0) g = gradient(Ab,x); + if (k % parameters_.reset() == 0) g = Ab.gradient(x); // axpy(alpha, Ab ^ Ad, g); // g += alpha*(Ab^Ad) - else transposeMultiplyAdd(Ab, alpha, Ad, g); + else Ab.transposeMultiplyAdd(alpha, Ad, g); // check for convergence double new_gamma = dot(g, g); @@ -105,14 +105,14 @@ namespace gtsam { else { double beta = new_gamma / gamma; // d = g + d*beta; - scal(beta, d); + d *= beta; axpy(1.0, g, d); } gamma = new_gamma; // In-place recalculation Ad <- A*d to avoid re-allocating Ad - multiplyInPlace(Ab, d, Ad); + Ab.multiplyInPlace(d, Ad); return false; } diff --git a/gtsam/linear/iterative.h b/gtsam/linear/iterative.h index 7e0609f37..1ba0a7423 100644 --- a/gtsam/linear/iterative.h +++ b/gtsam/linear/iterative.h @@ -68,28 +68,28 @@ namespace gtsam { * Print with optional string */ void print (const std::string& s = "System") const; + + /** gradient of objective function 0.5*|Ax-b_|^2 at x = A_'*(Ax-b_) */ + Vector gradient(const Vector& x) const { + return A() ^ (A() * x - b()); + } + + /** Apply operator A */ + Vector operator*(const Vector& x) const { + return A() * x; + } + + /** Apply operator A in place */ + void multiplyInPlace(const Vector& x, Vector& e) const { + e = A() * x; + } + + /** x += alpha* A'*e */ + void transposeMultiplyAdd(double alpha, const Vector& e, Vector& x) const { + gtsam::transposeMultiplyAdd(alpha, A(), e, x); + } }; - /** gradient of objective function 0.5*|Ax-b_|^2 at x = A_'*(Ax-b_) */ - inline Vector gradient(const System& system, const Vector& x) { - return system.A() ^ (system.A() * x - system.b()); - } - - /** Apply operator A */ - inline Vector operator*(const System& system, const Vector& x) { - return system.A() * x; - } - - /** Apply operator A in place */ - inline void multiplyInPlace(const System& system, const Vector& x, Vector& e) { - e = system.A() * x; - } - - /** x += alpha* A'*e */ - inline void transposeMultiplyAdd(const System& system, double alpha, const Vector& e, Vector& x) { - transposeMultiplyAdd(alpha,system.A(),e,x); - } - /** * Method of steepest gradients, System version */ diff --git a/gtsam/linear/linearAlgorithms-inst.h b/gtsam/linear/linearAlgorithms-inst.h new file mode 100644 index 000000000..4722a9b6d --- /dev/null +++ b/gtsam/linear/linearAlgorithms-inst.h @@ -0,0 +1,143 @@ +/* ---------------------------------------------------------------------------- + + * 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 linearAlgorithms-inst.h + * @brief Templated algorithms that are used in multiple places in linear + * @author Richard Roberts + */ + +#include +#include +#include + +#include +#include + +namespace gtsam +{ + namespace internal + { + namespace linearAlgorithms + { + /* ************************************************************************* */ + struct OptimizeData { + boost::optional parentData; + FastMap cliqueResults; + //VectorValues ancestorResults; + //VectorValues results; + }; + + /* ************************************************************************* */ + /** Pre-order visitor for back-substitution in a Bayes tree. The visitor function operator()() + * optimizes the clique given the solution for the parents, and returns the solution for the + * clique's frontal variables. In addition, it adds the solution to a global collected + * solution that will finally be returned to the user. The reason we pass the individual + * clique solutions between nodes is to avoid log(n) lookups over all variables, they instead + * then are only over a node's parent variables. */ + template + struct OptimizeClique + { + VectorValues collectedResult; + + OptimizeData operator()( + const boost::shared_ptr& clique, + OptimizeData& parentData) + { + OptimizeData myData; + myData.parentData = parentData; + // Take any ancestor results we'll need + BOOST_FOREACH(Key parent, clique->conditional_->parents()) + myData.cliqueResults.insert(std::make_pair(parent, myData.parentData->cliqueResults.at(parent))); + // Solve and store in our results + //collectedResult.insert(clique->conditional()->solve(collectedResult/*myData.ancestorResults*/)); + { + GaussianConditional& c = *clique->conditional(); + // Solve matrix + Vector xS; + { + // Count dimensions of vector + DenseIndex dim = 0; + FastVector parentPointers; + parentPointers.reserve(clique->conditional()->nrParents()); + BOOST_FOREACH(Key parent, clique->conditional()->parents()) { + parentPointers.push_back(myData.cliqueResults.at(parent)); + dim += parentPointers.back()->second.size(); + } + + // Fill parent vector + xS.resize(dim); + DenseIndex vectorPos = 0; + BOOST_FOREACH(const VectorValues::const_iterator& parentPointer, parentPointers) { + const Vector& parentVector = parentPointer->second; + xS.block(vectorPos,0,parentVector.size(),1) = parentVector.block(0,0,parentVector.size(),1); + vectorPos += parentVector.size(); + } + } + xS = c.getb() - c.get_S() * xS; + Vector soln = c.get_R().triangularView().solve(xS); + + // Check for indeterminant solution + if(soln.hasNaN()) throw IndeterminantLinearSystemException(c.keys().front()); + + // Insert solution into a VectorValues + DenseIndex vectorPosition = 0; + for(GaussianConditional::const_iterator frontal = c.beginFrontals(); frontal != c.endFrontals(); ++frontal) { + VectorValues::const_iterator r = + collectedResult.insert(*frontal, soln.segment(vectorPosition, c.getDim(frontal))); + myData.cliqueResults.insert(make_pair(r->first, r)); + vectorPosition += c.getDim(frontal); + } + } + return myData; + } + }; + + /* ************************************************************************* */ + //OptimizeData OptimizePreVisitor(const GaussianBayesTreeClique::shared_ptr& clique, OptimizeData& parentData) + //{ + // // Create data - holds a pointer to our parent, a copy of parent solution, and our results + // OptimizeData myData; + // myData.parentData = parentData; + // // Take any ancestor results we'll need + // BOOST_FOREACH(Key parent, clique->conditional_->parents()) + // myData.ancestorResults.insert(parent, myData.parentData->ancestorResults[parent]); + // // Solve and store in our results + // myData.results.insert(clique->conditional()->solve(myData.ancestorResults)); + // myData.ancestorResults.insert(myData.results); + // return myData; + //} + + /* ************************************************************************* */ + //void OptimizePostVisitor(const GaussianBayesTreeClique::shared_ptr& clique, OptimizeData& myData) + //{ + // // Conglomerate our results to the parent + // myData.parentData->results.insert(myData.results); + //} + + /* ************************************************************************* */ + template + VectorValues optimizeBayesTree(const BAYESTREE& bayesTree) + { + gttic(linear_optimizeBayesTree); + //internal::OptimizeData rootData; // Will hold final solution + //treeTraversal::DepthFirstForest(*this, rootData, internal::OptimizePreVisitor, internal::OptimizePostVisitor); + //return rootData.results; + OptimizeData rootData; + OptimizeClique preVisitor; + treeTraversal::no_op postVisitor; + TbbOpenMPMixedScope threadLimiter; // Limits OpenMP threads since we're mixing TBB and OpenMP + treeTraversal::DepthFirstForestParallel(bayesTree, rootData, preVisitor, postVisitor); + return preVisitor.collectedResult; + } + } + } +} diff --git a/gtsam/linear/linearExceptions.cpp b/gtsam/linear/linearExceptions.cpp new file mode 100644 index 000000000..3f62ed6d8 --- /dev/null +++ b/gtsam/linear/linearExceptions.cpp @@ -0,0 +1,63 @@ +/* ---------------------------------------------------------------------------- + + * 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 linearExceptions.cpp + * @brief Exceptions that may be thrown by linear solver components + * @author Richard Roberts + * @date Aug 17, 2012 + */ + +#include + +#include + +namespace gtsam { + + /* ************************************************************************* */ + const char* IndeterminantLinearSystemException::what() const throw() + { + if(!description_) + description_ = String( + "\nIndeterminant linear system detected while working near variable\n" + + boost::lexical_cast(j_) + ".\n" + "\n\ +Thrown when a linear system is ill-posed. The most common cause for this\n\ +error is having underconstrained variables. Mathematically, the system is\n\ +underdetermined. See the GTSAM Doxygen documentation at\n\ +http://borg.cc.gatech.edu/ on gtsam::IndeterminantLinearSystemException for\n\ +more information."); + return description_->c_str(); + } + + /* ************************************************************************* */ + const char* InvalidNoiseModel::what() const throw() { + if(description_.empty()) + description_ = (boost::format( + "A JacobianFactor was attempted to be constructed or modified to use a\n" + "noise model of incompatible dimension. The JacobianFactor has\n" + "dimensionality (i.e. length of error vector) %d but the provided noise\n" + "model has dimensionality %d.") % factorDims % noiseModelDims).str(); + return description_.c_str(); + } + + /* ************************************************************************* */ + const char* InvalidMatrixBlock::what() const throw() { + if(description_.empty()) + description_ = (boost::format( + "A JacobianFactor was attempted to be constructed with a matrix block of\n" + "inconsistent dimension. The JacobianFactor has %d rows (i.e. length of\n" + "error vector) but the provided matrix block has %d rows.") + % factorRows % blockRows).str(); + return description_.c_str(); + } + + } diff --git a/gtsam/linear/linearExceptions.h b/gtsam/linear/linearExceptions.h index 8b036df0a..63bc61e80 100644 --- a/gtsam/linear/linearExceptions.h +++ b/gtsam/linear/linearExceptions.h @@ -92,26 +92,55 @@ namespace gtsam { ordered in elimination order and occupy scalars in the same way as described for Jacobian columns in the previous bullet. */ - class IndeterminantLinearSystemException : public std::exception { - Index j_; - mutable std::string what_; + class GTSAM_EXPORT IndeterminantLinearSystemException : public ThreadsafeException { + Key j_; public: - IndeterminantLinearSystemException(Index j) throw() : j_(j) {} + IndeterminantLinearSystemException(Key j) throw() : j_(j) {} virtual ~IndeterminantLinearSystemException() throw() {} - Index nearbyVariable() const { return j_; } - virtual const char* what() const throw() { - if(what_.empty()) - what_ = -"\nIndeterminant linear system detected while working near variable with\n" -"index " + boost::lexical_cast(j_) + " in ordering.\n" -"\n\ -Thrown when a linear system is ill-posed. The most common cause for this\n\ -error is having underconstrained variables. Mathematically, the system is\n\ -either underdetermined, or its quadratic error function is concave in some\n\ -directions. See the GTSAM Doxygen documentation at http://borg.cc.gatech.edu/ \n\ -on gtsam::IndeterminantLinearSystemException for more information.\n"; - return what_.c_str(); - } + Key nearbyVariable() const { return j_; } + virtual const char* what() const throw(); }; -} + /* ************************************************************************* */ + /** An exception indicating that the noise model dimension passed into a + * JacobianFactor has a different dimensionality than the factor. */ + class GTSAM_EXPORT InvalidNoiseModel : public ThreadsafeException { + public: + const DenseIndex factorDims; ///< The dimensionality of the factor + const DenseIndex noiseModelDims; ///< The dimensionality of the noise model + + InvalidNoiseModel(DenseIndex factorDims, DenseIndex noiseModelDims) : + factorDims(factorDims), noiseModelDims(noiseModelDims) {} + virtual ~InvalidNoiseModel() throw() {} + + virtual const char* what() const throw(); + + private: + mutable std::string description_; + }; + + /* ************************************************************************* */ + /** An exception indicating that a matrix block passed into a + * JacobianFactor has a different dimensionality than the factor. */ + class GTSAM_EXPORT InvalidMatrixBlock : public ThreadsafeException { + public: + const DenseIndex factorRows; ///< The dimensionality of the factor + const DenseIndex blockRows; ///< The dimensionality of the noise model + + InvalidMatrixBlock(DenseIndex factorRows, DenseIndex blockRows) : + factorRows(factorRows), blockRows(blockRows) {} + virtual ~InvalidMatrixBlock() throw() {} + + virtual const char* what() const throw(); + + private: + mutable std::string description_; + }; + + /* ************************************************************************* */ + class InvalidDenseElimination : public ThreadsafeException { + public: + InvalidDenseElimination(const char *message) : ThreadsafeException(message) {} + }; + + } diff --git a/gtsam/linear/tests/CMakeLists.txt b/gtsam/linear/tests/CMakeLists.txt new file mode 100644 index 000000000..d1aafb4ea --- /dev/null +++ b/gtsam/linear/tests/CMakeLists.txt @@ -0,0 +1,6 @@ +gtsamAddTestsGlob(linear "test*.cpp" "" "gtsam") + +if(MSVC) + set_property(SOURCE "${CMAKE_CURRENT_SOURCE_DIR}/testSerializationLinear.cpp" + APPEND PROPERTY COMPILE_FLAGS "/bigobj") +endif() diff --git a/gtsam/linear/tests/testErrors.cpp b/gtsam/linear/tests/testErrors.cpp index c8932a45e..c78382316 100644 --- a/gtsam/linear/tests/testErrors.cpp +++ b/gtsam/linear/tests/testErrors.cpp @@ -29,12 +29,12 @@ using namespace gtsam; TEST( Errors, arithmetic ) { Errors e; - e += Vector_(2,1.0,2.0), Vector_(3,3.0,4.0,5.0); + e += (Vector(2) << 1.0,2.0), (Vector(3) << 3.0,4.0,5.0); DOUBLES_EQUAL(1+4+9+16+25,dot(e,e),1e-9); axpy(2.0,e,e); Errors expected; - expected += Vector_(2,3.0,6.0), Vector_(3,9.0,12.0,15.0); + expected += (Vector(2) << 3.0,6.0), (Vector(3) << 9.0,12.0,15.0); CHECK(assert_equal(expected,e)); } diff --git a/gtsam/linear/tests/testGaussianBayesNet.cpp b/gtsam/linear/tests/testGaussianBayesNet.cpp new file mode 100644 index 000000000..608e9b1e1 --- /dev/null +++ b/gtsam/linear/tests/testGaussianBayesNet.cpp @@ -0,0 +1,217 @@ +/* ---------------------------------------------------------------------------- + + * 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 testGaussianBayesNet.cpp + * @brief Unit tests for GaussianBayesNet + * @author Frank Dellaert + */ + +// STL/C++ +#include +#include +#include +#include +#include + +#include // for operator += +using namespace boost::assign; + +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +static const Key _x_=0, _y_=1; + +static GaussianBayesNet smallBayesNet = list_of + (GaussianConditional(_x_, (Vector(1) << 9.0), (Matrix(1, 1) << 1.0), _y_, (Matrix(1, 1) << 1.0))) + (GaussianConditional(_y_, (Vector(1) << 5.0), (Matrix(1, 1) << 1.0))); + +/* ************************************************************************* */ +TEST( GaussianBayesNet, matrix ) +{ + Matrix R; Vector d; + boost::tie(R,d) = smallBayesNet.matrix(); // find matrix and RHS + + Matrix R1 = (Matrix(2, 2) << + 1.0, 1.0, + 0.0, 1.0 + ); + Vector d1 = (Vector(2) << 9.0, 5.0); + + EXPECT(assert_equal(R,R1)); + EXPECT(assert_equal(d,d1)); +} + +/* ************************************************************************* */ +TEST( GaussianBayesNet, optimize ) +{ + VectorValues actual = smallBayesNet.optimize(); + + VectorValues expected = map_list_of + (_x_, (Vector(1) << 4.0)) + (_y_, (Vector(1) << 5.0)); + + EXPECT(assert_equal(expected,actual)); +} + +/* ************************************************************************* */ +TEST( GaussianBayesNet, optimizeIncomplete ) +{ + static GaussianBayesNet incompleteBayesNet = list_of + (GaussianConditional(_x_, (Vector(1) << 9.0), (Matrix(1, 1) << 1.0), _y_, (Matrix(1, 1) << 1.0))); + + VectorValues solutionForMissing = map_list_of + (_y_, (Vector(1) << 5.0)); + + VectorValues actual = incompleteBayesNet.optimize(solutionForMissing); + + VectorValues expected = map_list_of + (_x_, (Vector(1) << 4.0)) + (_y_, (Vector(1) << 5.0)); + + EXPECT(assert_equal(expected,actual)); +} + +/* ************************************************************************* */ +TEST( GaussianBayesNet, optimize3 ) +{ + // y = R*x, x=inv(R)*y + // 4 = 1 1 -1 + // 5 1 5 + // NOTE: we are supplying a new RHS here + + VectorValues expected = map_list_of + (_x_, (Vector(1) << -1.0)) + (_y_, (Vector(1) << 5.0)); + + // Test different RHS version + VectorValues gx = map_list_of + (_x_, (Vector(1) << 4.0)) + (_y_, (Vector(1) << 5.0)); + VectorValues actual = smallBayesNet.backSubstitute(gx); + EXPECT(assert_equal(expected, actual)); +} + +/* ************************************************************************* */ +TEST( GaussianBayesNet, backSubstituteTranspose ) +{ + // x=R'*y, expected=inv(R')*x + // 2 = 1 2 + // 5 1 1 3 + VectorValues + x = map_list_of + (_x_, (Vector(1) << 2.0)) + (_y_, (Vector(1) << 5.0)), + expected = map_list_of + (_x_, (Vector(1) << 2.0)) + (_y_, (Vector(1) << 3.0)); + + VectorValues actual = smallBayesNet.backSubstituteTranspose(x); + EXPECT(assert_equal(expected, actual)); +} + +/* ************************************************************************* */ +// Tests computing Determinant +TEST( GaussianBayesNet, DeterminantTest ) +{ + GaussianBayesNet cbn; + cbn += GaussianConditional( + 0, (Vector(2) << 3.0, 4.0 ), (Matrix(2, 2) << 1.0, 3.0, 0.0, 4.0 ), + 1, (Matrix(2, 2) << 2.0, 1.0, 2.0, 3.0), noiseModel::Isotropic::Sigma(2, 2.0)); + + cbn += GaussianConditional( + 1, (Vector(2) << 5.0, 6.0 ), (Matrix(2, 2) << 1.0, 1.0, 0.0, 3.0 ), + 2, (Matrix(2, 2) << 1.0, 0.0, 5.0, 2.0), noiseModel::Isotropic::Sigma(2, 2.0)); + + cbn += GaussianConditional( + 3, (Vector(2) << 7.0, 8.0 ), (Matrix(2, 2) << 1.0, 1.0, 0.0, 5.0 ), noiseModel::Isotropic::Sigma(2, 2.0)); + + double expectedDeterminant = 60.0 / 64.0; + double actualDeterminant = cbn.determinant(); + + EXPECT_DOUBLES_EQUAL( expectedDeterminant, actualDeterminant, 1e-9); +} + +/* ************************************************************************* */ +namespace { + double computeError(const GaussianBayesNet& gbn, const LieVector& values) + { + pair Rd = GaussianFactorGraph(gbn).jacobian(); + return 0.5 * (Rd.first * values - Rd.second).squaredNorm(); + } +} + +/* ************************************************************************* */ +TEST(GaussianBayesNet, ComputeSteepestDescentPoint) { + + // Create an arbitrary Bayes Net + GaussianBayesNet gbn; + gbn += GaussianConditional::shared_ptr(new GaussianConditional( + 0, (Vector(2) << 1.0,2.0), (Matrix(2, 2) << 3.0,4.0,0.0,6.0), + 3, (Matrix(2, 2) << 7.0,8.0,9.0,10.0), + 4, (Matrix(2, 2) << 11.0,12.0,13.0,14.0))); + gbn += GaussianConditional::shared_ptr(new GaussianConditional( + 1, (Vector(2) << 15.0,16.0), (Matrix(2, 2) << 17.0,18.0,0.0,20.0), + 2, (Matrix(2, 2) << 21.0,22.0,23.0,24.0), + 4, (Matrix(2, 2) << 25.0,26.0,27.0,28.0))); + gbn += GaussianConditional::shared_ptr(new GaussianConditional( + 2, (Vector(2) << 29.0,30.0), (Matrix(2, 2) << 31.0,32.0,0.0,34.0), + 3, (Matrix(2, 2) << 35.0,36.0,37.0,38.0))); + gbn += GaussianConditional::shared_ptr(new GaussianConditional( + 3, (Vector(2) << 39.0,40.0), (Matrix(2, 2) << 41.0,42.0,0.0,44.0), + 4, (Matrix(2, 2) << 45.0,46.0,47.0,48.0))); + gbn += GaussianConditional::shared_ptr(new GaussianConditional( + 4, (Vector(2) << 49.0,50.0), (Matrix(2, 2) << 51.0,52.0,0.0,54.0))); + + // Compute the Hessian numerically + Matrix hessian = numericalHessian( + boost::function(boost::bind(&computeError, gbn, _1)), + LieVector(Vector::Zero(GaussianFactorGraph(gbn).jacobian().first.cols()))); + + // Compute the gradient numerically + Vector gradient = numericalGradient( + boost::function(boost::bind(&computeError, gbn, _1)), + LieVector(Vector::Zero(GaussianFactorGraph(gbn).jacobian().first.cols()))); + + // Compute the gradient using dense matrices + Matrix augmentedHessian = GaussianFactorGraph(gbn).augmentedHessian(); + LONGS_EQUAL(11, (long)augmentedHessian.cols()); + Vector denseMatrixGradient = -augmentedHessian.col(10).segment(0,10); + EXPECT(assert_equal(gradient, denseMatrixGradient, 1e-5)); + + // Compute the steepest descent point + double step = -gradient.squaredNorm() / (gradient.transpose() * hessian * gradient)(0); + Vector expected = gradient * step; + + // Compute the steepest descent point with the dogleg function + VectorValues actual = gbn.optimizeGradientSearch(); + + // Check that points agree + FastVector keys = list_of(0)(1)(2)(3)(4); + Vector actualAsVector = actual.vector(keys); + EXPECT(assert_equal(expected, actualAsVector, 1e-5)); + + // Check that point causes a decrease in error + double origError = GaussianFactorGraph(gbn).error(VectorValues::Zero(actual)); + double newError = GaussianFactorGraph(gbn).error(actual); + EXPECT(newError < origError); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ diff --git a/gtsam/linear/tests/testGaussianBayesTree.cpp b/gtsam/linear/tests/testGaussianBayesTree.cpp new file mode 100644 index 000000000..8f37cac0c --- /dev/null +++ b/gtsam/linear/tests/testGaussianBayesTree.cpp @@ -0,0 +1,290 @@ +/* ---------------------------------------------------------------------------- + + * 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 testGaussianBayesTree.cpp + * @date Jul 8, 2010 + * @author Kai Ni + */ + +#include +#include + +#include +#include // for operator += +#include // for operator += +using namespace boost::assign; + +#include +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +namespace { + const Key x1=1, x2=2, x3=3, x4=4; + const SharedDiagonal chainNoise = noiseModel::Isotropic::Sigma(1, 0.5); + const GaussianFactorGraph chain = list_of + (JacobianFactor(x2, (Matrix(1, 1) << 1.), x1, (Matrix(1, 1) << 1.), (Vector(1) << 1.), chainNoise)) + (JacobianFactor(x2, (Matrix(1, 1) << 1.), x3, (Matrix(1, 1) << 1.), (Vector(1) << 1.), chainNoise)) + (JacobianFactor(x3, (Matrix(1, 1) << 1.), x4, (Matrix(1, 1) << 1.), (Vector(1) << 1.), chainNoise)) + (JacobianFactor(x4, (Matrix(1, 1) << 1.), (Vector(1) << 1.), chainNoise)); + const Ordering chainOrdering = Ordering(list_of(x2)(x1)(x3)(x4)); + + /* ************************************************************************* */ + // Helper functions for below + GaussianBayesTreeClique::shared_ptr MakeClique(const GaussianConditional& conditional) + { + return boost::make_shared( + boost::make_shared(conditional)); + } + + template + GaussianBayesTreeClique::shared_ptr MakeClique( + const GaussianConditional& conditional, const CHILDREN& children) + { + GaussianBayesTreeClique::shared_ptr clique = + boost::make_shared( + boost::make_shared(conditional)); + clique->children.assign(children.begin(), children.end()); + for(typename CHILDREN::const_iterator child = children.begin(); child != children.end(); ++child) + (*child)->parent_ = clique; + return clique; + } +} + +/* ************************************************************************* */ +/** + * x1 - x2 - x3 - x4 + * x3 x4 + * x2 x1 : x3 + * + * x2 x1 x3 x4 b + * 1 1 1 + * 1 1 1 + * 1 1 1 + * 1 1 + * + * 1 0 0 1 + */ +TEST( GaussianBayesTree, eliminate ) +{ + GaussianBayesTree bt = *chain.eliminateMultifrontal(chainOrdering); + + Matrix two = (Matrix(1, 1) << 2.); + Matrix one = (Matrix(1, 1) << 1.); + + GaussianBayesTree bayesTree_expected; + bayesTree_expected.insertRoot( + MakeClique(GaussianConditional(pair_list_of(x3, (Matrix(2, 1) << 2., 0.)) (x4, (Matrix(2, 1) << 2., 2.)), 2, (Vector(2) << 2., 2.)), list_of + (MakeClique(GaussianConditional(pair_list_of(x2, (Matrix(2, 1) << -2.*sqrt(2.), 0.)) (x1, (Matrix(2, 1) << -sqrt(2.), -sqrt(2.))) (x3, (Matrix(2, 1) << -sqrt(2.), sqrt(2.))), 2, (Vector(2) << -2.*sqrt(2.), 0.)))))); + + EXPECT(assert_equal(bayesTree_expected, bt)); +} + +/* ************************************************************************* */ +TEST( GaussianBayesTree, optimizeMultiFrontal ) +{ + VectorValues expected = pair_list_of + (x1, (Vector(1) << 0.)) + (x2, (Vector(1) << 1.)) + (x3, (Vector(1) << 0.)) + (x4, (Vector(1) << 1.)); + + VectorValues actual = chain.eliminateMultifrontal(chainOrdering)->optimize(); + EXPECT(assert_equal(expected,actual)); +} + +/* ************************************************************************* */ +TEST(GaussianBayesTree, complicatedMarginal) { + + // Create the conditionals to go in the BayesTree + GaussianBayesTree bt; + bt.insertRoot( + MakeClique(GaussianConditional(pair_list_of (11, (Matrix(3,1) << 0.0971, 0, 0)) + (12, (Matrix(3,2) << 0.3171, 0.4387, 0.9502, 0.3816, 0, 0.7655)), + 2, (Vector(3) << 0.2638, 0.1455, 0.1361)), list_of + (MakeClique(GaussianConditional(pair_list_of (9, (Matrix(3,1) << 0.7952, 0, 0)) + (10, (Matrix(3,2) << 0.4456, 0.7547, 0.6463, 0.2760, 0, 0.6797)) + (11, (Matrix(3,1) << 0.6551, 0.1626, 0.1190)) + (12, (Matrix(3,2) << 0.4984, 0.5853, 0.9597, 0.2238, 0.3404, 0.7513)), + 2, (Vector(3) << 0.4314, 0.9106, 0.1818)))) + (MakeClique(GaussianConditional(pair_list_of (7, (Matrix(3,1) << 0.2551, 0, 0)) + (8, (Matrix(3,2) << 0.8909, 0.1386, 0.9593, 0.1493, 0, 0.2575)) + (11, (Matrix(3,1) << 0.8407, 0.2543, 0.8143)), + 2, (Vector(3) << 0.3998, 0.2599, 0.8001)), list_of + (MakeClique(GaussianConditional(pair_list_of (5, (Matrix(3,1) << 0.2435, 0, 0)) + (6, (Matrix(3,2) << 0.4733, 0.1966, 0.3517, 0.2511, 0.8308, 0.0)) + // NOTE the non-upper-triangular form + // here since this test was written when we had column permutations + // from LDL. The code still works currently (does not enfore + // upper-triangularity in this case) but this test will need to be + // redone if this stops working in the future + (7, (Matrix(3,1) << 0.5853, 0.5497, 0.9172)) + (8, (Matrix(3,2) << 0.2858, 0.3804, 0.7572, 0.5678, 0.7537, 0.0759)), + 2, (Vector(3) << 0.8173, 0.8687, 0.0844)), list_of + (MakeClique(GaussianConditional(pair_list_of (3, (Matrix(3,1) << 0.0540, 0, 0)) + (4, (Matrix(3,2) << 0.9340, 0.4694, 0.1299, 0.0119, 0, 0.3371)) + (6, (Matrix(3,2) << 0.1622, 0.5285, 0.7943, 0.1656, 0.3112, 0.6020)), + 2, (Vector(3) << 0.9619, 0.0046, 0.7749)))) + (MakeClique(GaussianConditional(pair_list_of (1, (Matrix(3,1) << 0.2630, 0, 0)) + (2, (Matrix(3,2) << 0.7482, 0.2290, 0.4505, 0.9133, 0, 0.1524)) + (5, (Matrix(3,1) << 0.8258, 0.5383, 0.9961)), + 2, (Vector(3) << 0.0782, 0.4427, 0.1067)))))))))); + + // Marginal on 5 + Matrix expectedCov = (Matrix(1,1) << 236.5166); + //GaussianConditional actualJacobianChol = *bt.marginalFactor(5, EliminateCholesky); + GaussianConditional actualJacobianQR = *bt.marginalFactor(5, EliminateQR); + //EXPECT(assert_equal(actualJacobianChol, actualJacobianQR)); // Check that Chol and QR obtained marginals are the same + LONGS_EQUAL(1, (long)actualJacobianQR.rows()); + LONGS_EQUAL(1, (long)actualJacobianQR.size()); + LONGS_EQUAL(5, (long)actualJacobianQR.keys()[0]); + Matrix actualA = actualJacobianQR.getA(actualJacobianQR.begin()); + Matrix actualCov = inverse(actualA.transpose() * actualA); + EXPECT(assert_equal(expectedCov, actualCov, 1e-1)); + + // Marginal on 6 +// expectedCov = (Matrix(2,2) << +// 8471.2, 2886.2, +// 2886.2, 1015.8); + expectedCov = (Matrix(2,2) << + 1015.8, 2886.2, + 2886.2, 8471.2); + //actualJacobianChol = bt.marginalFactor(6, EliminateCholesky); + actualJacobianQR = *bt.marginalFactor(6, EliminateQR); + //EXPECT(assert_equal(actualJacobianChol, actualJacobianQR)); // Check that Chol and QR obtained marginals are the same + LONGS_EQUAL(2, (long)actualJacobianQR.rows()); + LONGS_EQUAL(1, (long)actualJacobianQR.size()); + LONGS_EQUAL(6, (long)actualJacobianQR.keys()[0]); + actualA = actualJacobianQR.getA(actualJacobianQR.begin()); + actualCov = inverse(actualA.transpose() * actualA); + EXPECT(assert_equal(expectedCov, actualCov, 1e1)); +} + +namespace { + /* ************************************************************************* */ + double computeError(const GaussianBayesTree& gbt, const LieVector& values) + { + pair Rd = GaussianFactorGraph(gbt).jacobian(); + return 0.5 * (Rd.first * values - Rd.second).squaredNorm(); + } +} + +/* ************************************************************************* */ +TEST(GaussianBayesTree, ComputeSteepestDescentPointBT) { + + // Create an arbitrary Bayes Tree + GaussianBayesTree bt; + bt.insertRoot(MakeClique(GaussianConditional( + pair_list_of + (2, (Matrix(6, 2) << + 31.0,32.0, + 0.0,34.0, + 0.0,0.0, + 0.0,0.0, + 0.0,0.0, + 0.0,0.0)) + (3, (Matrix(6, 2) << + 35.0,36.0, + 37.0,38.0, + 41.0,42.0, + 0.0,44.0, + 0.0,0.0, + 0.0,0.0)) + (4, (Matrix(6, 2) << + 0.0,0.0, + 0.0,0.0, + 45.0,46.0, + 47.0,48.0, + 51.0,52.0, + 0.0,54.0)), + 3, (Vector(6) << 29.0,30.0,39.0,40.0,49.0,50.0)), list_of + (MakeClique(GaussianConditional( + pair_list_of + (0, (Matrix(4, 2) << + 3.0,4.0, + 0.0,6.0, + 0.0,0.0, + 0.0,0.0)) + (1, (Matrix(4, 2) << + 0.0,0.0, + 0.0,0.0, + 17.0,18.0, + 0.0,20.0)) + (2, (Matrix(4, 2) << + 0.0,0.0, + 0.0,0.0, + 21.0,22.0, + 23.0,24.0)) + (3, (Matrix(4, 2) << + 7.0,8.0, + 9.0,10.0, + 0.0,0.0, + 0.0,0.0)) + (4, (Matrix(4, 2) << + 11.0,12.0, + 13.0,14.0, + 25.0,26.0, + 27.0,28.0)), + 2, (Vector(4) << 1.0,2.0,15.0,16.0)))))); + + // Compute the Hessian numerically + Matrix hessian = numericalHessian( + boost::function(boost::bind(&computeError, bt, _1)), + LieVector(Vector::Zero(GaussianFactorGraph(bt).jacobian().first.cols()))); + + // Compute the gradient numerically + Vector gradient = numericalGradient( + boost::function(boost::bind(&computeError, bt, _1)), + LieVector(Vector::Zero(GaussianFactorGraph(bt).jacobian().first.cols()))); + + // Compute the gradient using dense matrices + Matrix augmentedHessian = GaussianFactorGraph(bt).augmentedHessian(); + LONGS_EQUAL(11, (long)augmentedHessian.cols()); + Vector denseMatrixGradient = -augmentedHessian.col(10).segment(0,10); + EXPECT(assert_equal(gradient, denseMatrixGradient, 1e-5)); + + // Compute the steepest descent point + double step = -gradient.squaredNorm() / (gradient.transpose() * hessian * gradient)(0); + Vector expected = gradient * step; + + // Known steepest descent point from Bayes' net version + VectorValues expectedFromBN = pair_list_of + (0, (Vector(2) << 0.000129034, 0.000688183)) + (1, (Vector(2) << 0.0109679, 0.0253767)) + (2, (Vector(2) << 0.0680441, 0.114496)) + (3, (Vector(2) << 0.16125, 0.241294)) + (4, (Vector(2) << 0.300134, 0.423233)); + + // Compute the steepest descent point with the dogleg function + VectorValues actual = bt.optimizeGradientSearch(); + + // Check that points agree + FastVector keys = list_of(0)(1)(2)(3)(4); + EXPECT(assert_equal(expected, actual.vector(keys), 1e-5)); + EXPECT(assert_equal(expectedFromBN, actual, 1e-5)); + + // Check that point causes a decrease in error + double origError = GaussianFactorGraph(bt).error(VectorValues::Zero(actual)); + double newError = GaussianFactorGraph(bt).error(actual); + EXPECT(newError < origError); +} + + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ diff --git a/gtsam/linear/tests/testGaussianConditional.cpp b/gtsam/linear/tests/testGaussianConditional.cpp index 86d4b28b4..6bb8a05e1 100644 --- a/gtsam/linear/tests/testGaussianConditional.cpp +++ b/gtsam/linear/tests/testGaussianConditional.cpp @@ -19,6 +19,8 @@ #include #include +#include +#include #include #include #include @@ -28,89 +30,71 @@ #include #include #include +#include +#include using namespace gtsam; using namespace std; using namespace boost::assign; static const double tol = 1e-5; -static const Index _x_=0, _x1_=1, _l1_=2; -Matrix R = Matrix_(2,2, +static Matrix R = (Matrix(2, 2) << -12.1244, -5.1962, 0., 4.6904); /* ************************************************************************* */ TEST(GaussianConditional, constructor) { - Matrix S1 = Matrix_(2,2, + Matrix S1 = (Matrix(2, 2) << -5.2786, -8.6603, 5.0254, 5.5432); - Matrix S2 = Matrix_(2,2, + Matrix S2 = (Matrix(2, 2) << -10.5573, -5.9385, 5.5737, 3.0153); - Matrix S3 = Matrix_(2,2, + Matrix S3 = (Matrix(2, 2) << -11.3820, -7.2581, -3.0153, -3.5635); - Vector d = Vector_(2, 1.0, 2.0); - Vector s = Vector_(2, 3.0, 4.0); + Vector d = (Vector(2) << 1.0, 2.0); + SharedDiagonal s = noiseModel::Diagonal::Sigmas((Vector(2) << 3.0, 4.0)); - list > terms; - terms += - make_pair(3, S1), - make_pair(5, S2), - make_pair(7, S3); + vector > terms = pair_list_of + (1, R) + (3, S1) + (5, S2) + (7, S3); - GaussianConditional actual(1, d, R, terms, s); + GaussianConditional actual(terms, 1, d, s); GaussianConditional::const_iterator it = actual.beginFrontals(); - EXPECT(assert_equal(Index(1), *it)); + EXPECT(assert_equal(Key(1), *it)); EXPECT(assert_equal(R, actual.get_R())); ++ it; EXPECT(it == actual.endFrontals()); it = actual.beginParents(); - EXPECT(assert_equal(Index(3), *it)); + EXPECT(assert_equal(Key(3), *it)); EXPECT(assert_equal(S1, actual.get_S(it))); ++ it; - EXPECT(assert_equal(Index(5), *it)); + EXPECT(assert_equal(Key(5), *it)); EXPECT(assert_equal(S2, actual.get_S(it))); ++ it; - EXPECT(assert_equal(Index(7), *it)); + EXPECT(assert_equal(Key(7), *it)); EXPECT(assert_equal(S3, actual.get_S(it))); ++it; EXPECT(it == actual.endParents()); EXPECT(assert_equal(d, actual.get_d())); - EXPECT(assert_equal(s, actual.get_sigmas())); + EXPECT(assert_equal(*s, *actual.get_model())); // test copy constructor GaussianConditional copied(actual); EXPECT(assert_equal(d, copied.get_d())); - EXPECT(assert_equal(s, copied.get_sigmas())); - EXPECT(assert_equal(R, copied.get_R())); -} - -/* ************************************************************************* */ - -GaussianConditional construct() { - Vector d = Vector_(2, 1.0, 2.0); - Vector s = Vector_(2, 3.0, 4.0); - GaussianConditional::shared_ptr shared(new GaussianConditional(1, d, R, s)); - return *shared; -} - -TEST(GaussianConditional, return_value) -{ - Vector d = Vector_(2, 1.0, 2.0); - Vector s = Vector_(2, 3.0, 4.0); - GaussianConditional copied = construct(); - EXPECT(assert_equal(d, copied.get_d())); - EXPECT(assert_equal(s, copied.get_sigmas())); + EXPECT(assert_equal(*s, *copied.get_model())); EXPECT(assert_equal(R, copied.get_R())); } @@ -130,16 +114,13 @@ TEST( GaussianConditional, equals ) R(0,0) = 0.1 ; R(1,0) = 0.3; R(0,1) = 0.0 ; R(1,1) = 0.34; - Vector tau(2); - tau(0) = 1.0; - tau(1) = 0.34; + SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(2) << 1.0, 0.34)); - Vector d(2); - d(0) = 0.2; d(1) = 0.5; + Vector d = (Vector(2) << 0.2, 0.5); GaussianConditional - expected(_x_,d, R, _x1_, A1, _l1_, A2, tau), - actual(_x_,d, R, _x1_, A1, _l1_, A2, tau); + expected(1, d, R, 2, A1, 10, A2, model), + actual(1, d, R, 2, A1, 10, A2, model); EXPECT( expected.equals(actual) ); } @@ -152,77 +133,66 @@ TEST( GaussianConditional, solve ) expectedX(0) = 20-3-11 ; expectedX(1) = 40-7-15; // create a conditional Gaussian node - Matrix R = Matrix_(2,2, 1., 0., + Matrix R = (Matrix(2, 2) << 1., 0., 0., 1.); - Matrix A1 = Matrix_(2,2, 1., 2., + Matrix A1 = (Matrix(2, 2) << 1., 2., 3., 4.); - Matrix A2 = Matrix_(2,2, 5., 6., + Matrix A2 = (Matrix(2, 2) << 5., 6., 7., 8.); - Vector d(2); - d(0) = 20.0; d(1) = 40.0; + Vector d(2); d << 20.0, 40.0; - Vector tau = ones(2); + GaussianConditional cg(1, d, R, 2, A1, 10, A2); - GaussianConditional cg(_x_, d, R, _x1_, A1, _l1_, A2, tau); + Vector sx1(2); sx1 << 1.0, 1.0; + Vector sl1(2); sl1 << 1.0, 1.0; - Vector sx1(2); - sx1(0) = 1.0; sx1(1) = 1.0; + VectorValues expected = map_list_of + (1, expectedX) + (2, sx1) + (10, sl1); - Vector sl1(2); - sl1(0) = 1.0; sl1(1) = 1.0; + VectorValues solution = map_list_of + (2, sx1) // parents + (10, sl1); + solution.insert(cg.solve(solution)); - VectorValues solution(vector(3, 2)); - solution[_x_] = d; - solution[_x1_] = sx1; // parents - solution[_l1_] = sl1; - - VectorValues expected(vector(3, 2)); - expected[_x_] = expectedX; - expected[_x1_] = sx1; - expected[_l1_] = sl1; - cg.solveInPlace(solution); - - EXPECT(assert_equal(expected, solution, 0.0001)); + EXPECT(assert_equal(expected, solution, tol)); } /* ************************************************************************* */ TEST( GaussianConditional, solve_simple ) { - Matrix full_matrix = Matrix_(4, 7, + // 2 variables, frontal has dim=4 + VerticalBlockMatrix blockMatrix(list_of(4)(2)(1), 4); + blockMatrix.matrix() << 1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 0.1, 0.0, 1.0, 0.0, 2.0, 0.0, 3.0, 0.2, 0.0, 0.0, 3.0, 0.0, 4.0, 0.0, 0.3, - 0.0, 0.0, 0.0, 3.0, 0.0, 4.0, 0.4); + 0.0, 0.0, 0.0, 3.0, 0.0, 4.0, 0.4; // solve system as a non-multifrontal version first - // 2 variables, frontal has dim=4 - vector dims; dims += 4, 2, 1; - GaussianConditional::rsd_type matrices(full_matrix, dims.begin(), dims.end()); - Vector sigmas = ones(4); - vector cgdims; cgdims += _x_, _x1_; - GaussianConditional cg(cgdims.begin(), cgdims.end(), 1, matrices, sigmas); + GaussianConditional cg(list_of(1)(2), 1, blockMatrix); // partial solution - Vector sx1 = Vector_(2, 9.0, 10.0); + Vector sx1 = (Vector(2) << 9.0, 10.0); - // elimination order; _x_, _x1_ - vector vdim; vdim += 4, 2; - VectorValues actual(vdim); - actual[_x1_] = sx1; // parent + // elimination order: 1, 2 + VectorValues actual = map_list_of + (2, sx1); // parent - VectorValues expected(vdim); - expected[_x1_] = sx1; - expected[_x_] = Vector_(4, -3.1,-3.4,-11.9,-13.2); + VectorValues expected = map_list_of + (2, sx1) + (1, (Vector(4) << -3.1,-3.4,-11.9,-13.2)); // verify indices/size - EXPECT_LONGS_EQUAL(2, cg.size()); - EXPECT_LONGS_EQUAL(4, cg.dim()); + EXPECT_LONGS_EQUAL(2, (long)cg.size()); + EXPECT_LONGS_EQUAL(4, (long)cg.rows()); // solve and verify - cg.solveInPlace(actual); + actual.insert(cg.solve(actual)); EXPECT(assert_equal(expected, actual, tol)); } @@ -230,93 +200,87 @@ TEST( GaussianConditional, solve_simple ) TEST( GaussianConditional, solve_multifrontal ) { // create full system, 3 variables, 2 frontals, all 2 dim - Matrix full_matrix = Matrix_(4, 7, + VerticalBlockMatrix blockMatrix(list_of(2)(2)(2)(1), 4); + blockMatrix.matrix() << 1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 0.1, 0.0, 1.0, 0.0, 2.0, 0.0, 3.0, 0.2, 0.0, 0.0, 3.0, 0.0, 4.0, 0.0, 0.3, - 0.0, 0.0, 0.0, 3.0, 0.0, 4.0, 0.4); + 0.0, 0.0, 0.0, 3.0, 0.0, 4.0, 0.4; // 3 variables, all dim=2 - vector dims; dims += 2, 2, 2, 1; - GaussianConditional::rsd_type matrices(full_matrix, dims.begin(), dims.end()); - Vector sigmas = ones(4); - vector cgdims; cgdims += _x_, _x1_, _l1_; - GaussianConditional cg(cgdims.begin(), cgdims.end(), 2, matrices, sigmas); + GaussianConditional cg(list_of(1)(2)(10), 2, blockMatrix); - EXPECT(assert_equal(Vector_(4, 0.1, 0.2, 0.3, 0.4), cg.get_d())); + EXPECT(assert_equal(Vector(blockMatrix.full().rightCols(1)), cg.get_d())); // partial solution - Vector sl1 = Vector_(2, 9.0, 10.0); + Vector sl1 = (Vector(2) << 9.0, 10.0); // elimination order; _x_, _x1_, _l1_ - VectorValues actual(vector(3, 2)); - actual[_l1_] = sl1; // parent + VectorValues actual = map_list_of + (10, sl1); // parent - VectorValues expected(vector(3, 2)); - expected[_x_] = Vector_(2, -3.1,-3.4); - expected[_x1_] = Vector_(2, -11.9,-13.2); - expected[_l1_] = sl1; + VectorValues expected = map_list_of + (1, (Vector)(Vector(2) << -3.1,-3.4)) + (2, (Vector)(Vector(2) << -11.9,-13.2)) + (10, sl1); // verify indices/size - EXPECT_LONGS_EQUAL(3, cg.size()); - EXPECT_LONGS_EQUAL(4, cg.dim()); + EXPECT_LONGS_EQUAL(3, (long)cg.size()); + EXPECT_LONGS_EQUAL(4, (long)cg.rows()); // solve and verify - cg.solveInPlace(actual); + actual.insert(cg.solve(actual)); EXPECT(assert_equal(expected, actual, tol)); } /* ************************************************************************* */ TEST( GaussianConditional, solveTranspose ) { - static const Index _y_=1; /** create small Chordal Bayes Net x <- y * x y d * 1 1 9 * 1 5 */ - Matrix R11 = Matrix_(1, 1, 1.0), S12 = Matrix_(1, 1, 1.0); - Matrix R22 = Matrix_(1, 1, 1.0); + Matrix R11 = (Matrix(1, 1) << 1.0), S12 = (Matrix(1, 1) << 1.0); + Matrix R22 = (Matrix(1, 1) << 1.0); Vector d1(1), d2(1); d1(0) = 9; d2(0) = 5; - Vector tau(1); - tau(0) = 1.0; // define nodes and specify in reverse topological sort (i.e. parents last) - GaussianConditional::shared_ptr Px_y(new GaussianConditional(_x_, d1, R11, _y_, S12, tau)); - GaussianConditional::shared_ptr Py(new GaussianConditional(_y_, d2, R22, tau)); - GaussianBayesNet cbn; - cbn.push_back(Px_y); - cbn.push_back(Py); + GaussianBayesNet cbn = list_of + (GaussianConditional(1, d1, R11, 2, S12)) + (GaussianConditional(1, d2, R22)); // x=R'*y, y=inv(R')*x // 2 = 1 2 // 5 1 1 3 - VectorValues y(vector(2,1)), x(vector(2,1)); - x[_x_] = Vector_(1,2.); - x[_y_] = Vector_(1,5.); - y[_x_] = Vector_(1,2.); - y[_y_] = Vector_(1,3.); + VectorValues + x = map_list_of + (1, (Vector(1) << 2.)) + (2, (Vector(1) << 5.)), + y = map_list_of + (1, (Vector(1) << 2.)) + (2, (Vector(1) << 3.)); // test functional version - VectorValues actual = backSubstituteTranspose(cbn,x); - CHECK(assert_equal(y,actual)); + VectorValues actual = cbn.backSubstituteTranspose(x); + CHECK(assert_equal(y, actual)); } /* ************************************************************************* */ TEST( GaussianConditional, information ) { // Create R matrix - Matrix R = (Matrix(4,4) << + Matrix R(4,4); R << 1, 2, 3, 4, 0, 5, 6, 7, 0, 0, 8, 9, - 0, 0, 0, 10).finished(); + 0, 0, 0, 10; // Create conditional - GaussianConditional conditional(0, Vector::Zero(4), R, Vector::Constant(4, 1.0)); + GaussianConditional conditional(0, Vector::Zero(4), R); // Expected information matrix (using permuted R) Matrix IExpected = R.transpose() * R; @@ -330,21 +294,21 @@ TEST( GaussianConditional, information ) { TEST( GaussianConditional, isGaussianFactor ) { // Create R matrix - Matrix R = (Matrix(4,4) << + Matrix R(4,4); R << 1, 2, 3, 4, 0, 5, 6, 7, 0, 0, 8, 9, - 0, 0, 0, 10).finished(); + 0, 0, 0, 10; // Create a conditional - GaussianConditional conditional(0, Vector::Zero(4), R, Vector::Constant(4, 1.0)); + GaussianConditional conditional(0, Vector::Zero(4), R); // Expected information matrix computed by conditional Matrix IExpected = conditional.information(); // Expected information matrix computed by a factor - JacobianFactor jf = *conditional.toFactor(); - Matrix IActual = jf.getA(jf.begin()).transpose() * jf.getA(jf.begin()); + JacobianFactor jf = conditional; + Matrix IActual = jf.information(); EXPECT(assert_equal(IExpected, IActual)); } diff --git a/gtsam/linear/tests/testGaussianDensity.cpp b/gtsam/linear/tests/testGaussianDensity.cpp index 7e7ecf5f8..f87862399 100644 --- a/gtsam/linear/tests/testGaussianDensity.cpp +++ b/gtsam/linear/tests/testGaussianDensity.cpp @@ -25,16 +25,16 @@ using namespace std; /* ************************************************************************* */ TEST(GaussianDensity, constructor) { - Matrix R = Matrix_(2,2, + Matrix R = (Matrix(2,2) << -12.1244, -5.1962, 0., 4.6904); - Vector d = Vector_(2, 1.0, 2.0), s = Vector_(2, 3.0, 4.0); - GaussianConditional conditional(1, d, R, s); + Vector d = (Vector(2) << 1.0, 2.0), s = (Vector(2) << 3.0, 4.0); + GaussianConditional conditional(1, d, R, noiseModel::Diagonal::Sigmas(s)); GaussianDensity copied(conditional); EXPECT(assert_equal(d, copied.get_d())); - EXPECT(assert_equal(s, copied.get_sigmas())); + EXPECT(assert_equal(s, copied.get_model()->sigmas())); } /* ************************************************************************* */ diff --git a/gtsam/linear/tests/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp index bdc64d849..a81c2243a 100644 --- a/gtsam/linear/tests/testGaussianFactorGraph.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraph.cpp @@ -10,10 +10,12 @@ * -------------------------------------------------------------------------- */ /** - * @file testGaussianFactor.cpp - * @brief Unit tests for Linear Factor + * @file testGaussianFactorGraphUnordered.cpp + * @brief Unit tests for Linear Factor Graph * @author Christian Potthast * @author Frank Dellaert + * @author Luca Carlone + * @author Richard Roberts **/ #include // for operator += @@ -23,33 +25,19 @@ using namespace boost::assign; #include #include +#include #include #include #include -#include +#include +#include using namespace std; using namespace gtsam; -using namespace boost; - -static SharedDiagonal - sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1), sigma_02 = noiseModel::Isotropic::Sigma(2,0.2), - constraintModel = noiseModel::Constrained::All(2); - -static GaussianFactorGraph createSimpleGaussianFactorGraph() { - GaussianFactorGraph fg; - SharedDiagonal unit2 = noiseModel::Unit::Create(2); - // linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_] - fg.add(2, 10*eye(2), -1.0*ones(2), unit2); - // odometry between x1 and x2: x2-x1=[0.2;-0.1] - fg.add(2, -10*eye(2), 0, 10*eye(2), Vector_(2, 2.0, -1.0), unit2); - // measurement between x1 and l1: l1-x1=[0.0;0.2] - fg.add(2, -5*eye(2), 1, 5*eye(2), Vector_(2, 0.0, 1.0), unit2); - // measurement between x2 and l1: l1-x2=[-0.2;0.3] - fg.add(0, -5*eye(2), 1, 5*eye(2), Vector_(2, -1.0, 1.5), unit2); - return fg; -} +//static SharedDiagonal +// sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1), sigma_02 = noiseModel::Isotropic::Sigma(2,0.2), +// constraintModel = noiseModel::Constrained::All(2); /* ************************************************************************* */ TEST(GaussianFactorGraph, initialization) { @@ -57,17 +45,17 @@ TEST(GaussianFactorGraph, initialization) { GaussianFactorGraph fg; SharedDiagonal unit2 = noiseModel::Unit::Create(2); - fg.add(0, 10*eye(2), -1.0*ones(2), unit2); - fg.add(0, -10*eye(2),1, 10*eye(2), Vector_(2, 2.0, -1.0), unit2); - fg.add(0, -5*eye(2), 2, 5*eye(2), Vector_(2, 0.0, 1.0), unit2); - fg.add(1, -5*eye(2), 2, 5*eye(2), Vector_(2, -1.0, 1.5), unit2); + fg += + JacobianFactor(0, 10*eye(2), -1.0*ones(2), unit2), + JacobianFactor(0, -10*eye(2),1, 10*eye(2), (Vector(2) << 2.0, -1.0), unit2), + JacobianFactor(0, -5*eye(2), 2, 5*eye(2), (Vector(2) << 0.0, 1.0), unit2), + JacobianFactor(1, -5*eye(2), 2, 5*eye(2), (Vector(2) << -1.0, 1.5), unit2); - EXPECT_LONGS_EQUAL(4, fg.size()); - JacobianFactor factor = *boost::dynamic_pointer_cast(fg[0]); + EXPECT_LONGS_EQUAL(4, (long)fg.size()); // Test sparse, which takes a vector and returns a matrix, used in MATLAB // Note that this the augmented vector and the RHS is in column 7 - Matrix expectedIJS = Matrix_(3,22, + Matrix expectedIJS = (Matrix(3, 22) << 1., 2., 1., 2., 3., 4., 3., 4., 3., 4., 5., 6., 5., 6., 5., 6., 7., 8., 7., 8., 7., 8., 1., 2., 7., 7., 1., 2., 3., 4., 7., 7., 1., 2., 5., 6., 7., 7., 3., 4., 5., 6., 7., 7., 10., 10., -1., -1., -10., -10., 10., 10., 2., -1., -5., -5., 5., 5., 0., 1., -5., -5., 5., 5., -1., 1.5 @@ -76,316 +64,6 @@ TEST(GaussianFactorGraph, initialization) { EQUALITY(expectedIJS, actualIJS); } -/* ************************************************************************* */ -TEST(GaussianFactorGraph, CombineJacobians) -{ - Matrix A01 = Matrix_(3,3, - 1.0, 0.0, 0.0, - 0.0, 1.0, 0.0, - 0.0, 0.0, 1.0); - Vector b0 = Vector_(3, 1.5, 1.5, 1.5); - Vector s0 = Vector_(3, 1.6, 1.6, 1.6); - - Matrix A10 = Matrix_(3,3, - 2.0, 0.0, 0.0, - 0.0, 2.0, 0.0, - 0.0, 0.0, 2.0); - Matrix A11 = Matrix_(3,3, - -2.0, 0.0, 0.0, - 0.0, -2.0, 0.0, - 0.0, 0.0, -2.0); - Vector b1 = Vector_(3, 2.5, 2.5, 2.5); - Vector s1 = Vector_(3, 2.6, 2.6, 2.6); - - Matrix A21 = Matrix_(3,3, - 3.0, 0.0, 0.0, - 0.0, 3.0, 0.0, - 0.0, 0.0, 3.0); - Vector b2 = Vector_(3, 3.5, 3.5, 3.5); - Vector s2 = Vector_(3, 3.6, 3.6, 3.6); - - GaussianFactorGraph gfg; - gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true)); - gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true)); - gfg.add(1, A21, b2, noiseModel::Diagonal::Sigmas(s2, true)); - - // Convert to Jacobians (inefficient copy of all factors instead of selectively converting only Hessians) - FactorGraph jacobians; - BOOST_FOREACH(const GaussianFactorGraph::sharedFactor& factor, gfg) { - jacobians.push_back(boost::make_shared(*factor)); - } - - // Combine Jacobians into a single dense factor - JacobianFactor actual = *CombineJacobians(jacobians, VariableSlots(gfg)); - - Matrix zero3x3 = zeros(3,3); - Matrix A0 = gtsam::stack(3, &zero3x3, &A10, &zero3x3); - Matrix A1 = gtsam::stack(3, &A01, &A11, &A21); - Vector b = gtsam::concatVectors(3, &b0, &b1, &b2); - Vector sigmas = gtsam::concatVectors(3, &s0, &s1, &s2); - - JacobianFactor expected(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true)); - - EXPECT(assert_equal(expected, actual)); -} - -/* ************************************************************************* */ -TEST(GaussianFactor, CombineAndEliminate) -{ - Matrix A01 = Matrix_(3,3, - 1.0, 0.0, 0.0, - 0.0, 1.0, 0.0, - 0.0, 0.0, 1.0); - Vector b0 = Vector_(3, 1.5, 1.5, 1.5); - Vector s0 = Vector_(3, 1.6, 1.6, 1.6); - - Matrix A10 = Matrix_(3,3, - 2.0, 0.0, 0.0, - 0.0, 2.0, 0.0, - 0.0, 0.0, 2.0); - Matrix A11 = Matrix_(3,3, - -2.0, 0.0, 0.0, - 0.0, -2.0, 0.0, - 0.0, 0.0, -2.0); - Vector b1 = Vector_(3, 2.5, 2.5, 2.5); - Vector s1 = Vector_(3, 2.6, 2.6, 2.6); - - Matrix A21 = Matrix_(3,3, - 3.0, 0.0, 0.0, - 0.0, 3.0, 0.0, - 0.0, 0.0, 3.0); - Vector b2 = Vector_(3, 3.5, 3.5, 3.5); - Vector s2 = Vector_(3, 3.6, 3.6, 3.6); - - GaussianFactorGraph gfg; - gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true)); - gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true)); - gfg.add(1, A21, b2, noiseModel::Diagonal::Sigmas(s2, true)); - - Matrix zero3x3 = zeros(3,3); - Matrix A0 = gtsam::stack(3, &A10, &zero3x3, &zero3x3); - Matrix A1 = gtsam::stack(3, &A11, &A01, &A21); - Vector b = gtsam::concatVectors(3, &b1, &b0, &b2); - Vector sigmas = gtsam::concatVectors(3, &s1, &s0, &s2); - - JacobianFactor expectedFactor(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true)); - GaussianConditional::shared_ptr expectedBN = expectedFactor.eliminate(1); - - GaussianConditional::shared_ptr actualBN; - GaussianFactor::shared_ptr actualFactor; - boost::tie(actualBN, actualFactor) = EliminateQR(gfg, 1); - JacobianFactor::shared_ptr actualJacobian = boost::dynamic_pointer_cast< - JacobianFactor>(actualFactor); - - EXPECT(assert_equal(*expectedBN, *actualBN)); - EXPECT(assert_equal(expectedFactor, *actualJacobian)); -} - -/* ************************************************************************* */ -TEST(GaussianFactor, eliminateFrontals) -{ - // Augmented Ab test case for whole factor graph - Matrix Ab = Matrix_(14,11, - 4., 0., 1., 4., 1., 0., 3., 6., 8., 8., 1., - 9., 2., 0., 1., 6., 3., 9., 6., 6., 9., 4., - 5., 3., 7., 9., 5., 5., 9., 1., 3., 7., 0., - 5., 6., 5., 7., 9., 4., 0., 1., 1., 3., 5., - 0., 0., 4., 5., 6., 6., 7., 9., 4., 5., 4., - 0., 0., 9., 4., 8., 6., 2., 1., 4., 1., 6., - 0., 0., 6., 0., 4., 2., 4., 0., 1., 9., 6., - 0., 0., 6., 6., 4., 4., 5., 5., 5., 8., 6., - 0., 0., 0., 0., 8., 0., 9., 8., 2., 8., 0., - 0., 0., 0., 0., 0., 9., 4., 6., 3., 2., 0., - 0., 0., 0., 0., 1., 1., 9., 1., 5., 5., 3., - 0., 0., 0., 0., 1., 1., 3., 3., 2., 0., 5., - 0., 0., 0., 0., 0., 0., 0., 0., 2., 4., 6., - 0., 0., 0., 0., 0., 0., 0., 0., 6., 3., 4.); - - // Create first factor (from pieces of Ab) - list > terms1; - - terms1 += - make_pair( 3, Matrix(Ab.block(0, 0, 4, 2))), - make_pair( 5, Matrix(Ab.block(0, 2, 4, 2))), - make_pair( 7, Matrix(Ab.block(0, 4, 4, 2))), - make_pair( 9, Matrix(Ab.block(0, 6, 4, 2))), - make_pair(11, Matrix(Ab.block(0, 8, 4, 2))); - Vector b1 = Ab.col(10).segment(0, 4); - JacobianFactor::shared_ptr factor1(new JacobianFactor(terms1, b1, noiseModel::Isotropic::Sigma(4, 0.5))); - - // Create second factor - list > terms2; - terms2 += - make_pair(5, Matrix(Ab.block(4, 2, 4, 2))), - make_pair(7, Matrix(Ab.block(4, 4, 4, 2))), - make_pair(9, Matrix(Ab.block(4, 6, 4, 2))), - make_pair(11,Matrix(Ab.block(4, 8, 4, 2))); - Vector b2 = Ab.col(10).segment(4, 4); - JacobianFactor::shared_ptr factor2(new JacobianFactor(terms2, b2, noiseModel::Isotropic::Sigma(4, 0.5))); - - // Create third factor - list > terms3; - terms3 += - make_pair(7, Matrix(Ab.block(8, 4, 4, 2))), - make_pair(9, Matrix(Ab.block(8, 6, 4, 2))), - make_pair(11,Matrix(Ab.block(8, 8, 4, 2))); - Vector b3 = Ab.col(10).segment(8, 4); - JacobianFactor::shared_ptr factor3(new JacobianFactor(terms3, b3, noiseModel::Isotropic::Sigma(4, 0.5))); - - // Create fourth factor - list > terms4; - terms4 += - make_pair(11, Matrix(Ab.block(12, 8, 2, 2))); - Vector b4 = Ab.col(10).segment(12, 2); - JacobianFactor::shared_ptr factor4(new JacobianFactor(terms4, b4, noiseModel::Isotropic::Sigma(2, 0.5))); - - // Create factor graph - GaussianFactorGraph factors; - factors.push_back(factor1); - factors.push_back(factor2); - factors.push_back(factor3); - factors.push_back(factor4); - - // extract the dense matrix for the graph - Matrix actualDense = factors.augmentedJacobian(); - EXPECT(assert_equal(2.0 * Ab, actualDense)); - - // Convert to Jacobians, inefficient copy of all factors instead of selectively converting only Hessians - FactorGraph jacobians; - BOOST_FOREACH(const GaussianFactorGraph::sharedFactor& factor, factors) { - jacobians.push_back(boost::make_shared(*factor)); - } - - // Create combined factor - JacobianFactor combined(*CombineJacobians(jacobians, VariableSlots(factors))); - - // Copies factors as they will be eliminated in place - JacobianFactor actualFactor_QR = combined; - JacobianFactor actualFactor_Chol = combined; - - // Expected augmented matrix, both GaussianConditional (first 6 rows) and remaining factor (next 4 rows) - Matrix R = 2.0*Matrix_(11,11, - -12.1244, -5.1962, -5.2786, -8.6603, -10.5573, -5.9385, -11.3820, -7.2581, -8.7427, -13.4440, -5.3611, - 0., 4.6904, 5.0254, 5.5432, 5.5737, 3.0153, -3.0153, -3.5635, -3.9290, -2.7412, 2.1625, - 0., 0., -13.8160, -8.7166, -10.2245, -8.8666, -8.7632, -5.2544, -6.9192, -10.5537, -9.3250, - 0., 0., 0., 6.5033, -1.1453, 1.3179, 2.5768, 5.5503, 3.6524, 1.3491, -2.5676, - 0., 0., 0., 0., -9.6242, -2.1148, -9.3509, -10.5846, -3.5366, -6.8561, -3.2277, - 0., 0., 0., 0., 0., 9.7887, 4.3551, 5.7572, 2.7876, 0.1611, 1.1769, - 0., 0., 0., 0., 0., 0., -11.1139, -0.6521, -2.1943, -7.5529, -0.9081, - 0., 0., 0., 0., 0., 0., 0., -4.6479, -1.9367, -6.5170, -3.7685, - 0., 0., 0., 0., 0., 0., 0., 0., 8.2503, 3.3757, 6.8476, - 0., 0., 0., 0., 0., 0., 0., 0., 0., -5.7095, -0.0090, - 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., -7.1635); - - // Expected conditional on first variable from first 2 rows of R - Matrix R1 = sub(R, 0,2, 0,2); - list > cterms1; - cterms1 += - make_pair(5, sub(R, 0,2, 2,4 )), - make_pair(7, sub(R, 0,2, 4,6 )), - make_pair(9, sub(R, 0,2, 6,8 )), - make_pair(11,sub(R, 0,2, 8,10)); - Vector d1 = R.col(10).segment(0,2); - GaussianConditional::shared_ptr cond1(new GaussianConditional(3, d1, R1, cterms1, ones(2))); - - // Expected conditional on second variable from next 2 rows of R - Matrix R2 = sub(R, 2,4, 2,4); - list > cterms2; - cterms2 += - make_pair(7, sub(R, 2,4, 4,6)), - make_pair(9, sub(R, 2,4, 6,8)), - make_pair(11,sub(R, 2,4, 8,10)); - Vector d2 = R.col(10).segment(2,2); - GaussianConditional::shared_ptr cond2(new GaussianConditional(5, d2, R2, cterms2, ones(2))); - - // Expected conditional on third variable from next 2 rows of R - Matrix R3 = sub(R, 4,6, 4,6); - list > cterms3; - cterms3 += - make_pair(9, sub(R, 4,6, 6,8)), - make_pair(11, sub(R, 4,6, 8,10)); - Vector d3 = R.col(10).segment(4,2); - GaussianConditional::shared_ptr cond3(new GaussianConditional(7, d3, R3, cterms3, ones(2))); - - // Create expected Bayes net fragment from three conditionals above -// GaussianBayesNet expectedFragment; -// expectedFragment.push_back(cond1); -// expectedFragment.push_back(cond2); -// expectedFragment.push_back(cond3); - Index ikeys[] = {3,5,7,9,11}; - std::vector keys(ikeys, ikeys + sizeof(ikeys)/sizeof(Index)); - size_t dims[] = { 2,2,2,2,2,1 }; - size_t height = 11; - VerticalBlockView Rblock(R, dims, dims+6, height); - GaussianConditional::shared_ptr expectedFragment( new GaussianConditional(keys.begin(), keys.end(), 3, - Rblock, ones(6)) ); - - // Get expected matrices for remaining factor - Matrix Ae1 = sub(R, 6,10, 6,8); - Matrix Ae2 = sub(R, 6,10, 8,10); - Vector be = R.col(10).segment(6, 4); - - // Eliminate (3 frontal variables, 6 scalar columns) using QR !!!! - GaussianConditional::shared_ptr actualFragment_QR = actualFactor_QR.eliminate(3); - - EXPECT(assert_equal(*expectedFragment, *actualFragment_QR, 0.001)); - EXPECT(assert_equal(size_t(2), actualFactor_QR.keys().size())); - EXPECT(assert_equal(Index(9), actualFactor_QR.keys()[0])); - EXPECT(assert_equal(Index(11), actualFactor_QR.keys()[1])); - EXPECT(assert_equal(Ae1, actualFactor_QR.getA(actualFactor_QR.begin()), 0.001)); - EXPECT(assert_equal(Ae2, actualFactor_QR.getA(actualFactor_QR.begin()+1), 0.001)); - EXPECT(assert_equal(be, actualFactor_QR.getb(), 0.001)); - EXPECT(assert_equal(ones(4), actualFactor_QR.get_model()->sigmas(), 0.001)); - - // Eliminate (3 frontal variables, 6 scalar columns) using Cholesky !!!! -#ifdef BROKEN - GaussianBayesNet actualFragment_Chol = *actualFactor_Chol.eliminate(3, JacobianFactor::SOLVE_CHOLESKY); - EXPECT(assert_equal(expectedFragment, actualFragment_Chol, 0.001)); - EXPECT(assert_equal(size_t(2), actualFactor_Chol.keys().size())); - EXPECT(assert_equal(Index(9), actualFactor_Chol.keys()[0])); - EXPECT(assert_equal(Index(11), actualFactor_Chol.keys()[1])); - EXPECT(assert_equal(Ae1, actualFactor_Chol.getA(actualFactor_Chol.begin()), 0.001)); //// - EXPECT(linear_dependent(Ae2, actualFactor_Chol.getA(actualFactor_Chol.begin()+1), 0.001)); - EXPECT(assert_equal(be, actualFactor_Chol.getb(), 0.001)); //// - EXPECT(assert_equal(ones(4), actualFactor_Chol.get_sigmas(), 0.001)); -#endif -} - -/* ************************************************************************* */ -TEST(GaussianFactor, permuteWithInverse) -{ - Matrix A1 = Matrix_(2,2, - 1.0, 2.0, - 3.0, 4.0); - Matrix A2 = Matrix_(2,1, - 5.0, - 6.0); - Matrix A3 = Matrix_(2,3, - 7.0, 8.0, 9.0, - 10.0, 11.0, 12.0); - Vector b = Vector_(2, 13.0, 14.0); - - Permutation inversePermutation(6); - inversePermutation[0] = 5; - inversePermutation[1] = 4; - inversePermutation[2] = 3; - inversePermutation[3] = 2; - inversePermutation[4] = 1; - inversePermutation[5] = 0; - - JacobianFactor actual(1, A1, 3, A2, 5, A3, b, noiseModel::Isotropic::Sigma(2, 1.0)); - GaussianFactorGraph actualFG; actualFG.push_back(JacobianFactor::shared_ptr(new JacobianFactor(actual))); - VariableIndex actualIndex(actualFG); - actual.permuteWithInverse(inversePermutation); -// actualIndex.permute(*inversePermutation.inverse()); - - JacobianFactor expected(4, A1, 2, A2, 0, A3, b, noiseModel::Isotropic::Sigma(2, 1.0)); - GaussianFactorGraph expectedFG; expectedFG.push_back(JacobianFactor::shared_ptr(new JacobianFactor(expected))); -// GaussianVariableIndex expectedIndex(expectedFG); - - EXPECT(assert_equal(expected, actual)); -} - /* ************************************************************************* */ TEST(GaussianFactorGraph, sparseJacobian) { // Create factor graph: @@ -396,7 +74,7 @@ TEST(GaussianFactorGraph, sparseJacobian) { // 0 0 0 14 15 16 // Expected - NOTE that we transpose this! - Matrix expected = Matrix_(16,3, + Matrix expectedT = (Matrix(16, 3) << 1., 1., 2., 1., 2., 4., 1., 3., 6., @@ -412,12 +90,14 @@ TEST(GaussianFactorGraph, sparseJacobian) { 4., 4.,28., 4., 5.,30., 3., 6.,26., - 4., 6.,32.).transpose(); + 4., 6.,32.); + + Matrix expected = expectedT.transpose(); GaussianFactorGraph gfg; SharedDiagonal model = noiseModel::Isotropic::Sigma(2, 0.5); - gfg.add(0, Matrix_(2,3, 1., 2., 3., 5., 6., 7.), Vector_(2, 4., 8.), model); - gfg.add(0, Matrix_(2,3, 9.,10., 0., 0., 0., 0.), 1, Matrix_(2,2, 11., 12., 14., 15.), Vector_(2, 13.,16.), model); + gfg.add(0, (Matrix(2, 3) << 1., 2., 3., 5., 6., 7.), (Vector(2) << 4., 8.), model); + gfg.add(0, (Matrix(2, 3) << 9.,10., 0., 0., 0., 0.), 1, (Matrix(2, 2) << 11., 12., 14., 15.), (Vector(2) << 13.,16.), model); Matrix actual = gfg.sparseJacobian_(); @@ -433,104 +113,315 @@ TEST(GaussianFactorGraph, matrices) { // 9 10 0 11 12 13 // 0 0 0 14 15 16 + Matrix A00 = (Matrix(2, 3) << 1, 2, 3, 5, 6, 7); + Matrix A10 = (Matrix(2, 3) << 9, 10, 0, 0, 0, 0); + Matrix A11 = (Matrix(2, 2) << 11, 12, 14, 15); + GaussianFactorGraph gfg; SharedDiagonal model = noiseModel::Unit::Create(2); - gfg.add(0, Matrix_(2,3, 1., 2., 3., 5., 6., 7.), Vector_(2, 4., 8.), model); - gfg.add(0, Matrix_(2,3, 9.,10., 0., 0., 0., 0.), 1, Matrix_(2,2, 11., 12., 14., 15.), Vector_(2, 13.,16.), model); + gfg.add(0, A00, (Vector(2) << 4., 8.), model); + gfg.add(0, A10, 1, A11, (Vector(2) << 13.,16.), model); - Matrix jacobian(4,6); - jacobian << + Matrix Ab(4,6); + Ab << 1, 2, 3, 0, 0, 4, 5, 6, 7, 0, 0, 8, 9,10, 0,11,12,13, 0, 0, 0,14,15,16; - Matrix expectedJacobian = jacobian; - Matrix expectedHessian = jacobian.transpose() * jacobian; - Matrix expectedA = jacobian.leftCols(jacobian.cols()-1); - Vector expectedb = jacobian.col(jacobian.cols()-1); - Matrix expectedL = expectedA.transpose() * expectedA; - Vector expectedeta = expectedA.transpose() * expectedb; + // augmented versions + EXPECT(assert_equal(Ab, gfg.augmentedJacobian())); + EXPECT(assert_equal(Ab.transpose() * Ab, gfg.augmentedHessian())); - Matrix actualJacobian = gfg.augmentedJacobian(); - Matrix actualHessian = gfg.augmentedHessian(); + // jacobian + Matrix A = Ab.leftCols(Ab.cols()-1); + Vector b = Ab.col(Ab.cols()-1); Matrix actualA; Vector actualb; boost::tie(actualA,actualb) = gfg.jacobian(); - Matrix actualL; Vector actualeta; boost::tie(actualL,actualeta) = gfg.hessian(); + EXPECT(assert_equal(A, actualA)); + EXPECT(assert_equal(b, actualb)); - EXPECT(assert_equal(expectedJacobian, actualJacobian)); - EXPECT(assert_equal(expectedHessian, actualHessian)); - EXPECT(assert_equal(expectedA, actualA)); - EXPECT(assert_equal(expectedb, actualb)); - EXPECT(assert_equal(expectedL, actualL)); - EXPECT(assert_equal(expectedeta, actualeta)); + // hessian + Matrix L = A.transpose() * A; + Vector eta = A.transpose() * b; + Matrix actualL; Vector actualeta; boost::tie(actualL,actualeta) = gfg.hessian(); + EXPECT(assert_equal(L, actualL)); + EXPECT(assert_equal(eta, actualeta)); + + // hessianBlockDiagonal + VectorValues expectLdiagonal; // Make explicit that diagonal is sum-squares of columns + expectLdiagonal.insert(0, (Vector(3) << 1+25+81, 4+36+100, 9+49)); + expectLdiagonal.insert(1, (Vector(2) << 121+196, 144+225)); + EXPECT(assert_equal(expectLdiagonal, gfg.hessianDiagonal())); + + // hessianBlockDiagonal + map actualBD = gfg.hessianBlockDiagonal(); + LONGS_EQUAL(2,actualBD.size()); + EXPECT(assert_equal(A00.transpose()*A00 + A10.transpose()*A10,actualBD[0])); + EXPECT(assert_equal(A11.transpose()*A11,actualBD[1])); +} + +/* ************************************************************************* */ +static GaussianFactorGraph createSimpleGaussianFactorGraph() { + GaussianFactorGraph fg; + SharedDiagonal unit2 = noiseModel::Unit::Create(2); + // linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_] + fg += JacobianFactor(2, 10*eye(2), -1.0*ones(2), unit2); + // odometry between x1 and x2: x2-x1=[0.2;-0.1] + fg += JacobianFactor(0, 10*eye(2), 2, -10*eye(2), (Vector(2) << 2.0, -1.0), unit2); + // measurement between x1 and l1: l1-x1=[0.0;0.2] + fg += JacobianFactor(1, 5*eye(2), 2, -5*eye(2), (Vector(2) << 0.0, 1.0), unit2); + // measurement between x2 and l1: l1-x2=[-0.2;0.3] + fg += JacobianFactor(0, -5*eye(2), 1, 5*eye(2), (Vector(2) << -1.0, 1.5), unit2); + return fg; +} + +/* ************************************************************************* */ +TEST( GaussianFactorGraph, gradient ) +{ + GaussianFactorGraph fg = createSimpleGaussianFactorGraph(); + + // Construct expected gradient + // 2*f(x) = 100*(x1+c[X(1)])^2 + 100*(x2-x1-[0.2;-0.1])^2 + 25*(l1-x1-[0.0;0.2])^2 + 25*(l1-x2-[-0.2;0.3])^2 + // worked out: df/dx1 = 100*[0.1;0.1] + 100*[0.2;-0.1]) + 25*[0.0;0.2] = [10+20;10-10+5] = [30;5] + VectorValues expected = map_list_of + (1, (Vector(2) << 5.0, -12.5)) + (2, (Vector(2) << 30.0, 5.0)) + (0, (Vector(2) << -25.0, 17.5)); + + // Check the gradient at delta=0 + VectorValues zero = VectorValues::Zero(expected); + VectorValues actual = fg.gradient(zero); + EXPECT(assert_equal(expected, actual)); + EXPECT(assert_equal(expected, fg.gradientAtZero())); + + // Check the gradient at the solution (should be zero) + VectorValues solution = fg.optimize(); + VectorValues actual2 = fg.gradient(solution); + EXPECT(assert_equal(VectorValues::Zero(solution), actual2)); +} + +/* ************************************************************************* */ +TEST( GaussianFactorGraph, transposeMultiplication ) +{ + GaussianFactorGraph A = createSimpleGaussianFactorGraph(); + + Errors e; e += + (Vector(2) << 0.0, 0.0), + (Vector(2) << 15.0, 0.0), + (Vector(2) << 0.0,-5.0), + (Vector(2) << -7.5,-5.0); + + VectorValues expected; + expected.insert(1, (Vector(2) << -37.5,-50.0)); + expected.insert(2, (Vector(2) << -150.0, 25.0)); + expected.insert(0, (Vector(2) << 187.5, 25.0)); + + VectorValues actual = A.transposeMultiply(e); + EXPECT(assert_equal(expected, actual)); +} + +/* ************************************************************************* */ +TEST(GaussianFactorGraph, eliminate_empty ) +{ + // eliminate an empty factor + GaussianFactorGraph gfg; + gfg.add(JacobianFactor()); + GaussianBayesNet::shared_ptr actualBN; + GaussianFactorGraph::shared_ptr remainingGFG; + boost::tie(actualBN, remainingGFG) = gfg.eliminatePartialSequential(Ordering()); + + // expected Bayes net is empty + GaussianBayesNet expectedBN; + + // expected remaining graph should be the same as the original, still containing the empty factor + GaussianFactorGraph expectedLF = gfg; + + // check if the result matches + EXPECT(assert_equal(*actualBN, expectedBN)); + EXPECT(assert_equal(*remainingGFG, expectedLF)); +} + +/* ************************************************************************* */ +TEST( GaussianFactorGraph, matrices2 ) +{ + GaussianFactorGraph gfg = createSimpleGaussianFactorGraph(); + Matrix A; Vector b; boost::tie(A,b) = gfg.jacobian(); + Matrix AtA; Vector eta; boost::tie(AtA,eta) = gfg.hessian(); + EXPECT(assert_equal(A.transpose()*A, AtA)); + EXPECT(assert_equal(A.transpose()*b, eta)); +} + + +/* ************************************************************************* */ +TEST( GaussianFactorGraph, multiplyHessianAdd ) +{ + GaussianFactorGraph gfg = createSimpleGaussianFactorGraph(); + + VectorValues x = map_list_of + (0, (Vector(2) << 1,2)) + (1, (Vector(2) << 3,4)) + (2, (Vector(2) << 5,6)); + + VectorValues expected; + expected.insert(0, (Vector(2) << -450, -450)); + expected.insert(1, (Vector(2) << 0, 0)); + expected.insert(2, (Vector(2) << 950, 1050)); + + VectorValues actual; + gfg.multiplyHessianAdd(1.0, x, actual); + EXPECT(assert_equal(expected, actual)); + + // now, do it with non-zero y + gfg.multiplyHessianAdd(1.0, x, actual); + EXPECT(assert_equal(2*expected, actual)); +} + +/* ************************************************************************* */ +static GaussianFactorGraph createGaussianFactorGraphWithHessianFactor() { + GaussianFactorGraph gfg = createSimpleGaussianFactorGraph(); + gfg += HessianFactor(1, 2, 100*eye(2,2), zeros(2,2), (Vector(2) << 0.0, 1.0), + 400*eye(2,2), (Vector(2) << 1.0, 1.0), 3.0); + return gfg; +} + +/* ************************************************************************* */ +TEST( GaussianFactorGraph, multiplyHessianAdd2 ) +{ + GaussianFactorGraph gfg = createGaussianFactorGraphWithHessianFactor(); + + // brute force + Matrix AtA; Vector eta; boost::tie(AtA,eta) = gfg.hessian(); + Vector X(6); X<<1,2,3,4,5,6; + Vector Y(6); Y<<-450, -450, 300, 400, 2950, 3450; + EXPECT(assert_equal(Y,AtA*X)); + + VectorValues x = map_list_of + (0, (Vector(2) << 1,2)) + (1, (Vector(2) << 3,4)) + (2, (Vector(2) << 5,6)); + + VectorValues expected; + expected.insert(0, (Vector(2) << -450, -450)); + expected.insert(1, (Vector(2) << 300, 400)); + expected.insert(2, (Vector(2) << 2950, 3450)); + + VectorValues actual; + gfg.multiplyHessianAdd(1.0, x, actual); + EXPECT(assert_equal(expected, actual)); + + // now, do it with non-zero y + gfg.multiplyHessianAdd(1.0, x, actual); + EXPECT(assert_equal(2*expected, actual)); +} + +/* ************************************************************************* */ +TEST( GaussianFactorGraph, multiplyHessianAdd3 ) +{ + GaussianFactorGraph gfg = createGaussianFactorGraphWithHessianFactor(); + + // brute force + Matrix AtA; Vector eta; boost::tie(AtA,eta) = gfg.hessian(); + Vector X(6); X<<1,2,3,4,5,6; + Vector Y(6); Y<<-450, -450, 300, 400, 2950, 3450; + EXPECT(assert_equal(Y,AtA*X)); + + double* x = &X[0]; + + Vector fast_y = gtsam::zero(6); + gfg.multiplyHessianAdd(1.0, x, fast_y.data()); + EXPECT(assert_equal(Y, fast_y)); + + // now, do it with non-zero y + gfg.multiplyHessianAdd(1.0, x, fast_y.data()); + EXPECT(assert_equal(2*Y, fast_y)); + +} + + +/* ************************************************************************* */ +TEST( GaussianFactorGraph, matricesMixed ) +{ + GaussianFactorGraph gfg = createGaussianFactorGraphWithHessianFactor(); + Matrix A; Vector b; boost::tie(A,b) = gfg.jacobian(); // incorrect ! + Matrix AtA; Vector eta; boost::tie(AtA,eta) = gfg.hessian(); // correct + EXPECT(assert_equal(A.transpose()*A, AtA)); + Vector expected = - (Vector(6) << -25, 17.5, 5, -13.5, 29, 4); + EXPECT(assert_equal(expected, eta)); + EXPECT(assert_equal(A.transpose()*b, eta)); +} + + +/* ************************************************************************* */ +TEST( GaussianFactorGraph, gradientAtZero ) +{ + GaussianFactorGraph gfg = createGaussianFactorGraphWithHessianFactor(); + VectorValues expected; + VectorValues actual = gfg.gradientAtZero(); + expected.insert(0, (Vector(2) << -25, 17.5)); + expected.insert(1, (Vector(2) << 5, -13.5)); + expected.insert(2, (Vector(2) << 29, 4)); + EXPECT(assert_equal(expected, actual)); +} + +/* ************************************************************************* */ +TEST( GaussianFactorGraph, clone ) { + // 2 variables, frontal has dim=4 + VerticalBlockMatrix blockMatrix(list_of(4)(2)(1), 4); + blockMatrix.matrix() << + 1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 0.1, + 0.0, 1.0, 0.0, 2.0, 0.0, 3.0, 0.2, + 0.0, 0.0, 3.0, 0.0, 4.0, 0.0, 0.3, + 0.0, 0.0, 0.0, 3.0, 0.0, 4.0, 0.4; + GaussianConditional cg(list_of(1)(2), 1, blockMatrix); + + GaussianFactorGraph init_graph = createGaussianFactorGraphWithHessianFactor(); + init_graph.push_back(GaussianFactor::shared_ptr()); /// Add null factor + init_graph.push_back(GaussianConditional(cg)); + + GaussianFactorGraph exp_graph = createGaussianFactorGraphWithHessianFactor(); // Created separately + exp_graph.push_back(GaussianFactor::shared_ptr()); /// Add null factor + exp_graph.push_back(GaussianConditional(cg)); + + GaussianFactorGraph actCloned = init_graph.clone(); + EXPECT(assert_equal(init_graph, actCloned)); // Same as the original version + + // Apply an in-place change to init_graph and compare + JacobianFactor::shared_ptr jacFactor0 = boost::dynamic_pointer_cast(init_graph.at(0)); + CHECK(jacFactor0); + jacFactor0->getA(jacFactor0->begin()) *= 7.; + EXPECT(assert_inequal(init_graph, exp_graph)); + EXPECT(assert_equal(exp_graph, actCloned)); +} + +/* ************************************************************************* */ +TEST( GaussianFactorGraph, negate ) { + GaussianFactorGraph init_graph = createGaussianFactorGraphWithHessianFactor(); + init_graph.push_back(GaussianFactor::shared_ptr()); /// Add null factor + GaussianFactorGraph actNegation = init_graph.negate(); + GaussianFactorGraph expNegation; + expNegation.push_back(init_graph.at(0)->negate()); + expNegation.push_back(init_graph.at(1)->negate()); + expNegation.push_back(init_graph.at(2)->negate()); + expNegation.push_back(init_graph.at(3)->negate()); + expNegation.push_back(init_graph.at(4)->negate()); + expNegation.push_back(GaussianFactor::shared_ptr()); + EXPECT(assert_equal(expNegation, actNegation)); +} + +/* ************************************************************************* */ +TEST( GaussianFactorGraph, hessianDiagonal ) +{ + GaussianFactorGraph gfg = createGaussianFactorGraphWithHessianFactor(); + VectorValues expected; + Matrix infoMatrix = gfg.hessian().first; + Vector d = infoMatrix.diagonal(); + + VectorValues actual = gfg.hessianDiagonal(); + expected.insert(0, d.segment<2>(0)); + expected.insert(1, d.segment<2>(2)); + expected.insert(2, d.segment<2>(4)); + EXPECT(assert_equal(expected, actual)); } - -/* ************************************************************************* */ -TEST( GaussianFactorGraph, gradient ) -{ - GaussianFactorGraph fg = createSimpleGaussianFactorGraph(); - - // Construct expected gradient - VectorValues expected; - - // 2*f(x) = 100*(x1+c[X(1)])^2 + 100*(x2-x1-[0.2;-0.1])^2 + 25*(l1-x1-[0.0;0.2])^2 + 25*(l1-x2-[-0.2;0.3])^2 - // worked out: df/dx1 = 100*[0.1;0.1] + 100*[0.2;-0.1]) + 25*[0.0;0.2] = [10+20;10-10+5] = [30;5] - expected.insert(1,Vector_(2, 5.0,-12.5)); - expected.insert(2,Vector_(2, 30.0, 5.0)); - expected.insert(0,Vector_(2,-25.0, 17.5)); - - // Check the gradient at delta=0 - VectorValues zero = VectorValues::Zero(expected); - VectorValues actual = gradient(fg, zero); - EXPECT(assert_equal(expected,actual)); - - // Check the gradient at the solution (should be zero) - VectorValues solution = *GaussianSequentialSolver(fg).optimize(); - VectorValues actual2 = gradient(fg, solution); - EXPECT(assert_equal(VectorValues::Zero(solution), actual2)); -} - -/* ************************************************************************* */ -TEST( GaussianFactorGraph, transposeMultiplication ) -{ - GaussianFactorGraph A = createSimpleGaussianFactorGraph(); - - VectorValues e; - e.insert(0, Vector_(2, 0.0, 0.0)); - e.insert(1, Vector_(2,15.0, 0.0)); - e.insert(2, Vector_(2, 0.0,-5.0)); - e.insert(3, Vector_(2,-7.5,-5.0)); - - VectorValues expected; - expected.insert(1, Vector_(2, -37.5,-50.0)); - expected.insert(2, Vector_(2,-150.0, 25.0)); - expected.insert(0, Vector_(2, 187.5, 25.0)); - - VectorValues actual = VectorValues::SameStructure(expected); - transposeMultiply(A, e, actual); - EXPECT(assert_equal(expected,actual)); -} - -/* ************************************************************************* */ -TEST(GaussianFactorGraph, eliminate_empty ) -{ - // eliminate an empty factor - GaussianFactorGraph gfg; - gfg.push_back(boost::make_shared()); - GaussianConditional::shared_ptr actualCG; - GaussianFactorGraph remainingGFG; - boost::tie(actualCG, remainingGFG) = gfg.eliminateOne(0); - - // expected Conditional Gaussian is just a parent-less node with P(x)=1 - GaussianConditional expectedCG(0, Vector(), Matrix(), Vector()); - - // expected remaining graph should be the same as the original, still empty :-) - GaussianFactorGraph expectedLF = gfg; - - // check if the result matches - EXPECT(actualCG->equals(expectedCG)); - EXPECT(remainingGFG.equals(expectedLF)); -} /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} diff --git a/gtsam/linear/tests/testGaussianJunctionTree.cpp b/gtsam/linear/tests/testGaussianJunctionTree.cpp deleted file mode 100644 index 71d32cb1e..000000000 --- a/gtsam/linear/tests/testGaussianJunctionTree.cpp +++ /dev/null @@ -1,309 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 testGaussianJunctionTree.cpp - * @date Jul 8, 2010 - * @author Kai Ni - */ - -#include -#include - -#include -#include // for operator += -#include // for operator += -using namespace boost::assign; - -#include -#include -#include -#include -#include -#include - -using namespace std; -using namespace gtsam; - -static const Index x2=0, x1=1, x3=2, x4=3; - -static GaussianFactorGraph createChain() { - - typedef GaussianFactorGraph::sharedFactor Factor; - SharedDiagonal model = noiseModel::Isotropic::Sigma(1, 0.5); - Factor factor1(new JacobianFactor(x2, Matrix_(1,1,1.), x1, Matrix_(1,1,1.), Vector_(1,1.), model)); - Factor factor2(new JacobianFactor(x2, Matrix_(1,1,1.), x3, Matrix_(1,1,1.), Vector_(1,1.), model)); - Factor factor3(new JacobianFactor(x3, Matrix_(1,1,1.), x4, Matrix_(1,1,1.), Vector_(1,1.), model)); - Factor factor4(new JacobianFactor(x4, Matrix_(1,1,1.), Vector_(1,1.), model)); - - GaussianFactorGraph fg; - fg.push_back(factor1); - fg.push_back(factor2); - fg.push_back(factor3); - fg.push_back(factor4); - - return fg; -} - -/* ************************************************************************* */ -/** - * x1 - x2 - x3 - x4 - * x3 x4 - * x2 x1 : x3 - * - * x2 x1 x3 x4 b - * 1 1 1 - * 1 1 1 - * 1 1 1 - * 1 1 - * - * 1 0 0 1 - */ -TEST( GaussianJunctionTree, eliminate ) -{ - GaussianFactorGraph fg = createChain(); - GaussianJunctionTree junctionTree(fg); - BayesTree::sharedClique rootClique = junctionTree.eliminate(&EliminateQR); - - typedef BayesTree::sharedConditional sharedConditional; - Matrix two = Matrix_(1,1,2.); - Matrix one = Matrix_(1,1,1.); - - BayesTree bayesTree_expected; - Index keys_root[] = {x3,x4}; - Matrix rsd_root = Matrix_(2,3, 2., 2., 2., 0., 2., 2.); - size_t dim_root[] = {1, 1, 1}; - sharedConditional root_expected(new GaussianConditional(keys_root, keys_root+2, 2, - VerticalBlockView(rsd_root, dim_root, dim_root+3, 2), ones(2))); - BayesTree::sharedClique rootClique_expected(new BayesTree::Clique(root_expected)); - - Index keys_child[] = {x2,x1,x3}; - Matrix rsd_child = Matrix_(2,4, 2., 1., 1., 2., 0., -1., 1., 0.); - size_t dim_child[] = {1, 1, 1, 1}; - sharedConditional child_expected(new GaussianConditional(keys_child, keys_child+3, 2, - VerticalBlockView(rsd_child, dim_child, dim_child+4, 2), ones(2))); - BayesTree::sharedClique childClique_expected(new BayesTree::Clique(child_expected)); - - bayesTree_expected.insert(rootClique_expected); - bayesTree_expected.insert(childClique_expected); - -// bayesTree_expected.insert(sharedConditional(new GaussianConditional(x4, Vector_(1,2.), two, Vector_(1,1.)))); -// bayesTree_expected.insert(sharedConditional(new GaussianConditional(x3, Vector_(1,2.), two, x4, two, Vector_(1,1.)))); -// bayesTree_expected.insert(sharedConditional(new GaussianConditional(x1, Vector_(1,0.), one*(-1), x3, one, Vector_(1,1.)))); -// bayesTree_expected.insert(sharedConditional(new GaussianConditional(x2, Vector_(1,2.), two, x1, one, x3, one, Vector_(1,1.)))); - CHECK(assert_equal(*bayesTree_expected.root(), *rootClique)); - EXPECT(assert_equal(*(bayesTree_expected.root()->children().front()), *(rootClique->children().front()))); -} - -/* ************************************************************************* */ -TEST( GaussianJunctionTree, GBNConstructor ) -{ - GaussianFactorGraph fg = createChain(); - GaussianJunctionTree jt(fg); - BayesTree::sharedClique root = jt.eliminate(&EliminateQR); - BayesTree expected; - expected.insert(root); - - GaussianBayesNet bn(*GaussianSequentialSolver(fg).eliminate()); - BayesTree actual(bn); - - EXPECT(assert_equal(expected, actual)); -} - -/* ************************************************************************* */ -TEST( GaussianJunctionTree, optimizeMultiFrontal ) -{ - GaussianFactorGraph fg = createChain(); - GaussianJunctionTree tree(fg); - - VectorValues actual = tree.optimize(&EliminateQR); - VectorValues expected(vector(4,1)); - expected[x1] = Vector_(1, 0.); - expected[x2] = Vector_(1, 1.); - expected[x3] = Vector_(1, 0.); - expected[x4] = Vector_(1, 1.); - EXPECT(assert_equal(expected,actual)); -} - -/* ************************************************************************* */ -TEST(GaussianJunctionTree, complicatedMarginal) { - - // Create the conditionals to go in the BayesTree - GaussianConditional::shared_ptr R_1_2(new GaussianConditional( - pair_list_of - (1, (Matrix(3,1) << - 0.2630, - 0, - 0).finished()) - (2, (Matrix(3,2) << - 0.7482, 0.2290, - 0.4505, 0.9133, - 0, 0.1524).finished()) - (5, (Matrix(3,1) << - 0.8258, - 0.5383, - 0.9961).finished()), - 2, (Vector(3) << 0.0782, 0.4427, 0.1067).finished(), ones(3))); - GaussianConditional::shared_ptr R_3_4(new GaussianConditional( - pair_list_of - (3, (Matrix(3,1) << - 0.0540, - 0, - 0).finished()) - (4, (Matrix(3,2) << - 0.9340, 0.4694, - 0.1299, 0.0119, - 0, 0.3371).finished()) - (6, (Matrix(3,2) << - 0.1622, 0.5285, - 0.7943, 0.1656, - 0.3112, 0.6020).finished()), - 2, (Vector(3) << 0.9619, 0.0046, 0.7749).finished(), ones(3))); -// GaussianConditional::shared_ptr R_5_6(new GaussianConditional( -// pair_list_of -// (5, (Matrix(3,1) << -// 0.2435, -// 0, -// 0).finished()) -// (6, (Matrix(3,2) << -// 0.4733, 0.1966, -// 0.9022, 0.0979, -// 0.0, 0.2312).finished()) // Attempted to recreate without permutation -// (7, (Matrix(3,1) << -// 0.5853, -// 1.0589, -// 0.1487).finished()) -// (8, (Matrix(3,2) << -// 0.2858, 0.3804, -// 0.9893, 0.2912, -// 0.4035, 0.4933).finished()), -// 2, (Vector(3) << 0.8173, 0.4164, 0.7671).finished(), ones(3))); - GaussianConditional::shared_ptr R_5_6(new GaussianConditional( - pair_list_of - (5, (Matrix(3,1) << - 0.2435, - 0, - 0).finished()) - (6, (Matrix(3,2) << - 0.4733, 0.1966, - 0.3517, 0.2511, - 0.8308, 0.0).finished()) // NOTE the non-upper-triangular form - // here since this test was written when we had column permutations - // from LDL. The code still works currently (does not enfore - // upper-triangularity in this case) but this test will need to be - // redone if this stops working in the future - (7, (Matrix(3,1) << - 0.5853, - 0.5497, - 0.9172).finished()) - (8, (Matrix(3,2) << - 0.2858, 0.3804, - 0.7572, 0.5678, - 0.7537, 0.0759).finished()), - 2, (Vector(3) << 0.8173, 0.8687, 0.0844).finished(), ones(3))); - GaussianConditional::shared_ptr R_7_8(new GaussianConditional( - pair_list_of - (7, (Matrix(3,1) << - 0.2551, - 0, - 0).finished()) - (8, (Matrix(3,2) << - 0.8909, 0.1386, - 0.9593, 0.1493, - 0, 0.2575).finished()) - (11, (Matrix(3,1) << - 0.8407, - 0.2543, - 0.8143).finished()), - 2, (Vector(3) << 0.3998, 0.2599, 0.8001).finished(), ones(3))); - GaussianConditional::shared_ptr R_9_10(new GaussianConditional( - pair_list_of - (9, (Matrix(3,1) << - 0.7952, - 0, - 0).finished()) - (10, (Matrix(3,2) << - 0.4456, 0.7547, - 0.6463, 0.2760, - 0, 0.6797).finished()) - (11, (Matrix(3,1) << - 0.6551, - 0.1626, - 0.1190).finished()) - (12, (Matrix(3,2) << - 0.4984, 0.5853, - 0.9597, 0.2238, - 0.3404, 0.7513).finished()), - 2, (Vector(3) << 0.4314, 0.9106, 0.1818).finished(), ones(3))); - GaussianConditional::shared_ptr R_11_12(new GaussianConditional( - pair_list_of - (11, (Matrix(3,1) << - 0.0971, - 0, - 0).finished()) - (12, (Matrix(3,2) << - 0.3171, 0.4387, - 0.9502, 0.3816, - 0, 0.7655).finished()), - 2, (Vector(3) << 0.2638, 0.1455, 0.1361).finished(), ones(3))); - - // Gaussian Bayes Tree - typedef BayesTree GaussianBayesTree; - typedef GaussianBayesTree::Clique Clique; - typedef GaussianBayesTree::sharedClique sharedClique; - - // Create Bayes Tree - GaussianBayesTree bt; - bt.insert(sharedClique(new Clique(R_11_12))); - bt.insert(sharedClique(new Clique(R_9_10))); - bt.insert(sharedClique(new Clique(R_7_8))); - bt.insert(sharedClique(new Clique(R_5_6))); - bt.insert(sharedClique(new Clique(R_3_4))); - bt.insert(sharedClique(new Clique(R_1_2))); - - // Marginal on 5 - Matrix expectedCov = (Matrix(1,1) << 236.5166).finished(); - JacobianFactor::shared_ptr actualJacobianChol= boost::dynamic_pointer_cast( - bt.marginalFactor(5, EliminateCholesky)); - JacobianFactor::shared_ptr actualJacobianQR = boost::dynamic_pointer_cast( - bt.marginalFactor(5, EliminateQR)); - CHECK(assert_equal(*actualJacobianChol, *actualJacobianQR)); // Check that Chol and QR obtained marginals are the same - LONGS_EQUAL(1, actualJacobianChol->rows()); - LONGS_EQUAL(1, actualJacobianChol->size()); - LONGS_EQUAL(5, actualJacobianChol->keys()[0]); - Matrix actualA = actualJacobianChol->getA(actualJacobianChol->begin()); - Matrix actualCov = inverse(actualA.transpose() * actualA); - EXPECT(assert_equal(expectedCov, actualCov, 1e-1)); - - // Marginal on 6 -// expectedCov = (Matrix(2,2) << -// 8471.2, 2886.2, -// 2886.2, 1015.8).finished(); - expectedCov = (Matrix(2,2) << - 1015.8, 2886.2, - 2886.2, 8471.2).finished(); - actualJacobianChol = boost::dynamic_pointer_cast( - bt.marginalFactor(6, EliminateCholesky)); - actualJacobianQR = boost::dynamic_pointer_cast( - bt.marginalFactor(6, EliminateQR)); - CHECK(assert_equal(*actualJacobianChol, *actualJacobianQR)); // Check that Chol and QR obtained marginals are the same - LONGS_EQUAL(2, actualJacobianChol->rows()); - LONGS_EQUAL(1, actualJacobianChol->size()); - LONGS_EQUAL(6, actualJacobianChol->keys()[0]); - actualA = actualJacobianChol->getA(actualJacobianChol->begin()); - actualCov = inverse(actualA.transpose() * actualA); - EXPECT(assert_equal(expectedCov, actualCov, 1e1)); - -} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr);} -/* ************************************************************************* */ diff --git a/gtsam/linear/tests/testHessianFactor.cpp b/gtsam/linear/tests/testHessianFactor.cpp index dccbd17da..84c16bd9c 100644 --- a/gtsam/linear/tests/testHessianFactor.cpp +++ b/gtsam/linear/tests/testHessianFactor.cpp @@ -25,6 +25,8 @@ #include #include #include +#include +#include #include #include @@ -36,7 +38,8 @@ using namespace gtsam; const double tol = 1e-5; /* ************************************************************************* */ -TEST(HessianFactor, emptyConstructor) { +TEST(HessianFactor, emptyConstructor) +{ HessianFactor f; DOUBLES_EQUAL(0.0, f.constantTerm(), 1e-9); // Constant term should be zero EXPECT(assert_equal(Vector(), f.linearTerm())); // Linear term should be empty @@ -45,106 +48,70 @@ TEST(HessianFactor, emptyConstructor) { } /* ************************************************************************* */ -TEST(HessianFactor, ConversionConstructor) { +TEST(HessianFactor, ConversionConstructor) +{ + HessianFactor expected(list_of(0)(1), + SymmetricBlockMatrix(list_of(2)(4)(1), (Matrix(7,7) << + 125.0000, 0.0, -25.0000, 0.0, -100.0000, 0.0, 25.0000, + 0.0, 125.0000, 0.0, -25.0000, 0.0, -100.0000, -17.5000, + -25.0000, 0.0, 25.0000, 0.0, 0.0, 0.0, -5.0000, + 0.0, -25.0000, 0.0, 25.0000, 0.0, 0.0, 7.5000, + -100.0000, 0.0, 0.0, 0.0, 100.0000, 0.0, -20.0000, + 0.0, -100.0000, 0.0, 0.0, 0.0, 100.0000, 10.0000, + 25.0000, -17.5000, -5.0000, 7.5000, -20.0000, 10.0000, 8.2500))); - HessianFactor expected; - expected.keys_.push_back(0); - expected.keys_.push_back(1); - size_t dims[] = { 2, 4, 1 }; - expected.info_.resize(dims, dims+3, false); - expected.matrix_ = Matrix_(7,7, - 125.0000, 0.0, -25.0000, 0.0, -100.0000, 0.0, 25.0000, - 0.0, 125.0000, 0.0, -25.0000, 0.0, -100.0000, -17.5000, - -25.0000, 0.0, 25.0000, 0.0, 0.0, 0.0, -5.0000, - 0.0, -25.0000, 0.0, 25.0000, 0.0, 0.0, 7.5000, - -100.0000, 0.0, 0.0, 0.0, 100.0000, 0.0, -20.0000, - 0.0, -100.0000, 0.0, 0.0, 0.0, 100.0000, 10.0000, - 25.0000, -17.5000, -5.0000, 7.5000, -20.0000, 10.0000, 8.2500); + JacobianFactor jacobian( + 0, (Matrix(4,2) << -1., 0., + +0.,-1., + 1., 0., + +0.,1.), + 1, (Matrix(4,4) << 1., 0., 0.00, 0., // f4 + 0., 1., 0.00, 0., // f4 + 0., 0., -1., 0., // f2 + 0., 0., 0.00, -1.), // f2 + (Vector(4) << -0.2, 0.3, 0.2, -0.1), + noiseModel::Diagonal::Sigmas((Vector(4) << 0.2, 0.2, 0.1, 0.1))); - // sigmas - double sigma1 = 0.2; - double sigma2 = 0.1; - Vector sigmas = Vector_(4, sigma1, sigma1, sigma2, sigma2); + HessianFactor actual(jacobian); - // the combined linear factor - Matrix Ax2 = Matrix_(4,2, - // x2 - -1., 0., - +0.,-1., - 1., 0., - +0.,1. - ); - - Matrix Al1x1 = Matrix_(4,4, - // l1 x1 - 1., 0., 0.00, 0., // f4 - 0., 1., 0.00, 0., // f4 - 0., 0., -1., 0., // f2 - 0., 0., 0.00,-1. // f2 - ); - - // the RHS - Vector b2(4); - b2(0) = -0.2; - b2(1) = 0.3; - b2(2) = 0.2; - b2(3) = -0.1; - - vector > meas; - meas.push_back(make_pair(0, Ax2)); - meas.push_back(make_pair(1, Al1x1)); - JacobianFactor combined(meas, b2, noiseModel::Diagonal::Sigmas(sigmas)); - - HessianFactor actual(combined); - - VectorValues values(std::vector(dims, dims+2)); - values[0] = Vector_(2, 1.0, 2.0); - values[1] = Vector_(4, 3.0, 4.0, 5.0, 6.0); - - EXPECT_LONGS_EQUAL(2, actual.size()); + VectorValues values = pair_list_of + (0, (Vector(2) << 1.0, 2.0)) + (1, (Vector(4) << 3.0, 4.0, 5.0, 6.0)); + EXPECT_LONGS_EQUAL(2, (long)actual.size()); EXPECT(assert_equal(expected, actual, 1e-9)); - - // error terms - EXPECT_DOUBLES_EQUAL(combined.error(values), actual.error(values), 1e-9); + EXPECT_DOUBLES_EQUAL(jacobian.error(values), actual.error(values), 1e-9); } /* ************************************************************************* */ TEST(HessianFactor, Constructor1) { - Matrix G = Matrix_(2,2, 3.0, 5.0, 0.0, 6.0); - Vector g = Vector_(2, -8.0, -9.0); + Matrix G = (Matrix(2,2) << 3.0, 5.0, 5.0, 6.0); + Vector g = (Vector(2) << -8.0, -9.0); double f = 10.0; - - Vector dxv = Vector_(2, 1.5, 2.5); - - vector dims; - dims.push_back(2); - VectorValues dx(dims); - - dx[0] = dxv; - HessianFactor factor(0, G, g, f); // extract underlying parts - Matrix info_matrix = factor.info_.range(0, 1, 0, 1); - EXPECT(assert_equal(Matrix(G), info_matrix)); + EXPECT(assert_equal(G, Matrix(factor.info(factor.begin(), factor.begin())))); EXPECT_DOUBLES_EQUAL(f, factor.constantTerm(), 1e-10); - EXPECT(assert_equal(g, Vector(factor.linearTerm()), 1e-10)); - EXPECT_LONGS_EQUAL(1, factor.size()); + EXPECT(assert_equal(g, Vector(factor.linearTerm()))); + EXPECT_LONGS_EQUAL(1, (long)factor.size()); + + VectorValues dx = pair_list_of(0, (Vector(2) << 1.5, 2.5)); // error 0.5*(f - 2*x'*g + x'*G*x) double expected = 80.375; double actual = factor.error(dx); - double expected_manual = 0.5 * (f - 2.0 * dxv.dot(g) + dxv.transpose() * G.selfadjointView() * dxv); + double expected_manual = 0.5 * (f - 2.0 * dx[0].dot(g) + dx[0].transpose() * G.selfadjointView() * dx[0]); EXPECT_DOUBLES_EQUAL(expected, expected_manual, 1e-10); - DOUBLES_EQUAL(expected, actual, 1e-10); + EXPECT_DOUBLES_EQUAL(expected, actual, 1e-10); } + /* ************************************************************************* */ TEST(HessianFactor, Constructor1b) { - Vector mu = Vector_(2,1.0,2.0); + Vector mu = (Vector(2) << 1.0,2.0); Matrix Sigma = eye(2,2); HessianFactor factor(0, mu, Sigma); @@ -154,33 +121,28 @@ TEST(HessianFactor, Constructor1b) double f = dot(g,mu); // Check - Matrix info_matrix = factor.info_.range(0, 1, 0, 1); - EXPECT(assert_equal(Matrix(G), info_matrix)); + EXPECT(assert_equal(G, Matrix(factor.info(factor.begin(), factor.begin())))); EXPECT_DOUBLES_EQUAL(f, factor.constantTerm(), 1e-10); - EXPECT(assert_equal(g, Vector(factor.linearTerm()), 1e-10)); - EXPECT_LONGS_EQUAL(1, factor.size()); + EXPECT(assert_equal(g, Vector(factor.linearTerm()))); + EXPECT_LONGS_EQUAL(1, (long)factor.size()); } /* ************************************************************************* */ TEST(HessianFactor, Constructor2) { - Matrix G11 = Matrix_(1,1, 1.0); - Matrix G12 = Matrix_(1,2, 2.0, 4.0); - Matrix G22 = Matrix_(2,2, 3.0, 5.0, 0.0, 6.0); - Vector g1 = Vector_(1, -7.0); - Vector g2 = Vector_(2, -8.0, -9.0); + Matrix G11 = (Matrix(1,1) << 1.0); + Matrix G12 = (Matrix(1,2) << 2.0, 4.0); + Matrix G22 = (Matrix(2,2) << 3.0, 5.0, 5.0, 6.0); + Vector g1 = (Vector(1) << -7.0); + Vector g2 = (Vector(2) << -8.0, -9.0); double f = 10.0; - Vector dx0 = Vector_(1, 0.5); - Vector dx1 = Vector_(2, 1.5, 2.5); + Vector dx0 = (Vector(1) << 0.5); + Vector dx1 = (Vector(2) << 1.5, 2.5); - vector dims; - dims.push_back(1); - dims.push_back(2); - VectorValues dx(dims); - - dx[0] = dx0; - dx[1] = dx1; + VectorValues dx = pair_list_of + (0, dx0) + (1, dx1); HessianFactor factor(0, 1, G11, G12, g1, G22, g2, f); @@ -188,7 +150,7 @@ TEST(HessianFactor, Constructor2) double actual = factor.error(dx); DOUBLES_EQUAL(expected, actual, 1e-10); - LONGS_EQUAL(4, factor.rows()); + LONGS_EQUAL(4, (long)factor.rows()); DOUBLES_EQUAL(10.0, factor.constantTerm(), 1e-10); Vector linearExpected(3); linearExpected << g1, g2; @@ -199,45 +161,39 @@ TEST(HessianFactor, Constructor2) EXPECT(assert_equal(G22, factor.info(factor.begin()+1, factor.begin()+1))); // Check case when vector values is larger than factor - dims.push_back(2); - VectorValues dxLarge(dims); - dxLarge[0] = dx0; - dxLarge[1] = dx1; - dxLarge[2] = Vector_(2, 0.1, 0.2); + VectorValues dxLarge = pair_list_of + (0, dx0) + (1, dx1) + (2, (Vector(2) << 0.1, 0.2)); EXPECT_DOUBLES_EQUAL(expected, factor.error(dxLarge), 1e-10); } /* ************************************************************************* */ TEST(HessianFactor, Constructor3) { - Matrix G11 = Matrix_(1,1, 1.0); - Matrix G12 = Matrix_(1,2, 2.0, 4.0); - Matrix G13 = Matrix_(1,3, 3.0, 6.0, 9.0); + Matrix G11 = (Matrix(1,1) << 1.0); + Matrix G12 = (Matrix(1,2) << 2.0, 4.0); + Matrix G13 = (Matrix(1,3) << 3.0, 6.0, 9.0); - Matrix G22 = Matrix_(2,2, 3.0, 5.0, 0.0, 6.0); - Matrix G23 = Matrix_(2,3, 4.0, 6.0, 8.0, 1.0, 2.0, 4.0); + Matrix G22 = (Matrix(2,2) << 3.0, 5.0, 5.0, 6.0); + Matrix G23 = (Matrix(2,3) << 4.0, 6.0, 8.0, 1.0, 2.0, 4.0); - Matrix G33 = Matrix_(3,3, 1.0, 2.0, 3.0, 0.0, 5.0, 6.0, 0.0, 0.0, 9.0); + Matrix G33 = (Matrix(3,3) << 1.0, 2.0, 3.0, 2.0, 5.0, 6.0, 3.0, 6.0, 9.0); - Vector g1 = Vector_(1, -7.0); - Vector g2 = Vector_(2, -8.0, -9.0); - Vector g3 = Vector_(3, 1.0, 2.0, 3.0); + Vector g1 = (Vector(1) << -7.0); + Vector g2 = (Vector(2) << -8.0, -9.0); + Vector g3 = (Vector(3) << 1.0, 2.0, 3.0); double f = 10.0; - Vector dx0 = Vector_(1, 0.5); - Vector dx1 = Vector_(2, 1.5, 2.5); - Vector dx2 = Vector_(3, 1.5, 2.5, 3.5); + Vector dx0 = (Vector(1) << 0.5); + Vector dx1 = (Vector(2) << 1.5, 2.5); + Vector dx2 = (Vector(3) << 1.5, 2.5, 3.5); - vector dims; - dims.push_back(1); - dims.push_back(2); - dims.push_back(3); - VectorValues dx(dims); - - dx[0] = dx0; - dx[1] = dx1; - dx[2] = dx2; + VectorValues dx = pair_list_of + (0, dx0) + (1, dx1) + (2, dx2); HessianFactor factor(0, 1, 2, G11, G12, G13, g1, G22, G23, g2, G33, g3, f); @@ -245,7 +201,7 @@ TEST(HessianFactor, Constructor3) double actual = factor.error(dx); DOUBLES_EQUAL(expected, actual, 1e-10); - LONGS_EQUAL(7, factor.rows()); + LONGS_EQUAL(7, (long)factor.rows()); DOUBLES_EQUAL(10.0, factor.constantTerm(), 1e-10); Vector linearExpected(6); linearExpected << g1, g2, g3; @@ -262,37 +218,31 @@ TEST(HessianFactor, Constructor3) /* ************************************************************************* */ TEST(HessianFactor, ConstructorNWay) { - Matrix G11 = Matrix_(1,1, 1.0); - Matrix G12 = Matrix_(1,2, 2.0, 4.0); - Matrix G13 = Matrix_(1,3, 3.0, 6.0, 9.0); + Matrix G11 = (Matrix(1,1) << 1.0); + Matrix G12 = (Matrix(1,2) << 2.0, 4.0); + Matrix G13 = (Matrix(1,3) << 3.0, 6.0, 9.0); - Matrix G22 = Matrix_(2,2, 3.0, 5.0, 0.0, 6.0); - Matrix G23 = Matrix_(2,3, 4.0, 6.0, 8.0, 1.0, 2.0, 4.0); + Matrix G22 = (Matrix(2,2) << 3.0, 5.0, 5.0, 6.0); + Matrix G23 = (Matrix(2,3) << 4.0, 6.0, 8.0, 1.0, 2.0, 4.0); - Matrix G33 = Matrix_(3,3, 1.0, 2.0, 3.0, 0.0, 5.0, 6.0, 0.0, 0.0, 9.0); + Matrix G33 = (Matrix(3,3) << 1.0, 2.0, 3.0, 2.0, 5.0, 6.0, 3.0, 6.0, 9.0); - Vector g1 = Vector_(1, -7.0); - Vector g2 = Vector_(2, -8.0, -9.0); - Vector g3 = Vector_(3, 1.0, 2.0, 3.0); + Vector g1 = (Vector(1) << -7.0); + Vector g2 = (Vector(2) << -8.0, -9.0); + Vector g3 = (Vector(3) << 1.0, 2.0, 3.0); double f = 10.0; - Vector dx0 = Vector_(1, 0.5); - Vector dx1 = Vector_(2, 1.5, 2.5); - Vector dx2 = Vector_(3, 1.5, 2.5, 3.5); + Vector dx0 = (Vector(1) << 0.5); + Vector dx1 = (Vector(2) << 1.5, 2.5); + Vector dx2 = (Vector(3) << 1.5, 2.5, 3.5); - vector dims; - dims.push_back(1); - dims.push_back(2); - dims.push_back(3); - VectorValues dx(dims); + VectorValues dx = pair_list_of + (0, dx0) + (1, dx1) + (2, dx2); - dx[0] = dx0; - dx[1] = dx1; - dx[2] = dx2; - - - std::vector js; + std::vector js; js.push_back(0); js.push_back(1); js.push_back(2); std::vector Gs; Gs.push_back(G11); Gs.push_back(G12); Gs.push_back(G13); Gs.push_back(G22); Gs.push_back(G23); Gs.push_back(G33); @@ -304,7 +254,7 @@ TEST(HessianFactor, ConstructorNWay) double actual = factor.error(dx); DOUBLES_EQUAL(expected, actual, 1e-10); - LONGS_EQUAL(7, factor.rows()); + LONGS_EQUAL(7, (long)factor.rows()); DOUBLES_EQUAL(10.0, factor.constantTerm(), 1e-10); Vector linearExpected(6); linearExpected << g1, g2, g3; @@ -319,88 +269,32 @@ TEST(HessianFactor, ConstructorNWay) } /* ************************************************************************* */ -TEST_UNSAFE(HessianFactor, CopyConstructor_and_assignment) +TEST(HessianFactor, CombineAndEliminate) { - Matrix G11 = Matrix_(1,1, 1.0); - Matrix G12 = Matrix_(1,2, 2.0, 4.0); - Matrix G22 = Matrix_(2,2, 3.0, 5.0, 0.0, 6.0); - Vector g1 = Vector_(1, -7.0); - Vector g2 = Vector_(2, -8.0, -9.0); - double f = 10.0; - - Vector dx0 = Vector_(1, 0.5); - Vector dx1 = Vector_(2, 1.5, 2.5); - - vector dims; - dims.push_back(1); - dims.push_back(2); - VectorValues dx(dims); - - dx[0] = dx0; - dx[1] = dx1; - - HessianFactor originalFactor(0, 1, G11, G12, g1, G22, g2, f); - - // Make a copy - HessianFactor copy1(originalFactor); - - double expected = 90.5; - double actual = copy1.error(dx); - - DOUBLES_EQUAL(expected, actual, 1e-10); - LONGS_EQUAL(4, copy1.rows()); - DOUBLES_EQUAL(10.0, copy1.constantTerm(), 1e-10); - - Vector linearExpected(3); linearExpected << g1, g2; - EXPECT(assert_equal(linearExpected, copy1.linearTerm())); - - EXPECT(assert_equal(G11, copy1.info(copy1.begin(), copy1.begin()))); - EXPECT(assert_equal(G12, copy1.info(copy1.begin(), copy1.begin()+1))); - EXPECT(assert_equal(G22, copy1.info(copy1.begin()+1, copy1.begin()+1))); - - // Make a copy using the assignment operator - HessianFactor copy2; - copy2 = HessianFactor(originalFactor); // Make a temporary to make sure copying does not shared references - - actual = copy2.error(dx); - DOUBLES_EQUAL(expected, actual, 1e-10); - LONGS_EQUAL(4, copy2.rows()); - DOUBLES_EQUAL(10.0, copy2.constantTerm(), 1e-10); - - EXPECT(assert_equal(linearExpected, copy2.linearTerm())); - - EXPECT(assert_equal(G11, copy2.info(copy2.begin(), copy2.begin()))); - EXPECT(assert_equal(G12, copy2.info(copy2.begin(), copy2.begin()+1))); - EXPECT(assert_equal(G22, copy2.info(copy2.begin()+1, copy2.begin()+1))); -} - -/* ************************************************************************* */ -TEST_UNSAFE(HessianFactor, CombineAndEliminate) -{ - Matrix A01 = Matrix_(3,3, + Matrix A01 = (Matrix(3,3) << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0); - Vector b0 = Vector_(3, 1.5, 1.5, 1.5); - Vector s0 = Vector_(3, 1.6, 1.6, 1.6); + Vector b0 = (Vector(3) << 1.5, 1.5, 1.5); + Vector s0 = (Vector(3) << 1.6, 1.6, 1.6); - Matrix A10 = Matrix_(3,3, + Matrix A10 = (Matrix(3,3) << 2.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 2.0); - Matrix A11 = Matrix_(3,3, + Matrix A11 = (Matrix(3,3) << -2.0, 0.0, 0.0, 0.0, -2.0, 0.0, 0.0, 0.0, -2.0); - Vector b1 = Vector_(3, 2.5, 2.5, 2.5); - Vector s1 = Vector_(3, 2.6, 2.6, 2.6); + Vector b1 = (Vector(3) << 2.5, 2.5, 2.5); + Vector s1 = (Vector(3) << 2.6, 2.6, 2.6); - Matrix A21 = Matrix_(3,3, + Matrix A21 = (Matrix(3,3) << 3.0, 0.0, 0.0, 0.0, 3.0, 0.0, 0.0, 0.0, 3.0); - Vector b2 = Vector_(3, 3.5, 3.5, 3.5); - Vector s2 = Vector_(3, 3.6, 3.6, 3.6); + Vector b2 = (Vector(3) << 3.5, 3.5, 3.5); + Vector s2 = (Vector(3) << 3.6, 3.6, 3.6); GaussianFactorGraph gfg; gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true)); @@ -416,29 +310,18 @@ TEST_UNSAFE(HessianFactor, CombineAndEliminate) // create a full, uneliminated version of the factor JacobianFactor expectedFactor(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true)); - // perform elimination - GaussianConditional::shared_ptr expectedBN = expectedFactor.eliminate(1); - - // create expected Hessian after elimination - HessianFactor expectedCholeskyFactor(expectedFactor); - - // Convert all factors to hessians - FactorGraph hessians; - BOOST_FOREACH(const GaussianFactorGraph::sharedFactor& factor, gfg) { - if(boost::shared_ptr hf = boost::dynamic_pointer_cast(factor)) - hessians.push_back(hf); - else if(boost::shared_ptr jf = boost::dynamic_pointer_cast(factor)) - hessians.push_back(boost::make_shared(*jf)); - else - CHECK(false); - } + // perform elimination on jacobian + GaussianConditional::shared_ptr expectedConditional; + JacobianFactor::shared_ptr expectedRemainingFactor; + boost::tie(expectedConditional, expectedRemainingFactor) = expectedFactor.eliminate(Ordering(list_of(0))); // Eliminate - GaussianFactorGraph::EliminationResult actualCholesky = EliminateCholesky(gfg, 1); - HessianFactor::shared_ptr actualFactor = boost::dynamic_pointer_cast(actualCholesky.second); + GaussianConditional::shared_ptr actualConditional; + HessianFactor::shared_ptr actualCholeskyFactor; + boost::tie(actualConditional, actualCholeskyFactor) = EliminateCholesky(gfg, Ordering(list_of(0))); - EXPECT(assert_equal(*expectedBN, *actualCholesky.first, 1e-6)); - EXPECT(assert_equal(expectedCholeskyFactor, *actualFactor, 1e-6)); + EXPECT(assert_equal(*expectedConditional, *actualConditional, 1e-6)); + EXPECT(assert_equal(HessianFactor(*expectedRemainingFactor), *actualCholeskyFactor, 1e-6)); } /* ************************************************************************* */ @@ -447,10 +330,10 @@ TEST(HessianFactor, eliminate2 ) // sigmas double sigma1 = 0.2; double sigma2 = 0.1; - Vector sigmas = Vector_(4, sigma1, sigma1, sigma2, sigma2); + Vector sigmas = (Vector(4) << sigma1, sigma1, sigma2, sigma2); // the combined linear factor - Matrix Ax2 = Matrix_(4,2, + Matrix Ax2 = (Matrix(4,2) << // x2 -1., 0., +0.,-1., @@ -458,7 +341,7 @@ TEST(HessianFactor, eliminate2 ) +0.,1. ); - Matrix Al1x1 = Matrix_(4,4, + Matrix Al1x1 = (Matrix(4,4) << // l1 x1 1., 0., 0.00, 0., // f4 0., 1., 0.00, 0., // f4 @@ -473,152 +356,66 @@ TEST(HessianFactor, eliminate2 ) b2(2) = 0.2; b2(3) = -0.1; - vector > meas; + vector > meas; meas.push_back(make_pair(0, Ax2)); meas.push_back(make_pair(1, Al1x1)); JacobianFactor combined(meas, b2, noiseModel::Diagonal::Sigmas(sigmas)); // eliminate the combined factor HessianFactor::shared_ptr combinedLF_Chol(new HessianFactor(combined)); - FactorGraph combinedLFG_Chol; - combinedLFG_Chol.push_back(combinedLF_Chol); + GaussianFactorGraph combinedLFG_Chol = list_of(combinedLF_Chol); - GaussianFactorGraph::EliminationResult actual_Chol = EliminateCholesky( - combinedLFG_Chol, 1); - HessianFactor::shared_ptr actualFactor = boost::dynamic_pointer_cast< - HessianFactor>(actual_Chol.second); + std::pair actual_Chol = + EliminateCholesky(combinedLFG_Chol, Ordering(list_of(0))); // create expected Conditional Gaussian double oldSigma = 0.0894427; // from when R was made unit - Matrix R11 = Matrix_(2,2, + Matrix R11 = (Matrix(2,2) << 1.00, 0.00, 0.00, 1.00 )/oldSigma; - Matrix S12 = Matrix_(2,4, + Matrix S12 = (Matrix(2,4) << -0.20, 0.00,-0.80, 0.00, +0.00,-0.20,+0.00,-0.80 )/oldSigma; - Vector d = Vector_(2,0.2,-0.14)/oldSigma; - GaussianConditional expectedCG(0,d,R11,1,S12,ones(2)); - EXPECT(assert_equal(expectedCG,*actual_Chol.first,1e-4)); + Vector d = (Vector(2) << 0.2,-0.14)/oldSigma; + GaussianConditional expectedCG(0, d, R11, 1, S12); + EXPECT(assert_equal(expectedCG, *actual_Chol.first, 1e-4)); // the expected linear factor double sigma = 0.2236; - Matrix Bl1x1 = Matrix_(2,4, + Matrix Bl1x1 = (Matrix(2,4) << // l1 x1 1.00, 0.00, -1.00, 0.00, 0.00, 1.00, +0.00, -1.00 )/sigma; - Vector b1 = Vector_(2,0.0,0.894427); + Vector b1 = (Vector(2) << 0.0,0.894427); JacobianFactor expectedLF(1, Bl1x1, b1, noiseModel::Isotropic::Sigma(2,1.0)); - EXPECT(assert_equal(HessianFactor(expectedLF), *actualFactor, 1.5e-3)); -} - -/* ************************************************************************* */ -TEST(HessianFactor, eliminateUnsorted) { - - JacobianFactor::shared_ptr factor1( - new JacobianFactor(0, - Matrix_(3,3, - 44.7214, 0.0, 0.0, - 0.0, 44.7214, 0.0, - 0.0, 0.0, 44.7214), - 1, - Matrix_(3,3, - -0.179168, -44.721, 0.717294, - 44.721, -0.179168, -44.9138, - 0.0, 0.0, -44.7214), - Vector_(3, 1.98916e-17, -4.96503e-15, -7.75792e-17), - noiseModel::Unit::Create(3))); - HessianFactor::shared_ptr unsorted_factor2( - new HessianFactor(JacobianFactor(0, - Matrix_(6,3, - 25.8367, 0.1211, 0.0593, - 0.0, 23.4099, 30.8733, - 0.0, 0.0, 25.8729, - 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0), - 1, - Matrix_(6,3, - 25.7429, -1.6897, 0.4587, - 1.6400, 23.3095, -8.4816, - 0.0034, 0.0509, -25.7855, - 0.9997, -0.0002, 0.0824, - 0.0, 0.9973, 0.9517, - 0.0, 0.0, 0.9973), - Vector_(6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0), - noiseModel::Unit::Create(6)))); - Permutation permutation(2); - permutation[0] = 1; - permutation[1] = 0; - unsorted_factor2->permuteWithInverse(permutation); - - HessianFactor::shared_ptr sorted_factor2( - new HessianFactor(JacobianFactor(0, - Matrix_(6,3, - 25.7429, -1.6897, 0.4587, - 1.6400, 23.3095, -8.4816, - 0.0034, 0.0509, -25.7855, - 0.9997, -0.0002, 0.0824, - 0.0, 0.9973, 0.9517, - 0.0, 0.0, 0.9973), - 1, - Matrix_(6,3, - 25.8367, 0.1211, 0.0593, - 0.0, 23.4099, 30.8733, - 0.0, 0.0, 25.8729, - 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0), - Vector_(6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0), - noiseModel::Unit::Create(6)))); - - GaussianFactorGraph sortedGraph; -// sortedGraph.push_back(factor1); - sortedGraph.push_back(sorted_factor2); - - GaussianFactorGraph unsortedGraph; -// unsortedGraph.push_back(factor1); - unsortedGraph.push_back(unsorted_factor2); - - GaussianConditional::shared_ptr expected_bn; - GaussianFactor::shared_ptr expected_factor; - boost::tie(expected_bn, expected_factor) = - EliminatePreferCholesky(sortedGraph, 1); - - GaussianConditional::shared_ptr actual_bn; - GaussianFactor::shared_ptr actual_factor; - boost::tie(actual_bn, actual_factor) = - EliminatePreferCholesky(unsortedGraph, 1); - - EXPECT(assert_equal(*expected_bn, *actual_bn, 1e-10)); - EXPECT(assert_equal(*expected_factor, *actual_factor, 1e-10)); + EXPECT(assert_equal(HessianFactor(expectedLF), *actual_Chol.second, 1.5e-3)); } /* ************************************************************************* */ TEST(HessianFactor, combine) { // update the information matrix with a single jacobian factor - Matrix A0 = Matrix_(2, 2, + Matrix A0 = (Matrix(2, 2) << 11.1803399, 0.0, 0.0, 11.1803399); - Matrix A1 = Matrix_(2, 2, + Matrix A1 = (Matrix(2, 2) << -2.23606798, 0.0, 0.0, -2.23606798); - Matrix A2 = Matrix_(2, 2, + Matrix A2 = (Matrix(2, 2) << -8.94427191, 0.0, 0.0, -8.94427191); - Vector b = Vector_(2, 2.23606798,-1.56524758); + Vector b = (Vector(2) << 2.23606798,-1.56524758); SharedDiagonal model = noiseModel::Diagonal::Sigmas(ones(2)); GaussianFactor::shared_ptr f(new JacobianFactor(0, A0, 1, A1, 2, A2, b, model)); - FactorGraph factors; - factors.push_back(f); + GaussianFactorGraph factors = list_of(f); // Form Ab' * Ab HessianFactor actual(factors); - Matrix expected = Matrix_(7, 7, + Matrix expected = (Matrix(7, 7) << 125.0000, 0.0, -25.0000, 0.0, -100.0000, 0.0, 25.0000, 0.0, 125.0000, 0.0, -25.0000, 0.0, -100.0000, -17.5000, -25.0000, 0.0, 5.0000, 0.0, 20.0000, 0.0, -5.0000, @@ -626,10 +423,56 @@ TEST(HessianFactor, combine) { -100.0000, 0.0, 20.0000, 0.0, 80.0000, 0.0, -20.0000, 0.0, -100.0000, 0.0, 20.0000, 0.0, 80.0000, 14.0000, 25.0000, -17.5000, -5.0000, 3.5000, -20.0000, 14.0000, 7.4500); - EXPECT(assert_equal(Matrix(expected.triangularView()), - Matrix(actual.matrix_.triangularView()), tol)); + EXPECT(assert_equal(expected, Matrix(actual.matrixObject().full()), tol)); } + +/* ************************************************************************* */ +TEST(HessianFactor, gradientAtZero) +{ + Matrix G11 = (Matrix(1, 1) << 1); + Matrix G12 = (Matrix(1, 2) << 0, 0); + Matrix G22 = (Matrix(2, 2) << 1, 0, 0, 1); + Vector g1 = (Vector(1) << -7); + Vector g2 = (Vector(2) << -8, -9); + double f = 194; + + HessianFactor factor(0, 1, G11, G12, g1, G22, g2, f); + + // test gradient at zero + VectorValues expectedG = pair_list_of(0, -g1) (1, -g2); + Matrix A; Vector b; boost::tie(A,b) = factor.jacobian(); + FastVector keys; keys += 0,1; + EXPECT(assert_equal(-A.transpose()*b, expectedG.vector(keys))); + VectorValues actualG = factor.gradientAtZero(); + EXPECT(assert_equal(expectedG, actualG)); +} + +/* ************************************************************************* */ +TEST(HessianFactor, hessianDiagonal) +{ + Matrix G11 = (Matrix(1, 1) << 1); + Matrix G12 = (Matrix(1, 2) << 0, 0); + Matrix G22 = (Matrix(2, 2) << 1, 0, 0, 1); + Vector g1 = (Vector(1) << -7); + Vector g2 = (Vector(2) << -8, -9); + double f = 194; + + HessianFactor factor(0, 1, G11, G12, g1, G22, g2, f); + + // hessianDiagonal + VectorValues expected; + expected.insert(0, (Vector(1) << 1)); + expected.insert(1, (Vector(2) << 1,1)); + EXPECT(assert_equal(expected, factor.hessianDiagonal())); + + // hessianBlockDiagonal + map actualBD = factor.hessianBlockDiagonal(); + LONGS_EQUAL(2,actualBD.size()); + EXPECT(assert_equal(G11,actualBD[0])); + EXPECT(assert_equal(G22,actualBD[1])); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ diff --git a/gtsam/linear/tests/testJacobianFactor.cpp b/gtsam/linear/tests/testJacobianFactor.cpp index fe13fdeac..1fc7365e7 100644 --- a/gtsam/linear/tests/testJacobianFactor.cpp +++ b/gtsam/linear/tests/testJacobianFactor.cpp @@ -20,76 +20,109 @@ #include #include -#include -#include #include +#include +#include + +#include +#include +#include +#include using namespace std; using namespace gtsam; +using namespace boost::assign; -static const Index _x0_=0, _x1_=1, _x2_=2, _x_=5, _y_=6, _l11_=8; +namespace { + namespace simple { + // Terms we'll use + const vector > terms = list_of > + (make_pair(5, Matrix3::Identity())) + (make_pair(10, 2*Matrix3::Identity())) + (make_pair(15, 3*Matrix3::Identity())); -static SharedDiagonal constraintModel = noiseModel::Constrained::All(2); - -/* ************************************************************************* */ -TEST(JacobianFactor, constructor) -{ - Vector b = Vector_(3, 1., 2., 3.); - SharedDiagonal noise = noiseModel::Diagonal::Sigmas(Vector_(3,1.,1.,1.)); - std::list > terms; - terms.push_back(make_pair(_x0_, eye(3))); - terms.push_back(make_pair(_x1_, 2.*eye(3))); - JacobianFactor actual(terms, b, noise); - JacobianFactor expected(_x0_, eye(3), _x1_, 2.*eye(3), b, noise); - EXPECT(assert_equal(expected, actual)); + // RHS and sigmas + const Vector b = (Vector(3) << 1., 2., 3.); + const SharedDiagonal noise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 0.5)); + } } /* ************************************************************************* */ -TEST(JacobianFactor, constructor2) +TEST(JacobianFactor, constructors_and_accessors) { - Vector b = Vector_(3, 1., 2., 3.); - SharedDiagonal noise = noiseModel::Diagonal::Sigmas(Vector_(3,1.,1.,1.)); - std::list > terms; - terms.push_back(make_pair(_x0_, eye(3))); - terms.push_back(make_pair(_x1_, 2.*eye(3))); - const JacobianFactor actual(terms, b, noise); + using namespace simple; - JacobianFactor::const_iterator key0 = actual.begin(); - JacobianFactor::const_iterator key1 = key0 + 1; - EXPECT(assert_equal(*key0, _x0_)); - EXPECT(assert_equal(*key1, _x1_)); - EXPECT(!actual.empty()); - EXPECT_LONGS_EQUAL(3, actual.Ab_.nBlocks()); - - Matrix actualA0 = actual.getA(key0); - Matrix actualA1 = actual.getA(key1); - Vector actualb = actual.getb(); - - EXPECT(assert_equal(eye(3), actualA0)); - EXPECT(assert_equal(2.*eye(3), actualA1)); - EXPECT(assert_equal(b, actualb)); -} - -/* ************************************************************************* */ - -JacobianFactor construct() { - Matrix A = Matrix_(2,2, 1.,2.,3.,4.); - Vector b = Vector_(2, 1.0, 2.0); - SharedDiagonal s = noiseModel::Diagonal::Sigmas(Vector_(2, 3.0, 4.0)); - JacobianFactor::shared_ptr shared( - new JacobianFactor(0, A, b, s)); - return *shared; -} - -TEST(JacobianFactor, return_value) -{ - Matrix A = Matrix_(2,2, 1.,2.,3.,4.); - Vector b = Vector_(2, 1.0, 2.0); - SharedDiagonal s = noiseModel::Diagonal::Sigmas(Vector_(2, 3.0, 4.0)); - JacobianFactor copied = construct(); - EXPECT(assert_equal(b, copied.getb())); - EXPECT(assert_equal(*s, *copied.get_model())); - EXPECT(assert_equal(A, copied.getA(copied.begin()))); + // Test for using different numbers of terms + { + // b vector only constructor + JacobianFactor expected( + boost::make_iterator_range(terms.begin(), terms.begin()), b); + JacobianFactor actual(b); + EXPECT(assert_equal(expected, actual)); + EXPECT(assert_equal(b, expected.getb())); + EXPECT(assert_equal(b, actual.getb())); + EXPECT(!expected.get_model()); + EXPECT(!actual.get_model()); + } + { + // One term constructor + JacobianFactor expected( + boost::make_iterator_range(terms.begin(), terms.begin() + 1), b, noise); + JacobianFactor actual(terms[0].first, terms[0].second, b, noise); + EXPECT(assert_equal(expected, actual)); + LONGS_EQUAL((long)terms[0].first, (long)actual.keys().back()); + EXPECT(assert_equal(terms[0].second, actual.getA(actual.end() - 1))); + EXPECT(assert_equal(b, expected.getb())); + EXPECT(assert_equal(b, actual.getb())); + EXPECT(noise == expected.get_model()); + EXPECT(noise == actual.get_model()); + } + { + // Two term constructor + JacobianFactor expected( + boost::make_iterator_range(terms.begin(), terms.begin() + 2), b, noise); + JacobianFactor actual(terms[0].first, terms[0].second, + terms[1].first, terms[1].second, b, noise); + EXPECT(assert_equal(expected, actual)); + LONGS_EQUAL((long)terms[1].first, (long)actual.keys().back()); + EXPECT(assert_equal(terms[1].second, actual.getA(actual.end() - 1))); + EXPECT(assert_equal(b, expected.getb())); + EXPECT(assert_equal(b, actual.getb())); + EXPECT(noise == expected.get_model()); + EXPECT(noise == actual.get_model()); + } + { + // Three term constructor + JacobianFactor expected( + boost::make_iterator_range(terms.begin(), terms.begin() + 3), b, noise); + JacobianFactor actual(terms[0].first, terms[0].second, + terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, noise); + EXPECT(assert_equal(expected, actual)); + LONGS_EQUAL((long)terms[2].first, (long)actual.keys().back()); + EXPECT(assert_equal(terms[2].second, actual.getA(actual.end() - 1))); + EXPECT(assert_equal(b, expected.getb())); + EXPECT(assert_equal(b, actual.getb())); + EXPECT(noise == expected.get_model()); + EXPECT(noise == actual.get_model()); + } + { + // VerticalBlockMatrix constructor + JacobianFactor expected( + boost::make_iterator_range(terms.begin(), terms.begin() + 3), b, noise); + VerticalBlockMatrix blockMatrix(list_of(3)(3)(3)(1), 3); + blockMatrix(0) = terms[0].second; + blockMatrix(1) = terms[1].second; + blockMatrix(2) = terms[2].second; + blockMatrix(3) = b; + JacobianFactor actual(terms | boost::adaptors::map_keys, blockMatrix, noise); + EXPECT(assert_equal(expected, actual)); + LONGS_EQUAL((long)terms[2].first, (long)actual.keys().back()); + EXPECT(assert_equal(terms[2].second, actual.getA(actual.end() - 1))); + EXPECT(assert_equal(b, expected.getb())); + EXPECT(assert_equal(b, actual.getb())); + EXPECT(noise == expected.get_model()); + EXPECT(noise == actual.get_model()); + } } /* ************************************************************************* */ @@ -98,163 +131,164 @@ TEST(JabobianFactor, Hessian_conversion) { 1.57, 2.695, -1.1, -2.35, 2.695, 11.3125, -0.65, -10.225, -1.1, -0.65, 1, 0.5, - -2.35, -10.225, 0.5, 9.25).finished(), - (Vector(4) << -7.885, -28.5175, 2.75, 25.675).finished(), + -2.35, -10.225, 0.5, 9.25), + (Vector(4) << -7.885, -28.5175, 2.75, 25.675), 73.1725); JacobianFactor expected(0, (Matrix(2,4) << 1.2530, 2.1508, -0.8779, -1.8755, - 0, 2.5858, 0.4789, -2.3943).finished(), - (Vector(2) << -6.2929, -5.7941).finished(), - noiseModel::Unit::Create(2)); + 0, 2.5858, 0.4789, -2.3943), + (Vector(2) << -6.2929, -5.7941)); - JacobianFactor actual(hessian); - - EXPECT(assert_equal(expected, actual, 1e-3)); + EXPECT(assert_equal(expected, JacobianFactor(hessian), 1e-3)); } /* ************************************************************************* */ -TEST( JacobianFactor, constructor_combined){ - double sigma1 = 0.0957; - Matrix A11(2,2); - A11(0,0) = 1; A11(0,1) = 0; - A11(1,0) = 0; A11(1,1) = 1; - Vector b(2); - b(0) = 2; b(1) = -1; - JacobianFactor::shared_ptr f1(new JacobianFactor(0, A11, b*sigma1, noiseModel::Isotropic::Sigma(2,sigma1))); +TEST( JacobianFactor, construct_from_graph) +{ + GaussianFactorGraph factors; + + double sigma1 = 0.1; + Matrix A11 = Matrix::Identity(2,2); + Vector b1(2); b1 << 2, -1; + factors.add(JacobianFactor(10, A11, b1, noiseModel::Isotropic::Sigma(2, sigma1))); double sigma2 = 0.5; - A11(0,0) = 1; A11(0,1) = 0; - A11(1,0) = 0; A11(1,1) = -1; - b(0) = 4 ; b(1) = -5; - JacobianFactor::shared_ptr f2(new JacobianFactor(0, A11, b*sigma2, noiseModel::Isotropic::Sigma(2,sigma2))); + Matrix A21 = -2 * Matrix::Identity(2,2); + Matrix A22 = 3 * Matrix::Identity(2,2); + Vector b2(2); b2 << 4, -5; + factors.add(JacobianFactor(10, A21, 8, A22, b2, noiseModel::Isotropic::Sigma(2, sigma2))); - double sigma3 = 0.25; - A11(0,0) = 1; A11(0,1) = 0; - A11(1,0) = 0; A11(1,1) = -1; - b(0) = 3 ; b(1) = -88; - JacobianFactor::shared_ptr f3(new JacobianFactor(0, A11, b*sigma3, noiseModel::Isotropic::Sigma(2,sigma3))); + double sigma3 = 1.0; + Matrix A32 = -4 * Matrix::Identity(2,2); + Matrix A33 = 5 * Matrix::Identity(2,2); + Vector b3(2); b3 << 3, -6; + factors.add(JacobianFactor(8, A32, 12, A33, b3, noiseModel::Isotropic::Sigma(2, sigma3))); - // TODO: find a real sigma value for this example - double sigma4 = 0.1; - A11(0,0) = 6; A11(0,1) = 0; - A11(1,0) = 0; A11(1,1) = 7; - b(0) = 5 ; b(1) = -6; - JacobianFactor::shared_ptr f4(new JacobianFactor(0, A11*sigma4, b*sigma4, noiseModel::Isotropic::Sigma(2,sigma4))); + Matrix A1(6,2); A1 << A11, A21, Matrix::Zero(2,2); + Matrix A2(6,2); A2 << Matrix::Zero(2,2), A22, A32; + Matrix A3(6,2); A3 << Matrix::Zero(4,2), A33; + Vector b(6); b << b1, b2, b3; + Vector sigmas(6); sigmas << sigma1, sigma1, sigma2, sigma2, sigma3, sigma3; + JacobianFactor expected(10, A1, 8, A2, 12, A3, b, noiseModel::Diagonal::Sigmas(sigmas)); - GaussianFactorGraph lfg; - lfg.push_back(f1); - lfg.push_back(f2); - lfg.push_back(f3); - lfg.push_back(f4); - JacobianFactor combined(lfg); + // The ordering here specifies the order in which the variables will appear in the combined factor + JacobianFactor actual(factors, Ordering(list_of(10)(8)(12))); - Vector sigmas = Vector_(8, sigma1, sigma1, sigma2, sigma2, sigma3, sigma3, sigma4, sigma4); - Matrix A22(8,2); - A22(0,0) = 1; A22(0,1) = 0; - A22(1,0) = 0; A22(1,1) = 1; - A22(2,0) = 1; A22(2,1) = 0; - A22(3,0) = 0; A22(3,1) = -1; - A22(4,0) = 1; A22(4,1) = 0; - A22(5,0) = 0; A22(5,1) = -1; - A22(6,0) = 0.6; A22(6,1) = 0; - A22(7,0) = 0; A22(7,1) = 0.7; - Vector exb(8); - exb(0) = 2*sigma1 ; exb(1) = -1*sigma1; exb(2) = 4*sigma2 ; exb(3) = -5*sigma2; - exb(4) = 3*sigma3 ; exb(5) = -88*sigma3; exb(6) = 5*sigma4 ; exb(7) = -6*sigma4; - - vector > meas; - meas.push_back(make_pair(0, A22)); - JacobianFactor expected(meas, exb, noiseModel::Diagonal::Sigmas(sigmas)); - EXPECT(assert_equal(expected,combined)); + EXPECT(assert_equal(expected, actual)); } /* ************************************************************************* */ -TEST(JacobianFactor, linearFactorN){ - Matrix I = eye(2); - GaussianFactorGraph f; - SharedDiagonal model = noiseModel::Isotropic::Sigma(2,1.0); - f.push_back(JacobianFactor::shared_ptr(new JacobianFactor(0, I, Vector_(2, 10.0, 5.0), model))); - f.push_back(JacobianFactor::shared_ptr(new JacobianFactor(0, -10 * I, 1, 10 * I, Vector_(2, 1.0, -2.0), model))); - f.push_back(JacobianFactor::shared_ptr(new JacobianFactor(1, -10 * I, 2, 10 * I, Vector_(2, 1.5, -1.5), model))); - f.push_back(JacobianFactor::shared_ptr(new JacobianFactor(2, -10 * I, 3, 10 * I, Vector_(2, 2.0, -1.0), model))); +TEST(JacobianFactor, error) +{ + JacobianFactor factor(simple::terms, simple::b, simple::noise); - JacobianFactor combinedFactor(f); + VectorValues values; + values.insert(5, Vector::Constant(3, 1.0)); + values.insert(10, Vector::Constant(3, 0.5)); + values.insert(15, Vector::Constant(3, 1.0/3.0)); - vector > combinedMeasurement; - combinedMeasurement.push_back(make_pair(0, Matrix_(8,2, - 1.0, 0.0, - 0.0, 1.0, - -10.0, 0.0, - 0.0,-10.0, - 0.0, 0.0, - 0.0, 0.0, - 0.0, 0.0, - 0.0, 0.0))); - combinedMeasurement.push_back(make_pair(1, Matrix_(8,2, - 0.0, 0.0, - 0.0, 0.0, - 10.0, 0.0, - 0.0, 10.0, - -10.0, 0.0, - 0.0,-10.0, - 0.0, 0.0, - 0.0, 0.0))); - combinedMeasurement.push_back(make_pair(2, Matrix_(8,2, - 0.0, 0.0, - 0.0, 0.0, - 0.0, 0.0, - 0.0, 0.0, - 10.0, 0.0, - 0.0, 10.0, - -10.0, 0.0, - 0.0,-10.0))); - combinedMeasurement.push_back(make_pair(3, Matrix_(8,2, - 0.0, 0.0, - 0.0, 0.0, - 0.0, 0.0, - 0.0, 0.0, - 0.0, 0.0, - 0.0, 0.0, - 10.0, 0.0, - 0.0,10.0))); - Vector b = Vector_(8, - 10.0, 5.0, 1.0, -2.0, 1.5, -1.5, 2.0, -1.0); - - Vector sigmas = repeat(8,1.0); - JacobianFactor expected(combinedMeasurement, b, noiseModel::Diagonal::Sigmas(sigmas)); - EXPECT(assert_equal(expected,combinedFactor)); -} - -/* ************************************************************************* */ -TEST(JacobianFactor, error) { - Vector b = Vector_(3, 1., 2., 3.); - SharedDiagonal noise = noiseModel::Diagonal::Sigmas(Vector_(3,2.,2.,2.)); - std::list > terms; - terms.push_back(make_pair(_x0_, eye(3))); - terms.push_back(make_pair(_x1_, 2.*eye(3))); - const JacobianFactor jf(terms, b, noise); - - VectorValues values(2, 3); - values[0] = Vector_(3, 1.,2.,3.); - values[1] = Vector_(3, 4.,5.,6.); - - Vector expected_unwhitened = Vector_(3, 8., 10., 12.); - Vector actual_unwhitened = jf.unweighted_error(values); + Vector expected_unwhitened(3); expected_unwhitened << 2.0, 1.0, 0.0; + Vector actual_unwhitened = factor.unweighted_error(values); EXPECT(assert_equal(expected_unwhitened, actual_unwhitened)); - Vector expected_whitened = Vector_(3, 4., 5., 6.); - Vector actual_whitened = jf.error_vector(values); + Vector expected_whitened(3); expected_whitened << 4.0, 2.0, 0.0; + Vector actual_whitened = factor.error_vector(values); EXPECT(assert_equal(expected_whitened, actual_whitened)); - // check behavior when there are more values than in this factor - VectorValues largeValues(3, 3); - largeValues[0] = Vector_(3, 1.,2.,3.); - largeValues[1] = Vector_(3, 4.,5.,6.); - largeValues[2] = Vector_(3, 7.,8.,9.); + double expected_error = 0.5 * expected_whitened.squaredNorm(); + double actual_error = factor.error(values); + DOUBLES_EQUAL(expected_error, actual_error, 1e-10); +} - EXPECT(assert_equal(expected_unwhitened, jf.unweighted_error(largeValues))); - EXPECT(assert_equal(expected_whitened, jf.error_vector(largeValues))); +/* ************************************************************************* */ +TEST(JacobianFactor, matrices_NULL) +{ + // Make sure everything works with NULL noise model + JacobianFactor factor(simple::terms, simple::b); + + Matrix jacobianExpected(3, 9); + jacobianExpected << simple::terms[0].second, simple::terms[1].second, simple::terms[2].second; + Vector rhsExpected = simple::b; + Matrix augmentedJacobianExpected(3, 10); + augmentedJacobianExpected << jacobianExpected, rhsExpected; + + Matrix augmentedHessianExpected = + augmentedJacobianExpected.transpose() * augmentedJacobianExpected; + + // Hessian + EXPECT(assert_equal(Matrix(augmentedHessianExpected.topLeftCorner(9,9)), factor.information())); + EXPECT(assert_equal(augmentedHessianExpected, factor.augmentedInformation())); + + // Whitened Jacobian + EXPECT(assert_equal(jacobianExpected, factor.jacobian().first)); + EXPECT(assert_equal(rhsExpected, factor.jacobian().second)); + EXPECT(assert_equal(augmentedJacobianExpected, factor.augmentedJacobian())); + + // Unwhitened Jacobian + EXPECT(assert_equal(jacobianExpected, factor.jacobianUnweighted().first)); + EXPECT(assert_equal(rhsExpected, factor.jacobianUnweighted().second)); + EXPECT(assert_equal(augmentedJacobianExpected, factor.augmentedJacobianUnweighted())); + + // hessianDiagonal + VectorValues expectDiagonal; + expectDiagonal.insert(5, ones(3)); + expectDiagonal.insert(10, 4*ones(3)); + expectDiagonal.insert(15, 9*ones(3)); + EXPECT(assert_equal(expectDiagonal, factor.hessianDiagonal())); + + // hessianBlockDiagonal + map actualBD = factor.hessianBlockDiagonal(); + LONGS_EQUAL(3,actualBD.size()); + EXPECT(assert_equal(1*eye(3),actualBD[5])); + EXPECT(assert_equal(4*eye(3),actualBD[10])); + EXPECT(assert_equal(9*eye(3),actualBD[15])); +} + +/* ************************************************************************* */ +TEST(JacobianFactor, matrices) +{ + // And now witgh a non-unit noise model + JacobianFactor factor(simple::terms, simple::b, simple::noise); + + Matrix jacobianExpected(3, 9); + jacobianExpected << simple::terms[0].second, simple::terms[1].second, simple::terms[2].second; + Vector rhsExpected = simple::b; + Matrix augmentedJacobianExpected(3, 10); + augmentedJacobianExpected << jacobianExpected, rhsExpected; + + Matrix augmentedHessianExpected = + augmentedJacobianExpected.transpose() * simple::noise->R().transpose() + * simple::noise->R() * augmentedJacobianExpected; + + // Hessian + EXPECT(assert_equal(Matrix(augmentedHessianExpected.topLeftCorner(9,9)), factor.information())); + EXPECT(assert_equal(augmentedHessianExpected, factor.augmentedInformation())); + + // Whitened Jacobian + EXPECT(assert_equal(simple::noise->R() * jacobianExpected, factor.jacobian().first)); + EXPECT(assert_equal(simple::noise->R() * rhsExpected, factor.jacobian().second)); + EXPECT(assert_equal(simple::noise->R() * augmentedJacobianExpected, factor.augmentedJacobian())); + + // Unwhitened Jacobian + EXPECT(assert_equal(jacobianExpected, factor.jacobianUnweighted().first)); + EXPECT(assert_equal(rhsExpected, factor.jacobianUnweighted().second)); + EXPECT(assert_equal(augmentedJacobianExpected, factor.augmentedJacobianUnweighted())); + + // hessianDiagonal + VectorValues expectDiagonal; + // below we divide by the variance 0.5^2 + expectDiagonal.insert(5, (Vector(3) << 1, 1, 1)/0.25); + expectDiagonal.insert(10, (Vector(3) << 4, 4, 4)/0.25); + expectDiagonal.insert(15, (Vector(3) << 9, 9, 9)/0.25); + EXPECT(assert_equal(expectDiagonal, factor.hessianDiagonal())); + + // hessianBlockDiagonal + map actualBD = factor.hessianBlockDiagonal(); + LONGS_EQUAL(3,actualBD.size()); + EXPECT(assert_equal(4*eye(3),actualBD[5])); + EXPECT(assert_equal(16*eye(3),actualBD[10])); + EXPECT(assert_equal(36*eye(3),actualBD[15])); } /* ************************************************************************* */ @@ -263,24 +297,103 @@ TEST(JacobianFactor, operators ) SharedDiagonal sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1); Matrix I = eye(2); - Vector b = Vector_(2,0.2,-0.1); - JacobianFactor lf(_x1_, -I, _x2_, I, b, sigma0_1); + Vector b = (Vector(2) << 0.2,-0.1); + JacobianFactor lf(1, -I, 2, I, b, sigma0_1); VectorValues c; - c.insert(_x1_, Vector_(2,10.,20.)); - c.insert(_x2_, Vector_(2,30.,60.)); + c.insert(1, (Vector(2) << 10.,20.)); + c.insert(2, (Vector(2) << 30.,60.)); // test A*x - Vector expectedE = Vector_(2,200.,400.), e = lf*c; - EXPECT(assert_equal(expectedE,e)); + Vector expectedE = (Vector(2) << 200.,400.); + Vector actualE = lf * c; + EXPECT(assert_equal(expectedE, actualE)); // test A^e VectorValues expectedX; - expectedX.insert(_x1_, Vector_(2,-2000.,-4000.)); - expectedX.insert(_x2_, Vector_(2, 2000., 4000.)); + expectedX.insert(1, (Vector(2) << -2000.,-4000.)); + expectedX.insert(2, (Vector(2) << 2000., 4000.)); VectorValues actualX = VectorValues::Zero(expectedX); - lf.transposeMultiplyAdd(1.0, e, actualX); + lf.transposeMultiplyAdd(1.0, actualE, actualX); EXPECT(assert_equal(expectedX, actualX)); + + // test gradient at zero + Matrix A; Vector b2; boost::tie(A,b2) = lf.jacobian(); + VectorValues expectedG; + expectedG.insert(1, (Vector(2) << 20,-10)); + expectedG.insert(2, (Vector(2) << -20, 10)); + FastVector keys; keys += 1,2; + EXPECT(assert_equal(-A.transpose()*b2, expectedG.vector(keys))); + VectorValues actualG = lf.gradientAtZero(); + EXPECT(assert_equal(expectedG, actualG)); +} + +/* ************************************************************************* */ +TEST(JacobianFactor, default_error ) +{ + JacobianFactor f; + double actual = f.error(VectorValues()); + DOUBLES_EQUAL(0.0, actual, 1e-15); +} + +//* ************************************************************************* */ +TEST(JacobianFactor, empty ) +{ + // create an empty factor + JacobianFactor f; + EXPECT(f.empty()); +} + +/* ************************************************************************* */ +TEST(JacobianFactor, eliminate) +{ + Matrix A01 = (Matrix(3, 3) << + 1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0); + Vector b0 = (Vector(3) << 1.5, 1.5, 1.5); + Vector s0 = (Vector(3) << 1.6, 1.6, 1.6); + + Matrix A10 = (Matrix(3, 3) << + 2.0, 0.0, 0.0, + 0.0, 2.0, 0.0, + 0.0, 0.0, 2.0); + Matrix A11 = (Matrix(3, 3) << + -2.0, 0.0, 0.0, + 0.0, -2.0, 0.0, + 0.0, 0.0, -2.0); + Vector b1 = (Vector(3) << 2.5, 2.5, 2.5); + Vector s1 = (Vector(3) << 2.6, 2.6, 2.6); + + Matrix A21 = (Matrix(3, 3) << + 3.0, 0.0, 0.0, + 0.0, 3.0, 0.0, + 0.0, 0.0, 3.0); + Vector b2 = (Vector(3) << 3.5, 3.5, 3.5); + Vector s2 = (Vector(3) << 3.6, 3.6, 3.6); + + GaussianFactorGraph gfg; + gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true)); + gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true)); + gfg.add(1, A21, b2, noiseModel::Diagonal::Sigmas(s2, true)); + + Matrix zero3x3 = zeros(3,3); + Matrix A0 = gtsam::stack(3, &A10, &zero3x3, &zero3x3); + Matrix A1 = gtsam::stack(3, &A11, &A01, &A21); + Vector b = gtsam::concatVectors(3, &b1, &b0, &b2); + Vector sigmas = gtsam::concatVectors(3, &s1, &s0, &s2); + + JacobianFactor combinedFactor(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true)); + GaussianFactorGraph::EliminationResult expected = combinedFactor.eliminate(list_of(0)); + JacobianFactor::shared_ptr expectedJacobian = boost::dynamic_pointer_cast< + JacobianFactor>(expected.second); + + GaussianFactorGraph::EliminationResult actual = EliminateQR(gfg, list_of(0)); + JacobianFactor::shared_ptr actualJacobian = boost::dynamic_pointer_cast< + JacobianFactor>(actual.second); + + EXPECT(assert_equal(*expected.first, *actual.first)); + EXPECT(assert_equal(*expectedJacobian, *actualJacobian)); } /* ************************************************************************* */ @@ -289,24 +402,24 @@ TEST(JacobianFactor, eliminate2 ) // sigmas double sigma1 = 0.2; double sigma2 = 0.1; - Vector sigmas = Vector_(4, sigma1, sigma1, sigma2, sigma2); + Vector sigmas = (Vector(4) << sigma1, sigma1, sigma2, sigma2); // the combined linear factor - Matrix Ax2 = Matrix_(4,2, - // x2 - -1., 0., - +0.,-1., - 1., 0., - +0.,1. - ); + Matrix Ax2 = (Matrix(4, 2) << + // x2 + -1., 0., + +0.,-1., + 1., 0., + +0.,1. + ); - Matrix Al1x1 = Matrix_(4,4, - // l1 x1 - 1., 0., 0.00, 0., // f4 - 0., 1., 0.00, 0., // f4 - 0., 0., -1., 0., // f2 - 0., 0., 0.00,-1. // f2 - ); + Matrix Al1x1 = (Matrix(4, 4) << + // l1 x1 + 1., 0., 0.00, 0., // f4 + 0., 1., 0.00, 0., // f4 + 0., 0., -1., 0., // f2 + 0., 0., 0.00,-1. // f2 + ); // the RHS Vector b2(4); @@ -315,90 +428,104 @@ TEST(JacobianFactor, eliminate2 ) b2(2) = 0.2; b2(3) = -0.1; - vector > meas; - meas.push_back(make_pair(_x2_, Ax2)); - meas.push_back(make_pair(_l11_, Al1x1)); + vector > meas; + meas.push_back(make_pair(2, Ax2)); + meas.push_back(make_pair(11, Al1x1)); JacobianFactor combined(meas, b2, noiseModel::Diagonal::Sigmas(sigmas)); // eliminate the combined factor - GaussianConditional::shared_ptr actualCG_QR; - JacobianFactor::shared_ptr actualLF_QR(new JacobianFactor(combined)); - actualCG_QR = actualLF_QR->eliminateFirst(); + pair + actual = combined.eliminate(Ordering(list_of(2))); // create expected Conditional Gaussian double oldSigma = 0.0894427; // from when R was made unit - Matrix R11 = Matrix_(2,2, - 1.00, 0.00, - 0.00, 1.00 - )/oldSigma; - Matrix S12 = Matrix_(2,4, - -0.20, 0.00,-0.80, 0.00, - +0.00,-0.20,+0.00,-0.80 - )/oldSigma; - Vector d = Vector_(2,0.2,-0.14)/oldSigma; - GaussianConditional expectedCG(_x2_,d,R11,_l11_,S12,ones(2)); + Matrix R11 = (Matrix(2, 2) << + 1.00, 0.00, + 0.00, 1.00 + )/oldSigma; + Matrix S12 = (Matrix(2, 4) << + -0.20, 0.00,-0.80, 0.00, + +0.00,-0.20,+0.00,-0.80 + )/oldSigma; + Vector d = (Vector(2) << 0.2,-0.14)/oldSigma; + GaussianConditional expectedCG(2, d, R11, 11, S12); - EXPECT_LONGS_EQUAL(0, actualCG_QR->rsd().firstBlock()); - EXPECT_LONGS_EQUAL(0, actualCG_QR->rsd().rowStart()); - EXPECT_LONGS_EQUAL(2, actualCG_QR->rsd().rowEnd()); - EXPECT_LONGS_EQUAL(3, actualCG_QR->rsd().nBlocks()); - EXPECT(assert_equal(expectedCG,*actualCG_QR,1e-4)); + EXPECT(assert_equal(expectedCG, *actual.first, 1e-4)); // the expected linear factor double sigma = 0.2236; - Matrix Bl1x1 = Matrix_(2,4, - // l1 x1 - 1.00, 0.00, -1.00, 0.00, - 0.00, 1.00, +0.00, -1.00 - )/sigma; - Vector b1 = Vector_(2,0.0,0.894427); - JacobianFactor expectedLF(_l11_, Bl1x1, b1, noiseModel::Isotropic::Sigma(2,1.0)); - EXPECT(assert_equal(expectedLF,*actualLF_QR,1e-3)); + Matrix Bl1x1 = (Matrix(2, 4) << + // l1 x1 + 1.00, 0.00, -1.00, 0.00, + 0.00, 1.00, +0.00, -1.00 + )/sigma; + Vector b1 = (Vector(2) << 0.0, 0.894427); + JacobianFactor expectedLF(11, Bl1x1, b1); + EXPECT(assert_equal(expectedLF, *actual.second,1e-3)); } /* ************************************************************************* */ -TEST(JacobianFactor, default_error ) +TEST(JacobianFactor, EliminateQR) { - JacobianFactor f; - vector dims; - VectorValues c(dims); - double actual = f.error(c); - EXPECT(actual==0.0); -} + // Augmented Ab test case for whole factor graph + Matrix Ab = (Matrix(14, 11) << + 4., 0., 1., 4., 1., 0., 3., 6., 8., 8., 1., + 9., 2., 0., 1., 6., 3., 9., 6., 6., 9., 4., + 5., 3., 7., 9., 5., 5., 9., 1., 3., 7., 0., + 5., 6., 5., 7., 9., 4., 0., 1., 1., 3., 5., + 0., 0., 4., 5., 6., 6., 7., 9., 4., 5., 4., + 0., 0., 9., 4., 8., 6., 2., 1., 4., 1., 6., + 0., 0., 6., 0., 4., 2., 4., 0., 1., 9., 6., + 0., 0., 6., 6., 4., 4., 5., 5., 5., 8., 6., + 0., 0., 0., 0., 8., 0., 9., 8., 2., 8., 0., + 0., 0., 0., 0., 0., 9., 4., 6., 3., 2., 0., + 0., 0., 0., 0., 1., 1., 9., 1., 5., 5., 3., + 0., 0., 0., 0., 1., 1., 3., 3., 2., 0., 5., + 0., 0., 0., 0., 0., 0., 0., 0., 2., 4., 6., + 0., 0., 0., 0., 0., 0., 0., 0., 6., 3., 4.); -//* ************************************************************************* */ -TEST(JacobianFactor, empty ) -{ - // create an empty factor - JacobianFactor f; - EXPECT(f.empty()==true); -} + // Create factor graph + const SharedDiagonal sig_4D = noiseModel::Isotropic::Sigma(4, 0.5); + const SharedDiagonal sig_2D = noiseModel::Isotropic::Sigma(2, 0.5); + GaussianFactorGraph factors = list_of + (JacobianFactor(list_of(3)(5)(7)(9)(11), VerticalBlockMatrix(list_of(2)(2)(2)(2)(2)(1), Ab.block(0, 0, 4, 11)), sig_4D)) + (JacobianFactor(list_of(5)(7)(9)(11), VerticalBlockMatrix(list_of(2)(2)(2)(2)(1), Ab.block(4, 2, 4, 9)), sig_4D)) + (JacobianFactor(list_of(7)(9)(11), VerticalBlockMatrix(list_of(2)(2)(2)(1), Ab.block(8, 4, 4, 7)), sig_4D)) + (JacobianFactor(list_of(11), VerticalBlockMatrix(list_of(2)(1), Ab.block(12, 8, 2, 3)), sig_2D)); -/* ************************************************************************* */ -// small aux. function to print out lists of anything -template -void print(const list& i) { - copy(i.begin(), i.end(), ostream_iterator (cout, ",")); - cout << endl; -} + // extract the dense matrix for the graph + Matrix actualDense = factors.augmentedJacobian(); + EXPECT(assert_equal(2.0 * Ab, actualDense)); -/* ************************************************************************* */ -TEST(JacobianFactor, CONSTRUCTOR_GaussianConditional ) -{ - Matrix R11 = eye(2); - Matrix S12 = Matrix_(2,2, - -0.200001, 0.00, - +0.00,-0.200001 - ); - Vector d(2); d(0) = 2.23607; d(1) = -1.56525; - Vector sigmas =repeat(2,0.29907); - GaussianConditional::shared_ptr CG(new GaussianConditional(_x2_,d,R11,_l11_,S12,sigmas)); + // Expected augmented matrix, both GaussianConditional (first 6 rows) and remaining factor (next 4 rows) + Matrix R = 2.0 * (Matrix(11, 11) << + -12.1244, -5.1962, -5.2786, -8.6603, -10.5573, -5.9385, -11.3820, -7.2581, -8.7427, -13.4440, -5.3611, + 0., 4.6904, 5.0254, 5.5432, 5.5737, 3.0153, -3.0153, -3.5635, -3.9290, -2.7412, 2.1625, + 0., 0., -13.8160, -8.7166, -10.2245, -8.8666, -8.7632, -5.2544, -6.9192, -10.5537, -9.3250, + 0., 0., 0., 6.5033, -1.1453, 1.3179, 2.5768, 5.5503, 3.6524, 1.3491, -2.5676, + 0., 0., 0., 0., -9.6242, -2.1148, -9.3509, -10.5846, -3.5366, -6.8561, -3.2277, + 0., 0., 0., 0., 0., 9.7887, 4.3551, 5.7572, 2.7876, 0.1611, 1.1769, + 0., 0., 0., 0., 0., 0., -11.1139, -0.6521, -2.1943, -7.5529, -0.9081, + 0., 0., 0., 0., 0., 0., 0., -4.6479, -1.9367, -6.5170, -3.7685, + 0., 0., 0., 0., 0., 0., 0., 0., 8.2503, 3.3757, 6.8476, + 0., 0., 0., 0., 0., 0., 0., 0., 0., -5.7095, -0.0090, + 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., -7.1635); - // Call the constructor we are testing ! - JacobianFactor actualLF(*CG); + GaussianConditional expectedFragment( + list_of(3)(5)(7)(9)(11), 3, VerticalBlockMatrix(list_of(2)(2)(2)(2)(2)(1), R)); - JacobianFactor expectedLF(_x2_,R11,_l11_,S12,d, noiseModel::Diagonal::Sigmas(sigmas)); - EXPECT(assert_equal(expectedLF,actualLF,1e-5)); + // Eliminate (3 frontal variables, 6 scalar columns) using QR !!!! + GaussianFactorGraph::EliminationResult actual = EliminateQR(factors, list_of(3)(5)(7)); + const JacobianFactor &actualJF = dynamic_cast(*actual.second); + + EXPECT(assert_equal(expectedFragment, *actual.first, 0.001)); + EXPECT(assert_equal(size_t(2), actualJF.keys().size())); + EXPECT(assert_equal(Key(9), actualJF.keys()[0])); + EXPECT(assert_equal(Key(11), actualJF.keys()[1])); + EXPECT(assert_equal(Matrix(R.block(6, 6, 4, 2)), actualJF.getA(actualJF.begin()), 0.001)); + EXPECT(assert_equal(Matrix(R.block(6, 8, 4, 2)), actualJF.getA(actualJF.begin()+1), 0.001)); + EXPECT(assert_equal(Vector(R.col(10).segment(6, 4)), actualJF.getb(), 0.001)); + EXPECT(!actualJF.get_model()); } /* ************************************************************************* */ @@ -406,21 +533,19 @@ TEST ( JacobianFactor, constraint_eliminate1 ) { // construct a linear constraint Vector v(2); v(0)=1.2; v(1)=3.4; - Index key = _x0_; - JacobianFactor lc(key, eye(2), v, constraintModel); + JacobianFactor lc(1, eye(2), v, noiseModel::Constrained::All(2)); // eliminate it - GaussianConditional::shared_ptr actualCG; - JacobianFactor::shared_ptr actualLF(new JacobianFactor(lc)); - actualCG = actualLF->eliminateFirst(); + pair + actual = lc.eliminate(list_of(1)); // verify linear factor - EXPECT(actualLF->size() == 0); + EXPECT(actual.second->size() == 0); // verify conditional Gaussian - Vector sigmas = Vector_(2, 0.0, 0.0); - GaussianConditional expCG(_x0_, v, eye(2), sigmas); - EXPECT(assert_equal(expCG, *actualCG)); + Vector sigmas = (Vector(2) << 0.0, 0.0); + GaussianConditional expCG(1, v, eye(2), noiseModel::Diagonal::Sigmas(sigmas)); + EXPECT(assert_equal(expCG, *actual.first)); } /* ************************************************************************* */ @@ -440,27 +565,31 @@ TEST ( JacobianFactor, constraint_eliminate2 ) A2(0,0) = 1.0 ; A2(0,1) = 2.0; A2(1,0) = 2.0 ; A2(1,1) = 4.0; - JacobianFactor lc(_x_, A1, _y_, A2, b, constraintModel); + JacobianFactor lc(1, A1, 2, A2, b, noiseModel::Constrained::All(2)); // eliminate x and verify results - GaussianConditional::shared_ptr actualCG; - JacobianFactor::shared_ptr actualLF(new JacobianFactor(lc)); - actualCG = actualLF->eliminateFirst(); + pair + actual = lc.eliminate(list_of(1)); - // LF should be null - JacobianFactor expectedLF; - EXPECT(assert_equal(*actualLF, expectedLF)); + // LF should be empty + // It's tricky to create Eigen matrices that are only zero along one dimension + Matrix m(1,2); + Matrix Aempty = m.topRows(0); + Vector bempty = m.block(0,0,0,1); + JacobianFactor expectedLF(2, Aempty, bempty, noiseModel::Constrained::All(0)); + EXPECT(assert_equal(expectedLF, *actual.second)); // verify CG - Matrix R = Matrix_(2, 2, + Matrix R = (Matrix(2, 2) << 1.0, 2.0, 0.0, 1.0); - Matrix S = Matrix_(2,2, + Matrix S = (Matrix(2, 2) << 1.0, 2.0, 0.0, 0.0); - Vector d = Vector_(2, 3.0, 0.6666); - GaussianConditional expectedCG(_x_, d, R, _y_, S, zero(2)); - EXPECT(assert_equal(expectedCG, *actualCG, 1e-4)); + Vector d = (Vector(2) << 3.0, 0.6666); + Vector sigmas = (Vector(2) << 0.0, 0.0); + GaussianConditional expectedCG(1, d, R, 2, S, noiseModel::Diagonal::Sigmas(sigmas)); + EXPECT(assert_equal(expectedCG, *actual.first, 1e-4)); } /* ************************************************************************* */ diff --git a/gtsam/linear/tests/testKalmanFilter.cpp b/gtsam/linear/tests/testKalmanFilter.cpp index 083e942c3..87ff419c8 100644 --- a/gtsam/linear/tests/testKalmanFilter.cpp +++ b/gtsam/linear/tests/testKalmanFilter.cpp @@ -30,7 +30,7 @@ using namespace gtsam; /** Small 2D point class implemented as a Vector */ struct State: Vector { State(double x, double y) : - Vector(Vector_(2, x, y)) { + Vector((Vector(2) << x, y)) { } }; @@ -49,7 +49,7 @@ TEST( KalmanFilter, constructor ) { // Assert it has the correct mean, covariance and information EXPECT(assert_equal(x_initial, p1->mean())); - Matrix Sigma = Matrix_(2, 2, 0.01, 0.0, 0.0, 0.01); + Matrix Sigma = (Matrix(2, 2) << 0.01, 0.0, 0.0, 0.01); EXPECT(assert_equal(Sigma, p1->covariance())); EXPECT(assert_equal(inverse(Sigma), p1->information())); @@ -67,7 +67,7 @@ TEST( KalmanFilter, linear1 ) { // Create the controls and measurement properties for our example Matrix F = eye(2, 2); Matrix B = eye(2, 2); - Vector u = Vector_(2, 1.0, 0.0); + Vector u = (Vector(2) << 1.0, 0.0); SharedDiagonal modelQ = noiseModel::Isotropic::Sigma(2, 0.1); Matrix Q = 0.01*eye(2, 2); Matrix H = eye(2, 2); @@ -124,21 +124,21 @@ TEST( KalmanFilter, linear1 ) { // Run iteration 3 KalmanFilter::State p3p = kf.predict(p2, F, B, u, modelQ); EXPECT(assert_equal(expected3, p3p->mean())); - LONGS_EQUAL(3, KalmanFilter::step(p3p)); + LONGS_EQUAL(3, (long)KalmanFilter::step(p3p)); KalmanFilter::State p3 = kf.update(p3p, H, z3, modelR); EXPECT(assert_equal(expected3, p3->mean())); - LONGS_EQUAL(3, KalmanFilter::step(p3)); + LONGS_EQUAL(3, (long)KalmanFilter::step(p3)); } /* ************************************************************************* */ TEST( KalmanFilter, predict ) { // Create dynamics model - Matrix F = Matrix_(2, 2, 1.0, 0.1, 0.2, 1.1); - Matrix B = Matrix_(2, 3, 1.0, 0.1, 0.2, 1.1, 1.2, 0.8); - Vector u = Vector_(3, 1.0, 0.0, 2.0); - Matrix R = Matrix_(2, 2, 1.0, 0.5, 0.0, 3.0); + Matrix F = (Matrix(2, 2) << 1.0, 0.1, 0.2, 1.1); + Matrix B = (Matrix(2, 3) << 1.0, 0.1, 0.2, 1.1, 1.2, 0.8); + Vector u = (Vector(3) << 1.0, 0.0, 2.0); + Matrix R = (Matrix(2, 2) << 1.0, 0.5, 0.0, 3.0); Matrix M = trans(R)*R; Matrix Q = inverse(M); @@ -168,7 +168,7 @@ TEST( KalmanFilter, predict ) { TEST( KalmanFilter, QRvsCholesky ) { Vector mean = ones(9); - Matrix covariance = 1e-6*Matrix_(9, 9, + Matrix covariance = 1e-6 * (Matrix(9, 9) << 15.0, -6.2, 0.0, 0.0, 0.0, 0.0, 0.0, 63.8, -0.6, -6.2, 21.9, -0.0, 0.0, 0.0, 0.0, -63.8, -0.0, -0.1, 0.0, -0.0, 100.0, 0.0, 0.0, 0.0, 0.0, 0.1, -0.0, @@ -187,7 +187,7 @@ TEST( KalmanFilter, QRvsCholesky ) { KalmanFilter::State p0b = kfb.init(mean, covariance); // Set up dynamics update - Matrix Psi_k = 1e-6*Matrix_(9, 9, + Matrix Psi_k = 1e-6 * (Matrix(9, 9) << 1000000.0, 0.0, 0.0, -19200.0, 600.0, -0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 600.0, 19200.0, 200.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, -0.0, -200.0, 19200.0, 0.0, 0.0, 0.0, @@ -199,7 +199,7 @@ TEST( KalmanFilter, QRvsCholesky ) { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0); Matrix B = zeros(9, 1); Vector u = zero(1); - Matrix dt_Q_k = 1e-6*Matrix_(9, 9, + Matrix dt_Q_k = 1e-6 * (Matrix(9, 9) << 33.7, 3.1, -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3.1, 126.4, -0.3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.0, -0.3, 88.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, @@ -219,10 +219,10 @@ TEST( KalmanFilter, QRvsCholesky ) { EXPECT(assert_equal(pa->information(), pb->information(), 1e-7)); // and in addition attain the correct covariance - Vector expectedMean = Vector_(9, 0.9814, 1.0200, 1.0190, 1., 1., 1., 1., 1., 1.); + Vector expectedMean = (Vector(9) << 0.9814, 1.0200, 1.0190, 1., 1., 1., 1., 1., 1.); EXPECT(assert_equal(expectedMean, pa->mean(), 1e-7)); EXPECT(assert_equal(expectedMean, pb->mean(), 1e-7)); - Matrix expected = 1e-6*Matrix_(9, 9, + Matrix expected = 1e-6 * (Matrix(9, 9) << 48.8, -3.1, -0.0, -0.4, -0.4, 0.0, 0.0, 63.8, -0.6, -3.1, 148.4, -0.3, 0.5, 1.7, 0.2, -63.8, 0.0, -0.1, -0.0, -0.3, 188.0, -0.0, 0.2, 1.2, 0.0, 0.1, 0.0, @@ -236,12 +236,12 @@ TEST( KalmanFilter, QRvsCholesky ) { EXPECT(assert_equal(expected, pb->covariance(), 1e-7)); // prepare update - Matrix H = 1e-3*Matrix_(3, 9, + Matrix H = 1e-3 * (Matrix(3, 9) << 0.0, 9795.9, 83.6, 0.0, 0.0, 0.0, 1000.0, 0.0, 0.0, -9795.9, 0.0, -5.2, 0.0, 0.0, 0.0, 0.0, 1000.0, 0.0, -83.6, 5.2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000.); - Vector z = Vector_(3, 0.2599 , 1.3327 , 0.2007); - Vector sigmas = Vector_(3, 0.3323 , 0.2470 , 0.1904); + Vector z = (Vector(3) << 0.2599 , 1.3327 , 0.2007); + Vector sigmas = (Vector(3) << 0.3323 , 0.2470 , 0.1904); SharedDiagonal modelR = noiseModel::Diagonal::Sigmas(sigmas); // do update @@ -253,10 +253,10 @@ TEST( KalmanFilter, QRvsCholesky ) { EXPECT(assert_equal(pa2->information(), pb2->information(), 1e-7)); // and in addition attain the correct mean and covariance - Vector expectedMean2 = Vector_(9, 0.9207, 0.9030, 1.0178, 1.0002, 0.9992, 0.9998, 0.9981, 1.0035, 0.9882); + Vector expectedMean2 = (Vector(9) << 0.9207, 0.9030, 1.0178, 1.0002, 0.9992, 0.9998, 0.9981, 1.0035, 0.9882); EXPECT(assert_equal(expectedMean2, pa2->mean(), 1e-4));// not happy with tolerance here ! EXPECT(assert_equal(expectedMean2, pb2->mean(), 1e-4));// is something still amiss? - Matrix expected2 = 1e-6*Matrix_(9, 9, + Matrix expected2 = 1e-6 * (Matrix(9, 9) << 46.1, -2.6, -0.0, -0.4, -0.4, 0.0, 0.0, 63.9, -0.5, -2.6, 132.8, -0.5, 0.4, 1.5, 0.2, -64.0, -0.0, -0.1, -0.0, -0.5, 188.0, -0.0, 0.2, 1.2, -0.0, 0.1, 0.0, diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 2165038bf..9d87a163b 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -34,11 +34,11 @@ using namespace noiseModel; using namespace boost::assign; static double sigma = 2, s_1=1.0/sigma, var = sigma*sigma, prc = 1.0/var; -static Matrix R = Matrix_(3, 3, +static Matrix R = (Matrix(3, 3) << s_1, 0.0, 0.0, 0.0, s_1, 0.0, 0.0, 0.0, s_1); -static Matrix Sigma = Matrix_(3, 3, +static Matrix Sigma = (Matrix(3, 3) << var, 0.0, 0.0, 0.0, var, 0.0, 0.0, 0.0, var); @@ -48,17 +48,17 @@ static Matrix Sigma = Matrix_(3, 3, /* ************************************************************************* */ TEST(NoiseModel, constructors) { - Vector whitened = Vector_(3,5.0,10.0,15.0); - Vector unwhitened = Vector_(3,10.0,20.0,30.0); + Vector whitened = (Vector(3) << 5.0,10.0,15.0); + Vector unwhitened = (Vector(3) << 10.0,20.0,30.0); // Construct noise models vector m; m.push_back(Gaussian::SqrtInformation(R)); m.push_back(Gaussian::Covariance(Sigma)); //m.push_back(Gaussian::Information(Q)); - m.push_back(Diagonal::Sigmas(Vector_(3, sigma, sigma, sigma))); - m.push_back(Diagonal::Variances(Vector_(3, var, var, var))); - m.push_back(Diagonal::Precisions(Vector_(3, prc, prc, prc))); + m.push_back(Diagonal::Sigmas((Vector(3) << sigma, sigma, sigma))); + m.push_back(Diagonal::Variances((Vector(3) << var, var, var))); + m.push_back(Diagonal::Precisions((Vector(3) << prc, prc, prc))); m.push_back(Isotropic::Sigma(3, sigma)); m.push_back(Isotropic::Variance(3, var)); m.push_back(Isotropic::Precision(3, prc)); @@ -77,7 +77,7 @@ TEST(NoiseModel, constructors) DOUBLES_EQUAL(distance,mi->Mahalanobis(unwhitened),1e-9); // test R matrix - Matrix expectedR(Matrix_(3, 3, + Matrix expectedR((Matrix(3, 3) << s_1, 0.0, 0.0, 0.0, s_1, 0.0, 0.0, 0.0, s_1)); @@ -86,12 +86,12 @@ TEST(NoiseModel, constructors) EXPECT(assert_equal(expectedR,mi->R())); // test Whiten operator - Matrix H(Matrix_(3, 4, + Matrix H((Matrix(3, 4) << 0.0, 0.0, 1.0, 1.0, 0.0, 1.0, 0.0, 1.0, 1.0, 0.0, 0.0, 1.0)); - Matrix expected(Matrix_(3, 4, + Matrix expected((Matrix(3, 4) << 0.0, 0.0, s_1, s_1, 0.0, s_1, 0.0, s_1, s_1, 0.0, 0.0, s_1)); @@ -107,7 +107,7 @@ TEST(NoiseModel, constructors) /* ************************************************************************* */ TEST(NoiseModel, Unit) { - Vector v = Vector_(3,5.0,10.0,15.0); + Vector v = (Vector(3) << 5.0,10.0,15.0); Gaussian::shared_ptr u(Unit::Create(3)); EXPECT(assert_equal(v,u->whiten(v))); } @@ -117,8 +117,8 @@ TEST(NoiseModel, equals) { Gaussian::shared_ptr g1 = Gaussian::SqrtInformation(R), g2 = Gaussian::SqrtInformation(eye(3,3)); - Diagonal::shared_ptr d1 = Diagonal::Sigmas(Vector_(3, sigma, sigma, sigma)), - d2 = Diagonal::Sigmas(Vector_(3, 0.1, 0.2, 0.3)); + Diagonal::shared_ptr d1 = Diagonal::Sigmas((Vector(3) << sigma, sigma, sigma)), + d2 = Diagonal::Sigmas((Vector(3) << 0.1, 0.2, 0.3)); Isotropic::shared_ptr i1 = Isotropic::Sigma(3, sigma), i2 = Isotropic::Sigma(3, 0.7); @@ -136,7 +136,7 @@ TEST(NoiseModel, equals) ///* ************************************************************************* */ //TEST(NoiseModel, ConstrainedSmart ) //{ -// Gaussian::shared_ptr nonconstrained = Constrained::MixedSigmas(Vector_(3, sigma, 0.0, sigma), true); +// Gaussian::shared_ptr nonconstrained = Constrained::MixedSigmas((Vector(3) << sigma, 0.0, sigma), true); // Diagonal::shared_ptr n1 = boost::dynamic_pointer_cast(nonconstrained); // Constrained::shared_ptr n2 = boost::dynamic_pointer_cast(nonconstrained); // EXPECT(n1); @@ -155,15 +155,14 @@ TEST(NoiseModel, ConstrainedConstructors ) Constrained::shared_ptr actual; size_t d = 3; double m = 100.0; - Vector sigmas = Vector_(3, sigma, 0.0, 0.0); - Vector mu = Vector_(3, 200.0, 300.0, 400.0); + Vector sigmas = (Vector(3) << sigma, 0.0, 0.0); + Vector mu = (Vector(3) << 200.0, 300.0, 400.0); actual = Constrained::All(d); // TODO: why should this be a thousand ??? Dummy variable? EXPECT(assert_equal(gtsam::repeat(d, 1000.0), actual->mu())); EXPECT(assert_equal(gtsam::repeat(d, 0), actual->sigmas())); - double Inf = numeric_limits::infinity(); - EXPECT(assert_equal(gtsam::repeat(d, Inf), actual->invsigmas())); - EXPECT(assert_equal(gtsam::repeat(d, Inf), actual->precisions())); + EXPECT(assert_equal(gtsam::repeat(d, 0), actual->invsigmas())); // Actually zero as dummy value + EXPECT(assert_equal(gtsam::repeat(d, 0), actual->precisions())); // Actually zero as dummy value actual = Constrained::All(d, m); EXPECT(assert_equal(gtsam::repeat(d, m), actual->mu())); @@ -181,12 +180,12 @@ TEST(NoiseModel, ConstrainedConstructors ) /* ************************************************************************* */ TEST(NoiseModel, ConstrainedMixed ) { - Vector feasible = Vector_(3, 1.0, 0.0, 1.0), - infeasible = Vector_(3, 1.0, 1.0, 1.0); - Diagonal::shared_ptr d = Constrained::MixedSigmas(Vector_(3, sigma, 0.0, sigma)); + Vector feasible = (Vector(3) << 1.0, 0.0, 1.0), + infeasible = (Vector(3) << 1.0, 1.0, 1.0); + Diagonal::shared_ptr d = Constrained::MixedSigmas((Vector(3) << sigma, 0.0, sigma)); // NOTE: we catch constrained variables elsewhere, so whitening does nothing - EXPECT(assert_equal(Vector_(3, 0.5, 1.0, 0.5),d->whiten(infeasible))); - EXPECT(assert_equal(Vector_(3, 0.5, 0.0, 0.5),d->whiten(feasible))); + EXPECT(assert_equal((Vector(3) << 0.5, 1.0, 0.5),d->whiten(infeasible))); + EXPECT(assert_equal((Vector(3) << 0.5, 0.0, 0.5),d->whiten(feasible))); DOUBLES_EQUAL(1000.0 + 0.25 + 0.25,d->distance(infeasible),1e-9); DOUBLES_EQUAL(0.5,d->distance(feasible),1e-9); @@ -195,13 +194,13 @@ TEST(NoiseModel, ConstrainedMixed ) /* ************************************************************************* */ TEST(NoiseModel, ConstrainedAll ) { - Vector feasible = Vector_(3, 0.0, 0.0, 0.0), - infeasible = Vector_(3, 1.0, 1.0, 1.0); + Vector feasible = (Vector(3) << 0.0, 0.0, 0.0), + infeasible = (Vector(3) << 1.0, 1.0, 1.0); Constrained::shared_ptr i = Constrained::All(3); // NOTE: we catch constrained variables elsewhere, so whitening does nothing - EXPECT(assert_equal(Vector_(3, 1.0, 1.0, 1.0),i->whiten(infeasible))); - EXPECT(assert_equal(Vector_(3, 0.0, 0.0, 0.0),i->whiten(feasible))); + EXPECT(assert_equal((Vector(3) << 1.0, 1.0, 1.0),i->whiten(infeasible))); + EXPECT(assert_equal((Vector(3) << 0.0, 0.0, 0.0),i->whiten(feasible))); DOUBLES_EQUAL(1000.0 * 3.0,i->distance(infeasible),1e-9); DOUBLES_EQUAL(0.0,i->distance(feasible),1e-9); @@ -210,15 +209,15 @@ TEST(NoiseModel, ConstrainedAll ) /* ************************************************************************* */ namespace exampleQR { // create a matrix to eliminate - Matrix Ab = Matrix_(4, 6+1, + Matrix Ab = (Matrix(4, 7) << -1., 0., 1., 0., 0., 0., -0.2, 0., -1., 0., 1., 0., 0., 0.3, 1., 0., 0., 0., -1., 0., 0.2, 0., 1., 0., 0., 0., -1., -0.1); - Vector sigmas = Vector_(4, 0.2, 0.2, 0.1, 0.1); + Vector sigmas = (Vector(4) << 0.2, 0.2, 0.1, 0.1); // the matrix AB yields the following factorized version: - Matrix Rd = Matrix_(4, 6+1, + Matrix Rd = (Matrix(4, 7) << 11.1803, 0.0, -2.23607, 0.0, -8.94427, 0.0, 2.23607, 0.0, 11.1803, 0.0, -2.23607, 0.0, -8.94427,-1.56525, 0.0, 0.0, 4.47214, 0.0, -4.47214, 0.0, 0.0, @@ -233,13 +232,12 @@ TEST( NoiseModel, QR ) Matrix Ab2 = exampleQR::Ab; // otherwise overwritten ! // Expected result - Vector expectedSigmas = Vector_(4, 0.0894427, 0.0894427, 0.223607, 0.223607); + Vector expectedSigmas = (Vector(4) << 0.0894427, 0.0894427, 0.223607, 0.223607); SharedDiagonal expectedModel = noiseModel::Diagonal::Sigmas(expectedSigmas); // Call Gaussian version SharedDiagonal actual1 = exampleQR::diagonal->QR(Ab1); - SharedDiagonal expected = noiseModel::Unit::Create(4); - EXPECT(assert_equal(*expected,*actual1)); + EXPECT(!actual1); EXPECT(linear_dependent(exampleQR::Rd,Ab1,1e-4)); // Ab was modified in place !!! // Call Constrained version @@ -247,7 +245,7 @@ TEST( NoiseModel, QR ) SharedDiagonal actual2 = constrained->QR(Ab2); SharedDiagonal expectedModel2 = noiseModel::Diagonal::Sigmas(expectedSigmas); EXPECT(assert_equal(*expectedModel2,*actual2,1e-6)); - Matrix expectedRd2 = Matrix_(4, 6+1, + Matrix expectedRd2 = (Matrix(4, 7) << 1., 0., -0.2, 0., -0.8, 0., 0.2, 0., 1., 0.,-0.2, 0., -0.8,-0.14, 0., 0., 1., 0., -1., 0., 0.0, @@ -259,10 +257,10 @@ TEST( NoiseModel, QR ) TEST(NoiseModel, QRNan ) { SharedDiagonal constrained = noiseModel::Constrained::All(2); - Matrix Ab = Matrix_(2, 5, 1., 2., 1., 2., 3., 2., 1., 2., 4., 4.); + Matrix Ab = (Matrix(2, 5) << 1., 2., 1., 2., 3., 2., 1., 2., 4., 4.); SharedDiagonal expected = noiseModel::Constrained::All(2); - Matrix expectedAb = Matrix_(2, 5, 1., 2., 1., 2., 3., 0., 1., 0., 0., 2.0/3); + Matrix expectedAb = (Matrix(2, 5) << 1., 2., 1., 2., 3., 0., 1., 0., 0., 2.0/3); SharedDiagonal actual = constrained->QR(Ab); EXPECT(assert_equal(*expected,*actual)); @@ -290,7 +288,7 @@ TEST(NoiseModel, ScalarOrVector ) /* ************************************************************************* */ TEST(NoiseModel, WhitenInPlace) { - Vector sigmas = Vector_(3, 0.1, 0.1, 0.1); + Vector sigmas = (Vector(3) << 0.1, 0.1, 0.1); SharedDiagonal model = Diagonal::Sigmas(sigmas); Matrix A = eye(3); model->WhitenInPlace(A); @@ -313,8 +311,8 @@ TEST(NoiseModel, robustFunction) TEST(NoiseModel, robustNoise) { const double k = 10.0, error1 = 1.0, error2 = 100.0; - Matrix A = Matrix_(2, 2, 1.0, 10.0, 100.0, 1000.0); - Vector b = Vector_(2, error1, error2); + Matrix A = (Matrix(2, 2) << 1.0, 10.0, 100.0, 1000.0); + Vector b = (Vector(2) << error1, error2); const Robust::shared_ptr robust = Robust::Create( mEstimator::Huber::Create(k, mEstimator::Huber::Scalar), Unit::Create(2)); diff --git a/gtsam/linear/tests/testSampler.cpp b/gtsam/linear/tests/testSampler.cpp index 1d041d017..9db8b7edc 100644 --- a/gtsam/linear/tests/testSampler.cpp +++ b/gtsam/linear/tests/testSampler.cpp @@ -24,7 +24,7 @@ const double tol = 1e-5; /* ************************************************************************* */ TEST(testSampler, basic) { - Vector sigmas = Vector_(3, 1.0, 0.1, 0.0); + Vector sigmas = (Vector(3) << 1.0, 0.1, 0.0); noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(sigmas); char seed = 'A'; Sampler sampler1(model, seed), sampler2(model, 1), sampler3(model, 1); diff --git a/gtsam/linear/tests/testSerializationLinear.cpp b/gtsam/linear/tests/testSerializationLinear.cpp index b258fb52e..5e08a7827 100644 --- a/gtsam/linear/tests/testSerializationLinear.cpp +++ b/gtsam/linear/tests/testSerializationLinear.cpp @@ -19,10 +19,12 @@ #include #include #include -#include #include #include +#include +using namespace boost::assign; + #include #include @@ -47,10 +49,10 @@ BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); /* ************************************************************************* */ // example noise models -static noiseModel::Diagonal::shared_ptr diag3 = noiseModel::Diagonal::Sigmas(Vector_(3, 0.1, 0.2, 0.3)); +static noiseModel::Diagonal::shared_ptr diag3 = noiseModel::Diagonal::Sigmas((Vector(3) << 0.1, 0.2, 0.3)); static noiseModel::Gaussian::shared_ptr gaussian3 = noiseModel::Gaussian::SqrtInformation(2.0 * eye(3,3)); static noiseModel::Isotropic::shared_ptr iso3 = noiseModel::Isotropic::Sigma(3, 0.2); -static noiseModel::Constrained::shared_ptr constrained3 = noiseModel::Constrained::MixedSigmas(Vector_(3, 0.0, 0.0, 0.1)); +static noiseModel::Constrained::shared_ptr constrained3 = noiseModel::Constrained::MixedSigmas((Vector(3) << 0.0, 0.0, 0.1)); static noiseModel::Unit::shared_ptr unit3 = noiseModel::Unit::Create(3); /* ************************************************************************* */ @@ -68,13 +70,13 @@ TEST (Serialization, noiseModels) { EXPECT(equalsDereferencedXML(iso3)); EXPECT(equalsDereferencedBinary(iso3)); - EXPECT(equalsDereferenced(constrained3)); - EXPECT(equalsDereferencedXML(constrained3)); - EXPECT(equalsDereferencedBinary(constrained3)); - EXPECT(equalsDereferenced(unit3)); EXPECT(equalsDereferencedXML(unit3)); EXPECT(equalsDereferencedBinary(unit3)); + + EXPECT(equalsDereferencedBinary(constrained3)); + EXPECT(equalsDereferenced(constrained3)); + EXPECT(equalsDereferencedXML(constrained3)); } /* ************************************************************************* */ @@ -100,9 +102,9 @@ TEST (Serialization, SharedNoiseModel_noiseModels) { EXPECT(equalsDereferencedXML(unit3)); EXPECT(equalsDereferencedBinary(unit3)); + EXPECT(equalsDereferencedBinary(constrained3)); EXPECT(equalsDereferenced(constrained3)); EXPECT(equalsDereferencedXML(constrained3)); - EXPECT(equalsDereferencedBinary(constrained3)); } /* ************************************************************************* */ @@ -119,34 +121,31 @@ TEST (Serialization, SharedDiagonal_noiseModels) { EXPECT(equalsDereferencedXML(unit3)); EXPECT(equalsDereferencedBinary(unit3)); + EXPECT(equalsDereferencedBinary(constrained3)); EXPECT(equalsDereferenced(constrained3)); EXPECT(equalsDereferencedXML(constrained3)); - EXPECT(equalsDereferencedBinary(constrained3)); } -/* ************************************************************************* */ -// Linear components -/* ************************************************************************* */ - /* Create GUIDs for factors */ /* ************************************************************************* */ BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor"); BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor"); +BOOST_CLASS_EXPORT_GUID(gtsam::GaussianConditional , "gtsam::GaussianConditional"); /* ************************************************************************* */ TEST (Serialization, linear_factors) { VectorValues values; - values.insert(0, Vector_(1, 1.0)); - values.insert(1, Vector_(2, 2.0,3.0)); - values.insert(2, Vector_(2, 4.0,5.0)); + values.insert(0, (Vector(1) << 1.0)); + values.insert(1, (Vector(2) << 2.0,3.0)); + values.insert(2, (Vector(2) << 4.0,5.0)); EXPECT(equalsObj(values)); EXPECT(equalsXML(values)); EXPECT(equalsBinary(values)); - Index i1 = 4, i2 = 7; + Key i1 = 4, i2 = 7; Matrix A1 = eye(3), A2 = -1.0 * eye(3); Vector b = ones(3); - SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector_(3, 1.0, 2.0, 3.0)); + SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(3) << 1.0, 2.0, 3.0)); JacobianFactor jacobianfactor(i1, A1, i2, A2, b, model); EXPECT(equalsObj(jacobianfactor)); EXPECT(equalsXML(jacobianfactor)); @@ -160,17 +159,63 @@ TEST (Serialization, linear_factors) { /* ************************************************************************* */ TEST (Serialization, gaussian_conditional) { - Matrix A1 = Matrix_(2,2, 1., 2., 3., 4.); - Matrix A2 = Matrix_(2,2, 6., 0.2, 8., 0.4); - Matrix R = Matrix_(2,2, 0.1, 0.3, 0.0, 0.34); + Matrix A1 = (Matrix(2, 2) << 1., 2., 3., 4.); + Matrix A2 = (Matrix(2, 2) << 6., 0.2, 8., 0.4); + Matrix R = (Matrix(2, 2) << 0.1, 0.3, 0.0, 0.34); Vector d(2); d << 0.2, 0.5; - GaussianConditional cg(0, d, R, 1, A1, 2, A2, ones(2)); + GaussianConditional cg(0, d, R, 1, A1, 2, A2); EXPECT(equalsObj(cg)); EXPECT(equalsXML(cg)); EXPECT(equalsBinary(cg)); } +/* ************************************************************************* */ +TEST (Serialization, gaussian_factor_graph) { + GaussianFactorGraph graph; + { + Matrix A1 = (Matrix(2, 2) << 1., 2., 3., 4.); + Matrix A2 = (Matrix(2, 2) << 6., 0.2, 8., 0.4); + Matrix R = (Matrix(2, 2) << 0.1, 0.3, 0.0, 0.34); + Vector d(2); d << 0.2, 0.5; + GaussianConditional cg(0, d, R, 1, A1, 2, A2); + graph.push_back(cg); + } + + { + Key i1 = 4, i2 = 7; + Matrix A1 = eye(3), A2 = -1.0 * eye(3); + Vector b = ones(3); + SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(3) << 1.0, 2.0, 3.0)); + JacobianFactor jacobianfactor(i1, A1, i2, A2, b, model); + HessianFactor hessianfactor(jacobianfactor); + graph.push_back(jacobianfactor); + graph.push_back(hessianfactor); + } + EXPECT(equalsObj(graph)); + EXPECT(equalsXML(graph)); + EXPECT(equalsBinary(graph)); +} + +/* ************************************************************************* */ +TEST (Serialization, gaussian_bayes_tree) { + const Key x1=1, x2=2, x3=3, x4=4; + const Ordering chainOrdering = Ordering(list_of(x2)(x1)(x3)(x4)); + const SharedDiagonal chainNoise = noiseModel::Isotropic::Sigma(1, 0.5); + const GaussianFactorGraph chain = list_of + (JacobianFactor(x2, (Matrix(1, 1) << 1.), x1, (Matrix(1, 1) << 1.), (Vector(1) << 1.), chainNoise)) + (JacobianFactor(x2, (Matrix(1, 1) << 1.), x3, (Matrix(1, 1) << 1.), (Vector(1) << 1.), chainNoise)) + (JacobianFactor(x3, (Matrix(1, 1) << 1.), x4, (Matrix(1, 1) << 1.), (Vector(1) << 1.), chainNoise)) + (JacobianFactor(x4, (Matrix(1, 1) << 1.), (Vector(1) << 1.), chainNoise)); + + GaussianBayesTree init = *chain.eliminateMultifrontal(chainOrdering); + GaussianBayesTree expected = *chain.eliminateMultifrontal(chainOrdering); + GaussianBayesTree actual; + + std::string serialized = serialize(init); + deserialize(serialized, actual); + EXPECT(assert_equal(expected, actual)); +} /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } diff --git a/gtsam/linear/tests/testVectorValues.cpp b/gtsam/linear/tests/testVectorValues.cpp index c416937ed..75114921b 100644 --- a/gtsam/linear/tests/testVectorValues.cpp +++ b/gtsam/linear/tests/testVectorValues.cpp @@ -15,30 +15,34 @@ * @date Sep 16, 2010 */ -#include - #include #include -#include #include +#include +#include +#include + using namespace std; using namespace boost::assign; +using boost::adaptors::map_keys; using namespace gtsam; /* ************************************************************************* */ -TEST(VectorValues, insert) { +TEST(VectorValues, basics) +{ + // Tests insert(), size(), dim(), exists(), vector() - // insert, with out-of-order indices + // insert VectorValues actual; - actual.insert(0, Vector_(1, 1.0)); - actual.insert(1, Vector_(2, 2.0, 3.0)); - actual.insert(5, Vector_(2, 6.0, 7.0)); - actual.insert(2, Vector_(2, 4.0, 5.0)); + actual.insert(0, (Vector(1) << 1)); + actual.insert(1, (Vector(2) << 2, 3)); + actual.insert(5, (Vector(2) << 6, 7)); + actual.insert(2, (Vector(2) << 4, 5)); // Check dimensions - LONGS_EQUAL(6, actual.size()); + LONGS_EQUAL(4, actual.size()); LONGS_EQUAL(1, actual.dim(0)); LONGS_EQUAL(2, actual.dim(1)); LONGS_EQUAL(2, actual.dim(2)); @@ -54,11 +58,12 @@ TEST(VectorValues, insert) { EXPECT(!actual.exists(6)); // Check values - EXPECT(assert_equal(Vector_(1, 1.0), actual[0])); - EXPECT(assert_equal(Vector_(2, 2.0, 3.0), actual[1])); - EXPECT(assert_equal(Vector_(2, 4.0, 5.0), actual[2])); - EXPECT(assert_equal(Vector_(2, 6.0, 7.0), actual[5])); - EXPECT(assert_equal(Vector_(7, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0), actual.asVector())); + EXPECT(assert_equal((Vector(1) << 1), actual[0])); + EXPECT(assert_equal((Vector(2) << 2, 3), actual[1])); + EXPECT(assert_equal((Vector(2) << 4, 5), actual[2])); + EXPECT(assert_equal((Vector(2) << 6, 7), actual[5])); + FastVector keys = list_of(0)(1)(2)(5); + EXPECT(assert_equal((Vector(7) << 1, 2, 3, 4, 5, 6, 7), actual.vector(keys))); // Check exceptions CHECK_EXCEPTION(actual.insert(1, Vector()), invalid_argument); @@ -66,419 +71,154 @@ TEST(VectorValues, insert) { } /* ************************************************************************* */ -TEST(VectorValues, dimsConstructor) { - vector dims; - dims.push_back(1); - dims.push_back(2); - dims.push_back(2); - - VectorValues actual(dims); - actual[0] = Vector_(1, 1.0); - actual[1] = Vector_(2, 2.0, 3.0); - actual[2] = Vector_(2, 4.0, 5.0); - - // Check dimensions - LONGS_EQUAL(3, actual.size()); - LONGS_EQUAL(1, actual.dim(0)); - LONGS_EQUAL(2, actual.dim(1)); - LONGS_EQUAL(2, actual.dim(2)); - - // Check values - EXPECT(assert_equal(Vector_(1, 1.0), actual[0])); - EXPECT(assert_equal(Vector_(2, 2.0, 3.0), actual[1])); - EXPECT(assert_equal(Vector_(2, 4.0, 5.0), actual[2])); - EXPECT(assert_equal(Vector_(5, 1.0, 2.0, 3.0, 4.0, 5.0), actual.asVector())); -} - -/* ************************************************************************* */ -TEST(VectorValues, copyConstructor) { - - // insert, with out-of-order indices - VectorValues original; - original.insert(0, Vector_(1, 1.0)); - original.insert(1, Vector_(2, 2.0, 3.0)); - original.insert(5, Vector_(2, 6.0, 7.0)); - original.insert(2, Vector_(2, 4.0, 5.0)); - - VectorValues actual(original); - - // Check dimensions - LONGS_EQUAL(6, actual.size()); - LONGS_EQUAL(1, actual.dim(0)); - LONGS_EQUAL(2, actual.dim(1)); - LONGS_EQUAL(2, actual.dim(2)); - LONGS_EQUAL(2, actual.dim(5)); - - // Logic - EXPECT(actual.exists(0)); - EXPECT(actual.exists(1)); - EXPECT(actual.exists(2)); - EXPECT(!actual.exists(3)); - EXPECT(!actual.exists(4)); - EXPECT(actual.exists(5)); - EXPECT(!actual.exists(6)); - - // Check values - EXPECT(assert_equal(Vector_(1, 1.0), actual[0])); - EXPECT(assert_equal(Vector_(2, 2.0, 3.0), actual[1])); - EXPECT(assert_equal(Vector_(2, 4.0, 5.0), actual[2])); - EXPECT(assert_equal(Vector_(2, 6.0, 7.0), actual[5])); - EXPECT(assert_equal(Vector_(7, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0), actual.asVector())); - - // Check exceptions - CHECK_EXCEPTION(actual.insert(1, Vector()), invalid_argument); -} - -/* ************************************************************************* */ -TEST(VectorValues, assignment) { - - VectorValues actual; - - { - // insert, with out-of-order indices - VectorValues original; - original.insert(0, Vector_(1, 1.0)); - original.insert(1, Vector_(2, 2.0, 3.0)); - original.insert(5, Vector_(2, 6.0, 7.0)); - original.insert(2, Vector_(2, 4.0, 5.0)); - actual = original; - } - - // Check dimensions - LONGS_EQUAL(6, actual.size()); - LONGS_EQUAL(1, actual.dim(0)); - LONGS_EQUAL(2, actual.dim(1)); - LONGS_EQUAL(2, actual.dim(2)); - LONGS_EQUAL(2, actual.dim(5)); - - // Logic - EXPECT(actual.exists(0)); - EXPECT(actual.exists(1)); - EXPECT(actual.exists(2)); - EXPECT(!actual.exists(3)); - EXPECT(!actual.exists(4)); - EXPECT(actual.exists(5)); - EXPECT(!actual.exists(6)); - - // Check values - EXPECT(assert_equal(Vector_(1, 1.0), actual[0])); - EXPECT(assert_equal(Vector_(2, 2.0, 3.0), actual[1])); - EXPECT(assert_equal(Vector_(2, 4.0, 5.0), actual[2])); - EXPECT(assert_equal(Vector_(2, 6.0, 7.0), actual[5])); - EXPECT(assert_equal(Vector_(7, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0), actual.asVector())); - - // Check exceptions - CHECK_EXCEPTION(actual.insert(1, Vector()), invalid_argument); -} - -/* ************************************************************************* */ -TEST(VectorValues, SameStructure) { - // insert, with out-of-order indices - VectorValues original; - original.insert(0, Vector_(1, 1.0)); - original.insert(1, Vector_(2, 2.0, 3.0)); - original.insert(5, Vector_(2, 6.0, 7.0)); - original.insert(2, Vector_(2, 4.0, 5.0)); - - VectorValues actual(VectorValues::SameStructure(original)); - - // Check dimensions - LONGS_EQUAL(6, actual.size()); - LONGS_EQUAL(1, actual.dim(0)); - LONGS_EQUAL(2, actual.dim(1)); - LONGS_EQUAL(2, actual.dim(2)); - LONGS_EQUAL(2, actual.dim(5)); - - // Logic - EXPECT(actual.exists(0)); - EXPECT(actual.exists(1)); - EXPECT(actual.exists(2)); - EXPECT(!actual.exists(3)); - EXPECT(!actual.exists(4)); - EXPECT(actual.exists(5)); - EXPECT(!actual.exists(6)); - - // Check exceptions - CHECK_EXCEPTION(actual.insert(1, Vector()), invalid_argument); -} - -/* ************************************************************************* */ -TEST(VectorValues, Zero_fromModel) { - // insert, with out-of-order indices - VectorValues original; - original.insert(0, Vector_(1, 1.0)); - original.insert(1, Vector_(2, 2.0, 3.0)); - original.insert(5, Vector_(2, 6.0, 7.0)); - original.insert(2, Vector_(2, 4.0, 5.0)); - - VectorValues actual(VectorValues::Zero(original)); - - // Check dimensions - LONGS_EQUAL(6, actual.size()); - LONGS_EQUAL(1, actual.dim(0)); - LONGS_EQUAL(2, actual.dim(1)); - LONGS_EQUAL(2, actual.dim(2)); - LONGS_EQUAL(2, actual.dim(5)); - - // Values - EXPECT(assert_equal(Vector::Zero(1), actual[0])); - EXPECT(assert_equal(Vector::Zero(2), actual[1])); - EXPECT(assert_equal(Vector::Zero(2), actual[5])); - EXPECT(assert_equal(Vector::Zero(2), actual[2])); - - // Logic - EXPECT(actual.exists(0)); - EXPECT(actual.exists(1)); - EXPECT(actual.exists(2)); - EXPECT(!actual.exists(3)); - EXPECT(!actual.exists(4)); - EXPECT(actual.exists(5)); - EXPECT(!actual.exists(6)); - - // Check exceptions - CHECK_EXCEPTION(actual.insert(1, Vector()), invalid_argument); -} - -/* ************************************************************************* */ -TEST(VectorValues, Zero_fromDims) { - vector dims; - dims.push_back(1); - dims.push_back(2); - dims.push_back(2); - - VectorValues actual(VectorValues::Zero(dims)); - - // Check dimensions - LONGS_EQUAL(3, actual.size()); - LONGS_EQUAL(1, actual.dim(0)); - LONGS_EQUAL(2, actual.dim(1)); - LONGS_EQUAL(2, actual.dim(2)); - - // Values - EXPECT(assert_equal(Vector::Zero(1), actual[0])); - EXPECT(assert_equal(Vector::Zero(2), actual[1])); - EXPECT(assert_equal(Vector::Zero(2), actual[2])); -} - -/* ************************************************************************* */ -TEST(VectorValues, Zero_fromUniform) { - VectorValues actual(VectorValues::Zero(3, 2)); - - // Check dimensions - LONGS_EQUAL(3, actual.size()); - LONGS_EQUAL(2, actual.dim(0)); - LONGS_EQUAL(2, actual.dim(1)); - LONGS_EQUAL(2, actual.dim(2)); - - // Values - EXPECT(assert_equal(Vector::Zero(2), actual[0])); - EXPECT(assert_equal(Vector::Zero(2), actual[1])); - EXPECT(assert_equal(Vector::Zero(2), actual[2])); -} - -/* ************************************************************************* */ -TEST(VectorValues, resizeLike) { - // insert, with out-of-order indices - VectorValues original; - original.insert(0, Vector_(1, 1.0)); - original.insert(1, Vector_(2, 2.0, 3.0)); - original.insert(5, Vector_(2, 6.0, 7.0)); - original.insert(2, Vector_(2, 4.0, 5.0)); - - VectorValues actual(10, 3); - actual.resizeLike(original); - - // Check dimensions - LONGS_EQUAL(6, actual.size()); - LONGS_EQUAL(1, actual.dim(0)); - LONGS_EQUAL(2, actual.dim(1)); - LONGS_EQUAL(2, actual.dim(2)); - LONGS_EQUAL(2, actual.dim(5)); - - // Logic - EXPECT(actual.exists(0)); - EXPECT(actual.exists(1)); - EXPECT(actual.exists(2)); - EXPECT(!actual.exists(3)); - EXPECT(!actual.exists(4)); - EXPECT(actual.exists(5)); - EXPECT(!actual.exists(6)); - - // Check exceptions - CHECK_EXCEPTION(actual.insert(1, Vector()), invalid_argument); -} - -/* ************************************************************************* */ -TEST(VectorValues, resize_fromUniform) { - VectorValues actual(4, 10); - actual.resize(3, 2); - - actual[0] = Vector_(2, 1.0, 2.0); - actual[1] = Vector_(2, 2.0, 3.0); - actual[2] = Vector_(2, 4.0, 5.0); - - // Check dimensions - LONGS_EQUAL(3, actual.size()); - LONGS_EQUAL(2, actual.dim(0)); - LONGS_EQUAL(2, actual.dim(1)); - LONGS_EQUAL(2, actual.dim(2)); - - // Check values - EXPECT(assert_equal(Vector_(2, 1.0, 2.0), actual[0])); - EXPECT(assert_equal(Vector_(2, 2.0, 3.0), actual[1])); - EXPECT(assert_equal(Vector_(2, 4.0, 5.0), actual[2])); - EXPECT(assert_equal(Vector_(6, 1.0, 2.0, 2.0, 3.0, 4.0, 5.0), actual.asVector())); -} - -/* ************************************************************************* */ -TEST(VectorValues, resize_fromDims) { - vector dims; - dims.push_back(1); - dims.push_back(2); - dims.push_back(2); - - VectorValues actual(4, 10); - actual.resize(dims); - actual[0] = Vector_(1, 1.0); - actual[1] = Vector_(2, 2.0, 3.0); - actual[2] = Vector_(2, 4.0, 5.0); - - // Check dimensions - LONGS_EQUAL(3, actual.size()); - LONGS_EQUAL(1, actual.dim(0)); - LONGS_EQUAL(2, actual.dim(1)); - LONGS_EQUAL(2, actual.dim(2)); - - // Check values - EXPECT(assert_equal(Vector_(1, 1.0), actual[0])); - EXPECT(assert_equal(Vector_(2, 2.0, 3.0), actual[1])); - EXPECT(assert_equal(Vector_(2, 4.0, 5.0), actual[2])); - EXPECT(assert_equal(Vector_(5, 1.0, 2.0, 3.0, 4.0, 5.0), actual.asVector())); -} - -/* ************************************************************************* */ -TEST(VectorValues, append) { - // insert - VectorValues actual; - actual.insert(0, Vector_(1, 1.0)); - actual.insert(1, Vector_(2, 2.0, 3.0)); - actual.insert(2, Vector_(2, 4.0, 5.0)); - - // append - vector dims(2); - dims[0] = 3; - dims[1] = 5; - actual.append(dims); - - // Check dimensions - LONGS_EQUAL(5, actual.size()); - LONGS_EQUAL(1, actual.dim(0)); - LONGS_EQUAL(2, actual.dim(1)); - LONGS_EQUAL(2, actual.dim(2)); - LONGS_EQUAL(3, actual.dim(3)); - LONGS_EQUAL(5, actual.dim(4)); - - // Logic - EXPECT(actual.exists(0)); - EXPECT(actual.exists(1)); - EXPECT(actual.exists(2)); - EXPECT(actual.exists(3)); - EXPECT(actual.exists(4)); - EXPECT(!actual.exists(5)); - - // Check values - EXPECT(assert_equal(Vector_(1, 1.0), actual[0])); - EXPECT(assert_equal(Vector_(2, 2.0, 3.0), actual[1])); - EXPECT(assert_equal(Vector_(2, 4.0, 5.0), actual[2])); - - // Check exceptions - CHECK_EXCEPTION(actual.insert(3, Vector()), invalid_argument); -} - -/* ************************************************************************* */ -TEST(VectorValues, append_withValuesFromVector) { - // Constructor with initial values - vector dims(3); - dims[0] = 1; - dims[1] = 2; - dims[2] = 2; - Vector d = Vector_(5, 1.0, 2.0, 3.0, 4.0, 5.0); - VectorValues actual(d, dims); - +TEST(VectorValues, combine) +{ VectorValues expected; - expected.insert(0, Vector_(1, 1.0)); - expected.insert(1, Vector_(2, 2.0, 3.0)); - expected.insert(2, Vector_(2, 4.0, 5.0)); + expected.insert(0, (Vector(1) << 1)); + expected.insert(1, (Vector(2) << 2, 3)); + expected.insert(5, (Vector(2) << 6, 7)); + expected.insert(2, (Vector(2) << 4, 5)); - EXPECT(assert_equal(expected, actual)); + VectorValues first; + first.insert(0, (Vector(1) << 1)); + first.insert(1, (Vector(2) << 2, 3)); - // Append with initial values - expected.insert(3, Vector_(1, 6.0)); - expected.insert(4, Vector_(3, 7.0, 8.0, 9.0)); + VectorValues second; + second.insert(5, (Vector(2) << 6, 7)); + second.insert(2, (Vector(2) << 4, 5)); - vector dims2(2); - dims2[0] = 1; - dims2[1] = 3; - Vector d2 = Vector_(4, 6.0, 7.0, 8.0, 9.0); - actual.append(d2, dims2); + VectorValues actual(first, second); EXPECT(assert_equal(expected, actual)); } /* ************************************************************************* */ -TEST(VectorValues, hasSameStructure) { - VectorValues v1(2, 3); - VectorValues v2(3, 2); - VectorValues v3(4, 2); - VectorValues v4(4, 2); - - EXPECT(!v1.hasSameStructure(v2)); - EXPECT(!v2.hasSameStructure(v3)); - EXPECT(v3.hasSameStructure(v4)); - EXPECT(VectorValues().hasSameStructure(VectorValues())); - EXPECT(!v1.hasSameStructure(VectorValues())); -} - - -/* ************************************************************************* */ -TEST(VectorValues, permute) { - - VectorValues original; - original.insert(0, Vector_(1, 1.0)); - original.insert(1, Vector_(2, 2.0, 3.0)); - original.insert(2, Vector_(2, 4.0, 5.0)); - original.insert(3, Vector_(2, 6.0, 7.0)); - - VectorValues expected; - expected.insert(0, Vector_(2, 4.0, 5.0)); // from 2 - expected.insert(1, Vector_(1, 1.0)); // from 0 - expected.insert(2, Vector_(2, 6.0, 7.0)); // from 3 - expected.insert(3, Vector_(2, 2.0, 3.0)); // from 1 - - Permutation permutation(4); - permutation[0] = 2; - permutation[1] = 0; - permutation[2] = 3; - permutation[3] = 1; - - VectorValues actual = original; - actual.permuteInPlace(permutation); - - EXPECT(assert_equal(expected, actual)); -} - -/* ************************************************************************* */ -TEST(VectorValues, subvector) { +TEST(VectorValues, subvector) +{ VectorValues init; - init.insert(0, Vector_(1, 1.0)); - init.insert(1, Vector_(2, 2.0, 3.0)); - init.insert(2, Vector_(2, 4.0, 5.0)); - init.insert(3, Vector_(2, 6.0, 7.0)); + init.insert(10, (Vector(1) << 1)); + init.insert(11, (Vector(2) << 2, 3)); + init.insert(12, (Vector(2) << 4, 5)); + init.insert(13, (Vector(2) << 6, 7)); - std::vector indices; - indices += 0, 2, 3; - Vector expSubVector = Vector_(5, 1.0, 4.0, 5.0, 6.0, 7.0); - EXPECT(assert_equal(expSubVector, init.vector(indices))); + std::vector keys; + keys += 10, 12, 13; + Vector expSubVector = (Vector(5) << 1, 4, 5, 6, 7); + EXPECT(assert_equal(expSubVector, init.vector(keys))); +} + +/* ************************************************************************* */ +TEST(VectorValues, LinearAlgebra) +{ + VectorValues test1; + test1.insert(0, (Vector(1) << 1)); + test1.insert(1, (Vector(2) << 2, 3)); + test1.insert(5, (Vector(2) << 6, 7)); + test1.insert(2, (Vector(2) << 4, 5)); + + VectorValues test2; + test2.insert(0, (Vector(1) << 6)); + test2.insert(1, (Vector(2) << 1, 6)); + test2.insert(5, (Vector(2) << 4, 3)); + test2.insert(2, (Vector(2) << 1, 8)); + + // Dot product + double dotExpected = test1.vector().dot(test2.vector()); + double dotActual = test1.dot(test2); + DOUBLES_EQUAL(dotExpected, dotActual, 1e-10); + + // Norm + double normExpected = test1.vector().norm(); + double normActual = test1.norm(); + DOUBLES_EQUAL(normExpected, normActual, 1e-10); + + // Squared norm + double sqNormExpected = test1.vector().norm(); + double sqNormActual = test1.norm(); + DOUBLES_EQUAL(sqNormExpected, sqNormActual, 1e-10); + + // Addition + Vector sumExpected = test1.vector() + test2.vector(); + VectorValues sumActual = test1 + test2; + EXPECT(sumActual.hasSameStructure(test1)); + EXPECT(assert_equal(sumExpected, sumActual.vector())); + Vector sum2Expected = sumActual.vector() + test1.vector(); + VectorValues sum2Actual = sumActual; + sum2Actual += test1; + EXPECT(assert_equal(sum2Expected, sum2Actual.vector())); + + // Add to empty + VectorValues sumActual3; + sumActual3.addInPlace_(test1); + sumActual3.addInPlace_(test2); + EXPECT(assert_equal(sumExpected, sumActual3.vector())); + + // Subtraction + Vector difExpected = test1.vector() - test2.vector(); + VectorValues difActual = test1 - test2; + EXPECT(difActual.hasSameStructure(test1)); + EXPECT(assert_equal(difExpected, difActual.vector())); + + // Scaling + Vector scalExpected = test1.vector() * 5.0; + VectorValues scalActual = test1; + scalActual *= 5.0; + EXPECT(assert_equal(scalExpected, scalActual.vector())); + VectorValues scal2Actual = 5.0 * test1; + EXPECT(assert_equal(scalExpected, scal2Actual.vector())); +} + +/* ************************************************************************* */ +TEST(VectorValues, convert) +{ + Vector x(7); + x << 1, 2, 3, 4, 5, 6, 7; + + VectorValues expected; + expected.insert(0, (Vector(1) << 1)); + expected.insert(1, (Vector(2) << 2, 3)); + expected.insert(2, (Vector(2) << 4, 5)); + expected.insert(5, (Vector(2) << 6, 7)); + + std::map dims; + dims.insert(make_pair(0,1)); + dims.insert(make_pair(1,2)); + dims.insert(make_pair(2,2)); + dims.insert(make_pair(5,2)); + VectorValues actual(x,dims); + EXPECT(assert_equal(expected, actual)); + + // Test other direction, note vector() is not guaranteed to give right result + FastVector keys = list_of(0)(1)(2)(5); + EXPECT(assert_equal(x, actual.vector(keys))); + + // Test version with dims argument + EXPECT(assert_equal(x, actual.vector(dims))); +} + +/* ************************************************************************* */ +TEST(VectorValues, vector_sub) +{ + VectorValues vv; + vv.insert(0, (Vector(1) << 1)); + vv.insert(1, (Vector(2) << 2, 3)); + vv.insert(2, (Vector(2) << 4, 5)); + vv.insert(5, (Vector(2) << 6, 7)); + vv.insert(7, (Vector(2) << 8, 9)); + + std::map dims; + dims.insert(make_pair(0,1)); + dims.insert(make_pair(5,2)); + + Vector expected(3); + expected << 1, 6, 7; + + // Test FastVector version + FastVector keys = list_of(0)(5); + EXPECT(assert_equal(expected, vv.vector(keys))); + + // Test version with dims argument + EXPECT(assert_equal(expected, vv.vector(dims))); } /* ************************************************************************* */ diff --git a/gtsam/linear/tests/timeGaussianFactor.cpp b/gtsam/linear/tests/timeGaussianFactor.cpp index 3938abc8b..0ffdd1cfa 100644 --- a/gtsam/linear/tests/timeGaussianFactor.cpp +++ b/gtsam/linear/tests/timeGaussianFactor.cpp @@ -53,7 +53,7 @@ static const Index _x1_=1, _x2_=2, _l1_=3; int main() { // create a linear factor - Matrix Ax2 = Matrix_(8,2, + Matrix Ax2 = (Mat(8, 2) << // x2 -5., 0., +0.,-5., @@ -65,7 +65,7 @@ int main() +0.,10. ); - Matrix Al1 = Matrix_(8,10, + Matrix Al1 = (Mat(8, 10) << // l1 5., 0.,1.,2.,3.,4.,5.,6.,7.,8., 0., 5.,1.,2.,3.,4.,5.,6.,7.,8., @@ -77,7 +77,7 @@ int main() 0., 0.,1.,2.,3.,4.,5.,6.,7.,8. ); - Matrix Ax1 = Matrix_(8,2, + Matrix Ax1 = (Mat(8, 2) << // x1 0.00, 0.,1.,2.,3.,4.,5.,6.,7.,8., 0.00, 0.,1.,2.,3.,4.,5.,6.,7.,8., diff --git a/gtsam/linear/tests/timeSLAMlike.cpp b/gtsam/linear/tests/timeSLAMlike.cpp index 340e16a79..479c03940 100644 --- a/gtsam/linear/tests/timeSLAMlike.cpp +++ b/gtsam/linear/tests/timeSLAMlike.cpp @@ -18,8 +18,6 @@ #include #include -#include -#include #include #include diff --git a/gtsam/navigation/.gitignore b/gtsam/navigation/.gitignore new file mode 100644 index 000000000..798d17f47 --- /dev/null +++ b/gtsam/navigation/.gitignore @@ -0,0 +1 @@ +/*.*~ diff --git a/gtsam/navigation/AttitudeFactor.h b/gtsam/navigation/AttitudeFactor.h index 44dd4183a..64603bd99 100644 --- a/gtsam/navigation/AttitudeFactor.h +++ b/gtsam/navigation/AttitudeFactor.h @@ -61,7 +61,7 @@ public: * Version of AttitudeFactor for Rot3 * @addtogroup Navigation */ -class Rot3AttitudeFactor: public NoiseModelFactor1, public AttitudeFactor { +class GTSAM_EXPORT Rot3AttitudeFactor: public NoiseModelFactor1, public AttitudeFactor { typedef NoiseModelFactor1 Base; @@ -129,7 +129,7 @@ private: * Version of AttitudeFactor for Pose3 * @addtogroup Navigation */ -class Pose3AttitudeFactor: public NoiseModelFactor1, +class GTSAM_EXPORT Pose3AttitudeFactor: public NoiseModelFactor1, public AttitudeFactor { typedef NoiseModelFactor1 Base; diff --git a/gtsam/navigation/CMakeLists.txt b/gtsam/navigation/CMakeLists.txt index 755098309..3e82af774 100644 --- a/gtsam/navigation/CMakeLists.txt +++ b/gtsam/navigation/CMakeLists.txt @@ -2,30 +2,10 @@ file(GLOB navigation_headers "*.h") install(FILES ${navigation_headers} DESTINATION include/gtsam/navigation) -# Components to link tests in this subfolder against -set(navigation_local_libs - nonlinear - linear - inference - geometry - base - ccolamd - GeographicLib -) - -# Exclude tests that don't work -set (navigation_excluded_tests "") - -include_directories( - ${PROJECT_SOURCE_DIR}/gtsam/3rdparty/GeographicLib/include/ -) - # Add all tests -if (GTSAM_BUILD_TESTS) - gtsam_add_subdir_tests(navigation "${navigation_local_libs}" "${gtsam-default};GeographicLib" "${navigation_excluded_tests}") -endif() +add_subdirectory(tests) # Build timing scripts if (GTSAM_BUILD_TIMING) - gtsam_add_subdir_timing(navigation "${navigation_local_libs}" "${gtsam-default}" "${navigation_excluded_files}") + gtsam_add_subdir_timing(navigation "gtsam" "gtsam" "${navigation_excluded_files}") endif(GTSAM_BUILD_TIMING) diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 0a7fb270c..559568c84 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -11,7 +11,7 @@ /** * @file CombinedImuFactor.h - * @author Luca Carlone, Stephen Williams, Richard Roberts + * @author Luca Carlone, Stephen Williams, Richard Roberts, Vadim Indelman **/ #pragma once @@ -48,39 +48,6 @@ namespace gtsam { /** Struct to store results of preintegrating IMU measurements. Can be build * incrementally so as to avoid costly integration at time of factor construction. */ - /** Right Jacobian for Exponential map in SO(3) - equation (10.86) and following equations in [1] */ - static Matrix3 rightJacobianExpMapSO3(const Vector3& x) { - // x is the axis-angle representation (exponential coordinates) for a rotation - double normx = norm_2(x); // rotation angle - Matrix3 Jr; - if (normx < 10e-8){ - Jr = Matrix3::Identity(); - } - else{ - const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ - Jr = Matrix3::Identity() - ((1-cos(normx))/(normx*normx)) * X + - ((normx-sin(normx))/(normx*normx*normx)) * X * X; // right Jacobian - } - return Jr; - } - - /** Right Jacobian for Log map in SO(3) - equation (10.86) and following equations in [1] */ - static Matrix3 rightJacobianExpMapSO3inverse(const Vector3& x) { - // x is the axis-angle representation (exponential coordinates) for a rotation - double normx = norm_2(x); // rotation angle - Matrix3 Jrinv; - - if (normx < 10e-8){ - Jrinv = Matrix3::Identity(); - } - else{ - const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ - Jrinv = Matrix3::Identity() + - 0.5 * X + (1/(normx*normx) - (1+cos(normx))/(2*normx * sin(normx)) ) * X * X; - } - return Jrinv; - } - /** CombinedPreintegratedMeasurements accumulates (integrates) the IMU measurements (rotation rates and accelerations) * and the corresponding covariance matrix. The measurements are then used to build the Preintegrated IMU factor*/ class CombinedPreintegratedMeasurements { @@ -187,7 +154,7 @@ namespace gtsam { const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement - const Matrix3 Jr_theta_incr = rightJacobianExpMapSO3(theta_incr); // Right jacobian computed at theta_incr + const Matrix3 Jr_theta_incr = Rot3::rightJacobianExpMapSO3(theta_incr); // Right jacobian computed at theta_incr // Update Jacobians /* ----------------------------------------------------------------------------------------------------------------------- */ @@ -208,11 +175,11 @@ namespace gtsam { Matrix3 Z_3x3 = Matrix3::Zero(); Matrix3 I_3x3 = Matrix3::Identity(); const Vector3 theta_i = Rot3::Logmap(deltaRij); // parametrization of so(3) - const Matrix3 Jr_theta_i = rightJacobianExpMapSO3(theta_i); + const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3(theta_i); Rot3 Rot_j = deltaRij * Rincr; const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3) - const Matrix3 Jrinv_theta_j = rightJacobianExpMapSO3inverse(theta_j); + const Matrix3 Jrinv_theta_j = Rot3::rightJacobianExpMapSO3inverse(theta_j); // Single Jacobians to propagate covariance Matrix3 H_pos_pos = I_3x3; @@ -236,7 +203,7 @@ namespace gtsam { // overall Jacobian wrt preintegrated measurements (df/dx) Matrix F(15,15); F << H_pos_pos, H_pos_vel, H_pos_angles, Z_3x3, Z_3x3, - H_vel_pos, H_vel_vel, H_vel_angles, H_vel_biasacc, Z_3x3, + H_vel_pos, H_vel_vel, H_vel_angles, H_vel_biasacc, Z_3x3, H_angles_pos, H_angles_vel, H_angles_angles, Z_3x3, H_angles_biasomega, Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3; @@ -274,6 +241,7 @@ namespace gtsam { // Update preintegrated measurements /* ----------------------------------------------------------------------------------------------------------------------- */ + // deltaPij += deltaVij * deltaT; deltaPij += deltaVij * deltaT + 0.5 * deltaRij.matrix() * biasHat.correctAccelerometer(measuredAcc) * deltaT*deltaT; deltaVij += deltaRij.matrix() * correctedAcc * deltaT; deltaRij = deltaRij * Rincr; @@ -435,11 +403,11 @@ namespace gtsam { const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j)); - const Matrix3 Jr_theta_bcc = rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected); + const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected); const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij); - const Matrix3 Jrinv_fRhat = rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat)); + const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat)); if(H1) { H1->resize(15,6); @@ -514,8 +482,8 @@ namespace gtsam { } if(H5) { - const Matrix3 Jrinv_theta_bc = rightJacobianExpMapSO3inverse(theta_biascorrected); - const Matrix3 Jr_JbiasOmegaIncr = rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega * biasOmegaIncr); + const Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected); + const Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega * biasOmegaIncr); const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega; H5->resize(15,6); diff --git a/gtsam/navigation/GPSFactor.h b/gtsam/navigation/GPSFactor.h index f4ef6c49a..6902f81f1 100644 --- a/gtsam/navigation/GPSFactor.h +++ b/gtsam/navigation/GPSFactor.h @@ -31,7 +31,7 @@ namespace gtsam { * See Farrell08book or e.g. http://www.dirsig.org/docs/new/coordinates.html * @addtogroup Navigation */ -class GPSFactor: public NoiseModelFactor1 { +class GTSAM_EXPORT GPSFactor: public NoiseModelFactor1 { private: diff --git a/gtsam/navigation/ImuBias.h b/gtsam/navigation/ImuBias.h index b535f5179..32911e589 100644 --- a/gtsam/navigation/ImuBias.h +++ b/gtsam/navigation/ImuBias.h @@ -113,7 +113,7 @@ namespace imuBias { // // return measurement - biasGyro_ - w_earth_rate_I; // -//// Vector bias_gyro_temp(Vector_(3, -bias_gyro_(0), bias_gyro_(1), bias_gyro_(2))); +//// Vector bias_gyro_temp((Vector(3) << -bias_gyro_(0), bias_gyro_(1), bias_gyro_(2))); //// return measurement - bias_gyro_temp - R_G_to_I * w_earth_rate_G; // } diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index c3753e5e7..b1e20297e 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -11,7 +11,7 @@ /** * @file ImuFactor.h - * @author Luca Carlone, Stephen Williams, Richard Roberts + * @author Luca Carlone, Stephen Williams, Richard Roberts, Vadim Indelman **/ #pragma once @@ -47,39 +47,6 @@ namespace gtsam { /** Struct to store results of preintegrating IMU measurements. Can be build * incrementally so as to avoid costly integration at time of factor construction. */ - /** Right Jacobian for Exponential map in SO(3) - equation (10.86) and following equations in [1] */ - static Matrix3 rightJacobianExpMapSO3(const Vector3& x) { - // x is the axis-angle representation (exponential coordinates) for a rotation - double normx = norm_2(x); // rotation angle - Matrix3 Jr; - if (normx < 10e-8){ - Jr = Matrix3::Identity(); - } - else{ - const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ - Jr = Matrix3::Identity() - ((1-cos(normx))/(normx*normx)) * X + - ((normx-sin(normx))/(normx*normx*normx)) * X * X; // right Jacobian - } - return Jr; - } - - /** Right Jacobian for Log map in SO(3) - equation (10.86) and following equations in [1] */ - static Matrix3 rightJacobianExpMapSO3inverse(const Vector3& x) { - // x is the axis-angle representation (exponential coordinates) for a rotation - double normx = norm_2(x); // rotation angle - Matrix3 Jrinv; - - if (normx < 10e-8){ - Jrinv = Matrix3::Identity(); - } - else{ - const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ - Jrinv = Matrix3::Identity() + - 0.5 * X + (1/(normx*normx) - (1+cos(normx))/(2*normx * sin(normx)) ) * X * X; - } - return Jrinv; - } - /** CombinedPreintegratedMeasurements accumulates (integrates) the IMU measurements (rotation rates and accelerations) * and the corresponding covariance matrix. The measurements are then used to build the Preintegrated IMU factor*/ class PreintegratedMeasurements { @@ -182,7 +149,7 @@ namespace gtsam { const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement - const Matrix3 Jr_theta_incr = rightJacobianExpMapSO3(theta_incr); // Right jacobian computed at theta_incr + const Matrix3 Jr_theta_incr = Rot3::rightJacobianExpMapSO3(theta_incr); // Right jacobian computed at theta_incr // Update Jacobians /* ----------------------------------------------------------------------------------------------------------------------- */ @@ -200,11 +167,11 @@ namespace gtsam { Matrix3 Z_3x3 = Matrix3::Zero(); Matrix3 I_3x3 = Matrix3::Identity(); const Vector3 theta_i = Rot3::Logmap(deltaRij); // parametrization of so(3) - const Matrix3 Jr_theta_i = rightJacobianExpMapSO3(theta_i); + const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3(theta_i); Rot3 Rot_j = deltaRij * Rincr; const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3) - const Matrix3 Jrinv_theta_j = rightJacobianExpMapSO3inverse(theta_j); + const Matrix3 Jrinv_theta_j = Rot3::rightJacobianExpMapSO3inverse(theta_j); // Update preintegrated measurements covariance: as in [2] we consider a first order propagation that // can be seen as a prediction phase in an EKF framework @@ -306,6 +273,7 @@ namespace gtsam { /** Shorthand for a smart pointer to a factor */ typedef boost::shared_ptr shared_ptr; + /** Default constructor - only use for serialization */ ImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()) {} @@ -396,11 +364,11 @@ namespace gtsam { const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j)); - const Matrix3 Jr_theta_bcc = rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected); + const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected); const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij); - const Matrix3 Jrinv_fRhat = rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat)); + const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat)); if(H1) { H1->resize(9,6); @@ -456,8 +424,8 @@ namespace gtsam { } if(H5) { - const Matrix3 Jrinv_theta_bc = rightJacobianExpMapSO3inverse(theta_biascorrected); - const Matrix3 Jr_JbiasOmegaIncr = rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega * biasOmegaIncr); + const Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected); + const Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega * biasOmegaIncr); const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega; H5->resize(9,6); diff --git a/gtsam/navigation/tests/CMakeLists.txt b/gtsam/navigation/tests/CMakeLists.txt new file mode 100644 index 000000000..b03b8167c --- /dev/null +++ b/gtsam/navigation/tests/CMakeLists.txt @@ -0,0 +1,32 @@ +set(test_link_libraries gtsam) +set(tests_excluded "") + +# Decide whether to use installed GeographicLib or the one built in GTSAM. +# If we are not installing GeographicLib and it's not installed already, +# disable the unit tests that require it. +if(GTSAM_INSTALL_GEOGRAPHICLIB) + # If we're installing GeographicLib, use the one we're compiling + include_directories(${PROJECT_SOURCE_DIR}/gtsam/3rdparty/GeographicLib/include) + if(MSVC) + list(APPEND test_link_libraries GeographicLib_STATIC) + else() + list(APPEND test_link_libraries GeographicLib) + endif() + +else() + if(GEOGRAPHICLIB_FOUND) + # If we're not installing, but it's already installed, use the installed one + include_directories(${GeographicLib_INCLUDE_DIRS}) + list(APPEND test_link_libraries ) + #if(MSVC) + # list(APPEND test_link_libraries GeographicLib_STATIC) + #else() + list(APPEND test_link_libraries ${GeographicLib_LIBRARIES}) + #endif() + else() + # We don't have GeographicLib + set(tests_excluded testGeographicLib.cpp testGPSFactor.cpp testMagFactor.cpp) + endif() +endif() + +gtsamAddTestsGlob(navigation "test*.cpp" "${tests_excluded}" "${test_link_libraries}") diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index 7330f6aa3..10d582287 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -16,7 +16,7 @@ */ #include -#include +#include #include #include #include @@ -40,20 +40,6 @@ using symbol_shorthand::B; /* ************************************************************************* */ namespace { -Vector callEvaluateError(const ImuFactor& factor, - const Pose3& pose_i, const LieVector& vel_i, const Pose3& pose_j, const LieVector& vel_j, - const imuBias::ConstantBias& bias) -{ - return factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias); -} - -Rot3 evaluateRotationError(const ImuFactor& factor, - const Pose3& pose_i, const LieVector& vel_i, const Pose3& pose_j, const LieVector& vel_j, - const imuBias::ConstantBias& bias) -{ - return Rot3::Expmap(factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).tail(3) ) ; -} - ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements( const imuBias::ConstantBias& bias, const list& measuredAccs, @@ -108,17 +94,6 @@ Rot3 evaluatePreintegratedMeasurementsRotation( measuredAccs, measuredOmegas, deltaTs).deltaRij; } -Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega, const double deltaT) -{ - return Rot3::Expmap((measuredOmega - biasOmega) * deltaT); -} - - -Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta) -{ - return Rot3::Logmap( Rot3::Expmap(thetahat).compose( Rot3::Expmap(deltatheta) ) ); -} - } /* ************************************************************************* */ @@ -168,9 +143,9 @@ TEST( CombinedImuFactor, ErrorWithBiases ) imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) imuBias::ConstantBias bias2(Vector3(0.2, 0.2, 0), Vector3(1, 0, 0.3)); // Biases (acc, rot) Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0)); - LieVector v1(3, 0.5, 0.0, 0.0); + LieVector v1((Vector(3) << 0.5, 0.0, 0.0)); Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0)); - LieVector v2(3, 0.5, 0.0, 0.0); + LieVector v2((Vector(3) << 0.5, 0.0, 0.0)); // Measurements Vector3 gravity; gravity << 0, 0, 9.81; diff --git a/gtsam/navigation/tests/testGeographicLib.cpp b/gtsam/navigation/tests/testGeographicLib.cpp index 44964c569..aaa01b54d 100644 --- a/gtsam/navigation/tests/testGeographicLib.cpp +++ b/gtsam/navigation/tests/testGeographicLib.cpp @@ -19,6 +19,7 @@ #include #include +#include #include #include diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index e312df60c..6cd286f68 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -17,7 +17,7 @@ #include #include -#include +#include #include #include #include @@ -167,9 +167,9 @@ TEST( ImuFactor, Error ) // Linearization point imuBias::ConstantBias bias; // Bias Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0)); - LieVector v1(3, 0.5, 0.0, 0.0); + LieVector v1((Vector(3) << 0.5, 0.0, 0.0)); Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/100.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0)); - LieVector v2(3, 0.5, 0.0, 0.0); + LieVector v2((Vector(3) << 0.5, 0.0, 0.0)); // Measurements Vector3 gravity; gravity << 0, 0, 9.81; @@ -238,16 +238,16 @@ TEST( ImuFactor, ErrorWithBiases ) // Linearization point // Vector bias(6); bias << 0.2, 0, 0, 0.1, 0, 0; // Biases (acc, rot) // Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0)); -// LieVector v1(3, 0.5, 0.0, 0.0); +// LieVector v1((Vector(3) << 0.5, 0.0, 0.0)); // Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/10.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0)); -// LieVector v2(3, 0.5, 0.0, 0.0); +// LieVector v2((Vector(3) << 0.5, 0.0, 0.0)); imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0)); - LieVector v1(3, 0.5, 0.0, 0.0); + LieVector v1((Vector(3) << 0.5, 0.0, 0.0)); Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0)); - LieVector v2(3, 0.5, 0.0, 0.0); + LieVector v2((Vector(3) << 0.5, 0.0, 0.0)); // Measurements Vector3 gravity; gravity << 0, 0, 9.81; @@ -319,7 +319,7 @@ TEST( ImuFactor, PartialDerivativeExpmap ) Matrix expectedDelRdelBiasOmega = numericalDerivative11(boost::bind( &evaluateRotation, measuredOmega, _1, deltaT), LieVector(biasOmega)); - const Matrix3 Jr = ImuFactor::rightJacobianExpMapSO3((measuredOmega - biasOmega) * deltaT); + const Matrix3 Jr = Rot3::rightJacobianExpMapSO3((measuredOmega - biasOmega) * deltaT); Matrix3 actualdelRdelBiasOmega = - Jr * deltaT; // the delta bias appears with the minus sign @@ -371,7 +371,7 @@ TEST( ImuFactor, fistOrderExponential ) Vector3 deltabiasOmega; deltabiasOmega << alpha,alpha,alpha; - const Matrix3 Jr = ImuFactor::rightJacobianExpMapSO3((measuredOmega - biasOmega) * deltaT); + const Matrix3 Jr = Rot3::rightJacobianExpMapSO3((measuredOmega - biasOmega) * deltaT); Matrix3 delRdelBiasOmega = - Jr * deltaT; // the delta bias appears with the minus sign @@ -445,9 +445,9 @@ TEST( ImuFactor, FirstOrderPreIntegratedMeasurements ) //{ // // Linearization point // Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0)); -// LieVector v1(3, 0.5, 0.0, 0.0); +// LieVector v1((Vector(3) << 0.5, 0.0, 0.0)); // Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/100.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0)); -// LieVector v2(3, 0.5, 0.0, 0.0); +// LieVector v2((Vector(3) << 0.5, 0.0, 0.0)); // imuBias::ConstantBias bias(Vector3(0.001, 0.002, 0.008), Vector3(0.002, 0.004, 0.012)); // // // Pre-integrator @@ -501,9 +501,9 @@ TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement ) imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0)); - LieVector v1(3, 0.5, 0.0, 0.0); + LieVector v1((Vector(3) << 0.5, 0.0, 0.0)); Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0)); - LieVector v2(3, 0.5, 0.0, 0.0); + LieVector v2((Vector(3) << 0.5, 0.0, 0.0)); // Measurements Vector3 gravity; gravity << 0, 0, 9.81; diff --git a/gtsam/nonlinear/CMakeLists.txt b/gtsam/nonlinear/CMakeLists.txt index 4dcb5e8d8..b4d288104 100644 --- a/gtsam/nonlinear/CMakeLists.txt +++ b/gtsam/nonlinear/CMakeLists.txt @@ -2,29 +2,11 @@ file(GLOB nonlinear_headers "*.h") install(FILES ${nonlinear_headers} DESTINATION include/gtsam/nonlinear) -# Components to link tests in this subfolder against -set(nonlinear_local_libs - nonlinear - linear - inference - geometry - base - ccolamd -) - -# Files to exclude from compilation of tests and timing scripts -set(nonlinear_excluded_files -# "${CMAKE_CURRENT_SOURCE_DIR}/tests/testTypedDiscreteFactor.cpp" # Example of excluding a test - "" # Add to this list, with full path, to exclude -) - # Build tests -if (GTSAM_BUILD_TESTS) - gtsam_add_subdir_tests(nonlinear "${nonlinear_local_libs}" "${gtsam-default}" "${nonlinear_excluded_files}") -endif(GTSAM_BUILD_TESTS) +add_subdirectory(tests) # Build timing scripts if (GTSAM_BUILD_TIMING) - gtsam_add_subdir_timing(nonlinear "${nonlinear_local_libs}" "${gtsam-default}" "${nonlinear_excluded_files}") + gtsam_add_subdir_timing(nonlinear "gtsam" "gtsam" "${nonlinear_excluded_files}") endif(GTSAM_BUILD_TIMING) diff --git a/gtsam/nonlinear/DoglegOptimizer.cpp b/gtsam/nonlinear/DoglegOptimizer.cpp index 6356a4c27..03da76eb6 100644 --- a/gtsam/nonlinear/DoglegOptimizer.cpp +++ b/gtsam/nonlinear/DoglegOptimizer.cpp @@ -17,10 +17,11 @@ */ #include - -#include -#include #include +#include +#include +#include +#include #include @@ -53,8 +54,7 @@ std::string DoglegParams::verbosityDLTranslator(VerbosityDL verbosityDL) const { void DoglegOptimizer::iterate(void) { // Linearize graph - const Ordering& ordering = *params_.ordering; - GaussianFactorGraph::shared_ptr linear = graph_.linearize(state_.values, ordering); + GaussianFactorGraph::shared_ptr linear = graph_.linearize(state_.values); // Pull out parameters we'll use const bool dlVerbose = (params_.verbosityDL > DoglegParams::SILENT); @@ -63,16 +63,21 @@ void DoglegOptimizer::iterate(void) { DoglegOptimizerImpl::IterationResult result; if ( params_.isMultifrontal() ) { - GaussianBayesTree bt; - bt.insert(GaussianJunctionTree(*linear).eliminate(params_.getEliminationFunction())); - result = DoglegOptimizerImpl::Iterate(state_.Delta, DoglegOptimizerImpl::ONE_STEP_PER_ITERATION, bt, graph_, state_.values, ordering, state_.error, dlVerbose); + GaussianBayesTree bt = *linear->eliminateMultifrontal(*params_.ordering, params_.getEliminationFunction()); + VectorValues dx_u = bt.optimizeGradientSearch(); + VectorValues dx_n = bt.optimize(); + result = DoglegOptimizerImpl::Iterate(state_.Delta, DoglegOptimizerImpl::ONE_STEP_PER_ITERATION, + dx_u, dx_n, bt, graph_, state_.values, state_.error, dlVerbose); } else if ( params_.isSequential() ) { - GaussianBayesNet::shared_ptr bn = EliminationTree::Create(*linear)->eliminate(params_.getEliminationFunction()); - result = DoglegOptimizerImpl::Iterate(state_.Delta, DoglegOptimizerImpl::ONE_STEP_PER_ITERATION, *bn, graph_, state_.values, ordering, state_.error, dlVerbose); + GaussianBayesNet bn = *linear->eliminateSequential(*params_.ordering, params_.getEliminationFunction()); + VectorValues dx_u = bn.optimizeGradientSearch(); + VectorValues dx_n = bn.optimize(); + result = DoglegOptimizerImpl::Iterate(state_.Delta, DoglegOptimizerImpl::ONE_STEP_PER_ITERATION, + dx_u, dx_n, bn, graph_, state_.values, state_.error, dlVerbose); } else if ( params_.isCG() ) { - throw runtime_error("todo: "); + throw runtime_error("Dogleg is not currently compatible with the linear conjugate gradient solver"); } else { throw runtime_error("Optimization parameter is invalid: DoglegParams::elimination"); @@ -82,10 +87,17 @@ void DoglegOptimizer::iterate(void) { if(params_.verbosity >= NonlinearOptimizerParams::DELTA) result.dx_d.print("delta"); // Create new state with new values and new error - state_.values = state_.values.retract(result.dx_d, ordering); + state_.values = state_.values.retract(result.dx_d); state_.error = result.f_error; state_.Delta = result.Delta; ++state_.iterations; } +/* ************************************************************************* */ +DoglegParams DoglegOptimizer::ensureHasOrdering(DoglegParams params, const NonlinearFactorGraph& graph) const { + if(!params.ordering) + params.ordering = Ordering::COLAMD(graph); + return params; +} + } /* namespace gtsam */ diff --git a/gtsam/nonlinear/DoglegOptimizer.h b/gtsam/nonlinear/DoglegOptimizer.h index 86a8b5570..8a43b4d96 100644 --- a/gtsam/nonlinear/DoglegOptimizer.h +++ b/gtsam/nonlinear/DoglegOptimizer.h @@ -18,7 +18,7 @@ #pragma once -#include +#include namespace gtsam { @@ -29,7 +29,7 @@ class DoglegOptimizer; * common to all nonlinear optimization algorithms. This class also contains * all of those parameters. */ -class GTSAM_EXPORT DoglegParams : public SuccessiveLinearizationParams { +class GTSAM_EXPORT DoglegParams : public NonlinearOptimizerParams { public: /** See DoglegParams::dlVerbosity */ enum VerbosityDL { @@ -46,7 +46,7 @@ public: virtual ~DoglegParams() {} virtual void print(const std::string& str = "") const { - SuccessiveLinearizationParams::print(str); + NonlinearOptimizerParams::print(str); std::cout << " deltaInitial: " << deltaInitial << "\n"; std::cout.flush(); } @@ -105,7 +105,7 @@ public: */ DoglegOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues, const DoglegParams& params = DoglegParams()) : - NonlinearOptimizer(graph), params_(ensureHasOrdering(params, graph, initialValues)), state_(graph, initialValues, params_) {} + NonlinearOptimizer(graph), params_(ensureHasOrdering(params, graph)), state_(graph, initialValues, params_) {} /** Standard constructor, requires a nonlinear factor graph, initial * variable assignments, and optimization parameters. For convenience this @@ -158,11 +158,7 @@ protected: virtual const NonlinearOptimizerState& _state() const { return state_; } /** Internal function for computing a COLAMD ordering if no ordering is specified */ - DoglegParams ensureHasOrdering(DoglegParams params, const NonlinearFactorGraph& graph, const Values& values) const { - if(!params.ordering) - params.ordering = *graph.orderingCOLAMD(values); - return params; - } + DoglegParams ensureHasOrdering(DoglegParams params, const NonlinearFactorGraph& graph) const; }; } diff --git a/gtsam/nonlinear/DoglegOptimizerImpl.h b/gtsam/nonlinear/DoglegOptimizerImpl.h index ae659f092..a6c04681b 100644 --- a/gtsam/nonlinear/DoglegOptimizerImpl.h +++ b/gtsam/nonlinear/DoglegOptimizerImpl.h @@ -18,25 +18,16 @@ #include -#include -#include // To get optimize(BayesTree) -//#include -#include +#include +#include namespace gtsam { /** This class contains the implementation of the Dogleg algorithm. It is used * by DoglegOptimizer and can be used to easily put together custom versions of * Dogleg. Each function is well-documented and unit-tested. The notation - * here matches that in "trustregion.pdf" in gtsam_experimental/doc, see this - * file for further explanation of the computations performed by this class. - * - * \tparam VALUES The Values or TupleValues type to hold the values to be - * estimated. - * - * \tparam GAUSSIAN_SOLVER The linear solver to use at each iteration, - * currently either GaussianSequentialSolver or GaussianMultifrontalSolver. - * The latter is typically faster, especially for non-trivial problems. + * here matches that in "trustregion.pdf" in doc, see this file for further + * explanation of the computations performed by this class. */ struct GTSAM_EXPORT DoglegOptimizerImpl { @@ -102,8 +93,8 @@ struct GTSAM_EXPORT DoglegOptimizerImpl { */ template static IterationResult Iterate( - double Delta, TrustRegionAdaptationMode mode, const M& Rd, - const F& f, const VALUES& x0, const Ordering& ordering, const double f_error, const bool verbose=false); + double Delta, TrustRegionAdaptationMode mode, const VectorValues& dx_u, const VectorValues& dx_n, + const M& Rd, const F& f, const VALUES& x0, const double f_error, const bool verbose=false); /** * Compute the dogleg point given a trust region radius \f$ \Delta \f$. The @@ -145,26 +136,12 @@ struct GTSAM_EXPORT DoglegOptimizerImpl { /* ************************************************************************* */ template typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate( - double Delta, TrustRegionAdaptationMode mode, const M& Rd, - const F& f, const VALUES& x0, const Ordering& ordering, const double f_error, const bool verbose) { - - // Compute steepest descent and Newton's method points - gttic(optimizeGradientSearch); - gttic(allocateVectorValues); - VectorValues dx_u = *allocateVectorValues(Rd); - gttoc(allocateVectorValues); - gttic(optimizeGradientSearchInPlace); - optimizeGradientSearchInPlace(Rd, dx_u); - gttoc(optimizeGradientSearchInPlace); - gttoc(optimizeGradientSearch); - gttic(optimizeInPlace); - VectorValues dx_n(VectorValues::SameStructure(dx_u)); - optimizeInPlace(Rd, dx_n); - gttoc(optimizeInPlace); - gttic(jfg_error); - const GaussianFactorGraph jfg(Rd); - const double M_error = jfg.error(VectorValues::Zero(dx_u)); - gttoc(jfg_error); + double Delta, TrustRegionAdaptationMode mode, const VectorValues& dx_u, const VectorValues& dx_n, + const M& Rd, const F& f, const VALUES& x0, const double f_error, const bool verbose) +{ + gttic(M_error); + const double M_error = Rd.error(VectorValues::Zero(dx_u)); + gttoc(M_error); // Result to return IterationResult result; @@ -181,7 +158,7 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate( gttic(retract); // Compute expmapped solution - const VALUES x_d(x0.retract(result.dx_d, ordering)); + const VALUES x_d(x0.retract(result.dx_d)); gttoc(retract); gttic(decrease_in_f); @@ -189,10 +166,10 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate( result.f_error = f.error(x_d); gttoc(decrease_in_f); - gttic(decrease_in_M); + gttic(new_M_error); // Compute decrease in M - const double new_M_error = jfg.error(result.dx_d); - gttoc(decrease_in_M); + const double new_M_error = Rd.error(result.dx_d); + gttoc(new_M_error); if(verbose) std::cout << std::setprecision(15) << "f error: " << f_error << " -> " << result.f_error << std::endl; if(verbose) std::cout << std::setprecision(15) << "M error: " << M_error << " -> " << new_M_error << std::endl; diff --git a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h index e56bc3cda..5765eca01 100644 --- a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h +++ b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h @@ -20,7 +20,6 @@ #include #include -#include #include #include @@ -29,45 +28,40 @@ namespace gtsam { /* ************************************************************************* */ template typename ExtendedKalmanFilter::T ExtendedKalmanFilter::solve_( - const GaussianFactorGraph& linearFactorGraph, const Ordering& ordering, + const GaussianFactorGraph& linearFactorGraph, const Values& linearizationPoints, Key lastKey, - JacobianFactor::shared_ptr& newPrior) const { - - // Extract the Index of the provided last key - gtsam::Index lastIndex = ordering.at(lastKey); - + JacobianFactor::shared_ptr& newPrior) const + { + // Compute the marginal on the last key // Solve the linear factor graph, converting it into a linear Bayes Network // P(x0,x1) = P(x0|x1)*P(x1) - GaussianSequentialSolver solver(linearFactorGraph); - GaussianBayesNet::shared_ptr linearBayesNet = solver.eliminate(); + Ordering lastKeyAsOrdering; + lastKeyAsOrdering += lastKey; + const GaussianConditional::shared_ptr marginal = + linearFactorGraph.marginalMultifrontalBayesNet(lastKeyAsOrdering)->front(); - // Extract the current estimate of x1,P1 from the Bayes Network - VectorValues result = optimize(*linearBayesNet); - T x = linearizationPoints.at(lastKey).retract(result[lastIndex]); + // Extract the current estimate of x1,P1 + VectorValues result = marginal->solve(VectorValues()); + T x = linearizationPoints.at(lastKey).retract(result[lastKey]); // Create a Jacobian Factor from the root node of the produced Bayes Net. // This will act as a prior for the next iteration. // The linearization point of this prior must be moved to the new estimate of x, // and the key/index needs to be reset to 0, the first key in the next iteration. - const GaussianConditional::shared_ptr& cg = linearBayesNet->back(); - assert(cg->nrFrontals() == 1); - assert(cg->nrParents() == 0); - // TODO: Find a way to create the correct Jacobian Factor in a single pass - JacobianFactor tmpPrior = JacobianFactor(*cg); - newPrior = JacobianFactor::shared_ptr( - new JacobianFactor( - 0, - tmpPrior.getA(tmpPrior.begin()), - tmpPrior.getb() - - tmpPrior.getA(tmpPrior.begin()) * result[lastIndex], - tmpPrior.get_model())); + assert(marginal->nrFrontals() == 1); + assert(marginal->nrParents() == 0); + newPrior = boost::make_shared( + marginal->keys().front(), + marginal->getA(marginal->begin()), + marginal->getb() - marginal->getA(marginal->begin()) * result[lastKey], + marginal->get_model()); return x; } /* ************************************************************************* */ template - ExtendedKalmanFilter::ExtendedKalmanFilter(T x_initial, + ExtendedKalmanFilter::ExtendedKalmanFilter(Key key_initial, T x_initial, noiseModel::Gaussian::shared_ptr P_initial) { // Set the initial linearization point to the provided mean @@ -77,7 +71,7 @@ namespace gtsam { // Since x0 is set to the provided mean, the b vector in the prior will be zero // TODO Frank asks: is there a reason why noiseModel is not simply P_initial ? priorFactor_ = JacobianFactor::shared_ptr( - new JacobianFactor(0, P_initial->R(), Vector::Zero(x_initial.dim()), + new JacobianFactor(key_initial, P_initial->R(), Vector::Zero(x_initial.dim()), noiseModel::Unit::Create(P_initial->dim()))); } @@ -94,11 +88,6 @@ namespace gtsam { Key x0 = motionFactor.key1(); Key x1 = motionFactor.key2(); - // Create an elimination ordering - Ordering ordering; - ordering.insert(x0, 0); - ordering.insert(x1, 1); - // Create a set of linearization points Values linearizationPoints; linearizationPoints.insert(x0, x_); @@ -112,11 +101,11 @@ namespace gtsam { // Linearize motion model and add it to the Kalman Filter graph linearFactorGraph.push_back( - motionFactor.linearize(linearizationPoints, ordering)); + motionFactor.linearize(linearizationPoints)); // Solve the factor graph and update the current state estimate // and the posterior for the next iteration. - x_ = solve_(linearFactorGraph, ordering, linearizationPoints, x1, priorFactor_); + x_ = solve_(linearFactorGraph, linearizationPoints, x1, priorFactor_); return x_; } @@ -133,10 +122,6 @@ namespace gtsam { // Create Keys Key x0 = measurementFactor.key(); - // Create an elimination ordering - Ordering ordering; - ordering.insert(x0, 0); - // Create a set of linearization points Values linearizationPoints; linearizationPoints.insert(x0, x_); @@ -149,11 +134,11 @@ namespace gtsam { // Linearize measurement factor and add it to the Kalman Filter graph linearFactorGraph.push_back( - measurementFactor.linearize(linearizationPoints, ordering)); + measurementFactor.linearize(linearizationPoints)); // Solve the factor graph and update the current state estimate // and the prior factor for the next iteration - x_ = solve_(linearFactorGraph, ordering, linearizationPoints, x0, priorFactor_); + x_ = solve_(linearFactorGraph, linearizationPoints, x0, priorFactor_); return x_; } diff --git a/gtsam/nonlinear/ExtendedKalmanFilter.h b/gtsam/nonlinear/ExtendedKalmanFilter.h index a71a5332a..7bbd14980 100644 --- a/gtsam/nonlinear/ExtendedKalmanFilter.h +++ b/gtsam/nonlinear/ExtendedKalmanFilter.h @@ -54,7 +54,7 @@ namespace gtsam { JacobianFactor::shared_ptr priorFactor_; // density T solve_(const GaussianFactorGraph& linearFactorGraph, - const Ordering& ordering, const Values& linearizationPoints, + const Values& linearizationPoints, Key x, JacobianFactor::shared_ptr& newPrior) const; public: @@ -62,7 +62,7 @@ namespace gtsam { /// @name Standard Constructors /// @{ - ExtendedKalmanFilter(T x_initial, + ExtendedKalmanFilter(Key key_initial, T x_initial, noiseModel::Gaussian::shared_ptr P_initial); /// @} diff --git a/gtsam/nonlinear/GaussNewtonOptimizer.cpp b/gtsam/nonlinear/GaussNewtonOptimizer.cpp index 06e9ab148..823f3a6ac 100644 --- a/gtsam/nonlinear/GaussNewtonOptimizer.cpp +++ b/gtsam/nonlinear/GaussNewtonOptimizer.cpp @@ -17,6 +17,8 @@ */ #include +#include +#include using namespace std; @@ -28,18 +30,27 @@ void GaussNewtonOptimizer::iterate() { const NonlinearOptimizerState& current = state_; // Linearize graph - GaussianFactorGraph::shared_ptr linear = graph_.linearize(current.values, *params_.ordering); + GaussianFactorGraph::shared_ptr linear = graph_.linearize(current.values); // Solve Factor Graph - const VectorValues delta = solveGaussianFactorGraph(*linear, params_); + const VectorValues delta = solve(*linear, current.values, params_); // Maybe show output if(params_.verbosity >= NonlinearOptimizerParams::DELTA) delta.print("delta"); // Create new state with new values and new error - state_.values = current.values.retract(delta, *params_.ordering); + state_.values = current.values.retract(delta); state_.error = graph_.error(state_.values); ++ state_.iterations; } +/* ************************************************************************* */ +GaussNewtonParams GaussNewtonOptimizer::ensureHasOrdering( + GaussNewtonParams params, const NonlinearFactorGraph& graph) const +{ + if(!params.ordering) + params.ordering = Ordering::COLAMD(graph); + return params; +} + } /* namespace gtsam */ diff --git a/gtsam/nonlinear/GaussNewtonOptimizer.h b/gtsam/nonlinear/GaussNewtonOptimizer.h index 67daf7305..8b41a979c 100644 --- a/gtsam/nonlinear/GaussNewtonOptimizer.h +++ b/gtsam/nonlinear/GaussNewtonOptimizer.h @@ -18,7 +18,7 @@ #pragma once -#include +#include namespace gtsam { @@ -27,7 +27,7 @@ class GaussNewtonOptimizer; /** Parameters for Gauss-Newton optimization, inherits from * NonlinearOptimizationParams. */ -class GTSAM_EXPORT GaussNewtonParams : public SuccessiveLinearizationParams { +class GTSAM_EXPORT GaussNewtonParams : public NonlinearOptimizerParams { }; class GTSAM_EXPORT GaussNewtonState : public NonlinearOptimizerState { @@ -61,7 +61,7 @@ public: */ GaussNewtonOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues, const GaussNewtonParams& params = GaussNewtonParams()) : - NonlinearOptimizer(graph), params_(ensureHasOrdering(params, graph, initialValues)), state_(graph, initialValues) {} + NonlinearOptimizer(graph), params_(ensureHasOrdering(params, graph)), state_(graph, initialValues) {} /** Standard constructor, requires a nonlinear factor graph, initial * variable assignments, and optimization parameters. For convenience this @@ -110,11 +110,7 @@ protected: virtual const NonlinearOptimizerState& _state() const { return state_; } /** Internal function for computing a COLAMD ordering if no ordering is specified */ - GaussNewtonParams ensureHasOrdering(GaussNewtonParams params, const NonlinearFactorGraph& graph, const Values& values) const { - if(!params.ordering) - params.ordering = *graph.orderingCOLAMD(values); - return params; - } + GaussNewtonParams ensureHasOrdering(GaussNewtonParams params, const NonlinearFactorGraph& graph) const; }; diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index ffdbd6d10..41ee52b3a 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -17,11 +17,10 @@ */ #include -#include // for selective linearization thresholds +#include // for selective linearization thresholds #include #include #include -#include using namespace std; @@ -30,140 +29,101 @@ namespace gtsam { /* ************************************************************************* */ void ISAM2::Impl::AddVariables( const Values& newTheta, Values& theta, VectorValues& delta, - VectorValues& deltaNewton, VectorValues& RgProd, vector& replacedKeys, - Ordering& ordering, const KeyFormatter& keyFormatter) { + VectorValues& deltaNewton, VectorValues& RgProd, + const KeyFormatter& keyFormatter) +{ const bool debug = ISDEBUG("ISAM2 AddVariables"); theta.insert(newTheta); if(debug) newTheta.print("The new variables are: "); - // Add the new keys onto the ordering, add zeros to the delta for the new variables - std::vector dims(newTheta.dims(*newTheta.orderingArbitrary())); - if(debug) cout << "New variables have total dimensionality " << accumulate(dims.begin(), dims.end(), 0) << endl; - const size_t originalnVars = delta.size(); - delta.append(dims); - deltaNewton.append(dims); - RgProd.append(dims); - for(Index j = originalnVars; j < delta.size(); ++j) { - delta[j].setZero(); - deltaNewton[j].setZero(); - RgProd[j].setZero(); - } - { - Index nextVar = originalnVars; - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, newTheta) { - ordering.insert(key_value.key, nextVar); - if(debug) cout << "Adding variable " << keyFormatter(key_value.key) << " with order " << nextVar << endl; - ++ nextVar; - } - assert(ordering.size() == delta.size()); - } - replacedKeys.resize(ordering.size(), false); + // Add zeros into the VectorValues + delta.insert(newTheta.zeroVectors()); + deltaNewton.insert(newTheta.zeroVectors()); + RgProd.insert(newTheta.zeroVectors()); } /* ************************************************************************* */ -void ISAM2::Impl::RemoveVariables(const FastSet& unusedKeys, const ISAM2Clique::shared_ptr& root, +void ISAM2::Impl::AddFactorsStep1(const NonlinearFactorGraph& newFactors, bool useUnusedSlots, + NonlinearFactorGraph& nonlinearFactors, FastVector& newFactorIndices) +{ + newFactorIndices.resize(newFactors.size()); + + if(useUnusedSlots) + { + size_t globalFactorIndex = 0; + for(size_t newFactorIndex = 0; newFactorIndex < newFactors.size(); ++newFactorIndex) + { + // Loop to find the next available factor slot + do + { + // If we need to add more factors than we have room for, resize nonlinearFactors, + // filling the new slots with NULL factors. Otherwise, check if the current + // factor in nonlinearFactors is already used, and if so, increase + // globalFactorIndex. If the current factor in nonlinearFactors is unused, break + // out of the loop and use the current slot. + if(globalFactorIndex >= nonlinearFactors.size()) + nonlinearFactors.resize(nonlinearFactors.size() + newFactors.size() - newFactorIndex); + else if(nonlinearFactors[globalFactorIndex]) + ++ globalFactorIndex; + else + break; + } while(true); + + // Use the current slot, updating nonlinearFactors and newFactorSlots. + nonlinearFactors[globalFactorIndex] = newFactors[newFactorIndex]; + newFactorIndices[newFactorIndex] = globalFactorIndex; + } + } + else + { + // We're not looking for unused slots, so just add the factors at the end. + for(size_t i = 0; i < newFactors.size(); ++i) + newFactorIndices[i] = i + nonlinearFactors.size(); + nonlinearFactors.push_back(newFactors); + } +} + +/* ************************************************************************* */ +void ISAM2::Impl::RemoveVariables(const FastSet& unusedKeys, const FastVector& roots, Values& theta, VariableIndex& variableIndex, VectorValues& delta, VectorValues& deltaNewton, VectorValues& RgProd, - std::vector& replacedKeys, Ordering& ordering, Base::Nodes& nodes, - GaussianFactorGraph& linearFactors, FastSet& fixedVariables) { - - // Get indices of unused keys - vector unusedIndices; unusedIndices.reserve(unusedKeys.size()); - BOOST_FOREACH(Key key, unusedKeys) { unusedIndices.push_back(ordering[key]); } - - // Create a permutation that shifts the unused variables to the end - Permutation unusedToEnd = Permutation::PushToBack(unusedIndices, delta.size()); - Permutation unusedToEndInverse = *unusedToEnd.inverse(); - - // Use the permutation to remove unused variables while shifting all the others to take up the space - variableIndex.permuteInPlace(unusedToEnd); - variableIndex.removeUnusedAtEnd(unusedIndices.size()); - { - // Create a vector of variable dimensions with the unused ones removed - // by applying the unusedToEnd permutation to the original vector of - // variable dimensions. We only allocate space in the shifted dims - // vector for the used variables, so that the unused ones are dropped - // when the permutation is applied. - vector originalDims = delta.dims(); - vector dims(delta.size() - unusedIndices.size()); - unusedToEnd.applyToCollection(dims, originalDims); - - // Copy from the old data structures to new ones, only iterating up to - // the number of used variables, and applying the unusedToEnd permutation - // in order to remove the unused variables. - VectorValues newDelta(dims); - VectorValues newDeltaNewton(dims); - VectorValues newDeltaGradSearch(dims); - std::vector newReplacedKeys(replacedKeys.size() - unusedIndices.size()); - Base::Nodes newNodes(replacedKeys.size() - unusedIndices.size()); - - for(size_t j = 0; j < dims.size(); ++j) { - newDelta[j] = delta[unusedToEnd[j]]; - newDeltaNewton[j] = deltaNewton[unusedToEnd[j]]; - newDeltaGradSearch[j] = RgProd[unusedToEnd[j]]; - newReplacedKeys[j] = replacedKeys[unusedToEnd[j]]; - newNodes[j] = nodes[unusedToEnd[j]]; - } - - // Swap the new data structures with the outputs of this function - delta.swap(newDelta); - deltaNewton.swap(newDeltaNewton); - RgProd.swap(newDeltaGradSearch); - replacedKeys.swap(newReplacedKeys); - nodes.swap(newNodes); - } - - // Reorder and remove from ordering, solution, and fixed keys - ordering.permuteInPlace(unusedToEnd); - BOOST_REVERSE_FOREACH(Key key, unusedKeys) { -#ifndef NDEBUG - Ordering::value_type removed = -#endif - ordering.pop_back(); - assert(removed.first == key); - theta.erase(key); - fixedVariables.erase(key); - } - - // Finally, permute references to variables - if(root) - root->permuteWithInverse(unusedToEndInverse); - linearFactors.permuteWithInverse(unusedToEndInverse); + FastSet& replacedKeys, Base::Nodes& nodes, + FastSet& fixedVariables) +{ + variableIndex.removeUnusedVariables(unusedKeys.begin(), unusedKeys.end()); + BOOST_FOREACH(Key key, unusedKeys) { + delta.erase(key); + deltaNewton.erase(key); + RgProd.erase(key); + replacedKeys.erase(key); + nodes.unsafe_erase(key); + theta.erase(key); + fixedVariables.erase(key); + } } /* ************************************************************************* */ -FastSet ISAM2::Impl::IndicesFromFactors(const Ordering& ordering, const NonlinearFactorGraph& factors) { - FastSet indices; - BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, factors) { - BOOST_FOREACH(Key key, factor->keys()) { - indices.insert(ordering[key]); +FastSet ISAM2::Impl::CheckRelinearizationFull(const VectorValues& delta, + const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) +{ + FastSet relinKeys; + + if(const double* threshold = boost::get(&relinearizeThreshold)) + { + BOOST_FOREACH(const VectorValues::KeyValuePair& key_delta, delta) { + double maxDelta = key_delta.second.lpNorm(); + if(maxDelta >= *threshold) + relinKeys.insert(key_delta.first); } } - return indices; -} - -/* ************************************************************************* */ -FastSet ISAM2::Impl::CheckRelinearizationFull(const VectorValues& delta, const Ordering& ordering, - const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter) { - FastSet relinKeys; - - if(relinearizeThreshold.type() == typeid(double)) { - double threshold = boost::get(relinearizeThreshold); - for(Index var=0; var(); - if(maxDelta >= threshold) { - relinKeys.insert(var); - } - } - } else if(relinearizeThreshold.type() == typeid(FastMap)) { - const FastMap& thresholds = boost::get >(relinearizeThreshold); - BOOST_FOREACH(const Ordering::value_type& key_index, ordering) { - const Vector& threshold = thresholds.find(Symbol(key_index.first).chr())->second; - Index j = key_index.second; - if(threshold.rows() != delta[j].rows()) - throw std::invalid_argument("Relinearization threshold vector dimensionality passed into iSAM2 parameters does not match actual variable dimensionality"); - if((delta[j].array().abs() > threshold.array()).any()) - relinKeys.insert(j); + else if(const FastMap* thresholds = boost::get >(&relinearizeThreshold)) + { + BOOST_FOREACH(const VectorValues::KeyValuePair& key_delta, delta) { + const Vector& threshold = thresholds->find(Symbol(key_delta.first).chr())->second; + if(threshold.rows() != key_delta.second.rows()) + throw std::invalid_argument("Relinearization threshold vector dimensionality for '" + std::string(1, Symbol(key_delta.first).chr()) + "' passed into iSAM2 parameters does not match actual variable dimensionality."); + if((key_delta.second.array().abs() > threshold.array()).any()) + relinKeys.insert(key_delta.first); } } @@ -171,11 +131,12 @@ FastSet ISAM2::Impl::CheckRelinearizationFull(const VectorValues& delta, } /* ************************************************************************* */ -void CheckRelinearizationRecursiveDouble(FastSet& relinKeys, double threshold, const VectorValues& delta, const ISAM2Clique::shared_ptr& clique) { - +void CheckRelinearizationRecursiveDouble(FastSet& relinKeys, double threshold, + const VectorValues& delta, const ISAM2Clique::shared_ptr& clique) +{ // Check the current clique for relinearization bool relinearize = false; - BOOST_FOREACH(Index var, clique->conditional()->keys()) { + BOOST_FOREACH(Key var, *clique->conditional()) { double maxDelta = delta[var].lpNorm(); if(maxDelta >= threshold) { relinKeys.insert(var); @@ -185,31 +146,31 @@ void CheckRelinearizationRecursiveDouble(FastSet& relinKeys, double thres // If this node was relinearized, also check its children if(relinearize) { - BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children()) { + BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children) { CheckRelinearizationRecursiveDouble(relinKeys, threshold, delta, child); } } } /* ************************************************************************* */ -void CheckRelinearizationRecursiveMap(FastSet& relinKeys, const FastMap& thresholds, const VectorValues& delta, const Ordering& ordering, const ISAM2Clique::shared_ptr& clique) { - +void CheckRelinearizationRecursiveMap(FastSet& relinKeys, const FastMap& thresholds, + const VectorValues& delta, + const ISAM2Clique::shared_ptr& clique) +{ // Check the current clique for relinearization bool relinearize = false; - BOOST_FOREACH(Index var, clique->conditional()->keys()) { - - // Lookup the key associated with this index - Key key = ordering.key(var); - + BOOST_FOREACH(Key var, *clique->conditional()) { // Find the threshold for this variable type - const Vector& threshold = thresholds.find(Symbol(key).chr())->second; + const Vector& threshold = thresholds.find(Symbol(var).chr())->second; + + const Vector& deltaVar = delta[var]; // Verify the threshold vector matches the actual variable size - if(threshold.rows() != delta[var].rows()) - throw std::invalid_argument("Relinearization threshold vector dimensionality passed into iSAM2 parameters does not match actual variable dimensionality"); + if(threshold.rows() != deltaVar.rows()) + throw std::invalid_argument("Relinearization threshold vector dimensionality for '" + std::string(1, Symbol(var).chr()) + "' passed into iSAM2 parameters does not match actual variable dimensionality."); // Check for relinearization - if((delta[var].array().abs() > threshold.array()).any()) { + if((deltaVar.array().abs() > threshold.array()).any()) { relinKeys.insert(var); relinearize = true; } @@ -217,52 +178,54 @@ void CheckRelinearizationRecursiveMap(FastSet& relinKeys, const FastMapchildren()) { - CheckRelinearizationRecursiveMap(relinKeys, thresholds, delta, ordering, child); + BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children) { + CheckRelinearizationRecursiveMap(relinKeys, thresholds, delta, child); } } } /* ************************************************************************* */ -FastSet ISAM2::Impl::CheckRelinearizationPartial(const ISAM2Clique::shared_ptr& root, const VectorValues& delta, const Ordering& ordering, - const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter) { - - FastSet relinKeys; - - if(root) { - if(relinearizeThreshold.type() == typeid(double)) { +FastSet ISAM2::Impl::CheckRelinearizationPartial(const FastVector& roots, + const VectorValues& delta, + const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) +{ + FastSet relinKeys; + BOOST_FOREACH(const ISAM2::sharedClique& root, roots) { + if(relinearizeThreshold.type() == typeid(double)) CheckRelinearizationRecursiveDouble(relinKeys, boost::get(relinearizeThreshold), delta, root); - } else if(relinearizeThreshold.type() == typeid(FastMap)) { - CheckRelinearizationRecursiveMap(relinKeys, boost::get >(relinearizeThreshold), delta, ordering, root); - } + else if(relinearizeThreshold.type() == typeid(FastMap)) + CheckRelinearizationRecursiveMap(relinKeys, boost::get >(relinearizeThreshold), delta, root); } - return relinKeys; } /* ************************************************************************* */ -void ISAM2::Impl::FindAll(ISAM2Clique::shared_ptr clique, FastSet& keys, const vector& markedMask) { +void ISAM2::Impl::FindAll(ISAM2Clique::shared_ptr clique, FastSet& keys, const FastSet& markedMask) +{ static const bool debug = false; // does the separator contain any of the variables? bool found = false; - BOOST_FOREACH(const Index& key, (*clique)->parents()) { - if (markedMask[key]) + BOOST_FOREACH(Key key, clique->conditional()->parents()) { + if (markedMask.exists(key)) { found = true; + break; + } } if (found) { // then add this clique - keys.insert((*clique)->beginFrontals(), (*clique)->endFrontals()); + keys.insert(clique->conditional()->beginFrontals(), clique->conditional()->endFrontals()); if(debug) clique->print("Key(s) marked in clique "); - if(debug) cout << "so marking key " << (*clique)->keys().front() << endl; + if(debug) cout << "so marking key " << clique->conditional()->front() << endl; } - BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children_) { + BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children) { FindAll(child, keys, markedMask); } } /* ************************************************************************* */ -void ISAM2::Impl::ExpmapMasked(Values& values, const VectorValues& delta, const Ordering& ordering, - const vector& mask, boost::optional invalidateIfDebug, const KeyFormatter& keyFormatter) { +void ISAM2::Impl::ExpmapMasked(Values& values, const VectorValues& delta, + const FastSet& mask, boost::optional invalidateIfDebug, const KeyFormatter& keyFormatter) +{ // If debugging, invalidate if requested, otherwise do not invalidate. // Invalidating means setting expmapped entries to Inf, to trigger assertions // if we try to re-use them. @@ -270,23 +233,22 @@ void ISAM2::Impl::ExpmapMasked(Values& values, const VectorValues& delta, const invalidateIfDebug = boost::none; #endif - assert(values.size() == ordering.size()); - assert(delta.size() == ordering.size()); + assert(values.size() == delta.size()); Values::iterator key_value; - Ordering::const_iterator key_index; - for(key_value = values.begin(), key_index = ordering.begin(); - key_value != values.end() && key_index != ordering.end(); ++key_value, ++key_index) { - assert(key_value->key == key_index->first); - const Index var = key_index->second; - if(ISDEBUG("ISAM2 update verbose")) { - if(mask[var]) - cout << "expmap " << keyFormatter(key_value->key) << " (j = " << var << "), delta = " << delta[var].transpose() << endl; - else - cout << " " << keyFormatter(key_value->key) << " (j = " << var << "), delta = " << delta[var].transpose() << endl; - } + VectorValues::const_iterator key_delta; +#ifdef GTSAM_USE_TBB + for(key_value = values.begin(); key_value != values.end(); ++key_value) + { + key_delta = delta.find(key_value->key); +#else + for(key_value = values.begin(), key_delta = delta.begin(); key_value != values.end(); ++key_value, ++key_delta) + { + assert(key_value->key == key_delta->first); +#endif + Key var = key_value->key; assert(delta[var].size() == (int)key_value->value.dim()); - assert(delta[var].unaryExpr(ptr_fun(isfinite)).all()); - if(mask[var]) { + assert(delta[var].allFinite()); + if(mask.exists(var)) { Value* retracted = key_value->value.retract_(delta[var]); key_value->value = *retracted; retracted->deallocate_(); @@ -296,134 +258,36 @@ void ISAM2::Impl::ExpmapMasked(Values& values, const VectorValues& delta, const } } -/* ************************************************************************* */ -ISAM2::Impl::PartialSolveResult -ISAM2::Impl::PartialSolve(GaussianFactorGraph& factors, - const FastSet& keys, const ReorderingMode& reorderingMode, bool useQR) { - - const bool debug = ISDEBUG("ISAM2 recalculate"); - - PartialSolveResult result; - - gttic(select_affected_variables); -#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS - // Debug check that all variables involved in the factors to be re-eliminated - // are in affectedKeys, since we will use it to select a subset of variables. - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) { - BOOST_FOREACH(Index key, factor->keys()) { - assert(find(keys.begin(), keys.end(), key) != keys.end()); - } - } -#endif - Permutation affectedKeysSelector(keys.size()); // Create a permutation that pulls the affected keys to the front - Permutation affectedKeysSelectorInverse(keys.size() > 0 ? *keys.rbegin()+1 : 0 /*ordering_.nVars()*/); // And its inverse -#ifndef NDEBUG - // If debugging, fill with invalid values that will trip asserts if dereferenced - std::fill(affectedKeysSelectorInverse.begin(), affectedKeysSelectorInverse.end(), numeric_limits::max()); -#endif - { Index position=0; BOOST_FOREACH(Index var, keys) { - affectedKeysSelector[position] = var; - affectedKeysSelectorInverse[var] = position; - ++ position; } } - if(debug) affectedKeysSelector.print("affectedKeysSelector: "); - if(debug) affectedKeysSelectorInverse.print("affectedKeysSelectorInverse: "); - factors.permuteWithInverse(affectedKeysSelectorInverse); - if(debug) factors.print("Factors to reorder/re-eliminate: "); - gttoc(select_affected_variables); - gttic(variable_index); - VariableIndex affectedFactorsIndex(factors); // Create a variable index for the factors to be re-eliminated - if(debug) affectedFactorsIndex.print("affectedFactorsIndex: "); - gttoc(variable_index); - gttic(ccolamd); - vector cmember(affectedKeysSelector.size(), 0); - if(reorderingMode.constrain == ReorderingMode::CONSTRAIN_LAST) { - assert(reorderingMode.constrainedKeys); - if(!reorderingMode.constrainedKeys->empty()) { - typedef std::pair Index_Group; - if(keys.size() > reorderingMode.constrainedKeys->size()) { // Only if some variables are unconstrained - BOOST_FOREACH(const Index_Group& index_group, *reorderingMode.constrainedKeys) { - cmember[affectedKeysSelectorInverse[index_group.first]] = index_group.second; } - } else { - int minGroup = *boost::range::min_element(boost::adaptors::values(*reorderingMode.constrainedKeys)); - BOOST_FOREACH(const Index_Group& index_group, *reorderingMode.constrainedKeys) { - cmember[affectedKeysSelectorInverse[index_group.first]] = index_group.second - minGroup; } - } - } - } - Permutation::shared_ptr affectedColamd(inference::PermutationCOLAMD_(affectedFactorsIndex, cmember)); - gttoc(ccolamd); - gttic(ccolamd_permutations); - Permutation::shared_ptr affectedColamdInverse(affectedColamd->inverse()); - if(debug) affectedColamd->print("affectedColamd: "); - if(debug) affectedColamdInverse->print("affectedColamdInverse: "); - result.reorderingSelector = affectedKeysSelector; - result.reorderingPermutation = *affectedColamd; - result.reorderingInverse = internal::Reduction::CreateFromPartialPermutation(affectedKeysSelector, *affectedColamdInverse); - gttoc(ccolamd_permutations); - - gttic(permute_affected_variable_index); - affectedFactorsIndex.permuteInPlace(*affectedColamd); - gttoc(permute_affected_variable_index); - - gttic(permute_affected_factors); - factors.permuteWithInverse(*affectedColamdInverse); - gttoc(permute_affected_factors); - - if(debug) factors.print("Colamd-ordered affected factors: "); - -#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS - VariableIndex fromScratchIndex(factors); - assert(assert_equal(fromScratchIndex, affectedFactorsIndex)); -#endif - - // eliminate into a Bayes net - gttic(eliminate); - JunctionTree jt(factors, affectedFactorsIndex); - if(!useQR) - result.bayesTree = jt.eliminate(EliminatePreferCholesky); - else - result.bayesTree = jt.eliminate(EliminateQR); - gttoc(eliminate); - - gttic(permute_eliminated); - if(result.bayesTree) result.bayesTree->permuteWithInverse(affectedKeysSelector); - if(debug && result.bayesTree) { - cout << "Full var-ordered eliminated BT:\n"; - result.bayesTree->printTree(""); - } - // Undo permutation on our subset of cached factors, we must later permute *all* of the cached factors - factors.permuteWithInverse(*affectedColamd); - factors.permuteWithInverse(affectedKeysSelector); - gttoc(permute_eliminated); - - return result; -} - /* ************************************************************************* */ namespace internal { inline static void optimizeInPlace(const boost::shared_ptr& clique, VectorValues& result) { // parents are assumed to already be solved and available in result - clique->conditional()->solveInPlace(result); + result.update(clique->conditional()->solve(result)); // starting from the root, call optimize on each conditional - BOOST_FOREACH(const boost::shared_ptr& child, clique->children_) + BOOST_FOREACH(const boost::shared_ptr& child, clique->children) optimizeInPlace(child, result); } } /* ************************************************************************* */ -size_t ISAM2::Impl::UpdateDelta(const boost::shared_ptr& root, std::vector& replacedKeys, VectorValues& delta, double wildfireThreshold) { +size_t ISAM2::Impl::UpdateGaussNewtonDelta(const FastVector& roots, + const FastSet& replacedKeys, VectorValues& delta, double wildfireThreshold) { size_t lastBacksubVariableCount; if (wildfireThreshold <= 0.0) { // Threshold is zero or less, so do a full recalculation - internal::optimizeInPlace(root, delta); + BOOST_FOREACH(const ISAM2::sharedClique& root, roots) + internal::optimizeInPlace(root, delta); lastBacksubVariableCount = delta.size(); } else { // Optimize with wildfire - lastBacksubVariableCount = optimizeWildfireNonRecursive(root, wildfireThreshold, replacedKeys, delta); // modifies delta_ + lastBacksubVariableCount = 0; + BOOST_FOREACH(const ISAM2::sharedClique& root, roots) + lastBacksubVariableCount += optimizeWildfireNonRecursive( + root, wildfireThreshold, replacedKeys, delta); // modifies delta #ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS for(size_t j=0; j& root, std: #endif } - // Clear replacedKeys - replacedKeys.assign(replacedKeys.size(), false); - return lastBacksubVariableCount; } /* ************************************************************************* */ namespace internal { -void updateDoglegDeltas(const boost::shared_ptr& clique, const std::vector& replacedKeys, - const VectorValues& grad, VectorValues& deltaNewton, VectorValues& RgProd, size_t& varsUpdated) { +void updateRgProd(const boost::shared_ptr& clique, const FastSet& replacedKeys, + const VectorValues& grad, VectorValues& RgProd, size_t& varsUpdated) { // Check if any frontal or separator keys were recalculated, if so, we need // update deltas and recurse to children, but if not, we do not need to // recurse further because of the running separator property. bool anyReplaced = false; - BOOST_FOREACH(Index j, *clique->conditional()) { - if(replacedKeys[j]) { + BOOST_FOREACH(Key j, *clique->conditional()) { + if(replacedKeys.exists(j)) { anyReplaced = true; break; } @@ -456,43 +317,58 @@ void updateDoglegDeltas(const boost::shared_ptr& clique, const std: if(anyReplaced) { // Update the current variable // Get VectorValues slice corresponding to current variables - Vector gR = internal::extractVectorValuesSlices(grad, (*clique)->beginFrontals(), (*clique)->endFrontals()); - Vector gS = internal::extractVectorValuesSlices(grad, (*clique)->beginParents(), (*clique)->endParents()); + Vector gR = grad.vector(FastVector(clique->conditional()->beginFrontals(), clique->conditional()->endFrontals())); + Vector gS = grad.vector(FastVector(clique->conditional()->beginParents(), clique->conditional()->endParents())); // Compute R*g and S*g for this clique - Vector RSgProd = (*clique)->get_R() * gR + (*clique)->get_S() * gS; + Vector RSgProd = clique->conditional()->get_R() * gR + clique->conditional()->get_S() * gS; // Write into RgProd vector - internal::writeVectorValuesSlices(RSgProd, RgProd, (*clique)->beginFrontals(), (*clique)->endFrontals()); + DenseIndex vectorPosition = 0; + BOOST_FOREACH(Key frontal, clique->conditional()->frontals()) { + Vector& RgProdValue = RgProd[frontal]; + RgProdValue = RSgProd.segment(vectorPosition, RgProdValue.size()); + vectorPosition += RgProdValue.size(); + } // Now solve the part of the Newton's method point for this clique (back-substitution) //(*clique)->solveInPlace(deltaNewton); - varsUpdated += (*clique)->nrFrontals(); + varsUpdated += clique->conditional()->nrFrontals(); // Recurse to children - BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children()) { - updateDoglegDeltas(child, replacedKeys, grad, deltaNewton, RgProd, varsUpdated); } + BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children) { + updateRgProd(child, replacedKeys, grad, RgProd, varsUpdated); } } } } /* ************************************************************************* */ -size_t ISAM2::Impl::UpdateDoglegDeltas(const ISAM2& isam, double wildfireThreshold, std::vector& replacedKeys, - VectorValues& deltaNewton, VectorValues& RgProd) { - - // Get gradient - VectorValues grad = *allocateVectorValues(isam); - gradientAtZero(isam, grad); +size_t ISAM2::Impl::UpdateRgProd(const ISAM2::Roots& roots, const FastSet& replacedKeys, + const VectorValues& gradAtZero, VectorValues& RgProd) { // Update variables size_t varsUpdated = 0; - internal::updateDoglegDeltas(isam.root(), replacedKeys, grad, deltaNewton, RgProd, varsUpdated); - optimizeWildfireNonRecursive(isam.root(), wildfireThreshold, replacedKeys, deltaNewton); - - replacedKeys.assign(replacedKeys.size(), false); + BOOST_FOREACH(const ISAM2::sharedClique& root, roots) { + internal::updateRgProd(root, replacedKeys, gradAtZero, RgProd, varsUpdated); + } return varsUpdated; } + +/* ************************************************************************* */ +VectorValues ISAM2::Impl::ComputeGradientSearch(const VectorValues& gradAtZero, + const VectorValues& RgProd) +{ + // Compute gradient squared-magnitude + const double gradientSqNorm = gradAtZero.dot(gradAtZero); + + // Compute minimizing step size + double RgNormSq = RgProd.vector().squaredNorm(); + double step = -gradientSqNorm / RgNormSq; + + // Compute steepest descent point + return step * gradAtZero; +} } diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index 07c27c896..a8d03df13 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -26,16 +26,13 @@ struct GTSAM_EXPORT ISAM2::Impl { struct GTSAM_EXPORT PartialSolveResult { ISAM2::sharedClique bayesTree; - Permutation reorderingSelector; - Permutation reorderingPermutation; - internal::Reduction reorderingInverse; }; struct GTSAM_EXPORT ReorderingMode { size_t nFullSystemVars; enum { /*AS_ADDED,*/ COLAMD } algorithm; enum { NO_CONSTRAINT, CONSTRAIN_LAST } constrain; - boost::optional > constrainedKeys; + boost::optional > constrainedKeys; }; /** @@ -48,25 +45,22 @@ struct GTSAM_EXPORT ISAM2::Impl { * @param keyFormatter Formatter for printing nonlinear keys during debugging */ static void AddVariables(const Values& newTheta, Values& theta, VectorValues& delta, - VectorValues& deltaNewton, VectorValues& RgProd, std::vector& replacedKeys, - Ordering& ordering, const KeyFormatter& keyFormatter = DefaultKeyFormatter); + VectorValues& deltaNewton, VectorValues& RgProd, + const KeyFormatter& keyFormatter = DefaultKeyFormatter); + + /// Perform the first part of the bookkeeping updates for adding new factors. Adds them to the + /// complete list of nonlinear factors, and populates the list of new factor indices, both + /// optionally finding and reusing empty factor slots. + static void AddFactorsStep1(const NonlinearFactorGraph& newFactors, bool useUnusedSlots, + NonlinearFactorGraph& nonlinearFactors, FastVector& newFactorIndices); /** * Remove variables from the ISAM2 system. */ - static void RemoveVariables(const FastSet& unusedKeys, const ISAM2Clique::shared_ptr& root, + static void RemoveVariables(const FastSet& unusedKeys, const FastVector& roots, Values& theta, VariableIndex& variableIndex, VectorValues& delta, VectorValues& deltaNewton, - VectorValues& RgProd, std::vector& replacedKeys, Ordering& ordering, Base::Nodes& nodes, - GaussianFactorGraph& linearFactors, FastSet& fixedVariables); - - /** - * Extract the set of variable indices from a NonlinearFactorGraph. For each Symbol - * in each NonlinearFactor, obtains the index by calling ordering[symbol]. - * @param ordering The current ordering from which to obtain the variable indices - * @param factors The factors from which to extract the variables - * @return The set of variables indices from the factors - */ - static FastSet IndicesFromFactors(const Ordering& ordering, const NonlinearFactorGraph& factors); + VectorValues& RgProd, FastSet& replacedKeys, Base::Nodes& nodes, + FastSet& fixedVariables); /** * Find the set of variables to be relinearized according to relinearizeThreshold. @@ -77,8 +71,8 @@ struct GTSAM_EXPORT ISAM2::Impl { * @return The set of variable indices in delta whose magnitude is greater than or * equal to relinearizeThreshold */ - static FastSet CheckRelinearizationFull(const VectorValues& delta, const Ordering& ordering, - const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter = DefaultKeyFormatter); + static FastSet CheckRelinearizationFull(const VectorValues& delta, + const ISAM2Params::RelinearizationThreshold& relinearizeThreshold); /** * Find the set of variables to be relinearized according to relinearizeThreshold. @@ -91,8 +85,8 @@ struct GTSAM_EXPORT ISAM2::Impl { * @return The set of variable indices in delta whose magnitude is greater than or * equal to relinearizeThreshold */ - static FastSet CheckRelinearizationPartial(const ISAM2Clique::shared_ptr& root, const VectorValues& delta, const Ordering& ordering, - const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter = DefaultKeyFormatter); + static FastSet CheckRelinearizationPartial(const FastVector& roots, + const VectorValues& delta, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold); /** * Recursively search this clique and its children for marked keys appearing @@ -109,7 +103,7 @@ struct GTSAM_EXPORT ISAM2::Impl { * * Alternatively could we trace up towards the root for each variable here? */ - static void FindAll(ISAM2Clique::shared_ptr clique, FastSet& keys, const std::vector& markedMask); + static void FindAll(ISAM2Clique::shared_ptr clique, FastSet& keys, const FastSet& markedMask); /** * Apply expmap to the given values, but only for indices appearing in @@ -125,31 +119,28 @@ struct GTSAM_EXPORT ISAM2::Impl { * @param keyFormatter Formatter for printing nonlinear keys during debugging */ static void ExpmapMasked(Values& values, const VectorValues& delta, - const Ordering& ordering, const std::vector& mask, + const FastSet& mask, boost::optional invalidateIfDebug = boost::none, const KeyFormatter& keyFormatter = DefaultKeyFormatter); /** - * Reorder and eliminate factors. These factors form a subset of the full - * problem, so along with the BayesTree we get a partial reordering of the - * problem that needs to be applied to the other data in ISAM2, which is the - * VariableIndex, the delta, the ordering, and the orphans (including cached - * factors). - * \param factors The factors to eliminate, representing part of the full - * problem. This is permuted during use and so is cleared when this function - * returns in order to invalidate it. - * \param keys The set of indices used in \c factors. - * \param useQR Whether to use QR (if true), or Cholesky (if false). - * \return The eliminated BayesTree and the permutation to be applied to the - * rest of the ISAM2 data. + * Update the Newton's method step point, using wildfire */ - static PartialSolveResult PartialSolve(GaussianFactorGraph& factors, const FastSet& keys, - const ReorderingMode& reorderingMode, bool useQR); + static size_t UpdateGaussNewtonDelta(const FastVector& roots, + const FastSet& replacedKeys, VectorValues& delta, double wildfireThreshold); - static size_t UpdateDelta(const boost::shared_ptr& root, std::vector& replacedKeys, VectorValues& delta, double wildfireThreshold); - - static size_t UpdateDoglegDeltas(const ISAM2& isam, double wildfireThreshold, std::vector& replacedKeys, - VectorValues& deltaNewton, VectorValues& RgProd); + /** + * Update the RgProd (R*g) incrementally taking into account which variables + * have been recalculated in \c replacedKeys. Only used in Dogleg. + */ + static size_t UpdateRgProd(const ISAM2::Roots& roots, const FastSet& replacedKeys, + const VectorValues& gradAtZero, VectorValues& RgProd); + + /** + * Compute the gradient-search point. Only used in Dogleg. + */ + static VectorValues ComputeGradientSearch(const VectorValues& gradAtZero, + const VectorValues& RgProd); }; diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h index 4a9cf105a..9248617b5 100644 --- a/gtsam/nonlinear/ISAM2-inl.h +++ b/gtsam/nonlinear/ISAM2-inl.h @@ -28,8 +28,7 @@ namespace gtsam { /* ************************************************************************* */ template VALUE ISAM2::calculateEstimate(Key key) const { - const Index index = getOrdering()[key]; - const Vector& delta = getDelta()[index]; + const Vector& delta = getDelta()[key]; return theta_.at(key).retract(delta); } @@ -37,24 +36,25 @@ VALUE ISAM2::calculateEstimate(Key key) const { namespace internal { template void optimizeWildfire(const boost::shared_ptr& clique, double threshold, - std::vector& changed, const std::vector& replaced, VectorValues& delta, int& count) { + FastSet& changed, const FastSet& replaced, VectorValues& delta, size_t& count) +{ // if none of the variables in this clique (frontal and separator!) changed // significantly, then by the running intersection property, none of the // cliques in the children need to be processed // Are any clique variables part of the tree that has been redone? - bool cliqueReplaced = replaced[(*clique)->frontals().front()]; + bool cliqueReplaced = replaced.exists((*clique)->frontals().front()); #ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS - BOOST_FOREACH(Index frontal, (*clique)->frontals()) { - assert(cliqueReplaced == replaced[frontal]); + BOOST_FOREACH(Key frontal, clique->conditional()->frontals()) { + assert(cliqueReplaced == replaced.exists(frontal)); } #endif // If not redone, then has one of the separator variables changed significantly? bool recalculate = cliqueReplaced; if(!recalculate) { - BOOST_FOREACH(Index parent, (*clique)->parents()) { - if(changed[parent]) { + BOOST_FOREACH(Key parent, clique->conditional()->parents()) { + if(changed.exists(parent)) { recalculate = true; break; } @@ -66,22 +66,22 @@ void optimizeWildfire(const boost::shared_ptr& clique, double threshold, if(recalculate) { // Temporary copy of the original values, to check how much they change - std::vector originalValues((*clique)->nrFrontals()); + FastVector originalValues(clique->conditional()->nrFrontals()); GaussianConditional::const_iterator it; - for(it = (*clique)->beginFrontals(); it!=(*clique)->endFrontals(); it++) { - originalValues[it - (*clique)->beginFrontals()] = delta[*it]; + for(it = clique->conditional()->beginFrontals(); it!=clique->conditional()->endFrontals(); it++) { + originalValues[it - clique->conditional()->beginFrontals()] = delta[*it]; } // Back-substitute - (*clique)->solveInPlace(delta); - count += (*clique)->nrFrontals(); + delta.update(clique->conditional()->solve(delta)); + count += clique->conditional()->nrFrontals(); // Whether the values changed above a threshold, or always true if the // clique was replaced. bool valuesChanged = cliqueReplaced; - for(it = (*clique)->beginFrontals(); it!=(*clique)->endFrontals(); it++) { + for(it = clique->conditional()->beginFrontals(); it != clique->conditional()->endFrontals(); it++) { if(!valuesChanged) { - const Vector& oldValue(originalValues[it - (*clique)->beginFrontals()]); + const Vector& oldValue(originalValues[it - clique->conditional()->beginFrontals()]); const Vector& newValue(delta[*it]); if((oldValue - newValue).lpNorm() >= threshold) { valuesChanged = true; @@ -94,18 +94,18 @@ void optimizeWildfire(const boost::shared_ptr& clique, double threshold, // If the values were above the threshold or this clique was replaced if(valuesChanged) { // Set changed flag for each frontal variable and leave the new values - BOOST_FOREACH(Index frontal, (*clique)->frontals()) { - changed[frontal] = true; + BOOST_FOREACH(Key frontal, clique->conditional()->frontals()) { + changed.insert(frontal); } } else { // Replace with the old values - for(it = (*clique)->beginFrontals(); it!=(*clique)->endFrontals(); it++) { - delta[*it] = originalValues[it - (*clique)->beginFrontals()]; + for(it = clique->conditional()->beginFrontals(); it != clique->conditional()->endFrontals(); it++) { + delta[*it] = originalValues[it - clique->conditional()->beginFrontals()]; } } // Recurse to children - BOOST_FOREACH(const typename CLIQUE::shared_ptr& child, clique->children_) { + BOOST_FOREACH(const typename CLIQUE::shared_ptr& child, clique->children) { optimizeWildfire(child, threshold, changed, replaced, delta, count); } } @@ -113,24 +113,25 @@ void optimizeWildfire(const boost::shared_ptr& clique, double threshold, template bool optimizeWildfireNode(const boost::shared_ptr& clique, double threshold, - std::vector& changed, const std::vector& replaced, VectorValues& delta, int& count) { + FastSet& changed, const FastSet& replaced, VectorValues& delta, size_t& count) +{ // if none of the variables in this clique (frontal and separator!) changed // significantly, then by the running intersection property, none of the // cliques in the children need to be processed // Are any clique variables part of the tree that has been redone? - bool cliqueReplaced = replaced[(*clique)->frontals().front()]; + bool cliqueReplaced = replaced.exists(clique->conditional()->frontals().front()); #ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS - BOOST_FOREACH(Index frontal, (*clique)->frontals()) { - assert(cliqueReplaced == replaced[frontal]); + BOOST_FOREACH(Key frontal, clique->conditional()->frontals()) { + assert(cliqueReplaced == replaced.exists(frontal)); } #endif // If not redone, then has one of the separator variables changed significantly? bool recalculate = cliqueReplaced; if(!recalculate) { - BOOST_FOREACH(Index parent, (*clique)->parents()) { - if(changed[parent]) { + BOOST_FOREACH(Key parent, clique->conditional()->parents()) { + if(changed.exists(parent)) { recalculate = true; break; } @@ -139,25 +140,81 @@ bool optimizeWildfireNode(const boost::shared_ptr& clique, double thresh // Solve clique if it was replaced, or if any parents were changed above the // threshold or themselves replaced. - if(recalculate) { - + if(recalculate) + { // Temporary copy of the original values, to check how much they change - std::vector originalValues((*clique)->nrFrontals()); + FastVector originalValues(clique->conditional()->nrFrontals()); GaussianConditional::const_iterator it; - for(it = (*clique)->beginFrontals(); it!=(*clique)->endFrontals(); it++) { - originalValues[it - (*clique)->beginFrontals()] = delta[*it]; + for(it = clique->conditional()->beginFrontals(); it != clique->conditional()->endFrontals(); it++) { + originalValues[it - clique->conditional()->beginFrontals()] = delta[*it]; } - // Back-substitute - (*clique)->solveInPlace(delta); - count += (*clique)->nrFrontals(); + // Back-substitute - special version stores solution pointers in cliques for fast access. + { + // Create solution part pointers if necessary and possible - necessary if solnPointers_ is + // empty, and possible if either we're a root, or we have a parent with valid solnPointers_. + boost::shared_ptr parent = clique->parent_.lock(); + if(clique->solnPointers_.empty() && (clique->isRoot() || !parent->solnPointers_.empty())) + { + BOOST_FOREACH(Key key, clique->conditional()->frontals()) + clique->solnPointers_.insert(std::make_pair(key, delta.find(key))); + BOOST_FOREACH(Key key, clique->conditional()->parents()) + clique->solnPointers_.insert(std::make_pair(key, parent->solnPointers_.at(key))); + } + + // See if we can use solution part pointers - we can if they either already existed or were + // created above. + if(!clique->solnPointers_.empty()) + { + GaussianConditional& c = *clique->conditional(); + // Solve matrix + Vector xS; + { + // Count dimensions of vector + DenseIndex dim = 0; + FastVector parentPointers; + parentPointers.reserve(clique->conditional()->nrParents()); + BOOST_FOREACH(Key parent, clique->conditional()->parents()) { + parentPointers.push_back(clique->solnPointers_.at(parent)); + dim += parentPointers.back()->second.size(); + } + + // Fill parent vector + xS.resize(dim); + DenseIndex vectorPos = 0; + BOOST_FOREACH(const VectorValues::const_iterator& parentPointer, parentPointers) { + const Vector& parentVector = parentPointer->second; + xS.block(vectorPos,0,parentVector.size(),1) = parentVector.block(0,0,parentVector.size(),1); + vectorPos += parentVector.size(); + } + } + xS = c.getb() - c.get_S() * xS; + Vector soln = c.get_R().triangularView().solve(xS); + + // Check for indeterminant solution + if(soln.hasNaN()) throw IndeterminantLinearSystemException(c.keys().front()); + + // Insert solution into a VectorValues + DenseIndex vectorPosition = 0; + for(GaussianConditional::const_iterator frontal = c.beginFrontals(); frontal != c.endFrontals(); ++frontal) { + clique->solnPointers_.at(*frontal)->second = soln.segment(vectorPosition, c.getDim(frontal)); + vectorPosition += c.getDim(frontal); + } + } + else + { + // Just call plain solve because we couldn't use solution pointers. + delta.update(clique->conditional()->solve(delta)); + } + } + count += clique->conditional()->nrFrontals(); // Whether the values changed above a threshold, or always true if the // clique was replaced. bool valuesChanged = cliqueReplaced; - for(it = (*clique)->beginFrontals(); it!=(*clique)->endFrontals(); it++) { + for(it = clique->conditional()->beginFrontals(); it != clique->conditional()->endFrontals(); it++) { if(!valuesChanged) { - const Vector& oldValue(originalValues[it - (*clique)->beginFrontals()]); + const Vector& oldValue(originalValues[it - clique->conditional()->beginFrontals()]); const Vector& newValue(delta[*it]); if((oldValue - newValue).lpNorm() >= threshold) { valuesChanged = true; @@ -170,16 +227,15 @@ bool optimizeWildfireNode(const boost::shared_ptr& clique, double thresh // If the values were above the threshold or this clique was replaced if(valuesChanged) { // Set changed flag for each frontal variable and leave the new values - BOOST_FOREACH(Index frontal, (*clique)->frontals()) { - changed[frontal] = true; + BOOST_FOREACH(Key frontal, clique->conditional()->frontals()) { + changed.insert(frontal); } } else { // Replace with the old values - for(it = (*clique)->beginFrontals(); it!=(*clique)->endFrontals(); it++) { - delta[*it] = originalValues[it - (*clique)->beginFrontals()]; + for(it = clique->conditional()->beginFrontals(); it != clique->conditional()->endFrontals(); it++) { + delta[*it] = originalValues[it - clique->conditional()->beginFrontals()]; } } - } return recalculate; @@ -189,8 +245,8 @@ bool optimizeWildfireNode(const boost::shared_ptr& clique, double thresh /* ************************************************************************* */ template -int optimizeWildfire(const boost::shared_ptr& root, double threshold, const std::vector& keys, VectorValues& delta) { - std::vector changed(keys.size(), false); +size_t optimizeWildfire(const boost::shared_ptr& root, double threshold, const FastSet& keys, VectorValues& delta) { + FastSet changed; int count = 0; // starting from the root, call optimize on each conditional if(root) @@ -200,9 +256,10 @@ int optimizeWildfire(const boost::shared_ptr& root, double threshold, co /* ************************************************************************* */ template -int optimizeWildfireNonRecursive(const boost::shared_ptr& root, double threshold, const std::vector& keys, VectorValues& delta) { - std::vector changed(keys.size(), false); - int count = 0; +size_t optimizeWildfireNonRecursive(const boost::shared_ptr& root, double threshold, const FastSet& keys, VectorValues& delta) +{ + FastSet changed; + size_t count = 0; if (root) { std::stack > travStack; @@ -213,7 +270,7 @@ int optimizeWildfireNonRecursive(const boost::shared_ptr& root, double t travStack.pop(); bool recalculate = internal::optimizeWildfireNode(currentNode, threshold, changed, keys, delta, count); if (recalculate) { - BOOST_FOREACH(const typename CLIQUE::shared_ptr& child, currentNode->children_) { + BOOST_FOREACH(const typename CLIQUE::shared_ptr& child, currentNode->children) { travStack.push(child); } } @@ -226,11 +283,11 @@ int optimizeWildfireNonRecursive(const boost::shared_ptr& root, double t /* ************************************************************************* */ template void nnz_internal(const boost::shared_ptr& clique, int& result) { - int dimR = (*clique)->dim(); - int dimSep = (*clique)->get_S().cols() - dimR; + int dimR = (int)clique->conditional()->rows(); + int dimSep = (int)clique->conditional()->get_S().cols(); result += ((dimR+1)*dimR)/2 + dimSep*dimR; // traverse the children - BOOST_FOREACH(const typename CLIQUE::shared_ptr& child, clique->children_) { + BOOST_FOREACH(const typename CLIQUE::shared_ptr& child, clique->children) { nnz_internal(child, result); } } diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 2275ec2be..6d6785b14 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -19,29 +19,63 @@ #include // for operator += using namespace boost::assign; #include -#include +#include #include +namespace br { using namespace boost::range; using namespace boost::adaptors; } #include #include -#include -#include -#include +#include +#include +#include // We need the inst file because we'll make a special JT templated on ISAM2 +#include #include #include +#include #include #include #include #include +using namespace std; + namespace gtsam { -using namespace std; +// Instantiate base classes +template class BayesTreeCliqueBase; +template class BayesTree; static const bool disableReordering = false; static const double batchThreshold = 0.65; +/* ************************************************************************* */ +// Special BayesTree class that uses ISAM2 cliques - this is the result of reeliminating ISAM2 +// subtrees. +class ISAM2BayesTree : public ISAM2::Base +{ +public: + typedef ISAM2::Base Base; + typedef ISAM2BayesTree This; + typedef boost::shared_ptr shared_ptr; + + ISAM2BayesTree() {} +}; + +/* ************************************************************************* */ +// Special JunctionTree class that produces ISAM2 BayesTree cliques, used for reeliminating ISAM2 +// subtrees. +class ISAM2JunctionTree : public JunctionTree +{ +public: + typedef JunctionTree Base; + typedef ISAM2JunctionTree This; + typedef boost::shared_ptr shared_ptr; + + ISAM2JunctionTree(const GaussianEliminationTree& eliminationTree) : + Base(eliminationTree) {} +}; + /* ************************************************************************* */ std::string ISAM2DoglegParams::adaptationModeTranslator(const DoglegOptimizerImpl::TrustRegionAdaptationMode& adaptationMode) const { std::string s; @@ -85,82 +119,70 @@ std::string ISAM2Params::factorizationTranslator(const ISAM2Params::Factorizatio } /* ************************************************************************* */ -ISAM2::ISAM2(const ISAM2Params& params): - deltaDoglegUptodate_(true), deltaUptodate_(true), params_(params) { +void ISAM2Clique::setEliminationResult(const FactorGraphType::EliminationResult& eliminationResult) +{ + conditional_ = eliminationResult.first; + cachedFactor_ = eliminationResult.second; + // Compute gradient contribution + gradientContribution_.resize(conditional_->cols() - 1); + // Rewrite -(R * P')'*d as -(d' * R * P')' for computational speed reasons + gradientContribution_ << -conditional_->get_R().transpose() * conditional_->get_d(), + -conditional_->get_S().transpose() * conditional_->get_d(); +} + +/* ************************************************************************* */ +bool ISAM2Clique::equals(const This& other, double tol) const { + return Base::equals(other) && + ((!cachedFactor_ && !other.cachedFactor_) + || (cachedFactor_ && other.cachedFactor_ + && cachedFactor_->equals(*other.cachedFactor_, tol))); +} + +/* ************************************************************************* */ +void ISAM2Clique::print(const std::string& s, const KeyFormatter& formatter) const +{ + Base::print(s,formatter); + if(cachedFactor_) + cachedFactor_->print(s + "Cached: ", formatter); + else + std::cout << s << "Cached empty" << std::endl; + if(gradientContribution_.rows() != 0) + gtsam::print(gradientContribution_, "Gradient contribution: "); +} + +/* ************************************************************************* */ +ISAM2::ISAM2(const ISAM2Params& params): params_(params), update_count_(0) { if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) doglegDelta_ = boost::get(params_.optimizationParams).initialDelta; } /* ************************************************************************* */ -ISAM2::ISAM2(): - deltaDoglegUptodate_(true), deltaUptodate_(true) { +ISAM2::ISAM2() : update_count_(0) { if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) doglegDelta_ = boost::get(params_.optimizationParams).initialDelta; } /* ************************************************************************* */ -ISAM2::ISAM2(const ISAM2& other) { - *this = other; +bool ISAM2::equals(const ISAM2& other, double tol) const { + return Base::equals(other, tol) + && theta_.equals(other.theta_, tol) && variableIndex_.equals(other.variableIndex_, tol) + && nonlinearFactors_.equals(other.nonlinearFactors_, tol) + && fixedVariables_ == other.fixedVariables_; } /* ************************************************************************* */ -ISAM2& ISAM2::operator=(const ISAM2& rhs) { - // Copy BayesTree - this->Base::operator=(rhs); - - // Copy our variables - // When we have Permuted<...>, it is only necessary to copy this permuted - // view and not the original, because copying the permuted view automatically - // copies the original. - theta_ = rhs.theta_; - variableIndex_ = rhs.variableIndex_; - delta_ = rhs.delta_; - deltaNewton_ = rhs.deltaNewton_; - RgProd_ = rhs.RgProd_; - deltaDoglegUptodate_ = rhs.deltaDoglegUptodate_; - deltaUptodate_ = rhs.deltaUptodate_; - deltaReplacedMask_ = rhs.deltaReplacedMask_; - nonlinearFactors_ = rhs.nonlinearFactors_; - - linearFactors_ = GaussianFactorGraph(); - linearFactors_.reserve(rhs.linearFactors_.size()); - BOOST_FOREACH(const GaussianFactor::shared_ptr& linearFactor, rhs.linearFactors_) { - linearFactors_.push_back(linearFactor ? linearFactor->clone() : GaussianFactor::shared_ptr()); } - - ordering_ = rhs.ordering_; - params_ = rhs.params_; - doglegDelta_ = rhs.doglegDelta_; - - lastAffectedVariableCount = rhs.lastAffectedVariableCount; - lastAffectedFactorCount = rhs.lastAffectedFactorCount; - lastAffectedCliqueCount = rhs.lastAffectedCliqueCount; - lastAffectedMarkedCount = rhs.lastAffectedMarkedCount; - lastBacksubVariableCount = rhs.lastBacksubVariableCount; - lastNnzTop = rhs.lastNnzTop; - - return *this; -} - -/* ************************************************************************* */ -FastList ISAM2::getAffectedFactors(const FastList& keys) const { +FastSet ISAM2::getAffectedFactors(const FastList& keys) const { static const bool debug = false; if(debug) cout << "Getting affected factors for "; - if(debug) { BOOST_FOREACH(const Index key, keys) { cout << key << " "; } } + if(debug) { BOOST_FOREACH(const Key key, keys) { cout << key << " "; } } if(debug) cout << endl; - FactorGraph allAffected; - FastList indices; - BOOST_FOREACH(const Index key, keys) { -// const list l = nonlinearFactors_.factors(key); -// indices.insert(indices.begin(), l.begin(), l.end()); + NonlinearFactorGraph allAffected; + FastSet indices; + BOOST_FOREACH(const Key key, keys) { const VariableIndex::Factors& factors(variableIndex_[key]); - BOOST_FOREACH(size_t factor, factors) { - if(debug) cout << "Variable " << key << " affects factor " << factor << endl; - indices.push_back(factor); - } + indices.insert(factors.begin(), factors.end()); } - indices.sort(); - indices.unique(); if(debug) cout << "Affected factors are: "; if(debug) { BOOST_FOREACH(const size_t index, indices) { cout << index << " "; } } if(debug) cout << endl; @@ -170,44 +192,44 @@ FastList ISAM2::getAffectedFactors(const FastList& keys) const { /* ************************************************************************* */ // retrieve all factors that ONLY contain the affected variables // (note that the remaining stuff is summarized in the cached factors) -FactorGraph::shared_ptr -ISAM2::relinearizeAffectedFactors(const FastList& affectedKeys, const FastSet& relinKeys) const { +GaussianFactorGraph::shared_ptr +ISAM2::relinearizeAffectedFactors(const FastList& affectedKeys, const FastSet& relinKeys) const +{ gttic(getAffectedFactors); - FastList candidates = getAffectedFactors(affectedKeys); + FastSet candidates = getAffectedFactors(affectedKeys); gttoc(getAffectedFactors); NonlinearFactorGraph nonlinearAffectedFactors; gttic(affectedKeysSet); // for fast lookup below - FastSet affectedKeysSet; + FastSet affectedKeysSet; affectedKeysSet.insert(affectedKeys.begin(), affectedKeys.end()); gttoc(affectedKeysSet); gttic(check_candidates_and_linearize); - FactorGraph::shared_ptr linearized = boost::make_shared >(); + GaussianFactorGraph::shared_ptr linearized = boost::make_shared(); BOOST_FOREACH(size_t idx, candidates) { bool inside = true; bool useCachedLinear = params_.cacheLinearizedFactors; BOOST_FOREACH(Key key, nonlinearFactors_[idx]->keys()) { - Index var = ordering_[key]; - if(affectedKeysSet.find(var) == affectedKeysSet.end()) { + if(affectedKeysSet.find(key) == affectedKeysSet.end()) { inside = false; break; } - if(useCachedLinear && relinKeys.find(var) != relinKeys.end()) + if(useCachedLinear && relinKeys.find(key) != relinKeys.end()) useCachedLinear = false; } if(inside) { if(useCachedLinear) { #ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS assert(linearFactors_[idx]); - assert(linearFactors_[idx]->keys() == nonlinearFactors_[idx]->symbolic(ordering_)->keys()); + assert(linearFactors_[idx]->keys() == nonlinearFactors_[idx]->keys()); #endif linearized->push_back(linearFactors_[idx]); } else { - GaussianFactor::shared_ptr linearFactor = nonlinearFactors_[idx]->linearize(theta_, ordering_); + GaussianFactor::shared_ptr linearFactor = nonlinearFactors_[idx]->linearize(theta_); linearized->push_back(linearFactor); if(params_.cacheLinearizedFactors) { #ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS @@ -225,27 +247,25 @@ ISAM2::relinearizeAffectedFactors(const FastList& affectedKeys, const Fas /* ************************************************************************* */ // find intermediate (linearized) factors from cache that are passed into the affected area + GaussianFactorGraph ISAM2::getCachedBoundaryFactors(Cliques& orphans) { - - static const bool debug = false; - GaussianFactorGraph cachedBoundary; BOOST_FOREACH(sharedClique orphan, orphans) { - // find the last variable that was eliminated - Index key = (*orphan)->frontals().back(); // retrieve the cached factor and add to boundary cachedBoundary.push_back(orphan->cachedFactor()); - if(debug) { cout << "Cached factor for variable " << key; orphan->cachedFactor()->print(""); } } return cachedBoundary; } -boost::shared_ptr > ISAM2::recalculate(const FastSet& markedKeys, - const FastSet& relinKeys, const FastVector& observedKeys, const FastSet& unusedIndices, - const boost::optional >& constrainKeys, ISAM2Result& result) { - +/* ************************************************************************* */ +boost::shared_ptr > ISAM2::recalculate(const FastSet& markedKeys, const FastSet& relinKeys, + const vector& observedKeys, + const FastSet& unusedIndices, + const boost::optional >& constrainKeys, + ISAM2Result& result) +{ // TODO: new factors are linearized twice, the newFactors passed in are not used. const bool debug = ISDEBUG("ISAM2 recalculate"); @@ -272,10 +292,10 @@ boost::shared_ptr > ISAM2::recalculate(const FastSet& mark if(debug) { cout << "markedKeys: "; - BOOST_FOREACH(const Index key, markedKeys) { cout << key << " "; } + BOOST_FOREACH(const Key key, markedKeys) { cout << key << " "; } cout << endl; cout << "observedKeys: "; - BOOST_FOREACH(const Index key, observedKeys) { cout << key << " "; } + BOOST_FOREACH(const Key key, observedKeys) { cout << key << " "; } cout << endl; } @@ -284,13 +304,10 @@ boost::shared_ptr > ISAM2::recalculate(const FastSet& mark // (b) Store orphaned sub-trees \BayesTree_{O} of removed cliques. gttic(removetop); Cliques orphans; - BayesNet affectedBayesNet; - this->removeTop(markedKeys, affectedBayesNet, orphans); + GaussianBayesNet affectedBayesNet; + this->removeTop(FastVector(markedKeys.begin(), markedKeys.end()), affectedBayesNet, orphans); gttoc(removetop); - if(debug) affectedBayesNet.print("Removed top: "); - if(debug) orphans.print("Orphans: "); - // FactorGraph factors(affectedBayesNet); // bug was here: we cannot reuse the original factors, because then the cached factors get messed up // [all the necessary data is actually contained in the affectedBayesNet, including what was passed in from the boundaries, @@ -304,78 +321,60 @@ boost::shared_ptr > ISAM2::recalculate(const FastSet& mark // ordering provides all keys in conditionals, there cannot be others because path to root included gttic(affectedKeys); - FastList affectedKeys = affectedBayesNet.ordering(); + FastList affectedKeys; + BOOST_FOREACH(const ConditionalType::shared_ptr& conditional, affectedBayesNet) + affectedKeys.insert(affectedKeys.end(), conditional->beginFrontals(), conditional->endFrontals()); gttoc(affectedKeys); - boost::shared_ptr > affectedKeysSet(new FastSet()); // Will return this result - - if(affectedKeys.size() >= theta_.size() * batchThreshold) { + boost::shared_ptr > affectedKeysSet(new FastSet()); // Will return this result + if(affectedKeys.size() >= theta_.size() * batchThreshold) + { + // Do a batch step - reorder and relinearize all variables gttic(batch); gttic(add_keys); - BOOST_FOREACH(const Ordering::value_type& key_index, ordering_) { affectedKeysSet->insert(key_index.second); } + br::copy(variableIndex_ | br::map_keys, std::inserter(*affectedKeysSet, affectedKeysSet->end())); gttoc(add_keys); - gttic(reorder); - gttic(CCOLAMD); - // Do a batch step - reorder and relinearize all variables - vector cmember(theta_.size(), 0); - if(constrainKeys) { - if(!constrainKeys->empty()) { - typedef std::pair Index_Group; - if(theta_.size() > constrainKeys->size()) { // Only if some variables are unconstrained - BOOST_FOREACH(const Index_Group& index_group, *constrainKeys) { - cmember[index_group.first] = index_group.second; } - } else { - int minGroup = *boost::range::min_element(boost::adaptors::values(*constrainKeys)); - BOOST_FOREACH(const Index_Group& index_group, *constrainKeys) { - cmember[index_group.first] = index_group.second - minGroup; } - } + gttic(ordering); + Ordering order; + if(constrainKeys) + { + order = Ordering::COLAMDConstrained(variableIndex_, *constrainKeys); + } + else + { + if(theta_.size() > observedKeys.size()) + { + // Only if some variables are unconstrained + FastMap constraintGroups; + BOOST_FOREACH(Key var, observedKeys) + constraintGroups[var] = 1; + order = Ordering::COLAMDConstrained(variableIndex_, constraintGroups); } - } else { - if(theta_.size() > observedKeys.size()) { // Only if some variables are unconstrained - BOOST_FOREACH(Index var, observedKeys) { cmember[var] = 1; } + else + { + order = Ordering::COLAMD(variableIndex_); } } - Permutation::shared_ptr colamd(inference::PermutationCOLAMD_(variableIndex_, cmember)); - Permutation::shared_ptr colamdInverse(colamd->inverse()); - gttoc(CCOLAMD); - - // Reorder - gttic(permute_global_variable_index); - variableIndex_.permuteInPlace(*colamd); - gttoc(permute_global_variable_index); - gttic(permute_delta); - delta_.permuteInPlace(*colamd); - deltaNewton_.permuteInPlace(*colamd); - RgProd_.permuteInPlace(*colamd); - gttoc(permute_delta); - gttic(permute_ordering); - ordering_.permuteInPlace(*colamd); - gttoc(permute_ordering); - gttoc(reorder); + gttoc(ordering); gttic(linearize); - GaussianFactorGraph linearized = *nonlinearFactors_.linearize(theta_, ordering_); + GaussianFactorGraph linearized = *nonlinearFactors_.linearize(theta_); if(params_.cacheLinearizedFactors) linearFactors_ = linearized; gttoc(linearize); gttic(eliminate); - JunctionTree jt(linearized, variableIndex_); - sharedClique newRoot; - if(params_.factorization == ISAM2Params::CHOLESKY) - newRoot = jt.eliminate(EliminatePreferCholesky); - else if(params_.factorization == ISAM2Params::QR) - newRoot = jt.eliminate(EliminateQR); - else assert(false); - if(debug) newRoot->print("Eliminated: "); + ISAM2BayesTree::shared_ptr bayesTree = ISAM2JunctionTree(GaussianEliminationTree(linearized, variableIndex_, order)) + .eliminate(params_.getEliminationFunction()).first; gttoc(eliminate); gttic(insert); this->clear(); - this->insert(newRoot); + this->roots_.insert(this->roots_.end(), bayesTree->roots().begin(), bayesTree->roots().end()); + this->nodes_.insert(bayesTree->nodes().begin(), bayesTree->nodes().end()); gttoc(insert); result.variablesReeliminated = affectedKeysSet->size(); @@ -399,7 +398,7 @@ boost::shared_ptr > ISAM2::recalculate(const FastSet& mark gttic(incremental); // 2. Add the new factors \Factors' into the resulting factor graph - FastList affectedAndNewKeys; + FastList affectedAndNewKeys; affectedAndNewKeys.insert(affectedAndNewKeys.end(), affectedKeys.begin(), affectedKeys.end()); affectedAndNewKeys.insert(affectedAndNewKeys.end(), observedKeys.begin(), observedKeys.end()); gttic(relinearizeAffected); @@ -407,12 +406,12 @@ boost::shared_ptr > ISAM2::recalculate(const FastSet& mark if(debug) factors.print("Relinearized factors: "); gttoc(relinearizeAffected); - if(debug) { cout << "Affected keys: "; BOOST_FOREACH(const Index key, affectedKeys) { cout << key << " "; } cout << endl; } + if(debug) { cout << "Affected keys: "; BOOST_FOREACH(const Key key, affectedKeys) { cout << key << " "; } cout << endl; } // Reeliminated keys for detailed results if(params_.enableDetailedResults) { - BOOST_FOREACH(Index index, affectedAndNewKeys) { - result.detail->variableStatus[ordering_.key(index)].isReeliminated = true; + BOOST_FOREACH(Key key, affectedAndNewKeys) { + result.detail->variableStatus[key].isReeliminated = true; } } @@ -436,6 +435,13 @@ boost::shared_ptr > ISAM2::recalculate(const FastSet& mark factors.push_back(cachedBoundary); gttoc(cached); + gttic(orphans); + // Add the orphaned subtrees + BOOST_FOREACH(const sharedClique& orphan, orphans) + factors += boost::make_shared >(orphan); + gttoc(orphans); + + // END OF COPIED CODE // 3. Re-order and eliminate the factor graph into a Bayes net (Algorithm [alg:eliminate]), and re-assemble into a new Bayes tree (Algorithm [alg:BayesTree]) @@ -449,98 +455,55 @@ boost::shared_ptr > ISAM2::recalculate(const FastSet& mark affectedKeysSet->insert(affectedKeys.begin(), affectedKeys.end()); gttoc(list_to_set); - gttic(PartialSolve); - Impl::ReorderingMode reorderingMode; - reorderingMode.nFullSystemVars = ordering_.size(); - reorderingMode.algorithm = Impl::ReorderingMode::COLAMD; - reorderingMode.constrain = Impl::ReorderingMode::CONSTRAIN_LAST; - if(constrainKeys) { - reorderingMode.constrainedKeys = *constrainKeys; - } else { - reorderingMode.constrainedKeys = FastMap(); - BOOST_FOREACH(Index var, observedKeys) { reorderingMode.constrainedKeys->insert(make_pair(var, 1)); } - } - FastSet affectedUsedKeys = *affectedKeysSet; // Remove unused keys from the set we pass to PartialSolve - BOOST_FOREACH(Index unused, unusedIndices) { - affectedUsedKeys.erase(unused); - } - // Remove unaffected keys from the constraints - FastMap::iterator iter = reorderingMode.constrainedKeys->begin(); - while(iter != reorderingMode.constrainedKeys->end()) { - if(affectedUsedKeys.find(iter->first) == affectedUsedKeys.end()) { - reorderingMode.constrainedKeys->erase(iter++); - } else { - ++iter; - } - } - Impl::PartialSolveResult partialSolveResult = - Impl::PartialSolve(factors, affectedUsedKeys, reorderingMode, (params_.factorization == ISAM2Params::QR)); - gttoc(PartialSolve); + VariableIndex affectedFactorsVarIndex(factors); - // We now need to permute everything according this partial reordering: the - // delta vector, the global ordering, and the factors we're about to - // re-eliminate. The reordered variables are also mentioned in the - // orphans and the leftover cached factors. - gttic(permute_global_variable_index); - variableIndex_.permuteInPlace(partialSolveResult.reorderingSelector, partialSolveResult.reorderingPermutation); - gttoc(permute_global_variable_index); - gttic(permute_delta); - delta_.permuteInPlace(partialSolveResult.reorderingSelector, partialSolveResult.reorderingPermutation); - deltaNewton_.permuteInPlace(partialSolveResult.reorderingSelector, partialSolveResult.reorderingPermutation); - RgProd_.permuteInPlace(partialSolveResult.reorderingSelector, partialSolveResult.reorderingPermutation); - gttoc(permute_delta); - gttic(permute_ordering); - ordering_.permuteInPlace(partialSolveResult.reorderingSelector, partialSolveResult.reorderingPermutation); - gttoc(permute_ordering); - if(params_.cacheLinearizedFactors) { - gttic(permute_cached_linear); - //linearFactors_.permuteWithInverse(partialSolveResult.fullReorderingInverse); - FastList permuteLinearIndices = getAffectedFactors(affectedAndNewKeys); - BOOST_FOREACH(size_t idx, permuteLinearIndices) { - linearFactors_[idx]->reduceWithInverse(partialSolveResult.reorderingInverse); - } - gttoc(permute_cached_linear); + gttic(ordering_constraints); + // Create ordering constraints + FastMap constraintGroups; + if(constrainKeys) { + constraintGroups = *constrainKeys; + } else { + constraintGroups = FastMap(); + const int group = observedKeys.size() < affectedFactorsVarIndex.size() + ? 1 : 0; + BOOST_FOREACH(Key var, observedKeys) + constraintGroups.insert(make_pair(var, group)); } + // Remove unaffected keys from the constraints + for(FastMap::iterator iter = constraintGroups.begin(); iter != constraintGroups.end(); /*Incremented in loop ++iter*/) { + if(unusedIndices.exists(iter->first) || !affectedKeysSet->exists(iter->first)) + constraintGroups.erase(iter ++); + else + ++ iter; + } + gttoc(ordering_constraints); + + // Generate ordering + gttic(Ordering); + Ordering ordering = Ordering::COLAMDConstrained(affectedFactorsVarIndex, constraintGroups); + gttoc(Ordering); + + ISAM2BayesTree::shared_ptr bayesTree = ISAM2JunctionTree(GaussianEliminationTree( + factors, affectedFactorsVarIndex, ordering)).eliminate(params_.getEliminationFunction()).first; + gttoc(reorder_and_eliminate); gttic(reassemble); - if(partialSolveResult.bayesTree) { - assert(!this->root_); - this->insert(partialSolveResult.bayesTree); - } + this->roots_.insert(this->roots_.end(), bayesTree->roots().begin(), bayesTree->roots().end()); + this->nodes_.insert(bayesTree->nodes().begin(), bayesTree->nodes().end()); gttoc(reassemble); - // 4. Insert the orphans back into the new Bayes tree. - gttic(orphans); - gttic(permute); - BOOST_FOREACH(sharedClique orphan, orphans) { - (void)orphan->reduceSeparatorWithInverse(partialSolveResult.reorderingInverse); - } - gttoc(permute); - gttic(insert); - // add orphans to the bottom of the new tree - BOOST_FOREACH(sharedClique orphan, orphans) { - // Because the affectedKeysSelector is sorted, the orphan separator keys - // will be sorted correctly according to the new elimination order after - // applying the permutation, so findParentClique, which looks for the - // lowest-ordered parent, will still work. - Index parentRepresentative = Base::findParentClique((*orphan)->parents()); - sharedClique parent = (*this)[parentRepresentative]; - parent->children_ += orphan; - orphan->parent_ = parent; // set new parent! - } - gttoc(insert); - gttoc(orphans); + // 4. The orphans have already been inserted during elimination gttoc(incremental); } // Root clique variables for detailed results if(params_.enableDetailedResults) { - BOOST_FOREACH(Index index, this->root()->conditional()->frontals()) { - result.detail->variableStatus[ordering_.key(index)].inRootClique = true; - } + BOOST_FOREACH(const sharedNode& root, this->roots()) + BOOST_FOREACH(Key var, *root->conditional()) + result.detail->variableStatus[var].inRootClique = true; } return affectedKeysSet; @@ -548,15 +511,17 @@ boost::shared_ptr > ISAM2::recalculate(const FastSet& mark /* ************************************************************************* */ ISAM2Result ISAM2::update( - const NonlinearFactorGraph& newFactors, const Values& newTheta, const FastVector& removeFactorIndices, + const NonlinearFactorGraph& newFactors, const Values& newTheta, const vector& removeFactorIndices, const boost::optional >& constrainedKeys, const boost::optional >& noRelinKeys, - const boost::optional >& extraReelimKeys, bool force_relinearize) { + const boost::optional >& extraReelimKeys, bool force_relinearize) +{ const bool debug = ISDEBUG("ISAM2 update"); const bool verbose = ISDEBUG("ISAM2 update verbose"); - static int count = 0; - count++; + gttic(ISAM2_update); + + this->update_count_++; lastAffectedVariableCount = 0; lastAffectedFactorCount = 0; @@ -567,7 +532,8 @@ ISAM2Result ISAM2::update( ISAM2Result result; if(params_.enableDetailedResults) result.detail = ISAM2Result::DetailedResults(); - const bool relinearizeThisStep = force_relinearize || (params_.enableRelinearization && count % params_.relinearizeSkip == 0); + const bool relinearizeThisStep = force_relinearize + || (params_.enableRelinearization && update_count_ % params_.relinearizeSkip == 0); if(verbose) { cout << "ISAM2::update\n"; @@ -582,14 +548,10 @@ ISAM2Result ISAM2::update( } gttic(push_back_factors); - // Add the new factor indices to the result struct - result.newFactorsIndices.resize(newFactors.size()); - for(size_t i=0; i unusedKeys; - FastSet unusedIndices; + FastSet unusedIndices; { // Get keys from removed factors and new factors, and compute unused keys, // i.e., keys that are empty now and do not appear in the new factors. FastSet removedAndEmpty; BOOST_FOREACH(Key key, removeFactors.keys()) { - if(variableIndex_[ordering_[key]].empty()) + if(variableIndex_[key].empty()) removedAndEmpty.insert(removedAndEmpty.end(), key); } FastSet newFactorSymbKeys = newFactors.keys(); @@ -620,14 +582,14 @@ ISAM2Result ISAM2::update( // Get indices for unused keys BOOST_FOREACH(Key key, unusedKeys) { - unusedIndices.insert(unusedIndices.end(), ordering_[key]); + unusedIndices.insert(unusedIndices.end(), key); } } gttoc(push_back_factors); gttic(add_new_variables); // 2. Initialize any new variables \Theta_{new} and add \Theta:=\Theta\cup\Theta_{new}. - Impl::AddVariables(newTheta, theta_, delta_, deltaNewton_, RgProd_, deltaReplacedMask_, ordering_); + Impl::AddVariables(newTheta, theta_, delta_, deltaNewton_, RgProd_); // New keys for detailed results if(params_.enableDetailedResults) { BOOST_FOREACH(Key key, newTheta.keys()) { result.detail->variableStatus[key].isNew = true; } } @@ -640,82 +602,85 @@ ISAM2Result ISAM2::update( gttic(gather_involved_keys); // 3. Mark linear update - FastSet markedKeys = Impl::IndicesFromFactors(ordering_, newFactors); // Get keys from new factors + FastSet markedKeys = newFactors.keys(); // Get keys from new factors // Also mark keys involved in removed factors { - FastSet markedRemoveKeys = Impl::IndicesFromFactors(ordering_, removeFactors); // Get keys involved in removed factors + FastSet markedRemoveKeys = removeFactors.keys(); // Get keys involved in removed factors markedKeys.insert(markedRemoveKeys.begin(), markedRemoveKeys.end()); // Add to the overall set of marked keys } // Also mark any provided extra re-eliminate keys if(extraReelimKeys) { BOOST_FOREACH(Key key, *extraReelimKeys) { - markedKeys.insert(ordering_.at(key)); + markedKeys.insert(key); } } // Observed keys for detailed results if(params_.enableDetailedResults) { - BOOST_FOREACH(Index index, markedKeys) { - result.detail->variableStatus[ordering_.key(index)].isObserved = true; + BOOST_FOREACH(Key key, markedKeys) { + result.detail->variableStatus[key].isObserved = true; } } // NOTE: we use assign instead of the iterator constructor here because this // is a vector of size_t, so the constructor unintentionally resolves to - // vector(size_t count, Index value) instead of the iterator constructor. - FastVector observedKeys; observedKeys.reserve(markedKeys.size()); - BOOST_FOREACH(Index index, markedKeys) { + // vector(size_t count, Key value) instead of the iterator constructor. + FastVector observedKeys; observedKeys.reserve(markedKeys.size()); + BOOST_FOREACH(Key index, markedKeys) { if(unusedIndices.find(index) == unusedIndices.end()) // Only add if not unused observedKeys.push_back(index); // Make a copy of these, as we'll soon add to them } gttoc(gather_involved_keys); // Check relinearization if we're at the nth step, or we are using a looser loop relin threshold - FastSet relinKeys; + FastSet relinKeys; if (relinearizeThisStep) { gttic(gather_relinearize_keys); // 4. Mark keys in \Delta above threshold \beta: J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}. if(params_.enablePartialRelinearizationCheck) - relinKeys = Impl::CheckRelinearizationPartial(root_, delta_, ordering_, params_.relinearizeThreshold); + relinKeys = Impl::CheckRelinearizationPartial(roots_, delta_, params_.relinearizeThreshold); else - relinKeys = Impl::CheckRelinearizationFull(delta_, ordering_, params_.relinearizeThreshold); - if(disableReordering) relinKeys = Impl::CheckRelinearizationFull(delta_, ordering_, 0.0); // This is used for debugging + relinKeys = Impl::CheckRelinearizationFull(delta_, params_.relinearizeThreshold); + if(disableReordering) relinKeys = Impl::CheckRelinearizationFull(delta_, 0.0); // This is used for debugging // Remove from relinKeys any keys whose linearization points are fixed BOOST_FOREACH(Key key, fixedVariables_) { - relinKeys.erase(ordering_[key]); + relinKeys.erase(key); } if(noRelinKeys) { BOOST_FOREACH(Key key, *noRelinKeys) { - relinKeys.erase(ordering_[key]); + relinKeys.erase(key); } } // Above relin threshold keys for detailed results if(params_.enableDetailedResults) { - BOOST_FOREACH(Index index, relinKeys) { - result.detail->variableStatus[ordering_.key(index)].isAboveRelinThreshold = true; - result.detail->variableStatus[ordering_.key(index)].isRelinearized = true; } } + BOOST_FOREACH(Key key, relinKeys) { + result.detail->variableStatus[key].isAboveRelinThreshold = true; + result.detail->variableStatus[key].isRelinearized = true; } } // Add the variables being relinearized to the marked keys - vector markedRelinMask(ordering_.size(), false); - BOOST_FOREACH(const Index j, relinKeys) { markedRelinMask[j] = true; } + FastSet markedRelinMask; + BOOST_FOREACH(const Key key, relinKeys) + markedRelinMask.insert(key); markedKeys.insert(relinKeys.begin(), relinKeys.end()); gttoc(gather_relinearize_keys); gttic(fluid_find_all); // 5. Mark all cliques that involve marked variables \Theta_{J} and all their ancestors. - if (!relinKeys.empty() && this->root()) { - // add other cliques that have the marked ones in the separator - Impl::FindAll(this->root(), markedKeys, markedRelinMask); + if (!relinKeys.empty()) { + BOOST_FOREACH(const sharedClique& root, roots_) + // add other cliques that have the marked ones in the separator + Impl::FindAll(root, markedKeys, markedRelinMask); // Relin involved keys for detailed results if(params_.enableDetailedResults) { - FastSet involvedRelinKeys; - Impl::FindAll(this->root(), involvedRelinKeys, markedRelinMask); - BOOST_FOREACH(Index index, involvedRelinKeys) { - if(!result.detail->variableStatus[ordering_.key(index)].isAboveRelinThreshold) { - result.detail->variableStatus[ordering_.key(index)].isRelinearizeInvolved = true; - result.detail->variableStatus[ordering_.key(index)].isRelinearized = true; } } + FastSet involvedRelinKeys; + BOOST_FOREACH(const sharedClique& root, roots_) + Impl::FindAll(root, involvedRelinKeys, markedRelinMask); + BOOST_FOREACH(Key key, involvedRelinKeys) { + if(!result.detail->variableStatus[key].isAboveRelinThreshold) { + result.detail->variableStatus[key].isRelinearizeInvolved = true; + result.detail->variableStatus[key].isRelinearized = true; } } } } gttoc(fluid_find_all); @@ -723,7 +688,7 @@ ISAM2Result ISAM2::update( gttic(expmap); // 6. Update linearization point for marked variables: \Theta_{J}:=\Theta_{J}+\Delta_{J}. if (!relinKeys.empty()) - Impl::ExpmapMasked(theta_, delta_, ordering_, markedRelinMask, delta_); + Impl::ExpmapMasked(theta_, delta_, markedRelinMask, delta_); gttoc(expmap); result.variablesRelinearized = markedKeys.size(); @@ -735,53 +700,49 @@ ISAM2Result ISAM2::update( // 7. Linearize new factors if(params_.cacheLinearizedFactors) { gttic(linearize); - FactorGraph::shared_ptr linearFactors = newFactors.linearize(theta_, ordering_); - linearFactors_.push_back(*linearFactors); + GaussianFactorGraph::shared_ptr linearFactors = newFactors.linearize(theta_); + if(params_.findUnusedFactorSlots) + { + linearFactors_.resize(nonlinearFactors_.size()); + for(size_t newFactorI = 0; newFactorI < newFactors.size(); ++newFactorI) + linearFactors_[result.newFactorsIndices[newFactorI]] = (*linearFactors)[newFactorI]; + } + else + { + linearFactors_.push_back(*linearFactors); + } assert(nonlinearFactors_.size() == linearFactors_.size()); gttoc(linearize); - - gttic(augment_VI); - // Augment the variable index with the new factors - variableIndex_.augment(*linearFactors); - gttoc(augment_VI); - } else { - variableIndex_.augment(*newFactors.symbolic(ordering_)); } gttoc(linearize_new); + gttic(augment_VI); + // Augment the variable index with the new factors + if(params_.findUnusedFactorSlots) + variableIndex_.augment(newFactors, result.newFactorsIndices); + else + variableIndex_.augment(newFactors); + gttoc(augment_VI); + gttic(recalculate); // 8. Redo top of Bayes tree - // Convert constrained symbols to indices - boost::optional > constrainedIndices; - if(constrainedKeys) { - constrainedIndices = FastMap(); - typedef pair Key_Group; - BOOST_FOREACH(Key_Group key_group, *constrainedKeys) { - constrainedIndices->insert(make_pair(ordering_[key_group.first], key_group.second)); - } - } - boost::shared_ptr > replacedKeys; + boost::shared_ptr > replacedKeys; if(!markedKeys.empty() || !observedKeys.empty()) - replacedKeys = recalculate(markedKeys, relinKeys, observedKeys, unusedIndices, constrainedIndices, result); + replacedKeys = recalculate(markedKeys, relinKeys, observedKeys, unusedIndices, constrainedKeys, result); // Update replaced keys mask (accumulates until back-substitution takes place) - if(replacedKeys) { - BOOST_FOREACH(const Index var, *replacedKeys) { - deltaReplacedMask_[var] = true; } } + if(replacedKeys) + deltaReplacedMask_.insert(replacedKeys->begin(), replacedKeys->end()); gttoc(recalculate); - // After the top of the tree has been redone and may have index gaps from - // unused keys, condense the indices to remove gaps by rearranging indices - // in all data structures. + // Update data structures to remove unused keys if(!unusedKeys.empty()) { gttic(remove_variables); - Impl::RemoveVariables(unusedKeys, root_, theta_, variableIndex_, delta_, deltaNewton_, RgProd_, - deltaReplacedMask_, ordering_, Base::nodes_, linearFactors_, fixedVariables_); + Impl::RemoveVariables(unusedKeys, roots_, theta_, variableIndex_, delta_, deltaNewton_, RgProd_, + deltaReplacedMask_, Base::nodes_, fixedVariables_); gttoc(remove_variables); } result.cliques = this->nodes().size(); - deltaDoglegUptodate_ = false; - deltaUptodate_ = false; gttic(evaluate_error_after); if(params_.evaluateNonlinearError) @@ -792,28 +753,45 @@ ISAM2Result ISAM2::update( } /* ************************************************************************* */ -void ISAM2::marginalizeLeaves(const FastList& leafKeys, boost::optional&> marginalFactorsIndices, - boost::optional&> deletedFactorsIndices) +void ISAM2::marginalizeLeaves(const FastList& leafKeysList, + boost::optional&> marginalFactorsIndices, + boost::optional&> deletedFactorsIndices) { - // Convert set of keys into a set of indices - FastSet indices; - BOOST_FOREACH(Key key, leafKeys) { - indices.insert(ordering_[key]); - } + // Convert to ordered set + FastSet leafKeys(leafKeysList.begin(), leafKeysList.end()); // Keep track of marginal factors - map from clique to the marginal factors // that should be incorporated into it, passed up from it's children. - multimap marginalFactors; +// multimap marginalFactors; + map > marginalFactors; + + // Keep track of factors that get summarized by removing cliques + FastSet factorIndicesToRemove; + + // Keep track of variables removed in subtrees + FastSet leafKeysRemoved; // Remove each variable and its subtrees - BOOST_REVERSE_FOREACH(Index j, indices) { - if(nodes_[j]) { // If the index was not already removed by removing another subtree + BOOST_FOREACH(Key j, leafKeys) { + if(!leafKeysRemoved.exists(j)) { // If the index was not already removed by removing another subtree + + // Traverse up the tree to find the root of the marginalized subtree sharedClique clique = nodes_[j]; + while(!clique->parent_._empty()) + { + // Check if parent contains a marginalized leaf variable. Only need to check the first + // variable because it is the closest to the leaves. + sharedClique parent = clique->parent(); + if(leafKeys.exists(parent->conditional()->front())) + clique = parent; + else + break; + } // See if we should remove the whole clique bool marginalizeEntireClique = true; - BOOST_FOREACH(Index frontal, clique->conditional()->frontals()) { - if(indices.find(frontal) == indices.end()) { + BOOST_FOREACH(Key frontal, clique->conditional()->frontals()) { + if(!leafKeys.exists(frontal)) { marginalizeEntireClique = false; break; } } @@ -825,15 +803,23 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeys, boost::optionalparent(), marginalFactor)); + marginalFactors[clique->parent()->conditional()->front()].push_back(marginalFactor); // Now remove this clique and its subtree - all of its marginal // information has been stored in marginalFactors. const Cliques removedCliques = this->removeSubtree(clique); // Remove the subtree and throw away the cliques BOOST_FOREACH(const sharedClique& removedClique, removedCliques) { - marginalFactors.erase(removedClique); - BOOST_FOREACH(Index indexInClique, removedClique->conditional()->frontals()) { - if(indices.find(indexInClique) == indices.end()) - throw runtime_error("Requesting to marginalize variables that are not leaves, the ISAM2 object is now in an inconsistent state so should no longer be used."); } + marginalFactors.erase(removedClique->conditional()->front()); + leafKeysRemoved.insert(removedClique->conditional()->beginFrontals(), removedClique->conditional()->endFrontals()); + BOOST_FOREACH(Key frontal, removedClique->conditional()->frontals()) + { + // Add to factors to remove + const VariableIndex::Factors& involvedFactors = variableIndex_[frontal]; + factorIndicesToRemove.insert(involvedFactors.begin(), involvedFactors.end()); + + // Check for non-leaf keys + if(!leafKeys.exists(frontal)) + throw runtime_error("Requesting to marginalize variables that are not leaves, the ISAM2 object is now in an inconsistent state so should no longer be used."); + } } } else { @@ -847,10 +833,10 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeys, boost::optional factorsInSubtreeRoot; Cliques subtreesToRemove; - BOOST_FOREACH(const sharedClique& child, clique->children()) { + BOOST_FOREACH(const sharedClique& child, clique->children) { // Remove subtree if child depends on any marginalized keys - BOOST_FOREACH(Index parentIndex, child->conditional()->parents()) { - if(indices.find(parentIndex) != indices.end()) { + BOOST_FOREACH(Key parent, child->conditional()->parents()) { + if(leafKeys.exists(parent)) { subtreesToRemove.push_back(child); graph.push_back(child->cachedFactor()); // Add child marginal break; @@ -862,72 +848,75 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeys, boost::optionalremoveSubtree(childToRemove); // Remove the subtree and throw away the cliques childrenRemoved.insert(childrenRemoved.end(), removedCliques.begin(), removedCliques.end()); BOOST_FOREACH(const sharedClique& removedClique, removedCliques) { - marginalFactors.erase(removedClique); - BOOST_FOREACH(Index indexInClique, removedClique->conditional()->frontals()) { - if(indices.find(indexInClique) == indices.end()) - throw runtime_error("Requesting to marginalize variables that are not leaves, the ISAM2 object is now in an inconsistent state so should no longer be used."); } + marginalFactors.erase(removedClique->conditional()->front()); + leafKeysRemoved.insert(removedClique->conditional()->beginFrontals(), removedClique->conditional()->endFrontals()); + BOOST_FOREACH(Key frontal, removedClique->conditional()->frontals()) + { + // Add to factors to remove + const VariableIndex::Factors& involvedFactors = variableIndex_[frontal]; + factorIndicesToRemove.insert(involvedFactors.begin(), involvedFactors.end()); + + // Check for non-leaf keys + if(!leafKeys.exists(frontal)) + throw runtime_error("Requesting to marginalize variables that are not leaves, the ISAM2 object is now in an inconsistent state so should no longer be used."); + } } } - // Gather remaining children after we removed marginalized subtrees - vector orphans(clique->children().begin(), clique->children().end()); - // Add the factors that are pulled into the current clique by the marginalized variables. // These are the factors that involve *marginalized* frontal variables in this clique // but do not involve frontal variables of any of its children. - FastSet factorsFromMarginalizedInClique; - BOOST_FOREACH(Index indexInClique, clique->conditional()->frontals()) { - if(indices.find(indexInClique) != indices.end()) - factorsFromMarginalizedInClique.insert(variableIndex_[indexInClique].begin(), variableIndex_[indexInClique].end()); } + // TODO: reuse cached linear factors + FastSet factorsFromMarginalizedInClique_step1; + BOOST_FOREACH(Key frontal, clique->conditional()->frontals()) { + if(leafKeys.exists(frontal)) + factorsFromMarginalizedInClique_step1.insert(variableIndex_[frontal].begin(), variableIndex_[frontal].end()); } + // Remove any factors in subtrees that we're removing at this step BOOST_FOREACH(const sharedClique& removedChild, childrenRemoved) { - BOOST_FOREACH(Index indexInClique, removedChild->conditional()->frontals()) { + BOOST_FOREACH(Key indexInClique, removedChild->conditional()->frontals()) { BOOST_FOREACH(size_t factorInvolving, variableIndex_[indexInClique]) { - factorsFromMarginalizedInClique.erase(factorInvolving); } } } - BOOST_FOREACH(size_t i, factorsFromMarginalizedInClique) { - graph.push_back(nonlinearFactors_[i]->linearize(theta_, ordering_)); } - - // Remove the current clique - sharedClique parent = clique->parent(); - this->removeClique(clique); + factorsFromMarginalizedInClique_step1.erase(factorInvolving); } } } + // Create factor graph from factor indices + BOOST_FOREACH(size_t i, factorsFromMarginalizedInClique_step1) { + graph.push_back(nonlinearFactors_[i]->linearize(theta_)); } // Reeliminate the linear graph to get the marginal and discard the conditional - const FastSet cliqueFrontals(clique->conditional()->beginFrontals(), clique->conditional()->endFrontals()); - FastSet cliqueFrontalsToEliminate; - std::set_intersection(cliqueFrontals.begin(), cliqueFrontals.end(), indices.begin(), indices.end(), - std::inserter(cliqueFrontalsToEliminate, cliqueFrontalsToEliminate.end())); - vector cliqueFrontalsToEliminateV(cliqueFrontalsToEliminate.begin(), cliqueFrontalsToEliminate.end()); - pair eliminationResult1 = - graph.eliminate(cliqueFrontalsToEliminateV, - params_.factorization==ISAM2Params::QR ? EliminateQR : EliminatePreferCholesky); + const FastSet cliqueFrontals(clique->conditional()->beginFrontals(), clique->conditional()->endFrontals()); + FastVector cliqueFrontalsToEliminate; + std::set_intersection(cliqueFrontals.begin(), cliqueFrontals.end(), leafKeys.begin(), leafKeys.end(), + std::back_inserter(cliqueFrontalsToEliminate)); + pair eliminationResult1 = + params_.getEliminationFunction()(graph, Ordering(cliqueFrontalsToEliminate)); // Add the resulting marginal - BOOST_FOREACH(const GaussianFactor::shared_ptr& marginal, eliminationResult1.second) { - if(marginal) - marginalFactors.insert(make_pair(clique, marginal)); } + if(eliminationResult1.second) + marginalFactors[clique->conditional()->front()].push_back(eliminationResult1.second); - // Recover the conditional on the remaining subset of frontal variables - // of this clique being martially marginalized. - size_t nToEliminate = std::find(clique->conditional()->beginFrontals(), clique->conditional()->endFrontals(), j) - clique->conditional()->begin() + 1; - GaussianFactorGraph graph2; - graph2.push_back(clique->conditional()->toFactor()); - GaussianFactorGraph::EliminationResult eliminationResult2 = - params_.factorization == ISAM2Params::QR ? - EliminateQR(graph2, nToEliminate) : - EliminatePreferCholesky(graph2, nToEliminate); - GaussianFactorGraph graph3; - graph3.push_back(eliminationResult2.second); - GaussianFactorGraph::EliminationResult eliminationResult3 = - params_.factorization == ISAM2Params::QR ? - EliminateQR(graph3, clique->conditional()->nrFrontals() - nToEliminate) : - EliminatePreferCholesky(graph3, clique->conditional()->nrFrontals() - nToEliminate); - sharedClique newClique = boost::make_shared(make_pair(eliminationResult3.first, clique->cachedFactor())); + // Split the current clique + // Find the position of the last leaf key in this clique + DenseIndex nToRemove = 0; + while(leafKeys.exists(clique->conditional()->keys()[nToRemove])) + ++ nToRemove; - // Add the marginalized clique to the BayesTree - this->addClique(newClique, parent); + // Make the clique's matrix appear as a subset + const DenseIndex dimToRemove = clique->conditional()->matrixObject().offset(nToRemove); + clique->conditional()->matrixObject().firstBlock() = nToRemove; + clique->conditional()->matrixObject().rowStart() = dimToRemove; - // Add the orphans - BOOST_FOREACH(const sharedClique& orphan, orphans) { - this->addClique(orphan, newClique); } + // Change the keys in the clique + FastVector originalKeys; originalKeys.swap(clique->conditional()->keys()); + clique->conditional()->keys().assign(originalKeys.begin() + nToRemove, originalKeys.end()); + clique->conditional()->nrFrontals() -= nToRemove; + + // Add to factors to remove factors involved in frontals of current clique + BOOST_FOREACH(Key frontal, cliqueFrontalsToEliminate) + { + const VariableIndex::Factors& involvedFactors = variableIndex_[frontal]; + factorIndicesToRemove.insert(involvedFactors.begin(), involvedFactors.end()); + } + + // Add removed keys + leafKeysRemoved.insert(cliqueFrontalsToEliminate.begin(), cliqueFrontalsToEliminate.end()); } } } @@ -936,65 +925,83 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeys, boost::optional Clique_Factor; - BOOST_FOREACH(const Clique_Factor& clique_factor, marginalFactors) { - if(clique_factor.second) { - factorsToAdd.push_back(clique_factor.second); - if(marginalFactorsIndices) marginalFactorsIndices->push_back(nonlinearFactors_.size()); - nonlinearFactors_.push_back(boost::make_shared( - clique_factor.second, ordering_)); - if(params_.cacheLinearizedFactors) { - linearFactors_.push_back(clique_factor.second); - } - BOOST_FOREACH(Index factorIndex, *clique_factor.second) { - fixedVariables_.insert(ordering_.key(factorIndex)); + typedef pair > Key_Factors; + BOOST_FOREACH(const Key_Factors& key_factors, marginalFactors) { + BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, key_factors.second) { + if(factor) { + factorsToAdd.push_back(factor); + if(marginalFactorsIndices) + marginalFactorsIndices->push_back(nonlinearFactors_.size()); + nonlinearFactors_.push_back(boost::make_shared( + factor)); + if(params_.cacheLinearizedFactors) + linearFactors_.push_back(factor); + BOOST_FOREACH(Key factorKey, *factor) { + fixedVariables_.insert(factorKey); } } } } variableIndex_.augment(factorsToAdd); // Augment the variable index // Remove the factors to remove that have been summarized in the newly-added marginal factors - FastSet factorIndicesToRemove; - BOOST_FOREACH(Index j, indices) { - factorIndicesToRemove.insert(variableIndex_[j].begin(), variableIndex_[j].end()); } - vector removedFactorsIndices; - SymbolicFactorGraph removedFactors; + NonlinearFactorGraph removedFactors; BOOST_FOREACH(size_t i, factorIndicesToRemove) { - if(deletedFactorsIndices) deletedFactorsIndices->push_back(i); - removedFactorsIndices.push_back(i); - removedFactors.push_back(nonlinearFactors_[i]->symbolic(ordering_)); + removedFactors.push_back(nonlinearFactors_[i]); nonlinearFactors_.remove(i); if(params_.cacheLinearizedFactors) linearFactors_.remove(i); } - variableIndex_.remove(removedFactorsIndices, removedFactors); + variableIndex_.remove(factorIndicesToRemove.begin(), factorIndicesToRemove.end(), removedFactors); + + if(deletedFactorsIndices) + deletedFactorsIndices->assign(factorIndicesToRemove.begin(), factorIndicesToRemove.end()); // Remove the marginalized variables - Impl::RemoveVariables(FastSet(leafKeys.begin(), leafKeys.end()), root_, theta_, variableIndex_, delta_, deltaNewton_, RgProd_, - deltaReplacedMask_, ordering_, nodes_, linearFactors_, fixedVariables_); + Impl::RemoveVariables(FastSet(leafKeys.begin(), leafKeys.end()), roots_, theta_, variableIndex_, delta_, deltaNewton_, RgProd_, + deltaReplacedMask_, nodes_, fixedVariables_); } /* ************************************************************************* */ -void ISAM2::updateDelta(bool forceFullSolve) const { - +void ISAM2::updateDelta(bool forceFullSolve) const +{ + gttic(updateDelta); if(params_.optimizationParams.type() == typeid(ISAM2GaussNewtonParams)) { // If using Gauss-Newton, update with wildfireThreshold const ISAM2GaussNewtonParams& gaussNewtonParams = boost::get(params_.optimizationParams); const double effectiveWildfireThreshold = forceFullSolve ? 0.0 : gaussNewtonParams.wildfireThreshold; gttic(Wildfire_update); - lastBacksubVariableCount = Impl::UpdateDelta(this->root(), deltaReplacedMask_, delta_, effectiveWildfireThreshold); + lastBacksubVariableCount = Impl::UpdateGaussNewtonDelta( + roots_, deltaReplacedMask_, delta_, effectiveWildfireThreshold); + deltaReplacedMask_.clear(); gttoc(Wildfire_update); } else if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) { // If using Dogleg, do a Dogleg step const ISAM2DoglegParams& doglegParams = boost::get(params_.optimizationParams); + const double effectiveWildfireThreshold = forceFullSolve ? 0.0 : doglegParams.wildfireThreshold; // Do one Dogleg iteration gttic(Dogleg_Iterate); + + // Compute Newton's method step + gttic(Wildfire_update); + lastBacksubVariableCount = Impl::UpdateGaussNewtonDelta(roots_, deltaReplacedMask_, deltaNewton_, effectiveWildfireThreshold); + gttoc(Wildfire_update); + + // Compute steepest descent step + const VectorValues gradAtZero = this->gradientAtZero(); // Compute gradient + Impl::UpdateRgProd(roots_, deltaReplacedMask_, gradAtZero, RgProd_); // Update RgProd + const VectorValues dx_u = Impl::ComputeGradientSearch(gradAtZero, RgProd_); // Compute gradient search point + + // Clear replaced keys mask because now we've updated deltaNewton_ and RgProd_ + deltaReplacedMask_.clear(); + + // Compute dogleg point DoglegOptimizerImpl::IterationResult doglegResult(DoglegOptimizerImpl::Iterate( - *doglegDelta_, doglegParams.adaptationMode, *this, nonlinearFactors_, theta_, ordering_, nonlinearFactors_.error(theta_), doglegParams.verbose)); + *doglegDelta_, doglegParams.adaptationMode, dx_u, deltaNewton_, *this, nonlinearFactors_, + theta_, nonlinearFactors_.error(theta_), doglegParams.verbose)); gttoc(Dogleg_Iterate); gttic(Copy_dx_d); @@ -1003,162 +1010,78 @@ void ISAM2::updateDelta(bool forceFullSolve) const { delta_ = doglegResult.dx_d; // Copy the VectorValues containing with the linear solution gttoc(Copy_dx_d); } - - deltaUptodate_ = true; } /* ************************************************************************* */ Values ISAM2::calculateEstimate() const { - // We use ExpmapMasked here instead of regular expmap because the former - // handles Permuted - gttic(Copy_Values); - Values ret(theta_); - gttoc(Copy_Values); - gttic(getDelta); + gttic(ISAM2_calculateEstimate); const VectorValues& delta(getDelta()); - gttoc(getDelta); gttic(Expmap); - vector mask(ordering_.size(), true); - Impl::ExpmapMasked(ret, delta, ordering_, mask); + return theta_.retract(delta); gttoc(Expmap); - return ret; } /* ************************************************************************* */ const Value& ISAM2::calculateEstimate(Key key) const { - const Index index = getOrdering()[key]; - const Vector& delta = getDelta()[index]; + const Vector& delta = getDelta()[key]; return *theta_.at(key).retract_(delta); } /* ************************************************************************* */ Values ISAM2::calculateBestEstimate() const { - VectorValues delta(theta_.dims(ordering_)); - internal::optimizeInPlace(this->root(), delta); - return theta_.retract(delta, ordering_); + updateDelta(true); // Force full solve when updating delta_ + return theta_.retract(delta_); } /* ************************************************************************* */ -Matrix ISAM2::marginalCovariance(Index key) const { - return marginalFactor(ordering_[key], - params_.factorization == ISAM2Params::QR ? EliminateQR : EliminatePreferCholesky) - ->information().inverse(); +Matrix ISAM2::marginalCovariance(Key key) const { + return marginalFactor(key, params_.getEliminationFunction())->information().inverse(); } /* ************************************************************************* */ const VectorValues& ISAM2::getDelta() const { - if(!deltaUptodate_) + if(!deltaReplacedMask_.empty()) updateDelta(); return delta_; } /* ************************************************************************* */ -VectorValues optimize(const ISAM2& isam) { - gttic(allocateVectorValues); - VectorValues delta = *allocateVectorValues(isam); - gttoc(allocateVectorValues); - optimizeInPlace(isam, delta); - return delta; -} - -/* ************************************************************************* */ -void optimizeInPlace(const ISAM2& isam, VectorValues& delta) { - // We may need to update the solution calculations - if(!isam.deltaDoglegUptodate_) { - gttic(UpdateDoglegDeltas); - double wildfireThreshold = 0.0; - if(isam.params().optimizationParams.type() == typeid(ISAM2GaussNewtonParams)) - wildfireThreshold = boost::get(isam.params().optimizationParams).wildfireThreshold; - else if(isam.params().optimizationParams.type() == typeid(ISAM2DoglegParams)) - wildfireThreshold = boost::get(isam.params().optimizationParams).wildfireThreshold; - else - assert(false); - ISAM2::Impl::UpdateDoglegDeltas(isam, wildfireThreshold, isam.deltaReplacedMask_, isam.deltaNewton_, isam.RgProd_); - isam.deltaDoglegUptodate_ = true; - gttoc(UpdateDoglegDeltas); - } - - gttic(copy_delta); - delta = isam.deltaNewton_; - gttoc(copy_delta); -} - -/* ************************************************************************* */ -VectorValues optimizeGradientSearch(const ISAM2& isam) { - gttic(Allocate_VectorValues); - VectorValues grad = *allocateVectorValues(isam); - gttoc(Allocate_VectorValues); - - optimizeGradientSearchInPlace(isam, grad); - - return grad; -} - -/* ************************************************************************* */ -void optimizeGradientSearchInPlace(const ISAM2& isam, VectorValues& grad) { - // We may need to update the solution calcaulations - if(!isam.deltaDoglegUptodate_) { - gttic(UpdateDoglegDeltas); - double wildfireThreshold = 0.0; - if(isam.params().optimizationParams.type() == typeid(ISAM2GaussNewtonParams)) - wildfireThreshold = boost::get(isam.params().optimizationParams).wildfireThreshold; - else if(isam.params().optimizationParams.type() == typeid(ISAM2DoglegParams)) - wildfireThreshold = boost::get(isam.params().optimizationParams).wildfireThreshold; - else - assert(false); - ISAM2::Impl::UpdateDoglegDeltas(isam, wildfireThreshold, isam.deltaReplacedMask_, isam.deltaNewton_, isam.RgProd_); - isam.deltaDoglegUptodate_ = true; - gttoc(UpdateDoglegDeltas); - } - - gttic(Compute_Gradient); - // Compute gradient (call gradientAtZero function, which is defined for various linear systems) - gradientAtZero(isam, grad); - double gradientSqNorm = grad.dot(grad); - gttoc(Compute_Gradient); - - gttic(Compute_minimizing_step_size); - // Compute minimizing step size - double RgNormSq = isam.RgProd_.asVector().squaredNorm(); - double step = -gradientSqNorm / RgNormSq; - gttoc(Compute_minimizing_step_size); - - gttic(Compute_point); - // Compute steepest descent point - scal(step, grad); - gttoc(Compute_point); -} - -/* ************************************************************************* */ -VectorValues gradient(const ISAM2& bayesTree, const VectorValues& x0) { - return gradient(FactorGraph(bayesTree), x0); +double ISAM2::error(const VectorValues& x) const +{ + return GaussianFactorGraph(*this).error(x); } /* ************************************************************************* */ static void gradientAtZeroTreeAdder(const boost::shared_ptr& root, VectorValues& g) { // Loop through variables in each clique, adding contributions - int variablePosition = 0; + DenseIndex variablePosition = 0; for(GaussianConditional::const_iterator jit = root->conditional()->begin(); jit != root->conditional()->end(); ++jit) { - const int dim = root->conditional()->dim(jit); - g[*jit] += root->gradientContribution().segment(variablePosition, dim); + const DenseIndex dim = root->conditional()->getDim(jit); + pair pos_ins = + g.tryInsert(*jit, root->gradientContribution().segment(variablePosition, dim)); + if(!pos_ins.second) + pos_ins.first->second += root->gradientContribution().segment(variablePosition, dim); variablePosition += dim; } // Recursively add contributions from children typedef boost::shared_ptr sharedClique; - BOOST_FOREACH(const sharedClique& child, root->children()) { + BOOST_FOREACH(const sharedClique& child, root->children) { gradientAtZeroTreeAdder(child, g); } } /* ************************************************************************* */ -void gradientAtZero(const ISAM2& bayesTree, VectorValues& g) { - // Zero-out gradient - g.setZero(); - +VectorValues ISAM2::gradientAtZero() const +{ + // Create result + VectorValues g; + // Sum up contributions for each clique - if(bayesTree.root()) - gradientAtZeroTreeAdder(bayesTree.root(), g); + BOOST_FOREACH(const ISAM2::sharedClique& root, this->roots()) + gradientAtZeroTreeAdder(root, g); + + return g; } } diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index f3d8a3e20..a98ef851f 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -122,8 +122,8 @@ struct GTSAM_EXPORT ISAM2Params { * entries would be added with: * \code FastMap thresholds; - thresholds['x'] = Vector_(6, 0.1, 0.1, 0.1, 0.5, 0.5, 0.5); // 0.1 rad rotation threshold, 0.5 m translation threshold - thresholds['l'] = Vector_(3, 1.0, 1.0, 1.0); // 1.0 m landmark position threshold + thresholds['x'] = (Vector(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5); // 0.1 rad rotation threshold, 0.5 m translation threshold + thresholds['l'] = (Vector(3) << 1.0, 1.0, 1.0); // 1.0 m landmark position threshold params.relinearizeThreshold = thresholds; \endcode */ @@ -164,6 +164,11 @@ struct GTSAM_EXPORT ISAM2Params { */ bool enablePartialRelinearizationCheck; + /// When you will be removing many factors, e.g. when using ISAM2 as a fixed-lag smoother, enable this option to + /// add factors in the first available factor slots, to avoid accumulating NULL factor slots, at the cost of + /// having to search for slots every time a factor is added. + bool findUnusedFactorSlots; + /** Specify parameters as constructor arguments */ ISAM2Params( OptimizationParams _optimizationParams = ISAM2GaussNewtonParams(), ///< see ISAM2Params::optimizationParams @@ -178,7 +183,8 @@ struct GTSAM_EXPORT ISAM2Params { relinearizeSkip(_relinearizeSkip), enableRelinearization(_enableRelinearization), evaluateNonlinearError(_evaluateNonlinearError), factorization(_factorization), cacheLinearizedFactors(_cacheLinearizedFactors), keyFormatter(_keyFormatter), - enableDetailedResults(false), enablePartialRelinearizationCheck(false) {} + enableDetailedResults(false), enablePartialRelinearizationCheck(false), + findUnusedFactorSlots(false) {} void print(const std::string& str = "") const { std::cout << str << "\n"; @@ -204,6 +210,7 @@ struct GTSAM_EXPORT ISAM2Params { std::cout << "cacheLinearizedFactors: " << cacheLinearizedFactors << "\n"; std::cout << "enableDetailedResults: " << enableDetailedResults << "\n"; std::cout << "enablePartialRelinearizationCheck: " << enablePartialRelinearizationCheck << "\n"; + std::cout << "findUnusedFactorSlots: " << findUnusedFactorSlots << "\n"; std::cout.flush(); } @@ -232,6 +239,12 @@ struct GTSAM_EXPORT ISAM2Params { Factorization factorizationTranslator(const std::string& str) const; std::string factorizationTranslator(const Factorization& value) const; + + GaussianFactorGraph::Eliminate getEliminationFunction() const { + return factorization == CHOLESKY + ? (GaussianFactorGraph::Eliminate)EliminatePreferCholesky + : (GaussianFactorGraph::Eliminate)EliminateQR; + } }; @@ -295,7 +308,7 @@ struct GTSAM_EXPORT ISAM2Result { * factors passed as \c newFactors to ISAM2::update(). These indices may be * used later to refer to the factors in order to remove them. */ - std::vector newFactorsIndices; + FastVector newFactorsIndices; /** A struct holding detailed results, which must be enabled with * ISAM2Params::enableDetailedResults. @@ -342,10 +355,11 @@ struct GTSAM_EXPORT ISAM2Result { * Specialized Clique structure for ISAM2, incorporating caching and gradient contribution * TODO: more documentation */ -class GTSAM_EXPORT ISAM2Clique : public BayesTreeCliqueBase { +class GTSAM_EXPORT ISAM2Clique : public BayesTreeCliqueBase +{ public: typedef ISAM2Clique This; - typedef BayesTreeCliqueBase Base; + typedef BayesTreeCliqueBase Base; typedef boost::shared_ptr shared_ptr; typedef boost::weak_ptr weak_ptr; typedef GaussianConditional ConditionalType; @@ -353,30 +367,26 @@ public: Base::FactorType::shared_ptr cachedFactor_; Vector gradientContribution_; + FastMap solnPointers_; - /** Construct from a conditional */ - ISAM2Clique(const sharedConditional& conditional) : Base(conditional) { - throw std::runtime_error("ISAM2Clique should always be constructed with the elimination result constructor"); } + /// Default constructor + ISAM2Clique() : Base() {} - /** Construct from an elimination result */ - ISAM2Clique(const std::pair >& result) : - Base(result.first), cachedFactor_(result.second), - gradientContribution_(result.first->get_R().cols() + result.first->get_S().cols()) { - // Compute gradient contribution - const ConditionalType& conditional(*result.first); - // Rewrite -(R * P')'*d as -(d' * R * P')' for computational speed reasons - gradientContribution_ << -conditional.get_R().transpose() * conditional.get_d(), - -conditional.get_S().transpose() * conditional.get_d(); + /// Copy constructor, does *not* copy solution pointers as these are invalid in different trees. + ISAM2Clique(const ISAM2Clique& other) : + Base(other), cachedFactor_(other.cachedFactor_), gradientContribution_(other.gradientContribution_) {} + + /// Assignment operator, does *not* copy solution pointers as these are invalid in different trees. + ISAM2Clique& operator=(const ISAM2Clique& other) + { + Base::operator=(other); + cachedFactor_ = other.cachedFactor_; + gradientContribution_ = other.gradientContribution_; + return *this; } - /** Produce a deep copy, copying the cached factor and gradient contribution */ - shared_ptr clone() const { - shared_ptr copy(new ISAM2Clique(std::make_pair( - sharedConditional(new ConditionalType(*Base::conditional_)), - cachedFactor_ ? cachedFactor_->clone() : Base::FactorType::shared_ptr()))); - copy->gradientContribution_ = gradientContribution_; - return copy; - } + /// Overridden to also store the remaining factor and gradient contribution + void setEliminationResult(const FactorGraphType::EliminationResult& eliminationResult); /** Access the cached factor */ Base::FactorType::shared_ptr& cachedFactor() { return cachedFactor_; } @@ -384,35 +394,10 @@ public: /** Access the gradient contribution */ const Vector& gradientContribution() const { return gradientContribution_; } - bool equals(const This& other, double tol=1e-9) const { - return Base::equals(other) && - ((!cachedFactor_ && !other.cachedFactor_) - || (cachedFactor_ && other.cachedFactor_ - && cachedFactor_->equals(*other.cachedFactor_, tol))); - } + bool equals(const This& other, double tol=1e-9) const; /** print this node */ - void print(const std::string& s = "", - const IndexFormatter& formatter = DefaultIndexFormatter) const { - Base::print(s,formatter); - if(cachedFactor_) - cachedFactor_->print(s + "Cached: ", formatter); - else - std::cout << s << "Cached empty" << std::endl; - if(gradientContribution_.rows() != 0) - gtsam::print(gradientContribution_, "Gradient contribution: "); - } - - void permuteWithInverse(const Permutation& inversePermutation) { - if(cachedFactor_) cachedFactor_->permuteWithInverse(inversePermutation); - Base::permuteWithInverse(inversePermutation); - } - - bool reduceSeparatorWithInverse(const internal::Reduction& inverseReduction) { - bool changed = Base::reduceSeparatorWithInverse(inverseReduction); - if(changed) if(cachedFactor_) cachedFactor_->reduceWithInverse(inverseReduction); - return changed; - } + void print(const std::string& s = "", const KeyFormatter& formatter = DefaultKeyFormatter) const; private: @@ -436,7 +421,7 @@ private: * estimate of all variables. * */ -class ISAM2: public BayesTree { +class GTSAM_EXPORT ISAM2: public BayesTree { protected: @@ -454,32 +439,18 @@ protected: */ mutable VectorValues delta_; - mutable VectorValues deltaNewton_; - mutable VectorValues RgProd_; - mutable bool deltaDoglegUptodate_; - - /** Indicates whether the current delta is up-to-date, only used - * internally - delta will always be updated if necessary when it is - * requested with getDelta() or calculateEstimate(). - * - * This is \c mutable because it is used internally to not update delta_ - * until it is needed. - */ - mutable bool deltaUptodate_; + mutable VectorValues deltaNewton_; // Only used when using Dogleg - stores the Gauss-Newton update + mutable VectorValues RgProd_; // Only used when using Dogleg - stores R*g and is updated incrementally /** A cumulative mask for the variables that were replaced and have not yet * been updated in the linear solution delta_, this is only used internally, * delta will always be updated if necessary when requested with getDelta() * or calculateEstimate(). * - * This does not need to be permuted because any change in variable ordering - * that would cause a permutation will also mark variables as needing to be - * updated in this mask. - * * This is \c mutable because it is used internally to not update delta_ * until it is needed. */ - mutable std::vector deltaReplacedMask_; + mutable FastSet deltaReplacedMask_; // TODO: Make sure accessed in the right way /** All original nonlinear factors are stored here to use during relinearization */ NonlinearFactorGraph nonlinearFactors_; @@ -487,10 +458,6 @@ protected: /** The current linear factors, which are only updated as needed */ mutable GaussianFactorGraph linearFactors_; - /** The current elimination ordering Symbols to Index (integer) keys. - * We keep it up to date as we add and reorder variables. */ - Ordering ordering_; - /** The current parameters */ ISAM2Params params_; @@ -501,27 +468,28 @@ protected: * variables and thus cannot have their linearization points changed. */ FastSet fixedVariables_; + int update_count_; ///< Counter incremented every update(), used to determine periodic relinearization + public: typedef ISAM2 This; ///< This class - typedef BayesTree Base; ///< The BayesTree base class - - /** Create an empty ISAM2 instance */ - GTSAM_EXPORT ISAM2(const ISAM2Params& params); - - /** Create an empty ISAM2 instance using the default set of parameters (see ISAM2Params) */ - GTSAM_EXPORT ISAM2(); - - /** Copy constructor */ - GTSAM_EXPORT ISAM2(const ISAM2& other); - - /** Assignment operator */ - GTSAM_EXPORT ISAM2& operator=(const ISAM2& rhs); - + typedef BayesTree Base; ///< The BayesTree base class typedef Base::Clique Clique; ///< A clique typedef Base::sharedClique sharedClique; ///< Shared pointer to a clique typedef Base::Cliques Cliques; ///< List of Clique typedef from base class + /** Create an empty ISAM2 instance */ + ISAM2(const ISAM2Params& params); + + /** Create an empty ISAM2 instance using the default set of parameters (see ISAM2Params) */ + ISAM2(); + + /** default virtual destructor */ + virtual ~ISAM2() {} + + /** Compare equality */ + virtual bool equals(const ISAM2& other, double tol = 1e-9) const; + /** * Add new factors, updating the solution and relinearizing as needed. * @@ -550,8 +518,9 @@ public: * of the size of the linear delta. This allows the provided keys to be reordered. * @return An ISAM2Result struct containing information about the update */ - GTSAM_EXPORT ISAM2Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(), - const FastVector& removeFactorIndices = FastVector(), + virtual ISAM2Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), + const Values& newTheta = Values(), + const std::vector& removeFactorIndices = std::vector(), const boost::optional >& constrainedKeys = boost::none, const boost::optional >& noRelinKeys = boost::none, const boost::optional >& extraReelimKeys = boost::none, @@ -571,9 +540,9 @@ public: * If provided, 'deletedFactorsIndices' will be augmented with the factor graph * indices of any factor that was removed during the 'marginalizeLeaves' call */ - GTSAM_EXPORT void marginalizeLeaves(const FastList& leafKeys, - boost::optional&> marginalFactorsIndices = boost::none, - boost::optional&> deletedFactorsIndices = boost::none); + void marginalizeLeaves(const FastList& leafKeys, + boost::optional&> marginalFactorsIndices = boost::none, + boost::optional&> deletedFactorsIndices = boost::none); /** Access the current linearization point */ const Values& getLinearizationPoint() const { return theta_; } @@ -582,7 +551,7 @@ public: * This delta is incomplete because it was not updated below wildfire_threshold. If only * a single variable is needed, it is faster to call calculateEstimate(const KEY&). */ - GTSAM_EXPORT Values calculateEstimate() const; + Values calculateEstimate() const; /** Compute an estimate for a single variable using its incomplete linear delta computed * during the last update. This is faster than calling the no-argument version of @@ -600,10 +569,10 @@ public: * @param key * @return */ - GTSAM_EXPORT const Value& calculateEstimate(Key key) const; + const Value& calculateEstimate(Key key) const; /** Return marginal on any variable as a covariance matrix */ - GTSAM_EXPORT Matrix marginalCovariance(Index key) const; + Matrix marginalCovariance(Key key) const; /// @name Public members for non-typical usage /// @{ @@ -613,22 +582,22 @@ public: /** Compute an estimate using a complete delta computed by a full back-substitution. */ - GTSAM_EXPORT Values calculateBestEstimate() const; + Values calculateBestEstimate() const; /** Access the current delta, computed during the last call to update */ - GTSAM_EXPORT const VectorValues& getDelta() const; + const VectorValues& getDelta() const; + + /** Compute the linear error */ + double error(const VectorValues& x) const; /** Access the set of nonlinear factors */ - GTSAM_EXPORT const NonlinearFactorGraph& getFactorsUnsafe() const { return nonlinearFactors_; } - - /** Access the current ordering */ - GTSAM_EXPORT const Ordering& getOrdering() const { return ordering_; } + const NonlinearFactorGraph& getFactorsUnsafe() const { return nonlinearFactors_; } /** Access the nonlinear variable index */ - GTSAM_EXPORT const VariableIndex& getVariableIndex() const { return variableIndex_; } + const VariableIndex& getVariableIndex() const { return variableIndex_; } /** Access the nonlinear variable index */ - GTSAM_EXPORT const FastSet& getFixedVariables() const { return fixedVariables_; } + const FastSet& getFixedVariables() const { return fixedVariables_; } size_t lastAffectedVariableCount; size_t lastAffectedFactorCount; @@ -637,35 +606,33 @@ public: mutable size_t lastBacksubVariableCount; size_t lastNnzTop; - GTSAM_EXPORT const ISAM2Params& params() const { return params_; } + const ISAM2Params& params() const { return params_; } /** prints out clique statistics */ - GTSAM_EXPORT void printStats() const { getCliqueData().getStats().print(); } - + void printStats() const { getCliqueData().getStats().print(); } + + /** Compute the gradient of the energy function, \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} R x - d + * \right\Vert^2 \f$, centered around zero. The gradient about zero is \f$ -R^T d \f$. See also + * gradient(const GaussianBayesNet&, const VectorValues&). + * + * @return A VectorValues storing the gradient. + */ + VectorValues gradientAtZero() const; + /// @} -private: +protected: - GTSAM_EXPORT FastList getAffectedFactors(const FastList& keys) const; - GTSAM_EXPORT FactorGraph::shared_ptr relinearizeAffectedFactors(const FastList& affectedKeys, const FastSet& relinKeys) const; - GTSAM_EXPORT GaussianFactorGraph getCachedBoundaryFactors(Cliques& orphans); + FastSet getAffectedFactors(const FastList& keys) const; + GaussianFactorGraph::shared_ptr relinearizeAffectedFactors(const FastList& affectedKeys, const FastSet& relinKeys) const; + GaussianFactorGraph getCachedBoundaryFactors(Cliques& orphans); - GTSAM_EXPORT boost::shared_ptr > recalculate(const FastSet& markedKeys, const FastSet& relinKeys, - const FastVector& observedKeys, const FastSet& unusedIndices, const boost::optional >& constrainKeys, ISAM2Result& result); - // void linear_update(const GaussianFactorGraph& newFactors); - GTSAM_EXPORT void updateDelta(bool forceFullSolve = false) const; - - GTSAM_EXPORT friend void optimizeInPlace(const ISAM2&, VectorValues&); - GTSAM_EXPORT friend void optimizeGradientSearchInPlace(const ISAM2&, VectorValues&); + virtual boost::shared_ptr > recalculate(const FastSet& markedKeys, const FastSet& relinKeys, + const std::vector& observedKeys, const FastSet& unusedIndices, const boost::optional >& constrainKeys, ISAM2Result& result); + void updateDelta(bool forceFullSolve = false) const; }; // ISAM2 -/** Get the linear delta for the ISAM2 object, unpermuted the delta returned by ISAM2::getDelta() */ -GTSAM_EXPORT VectorValues optimize(const ISAM2& isam); - -/** Get the linear delta for the ISAM2 object, unpermuted the delta returned by ISAM2::getDelta() */ -GTSAM_EXPORT void optimizeInPlace(const ISAM2& isam, VectorValues& delta); - /// Optimize the BayesTree, starting from the root. /// @param replaced Needs to contain /// all variables that are contained in the top of the Bayes tree that has been @@ -678,73 +645,17 @@ GTSAM_EXPORT void optimizeInPlace(const ISAM2& isam, VectorValues& delta); /// variables are contained in the subtree. /// @return The number of variables that were solved for template -int optimizeWildfire(const boost::shared_ptr& root, - double threshold, const std::vector& replaced, VectorValues& delta); +size_t optimizeWildfire(const boost::shared_ptr& root, + double threshold, const FastSet& replaced, VectorValues& delta); template -int optimizeWildfireNonRecursive(const boost::shared_ptr& root, - double threshold, const std::vector& replaced, VectorValues& delta); - -/** - * Optimize along the gradient direction, with a closed-form computation to - * perform the line search. The gradient is computed about \f$ \delta x=0 \f$. - * - * This function returns \f$ \delta x \f$ that minimizes a reparametrized - * problem. The error function of a GaussianBayesNet is - * - * \f[ f(\delta x) = \frac{1}{2} |R \delta x - d|^2 = \frac{1}{2}d^T d - d^T R \delta x + \frac{1}{2} \delta x^T R^T R \delta x \f] - * - * with gradient and Hessian - * - * \f[ g(\delta x) = R^T(R\delta x - d), \qquad G(\delta x) = R^T R. \f] - * - * This function performs the line search in the direction of the - * gradient evaluated at \f$ g = g(\delta x = 0) \f$ with step size - * \f$ \alpha \f$ that minimizes \f$ f(\delta x = \alpha g) \f$: - * - * \f[ f(\alpha) = \frac{1}{2} d^T d + g^T \delta x + \frac{1}{2} \alpha^2 g^T G g \f] - * - * Optimizing by setting the derivative to zero yields - * \f$ \hat \alpha = (-g^T g) / (g^T G g) \f$. For efficiency, this function - * evaluates the denominator without computing the Hessian \f$ G \f$, returning - * - * \f[ \delta x = \hat\alpha g = \frac{-g^T g}{(R g)^T(R g)} \f] - */ -GTSAM_EXPORT VectorValues optimizeGradientSearch(const ISAM2& isam); - -/** In-place version of optimizeGradientSearch requiring pre-allocated VectorValues \c x */ -GTSAM_EXPORT void optimizeGradientSearchInPlace(const ISAM2& isam, VectorValues& grad); +size_t optimizeWildfireNonRecursive(const boost::shared_ptr& root, + double threshold, const FastSet& replaced, VectorValues& delta); /// calculate the number of non-zero entries for the tree starting at clique (use root for complete matrix) template int calculate_nnz(const boost::shared_ptr& clique); -/** - * Compute the gradient of the energy function, - * \f$ \nabla_{x=x_0} \left\Vert \Sigma^{-1} R x - d \right\Vert^2 \f$, - * centered around \f$ x = x_0 \f$. - * The gradient is \f$ R^T(Rx-d) \f$. - * This specialized version is used with ISAM2, where each clique stores its - * gradient contribution. - * @param bayesTree The Gaussian Bayes Tree $(R,d)$ - * @param x0 The center about which to compute the gradient - * @return The gradient as a VectorValues - */ -GTSAM_EXPORT VectorValues gradient(const ISAM2& bayesTree, const VectorValues& x0); - -/** - * Compute the gradient of the energy function, - * \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} R x - d \right\Vert^2 \f$, - * centered around zero. - * The gradient about zero is \f$ -R^T d \f$. See also gradient(const GaussianBayesNet&, const VectorValues&). - * This specialized version is used with ISAM2, where each clique stores its - * gradient contribution. - * @param bayesTree The Gaussian Bayes Tree $(R,d)$ - * @param [output] g A VectorValues to store the gradient, which must be preallocated, see allocateVectorValues - * @return The gradient as a VectorValues - */ -GTSAM_EXPORT void gradientAtZero(const ISAM2& bayesTree, VectorValues& g); - } /// namespace gtsam #include diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index 8505834b5..6be69e710 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -13,158 +13,318 @@ * @file LevenbergMarquardtOptimizer.cpp * @brief * @author Richard Roberts + * @author Luca Carlone * @date Feb 26, 2012 */ -#include - -#include #include -#include +#include +#include +#include +#include #include +#include #include +#include +#include using namespace std; namespace gtsam { +using boost::adaptors::map_values; + /* ************************************************************************* */ -LevenbergMarquardtParams::VerbosityLM LevenbergMarquardtParams::verbosityLMTranslator(const std::string &src) const { - std::string s = src; boost::algorithm::to_upper(s); - if (s == "SILENT") return LevenbergMarquardtParams::SILENT; - if (s == "LAMBDA") return LevenbergMarquardtParams::LAMBDA; - if (s == "TRYLAMBDA") return LevenbergMarquardtParams::TRYLAMBDA; - if (s == "TRYCONFIG") return LevenbergMarquardtParams::TRYCONFIG; - if (s == "TRYDELTA") return LevenbergMarquardtParams::TRYDELTA; - if (s == "DAMPED") return LevenbergMarquardtParams::DAMPED; +LevenbergMarquardtParams::VerbosityLM LevenbergMarquardtParams::verbosityLMTranslator( + const std::string &src) const { + std::string s = src; + boost::algorithm::to_upper(s); + if (s == "SILENT") + return LevenbergMarquardtParams::SILENT; + if (s == "LAMBDA") + return LevenbergMarquardtParams::LAMBDA; + if (s == "TRYLAMBDA") + return LevenbergMarquardtParams::TRYLAMBDA; + if (s == "TRYCONFIG") + return LevenbergMarquardtParams::TRYCONFIG; + if (s == "TRYDELTA") + return LevenbergMarquardtParams::TRYDELTA; + if (s == "DAMPED") + return LevenbergMarquardtParams::DAMPED; /* default is silent */ return LevenbergMarquardtParams::SILENT; } /* ************************************************************************* */ -std::string LevenbergMarquardtParams::verbosityLMTranslator(VerbosityLM value) const { +std::string LevenbergMarquardtParams::verbosityLMTranslator( + VerbosityLM value) const { std::string s; switch (value) { - case LevenbergMarquardtParams::SILENT: s = "SILENT" ; break; - case LevenbergMarquardtParams::LAMBDA: s = "LAMBDA" ; break; - case LevenbergMarquardtParams::TRYLAMBDA: s = "TRYLAMBDA" ; break; - case LevenbergMarquardtParams::TRYCONFIG: s = "TRYCONFIG" ; break; - case LevenbergMarquardtParams::TRYDELTA: s = "TRYDELTA" ; break; - case LevenbergMarquardtParams::DAMPED: s = "DAMPED" ; break; - default: s = "UNDEFINED" ; break; + case LevenbergMarquardtParams::SILENT: + s = "SILENT"; + break; + case LevenbergMarquardtParams::TERMINATION: + s = "TERMINATION"; + break; + case LevenbergMarquardtParams::LAMBDA: + s = "LAMBDA"; + break; + case LevenbergMarquardtParams::TRYLAMBDA: + s = "TRYLAMBDA"; + break; + case LevenbergMarquardtParams::TRYCONFIG: + s = "TRYCONFIG"; + break; + case LevenbergMarquardtParams::TRYDELTA: + s = "TRYDELTA"; + break; + case LevenbergMarquardtParams::DAMPED: + s = "DAMPED"; + break; + default: + s = "UNDEFINED"; + break; } return s; } /* ************************************************************************* */ void LevenbergMarquardtParams::print(const std::string& str) const { - SuccessiveLinearizationParams::print(str); + NonlinearOptimizerParams::print(str); std::cout << " lambdaInitial: " << lambdaInitial << "\n"; std::cout << " lambdaFactor: " << lambdaFactor << "\n"; std::cout << " lambdaUpperBound: " << lambdaUpperBound << "\n"; - std::cout << " verbosityLM: " << verbosityLMTranslator(verbosityLM) << "\n"; + std::cout << " lambdaLowerBound: " << lambdaLowerBound << "\n"; + std::cout << " minModelFidelity: " << minModelFidelity << "\n"; + std::cout << " diagonalDamping: " << diagonalDamping << "\n"; + std::cout << " min_diagonal: " << min_diagonal_ << "\n"; + std::cout << " max_diagonal: " << max_diagonal_ << "\n"; + std::cout << " verbosityLM: " + << verbosityLMTranslator(verbosityLM) << "\n"; std::cout.flush(); } +/* ************************************************************************* */ +GaussianFactorGraph::shared_ptr LevenbergMarquardtOptimizer::linearize() const { + return graph_.linearize(state_.values); +} + +/* ************************************************************************* */ +void LevenbergMarquardtOptimizer::increaseLambda() { + if (params_.useFixedLambdaFactor_) { + state_.lambda *= params_.lambdaFactor; + } else { + state_.lambda *= params_.lambdaFactor; + params_.lambdaFactor *= 2.0; + } + params_.reuse_diagonal_ = true; +} + +/* ************************************************************************* */ +void LevenbergMarquardtOptimizer::decreaseLambda(double stepQuality) { + + if (params_.useFixedLambdaFactor_) { + state_.lambda /= params_.lambdaFactor; + } else { + // CHECK_GT(step_quality, 0.0); + state_.lambda *= std::max(1.0 / 3.0, 1.0 - pow(2.0 * stepQuality - 1.0, 3)); + params_.lambdaFactor = 2.0; + } + state_.lambda = std::max(params_.lambdaLowerBound, state_.lambda); + params_.reuse_diagonal_ = false; + +} + +/* ************************************************************************* */ +GaussianFactorGraph LevenbergMarquardtOptimizer::buildDampedSystem( + const GaussianFactorGraph& linear) { + + gttic(damp); + if (params_.verbosityLM >= LevenbergMarquardtParams::DAMPED) + cout << "building damped system with lambda " << state_.lambda << endl; + + // Only retrieve diagonal vector when reuse_diagonal = false + if (params_.diagonalDamping && params_.reuse_diagonal_ == false) { + state_.hessianDiagonal = linear.hessianDiagonal(); + BOOST_FOREACH(Vector& v, state_.hessianDiagonal | map_values) { + for (int aa = 0; aa < v.size(); aa++) { + v(aa) = std::min(std::max(v(aa), params_.min_diagonal_), + params_.max_diagonal_); + v(aa) = sqrt(v(aa)); + } + } + } // reuse diagonal + + // for each of the variables, add a prior + double sigma = 1.0 / std::sqrt(state_.lambda); + GaussianFactorGraph damped = linear; + damped.reserve(damped.size() + state_.values.size()); + if (params_.diagonalDamping) { + BOOST_FOREACH(const VectorValues::KeyValuePair& key_vector, state_.hessianDiagonal) { + // Fill in the diagonal of A with diag(hessian) + try { + Matrix A = Eigen::DiagonalMatrix( + state_.hessianDiagonal.at(key_vector.first)); + size_t dim = key_vector.second.size(); + Vector b = Vector::Zero(dim); + SharedDiagonal model = noiseModel::Isotropic::Sigma(dim, sigma); + damped += boost::make_shared(key_vector.first, A, b, + model); + } catch (std::exception e) { + // Don't attempt any damping if no key found in diagonal + continue; + } + } + } else { + // Straightforward damping: + BOOST_FOREACH(const Values::KeyValuePair& key_value, state_.values) { + size_t dim = key_value.value.dim(); + Matrix A = Matrix::Identity(dim, dim); + Vector b = Vector::Zero(dim); + SharedDiagonal model = noiseModel::Isotropic::Sigma(dim, sigma); + damped += boost::make_shared(key_value.key, A, b, model); + } + } + gttoc(damp); + return damped; +} + /* ************************************************************************* */ void LevenbergMarquardtOptimizer::iterate() { gttic(LM_iterate); - // Linearize graph - GaussianFactorGraph::shared_ptr linear = graph_.linearize(state_.values, *params_.ordering); - // Pull out parameters we'll use const NonlinearOptimizerParams::Verbosity nloVerbosity = params_.verbosity; const LevenbergMarquardtParams::VerbosityLM lmVerbosity = params_.verbosityLM; + // Linearize graph + if (lmVerbosity >= LevenbergMarquardtParams::DAMPED) + cout << "linearizing = " << endl; + GaussianFactorGraph::shared_ptr linear = linearize(); + // Keep increasing lambda until we make make progress - while(true) { + while (true) { + if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) cout << "trying lambda = " << state_.lambda << endl; - // Add prior-factors - // TODO: replace this dampening with a backsubstitution approach - gttic(damp); - GaussianFactorGraph dampedSystem(*linear); - { - double sigma = 1.0 / std::sqrt(state_.lambda); - dampedSystem.reserve(dampedSystem.size() + dimensions_.size()); - // for each of the variables, add a prior - for(Index j=0; j= LevenbergMarquardtParams::DAMPED) dampedSystem.print("damped"); + + ++state_.totalNumberInnerIterations; // Try solving + double modelFidelity = 0.0; + bool step_is_successful = false; + bool stopSearchingLambda = false; + double newError; + Values newValues; + VectorValues delta; + + bool systemSolvedSuccessfully; try { - // Solve Damped Gaussian Factor Graph - const VectorValues delta = solveGaussianFactorGraph(dampedSystem, params_); + delta = solve(dampedSystem, state_.values, params_); + systemSolvedSuccessfully = true; + } catch (IndeterminantLinearSystemException& e) { + systemSolvedSuccessfully = false; + } - if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) cout << "linear delta norm = " << delta.norm() << endl; - if (lmVerbosity >= LevenbergMarquardtParams::TRYDELTA) delta.print("delta"); + if (systemSolvedSuccessfully) { + params_.reuse_diagonal_ = true; - // update values - gttic(retract); - Values newValues = state_.values.retract(delta, *params_.ordering); - gttoc(retract); + if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) + cout << "linear delta norm = " << delta.norm() << endl; + if (lmVerbosity >= LevenbergMarquardtParams::TRYDELTA) + delta.print("delta"); - // create new optimization state with more adventurous lambda - gttic(compute_error); - double error = graph_.error(newValues); - gttoc(compute_error); + // cost change in the linearized system (old - new) + double newlinearizedError = linear->error(delta); - if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) cout << "next error = " << error << endl; + double linearizedCostChange = state_.error - newlinearizedError; - if(error <= state_.error) { - state_.values.swap(newValues); - state_.error = error; - state_.lambda /= params_.lambdaFactor; - break; - } else { - // Either we're not cautious, or the same lambda was worse than the current error. - // The more adventurous lambda was worse too, so make lambda more conservative - // and keep the same values. - if(state_.lambda >= params_.lambdaUpperBound) { - if(nloVerbosity >= NonlinearOptimizerParams::ERROR) - cout << "Warning: Levenberg-Marquardt giving up because cannot decrease error with maximum lambda" << endl; - break; + if (linearizedCostChange >= 0) { // step is valid + // update values + gttic(retract); + newValues = state_.values.retract(delta); + gttoc(retract); + + // compute new error + gttic(compute_error); + if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) + cout << "calculating error" << endl; + newError = graph_.error(newValues); + gttoc(compute_error); + + // cost change in the original, nonlinear system (old - new) + double costChange = state_.error - newError; + + if (linearizedCostChange > 1e-15) { // the error has to decrease to satify this condition + // fidelity of linearized model VS original system between + modelFidelity = costChange / linearizedCostChange; + // if we decrease the error in the nonlinear system and modelFidelity is above threshold + step_is_successful = modelFidelity > params_.minModelFidelity; } else { - state_.lambda *= params_.lambdaFactor; + step_is_successful = true; // linearizedCostChange close to zero } + + double minAbsoluteTolerance = params_.relativeErrorTol * state_.error; + // if the change is small we terminate + if (fabs(costChange) < minAbsoluteTolerance) + stopSearchingLambda = true; + } - } catch(const IndeterminantLinearSystemException& e) { - if(lmVerbosity >= LevenbergMarquardtParams::LAMBDA) - cout << "Negative matrix, increasing lambda" << endl; - // Either we're not cautious, or the same lambda was worse than the current error. - // The more adventurous lambda was worse too, so make lambda more conservative - // and keep the same values. - if(state_.lambda >= params_.lambdaUpperBound) { - if(nloVerbosity >= NonlinearOptimizerParams::ERROR) - cout << "Warning: Levenberg-Marquardt giving up because cannot decrease error with maximum lambda" << endl; + } + + if (step_is_successful) { // we have successfully decreased the cost and we have good modelFidelity + state_.values.swap(newValues); + state_.error = newError; + decreaseLambda(modelFidelity); + break; + } else if (!stopSearchingLambda) { // we failed to solved the system or we had no decrease in cost + if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) + cout << "increasing lambda: old error (" << state_.error + << ") new error (" << newError << ")" << endl; + increaseLambda(); + + // check if lambda is too big + if (state_.lambda >= params_.lambdaUpperBound) { + if (nloVerbosity >= NonlinearOptimizerParams::TERMINATION) + cout + << "Warning: Levenberg-Marquardt giving up because cannot decrease error with maximum lambda" + << endl; break; - } else { - state_.lambda *= params_.lambdaFactor; } - } catch(...) { - throw; + } else { // the change in the cost is very small and it is not worth trying bigger lambdas + break; } } // end while - if (lmVerbosity >= LevenbergMarquardtParams::LAMBDA) - cout << "using lambda = " << state_.lambda << endl; - // Increment the iteration counter ++state_.iterations; } +/* ************************************************************************* */ +LevenbergMarquardtParams LevenbergMarquardtOptimizer::ensureHasOrdering( + LevenbergMarquardtParams params, const NonlinearFactorGraph& graph) const { + if (!params.ordering) + params.ordering = Ordering::COLAMD(graph); + return params; +} + } /* namespace gtsam */ + diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h index 5c769ea7b..47952f9e4 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h @@ -18,7 +18,10 @@ #pragma once -#include +#include +#include + +class NonlinearOptimizerMoreOptimizationTest; namespace gtsam { @@ -29,59 +32,123 @@ class LevenbergMarquardtOptimizer; * common to all nonlinear optimization algorithms. This class also contains * all of those parameters. */ -class GTSAM_EXPORT LevenbergMarquardtParams : public SuccessiveLinearizationParams { +class GTSAM_EXPORT LevenbergMarquardtParams: public NonlinearOptimizerParams { public: /** See LevenbergMarquardtParams::lmVerbosity */ enum VerbosityLM { - SILENT = 0, - LAMBDA, - TRYLAMBDA, - TRYCONFIG, - TRYDELTA, - DAMPED + SILENT = 0, TERMINATION, LAMBDA, TRYLAMBDA, TRYCONFIG, DAMPED, TRYDELTA }; - double lambdaInitial; ///< The initial Levenberg-Marquardt damping term (default: 1e-5) - double lambdaFactor; ///< The amount by which to multiply or divide lambda when adjusting lambda (default: 10.0) - double lambdaUpperBound; ///< The maximum lambda to try before assuming the optimization has failed (default: 1e5) - VerbosityLM verbosityLM; ///< The verbosity level for Levenberg-Marquardt (default: SILENT), see also NonlinearOptimizerParams::verbosity - - LevenbergMarquardtParams() : lambdaInitial(1e-5), lambdaFactor(10.0), lambdaUpperBound(1e5), verbosityLM(SILENT) {} - virtual ~LevenbergMarquardtParams() {} - - virtual void print(const std::string& str = "") const; - - inline double getlambdaInitial() const { return lambdaInitial; } - inline double getlambdaFactor() const { return lambdaFactor; } - inline double getlambdaUpperBound() const { return lambdaUpperBound; } - inline std::string getVerbosityLM() const { return verbosityLMTranslator(verbosityLM); } - - inline void setlambdaInitial(double value) { lambdaInitial = value; } - inline void setlambdaFactor(double value) { lambdaFactor = value; } - inline void setlambdaUpperBound(double value) { lambdaUpperBound = value; } - inline void setVerbosityLM(const std::string &s) { verbosityLM = verbosityLMTranslator(s); } - private: VerbosityLM verbosityLMTranslator(const std::string &s) const; std::string verbosityLMTranslator(VerbosityLM value) const; + +public: + + double lambdaInitial; ///< The initial Levenberg-Marquardt damping term (default: 1e-5) + double lambdaFactor; ///< The amount by which to multiply or divide lambda when adjusting lambda (default: 10.0) + double lambdaUpperBound; ///< The maximum lambda to try before assuming the optimization has failed (default: 1e5) + double lambdaLowerBound; ///< The minimum lambda used in LM (default: 0) + VerbosityLM verbosityLM; ///< The verbosity level for Levenberg-Marquardt (default: SILENT), see also NonlinearOptimizerParams::verbosity + double minModelFidelity; ///< Lower bound for the modelFidelity to accept the result of an LM iteration + std::string logFile; ///< an optional CSV log file, with [iteration, time, error, labda] + bool diagonalDamping; ///< if true, use diagonal of Hessian + bool reuse_diagonal_; ///< an additional option in Ceres for diagonalDamping (related to efficiency) + bool useFixedLambdaFactor_; ///< if true applies constant increase (or decrease) to lambda according to lambdaFactor + double min_diagonal_; ///< when using diagonal damping saturates the minimum diagonal entries (default: 1e-6) + double max_diagonal_; ///< when using diagonal damping saturates the maximum diagonal entries (default: 1e32) + + LevenbergMarquardtParams() : + lambdaInitial(1e-5), lambdaFactor(10.0), lambdaUpperBound(1e5), lambdaLowerBound( + 0.0), verbosityLM(SILENT), minModelFidelity(1e-3), + diagonalDamping(false), reuse_diagonal_(false), useFixedLambdaFactor_(true), + min_diagonal_(1e-6), max_diagonal_(1e32) { + } + virtual ~LevenbergMarquardtParams() { + } + + virtual void print(const std::string& str = "") const; + + inline double getlambdaInitial() const { + return lambdaInitial; + } + inline double getlambdaFactor() const { + return lambdaFactor; + } + inline double getlambdaUpperBound() const { + return lambdaUpperBound; + } + inline double getlambdaLowerBound() const { + return lambdaLowerBound; + } + inline std::string getVerbosityLM() const { + return verbosityLMTranslator(verbosityLM); + } + inline std::string getLogFile() const { + return logFile; + } + inline bool getDiagonalDamping() const { + return diagonalDamping; + } + + inline void setlambdaInitial(double value) { + lambdaInitial = value; + } + inline void setlambdaFactor(double value) { + lambdaFactor = value; + } + inline void setlambdaUpperBound(double value) { + lambdaUpperBound = value; + } + inline void setlambdaLowerBound(double value) { + lambdaLowerBound = value; + } + inline void setVerbosityLM(const std::string &s) { + verbosityLM = verbosityLMTranslator(s); + } + inline void setLogFile(const std::string &s) { + logFile = s; + } + inline void setDiagonalDamping(bool flag) { + diagonalDamping = flag; + } + + inline void setUseFixedLambdaFactor(bool flag) { + useFixedLambdaFactor_ = flag; + } }; /** * State for LevenbergMarquardtOptimizer */ -class GTSAM_EXPORT LevenbergMarquardtState : public NonlinearOptimizerState { +class GTSAM_EXPORT LevenbergMarquardtState: public NonlinearOptimizerState { public: double lambda; + int totalNumberInnerIterations; // The total number of inner iterations in the optimization (for each iteration, LM may try multiple iterations with different lambdas) + boost::posix_time::ptime startTime; + VectorValues hessianDiagonal; //only update hessianDiagonal when reuse_diagonal_ = false - LevenbergMarquardtState() {} + LevenbergMarquardtState() { + initTime(); + } - virtual ~LevenbergMarquardtState() {} + void initTime() { + startTime = boost::posix_time::microsec_clock::universal_time(); + } + + virtual ~LevenbergMarquardtState() { + } protected: - LevenbergMarquardtState(const NonlinearFactorGraph& graph, const Values& initialValues, const LevenbergMarquardtParams& params, unsigned int iterations = 0) : - NonlinearOptimizerState(graph, initialValues, iterations), lambda(params.lambdaInitial) {} + LevenbergMarquardtState(const NonlinearFactorGraph& graph, + const Values& initialValues, const LevenbergMarquardtParams& params, + unsigned int iterations = 0) : + NonlinearOptimizerState(graph, initialValues, iterations), lambda( + params.lambdaInitial), totalNumberInnerIterations(0) { + initTime(); + } friend class LevenbergMarquardtOptimizer; }; @@ -89,12 +156,11 @@ protected: /** * This class performs Levenberg-Marquardt nonlinear optimization */ -class GTSAM_EXPORT LevenbergMarquardtOptimizer : public NonlinearOptimizer { +class GTSAM_EXPORT LevenbergMarquardtOptimizer: public NonlinearOptimizer { protected: LevenbergMarquardtParams params_; ///< LM parameters - LevenbergMarquardtState state_; ///< optimization state - std::vector dimensions_; ///< undocumented + LevenbergMarquardtState state_; ///< optimization state public: typedef boost::shared_ptr shared_ptr; @@ -110,10 +176,12 @@ public: * @param initialValues The initial variable assignments * @param params The optimization parameters */ - LevenbergMarquardtOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues, - const LevenbergMarquardtParams& params = LevenbergMarquardtParams()) : - NonlinearOptimizer(graph), params_(ensureHasOrdering(params, graph, initialValues)), - state_(graph, initialValues, params_), dimensions_(initialValues.dims(*params_.ordering)) {} + LevenbergMarquardtOptimizer(const NonlinearFactorGraph& graph, + const Values& initialValues, const LevenbergMarquardtParams& params = + LevenbergMarquardtParams()) : + NonlinearOptimizer(graph), params_(ensureHasOrdering(params, graph)), state_( + graph, initialValues, params_) { + } /** Standard constructor, requires a nonlinear factor graph, initial * variable assignments, and optimization parameters. For convenience this @@ -122,13 +190,28 @@ public: * @param graph The nonlinear factor graph to optimize * @param initialValues The initial variable assignments */ - LevenbergMarquardtOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues, const Ordering& ordering) : - NonlinearOptimizer(graph), dimensions_(initialValues.dims(ordering)) { + LevenbergMarquardtOptimizer(const NonlinearFactorGraph& graph, + const Values& initialValues, const Ordering& ordering) : + NonlinearOptimizer(graph) { params_.ordering = ordering; - state_ = LevenbergMarquardtState(graph, initialValues, params_); } + state_ = LevenbergMarquardtState(graph, initialValues, params_); + } /// Access the current damping value - double lambda() const { return state_.lambda; } + double lambda() const { + return state_.lambda; + } + + // Apply policy to increase lambda if the current update was successful (stepQuality not used in the naive policy) + void increaseLambda(); + + // Apply policy to decrease lambda if the current update was NOT successful (stepQuality not used in the naive policy) + void decreaseLambda(double stepQuality); + + /// Access the current number of inner iterations + int getInnerIterations() const { + return state_.totalNumberInnerIterations; + } /// print virtual void print(const std::string& str = "") const { @@ -142,7 +225,8 @@ public: /// @{ /** Virtual destructor */ - virtual ~LevenbergMarquardtOptimizer() {} + virtual ~LevenbergMarquardtOptimizer() { + } /** Perform a single iteration, returning a new NonlinearOptimizer class * containing the updated variable assignments, which may be retrieved with @@ -151,32 +235,49 @@ public: virtual void iterate(); /** Read-only access the parameters */ - const LevenbergMarquardtParams& params() const { return params_; } + const LevenbergMarquardtParams& params() const { + return params_; + } /** Read/write access the parameters */ - LevenbergMarquardtParams& params() { return params_; } + LevenbergMarquardtParams& params() { + return params_; + } /** Read-only access the last state */ - const LevenbergMarquardtState& state() const { return state_; } + const LevenbergMarquardtState& state() const { + return state_; + } /** Read/write access the last state. When modifying the state, the error, etc. must be consistent before calling iterate() */ - LevenbergMarquardtState& state() { return state_; } + LevenbergMarquardtState& state() { + return state_; + } + + /** Build a damped system for a specific lambda */ + GaussianFactorGraph buildDampedSystem(const GaussianFactorGraph& linear); + friend class ::NonlinearOptimizerMoreOptimizationTest; /// @} protected: + /** Access the parameters (base class version) */ - virtual const NonlinearOptimizerParams& _params() const { return params_; } + virtual const NonlinearOptimizerParams& _params() const { + return params_; + } /** Access the state (base class version) */ - virtual const NonlinearOptimizerState& _state() const { return state_; } + virtual const NonlinearOptimizerState& _state() const { + return state_; + } /** Internal function for computing a COLAMD ordering if no ordering is specified */ - LevenbergMarquardtParams ensureHasOrdering(LevenbergMarquardtParams params, const NonlinearFactorGraph& graph, const Values& values) const { - if(!params.ordering) - params.ordering = *graph.orderingCOLAMD(values); - return params; - } + LevenbergMarquardtParams ensureHasOrdering(LevenbergMarquardtParams params, + const NonlinearFactorGraph& graph) const; + + /** linearize, can be overwritten */ + virtual GaussianFactorGraph::shared_ptr linearize() const; }; } diff --git a/gtsam/nonlinear/LinearContainerFactor.cpp b/gtsam/nonlinear/LinearContainerFactor.cpp index afbc4cfc7..f26001b16 100644 --- a/gtsam/nonlinear/LinearContainerFactor.cpp +++ b/gtsam/nonlinear/LinearContainerFactor.cpp @@ -6,20 +6,15 @@ */ #include +#include +#include +#include +#include #include namespace gtsam { -/* ************************************************************************* */ -void LinearContainerFactor::rekeyFactor(const Ordering& ordering) { - BOOST_FOREACH(Index& idx, factor_->keys()) { - Key fullKey = ordering.key(idx); - idx = fullKey; - keys_.push_back(fullKey); - } -} - /* ************************************************************************* */ void LinearContainerFactor::initializeLinearizationPoint(const Values& linearizationPoint) { if (!linearizationPoint.empty()) { @@ -35,48 +30,27 @@ void LinearContainerFactor::initializeLinearizationPoint(const Values& lineariza /* ************************************************************************* */ LinearContainerFactor::LinearContainerFactor(const GaussianFactor::shared_ptr& factor, const boost::optional& linearizationPoint) -: factor_(factor), linearizationPoint_(linearizationPoint) { - // Extract keys stashed in linear factor - BOOST_FOREACH(const Index& idx, factor_->keys()) - keys_.push_back(idx); +: NonlinearFactor(factor->keys()), factor_(factor), linearizationPoint_(linearizationPoint) { } /* ************************************************************************* */ LinearContainerFactor::LinearContainerFactor( - const JacobianFactor& factor, const Ordering& ordering, - const Values& linearizationPoint) -: factor_(factor.clone()) { - rekeyFactor(ordering); + const JacobianFactor& factor, const Values& linearizationPoint) +: NonlinearFactor(factor.keys()), factor_(factor.clone()) { initializeLinearizationPoint(linearizationPoint); } /* ************************************************************************* */ LinearContainerFactor::LinearContainerFactor( - const HessianFactor& factor, const Ordering& ordering, - const Values& linearizationPoint) -: factor_(factor.clone()) { - rekeyFactor(ordering); + const HessianFactor& factor, const Values& linearizationPoint) +: NonlinearFactor(factor.keys()), factor_(factor.clone()) { initializeLinearizationPoint(linearizationPoint); } /* ************************************************************************* */ LinearContainerFactor::LinearContainerFactor( - const GaussianFactor::shared_ptr& factor, const Ordering& ordering, - const Values& linearizationPoint) -: factor_(factor->clone()) { - rekeyFactor(ordering); - initializeLinearizationPoint(linearizationPoint); -} - -/* ************************************************************************* */ -LinearContainerFactor::LinearContainerFactor( - const GaussianFactor::shared_ptr& factor, - const Values& linearizationPoint) -: factor_(factor->clone()) -{ - // Extract keys stashed in linear factor - BOOST_FOREACH(const Index& idx, factor_->keys()) - keys_.push_back(idx); + const GaussianFactor::shared_ptr& factor, const Values& linearizationPoint) +: NonlinearFactor(factor->keys()), factor_(factor->clone()) { initializeLinearizationPoint(linearizationPoint); } @@ -112,19 +86,11 @@ double LinearContainerFactor::error(const Values& c) const { csub.insert(key, c.at(key)); // create dummy ordering for evaluation - Ordering ordering = *csub.orderingArbitrary(); - VectorValues delta = linearizationPoint_->localCoordinates(csub, ordering); - - // Change keys on stored factor - BOOST_FOREACH(gtsam::Index& index, factor_->keys()) - index = ordering[index]; + VectorValues delta = linearizationPoint_->localCoordinates(csub); // compute error double error = factor_->error(delta); - // change keys back - factor_->keys() = keys(); - return error; } @@ -137,63 +103,44 @@ size_t LinearContainerFactor::dim() const { } /* ************************************************************************* */ -GaussianFactor::shared_ptr LinearContainerFactor::order(const Ordering& ordering) const { - // clone factor - boost::shared_ptr result = factor_->clone(); - - // rekey - BOOST_FOREACH(Index& key, result->keys()) - key = ordering[key]; - - return result; -} - -/* ************************************************************************* */ -GaussianFactor::shared_ptr LinearContainerFactor::linearize( - const Values& c, const Ordering& ordering) const { +GaussianFactor::shared_ptr LinearContainerFactor::linearize(const Values& c) const { + // Clone factor and update as necessary + GaussianFactor::shared_ptr linFactor = factor_->clone(); if (!hasLinearizationPoint()) - return order(ordering); + return linFactor; // Extract subset of values Values subsetC; BOOST_FOREACH(const gtsam::Key& key, this->keys()) subsetC.insert(key, c.at(key)); - // Create a temp ordering for this factor - Ordering localOrdering = *subsetC.orderingArbitrary(); - // Determine delta between linearization points using new ordering - VectorValues delta = linearizationPoint_->localCoordinates(subsetC, localOrdering); + VectorValues delta = linearizationPoint_->localCoordinates(subsetC); - // clone and reorder linear factor to final ordering - GaussianFactor::shared_ptr linFactor = order(localOrdering); + // Apply changes due to relinearization if (isJacobian()) { JacobianFactor::shared_ptr jacFactor = boost::dynamic_pointer_cast(linFactor); jacFactor->getb() = -jacFactor->unweighted_error(delta); } else { HessianFactor::shared_ptr hesFactor = boost::dynamic_pointer_cast(linFactor); - size_t dim = hesFactor->linearTerm().size(); - Eigen::Block Gview = hesFactor->info().block(0, 0, dim, dim); - Vector deltaVector = delta.asVector(); - Vector G_delta = Gview.selfadjointView() * deltaVector; + SymmetricBlockMatrix::constBlock Gview = hesFactor->matrixObject().range(0, hesFactor->size(), 0, hesFactor->size()); + Vector deltaVector = delta.vector(keys()); + Vector G_delta = Gview.selfadjointView() * deltaVector; hesFactor->constantTerm() += deltaVector.dot(G_delta) - 2.0 * deltaVector.dot(hesFactor->linearTerm()); hesFactor->linearTerm() -= G_delta; } - // reset ordering - BOOST_FOREACH(Index& idx, linFactor->keys()) - idx = ordering[localOrdering.key(idx)]; return linFactor; } /* ************************************************************************* */ bool LinearContainerFactor::isJacobian() const { - return boost::dynamic_pointer_cast(factor_); + return dynamic_cast(factor_.get()) != 0; } /* ************************************************************************* */ bool LinearContainerFactor::isHessian() const { - return boost::dynamic_pointer_cast(factor_); + return dynamic_cast(factor_.get()) != 0; } /* ************************************************************************* */ @@ -207,33 +154,29 @@ HessianFactor::shared_ptr LinearContainerFactor::toHessian() const { } /* ************************************************************************* */ -GaussianFactor::shared_ptr LinearContainerFactor::negate(const Ordering& ordering) const { +GaussianFactor::shared_ptr LinearContainerFactor::negateToGaussian() const { GaussianFactor::shared_ptr result = factor_->negate(); - BOOST_FOREACH(Key& key, result->keys()) - key = ordering[key]; return result; } /* ************************************************************************* */ -NonlinearFactor::shared_ptr LinearContainerFactor::negate() const { +NonlinearFactor::shared_ptr LinearContainerFactor::negateToNonlinear() const { GaussianFactor::shared_ptr antifactor = factor_->negate(); // already has keys in place - return NonlinearFactor::shared_ptr(new LinearContainerFactor(antifactor,linearizationPoint_)); + return NonlinearFactor::shared_ptr(new LinearContainerFactor(antifactor, linearizationPoint_)); } /* ************************************************************************* */ NonlinearFactorGraph LinearContainerFactor::convertLinearGraph( - const GaussianFactorGraph& linear_graph, const Ordering& ordering, - const Values& linearizationPoint) { + const GaussianFactorGraph& linear_graph, const Values& linearizationPoint) +{ NonlinearFactorGraph result; BOOST_FOREACH(const GaussianFactor::shared_ptr& f, linear_graph) if (f) result.push_back(NonlinearFactorGraph::sharedFactor( - new LinearContainerFactor(f, ordering, linearizationPoint))); + new LinearContainerFactor(f, linearizationPoint))); return result; } /* ************************************************************************* */ } // \namespace gtsam - - diff --git a/gtsam/nonlinear/LinearContainerFactor.h b/gtsam/nonlinear/LinearContainerFactor.h index cc87e278a..d1420d029 100644 --- a/gtsam/nonlinear/LinearContainerFactor.h +++ b/gtsam/nonlinear/LinearContainerFactor.h @@ -9,11 +9,15 @@ #pragma once -#include #include + namespace gtsam { + // Forward declarations + class JacobianFactor; + class HessianFactor; + /** * Dummy version of a generic linear factor to be injected into a nonlinear factor graph * @@ -30,26 +34,18 @@ protected: LinearContainerFactor() {} /** direct copy constructor */ - LinearContainerFactor(const GaussianFactor::shared_ptr& factor, - const boost::optional& linearizationPoint); + LinearContainerFactor(const GaussianFactor::shared_ptr& factor, const boost::optional& linearizationPoint); public: - /** Primary constructor: store a linear factor and decode the ordering */ - LinearContainerFactor(const JacobianFactor& factor, const Ordering& ordering, - const Values& linearizationPoint = Values()); + /** Primary constructor: store a linear factor with optional linearization point */ + LinearContainerFactor(const JacobianFactor& factor, const Values& linearizationPoint = Values()); - /** Primary constructor: store a linear factor and decode the ordering */ - LinearContainerFactor(const HessianFactor& factor, const Ordering& ordering, - const Values& linearizationPoint = Values()); + /** Primary constructor: store a linear factor with optional linearization point */ + LinearContainerFactor(const HessianFactor& factor, const Values& linearizationPoint = Values()); /** Constructor from shared_ptr */ - LinearContainerFactor(const GaussianFactor::shared_ptr& factor, const Ordering& ordering, - const Values& linearizationPoint = Values()); - - /** Constructor from re-keyed factor: all indices assumed replaced with Key */ - LinearContainerFactor(const GaussianFactor::shared_ptr& factor, - const Values& linearizationPoint = Values()); + LinearContainerFactor(const GaussianFactor::shared_ptr& factor, const Values& linearizationPoint = Values()); // Access @@ -81,14 +77,10 @@ public: /** Extract the linearization point used in recalculating error */ const boost::optional& linearizationPoint() const { return linearizationPoint_; } - /** Apply the ordering to a graph - same as linearize(), but without needing a linearization point */ - GaussianFactor::shared_ptr order(const Ordering& ordering) const; - /** * Linearize to a GaussianFactor, with method depending on the presence of a linearizationPoint - * - With no linearization point, returns a reordered, but numerically identical, - * version of the existing stored linear factor - * - With a linearization point provided, returns a reordered and relinearized version of + * - With no linearization point, returns a cloned version of the stored linear factor. + * - With a linearization point provided, returns a relinearized version of * the linearized factor. * * The relinearization approach used computes a linear delta between the original linearization @@ -101,18 +93,17 @@ public: * TODO: better approximation of relinearization * TODO: switchable modes for approximation technique */ - GaussianFactor::shared_ptr linearize(const Values& c, const Ordering& ordering) const; + GaussianFactor::shared_ptr linearize(const Values& c) const; /** - * Creates an anti-factor directly and performs rekeying due to ordering + * Creates an anti-factor directly */ - GaussianFactor::shared_ptr negate(const Ordering& ordering) const; + GaussianFactor::shared_ptr negateToGaussian() const; /** - * Creates the equivalent anti-factor as another LinearContainerFactor, - * so it remains independent of ordering. + * Creates the equivalent anti-factor as another LinearContainerFactor. */ - NonlinearFactor::shared_ptr negate() const; + NonlinearFactor::shared_ptr negateToNonlinear() const; /** * Creates a shared_ptr clone of the factor - needs to be specialized to allow @@ -135,20 +126,19 @@ public: bool isHessian() const; /** Casts to JacobianFactor */ - JacobianFactor::shared_ptr toJacobian() const; + boost::shared_ptr toJacobian() const; /** Casts to HessianFactor */ - HessianFactor::shared_ptr toHessian() const; + boost::shared_ptr toHessian() const; /** * Utility function for converting linear graphs to nonlinear graphs * consisting of LinearContainerFactors. */ static NonlinearFactorGraph convertLinearGraph(const GaussianFactorGraph& linear_graph, - const Ordering& ordering, const Values& linearizationPoint = Values()); + const Values& linearizationPoint = Values()); protected: - void rekeyFactor(const Ordering& ordering); void initializeLinearizationPoint(const Values& linearizationPoint); private: @@ -167,4 +157,3 @@ private: } // \namespace gtsam - diff --git a/gtsam/nonlinear/Marginals.cpp b/gtsam/nonlinear/Marginals.cpp index 466a20a84..0dca18a1f 100644 --- a/gtsam/nonlinear/Marginals.cpp +++ b/gtsam/nonlinear/Marginals.cpp @@ -17,8 +17,8 @@ */ #include -#include -#include +#include +#include #include using namespace std; @@ -26,14 +26,12 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ -Marginals::Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization) { +Marginals::Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization) +{ gttic(MarginalsConstructor); - // Compute COLAMD ordering - ordering_ = *graph.orderingCOLAMD(solution); - // Linearize graph - graph_ = *graph.linearize(solution, ordering_); + graph_ = *graph.linearize(solution); // Store values values_ = solution; @@ -41,14 +39,14 @@ Marginals::Marginals(const NonlinearFactorGraph& graph, const Values& solution, // Compute BayesTree factorization_ = factorization; if(factorization_ == CHOLESKY) - bayesTree_ = *GaussianMultifrontalSolver(graph_, false).eliminate(); + bayesTree_ = *graph_.eliminateMultifrontal(boost::none, EliminatePreferCholesky); else if(factorization_ == QR) - bayesTree_ = *GaussianMultifrontalSolver(graph_, true).eliminate(); + bayesTree_ = *graph_.eliminateMultifrontal(boost::none, EliminateQR); } /* ************************************************************************* */ -void Marginals::print(const std::string& str, const KeyFormatter& keyFormatter) const { - ordering_.print(str+"Ordering: ", keyFormatter); +void Marginals::print(const std::string& str, const KeyFormatter& keyFormatter) const +{ graph_.print(str+"Graph: "); values_.print(str+"Solution: ", keyFormatter); bayesTree_.print(str+"Bayes Tree: "); @@ -62,38 +60,25 @@ Matrix Marginals::marginalCovariance(Key variable) const { /* ************************************************************************* */ Matrix Marginals::marginalInformation(Key variable) const { gttic(marginalInformation); - // Get linear key - Index index = ordering_[variable]; // Compute marginal GaussianFactor::shared_ptr marginalFactor; if(factorization_ == CHOLESKY) - marginalFactor = bayesTree_.marginalFactor(index, EliminatePreferCholesky); + marginalFactor = bayesTree_.marginalFactor(variable, EliminatePreferCholesky); else if(factorization_ == QR) - marginalFactor = bayesTree_.marginalFactor(index, EliminateQR); + marginalFactor = bayesTree_.marginalFactor(variable, EliminateQR); // Get information matrix (only store upper-right triangle) gttic(AsMatrix); return marginalFactor->information(); } -/* ************************************************************************* */ -JointMarginal::JointMarginal(const JointMarginal& other) : - blockView_(fullMatrix_) { - *this = other; -} - -/* ************************************************************************* */ -JointMarginal& JointMarginal::operator=(const JointMarginal& rhs) { - indices_ = rhs.indices_; - blockView_.assignNoalias(rhs.blockView_); - return *this; -} - /* ************************************************************************* */ JointMarginal Marginals::jointMarginalCovariance(const std::vector& variables) const { JointMarginal info = jointMarginalInformation(variables); - info.fullMatrix_ = info.fullMatrix_.inverse(); + info.blockMatrix_.full().triangularView() = + info.blockMatrix_.full().selfadjointView().llt().solve( + Matrix::Identity(info.blockMatrix_.full().rows(), info.blockMatrix_.full().rows())).triangularView(); return info; } @@ -102,61 +87,45 @@ JointMarginal Marginals::jointMarginalInformation(const std::vector& variab // If 2 variables, we can use the BayesTree::joint function, otherwise we // have to use sequential elimination. - if(variables.size() == 1) { + if(variables.size() == 1) + { Matrix info = marginalInformation(variables.front()); std::vector dims; dims.push_back(info.rows()); - Ordering indices; - indices.insert(variables.front(), 0); - return JointMarginal(info, dims, indices); - - } else { - // Obtain requested variables as ordered indices - vector indices(variables.size()); - for(size_t i=0; i usedIndices; - for(size_t i=0; i Index_Key; - BOOST_FOREACH(const Index_Key& index_key, usedIndices) { - variableConversion.insert(index_key.second, slot); - ++ slot; - } - } - - // Get dimensions from factor graph - std::vector dims(indices.size(), 0); - BOOST_FOREACH(Key key, variables) { - dims[variableConversion[key]] = values_.at(key).dim(); + jointFG = GaussianFactorGraph(*graph_.marginalMultifrontalBayesTree(variables, boost::none, EliminateQR)); } // Get information matrix Matrix augmentedInfo = jointFG.augmentedHessian(); Matrix info = augmentedInfo.topLeftCorner(augmentedInfo.rows()-1, augmentedInfo.cols()-1); - return JointMarginal(info, dims, variableConversion); + // Information matrix will be returned with sorted keys + std::vector variablesSorted = variables; + std::sort(variablesSorted.begin(), variablesSorted.end()); + + // Get dimensions from factor graph + std::vector dims; + dims.reserve(variablesSorted.size()); + BOOST_FOREACH(Key key, variablesSorted) { + dims.push_back(values_.at(key).dim()); + } + + return JointMarginal(info, dims, variablesSorted); } } @@ -164,12 +133,12 @@ JointMarginal Marginals::jointMarginalInformation(const std::vector& variab void JointMarginal::print(const std::string& s, const KeyFormatter& formatter) const { cout << s << "Joint marginal on keys "; bool first = true; - BOOST_FOREACH(const Ordering::value_type& key_index, indices_) { + BOOST_FOREACH(Key key, keys_) { if(!first) cout << ", "; else first = false; - cout << formatter(key_index.first); + cout << formatter(key); } cout << ". Use 'at' or 'operator()' to query matrix blocks." << endl; } diff --git a/gtsam/nonlinear/Marginals.h b/gtsam/nonlinear/Marginals.h index 94876dcf9..dd585e902 100644 --- a/gtsam/nonlinear/Marginals.h +++ b/gtsam/nonlinear/Marginals.h @@ -18,7 +18,6 @@ #pragma once -#include #include #include #include @@ -43,7 +42,6 @@ public: protected: GaussianFactorGraph graph_; - Ordering ordering_; Values values_; Factorization factorization_; GaussianBayesTree bayesTree_; @@ -81,12 +79,9 @@ public: class GTSAM_EXPORT JointMarginal { protected: - - typedef SymmetricBlockView BlockView; - - Matrix fullMatrix_; - BlockView blockView_; - Ordering indices_; + SymmetricBlockMatrix blockMatrix_; + std::vector keys_; + FastMap indices_; public: /** A block view of the joint marginal - this stores a reference to the @@ -94,7 +89,7 @@ public: * while this block view is needed, otherwise assign this block object to a * Matrix to store it. */ - typedef BlockView::constBlock Block; + typedef SymmetricBlockMatrix::constBlock Block; /** Access a block, corresponding to a pair of variables, of the joint * marginal. Each block is accessed by its "vertical position", @@ -110,7 +105,7 @@ public: * @param jVariable The nonlinear Key specifying the "horizontal position" of the requested block */ Block operator()(Key iVariable, Key jVariable) const { - return blockView_(indices_[iVariable], indices_[jVariable]); } + return blockMatrix_(indices_.at(iVariable), indices_.at(jVariable)); } /** Synonym for operator() */ Block at(Key iVariable, Key jVariable) const { @@ -121,20 +116,14 @@ public: * in scope while this view is needed. Otherwise assign this block object to a Matrix * to store it. */ - const Matrix& fullMatrix() const { return fullMatrix_; } - - /** Copy constructor */ - JointMarginal(const JointMarginal& other); - - /** Assignment operator */ - JointMarginal& operator=(const JointMarginal& rhs); + Eigen::SelfAdjointView fullMatrix() const { return blockMatrix_.matrix(); } /** Print */ void print(const std::string& s = "", const KeyFormatter& formatter = DefaultKeyFormatter) const; protected: - JointMarginal(const Matrix& fullMatrix, const std::vector& dims, const Ordering& indices) : - fullMatrix_(fullMatrix), blockView_(fullMatrix_, dims.begin(), dims.end()), indices_(indices) {} + JointMarginal(const Matrix& fullMatrix, const std::vector& dims, const std::vector& keys) : + blockMatrix_(dims, fullMatrix), keys_(keys), indices_(Ordering(keys).invert()) {} friend class Marginals; diff --git a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp index 90f663639..107ec7d3f 100644 --- a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp @@ -6,7 +6,6 @@ */ #include -#include #include #include #include @@ -19,18 +18,10 @@ namespace gtsam { /* Return the gradient vector of a nonlinear factor given a linearization point and a variable ordering * Can be moved to NonlinearFactorGraph.h if desired */ -void gradientInPlace(const NonlinearFactorGraph &nfg, const Values &values, const Ordering &ordering, VectorValues &g) { +VectorValues gradientInPlace(const NonlinearFactorGraph &nfg, const Values &values) { // Linearize graph - GaussianFactorGraph::shared_ptr linear = nfg.linearize(values, ordering); - FactorGraph jfg; jfg.reserve(linear->size()); - BOOST_FOREACH(const GaussianFactorGraph::sharedFactor& factor, *linear) { - if(boost::shared_ptr jf = boost::dynamic_pointer_cast(factor)) - jfg.push_back((jf)); - else - jfg.push_back(boost::make_shared(*factor)); - } - // compute the gradient direction - gradientAtZero(jfg, g); + GaussianFactorGraph::shared_ptr linear = nfg.linearize(values); + return linear->gradientAtZero(); } double NonlinearConjugateGradientOptimizer::System::error(const State &state) const { @@ -38,27 +29,26 @@ double NonlinearConjugateGradientOptimizer::System::error(const State &state) co } NonlinearConjugateGradientOptimizer::System::Gradient NonlinearConjugateGradientOptimizer::System::gradient(const State &state) const { - Gradient result = state.zeroVectors(ordering_); - gradientInPlace(graph_, state, ordering_, result); - return result; + return gradientInPlace(graph_, state); } NonlinearConjugateGradientOptimizer::System::State NonlinearConjugateGradientOptimizer::System::advance(const State ¤t, const double alpha, const Gradient &g) const { Gradient step = g; - scal(alpha, step); - return current.retract(step, ordering_); + step *= alpha; + return current.retract(step); } void NonlinearConjugateGradientOptimizer::iterate() { - size_t dummy ; - boost::tie(state_.values, dummy) = nonlinearConjugateGradient(System(graph_, *ordering_), state_.values, params_, true /* single iterations */); + int dummy ; + boost::tie(state_.values, dummy) = nonlinearConjugateGradient(System(graph_), state_.values, params_, true /* single iterations */); ++state_.iterations; state_.error = graph_.error(state_.values); } const Values& NonlinearConjugateGradientOptimizer::optimize() { - boost::tie(state_.values, state_.iterations) = nonlinearConjugateGradient(System(graph_, *ordering_), state_.values, params_, false /* up to convergent */); + boost::tie(state_.values, state_.iterations) = nonlinearConjugateGradient(System(graph_), state_.values, params_, false /* up to convergent */); state_.error = graph_.error(state_.values); return state_.values; } } /* namespace gtsam */ + diff --git a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h index 4cb5be573..1ad8fa2ab 100644 --- a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h +++ b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h @@ -31,10 +31,9 @@ class GTSAM_EXPORT NonlinearConjugateGradientOptimizer : public NonlinearOptimiz protected: const NonlinearFactorGraph &graph_; - const Ordering &ordering_; public: - System(const NonlinearFactorGraph &graph, const Ordering &ordering): graph_(graph), ordering_(ordering) {} + System(const NonlinearFactorGraph &graph): graph_(graph) {} double error(const State &state) const ; Gradient gradient(const State &state) const ; State advance(const State ¤t, const double alpha, const Gradient &g) const ; @@ -49,15 +48,12 @@ public: protected: States state_; Parameters params_; - Ordering::shared_ptr ordering_; - VectorValues::shared_ptr gradient_; public: NonlinearConjugateGradientOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues, const Parameters& params = Parameters()) - : Base(graph), state_(graph, initialValues), params_(params), ordering_(initialValues.orderingArbitrary()), - gradient_(new VectorValues(initialValues.zeroVectors(*ordering_))){} + : Base(graph), state_(graph, initialValues), params_(params) {} virtual ~NonlinearConjugateGradientOptimizer() {} virtual void iterate(); @@ -125,11 +121,11 @@ double lineSearch(const S &system, const V currentValues, const W &gradient) { * The last parameter is a switch between gradient-descent and conjugate gradient */ template -boost::tuple nonlinearConjugateGradient(const S &system, const V &initial, const NonlinearOptimizerParams ¶ms, const bool singleIteration, const bool gradientDescent = false) { +boost::tuple nonlinearConjugateGradient(const S &system, const V &initial, const NonlinearOptimizerParams ¶ms, const bool singleIteration, const bool gradientDescent = false) { // GTSAM_CONCEPT_MANIFOLD_TYPE(V); - Index iteration = 0; + int iteration = 0; // check if we're already close enough double currentError = system.error(initial); @@ -185,9 +181,5 @@ boost::tuple nonlinearConjugateGradient(const S &system, const V &ini return boost::tie(currentValues, iteration); } - - - - - } + diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index faee280b1..5d3319d97 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -153,12 +153,12 @@ namespace gtsam { } // Linearize is over-written, because base linearization tries to whiten - virtual GaussianFactor::shared_ptr linearize(const Values& x, const Ordering& ordering) const { + virtual GaussianFactor::shared_ptr linearize(const Values& x) const { const T& xj = x.at(this->key()); Matrix A; Vector b = evaluateError(xj, A); SharedDiagonal model = noiseModel::Constrained::All(b.size()); - return GaussianFactor::shared_ptr(new JacobianFactor(ordering[this->key()], A, b, model)); + return GaussianFactor::shared_ptr(new JacobianFactor(this->key(), A, b, model)); } /// @return a deep copy of this factor diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 8b9b2a5da..1092d8ac2 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -20,19 +20,14 @@ #pragma once -#include -#include - #include -#include - -#include -#include -#include -#include +#include #include -#include +#include +#include +#include + /** * Macro to add a standard clone function to a derived factor @@ -45,6 +40,8 @@ namespace gtsam { +using boost::assign::cref_list_of; + /* ************************************************************************* */ /** * Nonlinear factor base class @@ -54,51 +51,39 @@ namespace gtsam { * which are objects in non-linear manifolds (Lie groups). * \nosubgrouping */ -class NonlinearFactor: public Factor { +class NonlinearFactor: public Factor { protected: // Some handy typedefs - typedef Factor Base; + typedef Factor Base; typedef NonlinearFactor This; public: - typedef boost::shared_ptr shared_ptr; + typedef boost::shared_ptr shared_ptr; /// @name Standard Constructors /// @{ /** Default constructor for I/O only */ - NonlinearFactor() { - } + NonlinearFactor() {} /** - * Constructor from a vector of the keys involved in this factor + * Constructor from a collection of the keys involved in this factor */ - NonlinearFactor(const std::vector& keys) : + template + NonlinearFactor(const CONTAINER& keys) : Base(keys) {} - /** - * Constructor from iterators over the keys involved in this factor - */ - template - NonlinearFactor(ITERATOR beginKeys, ITERATOR endKeys) : - Base(beginKeys, endKeys) {} - - NonlinearFactor(Key key) : Base(key) {} ///< Convenience constructor for 1 key - NonlinearFactor(Key key1, Key key2) : Base(key1, key2) {} ///< Convenience constructor for 2 keys - NonlinearFactor(Key key1, Key key2, Key key3) : Base(key1, key2, key3) {} ///< Convenience constructor for 3 keys - NonlinearFactor(Key key1, Key key2, Key key3, Key key4) : Base(key1, key2, key3, key4) {} ///< Convenience constructor for 4 keys - NonlinearFactor(Key key1, Key key2, Key key3, Key key4, Key key5) : Base(key1, key2, key3, key4, key5) {} ///< Convenience constructor for 5 keys - NonlinearFactor(Key key1, Key key2, Key key3, Key key4, Key key5, Key key6) : Base(key1, key2, key3, key4, key5, key6) {} ///< Convenience constructor for 6 keys - /// @} /// @name Testable /// @{ /** print */ - virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + virtual void print(const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const + { std::cout << s << " keys = { "; BOOST_FOREACH(Key key, this->keys()) { std::cout << keyFormatter(key) << " "; } std::cout << "}" << std::endl; @@ -141,18 +126,18 @@ public: /** linearize to a GaussianFactor */ virtual boost::shared_ptr - linearize(const Values& c, const Ordering& ordering) const = 0; + linearize(const Values& c) const = 0; /** * Create a symbolic factor using the given ordering to determine the * variable indices. */ - virtual IndexFactor::shared_ptr symbolic(const Ordering& ordering) const { - std::vector indices(this->size()); - for(size_t j=0; jsize(); ++j) - indices[j] = ordering[this->keys()[j]]; - return IndexFactor::shared_ptr(new IndexFactor(indices)); - } + //virtual IndexFactor::shared_ptr symbolic(const Ordering& ordering) const { + // std::vector indices(this->size()); + // for(size_t j=0; jsize(); ++j) + // indices[j] = ordering[this->keys()[j]]; + // return IndexFactor::shared_ptr(new IndexFactor(indices)); + //} /** * Creates a shared_ptr clone of the factor - needs to be specialized to allow @@ -218,11 +203,10 @@ protected: public: - typedef boost::shared_ptr shared_ptr; + typedef boost::shared_ptr shared_ptr; /** Default constructor for I/O only */ - NoiseModelFactor() { - } + NoiseModelFactor() {} /** Destructor */ virtual ~NoiseModelFactor() {} @@ -230,17 +214,9 @@ public: /** * Constructor */ - template - NoiseModelFactor(const SharedNoiseModel& noiseModel, ITERATOR beginKeys, ITERATOR endKeys) - : Base(beginKeys, endKeys), noiseModel_(noiseModel) { - } - - NoiseModelFactor(const SharedNoiseModel& noiseModel, Key key) : Base(key), noiseModel_(noiseModel) {} ///< Convenience constructor for 1 key - NoiseModelFactor(const SharedNoiseModel& noiseModel, Key key1, Key key2) : Base(key1, key2), noiseModel_(noiseModel) {} ///< Convenience constructor for 2 keys - NoiseModelFactor(const SharedNoiseModel& noiseModel, Key key1, Key key2, Key key3) : Base(key1, key2, key3), noiseModel_(noiseModel) {} ///< Convenience constructor for 3 keys - NoiseModelFactor(const SharedNoiseModel& noiseModel, Key key1, Key key2, Key key3, Key key4) : Base(key1, key2, key3, key4), noiseModel_(noiseModel) {} ///< Convenience constructor for 4 keys - NoiseModelFactor(const SharedNoiseModel& noiseModel, Key key1, Key key2, Key key3, Key key4, Key key5) : Base(key1, key2, key3, key4, key5), noiseModel_(noiseModel) {} ///< Convenience constructor for 5 keys - NoiseModelFactor(const SharedNoiseModel& noiseModel, Key key1, Key key2, Key key3, Key key4, Key key5, Key key6) : Base(key1, key2, key3, key4, key5, key6), noiseModel_(noiseModel) {} ///< Convenience constructor for 6 keys + template + NoiseModelFactor(const SharedNoiseModel& noiseModel, const CONTAINER& keys) : + Base(keys), noiseModel_(noiseModel) {} protected: @@ -252,7 +228,9 @@ protected: public: /** Print */ - virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + virtual void print(const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const + { Base::print(s, keyFormatter); this->noiseModel_->print(" noise model: "); } @@ -260,7 +238,9 @@ public: /** Check if two factors are equal */ virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const { const NoiseModelFactor* e = dynamic_cast(&f); - return e && Base::equals(f, tol) && noiseModel_->equals(*e->noiseModel_, tol); + return e && Base::equals(f, tol) && + ((!noiseModel_ && !e->noiseModel_) || + (noiseModel_ && e->noiseModel_ && noiseModel_->equals(*e->noiseModel_, tol))); } /** get the dimension of the factor (number of rows on linearization) */ @@ -314,7 +294,7 @@ public: * \f$ Ax-b \approx h(x+\delta x)-z = h(x) + A \delta x - z \f$ * Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$ */ - boost::shared_ptr linearize(const Values& x, const Ordering& ordering) const { + boost::shared_ptr linearize(const Values& x) const { // Only linearize if the factor is active if (!this->active(x)) return boost::shared_ptr(); @@ -324,27 +304,38 @@ public: // Call evaluate error to get Jacobians and b vector std::vector A(this->size()); b = -unwhitenedError(x, A); - if((size_t) b.size() != noiseModel_->dim()) - throw std::invalid_argument("This factor was created with a NoiseModel of incorrect dimension."); + if(noiseModel_) + { + if((size_t) b.size() != noiseModel_->dim()) + throw std::invalid_argument("This factor was created with a NoiseModel of incorrect dimension."); - this->noiseModel_->WhitenSystem(A,b); + this->noiseModel_->WhitenSystem(A,b); + } - std::vector > terms(this->size()); + std::vector > terms(this->size()); // Fill in terms for(size_t j=0; jsize(); ++j) { - terms[j].first = ordering[this->keys()[j]]; + terms[j].first = this->keys()[j]; terms[j].second.swap(A[j]); } // TODO pass unwhitened + noise model to Gaussian factor - noiseModel::Constrained::shared_ptr constrained = + // For now, only linearized constrained factors have noise model at linear level!!! + if(noiseModel_) + { + noiseModel::Constrained::shared_ptr constrained = boost::dynamic_pointer_cast(this->noiseModel_); - if(constrained) - return GaussianFactor::shared_ptr( - new JacobianFactor(terms, b, constrained->unit())); + if(constrained) { + size_t augmentedDim = terms[0].second.rows() - constrained->dim(); + Vector augmentedZero = zero(augmentedDim); + Vector augmentedb = concatVectors(2, &b, &augmentedZero); + return GaussianFactor::shared_ptr(new JacobianFactor(terms, augmentedb, constrained->unit(augmentedDim))); + } + else + return GaussianFactor::shared_ptr(new JacobianFactor(terms, b)); + } else - return GaussianFactor::shared_ptr( - new JacobianFactor(terms, b, noiseModel::Unit::Create(b.size()))); + return GaussianFactor::shared_ptr(new JacobianFactor(terms, b)); } private: @@ -392,7 +383,7 @@ public: * @param key1 by which to look up X value in Values */ NoiseModelFactor1(const SharedNoiseModel& noiseModel, Key key1) : - Base(noiseModel, key1) {} + Base(noiseModel, cref_list_of<1>(key1)) {} /** Calls the 1-key specific version of evaluateError, which is pure virtual * so must be implemented in the derived class. @@ -461,7 +452,7 @@ public: * @param j2 key of the second variable */ NoiseModelFactor2(const SharedNoiseModel& noiseModel, Key j1, Key j2) : - Base(noiseModel, j1, j2) {} + Base(noiseModel, cref_list_of<2>(j1)(j2)) {} virtual ~NoiseModelFactor2() {} @@ -538,7 +529,7 @@ public: * @param j3 key of the third variable */ NoiseModelFactor3(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3) : - Base(noiseModel, j1, j2, j3) {} + Base(noiseModel, cref_list_of<3>(j1)(j2)(j3)) {} virtual ~NoiseModelFactor3() {} @@ -617,7 +608,7 @@ public: * @param j4 key of the fourth variable */ NoiseModelFactor4(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3, Key j4) : - Base(noiseModel, j1, j2, j3, j4) {} + Base(noiseModel, cref_list_of<4>(j1)(j2)(j3)(j4)) {} virtual ~NoiseModelFactor4() {} @@ -700,7 +691,7 @@ public: * @param j5 key of the fifth variable */ NoiseModelFactor5(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3, Key j4, Key j5) : - Base(noiseModel, j1, j2, j3, j4, j5) {} + Base(noiseModel, cref_list_of<5>(j1)(j2)(j3)(j4)(j5)) {} virtual ~NoiseModelFactor5() {} @@ -787,7 +778,7 @@ public: * @param j6 key of the fifth variable */ NoiseModelFactor6(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3, Key j4, Key j5, Key j6) : - Base(noiseModel, j1, j2, j3, j4, j5, j6) {} + Base(noiseModel, cref_list_of<6>(j1)(j2)(j3)(j4)(j5)(j6)) {} virtual ~NoiseModelFactor6() {} diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp index 66a658f25..63a1a2218 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.cpp +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -20,16 +20,26 @@ #include #include #include -#include -#include #include #include +#include +#include +#include +#include +#include #include +#ifdef GTSAM_USE_TBB +# include +#endif + using namespace std; namespace gtsam { +// Instantiate base classes +template class FactorGraph; + /* ************************************************************************* */ double NonlinearFactorGraph::probPrime(const Values& c) const { return exp(-0.5 * error(c)); @@ -45,6 +55,11 @@ void NonlinearFactorGraph::print(const std::string& str, const KeyFormatter& key } } +/* ************************************************************************* */ +bool NonlinearFactorGraph::equals(const NonlinearFactorGraph& other, double tol) const { + return Base::equals(other, tol); +} + /* ************************************************************************* */ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values, const GraphvizFormatting& graphvizFormatting, @@ -102,6 +117,27 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values, default: throw std::runtime_error("Invalid enum value"); } return Point2(x,y); + } else if(const Point3* p = dynamic_cast(&value)) { + double x, y; + switch (graphvizFormatting.paperHorizontalAxis) { + case GraphvizFormatting::X: x = p->x(); break; + case GraphvizFormatting::Y: x = p->y(); break; + case GraphvizFormatting::Z: x = p->z(); break; + case GraphvizFormatting::NEGX: x = -p->x(); break; + case GraphvizFormatting::NEGY: x = -p->y(); break; + case GraphvizFormatting::NEGZ: x = -p->z(); break; + default: throw std::runtime_error("Invalid enum value"); + } + switch (graphvizFormatting.paperVerticalAxis) { + case GraphvizFormatting::X: y = p->x(); break; + case GraphvizFormatting::Y: y = p->y(); break; + case GraphvizFormatting::Z: y = p->z(); break; + case GraphvizFormatting::NEGX: y = -p->x(); break; + case GraphvizFormatting::NEGY: y = -p->y(); break; + case GraphvizFormatting::NEGZ: y = -p->z(); break; + default: throw std::runtime_error("Invalid enum value"); + } + return Point2(x,y); } else { return boost::none; } @@ -127,6 +163,8 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values, } // Create nodes for each variable in the graph + bool firstTimePoses = true; + Key lastKey; BOOST_FOREACH(Key key, keys) { // Label the node with the label from the KeyFormatter stm << " var" << key << "[label=\"" << keyFormatter(key) << "\""; @@ -136,9 +174,18 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values, stm << ", pos=\"" << graphvizFormatting.scale*(xy->x() - minX) << "," << graphvizFormatting.scale*(xy->y() - minY) << "!\""; } stm << "];\n"; + + if (firstTimePoses) { + lastKey = key; + firstTimePoses = false; + } else { + stm << " var" << key << "--" << "var" << lastKey << ";\n"; + lastKey = key; + } } stm << "\n"; + if(graphvizFormatting.mergeSimilarFactors) { // Remove duplicate factors FastSet > structure; @@ -171,13 +218,39 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values, } else { // Create factors and variable connections for(size_t i = 0; i < this->size(); ++i) { - // Make each factor a dot - stm << " factor" << i << "[label=\"\", shape=point];\n"; + if(graphvizFormatting.plotFactorPoints){ + // Make each factor a dot + stm << " factor" << i << "[label=\"\", shape=point"; + { + map::const_iterator pos = graphvizFormatting.factorPositions.find(i); + if(pos != graphvizFormatting.factorPositions.end()) + stm << ", pos=\"" << graphvizFormatting.scale*(pos->second.x() - minX) << "," << graphvizFormatting.scale*(pos->second.y() - minY) << "!\""; + } + stm << "];\n"; - // Make factor-variable connections - if(this->at(i)) { - BOOST_FOREACH(Key key, *this->at(i)) { - stm << " var" << key << "--" << "factor" << i << ";\n"; } } + // Make factor-variable connections + if(graphvizFormatting.connectKeysToFactor && this->at(i)) { + BOOST_FOREACH(Key key, *this->at(i)) { + stm << " var" << key << "--" << "factor" << i << ";\n"; + } + } + + } + else { + if(this->at(i)) { + Key k; + bool firstTime = true; + BOOST_FOREACH(Key key, *this->at(i)) { + if(firstTime){ + k = key; + firstTime = false; + continue; + } + stm << " var" << key << "--" << "var" << k << ";\n"; + k = key; + } + } + } } } @@ -207,109 +280,88 @@ FastSet NonlinearFactorGraph::keys() const { } /* ************************************************************************* */ -Ordering::shared_ptr NonlinearFactorGraph::orderingCOLAMD( - const Values& config) const +Ordering NonlinearFactorGraph::orderingCOLAMD() const { - gttic(NonlinearFactorGraph_orderingCOLAMD); - - // Create symbolic graph and initial (iterator) ordering - SymbolicFactorGraph::shared_ptr symbolic; - Ordering::shared_ptr ordering; - boost::tie(symbolic, ordering) = this->symbolic(config); - - // Compute the VariableIndex (column-wise index) - VariableIndex variableIndex(*symbolic, ordering->size()); - if (config.size() != variableIndex.size()) throw std::runtime_error( - "orderingCOLAMD: some variables in the graph are not constrained!"); - - // Compute a fill-reducing ordering with COLAMD - Permutation::shared_ptr colamdPerm(inference::PermutationCOLAMD( - variableIndex)); - - // Permute the Ordering with the COLAMD ordering - ordering->permuteInPlace(*colamdPerm); - return ordering; + return Ordering::COLAMD(*this); } /* ************************************************************************* */ -Ordering::shared_ptr NonlinearFactorGraph::orderingCOLAMDConstrained(const Values& config, - const std::map& constraints) const +Ordering NonlinearFactorGraph::orderingCOLAMDConstrained(const FastMap& constraints) const { - gttic(NonlinearFactorGraph_orderingCOLAMDConstrained); - - // Create symbolic graph and initial (iterator) ordering - SymbolicFactorGraph::shared_ptr symbolic; - Ordering::shared_ptr ordering; - boost::tie(symbolic, ordering) = this->symbolic(config); - - // Compute the VariableIndex (column-wise index) - VariableIndex variableIndex(*symbolic, ordering->size()); - if (config.size() != variableIndex.size()) throw std::runtime_error( - "orderingCOLAMD: some variables in the graph are not constrained!"); - - // Create a constraint list with correct indices - typedef std::map::value_type constraint_type; - std::map index_constraints; - BOOST_FOREACH(const constraint_type& p, constraints) - index_constraints[ordering->at(p.first)] = p.second; - - // Compute a constrained fill-reducing ordering with COLAMD - Permutation::shared_ptr colamdPerm(inference::PermutationCOLAMDGrouped( - variableIndex, index_constraints)); - - // Permute the Ordering with the COLAMD ordering - ordering->permuteInPlace(*colamdPerm); - return ordering; + return Ordering::COLAMDConstrained(*this, constraints); } /* ************************************************************************* */ -SymbolicFactorGraph::shared_ptr NonlinearFactorGraph::symbolic(const Ordering& ordering) const { - gttic(NonlinearFactorGraph_symbolic_from_Ordering); - +SymbolicFactorGraph::shared_ptr NonlinearFactorGraph::symbolic() const +{ // Generate the symbolic factor graph - SymbolicFactorGraph::shared_ptr symbolicfg(new SymbolicFactorGraph); - symbolicfg->reserve(this->size()); + SymbolicFactorGraph::shared_ptr symbolic = boost::make_shared(); + symbolic->reserve(this->size()); - BOOST_FOREACH(const sharedFactor& factor, this->factors_) { + BOOST_FOREACH(const sharedFactor& factor, *this) { if(factor) - symbolicfg->push_back(factor->symbolic(ordering)); + *symbolic += SymbolicFactor(*factor); else - symbolicfg->push_back(SymbolicFactorGraph::sharedFactor()); + *symbolic += SymbolicFactorGraph::sharedFactor(); } - return symbolicfg; + return symbolic; } /* ************************************************************************* */ -pair NonlinearFactorGraph::symbolic( - const Values& config) const -{ - gttic(NonlinearFactorGraph_symbolic_from_Values); +namespace { + +#ifdef GTSAM_USE_TBB + struct _LinearizeOneFactor { + const NonlinearFactorGraph& graph; + const Values& linearizationPoint; + GaussianFactorGraph& result; + _LinearizeOneFactor(const NonlinearFactorGraph& graph, const Values& linearizationPoint, GaussianFactorGraph& result) : + graph(graph), linearizationPoint(linearizationPoint), result(result) {} + void operator()(const tbb::blocked_range& r) const + { + for(size_t i = r.begin(); i != r.end(); ++i) + { + if(graph[i]) + result[i] = graph[i]->linearize(linearizationPoint); + else + result[i] = GaussianFactor::shared_ptr(); + } + } + }; +#endif - // Generate an initial key ordering in iterator order - Ordering::shared_ptr ordering(config.orderingArbitrary()); - return make_pair(symbolic(*ordering), ordering); } /* ************************************************************************* */ -GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize( - const Values& config, const Ordering& ordering) const +GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize(const Values& linearizationPoint) const { gttic(NonlinearFactorGraph_linearize); // create an empty linear FG - GaussianFactorGraph::shared_ptr linearFG(new GaussianFactorGraph); + GaussianFactorGraph::shared_ptr linearFG = boost::make_shared(); + +#ifdef GTSAM_USE_TBB + + linearFG->resize(this->size()); + TbbOpenMPMixedScope threadLimiter; // Limits OpenMP threads since we're mixing TBB and OpenMP + tbb::parallel_for(tbb::blocked_range(0, this->size()), + _LinearizeOneFactor(*this, linearizationPoint, *linearFG)); + +#else + linearFG->reserve(this->size()); // linearize all factors BOOST_FOREACH(const sharedFactor& factor, this->factors_) { if(factor) { - GaussianFactor::shared_ptr lf = factor->linearize(config, ordering); - if (lf) linearFG->push_back(lf); + (*linearFG) += factor->linearize(linearizationPoint); } else - linearFG->push_back(GaussianFactor::shared_ptr()); + (*linearFG) += GaussianFactor::shared_ptr(); } +#endif + return linearFG; } diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h index 0bb9e4bde..35e532262 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.h +++ b/gtsam/nonlinear/NonlinearFactorGraph.h @@ -22,12 +22,17 @@ #pragma once #include -#include -#include #include +#include namespace gtsam { + // Forward declarations + class Values; + class Ordering; + class GaussianFactorGraph; + class SymbolicFactorGraph; + /** * Formatting options when saving in GraphViz format using * NonlinearFactorGraph::saveGraph. @@ -40,13 +45,16 @@ namespace gtsam { double figureHeightInches; ///< The figure height on paper in inches double scale; ///< Scale all positions to reduce / increase density bool mergeSimilarFactors; ///< Merge multiple factors that have the same connectivity + bool plotFactorPoints; ///< Plots each factor as a dot between the variables + bool connectKeysToFactor; ///< Draw a line from each key within a factor to the dot of the factor std::map factorPositions; ///< (optional for each factor) Manually specify factor "dot" positions. /// Default constructor sets up robot coordinates. Paper horizontal is robot Y, /// paper vertical is robot X. Default figure size of 5x5 in. GraphvizFormatting() : paperHorizontalAxis(Y), paperVerticalAxis(X), figureWidthInches(5), figureHeightInches(5), scale(1), - mergeSimilarFactors(false) {} + mergeSimilarFactors(false), plotFactorPoints(true), + connectKeysToFactor(true) {} }; @@ -58,59 +66,58 @@ namespace gtsam { * tangent vector space at the linearization point. Because the tangent space is a true * vector space, the config type will be an VectorValues in that linearized factor graph. */ - class NonlinearFactorGraph: public FactorGraph { + class GTSAM_EXPORT NonlinearFactorGraph: public FactorGraph { public: typedef FactorGraph Base; - typedef boost::shared_ptr shared_ptr; - typedef boost::shared_ptr sharedFactor; + typedef NonlinearFactorGraph This; + typedef boost::shared_ptr shared_ptr; + + /** Default constructor */ + NonlinearFactorGraph() {} + + /** Construct from iterator over factors */ + template + NonlinearFactorGraph(ITERATOR firstFactor, ITERATOR lastFactor) : Base(firstFactor, lastFactor) {} + + /** Construct from container of factors (shared_ptr or plain objects) */ + template + explicit NonlinearFactorGraph(const CONTAINER& factors) : Base(factors) {} + + /** Implicit copy/downcast constructor to override explicit template container constructor */ + template + NonlinearFactorGraph(const FactorGraph& graph) : Base(graph) {} /** print just calls base class */ - GTSAM_EXPORT void print(const std::string& str = "NonlinearFactorGraph: ", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + void print(const std::string& str = "NonlinearFactorGraph: ", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + + /** Test equality */ + bool equals(const NonlinearFactorGraph& other, double tol = 1e-9) const; /** Write the graph in GraphViz format for visualization */ - GTSAM_EXPORT void saveGraph(std::ostream& stm, const Values& values = Values(), + void saveGraph(std::ostream& stm, const Values& values = Values(), const GraphvizFormatting& graphvizFormatting = GraphvizFormatting(), const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; /** return keys as an ordered set - ordering is by key value */ - GTSAM_EXPORT FastSet keys() const; + FastSet keys() const; /** unnormalized error, \f$ 0.5 \sum_i (h_i(X_i)-z)^2/\sigma^2 \f$ in the most common case */ - GTSAM_EXPORT double error(const Values& c) const; + double error(const Values& c) const; /** Unnormalized probability. O(n) */ - GTSAM_EXPORT double probPrime(const Values& c) const; - - /// Add a factor by value - copies the factor object - void add(const NonlinearFactor& factor) { - this->push_back(factor.clone()); - } - - /// Add a factor by pointer - stores pointer without copying factor object - void add(const sharedFactor& factor) { - this->push_back(factor); - } + double probPrime(const Values& c) const; /** - * Create a symbolic factor graph using an existing ordering + * Create a symbolic factor graph */ - GTSAM_EXPORT SymbolicFactorGraph::shared_ptr symbolic(const Ordering& ordering) const; - - /** - * Create a symbolic factor graph and initial variable ordering that can - * be used for graph operations like determining a fill-reducing ordering. - * The graph and ordering should be permuted after such a fill-reducing - * ordering is found. - */ - GTSAM_EXPORT std::pair - symbolic(const Values& config) const; + boost::shared_ptr symbolic() const; /** * Compute a fill-reducing ordering using COLAMD. */ - GTSAM_EXPORT Ordering::shared_ptr orderingCOLAMD(const Values& config) const; + Ordering orderingCOLAMD() const; /** * Compute a fill-reducing ordering with constraints using CCOLAMD @@ -120,19 +127,17 @@ namespace gtsam { * indices need to appear in the constraints, unconstrained is assumed for all * other variables */ - GTSAM_EXPORT Ordering::shared_ptr orderingCOLAMDConstrained(const Values& config, - const std::map& constraints) const; + Ordering orderingCOLAMDConstrained(const FastMap& constraints) const; /** * linearize a nonlinear factor graph */ - GTSAM_EXPORT boost::shared_ptr - linearize(const Values& config, const Ordering& ordering) const; + boost::shared_ptr linearize(const Values& linearizationPoint) const; /** * Clone() performs a deep-copy of the graph, including all of the factors */ - GTSAM_EXPORT NonlinearFactorGraph clone() const; + NonlinearFactorGraph clone() const; /** * Rekey() performs a deep-copy of all of the factors, and changes @@ -143,7 +148,7 @@ namespace gtsam { * @param rekey_mapping is a map of old->new keys * @result a cloned graph with updated keys */ - GTSAM_EXPORT NonlinearFactorGraph rekey(const std::map& rekey_mapping) const; + NonlinearFactorGraph rekey(const std::map& rekey_mapping) const; private: diff --git a/gtsam/nonlinear/NonlinearISAM.cpp b/gtsam/nonlinear/NonlinearISAM.cpp index 214c1c887..2405ca222 100644 --- a/gtsam/nonlinear/NonlinearISAM.cpp +++ b/gtsam/nonlinear/NonlinearISAM.cpp @@ -18,8 +18,7 @@ #include #include -#include -#include +#include #include @@ -32,12 +31,11 @@ namespace gtsam { /* ************************************************************************* */ void NonlinearISAM::saveGraph(const string& s, const KeyFormatter& keyFormatter) const { - isam_.saveGraph(s, OrderingIndexFormatter(ordering_, keyFormatter)); + isam_.saveGraph(s, keyFormatter); } /* ************************************************************************* */ -void NonlinearISAM::update(const NonlinearFactorGraph& newFactors, - const Values& initialValues) { +void NonlinearISAM::update(const NonlinearFactorGraph& newFactors, const Values& initialValues) { if(newFactors.size() > 0) { @@ -53,15 +51,10 @@ void NonlinearISAM::update(const NonlinearFactorGraph& newFactors, // TODO: optimize for whole config? linPoint_.insert(initialValues); - // Augment ordering - // TODO: allow for ordering constraints within the new variables - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, initialValues) - ordering_.insert(key_value.key, ordering_.size()); - - boost::shared_ptr linearizedNewFactors = newFactors.linearize(linPoint_, ordering_); + boost::shared_ptr linearizedNewFactors = newFactors.linearize(linPoint_); // Update ISAM - isam_.update(*linearizedNewFactors); + isam_.update(*linearizedNewFactors, eliminationFunction_); } } @@ -76,16 +69,10 @@ void NonlinearISAM::reorder_relinearize() { isam_.clear(); - // Compute an ordering - // TODO: allow for constrained ordering here - ordering_ = *factors_.orderingCOLAMD(newLinPoint); - - // Create a linear factor graph at the new linearization point - // TODO: decouple relinearization and reordering to avoid - boost::shared_ptr gfg = factors_.linearize(newLinPoint, ordering_); - // Just recreate the whole BayesTree - isam_.update(*gfg); + // TODO: allow for constrained ordering here + // TODO: decouple relinearization and reordering to avoid + isam_.update(*factors_.linearize(newLinPoint), eliminationFunction_); // Update linearization point linPoint_ = newLinPoint; @@ -95,14 +82,14 @@ void NonlinearISAM::reorder_relinearize() { /* ************************************************************************* */ Values NonlinearISAM::estimate() const { if(isam_.size() > 0) - return linPoint_.retract(optimize(isam_), ordering_); + return linPoint_.retract(isam_.optimize()); else return linPoint_; } /* ************************************************************************* */ Matrix NonlinearISAM::marginalCovariance(Key key) const { - return isam_.marginalCovariance(ordering_[key]); + return isam_.marginalCovariance(key); } /* ************************************************************************* */ @@ -110,7 +97,6 @@ void NonlinearISAM::print(const string& s, const KeyFormatter& keyFormatter) con cout << s << "ReorderInterval: " << reorderInterval_ << " Current Count: " << reorderCounter_ << endl; isam_.print("GaussianISAM:\n"); linPoint_.print("Linearization Point:\n", keyFormatter); - ordering_.print("System Ordering:\n", keyFormatter); factors_.print("Nonlinear Graph:\n", keyFormatter); } diff --git a/gtsam/nonlinear/NonlinearISAM.h b/gtsam/nonlinear/NonlinearISAM.h index ba7393d81..9c4b3d3c4 100644 --- a/gtsam/nonlinear/NonlinearISAM.h +++ b/gtsam/nonlinear/NonlinearISAM.h @@ -33,9 +33,6 @@ protected: /** The current linearization point */ Values linPoint_; - /** The ordering */ - gtsam::Ordering ordering_; - /** The original factors, used when relinearizing */ NonlinearFactorGraph factors_; @@ -43,6 +40,9 @@ protected: int reorderInterval_; int reorderCounter_; + /** The elimination function */ + GaussianFactorGraph::Eliminate eliminationFunction_; + public: /// @name Standard Constructors @@ -55,7 +55,9 @@ public: * 1 (default) reorders every time, in worse case is batch every update * typical values are 50 or 100 */ - NonlinearISAM(int reorderInterval = 1) : reorderInterval_(reorderInterval), reorderCounter_(0) {} + NonlinearISAM(int reorderInterval = 1, + const GaussianFactorGraph::Eliminate& eliminationFunction = GaussianFactorGraph::EliminationTraitsType::DefaultEliminate) : + reorderInterval_(reorderInterval), reorderCounter_(0), eliminationFunction_(eliminationFunction) {} /// @} /// @name Standard Interface @@ -75,9 +77,6 @@ public: /** Return the current linearization point */ const Values& getLinearizationPoint() const { return linPoint_; } - /** Get the ordering */ - const gtsam::Ordering& getOrdering() const { return ordering_; } - /** get underlying nonlinear graph */ const NonlinearFactorGraph& getFactorsUnsafe() const { return factors_; } @@ -104,15 +103,8 @@ public: /** Relinearization and reordering of variables */ void reorder_relinearize(); - /** manually add a key to the end of the ordering */ - void addKey(Key key) { ordering_.push_back(key); } - - /** replace the current ordering */ - void setOrdering(const Ordering& new_ordering) { ordering_ = new_ordering; } - /// @} }; } // \namespace gtsam - diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index 3b5ec556e..54e5cb9e3 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -16,52 +16,27 @@ * @date Jul 17, 2010 */ +#include + +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include #include #include -#include -#include + using namespace std; namespace gtsam { -/* ************************************************************************* */ -NonlinearOptimizerParams::Verbosity NonlinearOptimizerParams::verbosityTranslator(const std::string &src) const { - std::string s = src; boost::algorithm::to_upper(s); - if (s == "SILENT") return NonlinearOptimizerParams::SILENT; - if (s == "ERROR") return NonlinearOptimizerParams::ERROR; - if (s == "VALUES") return NonlinearOptimizerParams::VALUES; - if (s == "DELTA") return NonlinearOptimizerParams::DELTA; - if (s == "LINEAR") return NonlinearOptimizerParams::LINEAR; - - /* default is silent */ - return NonlinearOptimizerParams::SILENT; -} - -/* ************************************************************************* */ -std::string NonlinearOptimizerParams::verbosityTranslator(Verbosity value) const { - std::string s; - switch (value) { - case NonlinearOptimizerParams::SILENT: s = "SILENT"; break; - case NonlinearOptimizerParams::ERROR: s = "ERROR"; break; - case NonlinearOptimizerParams::VALUES: s = "VALUES"; break; - case NonlinearOptimizerParams::DELTA: s = "DELTA"; break; - case NonlinearOptimizerParams::LINEAR: s = "LINEAR"; break; - default: s = "UNDEFINED"; break; - } - return s; -} - -/* ************************************************************************* */ -void NonlinearOptimizerParams::print(const std::string& str) const { - std::cout << str << "\n"; - std::cout << "relative decrease threshold: " << relativeErrorTol << "\n"; - std::cout << "absolute decrease threshold: " << absoluteErrorTol << "\n"; - std::cout << " total error threshold: " << errorTol << "\n"; - std::cout << " maximum iterations: " << maxIterations << "\n"; - std::cout << " verbosity: " << verbosityTranslator(verbosity) << "\n"; - std::cout.flush(); -} - /* ************************************************************************* */ void NonlinearOptimizer::defaultOptimize() { @@ -80,8 +55,12 @@ void NonlinearOptimizer::defaultOptimize() { if (params.verbosity >= NonlinearOptimizerParams::ERROR) cout << "Initial error: " << currentError << endl; // Return if we already have too many iterations - if(this->iterations() >= params.maxIterations) + if(this->iterations() >= params.maxIterations){ + if (params.verbosity >= NonlinearOptimizerParams::TERMINATION) { + cout << "iterations: " << this->iterations() << " >? " << params.maxIterations << endl; + } return; + } // Iterative loop do { @@ -97,7 +76,7 @@ void NonlinearOptimizer::defaultOptimize() { params.errorTol, currentError, this->error(), params.verbosity)); // Printing if verbose - if (params.verbosity >= NonlinearOptimizerParams::ERROR && + if (params.verbosity >= NonlinearOptimizerParams::TERMINATION && this->iterations() >= params.maxIterations) cout << "Terminating because reached maximum iterations" << endl; } @@ -114,6 +93,36 @@ const Values& NonlinearOptimizer::optimizeSafely() { } } +/* ************************************************************************* */ +VectorValues NonlinearOptimizer::solve(const GaussianFactorGraph &gfg, + const Values& initial, const NonlinearOptimizerParams& params) const { + VectorValues delta; + if (params.isMultifrontal()) { + delta = gfg.optimize(*params.ordering, params.getEliminationFunction()); + } else if (params.isSequential()) { + delta = gfg.eliminateSequential(*params.ordering, + params.getEliminationFunction())->optimize(); + } else if (params.isCG()) { + if (!params.iterativeParams) + throw std::runtime_error( + "NonlinearOptimizer::solve: cg parameter has to be assigned ..."); + if (boost::dynamic_pointer_cast( + params.iterativeParams)) { + SubgraphSolver solver(gfg, + dynamic_cast(*params.iterativeParams), + *params.ordering); + delta = solver.optimize(); + } else { + throw std::runtime_error( + "NonlinearOptimizer::solve: special cg parameter type is not handled in LM solver ..."); + } + } else { + throw std::runtime_error( + "NonlinearOptimizer::solve: Optimization parameter is invalid"); + } + return delta; +} + /* ************************************************************************* */ bool checkConvergence(double relativeErrorTreshold, double absoluteErrorTreshold, double errorThreshold, double currentError, double newError, @@ -147,14 +156,20 @@ bool checkConvergence(double relativeErrorTreshold, double absoluteErrorTreshold } bool converged = (relativeErrorTreshold && (relativeDecrease <= relativeErrorTreshold)) || (absoluteDecrease <= absoluteErrorTreshold); - if (verbosity >= NonlinearOptimizerParams::ERROR && converged) { + if (verbosity >= NonlinearOptimizerParams::TERMINATION && converged) { + if(absoluteDecrease >= 0.0) cout << "converged" << endl; else cout << "Warning: stopping nonlinear iterations because error increased" << endl; + + cout << "errorThreshold: " << newError << " +#include namespace gtsam { class NonlinearOptimizer; -/** The common parameters for Nonlinear optimizers. Most optimizers - * deriving from NonlinearOptimizer also subclass the parameters. - */ -class GTSAM_EXPORT NonlinearOptimizerParams { -public: - /** See NonlinearOptimizerParams::verbosity */ - enum Verbosity { - SILENT, - ERROR, - VALUES, - DELTA, - LINEAR - }; - - size_t maxIterations; ///< The maximum iterations to stop iterating (default 100) - double relativeErrorTol; ///< The maximum relative error decrease to stop iterating (default 1e-5) - double absoluteErrorTol; ///< The maximum absolute error decrease to stop iterating (default 1e-5) - double errorTol; ///< The maximum total error to stop iterating (default 0.0) - Verbosity verbosity; ///< The printing verbosity during optimization (default SILENT) - - NonlinearOptimizerParams() : - maxIterations(100), relativeErrorTol(1e-5), absoluteErrorTol(1e-5), - errorTol(0.0), verbosity(SILENT) {} - - virtual ~NonlinearOptimizerParams() {} - virtual void print(const std::string& str = "") const ; - - size_t getMaxIterations() const { return maxIterations; } - double getRelativeErrorTol() const { return relativeErrorTol; } - double getAbsoluteErrorTol() const { return absoluteErrorTol; } - double getErrorTol() const { return errorTol; } - std::string getVerbosity() const { return verbosityTranslator(verbosity); } - - void setMaxIterations(size_t value) { maxIterations = value; } - void setRelativeErrorTol(double value) { relativeErrorTol = value; } - void setAbsoluteErrorTol(double value) { absoluteErrorTol = value; } - void setErrorTol(double value) { errorTol = value ; } - void setVerbosity(const std::string &src) { verbosity = verbosityTranslator(src); } - -private: - Verbosity verbosityTranslator(const std::string &s) const; - std::string verbosityTranslator(Verbosity value) const; -}; - /** * Base class for a nonlinear optimization state, including the current estimate * of the variable values, error, and number of iterations. Optimizers derived @@ -85,7 +42,7 @@ public: double error; /** The number of optimization iterations performed. */ - unsigned int iterations; + int iterations; NonlinearOptimizerState() {} @@ -209,7 +166,7 @@ public: double error() const { return _state().error; } /// return number of iterations - unsigned int iterations() const { return _state().iterations; } + int iterations() const { return _state().iterations; } /// return values const Values& values() const { return _state().values; } @@ -222,6 +179,10 @@ public: /** Virtual destructor */ virtual ~NonlinearOptimizer() {} + /** Default function to do linear solve, i.e. optimize a GaussianFactorGraph */ + virtual VectorValues solve(const GaussianFactorGraph &gfg, + const Values& initial, const NonlinearOptimizerParams& params) const; + /** Perform a single iteration, returning a new NonlinearOptimizer class * containing the updated variable assignments, which may be retrieved with * values(). @@ -251,6 +212,6 @@ protected: */ GTSAM_EXPORT bool checkConvergence(double relativeErrorTreshold, double absoluteErrorTreshold, double errorThreshold, - double currentError, double newError, NonlinearOptimizerParams::Verbosity verbosity); + double currentError, double newError, NonlinearOptimizerParams::Verbosity verbosity = NonlinearOptimizerParams::SILENT); } // gtsam diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.cpp b/gtsam/nonlinear/NonlinearOptimizerParams.cpp new file mode 100644 index 000000000..7b0e73985 --- /dev/null +++ b/gtsam/nonlinear/NonlinearOptimizerParams.cpp @@ -0,0 +1,160 @@ +/** + * @file NonlinearOptimizerParams.cpp + * @brief Parameters for nonlinear optimization + * @date Jul 24, 2012 + * @author Yong-Dian Jian + * @author Richard Roberts + * @author Frank Dellaert + */ + +#include +#include + +namespace gtsam { + +/* ************************************************************************* */ +NonlinearOptimizerParams::Verbosity NonlinearOptimizerParams::verbosityTranslator( + const std::string &src) const { + std::string s = src; + boost::algorithm::to_upper(s); + if (s == "SILENT") + return NonlinearOptimizerParams::SILENT; + if (s == "ERROR") + return NonlinearOptimizerParams::ERROR; + if (s == "VALUES") + return NonlinearOptimizerParams::VALUES; + if (s == "DELTA") + return NonlinearOptimizerParams::DELTA; + if (s == "LINEAR") + return NonlinearOptimizerParams::LINEAR; + if (s == "TERMINATION") + return NonlinearOptimizerParams::TERMINATION; + + /* default is silent */ + return NonlinearOptimizerParams::SILENT; +} + +/* ************************************************************************* */ +std::string NonlinearOptimizerParams::verbosityTranslator( + Verbosity value) const { + std::string s; + switch (value) { + case NonlinearOptimizerParams::SILENT: + s = "SILENT"; + break; + case NonlinearOptimizerParams::TERMINATION: + s = "TERMINATION"; + break; + case NonlinearOptimizerParams::ERROR: + s = "ERROR"; + break; + case NonlinearOptimizerParams::VALUES: + s = "VALUES"; + break; + case NonlinearOptimizerParams::DELTA: + s = "DELTA"; + break; + case NonlinearOptimizerParams::LINEAR: + s = "LINEAR"; + break; + default: + s = "UNDEFINED"; + break; + } + return s; +} + +/* ************************************************************************* */ +void NonlinearOptimizerParams::setIterativeParams( + const SubgraphSolverParameters ¶ms) { + iterativeParams = boost::make_shared(params); +} + +/* ************************************************************************* */ +void NonlinearOptimizerParams::print(const std::string& str) const { + + //NonlinearOptimizerParams::print(str); + std::cout << str << "\n"; + std::cout << "relative decrease threshold: " << relativeErrorTol << "\n"; + std::cout << "absolute decrease threshold: " << absoluteErrorTol << "\n"; + std::cout << " total error threshold: " << errorTol << "\n"; + std::cout << " maximum iterations: " << maxIterations << "\n"; + std::cout << " verbosity: " << verbosityTranslator(verbosity) + << "\n"; + std::cout.flush(); + + switch (linearSolverType) { + case MULTIFRONTAL_CHOLESKY: + std::cout << " linear solver type: MULTIFRONTAL CHOLESKY\n"; + break; + case MULTIFRONTAL_QR: + std::cout << " linear solver type: MULTIFRONTAL QR\n"; + break; + case SEQUENTIAL_CHOLESKY: + std::cout << " linear solver type: SEQUENTIAL CHOLESKY\n"; + break; + case SEQUENTIAL_QR: + std::cout << " linear solver type: SEQUENTIAL QR\n"; + break; + case CHOLMOD: + std::cout << " linear solver type: CHOLMOD\n"; + break; + case CONJUGATE_GRADIENT: + std::cout << " linear solver type: CONJUGATE GRADIENT\n"; + break; + default: + std::cout << " linear solver type: (invalid)\n"; + break; + } + + if (ordering) + std::cout << " ordering: custom\n"; + else + std::cout << " ordering: COLAMD\n"; + + std::cout.flush(); +} + +/* ************************************************************************* */ +std::string NonlinearOptimizerParams::linearSolverTranslator( + LinearSolverType linearSolverType) const { + switch (linearSolverType) { + case MULTIFRONTAL_CHOLESKY: + return "MULTIFRONTAL_CHOLESKY"; + case MULTIFRONTAL_QR: + return "MULTIFRONTAL_QR"; + case SEQUENTIAL_CHOLESKY: + return "SEQUENTIAL_CHOLESKY"; + case SEQUENTIAL_QR: + return "SEQUENTIAL_QR"; + case CONJUGATE_GRADIENT: + return "CONJUGATE_GRADIENT"; + case CHOLMOD: + return "CHOLMOD"; + default: + throw std::invalid_argument( + "Unknown linear solver type in SuccessiveLinearizationOptimizer"); + } +} + +/* ************************************************************************* */ +NonlinearOptimizerParams::LinearSolverType NonlinearOptimizerParams::linearSolverTranslator( + const std::string& linearSolverType) const { + if (linearSolverType == "MULTIFRONTAL_CHOLESKY") + return MULTIFRONTAL_CHOLESKY; + if (linearSolverType == "MULTIFRONTAL_QR") + return MULTIFRONTAL_QR; + if (linearSolverType == "SEQUENTIAL_CHOLESKY") + return SEQUENTIAL_CHOLESKY; + if (linearSolverType == "SEQUENTIAL_QR") + return SEQUENTIAL_QR; + if (linearSolverType == "CONJUGATE_GRADIENT") + return CONJUGATE_GRADIENT; + if (linearSolverType == "CHOLMOD") + return CHOLMOD; + throw std::invalid_argument( + "Unknown linear solver type in SuccessiveLinearizationOptimizer"); +} +/* ************************************************************************* */ + +} // namespace diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h new file mode 100644 index 000000000..10d468384 --- /dev/null +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -0,0 +1,165 @@ +/* ---------------------------------------------------------------------------- + + * 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 NonlinearOptimizerParams.h + * @brief Parameters for nonlinear optimization + * @author Yong-Dian Jian + * @author Richard Roberts + * @author Frank Dellaert + * @date Apr 1, 2012 + */ + +#pragma once + +#include +#include +#include +#include + +namespace gtsam { + +/** The common parameters for Nonlinear optimizers. Most optimizers + * deriving from NonlinearOptimizer also subclass the parameters. + */ +class GTSAM_EXPORT NonlinearOptimizerParams { +public: + /** See NonlinearOptimizerParams::verbosity */ + enum Verbosity { + SILENT, TERMINATION, ERROR, VALUES, DELTA, LINEAR + }; + + int maxIterations; ///< The maximum iterations to stop iterating (default 100) + double relativeErrorTol; ///< The maximum relative error decrease to stop iterating (default 1e-5) + double absoluteErrorTol; ///< The maximum absolute error decrease to stop iterating (default 1e-5) + double errorTol; ///< The maximum total error to stop iterating (default 0.0) + Verbosity verbosity; ///< The printing verbosity during optimization (default SILENT) + + NonlinearOptimizerParams() : + maxIterations(100), relativeErrorTol(1e-5), absoluteErrorTol(1e-5), errorTol( + 0.0), verbosity(SILENT), linearSolverType(MULTIFRONTAL_CHOLESKY) { + } + + virtual ~NonlinearOptimizerParams() { + } + virtual void print(const std::string& str = "") const; + + int getMaxIterations() const { + return maxIterations; + } + double getRelativeErrorTol() const { + return relativeErrorTol; + } + double getAbsoluteErrorTol() const { + return absoluteErrorTol; + } + double getErrorTol() const { + return errorTol; + } + std::string getVerbosity() const { + return verbosityTranslator(verbosity); + } + + void setMaxIterations(int value) { + maxIterations = value; + } + void setRelativeErrorTol(double value) { + relativeErrorTol = value; + } + void setAbsoluteErrorTol(double value) { + absoluteErrorTol = value; + } + void setErrorTol(double value) { + errorTol = value; + } + void setVerbosity(const std::string &src) { + verbosity = verbosityTranslator(src); + } + +private: + Verbosity verbosityTranslator(const std::string &s) const; + std::string verbosityTranslator(Verbosity value) const; + + // Successive Linearization Parameters + +public: + + /** See NonlinearOptimizerParams::linearSolverType */ + + enum LinearSolverType { + MULTIFRONTAL_CHOLESKY, + MULTIFRONTAL_QR, + SEQUENTIAL_CHOLESKY, + SEQUENTIAL_QR, + CONJUGATE_GRADIENT, /* Experimental Flag */ + CHOLMOD, /* Experimental Flag */ + }; + + LinearSolverType linearSolverType; ///< The type of linear solver to use in the nonlinear optimizer + boost::optional ordering; ///< The variable elimination ordering, or empty to use COLAMD (default: empty) + IterativeOptimizationParameters::shared_ptr iterativeParams; ///< The container for iterativeOptimization parameters. used in CG Solvers. + + inline bool isMultifrontal() const { + return (linearSolverType == MULTIFRONTAL_CHOLESKY) + || (linearSolverType == MULTIFRONTAL_QR); + } + + inline bool isSequential() const { + return (linearSolverType == SEQUENTIAL_CHOLESKY) + || (linearSolverType == SEQUENTIAL_QR); + } + + inline bool isCholmod() const { + return (linearSolverType == CHOLMOD); + } + + inline bool isCG() const { + return (linearSolverType == CONJUGATE_GRADIENT); + } + + GaussianFactorGraph::Eliminate getEliminationFunction() const { + switch (linearSolverType) { + case MULTIFRONTAL_CHOLESKY: + case SEQUENTIAL_CHOLESKY: + return EliminatePreferCholesky; + + case MULTIFRONTAL_QR: + case SEQUENTIAL_QR: + return EliminateQR; + + default: + throw std::runtime_error( + "Nonlinear optimization parameter \"factorization\" is invalid"); + } + } + + std::string getLinearSolverType() const { + return linearSolverTranslator(linearSolverType); + } + + void setLinearSolverType(const std::string& solver) { + linearSolverType = linearSolverTranslator(solver); + } + void setIterativeParams(const SubgraphSolverParameters& params); + void setOrdering(const Ordering& ordering) { + this->ordering = ordering; + } + +private: + std::string linearSolverTranslator(LinearSolverType linearSolverType) const; + LinearSolverType linearSolverTranslator( + const std::string& linearSolverType) const; +}; + +// For backward compatibility: +typedef NonlinearOptimizerParams SuccessiveLinearizationParams; + +} /* namespace gtsam */ diff --git a/gtsam/nonlinear/Ordering.cpp b/gtsam/nonlinear/Ordering.cpp deleted file mode 100644 index 4ab3a2b75..000000000 --- a/gtsam/nonlinear/Ordering.cpp +++ /dev/null @@ -1,130 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 Ordering.cpp - * @author Richard Roberts - * @date Sep 2, 2010 - */ - -#include "Ordering.h" - -#include -#include -#include - -using namespace std; - -namespace gtsam { - -/* ************************************************************************* */ -Ordering::Ordering(const std::list & L) { - int i = 0; - BOOST_FOREACH( Key s, L ) - insert(s, i++) ; -} - -/* ************************************************************************* */ -Ordering::Ordering(const Ordering& other) : order_(other.order_), orderingIndex_(other.size()) { - for(iterator item = order_.begin(); item != order_.end(); ++item) - orderingIndex_[item->second] = item; -} - -/* ************************************************************************* */ -Ordering& Ordering::operator=(const Ordering& rhs) { - order_ = rhs.order_; - orderingIndex_.resize(rhs.size()); - for(iterator item = order_.begin(); item != order_.end(); ++item) - orderingIndex_[item->second] = item; - return *this; -} - -/* ************************************************************************* */ -void Ordering::permuteInPlace(const Permutation& permutation) { - gttic(Ordering_permuteInPlace); - // Allocate new index and permute in map iterators - OrderingIndex newIndex(permutation.size()); - for(size_t j = 0; j < newIndex.size(); ++j) { - newIndex[j] = orderingIndex_[permutation[j]]; // Assign the iterator - newIndex[j]->second = j; // Change the stored index to its permuted value - } - // Swap the new index into this Ordering class - orderingIndex_.swap(newIndex); -} - -/* ************************************************************************* */ -void Ordering::permuteInPlace(const Permutation& selector, const Permutation& permutation) { - if(selector.size() != permutation.size()) - throw invalid_argument("Ordering::permuteInPlace (partial permutation version) called with selector and permutation of different sizes."); - // Create new index the size of the permuted entries - OrderingIndex newIndex(selector.size()); - // Permute the affected entries into the new index - for(size_t dstSlot = 0; dstSlot < selector.size(); ++dstSlot) - newIndex[dstSlot] = orderingIndex_[selector[permutation[dstSlot]]]; - // Put the affected entries back in the new order and fix the indices - for(size_t slot = 0; slot < selector.size(); ++slot) { - orderingIndex_[selector[slot]] = newIndex[slot]; - orderingIndex_[selector[slot]]->second = selector[slot]; - } -} - -/* ************************************************************************* */ -void Ordering::print(const 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; - BOOST_FOREACH(const Map::iterator& index_key, orderingIndex_) { - if(index_key->second % varsPerLine != 0) - cout << ", "; - cout << index_key->second<< ":" << keyFormatter(index_key->first); - if(index_key->second % varsPerLine == varsPerLine - 1) { - cout << "\n"; - endedOnNewline = true; - } else { - endedOnNewline = false; - } - } - if(!endedOnNewline) - cout << "\n"; - cout.flush(); -} - -/* ************************************************************************* */ -bool Ordering::equals(const Ordering& rhs, double tol) const { - return order_ == rhs.order_; -} - -/* ************************************************************************* */ -Ordering::value_type Ordering::pop_back() { - iterator lastItem = orderingIndex_.back(); // Get the map iterator to the highest-index entry - value_type removed = *lastItem; // Save the key-index pair to return - order_.erase(lastItem); // Erase the entry from the map - orderingIndex_.pop_back(); // Erase the entry from the index - return removed; // Return the removed item -} - -/* ************************************************************************* */ -void Unordered::print(const string& s) const { - cout << s << " (" << size() << "):"; - BOOST_FOREACH(Index key, *this) - cout << " " << key; - cout << endl; -} - -/* ************************************************************************* */ -bool Unordered::equals(const Unordered &other, double tol) const { - return *this == other; -} - -} diff --git a/gtsam/nonlinear/Ordering.h b/gtsam/nonlinear/Ordering.h deleted file mode 100644 index bc69ff1bd..000000000 --- a/gtsam/nonlinear/Ordering.h +++ /dev/null @@ -1,277 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 Ordering.h - * @author Richard Roberts - * @date Sep 2, 2010 - */ - -#pragma once - -#include -#include -#include - -#include -#include -#include -#include - -namespace gtsam { - -/** - * An ordering is a map from symbols (non-typed keys) to integer indices - * \nosubgrouping - */ -class GTSAM_EXPORT Ordering { -protected: - typedef FastMap Map; - typedef std::vector OrderingIndex; - Map order_; - OrderingIndex orderingIndex_; - -public: - - typedef boost::shared_ptr shared_ptr; - - typedef std::pair value_type; - typedef Map::iterator iterator; - typedef Map::const_iterator const_iterator; - - /// @name Standard Constructors - /// @{ - - /// Default constructor for empty ordering - Ordering() {} - - /// Copy constructor - Ordering(const Ordering& other); - - /// Construct from list, assigns order indices sequentially to list items. - Ordering(const std::list & L); - - /// Assignment operator - Ordering& operator=(const Ordering& rhs); - - /// @} - /// @name Standard Interface - /// @{ - - /** The actual number of variables in this ordering, i.e. not including missing indices in the count. See also nVars(). */ - Index size() const { return orderingIndex_.size(); } - - const_iterator begin() const { return order_.begin(); } /**< Iterator in order of sorted symbols, not in elimination/index order! */ - const_iterator end() const { return order_.end(); } /**< Iterator in order of sorted symbols, not in elimination/index order! */ - - Index at(Key key) const { return operator[](key); } ///< Synonym for operator[](Key) const - Key key(Index index) const { - if(index >= orderingIndex_.size()) - throw std::out_of_range("Attempting to access out-of-range index in Ordering"); - else - return orderingIndex_[index]->first; } - - /** Assigns the ordering index of the requested \c key into \c index if the symbol - * is present in the ordering, otherwise does not modify \c index. The - * return value indicates whether the symbol is in fact present in the - * ordering. - * @param key The key whose index you request - * @param [out] index Reference into which to write the index of the requested key, if the key is present. - * @return true if the key is present and \c index was modified, false otherwise. - */ - bool tryAt(Key key, Index& index) const { - const_iterator i = order_.find(key); - if(i != order_.end()) { - index = i->second; - return true; - } else - return false; } - - /// Access the index for the requested key, throws std::out_of_range if the - /// key is not present in the ordering (note that this differs from the - /// behavior of std::map) - Index& operator[](Key key) { - iterator i=order_.find(key); - if(i == order_.end()) throw std::out_of_range( - std::string("Attempting to access a key from an ordering that does not contain that key:") + DefaultKeyFormatter(key)); - else return i->second; } - - /// Access the index for the requested key, throws std::out_of_range if the - /// key is not present in the ordering (note that this differs from the - /// behavior of std::map) - Index operator[](Key key) const { - const_iterator i=order_.find(key); - if(i == order_.end()) throw std::out_of_range( - std::string("Attempting to access a key from an ordering that does not contain that key:") + DefaultKeyFormatter(key)); - else return i->second; } - - /** Returns an iterator pointing to the symbol/index pair with the requested, - * or the end iterator if it does not exist. - * - * @return An iterator pointing to the symbol/index pair with the requested, - * or the end iterator if it does not exist. - */ - iterator find(Key key) { return order_.find(key); } - - /** Returns an iterator pointing to the symbol/index pair with the requested, - * or the end iterator if it does not exist. - * - * @return An iterator pointing to the symbol/index pair with the requested, - * or the end iterator if it does not exist. - */ - const_iterator find(Key key) const { return order_.find(key); } - - /** Insert a key-index pair, but will fail if the key is already present */ - iterator insert(const value_type& key_order) { - std::pair it_ok(order_.insert(key_order)); - if(it_ok.second) { - if(key_order.second >= orderingIndex_.size()) - orderingIndex_.resize(key_order.second + 1); - orderingIndex_[key_order.second] = it_ok.first; - return it_ok.first; - } else - throw std::invalid_argument(std::string("Attempting to insert a key into an ordering that already contains that key")); } - - /// Test if the key exists in the ordering. - bool exists(Key key) const { return order_.count(key) > 0; } - - /** Insert a key-index pair, but will fail if the key is already present */ - iterator insert(Key key, Index order) { return insert(std::make_pair(key,order)); } - - /// Adds a new key to the ordering with an index of one greater than the current highest index. - Index push_back(Key key) { return insert(std::make_pair(key, orderingIndex_.size()))->second; } - - /// @} - /// @name Advanced Interface - /// @{ - - /** - * Iterator in order of sorted symbols, not in elimination/index order! - */ - iterator begin() { return order_.begin(); } - - /** - * Iterator in order of sorted symbols, not in elimination/index order! - */ - iterator end() { return order_.end(); } - - /** Remove the last (last-ordered, not highest-sorting key) symbol/index pair - * from the ordering (this version is \f$ O(n) \f$, use it when you do not - * know the last-ordered key). - * - * If you already know the last-ordered symbol, call popback(Key) - * that accepts this symbol as an argument. - * - * @return The symbol and index that were removed. - */ - value_type pop_back(); - - /** - * += operator allows statements like 'ordering += x0,x1,x2,x3;', which are - * very useful for unit tests. This functionality is courtesy of - * boost::assign. - */ - inline boost::assign::list_inserter, Key> - operator+=(Key key) { - return boost::assign::make_list_inserter(boost::assign_detail::call_push_back(*this))(key); } - - /** - * Reorder the variables with a permutation. This is typically used - * internally, permuting an initial key-sorted ordering into a fill-reducing - * ordering. - */ - void permuteInPlace(const Permutation& permutation); - - void permuteInPlace(const Permutation& selector, const Permutation& permutation); - - /// Synonym for operator[](Key) - Index& at(Key key) { return operator[](key); } - - /// @} - /// @name Testable - /// @{ - - /** print (from Testable) for testing and debugging */ - void print(const std::string& str = "Ordering:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; - - /** equals (from Testable) for testing and debugging */ - bool equals(const Ordering& rhs, double tol = 0.0) const; - - /// @} - -private: - - /** Serialization function */ - friend class boost::serialization::access; - template - void save(ARCHIVE & ar, const unsigned int version) const - { - ar & BOOST_SERIALIZATION_NVP(order_); - size_t size_ = orderingIndex_.size(); // Save only the size but not the iterators - ar & BOOST_SERIALIZATION_NVP(size_); - } - template - void load(ARCHIVE & ar, const unsigned int version) - { - ar & BOOST_SERIALIZATION_NVP(order_); - size_t size_; - ar & BOOST_SERIALIZATION_NVP(size_); - orderingIndex_.resize(size_); - for(iterator item = order_.begin(); item != order_.end(); ++item) - orderingIndex_[item->second] = item; // Assign the iterators - } - BOOST_SERIALIZATION_SPLIT_MEMBER() -}; // \class Ordering - -/** - * @class Unordered - * @brief a set of unordered indices - */ -class Unordered: public std::set { -public: - /** Default constructor creates empty ordering */ - Unordered() { } - - /** Create from a single symbol */ - Unordered(Index key) { insert(key); } - - /** Copy constructor */ - Unordered(const std::set& keys_in) : std::set(keys_in) {} - - /** whether a key exists */ - bool exists(const Index& key) { return find(key) != end(); } - - // Testable - GTSAM_EXPORT void print(const std::string& s = "Unordered") const; - GTSAM_EXPORT bool equals(const Unordered &t, double tol=0) const; -}; - -// Create an index formatter that looks up the Key in an inverse ordering, then -// formats the key using the provided key formatter, used in saveGraph. -class GTSAM_EXPORT OrderingIndexFormatter { -private: - Ordering ordering_; - KeyFormatter keyFormatter_; -public: - OrderingIndexFormatter(const Ordering& ordering, const KeyFormatter& keyFormatter) : - ordering_(ordering), keyFormatter_(keyFormatter) {} - std::string operator()(Index index) { - return keyFormatter_(ordering_.key(index)); } -}; - -/// Version of orderingIndexFormatter using multi-robot formatter -struct GTSAM_EXPORT MultiRobotLinearFormatter : gtsam::OrderingIndexFormatter { - MultiRobotLinearFormatter(const gtsam::Ordering& ordering) - : gtsam::OrderingIndexFormatter(ordering, MultiRobotKeyFormatter) {} -}; - -} // \namespace gtsam - diff --git a/gtsam/nonlinear/SuccessiveLinearizationOptimizer.cpp b/gtsam/nonlinear/SuccessiveLinearizationOptimizer.cpp deleted file mode 100644 index 0e8e4d748..000000000 --- a/gtsam/nonlinear/SuccessiveLinearizationOptimizer.cpp +++ /dev/null @@ -1,82 +0,0 @@ -/** - * @file SuccessiveLinearizationOptimizer.cpp - * @brief - * @date Jul 24, 2012 - * @author Yong-Dian Jian - */ - -#include -#include -#include -#include -#include -#include -#include - -namespace gtsam { - -void SuccessiveLinearizationParams::setIterativeParams(const SubgraphSolverParameters ¶ms) { - iterativeParams = boost::make_shared(params); -} - -void SuccessiveLinearizationParams::print(const std::string& str) const { - NonlinearOptimizerParams::print(str); - switch ( linearSolverType ) { - case MULTIFRONTAL_CHOLESKY: - std::cout << " linear solver type: MULTIFRONTAL CHOLESKY\n"; - break; - case MULTIFRONTAL_QR: - std::cout << " linear solver type: MULTIFRONTAL QR\n"; - break; - case SEQUENTIAL_CHOLESKY: - std::cout << " linear solver type: SEQUENTIAL CHOLESKY\n"; - break; - case SEQUENTIAL_QR: - std::cout << " linear solver type: SEQUENTIAL QR\n"; - break; - case CHOLMOD: - std::cout << " linear solver type: CHOLMOD\n"; - break; - case CONJUGATE_GRADIENT: - std::cout << " linear solver type: CONJUGATE GRADIENT\n"; - break; - default: - std::cout << " linear solver type: (invalid)\n"; - break; - } - - if(ordering) - std::cout << " ordering: custom\n"; - else - std::cout << " ordering: COLAMD\n"; - - std::cout.flush(); -} - -VectorValues solveGaussianFactorGraph(const GaussianFactorGraph &gfg, const SuccessiveLinearizationParams ¶ms) { - gttic(solveGaussianFactorGraph); - VectorValues delta; - if (params.isMultifrontal()) { - delta = GaussianJunctionTree(gfg).optimize(params.getEliminationFunction()); - } else if(params.isSequential()) { - const boost::shared_ptr gbn = - EliminationTree::Create(gfg)->eliminate(params.getEliminationFunction()); - delta = gtsam::optimize(*gbn); - } - else if ( params.isCG() ) { - if ( !params.iterativeParams ) throw std::runtime_error("solveGaussianFactorGraph: cg parameter has to be assigned ..."); - if ( boost::dynamic_pointer_cast(params.iterativeParams) ) { - SubgraphSolver solver (gfg, *boost::dynamic_pointer_cast(params.iterativeParams)); - delta = solver.optimize(); - } - else { - throw std::runtime_error("solveGaussianFactorGraph: special cg parameter type is not handled in LM solver ..."); - } - } - else { - throw std::runtime_error("solveGaussianFactorGraph: Optimization parameter is invalid"); - } - return delta; -} - -} diff --git a/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h b/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h deleted file mode 100644 index bdedd1987..000000000 --- a/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h +++ /dev/null @@ -1,106 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 SuccessiveLinearizationOptimizer.h - * @brief - * @author Richard Roberts - * @date Apr 1, 2012 - */ - -#pragma once - -#include -#include - -namespace gtsam { - -class GTSAM_EXPORT SuccessiveLinearizationParams : public NonlinearOptimizerParams { -public: - /** See SuccessiveLinearizationParams::linearSolverType */ - enum LinearSolverType { - MULTIFRONTAL_CHOLESKY, - MULTIFRONTAL_QR, - SEQUENTIAL_CHOLESKY, - SEQUENTIAL_QR, - CONJUGATE_GRADIENT, /* Experimental Flag */ - CHOLMOD, /* Experimental Flag */ - }; - - LinearSolverType linearSolverType; ///< The type of linear solver to use in the nonlinear optimizer - boost::optional ordering; ///< The variable elimination ordering, or empty to use COLAMD (default: empty) - IterativeOptimizationParameters::shared_ptr iterativeParams; ///< The container for iterativeOptimization parameters. used in CG Solvers. - - SuccessiveLinearizationParams() : linearSolverType(MULTIFRONTAL_CHOLESKY) {} - virtual ~SuccessiveLinearizationParams() {} - - inline bool isMultifrontal() const { - return (linearSolverType == MULTIFRONTAL_CHOLESKY) || (linearSolverType == MULTIFRONTAL_QR); } - - inline bool isSequential() const { - return (linearSolverType == SEQUENTIAL_CHOLESKY) || (linearSolverType == SEQUENTIAL_QR); } - - inline bool isCholmod() const { return (linearSolverType == CHOLMOD); } - - inline bool isCG() const { return (linearSolverType == CONJUGATE_GRADIENT); } - - virtual void print(const std::string& str) const; - - GaussianFactorGraph::Eliminate getEliminationFunction() const { - switch (linearSolverType) { - case MULTIFRONTAL_CHOLESKY: - case SEQUENTIAL_CHOLESKY: - return EliminatePreferCholesky; - - case MULTIFRONTAL_QR: - case SEQUENTIAL_QR: - return EliminateQR; - - default: - throw std::runtime_error("Nonlinear optimization parameter \"factorization\" is invalid"); - return EliminateQR; - break; - } - } - - std::string getLinearSolverType() const { return linearSolverTranslator(linearSolverType); } - - void setLinearSolverType(const std::string& solver) { linearSolverType = linearSolverTranslator(solver); } - void setIterativeParams(const SubgraphSolverParameters ¶ms); - void setOrdering(const Ordering& ordering) { this->ordering = ordering; } - -private: - std::string linearSolverTranslator(LinearSolverType linearSolverType) const { - switch(linearSolverType) { - case MULTIFRONTAL_CHOLESKY: return "MULTIFRONTAL_CHOLESKY"; - case MULTIFRONTAL_QR: return "MULTIFRONTAL_QR"; - case SEQUENTIAL_CHOLESKY: return "SEQUENTIAL_CHOLESKY"; - case SEQUENTIAL_QR: return "SEQUENTIAL_QR"; - case CONJUGATE_GRADIENT: return "CONJUGATE_GRADIENT"; - case CHOLMOD: return "CHOLMOD"; - default: throw std::invalid_argument("Unknown linear solver type in SuccessiveLinearizationOptimizer"); - } - } - LinearSolverType linearSolverTranslator(const std::string& linearSolverType) const { - if(linearSolverType == "MULTIFRONTAL_CHOLESKY") return MULTIFRONTAL_CHOLESKY; - if(linearSolverType == "MULTIFRONTAL_QR") return MULTIFRONTAL_QR; - if(linearSolverType == "SEQUENTIAL_CHOLESKY") return SEQUENTIAL_CHOLESKY; - if(linearSolverType == "SEQUENTIAL_QR") return SEQUENTIAL_QR; - if(linearSolverType == "CONJUGATE_GRADIENT") return CONJUGATE_GRADIENT; - if(linearSolverType == "CHOLMOD") return CHOLMOD; - throw std::invalid_argument("Unknown linear solver type in SuccessiveLinearizationOptimizer"); - } -}; - -/* a wrapper for solving a GaussianFactorGraph according to the parameters */ -GTSAM_EXPORT VectorValues solveGaussianFactorGraph(const GaussianFactorGraph &gfg, const SuccessiveLinearizationParams ¶ms) ; - -} /* namespace gtsam */ diff --git a/gtsam/nonlinear/Symbol.h b/gtsam/nonlinear/Symbol.h index 2c406be34..caac14bf7 100644 --- a/gtsam/nonlinear/Symbol.h +++ b/gtsam/nonlinear/Symbol.h @@ -11,154 +11,12 @@ /** * @file Symbol.h - * @date Jan 12, 2010 - * @author: Frank Dellaert - * @author: Richard Roberts + * @date Sept 1, 2013 + * @author Frank Dellaert + * @brief Symbol.h was moved to inference directory, this header was retained for compatibility */ #pragma once -#include -#include - -namespace gtsam { - -/** - * Character and index key used in VectorValues, GaussianFactorGraph, - * GaussianFactor, etc. These keys are generated at runtime from TypedSymbol - * keys when linearizing a nonlinear factor graph. This key is not type - * safe, so cannot be used with any Nonlinear* classes. - */ -class GTSAM_EXPORT Symbol { -protected: - unsigned char c_; - size_t j_; - -public: - - /** Default constructor */ - Symbol() : - c_(0), j_(0) { - } - - /** Copy constructor */ - Symbol(const Symbol& key) : - c_(key.c_), j_(key.j_) { - } - - /** Constructor */ - Symbol(unsigned char c, size_t j) : - c_(c), j_(j) { - } - - /** Constructor that decodes an integer Key */ - Symbol(Key key); - - /** return Key (integer) representation */ - Key key() const; - - /** Cast to integer */ - operator Key() const { return key(); } - - /// Print - void print(const std::string& s = "") const; - - /// Check equality - bool equals(const Symbol& expected, double tol = 0.0) const; - - /** Retrieve key character */ - unsigned char chr() const { - return c_; - } - - /** Retrieve key index */ - size_t index() const { - return j_; - } - - /** Create a string from the key */ - operator std::string() const; - - /** Comparison for use in maps */ - bool operator<(const Symbol& comp) const { - return c_ < comp.c_ || (comp.c_ == c_ && j_ < comp.j_); - } - - /** Comparison for use in maps */ - bool operator==(const Symbol& comp) const { - return comp.c_ == c_ && comp.j_ == j_; - } - - /** Comparison for use in maps */ - bool operator==(Key comp) const { - return comp == (Key)(*this); - } - - /** Comparison for use in maps */ - bool operator!=(const Symbol& comp) const { - return comp.c_ != c_ || comp.j_ != j_; - } - - /** Comparison for use in maps */ - bool operator!=(Key comp) const { - return comp != (Key)(*this); - } - - /** Return a filter function that returns true when evaluated on a Key whose - * character (when converted to a Symbol) matches \c c. Use this with the - * Values::filter() function to retrieve all key-value pairs with the - * requested character. - */ - static boost::function ChrTest(unsigned char c); - -private: - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(c_); - ar & BOOST_SERIALIZATION_NVP(j_); - } -}; - -/** Create a symbol key from a character and index, i.e. x5. */ -inline Key symbol(unsigned char c, size_t j) { return (Key)Symbol(c,j); } - -/** Return the character portion of a symbol key. */ -inline unsigned char symbolChr(Key key) { return Symbol(key).chr(); } - -/** Return the index portion of a symbol key. */ -inline size_t symbolIndex(Key key) { return Symbol(key).index(); } - -namespace symbol_shorthand { -inline Key A(size_t j) { return Symbol('a', j); } -inline Key B(size_t j) { return Symbol('b', j); } -inline Key C(size_t j) { return Symbol('c', j); } -inline Key D(size_t j) { return Symbol('d', j); } -inline Key E(size_t j) { return Symbol('e', j); } -inline Key F(size_t j) { return Symbol('f', j); } -inline Key G(size_t j) { return Symbol('g', j); } -inline Key H(size_t j) { return Symbol('h', j); } -inline Key I(size_t j) { return Symbol('i', j); } -inline Key J(size_t j) { return Symbol('j', j); } -inline Key K(size_t j) { return Symbol('k', j); } -inline Key L(size_t j) { return Symbol('l', j); } -inline Key M(size_t j) { return Symbol('m', j); } -inline Key N(size_t j) { return Symbol('n', j); } -inline Key O(size_t j) { return Symbol('o', j); } -inline Key P(size_t j) { return Symbol('p', j); } -inline Key Q(size_t j) { return Symbol('q', j); } -inline Key R(size_t j) { return Symbol('r', j); } -inline Key S(size_t j) { return Symbol('s', j); } -inline Key T(size_t j) { return Symbol('t', j); } -inline Key U(size_t j) { return Symbol('u', j); } -inline Key V(size_t j) { return Symbol('v', j); } -inline Key W(size_t j) { return Symbol('w', j); } -inline Key X(size_t j) { return Symbol('x', j); } -inline Key Y(size_t j) { return Symbol('y', j); } -inline Key Z(size_t j) { return Symbol('z', j); } -} - -} // namespace gtsam +#include diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index 162f353e8..1c96eed64 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -26,6 +26,8 @@ #include +#include + #include #include // Only so Eclipse finds class definition diff --git a/gtsam/nonlinear/Values.cpp b/gtsam/nonlinear/Values.cpp index b95e8932a..b92e54143 100644 --- a/gtsam/nonlinear/Values.cpp +++ b/gtsam/nonlinear/Values.cpp @@ -23,6 +23,7 @@ */ #include +#include #include @@ -76,51 +77,41 @@ namespace gtsam { } /* ************************************************************************* */ - VectorValues Values::zeroVectors(const Ordering& ordering) const { - return VectorValues::Zero(this->dims(ordering)); - } - - /* ************************************************************************* */ - Values Values::retract(const VectorValues& delta, const Ordering& ordering) const { + Values Values::retract(const VectorValues& delta) const + { Values result; for(const_iterator key_value = begin(); key_value != end(); ++key_value) { - const Vector& singleDelta = delta[ordering[key_value->key]]; // Delta for this value + VectorValues::const_iterator vector_item = delta.find(key_value->key); Key key = key_value->key; // Non-const duplicate to deal with non-const insert argument - Value* retractedValue(key_value->value.retract_(singleDelta)); // Retract - result.values_.insert(key, retractedValue); // Add retracted result directly to result values + if(vector_item != delta.end()) { +// const Vector& singleDelta = delta[key_value->key]; // Delta for this value + const Vector& singleDelta = vector_item->second; + Value* retractedValue(key_value->value.retract_(singleDelta)); // Retract + result.values_.insert(key, retractedValue); // Add retracted result directly to result values + } else { + result.values_.insert(key, key_value->value.clone_()); // Add original version to result values + } } return result; } /* ************************************************************************* */ - VectorValues Values::localCoordinates(const Values& cp, const Ordering& ordering) const { - VectorValues result(this->dims(ordering)); + VectorValues Values::localCoordinates(const Values& cp) const { if(this->size() != cp.size()) throw DynamicValuesMismatched(); + VectorValues result; for(const_iterator it1=this->begin(), it2=cp.begin(); it1!=this->end(); ++it1, ++it2) { if(it1->key != it2->key) throw DynamicValuesMismatched(); // If keys do not match // Will throw a dynamic_cast exception if types do not match // NOTE: this is separate from localCoordinates(cp, ordering, result) due to at() vs. insert - result.at(ordering[it1->key]) = it1->value.localCoordinates_(it2->value); + result.insert(it1->key, it1->value.localCoordinates_(it2->value)); } return result; } - /* ************************************************************************* */ - void Values::localCoordinates(const Values& cp, const Ordering& ordering, VectorValues& result) const { - if(this->size() != cp.size()) - throw DynamicValuesMismatched(); - for(const_iterator it1=this->begin(), it2=cp.begin(); it1!=this->end(); ++it1, ++it2) { - if(it1->key != it2->key) - throw DynamicValuesMismatched(); // If keys do not match - // Will throw a dynamic_cast exception if types do not match - result.insert(ordering[it1->key], it1->value.localCoordinates_(it2->value)); - } - } - /* ************************************************************************* */ const Value& Values::at(Key j) const { // Find the item @@ -134,8 +125,7 @@ namespace gtsam { /* ************************************************************************* */ void Values::insert(Key j, const Value& val) { - Key key = j; // Non-const duplicate to deal with non-const insert argument - std::pair insertResult = values_.insert(key, val.clone_()); + std::pair insertResult = tryInsert(j, val); if(!insertResult.second) throw ValuesKeyAlreadyExists(j); } @@ -148,6 +138,12 @@ namespace gtsam { } } + /* ************************************************************************* */ + std::pair Values::tryInsert(Key j, const Value& value) { + std::pair result = values_.insert(j, value.clone_()); + return std::make_pair(boost::make_transform_iterator(result.first, &make_deref_pair), result.second); + } + /* ************************************************************************* */ void Values::update(Key j, const Value& val) { // Find the value to update @@ -192,16 +188,6 @@ namespace gtsam { return *this; } - /* ************************************************************************* */ - vector Values::dims(const Ordering& ordering) const { - assert(ordering.size() == this->size()); // reads off of end of array if difference in size - vector result(values_.size()); - BOOST_FOREACH(const ConstKeyValuePair& key_value, *this) { - result[ordering[key_value.key]] = key_value.value.dim(); - } - return result; - } - /* ************************************************************************* */ size_t Values::dim() const { size_t result = 0; @@ -212,12 +198,11 @@ namespace gtsam { } /* ************************************************************************* */ - Ordering::shared_ptr Values::orderingArbitrary(Index firstVar) const { - Ordering::shared_ptr ordering(new Ordering); - for(const_iterator key_value = begin(); key_value != end(); ++key_value) { - ordering->insert(key_value->key, firstVar++); - } - return ordering; + VectorValues Values::zeroVectors() const { + VectorValues result; + BOOST_FOREACH(const ConstKeyValuePair& key_value, *this) + result.insert(key_value.key, Vector::Zero(key_value.value.dim())); + return result; } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index e1f8def88..e4680c801 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -24,12 +24,7 @@ #pragma once -#include -#include -#include -#include -#include - +#include #include #include #include @@ -49,9 +44,14 @@ #include #include +#include +#include +#include + namespace gtsam { // Forward declarations / utilities + class VectorValues; class ValueCloneAllocator; class ValueAutomaticCasting; template static bool _truePredicate(const T&) { return true; } @@ -200,15 +200,24 @@ namespace gtsam { * not found. */ const_iterator find(Key j) const { return boost::make_transform_iterator(values_.find(j), &make_const_deref_pair); } + /** Find the element greater than or equal to the specified key. */ + iterator lower_bound(Key j) { return boost::make_transform_iterator(values_.lower_bound(j), &make_deref_pair); } + + /** Find the element greater than or equal to the specified key. */ + const_iterator lower_bound(Key j) const { return boost::make_transform_iterator(values_.lower_bound(j), &make_const_deref_pair); } + + /** Find the lowest-ordered element greater than the specified key. */ + iterator upper_bound(Key j) { return boost::make_transform_iterator(values_.upper_bound(j), &make_deref_pair); } + + /** Find the lowest-ordered element greater than the specified key. */ + const_iterator upper_bound(Key j) const { return boost::make_transform_iterator(values_.upper_bound(j), &make_const_deref_pair); } + /** The number of variables in this config */ size_t size() const { return values_.size(); } /** whether the config is empty */ bool empty() const { return values_.empty(); } - /** Get a zero VectorValues of the correct structure */ - VectorValues zeroVectors(const Ordering& ordering) const; - const_iterator begin() const { return boost::make_transform_iterator(values_.begin(), &make_const_deref_pair); } const_iterator end() const { return boost::make_transform_iterator(values_.end(), &make_const_deref_pair); } iterator begin() { return boost::make_transform_iterator(values_.begin(), &make_deref_pair); } @@ -222,13 +231,10 @@ namespace gtsam { /// @{ /** Add a delta config to current config and returns a new config */ - Values retract(const VectorValues& delta, const Ordering& ordering) const; + Values retract(const VectorValues& delta) const; /** Get a delta config about a linearization point c0 (*this) */ - VectorValues localCoordinates(const Values& cp, const Ordering& ordering) const; - - /** Get a delta config about a linearization point c0 (*this) - assumes uninitialized delta */ - void localCoordinates(const Values& cp, const Ordering& ordering, VectorValues& delta) const; + VectorValues localCoordinates(const Values& cp) const; ///@} @@ -238,6 +244,12 @@ namespace gtsam { /** Add a set of variables, throws KeyAlreadyExists if a key is already present */ void insert(const Values& values); + /** insert that mimics the STL map insert - if the value already exists, the map is not modified + * and an iterator to the existing value is returned, along with 'false'. If the value did not + * exist, it is inserted and an iterator pointing to the new element, along with 'true', is + * returned. */ + std::pair tryInsert(Key j, const Value& value); + /** single element change of existing element */ void update(Key j, const Value& val); @@ -262,19 +274,11 @@ namespace gtsam { /** Remove all variables from the config */ void clear() { values_.clear(); } - /** Create an array of variable dimensions using the given ordering (\f$ O(n) \f$) */ - std::vector dims(const Ordering& ordering) const; - /** Compute the total dimensionality of all values (\f$ O(n) \f$) */ size_t dim() const; - /** - * Generate a default ordering, simply in key sort order. To instead - * create a fill-reducing ordering, use - * NonlinearFactorGraph::orderingCOLAMD(). Alternatively, you may permute - * this ordering yourself (as orderingCOLAMD() does internally). - */ - Ordering::shared_ptr orderingArbitrary(Index firstVar = 0) const; + /** Return a VectorValues of zero vectors for each variable in this Values */ + VectorValues zeroVectors() const; /** * Return a filtered view of this Values class, without copying any data. diff --git a/gtsam/nonlinear/WhiteNoiseFactor.h b/gtsam/nonlinear/WhiteNoiseFactor.h index 910444cf8..852ad147c 100644 --- a/gtsam/nonlinear/WhiteNoiseFactor.h +++ b/gtsam/nonlinear/WhiteNoiseFactor.h @@ -72,14 +72,14 @@ namespace gtsam { * So f = 2 f(x), g = -df(x), G = ddf(x) */ static HessianFactor::shared_ptr linearize(double z, double u, double p, - Index j1, Index j2) { + Key j1, Key j2) { double e = u - z, e2 = e * e; double c = 2 * logSqrt2PI - log(p) + e2 * p; - Vector g1 = Vector_(1, -e * p); - Vector g2 = Vector_(1, 0.5 / p - 0.5 * e2); - Matrix G11 = Matrix_(1, 1, p); - Matrix G12 = Matrix_(1, 1, e); - Matrix G22 = Matrix_(1, 1, 0.5 / (p * p)); + Vector g1 = (Vector(1) << -e * p); + Vector g2 = (Vector(1) << 0.5 / p - 0.5 * e2); + Matrix G11 = (Matrix(1, 1) << p); + Matrix G12 = (Matrix(1, 1) << e); + Matrix G22 = (Matrix(1, 1) << 0.5 / (p * p)); return HessianFactor::shared_ptr( new HessianFactor(j1, j2, G11, G12, g1, G22, g2, c)); } @@ -137,29 +137,28 @@ namespace gtsam { * TODO: Where is this used? should disappear. */ virtual Vector unwhitenedError(const Values& x) const { - return Vector_(1, std::sqrt(2 * error(x))); + return (Vector(1) << std::sqrt(2 * error(x))); } /** * Create a symbolic factor using the given ordering to determine the * variable indices. */ - virtual IndexFactor::shared_ptr symbolic(const Ordering& ordering) const { - const Index j1 = ordering[meanKey_], j2 = ordering[precisionKey_]; - return IndexFactor::shared_ptr(new IndexFactor(j1, j2)); - } +// virtual IndexFactor::shared_ptr symbolic(const Ordering& ordering) const { +// const Key j1 = ordering[meanKey_], j2 = ordering[precisionKey_]; +// return IndexFactor::shared_ptr(new IndexFactor(j1, j2)); +// } /// @} /// @name Advanced Interface /// @{ /// linearize returns a Hessianfactor that is an approximation of error(p) - virtual boost::shared_ptr linearize(const Values& x, - const Ordering& ordering) const { + virtual boost::shared_ptr linearize(const Values& x) const { double u = x.at(meanKey_); double p = x.at(precisionKey_); - Index j1 = ordering[meanKey_]; - Index j2 = ordering[precisionKey_]; + Key j1 = meanKey_; + Key j2 = precisionKey_; return linearize(z_, u, p, j1, j2); } diff --git a/gtsam/nonlinear/nonlinearExceptions.h b/gtsam/nonlinear/nonlinearExceptions.h index 6f512f65f..413610f82 100644 --- a/gtsam/nonlinear/nonlinearExceptions.h +++ b/gtsam/nonlinear/nonlinearExceptions.h @@ -17,10 +17,11 @@ */ #pragma once -#include #include #include +#include + namespace gtsam { /** diff --git a/gtsam/nonlinear/summarization.cpp b/gtsam/nonlinear/summarization.cpp deleted file mode 100644 index acd222d43..000000000 --- a/gtsam/nonlinear/summarization.cpp +++ /dev/null @@ -1,86 +0,0 @@ -/** - * @file summarization.cpp - * - * @date Jun 22, 2012 - * @author Alex Cunningham - */ - -#include -#include -#include - -using namespace std; - -namespace gtsam { - -/* ************************************************************************* */ -std::pair -summarize(const NonlinearFactorGraph& graph, const Values& values, - const KeySet& saved_keys, SummarizationMode mode) { - const size_t nrEliminatedKeys = values.size() - saved_keys.size(); - - // If we aren't eliminating anything, linearize and return - if (!nrEliminatedKeys || saved_keys.empty()) { - Ordering ordering = *values.orderingArbitrary(); - GaussianFactorGraph linear_graph = *graph.linearize(values, ordering); - return make_pair(linear_graph, ordering); - } - - // Compute a constrained ordering with variables grouped to end - std::map ordering_constraints; - - // group all saved variables together - BOOST_FOREACH(const gtsam::Key& key, saved_keys) - ordering_constraints.insert(make_pair(key, 1)); - - Ordering ordering = *graph.orderingCOLAMDConstrained(values, ordering_constraints); - - // Linearize the system - GaussianFactorGraph full_graph = *graph.linearize(values, ordering); - GaussianFactorGraph summarized_system; - - std::vector indices; - BOOST_FOREACH(const Key& k, saved_keys) - indices.push_back(ordering[k]); - - // PARTIAL_QR = 0, /// Uses QR solver to eliminate, does not require fully constrained system - // PARTIAL_CHOLESKY = 1, /// Uses Cholesky solver, does not require fully constrained system - // SEQUENTIAL_QR = 2, /// Uses QR to compute full joint graph (needs fully constrained system) - // SEQUENTIAL_CHOLESKY = 3 /// Uses Cholesky to compute full joint graph (needs fully constrained system) - - switch (mode) { - case PARTIAL_QR: { - summarized_system.push_back(EliminateQR(full_graph, nrEliminatedKeys).second); - break; - } - case PARTIAL_CHOLESKY: { - summarized_system.push_back(EliminateCholesky(full_graph, nrEliminatedKeys).second); - break; - } - case SEQUENTIAL_QR: { - GaussianSequentialSolver solver(full_graph, true); - summarized_system.push_back(*solver.jointFactorGraph(indices)); - break; - } - case SEQUENTIAL_CHOLESKY: { - GaussianSequentialSolver solver(full_graph, false); - summarized_system.push_back(*solver.jointFactorGraph(indices)); - break; - } - } - return make_pair(summarized_system, ordering); -} - -/* ************************************************************************* */ -NonlinearFactorGraph summarizeAsNonlinearContainer( - const NonlinearFactorGraph& graph, const Values& values, - const KeySet& saved_keys, SummarizationMode mode) { - GaussianFactorGraph summarized_graph; - Ordering ordering; - boost::tie(summarized_graph, ordering) = summarize(graph, values, saved_keys, mode); - return LinearContainerFactor::convertLinearGraph(summarized_graph, ordering); -} - -/* ************************************************************************* */ -} // \namespace gtsam - diff --git a/gtsam/nonlinear/summarization.h b/gtsam/nonlinear/summarization.h deleted file mode 100644 index c25d5d934..000000000 --- a/gtsam/nonlinear/summarization.h +++ /dev/null @@ -1,61 +0,0 @@ -/** - * @file summarization.h - * - * @brief Types and utility functions for summarization - * - * @date Jun 22, 2012 - * @author Alex Cunningham - */ - -#pragma once - -#include -#include - -#include -#include - -namespace gtsam { - -// Flag to control how summarization should be performed -typedef enum { - PARTIAL_QR = 0, /// Uses QR solver to eliminate, does not require fully constrained system - PARTIAL_CHOLESKY = 1, /// Uses Cholesky solver, does not require fully constrained system - SEQUENTIAL_QR = 2, /// Uses QR to compute full joint graph (needs fully constrained system) - SEQUENTIAL_CHOLESKY = 3 /// Uses Cholesky to compute full joint graph (needs fully constrained system) -} SummarizationMode; - -/** - * Summarization function to remove a subset of variables from a system with the - * sequential solver. This does not require that the system be fully constrained. - * - * Requirement: set of keys in the graph should match the set of keys in the - * values structure. - * - * @param graph A full nonlinear graph - * @param values The chosen linearization point - * @param saved_keys is the set of keys for variables that should remain - * @param mode controls what elimination technique and requirements to use - * @return a pair of the remaining graph and the ordering used for linearization - */ -std::pair GTSAM_EXPORT -summarize(const NonlinearFactorGraph& graph, const Values& values, - const KeySet& saved_keys, SummarizationMode mode = PARTIAL_QR); - -/** - * Performs the same summarization technique used in summarize(), but returns the - * result as a NonlinearFactorGraph comprised of LinearContainerFactors. - * - * @param graph A full nonlinear graph - * @param values The chosen linearization point - * @param saved_keys is the set of keys for variables that should remain - * @param useQR uses QR as the elimination algorithm if true, Cholesky otherwise - * @return a NonlinearFactorGraph with linear factors - */ -NonlinearFactorGraph GTSAM_EXPORT summarizeAsNonlinearContainer( - const NonlinearFactorGraph& graph, const Values& values, - const KeySet& saved_keys, SummarizationMode mode = PARTIAL_QR); - -} // \namespace gtsam - - diff --git a/gtsam/nonlinear/tests/CMakeLists.txt b/gtsam/nonlinear/tests/CMakeLists.txt new file mode 100644 index 000000000..69a3700f2 --- /dev/null +++ b/gtsam/nonlinear/tests/CMakeLists.txt @@ -0,0 +1 @@ +gtsamAddTestsGlob(nonlinear "test*.cpp" "" "gtsam") diff --git a/gtsam/nonlinear/tests/testLinearContainerFactor.cpp b/gtsam/nonlinear/tests/testLinearContainerFactor.cpp index 6c4617171..fd70519dc 100644 --- a/gtsam/nonlinear/tests/testLinearContainerFactor.cpp +++ b/gtsam/nonlinear/tests/testLinearContainerFactor.cpp @@ -8,6 +8,8 @@ #include #include #include +#include +#include #include #include #include @@ -17,7 +19,7 @@ using namespace std; using namespace boost::assign; using namespace gtsam; -const gtsam::noiseModel::Diagonal::shared_ptr diag_model2 = noiseModel::Diagonal::Sigmas(Vector_(2, 1.0, 1.0)); +const gtsam::noiseModel::Diagonal::shared_ptr diag_model2 = noiseModel::Diagonal::Sigmas((Vector(2) << 1.0, 1.0)); const double tol = 1e-5; gtsam::Key l1 = 101, l2 = 102, x1 = 1, x2 = 2; @@ -28,26 +30,24 @@ Pose2 poseA1(0.0, 0.0, 0.0), poseA2(2.0, 0.0, 0.0); /* ************************************************************************* */ TEST( testLinearContainerFactor, generic_jacobian_factor ) { - Ordering initOrdering; initOrdering += x1, x2, l1, l2; - - Matrix A1 = Matrix_(2,2, + Matrix A1 = (Matrix(2, 2) << 2.74222, -0.0067457, 0.0, 2.63624); - Matrix A2 = Matrix_(2,2, + Matrix A2 = (Matrix(2, 2) << -0.0455167, -0.0443573, -0.0222154, -0.102489); - Vector b = Vector_(2, 0.0277052, + Vector b = (Vector(2) << 0.0277052, -0.0533393); - JacobianFactor expLinFactor(initOrdering[l1], A1, initOrdering[l2], A2, b, diag_model2); + JacobianFactor expLinFactor(l1, A1, l2, A2, b, diag_model2); - LinearContainerFactor actFactor(expLinFactor, initOrdering); + LinearContainerFactor actFactor(expLinFactor); EXPECT_LONGS_EQUAL(2, actFactor.size()); EXPECT(actFactor.isJacobian()); EXPECT(!actFactor.isHessian()); // check keys - std::vector expKeys; expKeys += l1, l2; + FastVector expKeys; expKeys += l1, l2; EXPECT(assert_container_equality(expKeys, actFactor.keys())); Values values; @@ -56,32 +56,24 @@ TEST( testLinearContainerFactor, generic_jacobian_factor ) { values.insert(x1, poseA1); values.insert(x2, poseA2); - // Check reconstruction from same ordering - GaussianFactor::shared_ptr actLinearizationA = actFactor.linearize(values, initOrdering); + // Check reconstruction + GaussianFactor::shared_ptr actLinearizationA = actFactor.linearize(values); EXPECT(assert_equal(*expLinFactor.clone(), *actLinearizationA, tol)); - - // Check reconstruction from new ordering - Ordering newOrdering; newOrdering += x1, l1, x2, l2; - GaussianFactor::shared_ptr actLinearizationB = actFactor.linearize(values, newOrdering); - JacobianFactor expLinFactor2(newOrdering[l1], A1, newOrdering[l2], A2, b, diag_model2); - EXPECT(assert_equal(*expLinFactor2.clone(), *actLinearizationB, tol)); } /* ************************************************************************* */ TEST( testLinearContainerFactor, jacobian_factor_withlinpoints ) { - Ordering ordering; ordering += x1, x2, l1, l2; - - Matrix A1 = Matrix_(2,2, + Matrix A1 = (Matrix(2, 2) << 2.74222, -0.0067457, 0.0, 2.63624); - Matrix A2 = Matrix_(2,2, + Matrix A2 = (Matrix(2, 2) << -0.0455167, -0.0443573, -0.0222154, -0.102489); - Vector b = Vector_(2, 0.0277052, + Vector b = (Vector(2) << 0.0277052, -0.0533393); - JacobianFactor expLinFactor(ordering[l1], A1, ordering[l2], A2, b, diag_model2); + JacobianFactor expLinFactor(l1, A1, l2, A2, b, diag_model2); Values values; values.insert(l1, landmark1); @@ -89,8 +81,8 @@ TEST( testLinearContainerFactor, jacobian_factor_withlinpoints ) { values.insert(x1, poseA1); values.insert(x2, poseA2); - LinearContainerFactor actFactor(expLinFactor, ordering, values); - LinearContainerFactor actFactorNolin(expLinFactor, ordering); + LinearContainerFactor actFactor(expLinFactor, values); + LinearContainerFactor actFactorNolin(expLinFactor); EXPECT(assert_equal(actFactor, actFactor, tol)); EXPECT(assert_inequal(actFactor, actFactorNolin, tol)); @@ -105,48 +97,46 @@ TEST( testLinearContainerFactor, jacobian_factor_withlinpoints ) { EXPECT(assert_equal(expLinPoint, *actFactor.linearizationPoint())); // Check error evaluation - Vector delta_l1 = Vector_(2, 1.0, 2.0); - Vector delta_l2 = Vector_(2, 3.0, 4.0); + Vector delta_l1 = (Vector(2) << 1.0, 2.0); + Vector delta_l2 = (Vector(2) << 3.0, 4.0); - VectorValues delta = values.zeroVectors(ordering); - delta.at(ordering[l1]) = delta_l1; - delta.at(ordering[l2]) = delta_l2; - Values noisyValues = values.retract(delta, ordering); + VectorValues delta = values.zeroVectors(); + delta.at(l1) = delta_l1; + delta.at(l2) = delta_l2; + Values noisyValues = values.retract(delta); double expError = expLinFactor.error(delta); EXPECT_DOUBLES_EQUAL(expError, actFactor.error(noisyValues), tol); - EXPECT_DOUBLES_EQUAL(expLinFactor.error(values.zeroVectors(ordering)), actFactor.error(values), tol); + EXPECT_DOUBLES_EQUAL(expLinFactor.error(values.zeroVectors()), actFactor.error(values), tol); // Check linearization with corrections for updated linearization point - Ordering newOrdering; newOrdering += x1, l1, x2, l2; - GaussianFactor::shared_ptr actLinearizationB = actFactor.linearize(noisyValues, newOrdering); + GaussianFactor::shared_ptr actLinearizationB = actFactor.linearize(noisyValues); Vector bprime = b - A1 * delta_l1 - A2 * delta_l2; - JacobianFactor expLinFactor2(newOrdering[l1], A1, newOrdering[l2], A2, bprime, diag_model2); + JacobianFactor expLinFactor2(l1, A1, l2, A2, bprime, diag_model2); EXPECT(assert_equal(*expLinFactor2.clone(), *actLinearizationB, tol)); } /* ************************************************************************* */ TEST( testLinearContainerFactor, generic_hessian_factor ) { - Matrix G11 = Matrix_(1,1, 1.0); - Matrix G12 = Matrix_(1,2, 2.0, 4.0); - Matrix G13 = Matrix_(1,3, 3.0, 6.0, 9.0); + Matrix G11 = (Matrix(1, 1) << 1.0); + Matrix G12 = (Matrix(1, 2) << 2.0, 4.0); + Matrix G13 = (Matrix(1, 3) << 3.0, 6.0, 9.0); - Matrix G22 = Matrix_(2,2, 3.0, 5.0, + Matrix G22 = (Matrix(2, 2) << 3.0, 5.0, 0.0, 6.0); - Matrix G23 = Matrix_(2,3, 4.0, 6.0, 8.0, + Matrix G23 = (Matrix(2, 3) << 4.0, 6.0, 8.0, 1.0, 2.0, 4.0); - Matrix G33 = Matrix_(3,3, 1.0, 2.0, 3.0, + Matrix G33 = (Matrix(3, 3) << 1.0, 2.0, 3.0, 0.0, 5.0, 6.0, 0.0, 0.0, 9.0); - Vector g1 = Vector_(1, -7.0); - Vector g2 = Vector_(2, -8.0, -9.0); - Vector g3 = Vector_(3, 1.0, 2.0, 3.0); + Vector g1 = (Vector(1) << -7.0); + Vector g2 = (Vector(2) << -8.0, -9.0); + Vector g3 = (Vector(3) << 1.0, 2.0, 3.0); double f = 10.0; - Ordering initOrdering; initOrdering += x1, x2, l1, l2; - HessianFactor initFactor(initOrdering[x1], initOrdering[x2], initOrdering[l1], + HessianFactor initFactor(x1, x2, l1, G11, G12, G13, g1, G22, G23, g2, G33, g3, f); Values values; @@ -155,18 +145,12 @@ TEST( testLinearContainerFactor, generic_hessian_factor ) { values.insert(x1, poseA1); values.insert(x2, poseA2); - LinearContainerFactor actFactor(initFactor, initOrdering); + LinearContainerFactor actFactor(initFactor); EXPECT(!actFactor.isJacobian()); EXPECT(actFactor.isHessian()); - GaussianFactor::shared_ptr actLinearization1 = actFactor.linearize(values, initOrdering); + GaussianFactor::shared_ptr actLinearization1 = actFactor.linearize(values); EXPECT(assert_equal(*initFactor.clone(), *actLinearization1, tol)); - - Ordering newOrdering; newOrdering += l1, x1, x2, l2; - HessianFactor expLinFactor(newOrdering[x1], newOrdering[x2], newOrdering[l1], - G11, G12, G13, g1, G22, G23, g2, G33, g3, f); - GaussianFactor::shared_ptr actLinearization2 = actFactor.linearize(values, newOrdering); - EXPECT(assert_equal(*expLinFactor.clone(), *actLinearization2, tol)); } /* ************************************************************************* */ @@ -174,21 +158,21 @@ TEST( testLinearContainerFactor, hessian_factor_withlinpoints ) { // 2 variable example, one pose, one landmark (planar) // Initial ordering: x1, l1 - Matrix G11 = Matrix_(3,3, + Matrix G11 = (Matrix(3, 3) << 1.0, 2.0, 3.0, 0.0, 5.0, 6.0, 0.0, 0.0, 9.0); - Matrix G12 = Matrix_(3,2, + Matrix G12 = (Matrix(3, 2) << 1.0, 2.0, 3.0, 5.0, 4.0, 6.0); - Vector g1 = Vector_(3, 1.0, 2.0, 3.0); + Vector g1 = (Vector(3) << 1.0, 2.0, 3.0); - Matrix G22 = Matrix_(2,2, + Matrix G22 = (Matrix(2, 2) << 0.5, 0.2, 0.0, 0.6); - Vector g2 = Vector_(2, -8.0, -9.0); + Vector g2 = (Vector(2) << -8.0, -9.0); double f = 10.0; @@ -196,8 +180,7 @@ TEST( testLinearContainerFactor, hessian_factor_withlinpoints ) { Matrix G(5,5); G << G11, G12, Matrix::Zero(2,3), G22; - Ordering ordering; ordering += x1, x2, l1; - HessianFactor initFactor(ordering[x1], ordering[l1], G11, G12, g1, G22, g2, f); + HessianFactor initFactor(x1, l1, G11, G12, g1, G22, g2, f); Values linearizationPoint, expLinPoints; linearizationPoint.insert(l1, landmark1); @@ -205,7 +188,7 @@ TEST( testLinearContainerFactor, hessian_factor_withlinpoints ) { expLinPoints = linearizationPoint; linearizationPoint.insert(x2, poseA2); - LinearContainerFactor actFactor(initFactor, ordering, linearizationPoint); + LinearContainerFactor actFactor(initFactor, linearizationPoint); EXPECT(!actFactor.isJacobian()); EXPECT(actFactor.isHessian()); @@ -214,24 +197,24 @@ TEST( testLinearContainerFactor, hessian_factor_withlinpoints ) { EXPECT(assert_equal(expLinPoints, actLinPoint)); // Create delta - Vector delta_l1 = Vector_(2, 1.0, 2.0); - Vector delta_x1 = Vector_(3, 3.0, 4.0, 0.5); - Vector delta_x2 = Vector_(3, 6.0, 7.0, 0.3); + Vector delta_l1 = (Vector(2) << 1.0, 2.0); + Vector delta_x1 = (Vector(3) << 3.0, 4.0, 0.5); + Vector delta_x2 = (Vector(3) << 6.0, 7.0, 0.3); // Check error calculation - VectorValues delta = linearizationPoint.zeroVectors(ordering); - delta.at(ordering[l1]) = delta_l1; - delta.at(ordering[x1]) = delta_x1; - delta.at(ordering[x2]) = delta_x2; - EXPECT(assert_equal(Vector_(5, 3.0, 4.0, 0.5, 1.0, 2.0), delta.vector(initFactor.keys()))); - Values noisyValues = linearizationPoint.retract(delta, ordering); + VectorValues delta = linearizationPoint.zeroVectors(); + delta.at(l1) = delta_l1; + delta.at(x1) = delta_x1; + delta.at(x2) = delta_x2; + EXPECT(assert_equal((Vector(5) << 3.0, 4.0, 0.5, 1.0, 2.0), delta.vector(initFactor.keys()))); + Values noisyValues = linearizationPoint.retract(delta); double expError = initFactor.error(delta); EXPECT_DOUBLES_EQUAL(expError, actFactor.error(noisyValues), tol); - EXPECT_DOUBLES_EQUAL(initFactor.error(linearizationPoint.zeroVectors(ordering)), actFactor.error(linearizationPoint), tol); + EXPECT_DOUBLES_EQUAL(initFactor.error(linearizationPoint.zeroVectors()), actFactor.error(linearizationPoint), tol); // Compute updated versions - Vector dv = Vector_(5, 3.0, 4.0, 0.5, 1.0, 2.0); + Vector dv = (Vector(5) << 3.0, 4.0, 0.5, 1.0, 2.0); Vector g(5); g << g1, g2; Vector g_prime = g - G.selfadjointView() * dv; @@ -239,26 +222,19 @@ TEST( testLinearContainerFactor, hessian_factor_withlinpoints ) { Vector g1_prime = g_prime.head(3); Vector g2_prime = g_prime.tail(2); double f_prime = f + dv.transpose() * G.selfadjointView() * dv - 2.0 * dv.transpose() * g; - HessianFactor expNewFactor(ordering[x1], ordering[l1], G11, G12, g1_prime, G22, g2_prime, f_prime); - EXPECT(assert_equal(*expNewFactor.clone(), *actFactor.linearize(noisyValues, ordering), tol)); + HessianFactor expNewFactor(x1, l1, G11, G12, g1_prime, G22, g2_prime, f_prime); + EXPECT(assert_equal(*expNewFactor.clone(), *actFactor.linearize(noisyValues), tol)); } /* ************************************************************************* */ TEST( testLinearContainerFactor, creation ) { // Create a set of local keys (No robot label) - Key l1 = 11, l2 = 12, - l3 = 13, l4 = 14, - l5 = 15, l6 = 16, - l7 = 17, l8 = 18; - - // creating an ordering to decode the linearized factor - Ordering ordering; - ordering += l1,l2,l3,l4,l5,l6,l7,l8; + Key l1 = 11, l3 = 13, l5 = 15; // create a linear factor SharedDiagonal model = noiseModel::Unit::Create(2); JacobianFactor::shared_ptr linear_factor(new JacobianFactor( - ordering[l3], eye(2,2), ordering[l5], 2.0 * eye(2,2), zero(2), model)); + l3, eye(2,2), l5, 2.0 * eye(2,2), zero(2), model)); // create a set of values - build with full set of values gtsam::Values full_values, exp_values; @@ -267,10 +243,10 @@ TEST( testLinearContainerFactor, creation ) { exp_values = full_values; full_values.insert(l1, Point2(3.0, 7.0)); - LinearContainerFactor actual(linear_factor, ordering, full_values); + LinearContainerFactor actual(linear_factor, full_values); // Verify the keys - std::vector expKeys; + FastVector expKeys; expKeys += l3, l5; EXPECT(assert_container_equality(expKeys, actual.keys())); @@ -284,9 +260,6 @@ TEST( testLinearContainerFactor, jacobian_relinearize ) // Create a Between Factor from a Point3. This is actually a linear factor. gtsam::Key key1(1); gtsam::Key key2(2); - gtsam::Ordering ordering; - ordering.push_back(key1); - ordering.push_back(key2); gtsam::Values linpoint1; linpoint1.insert(key1, gtsam::Point3(-22.4, +8.5, +2.4)); linpoint1.insert(key2, gtsam::Point3(-21.0, +5.0, +21.0)); @@ -296,8 +269,8 @@ TEST( testLinearContainerFactor, jacobian_relinearize ) gtsam::BetweenFactor betweenFactor(key1, key2, measured, model); // Create a jacobian container factor at linpoint 1 - gtsam::JacobianFactor::shared_ptr jacobian(new gtsam::JacobianFactor(*betweenFactor.linearize(linpoint1, ordering))); - gtsam::LinearContainerFactor jacobianContainer(jacobian, ordering, linpoint1); + gtsam::JacobianFactor::shared_ptr jacobian(new gtsam::JacobianFactor(*betweenFactor.linearize(linpoint1))); + gtsam::LinearContainerFactor jacobianContainer(jacobian, linpoint1); // Create a second linearization point gtsam::Values linpoint2; @@ -310,8 +283,8 @@ TEST( testLinearContainerFactor, jacobian_relinearize ) EXPECT_DOUBLES_EQUAL(expected_error, actual_error, 1e-9 ); // Re-linearize around the new point and check the factors - gtsam::GaussianFactor::shared_ptr expected_factor = betweenFactor.linearize(linpoint2, ordering); - gtsam::GaussianFactor::shared_ptr actual_factor = jacobianContainer.linearize(linpoint2, ordering); + gtsam::GaussianFactor::shared_ptr expected_factor = betweenFactor.linearize(linpoint2); + gtsam::GaussianFactor::shared_ptr actual_factor = jacobianContainer.linearize(linpoint2); CHECK(gtsam::assert_equal(*expected_factor, *actual_factor)); } @@ -321,9 +294,6 @@ TEST( testLinearContainerFactor, hessian_relinearize ) // Create a Between Factor from a Point3. This is actually a linear factor. gtsam::Key key1(1); gtsam::Key key2(2); - gtsam::Ordering ordering; - ordering.push_back(key1); - ordering.push_back(key2); gtsam::Values linpoint1; linpoint1.insert(key1, gtsam::Point3(-22.4, +8.5, +2.4)); linpoint1.insert(key2, gtsam::Point3(-21.0, +5.0, +21.0)); @@ -333,8 +303,8 @@ TEST( testLinearContainerFactor, hessian_relinearize ) gtsam::BetweenFactor betweenFactor(key1, key2, measured, model); // Create a hessian container factor at linpoint 1 - gtsam::HessianFactor::shared_ptr hessian(new gtsam::HessianFactor(*betweenFactor.linearize(linpoint1, ordering))); - gtsam::LinearContainerFactor hessianContainer(hessian, ordering, linpoint1); + gtsam::HessianFactor::shared_ptr hessian(new gtsam::HessianFactor(*betweenFactor.linearize(linpoint1))); + gtsam::LinearContainerFactor hessianContainer(hessian, linpoint1); // Create a second linearization point gtsam::Values linpoint2; @@ -347,8 +317,8 @@ TEST( testLinearContainerFactor, hessian_relinearize ) EXPECT_DOUBLES_EQUAL(expected_error, actual_error, 1e-9 ); // Re-linearize around the new point and check the factors - gtsam::GaussianFactor::shared_ptr expected_factor = gtsam::HessianFactor::shared_ptr(new gtsam::HessianFactor(*betweenFactor.linearize(linpoint2, ordering))); - gtsam::GaussianFactor::shared_ptr actual_factor = hessianContainer.linearize(linpoint2, ordering); + gtsam::GaussianFactor::shared_ptr expected_factor = gtsam::HessianFactor::shared_ptr(new gtsam::HessianFactor(*betweenFactor.linearize(linpoint2))); + gtsam::GaussianFactor::shared_ptr actual_factor = hessianContainer.linearize(linpoint2); CHECK(gtsam::assert_equal(*expected_factor, *actual_factor)); } diff --git a/gtsam/nonlinear/tests/testOrdering.cpp b/gtsam/nonlinear/tests/testOrdering.cpp deleted file mode 100644 index a62278d1e..000000000 --- a/gtsam/nonlinear/tests/testOrdering.cpp +++ /dev/null @@ -1,113 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 testOrdering - * @author Alex Cunningham - */ - -#include -#include -#include -#include - -using namespace std; -using namespace gtsam; - -/* ************************************************************************* */ -TEST( Ordering, simple_modifications ) { - Ordering ordering; - - // create an ordering - Symbol x1('x', 1), x2('x', 2), x3('x', 3), x4('x', 4); - ordering += x1, x2, x3, x4; - - EXPECT_LONGS_EQUAL(0, ordering[x1]); - EXPECT_LONGS_EQUAL(1, ordering[x2]); - EXPECT_LONGS_EQUAL(2, ordering[x3]); - EXPECT_LONGS_EQUAL(3, ordering[x4]); - EXPECT_LONGS_EQUAL(Key(x1), ordering.key(0)); - EXPECT_LONGS_EQUAL(Key(x2), ordering.key(1)); - EXPECT_LONGS_EQUAL(Key(x3), ordering.key(2)); - EXPECT_LONGS_EQUAL(Key(x4), ordering.key(3)); - - // pop the last two elements - Ordering::value_type x4p = ordering.pop_back(); - EXPECT_LONGS_EQUAL(3, ordering.size()); - EXPECT(assert_equal(x4, x4p.first)); - - Index x3p = ordering.pop_back().second; - EXPECT_LONGS_EQUAL(2, ordering.size()); - EXPECT_LONGS_EQUAL(2, (int)x3p); - - // reassemble back make the ordering 1, 2, 4, 3 - EXPECT_LONGS_EQUAL(2, ordering.push_back(x4)); - EXPECT_LONGS_EQUAL(3, ordering.push_back(x3)); - - EXPECT_LONGS_EQUAL(2, ordering[x4]); - EXPECT_LONGS_EQUAL(3, ordering[x3]); - - EXPECT_LONGS_EQUAL(Key(x4), ordering.key(2)); - EXPECT_LONGS_EQUAL(Key(x3), ordering.key(3)); - - // verify - Ordering expectedFinal; - expectedFinal += x1, x2, x4, x3; - EXPECT(assert_equal(expectedFinal, ordering)); -} - -/* ************************************************************************* */ -TEST(Ordering, permute) { - Ordering ordering; - ordering += 2, 4, 6, 8; - - Ordering expected; - expected += 2, 8, 6, 4; - - Permutation permutation(4); - permutation[0] = 0; - permutation[1] = 3; - permutation[2] = 2; - permutation[3] = 1; - - Ordering actual = ordering; - actual.permuteInPlace(permutation); - - EXPECT(assert_equal(expected, actual)); - - EXPECT_LONGS_EQUAL(0, actual[2]); - EXPECT_LONGS_EQUAL(1, actual[8]); - EXPECT_LONGS_EQUAL(2, actual[6]); - EXPECT_LONGS_EQUAL(3, actual[4]); - EXPECT_LONGS_EQUAL(2, actual.key(0)); - EXPECT_LONGS_EQUAL(8, actual.key(1)); - EXPECT_LONGS_EQUAL(6, actual.key(2)); - EXPECT_LONGS_EQUAL(4, actual.key(3)); -} - -/* ************************************************************************* */ -TEST( Ordering, invert ) { - // creates a map with the opposite mapping: Index->Key - Ordering ordering; - - // create an ordering - Symbol x1('x', 1), x2('x', 2), x3('x', 3), x4('x', 4); - ordering += x1, x2, x3, x4; - - EXPECT_LONGS_EQUAL(Key(x1), ordering.key(0)); - EXPECT_LONGS_EQUAL(Key(x2), ordering.key(1)); - EXPECT_LONGS_EQUAL(Key(x3), ordering.key(2)); - EXPECT_LONGS_EQUAL(Key(x4), ordering.key(3)); -} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ diff --git a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp index 95bb4d93e..ee2f0d793 100644 --- a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp +++ b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp @@ -17,7 +17,7 @@ */ #include -#include +#include #include #include #include diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index 227f20834..041ea0387 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -15,7 +15,8 @@ */ #include -#include +#include +#include #include #include #include @@ -24,6 +25,7 @@ #include #include // for operator += +#include using namespace boost::assign; #include #include @@ -63,7 +65,8 @@ public: TEST( Values, equals1 ) { Values expected; - LieVector v(3, 5.0, 6.0, 7.0); + LieVector v((Vector(3) << 5.0, 6.0, 7.0)); + expected.insert(key1,v); Values actual; actual.insert(key1,v); @@ -74,8 +77,9 @@ TEST( Values, equals1 ) TEST( Values, equals2 ) { Values cfg1, cfg2; - LieVector v1(3, 5.0, 6.0, 7.0); - LieVector v2(3, 5.0, 6.0, 8.0); + LieVector v1((Vector(3) << 5.0, 6.0, 7.0)); + LieVector v2((Vector(3) << 5.0, 6.0, 8.0)); + cfg1.insert(key1, v1); cfg2.insert(key1, v2); CHECK(!cfg1.equals(cfg2)); @@ -86,8 +90,9 @@ TEST( Values, equals2 ) TEST( Values, equals_nan ) { Values cfg1, cfg2; - LieVector v1(3, 5.0, 6.0, 7.0); - LieVector v2(3, inf, inf, inf); + LieVector v1((Vector(3) << 5.0, 6.0, 7.0)); + LieVector v2((Vector(3) << inf, inf, inf)); + cfg1.insert(key1, v1); cfg2.insert(key1, v2); CHECK(!cfg1.equals(cfg2)); @@ -98,10 +103,11 @@ TEST( Values, equals_nan ) TEST( Values, insert_good ) { Values cfg1, cfg2, expected; - LieVector v1(3, 5.0, 6.0, 7.0); - LieVector v2(3, 8.0, 9.0, 1.0); - LieVector v3(3, 2.0, 4.0, 3.0); - LieVector v4(3, 8.0, 3.0, 7.0); + LieVector v1((Vector(3) << 5.0, 6.0, 7.0)); + LieVector v2((Vector(3) << 8.0, 9.0, 1.0)); + LieVector v3((Vector(3) << 2.0, 4.0, 3.0)); + LieVector v4((Vector(3) << 8.0, 3.0, 7.0)); + cfg1.insert(key1, v1); cfg1.insert(key2, v2); cfg2.insert(key3, v4); @@ -119,10 +125,11 @@ TEST( Values, insert_good ) TEST( Values, insert_bad ) { Values cfg1, cfg2; - LieVector v1(3, 5.0, 6.0, 7.0); - LieVector v2(3, 8.0, 9.0, 1.0); - LieVector v3(3, 2.0, 4.0, 3.0); - LieVector v4(3, 8.0, 3.0, 7.0); + LieVector v1((Vector(3) << 5.0, 6.0, 7.0)); + LieVector v2((Vector(3) << 8.0, 9.0, 1.0)); + LieVector v3((Vector(3) << 2.0, 4.0, 3.0)); + LieVector v4((Vector(3) << 8.0, 3.0, 7.0)); + cfg1.insert(key1, v1); cfg1.insert(key2, v2); cfg2.insert(key2, v3); @@ -135,8 +142,8 @@ TEST( Values, insert_bad ) TEST( Values, update_element ) { Values cfg; - LieVector v1(3, 5.0, 6.0, 7.0); - LieVector v2(3, 8.0, 9.0, 1.0); + LieVector v1((Vector(3) << 5.0, 6.0, 7.0)); + LieVector v2((Vector(3) << 8.0, 9.0, 1.0)); cfg.insert(key1, v1); CHECK(cfg.size() == 1); @@ -147,12 +154,40 @@ TEST( Values, update_element ) CHECK(assert_equal(v2, cfg.at(key1))); } +/* ************************************************************************* */ +TEST(Values, basic_functions) +{ + Values values; + const Values& values_c = values; + values.insert(2, LieVector()); + values.insert(4, LieVector()); + values.insert(6, LieVector()); + values.insert(8, LieVector()); + + // find + EXPECT_LONGS_EQUAL(4, values.find(4)->key); + EXPECT_LONGS_EQUAL(4, values_c.find(4)->key); + + // lower_bound + EXPECT_LONGS_EQUAL(4, values.lower_bound(4)->key); + EXPECT_LONGS_EQUAL(4, values_c.lower_bound(4)->key); + EXPECT_LONGS_EQUAL(4, values.lower_bound(3)->key); + EXPECT_LONGS_EQUAL(4, values_c.lower_bound(3)->key); + + // upper_bound + EXPECT_LONGS_EQUAL(6, values.upper_bound(4)->key); + EXPECT_LONGS_EQUAL(6, values_c.upper_bound(4)->key); + EXPECT_LONGS_EQUAL(4, values.upper_bound(3)->key); + EXPECT_LONGS_EQUAL(4, values_c.upper_bound(3)->key); + +} + ///* ************************************************************************* */ //TEST(Values, dim_zero) //{ // Values config0; -// config0.insert(key1, LieVector(2, 2.0, 3.0)); -// config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); +// config0.insert(key1, LieVector((Vector(2) << 2.0, 3.0)); +// config0.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0)); // LONGS_EQUAL(5, config0.dim()); // // VectorValues expected; @@ -165,53 +200,51 @@ TEST( Values, update_element ) TEST(Values, expmap_a) { Values config0; - config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0)); - config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); + config0.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0))); + config0.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0))); - Ordering ordering(*config0.orderingArbitrary()); - VectorValues increment(config0.dims(ordering)); - increment[ordering[key1]] = Vector_(3, 1.0, 1.1, 1.2); - increment[ordering[key2]] = Vector_(3, 1.3, 1.4, 1.5); + VectorValues increment = pair_list_of + (key1, (Vector(3) << 1.0, 1.1, 1.2)) + (key2, (Vector(3) << 1.3, 1.4, 1.5)); Values expected; - expected.insert(key1, LieVector(3, 2.0, 3.1, 4.2)); - expected.insert(key2, LieVector(3, 6.3, 7.4, 8.5)); + expected.insert(key1, LieVector((Vector(3) << 2.0, 3.1, 4.2))); + expected.insert(key2, LieVector((Vector(3) << 6.3, 7.4, 8.5))); - CHECK(assert_equal(expected, config0.retract(increment, ordering))); + CHECK(assert_equal(expected, config0.retract(increment))); } /* ************************************************************************* */ TEST(Values, expmap_b) { Values config0; - config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0)); - config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); + config0.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0))); + config0.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0))); - Ordering ordering(*config0.orderingArbitrary()); - VectorValues increment(VectorValues::Zero(config0.dims(ordering))); - increment[ordering[key2]] = LieVector(3, 1.3, 1.4, 1.5); + VectorValues increment = pair_list_of + (key2, (Vector(3) << 1.3, 1.4, 1.5)); Values expected; - expected.insert(key1, LieVector(3, 1.0, 2.0, 3.0)); - expected.insert(key2, LieVector(3, 6.3, 7.4, 8.5)); + expected.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0))); + expected.insert(key2, LieVector((Vector(3) << 6.3, 7.4, 8.5))); - CHECK(assert_equal(expected, config0.retract(increment, ordering))); + CHECK(assert_equal(expected, config0.retract(increment))); } /* ************************************************************************* */ //TEST(Values, expmap_c) //{ // Values config0; -// config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0)); -// config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); +// config0.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0))); +// config0.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0))); // // Vector increment = LieVector(6, // 1.0, 1.1, 1.2, // 1.3, 1.4, 1.5); // // Values expected; -// expected.insert(key1, LieVector(3, 2.0, 3.1, 4.2)); -// expected.insert(key2, LieVector(3, 6.3, 7.4, 8.5)); +// expected.insert(key1, LieVector((Vector(3) << 2.0, 3.1, 4.2))); +// expected.insert(key2, LieVector((Vector(3) << 6.3, 7.4, 8.5))); // // CHECK(assert_equal(expected, config0.retract(increment))); //} @@ -220,8 +253,8 @@ TEST(Values, expmap_b) TEST(Values, expmap_d) { Values config0; - config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0)); - config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); + config0.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0))); + config0.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0))); //config0.print("config0"); CHECK(equal(config0, config0)); CHECK(config0.equals(config0)); @@ -238,18 +271,16 @@ TEST(Values, expmap_d) TEST(Values, localCoordinates) { Values valuesA; - valuesA.insert(key1, LieVector(3, 1.0, 2.0, 3.0)); - valuesA.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); + valuesA.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0))); + valuesA.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0))); - Ordering ordering = *valuesA.orderingArbitrary(); + VectorValues expDelta = pair_list_of + (key1, (Vector(3) << 0.1, 0.2, 0.3)) + (key2, (Vector(3) << 0.4, 0.5, 0.6)); - VectorValues expDelta = valuesA.zeroVectors(ordering); -// expDelta.at(ordering[key1]) = Vector_(3, 0.1, 0.2, 0.3); -// expDelta.at(ordering[key2]) = Vector_(3, 0.4, 0.5, 0.6); + Values valuesB = valuesA.retract(expDelta); - Values valuesB = valuesA.retract(expDelta, ordering); - - EXPECT(assert_equal(expDelta, valuesA.localCoordinates(valuesB, ordering))); + EXPECT(assert_equal(expDelta, valuesA.localCoordinates(valuesB))); } /* ************************************************************************* */ @@ -277,28 +308,28 @@ TEST(Values, extract_keys) TEST(Values, exists_) { Values config0; - config0.insert(key1, LieVector(Vector_(1, 1.))); - config0.insert(key2, LieVector(Vector_(1, 2.))); + config0.insert(key1, LieVector((Vector(1) << 1.))); + config0.insert(key2, LieVector((Vector(1) << 2.))); boost::optional v = config0.exists(key1); - CHECK(assert_equal(Vector_(1, 1.),*v)); + CHECK(assert_equal((Vector(1) << 1.),*v)); } /* ************************************************************************* */ TEST(Values, update) { Values config0; - config0.insert(key1, LieVector(1, 1.)); - config0.insert(key2, LieVector(1, 2.)); + config0.insert(key1, LieVector((Vector(1) << 1.))); + config0.insert(key2, LieVector((Vector(1) << 2.))); Values superset; - superset.insert(key1, LieVector(1, -1.)); - superset.insert(key2, LieVector(1, -2.)); + superset.insert(key1, LieVector((Vector(1) << -1.))); + superset.insert(key2, LieVector((Vector(1) << -2.))); config0.update(superset); Values expected; - expected.insert(key1, LieVector(1, -1.)); - expected.insert(key2, LieVector(1, -2.)); + expected.insert(key1, LieVector((Vector(1) << -1.))); + expected.insert(key2, LieVector((Vector(1) << -2.))); CHECK(assert_equal(expected,config0)); } @@ -318,14 +349,14 @@ TEST(Values, filter) { // Filter by key int i = 0; Values::Filtered filtered = values.filter(boost::bind(std::greater_equal(), _1, 2)); - EXPECT_LONGS_EQUAL(2, filtered.size()); + EXPECT_LONGS_EQUAL(2, (long)filtered.size()); BOOST_FOREACH(const Values::Filtered<>::KeyValuePair& key_value, filtered) { if(i == 0) { - LONGS_EQUAL(2, key_value.key); + LONGS_EQUAL(2, (long)key_value.key); EXPECT(typeid(Pose2) == typeid(key_value.value)); EXPECT(assert_equal(pose2, dynamic_cast(key_value.value))); } else if(i == 1) { - LONGS_EQUAL(3, key_value.key); + LONGS_EQUAL(3, (long)key_value.key); EXPECT(typeid(Pose3) == typeid(key_value.value)); EXPECT(assert_equal(pose3, dynamic_cast(key_value.value))); } else { @@ -333,7 +364,7 @@ TEST(Values, filter) { } ++ i; } - EXPECT_LONGS_EQUAL(2, i); + EXPECT_LONGS_EQUAL(2, (long)i); // construct a values with the view Values actualSubValues1(filtered); @@ -345,20 +376,20 @@ TEST(Values, filter) { // Filter by type i = 0; Values::ConstFiltered pose_filtered = values.filter(); - EXPECT_LONGS_EQUAL(2, pose_filtered.size()); + EXPECT_LONGS_EQUAL(2, (long)pose_filtered.size()); BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, pose_filtered) { if(i == 0) { - EXPECT_LONGS_EQUAL(1, key_value.key); + EXPECT_LONGS_EQUAL(1, (long)key_value.key); EXPECT(assert_equal(pose1, key_value.value)); } else if(i == 1) { - EXPECT_LONGS_EQUAL(3, key_value.key); + EXPECT_LONGS_EQUAL(3, (long)key_value.key); EXPECT(assert_equal(pose3, key_value.value)); } else { EXPECT(false); } ++ i; } - EXPECT_LONGS_EQUAL(2, i); + EXPECT_LONGS_EQUAL(2, (long)i); // construct a values with the view Values actualSubValues2(pose_filtered); @@ -384,17 +415,17 @@ TEST(Values, Symbol_filter) { int i = 0; BOOST_FOREACH(const Values::Filtered::KeyValuePair& key_value, values.filter(Symbol::ChrTest('y'))) { if(i == 0) { - LONGS_EQUAL(Symbol('y',1), key_value.key); + LONGS_EQUAL(Symbol('y', 1), (long)key_value.key); EXPECT(assert_equal(pose1, dynamic_cast(key_value.value))); } else if(i == 1) { - LONGS_EQUAL(Symbol('y',3), key_value.key); + LONGS_EQUAL(Symbol('y', 3), (long)key_value.key); EXPECT(assert_equal(pose3, dynamic_cast(key_value.value))); } else { EXPECT(false); } ++ i; } - LONGS_EQUAL(2, i); + LONGS_EQUAL(2, (long)i); } /* ************************************************************************* */ @@ -405,16 +436,16 @@ TEST(Values, Destructors) { { TestValue value1; TestValue value2; - LONGS_EQUAL(2, TestValueData::ConstructorCount); - LONGS_EQUAL(0, TestValueData::DestructorCount); + LONGS_EQUAL(2, (long)TestValueData::ConstructorCount); + LONGS_EQUAL(0, (long)TestValueData::DestructorCount); values.insert(0, value1); values.insert(1, value2); } - LONGS_EQUAL(4, TestValueData::ConstructorCount); - LONGS_EQUAL(2, TestValueData::DestructorCount); + LONGS_EQUAL(4, (long)TestValueData::ConstructorCount); + LONGS_EQUAL(2, (long)TestValueData::DestructorCount); } - LONGS_EQUAL(4, TestValueData::ConstructorCount); - LONGS_EQUAL(4, TestValueData::DestructorCount); + LONGS_EQUAL(4, (long)TestValueData::ConstructorCount); + LONGS_EQUAL(4, (long)TestValueData::DestructorCount); } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/tests/testWhiteNoiseFactor.cpp b/gtsam/nonlinear/tests/testWhiteNoiseFactor.cpp index a141641e3..1cd042428 100644 --- a/gtsam/nonlinear/tests/testWhiteNoiseFactor.cpp +++ b/gtsam/nonlinear/tests/testWhiteNoiseFactor.cpp @@ -30,7 +30,7 @@ TEST( WhiteNoiseFactor, constructor ) double z = 0.1; Key meanKey=1, precisionKey=2; WhiteNoiseFactor factor(z,meanKey, precisionKey); - LONGS_EQUAL(2,factor.dim()); + LONGS_EQUAL(2, (long)factor.dim()); } /* ************************************************************************* */ diff --git a/gtsam/slam/AntiFactor.h b/gtsam/slam/AntiFactor.h index fe1b93bde..ae794db6a 100644 --- a/gtsam/slam/AntiFactor.h +++ b/gtsam/slam/AntiFactor.h @@ -47,7 +47,7 @@ namespace gtsam { AntiFactor() {} /** constructor - Creates the equivalent AntiFactor from an existing factor */ - AntiFactor(NonlinearFactor::shared_ptr factor) : Base(factor->begin(), factor->end()), factor_(factor) {} + AntiFactor(NonlinearFactor::shared_ptr factor) : Base(factor->keys()), factor_(factor) {} virtual ~AntiFactor() {} @@ -94,11 +94,10 @@ namespace gtsam { * returns a Jacobian instead of a full Hessian), but with the opposite sign. This * effectively cancels the effect of the original factor on the factor graph. */ - boost::shared_ptr - linearize(const Values& c, const Ordering& ordering) const { + boost::shared_ptr linearize(const Values& c) const { // Generate the linearized factor from the contained nonlinear factor - GaussianFactor::shared_ptr gaussianFactor = factor_->linearize(c, ordering); + GaussianFactor::shared_ptr gaussianFactor = factor_->linearize(c); // return the negated version of the factor return gaussianFactor->negate(); diff --git a/gtsam/slam/BearingRangeFactor.h b/gtsam/slam/BearingRangeFactor.h index 6f00c81a6..cf5760695 100644 --- a/gtsam/slam/BearingRangeFactor.h +++ b/gtsam/slam/BearingRangeFactor.h @@ -96,7 +96,7 @@ namespace gtsam { Vector e1 = Rot::Logmap(measuredBearing_.between(y1)); double y2 = pose.range(point, H21_, H22_); - Vector e2 = Vector_(1, y2 - measuredRange_); + Vector e2 = (Vector(1) << y2 - measuredRange_); if (H1) *H1 = gtsam::stack(2, &H11, &H21); if (H2) *H2 = gtsam::stack(2, &H12, &H22); diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index fd53ab1c7..1b630374c 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -20,7 +20,6 @@ #include #include #include -#include namespace gtsam { diff --git a/gtsam/slam/CMakeLists.txt b/gtsam/slam/CMakeLists.txt index c9497f167..b348c4aa3 100644 --- a/gtsam/slam/CMakeLists.txt +++ b/gtsam/slam/CMakeLists.txt @@ -4,32 +4,12 @@ set (slam_excluded_headers #"") ) file(GLOB slam_headers "*.h") -list(REMOVE_ITEM slam_headers ${slam_excluded_headers}) install(FILES ${slam_headers} DESTINATION include/gtsam/slam) -# Components to link tests in this subfolder against -set(slam_local_libs - slam - nonlinear - linear - inference - geometry - base - ccolamd -) - -# Files to exclude from compilation of tests and timing scripts -set(slam_excluded_files - "${CMAKE_CURRENT_SOURCE_DIR}/tests/testSerialization.cpp" -# "" # Add to this list, with full path, to exclude -) - # Build tests -if (GTSAM_BUILD_TESTS) - gtsam_add_subdir_tests(slam "${slam_local_libs}" "${gtsam-default}" "${slam_excluded_files}") -endif(GTSAM_BUILD_TESTS) +add_subdirectory(tests) # Build timing scripts if (GTSAM_BUILD_TIMING) - gtsam_add_subdir_timing(slam "${slam_local_libs}" "${gtsam-default}" "${slam_excluded_files}") + gtsam_add_subdir_timing(slam "gtsam" "gtsam" "${slam_excluded_files}") endif(GTSAM_BUILD_TIMING) diff --git a/gtsam/slam/EssentialMatrixConstraint.h b/gtsam/slam/EssentialMatrixConstraint.h index f2eb76e2d..0e4fb48cf 100644 --- a/gtsam/slam/EssentialMatrixConstraint.h +++ b/gtsam/slam/EssentialMatrixConstraint.h @@ -27,7 +27,7 @@ namespace gtsam { * Binary factor between two Pose3 variables induced by an EssentialMatrix measurement * @addtogroup SLAM */ -class EssentialMatrixConstraint: public NoiseModelFactor2 { +class GTSAM_EXPORT EssentialMatrixConstraint: public NoiseModelFactor2 { private: diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index 02664af41..9e6f3f8ba 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -155,7 +155,7 @@ public: // We have point x,y in image 1 // Given a depth Z, the corresponding 3D point P1 = Z*(x,y,1) = (x,y,1)/d - // We then convert to second camera by P2 = 1R2�*(P1-1T2) + // We then convert to second camera by P2 = 1R2'*(P1-1T2) // The homogeneous coordinates of can be written as // 2R1*(P1-1T2) == 2R1*d*(P1-1T2) == 2R1*((x,y,1)-d*1T2) // where we multiplied with d which yields equivalent homogeneous coordinates. @@ -218,7 +218,7 @@ public: * Constructor * @param pA point in first camera, in calibrated coordinates * @param pB point in second camera, in calibrated coordinates - * @param cRb extra rotation from "body" to "camera" frame + * @param bRc extra rotation between "body" and "camera" frame * @param model noise model should be in calibrated coordinates, as well */ EssentialMatrixFactor3(Key key1, Key key2, const Point2& pA, const Point2& pB, diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index ea3a842bc..9142f9d3d 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -101,8 +101,8 @@ namespace gtsam { if (H2) *H2 = zeros(2, point.dim()); std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << " behind Camera " << DefaultKeyFormatter(this->key1()) << std::endl; + return zero(2); } - return zero(2); } /** return the measured */ diff --git a/gtsam/slam/PoseTranslationPrior.h b/gtsam/slam/PoseTranslationPrior.h index ada85667f..2d9b3fdd4 100644 --- a/gtsam/slam/PoseTranslationPrior.h +++ b/gtsam/slam/PoseTranslationPrior.h @@ -36,6 +36,9 @@ protected: public: + /** default constructor - only use for serialization */ + PoseTranslationPrior() {} + /** standard constructor */ PoseTranslationPrior(Key key, const Translation& measured, const noiseModel::Base::shared_ptr& model) : Base(model, key), measured_(measured) { diff --git a/gtsam/slam/RangeFactor.h b/gtsam/slam/RangeFactor.h index 7b807d88f..fe140a298 100644 --- a/gtsam/slam/RangeFactor.h +++ b/gtsam/slam/RangeFactor.h @@ -73,7 +73,7 @@ namespace gtsam { } else { hx = pose.range(point, H1, H2); } - return Vector_(1, hx - measured_); + return (Vector(1) << hx - measured_); } /** return the measured */ diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index e84ff9671..4a7166f38 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -24,7 +24,7 @@ #include #include -#include +#include #include #include #include @@ -180,7 +180,7 @@ pair load2D( noiseModel::Diagonal::shared_ptr measurementNoise = noiseModel::Diagonal::Sigmas((Vector(2) << bearing_std, range_std)); - graph->add(BearingRangeFactor(id1, id2, bearing, range, measurementNoise)); + *graph += BearingRangeFactor(id1, id2, bearing, range, measurementNoise); // Insert poses or points if they do not exist yet if (!initial->exists(id1)) @@ -223,13 +223,13 @@ pair load2D( } // Add to graph - graph->add(BearingRangeFactor(id1, L(id2), bearing, range, measurementNoise)); + *graph += BearingRangeFactor(id1, L(id2), bearing, range, measurementNoise); } is.ignore(LINESIZE, '\n'); } cout << "load2D read a graph file with " << initial->size() - << " vertices and " << graph->nrFactors() << " factors" << endl; + << " vertices and " << graph->nrFactors() << " factors" << endl; return make_pair(graph, initial); } @@ -387,7 +387,7 @@ pair load2D_robust( noiseModel::Diagonal::shared_ptr measurementNoise = noiseModel::Diagonal::Sigmas((Vector(2) << bearing_std, range_std)); - graph->add(BearingRangeFactor(id1, id2, bearing, range, measurementNoise)); + *graph += BearingRangeFactor(id1, id2, bearing, range, measurementNoise); // Insert poses or points if they do not exist yet if (!initial->exists(id1)) @@ -403,7 +403,7 @@ pair load2D_robust( } cout << "load2D read a graph file with " << initial->size() - << " vertices and " << graph->nrFactors() << " factors" << endl; + << " vertices and " << graph->nrFactors() << " factors" << endl; return make_pair(graph, initial); } @@ -669,52 +669,75 @@ bool writeBAL(const string& filename, SfM_data &data) return true; } -bool writeBALfromValues(const string& filename, SfM_data &data, Values& values){ +bool writeBALfromValues(const string& filename, const SfM_data &data, Values& values){ - // CHECKS + SfM_data dataValues = data; + + // Store poses or cameras in SfM_data Values valuesPoses = values.filter(); - if( valuesPoses.size() != data.number_cameras()){ - cout << "writeBALfromValues: different number of cameras in SfM_data (#cameras= " << data.number_cameras() - <<") and values (#cameras " << valuesPoses.size() << ")!!" << endl; - return false; - } - Values valuesPoints = values.filter(); - if( valuesPoints.size() != data.number_tracks()){ - cout << "writeBALfromValues: different number of points in SfM_data (#points= " << data.number_tracks() - <<") and values (#points " << valuesPoints.size() << ")!!" << endl; - } - if(valuesPoints.size() + valuesPoses.size() != values.size()){ - cout << "writeBALfromValues write only poses and points values!!" << endl; - return false; - } - if(valuesPoints.size()==0 || valuesPoses.size()==0){ - cout << "writeBALfromValues: No point or pose in values!!" << endl; - return false; - } - - for (size_t i = 0; i < data.number_cameras(); i++){ // for each camera - Key poseKey = symbol('x',i); - Pose3 pose = values.at(poseKey); - Cal3Bundler K = data.cameras[i].calibration(); - PinholeCamera camera(pose, K); - data.cameras[i] = camera; - } - - for (size_t j = 0; j < data.number_tracks(); j++){ // for each point - Key pointKey = symbol('l',j); - if(values.exists(pointKey)){ - Point3 point = values.at(pointKey); - data.tracks[j].p = point; + if( valuesPoses.size() == dataValues.number_cameras() ){ // we only estimated camera poses + for (size_t i = 0; i < dataValues.number_cameras(); i++){ // for each camera + Key poseKey = symbol('x',i); + Pose3 pose = values.at(poseKey); + Cal3Bundler K = dataValues.cameras[i].calibration(); + PinholeCamera camera(pose, K); + dataValues.cameras[i] = camera; + } + } else { + Values valuesCameras = values.filter< PinholeCamera >(); + if ( valuesCameras.size() == dataValues.number_cameras() ){ // we only estimated camera poses and calibration + for (size_t i = 0; i < dataValues.number_cameras(); i++){ // for each camera + Key cameraKey = i; // symbol('c',i); + PinholeCamera camera = values.at >(cameraKey); + dataValues.cameras[i] = camera; + } }else{ - data.tracks[j].r = 1.0; - data.tracks[j].g = 0.0; - data.tracks[j].b = 0.0; - data.tracks[j].p = Point3(); + cout << "writeBALfromValues: different number of cameras in SfM_dataValues (#cameras= " << dataValues.number_cameras() + <<") and values (#cameras " << valuesPoses.size() << ", #poses " << valuesCameras.size() << ")!!" << endl; + return false; } } - return writeBAL(filename, data); + // Store 3D points in SfM_data + Values valuesPoints = values.filter(); + if( valuesPoints.size() != dataValues.number_tracks()){ + cout << "writeBALfromValues: different number of points in SfM_dataValues (#points= " << dataValues.number_tracks() + <<") and values (#points " << valuesPoints.size() << ")!!" << endl; + } + + for (size_t j = 0; j < dataValues.number_tracks(); j++){ // for each point + Key pointKey = symbol('l',j); + if(values.exists(pointKey)){ + Point3 point = values.at(pointKey); + dataValues.tracks[j].p = point; + }else{ + dataValues.tracks[j].r = 1.0; + dataValues.tracks[j].g = 0.0; + dataValues.tracks[j].b = 0.0; + dataValues.tracks[j].p = Point3(); + } + } + + // Write SfM_data to file + return writeBAL(filename, dataValues); } +Values initialCamerasEstimate(const SfM_data& db) { + Values initial; + size_t i = 0; // NO POINTS: j = 0; + BOOST_FOREACH(const SfM_Camera& camera, db.cameras) + initial.insert(i++, camera); + return initial; +} + +Values initialCamerasAndPointsEstimate(const SfM_data& db) { + Values initial; + size_t i = 0, j = 0; + BOOST_FOREACH(const SfM_Camera& camera, db.cameras) + initial.insert((i++), camera); + BOOST_FOREACH(const SfM_Track& track, db.tracks) + initial.insert(P(j++), track.p); + return initial; +} } // \namespace gtsam diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index bd5a28cdd..64ccd37b5 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -141,10 +141,12 @@ GTSAM_EXPORT bool writeBAL(const std::string& filename, SfM_data &data); * while camera poses and values are read from Values) * @param filename The name of the BAL file to write * @param data SfM structure where the data is stored - * @param values structure where the graph values are stored + * @param values structure where the graph values are stored (values can be either Pose3 or PinholeCamera for the + * cameras, and should be Point3 for the 3D points). Note that the current version + * assumes that the keys are "x1" for pose 1 (or "c1" for camera 1) and "l1" for landmark 1 * @return true if the parsing was successful, false otherwise */ -GTSAM_EXPORT bool writeBALfromValues(const std::string& filename, SfM_data &data, Values& values); +GTSAM_EXPORT bool writeBALfromValues(const std::string& filename, const SfM_data &data, Values& values); /** * @brief This function converts an openGL camera pose to an GTSAM camera pose @@ -173,4 +175,19 @@ GTSAM_EXPORT Pose3 gtsam2openGL(const Rot3& R, double tx, double ty, double tz); */ GTSAM_EXPORT Pose3 gtsam2openGL(const Pose3& PoseGTSAM); +/** + * @brief This function creates initial values for cameras from db + * @param SfM_data + * @return Values + */ +GTSAM_EXPORT Values initialCamerasEstimate(const SfM_data& db); + +/** + * @brief This function creates initial values for cameras and points from db + * @param SfM_data + * @return Values + */ +GTSAM_EXPORT Values initialCamerasAndPointsEstimate(const SfM_data& db); + + } // namespace gtsam diff --git a/gtsam/slam/tests/CMakeLists.txt b/gtsam/slam/tests/CMakeLists.txt new file mode 100644 index 000000000..1205388a1 --- /dev/null +++ b/gtsam/slam/tests/CMakeLists.txt @@ -0,0 +1 @@ +gtsamAddTestsGlob(slam "test*.cpp" "" "gtsam") diff --git a/gtsam/slam/tests/testAntiFactor.cpp b/gtsam/slam/tests/testAntiFactor.cpp index 1902bfa85..5d73bcab8 100644 --- a/gtsam/slam/tests/testAntiFactor.cpp +++ b/gtsam/slam/tests/testAntiFactor.cpp @@ -23,7 +23,10 @@ #include #include #include -#include +#include +#include +#include +#include #include using namespace std; @@ -41,21 +44,15 @@ TEST( AntiFactor, NegativeHessian) SharedNoiseModel sigma(noiseModel::Unit::Create(Pose3::Dim())); // Create a configuration corresponding to the ground truth - boost::shared_ptr values(new Values()); - values->insert(1, pose1); - values->insert(2, pose2); - - // Define an elimination ordering - Ordering::shared_ptr ordering(new Ordering()); - ordering->insert(1, 0); - ordering->insert(2, 1); - + Values values; + values.insert(1, pose1); + values.insert(2, pose2); // Create a "standard" factor BetweenFactor::shared_ptr originalFactor(new BetweenFactor(1, 2, z, sigma)); // Linearize it into a Jacobian Factor - GaussianFactor::shared_ptr originalJacobian = originalFactor->linearize(*values, *ordering); + GaussianFactor::shared_ptr originalJacobian = originalFactor->linearize(values); // Convert it to a Hessian Factor HessianFactor::shared_ptr originalHessian = HessianFactor::shared_ptr(new HessianFactor(*originalJacobian)); @@ -64,7 +61,7 @@ TEST( AntiFactor, NegativeHessian) AntiFactor::shared_ptr antiFactor(new AntiFactor(originalFactor)); // Linearize the AntiFactor into a Hessian Factor - GaussianFactor::shared_ptr antiGaussian = antiFactor->linearize(*values, *ordering); + GaussianFactor::shared_ptr antiGaussian = antiFactor->linearize(values); HessianFactor::shared_ptr antiHessian = boost::dynamic_pointer_cast(antiGaussian); @@ -97,39 +94,39 @@ TEST( AntiFactor, EquivalentBayesNet) Pose3 z(Rot3(), Point3(1, 1, 1)); SharedNoiseModel sigma(noiseModel::Unit::Create(Pose3::Dim())); - NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph()); - graph->add(PriorFactor(1, pose1, sigma)); - graph->add(BetweenFactor(1, 2, pose1.between(pose2), sigma)); + NonlinearFactorGraph graph; + graph.push_back(PriorFactor(1, pose1, sigma)); + graph.push_back(BetweenFactor(1, 2, pose1.between(pose2), sigma)); // Create a configuration corresponding to the ground truth - Values::shared_ptr values(new Values()); - values->insert(1, pose1); - values->insert(2, pose2); + Values values; + values.insert(1, pose1); + values.insert(2, pose2); // Define an elimination ordering - Ordering::shared_ptr ordering = graph->orderingCOLAMD(*values); + Ordering ordering = graph.orderingCOLAMD(); // Eliminate into a BayesNet - GaussianSequentialSolver solver1(*graph->linearize(*values, *ordering)); - GaussianBayesNet::shared_ptr expectedBayesNet = solver1.eliminate(); + GaussianFactorGraph lin_graph = *graph.linearize(values); + GaussianBayesNet::shared_ptr expectedBayesNet = lin_graph.eliminateSequential(ordering); // Back-substitute to find the optimal deltas - VectorValues expectedDeltas = optimize(*expectedBayesNet); + VectorValues expectedDeltas = expectedBayesNet->optimize(); // Add an additional factor between Pose1 and Pose2 BetweenFactor::shared_ptr f1(new BetweenFactor(1, 2, z, sigma)); - graph->push_back(f1); + graph.push_back(f1); // Add the corresponding AntiFactor between Pose1 and Pose2 AntiFactor::shared_ptr f2(new AntiFactor(f1)); - graph->push_back(f2); + graph.push_back(f2); // Again, Eliminate into a BayesNet - GaussianSequentialSolver solver2(*graph->linearize(*values, *ordering)); - GaussianBayesNet::shared_ptr actualBayesNet = solver2.eliminate(); + GaussianFactorGraph lin_graph1 = *graph.linearize(values); + GaussianBayesNet::shared_ptr actualBayesNet = lin_graph1.eliminateSequential(ordering); // Back-substitute to find the optimal deltas - VectorValues actualDeltas = optimize(*actualBayesNet); + VectorValues actualDeltas = actualBayesNet->optimize(); // Verify the BayesNets are identical CHECK(assert_equal(*expectedBayesNet, *actualBayesNet, 1e-5)); diff --git a/gtsam/slam/tests/testBetweenFactor.cpp b/gtsam/slam/tests/testBetweenFactor.cpp new file mode 100644 index 000000000..d0dbe7908 --- /dev/null +++ b/gtsam/slam/tests/testBetweenFactor.cpp @@ -0,0 +1,52 @@ +/** + * @file testBetweenFactor.cpp + * @brief + * @author Duy-Nguyen Ta + * @date Aug 2, 2013 + */ + +#include +#include +#include +#include +#include + +using namespace gtsam; +using namespace gtsam::symbol_shorthand; +using namespace gtsam::noiseModel; + +/** + * This TEST should fail. If you want it to pass, change noise to 0. + */ +TEST(BetweenFactor, Rot3) { + Rot3 R1 = Rot3::rodriguez(0.1, 0.2, 0.3); + Rot3 R2 = Rot3::rodriguez(0.4, 0.5, 0.6); + Rot3 noise = Rot3(); // Rot3::rodriguez(0.01, 0.01, 0.01); // Uncomment to make unit test fail + Rot3 measured = R1.between(R2)*noise ; + + BetweenFactor factor(R(1), R(2), measured, Isotropic::Sigma(3, 0.05)); + Matrix actualH1, actualH2; + Vector actual = factor.evaluateError(R1, R2, actualH1, actualH2); + + Vector expected = Rot3::Logmap(measured.inverse() * R1.between(R2)); + EXPECT(assert_equal(expected,actual/*, 1e-100*/)); // Uncomment to make unit test fail + + Matrix numericalH1 = numericalDerivative21( + boost::function(boost::bind( + &BetweenFactor::evaluateError, factor, _1, _2, boost::none, + boost::none)), R1, R2, 1e-5); + EXPECT(assert_equal(numericalH1,actualH1, 1E-5)); + + Matrix numericalH2 = numericalDerivative22( + boost::function(boost::bind( + &BetweenFactor::evaluateError, factor, _1, _2, boost::none, + boost::none)), R1, R2, 1e-5); + EXPECT(assert_equal(numericalH2,actualH2, 1E-5)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index f363fce11..f9a2cb34f 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -20,7 +20,7 @@ #include #include -#include +#include #include using namespace std; diff --git a/gtsam/slam/tests/testEssentialMatrixConstraint.cpp b/gtsam/slam/tests/testEssentialMatrixConstraint.cpp index 8fcb5b6e5..5184393ac 100644 --- a/gtsam/slam/tests/testEssentialMatrixConstraint.cpp +++ b/gtsam/slam/tests/testEssentialMatrixConstraint.cpp @@ -63,8 +63,8 @@ TEST( EssentialMatrixConstraint, test ) { factor.evaluateError(pose1, pose2, actualH1, actualH2); // Verify we get the expected error - CHECK(assert_equal(expectedH1, actualH1, 1e-9)); - CHECK(assert_equal(expectedH2, actualH2, 1e-9)); + CHECK(assert_equal(expectedH1, actualH1, 1e-5)); + CHECK(assert_equal(expectedH2, actualH2, 1e-5)); } /* ************************************************************************* */ diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 390c8a1fc..1e5674599 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -37,7 +37,7 @@ bool readOK = readBAL(filename, data); Rot3 c1Rc2 = data.cameras[1].pose().rotation(); Point3 c1Tc2 = data.cameras[1].pose().translation(); PinholeCamera camera2(data.cameras[1].pose(),Cal3_S2()); -EssentialMatrix trueE(c1Rc2, c1Tc2); +EssentialMatrix trueE(c1Rc2, Unit3(c1Tc2)); double baseline = 0.1; // actual baseline of the camera Point2 pA(size_t i) { @@ -128,7 +128,11 @@ TEST (EssentialMatrixFactor, minimization) { EssentialMatrix initialE = trueE.retract( (Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1)); initial.insert(1, initialE); +#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) + EXPECT_DOUBLES_EQUAL(643.26, graph.error(initial), 1e-2); +#else EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2); +#endif // Optimize LevenbergMarquardtParams parameters; @@ -297,7 +301,7 @@ SfM_data data; bool readOK = readBAL(filename, data); Rot3 aRb = data.cameras[1].pose().rotation(); Point3 aTb = data.cameras[1].pose().translation(); -EssentialMatrix trueE(aRb, aTb); +EssentialMatrix trueE(aRb, Unit3(aTb)); double baseline = 10; // actual baseline of the camera @@ -339,7 +343,12 @@ TEST (EssentialMatrixFactor, extraMinimization) { EssentialMatrix initialE = trueE.retract( (Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1)); initial.insert(1, initialE); + +#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) + EXPECT_DOUBLES_EQUAL(643.26, graph.error(initial), 1e-2); +#else EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2); +#endif // Optimize LevenbergMarquardtParams parameters; diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index fb38d7035..a7c91de3f 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -20,7 +20,7 @@ #include #include #include -#include +#include #include #include #include @@ -110,7 +110,7 @@ TEST( GeneralSFMFactor, error ) { Pose3 x1(R,t1); values.insert(X(1), GeneralCamera(x1)); Point3 l1; values.insert(L(1), l1); - EXPECT(assert_equal((Vector(2) << -3.0, 0.0), factor->unwhitenedError(values))); + EXPECT(assert_equal(((Vector) (Vector(2) << -3.0, 0.0)), factor->unwhitenedError(values))); } static const double baseline = 5.0 ; @@ -371,7 +371,7 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) { graph.addCameraConstraint(0, cameras[0]); // Constrain the scale of the problem with a soft range factor of 1m between the cameras - graph.add(RangeFactor(X(0), X(1), 2.0, noiseModel::Isotropic::Sigma(1, 10.0))); + graph.push_back(RangeFactor(X(0), X(1), 2.0, noiseModel::Isotropic::Sigma(1, 10.0))); const double reproj_error = 1e-5 ; @@ -386,8 +386,8 @@ TEST(GeneralSFMFactor, GeneralCameraPoseRange) { // Tests range factor between a GeneralCamera and a Pose3 Graph graph; graph.addCameraConstraint(0, GeneralCamera()); - graph.add(RangeFactor(X(0), X(1), 2.0, noiseModel::Isotropic::Sigma(1, 1.0))); - graph.add(PriorFactor(X(1), Pose3(Rot3(), Point3(1.0, 0.0, 0.0)), noiseModel::Isotropic::Sigma(6, 1.0))); + graph.push_back(RangeFactor(X(0), X(1), 2.0, noiseModel::Isotropic::Sigma(1, 1.0))); + graph.push_back(PriorFactor(X(1), Pose3(Rot3(), Point3(1.0, 0.0, 0.0)), noiseModel::Isotropic::Sigma(6, 1.0))); Values init; init.insert(X(0), GeneralCamera()); @@ -410,9 +410,9 @@ TEST(GeneralSFMFactor, GeneralCameraPoseRange) { TEST(GeneralSFMFactor, CalibratedCameraPoseRange) { // Tests range factor between a CalibratedCamera and a Pose3 NonlinearFactorGraph graph; - graph.add(PriorFactor(X(0), CalibratedCamera(), noiseModel::Isotropic::Sigma(6, 1.0))); - graph.add(RangeFactor(X(0), X(1), 2.0, noiseModel::Isotropic::Sigma(1, 1.0))); - graph.add(PriorFactor(X(1), Pose3(Rot3(), Point3(1.0, 0.0, 0.0)), noiseModel::Isotropic::Sigma(6, 1.0))); + graph.push_back(PriorFactor(X(0), CalibratedCamera(), noiseModel::Isotropic::Sigma(6, 1.0))); + graph.push_back(RangeFactor(X(0), X(1), 2.0, noiseModel::Isotropic::Sigma(1, 1.0))); + graph.push_back(PriorFactor(X(1), Pose3(Rot3(), Point3(1.0, 0.0, 0.0)), noiseModel::Isotropic::Sigma(6, 1.0))); Values init; init.insert(X(0), CalibratedCamera()); diff --git a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp index 9d15736d0..a0377f087 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp @@ -20,8 +20,7 @@ #include #include #include -#include -#include +#include #include #include #include @@ -369,7 +368,7 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_BA ) { graph.addCameraConstraint(0, cameras[0]); // Constrain the scale of the problem with a soft range factor of 1m between the cameras - graph.add(RangeFactor(X(0), X(1), 2.0, noiseModel::Isotropic::Sigma(1, 10.0))); + graph.push_back(RangeFactor(X(0), X(1), 2.0, noiseModel::Isotropic::Sigma(1, 10.0))); const double reproj_error = 1e-5 ; diff --git a/gtsam/slam/tests/testProjectionFactor.cpp b/gtsam/slam/tests/testProjectionFactor.cpp index 1066f3cd2..b3454269e 100644 --- a/gtsam/slam/tests/testProjectionFactor.cpp +++ b/gtsam/slam/tests/testProjectionFactor.cpp @@ -17,12 +17,13 @@ */ #include -#include +#include #include #include #include #include #include +#include #include using namespace std; diff --git a/gtsam/slam/tests/testRangeFactor.cpp b/gtsam/slam/tests/testRangeFactor.cpp index a50e1549b..4fa6164a1 100644 --- a/gtsam/slam/tests/testRangeFactor.cpp +++ b/gtsam/slam/tests/testRangeFactor.cpp @@ -24,6 +24,7 @@ #include #include #include +#include #include using namespace std; diff --git a/gtsam/slam/tests/testReferenceFrameFactor.cpp b/gtsam/slam/tests/testReferenceFrameFactor.cpp index 815ce391a..078bf85cd 100644 --- a/gtsam/slam/tests/testReferenceFrameFactor.cpp +++ b/gtsam/slam/tests/testReferenceFrameFactor.cpp @@ -23,7 +23,7 @@ #include #include #include -#include +#include #include #include #include @@ -116,7 +116,7 @@ TEST( ReferenceFrameFactor, jacobians_zero ) { } /* ************************************************************************* */ -TEST_UNSAFE( ReferenceFrameFactor, converge_trans ) { +TEST( ReferenceFrameFactor, converge_trans ) { // initial points Point2 local1(2.0, 2.0), local2(4.0, 5.0), @@ -136,15 +136,15 @@ TEST_UNSAFE( ReferenceFrameFactor, converge_trans ) { // Pose2 trans = transIdeal * Pose2(-200.0, 100.0, 2.0); // beyond pi/2 - fails NonlinearFactorGraph graph; - graph.add(PointReferenceFrameFactor(lB1, tA1, lA1)); - graph.add(PointReferenceFrameFactor(lB2, tA1, lA2)); + graph.push_back(PointReferenceFrameFactor(lB1, tA1, lA1)); + graph.push_back(PointReferenceFrameFactor(lB2, tA1, lA2)); // hard constraints on points double error_gain = 1000.0; - graph.add(NonlinearEquality(lA1, local1, error_gain)); - graph.add(NonlinearEquality(lA2, local2, error_gain)); - graph.add(NonlinearEquality(lB1, global1, error_gain)); - graph.add(NonlinearEquality(lB2, global2, error_gain)); + graph.push_back(NonlinearEquality(lA1, local1, error_gain)); + graph.push_back(NonlinearEquality(lA2, local2, error_gain)); + graph.push_back(NonlinearEquality(lB1, global1, error_gain)); + graph.push_back(NonlinearEquality(lB2, global2, error_gain)); // create initial estimate Values init; @@ -186,9 +186,9 @@ TEST( ReferenceFrameFactor, converge_local ) { NonlinearFactorGraph graph; double error_gain = 1000.0; - graph.add(PointReferenceFrameFactor(lB1, tA1, lA1)); - graph.add(NonlinearEquality(lB1, global, error_gain)); - graph.add(NonlinearEquality(tA1, trans, error_gain)); + graph.push_back(PointReferenceFrameFactor(lB1, tA1, lA1)); + graph.push_back(NonlinearEquality(lB1, global, error_gain)); + graph.push_back(NonlinearEquality(tA1, trans, error_gain)); // create initial estimate Values init; @@ -222,9 +222,9 @@ TEST( ReferenceFrameFactor, converge_global ) { NonlinearFactorGraph graph; double error_gain = 1000.0; - graph.add(PointReferenceFrameFactor(lB1, tA1, lA1)); - graph.add(NonlinearEquality(lA1, local, error_gain)); - graph.add(NonlinearEquality(tA1, trans, error_gain)); + graph.push_back(PointReferenceFrameFactor(lB1, tA1, lA1)); + graph.push_back(NonlinearEquality(lA1, local, error_gain)); + graph.push_back(NonlinearEquality(tA1, trans, error_gain)); // create initial estimate Values init; diff --git a/gtsam/slam/tests/testRotateFactor.cpp b/gtsam/slam/tests/testRotateFactor.cpp index ca924aaae..f36405318 100644 --- a/gtsam/slam/tests/testRotateFactor.cpp +++ b/gtsam/slam/tests/testRotateFactor.cpp @@ -59,7 +59,11 @@ TEST (RotateFactor, test) { EXPECT(assert_equal(zero(3), f.evaluateError(iRc), 1e-8)); Rot3 R = iRc.retract((Vector(3) << 0.1, 0.2, 0.1)); +#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) + Vector expectedE = (Vector(3) << -0.0248752, 0.202981, -0.0890529); +#else Vector expectedE = (Vector(3) << -0.0246305, 0.20197, -0.08867); +#endif EXPECT( assert_equal(expectedE, f.evaluateError(R), 1e-5)); Matrix actual, expected; @@ -97,7 +101,12 @@ TEST (RotateFactor, minimization) { double degree = M_PI / 180; Rot3 initialE = iRc.retract(degree * (Vector(3) << 20, -20, 20)); initial.insert(1, initialE); + +#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) + EXPECT_DOUBLES_EQUAL(3545.40, graph.error(initial), 1); +#else EXPECT_DOUBLES_EQUAL(3349, graph.error(initial), 1); +#endif // Optimize LevenbergMarquardtParams parameters; @@ -120,7 +129,13 @@ TEST (RotateDirectionsFactor, test) { EXPECT(assert_equal(zero(2), f.evaluateError(iRc), 1e-8)); Rot3 R = iRc.retract((Vector(3) << 0.1, 0.2, 0.1)); + +#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) + Vector expectedE = (Vector(2) << -0.0890529, -0.202981); +#else Vector expectedE = (Vector(2) << -0.08867, -0.20197); +#endif + EXPECT( assert_equal(expectedE, f.evaluateError(R), 1e-5)); Matrix actual, expected; @@ -160,7 +175,12 @@ TEST (RotateDirectionsFactor, minimization) { double degree = M_PI / 180; Rot3 initialE = iRc.retract(degree * (Vector(3) << 20, -20, 20)); initial.insert(1, initialE); + +#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) + EXPECT_DOUBLES_EQUAL(3335.9, graph.error(initial), 1); +#else EXPECT_DOUBLES_EQUAL(3162, graph.error(initial), 1); +#endif // Optimize LevenbergMarquardtParams parameters; diff --git a/gtsam/slam/tests/testStereoFactor.cpp b/gtsam/slam/tests/testStereoFactor.cpp index 8c36f0746..9411e3ccc 100644 --- a/gtsam/slam/tests/testStereoFactor.cpp +++ b/gtsam/slam/tests/testStereoFactor.cpp @@ -20,7 +20,7 @@ #include #include #include -#include +#include #include #include #include @@ -189,11 +189,11 @@ TEST( StereoFactor, singlePoint) { NonlinearFactorGraph graph; - graph.add(NonlinearEquality(X(1), camera1)); + graph.push_back(NonlinearEquality(X(1), camera1)); StereoPoint2 measurement(320, 320.0-50, 240); // arguments: measurement, sigma, cam#, measurement #, K, baseline (m) - graph.add(GenericStereoFactor(measurement, model, X(1), L(1), K)); + graph.push_back(GenericStereoFactor(measurement, model, X(1), L(1), K)); // Create a configuration corresponding to the ground truth Values values; diff --git a/gtsam/symbolic/CMakeLists.txt b/gtsam/symbolic/CMakeLists.txt new file mode 100644 index 000000000..6535755f8 --- /dev/null +++ b/gtsam/symbolic/CMakeLists.txt @@ -0,0 +1,12 @@ +# Install headers +file(GLOB symbolic_headers "*.h") +install(FILES ${symbolic_headers} DESTINATION include/gtsam/symbolic) + +# Build tests +add_subdirectory(tests) + +# Build timing scripts +if (GTSAM_BUILD_TIMING) + gtsam_add_subdir_timing(symbolic "gtsam" "gtsam" "${symbolic_excluded_files}") +endif(GTSAM_BUILD_TIMING) + diff --git a/gtsam/symbolic/SymbolicBayesNet.cpp b/gtsam/symbolic/SymbolicBayesNet.cpp new file mode 100644 index 000000000..c5a04eb0d --- /dev/null +++ b/gtsam/symbolic/SymbolicBayesNet.cpp @@ -0,0 +1,55 @@ +/* ---------------------------------------------------------------------------- + + * 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 SymbolicBayesNet.cpp + * @date Oct 29, 2009 + * @author Frank Dellaert + * @author Richard Roberts + */ + +#include +#include +#include + +#include + +namespace gtsam { + + // Instantiate base class + template class FactorGraph; + + /* ************************************************************************* */ + bool SymbolicBayesNet::equals(const This& bn, double tol) const + { + return Base::equals(bn, tol); + } + + /* ************************************************************************* */ + void SymbolicBayesNet::saveGraph(const std::string &s, const KeyFormatter& keyFormatter) const + { + std::ofstream of(s.c_str()); + of << "digraph G{\n"; + + BOOST_REVERSE_FOREACH(const sharedConditional& conditional, *this) { + SymbolicConditional::Frontals frontals = conditional->frontals(); + Key me = frontals.front(); + SymbolicConditional::Parents parents = conditional->parents(); + BOOST_FOREACH(Key p, parents) + of << p << "->" << me << std::endl; + } + + of << "}"; + of.close(); + } + + +} diff --git a/gtsam/symbolic/SymbolicBayesNet.h b/gtsam/symbolic/SymbolicBayesNet.h new file mode 100644 index 000000000..2e59a98a4 --- /dev/null +++ b/gtsam/symbolic/SymbolicBayesNet.h @@ -0,0 +1,84 @@ +/* ---------------------------------------------------------------------------- + + * 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 SymbolicBayesNet.h + * @date Oct 29, 2009 + * @author Frank Dellaert + * @author Richard Roberts + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + + /** Symbolic Bayes Net + * \nosubgrouping + */ + class GTSAM_EXPORT SymbolicBayesNet : public FactorGraph { + + public: + + typedef FactorGraph Base; + typedef SymbolicBayesNet This; + typedef SymbolicConditional ConditionalType; + typedef boost::shared_ptr shared_ptr; + typedef boost::shared_ptr sharedConditional; + + /// @name Standard Constructors + /// @{ + + /** Construct empty factor graph */ + SymbolicBayesNet() {} + + /** Construct from iterator over conditionals */ + template + SymbolicBayesNet(ITERATOR firstConditional, ITERATOR lastConditional) : Base(firstConditional, lastConditional) {} + + /** Construct from container of factors (shared_ptr or plain objects) */ + template + explicit SymbolicBayesNet(const CONTAINER& conditionals) : Base(conditionals) {} + + /** Implicit copy/downcast constructor to override explicit template container constructor */ + template + SymbolicBayesNet(const FactorGraph& graph) : Base(graph) {} + + /// @} + + /// @name Testable + /// @{ + + /** Check equality */ + bool equals(const This& bn, double tol = 1e-9) const; + + /// @} + + /// @name Standard Interface + /// @{ + + void saveGraph(const std::string &s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + + /// @} + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + } +}; + +} // namespace gtsam diff --git a/gtsam/symbolic/SymbolicBayesTree.cpp b/gtsam/symbolic/SymbolicBayesTree.cpp new file mode 100644 index 000000000..8797ba363 --- /dev/null +++ b/gtsam/symbolic/SymbolicBayesTree.cpp @@ -0,0 +1,40 @@ +/* ---------------------------------------------------------------------------- + + * 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 SymbolicBayesTree.h + * @date Oct 29, 2009 + * @author Frank Dellaert + * @author Richard Roberts + */ + +#include + +#include +#include +#include +#include +#include +#include + +namespace gtsam { + + // Instantiate base classes + template class BayesTreeCliqueBase; + template class BayesTree; + + /* ************************************************************************* */\ + bool SymbolicBayesTree::equals(const This& other, double tol /* = 1e-9 */) const + { + return Base::equals(other, tol); + } + +} diff --git a/gtsam/symbolic/SymbolicBayesTree.h b/gtsam/symbolic/SymbolicBayesTree.h new file mode 100644 index 000000000..526120a30 --- /dev/null +++ b/gtsam/symbolic/SymbolicBayesTree.h @@ -0,0 +1,73 @@ +/* ---------------------------------------------------------------------------- + + * 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 SymbolicBayesTree.h + * @date Oct 29, 2009 + * @author Frank Dellaert + * @author Richard Roberts + */ + +#pragma once + +#include +#include +#include +#include + +namespace gtsam { + + // Forward declarations + class SymbolicConditional; + + /* ************************************************************************* */ + /// A clique in a SymbolicBayesTree + class GTSAM_EXPORT SymbolicBayesTreeClique : + public BayesTreeCliqueBase + { + public: + typedef SymbolicBayesTreeClique This; + typedef BayesTreeCliqueBase Base; + typedef boost::shared_ptr shared_ptr; + typedef boost::weak_ptr weak_ptr; + SymbolicBayesTreeClique() {} + SymbolicBayesTreeClique(const boost::shared_ptr& conditional) : Base(conditional) {} + }; + + /* ************************************************************************* */ + /// A Bayes tree that represents the connectivity between variables but is not associated with any + /// probability functions. + class GTSAM_EXPORT SymbolicBayesTree : + public BayesTree + { + private: + typedef BayesTree Base; + + public: + typedef SymbolicBayesTree This; + typedef boost::shared_ptr shared_ptr; + + /** Default constructor, creates an empty Bayes tree */ + SymbolicBayesTree() {} + + /** check equality */ + bool equals(const This& other, double tol = 1e-9) const; + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + } + }; + +} diff --git a/gtsam/symbolic/SymbolicConditional.cpp b/gtsam/symbolic/SymbolicConditional.cpp new file mode 100644 index 000000000..d733c0937 --- /dev/null +++ b/gtsam/symbolic/SymbolicConditional.cpp @@ -0,0 +1,37 @@ +/* ---------------------------------------------------------------------------- + + * 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 SymbolicConditional.cpp + * @author Richard Roberts + * @date Oct 17, 2010 + */ + +#include +#include + +namespace gtsam { + + using namespace std; + + /* ************************************************************************* */ + void SymbolicConditional::print(const std::string& str, const KeyFormatter& keyFormatter) const + { + BaseConditional::print(str, keyFormatter); + } + + /* ************************************************************************* */ + bool SymbolicConditional::equals(const This& c, double tol) const + { + return BaseFactor::equals(c) && BaseConditional::equals(c); + } + +} diff --git a/gtsam/symbolic/SymbolicConditional.h b/gtsam/symbolic/SymbolicConditional.h new file mode 100644 index 000000000..01f7366d6 --- /dev/null +++ b/gtsam/symbolic/SymbolicConditional.h @@ -0,0 +1,124 @@ +/* ---------------------------------------------------------------------------- + + * 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 SymbolicConditional.h + * @author Richard Roberts + * @date Oct 17, 2010 + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + + /** + * SymbolicConditional is a conditional with keys but no probability + * data, produced by symbolic elimination of SymbolicFactor. + * + * It is also a SymbolicFactor, and thus derives from it. It + * derives also from Conditional, which is a generic interface + * class for conditionals. + * \nosubgrouping + */ + class GTSAM_EXPORT SymbolicConditional : + public SymbolicFactor, + public Conditional { + + public: + typedef SymbolicConditional This; /// Typedef to this class + typedef SymbolicFactor BaseFactor; /// Typedef to the factor base class + typedef Conditional BaseConditional; /// Typedef to the conditional base class + typedef boost::shared_ptr shared_ptr; /// Boost shared_ptr to this class + typedef BaseFactor::iterator iterator; /// iterator to keys + typedef BaseFactor::const_iterator const_iterator; /// const_iterator to keys + + /// @name Standard Constructors + /// @{ + + /** Empty Constructor to make serialization possible */ + SymbolicConditional() {} + + /** No parents */ + SymbolicConditional(Key j) : BaseFactor(j), BaseConditional(1) {} + + /** Single parent */ + SymbolicConditional(Key j, Key parent) : BaseFactor(j, parent), BaseConditional(1) {} + + /** Two parents */ + SymbolicConditional(Key j, Key parent1, Key parent2) : BaseFactor(j, parent1, parent2), BaseConditional(1) {} + + /** Three parents */ + SymbolicConditional(Key j, Key parent1, Key parent2, Key parent3) : BaseFactor(j, parent1, parent2, parent3), BaseConditional(1) {} + + /** Named constructor from an arbitrary number of keys and frontals */ + template + static SymbolicConditional FromIterators(ITERATOR firstKey, ITERATOR lastKey, size_t nrFrontals) + { + SymbolicConditional result; + (BaseFactor&)result = BaseFactor::FromIterators(firstKey, lastKey); + result.nrFrontals_ = nrFrontals; + return result; + } + + /** Named constructor from an arbitrary number of keys and frontals */ + template + static SymbolicConditional::shared_ptr FromIteratorsShared(ITERATOR firstKey, ITERATOR lastKey, size_t nrFrontals) + { + SymbolicConditional::shared_ptr result = boost::make_shared(); + result->keys_.assign(firstKey, lastKey); + result->nrFrontals_ = nrFrontals; + return result; + } + + /** Named constructor from an arbitrary number of keys and frontals */ + template + static SymbolicConditional FromKeys(const CONTAINER& keys, size_t nrFrontals) { + return FromIterators(keys.begin(), keys.end(), nrFrontals); + } + + /** Named constructor from an arbitrary number of keys and frontals */ + template + static SymbolicConditional::shared_ptr FromKeysShared(const CONTAINER& keys, size_t nrFrontals) { + return FromIteratorsShared(keys.begin(), keys.end(), nrFrontals); + } + + virtual ~SymbolicConditional() {} + + /// Copy this object as its actual derived type. + SymbolicFactor::shared_ptr clone() const { return boost::make_shared(*this); } + + /// @} + + /// @name Testable + + /** Print with optional formatter */ + void print(const std::string& str = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + + /** Check equality */ + bool equals(const This& c, double tol = 1e-9) const; + + /// @} + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(BaseFactor); + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(BaseConditional); + } + }; + +} diff --git a/gtsam/symbolic/SymbolicEliminationTree.cpp b/gtsam/symbolic/SymbolicEliminationTree.cpp new file mode 100644 index 000000000..bd11274bd --- /dev/null +++ b/gtsam/symbolic/SymbolicEliminationTree.cpp @@ -0,0 +1,44 @@ +/* ---------------------------------------------------------------------------- + + * 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 SymbolicEliminationTree.cpp + * @date Mar 29, 2013 + * @author Frank Dellaert + * @author Richard Roberts + */ + +#include +#include + +namespace gtsam { + + // Instantiate base class + template class EliminationTree; + + /* ************************************************************************* */ + SymbolicEliminationTree::SymbolicEliminationTree( + const SymbolicFactorGraph& factorGraph, const VariableIndex& structure, + const Ordering& order) : + Base(factorGraph, structure, order) {} + + /* ************************************************************************* */ + SymbolicEliminationTree::SymbolicEliminationTree( + const SymbolicFactorGraph& factorGraph, const Ordering& order) : + Base(factorGraph, order) {} + + /* ************************************************************************* */ + bool SymbolicEliminationTree::equals(const This& other, double tol) const + { + return Base::equals(other, tol); + } + +} diff --git a/gtsam/symbolic/SymbolicEliminationTree.h b/gtsam/symbolic/SymbolicEliminationTree.h new file mode 100644 index 000000000..5a44d451d --- /dev/null +++ b/gtsam/symbolic/SymbolicEliminationTree.h @@ -0,0 +1,62 @@ +/* ---------------------------------------------------------------------------- + + * 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 SymbolicEliminationTree.h + * @date Mar 29, 2013 + * @author Frank Dellaert + * @author Richard Roberts + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + + class GTSAM_EXPORT SymbolicEliminationTree : + public EliminationTree + { + public: + typedef EliminationTree Base; ///< Base class + typedef SymbolicEliminationTree This; ///< This class + typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class + + /** Build the elimination tree of a factor graph using pre-computed column structure. + * @param factorGraph The factor graph for which to build the elimination tree + * @param structure The set of factors involving each variable. If this is not precomputed, + * you can call the Create(const FactorGraph&) named constructor instead. + * @return The elimination tree */ + SymbolicEliminationTree(const SymbolicFactorGraph& factorGraph, + const VariableIndex& structure, const Ordering& order); + + /** Build the elimination tree of a factor graph. Note that this has to compute the column + * structure as a VariableIndex, so if you already have this precomputed, use the other + * constructor instead. + * @param factorGraph The factor graph for which to build the elimination tree */ + SymbolicEliminationTree(const SymbolicFactorGraph& factorGraph, + const Ordering& order); + + /** Test whether the tree is equal to another */ + bool equals(const This& other, double tol = 1e-9) const; + + private: + + /** Private default constructor for testing */ + SymbolicEliminationTree() {} + + friend class ::EliminationTreeTester; + + }; + +} diff --git a/gtsam/symbolic/SymbolicFactor-inst.h b/gtsam/symbolic/SymbolicFactor-inst.h new file mode 100644 index 000000000..56850e991 --- /dev/null +++ b/gtsam/symbolic/SymbolicFactor-inst.h @@ -0,0 +1,67 @@ +/* ---------------------------------------------------------------------------- + + * 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 SymbolicFactor-inst.h + * @author Richard Roberts + * @date Oct 17, 2010 + */ + +#pragma once + +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace gtsam +{ + namespace internal + { + /** Implementation of dense elimination function for symbolic factors. This is a templated + * version for internally doing symbolic elimination on any factor. */ + template + std::pair, boost::shared_ptr > + EliminateSymbolic(const FactorGraph& factors, const Ordering& keys) + { + // Gather all keys + FastSet allKeys; + BOOST_FOREACH(const boost::shared_ptr& factor, factors) { + allKeys.insert(factor->begin(), factor->end()); + } + + // Check keys + BOOST_FOREACH(Key key, keys) { + if(allKeys.find(key) == allKeys.end()) + throw std::runtime_error("Requested to eliminate a key that is not in the factors"); + } + + // Sort frontal keys + FastSet frontals(keys); + const size_t nFrontals = keys.size(); + + // Build a key vector with the frontals followed by the separator + FastVector orderedKeys(allKeys.size()); + std::copy(keys.begin(), keys.end(), orderedKeys.begin()); + std::set_difference(allKeys.begin(), allKeys.end(), frontals.begin(), frontals.end(), orderedKeys.begin() + nFrontals); + + // Return resulting conditional and factor + return std::make_pair( + SymbolicConditional::FromKeysShared(orderedKeys, nFrontals), + SymbolicFactor::FromIteratorsShared(orderedKeys.begin() + nFrontals, orderedKeys.end())); + } + } +} diff --git a/gtsam/symbolic/SymbolicFactor.cpp b/gtsam/symbolic/SymbolicFactor.cpp new file mode 100644 index 000000000..327de0c10 --- /dev/null +++ b/gtsam/symbolic/SymbolicFactor.cpp @@ -0,0 +1,51 @@ +/* ---------------------------------------------------------------------------- + + * 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 SymbolicFactor.cpp + * @author Richard Roberts + * @date Oct 17, 2010 + */ + +#include +#include +#include +#include +#include +#include + +using namespace std; + +namespace gtsam { + + /* ************************************************************************* */ + std::pair, boost::shared_ptr > + EliminateSymbolic(const SymbolicFactorGraph& factors, const Ordering& keys) + { + return internal::EliminateSymbolic(factors, keys); + } + + /* ************************************************************************* */ + bool SymbolicFactor::equals(const This& other, double tol) const + { + return Base::equals(other, tol); + } + + /* ************************************************************************* */ + std::pair, boost::shared_ptr > + SymbolicFactor::eliminate(const Ordering& keys) const + { + SymbolicFactorGraph graph; + graph += *this; // TODO: Is there a way to avoid copying this factor? + return EliminateSymbolic(graph, keys); + } + +} // gtsam diff --git a/gtsam/symbolic/SymbolicFactor.h b/gtsam/symbolic/SymbolicFactor.h new file mode 100644 index 000000000..0054f9fbc --- /dev/null +++ b/gtsam/symbolic/SymbolicFactor.h @@ -0,0 +1,161 @@ +/* ---------------------------------------------------------------------------- + + * 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 SymbolicFactor.h + * @author Richard Roberts + * @date Oct 17, 2010 + */ + +#pragma once + +#include +#include +#include +#include + +#include +#include + +namespace gtsam { + + // Forward declarations + class SymbolicConditional; + class Ordering; + + /** SymbolicFactor represents a symbolic factor that specifies graph topology but is not + * associated with any numerical function. + * \nosubgrouping */ + class GTSAM_EXPORT SymbolicFactor : public Factor { + + public: + + typedef SymbolicFactor This; + typedef Factor Base; + typedef SymbolicConditional ConditionalType; + + /** Overriding the shared_ptr typedef */ + typedef boost::shared_ptr shared_ptr; + + /// @name Standard Interface + /// @{ + + /** Default constructor for I/O */ + SymbolicFactor() {} + + /** Construct unary factor */ + explicit SymbolicFactor(Key j) : + Base(boost::assign::cref_list_of<1>(j)) {} + + /** Construct binary factor */ + SymbolicFactor(Key j1, Key j2) : + Base(boost::assign::cref_list_of<2>(j1)(j2)) {} + + /** Construct ternary factor */ + SymbolicFactor(Key j1, Key j2, Key j3) : + Base(boost::assign::cref_list_of<3>(j1)(j2)(j3)) {} + + /** Construct 4-way factor */ + SymbolicFactor(Key j1, Key j2, Key j3, Key j4) : + Base(boost::assign::cref_list_of<4>(j1)(j2)(j3)(j4)) {} + + /** Construct 5-way factor */ + SymbolicFactor(Key j1, Key j2, Key j3, Key j4, Key j5) : + Base(boost::assign::cref_list_of<5>(j1)(j2)(j3)(j4)(j5)) {} + + /** Construct 6-way factor */ + SymbolicFactor(Key j1, Key j2, Key j3, Key j4, Key j5, Key j6) : + Base(boost::assign::cref_list_of<6>(j1)(j2)(j3)(j4)(j5)(j6)) {} + + /** Create symbolic version of any factor */ + explicit SymbolicFactor(const Factor& factor) : Base(factor.keys()) {} + + virtual ~SymbolicFactor() {} + + /// Copy this object as its actual derived type. + SymbolicFactor::shared_ptr clone() const { return boost::make_shared(*this); } + + /// @} + + /// @name Testable + /// @{ + + bool equals(const This& other, double tol = 1e-9) const; + + /// @} + + /// @name Advanced Constructors + /// @{ + public: + /** Constructor from a collection of keys */ + template + static SymbolicFactor FromIterators(KEYITERATOR beginKey, KEYITERATOR endKey) { + return SymbolicFactor(Base::FromIterators(beginKey, endKey)); + } + + /** Constructor from a collection of keys */ + template + static SymbolicFactor::shared_ptr FromIteratorsShared(KEYITERATOR beginKey, KEYITERATOR endKey) { + SymbolicFactor::shared_ptr result = boost::make_shared(); + result->keys_.assign(beginKey, endKey); + return result; + } + + /** Constructor from a collection of keys - compatible with boost::assign::list_of and + * boost::assign::cref_list_of */ + template + static SymbolicFactor FromKeys(const CONTAINER& keys) { + return SymbolicFactor(Base::FromKeys(keys)); + } + + /** Constructor from a collection of keys - compatible with boost::assign::list_of and + * boost::assign::cref_list_of */ + template + static SymbolicFactor::shared_ptr FromKeysShared(const CONTAINER& keys) { + return FromIteratorsShared(keys.begin(), keys.end()); + } + + /// @} + + /// @name Standard Interface + /// @{ + + /** Whether the factor is empty (involves zero variables). */ + bool empty() const { return keys_.empty(); } + + /** Eliminate the variables in \c keys, in the order specified in \c keys, returning a + * conditional and marginal. */ + std::pair, boost::shared_ptr > + eliminate(const Ordering& keys) const; + + /// @} + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + } + }; // IndexFactor + + // Forward declarations + class SymbolicFactorGraph; + class Ordering; + + /** Dense elimination function for symbolic factors. This is usually provided as an argument to + * one of the factor graph elimination functions (see EliminateableFactorGraph). The factor + * graph elimination functions do sparse variable elimination, and use this function to eliminate + * single variables or variable cliques. */ + GTSAM_EXPORT std::pair, boost::shared_ptr > + EliminateSymbolic(const SymbolicFactorGraph& factors, const Ordering& keys); + +} diff --git a/gtsam/symbolic/SymbolicFactorGraph.cpp b/gtsam/symbolic/SymbolicFactorGraph.cpp new file mode 100644 index 000000000..574268472 --- /dev/null +++ b/gtsam/symbolic/SymbolicFactorGraph.cpp @@ -0,0 +1,63 @@ +/* ---------------------------------------------------------------------------- + + * 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 SymbolicFactorGraph.cpp + * @date Oct 29, 2009 + * @author Frank Dellaert + */ + +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace gtsam { + + // Instantiate base classes + template class FactorGraph; + template class EliminateableFactorGraph; + + using namespace std; + + /* ************************************************************************* */ + bool SymbolicFactorGraph::equals(const This& fg, double tol) const + { + return Base::equals(fg, tol); + } + + /* ************************************************************************* */ + void SymbolicFactorGraph::push_factor(Key key) { + push_back(boost::make_shared(key)); + } + + /* ************************************************************************* */ + void SymbolicFactorGraph::push_factor(Key key1, Key key2) { + push_back(boost::make_shared(key1,key2)); + } + + /* ************************************************************************* */ + void SymbolicFactorGraph::push_factor(Key key1, Key key2, Key key3) { + push_back(boost::make_shared(key1,key2,key3)); + } + + /* ************************************************************************* */ + void SymbolicFactorGraph::push_factor(Key key1, Key key2, Key key3, Key key4) { + push_back(boost::make_shared(key1,key2,key3,key4)); + } + + /* ************************************************************************* */ +} diff --git a/gtsam/symbolic/SymbolicFactorGraph.h b/gtsam/symbolic/SymbolicFactorGraph.h new file mode 100644 index 000000000..37aae2400 --- /dev/null +++ b/gtsam/symbolic/SymbolicFactorGraph.h @@ -0,0 +1,119 @@ +/* ---------------------------------------------------------------------------- + + * 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 SymbolicFactorGraph.h + * @date Oct 29, 2009 + * @author Frank Dellaert + * @author Richard Roberts + */ + +#pragma once + +#include +#include +#include +#include + +namespace gtsam { + + class SymbolicFactorGraph; + class SymbolicConditional; + class SymbolicBayesNet; + class SymbolicEliminationTree; + class SymbolicBayesTree; + class SymbolicJunctionTree; + + /* ************************************************************************* */ + template<> struct EliminationTraits + { + typedef SymbolicFactor FactorType; ///< Type of factors in factor graph + typedef SymbolicFactorGraph FactorGraphType; ///< Type of the factor graph (e.g. GaussianFactorGraph) + typedef SymbolicConditional ConditionalType; ///< Type of conditionals from elimination + typedef SymbolicBayesNet BayesNetType; ///< Type of Bayes net from sequential elimination + typedef SymbolicEliminationTree EliminationTreeType; ///< Type of elimination tree + typedef SymbolicBayesTree BayesTreeType; ///< Type of Bayes tree + typedef SymbolicJunctionTree JunctionTreeType; ///< Type of Junction tree + /// The default dense elimination function + static std::pair, boost::shared_ptr > + DefaultEliminate(const FactorGraphType& factors, const Ordering& keys) { + return EliminateSymbolic(factors, keys); } + }; + + /* ************************************************************************* */ + /** Symbolic Factor Graph + * \nosubgrouping + */ + class GTSAM_EXPORT SymbolicFactorGraph : + public FactorGraph, + public EliminateableFactorGraph + { + public: + + typedef SymbolicFactorGraph This; ///< Typedef to this class + typedef FactorGraph Base; ///< Typedef to base factor graph type + typedef EliminateableFactorGraph BaseEliminateable; ///< Typedef to base elimination class + typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class + + /// @name Standard Constructors + /// @{ + + /** Construct empty factor graph */ + SymbolicFactorGraph() {} + + /** Construct from iterator over factors */ + template + SymbolicFactorGraph(ITERATOR firstFactor, ITERATOR lastFactor) : Base(firstFactor, lastFactor) {} + + /** Construct from container of factors (shared_ptr or plain objects) */ + template + explicit SymbolicFactorGraph(const CONTAINER& factors) : Base(factors) {} + + /** Implicit copy/downcast constructor to override explicit template container constructor */ + template + SymbolicFactorGraph(const FactorGraph& graph) : Base(graph) {} + + /// @} + + /// @name Testable + /// @{ + + bool equals(const This& fg, double tol = 1e-9) const; + + /// @} + + /// @name Standard Interface + /// @{ + + /** Push back unary factor */ + void push_factor(Key key); + + /** Push back binary factor */ + void push_factor(Key key1, Key key2); + + /** Push back ternary factor */ + void push_factor(Key key1, Key key2, Key key3); + + /** Push back 4-way factor */ + void push_factor(Key key1, Key key2, Key key3, Key key4); + + /// @} + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + } + }; + +} // namespace gtsam diff --git a/gtsam/linear/GaussianFactor.cpp b/gtsam/symbolic/SymbolicISAM.cpp similarity index 52% rename from gtsam/linear/GaussianFactor.cpp rename to gtsam/symbolic/SymbolicISAM.cpp index 1bf47ec51..d7af82e5c 100644 --- a/gtsam/linear/GaussianFactor.cpp +++ b/gtsam/symbolic/SymbolicISAM.cpp @@ -10,21 +10,25 @@ * -------------------------------------------------------------------------- */ /** - * @file GaussianFactor.cpp - * @brief Linear Factor....A Gaussian - * @brief linearFactor - * @author Richard Roberts, Christian Potthast + * @file SymbolicISAM.cpp + * @date July 29, 2013 + * @author Frank Dellaert + * @author Richard Roberts */ -#include -#include +#include +#include namespace gtsam { - using namespace std; + // Instantiate base class + template class ISAM; /* ************************************************************************* */ - GaussianFactor::GaussianFactor(const GaussianConditional& c) : - IndexFactor(c) {} + SymbolicISAM::SymbolicISAM() {} + + /* ************************************************************************* */ + SymbolicISAM::SymbolicISAM(const SymbolicBayesTree& bayesTree) : + Base(bayesTree) {} } diff --git a/gtsam/symbolic/SymbolicISAM.h b/gtsam/symbolic/SymbolicISAM.h new file mode 100644 index 000000000..3f85facbf --- /dev/null +++ b/gtsam/symbolic/SymbolicISAM.h @@ -0,0 +1,46 @@ +/* ---------------------------------------------------------------------------- + + * 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 SymbolicISAM.h + * @date July 29, 2013 + * @author Frank Dellaert + * @author Richard Roberts + */ + +#pragma once + +#include +#include + +namespace gtsam { + + class GTSAM_EXPORT SymbolicISAM : public ISAM + { + public: + typedef ISAM Base; + typedef SymbolicISAM This; + typedef boost::shared_ptr shared_ptr; + + /// @name Standard Constructors + /// @{ + + /** Create an empty Bayes Tree */ + SymbolicISAM(); + + /** Copy constructor */ + SymbolicISAM(const SymbolicBayesTree& bayesTree); + + /// @} + + }; + +} diff --git a/gtsam/symbolic/SymbolicJunctionTree.cpp b/gtsam/symbolic/SymbolicJunctionTree.cpp new file mode 100644 index 000000000..00a52497b --- /dev/null +++ b/gtsam/symbolic/SymbolicJunctionTree.cpp @@ -0,0 +1,34 @@ +/* ---------------------------------------------------------------------------- + + * 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 SymbolicJunctionTree.cpp + * @date Mar 29, 2013 + * @author Frank Dellaert + * @author Richard Roberts + */ + +#include +#include +#include + +namespace gtsam { + + // Instantiate base class + template class ClusterTree; + template class JunctionTree; + + /* ************************************************************************* */ + SymbolicJunctionTree::SymbolicJunctionTree( + const SymbolicEliminationTree& eliminationTree) : + Base(eliminationTree) {} + +} diff --git a/gtsam/symbolic/SymbolicJunctionTree.h b/gtsam/symbolic/SymbolicJunctionTree.h new file mode 100644 index 000000000..709488cbf --- /dev/null +++ b/gtsam/symbolic/SymbolicJunctionTree.h @@ -0,0 +1,66 @@ +/* ---------------------------------------------------------------------------- + + * 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 SymbolicJunctionTree.h + * @date Mar 29, 2013 + * @author Frank Dellaert + * @author Richard Roberts + */ + +#include +#include +#include + +namespace gtsam { + + // Forward declarations + class SymbolicEliminationTree; + + /** + * A ClusterTree, i.e., a set of variable clusters with factors, arranged in a tree, with + * the additional property that it represents the clique tree associated with a Bayes net. + * + * In GTSAM a junction tree is an intermediate data structure in multifrontal + * variable elimination. Each node is a cluster of factors, along with a + * clique of variables that are eliminated all at once. In detail, every node k represents + * a clique (maximal fully connected subset) of an associated chordal graph, such as a + * chordal Bayes net resulting from elimination. + * + * The difference with the BayesTree is that a JunctionTree stores factors, whereas a + * BayesTree stores conditionals, that are the product of eliminating the factors in the + * corresponding JunctionTree cliques. + * + * The tree structure and elimination method are exactly analagous to the EliminationTree, + * except that in the JunctionTree, at each node multiple variables are eliminated at a time. + * + * \addtogroup Multifrontal + * \nosubgrouping + */ + class GTSAM_EXPORT SymbolicJunctionTree : + public JunctionTree { + public: + typedef JunctionTree Base; ///< Base class + typedef SymbolicJunctionTree This; ///< This class + typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class + + /** + * Build the elimination tree of a factor graph using pre-computed column structure. + * @param factorGraph The factor graph for which to build the elimination tree + * @param structure The set of factors involving each variable. If this is not + * precomputed, you can call the Create(const FactorGraph&) + * named constructor instead. + * @return The elimination tree + */ + SymbolicJunctionTree(const SymbolicEliminationTree& eliminationTree); + }; + +} diff --git a/gtsam/symbolic/tests/CMakeLists.txt b/gtsam/symbolic/tests/CMakeLists.txt new file mode 100644 index 000000000..6bef7a109 --- /dev/null +++ b/gtsam/symbolic/tests/CMakeLists.txt @@ -0,0 +1 @@ +gtsamAddTestsGlob(symbolic "test*.cpp" "" "gtsam") diff --git a/gtsam/symbolic/tests/symbolicExampleGraphs.h b/gtsam/symbolic/tests/symbolicExampleGraphs.h new file mode 100644 index 000000000..b70595ac9 --- /dev/null +++ b/gtsam/symbolic/tests/symbolicExampleGraphs.h @@ -0,0 +1,130 @@ +/* ---------------------------------------------------------------------------- + + * 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 symbolicExampleGraphs.cpp + * @date sept 15, 2012 + * @author Frank Dellaert + * @author Michael Kaess + * @author Viorela Ila + * @author Richard Roberts + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace gtsam { + namespace { + + const SymbolicFactorGraph simpleTestGraph1 = boost::assign::list_of + (boost::make_shared(0,1)) + (boost::make_shared(0,2)) + (boost::make_shared(1,4)) + (boost::make_shared(2,4)) + (boost::make_shared(3,4)); + + const SymbolicBayesNet simpleTestGraph1BayesNet = boost::assign::list_of + (boost::make_shared(0,1,2)) + (boost::make_shared(1,2,4)) + (boost::make_shared(2,4)) + (boost::make_shared(3,4)) + (boost::make_shared(4)); + + const SymbolicFactorGraph simpleTestGraph2 = boost::assign::list_of + (boost::make_shared(0,1)) + (boost::make_shared(0,2)) + (boost::make_shared(1,3)) + (boost::make_shared(1,4)) + (boost::make_shared(2,3)) + (boost::make_shared(4,5)); + + /** 1 - 0 - 2 - 3 */ + const SymbolicFactorGraph simpleChain = boost::assign::list_of + (boost::make_shared(1,0)) + (boost::make_shared(0,2)) + (boost::make_shared(2,3)); + + /* ************************************************************************* * + * 2 3 + * 0 1 : 2 + ****************************************************************************/ + SymbolicBayesTree __simpleChainBayesTree() { + SymbolicBayesTree result; + result.insertRoot(boost::make_shared( + boost::make_shared( + SymbolicConditional::FromKeys(boost::assign::list_of(2)(3), 2)))); + result.addClique(boost::make_shared( + boost::make_shared( + SymbolicConditional::FromKeys(boost::assign::list_of(0)(1)(2), 2))), + result.roots().front()); + return result; + } + + const SymbolicBayesTree simpleChainBayesTree = __simpleChainBayesTree(); + + /* ************************************************************************* */ + // Keys for ASIA example from the tutorial with A and D evidence + const Key _X_=gtsam::symbol_shorthand::X(0), _T_=gtsam::symbol_shorthand::T(0), + _S_=gtsam::symbol_shorthand::S(0), _E_=gtsam::symbol_shorthand::E(0), + _L_=gtsam::symbol_shorthand::L(0), _B_=gtsam::symbol_shorthand::B(0); + + // Factor graph for Asia example + const SymbolicFactorGraph asiaGraph = boost::assign::list_of + (boost::make_shared(_T_)) + (boost::make_shared(_S_)) + (boost::make_shared(_T_, _E_, _L_)) + (boost::make_shared(_L_, _S_)) + (boost::make_shared(_S_, _B_)) + (boost::make_shared(_E_, _B_)) + (boost::make_shared(_E_, _X_)); + + const SymbolicBayesNet asiaBayesNet = boost::assign::list_of + (boost::make_shared(_T_, _E_, _L_)) + (boost::make_shared(_X_, _E_)) + (boost::make_shared(_E_, _B_, _L_)) + (boost::make_shared(_S_, _B_, _L_)) + (boost::make_shared(_L_, _B_)) + (boost::make_shared(_B_)); + + SymbolicBayesTree __asiaBayesTree() { + SymbolicBayesTree result; + result.insertRoot(boost::make_shared( + boost::make_shared( + SymbolicConditional::FromKeys(boost::assign::list_of(_E_)(_L_)(_B_), 3)))); + result.addClique(boost::make_shared( + boost::make_shared( + SymbolicConditional::FromKeys(boost::assign::list_of(_S_)(_B_) (_L_), 1))), + result.roots().front()); + result.addClique(boost::make_shared( + boost::make_shared( + SymbolicConditional::FromKeys(boost::assign::list_of(_T_)(_E_)(_L_), 1))), + result.roots().front()); + result.addClique(boost::make_shared( + boost::make_shared( + SymbolicConditional::FromKeys(boost::assign::list_of(_X_)(_E_), 1))), + result.roots().front()); + return result; + } + + const SymbolicBayesTree asiaBayesTree = __asiaBayesTree(); + + /* ************************************************************************* */ + const Ordering asiaOrdering = boost::assign::list_of(_X_)(_T_)(_S_)(_E_)(_L_)(_B_); + + } +} diff --git a/gtsam/symbolic/tests/testSerializationSymbolic.cpp b/gtsam/symbolic/tests/testSerializationSymbolic.cpp new file mode 100644 index 000000000..64b06deec --- /dev/null +++ b/gtsam/symbolic/tests/testSerializationSymbolic.cpp @@ -0,0 +1,51 @@ +/* ---------------------------------------------------------------------------- + + * 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 testSerializationInference.cpp + * @brief + * @author Richard Roberts + * @date Feb 7, 2012 + */ + +#include + +#include +#include + +using namespace std; +using namespace gtsam; +using namespace gtsam::serializationTestHelpers; + +/* ************************************************************************* */ +TEST (Serialization, symbolic_graph) { + EXPECT(equalsObj(asiaGraph)); + EXPECT(equalsXML(asiaGraph)); + EXPECT(equalsBinary(asiaGraph)); +} + +/* ************************************************************************* */ +TEST (Serialization, symbolic_bn) { + EXPECT(equalsObj(asiaBayesNet)); + EXPECT(equalsXML(asiaBayesNet)); + EXPECT(equalsBinary(asiaBayesNet)); +} + +/* ************************************************************************* */ +TEST (Serialization, symbolic_bayes_tree ) { + EXPECT(equalsObj(asiaBayesTree)); + EXPECT(equalsXML(asiaBayesTree)); + EXPECT(equalsBinary(asiaBayesTree)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ diff --git a/gtsam/symbolic/tests/testSymbolicBayesNet.cpp b/gtsam/symbolic/tests/testSymbolicBayesNet.cpp new file mode 100644 index 000000000..99a09adc9 --- /dev/null +++ b/gtsam/symbolic/tests/testSymbolicBayesNet.cpp @@ -0,0 +1,99 @@ +/* ---------------------------------------------------------------------------- + + * 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 testSymbolicBayesNet.cpp + * @brief Unit tests for a symbolic Bayes chain + * @author Frank Dellaert + */ + +#include + +#include + +#include +#include +#include + +using namespace std; +using namespace gtsam; + +static const Key _L_ = 0; +static const Key _A_ = 1; +static const Key _B_ = 2; +static const Key _C_ = 3; +static const Key _D_ = 4; + +static SymbolicConditional::shared_ptr + B(new SymbolicConditional(_B_)), + L(new SymbolicConditional(_L_, _B_)); + +/* ************************************************************************* */ +TEST( SymbolicBayesNet, equals ) +{ + SymbolicBayesNet f1; + f1.push_back(B); + f1.push_back(L); + SymbolicBayesNet f2; + f2.push_back(L); + f2.push_back(B); + CHECK(f1.equals(f1)); + CHECK(!f1.equals(f2)); +} + +/* ************************************************************************* */ +TEST( SymbolicBayesNet, combine ) +{ + SymbolicConditional::shared_ptr + A(new SymbolicConditional(_A_,_B_,_C_)), + B(new SymbolicConditional(_B_,_C_)), + C(new SymbolicConditional(_C_)); + + // p(A|BC) + SymbolicBayesNet p_ABC; + p_ABC.push_back(A); + + // P(BC)=P(B|C)P(C) + SymbolicBayesNet p_BC; + p_BC.push_back(B); + p_BC.push_back(C); + + // P(ABC) = P(A|BC) P(BC) + p_ABC.push_back(p_BC); + + SymbolicBayesNet expected; + expected.push_back(A); + expected.push_back(B); + expected.push_back(C); + + CHECK(assert_equal(expected,p_ABC)); +} + +/* ************************************************************************* */ +TEST(SymbolicBayesNet, saveGraph) { + SymbolicBayesNet bn; + bn += SymbolicConditional(_A_, _B_); + std::vector keys; + keys.push_back(_B_); + keys.push_back(_C_); + keys.push_back(_D_); + bn += SymbolicConditional::FromKeys(keys,2); + bn += SymbolicConditional(_D_); + + bn.saveGraph("SymbolicBayesNet.dot"); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp new file mode 100644 index 000000000..c021c7fae --- /dev/null +++ b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp @@ -0,0 +1,700 @@ +/* ---------------------------------------------------------------------------- + + * 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 testSymbolicBayesTree.cpp + * @date sept 15, 2012 + * @author Frank Dellaert + * @author Michael Kaess + * @author Viorela Ila + */ + +#include +#include +#include +#include + +#include +#include +#include +#include +using namespace boost::assign; +using boost::adaptors::indirected; + +#include +#include + +using namespace std; +using namespace gtsam; +using namespace gtsam::symbol_shorthand; + +static bool debug = false; + +namespace { + + /* ************************************************************************* */ + // Helper functions for below + template + SymbolicBayesTreeClique::shared_ptr MakeClique(const KEYS& keys, DenseIndex nrFrontals) + { + return boost::make_shared( + boost::make_shared( + SymbolicConditional::FromKeys(keys, nrFrontals))); + } + + template + SymbolicBayesTreeClique::shared_ptr MakeClique( + const KEYS& keys, DenseIndex nrFrontals, const CHILDREN& children) + { + SymbolicBayesTreeClique::shared_ptr clique = + boost::make_shared( + boost::make_shared( + SymbolicConditional::FromKeys(keys, nrFrontals))); + clique->children.assign(children.begin(), children.end()); + for(typename CHILDREN::const_iterator child = children.begin(); child != children.end(); ++child) + (*child)->parent_ = clique; + return clique; + } + +} + +/* ************************************************************************* */ +TEST(SymbolicBayesTree, clear) +{ + SymbolicBayesTree bayesTree = asiaBayesTree; + bayesTree.clear(); + + SymbolicBayesTree expected; + + // Check whether cleared BayesTree is equal to a new BayesTree + CHECK(assert_equal(expected, bayesTree)); +} + +/* ************************************************************************* */ +TEST(SymbolicBayesTree, clique_structure) +{ + // l1 l2 + // / | / | + // x1 --- x2 --- x3 --- x4 --- x5 + // \ | + // l3 + SymbolicFactorGraph graph; + graph += SymbolicFactor(X(1), L(1)); + graph += SymbolicFactor(X(1), X(2)); + graph += SymbolicFactor(X(2), L(1)); + graph += SymbolicFactor(X(2), X(3)); + graph += SymbolicFactor(X(3), X(4)); + graph += SymbolicFactor(X(4), L(2)); + graph += SymbolicFactor(X(4), X(5)); + graph += SymbolicFactor(L(2), X(5)); + graph += SymbolicFactor(X(4), L(3)); + graph += SymbolicFactor(X(5), L(3)); + + SymbolicBayesTree expected; + expected.insertRoot( + MakeClique(list_of(X(2)) (X(3)), 2, list_of + (MakeClique(list_of(X(4)) (X(3)), 1, list_of + (MakeClique(list_of(X(5)) (L(2)) (X(4)), 2, list_of + (MakeClique(list_of(L(3)) (X(4)) (X(5)), 1)))))) + (MakeClique(list_of(X(1)) (L(1)) (X(2)), 2)))); + + Ordering order = list_of(X(1)) (L(3)) (L(1)) (X(5)) (X(2)) (L(2)) (X(4)) (X(3)); + + SymbolicBayesTree actual = *graph.eliminateMultifrontal(order); + + EXPECT(assert_equal(expected, actual)); +} + +/* ************************************************************************* * +Bayes Tree for testing conversion to a forest of orphans needed for incremental. + A,B + C|A E|B + D|C F|E + */ +/* ************************************************************************* */ +TEST( BayesTree, removePath ) +{ + const Key _A_=A(0), _B_=B(0), _C_=C(0), _D_=D(0), _E_=E(0), _F_=F(0); + + SymbolicBayesTree bayesTreeOrig; + bayesTreeOrig.insertRoot( + MakeClique(list_of(_A_)(_B_), 2, list_of + (MakeClique(list_of(_C_)(_A_), 1, list_of + (MakeClique(list_of(_D_)(_C_), 1)))) + (MakeClique(list_of(_E_)(_B_), 1, list_of + (MakeClique(list_of(_F_)(_E_), 1)))))); + + SymbolicBayesTree bayesTree = bayesTreeOrig; + + // remove C, expected outcome: factor graph with ABC, + // Bayes Tree now contains two orphan trees: D|C and E|B,F|E + SymbolicFactorGraph expected; + expected += SymbolicFactor(_A_,_B_); + expected += SymbolicFactor(_C_,_A_); + SymbolicBayesTree::Cliques expectedOrphans; + expectedOrphans += bayesTree[_D_], bayesTree[_E_]; + + SymbolicBayesNet bn; + SymbolicBayesTree::Cliques orphans; + bayesTree.removePath(bayesTree[_C_], bn, orphans); + SymbolicFactorGraph factors(bn); + CHECK(assert_equal(expected, factors)); + CHECK(assert_container_equal(expectedOrphans|indirected, orphans|indirected)); + + bayesTree = bayesTreeOrig; + + // remove E: factor graph with EB; E|B removed from second orphan tree + SymbolicFactorGraph expected2; + expected2 += SymbolicFactor(_A_,_B_); + expected2 += SymbolicFactor(_E_,_B_); + SymbolicBayesTree::Cliques expectedOrphans2; + expectedOrphans2 += bayesTree[_F_]; + expectedOrphans2 += bayesTree[_C_]; + + SymbolicBayesNet bn2; + SymbolicBayesTree::Cliques orphans2; + bayesTree.removePath(bayesTree[_E_], bn2, orphans2); + SymbolicFactorGraph factors2(bn2); + CHECK(assert_equal(expected2, factors2)); + CHECK(assert_container_equal(expectedOrphans2|indirected, orphans2|indirected)); +} + +/* ************************************************************************* */ +TEST( BayesTree, removePath2 ) +{ + SymbolicBayesTree bayesTree = asiaBayesTree; + + // Call remove-path with clique B + SymbolicBayesNet bn; + SymbolicBayesTree::Cliques orphans; + bayesTree.removePath(bayesTree[_B_], bn, orphans); + SymbolicFactorGraph factors(bn); + + // Check expected outcome + SymbolicFactorGraph expected; + expected += SymbolicFactor(_E_,_L_,_B_); + CHECK(assert_equal(expected, factors)); + SymbolicBayesTree::Cliques expectedOrphans; + expectedOrphans += bayesTree[_S_], bayesTree[_T_], bayesTree[_X_]; + CHECK(assert_container_equal(expectedOrphans|indirected, orphans|indirected)); +} + +/* ************************************************************************* */ +TEST(BayesTree, removePath3) +{ + SymbolicBayesTree bayesTree = asiaBayesTree; + + // Call remove-path with clique T + SymbolicBayesNet bn; + SymbolicBayesTree::Cliques orphans; + bayesTree.removePath(bayesTree[_T_], bn, orphans); + SymbolicFactorGraph factors(bn); + + // Check expected outcome + SymbolicFactorGraph expected; + expected += SymbolicFactor(_E_, _L_, _B_); + expected += SymbolicFactor(_T_, _E_, _L_); + CHECK(assert_equal(expected, factors)); + SymbolicBayesTree::Cliques expectedOrphans; + expectedOrphans += bayesTree[_S_], bayesTree[_X_]; + CHECK(assert_container_equal(expectedOrphans|indirected, orphans|indirected)); +} + +void getAllCliques(const SymbolicBayesTree::sharedClique& subtree, SymbolicBayesTree::Cliques& cliques) { + // Check if subtree exists + if (subtree) { + cliques.push_back(subtree); + // Recursive call over all child cliques + BOOST_FOREACH(SymbolicBayesTree::sharedClique& childClique, subtree->children) { + getAllCliques(childClique,cliques); + } + } +} + +/* ************************************************************************* */ +TEST( BayesTree, shortcutCheck ) +{ + const Key _A_=6, _B_=5, _C_=4, _D_=3, _E_=2, _F_=1, _G_=0; + SymbolicFactorGraph chain = list_of + (SymbolicFactor(_A_)) + (SymbolicFactor(_B_, _A_)) + (SymbolicFactor(_C_, _A_)) + (SymbolicFactor(_D_, _C_)) + (SymbolicFactor(_E_, _B_)) + (SymbolicFactor(_F_, _E_)) + (SymbolicFactor(_G_, _F_)); + SymbolicBayesTree bayesTree = *chain.eliminateMultifrontal( + Ordering(list_of(_G_)(_F_)(_E_)(_D_)(_C_)(_B_)(_A_))); + + //bayesTree.saveGraph("BT1.dot"); + + SymbolicBayesTree::sharedClique rootClique = bayesTree.roots().front(); + //rootClique->printTree(); + SymbolicBayesTree::Cliques allCliques; + getAllCliques(rootClique,allCliques); + + BOOST_FOREACH(SymbolicBayesTree::sharedClique& clique, allCliques) { + //clique->print("Clique#"); + SymbolicBayesNet bn = clique->shortcut(rootClique); + //bn.print("Shortcut:\n"); + //cout << endl; + } + + // Check if all the cached shortcuts are cleared + rootClique->deleteCachedShortcuts(); + BOOST_FOREACH(SymbolicBayesTree::sharedClique& clique, allCliques) { + bool notCleared = clique->cachedSeparatorMarginal(); + CHECK( notCleared == false); + } + EXPECT_LONGS_EQUAL(0, (long)rootClique->numCachedSeparatorMarginals()); + +// BOOST_FOREACH(SymbolicBayesTree::sharedClique& clique, allCliques) { +// clique->print("Clique#"); +// if(clique->cachedShortcut()){ +// bn = clique->cachedShortcut().get(); +// bn.print("Shortcut:\n"); +// } +// else +// cout << "Not Initialized" << endl; +// cout << endl; +// } +} + +/* ************************************************************************* */ +TEST( BayesTree, removeTop ) +{ + SymbolicBayesTree bayesTree = asiaBayesTree; + + // create a new factor to be inserted + //boost::shared_ptr newFactor(new IndexFactor(_S_,_B_)); + + // Remove the contaminated part of the Bayes tree + SymbolicBayesNet bn; + SymbolicBayesTree::Cliques orphans; + bayesTree.removeTop(list_of(_B_)(_S_), bn, orphans); + + // Check expected outcome + SymbolicBayesNet expected; + expected += SymbolicConditional::FromKeys(list_of(_E_)(_L_)(_B_), 3); + expected += SymbolicConditional::FromKeys(list_of(_S_)(_B_)(_L_), 1); + CHECK(assert_equal(expected, bn)); + + SymbolicBayesTree::Cliques expectedOrphans; + expectedOrphans += bayesTree[_T_], bayesTree[_X_]; + CHECK(assert_container_equal(expectedOrphans|indirected, orphans|indirected)); + + // Try removeTop again with a factor that should not change a thing + //boost::shared_ptr newFactor2(new IndexFactor(_B_)); + SymbolicBayesNet bn2; + SymbolicBayesTree::Cliques orphans2; + bayesTree.removeTop(list_of(_B_), bn2, orphans2); + SymbolicFactorGraph factors2(bn2); + SymbolicFactorGraph expected2; + CHECK(assert_equal(expected2, factors2)); + SymbolicBayesTree::Cliques expectedOrphans2; + CHECK(assert_container_equal(expectedOrphans2|indirected, orphans2|indirected)); +} + +/* ************************************************************************* */ +TEST( BayesTree, removeTop2 ) +{ + SymbolicBayesTree bayesTree = asiaBayesTree; + + // create two factors to be inserted + //SymbolicFactorGraph newFactors; + //newFactors.push_factor(_B_); + //newFactors.push_factor(_S_); + + // Remove the contaminated part of the Bayes tree + SymbolicBayesNet bn; + SymbolicBayesTree::Cliques orphans; + bayesTree.removeTop(list_of(_T_), bn, orphans); + + // Check expected outcome + SymbolicBayesNet expected = list_of + (SymbolicConditional::FromKeys(list_of(_E_)(_L_)(_B_), 3)) + (SymbolicConditional::FromKeys(list_of(_T_)(_E_)(_L_), 1)); + CHECK(assert_equal(expected, bn)); + + SymbolicBayesTree::Cliques expectedOrphans; + expectedOrphans += bayesTree[_S_], bayesTree[_X_]; + CHECK(assert_container_equal(expectedOrphans|indirected, orphans|indirected)); +} + +/* ************************************************************************* */ +TEST( BayesTree, removeTop3 ) +{ + SymbolicFactorGraph graph = list_of + (SymbolicFactor(L(5))) + (SymbolicFactor(X(4), L(5))) + (SymbolicFactor(X(2), X(4))) + (SymbolicFactor(X(3), X(2))); + SymbolicBayesTree bayesTree = *graph.eliminateMultifrontal( + Ordering(list_of (X(3)) (X(2)) (X(4)) (L(5)) )); + + // remove all + SymbolicBayesNet bn; + SymbolicBayesTree::Cliques orphans; + bayesTree.removeTop(list_of(L(5))(X(4))(X(2))(X(3)), bn, orphans); + + SymbolicBayesNet expectedBn = list_of + (SymbolicConditional::FromKeys(list_of(X(4))(L(5)), 2)) + (SymbolicConditional(X(2), X(4))) + (SymbolicConditional(X(3), X(2))); + EXPECT(assert_equal(expectedBn, bn)); + EXPECT(orphans.empty()); +} + +/* ************************************************************************* */ +TEST( BayesTree, removeTop4 ) +{ + SymbolicFactorGraph graph = list_of + (SymbolicFactor(L(5))) + (SymbolicFactor(X(4), L(5))) + (SymbolicFactor(X(2), X(4))) + (SymbolicFactor(X(3), X(2))); + SymbolicBayesTree bayesTree = *graph.eliminateMultifrontal( + Ordering(list_of (X(3)) (X(2)) (X(4)) (L(5)) )); + + // remove all + SymbolicBayesNet bn; + SymbolicBayesTree::Cliques orphans; + bayesTree.removeTop(list_of(X(2))(L(5))(X(4))(X(3)), bn, orphans); + + SymbolicBayesNet expectedBn = list_of + (SymbolicConditional::FromKeys(list_of(X(4))(L(5)), 2)) + (SymbolicConditional(X(2), X(4))) + (SymbolicConditional(X(3), X(2))); + EXPECT(assert_equal(expectedBn, bn)); + EXPECT(orphans.empty()); +} + +/* ************************************************************************* */ +TEST( BayesTree, removeTop5 ) +{ + // Remove top called with variables that are not in the Bayes tree + SymbolicFactorGraph graph = list_of + (SymbolicFactor(L(5))) + (SymbolicFactor(X(4), L(5))) + (SymbolicFactor(X(2), X(4))) + (SymbolicFactor(X(3), X(2))); + SymbolicBayesTree bayesTree = *graph.eliminateMultifrontal( + Ordering(list_of (X(3)) (X(2)) (X(4)) (L(5)) )); + + // Remove nonexistant + SymbolicBayesNet bn; + SymbolicBayesTree::Cliques orphans; + bayesTree.removeTop(list_of(X(10)), bn, orphans); + + SymbolicBayesNet expectedBn; + EXPECT(assert_equal(expectedBn, bn)); + EXPECT(orphans.empty()); +} + +/* ************************************************************************* */ +TEST( SymbolicBayesTree, thinTree ) { + + // create a thin-tree Bayesnet, a la Jean-Guillaume + SymbolicBayesNet bayesNet; + bayesNet.push_back(boost::make_shared(14)); + + bayesNet.push_back(boost::make_shared(13, 14)); + bayesNet.push_back(boost::make_shared(12, 14)); + + bayesNet.push_back(boost::make_shared(11, 13, 14)); + bayesNet.push_back(boost::make_shared(10, 13, 14)); + bayesNet.push_back(boost::make_shared(9, 12, 14)); + bayesNet.push_back(boost::make_shared(8, 12, 14)); + + bayesNet.push_back(boost::make_shared(7, 11, 13)); + bayesNet.push_back(boost::make_shared(6, 11, 13)); + bayesNet.push_back(boost::make_shared(5, 10, 13)); + bayesNet.push_back(boost::make_shared(4, 10, 13)); + + bayesNet.push_back(boost::make_shared(3, 9, 12)); + bayesNet.push_back(boost::make_shared(2, 9, 12)); + bayesNet.push_back(boost::make_shared(1, 8, 12)); + bayesNet.push_back(boost::make_shared(0, 8, 12)); + + if (debug) { + GTSAM_PRINT(bayesNet); + bayesNet.saveGraph("/tmp/symbolicBayesNet.dot"); + } + + // create a BayesTree out of a Bayes net + SymbolicBayesTree bayesTree = *SymbolicFactorGraph(bayesNet).eliminateMultifrontal(); + if (debug) { + GTSAM_PRINT(bayesTree); + bayesTree.saveGraph("/tmp/SymbolicBayesTree.dot"); + } + + SymbolicBayesTree::Clique::shared_ptr R = bayesTree.roots().front(); + + { + // check shortcut P(S9||R) to root + SymbolicBayesTree::Clique::shared_ptr c = bayesTree[9]; + SymbolicBayesNet shortcut = c->shortcut(R); + SymbolicBayesNet expected = list_of(SymbolicConditional(14, 11, 13)); + EXPECT(assert_equal(expected, shortcut)); + } + + { + // check shortcut P(S8||R) to root + SymbolicBayesTree::Clique::shared_ptr c = bayesTree[8]; + SymbolicBayesNet shortcut = c->shortcut(R); + SymbolicBayesNet expected = list_of + (SymbolicConditional(12, 14)) + (SymbolicConditional(14, 11, 13)); + EXPECT(assert_equal(expected, shortcut)); + } + + { + // check shortcut P(S4||R) to root + SymbolicBayesTree::Clique::shared_ptr c = bayesTree[4]; + SymbolicBayesNet shortcut = c->shortcut(R); + SymbolicBayesNet expected = list_of(SymbolicConditional(10, 11, 13)); + EXPECT(assert_equal(expected, shortcut)); + } + + { + // check shortcut P(S2||R) to root + SymbolicBayesTree::Clique::shared_ptr c = bayesTree[2]; + SymbolicBayesNet shortcut = c->shortcut(R); + SymbolicBayesNet expected = list_of(SymbolicConditional(9, 11, 12, 13)) + (SymbolicConditional(12, 11, 13)); + EXPECT(assert_equal(expected, shortcut)); + } + + { + // check shortcut P(S0||R) to root + SymbolicBayesTree::Clique::shared_ptr c = bayesTree[0]; + SymbolicBayesNet shortcut = c->shortcut(R); + SymbolicBayesNet expected = list_of(SymbolicConditional(8, 11, 12, 13)) + (SymbolicConditional(12, 11, 13)); + EXPECT(assert_equal(expected, shortcut)); + } + + SymbolicBayesNet::shared_ptr actualJoint; + + // Check joint P(8,2) + if (false) { // TODO, not disjoint + actualJoint = bayesTree.jointBayesNet(8, 2); + SymbolicBayesNet expected; + expected.push_back(boost::make_shared(8)); + expected.push_back(boost::make_shared(2, 8)); + EXPECT(assert_equal(expected, *actualJoint)); + } + + // Check joint P(1,2) + if (false) { // TODO, not disjoint + actualJoint = bayesTree.jointBayesNet(1, 2); + SymbolicBayesNet expected; + expected.push_back(boost::make_shared(2)); + expected.push_back(boost::make_shared(1, 2)); + EXPECT(assert_equal(expected, *actualJoint)); + } + + // Check joint P(2,6) + if (true) { + actualJoint = bayesTree.jointBayesNet(2, 6); + SymbolicBayesNet expected; + expected.push_back(boost::make_shared(2, 6)); + expected.push_back(boost::make_shared(6)); + EXPECT(assert_equal(expected, *actualJoint)); + } + + // Check joint P(4,6) + if (false) { // TODO, not disjoint + actualJoint = bayesTree.jointBayesNet(4, 6); + SymbolicBayesNet expected; + expected.push_back(boost::make_shared(6)); + expected.push_back(boost::make_shared(4, 6)); + EXPECT(assert_equal(expected, *actualJoint)); + } +} + +/* ************************************************************************* */ +TEST(SymbolicBayesTree, forest_joint) +{ + // Create forest + SymbolicBayesTreeClique::shared_ptr root1 = MakeClique(list_of(1), 1); + SymbolicBayesTreeClique::shared_ptr root2 = MakeClique(list_of(2), 1); + SymbolicBayesTree bayesTree; + bayesTree.insertRoot(root1); + bayesTree.insertRoot(root2); + + // Check joint + SymbolicBayesNet expected = list_of + (SymbolicConditional(1)) + (SymbolicConditional(2)); + SymbolicBayesNet actual = *bayesTree.jointBayesNet(1, 2); + + EXPECT(assert_equal(expected, actual)); +} + +/* ************************************************************************* * + Bayes tree for smoother with "natural" ordering: + C1 5 6 + C2 4 : 5 + C3 3 : 4 + C4 2 : 3 + C5 1 : 2 + C6 0 : 1 + **************************************************************************** */ + +TEST( SymbolicBayesTree, linear_smoother_shortcuts ) { + // Create smoother with 7 nodes + SymbolicFactorGraph smoother; + smoother.push_factor(0); + smoother.push_factor(0, 1); + smoother.push_factor(1, 2); + smoother.push_factor(2, 3); + smoother.push_factor(3, 4); + smoother.push_factor(4, 5); + smoother.push_factor(5, 6); + + // Eliminate in numerical order 0..6 + Ordering ordering(smoother.keys()); + SymbolicBayesNet bayesNet = *smoother.eliminateSequential(ordering); + + if (debug) { + GTSAM_PRINT(bayesNet); + bayesNet.saveGraph("/tmp/symbolicBayesNet.dot"); + } + + // create a BayesTree + SymbolicBayesTree bayesTree = *smoother.eliminateMultifrontal(ordering); + if (debug) { + GTSAM_PRINT(bayesTree); + bayesTree.saveGraph("/tmp/SymbolicBayesTree.dot"); + } + + SymbolicBayesTree::Clique::shared_ptr R = bayesTree.roots().front(); + + { + // check shortcut P(S2||R) to root + SymbolicBayesTree::Clique::shared_ptr c = bayesTree[4]; // 4 is frontal in C2 + SymbolicBayesNet shortcut = c->shortcut(R); + SymbolicBayesNet expected; + EXPECT(assert_equal(expected, shortcut)); + } + + { + // check shortcut P(S3||R) to root + SymbolicBayesTree::Clique::shared_ptr c = bayesTree[3]; // 3 is frontal in C3 + SymbolicBayesNet shortcut = c->shortcut(R); + SymbolicBayesNet expected = list_of(SymbolicConditional(4, 5)); + EXPECT(assert_equal(expected, shortcut)); + } + + { + // check shortcut P(S4||R) to root + SymbolicBayesTree::Clique::shared_ptr c = bayesTree[2]; // 2 is frontal in C4 + SymbolicBayesNet shortcut = c->shortcut(R); + SymbolicBayesNet expected = list_of(SymbolicConditional(3, 5)); + EXPECT(assert_equal(expected, shortcut)); + } +} + +/* ************************************************************************* */ +// from testSymbolicJunctionTree, which failed at one point +TEST(SymbolicBayesTree, complicatedMarginal) +{ + // Create the conditionals to go in the BayesTree + SymbolicBayesTreeClique::shared_ptr cur; + SymbolicBayesTreeClique::shared_ptr root = MakeClique(list_of(11)(12), 2); + cur = root; + + root->children += MakeClique(list_of(9)(10)(11)(12), 2); + root->children.back()->parent_ = root; + + root->children += MakeClique(list_of(7)(8)(11), 2); + root->children.back()->parent_ = root; + cur = root->children.back(); + + cur->children += MakeClique(list_of(5)(6)(7)(8), 2); + cur->children.back()->parent_ = cur; + cur = cur->children.back(); + + cur->children += MakeClique(list_of(3)(4)(6), 2); + cur->children.back()->parent_ = cur; + + cur->children += MakeClique(list_of(1)(2)(5), 2); + cur->children.back()->parent_ = cur; + + // Create Bayes Tree + SymbolicBayesTree bt; + bt.insertRoot(root); + if (debug) { + GTSAM_PRINT(bt); + bt.saveGraph("/tmp/SymbolicBayesTree.dot"); + } + + // Shortcut on 9 + { + SymbolicBayesTree::Clique::shared_ptr c = bt[9]; + SymbolicBayesNet shortcut = c->shortcut(root); + EXPECT(assert_equal(SymbolicBayesNet(), shortcut)); + } + + // Shortcut on 7 + { + SymbolicBayesTree::Clique::shared_ptr c = bt[7]; + SymbolicBayesNet shortcut = c->shortcut(root); + EXPECT(assert_equal(SymbolicBayesNet(), shortcut)); + } + + // Shortcut on 5 + { + SymbolicBayesTree::Clique::shared_ptr c = bt[5]; + SymbolicBayesNet shortcut = c->shortcut(root); + SymbolicBayesNet expected = list_of + (SymbolicConditional(7, 8, 11)) + (SymbolicConditional(8, 11)); + EXPECT(assert_equal(expected, shortcut)); + } + + // Shortcut on 3 + { + SymbolicBayesTree::Clique::shared_ptr c = bt[3]; + SymbolicBayesNet shortcut = c->shortcut(root); + SymbolicBayesNet expected = list_of(SymbolicConditional(6, 11)); + EXPECT(assert_equal(expected, shortcut)); + } + + // Shortcut on 1 + { + SymbolicBayesTree::Clique::shared_ptr c = bt[1]; + SymbolicBayesNet shortcut = c->shortcut(root); + SymbolicBayesNet expected = list_of(SymbolicConditional(5, 11)); + EXPECT(assert_equal(expected, shortcut)); + } + + // Marginal on 5 + { + SymbolicFactor::shared_ptr actual = bt.marginalFactor(5); + EXPECT(assert_equal(SymbolicFactor(5), *actual, 1e-1)); + } + + // Shortcut on 6 + { + SymbolicFactor::shared_ptr actual = bt.marginalFactor(6); + EXPECT(assert_equal(SymbolicFactor(6), *actual, 1e-1)); + } + +} +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + diff --git a/gtsam/symbolic/tests/testSymbolicConditional.cpp b/gtsam/symbolic/tests/testSymbolicConditional.cpp new file mode 100644 index 000000000..ddc602443 --- /dev/null +++ b/gtsam/symbolic/tests/testSymbolicConditional.cpp @@ -0,0 +1,121 @@ +/* ---------------------------------------------------------------------------- + + * 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 testSymbolicConditional.cpp + * @brief Unit tests for SymbolicConditional class + * @author Frank Dellaert + */ + +#include +using namespace boost::assign; +#include + +#include +#include +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +TEST( SymbolicConditional, empty ) +{ + SymbolicConditional c0; + LONGS_EQUAL(0, (long)c0.nrFrontals()); + LONGS_EQUAL(0, (long)c0.nrParents()); +} + +/* ************************************************************************* */ +TEST( SymbolicConditional, noParents ) +{ + SymbolicConditional c0(0); + LONGS_EQUAL(1, (long)c0.nrFrontals()); + LONGS_EQUAL(0, (long)c0.nrParents()); +} + +/* ************************************************************************* */ +TEST( SymbolicConditional, oneParents ) +{ + SymbolicConditional c0(0,1); + LONGS_EQUAL(1, (long)c0.nrFrontals()); + LONGS_EQUAL(1, (long)c0.nrParents()); +} + +/* ************************************************************************* */ +TEST( SymbolicConditional, twoParents ) +{ + SymbolicConditional c0(0,1,2); + LONGS_EQUAL(1, (long)c0.nrFrontals()); + LONGS_EQUAL(2, (long)c0.nrParents()); +} + +/* ************************************************************************* */ +TEST( SymbolicConditional, threeParents ) +{ + SymbolicConditional c0(0,1,2,3); + LONGS_EQUAL(1, (long)c0.nrFrontals()); + LONGS_EQUAL(3, (long)c0.nrParents()); +} + +/* ************************************************************************* */ +TEST( SymbolicConditional, fourParents ) +{ + SymbolicConditional c0 = SymbolicConditional::FromKeys( + list_of(0)(1)(2)(3)(4), 1); + LONGS_EQUAL(1, (long)c0.nrFrontals()); + LONGS_EQUAL(4, (long)c0.nrParents()); +} + +/* ************************************************************************* */ +TEST( SymbolicConditional, FromRange ) +{ + SymbolicConditional::shared_ptr c0 = + boost::make_shared( + SymbolicConditional::FromKeys(list_of(1)(2)(3)(4)(5), 2)); + LONGS_EQUAL(2, (long)c0->nrFrontals()); + LONGS_EQUAL(3, (long)c0->nrParents()); +} + +/* ************************************************************************* */ +TEST(SymbolicConditional, Constructors) +{ + SymbolicConditional expected(3, 4); + + SymbolicConditional actual1 = SymbolicConditional::FromKeys(expected.keys(), 1); + SymbolicConditional actual2 = SymbolicConditional::FromIterators(expected.begin(), expected.end(), 1); + SymbolicConditional actual3 = *SymbolicConditional::FromKeysShared(expected.keys(), 1); + SymbolicConditional actual4 = *SymbolicConditional::FromIteratorsShared(expected.begin(), expected.end(), 1); + + EXPECT(assert_equal(expected, actual1)); + EXPECT(assert_equal(expected, actual2)); + EXPECT(assert_equal(expected, actual3)); + EXPECT(assert_equal(expected, actual4)); +} + +/* ************************************************************************* */ +TEST( SymbolicConditional, equals ) +{ + SymbolicConditional c0(0, 1, 2), c1(0, 1, 2), c2(1, 2, 3), c3(3,4); + CHECK(c0.equals(c1)); + CHECK(c1.equals(c0)); + CHECK(!c0.equals(c2)); + CHECK(!c2.equals(c0)); + CHECK(!c0.equals(c3)); + CHECK(!c3.equals(c0)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp b/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp new file mode 100644 index 000000000..d22703b14 --- /dev/null +++ b/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp @@ -0,0 +1,157 @@ +/* ---------------------------------------------------------------------------- + + * 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 testSymbolicEliminationTree.cpp + * @brief + * @author Richard Roberts + * @date Oct 14, 2010 + */ + +#include + +#include +#include +#include + +#include +#include +#include + +#include "symbolicExampleGraphs.h" + +using namespace gtsam; +using namespace gtsam::symbol_shorthand; +using namespace std; +using boost::assign::list_of; + +class EliminationTreeTester { +public: + // build hardcoded tree + static SymbolicEliminationTree buildHardcodedTree(const SymbolicFactorGraph& fg) { + + SymbolicEliminationTree::sharedNode leaf0(new SymbolicEliminationTree::Node); + leaf0->key = 0; + leaf0->factors.push_back(fg[0]); + leaf0->factors.push_back(fg[1]); + + SymbolicEliminationTree::sharedNode node1(new SymbolicEliminationTree::Node); + node1->key = 1; + node1->factors.push_back(fg[2]); + node1->children.push_back(leaf0); + + SymbolicEliminationTree::sharedNode node2(new SymbolicEliminationTree::Node); + node2->key = 2; + node2->factors.push_back(fg[3]); + node2->children.push_back(node1); + + SymbolicEliminationTree::sharedNode leaf3(new SymbolicEliminationTree::Node); + leaf3->key = 3; + leaf3->factors.push_back(fg[4]); + + SymbolicEliminationTree::sharedNode root(new SymbolicEliminationTree::Node); + root->key = 4; + root->children.push_back(leaf3); + root->children.push_back(node2); + + SymbolicEliminationTree tree; + tree.roots_.push_back(root); + return tree; + } + + template + static SymbolicEliminationTree MakeTree(const ROOTS& roots) + { + SymbolicEliminationTree et; + et.roots_.assign(roots.begin(), roots.end()); + return et; + } +}; + +template +static SymbolicEliminationTree::sharedNode MakeNode(Key key, const FACTORS& factors) +{ + SymbolicEliminationTree::sharedNode node = boost::make_shared(); + node->key = key; + SymbolicFactorGraph factorsAsGraph = factors; + node->factors.assign(factorsAsGraph.begin(), factorsAsGraph.end()); + return node; +} + +template +static SymbolicEliminationTree::sharedNode MakeNode(Key key, const FACTORS& factors, const CHILDREN& children) +{ + SymbolicEliminationTree::sharedNode node = boost::make_shared(); + node->key = key; + SymbolicFactorGraph factorsAsGraph = factors; + node->factors.assign(factorsAsGraph.begin(), factorsAsGraph.end()); + node->children.assign(children.begin(), children.end()); + return node; +} + + +/* ************************************************************************* */ +TEST(EliminationTree, Create) +{ + SymbolicEliminationTree expected = + EliminationTreeTester::buildHardcodedTree(simpleTestGraph1); + + // Build from factor graph + Ordering order; + order += 0,1,2,3,4; + SymbolicEliminationTree actual(simpleTestGraph1, order); + + CHECK(assert_equal(expected, actual)); +} + +/* ************************************************************************* */ +TEST(EliminationTree, Create2) +{ + // l1 l2 + // / | / | + // x1 --- x2 --- x3 --- x4 --- x5 + // \ | + // l3 + SymbolicFactorGraph graph; + graph += SymbolicFactor(X(1), L(1)); + graph += SymbolicFactor(X(1), X(2)); + graph += SymbolicFactor(X(2), L(1)); + graph += SymbolicFactor(X(2), X(3)); + graph += SymbolicFactor(X(3), X(4)); + graph += SymbolicFactor(X(4), L(2)); + graph += SymbolicFactor(X(4), X(5)); + graph += SymbolicFactor(L(2), X(5)); + graph += SymbolicFactor(X(4), L(3)); + graph += SymbolicFactor(X(5), L(3)); + + SymbolicEliminationTree expected = EliminationTreeTester::MakeTree(list_of + (MakeNode(X(3), SymbolicFactorGraph(), list_of + (MakeNode(X(2), list_of(SymbolicFactor(X(2), X(3))), list_of + (MakeNode(L(1), list_of(SymbolicFactor(X(2), L(1))), list_of + (MakeNode(X(1), list_of(SymbolicFactor(X(1), L(1))) (SymbolicFactor(X(1), X(2))))))))) + (MakeNode(X(4), list_of(SymbolicFactor(X(3), X(4))), list_of + (MakeNode(L(2), list_of(SymbolicFactor(X(4), L(2))), list_of + (MakeNode(X(5), list_of(SymbolicFactor(X(4), X(5))) (SymbolicFactor(L(2), X(5))), list_of + (MakeNode(L(3), list_of(SymbolicFactor(X(4), L(3))) (SymbolicFactor(X(5), L(3)))))))))))))); + + Ordering order = list_of(X(1)) (L(3)) (L(1)) (X(5)) (X(2)) (L(2)) (X(4)) (X(3)); + + SymbolicEliminationTree actual(graph, order); + + EXPECT(assert_equal(expected, actual)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam/inference/tests/testSymbolicFactor.cpp b/gtsam/symbolic/tests/testSymbolicFactor.cpp similarity index 50% rename from gtsam/inference/tests/testSymbolicFactor.cpp rename to gtsam/symbolic/tests/testSymbolicFactor.cpp index a474c09a1..57c3a2d5f 100644 --- a/gtsam/inference/tests/testSymbolicFactor.cpp +++ b/gtsam/symbolic/tests/testSymbolicFactor.cpp @@ -17,37 +17,23 @@ #include #include -#include -#include -#include +#include +#include +#include #include +#include +#include #include using namespace std; using namespace gtsam; using namespace boost::assign; -/* ************************************************************************* */ -TEST(SymbolicFactor, constructor) { - - // Frontals sorted, parents not sorted - vector keys1; keys1 += 3, 4, 5, 9, 7, 8; - (void)IndexConditional(keys1, 3); - -// // Frontals not sorted -// vector keys2; keys2 += 3, 5, 4, 9, 7, 8; -// (void)IndexConditional::FromRange(keys2.begin(), keys2.end(), 3); - -// // Frontals not before parents -// vector keys3; keys3 += 3, 4, 5, 1, 7, 8; -// (void)IndexConditional::FromRange(keys3.begin(), keys3.end(), 3); -} - /* ************************************************************************* */ #ifdef TRACK_ELIMINATE TEST(SymbolicFactor, eliminate) { - vector keys; keys += 2, 3, 4, 6, 7, 9, 10, 11; + vector keys; keys += 2, 3, 4, 6, 7, 9, 10, 11; IndexFactor actual(keys.begin(), keys.end()); BayesNet fragment = *actual.eliminate(3); @@ -64,45 +50,42 @@ TEST(SymbolicFactor, eliminate) { CHECK(assert_equal(**fragmentCond++, *expected2)); } #endif + /* ************************************************************************* */ -TEST(SymbolicFactor, EliminateSymbolic) { - SymbolicFactorGraph factors; - factors.push_factor(2,4,6); - factors.push_factor(1,2,5); - factors.push_factor(0,3); +TEST(SymbolicFactor, Constructors) +{ + SymbolicFactor expected(3, 4); - IndexFactor expectedFactor(4,5,6); - std::vector keys; keys += 0,1,2,3,4,5,6; - IndexConditional::shared_ptr expectedConditional(new IndexConditional(keys, 4)); + SymbolicFactor actual1 = SymbolicFactor::FromKeys(expected.keys()); + SymbolicFactor actual2 = SymbolicFactor::FromIterators(expected.begin(), expected.end()); + SymbolicFactor actual3 = *SymbolicFactor::FromKeysShared(expected.keys()); + SymbolicFactor actual4 = *SymbolicFactor::FromIteratorsShared(expected.begin(), expected.end()); - IndexFactor::shared_ptr actualFactor; - IndexConditional::shared_ptr actualConditional; - boost::tie(actualConditional, actualFactor) = EliminateSymbolic(factors, 4); + EXPECT(assert_equal(expected, actual1)); + EXPECT(assert_equal(expected, actual2)); + EXPECT(assert_equal(expected, actual3)); + EXPECT(assert_equal(expected, actual4)); +} - CHECK(assert_equal(*expectedConditional, *actualConditional)); +/* ************************************************************************* */ +TEST(SymbolicFactor, EliminateSymbolic) +{ + const SymbolicFactorGraph factors = list_of + (SymbolicFactor(2,4,6)) + (SymbolicFactor(1,2,5)) + (SymbolicFactor(0,3)); + + const SymbolicFactor expectedFactor(4,5,6); + const SymbolicConditional expectedConditional = + SymbolicConditional::FromKeys(list_of(0)(1)(2)(3)(4)(5)(6), 4); + + SymbolicFactor::shared_ptr actualFactor; + SymbolicConditional::shared_ptr actualConditional; + boost::tie(actualConditional, actualFactor) = + EliminateSymbolic(factors, list_of(0)(1)(2)(3)); + + CHECK(assert_equal(expectedConditional, *actualConditional)); CHECK(assert_equal(expectedFactor, *actualFactor)); - -// BayesNet expected_bn; -// vector parents; -// -// parents.clear(); parents += 1,2,3,4,5,6; -// expected_bn.push_back(IndexConditional::shared_ptr(new IndexConditional(0, parents))); -// -// parents.clear(); parents += 2,3,4,5,6; -// expected_bn.push_back(IndexConditional::shared_ptr(new IndexConditional(1, parents))); -// -// parents.clear(); parents += 3,4,5,6; -// expected_bn.push_back(IndexConditional::shared_ptr(new IndexConditional(2, parents))); -// -// parents.clear(); parents += 4,5,6; -// expected_bn.push_back(IndexConditional::shared_ptr(new IndexConditional(3, parents))); -// -// BayesNet::shared_ptr actual_bn; -// IndexFactor::shared_ptr actual_factor; -// boost::tie(actual_bn, actual_factor) = EliminateSymbolic(factors, 4); -// -// CHECK(assert_equal(expected_bn, *actual_bn)); -// CHECK(assert_equal(expected_factor, *actual_factor)); } /* ************************************************************************* */ diff --git a/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp b/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp new file mode 100644 index 000000000..a5ad7519c --- /dev/null +++ b/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp @@ -0,0 +1,310 @@ +/* ---------------------------------------------------------------------------- + + * 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 testSymbolicFactorGraph.cpp + * @brief Unit tests for symbolic factor graphs + * @author Christian Potthast + **/ + +#include + +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; +using namespace boost::assign; + +/* ************************************************************************* */ +TEST(SymbolicFactorGraph, eliminateFullSequential) +{ + // Test with simpleTestGraph1 + Ordering order; + order += 0,1,2,3,4; + SymbolicBayesNet actual1 = *simpleTestGraph1.eliminateSequential(order); + EXPECT(assert_equal(simpleTestGraph1BayesNet, actual1)); + + // Test with Asia graph + SymbolicBayesNet actual2 = *asiaGraph.eliminateSequential(asiaOrdering); + EXPECT(assert_equal(asiaBayesNet, actual2)); +} + +/* ************************************************************************* */ +TEST(SymbolicFactorGraph, eliminatePartialSequential) +{ + // Eliminate 0 and 1 + const Ordering order = list_of(0)(1); + + const SymbolicBayesNet expectedBayesNet = list_of + (SymbolicConditional(0,1,2)) + (SymbolicConditional(1,2,3,4)); + + const SymbolicFactorGraph expectedSfg = list_of + (SymbolicFactor(2,3)) + (SymbolicFactor(4,5)) + (SymbolicFactor(2,3,4)); + + SymbolicBayesNet::shared_ptr actualBayesNet; + SymbolicFactorGraph::shared_ptr actualSfg; + boost::tie(actualBayesNet, actualSfg) = + simpleTestGraph2.eliminatePartialSequential(Ordering(list_of(0)(1))); + + EXPECT(assert_equal(expectedSfg, *actualSfg)); + EXPECT(assert_equal(expectedBayesNet, *actualBayesNet)); + + SymbolicBayesNet::shared_ptr actualBayesNet2; + SymbolicFactorGraph::shared_ptr actualSfg2; + boost::tie(actualBayesNet2, actualSfg2) = + simpleTestGraph2.eliminatePartialSequential(list_of(0)(1).convert_to_container >()); + + EXPECT(assert_equal(expectedSfg, *actualSfg2)); + EXPECT(assert_equal(expectedBayesNet, *actualBayesNet2)); +} + +/* ************************************************************************* */ +TEST(SymbolicFactorGraph, eliminateFullMultifrontal) +{ + Ordering ordering; ordering += 0,1,2,3; + SymbolicBayesTree actual1 = + *simpleChain.eliminateMultifrontal(ordering); + EXPECT(assert_equal(simpleChainBayesTree, actual1)); + + SymbolicBayesTree actual2 = + *asiaGraph.eliminateMultifrontal(asiaOrdering); + EXPECT(assert_equal(asiaBayesTree, actual2)); +} + +/* ************************************************************************* */ +TEST(SymbolicFactorGraph, eliminatePartialMultifrontal) +{ + SymbolicBayesTree expectedBayesTree; + SymbolicConditional::shared_ptr root = boost::make_shared( + SymbolicConditional::FromKeys(list_of(4)(5)(1), 2)); + expectedBayesTree.insertRoot(boost::make_shared(root)); + + SymbolicFactorGraph expectedFactorGraph = list_of + (SymbolicFactor(0,1)) + (SymbolicFactor(0,2)) + (SymbolicFactor(1,3)) + (SymbolicFactor(2,3)) + (SymbolicFactor(1)); + + SymbolicBayesTree::shared_ptr actualBayesTree; + SymbolicFactorGraph::shared_ptr actualFactorGraph; + boost::tie(actualBayesTree, actualFactorGraph) = + simpleTestGraph2.eliminatePartialMultifrontal(Ordering(list_of(4)(5))); + + EXPECT(assert_equal(expectedFactorGraph, *actualFactorGraph)); + EXPECT(assert_equal(expectedBayesTree, *actualBayesTree)); + + SymbolicBayesTree expectedBayesTree2; + SymbolicBayesTreeClique::shared_ptr root2 = boost::make_shared( + boost::make_shared(4,1)); + root2->children.push_back(boost::make_shared( + boost::make_shared(5,4))); + expectedBayesTree2.insertRoot(root2); + + SymbolicBayesTree::shared_ptr actualBayesTree2; + SymbolicFactorGraph::shared_ptr actualFactorGraph2; + boost::tie(actualBayesTree2, actualFactorGraph2) = + simpleTestGraph2.eliminatePartialMultifrontal(list_of(4)(5).convert_to_container >()); + + EXPECT(assert_equal(expectedFactorGraph, *actualFactorGraph2)); + EXPECT(assert_equal(expectedBayesTree2, *actualBayesTree2)); +} + +/* ************************************************************************* */ +TEST(SymbolicFactorGraph, marginalMultifrontalBayesNet) +{ + SymbolicBayesNet expectedBayesNet = list_of + (SymbolicConditional(0, 1, 2)) + (SymbolicConditional(1, 2, 3)) + (SymbolicConditional(2, 3)) + (SymbolicConditional(3)); + + SymbolicBayesNet actual1 = *simpleTestGraph2.marginalMultifrontalBayesNet( + Ordering(list_of(0)(1)(2)(3))); + EXPECT(assert_equal(expectedBayesNet, actual1)); +} + +/* ************************************************************************* */ +TEST(SymbolicFactorGraph, eliminate_disconnected_graph) { + SymbolicFactorGraph fg; + fg.push_factor(0, 1); + fg.push_factor(0, 2); + fg.push_factor(1, 2); + fg.push_factor(3, 4); + + // create expected Chordal bayes Net + SymbolicBayesNet expected; + expected.push_back(boost::make_shared(0,1,2)); + expected.push_back(boost::make_shared(1,2)); + expected.push_back(boost::make_shared(2)); + expected.push_back(boost::make_shared(3,4)); + expected.push_back(boost::make_shared(4)); + + Ordering order; + order += 0,1,2,3,4; + SymbolicBayesNet actual = *fg.eliminateSequential(order); + + EXPECT(assert_equal(expected,actual)); +} + +/* ************************************************************************* */ +//TEST(SymbolicFactorGraph, marginals) +//{ +// // Create factor graph +// SymbolicFactorGraph fg; +// fg.push_factor(0, 1); +// fg.push_factor(0, 2); +// fg.push_factor(1, 4); +// fg.push_factor(2, 4); +// fg.push_factor(3, 4); +// +// // eliminate +// SymbolicSequentialSolver solver(fg); +// SymbolicBayesNet::shared_ptr actual = solver.eliminate(); +// SymbolicBayesNet expected; +// expected.push_front(boost::make_shared(4)); +// expected.push_front(boost::make_shared(3, 4)); +// expected.push_front(boost::make_shared(2, 4)); +// expected.push_front(boost::make_shared(1, 2, 4)); +// expected.push_front(boost::make_shared(0, 1, 2)); +// EXPECT(assert_equal(expected,*actual)); +// +// { +// // jointBayesNet +// vector js; +// js.push_back(0); +// js.push_back(4); +// js.push_back(3); +// SymbolicBayesNet::shared_ptr actualBN = solver.jointBayesNet(js); +// SymbolicBayesNet expectedBN; +// expectedBN.push_front(boost::make_shared(3)); +// expectedBN.push_front(boost::make_shared(4, 3)); +// expectedBN.push_front(boost::make_shared(0, 4)); +// EXPECT( assert_equal(expectedBN,*actualBN)); +// +// // jointFactorGraph +// SymbolicFactorGraph::shared_ptr actualFG = solver.jointFactorGraph(js); +// SymbolicFactorGraph expectedFG; +// expectedFG.push_factor(0, 4); +// expectedFG.push_factor(4, 3); +// expectedFG.push_factor(3); +// EXPECT( assert_equal(expectedFG,(SymbolicFactorGraph)(*actualFG))); +// } +// +// { +// // jointBayesNet +// vector js; +// js.push_back(0); +// js.push_back(2); +// js.push_back(3); +// SymbolicBayesNet::shared_ptr actualBN = solver.jointBayesNet(js); +// SymbolicBayesNet expectedBN; +// expectedBN.push_front(boost::make_shared(2)); +// expectedBN.push_front(boost::make_shared(3, 2)); +// expectedBN.push_front(boost::make_shared(0, 3, 2)); +// EXPECT( assert_equal(expectedBN,*actualBN)); +// +// // jointFactorGraph +// SymbolicFactorGraph::shared_ptr actualFG = solver.jointFactorGraph(js); +// SymbolicFactorGraph expectedFG; +// expectedFG.push_factor(0, 3, 2); +// expectedFG.push_factor(3, 2); +// expectedFG.push_factor(2); +// EXPECT( assert_equal(expectedFG,(SymbolicFactorGraph)(*actualFG))); +// } +// +// { +// // conditionalBayesNet +// vector js; +// js.push_back(0); +// js.push_back(2); +// js.push_back(3); +// size_t nrFrontals = 2; +// SymbolicBayesNet::shared_ptr actualBN = // +// solver.conditionalBayesNet(js, nrFrontals); +// SymbolicBayesNet expectedBN; +// expectedBN.push_front(boost::make_shared(2, 3)); +// expectedBN.push_front(boost::make_shared(0, 2, 3)); +// EXPECT( assert_equal(expectedBN,*actualBN)); +// } +//} + +/* ************************************************************************* */ +TEST( SymbolicFactorGraph, constructFromBayesNet ) +{ + // create expected factor graph + SymbolicFactorGraph expected; + expected.push_factor(0, 1, 2); + expected.push_factor(1, 2); + expected.push_factor(1); + + // create Bayes Net + SymbolicBayesNet bayesNet; + bayesNet += SymbolicConditional(0, 1, 2); + bayesNet += SymbolicConditional(1, 2); + bayesNet += SymbolicConditional(1); + + // create actual factor graph from a Bayes Net + SymbolicFactorGraph actual(bayesNet); + + CHECK(assert_equal(expected, actual)); +} + +/* ************************************************************************* */ +TEST( SymbolicFactorGraph, constructFromBayesTree ) +{ + // create expected factor graph + SymbolicFactorGraph expected; + expected.push_factor(_E_, _L_, _B_); + expected.push_factor(_S_, _B_, _L_); + expected.push_factor(_T_, _E_, _L_); + expected.push_factor(_X_, _E_); + + // create actual factor graph + SymbolicFactorGraph actual(asiaBayesTree); + + CHECK(assert_equal(expected, actual)); +} + +/* ************************************************************************* */ +TEST( SymbolicFactorGraph, push_back ) +{ + // Create two factor graphs and expected combined graph + SymbolicFactorGraph fg1, fg2, expected; + + fg1.push_factor(1); + fg1.push_factor(0, 1); + + fg2.push_factor(1, 2); + fg2.push_factor(0, 2); + + expected.push_factor(1); + expected.push_factor(0, 1); + expected.push_factor(1, 2); + expected.push_factor(0, 2); + + // combine + SymbolicFactorGraph actual; + actual.push_back(fg1); + actual.push_back(fg2); + CHECK(assert_equal(expected, actual)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ diff --git a/gtsam/inference/tests/testISAM.cpp b/gtsam/symbolic/tests/testSymbolicISAM.cpp similarity index 73% rename from gtsam/inference/tests/testISAM.cpp rename to gtsam/symbolic/tests/testSymbolicISAM.cpp index 0e8f1a75d..52dfbacb8 100644 --- a/gtsam/inference/tests/testISAM.cpp +++ b/gtsam/symbolic/tests/testSymbolicISAM.cpp @@ -21,16 +21,12 @@ using namespace boost::assign; #include -#include -#include -#include -#include +#include +#include using namespace std; using namespace gtsam; -typedef ISAM SymbolicISAM; - /* ************************************************************************* */ // Some numbers that should be consistent among all smoother tests @@ -80,42 +76,27 @@ typedef ISAM SymbolicISAM; // bayesTree.insert(X); // return bayesTree; //} -// -///* ************************************************************************* */ -//TEST( ISAM, iSAM ) -//{ -// SymbolicISAM bayesTree = createAsiaSymbolicISAM(); -// -// // Now we modify the Bayes tree by inserting a new factor over B and S -// -// // New conditionals in modified top of the tree -// SymbolicConditional::shared_ptr -// S_(new SymbolicConditional("S")), -// B_(new SymbolicConditional("B", "S")), -// L_(new SymbolicConditional("L", "B", "S")); -// -// // Create expected Bayes tree -// SymbolicISAM expected; -// expected.insert(S_); -// expected.insert(B_); -// expected.insert(L_); -// expected.insert(E); -// expected.insert(T); -// expected.insert(X); -// -// // create new factors to be inserted -// SymbolicFactorGraph factorGraph; -// factorGraph.push_factor("B","S"); -// factorGraph.push_factor("B"); -// -// // do incremental inference -// bayesTree.update(factorGraph); -// -// // Check whether the same -// CHECK(assert_equal(expected,bayesTree)); -//} -// -///* ************************************************************************* */ + +/* ************************************************************************* */ +TEST( SymbolicISAM, iSAM ) +{ + // Now we modify the Bayes tree by inserting a new factor over B and S + + SymbolicFactorGraph fullGraph; + fullGraph += asiaGraph; + fullGraph += SymbolicFactor(_B_, _S_); + + // This ordering is chosen to match the one chosen by COLAMD during the ISAM update + SymbolicBayesTree expected = *fullGraph.eliminateMultifrontal(Ordering(list_of(_X_)(_B_)(_S_)(_E_)(_L_)(_T_))); + + // Add factor on B and S + SymbolicISAM actual = *asiaGraph.eliminateMultifrontal(); + + // Check whether the same + EXPECT(assert_equal(expected, (const SymbolicBayesTree&)actual)); +} + +/* ************************************************************************* */ //TEST( ISAM, iSAM_slam ) //{ // // Create using insert diff --git a/gtsam/symbolic/tests/testSymbolicJunctionTree.cpp b/gtsam/symbolic/tests/testSymbolicJunctionTree.cpp new file mode 100644 index 000000000..49b14bc07 --- /dev/null +++ b/gtsam/symbolic/tests/testSymbolicJunctionTree.cpp @@ -0,0 +1,65 @@ +/* ---------------------------------------------------------------------------- + + * 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 testJunctionTree.cpp + * @brief Unit tests for Junction Tree + * @author Kai Ni + * @author Frank Dellaert + */ + +#include +#include + +#include +#include +#include + +#include +using namespace boost::assign; + +#include "symbolicExampleGraphs.h" + +using namespace gtsam; +using namespace std; + +/* ************************************************************************* * + * 1 - 0 - 2 - 3 + * 2 3 + * 0 1 : 2 + ****************************************************************************/ +TEST( JunctionTree, constructor ) +{ + Ordering order; order += 0, 1, 2, 3; + + SymbolicJunctionTree actual(SymbolicEliminationTree(simpleChain, order)); + + SymbolicJunctionTree::Node::Keys + frontal1 = list_of(2)(3), + frontal2 = list_of(0)(1), + sep1, sep2 = list_of(2); + EXPECT(assert_container_equality(frontal1, actual.roots().front()->keys)); + //EXPECT(assert_equal(sep1, actual.roots().front()->separator)); + LONGS_EQUAL(1, (long)actual.roots().front()->factors.size()); + EXPECT(assert_container_equality(frontal2, actual.roots().front()->children.front()->keys)); + //EXPECT(assert_equal(sep2, actual.roots().front()->children.front()->separator)); + LONGS_EQUAL(2, (long)actual.roots().front()->children.front()->factors.size()); + EXPECT(assert_equal(*simpleChain[2], *actual.roots().front()->factors[0])); + EXPECT(assert_equal(*simpleChain[0], *actual.roots().front()->children.front()->factors[0])); + EXPECT(assert_equal(*simpleChain[1], *actual.roots().front()->children.front()->factors[1])); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam/inference/tests/testVariableIndex.cpp b/gtsam/symbolic/tests/testVariableIndex.cpp similarity index 72% rename from gtsam/inference/tests/testVariableIndex.cpp rename to gtsam/symbolic/tests/testVariableIndex.cpp index bc2af6565..56ce1f9a5 100644 --- a/gtsam/inference/tests/testVariableIndex.cpp +++ b/gtsam/symbolic/tests/testVariableIndex.cpp @@ -16,11 +16,15 @@ * @date Sep 26, 2010 */ +#include +#include +using namespace boost::assign; + #include #include #include -#include +#include using namespace std; using namespace gtsam; @@ -44,8 +48,37 @@ TEST(VariableIndex, augment) { VariableIndex actual(fg1); actual.augment(fg2); - LONGS_EQUAL(16, actual.nEntries()); - LONGS_EQUAL(8, actual.nFactors()); + LONGS_EQUAL(16, (long)actual.nEntries()); + LONGS_EQUAL(8, (long)actual.nFactors()); + EXPECT(assert_equal(expected, actual)); +} + +/* ************************************************************************* */ +TEST(VariableIndex, augment2) { + + SymbolicFactorGraph fg1, fg2; + fg1.push_factor(0, 1); + fg1.push_factor(0, 2); + fg1.push_factor(5, 9); + fg1.push_factor(2, 3); + fg2.push_factor(1, 3); + fg2.push_factor(2, 4); + fg2.push_factor(3, 5); + fg2.push_factor(5, 6); + + SymbolicFactorGraph fgCombined; + fgCombined.push_back(fg1); + fgCombined.push_back(SymbolicFactor::shared_ptr()); // Add an extra empty factor + fgCombined.push_back(fg2); + + VariableIndex expected(fgCombined); + + FastVector newIndices = list_of(5)(6)(7)(8); + VariableIndex actual(fg1); + actual.augment(fg2, newIndices); + + LONGS_EQUAL(16, (long) actual.nEntries()); + LONGS_EQUAL(9, (long) actual.nFactors()); EXPECT(assert_equal(expected, actual)); } @@ -71,11 +104,13 @@ TEST(VariableIndex, remove) { // The expected VariableIndex has the same factor indices as fgCombined but // with entries from fg1 removed, and still has all 10 variables. - VariableIndex expected(fg2removed, 10); + VariableIndex expected(fg2removed); VariableIndex actual(fgCombined); vector indices; indices.push_back(0); indices.push_back(1); indices.push_back(2); indices.push_back(3); - actual.remove(indices, fg1); + actual.remove(indices.begin(), indices.end(), fg1); + std::list unusedVariables; unusedVariables += 0, 9; + actual.removeUnusedVariables(unusedVariables.begin(), unusedVariables.end()); CHECK(assert_equal(expected, actual)); } @@ -108,7 +143,9 @@ TEST(VariableIndex, deep_copy) { VariableIndex clone(original); vector indices; indices.push_back(0); indices.push_back(1); indices.push_back(2); indices.push_back(3); - clone.remove(indices, fg1); + clone.remove(indices.begin(), indices.end(), fg1); + std::list unusedVariables; unusedVariables += 0, 9; + clone.removeUnusedVariables(unusedVariables.begin(), unusedVariables.end()); // When modifying the clone, the original should have stayed the same EXPECT(assert_equal(expectedOriginal, original)); diff --git a/gtsam_extra.cmake.in b/gtsam_extra.cmake.in new file mode 100644 index 000000000..89a97d51b --- /dev/null +++ b/gtsam_extra.cmake.in @@ -0,0 +1,18 @@ +# Extra CMake definitions for GTSAM + +set (GTSAM_VERSION_MAJOR @GTSAM_VERSION_MAJOR@) +set (GTSAM_VERSION_MINOR @GTSAM_VERSION_MINOR@) +set (GTSAM_VERSION_PATCH @GTSAM_VERSION_PATCH@) +set (GTSAM_VERSION_NUMERIC @GTSAM_VERSION_NUMERIC@) +set (GTSAM_VERSION_STRING "@GTSAM_VERSION_STRING@") + +set (GTSAM_USE_TBB @GTSAM_USE_TBB@) +set (GTSAM_DEFAULT_ALLOCATOR @GTSAM_DEFAULT_ALLOCATOR@) + +if("@GTSAM_USE_TBB@") + list(APPEND GTSAM_INCLUDE_DIR "@TBB_INCLUDE_DIRS@") +endif() + +if("@GTSAM_USE_EIGEN_MKL@") + list(APPEND GTSAM_INCLUDE_DIR "@MKL_INCLUDE_DIR@") +endif() diff --git a/gtsam_unstable/CMakeLists.txt b/gtsam_unstable/CMakeLists.txt index f59208ef6..a33a4548c 100644 --- a/gtsam_unstable/CMakeLists.txt +++ b/gtsam_unstable/CMakeLists.txt @@ -1,15 +1,18 @@ # Build full gtsam_unstable library as a single library -# and also build tests +# and also build tests set (gtsam_unstable_subdirs - base + base geometry discrete - linear dynamics nonlinear slam ) +if(GTSAM_BUILD_METIS) # Only build partition if metis is built + set (gtsam_unstable_subdirs ${gtsam_unstable_subdirs} partition) +endif(GTSAM_BUILD_METIS) + set(GTSAM_UNSTABLE_BOOST_LIBRARIES ${GTSAM_BOOST_LIBRARIES}) add_custom_target(check.unstable COMMAND ${CMAKE_CTEST_COMMAND} --output-on-failure) @@ -34,70 +37,67 @@ foreach(subdir ${gtsam_unstable_subdirs}) list(REMOVE_ITEM subdir_headers ${excluded_headers}) set(${subdir}_srcs ${subdir_srcs} ${subdir_headers}) gtsam_assign_source_folders("${${subdir}_srcs}") # Create MSVC structure - if (subdir_srcs AND GTSAM_BUILD_CONVENIENCE_LIBRARIES) - message(STATUS "Building Convenience Library: ${subdir}_unstable") - add_library("${subdir}_unstable" STATIC ${${subdir}_srcs}) - endif() # Build local library and tests - message(STATUS "Building ${subdir}_unstable") + message(STATUS "Building ${subdir}_unstable") add_subdirectory(${subdir}) endforeach(subdir) - + # assemble gtsam_unstable components set(gtsam_unstable_srcs - ${base_srcs} + ${base_srcs} ${geometry_srcs} ${discrete_srcs} ${dynamics_srcs} - ${linear_srcs} ${nonlinear_srcs} ${slam_srcs} ) +if(GTSAM_BUILD_METIS) # Only build partition if metis is built + set (gtsam_unstable_srcs ${gtsam_unstable_srcs} ${partition_srcs}) +endif(GTSAM_BUILD_METIS) + # Versions - same as core gtsam library set(gtsam_unstable_version ${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}) set(gtsam_unstable_soversion ${GTSAM_VERSION_MAJOR}) message(STATUS "GTSAM_UNSTABLE Version: ${gtsam_unstable_version}") message(STATUS "Install prefix: ${CMAKE_INSTALL_PREFIX}") - + # build shared and static versions of the library if (GTSAM_BUILD_STATIC_LIBRARY) message(STATUS "Building GTSAM_UNSTABLE - static") - add_library(gtsam_unstable-static STATIC ${gtsam_unstable_srcs}) - set_target_properties(gtsam_unstable-static PROPERTIES + add_library(gtsam_unstable STATIC ${gtsam_unstable_srcs}) + set_target_properties(gtsam_unstable PROPERTIES OUTPUT_NAME gtsam_unstable CLEAN_DIRECT_OUTPUT 1 VERSION ${gtsam_unstable_version} SOVERSION ${gtsam_unstable_soversion}) if(WIN32) # Add 'lib' prefix to static library to avoid filename collision with shared library - set_target_properties(gtsam_unstable-static PROPERTIES + set_target_properties(gtsam_unstable PROPERTIES PREFIX "lib" COMPILE_DEFINITIONS GTSAM_UNSTABLE_IMPORT_STATIC) endif() - target_link_libraries(gtsam_unstable-static gtsam-static ${GTSAM_UNSTABLE_BOOST_LIBRARIES}) - install(TARGETS gtsam_unstable-static EXPORT GTSAM-exports ARCHIVE DESTINATION lib) - list(APPEND GTSAM_EXPORTED_TARGETS gtsam_unstable-static) + target_link_libraries(gtsam_unstable gtsam ${GTSAM_UNSTABLE_BOOST_LIBRARIES}) + install(TARGETS gtsam_unstable EXPORT GTSAM-exports ARCHIVE DESTINATION lib) + list(APPEND GTSAM_EXPORTED_TARGETS gtsam_unstable) set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE) -endif() - -if (GTSAM_BUILD_SHARED_LIBRARY) +else() message(STATUS "Building GTSAM_UNSTABLE - shared") - add_library(gtsam_unstable-shared SHARED ${gtsam_unstable_srcs}) - set_target_properties(gtsam_unstable-shared PROPERTIES + add_library(gtsam_unstable SHARED ${gtsam_unstable_srcs}) + set_target_properties(gtsam_unstable PROPERTIES OUTPUT_NAME gtsam_unstable CLEAN_DIRECT_OUTPUT 1 VERSION ${gtsam_unstable_version} SOVERSION ${gtsam_unstable_soversion}) if(WIN32) - set_target_properties(gtsam_unstable-shared PROPERTIES + set_target_properties(gtsam_unstable PROPERTIES PREFIX "" DEFINE_SYMBOL GTSAM_UNSTABLE_EXPORTS RUNTIME_OUTPUT_DIRECTORY "${PROJECT_BINARY_DIR}/bin") endif() - target_link_libraries(gtsam_unstable-shared gtsam-shared ${GTSAM_UNSTABLE_BOOST_LIBRARIES}) - install(TARGETS gtsam_unstable-shared EXPORT GTSAM-exports LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin) - list(APPEND GTSAM_EXPORTED_TARGETS gtsam_unstable-shared) + target_link_libraries(gtsam_unstable gtsam ${GTSAM_UNSTABLE_BOOST_LIBRARIES}) + install(TARGETS gtsam_unstable EXPORT GTSAM-exports LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin) + list(APPEND GTSAM_EXPORTED_TARGETS gtsam_unstable) set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE) endif() @@ -105,23 +105,15 @@ endif() if (GTSAM_INSTALL_MATLAB_TOOLBOX) # Set up codegen include(GtsamMatlabWrap) - - # TODO: generate these includes programmatically - # Choose include flags depending on build process - set(MEX_INCLUDE_ROOT ${GTSAM_SOURCE_ROOT_DIR}) - set(MEX_LIB_ROOT ${CMAKE_BINARY_DIR}) # FIXME: is this used? - set(GTSAM_LIB_DIR ${MEX_LIB_ROOT}/gtsam) # FIXME: is this used? - set(GTSAM_UNSTABLE_LIB_DIR ${MEX_LIB_ROOT}/gtsam_unstable) # FIXME: is this used? - + # Generate, build and install toolbox - set(mexFlags -I${MEX_INCLUDE_ROOT} -I${Boost_INCLUDE_DIR} -I${CMAKE_BINARY_DIR}) - if("${gtsam-default}" STREQUAL "gtsam-static") + set(mexFlags "${GTSAM_BUILD_MEX_BINARY_FLAGS}") + if(GTSAM_BUILD_STATIC_LIBRARY) list(APPEND mexFlags -DGTSAM_IMPORT_STATIC) endif() - - # Macro to handle details of setting up targets - wrap_library(gtsam_unstable "${mexFlags}" "./" gtsam) + # Wrap + wrap_and_install_library(gtsam_unstable.h "gtsam" "" "${mexFlags}") endif(GTSAM_INSTALL_MATLAB_TOOLBOX) diff --git a/gtsam_unstable/base/BTree.h b/gtsam_unstable/base/BTree.h index fc8843f7a..baa6540e7 100644 --- a/gtsam_unstable/base/BTree.h +++ b/gtsam_unstable/base/BTree.h @@ -128,6 +128,12 @@ namespace gtsam { root_(new Node(l, keyValue, r)) { } + /** assignment operator */ + BTree & operator= (const BTree & other) { + root_ = other.root_; + return *this; + } + /** Check whether tree is empty */ bool empty() const { return !root_; diff --git a/gtsam_unstable/base/CMakeLists.txt b/gtsam_unstable/base/CMakeLists.txt index 420402015..d83e97d26 100644 --- a/gtsam_unstable/base/CMakeLists.txt +++ b/gtsam_unstable/base/CMakeLists.txt @@ -2,18 +2,10 @@ file(GLOB base_headers "*.h") install(FILES ${base_headers} DESTINATION include/gtsam_unstable/base) -# Components to link tests in this subfolder against -set(base_local_libs - base - base_unstable) - -set (base_full_libs - ${gtsam-default} - ${gtsam_unstable-default}) - -# Exclude tests that don't work -set (base_excluded_tests "") - # Add all tests -gtsam_add_subdir_tests(base_unstable "${base_local_libs}" "${base_full_libs}" "${base_excluded_tests}") -add_dependencies(check.unstable check.base_unstable) +add_subdirectory(tests) + +# Build timing scripts +if (GTSAM_BUILD_TIMING) + gtsam_add_subdir_timing(base_unstable "${base_full_libs}" "${base_full_libs}" "${base_excluded_files}") +endif(GTSAM_BUILD_TIMING) diff --git a/gtsam_unstable/base/DSF.h b/gtsam_unstable/base/DSF.h index 74455c6a9..c51d6003c 100644 --- a/gtsam_unstable/base/DSF.h +++ b/gtsam_unstable/base/DSF.h @@ -18,157 +18,196 @@ #pragma once +#include +#include #include #include #include #include -#include -#include namespace gtsam { +/** + * Disjoint Set Forest class + * + * Quoting from CLR: A disjoint-set data structure maintains a collection + * S = {S_1,S_2,...} of disjoint dynamic sets. Each set is identified by + * a representative, which is some member of the set. + * + * @addtogroup base + */ +template +class DSF: protected BTree { + +public: + typedef DSF Self; + typedef std::set Set; + typedef BTree Tree; + typedef std::pair KeyLabel; + + // constructor + DSF() : + Tree() { + } + + // constructor + DSF(const Tree& tree) : + Tree(tree) { + } + + // constructor with a list of unconnected keys + DSF(const std::list& keys) : + Tree() { + BOOST_FOREACH(const KEY& key, keys) + *this = this->add(key, key); + } + + // constructor with a set of unconnected keys + DSF(const std::set& keys) : + Tree() { + BOOST_FOREACH(const KEY& key, keys) + *this = this->add(key, key); + } + + // create a new singleton, does nothing if already exists + Self makeSet(const KEY& key) const { + if (this->mem(key)) + return *this; + else + return this->add(key, key); + } + + // create a new singleton, does nothing if already exists + void makeSetInPlace(const KEY& key) { + if (!this->mem(key)) + *this = this->add(key, key); + } + + // find the label of the set in which {key} lives + KEY findSet(const KEY& key) const { + KEY parent = this->find(key); + return parent == key ? key : findSet(parent); + } + + // return a new DSF where x and y are in the same set. No path compression + Self makeUnion(const KEY& key1, const KEY& key2) const { + DSF copy = *this; + copy.makeUnionInPlace(key1,key2); + return copy; + } + + // the in-place version of makeUnion + void makeUnionInPlace(const KEY& key1, const KEY& key2) { + *this = this->add(findSet_(key2), findSet_(key1)); + } + + // create a new singleton with two connected keys + Self makePair(const KEY& key1, const KEY& key2) const { + return makeSet(key1).makeSet(key2).makeUnion(key1, key2); + } + + // create a new singleton with a list of fully connected keys + Self makeList(const std::list& keys) const { + Self t = *this; + BOOST_FOREACH(const KEY& key, keys) + t = t.makePair(key, keys.front()); + return t; + } + + // return a dsf in which all find_set operations will be O(1) due to path compression. + DSF flatten() const { + DSF t = *this; + BOOST_FOREACH(const KeyLabel& pair, (Tree)t) + t.findSet_(pair.first); + return t; + } + + // maps f over all keys, must be invertible + DSF map(boost::function func) const { + DSF t; + BOOST_FOREACH(const KeyLabel& pair, (Tree)*this) + t = t.add(func(pair.first), func(pair.second)); + return t; + } + + // return the number of sets + size_t numSets() const { + size_t num = 0; + BOOST_FOREACH(const KeyLabel& pair, (Tree)*this) + if (pair.first == pair.second) + num++; + return num; + } + + // return the numer of keys + size_t size() const { + return Tree::size(); + } + + // return all sets, i.e. a partition of all elements + std::map sets() const { + std::map sets; + BOOST_FOREACH(const KeyLabel& pair, (Tree)*this) + sets[findSet(pair.second)].insert(pair.first); + return sets; + } + + // return a partition of the given elements {keys} + std::map partition(const std::list& keys) const { + std::map partitions; + BOOST_FOREACH(const KEY& key, keys) + partitions[findSet(key)].insert(key); + return partitions; + } + + // get the nodes in the tree with the given label + Set set(const KEY& label) const { + Set set; + BOOST_FOREACH(const KeyLabel& pair, (Tree)*this) { + if (pair.second == label || findSet(pair.second) == label) + set.insert(pair.first); + } + return set; + } + + /** equality */ + bool operator==(const Self& t) const { + return (Tree) *this == (Tree) t; + } + + /** inequality */ + bool operator!=(const Self& t) const { + return (Tree) *this != (Tree) t; + } + + // print the object + void print(const std::string& name = "DSF") const { + std::cout << name << std::endl; + BOOST_FOREACH(const KeyLabel& pair, (Tree)*this) + std::cout << (std::string) pair.first << " " << (std::string) pair.second + << std::endl; + } + +protected: + /** - * Disjoint Set Forest class - * - * Quoting from CLR: A disjoint-set data structure maintains a collection - * S = {S_1,S_2,...} of disjoint dynamic sets. Each set is identified by - * a representative, which is some member of the set. - * - * @addtogroup base + * same as findSet except with path compression: After we have traversed the path to + * the root, each parent pointer is made to directly point to it */ - template - class DSF : protected BTree { - - public: - typedef KEY Label; // label can be different from key, but for now they are same - typedef DSF Self; - typedef std::set Set; - typedef BTree Tree; - typedef std::pair KeyLabel; - - // constructor - DSF() : Tree() { } - - // constructor - DSF(const Tree& tree) : Tree(tree) {} - - // constructor with a list of unconnected keys - DSF(const std::list& keys) : Tree() { BOOST_FOREACH(const KEY& key, keys) *this = this->add(key, key); } - - // constructor with a set of unconnected keys - DSF(const std::set& keys) : Tree() { BOOST_FOREACH(const KEY& key, keys) *this = this->add(key, key); } - - // create a new singleton, does nothing if already exists - Self makeSet(const KEY& key) const { if (this->mem(key)) return *this; else return this->add(key, key); } - - // find the label of the set in which {key} lives - Label findSet(const KEY& key) const { - KEY parent = this->find(key); - return parent == key ? key : findSet(parent); } - - // return a new DSF where x and y are in the same set. Kai: the caml implementation is not const, and I followed - Self makeUnion(const KEY& key1, const KEY& key2) { return this->add(findSet_(key2), findSet_(key1)); } - - // the in-place version of makeUnion - void makeUnionInPlace(const KEY& key1, const KEY& key2) { *this = this->add(findSet_(key2), findSet_(key1)); } - - // create a new singleton with two connected keys - Self makePair(const KEY& key1, const KEY& key2) const { return makeSet(key1).makeSet(key2).makeUnion(key1, key2); } - - // create a new singleton with a list of fully connected keys - Self makeList(const std::list& keys) const { - Self t = *this; - BOOST_FOREACH(const KEY& key, keys) - t = t.makePair(key, keys.front()); - return t; + KEY findSet_(const KEY& key) { + KEY parent = this->find(key); + if (parent == key) + return parent; + else { + KEY label = findSet_(parent); + *this = this->add(key, label); + return label; } + } - // return a dsf in which all find_set operations will be O(1) due to path compression. - DSF flatten() const { - DSF t = *this; - BOOST_FOREACH(const KeyLabel& pair, (Tree)t) - t.findSet_(pair.first); - return t; - } +}; - // maps f over all keys, must be invertible - DSF map(boost::function func) const { - DSF t; - BOOST_FOREACH(const KeyLabel& pair, (Tree)*this) - t = t.add(func(pair.first), func(pair.second)); - return t; - } - - // return the number of sets - size_t numSets() const { - size_t num = 0; - BOOST_FOREACH(const KeyLabel& pair, (Tree)*this) - if (pair.first == pair.second) num++; - return num; - } - - // return the numer of keys - size_t size() const { return Tree::size(); } - - // return all sets, i.e. a partition of all elements - std::map sets() const { - std::map sets; - BOOST_FOREACH(const KeyLabel& pair, (Tree)*this) - sets[findSet(pair.second)].insert(pair.first); - return sets; - } - - // return a partition of the given elements {keys} - std::map partition(const std::list& keys) const { - std::map partitions; - BOOST_FOREACH(const KEY& key, keys) - partitions[findSet(key)].insert(key); - return partitions; - } - - // get the nodes in the tree with the given label - Set set(const Label& label) const { - Set set; - BOOST_FOREACH(const KeyLabel& pair, (Tree)*this) { - if (pair.second == label || findSet(pair.second) == label) - set.insert(pair.first); - } - return set; - } - - /** equality */ - bool operator==(const Self& t) const { return (Tree)*this == (Tree)t; } - - /** inequality */ - bool operator!=(const Self& t) const { return (Tree)*this != (Tree)t; } - - // print the object - void print(const std::string& name = "DSF") const { - std::cout << name << std::endl; - BOOST_FOREACH(const KeyLabel& pair, (Tree)*this) - std::cout << (std::string)pair.first << " " << (std::string)pair.second << std::endl; - } - - protected: - - /** - * same as findSet except with path compression: After we have traversed the path to - * the root, each parent pointer is made to directly point to it - */ - KEY findSet_(const KEY& key) { - KEY parent = this->find(key); - if (parent == key) - return parent; - else { - KEY label = findSet_(parent); - *this = this->add(key, label); - return label; - } - } - - }; - - // shortcuts - typedef DSF DSFInt; +// shortcuts +typedef DSF DSFInt; } // namespace gtsam diff --git a/gtsam_unstable/base/DSFMap.h b/gtsam_unstable/base/DSFMap.h new file mode 100644 index 000000000..6832c7f83 --- /dev/null +++ b/gtsam_unstable/base/DSFMap.h @@ -0,0 +1,123 @@ +/* ---------------------------------------------------------------------------- + + * 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 DSFMap.h + * @date Oct 26, 2013 + * @author Frank Dellaert + * @brief Allow for arbitrary type in DSF + */ + +#pragma once + +#include +#include +#include // Provides size_t + +namespace gtsam { + +/** + * Disjoint set forest using an STL map data structure underneath + * Uses rank compression and union by rank, iterator version + * @addtogroup base + */ +template +class DSFMap { + +protected: + + /// We store the forest in an STL map, but parents are done with pointers + struct Entry { + typename std::map::iterator parent_; + size_t rank_; + Entry() {} + }; + + typedef typename std::map Map; + typedef typename Map::iterator iterator; + mutable Map entries_; + + /// Given key, find iterator to initial entry + iterator find__(const KEY& key) const { + static const Entry empty; + iterator it = entries_.find(key); + // if key does not exist, create and return itself + if (it == entries_.end()) { + it = entries_.insert(std::make_pair(key, empty)).first; + it->second.parent_ = it; + it->second.rank_ = 0; + } + return it; + } + + /// Given iterator to initial entry, find the root Entry + iterator find_(const iterator& it) const { + // follow parent pointers until we reach set representative + iterator& parent = it->second.parent_; + if (parent != it) + parent = find_(parent); // not yet, recurse! + return parent; + } + + /// Given key, find the root Entry + inline iterator find_(const KEY& key) const { + iterator initial = find__(key); + return find_(initial); + } + +public: + + typedef std::set Set; + + /// constructor + DSFMap() { + } + + /// Given key, find the representative key for the set in which it lives + inline KEY find(const KEY& key) const { + iterator root = find_(key); + return root->first; + } + + /// Merge two sets + void merge(const KEY& x, const KEY& y) { + + // straight from http://en.wikipedia.org/wiki/Disjoint-set_data_structure + iterator xRoot = find_(x); + iterator yRoot = find_(y); + if (xRoot == yRoot) + return; + + // Merge sets + if (xRoot->second.rank_ < yRoot->second.rank_) + xRoot->second.parent_ = yRoot; + else if (xRoot->second.rank_ > yRoot->second.rank_) + yRoot->second.parent_ = xRoot; + else { + yRoot->second.parent_ = xRoot; + xRoot->second.rank_ = xRoot->second.rank_ + 1; + } + } + + /// return all sets, i.e. a partition of all elements + std::map sets() const { + std::map sets; + iterator it = entries_.begin(); + for (; it != entries_.end(); it++) { + iterator root = find_(it); + sets[root->first].insert(it->first); + } + return sets; + } + +}; + +} diff --git a/gtsam_unstable/base/FixedVector.h b/gtsam_unstable/base/FixedVector.h index 53a9190ce..dd3668087 100644 --- a/gtsam_unstable/base/FixedVector.h +++ b/gtsam_unstable/base/FixedVector.h @@ -45,23 +45,6 @@ public: std::copy(values, values+N, this->data()); } - /** - * nice constructor, dangerous as number of arguments must be exactly right - * and you have to pass doubles !!! always use 0.0 never 0 - * - * NOTE: this will throw warnings/explode if there is no argument - * before the variadic section, so there is a meaningless size argument. - */ - FixedVector(size_t n, ...) { - va_list ap; - va_start(ap, n); - for(size_t i = 0 ; i < N ; i++) { - double value = va_arg(ap, double); - (*this)(i) = value; - } - va_end(ap); - } - /** * Create vector initialized to a constant value * @param value constant value diff --git a/gtsam_unstable/base/tests/CMakeLists.txt b/gtsam_unstable/base/tests/CMakeLists.txt new file mode 100644 index 000000000..d103ec578 --- /dev/null +++ b/gtsam_unstable/base/tests/CMakeLists.txt @@ -0,0 +1 @@ +gtsamAddTestsGlob(base_unstable "test*.cpp" "" "gtsam_unstable") diff --git a/gtsam_unstable/base/tests/testDSF.cpp b/gtsam_unstable/base/tests/testDSF.cpp index a0c4ab714..c8828746b 100644 --- a/gtsam_unstable/base/tests/testDSF.cpp +++ b/gtsam_unstable/base/tests/testDSF.cpp @@ -16,13 +16,15 @@ * @brief unit tests for DSF */ -#include +#include + +#include + #include #include using namespace boost::assign; -#include -#include +#include using namespace std; using namespace gtsam; @@ -40,7 +42,7 @@ TEST(DSF, findSet) { dsf = dsf.makeSet(5); dsf = dsf.makeSet(6); dsf = dsf.makeSet(7); - CHECK(dsf.findSet(5) != dsf.findSet(7)); + EXPECT(dsf.findSet(5) != dsf.findSet(7)); } /* ************************************************************************* */ @@ -50,7 +52,7 @@ TEST(DSF, makeUnion) { dsf = dsf.makeSet(6); dsf = dsf.makeSet(7); dsf = dsf.makeUnion(5,7); - CHECK(dsf.findSet(5) == dsf.findSet(7)); + EXPECT(dsf.findSet(5) == dsf.findSet(7)); } /* ************************************************************************* */ @@ -60,7 +62,7 @@ TEST(DSF, makeUnion2) { dsf = dsf.makeSet(6); dsf = dsf.makeSet(7); dsf = dsf.makeUnion(7,5); - CHECK(dsf.findSet(5) == dsf.findSet(7)); + EXPECT(dsf.findSet(5) == dsf.findSet(7)); } /* ************************************************************************* */ @@ -71,7 +73,7 @@ TEST(DSF, makeUnion3) { dsf = dsf.makeSet(7); dsf = dsf.makeUnion(5,6); dsf = dsf.makeUnion(6,7); - CHECK(dsf.findSet(5) == dsf.findSet(7)); + EXPECT(dsf.findSet(5) == dsf.findSet(7)); } /* ************************************************************************* */ @@ -80,7 +82,7 @@ TEST(DSF, makePair) { dsf = dsf.makePair(0, 1); dsf = dsf.makePair(1, 2); dsf = dsf.makePair(3, 2); - CHECK(dsf.findSet(0) == dsf.findSet(3)); + EXPECT(dsf.findSet(0) == dsf.findSet(3)); } /* ************************************************************************* */ @@ -88,7 +90,7 @@ TEST(DSF, makeList) { DSFInt dsf; list keys; keys += 5, 6, 7; dsf = dsf.makeList(keys); - CHECK(dsf.findSet(5) == dsf.findSet(7)); + EXPECT(dsf.findSet(5) == dsf.findSet(7)); } /* ************************************************************************* */ @@ -111,7 +113,7 @@ TEST(DSF, sets) { LONGS_EQUAL(1, sets.size()); set expected; expected += 5, 6; - CHECK(expected == sets[dsf.findSet(5)]); + EXPECT(expected == sets[dsf.findSet(5)]); } /* ************************************************************************* */ @@ -126,7 +128,7 @@ TEST(DSF, sets2) { LONGS_EQUAL(1, sets.size()); set expected; expected += 5, 6, 7; - CHECK(expected == sets[dsf.findSet(5)]); + EXPECT(expected == sets[dsf.findSet(5)]); } /* ************************************************************************* */ @@ -140,7 +142,7 @@ TEST(DSF, sets3) { LONGS_EQUAL(2, sets.size()); set expected; expected += 5, 6; - CHECK(expected == sets[dsf.findSet(5)]); + EXPECT(expected == sets[dsf.findSet(5)]); } /* ************************************************************************* */ @@ -155,7 +157,7 @@ TEST(DSF, partition) { LONGS_EQUAL(1, partitions.size()); set expected; expected += 5; - CHECK(expected == partitions[dsf.findSet(5)]); + EXPECT(expected == partitions[dsf.findSet(5)]); } /* ************************************************************************* */ @@ -171,7 +173,7 @@ TEST(DSF, partition2) { LONGS_EQUAL(1, partitions.size()); set expected; expected += 7; - CHECK(expected == partitions[dsf.findSet(7)]); + EXPECT(expected == partitions[dsf.findSet(7)]); } /* ************************************************************************* */ @@ -187,7 +189,7 @@ TEST(DSF, partition3) { LONGS_EQUAL(2, partitions.size()); set expected; expected += 5; - CHECK(expected == partitions[dsf.findSet(5)]); + EXPECT(expected == partitions[dsf.findSet(5)]); } /* ************************************************************************* */ @@ -201,7 +203,7 @@ TEST(DSF, set) { LONGS_EQUAL(2, set.size()); std::set expected; expected += 5, 6; - CHECK(expected == set); + EXPECT(expected == set); } /* ************************************************************************* */ @@ -216,7 +218,7 @@ TEST(DSF, set2) { LONGS_EQUAL(3, set.size()); std::set expected; expected += 5, 6, 7; - CHECK(expected == set); + EXPECT(expected == set); } /* ************************************************************************* */ @@ -234,7 +236,7 @@ TEST(DSF, map) { expected = expected.makeSet(16); expected = expected.makeSet(17); expected = expected.makeUnion(15,16); - CHECK(actual == expected); + EXPECT(actual == expected); } /* ************************************************************************* */ @@ -253,7 +255,7 @@ TEST(DSF, flatten) { expected = expected.makePair(1, 5); expected = expected.makePair(1, 6); expected = expected.makePair(1, 7); - CHECK(actual == expected); + EXPECT(actual == expected); } /* ************************************************************************* */ @@ -265,13 +267,60 @@ TEST(DSF, flatten2) { dsf = dsf.makeUnion(x3,x4); dsf = dsf.makeUnion(x1,x3); - CHECK(dsf != dsf.flatten()); + EXPECT(dsf != dsf.flatten()); DSF expected2; expected2 = expected2.makePair(x1, x2); expected2 = expected2.makePair(x1, x3); expected2 = expected2.makePair(x1, x4); - CHECK(expected2 == dsf.flatten()); + EXPECT(expected2 == dsf.flatten()); +} + +/* ************************************************************************* */ +TEST(DSF, mergePairwiseMatches) { + + // Create some measurements with image index and feature index + typedef pair Measurement; + Measurement m11(1,1),m12(1,2),m14(1,4); // in image 1 + Measurement m22(2,2),m23(2,3),m25(2,5),m26(2,6); // in image 2 + + // Add them all + list measurements; + measurements += m11,m12,m14, m22,m23,m25,m26; + + // Create some "matches" + typedef pair Match; + list matches; + matches += Match(m11,m22), Match(m12,m23), Match(m14,m25), Match(m14,m26); + + // Merge matches + DSF dsf(measurements); + BOOST_FOREACH(const Match& m, matches) + dsf.makeUnionInPlace(m.first,m.second); + + // Check that sets are merged correctly + EXPECT(dsf.findSet(m11)==m11); + EXPECT(dsf.findSet(m12)==m12); + EXPECT(dsf.findSet(m14)==m14); + EXPECT(dsf.findSet(m22)==m11); + EXPECT(dsf.findSet(m23)==m12); + EXPECT(dsf.findSet(m25)==m14); + EXPECT(dsf.findSet(m26)==m14); + + // Check that we have three connected components + EXPECT_LONGS_EQUAL(3, dsf.numSets()); + + set expected1; expected1 += m11,m22; + set actual1 = dsf.set(m11); + EXPECT(expected1 == actual1); + + set expected2; expected2 += m12,m23; + set actual2 = dsf.set(m12); + EXPECT(expected2 == actual2); + + set expected3; expected3 += m14,m25,m26; + set actual3 = dsf.set(m14); + EXPECT(expected3 == actual3); } /* ************************************************************************* */ diff --git a/gtsam_unstable/base/tests/testDSFMap.cpp b/gtsam_unstable/base/tests/testDSFMap.cpp new file mode 100644 index 000000000..69723d99b --- /dev/null +++ b/gtsam_unstable/base/tests/testDSFMap.cpp @@ -0,0 +1,147 @@ +/* ---------------------------------------------------------------------------- + + * 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 testDSFMap.cpp + * @date Oct 26, 2013 + * @author Frank Dellaert + * @brief unit tests for DSFMap + */ + +#include + +#include +#include +#include +using namespace boost::assign; + +#include + +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +TEST(DSFMap, find) { + DSFMap dsf; + EXPECT(dsf.find(0)==0); + EXPECT(dsf.find(2)==2); + EXPECT(dsf.find(0)==0); + EXPECT(dsf.find(2)==2); + EXPECT(dsf.find(0) != dsf.find(2)); +} + +/* ************************************************************************* */ +TEST(DSFMap, merge) { + DSFMap dsf; + dsf.merge(0,2); + EXPECT(dsf.find(0) == dsf.find(2)); +} + +/* ************************************************************************* */ +TEST(DSFMap, merge2) { + DSFMap dsf; + dsf.merge(2,0); + EXPECT(dsf.find(0) == dsf.find(2)); +} + +/* ************************************************************************* */ +TEST(DSFMap, merge3) { + DSFMap dsf; + dsf.merge(0,1); + dsf.merge(1,2); + EXPECT(dsf.find(0) == dsf.find(2)); +} + +/* ************************************************************************* */ +TEST(DSFMap, mergePairwiseMatches) { + + // Create some "matches" + typedef pair Match; + list matches; + matches += Match(1,2), Match(2,3), Match(4,5), Match(4,6); + + // Merge matches + DSFMap dsf; + BOOST_FOREACH(const Match& m, matches) + dsf.merge(m.first,m.second); + + // Each point is now associated with a set, represented by one of its members + size_t rep1 = dsf.find(1), rep2 = dsf.find(4); + EXPECT_LONGS_EQUAL(rep1,dsf.find(2)); + EXPECT_LONGS_EQUAL(rep1,dsf.find(3)); + EXPECT_LONGS_EQUAL(rep2,dsf.find(5)); + EXPECT_LONGS_EQUAL(rep2,dsf.find(6)); +} + +/* ************************************************************************* */ +TEST(DSFMap, mergePairwiseMatches2) { + + // Create some measurements with image index and feature index + typedef pair Measurement; + Measurement m11(1,1),m12(1,2),m14(1,4); // in image 1 + Measurement m22(2,2),m23(2,3),m25(2,5),m26(2,6); // in image 2 + + // Add them all + list measurements; + measurements += m11,m12,m14, m22,m23,m25,m26; + + // Create some "matches" + typedef pair Match; + list matches; + matches += Match(m11,m22), Match(m12,m23), Match(m14,m25), Match(m14,m26); + + // Merge matches + DSFMap dsf; + BOOST_FOREACH(const Match& m, matches) + dsf.merge(m.first,m.second); + + // Check that sets are merged correctly + EXPECT(dsf.find(m22)==dsf.find(m11)); + EXPECT(dsf.find(m23)==dsf.find(m12)); + EXPECT(dsf.find(m25)==dsf.find(m14)); + EXPECT(dsf.find(m26)==dsf.find(m14)); +} + +/* ************************************************************************* */ +TEST(DSFMap, sets){ + // Create some "matches" + typedef pair Match; + typedef pair > key_pair; + list matches; + matches += Match(1,2), Match(2,3), Match(4,5), Match(4,6); + + // Merge matches + DSFMap dsf; + BOOST_FOREACH(const Match& m, matches) + dsf.merge(m.first,m.second); + + map > sets = dsf.sets(); + set s1, s2; + s1 += 1,2,3; + s2 += 4,5,6; + + /*BOOST_FOREACH(key_pair st, sets){ + cout << "Set " << st.first << " :{"; + BOOST_FOREACH(const size_t s, st.second) + cout << s << ", "; + cout << "}" << endl; + }*/ + + EXPECT(s1 == sets[1]); + EXPECT(s2 == sets[4]); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ + diff --git a/gtsam_unstable/base/tests/testFixedVector.cpp b/gtsam_unstable/base/tests/testFixedVector.cpp index 1fddf80ca..e5e297b7c 100644 --- a/gtsam_unstable/base/tests/testFixedVector.cpp +++ b/gtsam_unstable/base/tests/testFixedVector.cpp @@ -28,7 +28,7 @@ static const double tol = 1e-9; /* ************************************************************************* */ TEST( testFixedVector, conversions ) { double data1[] = {1.0, 2.0, 3.0}; - Vector v1 = Vector_(3, data1); + Vector v1 = (Vector(3) << data1[0], data1[1], data1[2]); TestVector3 fv1(v1), fv2(data1); Vector actFv2(fv2); @@ -37,7 +37,7 @@ TEST( testFixedVector, conversions ) { /* ************************************************************************* */ TEST( testFixedVector, variable_constructor ) { - TestVector3 act(3, 1.0, 2.0, 3.0); + TestVector3 act((Vector(3) << 1.0, 2.0, 3.0)); EXPECT_DOUBLES_EQUAL(1.0, act(0), tol); EXPECT_DOUBLES_EQUAL(2.0, act(1), tol); EXPECT_DOUBLES_EQUAL(3.0, act(2), tol); @@ -45,8 +45,9 @@ TEST( testFixedVector, variable_constructor ) { /* ************************************************************************* */ TEST( testFixedVector, equals ) { - TestVector3 vec1(3, 1.0, 2.0, 3.0), vec2(3, 1.0, 2.0, 3.0), vec3(3, 2.0, 3.0, 4.0); - TestVector5 vec4(5, 1.0, 2.0, 3.0, 4.0, 5.0); + TestVector3 vec1((Vector(3) << 1.0, 2.0, 3.0)), vec2((Vector(3) << 1.0, 2.0, 3.0)), + vec3((Vector(3) << 2.0, 3.0, 4.0)); + TestVector5 vec4((Vector(5) << 1.0, 2.0, 3.0, 4.0, 5.0)); EXPECT(assert_equal(vec1, vec1, tol)); EXPECT(assert_equal(vec1, vec2, tol)); @@ -60,23 +61,23 @@ TEST( testFixedVector, equals ) { /* ************************************************************************* */ TEST( testFixedVector, static_constructors ) { TestVector3 actZero = TestVector3::zero(); - TestVector3 expZero(3, 0.0, 0.0, 0.0); + TestVector3 expZero((Vector(3) << 0.0, 0.0, 0.0)); EXPECT(assert_equal(expZero, actZero, tol)); TestVector3 actOnes = TestVector3::ones(); - TestVector3 expOnes(3, 1.0, 1.0, 1.0); + TestVector3 expOnes((Vector(3) << 1.0, 1.0, 1.0)); EXPECT(assert_equal(expOnes, actOnes, tol)); TestVector3 actRepeat = TestVector3::repeat(2.3); - TestVector3 expRepeat(3, 2.3, 2.3, 2.3); + TestVector3 expRepeat((Vector(3) << 2.3, 2.3, 2.3)); EXPECT(assert_equal(expRepeat, actRepeat, tol)); TestVector3 actBasis = TestVector3::basis(1); - TestVector3 expBasis(3, 0.0, 1.0, 0.0); + TestVector3 expBasis((Vector(3) << 0.0, 1.0, 0.0)); EXPECT(assert_equal(expBasis, actBasis, tol)); TestVector3 actDelta = TestVector3::delta(1, 2.3); - TestVector3 expDelta(3, 0.0, 2.3, 0.0); + TestVector3 expDelta((Vector(3) << 0.0, 2.3, 0.0)); EXPECT(assert_equal(expDelta, actDelta, tol)); } diff --git a/gtsam_unstable/base/tests/timeDSFvariants.cpp b/gtsam_unstable/base/tests/timeDSFvariants.cpp new file mode 100644 index 000000000..134c318cb --- /dev/null +++ b/gtsam_unstable/base/tests/timeDSFvariants.cpp @@ -0,0 +1,120 @@ +/* ---------------------------------------------------------------------------- + + * 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 timeDSFvariants.cpp + * @brief Time different implementations of DSF + * @author Frank Dellaert + * @date Oct 26, 2013 + */ + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; +using namespace boost::assign; +using boost::timer; +using boost::format; + +int main(int argc, char* argv[]) { + + // Create CSV file for results + ofstream os("dsf-timing.csv"); + os << "images,points,matches,Base,Map,BTree" << endl; + + // loop over number of images + vector ms; + ms += 10, 20, 30, 40, 50, 100, 200, 300, 400, 500, 1000; + BOOST_FOREACH(size_t m,ms) { + // We use volatile here to make these appear to the optimizing compiler as + // if their values are only known at run-time. + volatile size_t n = 500; // number of points per image + volatile size_t N = m * n; // number of points per image + + volatile double fm = 0.1; // fraction of image pairs matched + volatile size_t np = fm * m * m / 2; // number of image pairs + volatile double fpm = 0.5; // fraction of points matched + volatile size_t nm = fpm * n * np; // number of matches + + cout << format("\nTesting with %1% images, %2% points, %3% matches\n") % m % N % nm; + cout << "Generating " << nm << " matches" << endl; + boost::variate_generator > rn( + boost::mt19937(), boost::uniform_int(0, N - 1)); + typedef pair Match; + vector matches; + matches.reserve(nm); + for (size_t k = 0; k < nm; k++) + matches.push_back(Match(rn(), rn())); + + os << format("%1%,%2%,%3%,") % m % N % nm; + + { + // DSFBase version + timer tim; + DSFBase dsf(N); // Allow for N keys + BOOST_FOREACH(const Match& m, matches) + dsf.merge(m.first, m.second); + os << tim.elapsed() << ","; + cout << format("DSFBase: %1% s") % tim.elapsed() << endl; + } + + { + // DSFMap version + timer tim; + DSFMap dsf; + BOOST_FOREACH(const Match& m, matches) + dsf.merge(m.first, m.second); + os << tim.elapsed() << endl; + cout << format("DSFMap: %1% s") % tim.elapsed() << endl; + } + + if (false) { + // DSF version, functional + timer tim; + DSF dsf; + for (size_t j = 0; j < N; j++) + dsf = dsf.makeSet(j); + BOOST_FOREACH(const Match& m, matches) + dsf = dsf.makeUnion(m.first, m.second); + os << tim.elapsed() << endl; + cout << format("DSF functional: %1% s") % tim.elapsed() << endl; + } + + if (false) { + // DSF version, in place - always slower - use functional ! + timer tim; + DSF dsf; + for (size_t j = 0; j < N; j++) + dsf.makeSetInPlace(j); + BOOST_FOREACH(const Match& m, matches) + dsf.makeUnionInPlace(m.first, m.second); + os << tim.elapsed() << ","; + cout << format("DSF in-place: %1% s") % tim.elapsed() << endl; + } + + } + + return 0; + +} diff --git a/gtsam_unstable/base/tests/timeDSFvariants.xlsx b/gtsam_unstable/base/tests/timeDSFvariants.xlsx new file mode 100644 index 000000000..45a21e271 Binary files /dev/null and b/gtsam_unstable/base/tests/timeDSFvariants.xlsx differ diff --git a/gtsam_unstable/base/tests/timeDSFvariants2.xlsx b/gtsam_unstable/base/tests/timeDSFvariants2.xlsx new file mode 100644 index 000000000..0a1574c5b Binary files /dev/null and b/gtsam_unstable/base/tests/timeDSFvariants2.xlsx differ diff --git a/gtsam_unstable/discrete/AllDiff.cpp b/gtsam_unstable/discrete/AllDiff.cpp index 5c6d2e518..b230aa570 100644 --- a/gtsam_unstable/discrete/AllDiff.cpp +++ b/gtsam_unstable/discrete/AllDiff.cpp @@ -21,9 +21,9 @@ namespace gtsam { /* ************************************************************************* */ void AllDiff::print(const std::string& s, - const IndexFormatter& formatter) const { + const KeyFormatter& formatter) const { std::cout << s << "AllDiff on "; - BOOST_FOREACH (Index dkey, keys_) + BOOST_FOREACH (Key dkey, keys_) std::cout << formatter(dkey) << " "; std::cout << std::endl; } @@ -31,7 +31,7 @@ namespace gtsam { /* ************************************************************************* */ double AllDiff::operator()(const Values& values) const { std::set < size_t > taken; // record values taken by keys - BOOST_FOREACH(Index dkey, keys_) { + BOOST_FOREACH(Key dkey, keys_) { size_t value = values.at(dkey); // get the value for that key if (taken.count(value)) return 0.0;// check if value alreday taken taken.insert(value);// if not, record it as taken and keep checking @@ -70,7 +70,7 @@ namespace gtsam { // Check all other domains for singletons and erase corresponding values // This is the same as arc-consistency on the equivalent binary constraints bool changed = false; - BOOST_FOREACH(Index k, keys_) + BOOST_FOREACH(Key k, keys_) if (k != j) { const Domain& Dk = domains[k]; if (Dk.isSingleton()) { // check if singleton @@ -88,7 +88,7 @@ namespace gtsam { Constraint::shared_ptr AllDiff::partiallyApply(const Values& values) const { DiscreteKeys newKeys; // loop over keys and add them only if they do not appear in values - BOOST_FOREACH(Index k, keys_) + BOOST_FOREACH(Key k, keys_) if (values.find(k) == values.end()) { newKeys.push_back(DiscreteKey(k,cardinalities_.at(k))); } @@ -99,7 +99,7 @@ namespace gtsam { Constraint::shared_ptr AllDiff::partiallyApply( const std::vector& domains) const { DiscreteFactor::Values known; - BOOST_FOREACH(Index k, keys_) { + BOOST_FOREACH(Key k, keys_) { const Domain& Dk = domains[k]; if (Dk.isSingleton()) known[k] = Dk.firstValue(); diff --git a/gtsam_unstable/discrete/AllDiff.h b/gtsam_unstable/discrete/AllDiff.h index def9ef96c..3e7296593 100644 --- a/gtsam_unstable/discrete/AllDiff.h +++ b/gtsam_unstable/discrete/AllDiff.h @@ -16,15 +16,15 @@ namespace gtsam { * General AllDiff constraint * Returns 1 if values for all keys are different, 0 otherwise * DiscreteFactors are all awkward in that they have to store two types of keys: - * for each variable we have a Index and an Index. In this factor, we + * for each variable we have a Key and an Key. In this factor, we * keep the Indices locally, and the Indices are stored in IndexFactor. */ class GTSAM_UNSTABLE_EXPORT AllDiff: public Constraint { - std::map cardinalities_; + std::map cardinalities_; DiscreteKey discreteKey(size_t i) const { - Index j = keys_[i]; + Key j = keys_[i]; return DiscreteKey(j,cardinalities_.at(j)); } @@ -35,7 +35,19 @@ namespace gtsam { // print virtual void print(const std::string& s = "", - const IndexFormatter& formatter = DefaultIndexFormatter) const; + const KeyFormatter& formatter = DefaultKeyFormatter) const; + + /// equals + bool equals(const DiscreteFactor& other, double tol) const { + if(!dynamic_cast(&other)) + return false; + else { + const AllDiff& f(static_cast(other)); + return cardinalities_.size() == f.cardinalities_.size() + && std::equal(cardinalities_.begin(), cardinalities_.end(), + f.cardinalities_.begin()); + } + } /// Calculate value = expensive ! virtual double operator()(const Values& values) const; diff --git a/gtsam_unstable/discrete/BinaryAllDiff.h b/gtsam_unstable/discrete/BinaryAllDiff.h index 49867117d..1ae5a008a 100644 --- a/gtsam_unstable/discrete/BinaryAllDiff.h +++ b/gtsam_unstable/discrete/BinaryAllDiff.h @@ -34,11 +34,21 @@ namespace gtsam { // print virtual void print(const std::string& s = "", - const IndexFormatter& formatter = DefaultIndexFormatter) const { + const KeyFormatter& formatter = DefaultKeyFormatter) const { std::cout << s << "BinaryAllDiff on " << formatter(keys_[0]) << " and " << formatter(keys_[1]) << std::endl; } + /// equals + bool equals(const DiscreteFactor& other, double tol) const { + if(!dynamic_cast(&other)) + return false; + else { + const BinaryAllDiff& f(static_cast(other)); + return (cardinality0_==f.cardinality0_) && (cardinality1_==f.cardinality1_); + } + } + /// Calculate value virtual double operator()(const Values& values) const { return (double) (values.at(keys_[0]) != values.at(keys_[1])); diff --git a/gtsam_unstable/discrete/CMakeLists.txt b/gtsam_unstable/discrete/CMakeLists.txt index 6f19c65ea..18346a45a 100644 --- a/gtsam_unstable/discrete/CMakeLists.txt +++ b/gtsam_unstable/discrete/CMakeLists.txt @@ -2,50 +2,8 @@ file(GLOB discrete_headers "*.h") install(FILES ${discrete_headers} DESTINATION include/gtsam_unstable/discrete) -# Components to link tests in this subfolder against -set(discrete_local_libs - discrete_unstable - discrete - inference - base - ccolamd -) - -set (discrete_full_libs - ${gtsam-default} - ${gtsam_unstable-default}) - -# Exclude tests that don't work -#set (discrete_excluded_tests -#"${CMAKE_CURRENT_SOURCE_DIR}/tests/testScheduler.cpp" -#) - - # Add all tests -gtsam_add_subdir_tests(discrete_unstable "${discrete_local_libs}" "${discrete_full_libs}" "${discrete_excluded_tests}") -add_dependencies(check.unstable check.discrete_unstable) +add_subdirectory(tests) -# List examples to build - comment out here to exclude from compilation -set(discrete_unstable_examples -schedulingExample -schedulingQuals12 -schedulingQuals13 -) - -if (GTSAM_BUILD_EXAMPLES) - foreach(example ${discrete_unstable_examples}) - add_executable(${example} "examples/${example}.cpp") - - # Disable building during make all/install - if (GTSAM_ENABLE_INSTALL_EXAMPLE_FIX) - set_target_properties(${example} PROPERTIES EXCLUDE_FROM_ALL ON) - endif() - - if(NOT MSVC) - add_dependencies(examples ${example}) - add_custom_target(${example}.run ${EXECUTABLE_OUTPUT_PATH}${example} ${ARGN}) - endif() - - target_link_libraries(${example} ${gtsam-default} ${gtsam_unstable-default}) - endforeach(example) -endif (GTSAM_BUILD_EXAMPLES) +# Add examples +add_subdirectory(examples) diff --git a/gtsam_unstable/discrete/CSP.cpp b/gtsam_unstable/discrete/CSP.cpp index 7b27fb866..ae716f246 100644 --- a/gtsam_unstable/discrete/CSP.cpp +++ b/gtsam_unstable/discrete/CSP.cpp @@ -7,7 +7,6 @@ #include #include -#include #include #include @@ -16,10 +15,9 @@ using namespace std; namespace gtsam { /// Find the best total assignment - can be expensive - CSP::sharedValues CSP::optimalAssignment() const { - DiscreteSequentialSolver solver(*this); - DiscreteBayesNet::shared_ptr chordal = solver.eliminate(); - sharedValues mpe = optimize(*chordal); + CSP::sharedValues CSP::optimalAssignment(OptionalOrdering ordering) const { + DiscreteBayesNet::shared_ptr chordal = this->eliminateSequential(ordering); + sharedValues mpe = chordal->optimize(); return mpe; } diff --git a/gtsam_unstable/discrete/CSP.h b/gtsam_unstable/discrete/CSP.h index 0786acbe7..923365c40 100644 --- a/gtsam_unstable/discrete/CSP.h +++ b/gtsam_unstable/discrete/CSP.h @@ -22,8 +22,8 @@ namespace gtsam { public: /** A map from keys to values */ - typedef std::vector Indices; - typedef Assignment Values; + typedef std::vector Indices; + typedef Assignment Values; typedef boost::shared_ptr sharedValues; public: @@ -60,7 +60,7 @@ namespace gtsam { // } /// Find the best total assignment - can be expensive - sharedValues optimalAssignment() const; + sharedValues optimalAssignment(OptionalOrdering ordering = boost::none) const; // /* // * Perform loopy belief propagation diff --git a/gtsam_unstable/discrete/Constraint.h b/gtsam_unstable/discrete/Constraint.h index 561e9a570..d8f4689a8 100644 --- a/gtsam_unstable/discrete/Constraint.h +++ b/gtsam_unstable/discrete/Constraint.h @@ -19,6 +19,7 @@ #include #include +#include namespace gtsam { @@ -37,18 +38,18 @@ namespace gtsam { protected: /// Construct n-way factor - Constraint(const std::vector& js) : + Constraint(const std::vector& js) : DiscreteFactor(js) { } /// Construct unary factor - Constraint(Index j) : - DiscreteFactor(j) { + Constraint(Key j) : + DiscreteFactor(boost::assign::cref_list_of<1>(j)) { } /// Construct binary factor - Constraint(Index j1, Index j2) : - DiscreteFactor(j1, j2) { + Constraint(Key j1, Key j2) : + DiscreteFactor(boost::assign::cref_list_of<2>(j1)(j2)) { } /// construct from container diff --git a/gtsam_unstable/discrete/Domain.cpp b/gtsam_unstable/discrete/Domain.cpp index 397c46626..94c977cb0 100644 --- a/gtsam_unstable/discrete/Domain.cpp +++ b/gtsam_unstable/discrete/Domain.cpp @@ -16,7 +16,7 @@ namespace gtsam { /* ************************************************************************* */ void Domain::print(const string& s, - const IndexFormatter& formatter) const { + const KeyFormatter& formatter) const { // cout << s << ": Domain on " << formatter(keys_[0]) << " (j=" << // formatter(keys_[0]) << ") with values"; // BOOST_FOREACH (size_t v,values_) cout << " " << v; @@ -57,12 +57,12 @@ namespace gtsam { } /* ************************************************************************* */ - bool Domain::checkAllDiff(const vector keys, vector& domains) { - Index j = keys_[0]; + bool Domain::checkAllDiff(const vector keys, vector& domains) { + Key j = keys_[0]; // for all values in this domain BOOST_FOREACH(size_t value, values_) { // for all connected domains - BOOST_FOREACH(Index k, keys) + BOOST_FOREACH(Key k, keys) // if any domain contains the value we cannot make this domain singleton if (k!=j && domains[k].contains(value)) goto found; diff --git a/gtsam_unstable/discrete/Domain.h b/gtsam_unstable/discrete/Domain.h index c2f793759..812da9932 100644 --- a/gtsam_unstable/discrete/Domain.h +++ b/gtsam_unstable/discrete/Domain.h @@ -67,7 +67,17 @@ namespace gtsam { // print virtual void print(const std::string& s = "", - const IndexFormatter& formatter = DefaultIndexFormatter) const; + const KeyFormatter& formatter = DefaultKeyFormatter) const; + + /// equals + bool equals(const DiscreteFactor& other, double tol) const { + if(!dynamic_cast(&other)) + return false; + else { + const Domain& f(static_cast(other)); + return (cardinality_==f.cardinality_) && (values_==f.values_); + } + } bool contains(size_t value) const { return values_.count(value)>0; @@ -94,7 +104,7 @@ namespace gtsam { * If found, we make this a singleton... Called in AllDiff::ensureArcConsistency * @param keys connected domains through alldiff */ - bool checkAllDiff(const std::vector keys, std::vector& domains); + bool checkAllDiff(const std::vector keys, std::vector& domains); /// Partially apply known values virtual Constraint::shared_ptr partiallyApply( diff --git a/gtsam_unstable/discrete/Scheduler.cpp b/gtsam_unstable/discrete/Scheduler.cpp index cf0e6f199..1f22fa0e6 100644 --- a/gtsam_unstable/discrete/Scheduler.cpp +++ b/gtsam_unstable/discrete/Scheduler.cpp @@ -7,7 +7,6 @@ #include #include -#include #include #include @@ -71,9 +70,9 @@ namespace gtsam { student.name_ = studentName; // We fix the ordering by assigning a higher index to the student // and numbering the areas lower - Index j = 3*maxNrStudents_ + nrStudents(); + Key j = 3*maxNrStudents_ + nrStudents(); student.key_ = DiscreteKey(j, nrTimeSlots()); - Index base = 3*nrStudents(); + Key base = 3*nrStudents(); student.keys_[0] = DiscreteKey(base+0, nrFaculty()); student.keys_[1] = DiscreteKey(base+1, nrFaculty()); student.keys_[2] = DiscreteKey(base+2, nrFaculty()); @@ -219,10 +218,10 @@ namespace gtsam { // Not intended to be general! Assumes very particular ordering ! cout << endl; for (size_t s = 0; s < nrStudents(); s++) { - Index j = 3*maxNrStudents_ + s; + Key j = 3*maxNrStudents_ + s; size_t slot = assignment->at(j); cout << studentName(s) << " slot: " << slotName_[slot] << endl; - Index base = 3*s; + Key base = 3*s; for (size_t area = 0; area < 3; area++) { size_t faculty = assignment->at(base+area); cout << setw(12) << studentArea(s,area) << ": " << facultyName_[faculty] @@ -246,7 +245,7 @@ namespace gtsam { void Scheduler::accumulateStats(sharedValues assignment, vector< size_t>& stats) const { for (size_t s = 0; s < nrStudents(); s++) { - Index base = 3*s; + Key base = 3*s; for (size_t area = 0; area < 3; area++) { size_t f = assignment->at(base+area); assert(feliminateSequential(defaultKeyOrdering); gttoc(my_eliminate); return chordal; } @@ -271,14 +272,14 @@ namespace gtsam { DiscreteBayesNet::shared_ptr chordal = eliminate(); if (ISDEBUG("Scheduler::optimalAssignment")) { - DiscreteBayesNet::const_reverse_iterator it = chordal->rbegin(); + DiscreteBayesNet::const_iterator it = chordal->end()-1; const Student & student = students_.front(); cout << endl; (*it)->print(student.name_); } gttic(my_optimize); - sharedValues mpe = optimize(*chordal); + sharedValues mpe = chordal->optimize(); gttoc(my_optimize); return mpe; } diff --git a/gtsam_unstable/discrete/SingleValue.cpp b/gtsam_unstable/discrete/SingleValue.cpp index f91c088a1..6324f14cd 100644 --- a/gtsam_unstable/discrete/SingleValue.cpp +++ b/gtsam_unstable/discrete/SingleValue.cpp @@ -17,7 +17,7 @@ namespace gtsam { /* ************************************************************************* */ void SingleValue::print(const string& s, - const IndexFormatter& formatter) const { + const KeyFormatter& formatter) const { cout << s << "SingleValue on " << "j=" << formatter(keys_[0]) << " with value " << value_ << endl; } diff --git a/gtsam_unstable/discrete/SingleValue.h b/gtsam_unstable/discrete/SingleValue.h index 574484694..43f06956d 100644 --- a/gtsam_unstable/discrete/SingleValue.h +++ b/gtsam_unstable/discrete/SingleValue.h @@ -32,7 +32,7 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; /// Constructor - SingleValue(Index key, size_t n, size_t value) : + SingleValue(Key key, size_t n, size_t value) : Constraint(key), cardinality_(n), value_(value) { } @@ -43,7 +43,17 @@ namespace gtsam { // print virtual void print(const std::string& s = "", - const IndexFormatter& formatter = DefaultIndexFormatter) const; + const KeyFormatter& formatter = DefaultKeyFormatter) const; + + /// equals + bool equals(const DiscreteFactor& other, double tol) const { + if(!dynamic_cast(&other)) + return false; + else { + const SingleValue& f(static_cast(other)); + return (cardinality_==f.cardinality_) && (value_==f.value_); + } + } /// Calculate value virtual double operator()(const Values& values) const; diff --git a/gtsam_unstable/discrete/examples/CMakeLists.txt b/gtsam_unstable/discrete/examples/CMakeLists.txt new file mode 100644 index 000000000..da06b7dfc --- /dev/null +++ b/gtsam_unstable/discrete/examples/CMakeLists.txt @@ -0,0 +1,5 @@ +set (excluded_examples + # fileToExclude.cpp +) + +gtsamAddExamplesGlob("*.cpp" "${excluded_examples}" "gtsam_unstable") diff --git a/gtsam_unstable/discrete/examples/schedulingExample.cpp b/gtsam_unstable/discrete/examples/schedulingExample.cpp index 60b8a197d..0679dcdfa 100644 --- a/gtsam_unstable/discrete/examples/schedulingExample.cpp +++ b/gtsam_unstable/discrete/examples/schedulingExample.cpp @@ -53,6 +53,11 @@ void addStudent(Scheduler& s, size_t i) { } /* ************************************************************************* */ Scheduler largeExample(size_t nrStudents = 7) { +// char cCurrentPath[FILENAME_MAX]; +// if (!getcwd(cCurrentPath, sizeof(cCurrentPath))) return errno; +// cCurrentPath[sizeof(cCurrentPath) - 1] = '\0'; /* not really required */ +// printf ("The current working directory is %s", cCurrentPath); + string path("../../../gtsam_unstable/discrete/examples/"); Scheduler s(nrStudents, path + "Doodle.csv"); @@ -156,7 +161,7 @@ void solveStaged(size_t addMutex = 2) { gttoc_(eliminate); // find root node - DiscreteConditional::shared_ptr root = *(chordal->rbegin()); + DiscreteConditional::shared_ptr root = chordal->back(); if (debug) root->print(""/*scheduler.studentName(s)*/); @@ -223,7 +228,7 @@ void sampleSolutions() { vector stats(19, 0); vector samples; for (size_t i = 0; i < 7; i++) { - samples.push_back(sample(*samplers[i])); + samples.push_back(samplers[i]->sample()); schedulers[i].accumulateStats(samples[i], stats); } size_t max = *max_element(stats.begin(), stats.end()); @@ -309,7 +314,7 @@ void accomodateStudent() { DiscreteBayesNet::shared_ptr chordal = scheduler.eliminate(); // find root node - DiscreteConditional::shared_ptr root = *(chordal->rbegin()); + DiscreteConditional::shared_ptr root = chordal->back(); if (debug) root->print(""/*scheduler.studentName(s)*/); // GTSAM_PRINT(*chordal); @@ -327,7 +332,7 @@ void accomodateStudent() { // sample schedules for (size_t n = 0; n < 10; n++) { - Scheduler::sharedValues sample0 = sample(*chordal); + Scheduler::sharedValues sample0 = chordal->sample(); scheduler.printAssignment(sample0); } } diff --git a/gtsam_unstable/discrete/examples/schedulingQuals12.cpp b/gtsam_unstable/discrete/examples/schedulingQuals12.cpp index 1cb61e942..3a7ea4d2a 100644 --- a/gtsam_unstable/discrete/examples/schedulingQuals12.cpp +++ b/gtsam_unstable/discrete/examples/schedulingQuals12.cpp @@ -130,7 +130,7 @@ void runLargeExample() { tictoc_finishedIteration(); tictoc_print(); for (size_t i=0;i<100;i++) { - DiscreteFactor::sharedValues assignment = sample(*chordal); + DiscreteFactor::sharedValues assignment = chordal->sample(); vector stats(scheduler.nrFaculty()); scheduler.accumulateStats(assignment, stats); size_t max = *max_element(stats.begin(), stats.end()); @@ -183,7 +183,10 @@ void solveStaged(size_t addMutex = 2) { gttoc_(eliminate); // find root node - DiscreteConditional::shared_ptr root = *(chordal->rbegin()); +// chordal->back()->print("back: "); +// chordal->front()->print("front: "); +// exit(0); + DiscreteConditional::shared_ptr root = chordal->back(); if (debug) root->print(""/*scheduler.studentName(s)*/); @@ -234,7 +237,7 @@ void sampleSolutions() { vector stats(19, 0); vector samples; for (size_t i = 0; i < NRSTUDENTS; i++) { - samples.push_back(sample(*samplers[i])); + samples.push_back(samplers[i]->sample()); schedulers[i].accumulateStats(samples[i], stats); } size_t max = *max_element(stats.begin(), stats.end()); diff --git a/gtsam_unstable/discrete/examples/schedulingQuals13.cpp b/gtsam_unstable/discrete/examples/schedulingQuals13.cpp index 5548e4300..0b5238783 100644 --- a/gtsam_unstable/discrete/examples/schedulingQuals13.cpp +++ b/gtsam_unstable/discrete/examples/schedulingQuals13.cpp @@ -208,7 +208,7 @@ void solveStaged(size_t addMutex = 2) { gttoc_(eliminate); // find root node - DiscreteConditional::shared_ptr root = *(chordal->rbegin()); + DiscreteConditional::shared_ptr root = chordal->back(); if (debug) root->print(""/*scheduler.studentName(s)*/); @@ -262,7 +262,7 @@ void sampleSolutions() { vector stats(nrFaculty, 0); vector samples; for (size_t i = 0; i < NRSTUDENTS; i++) { - samples.push_back(sample(*samplers[i])); + samples.push_back(samplers[i]->sample()); schedulers[i].accumulateStats(samples[i], stats); } size_t max = *max_element(stats.begin(), stats.end()); diff --git a/gtsam_unstable/discrete/tests/CMakeLists.txt b/gtsam_unstable/discrete/tests/CMakeLists.txt new file mode 100644 index 000000000..2687a760c --- /dev/null +++ b/gtsam_unstable/discrete/tests/CMakeLists.txt @@ -0,0 +1 @@ +gtsamAddTestsGlob(discrete_unstable "test*.cpp" "" "gtsam_unstable") diff --git a/gtsam_unstable/discrete/tests/testCSP.cpp b/gtsam_unstable/discrete/tests/testCSP.cpp index dd155e817..3dd493b1b 100644 --- a/gtsam_unstable/discrete/tests/testCSP.cpp +++ b/gtsam_unstable/discrete/tests/testCSP.cpp @@ -116,19 +116,25 @@ TEST_UNSAFE( CSP, WesternUS) csp.addAllDiff(CO,NM); // Solve - CSP::sharedValues mpe = csp.optimalAssignment(); + Ordering ordering; + ordering += Key(0),Key(1),Key(2),Key(3),Key(4),Key(5),Key(6),Key(7),Key(8),Key(9),Key(10); + CSP::sharedValues mpe = csp.optimalAssignment(ordering); // GTSAM_PRINT(*mpe); CSP::Values expected; insert(expected) (WA.first,1)(CA.first,1)(NV.first,3)(OR.first,0) (MT.first,1)(WY.first,0)(NM.first,3)(CO.first,2) (ID.first,2)(UT.first,1)(AZ.first,0); + + // TODO: Fix me! mpe result seems to be right. (See the printing) + // It has the same prob as the expected solution. + // Is mpe another solution, or the expected solution is unique??? EXPECT(assert_equal(expected,*mpe)); EXPECT_DOUBLES_EQUAL(1, csp(*mpe), 1e-9); // Write out the dual graph for hmetis #ifdef DUAL - VariableIndex index(csp); + VariableIndexOrdered index(csp); index.print("index"); ofstream os("/Users/dellaert/src/hmetis-1.5-osx-i686/US-West-dual.txt"); index.outputMetisFormat(os); diff --git a/gtsam_unstable/discrete/tests/testLoopyBelief.cpp b/gtsam_unstable/discrete/tests/testLoopyBelief.cpp new file mode 100644 index 000000000..6dd9dd1b5 --- /dev/null +++ b/gtsam_unstable/discrete/tests/testLoopyBelief.cpp @@ -0,0 +1,260 @@ +/** + * @file testLoopyBelief.cpp + * @brief + * @author Duy-Nguyen Ta + * @date Oct 11, 2013 + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace boost; +using namespace boost::assign; +using namespace gtsam; + +/** + * Loopy belief solver for graphs with only binary and unary factors + */ +class LoopyBelief { + + /** Star graph struct for each node, containing + * - the star graph itself + * - the product of original unary factors so we don't have to recompute it later, and + * - the factor indices of the corrected belief factors of the neighboring nodes + */ + typedef std::map CorrectedBeliefIndices; + struct StarGraph { + DiscreteFactorGraph::shared_ptr star; + CorrectedBeliefIndices correctedBeliefIndices; + DecisionTreeFactor::shared_ptr unary; + VariableIndex varIndex_; + StarGraph(const DiscreteFactorGraph::shared_ptr& _star, + const CorrectedBeliefIndices& _beliefIndices, + const DecisionTreeFactor::shared_ptr& _unary) : + star(_star), correctedBeliefIndices(_beliefIndices), unary(_unary), varIndex_( + *_star) { + } + + void print(const std::string& s = "") const { + cout << s << ":" << endl; + star->print("Star graph: "); + BOOST_FOREACH(Key key, correctedBeliefIndices | boost::adaptors::map_keys) { + cout << "Belief factor index for " << key << ": " + << correctedBeliefIndices.at(key) << endl; + } + if (unary) + unary->print("Unary: "); + } + }; + + typedef std::map StarGraphs; + StarGraphs starGraphs_; ///< star graph at each variable + +public: + /** Constructor + * Need all discrete keys to access node's cardinality for creating belief factors + * TODO: so troublesome!! + */ + LoopyBelief(const DiscreteFactorGraph& graph, + const std::map& allDiscreteKeys) : + starGraphs_(buildStarGraphs(graph, allDiscreteKeys)) { + } + + /// print + void print(const std::string& s = "") const { + cout << s << ":" << endl; + BOOST_FOREACH(Key key, starGraphs_ | boost::adaptors::map_keys) { + starGraphs_.at(key).print((boost::format("Node %d:") % key).str()); + } + } + + /// One step of belief propagation + DiscreteFactorGraph::shared_ptr iterate( + const std::map& allDiscreteKeys) { + static const bool debug = false; + static DiscreteConditional::shared_ptr dummyCond; // unused by-product of elimination + DiscreteFactorGraph::shared_ptr beliefs(new DiscreteFactorGraph()); + std::map > allMessages; + // Eliminate each star graph + BOOST_FOREACH(Key key, starGraphs_ | boost::adaptors::map_keys) { +// cout << "***** Node " << key << "*****" << endl; + // initialize belief to the unary factor from the original graph + DecisionTreeFactor::shared_ptr beliefAtKey; + + // keep intermediate messages to divide later + std::map messages; + + // eliminate each neighbor in this star graph one by one + BOOST_FOREACH(Key neighbor, starGraphs_.at(key).correctedBeliefIndices | boost::adaptors::map_keys) { + DiscreteFactorGraph subGraph; + BOOST_FOREACH(size_t factor, starGraphs_.at(key).varIndex_[neighbor]) { + subGraph.push_back(starGraphs_.at(key).star->at(factor)); + } + if (debug) subGraph.print("------- Subgraph:"); + DiscreteFactor::shared_ptr message; + boost::tie(dummyCond, message) = EliminateDiscrete(subGraph, + Ordering(list_of(neighbor))); + // store the new factor into messages + messages.insert(make_pair(neighbor, message)); + if (debug) message->print("------- Message: "); + + // Belief is the product of all messages and the unary factor + // Incorporate new the factor to belief + if (!beliefAtKey) + beliefAtKey = boost::dynamic_pointer_cast( + message); + else + beliefAtKey = + boost::make_shared( + (*beliefAtKey) + * (*boost::dynamic_pointer_cast( + message))); + } + if (starGraphs_.at(key).unary) + beliefAtKey = boost::make_shared( + (*beliefAtKey) * (*starGraphs_.at(key).unary)); + if (debug) beliefAtKey->print("New belief at key: "); + // normalize belief + double sum = 0.0; + for (size_t v = 0; v < allDiscreteKeys.at(key).second; ++v) { + DiscreteFactor::Values val; + val[key] = v; + sum += (*beliefAtKey)(val); + } + string sumFactorTable; + for (size_t v = 0; v < allDiscreteKeys.at(key).second; ++v) + sumFactorTable = (boost::format("%s %f") % sumFactorTable % sum).str(); + DecisionTreeFactor sumFactor(allDiscreteKeys.at(key), sumFactorTable); + if (debug) sumFactor.print("denomFactor: "); + beliefAtKey = boost::make_shared((*beliefAtKey) / sumFactor); + if (debug) beliefAtKey->print("New belief at key normalized: "); + beliefs->push_back(beliefAtKey); + allMessages[key] = messages; + } + + // Update corrected beliefs + VariableIndex beliefFactors(*beliefs); + BOOST_FOREACH(Key key, starGraphs_ | boost::adaptors::map_keys) { + std::map messages = allMessages[key]; + BOOST_FOREACH(Key neighbor, starGraphs_.at(key).correctedBeliefIndices | boost::adaptors::map_keys) { + DecisionTreeFactor correctedBelief = (*boost::dynamic_pointer_cast< + DecisionTreeFactor>(beliefs->at(beliefFactors[key].front()))) + / (*boost::dynamic_pointer_cast( + messages.at(neighbor))); + if (debug) correctedBelief.print("correctedBelief: "); + size_t beliefIndex = starGraphs_.at(neighbor).correctedBeliefIndices.at( + key); + starGraphs_.at(neighbor).star->replace(beliefIndex, + boost::make_shared(correctedBelief)); + } + } + + if (debug) print("After update: "); + + return beliefs; + } + +private: + /** + * Build star graphs for each node. + */ + StarGraphs buildStarGraphs(const DiscreteFactorGraph& graph, + const std::map& allDiscreteKeys) const { + StarGraphs starGraphs; + VariableIndex varIndex(graph); ///< access to all factors of each node + BOOST_FOREACH(Key key, varIndex | boost::adaptors::map_keys) { + // initialize to multiply with other unary factors later + DecisionTreeFactor::shared_ptr prodOfUnaries; + + // collect all factors involving this key in the original graph + DiscreteFactorGraph::shared_ptr star(new DiscreteFactorGraph()); + BOOST_FOREACH(size_t factorIdx, varIndex[key]) { + star->push_back(graph.at(factorIdx)); + + // accumulate unary factors + if (graph.at(factorIdx)->size() == 1) { + if (!prodOfUnaries) + prodOfUnaries = boost::dynamic_pointer_cast( + graph.at(factorIdx)); + else + prodOfUnaries = boost::make_shared( + *prodOfUnaries + * (*boost::dynamic_pointer_cast( + graph.at(factorIdx)))); + } + } + + // add the belief factor for each neighbor variable to this star graph + // also record the factor index for later modification + FastSet neighbors = star->keys(); + neighbors.erase(key); + CorrectedBeliefIndices correctedBeliefIndices; + BOOST_FOREACH(Key neighbor, neighbors) { + // TODO: default table for keys with more than 2 values? + string initialBelief; + for (size_t v = 0; v < allDiscreteKeys.at(neighbor).second - 1; ++v) { + initialBelief = initialBelief + "0.0 "; + } + initialBelief = initialBelief + "1.0"; + star->push_back( + DecisionTreeFactor(allDiscreteKeys.at(neighbor), initialBelief)); + correctedBeliefIndices.insert(make_pair(neighbor, star->size() - 1)); + } + starGraphs.insert( + make_pair(key, + StarGraph(star, correctedBeliefIndices, prodOfUnaries))); + } + return starGraphs; + } +}; + +/* ************************************************************************* */ + +TEST_UNSAFE(LoopyBelief, construction) { + // Variables: Cloudy, Sprinkler, Rain, Wet + DiscreteKey C(0, 2), S(1, 2), R(2, 2), W(3, 2); + + // Map from key to DiscreteKey for building belief factor. + // TODO: this is bad! + std::map allKeys = map_list_of(0, C)(1, S)(2, R)(3, W); + + // Build graph + DecisionTreeFactor pC(C, "0.5 0.5"); + DiscreteConditional pSC(S | C = "0.5/0.5 0.9/0.1"); + DiscreteConditional pRC(R | C = "0.8/0.2 0.2/0.8"); + DecisionTreeFactor pSR(S & R, "0.0 0.9 0.9 0.99"); + + DiscreteFactorGraph graph; + graph.push_back(pC); + graph.push_back(pSC); + graph.push_back(pRC); + graph.push_back(pSR); + + graph.print("graph: "); + + LoopyBelief solver(graph, allKeys); + solver.print("Loopy belief: "); + + // Main loop + for (size_t iter = 0; iter < 20; ++iter) { + cout << "==================================" << endl; + cout << "iteration: " << iter << endl; + DiscreteFactorGraph::shared_ptr beliefs = solver.iterate(allKeys); + beliefs->print(); + } + +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam_unstable/discrete/tests/testScheduler.cpp b/gtsam_unstable/discrete/tests/testScheduler.cpp index 406ca0c8e..fa68f04ea 100644 --- a/gtsam_unstable/discrete/tests/testScheduler.cpp +++ b/gtsam_unstable/discrete/tests/testScheduler.cpp @@ -74,7 +74,7 @@ DiscreteFactorGraph createExpected() { expected.addAllDiff(J1 & J2 & J3); // Mutual exclusion for students - expected.addAllDiff(A & J); + expected.addAllDiff(A, J); return expected; } diff --git a/gtsam_unstable/discrete/tests/testSudoku.cpp b/gtsam_unstable/discrete/tests/testSudoku.cpp index d2ce16a12..e2115e8bc 100644 --- a/gtsam_unstable/discrete/tests/testSudoku.cpp +++ b/gtsam_unstable/discrete/tests/testSudoku.cpp @@ -34,8 +34,8 @@ public: return dkeys_.at(IJ(i, j)); } - /// return Index for cell(i,j) - Index key(size_t i, size_t j) const { + /// return Key for cell(i,j) + Key key(size_t i, size_t j) const { return dkey(i, j).first; } @@ -45,7 +45,7 @@ public: // Create variables, ordering, and unary constraints va_list ap; va_start(ap, n); - Index k=0; + Key k=0; for (size_t i = 0; i < n; ++i) { for (size_t j = 0; j < n; ++j, ++k) { // create the key @@ -97,7 +97,7 @@ public: void printAssignment(DiscreteFactor::sharedValues assignment) const { for (size_t i = 0; i < n_; i++) { for (size_t j = 0; j < n_; j++) { - Index k = key(i, j); + Key k = key(i, j); cout << 1 + assignment->at(k) << " "; } cout << endl; @@ -179,7 +179,7 @@ TEST_UNSAFE( Sudoku, extreme) sudoku.runArcConsistency(9,10,PRINT); #ifdef METIS - VariableIndex index(sudoku); + VariableIndexOrdered index(sudoku); index.print("index"); ofstream os("/Users/dellaert/src/hmetis-1.5-osx-i686/extreme-dual.txt"); index.outputMetisFormat(os); diff --git a/gtsam_unstable/dynamics/CMakeLists.txt b/gtsam_unstable/dynamics/CMakeLists.txt index bf34eb476..66aef9455 100644 --- a/gtsam_unstable/dynamics/CMakeLists.txt +++ b/gtsam_unstable/dynamics/CMakeLists.txt @@ -2,25 +2,5 @@ file(GLOB dynamics_headers "*.h") install(FILES ${dynamics_headers} DESTINATION include/gtsam_unstable/dynamics) -# Components to link tests in this subfolder against -set(dynamics_local_libs - dynamics_unstable - slam - nonlinear - linear - inference - geometry - base - ccolamd -) - -set (dynamics_full_libs - ${gtsam-default} - ${gtsam_unstable-default}) - -# Exclude tests that don't work -set (dynamics_excluded_tests "") - # Add all tests -gtsam_add_subdir_tests(dynamics_unstable "${dynamics_local_libs}" "${dynamics_full_libs}" "${dynamics_excluded_tests}") -add_dependencies(check.unstable check.dynamics_unstable) +add_subdirectory(tests) diff --git a/gtsam_unstable/dynamics/PoseRTV.cpp b/gtsam_unstable/dynamics/PoseRTV.cpp index 99964c9db..2246baee1 100644 --- a/gtsam_unstable/dynamics/PoseRTV.cpp +++ b/gtsam_unstable/dynamics/PoseRTV.cpp @@ -15,7 +15,6 @@ namespace gtsam { using namespace std; static const Vector g = delta(3, 2, 9.81); -const double pi = M_PI; /* ************************************************************************* */ double bound(double a, double min, double max) { @@ -126,7 +125,7 @@ PoseRTV PoseRTV::planarDynamics(double vel_rate, double heading_rate, const Velocity3& v1 = v(); // Update vehicle heading - Rot3 r2 = r1.retract(Vector_(3, 0.0, 0.0, heading_rate * dt)); + Rot3 r2 = r1.retract((Vector(3) << 0.0, 0.0, heading_rate * dt)); const double yaw2 = r2.ypr()(0); // Update vehicle position @@ -150,7 +149,7 @@ PoseRTV PoseRTV::flyingDynamics( const Velocity3& v1 = v(); // Update vehicle heading (and normalise yaw) - Vector rot_rates = Vector_(3, 0.0, pitch_rate, heading_rate); + Vector rot_rates = (Vector(3) << 0.0, pitch_rate, heading_rate); Rot3 r2 = r1.retract(rot_rates*dt); // Work out dynamics on platform @@ -165,7 +164,7 @@ PoseRTV PoseRTV::flyingDynamics( Point3 forward(forward_accel, 0.0, 0.0); Vector Acc_n = yaw_correction_bn.rotate(forward).vector() // applies locally forward force in the global frame - - drag * Vector_(3, v1.x(), v1.y(), 0.0) // drag term dependent on v1 + - drag * (Vector(3) << v1.x(), v1.y(), 0.0) // drag term dependent on v1 + delta(3, 2, loss_lift - lift_control); // falling due to lift lost from pitch // Update Vehicle Position and Velocity diff --git a/gtsam_unstable/dynamics/SimpleHelicopter.h b/gtsam_unstable/dynamics/SimpleHelicopter.h index 94bbc6092..ebc430277 100644 --- a/gtsam_unstable/dynamics/SimpleHelicopter.h +++ b/gtsam_unstable/dynamics/SimpleHelicopter.h @@ -126,7 +126,7 @@ public: Matrix D_gravityBody_gk; Point3 gravityBody = gk.rotation().unrotate(Point3(0.0, 0.0, -9.81*m_), D_gravityBody_gk); - Vector f_ext = Vector_(6, 0.0, 0.0, 0.0, gravityBody.x(), gravityBody.y(), gravityBody.z()); + Vector f_ext = (Vector(6) << 0.0, 0.0, 0.0, gravityBody.x(), gravityBody.y(), gravityBody.z()); Vector hx = pk - pk_1 - h_*Fu_ - h_*f_ext; @@ -154,7 +154,7 @@ public: Vector pk_1 = Pose3::dExpInv_exp(-h_*xik_1).transpose()*Inertia_*xik_1; Point3 gravityBody = gk.rotation().unrotate(Point3(0.0, 0.0, -9.81*m_)); - Vector f_ext = Vector_(6, 0.0, 0.0, 0.0, gravityBody.x(), gravityBody.y(), gravityBody.z()); + Vector f_ext = (Vector(6) << 0.0, 0.0, 0.0, gravityBody.x(), gravityBody.y(), gravityBody.z()); Vector hx = pk - pk_1 - h_*Fu_ - h_*f_ext; diff --git a/gtsam_unstable/dynamics/tests/CMakeLists.txt b/gtsam_unstable/dynamics/tests/CMakeLists.txt new file mode 100644 index 000000000..493cef4fa --- /dev/null +++ b/gtsam_unstable/dynamics/tests/CMakeLists.txt @@ -0,0 +1 @@ +gtsamAddTestsGlob(dynamics_unstable "test*.cpp" "" "gtsam_unstable") diff --git a/gtsam_unstable/dynamics/tests/testIMUSystem.cpp b/gtsam_unstable/dynamics/tests/testIMUSystem.cpp index 0cefa315c..be7078d9a 100644 --- a/gtsam_unstable/dynamics/tests/testIMUSystem.cpp +++ b/gtsam_unstable/dynamics/tests/testIMUSystem.cpp @@ -56,9 +56,9 @@ TEST( testIMUSystem, optimize_chain ) { // create a simple chain of poses to generate IMU measurements const double dt = 1.0; PoseRTV pose1, - pose2(Point3(1.0, 1.0, 0.0), Rot3::ypr(0.1, 0.0, 0.0), Vector_(3, 2.0, 2.0, 0.0)), - pose3(Point3(2.0, 2.0, 0.0), Rot3::ypr(0.2, 0.0, 0.0), Vector_(3, 0.0, 0.0, 0.0)), - pose4(Point3(3.0, 3.0, 0.0), Rot3::ypr(0.3, 0.0, 0.0), Vector_(3, 2.0, 2.0, 0.0)); + pose2(Point3(1.0, 1.0, 0.0), Rot3::ypr(0.1, 0.0, 0.0), (Vector)(Vector(3) << 2.0, 2.0, 0.0)), + pose3(Point3(2.0, 2.0, 0.0), Rot3::ypr(0.2, 0.0, 0.0), (Vector)(Vector(3) << 0.0, 0.0, 0.0)), + pose4(Point3(3.0, 3.0, 0.0), Rot3::ypr(0.3, 0.0, 0.0), (Vector)(Vector(3) << 2.0, 2.0, 0.0)); // create measurements SharedDiagonal model = noiseModel::Unit::Create(6); @@ -69,13 +69,13 @@ TEST( testIMUSystem, optimize_chain ) { // assemble simple graph with IMU measurements and velocity constraints NonlinearFactorGraph graph; - graph.add(NonlinearEquality(x1, pose1)); - graph.add(IMUFactor(imu12, dt, x1, x2, model)); - graph.add(IMUFactor(imu23, dt, x2, x3, model)); - graph.add(IMUFactor(imu34, dt, x3, x4, model)); - graph.add(VelocityConstraint(x1, x2, dt)); - graph.add(VelocityConstraint(x2, x3, dt)); - graph.add(VelocityConstraint(x3, x4, dt)); + graph += NonlinearEquality(x1, pose1); + graph += IMUFactor(imu12, dt, x1, x2, model); + graph += IMUFactor(imu23, dt, x2, x3, model); + graph += IMUFactor(imu34, dt, x3, x4, model); + graph += VelocityConstraint(x1, x2, dt); + graph += VelocityConstraint(x2, x3, dt); + graph += VelocityConstraint(x3, x4, dt); // ground truth values Values true_values; @@ -103,9 +103,9 @@ TEST( testIMUSystem, optimize_chain_fullfactor ) { // create a simple chain of poses to generate IMU measurements const double dt = 1.0; PoseRTV pose1, - pose2(Point3(1.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Vector_(3, 1.0, 0.0, 0.0)), - pose3(Point3(2.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Vector_(3, 1.0, 0.0, 0.0)), - pose4(Point3(3.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Vector_(3, 1.0, 0.0, 0.0)); + pose2(Point3(1.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), (Vector)(Vector(3) << 1.0, 0.0, 0.0)), + pose3(Point3(2.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), (Vector)(Vector(3) << 1.0, 0.0, 0.0)), + pose4(Point3(3.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), (Vector)(Vector(3) << 1.0, 0.0, 0.0)); // create measurements SharedDiagonal model = noiseModel::Isotropic::Sigma(9, 1.0); @@ -116,10 +116,10 @@ TEST( testIMUSystem, optimize_chain_fullfactor ) { // assemble simple graph with IMU measurements and velocity constraints NonlinearFactorGraph graph; - graph.add(NonlinearEquality(x1, pose1)); - graph.add(FullIMUFactor(imu12, dt, x1, x2, model)); - graph.add(FullIMUFactor(imu23, dt, x2, x3, model)); - graph.add(FullIMUFactor(imu34, dt, x3, x4, model)); + graph += NonlinearEquality(x1, pose1); + graph += FullIMUFactor(imu12, dt, x1, x2, model); + graph += FullIMUFactor(imu23, dt, x2, x3, model); + graph += FullIMUFactor(imu34, dt, x3, x4, model); // ground truth values Values true_values; @@ -158,7 +158,7 @@ TEST( testIMUSystem, linear_trajectory) { Values true_traj, init_traj; NonlinearFactorGraph graph; - graph.add(NonlinearEquality(x0, start)); + graph += NonlinearEquality(x0, start); true_traj.insert(x0, start); init_traj.insert(x0, start); @@ -167,7 +167,7 @@ TEST( testIMUSystem, linear_trajectory) { for (size_t i=1; i(accel - g, gyro, dt, xA, xB, model)); + graph += FullIMUFactor(accel - g, gyro, dt, xA, xB, model); true_traj.insert(xB, cur_pose); init_traj.insert(xB, PoseRTV()); } diff --git a/gtsam_unstable/dynamics/tests/testPendulumFactors.cpp b/gtsam_unstable/dynamics/tests/testPendulumFactors.cpp index e841fba3a..ce176787c 100644 --- a/gtsam_unstable/dynamics/tests/testPendulumFactors.cpp +++ b/gtsam_unstable/dynamics/tests/testPendulumFactors.cpp @@ -4,7 +4,7 @@ */ #include -#include +#include #include /* ************************************************************************* */ diff --git a/gtsam_unstable/dynamics/tests/testPoseRTV.cpp b/gtsam_unstable/dynamics/tests/testPoseRTV.cpp index b362f2ca2..e024ce884 100644 --- a/gtsam_unstable/dynamics/tests/testPoseRTV.cpp +++ b/gtsam_unstable/dynamics/tests/testPoseRTV.cpp @@ -47,7 +47,7 @@ TEST( testPoseRTV, constructors ) { EXPECT(assert_equal(Velocity3(), state4.v(), tol)); EXPECT(assert_equal(Pose3(rot, pt), state4.pose(), tol)); - Vector vec_init = Vector_(9, 0.1, 0.2, 0.3, 1.0, 2.0, 3.0, 0.4, 0.5, 0.6); + Vector vec_init = (Vector(9) << 0.1, 0.2, 0.3, 1.0, 2.0, 3.0, 0.4, 0.5, 0.6); PoseRTV state5(vec_init); EXPECT(assert_equal(pt, state5.t(), tol)); EXPECT(assert_equal(rot, state5.R(), tol)); @@ -83,7 +83,7 @@ TEST( testPoseRTV, Lie ) { EXPECT(assert_equal(state1, state1.retract(zero(9)), tol)); EXPECT(assert_equal(zero(9), state1.localCoordinates(state1), tol)); - Vector delta = Vector_(9, 0.1, 0.1, 0.1, 0.2, 0.3, 0.4,-0.1,-0.2,-0.3); + Vector delta = (Vector(9) << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4,-0.1,-0.2,-0.3); Rot3 rot2 = rot.retract(repeat(3, 0.1)); Point3 pt2 = pt + rot * Point3(0.2, 0.3, 0.4); Velocity3 vel2 = vel + rot * Velocity3(-0.1,-0.2,-0.3); @@ -99,7 +99,7 @@ TEST( testPoseRTV, dynamics_identities ) { PoseRTV x0, x1, x2, x3, x4; const double dt = 0.1; - Vector accel = Vector_(3, 0.2, 0.0, 0.0), gyro = Vector_(3, 0.0, 0.0, 0.2); + Vector accel = (Vector(3) << 0.2, 0.0, 0.0), gyro = (Vector(3) << 0.0, 0.0, 0.2); Vector imu01 = zero(6), imu12 = zero(6), imu23 = zero(6), imu34 = zero(6); x1 = x0.generalDynamics(accel, gyro, dt); @@ -181,14 +181,14 @@ TEST( testPoseRTV, transformed_from_2 ) { TEST(testPoseRTV, RRTMbn) { EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMbn(zero(3)), tol)); EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMbn(Rot3()), tol)); - EXPECT(assert_equal(PoseRTV::RRTMbn(Vector_(3, 0.3, 0.2, 0.1)), PoseRTV::RRTMbn(Rot3::ypr(0.1, 0.2, 0.3)), tol)); + EXPECT(assert_equal(PoseRTV::RRTMbn((Vector(3) << 0.3, 0.2, 0.1)), PoseRTV::RRTMbn(Rot3::ypr(0.1, 0.2, 0.3)), tol)); } /* ************************************************************************* */ TEST(testPoseRTV, RRTMnb) { EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMnb(zero(3)), tol)); EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMnb(Rot3()), tol)); - EXPECT(assert_equal(PoseRTV::RRTMnb(Vector_(3, 0.3, 0.2, 0.1)), PoseRTV::RRTMnb(Rot3::ypr(0.1, 0.2, 0.3)), tol)); + EXPECT(assert_equal(PoseRTV::RRTMnb((Vector(3) << 0.3, 0.2, 0.1)), PoseRTV::RRTMnb(Rot3::ypr(0.1, 0.2, 0.3)), tol)); } /* ************************************************************************* */ diff --git a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp index 4c052f704..82197302b 100644 --- a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp +++ b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp @@ -5,7 +5,7 @@ #include #include -#include +#include #include /* ************************************************************************* */ @@ -14,28 +14,28 @@ using namespace gtsam::symbol_shorthand; const double tol=1e-5; const double h = 0.01; -const double deg2rad = M_PI/180.0; +//const double deg2rad = M_PI/180.0; //Pose3 g1(Rot3::ypr(deg2rad*10.0, deg2rad*20.0, deg2rad*30.0), Point3(100.0, 200.0, 300.0)); Pose3 g1(Rot3(), Point3(100.0, 0.0, 300.0)); -//LieVector v1(Vector_(6, 0.1, 0.05, 0.02, 10.0, 20.0, 30.0)); -LieVector V1_w(Vector_(6, 0.0, 0.0, M_PI/3, 0.0, 0.0, 30.0)); +//LieVector v1((Vector(6) << 0.1, 0.05, 0.02, 10.0, 20.0, 30.0)); +LieVector V1_w((Vector(6) << 0.0, 0.0, M_PI/3, 0.0, 0.0, 30.0)); LieVector V1_g1 = g1.inverse().Adjoint(V1_w); Pose3 g2(g1.retract(h*V1_g1, Pose3::EXPMAP)); //LieVector v2 = Pose3::Logmap(g1.between(g2)); double mass = 100.0; -Vector gamma2 = Vector_(2, 0.0, 0.0); // no shape -Vector u2 = Vector_(2, 0.0, 0.0); // no control at time 2 +Vector gamma2 = (Vector(2) << 0.0, 0.0); // no shape +Vector u2 = (Vector(2) << 0.0, 0.0); // no control at time 2 double distT = 1.0; // distance from the body-centered x axis to the big top motor double distR = 5.0; // distance from the body-centered z axis to the small motor -Matrix Mass = diag(Vector_(3, mass, mass, mass)); -Matrix Inertia = diag(Vector_(6, 2.0/5.0*mass*distR*distR, 2.0/5.0*mass*distR*distR, 2.0/5.0*mass*distR*distR, mass, mass, mass)); +Matrix Mass = diag((Vector(3) << mass, mass, mass)); +Matrix Inertia = diag((Vector(6) << 2.0/5.0*mass*distR*distR, 2.0/5.0*mass*distR*distR, 2.0/5.0*mass*distR*distR, mass, mass, mass)); Vector computeFu(const Vector& gamma, const Vector& control) { double gamma_r = gamma(0), gamma_p = gamma(1); - Matrix F = Matrix_(6, 2, distT*sin(gamma_r), 0.0, + Matrix F = (Matrix(6, 2) << distT*sin(gamma_r), 0.0, distT*sin(gamma_p*cos(gamma_r)), 0.0, 0.0, distR, sin(gamma_p)*cos(gamma_r), 0.0, @@ -94,13 +94,17 @@ TEST( Reconstruction, evaluateError) { EXPECT(assert_equal(numericalH1,H1,1e-5)); EXPECT(assert_equal(numericalH2,H2,1e-5)); - EXPECT(assert_equal(numericalH3,H3,1e-5)); +#ifdef GTSAM_USE_QUATERNIONS // TODO: why is the quaternion version much less accurate?? + EXPECT(assert_equal(numericalH3,H3,1e-3)); +#else + EXPECT(assert_equal(numericalH3,H3,1e-3)); +#endif } /* ************************************************************************* */ // Implement Newton-Euler equation for rigid body dynamics Vector newtonEuler(const Vector& Vb, const Vector& Fb, const Matrix& Inertia) { - Matrix W = Pose3::adjointMap(Vector_(6, Vb(0), Vb(1), Vb(2), 0., 0., 0.)); + Matrix W = Pose3::adjointMap((Vector(6) << Vb(0), Vb(1), Vb(2), 0., 0., 0.)); Vector dV = Inertia.inverse()*(Fb - W*Inertia*Vb); return dV; } diff --git a/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp b/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp index c6c6d3219..7188463b4 100644 --- a/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp +++ b/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp @@ -15,9 +15,9 @@ const Key x1 = 1, x2 = 2; const double dt = 1.0; PoseRTV origin, - pose1(Point3(0.5, 0.0, 0.0), Rot3::identity(), Vector_(3, 1.0, 0.0, 0.0)), + pose1(Point3(0.5, 0.0, 0.0), Rot3::identity(), (Vector)(Vector(3) << 1.0, 0.0, 0.0)), pose1a(Point3(0.5, 0.0, 0.0)), - pose2(Point3(1.5, 0.0, 0.0), Rot3::identity(), Vector_(3, 1.0, 0.0, 0.0)); + pose2(Point3(1.5, 0.0, 0.0), Rot3::identity(), (Vector)(Vector(3) << 1.0, 0.0, 0.0)); /* ************************************************************************* */ TEST( testVelocityConstraint, trapezoidal ) { diff --git a/gtsam_unstable/dynamics/tests/testVelocityConstraint3.cpp b/gtsam_unstable/dynamics/tests/testVelocityConstraint3.cpp index 55880ef97..db4b7c586 100644 --- a/gtsam_unstable/dynamics/tests/testVelocityConstraint3.cpp +++ b/gtsam_unstable/dynamics/tests/testVelocityConstraint3.cpp @@ -5,7 +5,7 @@ #include -#include +#include #include /* ************************************************************************* */ diff --git a/gtsam_unstable/examples/CMakeLists.txt b/gtsam_unstable/examples/CMakeLists.txt index 7af84a784..da06b7dfc 100644 --- a/gtsam_unstable/examples/CMakeLists.txt +++ b/gtsam_unstable/examples/CMakeLists.txt @@ -1,32 +1,5 @@ -if(NOT MSVC) - add_custom_target(unstable_examples) -endif() - -# Build example executables -FILE(GLOB example_srcs "*.cpp") -foreach(example_src ${example_srcs} ) - get_filename_component(example_base ${example_src} NAME_WE) - set( example_bin ${example_base} ) - message(STATUS "Adding Example ${example_bin}") - if(NOT MSVC) - add_dependencies(examples ${example_bin}) - endif() - add_executable(${example_bin} ${example_src}) - - # Disable building during make all/install - if (GTSAM_DISABLE_EXAMPLES_ON_INSTALL) - set_target_properties(${example_bin} PROPERTIES EXCLUDE_FROM_ALL ON) - endif() - - target_link_libraries(${example_bin} ${gtsam-default} ${gtsam_unstable-default}) - if(NOT MSVC) - add_custom_target(${example_bin}.run ${EXECUTABLE_OUTPUT_PATH}${example_bin} ${ARGN}) - endif() - - # Set up Visual Studio folder - if(MSVC) - set_property(TARGET ${example_bin} PROPERTY FOLDER "Examples") - endif() - -endforeach(example_src) +set (excluded_examples + # fileToExclude.cpp +) +gtsamAddExamplesGlob("*.cpp" "${excluded_examples}" "gtsam_unstable") diff --git a/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp b/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp index 65fc681c4..3c9b8d129 100644 --- a/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp +++ b/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp @@ -49,7 +49,7 @@ #include // We will use simple integer Keys to uniquely identify each robot pose. -#include +#include // We will use Pose2 variables (x, y, theta) to represent the robot positions #include @@ -84,9 +84,9 @@ int main(int argc, char** argv) { // Create a prior on the first pose, placing it at the origin Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin - noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); + noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1)); Key priorKey = 0; - newFactors.add(PriorFactor(priorKey, priorMean, priorNoise)); + newFactors.push_back(PriorFactor(priorKey, priorMean, priorNoise)); newValues.insert(priorKey, priorMean); // Initialize the first pose at the mean of the prior newTimestamps[priorKey] = 0.0; // Set the timestamp associated with this key to 0.0 seconds; @@ -110,12 +110,12 @@ int main(int argc, char** argv) { // Add odometry factors from two different sources with different error stats Pose2 odometryMeasurement1 = Pose2(0.61, -0.08, 0.02); - noiseModel::Diagonal::shared_ptr odometryNoise1 = noiseModel::Diagonal::Sigmas(Vector_(3, 0.1, 0.1, 0.05)); - newFactors.add(BetweenFactor(previousKey, currentKey, odometryMeasurement1, odometryNoise1)); + noiseModel::Diagonal::shared_ptr odometryNoise1 = noiseModel::Diagonal::Sigmas((Vector(3) << 0.1, 0.1, 0.05)); + newFactors.push_back(BetweenFactor(previousKey, currentKey, odometryMeasurement1, odometryNoise1)); Pose2 odometryMeasurement2 = Pose2(0.47, 0.03, 0.01); - noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas(Vector_(3, 0.05, 0.05, 0.05)); - newFactors.add(BetweenFactor(previousKey, currentKey, odometryMeasurement2, odometryNoise2)); + noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas((Vector(3) << 0.05, 0.05, 0.05)); + newFactors.push_back(BetweenFactor(previousKey, currentKey, odometryMeasurement2, odometryNoise2)); // Unlike the fixed-lag versions, the concurrent filter implementation // requires the user to supply the specify which keys to move to the smoother @@ -159,7 +159,7 @@ int main(int argc, char** argv) { Key loopKey1(1000 * (0.0)); Key loopKey2(1000 * (5.0)); Pose2 loopMeasurement = Pose2(9.5, 1.00, 0.00); - noiseModel::Diagonal::shared_ptr loopNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.5, 0.5, 0.25)); + noiseModel::Diagonal::shared_ptr loopNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 0.25)); NonlinearFactor::shared_ptr loopFactor(new BetweenFactor(loopKey1, loopKey2, loopMeasurement, loopNoise)); // This measurement cannot be added directly to the concurrent filter because it connects a filter state to a smoother state @@ -189,12 +189,12 @@ int main(int argc, char** argv) { // Add odometry factors from two different sources with different error stats Pose2 odometryMeasurement1 = Pose2(0.61, -0.08, 0.02); - noiseModel::Diagonal::shared_ptr odometryNoise1 = noiseModel::Diagonal::Sigmas(Vector_(3, 0.1, 0.1, 0.05)); - newFactors.add(BetweenFactor(previousKey, currentKey, odometryMeasurement1, odometryNoise1)); + noiseModel::Diagonal::shared_ptr odometryNoise1 = noiseModel::Diagonal::Sigmas((Vector(3) << 0.1, 0.1, 0.05)); + newFactors.push_back(BetweenFactor(previousKey, currentKey, odometryMeasurement1, odometryNoise1)); Pose2 odometryMeasurement2 = Pose2(0.47, 0.03, 0.01); - noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas(Vector_(3, 0.05, 0.05, 0.05)); - newFactors.add(BetweenFactor(previousKey, currentKey, odometryMeasurement2, odometryNoise2)); + noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas((Vector(3) << 0.05, 0.05, 0.05)); + newFactors.push_back(BetweenFactor(previousKey, currentKey, odometryMeasurement2, odometryNoise2)); // Unlike the fixed-lag versions, the concurrent filter implementation // requires the user to supply the specify which keys to marginalize @@ -245,7 +245,7 @@ int main(int argc, char** argv) { // Now run for a few more seconds so the concurrent smoother and filter have to to re-sync // Continue adding odometry factors until the loop closure may be incorporated into the concurrent smoother - for(double time = 8.0+deltaT; time <= 10.0; time += deltaT) { + for(double time = 8.0+deltaT; time <= 15.0; time += deltaT) { // Define the keys related to this timestamp Key previousKey(1000 * (time-deltaT)); @@ -262,12 +262,12 @@ int main(int argc, char** argv) { // Add odometry factors from two different sources with different error stats Pose2 odometryMeasurement1 = Pose2(0.61, -0.08, 0.02); - noiseModel::Diagonal::shared_ptr odometryNoise1 = noiseModel::Diagonal::Sigmas(Vector_(3, 0.1, 0.1, 0.05)); - newFactors.add(BetweenFactor(previousKey, currentKey, odometryMeasurement1, odometryNoise1)); + noiseModel::Diagonal::shared_ptr odometryNoise1 = noiseModel::Diagonal::Sigmas((Vector(3) << 0.1, 0.1, 0.05)); + newFactors.push_back(BetweenFactor(previousKey, currentKey, odometryMeasurement1, odometryNoise1)); Pose2 odometryMeasurement2 = Pose2(0.47, 0.03, 0.01); - noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas(Vector_(3, 0.05, 0.05, 0.05)); - newFactors.add(BetweenFactor(previousKey, currentKey, odometryMeasurement2, odometryNoise2)); + noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas((Vector(3) << 0.05, 0.05, 0.05)); + newFactors.push_back(BetweenFactor(previousKey, currentKey, odometryMeasurement2, odometryNoise2)); // Unlike the fixed-lag versions, the concurrent filter implementation // requires the user to supply the specify which keys to marginalize @@ -307,7 +307,7 @@ int main(int argc, char** argv) { // And to demonstrate the fixed-lag aspect, print the keys contained in each smoother after 3.0 seconds - cout << "After 10.0 seconds, each version contains to the following keys:" << endl; + cout << "After 15.0 seconds, each version contains to the following keys:" << endl; cout << " Concurrent Filter Keys: " << endl; BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, concurrentFilter.getLinearizationPoint()) { cout << setprecision(5) << " Key: " << key_value.key << endl; diff --git a/gtsam_unstable/examples/FixedLagSmootherExample.cpp b/gtsam_unstable/examples/FixedLagSmootherExample.cpp index 69712540c..0980630ad 100644 --- a/gtsam_unstable/examples/FixedLagSmootherExample.cpp +++ b/gtsam_unstable/examples/FixedLagSmootherExample.cpp @@ -45,7 +45,7 @@ #include // We will use simple integer Keys to uniquely identify each robot pose. -#include +#include // We will use Pose2 variables (x, y, theta) to represent the robot positions #include @@ -78,9 +78,9 @@ int main(int argc, char** argv) { // Create a prior on the first pose, placing it at the origin Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin - noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); + noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1)); Key priorKey = 0; - newFactors.add(PriorFactor(priorKey, priorMean, priorNoise)); + newFactors.push_back(PriorFactor(priorKey, priorMean, priorNoise)); newValues.insert(priorKey, priorMean); // Initialize the first pose at the mean of the prior newTimestamps[priorKey] = 0.0; // Set the timestamp associated with this key to 0.0 seconds; @@ -104,12 +104,12 @@ int main(int argc, char** argv) { // Add odometry factors from two different sources with different error stats Pose2 odometryMeasurement1 = Pose2(0.61, -0.08, 0.02); - noiseModel::Diagonal::shared_ptr odometryNoise1 = noiseModel::Diagonal::Sigmas(Vector_(3, 0.1, 0.1, 0.05)); - newFactors.add(BetweenFactor(previousKey, currentKey, odometryMeasurement1, odometryNoise1)); + noiseModel::Diagonal::shared_ptr odometryNoise1 = noiseModel::Diagonal::Sigmas((Vector(3) << 0.1, 0.1, 0.05)); + newFactors.push_back(BetweenFactor(previousKey, currentKey, odometryMeasurement1, odometryNoise1)); Pose2 odometryMeasurement2 = Pose2(0.47, 0.03, 0.01); - noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas(Vector_(3, 0.05, 0.05, 0.05)); - newFactors.add(BetweenFactor(previousKey, currentKey, odometryMeasurement2, odometryNoise2)); + noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas((Vector(3) << 0.05, 0.05, 0.05)); + newFactors.push_back(BetweenFactor(previousKey, currentKey, odometryMeasurement2, odometryNoise2)); // Update the smoothers with the new factors smootherBatch.update(newFactors, newValues, newTimestamps); diff --git a/gtsam_unstable/examples/SmartProjectionFactorExample_kitti.cpp b/gtsam_unstable/examples/SmartProjectionFactorExample_kitti.cpp deleted file mode 100644 index 1b0b32ff9..000000000 --- a/gtsam_unstable/examples/SmartProjectionFactorExample_kitti.cpp +++ /dev/null @@ -1,301 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 SmartProjectionFactorExample_kitti.cpp - * @brief Example usage of SmartProjectionFactor using real dataset - * @date August, 2013 - * @author Zsolt Kira - */ - -// Both relative poses and recovered trajectory poses will be stored as Pose2 objects -#include - -// Each variable in the system (poses and landmarks) must be identified with a unique key. -// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1). -// Here we will use Symbols -#include - -// We want to use iSAM2 to solve the range-SLAM problem incrementally -#include - -// iSAM2 requires as input a set set of new factors to be added stored in a factor graph, -// and initial guesses for any new variables used in the added factors -#include -#include - -// We will use a non-liear solver to batch-inituialize from the first 150 frames -#include - -// In GTSAM, measurement functions are represented as 'factors'. Several common factors -// have been provided with the library for solving robotics SLAM problems. -#include -#include -#include - -// Standard headers, added last, so we know headers above work on their own -#include -#include -#include - -using namespace std; -using namespace gtsam; -namespace NM = gtsam::noiseModel; - -using symbol_shorthand::X; -using symbol_shorthand::L; - -typedef PriorFactor Pose3Prior; - -//// Helper functions taken from VO code -// Loaded all pose values into list -Values::shared_ptr loadPoseValues(const string& filename) { - Values::shared_ptr values(new Values()); - bool addNoise = false; - - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); - - // read in camera poses - string full_filename = filename; - ifstream fin; - fin.open(full_filename.c_str()); - - int pose_id; - while (fin >> pose_id) { - double pose_matrix[16]; - for (int i = 0; i < 16; i++) { - fin >> pose_matrix[i]; - } - - if (addNoise) { - values->insert(Symbol('x',pose_id), Pose3(Matrix_(4, 4, pose_matrix)).compose(noise_pose)); - } else { - values->insert(Symbol('x',pose_id), Pose3(Matrix_(4, 4, pose_matrix))); - } - } - - fin.close(); - return values; -} - -// Loaded specific pose values that are in key list -Values::shared_ptr loadPoseValues(const string& filename, list keys) { - Values::shared_ptr values(new Values()); - std::list::iterator kit; - - // read in camera poses - string full_filename = filename; - ifstream fin; - fin.open(full_filename.c_str()); - - int pose_id; - while (fin >> pose_id) { - double pose_matrix[16]; - for (int i = 0; i < 16; i++) { - fin >> pose_matrix[i]; - } - kit = find (keys.begin(), keys.end(), X(pose_id)); - if (kit != keys.end()) { - cout << " Adding " << X(pose_id) << endl; - values->insert(Symbol('x',pose_id), Pose3(Matrix_(4, 4, pose_matrix))); - } - } - - fin.close(); - return values; -} - -// Load calibration info -Cal3_S2::shared_ptr loadCalibration(const string& filename) { - string full_filename = filename; - ifstream fin; - fin.open(full_filename.c_str()); - - // try loading from parent directory as backup - if(!fin) { - cerr << "Could not load " << full_filename; - exit(1); - } - - double fx, fy, s, u, v, b; - fin >> fx >> fy >> s >> u >> v >> b; - fin.close(); - - Cal3_S2::shared_ptr K(new Cal3_S2(fx, fy, s, u, v)); - return K; -} - -// main -int main(int argc, char** argv) { - - bool debug = false; - - // Set to true to use SmartProjectionFactor. Otherwise GenericProjectionFactor will be used - bool useSmartProjectionFactor = true; - - // Minimum number of views of a landmark before it is added to the graph (SmartProjectionFactor case only) - unsigned int minimumNumViews = 1; - - string HOME = getenv("HOME"); - //string input_dir = HOME + "/data/kitti/loop_closures_merged/"; - string input_dir = HOME + "/data/KITTI_00_200/"; - - typedef SmartProjectionFactor SmartFactor; - typedef GenericProjectionFactor ProjectionFactor; - static SharedNoiseModel pixel_sigma(noiseModel::Unit::Create(2)); - static SharedNoiseModel prior_model(noiseModel::Diagonal::Sigmas(Vector_(6, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01))); - NonlinearFactorGraph graph; - - // Load calibration - //Cal3_S2::shared_ptr K(new Cal3_S2(718.856, 718.856, 0.0, 607.1928, 185.2157)); - boost::shared_ptr K = loadCalibration(input_dir+"calibration.txt"); - K->print("Calibration"); - - // Load values from VO camera poses output - gtsam::Values::shared_ptr loaded_values = loadPoseValues(input_dir+"camera_poses.txt"); - graph.add(Pose3Prior(X(0),loaded_values->at(X(0)), prior_model)); - graph.add(Pose3Prior(X(1),loaded_values->at(X(1)), prior_model)); - //graph.print("thegraph"); - - // Read in kitti dataset - ifstream fin; - fin.open((input_dir+"stereo_factors.txt").c_str()); - if(!fin) { - cerr << "Could not open stereo_factors.txt" << endl; - exit(1); - } - - // read all measurements tracked by VO stereo - cout << "Loading stereo_factors.txt" << endl; - int count = 0; - Key currentLandmark = 0; - int numLandmarks = 0; - Key r, l; - double uL, uR, v, x, y, z; - std::list allViews; - std::vector views; - std::vector measurements; - Values values; - while (fin >> r >> l >> uL >> uR >> v >> x >> y >> z) { - if (debug) cout << "CurrentLandmark " << currentLandmark << " Landmark " << l << std::endl; - - if (useSmartProjectionFactor == false) { - if (loaded_values->exists(L(l)) == boost::none) { - Pose3 camera = loaded_values->at(X(r)); - Point3 worldPoint = camera.transform_from(Point3(x, y, z)); - loaded_values->insert(L(l), worldPoint); // add point; - } - - ProjectionFactor::shared_ptr projectionFactor(new ProjectionFactor(Point2(uL,v), pixel_sigma, X(r), L(l), K)); - graph.push_back(projectionFactor); - } - - if (currentLandmark != l && views.size() < minimumNumViews) { - // New landmark. Not enough views for previous landmark so move on. - if (debug) cout << "New landmark " << l << " with not enough view for previous one" << std::endl; - currentLandmark = l; - views.clear(); - measurements.clear(); - } else if (currentLandmark != l) { - // New landmark. Add previous landmark and associated views to new factor - if (debug) cout << "New landmark " << l << " with "<< views.size() << " views for previous landmark " << currentLandmark << std::endl; - - if (debug) cout << "Keys "; - BOOST_FOREACH(const Key& k, views) { - allViews.push_back(k); - if (debug) cout << k << " "; - } - if (debug) cout << endl; - - if (debug) { - cout << "Measurements "; - BOOST_FOREACH(const Point2& p, measurements) { - cout << p << " "; - } - cout << endl; - } - - if (useSmartProjectionFactor) { - SmartFactor::shared_ptr smartFactor(new SmartFactor(measurements, pixel_sigma, views, K)); - graph.push_back(smartFactor); - } - - numLandmarks++; - - currentLandmark = l; - views.clear(); - measurements.clear(); - } else { - // We have new view for current landmark, so add it to the list later - if (debug) cout << "New view for landmark " << l << " (" << views.size() << " total)" << std::endl; - } - - // Add view for new landmark - views += X(r); - measurements += Point2(uL,v); - - count++; - if(count==100000) { - cout << "Loading graph... " << graph.size() << endl; - count=0; - } - } - cout << "Graph size: " << graph.size() << endl; - - /* - - // If using only subset of graph, only read in values for keys that are used - - // Get all view in the graph and populate poses from VO output - // TODO: Handle loop closures properly - cout << "All Keys (" << allViews.size() << ")" << endl; - allViews.unique(); - cout << "All Keys (" << allViews.size() << ")" << endl; - - values = *loadPoseValues(input_dir+"camera_poses.txt", allViews); - BOOST_FOREACH(const Key& k, allViews) { - if (debug) cout << k << " "; - } - cout << endl; - */ - - cout << "Optimizing... " << endl; - - // Optimize! - LevenbergMarquardtParams params; - params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; - params.verbosity = NonlinearOptimizerParams::ERROR; - - params.lambdaInitial = 1; - params.lambdaFactor = 10; - params.maxIterations = 100; - params.relativeErrorTol = 1e-5; - params.absoluteErrorTol = 1.0; - params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; - params.verbosity = NonlinearOptimizerParams::ERROR; - params.linearSolverType = SuccessiveLinearizationParams::MULTIFRONTAL_CHOLESKY; - - LevenbergMarquardtOptimizer optimizer(graph, *loaded_values, params); - - Values result; - gttic_(SmartProjectionFactorExample_kitti); - result = optimizer.optimize(); - gttoc_(SmartProjectionFactorExample_kitti); - tictoc_finishedIteration_(); - - cout << "===================================================" << endl; - loaded_values->print("before optimization "); - result.print("results of kitti optimization "); - tictoc_print_(); - cout << "===================================================" << endl; - - exit(0); -} diff --git a/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp b/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp index ef56ceafb..6cc8a7b78 100644 --- a/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp +++ b/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp @@ -22,7 +22,7 @@ // Each variable in the system (poses and landmarks) must be identified with a unique key. // We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1). // Here we will use Symbols -#include +#include // We want to use iSAM2 to solve the range-SLAM problem incrementally #include @@ -129,7 +129,7 @@ int main(int argc, char** argv) { Pose2 pose0 = Pose2(-34.2086489999201, 45.3007639991120, M_PI - 2.02108900000000); NonlinearFactorGraph newFactors; - newFactors.add(PriorFactor(0, pose0, priorNoise)); + newFactors.push_back(PriorFactor(0, pose0, priorNoise)); ofstream os2( "/Users/dellaert/borg/gtsam/gtsam_unstable/examples/rangeResultLM.txt"); @@ -154,7 +154,7 @@ int main(int argc, char** argv) { if (smart) { BOOST_FOREACH(size_t jj,ids) { smartFactors[jj] = SmartPtr(new SmartRangeFactor(sigmaR)); - newFactors.add(smartFactors[jj]); + newFactors.push_back(smartFactors[jj]); } } @@ -173,7 +173,7 @@ int main(int argc, char** argv) { boost::tie(t, odometry) = timedOdometry; // add odometry factor - newFactors.add( + newFactors.push_back( BetweenFactor(i - 1, i, odometry, NM::Diagonal::Sigmas(odoSigmas))); @@ -198,7 +198,7 @@ int main(int argc, char** argv) { // Throw out obvious outliers based on current landmark estimates Vector error = factor.unwhitenedError(landmarkEstimates); if (k <= 200 || fabs(error[0]) < 5) - newFactors.add(factor); + newFactors.push_back(factor); } totalCount += 1; } diff --git a/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp b/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp index f52134c22..b689179a2 100644 --- a/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp +++ b/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp @@ -22,7 +22,7 @@ // Each variable in the system (poses and landmarks) must be identified with a unique key. // We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1). // Here we will use Symbols -#include +#include // We want to use iSAM2 to solve the range-SLAM problem incrementally #include @@ -124,7 +124,7 @@ int main(int argc, char** argv) { // Add prior on first pose Pose2 pose0 = Pose2(-34.2086489999201, 45.3007639991120, -2.02108900000000); NonlinearFactorGraph newFactors; - newFactors.add(PriorFactor(0, pose0, priorNoise)); + newFactors.push_back(PriorFactor(0, pose0, priorNoise)); // initialize points (Gaussian) Values initial; @@ -150,7 +150,7 @@ int main(int argc, char** argv) { boost::tie(t, odometry) = timedOdometry; // add odometry factor - newFactors.add( + newFactors.push_back( BetweenFactor(i - 1, i, odometry, NM::Diagonal::Sigmas(odoSigmas))); @@ -168,7 +168,7 @@ int main(int argc, char** argv) { // Throw out obvious outliers based on current landmark estimates Vector error = factor.unwhitenedError(landmarkEstimates); if (k <= 200 || fabs(error[0]) < 5) - newFactors.add(factor); + newFactors.push_back(factor); k = k + 1; countK = countK + 1; } diff --git a/gtsam_unstable/geometry/BearingS2.cpp b/gtsam_unstable/geometry/BearingS2.cpp index ea1bf0b74..3e1af8eb3 100644 --- a/gtsam_unstable/geometry/BearingS2.cpp +++ b/gtsam_unstable/geometry/BearingS2.cpp @@ -29,7 +29,7 @@ BearingS2 BearingS2::fromDownwardsObservation(const Pose3& A, const Point3& B) { Matrix Cnb = A.rotation().matrix().transpose(); // Cbc = [0,0,1;0,1,0;-1,0,0]; - Matrix Cbc = Matrix_(3,3, + Matrix Cbc = (Matrix(3,3) << 0.,0.,1., 0.,1.,0., -1.,0.,0.); @@ -69,7 +69,7 @@ BearingS2 BearingS2::retract(const Vector& v) const { /* ************************************************************************* */ Vector BearingS2::localCoordinates(const BearingS2& x) const { - return Vector_(2, azimuth_.localCoordinates(x.azimuth_)(0), + return (Vector(2) << azimuth_.localCoordinates(x.azimuth_)(0), elevation_.localCoordinates(x.elevation_)(0)); } diff --git a/gtsam_unstable/geometry/CMakeLists.txt b/gtsam_unstable/geometry/CMakeLists.txt index dee1db0b4..c8b7e250f 100644 --- a/gtsam_unstable/geometry/CMakeLists.txt +++ b/gtsam_unstable/geometry/CMakeLists.txt @@ -2,22 +2,5 @@ file(GLOB geometry_headers "*.h") install(FILES ${geometry_headers} DESTINATION include/gtsam_unstable/geometry) -# Components to link tests in this subfolder against -set(geometry_local_libs - geometry_unstable - geometry - base - ccolamd - linear -) - -set (geometry_full_libs - ${gtsam-default} - ${gtsam_unstable-default}) - -# Exclude tests that don't work -set (geometry_excluded_tests "") - # Add all tests -gtsam_add_subdir_tests(geometry_unstable "${geometry_local_libs}" "${geometry_full_libs}" "${geometry_excluded_tests}") -add_dependencies(check.unstable check.geometry_unstable) +add_subdirectory(tests) diff --git a/gtsam_unstable/geometry/InvDepthCamera3.h b/gtsam_unstable/geometry/InvDepthCamera3.h index 7b2a1b9fe..4eb7992a2 100644 --- a/gtsam_unstable/geometry/InvDepthCamera3.h +++ b/gtsam_unstable/geometry/InvDepthCamera3.h @@ -134,7 +134,7 @@ public: double H34 = 0; double H35 = cos_phi/rho; - *H2 = J2 * gtsam::Matrix_(3,5, + *H2 = J2 * (Matrix(3, 5) << H11, H12, H13, H14, H15, H21, H22, H23, H24, H25, H31, H32, H33, H34, H35); @@ -143,7 +143,7 @@ public: double H16 = -cos_phi*cos_theta/rho2; double H26 = -cos_phi*sin_theta/rho2; double H36 = -sin_phi/rho2; - *H3 = J2 * gtsam::Matrix_(3,1, + *H3 = J2 * (Matrix(3, 1) << H16, H26, H36); @@ -166,7 +166,7 @@ public: gtsam::Point3 ray = pw - pt; double theta = atan2(ray.y(), ray.x()); // longitude double phi = atan2(ray.z(), sqrt(ray.x()*ray.x()+ray.y()*ray.y())); - return std::make_pair(gtsam::LieVector(5, pt.x(),pt.y(),pt.z(), theta, phi), + return std::make_pair(gtsam::LieVector((Vector(5) << pt.x(),pt.y(),pt.z(), theta, phi)), gtsam::LieScalar(1./depth)); } diff --git a/gtsam_unstable/geometry/SimPolygon2D.cpp b/gtsam_unstable/geometry/SimPolygon2D.cpp index 85b592b98..15fd47236 100644 --- a/gtsam_unstable/geometry/SimPolygon2D.cpp +++ b/gtsam_unstable/geometry/SimPolygon2D.cpp @@ -145,7 +145,7 @@ SimPolygon2D SimPolygon2D::randomTriangle( // extend line by random dist and angle to get BC double dAB = randomDistance(mean_side_len, sigma_side_len, min_side_len); double tABC = randomAngle().theta(); - Pose2 xB = xA.retract(Vector_(3, dAB, 0.0, tABC)); + Pose2 xB = xA.retract((Vector(3) << dAB, 0.0, tABC)); // extend from B to find C double dBC = randomDistance(mean_side_len, sigma_side_len, min_side_len); diff --git a/gtsam_unstable/geometry/SimWall2D.cpp b/gtsam_unstable/geometry/SimWall2D.cpp index 78347a077..894312556 100644 --- a/gtsam_unstable/geometry/SimWall2D.cpp +++ b/gtsam_unstable/geometry/SimWall2D.cpp @@ -128,7 +128,7 @@ std::pair moveWithBounce(const Pose2& cur_pose, double step_size, // calculate angle to change by Rot2 dtheta = Rot2::fromAngle(angle_drift.sample()(0) + bias.theta()); - Pose2 test_pose = cur_pose.retract(Vector_(3, step_size, 0.0, Rot2::Logmap(dtheta)(0))); + Pose2 test_pose = cur_pose.retract((Vector(3) << step_size, 0.0, Rot2::Logmap(dtheta)(0))); // create a segment to use for intersection checking // find the closest intersection diff --git a/gtsam_unstable/geometry/TriangulationFactor.h b/gtsam_unstable/geometry/TriangulationFactor.h new file mode 100644 index 000000000..24b7918e3 --- /dev/null +++ b/gtsam_unstable/geometry/TriangulationFactor.h @@ -0,0 +1,197 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * testTriangulationFactor.h + * @date March 2, 2014 + * @author Frank Dellaert + */ + +#include +#include +#include +#include +#include + +namespace gtsam { + +/** + * Non-linear factor for a constraint derived from a 2D measurement. + * The calibration and pose are assumed known. + * @addtogroup SLAM + */ +template +class TriangulationFactor: public NoiseModelFactor1 { + +public: + + /// Camera type + typedef PinholeCamera Camera; + +protected: + + // Keep a copy of measurement and calibration for I/O + const Camera camera_; ///< Camera in which this landmark was seen + const Point2 measured_; ///< 2D measurement + + // verbosity handling for Cheirality Exceptions + const bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false) + const bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false) + +public: + + /// shorthand for base class type + typedef NoiseModelFactor1 Base; + + /// shorthand for this class + typedef TriangulationFactor This; + + /// shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; + + /// Default constructor + TriangulationFactor() : + throwCheirality_(false), verboseCheirality_(false) { + } + + /** + * Constructor with exception-handling flags + * @param camera is the camera in which unknown landmark is seen + * @param measured is the 2 dimensional location of point in image (the measurement) + * @param model is the standard deviation + * @param pointKey is the index of the landmark + * @param throwCheirality determines whether Cheirality exceptions are rethrown + * @param verboseCheirality determines whether exceptions are printed for Cheirality + */ + TriangulationFactor(const Camera& camera, const Point2& measured, + const SharedNoiseModel& model, Key pointKey, bool throwCheirality = false, + bool verboseCheirality = false) : + Base(model, pointKey), camera_(camera), measured_(measured), throwCheirality_( + throwCheirality), verboseCheirality_(verboseCheirality) { + if (model && model->dim() != 2) + throw std::invalid_argument( + "TriangulationFactor must be created with 2-dimensional noise model."); + } + + /** Virtual destructor */ + virtual ~TriangulationFactor() { + } + + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + /** + * print + * @param s optional string naming the factor + * @param keyFormatter optional formatter useful for printing Symbols + */ + void print(const std::string& s = "", const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const { + std::cout << s << "TriangulationFactor,"; + camera_.print("camera"); + measured_.print("z"); + Base::print("", keyFormatter); + } + + /// equals + virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { + const This *e = dynamic_cast(&p); + return e && Base::equals(p, tol) && this->camera_.equals(e->camera_, tol) + && this->measured_.equals(e->measured_, tol); + } + + /// Evaluate error h(x)-z and optionally derivatives + Vector evaluateError(const Point3& point, boost::optional H2 = + boost::none) const { + try { + Point2 error(camera_.project(point, boost::none, H2) - measured_); + return error.vector(); + } catch (CheiralityException& e) { + if (H2) + *H2 = zeros(2, 3); + if (verboseCheirality_) + std::cout << e.what() << ": Landmark " + << DefaultKeyFormatter(this->key()) << " moved behind camera" + << std::endl; + if (throwCheirality_) + throw e; + return ones(2) * 2.0 * camera_.calibration().fx(); + } + } + + /// thread-safe (?) scratch memory for linearize + mutable VerticalBlockMatrix Ab; + mutable Matrix A; + mutable Vector b; + + /** + * Linearize to a JacobianFactor, does not support constrained noise model ! + * \f$ Ax-b \approx h(x+\delta x)-z = h(x) + A \delta x - z \f$ + * Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$ + */ + boost::shared_ptr linearize(const Values& x) const { + // Only linearize if the factor is active + if (!this->active(x)) + return boost::shared_ptr(); + + // Allocate memory for Jacobian factor, do only once + if (Ab.rows() == 0) { + std::vector dimensions(1, 3); + Ab = VerticalBlockMatrix(dimensions, 2, true); + A.resize(2,3); + b.resize(2); + } + + // Would be even better if we could pass blocks to project + const Point3& point = x.at(key()); + b = -(camera_.project(point, boost::none, A) - measured_).vector(); + if (noiseModel_) + this->noiseModel_->WhitenSystem(A, b); + + Ab(0) = A; + Ab(1) = b; + + return boost::make_shared(this->keys_, Ab); + } + + /** return the measurement */ + const Point2& measured() const { + return measured_; + } + + /** return verbosity */ + inline bool verboseCheirality() const { + return verboseCheirality_; + } + + /** return flag for throwing cheirality exceptions */ + inline bool throwCheirality() const { + return throwCheirality_; + } + +private: + + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar & BOOST_SERIALIZATION_NVP(camera_); + ar & BOOST_SERIALIZATION_NVP(measured_); + ar & BOOST_SERIALIZATION_NVP(throwCheirality_); + ar & BOOST_SERIALIZATION_NVP(verboseCheirality_); + } +}; +} // \ namespace gtsam + diff --git a/gtsam_unstable/geometry/tests/CMakeLists.txt b/gtsam_unstable/geometry/tests/CMakeLists.txt new file mode 100644 index 000000000..e22da8652 --- /dev/null +++ b/gtsam_unstable/geometry/tests/CMakeLists.txt @@ -0,0 +1 @@ +gtsamAddTestsGlob(geometry_unstable "test*.cpp" "" "gtsam_unstable") diff --git a/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp b/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp index e899da0c9..b477d3e44 100644 --- a/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp +++ b/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp @@ -29,7 +29,7 @@ TEST( InvDepthFactor, Project1) { Point2 expected_uv = level_camera.project(landmark); InvDepthCamera3 inv_camera(level_pose, K); - LieVector inv_landmark(5, 1., 0., 1., 0., 0.); + LieVector inv_landmark((Vector(5) << 1., 0., 1., 0., 0.)); LieScalar inv_depth(1./4); Point2 actual_uv = inv_camera.project(inv_landmark, inv_depth); EXPECT(assert_equal(expected_uv, actual_uv)); @@ -45,7 +45,7 @@ TEST( InvDepthFactor, Project2) { Point2 expected = level_camera.project(landmark); InvDepthCamera3 inv_camera(level_pose, K); - LieVector diag_landmark(5, 0., 0., 1., M_PI/4., atan(1.0/sqrt(2.0))); + LieVector diag_landmark((Vector(5) << 0., 0., 1., M_PI/4., atan(1.0/sqrt(2.0)))); LieScalar inv_depth(1/sqrt(3.0)); Point2 actual = inv_camera.project(diag_landmark, inv_depth); EXPECT(assert_equal(expected, actual)); @@ -60,7 +60,7 @@ TEST( InvDepthFactor, Project3) { Point2 expected = level_camera.project(landmark); InvDepthCamera3 inv_camera(level_pose, K); - LieVector diag_landmark(5, 0., 0., 0., M_PI/4., atan(2./sqrt(2.0))); + LieVector diag_landmark((Vector(5) << 0., 0., 0., M_PI/4., atan(2./sqrt(2.0)))); LieScalar inv_depth( 1./sqrt(1.0+1+4)); Point2 actual = inv_camera.project(diag_landmark, inv_depth); EXPECT(assert_equal(expected, actual)); @@ -75,7 +75,7 @@ TEST( InvDepthFactor, Project4) { Point2 expected = level_camera.project(landmark); InvDepthCamera3 inv_camera(level_pose, K); - LieVector diag_landmark(5, 0., 0., 0., atan(4.0/1), atan(2./sqrt(1.+16.))); + LieVector diag_landmark((Vector(5) << 0., 0., 0., atan(4.0/1), atan(2./sqrt(1.+16.)))); LieScalar inv_depth(1./sqrt(1.+16.+4.)); Point2 actual = inv_camera.project(diag_landmark, inv_depth); EXPECT(assert_equal(expected, actual)); @@ -88,7 +88,7 @@ Point2 project_(const Pose3& pose, const LieVector& landmark, const LieScalar& i TEST( InvDepthFactor, Dproject_pose) { - LieVector landmark(6,0.1,0.2,0.3, 0.1,0.2); + LieVector landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2)); LieScalar inv_depth(1./4); Matrix expected = numericalDerivative31(project_,level_pose, landmark, inv_depth); InvDepthCamera3 inv_camera(level_pose,K); @@ -100,7 +100,7 @@ TEST( InvDepthFactor, Dproject_pose) /* ************************************************************************* */ TEST( InvDepthFactor, Dproject_landmark) { - LieVector landmark(5,0.1,0.2,0.3, 0.1,0.2); + LieVector landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2)); LieScalar inv_depth(1./4); Matrix expected = numericalDerivative32(project_,level_pose, landmark, inv_depth); InvDepthCamera3 inv_camera(level_pose,K); @@ -112,7 +112,7 @@ TEST( InvDepthFactor, Dproject_landmark) /* ************************************************************************* */ TEST( InvDepthFactor, Dproject_inv_depth) { - LieVector landmark(5,0.1,0.2,0.3, 0.1,0.2); + LieVector landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2)); LieScalar inv_depth(1./4); Matrix expected = numericalDerivative33(project_,level_pose, landmark, inv_depth); InvDepthCamera3 inv_camera(level_pose,K); @@ -124,7 +124,7 @@ TEST( InvDepthFactor, Dproject_inv_depth) /* ************************************************************************* */ TEST(InvDepthFactor, backproject) { - LieVector expected(5,0.,0.,1., 0.1,0.2); + LieVector expected((Vector(5) << 0.,0.,1., 0.1,0.2)); LieScalar inv_depth(1./4); InvDepthCamera3 inv_camera(level_pose,K); Point2 z = inv_camera.project(expected, inv_depth); @@ -140,7 +140,7 @@ TEST(InvDepthFactor, backproject) TEST(InvDepthFactor, backproject2) { // backwards facing camera - LieVector expected(5,-5.,-5.,2., 3., -0.1); + LieVector expected((Vector(5) << -5.,-5.,2., 3., -0.1)); LieScalar inv_depth(1./10); InvDepthCamera3 inv_camera(Pose3(Rot3::ypr(1.5,0.1, -1.5), Point3(-5, -5, 2)),K); Point2 z = inv_camera.project(expected, inv_depth); diff --git a/gtsam_unstable/geometry/tests/testPose3Upright.cpp b/gtsam_unstable/geometry/tests/testPose3Upright.cpp index 806f7dc16..dacf28992 100644 --- a/gtsam_unstable/geometry/tests/testPose3Upright.cpp +++ b/gtsam_unstable/geometry/tests/testPose3Upright.cpp @@ -72,7 +72,7 @@ TEST( testPose3Upright, manifold ) { EXPECT(assert_equal(x1, x1.retract(zero(4)), tol)); EXPECT(assert_equal(x2, x2.retract(zero(4)), tol)); - Vector delta12 = Vector_(4, 3.0, 0.0, 4.0, 0.0), delta21 = -delta12; + Vector delta12 = (Vector(4) << 3.0, 0.0, 4.0, 0.0), delta21 = -delta12; EXPECT(assert_equal(x2, x1.retract(delta12), tol)); EXPECT(assert_equal(x1, x2.retract(delta21), tol)); diff --git a/gtsam_unstable/geometry/tests/testTriangulation.cpp b/gtsam_unstable/geometry/tests/testTriangulation.cpp index 3f910e2a4..8d45311f1 100644 --- a/gtsam_unstable/geometry/tests/testTriangulation.cpp +++ b/gtsam_unstable/geometry/tests/testTriangulation.cpp @@ -16,13 +16,9 @@ * Author: cbeall3 */ -#include - -#include -#include - -#include #include +#include +#include #include #include @@ -31,69 +27,267 @@ using namespace std; using namespace gtsam; using namespace boost::assign; -/* ************************************************************************* */ +// Some common constants + +static const boost::shared_ptr sharedCal = // + boost::make_shared(1500, 1200, 0, 640, 480); + +// Looking along X-axis, 1 meter above ground plane (x-y) +static const Rot3 upright = Rot3::ypr(-M_PI / 2, 0., -M_PI / 2); +static const Pose3 pose1 = Pose3(upright, gtsam::Point3(0, 0, 1)); +PinholeCamera camera1(pose1, *sharedCal); + +// create second camera 1 meter to the right of first camera +static const Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); +PinholeCamera camera2(pose2, *sharedCal); + +// landmark ~5 meters infront of camera +static const Point3 landmark(5, 0.5, 1.2); + +// 1. Project two landmarks into two cameras and triangulate +Point2 z1 = camera1.project(landmark); +Point2 z2 = camera2.project(landmark); + +//****************************************************************************** TEST( triangulation, twoPoses) { - Cal3_S2 K(1500, 1200, 0, 640, 480); - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); - SimpleCamera level_camera(level_pose, K); - - // create second camera 1 meter to the right of first camera - Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1,0,0)); - SimpleCamera level_camera_right(level_pose_right, K); - - // landmark ~5 meters infront of camera - Point3 landmark(5, 0.5, 1.2); - - - // 1. Project two landmarks into two cameras and triangulate - Point2 level_uv = level_camera.project(landmark); - Point2 level_uv_right = level_camera_right.project(landmark); vector poses; vector measurements; - poses += level_pose, level_pose_right; - measurements += level_uv, level_uv_right; + poses += pose1, pose2; + measurements += z1, z2; - boost::optional triangulated_landmark = triangulatePoint3(poses, measurements, K); + bool optimize = true; + double rank_tol = 1e-9; + + boost::optional triangulated_landmark = triangulatePoint3(poses, + sharedCal, measurements, rank_tol, optimize); EXPECT(assert_equal(landmark, *triangulated_landmark, 1e-2)); - // 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) - measurements.at(0) += Point2(0.1,0.5); + measurements.at(0) += Point2(0.1, 0.5); measurements.at(1) += Point2(-0.2, 0.3); - boost::optional triangulated_landmark_noise = triangulatePoint3(poses, measurements, K); + boost::optional triangulated_landmark_noise = triangulatePoint3(poses, + sharedCal, measurements, rank_tol, optimize); EXPECT(assert_equal(landmark, *triangulated_landmark_noise, 1e-2)); - - - // 3. Add a slightly rotated third camera above, again with measurement noise - Pose3 pose_top = level_pose * Pose3(Rot3::ypr(0.1,0.2,0.1), Point3(0.1,-2,-.1)); - SimpleCamera camera_top(pose_top, K); - Point2 top_uv = camera_top.project(landmark); - - poses += pose_top; - measurements += top_uv + Point2(0.1, -0.1); - - boost::optional triangulated_3cameras = triangulatePoint3(poses, measurements, K); - EXPECT(assert_equal(landmark, *triangulated_3cameras, 1e-2)); - - - // 4. Test failure: Add a 4th camera facing the wrong way - Pose3 level_pose180 = Pose3(Rot3::ypr(M_PI/2, 0., -M_PI/2), Point3(0,0,1)); - SimpleCamera camera_180(level_pose180, K); - - CHECK_EXCEPTION(camera_180.project(landmark);, CheiralityException); - - poses += level_pose180; - measurements += Point2(400,400); - - boost::optional triangulated_4cameras = triangulatePoint3(poses, measurements, K); - EXPECT(boost::none == triangulated_4cameras); - } -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr);} -/* ************************************************************************* */ +//****************************************************************************** + +TEST( triangulation, twoPosesBundler) { + + boost::shared_ptr bundlerCal = // + boost::make_shared(1500, 0, 0, 640, 480); + PinholeCamera camera1(pose1, *bundlerCal); + PinholeCamera camera2(pose2, *bundlerCal); + + // 1. Project two landmarks into two cameras and triangulate + Point2 z1 = camera1.project(landmark); + Point2 z2 = camera2.project(landmark); + + vector poses; + vector measurements; + + poses += pose1, pose2; + measurements += z1, z2; + + bool optimize = true; + double rank_tol = 1e-9; + + boost::optional triangulated_landmark = triangulatePoint3(poses, + bundlerCal, measurements, rank_tol, optimize); + EXPECT(assert_equal(landmark, *triangulated_landmark, 1e-2)); + + // 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) + measurements.at(0) += Point2(0.1, 0.5); + measurements.at(1) += Point2(-0.2, 0.3); + + boost::optional triangulated_landmark_noise = triangulatePoint3(poses, + bundlerCal, measurements, rank_tol, optimize); + EXPECT(assert_equal(landmark, *triangulated_landmark_noise, 1e-2)); +} + +//****************************************************************************** +TEST( triangulation, fourPoses) { + vector poses; + vector measurements; + + poses += pose1, pose2; + measurements += z1, z2; + + boost::optional triangulated_landmark = triangulatePoint3(poses, + sharedCal, measurements); + EXPECT(assert_equal(landmark, *triangulated_landmark, 1e-2)); + + // 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) + measurements.at(0) += Point2(0.1, 0.5); + measurements.at(1) += Point2(-0.2, 0.3); + + boost::optional triangulated_landmark_noise = // + triangulatePoint3(poses, sharedCal, measurements); + EXPECT(assert_equal(landmark, *triangulated_landmark_noise, 1e-2)); + + // 3. Add a slightly rotated third camera above, again with measurement noise + Pose3 pose3 = pose1 * Pose3(Rot3::ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); + SimpleCamera camera3(pose3, *sharedCal); + Point2 z3 = camera3.project(landmark); + + poses += pose3; + measurements += z3 + Point2(0.1, -0.1); + + boost::optional triangulated_3cameras = // + triangulatePoint3(poses, sharedCal, measurements); + EXPECT(assert_equal(landmark, *triangulated_3cameras, 1e-2)); + + // Again with nonlinear optimization + boost::optional triangulated_3cameras_opt = triangulatePoint3(poses, + sharedCal, measurements, 1e-9, true); + EXPECT(assert_equal(landmark, *triangulated_3cameras_opt, 1e-2)); + + // 4. Test failure: Add a 4th camera facing the wrong way + Pose3 pose4 = Pose3(Rot3::ypr(M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); + SimpleCamera camera4(pose4, *sharedCal); + +#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION + CHECK_EXCEPTION(camera4.project(landmark);, CheiralityException); + + poses += pose4; + measurements += Point2(400, 400); + + CHECK_EXCEPTION(triangulatePoint3(poses, sharedCal, measurements), + TriangulationCheiralityException); +#endif +} + +//****************************************************************************** +TEST( triangulation, fourPoses_distinct_Ks) { + Cal3_S2 K1(1500, 1200, 0, 640, 480); + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + SimpleCamera camera1(pose1, K1); + + // create second camera 1 meter to the right of first camera + Cal3_S2 K2(1600, 1300, 0, 650, 440); + SimpleCamera camera2(pose2, K2); + + // 1. Project two landmarks into two cameras and triangulate + Point2 z1 = camera1.project(landmark); + Point2 z2 = camera2.project(landmark); + + vector cameras; + vector measurements; + + cameras += camera1, camera2; + measurements += z1, z2; + + boost::optional triangulated_landmark = // + triangulatePoint3(cameras, measurements); + EXPECT(assert_equal(landmark, *triangulated_landmark, 1e-2)); + + // 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) + measurements.at(0) += Point2(0.1, 0.5); + measurements.at(1) += Point2(-0.2, 0.3); + + boost::optional triangulated_landmark_noise = // + triangulatePoint3(cameras, measurements); + EXPECT(assert_equal(landmark, *triangulated_landmark_noise, 1e-2)); + + // 3. Add a slightly rotated third camera above, again with measurement noise + Pose3 pose3 = pose1 * Pose3(Rot3::ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); + Cal3_S2 K3(700, 500, 0, 640, 480); + SimpleCamera camera3(pose3, K3); + Point2 z3 = camera3.project(landmark); + + cameras += camera3; + measurements += z3 + Point2(0.1, -0.1); + + boost::optional triangulated_3cameras = // + triangulatePoint3(cameras, measurements); + EXPECT(assert_equal(landmark, *triangulated_3cameras, 1e-2)); + + // Again with nonlinear optimization + boost::optional triangulated_3cameras_opt = triangulatePoint3(cameras, + measurements, 1e-9, true); + EXPECT(assert_equal(landmark, *triangulated_3cameras_opt, 1e-2)); + + // 4. Test failure: Add a 4th camera facing the wrong way + Pose3 pose4 = Pose3(Rot3::ypr(M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); + Cal3_S2 K4(700, 500, 0, 640, 480); + SimpleCamera camera4(pose4, K4); + +#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION + CHECK_EXCEPTION(camera4.project(landmark);, CheiralityException); + + cameras += camera4; + measurements += Point2(400, 400); + CHECK_EXCEPTION(triangulatePoint3(cameras, measurements), + TriangulationCheiralityException); +#endif +} + +//****************************************************************************** +TEST( triangulation, twoIdenticalPoses) { + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + SimpleCamera camera1(pose1, *sharedCal); + + // 1. Project two landmarks into two cameras and triangulate + Point2 z1 = camera1.project(landmark); + + vector poses; + vector measurements; + + poses += pose1, pose1; + measurements += z1, z1; + + CHECK_EXCEPTION(triangulatePoint3(poses, sharedCal, measurements), + TriangulationUnderconstrainedException); +} + +//****************************************************************************** +/* + TEST( triangulation, onePose) { + // we expect this test to fail with a TriangulationUnderconstrainedException + // because there's only one camera observation + + Cal3_S2 *sharedCal(1500, 1200, 0, 640, 480); + + vector poses; + vector measurements; + + poses += Pose3(); + measurements += Point2(); + + CHECK_EXCEPTION(triangulatePoint3(poses, measurements, *sharedCal), + TriangulationUnderconstrainedException); + } + */ + +//****************************************************************************** +TEST( triangulation, TriangulationFactor ) { + // Create the factor with a measurement that is 3 pixels off in x + Key pointKey(1); + SharedNoiseModel model; + typedef TriangulationFactor<> Factor; + Factor factor(camera1, z1, model, pointKey, sharedCal); + + // Use the factor to calculate the Jacobians + Matrix HActual; + factor.evaluateError(landmark, HActual); + +// Matrix expectedH1 = numericalDerivative11( +// boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, _1, pose2, +// boost::none, boost::none), pose1); + // The expected Jacobian + Matrix HExpected = numericalDerivative11( + boost::bind(&Factor::evaluateError, &factor, _1, boost::none), landmark); + + // Verify the Jacobians are correct + CHECK(assert_equal(HExpected, HActual, 1e-3)); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** diff --git a/gtsam_unstable/geometry/triangulation.cpp b/gtsam_unstable/geometry/triangulation.cpp index 28270afdf..3017fdf7a 100644 --- a/gtsam_unstable/geometry/triangulation.cpp +++ b/gtsam_unstable/geometry/triangulation.cpp @@ -10,72 +10,85 @@ * -------------------------------------------------------------------------- */ /** - * @file triangulation.cpp + * @file triangulation.h * @brief Functions for triangulation + * @date July 31, 2013 * @author Chris Beall */ #include -#include -#include -#include -#include -using namespace std; -using namespace boost::assign; +#include +#include namespace gtsam { -/* ************************************************************************* */ -// See Hartley and Zisserman, 2nd Ed., page 312 -Point3 triangulateDLT(const vector& projection_matrices, - const vector& measurements) { +/** + * DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312 + * @param projection_matrices Projection matrices (K*P^-1) + * @param measurements 2D measurements + * @param rank_tol SVD rank tolerance + * @return Triangulated Point3 + */ +Point3 triangulateDLT(const std::vector& projection_matrices, + const std::vector& measurements, double rank_tol) { - Matrix A = Matrix_(projection_matrices.size() *2, 4); + // number of cameras + size_t m = projection_matrices.size(); - for(size_t i=0; i< projection_matrices.size(); i++) { - size_t row = i*2; + // Allocate DLT matrix + Matrix A = zeros(m * 2, 4); + + for (size_t i = 0; i < m; i++) { + size_t row = i * 2; const Matrix& projection = projection_matrices.at(i); const Point2& p = measurements.at(i); // build system of equations A.row(row) = p.x() * projection.row(2) - projection.row(0); - A.row(row+1) = p.y() * projection.row(2) - projection.row(1); + A.row(row + 1) = p.y() * projection.row(2) - projection.row(1); } int rank; double error; Vector v; - boost::tie(rank, error, v) = DLT(A); - return Point3(sub( (v / v(3)),0,3)); + boost::tie(rank, error, v) = DLT(A, rank_tol); + // std::cout << "s " << s.transpose() << std:endl; + + if (rank < 3) + throw(TriangulationUnderconstrainedException()); + + // Create 3D point from eigenvector + return Point3(sub((v / v(3)), 0, 3)); } -/* ************************************************************************* */ -boost::optional triangulatePoint3(const vector& poses, - const vector& measurements, const Cal3_S2& K) { +/// +/** + * Optimize for triangulation + * @param graph nonlinear factors for projection + * @param values initial values + * @param landmarkKey to refer to landmark + * @return refined Point3 + */ +Point3 optimize(const NonlinearFactorGraph& graph, const Values& values, + Key landmarkKey) { + // Maybe we should consider Gauss-Newton? + LevenbergMarquardtParams params; + params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + params.verbosity = NonlinearOptimizerParams::ERROR; + params.lambdaInitial = 1; + params.lambdaFactor = 10; + params.maxIterations = 100; + params.absoluteErrorTol = 1.0; + params.verbosityLM = LevenbergMarquardtParams::SILENT; + params.verbosity = NonlinearOptimizerParams::SILENT; + params.linearSolverType = NonlinearOptimizerParams::MULTIFRONTAL_CHOLESKY; - assert(poses.size() == measurements.size()); + LevenbergMarquardtOptimizer optimizer(graph, values, params); + Values result = optimizer.optimize(); - if(poses.size() < 2) - return boost::none; - - vector projection_matrices; - - // construct projection matrices from poses & calibration - BOOST_FOREACH(const Pose3& pose, poses) - projection_matrices += K.matrix() * sub(pose.inverse().matrix(),0,3,0,4); - - Point3 triangulated_point = triangulateDLT(projection_matrices, measurements); - - // verify that the triangulated point lies infront of all cameras - BOOST_FOREACH(const Pose3& pose, poses) { - const Point3& p_local = pose.transform_to(triangulated_point); - if(p_local.z() <= 0) - return boost::none; - } - - return triangulated_point; + return result.at(landmarkKey); } -/* ************************************************************************* */ -} // namespace gtsam +} // \namespace gtsam + diff --git a/gtsam_unstable/geometry/triangulation.h b/gtsam_unstable/geometry/triangulation.h index c8b55e8e7..f767514c1 100644 --- a/gtsam_unstable/geometry/triangulation.h +++ b/gtsam_unstable/geometry/triangulation.h @@ -18,28 +18,251 @@ #pragma once -#include -#include -#include -#include #include +#include +#include +#include +#include + +#include namespace gtsam { +/// Exception thrown by triangulateDLT when SVD returns rank < 3 +class TriangulationUnderconstrainedException: public std::runtime_error { +public: + TriangulationUnderconstrainedException() : + std::runtime_error("Triangulation Underconstrained Exception.") { + } +}; + +/// Exception thrown by triangulateDLT when landmark is behind one or more of the cameras +class TriangulationCheiralityException: public std::runtime_error { +public: + TriangulationCheiralityException() : + std::runtime_error( + "Triangulation Cheirality Exception: The resulting landmark is behind one or more cameras.") { + } +}; + +/** + * DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312 + * @param projection_matrices Projection matrices (K*P^-1) + * @param measurements 2D measurements + * @param rank_tol SVD rank tolerance + * @return Triangulated Point3 + */ +GTSAM_UNSTABLE_EXPORT Point3 triangulateDLT( + const std::vector& projection_matrices, + const std::vector& measurements, double rank_tol); + +/// +/** + * Create a factor graph with projection factors from poses and one calibration + * @param poses Camera poses + * @param sharedCal shared pointer to single calibration object + * @param measurements 2D measurements + * @param landmarkKey to refer to landmark + * @param initialEstimate + * @return graph and initial values + */ +template +std::pair triangulationGraph( + const std::vector& poses, boost::shared_ptr sharedCal, + const std::vector& measurements, Key landmarkKey, + const Point3& initialEstimate) { + Values values; + values.insert(landmarkKey, initialEstimate); // Initial landmark value + NonlinearFactorGraph graph; + static SharedNoiseModel unit2(noiseModel::Unit::Create(2)); + static SharedNoiseModel prior_model(noiseModel::Isotropic::Sigma(6, 1e-6)); + for (size_t i = 0; i < measurements.size(); i++) { + const Pose3& pose_i = poses[i]; + PinholeCamera camera_i(pose_i, *sharedCal); + graph.push_back(TriangulationFactor // + (camera_i, measurements[i], unit2, landmarkKey)); + } + return std::make_pair(graph, values); +} + +/** + * Create a factor graph with projection factors from pinhole cameras + * (each camera has a pose and calibration) + * @param cameras pinhole cameras + * @param measurements 2D measurements + * @param landmarkKey to refer to landmark + * @param initialEstimate + * @return graph and initial values + */ +template +std::pair triangulationGraph( + const std::vector >& cameras, + const std::vector& measurements, Key landmarkKey, + const Point3& initialEstimate) { + Values values; + values.insert(landmarkKey, initialEstimate); // Initial landmark value + NonlinearFactorGraph graph; + static SharedNoiseModel unit2(noiseModel::Unit::Create(2)); + static SharedNoiseModel prior_model(noiseModel::Isotropic::Sigma(6, 1e-6)); + for (size_t i = 0; i < measurements.size(); i++) { + const PinholeCamera& camera_i = cameras[i]; + graph.push_back(TriangulationFactor // + (camera_i, measurements[i], unit2, landmarkKey)); + } + return std::make_pair(graph, values); +} + +/// +/** + * Optimize for triangulation + * @param graph nonlinear factors for projection + * @param values initial values + * @param landmarkKey to refer to landmark + * @return refined Point3 + */ +GTSAM_UNSTABLE_EXPORT Point3 optimize(const NonlinearFactorGraph& graph, + const Values& values, Key landmarkKey); + +/** + * Given an initial estimate , refine a point using measurements in several cameras + * @param poses Camera poses + * @param sharedCal shared pointer to single calibration object + * @param measurements 2D measurements + * @param initialEstimate + * @return refined Point3 + */ +template +Point3 triangulateNonlinear(const std::vector& poses, + boost::shared_ptr sharedCal, + const std::vector& measurements, const Point3& initialEstimate) { + + // Create a factor graph and initial values + Values values; + NonlinearFactorGraph graph; + boost::tie(graph, values) = triangulationGraph(poses, sharedCal, measurements, + Symbol('p', 0), initialEstimate); + + return optimize(graph, values, Symbol('p', 0)); +} + +/** + * Given an initial estimate , refine a point using measurements in several cameras + * @param cameras pinhole cameras + * @param measurements 2D measurements + * @param initialEstimate + * @return refined Point3 + */ +template +Point3 triangulateNonlinear( + const std::vector >& cameras, + const std::vector& measurements, const Point3& initialEstimate) { + + // Create a factor graph and initial values + Values values; + NonlinearFactorGraph graph; + boost::tie(graph, values) = triangulationGraph(cameras, measurements, + Symbol('p', 0), initialEstimate); + + return optimize(graph, values, Symbol('p', 0)); +} + /** * Function to triangulate 3D landmark point from an arbitrary number * of poses (at least 2) using the DLT. The function checks that the * resulting point lies in front of all cameras, but has no other checks * to verify the quality of the triangulation. * @param poses A vector of camera poses + * @param sharedCal shared pointer to single calibration object * @param measurements A vector of camera measurements - * @param K The camera calibration - * @return Returns a Point3 on success, boost::none otherwise. + * @param rank tolerance, default 1e-9 + * @param optimize Flag to turn on nonlinear refinement of triangulation + * @return Returns a Point3 */ -GTSAM_UNSTABLE_EXPORT boost::optional triangulatePoint3(const std::vector& poses, - const std::vector& measurements, const Cal3_S2& K); +template +Point3 triangulatePoint3(const std::vector& poses, + boost::shared_ptr sharedCal, + const std::vector& measurements, double rank_tol = 1e-9, + bool optimize = false) { + assert(poses.size() == measurements.size()); + if (poses.size() < 2) + throw(TriangulationUnderconstrainedException()); + + // construct projection matrices from poses & calibration + std::vector projection_matrices; + BOOST_FOREACH(const Pose3& pose, poses) { + projection_matrices.push_back( + sharedCal->K() * sub(pose.inverse().matrix(), 0, 3, 0, 4)); + } + + // Triangulate linearly + Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol); + + // The n refine using non-linear optimization + if (optimize) + point = triangulateNonlinear(poses, sharedCal, measurements, point); + +#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION + // verify that the triangulated point lies infront of all cameras + BOOST_FOREACH(const Pose3& pose, poses) { + const Point3& p_local = pose.transform_to(point); + if (p_local.z() <= 0) + throw(TriangulationCheiralityException()); + } +#endif + + return point; +} + +/** + * Function to triangulate 3D landmark point from an arbitrary number + * of poses (at least 2) using the DLT. This function is similar to the one + * above, except that each camera has its own calibration. The function + * checks that the resulting point lies in front of all cameras, but has + * no other checks to verify the quality of the triangulation. + * @param cameras pinhole cameras + * @param measurements A vector of camera measurements + * @param rank tolerance, default 1e-9 + * @param optimize Flag to turn on nonlinear refinement of triangulation + * @return Returns a Point3 + */ +template +Point3 triangulatePoint3( + const std::vector >& cameras, + const std::vector& measurements, double rank_tol = 1e-9, + bool optimize = false) { + + size_t m = cameras.size(); + assert(measurements.size()==m); + + if (m < 2) + throw(TriangulationUnderconstrainedException()); + + // construct projection matrices from poses & calibration + typedef PinholeCamera Camera; + std::vector projection_matrices; + BOOST_FOREACH(const Camera& camera, cameras) + projection_matrices.push_back( + camera.calibration().K() + * sub(camera.pose().inverse().matrix(), 0, 3, 0, 4)); + + Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol); + + // The n refine using non-linear optimization + if (optimize) + point = triangulateNonlinear(cameras, measurements, point); + +#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION + // verify that the triangulated point lies infront of all cameras + BOOST_FOREACH(const Camera& camera, cameras) { + const Point3& p_local = camera.pose().transform_to(point); + if (p_local.z() <= 0) + throw(TriangulationCheiralityException()); + } +#endif + + return point; +} } // \namespace gtsam - diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index 702450bda..1aa840825 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -16,6 +16,10 @@ virtual class gtsam::noiseModel::Base; virtual class gtsam::noiseModel::Gaussian; virtual class gtsam::imuBias::ConstantBias; virtual class gtsam::NonlinearFactor; +virtual class gtsam::NoiseModelFactor; +virtual class gtsam::NoiseModelFactor2; +virtual class gtsam::NoiseModelFactor3; +virtual class gtsam::NoiseModelFactor4; virtual class gtsam::GaussianFactor; virtual class gtsam::HessianFactor; virtual class gtsam::JacobianFactor; @@ -316,13 +320,66 @@ virtual class BetweenFactorEM : gtsam::NonlinearFactor { const gtsam::noiseModel::Gaussian* model_inlier, const gtsam::noiseModel::Gaussian* model_outlier, double prior_inlier, double prior_outlier); + BetweenFactorEM(size_t key1, size_t key2, const T& relativePose, + const gtsam::noiseModel::Gaussian* model_inlier, const gtsam::noiseModel::Gaussian* model_outlier, + double prior_inlier, double prior_outlier, bool flag_bump_up_near_zero_probs); + Vector whitenedError(const gtsam::Values& x); Vector unwhitenedError(const gtsam::Values& x); Vector calcIndicatorProb(const gtsam::Values& x); + gtsam::Pose2 measured(); // TODO: figure out how to output a template instead + void set_flag_bump_up_near_zero_probs(bool flag); + bool get_flag_bump_up_near_zero_probs() const; void serializable() const; // enabling serialization functionality }; +#include +template +virtual class TransformBtwRobotsUnaryFactorEM : gtsam::NonlinearFactor { + TransformBtwRobotsUnaryFactorEM(size_t key, const T& relativePose, size_t keyA, size_t keyB, + const gtsam::Values& valA, const gtsam::Values& valB, + const gtsam::noiseModel::Gaussian* model_inlier, const gtsam::noiseModel::Gaussian* model_outlier, + double prior_inlier, double prior_outlier); + + TransformBtwRobotsUnaryFactorEM(size_t key, const T& relativePose, size_t keyA, size_t keyB, + const gtsam::Values& valA, const gtsam::Values& valB, + const gtsam::noiseModel::Gaussian* model_inlier, const gtsam::noiseModel::Gaussian* model_outlier, + double prior_inlier, double prior_outlier, bool flag_bump_up_near_zero_probs, bool start_with_M_step); + + Vector whitenedError(const gtsam::Values& x); + Vector unwhitenedError(const gtsam::Values& x); + Vector calcIndicatorProb(const gtsam::Values& x); + void setValAValB(const gtsam::Values valA, const gtsam::Values valB); + + void serializable() const; // enabling serialization functionality +}; + +#include +template +virtual class TransformBtwRobotsUnaryFactor : gtsam::NonlinearFactor { + TransformBtwRobotsUnaryFactor(size_t key, const T& relativePose, size_t keyA, size_t keyB, + const gtsam::Values& valA, const gtsam::Values& valB, + const gtsam::noiseModel::Gaussian* model); + + Vector whitenedError(const gtsam::Values& x); + Vector unwhitenedError(const gtsam::Values& x); + void setValAValB(const gtsam::Values valA, const gtsam::Values valB); + + void serializable() const; // enabling serialization functionality +}; + +#include +virtual class SmartRangeFactor : gtsam::NoiseModelFactor { + SmartRangeFactor(double s); + + void addRange(size_t key, double measuredRange); + gtsam::Point2 triangulate(const gtsam::Values& x) const; + void print(string s) const; + +}; + + #include template virtual class RangeFactor : gtsam::NonlinearFactor { @@ -466,132 +523,132 @@ virtual class DiscreteEulerPoincareHelicopter : gtsam::NonlinearFactor { //************************************************************************* // nonlinear //************************************************************************* -#include -gtsam::GaussianFactorGraph* summarizeGraphSequential( - const gtsam::GaussianFactorGraph& full_graph, const gtsam::KeyVector& indices); -gtsam::GaussianFactorGraph* summarizeGraphSequential( - const gtsam::GaussianFactorGraph& full_graph, const gtsam::Ordering& ordering, const gtsam::KeySet& saved_keys); +// #include +// gtsam::GaussianFactorGraph* summarizeGraphSequential( +// const gtsam::GaussianFactorGraph& full_graph, const gtsam::KeyVector& indices); +// gtsam::GaussianFactorGraph* summarizeGraphSequential( +// const gtsam::GaussianFactorGraph& full_graph, const gtsam::Ordering& ordering, const gtsam::KeySet& saved_keys); -#include -class FixedLagSmootherKeyTimestampMapValue { - FixedLagSmootherKeyTimestampMapValue(const gtsam::Key& key, double timestamp); - FixedLagSmootherKeyTimestampMapValue(const gtsam::FixedLagSmootherKeyTimestampMapValue& other); -}; - -class FixedLagSmootherKeyTimestampMap { - FixedLagSmootherKeyTimestampMap(); - FixedLagSmootherKeyTimestampMap(const gtsam::FixedLagSmootherKeyTimestampMap& other); - - // Note: no print function - - // common STL methods - size_t size() const; - bool empty() const; - void clear(); - - double at(const gtsam::Key& key) const; - void insert(const gtsam::FixedLagSmootherKeyTimestampMapValue& value); -}; - -class FixedLagSmootherResult { - size_t getIterations() const; - size_t getNonlinearVariables() const; - size_t getLinearVariables() const; - double getError() const; -}; - -#include -virtual class FixedLagSmoother { - void print(string s) const; - bool equals(const gtsam::FixedLagSmoother& rhs, double tol) const; - - gtsam::FixedLagSmootherKeyTimestampMap timestamps() const; - double smootherLag() const; - - gtsam::FixedLagSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FixedLagSmootherKeyTimestampMap& timestamps); - gtsam::Values calculateEstimate() const; -}; - -#include -virtual class BatchFixedLagSmoother : gtsam::FixedLagSmoother { - BatchFixedLagSmoother(); - BatchFixedLagSmoother(double smootherLag); - BatchFixedLagSmoother(double smootherLag, const gtsam::LevenbergMarquardtParams& params); - - gtsam::LevenbergMarquardtParams params() const; -}; - -#include -virtual class IncrementalFixedLagSmoother : gtsam::FixedLagSmoother { - IncrementalFixedLagSmoother(); - IncrementalFixedLagSmoother(double smootherLag); - IncrementalFixedLagSmoother(double smootherLag, const gtsam::ISAM2Params& params); - - gtsam::ISAM2Params params() const; -}; - -#include -virtual class ConcurrentFilter { - void print(string s) const; - bool equals(const gtsam::ConcurrentFilter& rhs, double tol) const; -}; - -virtual class ConcurrentSmoother { - void print(string s) const; - bool equals(const gtsam::ConcurrentSmoother& rhs, double tol) const; -}; - -// Synchronize function -void synchronize(gtsam::ConcurrentFilter& filter, gtsam::ConcurrentSmoother& smoother); - -#include -class ConcurrentBatchFilterResult { - size_t getIterations() const; - size_t getLambdas() const; - size_t getNonlinearVariables() const; - size_t getLinearVariables() const; - double getError() const; -}; - -virtual class ConcurrentBatchFilter : gtsam::ConcurrentFilter { - ConcurrentBatchFilter(); - ConcurrentBatchFilter(const gtsam::LevenbergMarquardtParams& parameters); - - gtsam::NonlinearFactorGraph getFactors() const; - gtsam::Values getLinearizationPoint() const; - gtsam::Ordering getOrdering() const; - gtsam::VectorValues getDelta() const; - - gtsam::ConcurrentBatchFilterResult update(); - gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors); - gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta); - gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyList& keysToMove); - gtsam::Values calculateEstimate() const; -}; - -#include -class ConcurrentBatchSmootherResult { - size_t getIterations() const; - size_t getLambdas() const; - size_t getNonlinearVariables() const; - size_t getLinearVariables() const; - double getError() const; -}; - -virtual class ConcurrentBatchSmoother : gtsam::ConcurrentSmoother { - ConcurrentBatchSmoother(); - ConcurrentBatchSmoother(const gtsam::LevenbergMarquardtParams& parameters); - - gtsam::NonlinearFactorGraph getFactors() const; - gtsam::Values getLinearizationPoint() const; - gtsam::Ordering getOrdering() const; - gtsam::VectorValues getDelta() const; - - gtsam::ConcurrentBatchSmootherResult update(); - gtsam::ConcurrentBatchSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors); - gtsam::ConcurrentBatchSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta); - gtsam::Values calculateEstimate() const; -}; +// #include +// class FixedLagSmootherKeyTimestampMapValue { +// FixedLagSmootherKeyTimestampMapValue(const gtsam::Key& key, double timestamp); +// FixedLagSmootherKeyTimestampMapValue(const gtsam::FixedLagSmootherKeyTimestampMapValue& other); +// }; +// +// class FixedLagSmootherKeyTimestampMap { +// FixedLagSmootherKeyTimestampMap(); +// FixedLagSmootherKeyTimestampMap(const gtsam::FixedLagSmootherKeyTimestampMap& other); +// +// // Note: no print function +// +// // common STL methods +// size_t size() const; +// bool empty() const; +// void clear(); +// +// double at(const gtsam::Key& key) const; +// void insert(const gtsam::FixedLagSmootherKeyTimestampMapValue& value); +// }; +// +// class FixedLagSmootherResult { +// size_t getIterations() const; +// size_t getNonlinearVariables() const; +// size_t getLinearVariables() const; +// double getError() const; +// }; +// +// #include +// virtual class FixedLagSmoother { +// void print(string s) const; +// bool equals(const gtsam::FixedLagSmoother& rhs, double tol) const; +// +// gtsam::FixedLagSmootherKeyTimestampMap timestamps() const; +// double smootherLag() const; +// +// gtsam::FixedLagSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FixedLagSmootherKeyTimestampMap& timestamps); +// gtsam::Values calculateEstimate() const; +// }; +// +// #include +// virtual class BatchFixedLagSmoother : gtsam::FixedLagSmoother { +// BatchFixedLagSmoother(); +// BatchFixedLagSmoother(double smootherLag); +// BatchFixedLagSmoother(double smootherLag, const gtsam::LevenbergMarquardtParams& params); +// +// gtsam::LevenbergMarquardtParams params() const; +// }; +// +// #include +// virtual class IncrementalFixedLagSmoother : gtsam::FixedLagSmoother { +// IncrementalFixedLagSmoother(); +// IncrementalFixedLagSmoother(double smootherLag); +// IncrementalFixedLagSmoother(double smootherLag, const gtsam::ISAM2Params& params); +// +// gtsam::ISAM2Params params() const; +// }; +// +// #include +// virtual class ConcurrentFilter { +// void print(string s) const; +// bool equals(const gtsam::ConcurrentFilter& rhs, double tol) const; +// }; +// +// virtual class ConcurrentSmoother { +// void print(string s) const; +// bool equals(const gtsam::ConcurrentSmoother& rhs, double tol) const; +// }; +// +// // Synchronize function +// void synchronize(gtsam::ConcurrentFilter& filter, gtsam::ConcurrentSmoother& smoother); +// +// #include +// class ConcurrentBatchFilterResult { +// size_t getIterations() const; +// size_t getLambdas() const; +// size_t getNonlinearVariables() const; +// size_t getLinearVariables() const; +// double getError() const; +// }; +// +// virtual class ConcurrentBatchFilter : gtsam::ConcurrentFilter { +// ConcurrentBatchFilter(); +// ConcurrentBatchFilter(const gtsam::LevenbergMarquardtParams& parameters); +// +// gtsam::NonlinearFactorGraph getFactors() const; +// gtsam::Values getLinearizationPoint() const; +// gtsam::Ordering getOrdering() const; +// gtsam::VectorValues getDelta() const; +// +// gtsam::ConcurrentBatchFilterResult update(); +// gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors); +// gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta); +// gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyList& keysToMove); +// gtsam::Values calculateEstimate() const; +// }; +// +// #include +// class ConcurrentBatchSmootherResult { +// size_t getIterations() const; +// size_t getLambdas() const; +// size_t getNonlinearVariables() const; +// size_t getLinearVariables() const; +// double getError() const; +// }; +// +// virtual class ConcurrentBatchSmoother : gtsam::ConcurrentSmoother { +// ConcurrentBatchSmoother(); +// ConcurrentBatchSmoother(const gtsam::LevenbergMarquardtParams& parameters); +// +// gtsam::NonlinearFactorGraph getFactors() const; +// gtsam::Values getLinearizationPoint() const; +// gtsam::Ordering getOrdering() const; +// gtsam::VectorValues getDelta() const; +// +// gtsam::ConcurrentBatchSmootherResult update(); +// gtsam::ConcurrentBatchSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors); +// gtsam::ConcurrentBatchSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta); +// gtsam::Values calculateEstimate() const; +// }; //************************************************************************* // slam @@ -655,4 +712,32 @@ class AHRS { void print(string s) const; }; +// Tectonic SAM Factors + +#include +#include + +//typedef gtsam::NoiseModelFactor2 NLPosePose; +virtual class DeltaFactor : gtsam::NoiseModelFactor { + DeltaFactor(size_t i, size_t j, const gtsam::Point2& measured, + const gtsam::noiseModel::Base* noiseModel); + void print(string s) const; +}; + +//typedef gtsam::NoiseModelFactor4 NLPosePosePosePoint; +virtual class DeltaFactorBase : gtsam::NoiseModelFactor { + DeltaFactorBase(size_t b1, size_t i, size_t b2, size_t j, + const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel); + void print(string s) const; +}; + +//typedef gtsam::NoiseModelFactor4 NLPosePosePosePose; +virtual class OdometryFactorBase : gtsam::NoiseModelFactor { + OdometryFactorBase(size_t b1, size_t i, size_t b2, size_t j, + const gtsam::Pose2& measured, const gtsam::noiseModel::Base* noiseModel); + void print(string s) const; +}; + } //\namespace gtsam diff --git a/gtsam_unstable/linear/CMakeLists.txt b/gtsam_unstable/linear/CMakeLists.txt deleted file mode 100644 index e73e95743..000000000 --- a/gtsam_unstable/linear/CMakeLists.txt +++ /dev/null @@ -1,25 +0,0 @@ -# Install headers -file(GLOB linear_headers "*.h") -install(FILES ${linear_headers} DESTINATION include/gtsam_unstable/linear) - -# Components to link tests in this subfolder against -set(linear_local_libs - linear_unstable - nonlinear - linear - inference - geometry - base - ccolamd -) - -set (linear_full_libs - ${gtsam-default} - ${gtsam_unstable-default}) - -# Exclude tests that don't work -set (base_excluded_tests "") - -# Add all tests -gtsam_add_subdir_tests(linear_unstable "${linear_local_libs}" "${linear_full_libs}" "${linear_excluded_tests}") -add_dependencies(check.unstable check.linear_unstable) diff --git a/gtsam_unstable/linear/bayesTreeOperations.cpp b/gtsam_unstable/linear/bayesTreeOperations.cpp deleted file mode 100644 index c2e84dec3..000000000 --- a/gtsam_unstable/linear/bayesTreeOperations.cpp +++ /dev/null @@ -1,132 +0,0 @@ -/** - * @file bayesTreeOperations.cpp - * - * @date Jun 22, 2012 - * @author Alex Cunningham - */ - -#include - -#include - -using namespace std; - -namespace gtsam { - -/* ************************************************************************* */ -std::set keysToIndices(const KeySet& keys, const Ordering& ordering) { - std::set result; - BOOST_FOREACH(const Key& key, keys) - result.insert(ordering[key]); - return result; -} - -/* ************************************************************************* */ -GaussianFactorGraph splitFactors(const GaussianFactorGraph& fullgraph) { - GaussianFactorGraph result; - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, fullgraph) { - GaussianFactorGraph split = splitFactor(factor); - result.push_back(split); - } - return result; -} - -/* ************************************************************************* */ -GaussianFactorGraph splitFactor(const GaussianFactor::shared_ptr& factor) { - GaussianFactorGraph result; - if (!factor) return result; - - // Needs to be a jacobian factor - just pass along hessians - JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast(factor); - if (!jf) { - result.push_back(factor); - return result; - } - - // Loop over variables and strip off factors using split conditionals - // Assumes upper triangular structure - JacobianFactor::const_iterator rowIt, colIt; - const size_t totalRows = jf->rows(); - size_t rowsRemaining = totalRows; - for (rowIt = jf->begin(); rowIt != jf->end() && rowsRemaining > 0; ++rowIt) { - // get dim of current variable - size_t varDim = jf->getDim(rowIt); - size_t startRow = totalRows - rowsRemaining; - size_t nrRows = std::min(rowsRemaining, varDim); - - // Extract submatrices - std::vector > matrices; - for (colIt = rowIt; colIt != jf->end(); ++colIt) { - Index idx = *colIt; - const Matrix subA = jf->getA(colIt).middleRows(startRow, nrRows); - matrices.push_back(make_pair(idx, subA)); - } - - Vector subB = jf->getb().segment(startRow, nrRows); - Vector sigmas = jf->get_model()->sigmas().segment(startRow, nrRows); - SharedDiagonal model; - if (jf->get_model()->isConstrained()) - model = noiseModel::Constrained::MixedSigmas(sigmas); - else - model = noiseModel::Diagonal::Sigmas(sigmas); - - // extract matrices from each - // assemble into new JacobianFactor - result.add(matrices, subB, model); - - rowsRemaining -= nrRows; - } - return result; -} - -/* ************************************************************************* */ -GaussianFactorGraph removePriors(const GaussianFactorGraph& fullgraph) { - GaussianFactorGraph result; - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, fullgraph) { - JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast(factor); - if (factor && (!jf || jf->size() > 1)) - result.push_back(factor); - } - return result; -} - -/* ************************************************************************* */ -void findCliques(const GaussianBayesTree::sharedClique& current_clique, - std::set& result) { - // Add the current clique - result.insert(current_clique->conditional()); - - // Add the parent if not root - if (!current_clique->isRoot()) - findCliques(current_clique->parent(), result); -} - -/* ************************************************************************* */ -std::set findAffectedCliqueConditionals( - const GaussianBayesTree& bayesTree, const std::set& savedIndices) { - std::set affected_cliques; - // FIXME: track previously found keys more efficiently - BOOST_FOREACH(const Index& index, savedIndices) { - GaussianBayesTree::sharedClique clique = bayesTree.nodes()[index]; - - // add path back to root to affected set - findCliques(clique, affected_cliques); - } - return affected_cliques; -} - -/* ************************************************************************* */ -std::deque -findPathCliques(const GaussianBayesTree::sharedClique& initial) { - std::deque result, parents; - if (initial->isRoot()) - return result; - result.push_back(initial->parent()); - parents = findPathCliques(initial->parent()); - result.insert(result.end(), parents.begin(), parents.end()); - return result; -} - -/* ************************************************************************* */ -} // \namespace gtsam - diff --git a/gtsam_unstable/linear/bayesTreeOperations.h b/gtsam_unstable/linear/bayesTreeOperations.h deleted file mode 100644 index 3f85dc12c..000000000 --- a/gtsam_unstable/linear/bayesTreeOperations.h +++ /dev/null @@ -1,102 +0,0 @@ -/** - * @file bayesTreeOperations.h - * - * @brief Types and utility functions for operating on linear systems - * - * @date Jun 22, 2012 - * @author Alex Cunningham - */ - -#pragma once - -#include -#include -#include -#include - -namespace gtsam { - -// Managing orderings - -/** Converts sets of keys to indices by way of orderings */ -GTSAM_UNSTABLE_EXPORT std::set keysToIndices(const KeySet& keys, const Ordering& ordering); - -// Linear Graph Operations - -/** - * Given a graph, splits each factor into factors where the dimension is - * that of the first variable. - */ -GTSAM_UNSTABLE_EXPORT GaussianFactorGraph splitFactors(const GaussianFactorGraph& fullgraph); - -/** - * Splits a factor into factors where the dimension is - * that of the first variable. - */ -GTSAM_UNSTABLE_EXPORT GaussianFactorGraph splitFactor(const GaussianFactor::shared_ptr& factor); - -/** Removes prior jacobian factors from the graph */ -GTSAM_UNSTABLE_EXPORT GaussianFactorGraph removePriors(const GaussianFactorGraph& fullgraph); - -// Bayes Tree / Conditional operations - -/** - * Given a Bayes Tree, return conditionals corresponding to cliques that have or - * are connected to a set of wanted variables. - * - * @return the set of conditionals extracted from cliques. - */ -GTSAM_UNSTABLE_EXPORT std::set findAffectedCliqueConditionals( - const GaussianBayesTree& bayesTree, const std::set& savedIndices); - -/** - * Recursively traverses from a given clique in a Bayes Tree and collects all of the conditionals - * Adds any new cliques from path to root to the result set. - * - * Note the use of a set of shared_ptr: this will sort/filter on unique *pointer* locations, - * which ensures unique cliques, but the order of the cliques is meaningless - */ -GTSAM_UNSTABLE_EXPORT void findCliqueConditionals(const GaussianBayesTree::sharedClique& current_clique, - std::set& result); - -/** - * Given a clique, returns a sequence of clique parents to the root, not including the - * given clique. - */ -GTSAM_UNSTABLE_EXPORT std::deque -findPathCliques(const GaussianBayesTree::sharedClique& initial); - -/** - * Liquefies a BayesTree into a GaussianFactorGraph recursively, given a - * root clique - * - * @param splitConditionals flag enables spliting multi-frontal conditionals into separate factors - */ -template -GaussianFactorGraph liquefy(const typename BAYESTREE::sharedClique& root, bool splitConditionals = false) { - GaussianFactorGraph result; - if (root && root->conditional()) { - GaussianConditional::shared_ptr conditional = root->conditional(); - if (!splitConditionals) - result.push_back(conditional->toFactor()); - else - result.push_back(splitFactor(conditional->toFactor())); - } - BOOST_FOREACH(const typename BAYESTREE::sharedClique& child, root->children()) - result.push_back(liquefy(child, splitConditionals)); - return result; -} - -/** - * Liquefies a BayesTree into a GaussianFactorGraph recursively, from a full bayes tree. - * - * @param splitConditionals flag enables spliting multi-frontal conditionals into separate factors - */ -template -GaussianFactorGraph liquefy(const BAYESTREE& bayesTree, bool splitConditionals = false) { - return liquefy(bayesTree.root(), splitConditionals); -} - -} // \namespace gtsam - - diff --git a/gtsam_unstable/linear/conditioning.cpp b/gtsam_unstable/linear/conditioning.cpp deleted file mode 100644 index c02f4d48c..000000000 --- a/gtsam_unstable/linear/conditioning.cpp +++ /dev/null @@ -1,205 +0,0 @@ -/** - * @file summarization.cpp - * - * @date Jun 22, 2012 - * @author Alex Cunningham - */ - -#include -#include - -using namespace std; - -namespace gtsam { - -/* ************************************************************************* */ -GaussianConditional::shared_ptr conditionDensity(const GaussianConditional::shared_ptr& initConditional, - const std::set& saved_indices, const VectorValues& solution) { - const bool verbose = false; - - if (!initConditional) - return initConditional; - - if (verbose) { - cout << "backsubSummarize() Starting" << endl; - initConditional->print("Full initial conditional"); - } - - // Check for presence of variables to remove - std::set frontalsToRemove, parentsToRemove; - BOOST_FOREACH(const Index& frontal, initConditional->frontals()) - if (!saved_indices.count(frontal)) - frontalsToRemove.insert(frontal); - - BOOST_FOREACH(const Index& parent, initConditional->parents()) - if (!saved_indices.count(parent)) - parentsToRemove.insert(parent); - - // If all variables in this conditional are to be saved, just return initial conditional - if (frontalsToRemove.empty() && parentsToRemove.empty()) - return initConditional; - - // If none of the frontal variables are to be saved, return empty pointer - if (frontalsToRemove.size() == initConditional->nrFrontals()) - return GaussianConditional::shared_ptr(); - - // Collect dimensions of the new conditional - if (verbose) cout << " Collecting dimensions" << endl; - size_t newTotalRows = 0, newTotalCols = 1; // Need spacing for RHS - size_t newNrFrontals = 0; - size_t oldOffset = 0; - vector newDims, oldDims; - vector oldColOffsets; - vector newIndices; - vector newIdxToOldIdx; // Access to arrays, maps from new var to old var - const vector& oldIndices = initConditional->keys(); - const size_t oldNrFrontals = initConditional->nrFrontals(); - GaussianConditional::const_iterator varIt = initConditional->beginFrontals(); - size_t oldIdx = 0; - for (; varIt != initConditional->endFrontals(); ++varIt) { - size_t varDim = initConditional->dim(varIt); - oldDims += varDim; - if (!frontalsToRemove.count(*varIt)) { - newTotalCols += varDim; - newTotalRows += varDim; - newDims += varDim; - newIndices += *varIt; - ++newNrFrontals; - newIdxToOldIdx += oldIdx; - oldColOffsets += oldOffset; - } - oldOffset += varDim; - ++oldIdx; - } - varIt = initConditional->beginParents(); - for (; varIt != initConditional->endParents(); ++varIt) { - size_t varDim = initConditional->dim(varIt); - oldDims += varDim; - if (!parentsToRemove.count(*varIt)) { - newTotalCols += varDim; - newDims += varDim; - newIndices += *varIt; - } - } - newDims += 1; // For the RHS - - // Initialize new conditional - Matrix full_matrix = Matrix::Zero(newTotalRows, newTotalCols); - Vector sigmas = zero(newTotalRows); - if (verbose) cout << " Initializing new conditional\nfull_matrix:\n" << full_matrix << endl; - - // Fill in full matrix - iterate over rows for each sub-conditional - const size_t oldRNrCols = initConditional->get_R().cols(); - size_t newColOffset = 0; - for (size_t newfrontalIdx=0; newfrontalIdx rblock = - initConditional->get_R().block(oldColOffset, oldColOffset, dim, oldRNrCols-oldColOffset); - if (verbose) cout << " rblock\n" << rblock << endl; - - - // set the R matrix for this var - full_matrix.block(newColOffset, newColOffset, dim, dim) = rblock.leftCols(dim); - if (verbose) cout << " full_matrix: set R\n" << full_matrix << endl; - - // set RHS - full_matrix.block(newColOffset, newTotalCols-1, dim, 1) = initConditional->get_d().segment(oldColOffset, dim); - if (verbose) cout << " full_matrix: set rhs\n" << full_matrix << endl; - - // set sigmas - sigmas.segment(newColOffset, dim) = initConditional->get_sigmas().segment(oldColOffset, dim); - - // add parents in R block, while updating rhs - // Loop through old variable list - size_t newParentStartCol = newColOffset + dim; - size_t oldParentStartCol = dim; // Copying from Rblock - offset already accounted for - for (size_t oldIdx = newIdxToOldIdx[newfrontalIdx]+1; oldIdxbeginParents(); - for (; oldParent != initConditional->endParents(); ++oldParent) { - Index parentKey = *oldParent; - size_t parentDim = initConditional->dim(oldParent); - if (verbose) cout << " Adding parents from S: parentKey: " << parentKey << " parentDim: " << parentDim << endl; - if (parentsToRemove.count(parentKey)) { - // Solve out the variable - const Vector& parentSol = solution.at(parentKey); - assert((size_t)parentSol.size() == parentDim); - full_matrix.block(newColOffset, newTotalCols-1, dim, 1) -= - initConditional->get_S(oldParent).middleRows(oldColOffset, dim) * parentSol; - if (verbose) cout << " full_matrix: update rhs from parent in S\n" << full_matrix << endl; - } else { - // Copy the matrix block - full_matrix.block(newColOffset, newParentStartCol, dim, parentDim) = - initConditional->get_S(oldParent).middleRows(oldColOffset, dim); - if (verbose) cout << " full_matrix: add parent from S\n" << full_matrix << endl; - } - } - - // Increment the rows - newColOffset += dim; - oldColOffset += dim; - } - - // Construct a new conditional - if (verbose) cout << "backsubSummarize() Complete!" << endl; - GaussianConditional::rsd_type matrices(full_matrix, newDims.begin(), newDims.end()); - return GaussianConditional::shared_ptr(new - GaussianConditional(newIndices.begin(), newIndices.end(), newNrFrontals, matrices, sigmas)); -} - -/* ************************************************************************* */ -GaussianFactorGraph conditionDensity(const GaussianBayesTree& bayesTree, - const std::set& saved_indices) { - const bool verbose = false; - - VectorValues solution = optimize(bayesTree); - - // FIXME: set of conditionals does not manage possibility of solving out whole separators - std::set affected_cliques = findAffectedCliqueConditionals(bayesTree, saved_indices); - - // Summarize conditionals separately - GaussianFactorGraph summarized_graph; - BOOST_FOREACH(const GaussianConditional::shared_ptr& conditional, affected_cliques) { - if (verbose) conditional->print("Initial conditional"); - GaussianConditional::shared_ptr reducedConditional = conditionDensity(conditional, saved_indices, solution); - if (reducedConditional) { - if (verbose) reducedConditional->print("Final conditional"); - summarized_graph.push_back(reducedConditional->toFactor()); - } else if (verbose) { - cout << "Conditional removed after summarization!" << endl; - } - } - return summarized_graph; -} - -/* ************************************************************************* */ - -} // \namespace gtsam - diff --git a/gtsam_unstable/linear/conditioning.h b/gtsam_unstable/linear/conditioning.h deleted file mode 100644 index fbd5d6918..000000000 --- a/gtsam_unstable/linear/conditioning.h +++ /dev/null @@ -1,44 +0,0 @@ -/** - * @file summarization.h - * - * @brief Types and utility functions for summarization - * - * @date Jun 22, 2012 - * @author Alex Cunningham - */ - -#pragma once - -#include -#include -#include -#include - -namespace gtsam { - -/** - * Backsubstitution-based conditioning - reduces conditionals to - * densities on a sub-set of variables. - * - * Corner cases: - * - If no frontal vars are saved, returns a null pointer - * - * @param initConditional the conditional from which to remove a variable - * @param saved_indices is the set of indices that should appear in the result - * @param solution is a full solution for the system - */ -GTSAM_UNSTABLE_EXPORT gtsam::GaussianConditional::shared_ptr conditionDensity(const gtsam::GaussianConditional::shared_ptr& initConditional, - const std::set& saved_indices, const gtsam::VectorValues& solution); - -/** - * Backsubstitution-based conditioning for a complete Bayes Tree - reduces - * conditionals by solving out variables to eliminate. Traverses the tree to - * add the correct dummy factors whenever a separator is eliminated. - */ -GTSAM_UNSTABLE_EXPORT gtsam::GaussianFactorGraph conditionDensity(const gtsam::GaussianBayesTree& bayesTree, - const std::set& saved_indices); - - -} // \namespace gtsam - - diff --git a/gtsam_unstable/linear/corruptInitialization.h b/gtsam_unstable/linear/corruptInitialization.h deleted file mode 100644 index de657f054..000000000 --- a/gtsam_unstable/linear/corruptInitialization.h +++ /dev/null @@ -1,56 +0,0 @@ -/** - * @file corruptInitialization.h - * - * @brief Utilities for using noisemodels to corrupt given initialization value - * - * @date Jun 22, 2012 - * @author Alex Cunningham - */ - -#pragma once - -#include - -namespace gtsam { - -/** given a noisemodel and a measurement, add noise to the measurement */ -template -T corruptWithNoise(const T& init, - const noiseModel::Base::shared_ptr& model, Sampler& sampler) { - Vector n = zero(model->dim()); - noiseModel::Diagonal::shared_ptr - diag_model = boost::dynamic_pointer_cast(model); - if (diag_model) - n = sampler.sampleNewModel(diag_model); - return init.retract(n); -} - -// specialization for doubles - just adds, rather than retract -template<> -inline double corruptWithNoise(const double& init, - const noiseModel::Base::shared_ptr& model, Sampler& sampler) { - double n = 0.0; - noiseModel::Diagonal::shared_ptr - diag_model = boost::dynamic_pointer_cast(model); - if (diag_model) - n = sampler.sampleNewModel(diag_model)(0); - return init + n; -} - -// specialization for doubles - just adds, rather than retract -template<> -inline Vector corruptWithNoise(const Vector& init, - const noiseModel::Base::shared_ptr& model, Sampler& sampler) { - Vector n = zero(init.size()); - noiseModel::Diagonal::shared_ptr - diag_model = boost::dynamic_pointer_cast(model); - if (diag_model) - n = sampler.sampleNewModel(diag_model); - return init + n; -} - -} // \namespace gtsam - - - - diff --git a/gtsam_unstable/linear/tests/testBayesTreeOperations.cpp b/gtsam_unstable/linear/tests/testBayesTreeOperations.cpp deleted file mode 100644 index efc5bba55..000000000 --- a/gtsam_unstable/linear/tests/testBayesTreeOperations.cpp +++ /dev/null @@ -1,374 +0,0 @@ -/** - * @file testLinearTools.cpp - * - * @brief - * - * @date Aug 27, 2012 - * @author Alex Cunningham - */ - -#include - -#include -#include - -#include - -using namespace gtsam; - -SharedDiagonal model2 = noiseModel::Diagonal::Sigmas(ones(2)); -SharedDiagonal model4 = noiseModel::Diagonal::Sigmas(ones(4)); -SharedDiagonal model6 = noiseModel::Diagonal::Sigmas(ones(6)); - -using namespace std; - -using symbol_shorthand::X; -using symbol_shorthand::L; - -static const double tol = 1e-4; - -/* ************************************************************************* */ -TEST( testBayesTreeOperations, splitFactor1 ) { - - // Build upper-triangular system - JacobianFactor initFactor( - 0,Matrix_(4, 2, - 1.0, 2.0, - 0.0, 3.0, - 0.0, 0.0, - 0.0, 0.0), - 1,Matrix_(4, 2, - 1.0, 2.0, - 9.0, 3.0, - 6.0, 8.0, - 0.0, 7.0), - Vector_(4, 0.1, 0.2, 0.3, 0.4), - model4); - - GaussianFactorGraph actSplit = splitFactor(initFactor.clone()); - GaussianFactorGraph expSplit; - - expSplit.add( - 0,Matrix_(2, 2, - 1.0, 2.0, - 0.0, 3.0), - 1,Matrix_(2, 2, - 1.0, 2.0, - 9.0, 3.0), - Vector_(2, 0.1, 0.2), - model2); - expSplit.add( - 1,Matrix_(2, 2, - 6.0, 8.0, - 0.0, 7.0), - Vector_(2, 0.3, 0.4), - model2); - - EXPECT(assert_equal(expSplit, actSplit)); -} - -/* ************************************************************************* */ -TEST( testBayesTreeOperations, splitFactor2 ) { - - // Build upper-triangular system - JacobianFactor initFactor( - 0,Matrix_(6, 2, - 1.0, 2.0, - 0.0, 3.0, - 0.0, 0.0, - 0.0, 0.0, - 0.0, 0.0, - 0.0, 0.0), - 1,Matrix_(6, 2, - 1.0, 2.0, - 9.0, 3.0, - 6.0, 8.0, - 0.0, 7.0, - 0.0, 0.0, - 0.0, 0.0), - 2,Matrix_(6, 2, - 1.1, 2.2, - 9.1, 3.2, - 6.1, 8.2, - 0.1, 7.2, - 0.1, 3.2, - 0.0, 1.2), - Vector_(6, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6), - model6); - - GaussianFactorGraph actSplit = splitFactor(initFactor.clone()); - GaussianFactorGraph expSplit; - - expSplit.add( - 0,Matrix_(2, 2, - 1.0, 2.0, - 0.0, 3.0), - 1,Matrix_(2, 2, - 1.0, 2.0, - 9.0, 3.0), - 2,Matrix_(2, 2, - 1.1, 2.2, - 9.1, 3.2), - Vector_(2, 0.1, 0.2), - model2); - expSplit.add( - 1,Matrix_(2, 2, - 6.0, 8.0, - 0.0, 7.0), - 2,Matrix_(2, 2, - 6.1, 8.2, - 0.1, 7.2), - Vector_(2, 0.3, 0.4), - model2); - expSplit.add( - 2,Matrix_(2, 2, - 0.1, 3.2, - 0.0, 1.2), - Vector_(2, 0.5, 0.6), - model2); - - EXPECT(assert_equal(expSplit, actSplit)); -} - -/* ************************************************************************* */ -TEST( testBayesTreeOperations, splitFactor3 ) { - - // Build upper-triangular system - JacobianFactor initFactor( - 0,Matrix_(4, 2, - 1.0, 2.0, - 0.0, 3.0, - 0.0, 0.0, - 0.0, 0.0), - 1,Matrix_(4, 2, - 1.0, 2.0, - 9.0, 3.0, - 6.0, 8.0, - 0.0, 7.0), - 2,Matrix_(4, 2, - 1.1, 2.2, - 9.1, 3.2, - 6.1, 8.2, - 0.1, 7.2), - Vector_(4, 0.1, 0.2, 0.3, 0.4), - model4); - - GaussianFactorGraph actSplit = splitFactor(initFactor.clone()); - GaussianFactorGraph expSplit; - - expSplit.add( - 0,Matrix_(2, 2, - 1.0, 2.0, - 0.0, 3.0), - 1,Matrix_(2, 2, - 1.0, 2.0, - 9.0, 3.0), - 2,Matrix_(2, 2, - 1.1, 2.2, - 9.1, 3.2), - Vector_(2, 0.1, 0.2), - model2); - expSplit.add( - 1,Matrix_(2, 2, - 6.0, 8.0, - 0.0, 7.0), - 2,Matrix_(2, 2, - 6.1, 8.2, - 0.1, 7.2), - Vector_(2, 0.3, 0.4), - model2); - - EXPECT(assert_equal(expSplit, actSplit)); -} - -/* ************************************************************************* */ -// Some numbers that should be consistent among all smoother tests - -//static double sigmax1 = 0.786153, /*sigmax2 = 1.0/1.47292,*/ sigmax3 = 0.671512, sigmax4 = -// 0.669534 /*, sigmax5 = sigmax3, sigmax6 = sigmax2*/, sigmax7 = sigmax1; - -/* ************************************************************************* */ -TEST( testBayesTreeOperations, liquefy ) { - using namespace example; - - // Create smoother with 7 nodes - Ordering ordering; - ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4); - GaussianFactorGraph smoother = createSmoother(7, ordering).first; - - // Create the Bayes tree - GaussianBayesTree bayesTree = *GaussianMultifrontalSolver(smoother).eliminate(); -// bayesTree.print("Full tree"); - - SharedDiagonal unit6 = noiseModel::Diagonal::Sigmas(Vector_(ones(6))); - SharedDiagonal unit4 = noiseModel::Diagonal::Sigmas(Vector_(ones(4))); - SharedDiagonal unit2 = noiseModel::Diagonal::Sigmas(Vector_(ones(2))); - - // Liquefy the tree back into a graph - { - GaussianFactorGraph actGraph = liquefy(bayesTree, false); // Doesn't split conditionals - GaussianFactorGraph expGraph; - - Matrix A12 = Matrix_(6, 2, - 1.73205081, 0.0, - 0.0, 1.73205081, - 0.0, 0.0, - 0.0, 0.0, - 0.0, 0.0, - 0.0, 0.0); - - Matrix A15 = Matrix_(6, 2, - -0.577350269, 0.0, - 0.0, -0.577350269, - 1.47196014, 0.0, - 0.0, 1.47196014, - 0.0, 0.0, - 0.0, 0.0); - - Matrix A16 = Matrix_(6, 2, - -0.577350269, 0.0, - 0.0, -0.577350269, - -0.226455407, 0.0, - 0.0, -0.226455407, - 1.49357599, 0.0, - 0.0, 1.49357599); - expGraph.add(2, A12, 5, A15, 6, A16, zeros(6,1), unit6); - - Matrix A21 = Matrix_(4, 2, - 1.73205081, 0.0, - 0.0, 1.73205081, - 0.0, 0.0, - 0.0, 0.0); - - Matrix A24 = Matrix_(4, 2, - -0.577350269, 0.0, - 0.0, -0.577350269, - 1.47196014, 0.0, - 0.0, 1.47196014); - - Matrix A26 = Matrix_(4, 2, - -0.577350269, 0.0, - 0.0, -0.577350269, - -0.226455407, 0.0, - 0.0, -0.226455407); - - expGraph.add(1, A21, 4, A24, 6, A26, zeros(4,1), unit4); - - Matrix A30 = Matrix_(2, 2, - 1.41421356, 0.0, - 0.0, 1.41421356); - - Matrix A34 = Matrix_(2, 2, - -0.707106781, 0.0, - 0.0, -0.707106781); - - expGraph.add(0, A30, 4, A34, zeros(2,1), unit2); - - Matrix A43 = Matrix_(2, 2, - 1.41421356, 0.0, - 0.0, 1.41421356); - Matrix A45 = Matrix_(2, 2, - -0.707106781, 0.0, - 0.0, -0.707106781); - - expGraph.add(3, A43, 5, A45, zeros(2,1), unit2); - - EXPECT(assert_equal(expGraph, actGraph, tol)); - } - - // Liquefy the tree back into a graph, splitting factors - { - GaussianFactorGraph actGraph = liquefy(bayesTree, true); - GaussianFactorGraph expGraph; - - // Conditional 1 - { - Matrix A12 = Matrix_(2, 2, - 1.73205081, 0.0, - 0.0, 1.73205081); - - Matrix A15 = Matrix_(2, 2, - -0.577350269, 0.0, - 0.0, -0.577350269); - - Matrix A16 = Matrix_(2, 2, - -0.577350269, 0.0, - 0.0, -0.577350269); - expGraph.add(2, A12, 5, A15, 6, A16, zeros(2,1), unit2); - } - - { - Matrix A15 = Matrix_(2, 2, - 1.47196014, 0.0, - 0.0, 1.47196014); - - Matrix A16 = Matrix_(2, 2, - -0.226455407, 0.0, - 0.0, -0.226455407); - expGraph.add(5, A15, 6, A16, zeros(2,1), unit2); - } - - { - Matrix A16 = Matrix_(2, 2, - 1.49357599, 0.0, - 0.0, 1.49357599); - expGraph.add(6, A16, zeros(2,1), unit2); - } - - // Conditional 2 - { - Matrix A21 = Matrix_(2, 2, - 1.73205081, 0.0, - 0.0, 1.73205081); - - Matrix A24 = Matrix_(2, 2, - -0.577350269, 0.0, - 0.0, -0.577350269); - - Matrix A26 = Matrix_(2, 2, - -0.577350269, 0.0, - 0.0, -0.577350269); - - expGraph.add(1, A21, 4, A24, 6, A26, zeros(2,1), unit2); - } - - { - Matrix A24 = Matrix_(2, 2, - 1.47196014, 0.0, - 0.0, 1.47196014); - - Matrix A26 = Matrix_(2, 2, - -0.226455407, 0.0, - 0.0, -0.226455407); - - expGraph.add(4, A24, 6, A26, zeros(2,1), unit2); - } - - // Conditional 3 - Matrix A30 = Matrix_(2, 2, - 1.41421356, 0.0, - 0.0, 1.41421356); - - Matrix A34 = Matrix_(2, 2, - -0.707106781, 0.0, - 0.0, -0.707106781); - - expGraph.add(0, A30, 4, A34, zeros(2,1), unit2); - - // Conditional 4 - Matrix A43 = Matrix_(2, 2, - 1.41421356, 0.0, - 0.0, 1.41421356); - Matrix A45 = Matrix_(2, 2, - -0.707106781, 0.0, - 0.0, -0.707106781); - - expGraph.add(3, A43, 5, A45, zeros(2,1), unit2); - - EXPECT(assert_equal(expGraph, actGraph, tol)); - } -} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ diff --git a/gtsam_unstable/linear/tests/testConditioning.cpp b/gtsam_unstable/linear/tests/testConditioning.cpp deleted file mode 100644 index 69646fbdc..000000000 --- a/gtsam_unstable/linear/tests/testConditioning.cpp +++ /dev/null @@ -1,250 +0,0 @@ -/** - * @file testConditioning.cpp - * - * @brief Experiments using backsubstitution for conditioning (not summarization, it turns out) - * - * @date Sep 3, 2012 - * @author Alex Cunningham - */ - -#include - -#include - -#include - -#include - -using namespace std; -using namespace boost::assign; -using namespace gtsam; - -const double tol = 1e-5; - -// Simple example -Matrix R = Matrix_(3,3, - 1.0,-2.0,-3.0, - 0.0, 3.0,-5.0, - 0.0, 0.0, 6.0); -Vector d = Vector_(3, - 0.1, 0.2, 0.3); -Vector x = Vector_(3, - 0.55, - 0.15, - 0.05); - -/* ************************************************************************* */ -TEST( testConditioning, directed_elimination_example ) { - - // create a 3-variable system from which to eliminate variables - // Scalar variables, pre-factorized into R,d system - // Use multifrontal representation - // Variables 0, 1, 2 - want to summarize out 1 - Vector expx = R.triangularView().solve(d); - EXPECT(assert_equal(x, expx, tol)); - EXPECT(assert_equal(Vector(R*x), d, tol)); - - // backsub-summarized version - Matrix Rprime = Matrix_(2,2, - 1.0,-3.0, - 0.0, 6.0); - Vector dprime = Vector_(2, - d(0) - R(0,1)*x(1), - d(2)); - Vector xprime = Vector_(2, - x(0), // Same solution, just smaller - x(2)); - EXPECT(assert_equal(Vector(Rprime*xprime), dprime, tol)); -} - -/* ************************************************************************* */ -TEST( testConditioning, directed_elimination_singlefrontal ) { - // Gaussian conditional with a single frontal variable, parent is to be removed - // Top row from above example - - Index root_key = 0, removed_key = 1, remaining_parent = 2; - Matrix R11 = Matrix_(1,1, 1.0), R22 = Matrix_(1,1, 3.0), S = Matrix_(1,1,-2.0), T = Matrix_(1,1,-3.0); - Vector d0 = d.segment(0,1), d1 = d.segment(1,1), sigmas = Vector_(1, 1.0); - GaussianConditional::shared_ptr initConditional(new - GaussianConditional(root_key, d0, R11, removed_key, S, remaining_parent, T, sigmas)); - - VectorValues solution; - solution.insert(0, x.segment(0,1)); - solution.insert(1, x.segment(1,1)); - solution.insert(2, x.segment(2,1)); - - std::set saved_indices; - saved_indices += root_key, remaining_parent; - - GaussianConditional::shared_ptr actSummarized = conditionDensity(initConditional, saved_indices, solution); - GaussianConditional::shared_ptr expSummarized(new - GaussianConditional(root_key, d0 - S*x(1), R11, remaining_parent, T, sigmas)); - - CHECK(actSummarized); - EXPECT(assert_equal(*expSummarized, *actSummarized, tol)); - - // Simple test of base case: if target index isn't present, return clone - GaussianConditional::shared_ptr actSummarizedSimple = conditionDensity(expSummarized, saved_indices, solution); - CHECK(actSummarizedSimple); - EXPECT(assert_equal(*expSummarized, *actSummarizedSimple, tol)); - - // case where frontal variable is to be eliminated - return null - GaussianConditional::shared_ptr removeFrontalInit(new - GaussianConditional(removed_key, d1, R22, remaining_parent, T, sigmas)); - GaussianConditional::shared_ptr actRemoveFrontal = conditionDensity(removeFrontalInit, saved_indices, solution); - EXPECT(!actRemoveFrontal); -} - -/* ************************************************************************* */ -TEST( testConditioning, directed_elimination_multifrontal ) { - // Use top two rows from the previous example - Index root_key = 0, removed_key = 1, remaining_parent = 2; - Matrix R11 = R.topLeftCorner(2,2), S = R.block(0,2,2,1), - Sprime = Matrix_(1,1,-2.0), R11prime = Matrix_(1,1, 1.0); - Vector d1 = d.segment(0,2), sigmas1 = Vector_(1, 1.0), sigmas2 = Vector_(2, 1.0, 1.0); - - - std::list > terms; - terms += make_pair(root_key, Matrix(R11.col(0))); - terms += make_pair(removed_key, Matrix(R11.col(1))); - terms += make_pair(remaining_parent, S); - GaussianConditional::shared_ptr initConditional(new GaussianConditional(terms, 2, d1, sigmas2)); - - VectorValues solution; - solution.insert(0, x.segment(0,1)); - solution.insert(1, x.segment(1,1)); - solution.insert(2, x.segment(2,1)); - - std::set saved_indices; - saved_indices += root_key, remaining_parent; - - GaussianConditional::shared_ptr actSummarized = conditionDensity(initConditional, saved_indices, solution); - GaussianConditional::shared_ptr expSummarized(new - GaussianConditional(root_key, d.segment(0,1) - Sprime*x(1), R11prime, remaining_parent, R.block(0,2,1,1), sigmas1)); - - CHECK(actSummarized); - EXPECT(assert_equal(*expSummarized, *actSummarized, tol)); -} - -/* ************************************************************************* */ -TEST( testConditioning, directed_elimination_multifrontal_multidim ) { - // use larger example, three frontal variables, dim = 2 each, two parents (one removed) - // Vars: 0, 1, 2, 3, 4; frontal: 0, 1, 2. parents: 3, 4; - // Remove 1, 3 - Matrix Rinit = Matrix_(6, 11, - 1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 1.0, 0.0, -1.0, 0.0, 0.1, - 0.0, 1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 1.0, 0.0, 1.0, 0.2, - 0.0, 0.0, 3.0, 0.0, 4.0, 0.0, 0.0,-1.0, 1.0, 0.0, 0.3, - 0.0, 0.0, 0.0, 4.0, 0.0, 4.0, 3.0, 2.0, 0.0, 9.0, 0.4, - 0.0, 0.0, 0.0, 0.0, 5.0, 0.0, 7.0, 0.0, 3.0, 0.0, 0.5, - 0.0, 0.0, 0.0, 0.0, 0.0, 4.0, 0.0, 8.0, 0.0, 6.0, 0.6); - - vector init_dims; init_dims += 2, 2, 2, 2, 2, 1; - GaussianConditional::rsd_type init_matrices(Rinit, init_dims.begin(), init_dims.end()); - Vector sigmas = ones(6); - vector init_keys; init_keys += 0, 1, 2, 3, 4; - GaussianConditional::shared_ptr initConditional(new - GaussianConditional(init_keys.begin(), init_keys.end(), 3, init_matrices, sigmas)); - - // Construct a solution vector - VectorValues solution; - solution.insert(0, zero(2)); - solution.insert(1, zero(2)); - solution.insert(2, zero(2)); - solution.insert(3, Vector_(2, 1.0, 2.0)); - solution.insert(4, Vector_(2, 3.0, 4.0)); - - initConditional->solveInPlace(solution); - - std::set saved_indices; - saved_indices += 0, 2, 4; - - GaussianConditional::shared_ptr actSummarized = conditionDensity(initConditional, saved_indices, solution); - CHECK(actSummarized); - - Matrix Rexp = Matrix_(4, 7, - 1.0, 0.0, 3.0, 0.0, -1.0, 0.0, 0.1, - 0.0, 1.0, 0.0, 3.0, 0.0, 1.0, 0.2, - 0.0, 0.0, 5.0, 0.0, 3.0, 0.0, 0.5, - 0.0, 0.0, 0.0, 4.0, 0.0, 6.0, 0.6); - - // Update rhs - Rexp.block(0, 6, 2, 1) -= Rinit.block(0, 2, 2, 2) * solution.at(1) + Rinit.block(0, 6, 2, 2) * solution.at(3); - Rexp.block(2, 6, 2, 1) -= Rinit.block(4, 6, 2, 2) * solution.at(3); - - vector exp_dims; exp_dims += 2, 2, 2, 1; - GaussianConditional::rsd_type exp_matrices(Rexp, exp_dims.begin(), exp_dims.end()); - Vector exp_sigmas = ones(4); - vector exp_keys; exp_keys += 0, 2, 4; - GaussianConditional expSummarized(exp_keys.begin(), exp_keys.end(), 2, exp_matrices, exp_sigmas); - - EXPECT(assert_equal(expSummarized, *actSummarized, tol)); -} - -/* ************************************************************************* */ -TEST( testConditioning, directed_elimination_multifrontal_multidim2 ) { - // Example from LinearAugmentedSystem - // 4 variables, last two in ordering kept - should be able to do this with no computation. - - vector init_dims; init_dims += 3, 3, 2, 2, 1; - - //Full initial conditional: density on [3] [4] [5] [6] - Matrix Rinit = Matrix_(10, 11, - 8.78422312, -0.0375455118, -0.0387376278, -5.059576, 0.0, 0.0, -0.0887200041, 0.00429643583, -0.130078263, 0.0193260727, 0.0, - 0.0, 8.46951839, 9.51456887, -0.0224291821, -5.24757636, 0.0, 0.0586258904, -0.173455825, 0.11090295, -0.330696013, 0.0, - 0.0, 0.0, 16.5539485, 0.00105159359, -2.35354497, -6.04085484, -0.0212095105, 0.0978729072, 0.00471054272, 0.0694956367, 0.0, - 0.0, 0.0, 0.0, 10.9015885, -0.0105694572, 0.000582715469, -0.0410535006, 0.00162772139, -0.0601433772, 0.0082824087,0.0, - 0.0, 0.0, 0.0, 0.0, 10.5531086, -1.34722553, 0.02438072, -0.0644224578, 0.0561372492, -0.148932792, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 21.4870439, -0.00443305851, 0.0234766354, 0.00484572411, 0.0101997356, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.73892865, 0.0242046766, -0.0459727048, 0.0445071938, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.61246954, 0.02287419, -0.102870789, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.04823446, -0.302033014, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.24068986, 0.0); - Vector dinit = Vector_(10, - -0.00186915, 0.00318554, 0.000592421, -0.000861, 0.00171528, 0.000274123, -0.0284011, 0.0275465, 0.0439795, -0.0222134); - Rinit.rightCols(1) = dinit; - Vector sigmas = ones(10); - - GaussianConditional::rsd_type init_matrices(Rinit, init_dims.begin(), init_dims.end()); - vector init_keys; init_keys += 3, 4, 5, 6; - GaussianConditional::shared_ptr initConditional(new - GaussianConditional(init_keys.begin(), init_keys.end(), 4, init_matrices, sigmas)); - - // Calculate a solution - VectorValues solution; - solution.insert(0, zero(3)); - solution.insert(1, zero(3)); - solution.insert(2, zero(3)); - solution.insert(3, zero(3)); - solution.insert(4, zero(3)); - solution.insert(5, zero(2)); - solution.insert(6, zero(2)); - - initConditional->solveInPlace(solution); - - // Perform summarization - std::set saved_indices; - saved_indices += 5, 6; - - GaussianConditional::shared_ptr actSummarized = conditionDensity(initConditional, saved_indices, solution); - CHECK(actSummarized); - - // Create expected value on [5], [6] - Matrix Rexp = Matrix_(4, 5, - 2.73892865, 0.0242046766, -0.0459727048, 0.0445071938, -0.0284011, - 0.0, 2.61246954, 0.02287419, -0.102870789, 0.0275465, - 0.0, 0.0, 2.04823446, -0.302033014, 0.0439795, - 0.0, 0.0, 0.0, 2.24068986, -0.0222134); - Vector expsigmas = ones(4); - - vector exp_dims; exp_dims += 2, 2, 1; - GaussianConditional::rsd_type exp_matrices(Rexp, exp_dims.begin(), exp_dims.end()); - vector exp_keys; exp_keys += 5, 6; - GaussianConditional expConditional(exp_keys.begin(), exp_keys.end(), 2, exp_matrices, expsigmas); - - EXPECT(assert_equal(expConditional, *actSummarized, tol)); -} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp index 167215db2..ed4b66616 100644 --- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp @@ -22,7 +22,7 @@ #include #include #include -#include +//#include #include namespace gtsam { @@ -55,17 +55,12 @@ FixedLagSmoother::Result BatchFixedLagSmoother::update(const NonlinearFactorGrap // Add the new variables to theta theta_.insert(newTheta); // Add new variables to the end of the ordering - std::vector dims; - dims.reserve(newTheta.size()); BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, newTheta) { ordering_.push_back(key_value.key); - dims.push_back(key_value.value.dim()); } // Augment Delta - delta_.append(dims); - for(size_t i = delta_.size() - dims.size(); i < delta_.size(); ++i) { - delta_[i].setZero(); - } + delta_.insert(newTheta.zeroVectors()); + // Add the new factors to the graph, updating the variable index insertFactors(newFactors); gttoc(augment_system); @@ -117,7 +112,7 @@ FixedLagSmoother::Result BatchFixedLagSmoother::update(const NonlinearFactorGrap /* ************************************************************************* */ void BatchFixedLagSmoother::insertFactors(const NonlinearFactorGraph& newFactors) { BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, newFactors) { - Index index; + Key index; // Insert the factor into an existing hole in the factor graph, if possible if(availableSlots_.size() > 0) { index = availableSlots_.front(); @@ -171,22 +166,11 @@ void BatchFixedLagSmoother::eraseKeys(const std::set& keys) { eraseKeyTimestampMap(keys); - // Permute the ordering such that the removed keys are at the end. - // This is a prerequisite for removing them from several structures - std::vector toBack; - BOOST_FOREACH(Key key, keys) { - toBack.push_back(ordering_.at(key)); - } - Permutation forwardPermutation = Permutation::PushToBack(toBack, ordering_.size()); - ordering_.permuteInPlace(forwardPermutation); - delta_.permuteInPlace(forwardPermutation); - // Remove marginalized keys from the ordering and delta - for(size_t i = 0; i < keys.size(); ++i) { - ordering_.pop_back(); - delta_.pop_back(); + BOOST_FOREACH(Key key, keys) { + ordering_.erase(std::find(ordering_.begin(), ordering_.end(), key)); + delta_.erase(key); } - } /* ************************************************************************* */ @@ -206,29 +190,8 @@ void BatchFixedLagSmoother::reorder(const std::set& marginalizeKeys) { std::cout << std::endl; } - // Calculate a variable index - VariableIndex variableIndex(*factors_.symbolic(ordering_), ordering_.size()); - // COLAMD groups will be used to place marginalize keys in Group 0, and everything else in Group 1 - int group0 = 0; - int group1 = marginalizeKeys.size() > 0 ? 1 : 0; - - // Initialize all variables to group1 - std::vector cmember(variableIndex.size(), group1); - - // Set all of the marginalizeKeys to Group0 - if(marginalizeKeys.size() > 0) { - BOOST_FOREACH(Key key, marginalizeKeys) { - cmember[ordering_.at(key)] = group0; - } - } - - // Generate the permutation - Permutation forwardPermutation = *inference::PermutationCOLAMD_(variableIndex, cmember); - - // Permute the ordering, variable index, and deltas - ordering_.permuteInPlace(forwardPermutation); - delta_.permuteInPlace(forwardPermutation); + ordering_ = Ordering::COLAMDConstrainedFirst(factors_, std::vector(marginalizeKeys.begin(), marginalizeKeys.end())); if(debug) { ordering_.print("New Ordering: "); @@ -264,7 +227,7 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() { double errorTol = parameters_.errorTol; // Create a Values that holds the current evaluation point - Values evalpoint = theta_.retract(delta_, ordering_); + Values evalpoint = theta_.retract(delta_); result.error = factors_.error(evalpoint); // check if we're already close enough @@ -288,7 +251,7 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() { gttic(optimizer_iteration); { // Linearize graph around the linearization point - GaussianFactorGraph linearFactorGraph = *factors_.linearize(theta_, ordering_); + GaussianFactorGraph linearFactorGraph = *factors_.linearize(theta_); // Keep increasing lambda until we make make progress while(true) { @@ -302,12 +265,12 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() { { // for each of the variables, add a prior at the current solution double sigma = 1.0 / std::sqrt(lambda); - for(size_t j=0; j 0)) { theta_.update(linearKeys_); BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, linearKeys_) { - Index index = ordering_.at(key_value.key); - delta_.at(index) = newDelta.at(index); + delta_.at(key_value.key) = newDelta.at(key_value.key); } } // Decrease lambda for next time @@ -406,12 +368,10 @@ void BatchFixedLagSmoother::marginalize(const std::set& marginalizeKeys) { // Identify all of the factors involving any marginalized variable. These must be removed. std::set removedFactorSlots; - VariableIndex variableIndex(*factors_.symbolic(ordering_), theta_.size()); + VariableIndex variableIndex(factors_); BOOST_FOREACH(Key key, marginalizeKeys) { - const FastList& slots = variableIndex[ordering_.at(key)]; - BOOST_FOREACH(size_t slot, slots) { - removedFactorSlots.insert(slot); - } + const FastList& slots = variableIndex[key]; + removedFactorSlots.insert(slots.begin(), slots.end()); } if(debug) { @@ -483,10 +443,10 @@ void BatchFixedLagSmoother::PrintSymbolicFactor(const NonlinearFactor::shared_pt } /* ************************************************************************* */ -void BatchFixedLagSmoother::PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor, const Ordering& ordering) { +void BatchFixedLagSmoother::PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor) { std::cout << "f("; - BOOST_FOREACH(Index index, factor->keys()) { - std::cout << " " << index << "[" << gtsam::DefaultKeyFormatter(ordering.key(index)) << "]"; + BOOST_FOREACH(Key key, factor->keys()) { + std::cout << " " << gtsam::DefaultKeyFormatter(key); } std::cout << " )" << std::endl; } @@ -500,102 +460,14 @@ void BatchFixedLagSmoother::PrintSymbolicGraph(const NonlinearFactorGraph& graph } /* ************************************************************************* */ -void BatchFixedLagSmoother::PrintSymbolicGraph(const GaussianFactorGraph& graph, const Ordering& ordering, const std::string& label) { +void BatchFixedLagSmoother::PrintSymbolicGraph(const GaussianFactorGraph& graph, const std::string& label) { std::cout << label << std::endl; BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, graph) { - PrintSymbolicFactor(factor, ordering); + PrintSymbolicFactor(factor); } } -/* ************************************************************************* */ -std::vector BatchFixedLagSmoother::EliminationForest::ComputeParents(const VariableIndex& structure) { - // Number of factors and variables - const size_t m = structure.nFactors(); - const size_t n = structure.size(); - static const Index none = std::numeric_limits::max(); - - // Allocate result parent vector and vector of last factor columns - std::vector parents(n, none); - std::vector prevCol(m, none); - - // for column j \in 1 to n do - for (Index j = 0; j < n; j++) { - // for row i \in Struct[A*j] do - BOOST_FOREACH(const size_t i, structure[j]) { - if (prevCol[i] != none) { - Index k = prevCol[i]; - // find root r of the current tree that contains k - Index r = k; - while (parents[r] != none) - r = parents[r]; - if (r != j) parents[r] = j; - } - prevCol[i] = j; - } - } - - return parents; -} - -/* ************************************************************************* */ -std::vector BatchFixedLagSmoother::EliminationForest::Create(const GaussianFactorGraph& factorGraph, const VariableIndex& structure) { - // Compute the tree structure - std::vector parents(ComputeParents(structure)); - - // Number of variables - const size_t n = structure.size(); - - static const Index none = std::numeric_limits::max(); - - // Create tree structure - std::vector trees(n); - for (Index k = 1; k <= n; k++) { - Index j = n - k; // Start at the last variable and loop down to 0 - trees[j].reset(new EliminationForest(j)); // Create a new node on this variable - if (parents[j] != none) // If this node has a parent, add it to the parent's children - trees[parents[j]]->add(trees[j]); - } - - // Hang factors in right places - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factorGraph) { - if(factor && factor->size() > 0) { - Index j = *std::min_element(factor->begin(), factor->end()); - if(j < structure.size()) - trees[j]->add(factor); - } - } - - return trees; -} - -/* ************************************************************************* */ -GaussianFactor::shared_ptr BatchFixedLagSmoother::EliminationForest::eliminateRecursive(GaussianFactorGraph::Eliminate function) { - - // Create the list of factors to be eliminated, initially empty, and reserve space - GaussianFactorGraph factors; - factors.reserve(this->factors_.size() + this->subTrees_.size()); - - // Add all factors associated with the current node - factors.push_back(this->factors_.begin(), this->factors_.end()); - - // for all subtrees, eliminate into Bayes net and a separator factor, added to [factors] - BOOST_FOREACH(const shared_ptr& child, subTrees_) - factors.push_back(child->eliminateRecursive(function)); - - // Combine all factors (from this node and from subtrees) into a joint factor - GaussianFactorGraph::EliminationResult eliminated(function(factors, 1)); - - return eliminated.second; -} - -/* ************************************************************************* */ -void BatchFixedLagSmoother::EliminationForest::removeChildrenIndices(std::set& indices, const BatchFixedLagSmoother::EliminationForest::shared_ptr& tree) { - BOOST_FOREACH(const EliminationForest::shared_ptr& child, tree->children()) { - indices.erase(child->key()); - removeChildrenIndices(indices, child); - } -} /* ************************************************************************* */ NonlinearFactorGraph BatchFixedLagSmoother::calculateMarginalFactors(const NonlinearFactorGraph& graph, const Values& theta, @@ -621,68 +493,20 @@ NonlinearFactorGraph BatchFixedLagSmoother::calculateMarginalFactors(const Nonli if(debug) std::cout << "BatchFixedLagSmoother::calculateMarginalFactors FINISH" << std::endl; return graph; } else { - // Create a subset of theta that only contains the required keys - Values values; - BOOST_FOREACH(Key key, allKeys) { - values.insert(key, theta.at(key)); - } - - // Calculate the ordering: [Others Root] - std::map constraints; - BOOST_FOREACH(Key key, marginalizeKeys) { - constraints[key] = 0; - } - BOOST_FOREACH(Key key, remainingKeys) { - constraints[key] = 1; - } - Ordering ordering = *graph.orderingCOLAMDConstrained(values, constraints); // Create the linear factor graph - GaussianFactorGraph linearFactorGraph = *graph.linearize(values, ordering); + GaussianFactorGraph linearFactorGraph = *graph.linearize(theta); + // .first is the eliminated Bayes tree, while .second is the remaining factor graph + GaussianFactorGraph marginalLinearFactors = *linearFactorGraph.eliminatePartialMultifrontal(std::vector(marginalizeKeys.begin(), marginalizeKeys.end())).second; - // Construct a variable index - VariableIndex variableIndex(linearFactorGraph, ordering.size()); - - // Construct an elimination tree to perform sparse elimination - std::vector forest( BatchFixedLagSmoother::EliminationForest::Create(linearFactorGraph, variableIndex) ); - - // This is a forest. Only the top-most node/index of each tree needs to be eliminated; all of the children will be eliminated automatically - // Find the subset of nodes/keys that must be eliminated - std::set indicesToEliminate; - BOOST_FOREACH(Key key, marginalizeKeys) { - indicesToEliminate.insert(ordering.at(key)); - } - BOOST_FOREACH(Key key, marginalizeKeys) { - EliminationForest::removeChildrenIndices(indicesToEliminate, forest.at(ordering.at(key))); - } - - if(debug) PrintKeySet(indicesToEliminate, "BatchFixedLagSmoother::calculateMarginalFactors Indices To Eliminate: "); - - // Eliminate each top-most key, returning a Gaussian Factor on some of the remaining variables - // Convert the marginal factors into Linear Container Factors + // Wrap in nonlinear container factors NonlinearFactorGraph marginalFactors; - BOOST_FOREACH(Index index, indicesToEliminate) { - GaussianFactor::shared_ptr gaussianFactor = forest.at(index)->eliminateRecursive(eliminateFunction); - if(gaussianFactor->size() > 0) { - LinearContainerFactor::shared_ptr marginalFactor(new LinearContainerFactor(gaussianFactor, ordering, values)); - marginalFactors.push_back(marginalFactor); - if(debug) { - std::cout << "BatchFixedLagSmoother::calculateMarginalFactors Marginal Factor: "; - PrintSymbolicFactor(marginalFactor); - } - } - } - - // Also add any remaining factors that were unaffected by marginalizing out the selected variables. - // These are part of the marginal on the remaining variables as well. - BOOST_FOREACH(Key key, remainingKeys) { - BOOST_FOREACH(const GaussianFactor::shared_ptr& gaussianFactor, forest.at(ordering.at(key))->factors()) { - LinearContainerFactor::shared_ptr marginalFactor(new LinearContainerFactor(gaussianFactor, ordering, values)); - marginalFactors.push_back(marginalFactor); - if(debug) { - std::cout << "BatchFixedLagSmoother::calculateMarginalFactors Remaining Factor: "; - PrintSymbolicFactor(marginalFactor); - } + marginalFactors.reserve(marginalLinearFactors.size()); + BOOST_FOREACH(const GaussianFactor::shared_ptr& gaussianFactor, marginalLinearFactors) { + marginalFactors += boost::make_shared(gaussianFactor, theta); + if(debug) { + std::cout << "BatchFixedLagSmoother::calculateMarginalFactors Marginal Factor: "; + PrintSymbolicFactor(marginalFactors.back()); } } diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h index b8597c79c..9b2903a14 100644 --- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h +++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h @@ -55,7 +55,7 @@ public: * a single variable is needed, it is faster to call calculateEstimate(const KEY&). */ Values calculateEstimate() const { - return theta_.retract(delta_, ordering_); + return theta_.retract(delta_); } /** Compute an estimate for a single variable using its incomplete linear delta computed @@ -66,8 +66,7 @@ public: */ template VALUE calculateEstimate(Key key) const { - const Index index = ordering_.at(key); - const Vector delta = delta_.at(index); + const Vector delta = delta_.at(key); return theta_.at(key).retract(delta); } @@ -104,7 +103,7 @@ public: protected: /** A typedef defining an Key-Factor mapping **/ - typedef std::map > FactorIndex; + typedef std::map > FactorIndex; /** The L-M optimization parameters **/ LevenbergMarquardtParams parameters_; @@ -155,57 +154,6 @@ protected: /** Marginalize out selected variables */ void marginalize(const std::set& marginalizableKeys); - - // A custom elimination tree that supports forests and partial elimination - class EliminationForest { - public: - typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class - - private: - typedef FastList Factors; - typedef FastList SubTrees; - typedef std::vector Conditionals; - - Index key_; ///< index associated with root - Factors factors_; ///< factors associated with root - SubTrees subTrees_; ///< sub-trees - - /** default constructor, private, as you should use Create below */ - EliminationForest(Index key = 0) : key_(key) {} - - /** - * Static internal function to build a vector of parent pointers using the - * algorithm of Gilbert et al., 2001, BIT. - */ - static std::vector ComputeParents(const VariableIndex& structure); - - /** add a factor, for Create use only */ - void add(const GaussianFactor::shared_ptr& factor) { factors_.push_back(factor); } - - /** add a subtree, for Create use only */ - void add(const shared_ptr& child) { subTrees_.push_back(child); } - - public: - - /** return the key associated with this tree node */ - Index key() const { return key_; } - - /** return the const reference of children */ - const SubTrees& children() const { return subTrees_; } - - /** return the const reference to the factors */ - const Factors& factors() const { return factors_; } - - /** Create an elimination tree from a factor graph */ - static std::vector Create(const GaussianFactorGraph& factorGraph, const VariableIndex& structure); - - /** Recursive routine that eliminates the factors arranged in an elimination tree */ - GaussianFactor::shared_ptr eliminateRecursive(GaussianFactorGraph::Eliminate function); - - /** Recursive function that helps find the top of each tree */ - static void removeChildrenIndices(std::set& indices, const EliminationForest::shared_ptr& tree); - }; - static NonlinearFactorGraph calculateMarginalFactors(const NonlinearFactorGraph& graph, const Values& theta, const std::set& marginalizeKeys, const GaussianFactorGraph::Eliminate& eliminateFunction); @@ -214,9 +162,9 @@ private: static void PrintKeySet(const std::set& keys, const std::string& label); static void PrintKeySet(const gtsam::FastSet& keys, const std::string& label); static void PrintSymbolicFactor(const NonlinearFactor::shared_ptr& factor); - static void PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor, const Ordering& ordering); + static void PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor); static void PrintSymbolicGraph(const NonlinearFactorGraph& graph, const std::string& label); - static void PrintSymbolicGraph(const GaussianFactorGraph& graph, const Ordering& ordering, const std::string& label); + static void PrintSymbolicGraph(const GaussianFactorGraph& graph, const std::string& label); }; // BatchFixedLagSmoother } /// namespace gtsam diff --git a/gtsam_unstable/nonlinear/CMakeLists.txt b/gtsam_unstable/nonlinear/CMakeLists.txt index e40ffdc09..9e0cb68e1 100644 --- a/gtsam_unstable/nonlinear/CMakeLists.txt +++ b/gtsam_unstable/nonlinear/CMakeLists.txt @@ -2,25 +2,5 @@ file(GLOB nonlinear_headers "*.h") install(FILES ${nonlinear_headers} DESTINATION include/gtsam_unstable/nonlinear) -# Components to link tests in this subfolder against -set(nonlinear_local_libs - nonlinear_unstable - nonlinear - linear - linear_unstable - inference - geometry - base - ccolamd -) - -set (nonlinear_full_libs - ${gtsam-default} - ${gtsam_unstable-default}) - -# Exclude tests that don't work -set (nonlinear_excluded_tests "") - # Add all tests -gtsam_add_subdir_tests(nonlinear_unstable "${nonlinear_local_libs}" "${nonlinear_full_libs}" "${nonlinear_excluded_tests}") -add_dependencies(check.unstable check.nonlinear_unstable) +add_subdirectory(tests) diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp index f0adef0c8..3bb9c7e44 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp @@ -61,7 +61,7 @@ void ConcurrentBatchFilter::PrintNonlinearFactorGraph(const NonlinearFactorGraph } /* ************************************************************************* */ -void ConcurrentBatchFilter::PrintLinearFactor(const GaussianFactor::shared_ptr& factor, const Ordering& ordering, +void ConcurrentBatchFilter::PrintLinearFactor(const GaussianFactor::shared_ptr& factor, const std::string& indent, const KeyFormatter& keyFormatter) { std::cout << indent; if(factor) { @@ -74,8 +74,8 @@ void ConcurrentBatchFilter::PrintLinearFactor(const GaussianFactor::shared_ptr& } else { std::cout << "g( "; } - BOOST_FOREACH(Index index, *factor) { - std::cout << keyFormatter(ordering.key(index)) << " "; + BOOST_FOREACH(Key key, *factor) { + std::cout << keyFormatter(key) << " "; } std::cout << ")" << std::endl; } else { @@ -84,35 +84,21 @@ void ConcurrentBatchFilter::PrintLinearFactor(const GaussianFactor::shared_ptr& } /* ************************************************************************* */ -void ConcurrentBatchFilter::PrintLinearFactorGraph(const GaussianFactorGraph& factors, const Ordering& ordering, +void ConcurrentBatchFilter::PrintLinearFactorGraph(const GaussianFactorGraph& factors, const std::string& indent, const std::string& title, const KeyFormatter& keyFormatter) { std::cout << indent << title << std::endl; BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) { - PrintLinearFactor(factor, ordering, indent + " ", keyFormatter); + PrintLinearFactor(factor, indent + " ", keyFormatter); } } -/* ************************************************************************* */ -template<> -void ConcurrentBatchFilter::PrintKeys(const Values& values, const std::string& indent, const std::string& title, const KeyFormatter& keyFormatter) { - FastList keys = values.keys(); - PrintKeys(keys, indent, title, keyFormatter); -} - -/* ************************************************************************* */ -template<> -void ConcurrentBatchFilter::PrintKeys(const NonlinearFactorGraph& graph, const std::string& indent, const std::string& title, const KeyFormatter& keyFormatter) { - FastSet keys = graph.keys(); - PrintKeys(keys, indent, title, keyFormatter); -} - /* ************************************************************************* */ void ConcurrentBatchFilter::print(const std::string& s, const KeyFormatter& keyFormatter) const { std::cout << s; PrintNonlinearFactorGraph(factors_, " ", "Factors:"); - PrintKeys(theta_, " ", "Values:"); + PrintKeys(theta_.keys(), " ", "Values:"); PrintNonlinearFactorGraph(smootherFactors_, " ", "Cached Smoother Factors:"); - PrintKeys(smootherValues_, " ", "Cached Smoother Values:"); + PrintKeys(smootherValues_.keys(), " ", "Cached Smoother Values:"); } /* ************************************************************************* */ @@ -149,25 +135,26 @@ ConcurrentBatchFilter::Result ConcurrentBatchFilter::update(const NonlinearFacto // Update all of the internal variables with the new information gttic(augment_system); + // Add the new variables to theta theta_.insert(newTheta); // Add new variables to the end of the ordering - std::vector dims; - dims.reserve(newTheta.size()); BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, newTheta) { ordering_.push_back(key_value.key); - dims.push_back(key_value.value.dim()); } // Augment Delta - delta_.append(dims); - for(size_t i = delta_.size() - dims.size(); i < delta_.size(); ++i) { - delta_[i].setZero(); - } + delta_.insert(newTheta.zeroVectors()); + // Add the new factors to the graph, updating the variable index result.newFactorsIndices = insertFactors(newFactors); - // Remove any user-specified factors from the graph - if(removeFactorIndices) + + if(removeFactorIndices){ + if(debug){ + std::cout << "ConcurrentBatchFilter::update removeFactorIndices " << std::endl; + } removeFactors(*removeFactorIndices); + } + gttoc(augment_system); if(debug) std::cout << "ConcurrentBatchFilter::update Reordering System ..." << std::endl; @@ -358,10 +345,7 @@ void ConcurrentBatchFilter::removeFactors(const std::vector& slots) { gttic(remove_factors); // For each factor slot to delete... - SymbolicFactorGraph factors; BOOST_FOREACH(size_t slot, slots) { - // Create a symbolic version for the variable index - factors.push_back(factors_.at(slot)->symbolic(ordering_)); // Remove the factor from the graph factors_.remove(slot); @@ -376,29 +360,13 @@ void ConcurrentBatchFilter::removeFactors(const std::vector& slots) { /* ************************************************************************* */ void ConcurrentBatchFilter::reorder(const boost::optional >& keysToMove) { - // Calculate the variable index - VariableIndex variableIndex(*factors_.symbolic(ordering_), ordering_.size()); - // COLAMD groups will be used to place marginalize keys in Group 0, and everything else in Group 1 - int group0 = 0; - int group1 = (keysToMove && (keysToMove->size() > 0) ) ? 1 : 0; - - // Initialize all variables to group1 - std::vector cmember(variableIndex.size(), group1); - - // Set all of the keysToMove to Group0 if(keysToMove && keysToMove->size() > 0) { - BOOST_FOREACH(Key key, *keysToMove) { - cmember[ordering_.at(key)] = group0; - } + ordering_ = Ordering::COLAMDConstrainedFirst(factors_, std::vector(keysToMove->begin(), keysToMove->end())); + }else{ + ordering_ = Ordering::COLAMD(factors_); } - // Generate the permutation - Permutation forwardPermutation = *inference::PermutationCOLAMD_(variableIndex, cmember); - - // Permute the ordering, variable index, and deltas - ordering_.permuteInPlace(forwardPermutation); - delta_.permuteInPlace(forwardPermutation); } /* ************************************************************************* */ @@ -426,7 +394,7 @@ void ConcurrentBatchFilter::optimize(const NonlinearFactorGraph& factors, Values double errorTol = parameters.errorTol; // Create a Values that holds the current evaluation point - Values evalpoint = theta.retract(delta, ordering); + Values evalpoint = theta.retract(delta); result.error = factors.error(evalpoint); // check if we're already close enough @@ -447,9 +415,9 @@ void ConcurrentBatchFilter::optimize(const NonlinearFactorGraph& factors, Values // Do next iteration gttic(optimizer_iteration); - { + // Linearize graph around the linearization point - GaussianFactorGraph linearFactorGraph = *factors.linearize(theta, ordering); + GaussianFactorGraph linearFactorGraph = *factors.linearize(theta); // Keep increasing lambda until we make make progress while(true) { @@ -461,24 +429,25 @@ void ConcurrentBatchFilter::optimize(const NonlinearFactorGraph& factors, Values GaussianFactorGraph dampedFactorGraph(linearFactorGraph); dampedFactorGraph.reserve(linearFactorGraph.size() + delta.size()); double sigma = 1.0 / std::sqrt(lambda); - { - // for each of the variables, add a prior at the current solution - for(size_t j=0; j 0) { theta.update(linearValues); BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, linearValues) { - Index index = ordering.at(key_value.key); - delta.at(index) = newDelta.at(index); + delta.at(key_value.key) = newDelta.at(key_value.key); } } + // Decrease lambda for next time lambda /= lambdaFactor; if(lambda < lambdaLowerBound) { @@ -526,7 +495,7 @@ void ConcurrentBatchFilter::optimize(const NonlinearFactorGraph& factors, Values } } } // end while - } + gttoc(optimizer_iteration); if(debug) { std::cout << "using lambda = " << lambda << std::endl; } @@ -562,13 +531,15 @@ void ConcurrentBatchFilter::moveSeparator(const FastList& keysToMove) { if(debug) { PrintKeys(keysToMove, "ConcurrentBatchFilter::moveSeparator ", "Keys To Move:", DefaultKeyFormatter); } + // Identify all of the new factors to be sent to the smoother (any factor involving keysToMove) std::vector removedFactorSlots; - VariableIndex variableIndex(*factors_.symbolic(ordering_), theta_.size()); + VariableIndex variableIndex(factors_); BOOST_FOREACH(Key key, keysToMove) { - const FastList& slots = variableIndex[ordering_.at(key)]; + const FastList& slots = variableIndex[key]; removedFactorSlots.insert(removedFactorSlots.end(), slots.begin(), slots.end()); } + // Sort and remove duplicates std::sort(removedFactorSlots.begin(), removedFactorSlots.end()); removedFactorSlots.erase(std::unique(removedFactorSlots.begin(), removedFactorSlots.end()), removedFactorSlots.end()); @@ -597,8 +568,8 @@ void ConcurrentBatchFilter::moveSeparator(const FastList& keysToMove) { if(debug) { PrintNonlinearFactorGraph(removedFactors, "ConcurrentBatchFilter::synchronize ", "Removed Factors:", DefaultKeyFormatter); PrintNonlinearFactorGraph(smootherShortcut_, "ConcurrentBatchFilter::synchronize ", "Old Shortcut:", DefaultKeyFormatter); - PrintKeys(smootherShortcut_, "ConcurrentBatchFilter::moveSeparator ", "Old Shortcut Keys:", DefaultKeyFormatter); - PrintKeys(separatorValues_, "ConcurrentBatchFilter::moveSeparator ", "Previous Separator Keys:", DefaultKeyFormatter); + PrintKeys(smootherShortcut_.keys(), "ConcurrentBatchFilter::moveSeparator ", "Old Shortcut Keys:", DefaultKeyFormatter); + PrintKeys(separatorValues_.keys(), "ConcurrentBatchFilter::moveSeparator ", "Previous Separator Keys:", DefaultKeyFormatter); } // Calculate the set of new separator keys: AffectedKeys + PreviousSeparatorKeys - KeysToMove @@ -677,22 +648,8 @@ void ConcurrentBatchFilter::moveSeparator(const FastList& keysToMove) { // Remove marginalized keys from values (and separator) BOOST_FOREACH(Key key, keysToMove) { theta_.erase(key); - } - - // Permute the ordering such that the removed keys are at the end. - // This is a prerequisite for removing them from several structures - std::vector toBack; - BOOST_FOREACH(Key key, keysToMove) { - toBack.push_back(ordering_.at(key)); - } - Permutation forwardPermutation = Permutation::PushToBack(toBack, ordering_.size()); - ordering_.permuteInPlace(forwardPermutation); - delta_.permuteInPlace(forwardPermutation); - - // Remove marginalized keys from the ordering and delta - for(size_t i = 0; i < keysToMove.size(); ++i) { - ordering_.pop_back(); - delta_.pop_back(); + ordering_.erase(std::find(ordering_.begin(), ordering_.end(), key)); + delta_.erase(key); } if(debug) std::cout << "ConcurrentBatchFilter::moveSeparator End" << std::endl; diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h index 96752bd4e..fce28b214 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h +++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h @@ -40,7 +40,13 @@ public: size_t lambdas; ///< The number of different L-M lambda factors that were tried during optimization size_t nonlinearVariables; ///< The number of variables that can be relinearized size_t linearVariables; ///< The number of variables that must keep a constant linearization point - std::vector newFactorsIndices; ///< The indices of the newly-added factors, in 1-to-1 correspondence with the factors passed in + + /** The indices of the newly-added factors, in 1-to-1 correspondence with the + * factors passed as \c newFactors update(). These indices may be + * used later to refer to the factors in order to remove them. + */ + std::vector newFactorsIndices; + double error; ///< The final factor graph error /// Constructor @@ -90,7 +96,7 @@ public: * If only a single variable is needed, it may be faster to call calculateEstimate(const KEY&). */ Values calculateEstimate() const { - return theta_.retract(delta_, ordering_); + return theta_.retract(delta_); } /** Compute the current best estimate of a single variable. This is generally faster than @@ -100,8 +106,7 @@ public: */ template VALUE calculateEstimate(Key key) const { - const Index index = ordering_.at(key); - const Vector delta = delta_.at(index); + const Vector delta = delta_.at(key); return theta_.at(key).retract(delta); } @@ -222,17 +227,16 @@ private: const std::string& indent = "", const std::string& title = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter); /** Print just the nonlinear keys in a linear factor */ - static void PrintLinearFactor(const GaussianFactor::shared_ptr& factor, const Ordering& ordering, + static void PrintLinearFactor(const GaussianFactor::shared_ptr& factor, const std::string& indent = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter); /** Print just the nonlinear keys in each linear factor for a whole Gaussian Factor Graph */ - static void PrintLinearFactorGraph(const GaussianFactorGraph& factors, const Ordering& ordering, + static void PrintLinearFactorGraph(const GaussianFactorGraph& factors, const std::string& indent = "", const std::string& title = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter); /** Print just the nonlinear keys contained inside a container */ template - static void PrintKeys(const Container& keys, const std::string& indent = "", - const std::string& title = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter); + static void PrintKeys(const Container& keys, const std::string& indent, const std::string& title, const KeyFormatter& keyFormatter = DefaultKeyFormatter); }; // ConcurrentBatchFilter diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp index bc3e591f9..8606538bf 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp @@ -46,7 +46,8 @@ bool ConcurrentBatchSmoother::equals(const ConcurrentSmoother& rhs, double tol) } /* ************************************************************************* */ -ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::update(const NonlinearFactorGraph& newFactors, const Values& newTheta) { +ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::update(const NonlinearFactorGraph& newFactors, const Values& newTheta, + const boost::optional< std::vector >& removeFactorIndices) { gttic(update); @@ -58,20 +59,20 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::update(const NonlinearF { // Add the new variables to theta theta_.insert(newTheta); + // Add new variables to the end of the ordering - std::vector dims; - dims.reserve(newTheta.size()); BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, newTheta) { ordering_.push_back(key_value.key); - dims.push_back(key_value.value.dim()); } + // Augment Delta - delta_.append(dims); - for(size_t i = delta_.size() - dims.size(); i < delta_.size(); ++i) { - delta_[i].setZero(); - } + delta_.insert(newTheta.zeroVectors()); + // Add the new factors to the graph, updating the variable index insertFactors(newFactors); + + if(removeFactorIndices) + removeFactors(*removeFactorIndices); } gttoc(augment_system); @@ -134,35 +135,29 @@ void ConcurrentBatchSmoother::synchronize(const NonlinearFactorGraph& smootherFa removeFactors(filterSummarizationSlots_); // Insert new linpoints into the values, augment the ordering, and store new dims to augment delta - std::vector dims; - dims.reserve(smootherValues.size() + separatorValues.size()); BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, smootherValues) { - Values::iterator iter = theta_.find(key_value.key); - if(iter == theta_.end()) { - theta_.insert(key_value.key, key_value.value); + std::pair iter_inserted = theta_.tryInsert(key_value.key, key_value.value); + if(iter_inserted.second) { + // If the insert succeeded ordering_.push_back(key_value.key); - dims.push_back(key_value.value.dim()); + delta_.insert(key_value.key, Vector::Zero(key_value.value.dim())); } else { - iter->value = key_value.value; + // If the element already existed in theta_ + iter_inserted.first->value = key_value.value; } } BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues) { - Values::iterator iter = theta_.find(key_value.key); - if(iter == theta_.end()) { - theta_.insert(key_value.key, key_value.value); + std::pair iter_inserted = theta_.tryInsert(key_value.key, key_value.value); + if(iter_inserted.second) { + // If the insert succeeded ordering_.push_back(key_value.key); - dims.push_back(key_value.value.dim()); + delta_.insert(key_value.key, Vector::Zero(key_value.value.dim())); } else { - iter->value = key_value.value; + // If the element already existed in theta_ + iter_inserted.first->value = key_value.value; } } - // Augment Delta - delta_.append(dims); - for(size_t i = delta_.size() - dims.size(); i < delta_.size(); ++i) { - delta_[i].setZero(); - } - // Insert the new smoother factors insertFactors(smootherFactors); @@ -217,10 +212,7 @@ void ConcurrentBatchSmoother::removeFactors(const std::vector& slots) { gttic(remove_factors); // For each factor slot to delete... - SymbolicFactorGraph factors; BOOST_FOREACH(size_t slot, slots) { - // Create a symbolic version for the variable index - factors.push_back(factors_.at(slot)->symbolic(ordering_)); // Remove the factor from the graph factors_.remove(slot); @@ -236,25 +228,11 @@ void ConcurrentBatchSmoother::removeFactors(const std::vector& slots) { void ConcurrentBatchSmoother::reorder() { // Recalculate the variable index - variableIndex_ = VariableIndex(*factors_.symbolic(ordering_)); + variableIndex_ = VariableIndex(factors_); - // Initialize all variables to group0 - std::vector cmember(variableIndex_.size(), 0); + FastList separatorKeys = separatorValues_.keys(); + ordering_ = Ordering::COLAMDConstrainedLast(variableIndex_, std::vector(separatorKeys.begin(), separatorKeys.end())); - // Set all of the separator keys to Group1 - if(separatorValues_.size() > 0) { - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { - cmember[ordering_.at(key_value.key)] = 1; - } - } - - // Generate the permutation - Permutation forwardPermutation = *inference::PermutationCOLAMD_(variableIndex_, cmember); - - // Permute the ordering, variable index, and deltas - ordering_.permuteInPlace(forwardPermutation); - variableIndex_.permuteInPlace(forwardPermutation); - delta_.permuteInPlace(forwardPermutation); } /* ************************************************************************* */ @@ -270,7 +248,7 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::optimize() { double lambda = parameters_.lambdaInitial; // Create a Values that holds the current evaluation point - Values evalpoint = theta_.retract(delta_, ordering_); + Values evalpoint = theta_.retract(delta_); result.error = factors_.error(evalpoint); if(result.error < parameters_.errorTol) { return result; @@ -286,7 +264,7 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::optimize() { gttic(optimizer_iteration); { // Linearize graph around the linearization point - GaussianFactorGraph linearFactorGraph = *factors_.linearize(theta_, ordering_); + GaussianFactorGraph linearFactorGraph = *factors_.linearize(theta_); // Keep increasing lambda until we make make progress while(true) { @@ -299,30 +277,31 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::optimize() { dampedFactorGraph.reserve(linearFactorGraph.size() + delta_.size()); { // for each of the variables, add a prior at the current solution - for(size_t j=0; j= LevenbergMarquardtParams::DAMPED) - dampedFactorGraph.print("damped"); + dampedFactorGraph.print("damped"); result.lambdas++; - + gttic(solve); // Solve Damped Gaussian Factor Graph - newDelta = GaussianJunctionTree(dampedFactorGraph).optimize(parameters_.getEliminationFunction()); + newDelta = dampedFactorGraph.optimize(ordering_, parameters_.getEliminationFunction()); // update the evalpoint with the new delta - evalpoint = theta_.retract(newDelta, ordering_); + evalpoint = theta_.retract(newDelta); gttoc(solve); if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) std::cout << "linear delta norm = " << newDelta.norm() << std::endl; if (lmVerbosity >= LevenbergMarquardtParams::TRYDELTA) - newDelta.print("delta"); + newDelta.print("delta"); // Evaluate the new error gttic(compute_error); @@ -330,7 +309,7 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::optimize() { gttoc(compute_error); if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) - std::cout << "next error = " << error << std::endl; + std::cout << "next error = " << error << std::endl; if(error < result.error) { // Keep this change @@ -344,10 +323,10 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::optimize() { if(separatorValues_.size() > 0) { theta_.update(separatorValues_); BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { - Index index = ordering_.at(key_value.key); - delta_.at(index) = newDelta.at(index); + delta_.at(key_value.key) = newDelta.at(key_value.key); } } + // Decrease lambda for next time lambda /= parameters_.lambdaFactor; // End this lambda search iteration @@ -371,7 +350,7 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::optimize() { std::cout << "using lambda = " << lambda << std::endl; result.iterations++; - } while(result.iterations < parameters_.maxIterations && + } while(result.iterations < (size_t)parameters_.maxIterations && !checkConvergence(parameters_.relativeErrorTol, parameters_.absoluteErrorTol, parameters_.errorTol, previousError, result.error, NonlinearOptimizerParams::SILENT)); return result; @@ -420,12 +399,12 @@ void ConcurrentBatchSmoother::PrintNonlinearFactor(const NonlinearFactor::shared } /* ************************************************************************* */ -void ConcurrentBatchSmoother::PrintLinearFactor(const GaussianFactor::shared_ptr& factor, const Ordering& ordering, const std::string& indent, const KeyFormatter& keyFormatter) { +void ConcurrentBatchSmoother::PrintLinearFactor(const GaussianFactor::shared_ptr& factor, const std::string& indent, const KeyFormatter& keyFormatter) { std::cout << indent; if(factor) { std::cout << "g( "; - BOOST_FOREACH(Index index, *factor) { - std::cout << keyFormatter(ordering.key(index)) << " "; + BOOST_FOREACH(Key key, *factor) { + std::cout << keyFormatter(key) << " "; } std::cout << ")" << std::endl; } else { diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h index 8c6ea7dda..d85109605 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h +++ b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h @@ -89,7 +89,7 @@ public: * If only a single variable is needed, it may be faster to call calculateEstimate(const KEY&). */ Values calculateEstimate() const { - return theta_.retract(delta_, ordering_); + return theta_.retract(delta_); } /** Compute the current best estimate of a single variable. This is generally faster than @@ -99,8 +99,7 @@ public: */ template VALUE calculateEstimate(Key key) const { - const Index index = ordering_.at(key); - const Vector delta = delta_.at(index); + const Vector delta = delta_.at(key); return theta_.at(key).retract(delta); } @@ -118,7 +117,8 @@ public: * in the smoother). There must not be any variables here that do not occur in newFactors, * and additionally, variables that were already in the system must not be included here. */ - virtual Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values()); + virtual Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(), + const boost::optional< std::vector >& removeFactorIndices = boost::none); /** * Perform any required operations before the synchronization process starts. @@ -196,7 +196,7 @@ private: const std::string& indent = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter); /** Print just the nonlinear keys in a linear factor */ - static void PrintLinearFactor(const GaussianFactor::shared_ptr& factor, const Ordering& ordering, + static void PrintLinearFactor(const GaussianFactor::shared_ptr& factor, const std::string& indent = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter); }; // ConcurrentBatchSmoother diff --git a/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp b/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp index 92a57bb5c..3b6b884f6 100644 --- a/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp @@ -50,151 +50,11 @@ void synchronize(ConcurrentFilter& filter, ConcurrentSmoother& smoother) { namespace internal { -// TODO: Remove this and replace with the standard Elimination Tree once GTSAM 3.0 is released and supports forests -// A custom elimination tree that supports forests and partial elimination -class EliminationForest { -public: - typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class - -private: - typedef FastList Factors; - typedef FastList SubTrees; - typedef std::vector Conditionals; - - Index key_; ///< index associated with root - Factors factors_; ///< factors associated with root - SubTrees subTrees_; ///< sub-trees - - /** default constructor, private, as you should use Create below */ - EliminationForest(Index key = 0) : key_(key) {} - - /** - * Static internal function to build a vector of parent pointers using the - * algorithm of Gilbert et al., 2001, BIT. - */ - static std::vector ComputeParents(const VariableIndex& structure); - - /** add a factor, for Create use only */ - void add(const GaussianFactor::shared_ptr& factor) { factors_.push_back(factor); } - - /** add a subtree, for Create use only */ - void add(const shared_ptr& child) { subTrees_.push_back(child); } - -public: - - /** return the key associated with this tree node */ - Index key() const { return key_; } - - /** return the const reference of children */ - const SubTrees& children() const { return subTrees_; } - - /** return the const reference to the factors */ - const Factors& factors() const { return factors_; } - - /** Create an elimination tree from a factor graph */ - static std::vector Create(const GaussianFactorGraph& factorGraph, const VariableIndex& structure); - - /** Recursive routine that eliminates the factors arranged in an elimination tree */ - GaussianFactor::shared_ptr eliminateRecursive(GaussianFactorGraph::Eliminate function); - - /** Recursive function that helps find the top of each tree */ - static void removeChildrenIndices(std::set& indices, const EliminationForest::shared_ptr& tree); -}; - -/* ************************************************************************* */ -std::vector EliminationForest::ComputeParents(const VariableIndex& structure) { - // Number of factors and variables - const size_t m = structure.nFactors(); - const size_t n = structure.size(); - - static const Index none = std::numeric_limits::max(); - - // Allocate result parent vector and vector of last factor columns - std::vector parents(n, none); - std::vector prevCol(m, none); - - // for column j \in 1 to n do - for (Index j = 0; j < n; j++) { - // for row i \in Struct[A*j] do - BOOST_FOREACH(const size_t i, structure[j]) { - if (prevCol[i] != none) { - Index k = prevCol[i]; - // find root r of the current tree that contains k - Index r = k; - while (parents[r] != none) - r = parents[r]; - if (r != j) parents[r] = j; - } - prevCol[i] = j; - } - } - - return parents; -} - -/* ************************************************************************* */ -std::vector EliminationForest::Create(const GaussianFactorGraph& factorGraph, const VariableIndex& structure) { - // Compute the tree structure - std::vector parents(ComputeParents(structure)); - - // Number of variables - const size_t n = structure.size(); - - static const Index none = std::numeric_limits::max(); - - // Create tree structure - std::vector trees(n); - for (Index k = 1; k <= n; k++) { - Index j = n - k; // Start at the last variable and loop down to 0 - trees[j].reset(new EliminationForest(j)); // Create a new node on this variable - if (parents[j] != none) // If this node has a parent, add it to the parent's children - trees[parents[j]]->add(trees[j]); - } - - // Hang factors in right places - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factorGraph) { - if(factor && factor->size() > 0) { - Index j = *std::min_element(factor->begin(), factor->end()); - if(j < structure.size()) - trees[j]->add(factor); - } - } - - return trees; -} - -/* ************************************************************************* */ -GaussianFactor::shared_ptr EliminationForest::eliminateRecursive(GaussianFactorGraph::Eliminate function) { - - // Create the list of factors to be eliminated, initially empty, and reserve space - GaussianFactorGraph factors; - factors.reserve(this->factors_.size() + this->subTrees_.size()); - - // Add all factors associated with the current node - factors.push_back(this->factors_.begin(), this->factors_.end()); - - // for all subtrees, eliminate into Bayes net and a separator factor, added to [factors] - BOOST_FOREACH(const shared_ptr& child, subTrees_) - factors.push_back(child->eliminateRecursive(function)); - - // Combine all factors (from this node and from subtrees) into a joint factor - GaussianFactorGraph::EliminationResult eliminated(function(factors, 1)); - - return eliminated.second; -} - -/* ************************************************************************* */ -void EliminationForest::removeChildrenIndices(std::set& indices, const EliminationForest::shared_ptr& tree) { - BOOST_FOREACH(const EliminationForest::shared_ptr& child, tree->children()) { - indices.erase(child->key()); - removeChildrenIndices(indices, child); - } -} - /* ************************************************************************* */ NonlinearFactorGraph calculateMarginalFactors(const NonlinearFactorGraph& graph, const Values& theta, const FastSet& remainingKeys, const GaussianFactorGraph::Eliminate& eliminateFunction) { + // Calculate the set of RootKeys = AllKeys \Intersect RemainingKeys FastSet rootKeys; FastSet allKeys(graph.keys()); @@ -208,59 +68,17 @@ NonlinearFactorGraph calculateMarginalFactors(const NonlinearFactorGraph& graph, // There are no keys to marginalize. Simply return the input factors return graph; } else { - // Create a subset of theta that only contains the required keys - Values values; - BOOST_FOREACH(Key key, allKeys) { - values.insert(key, theta.at(key)); - } - - // Calculate the ordering: [Others Root] - std::map constraints; - BOOST_FOREACH(Key key, marginalizeKeys) { - constraints[key] = 0; - } - BOOST_FOREACH(Key key, rootKeys) { - constraints[key] = 1; - } - Ordering ordering = *graph.orderingCOLAMDConstrained(values, constraints); - // Create the linear factor graph - GaussianFactorGraph linearFactorGraph = *graph.linearize(values, ordering); + GaussianFactorGraph linearFactorGraph = *graph.linearize(theta); + // .first is the eliminated Bayes tree, while .second is the remaining factor graph + GaussianFactorGraph marginalLinearFactors = *linearFactorGraph.eliminatePartialMultifrontal( + std::vector(marginalizeKeys.begin(), marginalizeKeys.end()), eliminateFunction).second; - // Construct a variable index - VariableIndex variableIndex(linearFactorGraph, ordering.size()); - - // Construct an elimination tree to perform sparse elimination - std::vector forest( EliminationForest::Create(linearFactorGraph, variableIndex) ); - - // This is a forest. Only the top-most node/index of each tree needs to be eliminated; all of the children will be eliminated automatically - // Find the subset of nodes/keys that must be eliminated - std::set indicesToEliminate; - BOOST_FOREACH(Key key, marginalizeKeys) { - indicesToEliminate.insert(ordering.at(key)); - } - BOOST_FOREACH(Key key, marginalizeKeys) { - EliminationForest::removeChildrenIndices(indicesToEliminate, forest.at(ordering.at(key))); - } - - // Eliminate each top-most key, returning a Gaussian Factor on some of the remaining variables - // Convert the marginal factors into Linear Container Factors + // Wrap in nonlinear container factors NonlinearFactorGraph marginalFactors; - BOOST_FOREACH(Index index, indicesToEliminate) { - GaussianFactor::shared_ptr gaussianFactor = forest.at(index)->eliminateRecursive(eliminateFunction); - if(gaussianFactor->size() > 0) { - LinearContainerFactor::shared_ptr marginalFactor(new LinearContainerFactor(gaussianFactor, ordering, values)); - marginalFactors.push_back(marginalFactor); - } - } - - // Also add any remaining factors that were unaffected by marginalizing out the selected variables. - // These are part of the marginal on the remaining variables as well. - BOOST_FOREACH(Key key, rootKeys) { - BOOST_FOREACH(const GaussianFactor::shared_ptr& gaussianFactor, forest.at(ordering.at(key))->factors()) { - LinearContainerFactor::shared_ptr marginalFactor(new LinearContainerFactor(gaussianFactor, ordering, values)); - marginalFactors.push_back(marginalFactor); - } + marginalFactors.reserve(marginalLinearFactors.size()); + BOOST_FOREACH(const GaussianFactor::shared_ptr& gaussianFactor, marginalLinearFactors) { + marginalFactors += boost::make_shared(gaussianFactor, theta); } return marginalFactors; diff --git a/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h b/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h index 32b6b5231..fb4b78fc2 100644 --- a/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h +++ b/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h @@ -23,6 +23,7 @@ #include #include #include +#include namespace gtsam { diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp index 7a3cfe888..9742aefd7 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp @@ -55,8 +55,9 @@ ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const No // Create the return result meta-data Result result; - // Remove any user-provided factors from iSAM2 + // We do not need to remove any factors at this time gtsam::FastVector removedFactors; + if(removeFactorIndices){ removedFactors.insert(removedFactors.end(), removeFactorIndices->begin(), removeFactorIndices->end()); } @@ -93,15 +94,14 @@ ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const No std::set markedKeys; BOOST_FOREACH(gtsam::Key key, *keysToMove) { if(isam2_.getLinearizationPoint().exists(key)) { - Index index = isam2_.getOrdering().at(key); - ISAM2Clique::shared_ptr clique = isam2_[index]; - GaussianConditional::const_iterator index_iter = clique->conditional()->begin(); - while(*index_iter != index) { - markedKeys.insert(isam2_.getOrdering().key(*index_iter)); - ++index_iter; + ISAM2Clique::shared_ptr clique = isam2_[key]; + GaussianConditional::const_iterator key_iter = clique->conditional()->begin(); + while(*key_iter != key) { + markedKeys.insert(*key_iter); + ++key_iter; } - BOOST_FOREACH(const gtsam::ISAM2Clique::shared_ptr& child, clique->children()) { - RecursiveMarkAffectedKeys(index, child, isam2_.getOrdering(), markedKeys); + BOOST_FOREACH(const gtsam::ISAM2Clique::shared_ptr& child, clique->children) { + RecursiveMarkAffectedKeys(key, child, markedKeys); } } } @@ -152,6 +152,8 @@ ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const No result.linearVariables = isam2_.getFixedVariables().size(); result.nonlinearVariables = isam2_.getLinearizationPoint().size() - result.linearVariables; result.newFactorsIndices = isam2Result.newFactorsIndices; + result.variablesReeliminated = isam2Result.variablesReeliminated; + result.variablesRelinearized = isam2Result.variablesRelinearized; // result.error = isam2_.getFactorsUnsafe().error(isam2_.calculateEstimate()); if(debug) std::cout << "ConcurrentIncrementalFilter::update End" << std::endl; @@ -205,7 +207,7 @@ void ConcurrentIncrementalFilter::synchronize(const NonlinearFactorGraph& smooth // Calculate the summarized factor on just the new separator keys NonlinearFactorGraph currentSmootherSummarization = internal::calculateMarginalFactors(graph, values, newSeparatorKeys, - isam2_.params().factorization == ISAM2Params::CHOLESKY ? EliminateCholesky : EliminateQR); + isam2_.params().getEliminationFunction()); // Remove the old factors on the separator and insert the new ones FastVector removeFactors(currentSmootherSummarizationSlots_.begin(), currentSmootherSummarizationSlots_.end()); @@ -261,20 +263,21 @@ void ConcurrentIncrementalFilter::postsync() { gttoc(postsync); } + /* ************************************************************************* */ -void ConcurrentIncrementalFilter::RecursiveMarkAffectedKeys(Index index, const ISAM2Clique::shared_ptr& clique, const Ordering& ordering, std::set& additionalKeys) { +void ConcurrentIncrementalFilter::RecursiveMarkAffectedKeys(const Key& key, const ISAM2Clique::shared_ptr& clique, std::set& additionalKeys) { // Check if the separator keys of the current clique contain the specified key - if(std::find(clique->conditional()->beginParents(), clique->conditional()->endParents(), index) != clique->conditional()->endParents()) { + if(std::find(clique->conditional()->beginParents(), clique->conditional()->endParents(), key) != clique->conditional()->endParents()) { // Mark the frontal keys of the current clique - BOOST_FOREACH(Index idx, clique->conditional()->frontals()) { - additionalKeys.insert(ordering.key(idx)); + BOOST_FOREACH(Key idx, clique->conditional()->frontals()) { + additionalKeys.insert(idx); } // Recursively mark all of the children - BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children()) { - RecursiveMarkAffectedKeys(index, child, ordering, additionalKeys); + BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children) { + RecursiveMarkAffectedKeys(key, child, additionalKeys); } } // If the key was not found in the separator/parents, then none of its children can have it either @@ -288,7 +291,7 @@ std::vector ConcurrentIncrementalFilter::FindAdjacentFactors(const ISAM2 std::vector removedFactorSlots; const VariableIndex& variableIndex = isam2.getVariableIndex(); BOOST_FOREACH(Key key, keys) { - const FastList& slots = variableIndex[isam2.getOrdering().at(key)]; + const FastList& slots = variableIndex[key]; removedFactorSlots.insert(removedFactorSlots.end(), slots.begin(), slots.end()); } @@ -329,7 +332,7 @@ void ConcurrentIncrementalFilter::updateShortcut(const NonlinearFactorGraph& rem values.insert(isam2_.getLinearizationPoint()); // Calculate the summarized factor on the shortcut keys smootherShortcut_ = internal::calculateMarginalFactors(graph, values, shortcutKeys, - isam2_.params().factorization == ISAM2Params::CHOLESKY ? EliminateCholesky : EliminateQR); + isam2_.params().getEliminationFunction()); } /* ************************************************************************* */ @@ -345,27 +348,23 @@ NonlinearFactorGraph ConcurrentIncrementalFilter::calculateFilterSummarization() // Find all cliques that contain any separator variables std::set separatorCliques; BOOST_FOREACH(Key key, separatorKeys) { - Index index = isam2_.getOrdering().at(key); - ISAM2Clique::shared_ptr clique = isam2_[index]; + ISAM2Clique::shared_ptr clique = isam2_[key]; separatorCliques.insert( clique ); } - // Create the set of clique keys - std::vector cliqueIndices; + // Create the set of clique keys LC: std::vector cliqueKeys; BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, separatorCliques) { - BOOST_FOREACH(Index index, clique->conditional()->frontals()) { - cliqueIndices.push_back(index); - cliqueKeys.push_back(isam2_.getOrdering().key(index)); + BOOST_FOREACH(Key key, clique->conditional()->frontals()) { + cliqueKeys.push_back(key); } } - std::sort(cliqueIndices.begin(), cliqueIndices.end()); std::sort(cliqueKeys.begin(), cliqueKeys.end()); // Gather all factors that involve only clique keys std::set cliqueFactorSlots; - BOOST_FOREACH(Index index, cliqueIndices) { - BOOST_FOREACH(size_t slot, isam2_.getVariableIndex()[index]) { + BOOST_FOREACH(Key key, cliqueKeys) { + BOOST_FOREACH(size_t slot, isam2_.getVariableIndex()[key]) { const NonlinearFactor::shared_ptr& factor = isam2_.getFactorsUnsafe().at(slot); if(factor) { std::set factorKeys(factor->begin(), factor->end()); @@ -391,7 +390,7 @@ NonlinearFactorGraph ConcurrentIncrementalFilter::calculateFilterSummarization() std::set childCliques; // Add all of the children BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, separatorCliques) { - childCliques.insert(clique->children().begin(), clique->children().end()); + childCliques.insert(clique->children.begin(), clique->children.end()); } // Remove any separator cliques that were added because they were children of other separator cliques BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, separatorCliques) { @@ -400,13 +399,13 @@ NonlinearFactorGraph ConcurrentIncrementalFilter::calculateFilterSummarization() // Augment the factor graph with cached factors from the children BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, childCliques) { - LinearContainerFactor::shared_ptr factor(new LinearContainerFactor(clique->cachedFactor(), isam2_.getOrdering(), isam2_.getLinearizationPoint())); + LinearContainerFactor::shared_ptr factor(new LinearContainerFactor(clique->cachedFactor(), isam2_.getLinearizationPoint())); graph.push_back( factor ); } // Calculate the marginal factors on the separator NonlinearFactorGraph filterSummarization = internal::calculateMarginalFactors(graph, isam2_.getLinearizationPoint(), separatorKeys, - isam2_.params().factorization == ISAM2Params::CHOLESKY ? EliminateCholesky : EliminateQR); + isam2_.params().getEliminationFunction()); return filterSummarization; } diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h index 6e5216642..4f2e1b0aa 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h @@ -30,6 +30,7 @@ namespace gtsam { class GTSAM_UNSTABLE_EXPORT ConcurrentIncrementalFilter : public virtual ConcurrentFilter { public: + typedef boost::shared_ptr shared_ptr; typedef ConcurrentFilter Base; ///< typedef for base class @@ -38,7 +39,15 @@ public: size_t iterations; ///< The number of optimizer iterations performed size_t nonlinearVariables; ///< The number of variables that can be relinearized size_t linearVariables; ///< The number of variables that must keep a constant linearization point - std::vector newFactorsIndices; ///< The indices of the newly-added factors, in 1-to-1 correspondence with the factors passed in + size_t variablesReeliminated; + size_t variablesRelinearized; + + /** The indices of the newly-added factors, in 1-to-1 correspondence with the + * factors passed as \c newFactors update(). These indices may be + * used later to refer to the factors in order to remove them. + */ + std::vector newFactorsIndices; + double error; ///< The final factor graph error /// Constructor @@ -69,13 +78,13 @@ public: } /** Access the current linearization point */ - const Values& getLinearizationPoint() const { - return isam2_.getLinearizationPoint(); + const ISAM2& getISAM2() const { + return isam2_; } - /** Access the current ordering */ - const Ordering& getOrdering() const { - return isam2_.getOrdering(); + /** Access the current linearization point */ + const Values& getLinearizationPoint() const { + return isam2_.getLinearizationPoint(); } /** Access the current set of deltas to the linearization point */ @@ -171,7 +180,7 @@ protected: private: /** Traverse the iSAM2 Bayes Tree, inserting all descendants of the provided index/key into 'additionalKeys' */ - static void RecursiveMarkAffectedKeys(Index index, const ISAM2Clique::shared_ptr& clique, const Ordering& ordering, std::set& additionalKeys); + static void RecursiveMarkAffectedKeys(const Key& key, const ISAM2Clique::shared_ptr& clique, std::set& additionalKeys); /** Find the set of iSAM2 factors adjacent to 'keys' */ static std::vector FindAdjacentFactors(const ISAM2& isam2, const FastList& keys, const std::vector& factorsToIgnore); diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp index 9a7072375..16a336695 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp @@ -44,13 +44,24 @@ bool ConcurrentIncrementalSmoother::equals(const ConcurrentSmoother& rhs, double } /* ************************************************************************* */ -ConcurrentIncrementalSmoother::Result ConcurrentIncrementalSmoother::update(const NonlinearFactorGraph& newFactors, const Values& newTheta) { +ConcurrentIncrementalSmoother::Result ConcurrentIncrementalSmoother::update(const NonlinearFactorGraph& newFactors, const Values& newTheta, + const boost::optional< std::vector >& removeFactorIndices) { gttic(update); // Create the return result meta-data Result result; + gtsam::FastVector removedFactors; + + if(removeFactorIndices){ + // Be very careful to this line + std::cout << "ConcurrentIncrementalSmoother::update removeFactorIndices - not implemented yet" << std::endl; + filterSummarizationSlots_.insert(filterSummarizationSlots_.end(), removeFactorIndices->begin(), removeFactorIndices->end()); + // before it was: + // removedFactors.insert(removedFactors.end(), removeFactorIndices->begin(), removeFactorIndices->end()); + } + // Constrain the separator keys to remain in the root // Also, mark the separator keys as fixed linearization points FastMap constrainedKeys; @@ -177,28 +188,24 @@ void ConcurrentIncrementalSmoother::updateSmootherSummarization() { // Find all cliques that contain any separator variables std::set separatorCliques; - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { - Index index = isam2_.getOrdering().at(key_value.key); - ISAM2Clique::shared_ptr clique = isam2_[index]; + BOOST_FOREACH(Key key, separatorValues_.keys()) { + ISAM2Clique::shared_ptr clique = isam2_[key]; separatorCliques.insert( clique ); } - // Create the set of clique keys - std::vector cliqueIndices; + // Create the set of clique keys LC: std::vector cliqueKeys; BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, separatorCliques) { - BOOST_FOREACH(Index index, clique->conditional()->frontals()) { - cliqueIndices.push_back(index); - cliqueKeys.push_back(isam2_.getOrdering().key(index)); + BOOST_FOREACH(Key key, clique->conditional()->frontals()) { + cliqueKeys.push_back(key); } } - std::sort(cliqueIndices.begin(), cliqueIndices.end()); std::sort(cliqueKeys.begin(), cliqueKeys.end()); // Gather all factors that involve only clique keys std::set cliqueFactorSlots; - BOOST_FOREACH(Index index, cliqueIndices) { - BOOST_FOREACH(size_t slot, isam2_.getVariableIndex()[index]) { + BOOST_FOREACH(Key key, cliqueKeys) { + BOOST_FOREACH(size_t slot, isam2_.getVariableIndex()[key]) { const NonlinearFactor::shared_ptr& factor = isam2_.getFactorsUnsafe().at(slot); if(factor) { std::set factorKeys(factor->begin(), factor->end()); @@ -224,7 +231,7 @@ void ConcurrentIncrementalSmoother::updateSmootherSummarization() { std::set childCliques; // Add all of the children BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, separatorCliques) { - childCliques.insert(clique->children().begin(), clique->children().end()); + childCliques.insert(clique->children.begin(), clique->children.end()); } // Remove any separator cliques that were added because they were children of other separator cliques BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, separatorCliques) { @@ -233,7 +240,7 @@ void ConcurrentIncrementalSmoother::updateSmootherSummarization() { // Augment the factor graph with cached factors from the children BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, childCliques) { - LinearContainerFactor::shared_ptr factor(new LinearContainerFactor(clique->cachedFactor(), isam2_.getOrdering(), isam2_.getLinearizationPoint())); + LinearContainerFactor::shared_ptr factor(new LinearContainerFactor(clique->cachedFactor(), isam2_.getLinearizationPoint())); graph.push_back( factor ); } @@ -245,7 +252,7 @@ void ConcurrentIncrementalSmoother::updateSmootherSummarization() { // Calculate the marginal factors on the separator smootherSummarization_ = internal::calculateMarginalFactors(graph, isam2_.getLinearizationPoint(), separatorKeys, - isam2_.params().factorization == ISAM2Params::CHOLESKY ? EliminateCholesky : EliminateQR); + isam2_.params().getEliminationFunction()); } /* ************************************************************************* */ diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h index 4178d45ff..5fe6effb2 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h @@ -72,11 +72,6 @@ public: return isam2_.getLinearizationPoint(); } - /** Access the current ordering */ - const Ordering& getOrdering() const { - return isam2_.getOrdering(); - } - /** Access the current set of deltas to the linearization point */ const VectorValues& getDelta() const { return isam2_.getDelta(); @@ -113,7 +108,8 @@ public: * in the smoother). There must not be any variables here that do not occur in newFactors, * and additionally, variables that were already in the system must not be included here. */ - Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values()); + Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(), + const boost::optional< std::vector >& removeFactorIndices = boost::none); /** * Perform any required operations before the synchronization process starts. diff --git a/gtsam_unstable/nonlinear/FixedLagSmoother.h b/gtsam_unstable/nonlinear/FixedLagSmoother.h index 89dba73ca..f212b38e7 100644 --- a/gtsam_unstable/nonlinear/FixedLagSmoother.h +++ b/gtsam_unstable/nonlinear/FixedLagSmoother.h @@ -21,9 +21,9 @@ #pragma once #include +#include #include #include -#include #include namespace gtsam { @@ -61,10 +61,10 @@ public: /** default constructor */ - FixedLagSmoother(double smootherLag = 0.0) : smootherLag_(smootherLag) { }; + FixedLagSmoother(double smootherLag = 0.0) : smootherLag_(smootherLag) { } /** destructor */ - virtual ~FixedLagSmoother() { }; + virtual ~FixedLagSmoother() { } /** Print the factor for debugging and testing (implementing Testable) */ virtual void print(const std::string& s = "FixedLagSmoother:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp index 2341fc449..b5556994c 100644 --- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp @@ -25,19 +25,19 @@ namespace gtsam { /* ************************************************************************* */ -void recursiveMarkAffectedKeys(const Index& index, const ISAM2Clique::shared_ptr& clique, const Ordering& ordering, std::set& additionalKeys) { +void recursiveMarkAffectedKeys(const Key& key, const ISAM2Clique::shared_ptr& clique, std::set& additionalKeys) { // Check if the separator keys of the current clique contain the specified key - if(std::find(clique->conditional()->beginParents(), clique->conditional()->endParents(), index) != clique->conditional()->endParents()) { + if(std::find(clique->conditional()->beginParents(), clique->conditional()->endParents(), key) != clique->conditional()->endParents()) { // Mark the frontal keys of the current clique - BOOST_FOREACH(Index i, clique->conditional()->frontals()) { - additionalKeys.insert(ordering.key(i)); + BOOST_FOREACH(Key i, clique->conditional()->frontals()) { + additionalKeys.insert(i); } // Recursively mark all of the children - BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children()) { - recursiveMarkAffectedKeys(index, child, ordering, additionalKeys); + BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children) { + recursiveMarkAffectedKeys(key, child, additionalKeys); } } // If the key was not found in the separator/parents, then none of its children can have it either @@ -106,10 +106,9 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update(const NonlinearFact // Mark additional keys between the marginalized keys and the leaves std::set additionalKeys; BOOST_FOREACH(gtsam::Key key, marginalizableKeys) { - gtsam::Index index = isam_.getOrdering().at(key); - gtsam::ISAM2Clique::shared_ptr clique = isam_[index]; - BOOST_FOREACH(const gtsam::ISAM2Clique::shared_ptr& child, clique->children()) { - recursiveMarkAffectedKeys(index, child, isam_.getOrdering(), additionalKeys); + gtsam::ISAM2Clique::shared_ptr clique = isam_[key]; + BOOST_FOREACH(const gtsam::ISAM2Clique::shared_ptr& child, clique->children) { + recursiveMarkAffectedKeys(key, child, additionalKeys); } } KeyList additionalMarkedKeys(additionalKeys.begin(), additionalKeys.end()); @@ -184,48 +183,52 @@ void IncrementalFixedLagSmoother::PrintKeySet(const std::set& keys, const s } /* ************************************************************************* */ -void IncrementalFixedLagSmoother::PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor, const gtsam::Ordering& ordering) { +void IncrementalFixedLagSmoother::PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor) { std::cout << "f("; - BOOST_FOREACH(Index index, factor->keys()) { - std::cout << " " << index << "[" << gtsam::DefaultKeyFormatter(ordering.key(index)) << "]"; + BOOST_FOREACH(gtsam::Key key, factor->keys()) { + std::cout << " " << gtsam::DefaultKeyFormatter(key); } std::cout << " )" << std::endl; } /* ************************************************************************* */ -void IncrementalFixedLagSmoother::PrintSymbolicGraph(const GaussianFactorGraph& graph, const gtsam::Ordering& ordering, const std::string& label) { +void IncrementalFixedLagSmoother::PrintSymbolicGraph(const GaussianFactorGraph& graph, const std::string& label) { std::cout << label << std::endl; BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, graph) { - PrintSymbolicFactor(factor, ordering); + PrintSymbolicFactor(factor); } } /* ************************************************************************* */ void IncrementalFixedLagSmoother::PrintSymbolicTree(const gtsam::ISAM2& isam, const std::string& label) { std::cout << label << std::endl; - if(isam.root()) - PrintSymbolicTreeHelper(isam.root(), isam.getOrdering()); + if(!isam.roots().empty()) + { + BOOST_FOREACH(const ISAM2::sharedClique& root, isam.roots()){ + PrintSymbolicTreeHelper(root); + } + } else std::cout << "{Empty Tree}" << std::endl; } /* ************************************************************************* */ -void IncrementalFixedLagSmoother::PrintSymbolicTreeHelper(const gtsam::ISAM2Clique::shared_ptr& clique, const gtsam::Ordering& ordering, const std::string indent) { +void IncrementalFixedLagSmoother::PrintSymbolicTreeHelper(const gtsam::ISAM2Clique::shared_ptr& clique, const std::string indent) { // Print the current clique std::cout << indent << "P( "; - BOOST_FOREACH(gtsam::Index index, clique->conditional()->frontals()) { - std::cout << gtsam::DefaultKeyFormatter(ordering.key(index)) << " "; + BOOST_FOREACH(gtsam::Key key, clique->conditional()->frontals()) { + std::cout << gtsam::DefaultKeyFormatter(key) << " "; } if(clique->conditional()->nrParents() > 0) std::cout << "| "; - BOOST_FOREACH(gtsam::Index index, clique->conditional()->parents()) { - std::cout << gtsam::DefaultKeyFormatter(ordering.key(index)) << " "; + BOOST_FOREACH(gtsam::Key key, clique->conditional()->parents()) { + std::cout << gtsam::DefaultKeyFormatter(key) << " "; } std::cout << ")" << std::endl; // Recursively print all of the children - BOOST_FOREACH(const gtsam::ISAM2Clique::shared_ptr& child, clique->children()) { - PrintSymbolicTreeHelper(child, ordering, indent+" "); + BOOST_FOREACH(const gtsam::ISAM2Clique::shared_ptr& child, clique->children) { + PrintSymbolicTreeHelper(child, indent+" "); } } diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h index 9434e3ecb..9f015ef19 100644 --- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h +++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h @@ -39,11 +39,11 @@ public: typedef boost::shared_ptr shared_ptr; /** default constructor */ - IncrementalFixedLagSmoother(double smootherLag = 0.0, const ISAM2Params& parameters = ISAM2Params()) : - FixedLagSmoother(smootherLag), isam_(parameters) { }; + IncrementalFixedLagSmoother(double smootherLag = 0.0, const ISAM2Params& parameters = ISAM2Params()) + : FixedLagSmoother(smootherLag), isam_(parameters) {} /** destructor */ - virtual ~IncrementalFixedLagSmoother() { }; + virtual ~IncrementalFixedLagSmoother() {} /** Print the factor for debugging and testing (implementing Testable) */ virtual void print(const std::string& s = "IncrementalFixedLagSmoother:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; @@ -89,11 +89,6 @@ public: return isam_.getLinearizationPoint(); } - /** Access the current ordering */ - const Ordering& getOrdering() const { - return isam_.getOrdering(); - } - /** Access the current set of deltas to the linearization point */ const VectorValues& getDelta() const { return isam_.getDelta(); @@ -113,10 +108,10 @@ protected: private: /** Private methods for printing debug information */ static void PrintKeySet(const std::set& keys, const std::string& label = "Keys:"); - static void PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor, const gtsam::Ordering& ordering); - static void PrintSymbolicGraph(const GaussianFactorGraph& graph, const gtsam::Ordering& ordering, const std::string& label = "Factor Graph:"); + static void PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor); + static void PrintSymbolicGraph(const GaussianFactorGraph& graph, const std::string& label = "Factor Graph:"); static void PrintSymbolicTree(const gtsam::ISAM2& isam, const std::string& label = "Bayes Tree:"); - static void PrintSymbolicTreeHelper(const gtsam::ISAM2Clique::shared_ptr& clique, const gtsam::Ordering& ordering, const std::string indent = ""); + static void PrintSymbolicTreeHelper(const gtsam::ISAM2Clique::shared_ptr& clique, const std::string indent = ""); }; // IncrementalFixedLagSmoother diff --git a/gtsam_unstable/nonlinear/LinearizedFactor.cpp b/gtsam_unstable/nonlinear/LinearizedFactor.cpp index eb5a83c79..f8dca75a4 100644 --- a/gtsam_unstable/nonlinear/LinearizedFactor.cpp +++ b/gtsam_unstable/nonlinear/LinearizedFactor.cpp @@ -22,38 +22,28 @@ namespace gtsam { /* ************************************************************************* */ -LinearizedGaussianFactor::LinearizedGaussianFactor(const GaussianFactor::shared_ptr& gaussian, const Ordering& ordering, const Values& lin_points) { +LinearizedGaussianFactor::LinearizedGaussianFactor( + const GaussianFactor::shared_ptr& gaussian, const Values& lin_points) +: NonlinearFactor(gaussian->keys()) +{ // Extract the keys and linearization points - BOOST_FOREACH(const Index& idx, gaussian->keys()) { - // find full symbol - if (idx < ordering.size()) { - Key key = ordering.key(idx); - - // extract linearization point - assert(lin_points.exists(key)); - this->lin_points_.insert(key, lin_points.at(key)); - - // store keys - this->keys_.push_back(key); - } else { - throw std::runtime_error("LinearizedGaussianFactor: could not find index in decoder!"); - } + BOOST_FOREACH(const Key& key, gaussian->keys()) { + // extract linearization point + assert(lin_points.exists(key)); + this->lin_points_.insert(key, lin_points.at(key)); } } - - /* ************************************************************************* */ -LinearizedJacobianFactor::LinearizedJacobianFactor() : Ab_(matrix_) { +// LinearizedJacobianFactor +/* ************************************************************************* */ +LinearizedJacobianFactor::LinearizedJacobianFactor() { } /* ************************************************************************* */ -LinearizedJacobianFactor::LinearizedJacobianFactor(const JacobianFactor::shared_ptr& jacobian, - const Ordering& ordering, const Values& lin_points) -: Base(jacobian, ordering, lin_points), Ab_(matrix_) { - - // Get the Ab matrix from the Jacobian factor, with any covariance baked in - AbMatrix fullMatrix = jacobian->matrix_augmented(true); +LinearizedJacobianFactor::LinearizedJacobianFactor( + const JacobianFactor::shared_ptr& jacobian, const Values& lin_points) +: Base(jacobian, lin_points) { // Create the dims array size_t *dims = (size_t *)alloca(sizeof(size_t) * (jacobian->size() + 1)); @@ -64,8 +54,9 @@ LinearizedJacobianFactor::LinearizedJacobianFactor(const JacobianFactor::shared_ dims[index] = 1; // Update the BlockInfo accessor - BlockAb Ab(fullMatrix, dims, dims+jacobian->size()+1); - Ab.swap(Ab_); + Ab_ = VerticalBlockMatrix(dims, dims+jacobian->size()+1, jacobian->rows()); + // Get the Ab matrix from the Jacobian factor, with any covariance baked in + Ab_.matrix() = jacobian->augmentedJacobian(); } /* ************************************************************************* */ @@ -110,12 +101,12 @@ double LinearizedJacobianFactor::error(const Values& c) const { /* ************************************************************************* */ boost::shared_ptr -LinearizedJacobianFactor::linearize(const Values& c, const Ordering& ordering) const { +LinearizedJacobianFactor::linearize(const Values& c) const { // Create the 'terms' data structure for the Jacobian constructor - std::vector > terms; + std::vector > terms; BOOST_FOREACH(Key key, keys()) { - terms.push_back(std::make_pair(ordering[key], this->A(key))); + terms.push_back(std::make_pair(key, this->A(key))); } // compute rhs @@ -140,22 +131,16 @@ Vector LinearizedJacobianFactor::error_vector(const Values& c) const { return errorVector; } - - - - - /* ************************************************************************* */ -LinearizedHessianFactor::LinearizedHessianFactor() : info_(matrix_) { +// LinearizedHessianFactor +/* ************************************************************************* */ +LinearizedHessianFactor::LinearizedHessianFactor() { } /* ************************************************************************* */ -LinearizedHessianFactor::LinearizedHessianFactor(const HessianFactor::shared_ptr& hessian, - const Ordering& ordering, const Values& lin_points) -: Base(hessian, ordering, lin_points), info_(matrix_) { - - // Copy the augmented matrix holding G, g, and f - Matrix fullMatrix = hessian->info(); +LinearizedHessianFactor::LinearizedHessianFactor( + const HessianFactor::shared_ptr& hessian, const Values& lin_points) +: Base(hessian, lin_points) { // Create the dims array size_t *dims = (size_t*)alloca(sizeof(size_t)*(hessian->size() + 1)); @@ -166,8 +151,9 @@ LinearizedHessianFactor::LinearizedHessianFactor(const HessianFactor::shared_ptr dims[index] = 1; // Update the BlockInfo accessor - BlockInfo infoMatrix(fullMatrix, dims, dims+hessian->size()+1); - infoMatrix.swap(info_); + info_ = SymmetricBlockMatrix(dims, dims+hessian->size()+1); + // Copy the augmented matrix holding G, g, and f + info_.full() = hessian->info(); } /* ************************************************************************* */ @@ -180,7 +166,7 @@ void LinearizedHessianFactor::print(const std::string& s, const KeyFormatter& ke std::cout << keyFormatter(key) << " "; std::cout << std::endl; - gtsam::print(Matrix(info_.range(0,info_.nBlocks(), 0,info_.nBlocks()).selfadjointView()), "Ab^T * Ab: "); + gtsam::print(Matrix(info_.full()), "Ab^T * Ab: "); lin_points_.print("Linearization Point: "); } @@ -191,9 +177,9 @@ bool LinearizedHessianFactor::equals(const NonlinearFactor& expected, double tol const This *e = dynamic_cast (&expected); if (e) { - Matrix thisMatrix = this->info_.full().selfadjointView(); + Matrix thisMatrix = this->info_.full(); thisMatrix(thisMatrix.rows()-1, thisMatrix.cols()-1) = 0.0; - Matrix rhsMatrix = e->info_.full().selfadjointView(); + Matrix rhsMatrix = e->info_.full(); rhsMatrix(rhsMatrix.rows()-1, rhsMatrix.cols()-1) = 0.0; return Base::equals(expected, tol) @@ -221,25 +207,14 @@ double LinearizedHessianFactor::error(const Values& c) const { // error 0.5*(f - 2*x'*g + x'*G*x) double f = constantTerm(); double xtg = dx.dot(linearTerm()); - double xGx = dx.transpose() * squaredTerm().selfadjointView() * dx; + double xGx = dx.transpose() * squaredTerm() * dx; return 0.5 * (f - 2.0 * xtg + xGx); } /* ************************************************************************* */ boost::shared_ptr -LinearizedHessianFactor::linearize(const Values& c, const Ordering& ordering) const { - - // Use the ordering to convert the keys into indices; - std::vector js; - BOOST_FOREACH(Key key, this->keys()){ - js.push_back(ordering.at(key)); - } - - // Make a copy of the info matrix - Matrix newMatrix; - SymmetricBlockView newInfo(newMatrix); - newInfo.assignNoalias(info_); +LinearizedHessianFactor::linearize(const Values& c) const { // Construct an error vector in key-order from the Values Vector dx = zero(dim()); @@ -254,28 +229,28 @@ LinearizedHessianFactor::linearize(const Values& c, const Ordering& ordering) co // f2 = f1 - 2*dx'*g1 + dx'*G1*dx //newInfo(this->size(), this->size())(0,0) += -2*dx.dot(linearTerm()) + dx.transpose() * squaredTerm().selfadjointView() * dx; - double f = constantTerm() - 2*dx.dot(linearTerm()) + dx.transpose() * squaredTerm().selfadjointView() * dx; + double f = constantTerm() - 2*dx.dot(linearTerm()) + dx.transpose() * squaredTerm() * dx; // g2 = g1 - G1*dx //newInfo.rangeColumn(0, this->size(), this->size(), 0) -= squaredTerm().selfadjointView() * dx; - Vector g = linearTerm() - squaredTerm().selfadjointView() * dx; + Vector g = linearTerm() - squaredTerm() * dx; std::vector gs; - for(size_t i = 0; i < info_.nBlocks()-1; ++i) { + for(DenseIndex i = 0; i < info_.nBlocks()-1; ++i) { gs.push_back(g.segment(info_.offset(i), info_.offset(i+1) - info_.offset(i))); } // G2 = G1 // Do Nothing std::vector Gs; - for(size_t i = 0; i < info_.nBlocks()-1; ++i) { - for(size_t j = i; j < info_.nBlocks()-1; ++j) { + for(DenseIndex i = 0; i < info_.nBlocks()-1; ++i) { + for(DenseIndex j = i; j < info_.nBlocks()-1; ++j) { Gs.push_back(info_(i,j)); } } // Create a Hessian Factor from the modified info matrix //return boost::shared_ptr(new HessianFactor(js, newInfo)); - return boost::shared_ptr(new HessianFactor(js, Gs, gs, f)); + return boost::shared_ptr(new HessianFactor(keys(), Gs, gs, f)); } } // \namespace aspn diff --git a/gtsam_unstable/nonlinear/LinearizedFactor.h b/gtsam_unstable/nonlinear/LinearizedFactor.h index 330df0c6a..7be219187 100644 --- a/gtsam_unstable/nonlinear/LinearizedFactor.h +++ b/gtsam_unstable/nonlinear/LinearizedFactor.h @@ -19,11 +19,9 @@ #include #include -#include #include #include #include -#include namespace gtsam { @@ -51,10 +49,9 @@ public: /** * @param gaussian: A jacobian or hessian factor - * @param ordering: The full ordering used to linearize this factor * @param lin_points: The linearization points for, at least, the variables used by this factor */ - LinearizedGaussianFactor(const GaussianFactor::shared_ptr& gaussian, const Ordering& ordering, const Values& lin_points); + LinearizedGaussianFactor(const GaussianFactor::shared_ptr& gaussian, const Values& lin_points); virtual ~LinearizedGaussianFactor() {}; @@ -87,12 +84,10 @@ public: /** shared pointer for convenience */ typedef boost::shared_ptr shared_ptr; - typedef Matrix AbMatrix; - typedef VerticalBlockView BlockAb; - typedef BlockAb::Block ABlock; - typedef BlockAb::constBlock constABlock; - typedef BlockAb::Column BVector; - typedef BlockAb::constColumn constBVector; + typedef VerticalBlockMatrix::Block ABlock; + typedef VerticalBlockMatrix::constBlock constABlock; + typedef VerticalBlockMatrix::Block::ColXpr BVector; + typedef VerticalBlockMatrix::constBlock::ConstColXpr constBVector; protected: @@ -101,8 +96,7 @@ protected: // KeyMatrixMap matrices_; // Vector b_; - AbMatrix matrix_; // the full matrix corresponding to the factor - BlockAb Ab_; // the block view of the full matrix + VerticalBlockMatrix Ab_; // the block view of the full matrix public: @@ -111,11 +105,9 @@ public: /** * @param jacobian: A jacobian factor - * @param ordering: The ordering used to linearize this factor * @param lin_points: The linearization points for, at least, the variables used by this factor */ - LinearizedJacobianFactor(const JacobianFactor::shared_ptr& jacobian, - const Ordering& ordering, const Values& lin_points); + LinearizedJacobianFactor(const JacobianFactor::shared_ptr& jacobian, const Values& lin_points); virtual ~LinearizedJacobianFactor() {} @@ -133,7 +125,7 @@ public: virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const; // access functions - const constBVector b() const { return Ab_.column(size(), 0); } + const constBVector b() const { return Ab_(size()).col(0); } const constABlock A() const { return Ab_.range(0, size()); }; const constABlock A(Key key) const { return Ab_(std::find(begin(), end(), key) - begin()); } @@ -148,8 +140,7 @@ public: * Reimplemented from NoiseModelFactor to directly copy out the * matrices and only update the RHS b with an updated residual */ - boost::shared_ptr linearize( - const Values& c, const Ordering& ordering) const; + boost::shared_ptr linearize(const Values& c) const; /** (A*x-b) */ Vector error_vector(const Values& c) const; @@ -161,7 +152,6 @@ private: void serialize(ARCHIVE & ar, const unsigned int version) { ar & boost::serialization::make_nvp("LinearizedJacobianFactor", boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(matrix_); ar & BOOST_SERIALIZATION_NVP(Ab_); } }; @@ -184,17 +174,15 @@ public: typedef boost::shared_ptr shared_ptr; /** hessian block data types */ - typedef Matrix InfoMatrix; ///< The full augmented Hessian - typedef SymmetricBlockView BlockInfo; ///< A blockwise view of the Hessian - typedef BlockInfo::Block Block; ///< A block from the Hessian matrix - typedef BlockInfo::constBlock constBlock; ///< A block from the Hessian matrix (const version) - typedef BlockInfo::Column Column; ///< A column containing the linear term h - typedef BlockInfo::constColumn constColumn; ///< A column containing the linear term h (const version) + typedef SymmetricBlockMatrix::Block Block; ///< A block from the Hessian matrix + typedef SymmetricBlockMatrix::constBlock constBlock; ///< A block from the Hessian matrix (const version) + typedef SymmetricBlockMatrix::Block::OffDiagonal::ColXpr Column; ///< A column containing the linear term h + typedef SymmetricBlockMatrix::constBlock::OffDiagonal::ColXpr constColumn; ///< A column containing the linear term h (const version) protected: - InfoMatrix matrix_; ///< The full augmented information matrix, s.t. the quadratic error is 0.5*[x -1]'*H*[x -1] - BlockInfo info_; ///< The block view of the full information matrix. + SymmetricBlockMatrix info_; ///< The block view of the full information matrix, s.t. the quadratic + /// error is 0.5*[x -1]'*H*[x -1] public: @@ -204,11 +192,9 @@ public: /** * Use this constructor with the ordering used to linearize the graph * @param hessian: A hessian factor - * @param ordering: The ordering used to linearize this factor * @param lin_points: The linearization points for, at least, the variables used by this factor */ - LinearizedHessianFactor(const HessianFactor::shared_ptr& hessian, - const Ordering& ordering, const Values& lin_points); + LinearizedHessianFactor(const HessianFactor::shared_ptr& hessian, const Values& lin_points); virtual ~LinearizedHessianFactor() {} @@ -234,11 +220,11 @@ public: * @param j Which block row to get, as an iterator pointing to the slot in this factor. You can * use, for example, begin() + 2 to get the 3rd variable in this factor. * @return The linear term \f$ g \f$ */ - constColumn linearTerm(const_iterator j) const { return info_.column(j - this->begin(), this->size(), 0); } + constColumn linearTerm(const_iterator j) const { return info_(j - this->begin(), this->size()).knownOffDiagonal().col(0); } /** Return the complete linear term \f$ g \f$ as described above. * @return The linear term \f$ g \f$ */ - constColumn linearTerm() const { return info_.rangeColumn(0, this->size(), this->size(), 0); }; + constColumn linearTerm() const { return info_.range(0, this->size(), this->size(), this->size() + 1).knownOffDiagonal().col(0); }; /** Return a view of the block at (j1,j2) of the upper-triangular part of the * squared term \f$ H \f$, no data is copied. See HessianFactor class documentation @@ -256,11 +242,11 @@ public: * See HessianFactor class documentation above to explain that only the * upper-triangular part of the information matrix is stored and returned by this function. */ - constBlock squaredTerm() const { return info_.range(0, this->size(), 0, this->size()); } + constBlock::SelfAdjointView squaredTerm() const { return info_.range(0, this->size(), 0, this->size()).selfadjointView(); } /** get the dimension of the factor (number of rows on linearization) */ - size_t dim() const { return matrix_.rows() - 1; }; + size_t dim() const { return info_.rows() - 1; }; /** Calculate the error of the factor */ double error(const Values& c) const; @@ -270,8 +256,7 @@ public: * Reimplemented from NoiseModelFactor to directly copy out the * matrices and only update the RHS b with an updated residual */ - boost::shared_ptr linearize( - const Values& c, const Ordering& ordering) const; + boost::shared_ptr linearize(const Values& c) const; private: /** Serialization function */ @@ -280,7 +265,6 @@ private: void serialize(ARCHIVE & ar, const unsigned int version) { ar & boost::serialization::make_nvp("LinearizedHessianFactor", boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(matrix_); ar & BOOST_SERIALIZATION_NVP(info_); } }; diff --git a/gtsam_unstable/nonlinear/sequentialSummarization.cpp b/gtsam_unstable/nonlinear/sequentialSummarization.cpp deleted file mode 100644 index fa0b58836..000000000 --- a/gtsam_unstable/nonlinear/sequentialSummarization.cpp +++ /dev/null @@ -1,35 +0,0 @@ -/** - * @file summarization.cpp - * - * @date Jun 22, 2012 - * @author Alex Cunningham - */ - -#include -#include - -#include - -using namespace std; - -namespace gtsam { - -/* ************************************************************************* */ -GaussianFactorGraph::shared_ptr summarizeGraphSequential( - const GaussianFactorGraph& full_graph, const std::vector& indices, bool useQR) { - GaussianSequentialSolver solver(full_graph, useQR); - return solver.jointFactorGraph(indices); -} - -/* ************************************************************************* */ -GaussianFactorGraph::shared_ptr summarizeGraphSequential( - const GaussianFactorGraph& full_graph, const Ordering& ordering, const KeySet& saved_keys, bool useQR) { - std::vector indices; - BOOST_FOREACH(const Key& k, saved_keys) - indices.push_back(ordering[k]); - return summarizeGraphSequential(full_graph, indices, useQR); -} - -/* ************************************************************************* */ -} // \namespace gtsam - diff --git a/gtsam_unstable/nonlinear/sequentialSummarization.h b/gtsam_unstable/nonlinear/sequentialSummarization.h deleted file mode 100644 index 4d783ebe1..000000000 --- a/gtsam_unstable/nonlinear/sequentialSummarization.h +++ /dev/null @@ -1,34 +0,0 @@ -/** - * @file summarization.h - * - * @brief Types and utility functions for summarization - * - * @date Jun 22, 2012 - * @author Alex Cunningham - */ - -#pragma once - -#include -#include - -#include -#include - -namespace gtsam { - -/** - * Summarization function that eliminates a set of variables (does not convert to Jacobians) - * NOTE: uses sequential solver - requires fully constrained system - */ -GaussianFactorGraph::shared_ptr GTSAM_UNSTABLE_EXPORT summarizeGraphSequential( - const GaussianFactorGraph& full_graph, const std::vector& indices, bool useQR = false); - -/** Summarization that also converts keys to indices */ -GaussianFactorGraph::shared_ptr GTSAM_UNSTABLE_EXPORT summarizeGraphSequential( - const GaussianFactorGraph& full_graph, const Ordering& ordering, - const KeySet& saved_keys, bool useQR = false); - -} // \namespace gtsam - - diff --git a/gtsam_unstable/nonlinear/tests/CMakeLists.txt b/gtsam_unstable/nonlinear/tests/CMakeLists.txt new file mode 100644 index 000000000..5b1fd07d4 --- /dev/null +++ b/gtsam_unstable/nonlinear/tests/CMakeLists.txt @@ -0,0 +1 @@ +gtsamAddTestsGlob(nonlinear_unstable "test*.cpp" "" "gtsam_unstable") diff --git a/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp index 3c7405da3..d1a4b8125 100644 --- a/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp @@ -18,28 +18,27 @@ #include #include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include #include -#include -#include -#include -#include -#include +#include +#include using namespace std; using namespace gtsam; + /* ************************************************************************* */ bool check_smoother(const NonlinearFactorGraph& fullgraph, const Values& fullinit, const BatchFixedLagSmoother& smoother, const Key& key) { - Ordering ordering = *fullgraph.orderingCOLAMD(fullinit); - GaussianFactorGraph linearized = *fullgraph.linearize(fullinit, ordering); - GaussianBayesNet gbn = *GaussianSequentialSolver(linearized).eliminate(); - VectorValues delta = optimize(gbn); - Values fullfinal = fullinit.retract(delta, ordering); + GaussianFactorGraph linearized = *fullgraph.linearize(fullinit); + VectorValues delta = linearized.optimize(); + Values fullfinal = fullinit.retract(delta); Point2 expected = fullfinal.at(key); Point2 actual = smoother.calculateEstimate(key); @@ -48,7 +47,7 @@ bool check_smoother(const NonlinearFactorGraph& fullgraph, const Values& fullini } /* ************************************************************************* */ -TEST_UNSAFE( BatchFixedLagSmoother, Example ) +TEST( BatchFixedLagSmoother, Example ) { // Test the BatchFixedLagSmoother in a pure linear environment. Thus, full optimization and // the BatchFixedLagSmoother should be identical (even with the linearized approximations at @@ -61,8 +60,8 @@ TEST_UNSAFE( BatchFixedLagSmoother, Example ) // SETDEBUG("BatchFixedLagSmoother calculateMarginalFactors", true); // Set up parameters - SharedDiagonal odometerNoise = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); - SharedDiagonal loopNoise = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); + SharedDiagonal odometerNoise = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.1)); + SharedDiagonal loopNoise = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.1)); // Create a Fixed-Lag Smoother typedef BatchFixedLagSmoother::KeyTimestampMap Timestamps; @@ -85,7 +84,7 @@ TEST_UNSAFE( BatchFixedLagSmoother, Example ) Values newValues; Timestamps newTimestamps; - newFactors.add(PriorFactor(key0, Point2(0.0, 0.0), odometerNoise)); + newFactors.push_back(PriorFactor(key0, Point2(0.0, 0.0), odometerNoise)); newValues.insert(key0, Point2(0.01, 0.01)); newTimestamps[key0] = 0.0; @@ -110,7 +109,7 @@ TEST_UNSAFE( BatchFixedLagSmoother, Example ) Values newValues; Timestamps newTimestamps; - newFactors.add(BetweenFactor(key1, key2, Point2(1.0, 0.0), odometerNoise)); + newFactors.push_back(BetweenFactor(key1, key2, Point2(1.0, 0.0), odometerNoise)); newValues.insert(key2, Point2(double(i)+0.1, -0.1)); newTimestamps[key2] = double(i); @@ -136,8 +135,8 @@ TEST_UNSAFE( BatchFixedLagSmoother, Example ) Values newValues; Timestamps newTimestamps; - newFactors.add(BetweenFactor(key1, key2, Point2(1.0, 0.0), odometerNoise)); - newFactors.add(BetweenFactor(Key(2), Key(5), Point2(3.5, 0.0), loopNoise)); + newFactors.push_back(BetweenFactor(key1, key2, Point2(1.0, 0.0), odometerNoise)); + newFactors.push_back(BetweenFactor(Key(2), Key(5), Point2(3.5, 0.0), loopNoise)); newValues.insert(key2, Point2(double(i)+0.1, -0.1)); newTimestamps[key2] = double(i); @@ -162,7 +161,7 @@ TEST_UNSAFE( BatchFixedLagSmoother, Example ) Values newValues; Timestamps newTimestamps; - newFactors.add(BetweenFactor(key1, key2, Point2(1.0, 0.0), odometerNoise)); + newFactors.push_back(BetweenFactor(key1, key2, Point2(1.0, 0.0), odometerNoise)); newValues.insert(key2, Point2(double(i)+0.1, -0.1)); newTimestamps[key2] = double(i); diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp index d3bbefd50..92332238a 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp @@ -23,10 +23,10 @@ #include #include #include -#include #include -#include -#include +#include +#include +#include #include #include #include @@ -39,13 +39,13 @@ namespace { // Set up initial pose, odometry difference, loop closure difference, and initialization errors const Pose3 poseInitial; -const Pose3 poseOdometry( Rot3::RzRyRx(Vector_(3, 0.05, 0.10, -0.75)), Point3(1.0, -0.25, 0.10) ); -const Pose3 poseError( Rot3::RzRyRx(Vector_(3, 0.01, 0.02, -0.1)), Point3(0.05, -0.05, 0.02) ); +const Pose3 poseOdometry( Rot3::RzRyRx((Vector(3) << 0.05, 0.10, -0.75)), Point3(1.0, -0.25, 0.10) ); +const Pose3 poseError( Rot3::RzRyRx((Vector(3) << 0.01, 0.02, -0.1)), Point3(0.05, -0.05, 0.02) ); // Set up noise models for the factors const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); -const SharedDiagonal noiseOdometery = noiseModel::Diagonal::Sigmas(Vector_(6, 0.1, 0.1, 0.1, 0.5, 0.5, 0.5)); -const SharedDiagonal noiseLoop = noiseModel::Diagonal::Sigmas(Vector_(6, 0.25, 0.25, 0.25, 1.0, 1.0, 1.0)); +const SharedDiagonal noiseOdometery = noiseModel::Diagonal::Sigmas((Vector(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5)); +const SharedDiagonal noiseLoop = noiseModel::Diagonal::Sigmas((Vector(6) << 0.25, 0.25, 0.25, 1.0, 1.0, 1.0)); /* ************************************************************************* */ Values BatchOptimize(const NonlinearFactorGraph& graph, const Values& theta, int maxIter = 100) { @@ -79,19 +79,13 @@ NonlinearFactorGraph CalculateMarginals(const NonlinearFactorGraph& factorGraph, ordering.push_back(key); } - GaussianFactorGraph linearGraph = *factorGraph.linearize(linPoint, ordering); + GaussianFactorGraph linearGraph = *factorGraph.linearize(linPoint); - // Create the set of marginalizable variables - std::vector linearIndices; - BOOST_FOREACH(Key key, keysToMarginalize) { - linearIndices.push_back(ordering.at(key)); - } - - std::pair marginal = linearGraph.eliminate(linearIndices, EliminateCholesky); + GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector(keysToMarginalize.begin(), keysToMarginalize.end()), EliminateCholesky).second; NonlinearFactorGraph LinearContainerForGaussianMarginals; - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, marginal.second) { - LinearContainerForGaussianMarginals.add(LinearContainerFactor(factor, ordering, linPoint)); + BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, marginal) { + LinearContainerForGaussianMarginals.push_back(LinearContainerFactor(factor, linPoint)); } return LinearContainerForGaussianMarginals; } @@ -140,8 +134,8 @@ TEST( ConcurrentBatchFilter, getFactors ) // Add some factors to the filter NonlinearFactorGraph newFactors1; - newFactors1.add(PriorFactor(1, poseInitial, noisePrior)); - newFactors1.add(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + newFactors1.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors1.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues1; newValues1.insert(1, Pose3()); newValues1.insert(2, newValues1.at(1).compose(poseOdometry)); @@ -157,8 +151,8 @@ TEST( ConcurrentBatchFilter, getFactors ) // Add some more factors to the filter NonlinearFactorGraph newFactors2; - newFactors2.add(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - newFactors2.add(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + newFactors2.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + newFactors2.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); Values newValues2; newValues2.insert(3, newValues1.at(2).compose(poseOdometry)); newValues2.insert(4, newValues2.at(3).compose(poseOdometry)); @@ -190,8 +184,8 @@ TEST( ConcurrentBatchFilter, getLinearizationPoint ) // Add some factors to the filter NonlinearFactorGraph newFactors1; - newFactors1.add(PriorFactor(1, poseInitial, noisePrior)); - newFactors1.add(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + newFactors1.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors1.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues1; newValues1.insert(1, Pose3()); newValues1.insert(2, newValues1.at(1).compose(poseOdometry)); @@ -207,8 +201,8 @@ TEST( ConcurrentBatchFilter, getLinearizationPoint ) // Add some more factors to the filter NonlinearFactorGraph newFactors2; - newFactors2.add(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - newFactors2.add(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + newFactors2.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + newFactors2.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); Values newValues2; newValues2.insert(3, newValues1.at(2).compose(poseOdometry)); newValues2.insert(4, newValues2.at(3).compose(poseOdometry)); @@ -252,8 +246,8 @@ TEST( ConcurrentBatchFilter, calculateEstimate ) // Add some factors to the filter NonlinearFactorGraph newFactors2; - newFactors2.add(PriorFactor(1, poseInitial, noisePrior)); - newFactors2.add(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + newFactors2.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors2.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues2; newValues2.insert(1, Pose3().compose(poseError)); newValues2.insert(2, newValues2.at(1).compose(poseOdometry).compose(poseError)); @@ -272,8 +266,8 @@ TEST( ConcurrentBatchFilter, calculateEstimate ) // Add some more factors to the filter NonlinearFactorGraph newFactors3; - newFactors3.add(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - newFactors3.add(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + newFactors3.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + newFactors3.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); Values newValues3; newValues3.insert(3, newValues2.at(2).compose(poseOdometry).compose(poseError)); newValues3.insert(4, newValues3.at(3).compose(poseOdometry).compose(poseError)); @@ -336,8 +330,8 @@ TEST( ConcurrentBatchFilter, update_multiple ) // Add some factors to the filter NonlinearFactorGraph newFactors2; - newFactors2.add(PriorFactor(1, poseInitial, noisePrior)); - newFactors2.add(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + newFactors2.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors2.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues2; newValues2.insert(1, Pose3().compose(poseError)); newValues2.insert(2, newValues2.at(1).compose(poseOdometry).compose(poseError)); @@ -356,8 +350,8 @@ TEST( ConcurrentBatchFilter, update_multiple ) // Add some more factors to the filter NonlinearFactorGraph newFactors3; - newFactors3.add(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - newFactors3.add(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + newFactors3.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + newFactors3.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); Values newValues3; newValues3.insert(3, newValues2.at(2).compose(poseOdometry).compose(poseError)); newValues3.insert(4, newValues3.at(3).compose(poseOdometry).compose(poseError)); @@ -386,10 +380,10 @@ TEST( ConcurrentBatchFilter, update_and_marginalize ) // Add some factors to the filter NonlinearFactorGraph newFactors; - newFactors.add(PriorFactor(1, poseInitial, noisePrior)); - newFactors.add(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - newFactors.add(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - newFactors.add(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); Values newValues; newValues.insert(1, Pose3().compose(poseError)); newValues.insert(2, newValues.at(1).compose(poseOdometry).compose(poseError)); @@ -409,9 +403,9 @@ TEST( ConcurrentBatchFilter, update_and_marginalize ) NonlinearFactorGraph partialGraph; - partialGraph.add(PriorFactor(1, poseInitial, noisePrior)); - partialGraph.add(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - partialGraph.add(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + partialGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); + partialGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + partialGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); Values partialValues; partialValues.insert(1, optimalValues.at(1)); @@ -425,12 +419,12 @@ TEST( ConcurrentBatchFilter, update_and_marginalize ) ordering.push_back(3); // Create the set of marginalizable variables - std::vector linearIndices; - linearIndices.push_back(ordering.at(1)); - linearIndices.push_back(ordering.at(2)); + std::vector linearIndices; + linearIndices.push_back(1); + linearIndices.push_back(2); - GaussianFactorGraph linearPartialGraph = *partialGraph.linearize(partialValues, ordering); - std::pair result = linearPartialGraph.eliminate(linearIndices, EliminateCholesky); + GaussianFactorGraph linearPartialGraph = *partialGraph.linearize(partialValues); + GaussianFactorGraph result = *linearPartialGraph.eliminatePartialMultifrontal(linearIndices, EliminateCholesky).second; NonlinearFactorGraph expectedGraph; @@ -443,10 +437,10 @@ TEST( ConcurrentBatchFilter, update_and_marginalize ) expectedGraph.push_back(NonlinearFactor::shared_ptr()); expectedGraph.push_back(NonlinearFactor::shared_ptr()); // ========================================================== - expectedGraph.add(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, result.second) { - expectedGraph.add(LinearContainerFactor(factor, ordering, partialValues)); + BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, result) { + expectedGraph.push_back(LinearContainerFactor(factor, partialValues)); } @@ -512,8 +506,8 @@ TEST( ConcurrentBatchFilter, synchronize_1 ) // Insert factors into the filter, but do not marginalize out any variables. // The synchronization should still be empty NonlinearFactorGraph newFactors; - newFactors.add(PriorFactor(1, poseInitial, noisePrior)); - newFactors.add(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues; newValues.insert(1, Pose3().compose(poseError)); newValues.insert(2, newValues.at(1).compose(poseOdometry).compose(poseError)); @@ -546,6 +540,7 @@ TEST( ConcurrentBatchFilter, synchronize_1 ) /* ************************************************************************* */ TEST( ConcurrentBatchFilter, synchronize_2 ) { + std::cout << "*********************** synchronize_2 ************************" << std::endl; // Create a set of optimizer parameters LevenbergMarquardtParams parameters; //parameters.maxIterations = 1; @@ -613,6 +608,7 @@ TEST( ConcurrentBatchFilter, synchronize_2 ) /* ************************************************************************* */ TEST( ConcurrentBatchFilter, synchronize_3 ) { + std::cout << "*********************** synchronize_3 ************************" << std::endl; // Create a set of optimizer parameters LevenbergMarquardtParams parameters; //parameters.maxIterations = 1; @@ -667,7 +663,7 @@ TEST( ConcurrentBatchFilter, synchronize_3 ) expectedFilterSeparatorValues.insert(2, optimalValues.at(2)); // ------------------------------------------------------------------------------ NonlinearFactorGraph partialGraph; - partialGraph.add(factor3); + partialGraph.push_back(factor3); Values partialValues; partialValues.insert(2, optimalValues.at(2)); @@ -752,7 +748,7 @@ TEST( ConcurrentBatchFilter, synchronize_4 ) expectedFilterSeparatorValues.insert(2, optimalValuesFilter.at(2)); // ------------------------------------------------------------------------------ NonlinearFactorGraph partialGraphFilter; - partialGraphFilter.add(factor3); + partialGraphFilter.push_back(factor3); Values partialValuesFilter; partialValuesFilter.insert(2, optimalValuesFilter.at(2)); @@ -790,6 +786,7 @@ TEST( ConcurrentBatchFilter, synchronize_4 ) /* ************************************************************************* */ TEST( ConcurrentBatchFilter, synchronize_5 ) { + std::cout << "*********************** synchronize_5 ************************" << std::endl; // Create a set of optimizer parameters LevenbergMarquardtParams parameters; parameters.maxIterations = 1; @@ -842,8 +839,8 @@ TEST( ConcurrentBatchFilter, synchronize_5 ) filter.postsync(); NonlinearFactorGraph expectedSmootherFactors; - expectedSmootherFactors.add(factor1); - expectedSmootherFactors.add(factor2); + expectedSmootherFactors.push_back(factor1); + expectedSmootherFactors.push_back(factor2); Values optimalValues = BatchOptimize(newFactors, newValues, 1); Values expectedSmootherValues; @@ -875,11 +872,11 @@ TEST( ConcurrentBatchFilter, synchronize_5 ) // currently the smoother contains factor 1 and 2 and node 1 and 2 NonlinearFactorGraph partialGraph; - partialGraph.add(factor1); - partialGraph.add(factor2); + partialGraph.push_back(factor1); + partialGraph.push_back(factor2); // we also assume that the smoother received an extra factor (e.g., a prior on 1) - partialGraph.add(factor1); + partialGraph.push_back(factor1); Values partialValues; // Batch optimization updates all linearization points but the ones of the separator @@ -908,8 +905,8 @@ TEST( ConcurrentBatchFilter, synchronize_5 ) filter.postsync(); NonlinearFactorGraph expectedSmootherFactors2; - expectedSmootherFactors2.add(factor3); - expectedSmootherFactors2.add(factor4); + expectedSmootherFactors2.push_back(factor3); + expectedSmootherFactors2.push_back(factor4); Values expectedSmootherValues2; expectedSmootherValues2.insert(2, optimalValues.at(2)); @@ -924,7 +921,7 @@ TEST( ConcurrentBatchFilter, synchronize_5 ) // ------------------------------------------------------------------------------ // This cannot be nonempty for the first call to synchronize NonlinearFactorGraph partialGraphFilter; - partialGraphFilter.add(factor5); + partialGraphFilter.push_back(factor5); Values partialValuesFilter; @@ -944,8 +941,8 @@ TEST( ConcurrentBatchFilter, synchronize_5 ) // Now we should check that the smooother summarization on the old separator is correctly propagated // on the new separator by the filter NonlinearFactorGraph partialGraphTransition; - partialGraphTransition.add(factor3); - partialGraphTransition.add(factor4); + partialGraphTransition.push_back(factor3); + partialGraphTransition.push_back(factor4); partialGraphTransition.push_back(smootherSummarization2); Values partialValuesTransition; @@ -967,7 +964,7 @@ TEST( ConcurrentBatchFilter, synchronize_5 ) expectedFilterGraph.push_back(factorEmpty); expectedFilterGraph.push_back(factorEmpty); expectedFilterGraph.push_back(factorEmpty); - expectedFilterGraph.add(factor5); + expectedFilterGraph.push_back(factor5); expectedFilterGraph.push_back(CalculateMarginals(partialGraphTransition, partialValuesTransition, keysToMarginalize3)); NonlinearFactorGraph actualFilterGraph; @@ -987,10 +984,10 @@ TEST( ConcurrentBatchFilter, CalculateMarginals_1 ) NonlinearFactor::shared_ptr factor4(new BetweenFactor(2, 3, poseOdometry, noiseOdometery)); NonlinearFactorGraph factorGraph; - factorGraph.add(factor1); - factorGraph.add(factor2); - factorGraph.add(factor3); - factorGraph.add(factor4); + factorGraph.push_back(factor1); + factorGraph.push_back(factor2); + factorGraph.push_back(factor3); + factorGraph.push_back(factor4); Values newValues; Pose3 value1 = Pose3().compose(poseError); @@ -1008,17 +1005,17 @@ TEST( ConcurrentBatchFilter, CalculateMarginals_1 ) ordering.push_back(2); ordering.push_back(3); - GaussianFactorGraph linearFactorGraph = *factorGraph.linearize(newValues, ordering); + GaussianFactorGraph linearFactorGraph = *factorGraph.linearize(newValues); // Create the set of marginalizable variables - std::vector linearIndices; - linearIndices.push_back(ordering.at(1)); + std::vector linearIndices; + linearIndices.push_back(1); - std::pair result = linearFactorGraph.eliminate(linearIndices, EliminateCholesky); + GaussianFactorGraph result = *linearFactorGraph.eliminatePartialMultifrontal(linearIndices, EliminateCholesky).second; NonlinearFactorGraph expectedMarginals; - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, result.second) { - expectedMarginals.add(LinearContainerFactor(factor, ordering, newValues)); + BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, result) { + expectedMarginals.push_back(LinearContainerFactor(factor, newValues)); } @@ -1041,10 +1038,10 @@ TEST( ConcurrentBatchFilter, CalculateMarginals_2 ) NonlinearFactor::shared_ptr factor4(new BetweenFactor(2, 3, poseOdometry, noiseOdometery)); NonlinearFactorGraph factorGraph; - factorGraph.add(factor1); - factorGraph.add(factor2); - factorGraph.add(factor3); - factorGraph.add(factor4); + factorGraph.push_back(factor1); + factorGraph.push_back(factor2); + factorGraph.push_back(factor3); + factorGraph.push_back(factor4); Values newValues; Pose3 value1 = Pose3().compose(poseError); @@ -1062,18 +1059,18 @@ TEST( ConcurrentBatchFilter, CalculateMarginals_2 ) ordering.push_back(2); ordering.push_back(3); - GaussianFactorGraph linearFactorGraph = *factorGraph.linearize(newValues, ordering); + GaussianFactorGraph linearFactorGraph = *factorGraph.linearize(newValues); // Create the set of marginalizable variables - std::vector linearIndices; - linearIndices.push_back(ordering.at(1)); - linearIndices.push_back(ordering.at(2)); + std::vector linearIndices; + linearIndices.push_back(1); + linearIndices.push_back(2); - std::pair result = linearFactorGraph.eliminate(linearIndices, EliminateCholesky); + GaussianFactorGraph result = *linearFactorGraph.eliminatePartialMultifrontal(linearIndices, EliminateCholesky).second; NonlinearFactorGraph expectedMarginals; - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, result.second) { - expectedMarginals.add(LinearContainerFactor(factor, ordering, newValues)); + BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, result) { + expectedMarginals.push_back(LinearContainerFactor(factor, newValues)); } @@ -1087,6 +1084,258 @@ TEST( ConcurrentBatchFilter, CalculateMarginals_2 ) CHECK(assert_equal(expectedMarginals, actualMarginals, 1e-6)); } +///* ************************************************************************* */ +TEST( ConcurrentBatchFilter, removeFactors_topology_1 ) +{ + std::cout << "*********************** removeFactors_topology_1 ************************" << std::endl; + + // Create a set of optimizer parameters + LevenbergMarquardtParams parameters; + ConcurrentBatchFilter filter(parameters); + + // Add some factors to the filter + NonlinearFactorGraph newFactors; + newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + + Values newValues; + newValues.insert(1, Pose3().compose(poseError)); + newValues.insert(2, newValues.at(1).compose(poseOdometry).compose(poseError)); + newValues.insert(3, newValues.at(2).compose(poseOdometry).compose(poseError)); + newValues.insert(4, newValues.at(3).compose(poseOdometry).compose(poseError)); + + // Specify a subset of variables to marginalize/move to the smoother + FastList keysToMove; + + // Update the filter: add all factors + filter.update(newFactors, newValues, keysToMove); + + // factor we want to remove + // NOTE: we can remove factors, paying attention that the remaining graph remains connected + // we remove a single factor, the number 1, which is a BetweenFactor(1, 2, poseOdometry, noiseOdometery); + std::vector removeFactorIndices(1,1); + + // Add no factors to the filter (we only want to test the removal) + NonlinearFactorGraph noFactors; + Values noValues; + filter.update(noFactors, noValues, keysToMove, removeFactorIndices); + + NonlinearFactorGraph actualGraph = filter.getFactors(); + + NonlinearFactorGraph expectedGraph; + expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); + // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + // we should add an empty one, so that the ordering and labeling of the factors is preserved + expectedGraph.push_back(NonlinearFactor::shared_ptr()); + expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + + CHECK(assert_equal(expectedGraph, actualGraph, 1e-6)); +} + +///* ************************************************************************* */ +TEST( ConcurrentBatchFilter, removeFactors_topology_2 ) +{ + std::cout << "*********************** removeFactors_topology_2 ************************" << std::endl; + // we try removing the last factor + + + // Create a set of optimizer parameters + LevenbergMarquardtParams parameters; + ConcurrentBatchFilter filter(parameters); + + // Add some factors to the filter + NonlinearFactorGraph newFactors; + newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + + Values newValues; + newValues.insert(1, Pose3().compose(poseError)); + newValues.insert(2, newValues.at(1).compose(poseOdometry).compose(poseError)); + newValues.insert(3, newValues.at(2).compose(poseOdometry).compose(poseError)); + newValues.insert(4, newValues.at(3).compose(poseOdometry).compose(poseError)); + + // Specify a subset of variables to marginalize/move to the smoother + FastList keysToMove; + + // Update the filter: add all factors + filter.update(newFactors, newValues, keysToMove); + + // factor we want to remove + // NOTE: we can remove factors, paying attention that the remaining graph remains connected + // we remove a single factor, the number 1, which is a BetweenFactor(1, 2, poseOdometry, noiseOdometery); + std::vector removeFactorIndices(1,4); + + // Add no factors to the filter (we only want to test the removal) + NonlinearFactorGraph noFactors; + Values noValues; + filter.update(noFactors, noValues, keysToMove, removeFactorIndices); + + NonlinearFactorGraph actualGraph = filter.getFactors(); + + NonlinearFactorGraph expectedGraph; + expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); + expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + // we should add an empty one, so that the ordering and labeling of the factors is preserved + expectedGraph.push_back(NonlinearFactor::shared_ptr()); + + CHECK(assert_equal(expectedGraph, actualGraph, 1e-6)); +} + + +///* ************************************************************************* */ +TEST( ConcurrentBatchFilter, removeFactors_topology_3 ) +{ + std::cout << "*********************** removeFactors_topology_3 ************************" << std::endl; + // we try removing the first factor + + // Create a set of optimizer parameters + LevenbergMarquardtParams parameters; + ConcurrentBatchFilter filter(parameters); + + // Add some factors to the filter + NonlinearFactorGraph newFactors; + newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + + Values newValues; + newValues.insert(1, Pose3().compose(poseError)); + newValues.insert(2, newValues.at(1).compose(poseOdometry).compose(poseError)); + newValues.insert(3, newValues.at(2).compose(poseOdometry).compose(poseError)); + newValues.insert(4, newValues.at(3).compose(poseOdometry).compose(poseError)); + + // Specify a subset of variables to marginalize/move to the smoother + FastList keysToMove; + + // Update the filter: add all factors + filter.update(newFactors, newValues, keysToMove); + + // factor we want to remove + // NOTE: we can remove factors, paying attention that the remaining graph remains connected + // we remove a single factor, the number 0, which is a BetweenFactor(1, 2, poseOdometry, noiseOdometery); + std::vector removeFactorIndices(1,0); + + // Add no factors to the filter (we only want to test the removal) + NonlinearFactorGraph noFactors; + Values noValues; + filter.update(noFactors, noValues, keysToMove, removeFactorIndices); + + NonlinearFactorGraph actualGraph = filter.getFactors(); + + NonlinearFactorGraph expectedGraph; + // we should add an empty one, so that the ordering and labeling of the factors is preserved + expectedGraph.push_back(NonlinearFactor::shared_ptr()); + expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); + expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + + CHECK(assert_equal(expectedGraph, actualGraph, 1e-6)); +} + +///* ************************************************************************* */ +TEST( ConcurrentBatchFilter, removeFactors_values ) +{ + std::cout << "*********************** removeFactors_values ************************" << std::endl; + // we try removing the last factor + + // Create a set of optimizer parameters + LevenbergMarquardtParams parameters; + ConcurrentBatchFilter filter(parameters); + + // Add some factors to the filter + NonlinearFactorGraph newFactors; + newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + + Values newValues; + newValues.insert(1, Pose3().compose(poseError)); + newValues.insert(2, newValues.at(1).compose(poseOdometry).compose(poseError)); + newValues.insert(3, newValues.at(2).compose(poseOdometry).compose(poseError)); + newValues.insert(4, newValues.at(3).compose(poseOdometry).compose(poseError)); + + // Specify a subset of variables to marginalize/move to the smoother + FastList keysToMove; + + // Update the filter: add all factors + filter.update(newFactors, newValues, keysToMove); + + // factor we want to remove + // NOTE: we can remove factors, paying attention that the remaining graph remains connected + // we remove a single factor, the number 4, which is a BetweenFactor(1, 2, poseOdometry, noiseOdometery); + std::vector removeFactorIndices(1,4); + + // Add no factors to the filter (we only want to test the removal) + NonlinearFactorGraph noFactors; + Values noValues; + filter.update(noFactors, noValues, keysToMove, removeFactorIndices); + + NonlinearFactorGraph actualGraph = filter.getFactors(); + Values actualValues = filter.calculateEstimate(); + + // note: factors are removed before the optimization + NonlinearFactorGraph expectedGraph; + expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); + expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + // we should add an empty one, so that the ordering and labeling of the factors is preserved + expectedGraph.push_back(NonlinearFactor::shared_ptr()); + + // Calculate expected factor graph and values + Values expectedValues = BatchOptimize(expectedGraph, newValues); + + CHECK(assert_equal(expectedGraph, actualGraph, 1e-6)); + CHECK(assert_equal(expectedValues, actualValues, 1e-6)); +} + +///* ************************************************************************* */ +//TEST( ConcurrentBatchFilter, synchronize_10 ) +//{ +// // Create a set of optimizer parameters +// LevenbergMarquardtParams parameters; +// parameters.maxIterations = 1; +// +// // Create a Concurrent Batch Filter +// ConcurrentBatchFilter filter(parameters); +// +// // Insert factors into the filter, and marginalize out several variables. +// // This test places several factors in the smoother side, while leaving +// // several factors on the filter side +//} +// +///* ************************************************************************* */ +//TEST( ConcurrentBatchFilter, synchronize_11 ) +//{ +// // Create a set of optimizer parameters +// LevenbergMarquardtParams parameters; +// parameters.maxIterations = 1; +// +// // Create a Concurrent Batch Filter +// ConcurrentBatchFilter filter(parameters); +// +// // Insert factors into the filter, and marginalize out several variables. +// +// // Generate a non-empty smoother update, simulating synchronizing with a non-empty smoother +//} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp index 9bcd9a27b..812c1d143 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp @@ -23,10 +23,10 @@ #include #include #include -#include #include -#include -#include +#include +#include +#include #include #include #include @@ -39,13 +39,13 @@ namespace { // Set up initial pose, odometry difference, loop closure difference, and initialization errors const Pose3 poseInitial; -const Pose3 poseOdometry( Rot3::RzRyRx(Vector_(3, 0.05, 0.10, -0.75)), Point3(1.0, -0.25, 0.10) ); -const Pose3 poseError( Rot3::RzRyRx(Vector_(3, 0.01, 0.02, -0.1)), Point3(0.05, -0.05, 0.02) ); +const Pose3 poseOdometry( Rot3::RzRyRx((Vector(3) << 0.05, 0.10, -0.75)), Point3(1.0, -0.25, 0.10) ); +const Pose3 poseError( Rot3::RzRyRx((Vector(3) << 0.01, 0.02, -0.1)), Point3(0.05, -0.05, 0.02) ); // Set up noise models for the factors const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); -const SharedDiagonal noiseOdometery = noiseModel::Diagonal::Sigmas(Vector_(6, 0.1, 0.1, 0.1, 0.5, 0.5, 0.5)); -const SharedDiagonal noiseLoop = noiseModel::Diagonal::Sigmas(Vector_(6, 0.25, 0.25, 0.25, 1.0, 1.0, 1.0)); +const SharedDiagonal noiseOdometery = noiseModel::Diagonal::Sigmas((Vector(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5)); +const SharedDiagonal noiseLoop = noiseModel::Diagonal::Sigmas((Vector(6) << 0.25, 0.25, 0.25, 1.0, 1.0, 1.0)); /* ************************************************************************* */ Values BatchOptimize(const NonlinearFactorGraph& graph, const Values& theta, int maxIter = 100) { @@ -104,8 +104,8 @@ TEST( ConcurrentBatchSmoother, getFactors ) // Add some factors to the smoother NonlinearFactorGraph newFactors1; - newFactors1.add(PriorFactor(1, poseInitial, noisePrior)); - newFactors1.add(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + newFactors1.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors1.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues1; newValues1.insert(1, Pose3()); newValues1.insert(2, newValues1.at(1).compose(poseOdometry)); @@ -121,8 +121,8 @@ TEST( ConcurrentBatchSmoother, getFactors ) // Add some more factors to the smoother NonlinearFactorGraph newFactors2; - newFactors2.add(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - newFactors2.add(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + newFactors2.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + newFactors2.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); Values newValues2; newValues2.insert(3, newValues1.at(2).compose(poseOdometry)); newValues2.insert(4, newValues2.at(3).compose(poseOdometry)); @@ -154,8 +154,8 @@ TEST( ConcurrentBatchSmoother, getLinearizationPoint ) // Add some factors to the smoother NonlinearFactorGraph newFactors1; - newFactors1.add(PriorFactor(1, poseInitial, noisePrior)); - newFactors1.add(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + newFactors1.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors1.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues1; newValues1.insert(1, Pose3()); newValues1.insert(2, newValues1.at(1).compose(poseOdometry)); @@ -171,8 +171,8 @@ TEST( ConcurrentBatchSmoother, getLinearizationPoint ) // Add some more factors to the smoother NonlinearFactorGraph newFactors2; - newFactors2.add(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - newFactors2.add(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + newFactors2.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + newFactors2.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); Values newValues2; newValues2.insert(3, newValues1.at(2).compose(poseOdometry)); newValues2.insert(4, newValues2.at(3).compose(poseOdometry)); @@ -216,8 +216,8 @@ TEST( ConcurrentBatchSmoother, calculateEstimate ) // Add some factors to the smoother NonlinearFactorGraph newFactors2; - newFactors2.add(PriorFactor(1, poseInitial, noisePrior)); - newFactors2.add(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + newFactors2.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors2.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues2; newValues2.insert(1, Pose3().compose(poseError)); newValues2.insert(2, newValues2.at(1).compose(poseOdometry).compose(poseError)); @@ -236,8 +236,8 @@ TEST( ConcurrentBatchSmoother, calculateEstimate ) // Add some more factors to the smoother NonlinearFactorGraph newFactors3; - newFactors3.add(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - newFactors3.add(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + newFactors3.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + newFactors3.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); Values newValues3; newValues3.insert(3, newValues2.at(2).compose(poseOdometry).compose(poseError)); newValues3.insert(4, newValues3.at(3).compose(poseOdometry).compose(poseError)); @@ -302,8 +302,8 @@ TEST( ConcurrentBatchSmoother, update_multiple ) // Add some factors to the smoother NonlinearFactorGraph newFactors2; - newFactors2.add(PriorFactor(1, poseInitial, noisePrior)); - newFactors2.add(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + newFactors2.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors2.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues2; newValues2.insert(1, Pose3().compose(poseError)); newValues2.insert(2, newValues2.at(1).compose(poseOdometry).compose(poseError)); @@ -322,8 +322,8 @@ TEST( ConcurrentBatchSmoother, update_multiple ) // Add some more factors to the smoother NonlinearFactorGraph newFactors3; - newFactors3.add(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - newFactors3.add(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + newFactors3.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + newFactors3.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); Values newValues3; newValues3.insert(3, newValues2.at(2).compose(poseOdometry).compose(poseError)); newValues3.insert(4, newValues3.at(3).compose(poseOdometry).compose(poseError)); @@ -391,8 +391,8 @@ TEST( ConcurrentBatchSmoother, synchronize_1 ) Ordering ordering; ordering.push_back(1); ordering.push_back(2); - filterSumarization.add(LinearContainerFactor(PriorFactor(1, poseInitial, noisePrior).linearize(filterSeparatorValues, ordering), ordering, filterSeparatorValues)); - filterSumarization.add(LinearContainerFactor(BetweenFactor(1, 2, poseOdometry, noiseOdometery).linearize(filterSeparatorValues, ordering), ordering, filterSeparatorValues)); + filterSumarization.push_back(LinearContainerFactor(PriorFactor(1, poseInitial, noisePrior).linearize(filterSeparatorValues), filterSeparatorValues)); + filterSumarization.push_back(LinearContainerFactor(BetweenFactor(1, 2, poseOdometry, noiseOdometery).linearize(filterSeparatorValues), filterSeparatorValues)); // Create expected values: the smoother output will be empty for this case NonlinearFactorGraph expectedSmootherSummarization; @@ -452,10 +452,10 @@ TEST( ConcurrentBatchSmoother, synchronize_2 ) ordering.push_back(2); filterSeparatorValues.insert(1, Pose3().compose(poseError)); filterSeparatorValues.insert(2, filterSeparatorValues.at(1).compose(poseOdometry).compose(poseError)); - filterSumarization.add(LinearContainerFactor(PriorFactor(1, poseInitial, noisePrior).linearize(filterSeparatorValues, ordering), ordering, filterSeparatorValues)); - filterSumarization.add(LinearContainerFactor(BetweenFactor(1, 2, poseOdometry, noiseOdometery).linearize(filterSeparatorValues, ordering), ordering, filterSeparatorValues)); - smootherFactors.add(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - smootherFactors.add(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + filterSumarization.push_back(LinearContainerFactor(PriorFactor(1, poseInitial, noisePrior).linearize(filterSeparatorValues), filterSeparatorValues)); + filterSumarization.push_back(LinearContainerFactor(BetweenFactor(1, 2, poseOdometry, noiseOdometery).linearize(filterSeparatorValues), filterSeparatorValues)); + smootherFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + smootherFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); smootherValues.insert(3, filterSeparatorValues.at(2).compose(poseOdometry).compose(poseError)); smootherValues.insert(4, smootherValues.at(3).compose(poseOdometry).compose(poseError)); @@ -523,11 +523,11 @@ TEST( ConcurrentBatchSmoother, synchronize_3 ) ordering.push_back(2); filterSeparatorValues.insert(1, Pose3().compose(poseError)); filterSeparatorValues.insert(2, filterSeparatorValues.at(1).compose(poseOdometry).compose(poseError)); - filterSumarization.add(LinearContainerFactor(PriorFactor(1, poseInitial, noisePrior).linearize(filterSeparatorValues, ordering), ordering, filterSeparatorValues)); - filterSumarization.add(LinearContainerFactor(BetweenFactor(1, 2, poseOdometry, noiseOdometery).linearize(filterSeparatorValues, ordering), ordering, filterSeparatorValues)); - smootherFactors.add(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - smootherFactors.add(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - smootherFactors.add(PriorFactor(4, poseInitial, noisePrior)); + filterSumarization.push_back(LinearContainerFactor(PriorFactor(1, poseInitial, noisePrior).linearize(filterSeparatorValues), filterSeparatorValues)); + filterSumarization.push_back(LinearContainerFactor(BetweenFactor(1, 2, poseOdometry, noiseOdometery).linearize(filterSeparatorValues), filterSeparatorValues)); + smootherFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + smootherFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + smootherFactors.push_back(PriorFactor(4, poseInitial, noisePrior)); smootherValues.insert(3, filterSeparatorValues.at(2).compose(poseOdometry).compose(poseError)); smootherValues.insert(4, smootherValues.at(3).compose(poseOdometry).compose(poseError)); @@ -557,25 +557,239 @@ TEST( ConcurrentBatchSmoother, synchronize_3 ) NonlinearFactorGraph allFactors = smootherFactors; Values allValues = smoother.getLinearizationPoint(); ordering = smoother.getOrdering(); // I'm really hoping this is an acceptable ordering... - GaussianFactorGraph::shared_ptr linearFactors = allFactors.linearize(allValues, ordering); + GaussianFactorGraph::shared_ptr linearFactors = allFactors.linearize(allValues); - FastSet eliminateIndices = linearFactors->keys(); + FastSet eliminateKeys = linearFactors->keys(); BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, filterSeparatorValues) { - Index index = ordering.at(key_value.key); - eliminateIndices.erase(index); + eliminateKeys.erase(key_value.key); } - std::vector variables(eliminateIndices.begin(), eliminateIndices.end()); - std::pair result = linearFactors->eliminate(variables, EliminateCholesky); + std::vector variables(eliminateKeys.begin(), eliminateKeys.end()); + GaussianFactorGraph result = *linearFactors->eliminatePartialMultifrontal(variables, EliminateCholesky).second; expectedSmootherSummarization.resize(0); - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, result.second) { - expectedSmootherSummarization.add(LinearContainerFactor(factor, ordering, allValues)); + BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, result) { + expectedSmootherSummarization.push_back(LinearContainerFactor(factor, allValues)); } CHECK(assert_equal(expectedSmootherSummarization, actualSmootherSummarization, 1e-6)); } +///* ************************************************************************* */ +TEST( ConcurrentBatchSmoother, removeFactors_topology_1 ) +{ + std::cout << "*********************** removeFactors_topology_1 ************************" << std::endl; + + // Create a set of optimizer parameters + LevenbergMarquardtParams parameters; + + // Create a Concurrent Batch Smoother + ConcurrentBatchSmoother smoother(parameters); + + // Add some factors to the smoother + NonlinearFactorGraph newFactors; + newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + + Values newValues; + newValues.insert(1, Pose3().compose(poseError)); + newValues.insert(2, newValues.at(1).compose(poseOdometry).compose(poseError)); + newValues.insert(3, newValues.at(2).compose(poseOdometry).compose(poseError)); + newValues.insert(4, newValues.at(3).compose(poseOdometry).compose(poseError)); + + + // Update the smoother: add all factors + smoother.update(newFactors, newValues); + + // factor we want to remove + // NOTE: we can remove factors, paying attention that the remaining graph remains connected + // we remove a single factor, the number 1, which is a BetweenFactor(1, 2, poseOdometry, noiseOdometery); + std::vector removeFactorIndices(1,1); + + // Add no factors to the smoother (we only want to test the removal) + NonlinearFactorGraph noFactors; + Values noValues; + smoother.update(noFactors, noValues, removeFactorIndices); + + NonlinearFactorGraph actualGraph = smoother.getFactors(); + + NonlinearFactorGraph expectedGraph; + expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); + // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + // we should add an empty one, so that the ordering and labeling of the factors is preserved + expectedGraph.push_back(NonlinearFactor::shared_ptr()); + expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + + CHECK(assert_equal(expectedGraph, actualGraph, 1e-6)); +} + +///* ************************************************************************* */ +TEST( ConcurrentBatchSmoother, removeFactors_topology_2 ) +{ + std::cout << "*********************** removeFactors_topology_2 ************************" << std::endl; + // we try removing the last factor + + // Create a set of optimizer parameters + LevenbergMarquardtParams parameters; + + // Create a Concurrent Batch Smoother + ConcurrentBatchSmoother smoother(parameters); + + // Add some factors to the smoother + NonlinearFactorGraph newFactors; + newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + + Values newValues; + newValues.insert(1, Pose3().compose(poseError)); + newValues.insert(2, newValues.at(1).compose(poseOdometry).compose(poseError)); + newValues.insert(3, newValues.at(2).compose(poseOdometry).compose(poseError)); + newValues.insert(4, newValues.at(3).compose(poseOdometry).compose(poseError)); + + // Update the smoother: add all factors + smoother.update(newFactors, newValues); + + // factor we want to remove + // NOTE: we can remove factors, paying attention that the remaining graph remains connected + // we remove a single factor, the number 1, which is a BetweenFactor(1, 2, poseOdometry, noiseOdometery); + std::vector removeFactorIndices(1,4); + + // Add no factors to the smoother (we only want to test the removal) + NonlinearFactorGraph noFactors; + Values noValues; + smoother.update(noFactors, noValues, removeFactorIndices); + + NonlinearFactorGraph actualGraph = smoother.getFactors(); + + NonlinearFactorGraph expectedGraph; + expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); + expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + // we should add an empty one, so that the ordering and labeling of the factors is preserved + expectedGraph.push_back(NonlinearFactor::shared_ptr()); + + CHECK(assert_equal(expectedGraph, actualGraph, 1e-6)); +} + + +///* ************************************************************************* */ +TEST( ConcurrentBatchSmoother, removeFactors_topology_3 ) +{ + std::cout << "*********************** removeFactors_topology_3 ************************" << std::endl; + // we try removing the first factor + + // Create a set of optimizer parameters + LevenbergMarquardtParams parameters; + ConcurrentBatchSmoother Smoother(parameters); + + // Add some factors to the Smoother + NonlinearFactorGraph newFactors; + newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + + Values newValues; + newValues.insert(1, Pose3().compose(poseError)); + newValues.insert(2, newValues.at(1).compose(poseOdometry).compose(poseError)); + newValues.insert(3, newValues.at(2).compose(poseOdometry).compose(poseError)); + newValues.insert(4, newValues.at(3).compose(poseOdometry).compose(poseError)); + + // Update the Smoother: add all factors + Smoother.update(newFactors, newValues); + + // factor we want to remove + // NOTE: we can remove factors, paying attention that the remaining graph remains connected + // we remove a single factor, the number 0, which is a BetweenFactor(1, 2, poseOdometry, noiseOdometery); + std::vector removeFactorIndices(1,0); + + // Add no factors to the Smoother (we only want to test the removal) + NonlinearFactorGraph noFactors; + Values noValues; + Smoother.update(noFactors, noValues, removeFactorIndices); + + NonlinearFactorGraph actualGraph = Smoother.getFactors(); + + NonlinearFactorGraph expectedGraph; + // we should add an empty one, so that the ordering and labeling of the factors is preserved + expectedGraph.push_back(NonlinearFactor::shared_ptr()); + expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); + expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + + CHECK(assert_equal(expectedGraph, actualGraph, 1e-6)); +} + +///* ************************************************************************* */ +TEST( ConcurrentBatchSmoother, removeFactors_values ) +{ + std::cout << "*********************** removeFactors_values ************************" << std::endl; + // we try removing the last factor + + // Create a set of optimizer parameters + LevenbergMarquardtParams parameters; + ConcurrentBatchSmoother Smoother(parameters); + + // Add some factors to the Smoother + NonlinearFactorGraph newFactors; + newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + + Values newValues; + newValues.insert(1, Pose3().compose(poseError)); + newValues.insert(2, newValues.at(1).compose(poseOdometry).compose(poseError)); + newValues.insert(3, newValues.at(2).compose(poseOdometry).compose(poseError)); + newValues.insert(4, newValues.at(3).compose(poseOdometry).compose(poseError)); + + // Update the Smoother: add all factors + Smoother.update(newFactors, newValues); + + // factor we want to remove + // NOTE: we can remove factors, paying attention that the remaining graph remains connected + // we remove a single factor, the number 4, which is a BetweenFactor(1, 2, poseOdometry, noiseOdometery); + std::vector removeFactorIndices(1,4); + + // Add no factors to the Smoother (we only want to test the removal) + NonlinearFactorGraph noFactors; + Values noValues; + Smoother.update(noFactors, noValues, removeFactorIndices); + + NonlinearFactorGraph actualGraph = Smoother.getFactors(); + Values actualValues = Smoother.calculateEstimate(); + + // note: factors are removed before the optimization + NonlinearFactorGraph expectedGraph; + expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); + expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + // we should add an empty one, so that the ordering and labeling of the factors is preserved + expectedGraph.push_back(NonlinearFactor::shared_ptr()); + + // Calculate expected factor graph and values + Values expectedValues = BatchOptimize(expectedGraph, newValues); + + CHECK(assert_equal(expectedGraph, actualGraph, 1e-6)); + CHECK(assert_equal(expectedValues, actualValues, 1e-6)); +} + + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp index b45cb2591..34f768562 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp @@ -23,10 +23,9 @@ #include #include #include -#include #include -#include -#include +#include +#include #include #include #include @@ -39,14 +38,14 @@ namespace { // Set up initial pose, odometry difference, loop closure difference, and initialization errors const Pose3 poseInitial; -const Pose3 poseOdometry( Rot3::RzRyRx(Vector_(3, 0.05, 0.10, -0.75)), Point3(1.0, -0.25, 0.10) ); -//const Pose3 poseError( Rot3::RzRyRx(Vector_(3, 0.01, 0.02, -0.1)), Point3(0.05, -0.05, 0.02) ); -const Pose3 poseError( Rot3::RzRyRx(Vector_(3, 0.1, 0.02, -0.1)), Point3(0.5, -0.05, 0.2) ); +const Pose3 poseOdometry( Rot3::RzRyRx((Vector(3) << 0.05, 0.10, -0.75)), Point3(1.0, -0.25, 0.10) ); +//const Pose3 poseError( Rot3::RzRyRx((Vector(3) << 0.01, 0.02, -0.1)), Point3(0.05, -0.05, 0.02) ); +const Pose3 poseError( Rot3::RzRyRx((Vector(3) << 0.1, 0.02, -0.1)), Point3(0.5, -0.05, 0.2) ); // Set up noise models for the factors const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); -const SharedDiagonal noiseOdometery = noiseModel::Diagonal::Sigmas(Vector_(6, 0.1, 0.1, 0.1, 0.5, 0.5, 0.5)); -const SharedDiagonal noiseLoop = noiseModel::Diagonal::Sigmas(Vector_(6, 0.25, 0.25, 0.25, 1.0, 1.0, 1.0)); +const SharedDiagonal noiseOdometery = noiseModel::Diagonal::Sigmas((Vector(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5)); +const SharedDiagonal noiseLoop = noiseModel::Diagonal::Sigmas((Vector(6) << 0.25, 0.25, 0.25, 1.0, 1.0, 1.0)); /* ************************************************************************* */ Values BatchOptimize(const NonlinearFactorGraph& graph, const Values& theta, int maxIter = 100) { @@ -59,7 +58,7 @@ Values BatchOptimize(const NonlinearFactorGraph& graph, const Values& theta, int // parameters.lambdaInitial = 1; // parameters.verbosity = NonlinearOptimizerParams::ERROR; // parameters.verbosityLM = ISAM2Params::DAMPED; -// parameters.linearSolverType = SuccessiveLinearizationParams::MULTIFRONTAL_QR; +// parameters.linearSolverType = NonlinearOptimizerParams::MULTIFRONTAL_QR; // it is the same as the input graph, but we removed the empty factors that may be present in the input graph NonlinearFactorGraph graphForISAM2; @@ -96,20 +95,16 @@ NonlinearFactorGraph CalculateMarginals(const NonlinearFactorGraph& factorGraph, ordering.push_back(key); } - GaussianFactorGraph linearGraph = *factorGraph.linearize(linPoint, ordering); - // Create the set of marginalizable variables - std::vector linearIndices; - BOOST_FOREACH(Key key, keysToMarginalize) { - linearIndices.push_back(ordering.at(key)); - } + GaussianFactorGraph linearGraph = *factorGraph.linearize(linPoint); - std::pair marginal = linearGraph.eliminate(linearIndices, EliminateCholesky); + GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector(keysToMarginalize.begin(), keysToMarginalize.end()), EliminateCholesky).second; NonlinearFactorGraph LinearContainerForGaussianMarginals; - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, marginal.second) { - LinearContainerForGaussianMarginals.add(LinearContainerFactor(factor, ordering, linPoint)); + BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, marginal) { + LinearContainerForGaussianMarginals.push_back(LinearContainerFactor(factor, linPoint)); } + return LinearContainerForGaussianMarginals; } @@ -158,8 +153,8 @@ TEST( ConcurrentIncrementalFilter, getFactors ) // Add some factors to the filter NonlinearFactorGraph newFactors1; - newFactors1.add(PriorFactor(1, poseInitial, noisePrior)); - newFactors1.add(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + newFactors1.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors1.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues1; newValues1.insert(1, Pose3()); newValues1.insert(2, newValues1.at(1).compose(poseOdometry)); @@ -175,8 +170,8 @@ TEST( ConcurrentIncrementalFilter, getFactors ) // Add some more factors to the filter NonlinearFactorGraph newFactors2; - newFactors2.add(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - newFactors2.add(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + newFactors2.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + newFactors2.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); Values newValues2; newValues2.insert(3, newValues1.at(2).compose(poseOdometry)); newValues2.insert(4, newValues2.at(3).compose(poseOdometry)); @@ -208,8 +203,8 @@ TEST( ConcurrentIncrementalFilter, getLinearizationPoint ) // Add some factors to the filter NonlinearFactorGraph newFactors1; - newFactors1.add(PriorFactor(1, poseInitial, noisePrior)); - newFactors1.add(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + newFactors1.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors1.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues1; newValues1.insert(1, Pose3()); newValues1.insert(2, newValues1.at(1).compose(poseOdometry)); @@ -225,8 +220,8 @@ TEST( ConcurrentIncrementalFilter, getLinearizationPoint ) // Add some more factors to the filter NonlinearFactorGraph newFactors2; - newFactors2.add(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - newFactors2.add(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + newFactors2.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + newFactors2.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); Values newValues2; newValues2.insert(3, newValues1.at(2).compose(poseOdometry)); newValues2.insert(4, newValues2.at(3).compose(poseOdometry)); @@ -242,12 +237,6 @@ TEST( ConcurrentIncrementalFilter, getLinearizationPoint ) CHECK(assert_equal(expected3, actual3)); } -/* ************************************************************************* */ -TEST( ConcurrentIncrementalFilter, getOrdering ) -{ - // TODO: Think about how to check ordering... -} - /* ************************************************************************* */ TEST( ConcurrentIncrementalFilter, getDelta ) { @@ -270,8 +259,8 @@ TEST( ConcurrentIncrementalFilter, calculateEstimate ) // Add some factors to the filter NonlinearFactorGraph newFactors2; - newFactors2.add(PriorFactor(1, poseInitial, noisePrior)); - newFactors2.add(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + newFactors2.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors2.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues2; newValues2.insert(1, Pose3().compose(poseError)); newValues2.insert(2, newValues2.at(1).compose(poseOdometry).compose(poseError)); @@ -290,8 +279,8 @@ TEST( ConcurrentIncrementalFilter, calculateEstimate ) // Add some more factors to the filter NonlinearFactorGraph newFactors3; - newFactors3.add(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - newFactors3.add(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + newFactors3.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + newFactors3.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); Values newValues3; newValues3.insert(3, newValues2.at(2).compose(poseOdometry).compose(poseError)); newValues3.insert(4, newValues3.at(3).compose(poseOdometry).compose(poseError)); @@ -354,8 +343,8 @@ TEST( ConcurrentIncrementalFilter, update_multiple ) // Add some factors to the filter NonlinearFactorGraph newFactors2; - newFactors2.add(PriorFactor(1, poseInitial, noisePrior)); - newFactors2.add(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + newFactors2.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors2.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues2; newValues2.insert(1, Pose3().compose(poseError)); newValues2.insert(2, newValues2.at(1).compose(poseOdometry).compose(poseError)); @@ -374,8 +363,8 @@ TEST( ConcurrentIncrementalFilter, update_multiple ) // Add some more factors to the filter NonlinearFactorGraph newFactors3; - newFactors3.add(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - newFactors3.add(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + newFactors3.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + newFactors3.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); Values newValues3; newValues3.insert(3, newValues2.at(2).compose(poseOdometry).compose(poseError)); newValues3.insert(4, newValues3.at(3).compose(poseOdometry).compose(poseError)); @@ -404,10 +393,10 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_1 ) // Add some factors to the filter NonlinearFactorGraph newFactors; - newFactors.add(PriorFactor(1, poseInitial, noisePrior)); - newFactors.add(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - newFactors.add(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - newFactors.add(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); Values newValues; newValues.insert(1, Pose3().compose(poseError)); newValues.insert(2, newValues.at(1).compose(poseOdometry).compose(poseError)); @@ -434,29 +423,13 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_1 ) // ---------------------------------------------------------------------------------------------- NonlinearFactorGraph partialGraph; - partialGraph.add(PriorFactor(1, poseInitial, noisePrior)); - partialGraph.add(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - partialGraph.add(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + partialGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); + partialGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + partialGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - Values partialValues; - // ISAM2 after the update is computing the delta, without applying them to the current lin point - partialValues.insert(1, newValues.at(1)); - partialValues.insert(2, newValues.at(2)); - partialValues.insert(3, newValues.at(3)); + GaussianFactorGraph linearGraph = *partialGraph.linearize(newValues); - // Create an ordering - Ordering ordering; - ordering.push_back(1); - ordering.push_back(2); - ordering.push_back(3); - - // Create the set of marginalizable variables - std::vector linearIndices; - linearIndices.push_back(ordering.at(1)); - linearIndices.push_back(ordering.at(2)); - - GaussianFactorGraph linearPartialGraph = *partialGraph.linearize(partialValues, ordering); - std::pair result = linearPartialGraph.eliminate(linearIndices, EliminateCholesky); + GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector(keysToMove.begin(), keysToMove.end()), EliminateCholesky).second; NonlinearFactorGraph expectedGraph; @@ -468,13 +441,13 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_1 ) expectedGraph.push_back(NonlinearFactor::shared_ptr()); expectedGraph.push_back(NonlinearFactor::shared_ptr()); // ========================================================== - expectedGraph.add(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, result.second) { + BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, marginal) { // the linearization point for the linear container is optional, but it is not used in the filter, // therefore if we add it here it will not pass the test - // expectedGraph.add(LinearContainerFactor(factor, ordering, partialValues)); - expectedGraph.add(LinearContainerFactor(factor, ordering)); + // expectedGraph.push_back(LinearContainerFactor(factor, ordering, partialValues)); + expectedGraph.push_back(LinearContainerFactor(factor)); } // ---------------------------------------------------------------------------------------------- @@ -503,10 +476,10 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_2 ) // Add some factors to the filter NonlinearFactorGraph newFactors; - newFactors.add(PriorFactor(1, poseInitial, noisePrior)); - newFactors.add(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - newFactors.add(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - newFactors.add(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); Values newValues; newValues.insert(1, Pose3().compose(poseError)); newValues.insert(2, newValues.at(1).compose(poseOdometry).compose(poseError)); @@ -534,48 +507,32 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_2 ) // ---------------------------------------------------------------------------------------------- NonlinearFactorGraph partialGraph; - partialGraph.add(PriorFactor(1, poseInitial, noisePrior)); - partialGraph.add(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - partialGraph.add(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + partialGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); + partialGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + partialGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - Values partialValues; - // ISAM2 after the update is computing the delta, without applying them to the current lin point - partialValues.insert(1, optimalValues.at(1)); - partialValues.insert(2, optimalValues.at(2)); - partialValues.insert(3, optimalValues.at(3)); + GaussianFactorGraph linearGraph = *partialGraph.linearize(optimalValues); - // Create an ordering - Ordering ordering; - ordering.push_back(1); - ordering.push_back(2); - ordering.push_back(3); + GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector(keysToMove.begin(), keysToMove.end()), EliminateCholesky).second; - // Create the set of marginalizable variables - std::vector linearIndices; - linearIndices.push_back(ordering.at(1)); - linearIndices.push_back(ordering.at(2)); + NonlinearFactorGraph expectedGraph; - GaussianFactorGraph linearPartialGraph = *partialGraph.linearize(partialValues, ordering); - std::pair result = linearPartialGraph.eliminate(linearIndices, EliminateCholesky); + // These three lines create three empty factors in expectedGraph: + // this is done since the equal function in NonlinearFactorGraph also cares about the ordering of the factors + // and in the actualGraph produced by the HMF we first insert 5 nonlinear factors, then we delete 3 of them, by + // substituting them with a linear marginal + expectedGraph.push_back(NonlinearFactor::shared_ptr()); + expectedGraph.push_back(NonlinearFactor::shared_ptr()); + expectedGraph.push_back(NonlinearFactor::shared_ptr()); + // ========================================================== + expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - NonlinearFactorGraph expectedGraph; - - // These three lines create three empty factors in expectedGraph: - // this is done since the equal function in NonlinearFactorGraph also cares about the ordering of the factors - // and in the actualGraph produced by the HMF we first insert 5 nonlinear factors, then we delete 3 of them, by - // substituting them with a linear marginal - expectedGraph.push_back(NonlinearFactor::shared_ptr()); - expectedGraph.push_back(NonlinearFactor::shared_ptr()); - expectedGraph.push_back(NonlinearFactor::shared_ptr()); - // ========================================================== - expectedGraph.add(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, result.second) { - // the linearization point for the linear container is optional, but it is not used in the filter, - // therefore if we add it here it will not pass the test - // expectedGraph.add(LinearContainerFactor(factor, ordering, partialValues)); - expectedGraph.add(LinearContainerFactor(factor, ordering)); - } + BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, marginal) { + // the linearization point for the linear container is optional, but it is not used in the filter, + // therefore if we add it here it will not pass the test + // expectedGraph.push_back(LinearContainerFactor(factor, ordering, partialValues)); + expectedGraph.push_back(LinearContainerFactor(factor)); + } // ---------------------------------------------------------------------------------------------- @@ -650,8 +607,8 @@ TEST( ConcurrentIncrementalFilter, synchronize_1 ) // Insert factors into the filter, but do not marginalize out any variables. // The synchronization should still be empty NonlinearFactorGraph newFactors; - newFactors.add(PriorFactor(1, poseInitial, noisePrior)); - newFactors.add(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues; newValues.insert(1, Pose3().compose(poseError)); newValues.insert(2, newValues.at(1).compose(poseOdometry).compose(poseError)); @@ -813,7 +770,7 @@ TEST( ConcurrentIncrementalFilter, synchronize_3 ) expectedFilterSeparatorValues.insert(2, newValues.at(2)); // ------------------------------------------------------------------------------ NonlinearFactorGraph partialGraph; - partialGraph.add(factor3); + partialGraph.push_back(factor3); Values partialValues; partialValues.insert(2, newValues.at(2)); @@ -902,7 +859,7 @@ TEST( ConcurrentIncrementalFilter, synchronize_4 ) expectedFilterSeparatorValues.insert(2, newValues.at(2)); // ------------------------------------------------------------------------------ NonlinearFactorGraph partialGraphFilter; - partialGraphFilter.add(factor3); + partialGraphFilter.push_back(factor3); Values partialValuesFilter; partialValuesFilter.insert(2, newValues.at(2)); @@ -995,8 +952,8 @@ TEST( ConcurrentIncrementalFilter, synchronize_5 ) filter.postsync(); NonlinearFactorGraph expectedSmootherFactors; - expectedSmootherFactors.add(factor1); - expectedSmootherFactors.add(factor2); + expectedSmootherFactors.push_back(factor1); + expectedSmootherFactors.push_back(factor2); Values optimalValues = BatchOptimize(newFactors, newValues, 1); Values expectedSmootherValues; @@ -1026,11 +983,11 @@ TEST( ConcurrentIncrementalFilter, synchronize_5 ) // currently the smoother contains factor 1 and 2 and node 1 and 2 NonlinearFactorGraph partialGraph; - partialGraph.add(factor1); - partialGraph.add(factor2); + partialGraph.push_back(factor1); + partialGraph.push_back(factor2); // we also assume that the smoother received an extra factor (e.g., a prior on 1) - partialGraph.add(factor1); + partialGraph.push_back(factor1); Values partialValues; // Incrementaloptimization updates all linearization points but the ones of the separator @@ -1059,8 +1016,8 @@ TEST( ConcurrentIncrementalFilter, synchronize_5 ) filter.postsync(); NonlinearFactorGraph expectedSmootherFactors2; - expectedSmootherFactors2.add(factor3); - expectedSmootherFactors2.add(factor4); + expectedSmootherFactors2.push_back(factor3); + expectedSmootherFactors2.push_back(factor4); Values expectedSmootherValues2; expectedSmootherValues2.insert(2, newValues.at(2)); @@ -1075,7 +1032,7 @@ TEST( ConcurrentIncrementalFilter, synchronize_5 ) // ------------------------------------------------------------------------------ // This cannot be nonempty for the first call to synchronize NonlinearFactorGraph partialGraphFilter; - partialGraphFilter.add(factor5); + partialGraphFilter.push_back(factor5); Values partialValuesFilter; @@ -1096,8 +1053,8 @@ TEST( ConcurrentIncrementalFilter, synchronize_5 ) // Now we should check that the smooother summarization on the old separator is correctly propagated // on the new separator by the filter NonlinearFactorGraph partialGraphTransition; - partialGraphTransition.add(factor3); - partialGraphTransition.add(factor4); + partialGraphTransition.push_back(factor3); + partialGraphTransition.push_back(factor4); partialGraphTransition.push_back(smootherSummarization2); Values partialValuesTransition; @@ -1119,7 +1076,7 @@ TEST( ConcurrentIncrementalFilter, synchronize_5 ) expectedFilterGraph.push_back(factorEmpty); expectedFilterGraph.push_back(factorEmpty); expectedFilterGraph.push_back(factorEmpty); - expectedFilterGraph.add(factor5); + expectedFilterGraph.push_back(factor5); expectedFilterGraph.push_back(factorEmpty); expectedFilterGraph.push_back(factorEmpty); expectedFilterGraph.push_back(factorEmpty); @@ -1143,10 +1100,10 @@ TEST( ConcurrentIncrementalFilter, CalculateMarginals_1 ) NonlinearFactor::shared_ptr factor4(new BetweenFactor(2, 3, poseOdometry, noiseOdometery)); NonlinearFactorGraph factorGraph; - factorGraph.add(factor1); - factorGraph.add(factor2); - factorGraph.add(factor3); - factorGraph.add(factor4); + factorGraph.push_back(factor1); + factorGraph.push_back(factor2); + factorGraph.push_back(factor3); + factorGraph.push_back(factor4); Values newValues; Pose3 value1 = Pose3().compose(poseError); @@ -1157,26 +1114,18 @@ TEST( ConcurrentIncrementalFilter, CalculateMarginals_1 ) newValues.insert(2, value2); newValues.insert(3, value3); - // We first manually - // Create an ordering - Ordering ordering; - ordering.push_back(1); - ordering.push_back(2); - ordering.push_back(3); - - GaussianFactorGraph linearFactorGraph = *factorGraph.linearize(newValues, ordering); - // Create the set of marginalizable variables - std::vector linearIndices; - linearIndices.push_back(ordering.at(1)); + std::vector linearIndices; + linearIndices.push_back(1); - std::pair result = linearFactorGraph.eliminate(linearIndices, EliminateCholesky); + GaussianFactorGraph linearGraph = *factorGraph.linearize(newValues); - NonlinearFactorGraph expectedMarginals; - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, result.second) { - expectedMarginals.add(LinearContainerFactor(factor, ordering, newValues)); + GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector(linearIndices.begin(), linearIndices.end()), EliminateCholesky).second; - } + NonlinearFactorGraph expectedMarginals; + BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, marginal) { + expectedMarginals.push_back(LinearContainerFactor(factor, newValues)); + } FastList keysToMarginalize; keysToMarginalize.push_back(1); @@ -1201,10 +1150,10 @@ TEST( ConcurrentIncrementalFilter, CalculateMarginals_2 ) NonlinearFactor::shared_ptr factor4(new BetweenFactor(2, 3, poseOdometry, noiseOdometery)); NonlinearFactorGraph factorGraph; - factorGraph.add(factor1); - factorGraph.add(factor2); - factorGraph.add(factor3); - factorGraph.add(factor4); + factorGraph.push_back(factor1); + factorGraph.push_back(factor2); + factorGraph.push_back(factor3); + factorGraph.push_back(factor4); Values newValues; Pose3 value1 = Pose3().compose(poseError); @@ -1215,28 +1164,24 @@ TEST( ConcurrentIncrementalFilter, CalculateMarginals_2 ) newValues.insert(2, value2); newValues.insert(3, value3); - // We first manually - // Create an ordering - Ordering ordering; - ordering.push_back(1); - ordering.push_back(2); - ordering.push_back(3); - GaussianFactorGraph linearFactorGraph = *factorGraph.linearize(newValues, ordering); - // Create the set of marginalizable variables - std::vector linearIndices; - linearIndices.push_back(ordering.at(1)); - linearIndices.push_back(ordering.at(2)); + // Create the set of marginalizable variables + std::vector linearIndices; + linearIndices.push_back(1); + linearIndices.push_back(2); - std::pair result = linearFactorGraph.eliminate(linearIndices, EliminateCholesky); + + GaussianFactorGraph linearGraph = *factorGraph.linearize(newValues); + + GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector(linearIndices.begin(), linearIndices.end()), EliminateCholesky).second; NonlinearFactorGraph expectedMarginals; - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, result.second) { - expectedMarginals.add(LinearContainerFactor(factor, ordering, newValues)); - + BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, marginal) { + expectedMarginals.push_back(LinearContainerFactor(factor, newValues)); } + FastList keysToMarginalize; keysToMarginalize.push_back(1); keysToMarginalize.push_back(2); @@ -1251,6 +1196,249 @@ TEST( ConcurrentIncrementalFilter, CalculateMarginals_2 ) +///* ************************************************************************* */ +TEST( ConcurrentIncrementalFilter, removeFactors_topology_1 ) +{ + std::cout << "*********************** removeFactors_topology_1 ************************" << std::endl; + + // Create a set of optimizer parameters + ISAM2Params parameters; + parameters.relinearizeThreshold = 0; + // ISAM2 checks whether to relinearize or not a variable only every relinearizeSkip steps and the + // default value for that is 10 (if you set that to zero the code will crash) + parameters.relinearizeSkip = 1; + + // Create a Concurrent IncrementalFilter + ConcurrentIncrementalFilter filter(parameters); + + // Add some factors to the filter + NonlinearFactorGraph newFactors; + newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + + Values newValues; + newValues.insert(1, Pose3().compose(poseError)); + newValues.insert(2, newValues.at(1).compose(poseOdometry).compose(poseError)); + newValues.insert(3, newValues.at(2).compose(poseOdometry).compose(poseError)); + newValues.insert(4, newValues.at(3).compose(poseOdometry).compose(poseError)); + + // Specify a subset of variables to marginalize/move to the smoother + FastList keysToMove; + + // Update the filter: add all factors + filter.update(newFactors, newValues, keysToMove); + + // factor we want to remove + // NOTE: we can remove factors, paying attention that the remaining graph remains connected + // we remove a single factor, the number 1, which is a BetweenFactor(1, 2, poseOdometry, noiseOdometery); + std::vector removeFactorIndices(1,1); + + // Add no factors to the filter (we only want to test the removal) + NonlinearFactorGraph noFactors; + Values noValues; + filter.update(noFactors, noValues, keysToMove, removeFactorIndices); + + NonlinearFactorGraph actualGraph = filter.getFactors(); + + NonlinearFactorGraph expectedGraph; + expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); + // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + // we should add an empty one, so that the ordering and labeling of the factors is preserved + expectedGraph.push_back(NonlinearFactor::shared_ptr()); + expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + + CHECK(assert_equal(expectedGraph, actualGraph, 1e-6)); +} + +/////* ************************************************************************* */ +TEST( ConcurrentIncrementalFilter, removeFactors_topology_2 ) +{ + std::cout << "*********************** removeFactors_topology_2 ************************" << std::endl; + // we try removing the last factor + + ISAM2Params parameters; + parameters.relinearizeThreshold = 0; + // ISAM2 checks whether to relinearize or not a variable only every relinearizeSkip steps and the + // default value for that is 10 (if you set that to zero the code will crash) + parameters.relinearizeSkip = 1; + + // Create a Concurrent IncrementalFilter + ConcurrentIncrementalFilter filter(parameters); + + // Add some factors to the filter + NonlinearFactorGraph newFactors; + newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + + Values newValues; + newValues.insert(1, Pose3().compose(poseError)); + newValues.insert(2, newValues.at(1).compose(poseOdometry).compose(poseError)); + newValues.insert(3, newValues.at(2).compose(poseOdometry).compose(poseError)); + newValues.insert(4, newValues.at(3).compose(poseOdometry).compose(poseError)); + + // Specify a subset of variables to marginalize/move to the smoother + FastList keysToMove; + + // Update the filter: add all factors + filter.update(newFactors, newValues, keysToMove); + + // factor we want to remove + // NOTE: we can remove factors, paying attention that the remaining graph remains connected + // we remove a single factor, the number 1, which is a BetweenFactor(1, 2, poseOdometry, noiseOdometery); + std::vector removeFactorIndices(1,4); + + // Add no factors to the filter (we only want to test the removal) + NonlinearFactorGraph noFactors; + Values noValues; + filter.update(noFactors, noValues, keysToMove, removeFactorIndices); + + NonlinearFactorGraph actualGraph = filter.getFactors(); + + NonlinearFactorGraph expectedGraph; + expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); + expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + // we should add an empty one, so that the ordering and labeling of the factors is preserved + expectedGraph.push_back(NonlinearFactor::shared_ptr()); + + CHECK(assert_equal(expectedGraph, actualGraph, 1e-6)); +} + + +/////* ************************************************************************* */ +TEST( ConcurrentIncrementalFilter, removeFactors_topology_3 ) +{ + std::cout << "*********************** removeFactors_topology_3 ************************" << std::endl; + // we try removing the first factor + + ISAM2Params parameters; + parameters.relinearizeThreshold = 0; + // ISAM2 checks whether to relinearize or not a variable only every relinearizeSkip steps and the + // default value for that is 10 (if you set that to zero the code will crash) + parameters.relinearizeSkip = 1; + + // Create a Concurrent IncrementalFilter + ConcurrentIncrementalFilter filter(parameters); + + // Add some factors to the filter + NonlinearFactorGraph newFactors; + newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + + Values newValues; + newValues.insert(1, Pose3().compose(poseError)); + newValues.insert(2, newValues.at(1).compose(poseOdometry).compose(poseError)); + newValues.insert(3, newValues.at(2).compose(poseOdometry).compose(poseError)); + newValues.insert(4, newValues.at(3).compose(poseOdometry).compose(poseError)); + + // Specify a subset of variables to marginalize/move to the smoother + FastList keysToMove; + + // Update the filter: add all factors + filter.update(newFactors, newValues, keysToMove); + + // factor we want to remove + // NOTE: we can remove factors, paying attention that the remaining graph remains connected + // we remove a single factor, the number 0, which is a BetweenFactor(1, 2, poseOdometry, noiseOdometery); + std::vector removeFactorIndices(1,0); + + // Add no factors to the filter (we only want to test the removal) + NonlinearFactorGraph noFactors; + Values noValues; + filter.update(noFactors, noValues, keysToMove, removeFactorIndices); + + NonlinearFactorGraph actualGraph = filter.getFactors(); + + NonlinearFactorGraph expectedGraph; + // we should add an empty one, so that the ordering and labeling of the factors is preserved + expectedGraph.push_back(NonlinearFactor::shared_ptr()); + expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); + expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + + CHECK(assert_equal(expectedGraph, actualGraph, 1e-6)); +} + +/////* ************************************************************************* */ +TEST( ConcurrentIncrementalFilter, removeFactors_values ) +{ + std::cout << "*********************** removeFactors_values ************************" << std::endl; + // we try removing the last factor + + ISAM2Params parameters; + parameters.relinearizeThreshold = 0; + // ISAM2 checks whether to relinearize or not a variable only every relinearizeSkip steps and the + // default value for that is 10 (if you set that to zero the code will crash) + parameters.relinearizeSkip = 1; + + // Create a Concurrent IncrementalFilter + ConcurrentIncrementalFilter filter(parameters); + + // Add some factors to the filter + NonlinearFactorGraph newFactors; + newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + + Values newValues; + newValues.insert(1, Pose3().compose(poseError)); + newValues.insert(2, newValues.at(1).compose(poseOdometry).compose(poseError)); + newValues.insert(3, newValues.at(2).compose(poseOdometry).compose(poseError)); + newValues.insert(4, newValues.at(3).compose(poseOdometry).compose(poseError)); + + // Specify a subset of variables to marginalize/move to the smoother + FastList keysToMove; + + // Update the filter: add all factors + filter.update(newFactors, newValues, keysToMove); + + // factor we want to remove + // NOTE: we can remove factors, paying attention that the remaining graph remains connected + // we remove a single factor, the number 4, which is a BetweenFactor(1, 2, poseOdometry, noiseOdometery); + std::vector removeFactorIndices(1,4); + + // Add no factors to the filter (we only want to test the removal) + NonlinearFactorGraph noFactors; + Values noValues; + filter.update(noFactors, noValues, keysToMove, removeFactorIndices); + + NonlinearFactorGraph actualGraph = filter.getFactors(); + Values actualValues = filter.getLinearizationPoint(); + + NonlinearFactorGraph expectedGraph; + expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); + expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + // we should add an empty one, so that the ordering and labeling of the factors is preserved + expectedGraph.push_back(NonlinearFactor::shared_ptr()); + + // Calculate expected factor graph and values + Values expectedValues = BatchOptimize(expectedGraph, newValues); + + CHECK(assert_equal(expectedGraph, actualGraph, 1e-6)); + CHECK(assert_equal(expectedValues, actualValues, 1e-6)); +} + + + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp index 0b91644e2..51c61b8ec 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp @@ -23,10 +23,10 @@ #include #include #include -#include #include -#include -#include +#include +#include +#include #include #include #include @@ -39,13 +39,13 @@ namespace { // Set up initial pose, odometry difference, loop closure difference, and initialization errors const Pose3 poseInitial; -const Pose3 poseOdometry( Rot3::RzRyRx(Vector_(3, 0.05, 0.10, -0.75)), Point3(1.0, -0.25, 0.10) ); -const Pose3 poseError( Rot3::RzRyRx(Vector_(3, 0.01, 0.02, -0.1)), Point3(0.05, -0.05, 0.02) ); +const Pose3 poseOdometry( Rot3::RzRyRx((Vector(3) << 0.05, 0.10, -0.75)), Point3(1.0, -0.25, 0.10) ); +const Pose3 poseError( Rot3::RzRyRx((Vector(3) << 0.01, 0.02, -0.1)), Point3(0.05, -0.05, 0.02) ); // Set up noise models for the factors const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); -const SharedDiagonal noiseOdometery = noiseModel::Diagonal::Sigmas(Vector_(6, 0.1, 0.1, 0.1, 0.5, 0.5, 0.5)); -const SharedDiagonal noiseLoop = noiseModel::Diagonal::Sigmas(Vector_(6, 0.25, 0.25, 0.25, 1.0, 1.0, 1.0)); +const SharedDiagonal noiseOdometery = noiseModel::Diagonal::Sigmas((Vector(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5)); +const SharedDiagonal noiseLoop = noiseModel::Diagonal::Sigmas((Vector(6) << 0.25, 0.25, 0.25, 1.0, 1.0, 1.0)); /* ************************************************************************* */ Values BatchOptimize(const NonlinearFactorGraph& graph, const Values& theta, int maxIter = 100) { @@ -58,7 +58,7 @@ Values BatchOptimize(const NonlinearFactorGraph& graph, const Values& theta, int // parameters.lambdaInitial = 1; // parameters.verbosity = NonlinearOptimizerParams::ERROR; // parameters.verbosityLM = ISAM2Params::DAMPED; -// parameters.linearSolverType = SuccessiveLinearizationParams::MULTIFRONTAL_QR; +// parameters.linearSolverType = NonlinearOptimizerParams::MULTIFRONTAL_QR; ISAM2 optimizer(parameters); optimizer.update( graph, theta ); @@ -115,8 +115,8 @@ TEST( ConcurrentIncrementalSmootherDL, getFactors ) // Add some factors to the smoother NonlinearFactorGraph newFactors1; - newFactors1.add(PriorFactor(1, poseInitial, noisePrior)); - newFactors1.add(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + newFactors1.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors1.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues1; newValues1.insert(1, Pose3()); newValues1.insert(2, newValues1.at(1).compose(poseOdometry)); @@ -132,8 +132,8 @@ TEST( ConcurrentIncrementalSmootherDL, getFactors ) // Add some more factors to the smoother NonlinearFactorGraph newFactors2; - newFactors2.add(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - newFactors2.add(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + newFactors2.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + newFactors2.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); Values newValues2; newValues2.insert(3, newValues1.at(2).compose(poseOdometry)); newValues2.insert(4, newValues2.at(3).compose(poseOdometry)); @@ -166,8 +166,8 @@ TEST( ConcurrentIncrementalSmootherDL, getLinearizationPoint ) // Add some factors to the smoother NonlinearFactorGraph newFactors1; - newFactors1.add(PriorFactor(1, poseInitial, noisePrior)); - newFactors1.add(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + newFactors1.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors1.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues1; newValues1.insert(1, Pose3()); newValues1.insert(2, newValues1.at(1).compose(poseOdometry)); @@ -183,8 +183,8 @@ TEST( ConcurrentIncrementalSmootherDL, getLinearizationPoint ) // Add some more factors to the smoother NonlinearFactorGraph newFactors2; - newFactors2.add(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - newFactors2.add(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + newFactors2.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + newFactors2.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); Values newValues2; newValues2.insert(3, newValues1.at(2).compose(poseOdometry)); newValues2.insert(4, newValues2.at(3).compose(poseOdometry)); @@ -200,12 +200,6 @@ TEST( ConcurrentIncrementalSmootherDL, getLinearizationPoint ) CHECK(assert_equal(expected3, actual3)); } -/* ************************************************************************* */ -TEST( ConcurrentIncrementalSmootherDL, getOrdering ) -{ - // TODO: Think about how to check ordering... -} - /* ************************************************************************* */ TEST( ConcurrentIncrementalSmootherDL, getDelta ) { @@ -229,8 +223,8 @@ TEST( ConcurrentIncrementalSmootherDL, calculateEstimate ) // Add some factors to the smoother NonlinearFactorGraph newFactors2; - newFactors2.add(PriorFactor(1, poseInitial, noisePrior)); - newFactors2.add(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + newFactors2.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors2.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues2; newValues2.insert(1, Pose3().compose(poseError)); newValues2.insert(2, newValues2.at(1).compose(poseOdometry).compose(poseError)); @@ -249,8 +243,8 @@ TEST( ConcurrentIncrementalSmootherDL, calculateEstimate ) // Add some more factors to the smoother NonlinearFactorGraph newFactors3; - newFactors3.add(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - newFactors3.add(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + newFactors3.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + newFactors3.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); Values newValues3; newValues3.insert(3, newValues2.at(2).compose(poseOdometry).compose(poseError)); newValues3.insert(4, newValues3.at(3).compose(poseOdometry).compose(poseError)); @@ -316,8 +310,8 @@ TEST( ConcurrentIncrementalSmootherDL, update_multiple ) // Add some factors to the smoother NonlinearFactorGraph newFactors2; - newFactors2.add(PriorFactor(1, poseInitial, noisePrior)); - newFactors2.add(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + newFactors2.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors2.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues2; newValues2.insert(1, Pose3().compose(poseError)); newValues2.insert(2, newValues2.at(1).compose(poseOdometry).compose(poseError)); @@ -336,8 +330,8 @@ TEST( ConcurrentIncrementalSmootherDL, update_multiple ) // Add some more factors to the smoother NonlinearFactorGraph newFactors3; - newFactors3.add(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - newFactors3.add(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + newFactors3.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + newFactors3.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); Values newValues3; newValues3.insert(3, newValues2.at(2).compose(poseOdometry).compose(poseError)); newValues3.insert(4, newValues3.at(3).compose(poseOdometry).compose(poseError)); @@ -403,11 +397,9 @@ TEST( ConcurrentIncrementalSmootherDL, synchronize_1 ) Values smootherValues, filterSeparatorValues; filterSeparatorValues.insert(1, Pose3().compose(poseError)); filterSeparatorValues.insert(2, filterSeparatorValues.at(1).compose(poseOdometry).compose(poseError)); - Ordering ordering; - ordering.push_back(1); - ordering.push_back(2); - filterSumarization.add(LinearContainerFactor(PriorFactor(1, poseInitial, noisePrior).linearize(filterSeparatorValues, ordering), ordering, filterSeparatorValues)); - filterSumarization.add(LinearContainerFactor(BetweenFactor(1, 2, poseOdometry, noiseOdometery).linearize(filterSeparatorValues, ordering), ordering, filterSeparatorValues)); + + filterSumarization.push_back(LinearContainerFactor(PriorFactor(1, poseInitial, noisePrior).linearize(filterSeparatorValues), filterSeparatorValues)); + filterSumarization.push_back(LinearContainerFactor(BetweenFactor(1, 2, poseOdometry, noiseOdometery).linearize(filterSeparatorValues), filterSeparatorValues)); // Create expected values: the smoother output will be empty for this case NonlinearFactorGraph expectedSmootherSummarization; @@ -467,15 +459,13 @@ TEST( ConcurrentIncrementalSmootherDL, synchronize_2 ) // Create a separator and cached smoother factors *from* the filter NonlinearFactorGraph smootherFactors, filterSumarization; Values smootherValues, filterSeparatorValues; - Ordering ordering; - ordering.push_back(1); - ordering.push_back(2); + filterSeparatorValues.insert(1, Pose3().compose(poseError)); filterSeparatorValues.insert(2, filterSeparatorValues.at(1).compose(poseOdometry).compose(poseError)); - filterSumarization.add(LinearContainerFactor(PriorFactor(1, poseInitial, noisePrior).linearize(filterSeparatorValues, ordering), ordering, filterSeparatorValues)); - filterSumarization.add(LinearContainerFactor(BetweenFactor(1, 2, poseOdometry, noiseOdometery).linearize(filterSeparatorValues, ordering), ordering, filterSeparatorValues)); - smootherFactors.add(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - smootherFactors.add(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + filterSumarization.push_back(LinearContainerFactor(PriorFactor(1, poseInitial, noisePrior).linearize(filterSeparatorValues), filterSeparatorValues)); + filterSumarization.push_back(LinearContainerFactor(BetweenFactor(1, 2, poseOdometry, noiseOdometery).linearize(filterSeparatorValues), filterSeparatorValues)); + smootherFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + smootherFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); smootherValues.insert(3, filterSeparatorValues.at(2).compose(poseOdometry).compose(poseError)); smootherValues.insert(4, smootherValues.at(3).compose(poseOdometry).compose(poseError)); @@ -548,16 +538,14 @@ TEST( ConcurrentIncrementalSmootherDL, synchronize_3 ) // Create a separator and cached smoother factors *from* the filter NonlinearFactorGraph smootherFactors, filterSumarization; Values smootherValues, filterSeparatorValues; - Ordering ordering; - ordering.push_back(1); - ordering.push_back(2); + filterSeparatorValues.insert(1, Pose3().compose(poseError)); filterSeparatorValues.insert(2, filterSeparatorValues.at(1).compose(poseOdometry).compose(poseError)); - filterSumarization.add(LinearContainerFactor(PriorFactor(1, poseInitial, noisePrior).linearize(filterSeparatorValues, ordering), ordering, filterSeparatorValues)); - filterSumarization.add(LinearContainerFactor(BetweenFactor(1, 2, poseOdometry, noiseOdometery).linearize(filterSeparatorValues, ordering), ordering, filterSeparatorValues)); - smootherFactors.add(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - smootherFactors.add(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - smootherFactors.add(PriorFactor(4, poseInitial, noisePrior)); + filterSumarization.push_back(LinearContainerFactor(PriorFactor(1, poseInitial, noisePrior).linearize(filterSeparatorValues), filterSeparatorValues)); + filterSumarization.push_back(LinearContainerFactor(BetweenFactor(1, 2, poseOdometry, noiseOdometery).linearize(filterSeparatorValues), filterSeparatorValues)); + smootherFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + smootherFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + smootherFactors.push_back(PriorFactor(4, poseInitial, noisePrior)); smootherValues.insert(3, filterSeparatorValues.at(2).compose(poseOdometry).compose(poseError)); smootherValues.insert(4, smootherValues.at(3).compose(poseOdometry).compose(poseError)); @@ -586,23 +574,21 @@ TEST( ConcurrentIncrementalSmootherDL, synchronize_3 ) // Check the optimized value of smoother state NonlinearFactorGraph allFactors = smootherFactors; Values allValues = smoother.getLinearizationPoint(); - ordering = smoother.getOrdering(); // I'm really hoping this is an acceptable ordering... - GaussianFactorGraph::shared_ptr LinFactorGraph = allFactors.linearize(allValues, ordering); + GaussianFactorGraph::shared_ptr LinFactorGraph = allFactors.linearize(allValues); // GaussianSequentialSolver GSS = GaussianSequentialSolver(*LinFactorGraph); // GaussianBayesNet::shared_ptr GBNsptr = GSS.eliminate(); - FastSet allkeys = LinFactorGraph->keys(); + FastSet allkeys = LinFactorGraph->keys(); BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, filterSeparatorValues) { - Index index = ordering.at(key_value.key); - allkeys.erase(index); + allkeys.erase(key_value.key); } - std::vector variables(allkeys.begin(), allkeys.end()); - std::pair result = LinFactorGraph->eliminate(variables, EliminateCholesky); + std::vector variables(allkeys.begin(), allkeys.end()); + std::pair result = LinFactorGraph->eliminatePartialSequential(variables, EliminateCholesky); expectedSmootherSummarization.resize(0); - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, result.second) { - expectedSmootherSummarization.add(LinearContainerFactor(factor, ordering, allValues)); + BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, *result.second) { + expectedSmootherSummarization.push_back(LinearContainerFactor(factor, allValues)); } CHECK(assert_equal(expectedSmootherSummarization, actualSmootherSummarization, 1e-6)); diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp index bdca9238c..1e3bc86c6 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp @@ -23,10 +23,9 @@ #include #include #include -#include #include -#include -#include +#include +#include #include #include #include @@ -39,13 +38,13 @@ namespace { // Set up initial pose, odometry difference, loop closure difference, and initialization errors const Pose3 poseInitial; -const Pose3 poseOdometry( Rot3::RzRyRx(Vector_(3, 0.05, 0.10, -0.75)), Point3(1.0, -0.25, 0.10) ); -const Pose3 poseError( Rot3::RzRyRx(Vector_(3, 0.01, 0.02, -0.1)), Point3(0.05, -0.05, 0.02) ); +const Pose3 poseOdometry( Rot3::RzRyRx((Vector(3) << 0.05, 0.10, -0.75)), Point3(1.0, -0.25, 0.10) ); +const Pose3 poseError( Rot3::RzRyRx((Vector(3) << 0.01, 0.02, -0.1)), Point3(0.05, -0.05, 0.02) ); // Set up noise models for the factors const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); -const SharedDiagonal noiseOdometery = noiseModel::Diagonal::Sigmas(Vector_(6, 0.1, 0.1, 0.1, 0.5, 0.5, 0.5)); -const SharedDiagonal noiseLoop = noiseModel::Diagonal::Sigmas(Vector_(6, 0.25, 0.25, 0.25, 1.0, 1.0, 1.0)); +const SharedDiagonal noiseOdometery = noiseModel::Diagonal::Sigmas((Vector(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5)); +const SharedDiagonal noiseLoop = noiseModel::Diagonal::Sigmas((Vector(6) << 0.25, 0.25, 0.25, 1.0, 1.0, 1.0)); /* ************************************************************************* */ Values BatchOptimize(const NonlinearFactorGraph& graph, const Values& theta, int maxIter = 100) { @@ -58,7 +57,7 @@ Values BatchOptimize(const NonlinearFactorGraph& graph, const Values& theta, int // parameters.lambdaInitial = 1; // parameters.verbosity = NonlinearOptimizerParams::ERROR; // parameters.verbosityLM = ISAM2Params::DAMPED; -// parameters.linearSolverType = SuccessiveLinearizationParams::MULTIFRONTAL_QR; +// parameters.linearSolverType = NonlinearOptimizerParams::MULTIFRONTAL_QR; ISAM2 optimizer(parameters); optimizer.update( graph, theta ); @@ -115,8 +114,8 @@ TEST( ConcurrentIncrementalSmootherGN, getFactors ) // Add some factors to the smoother NonlinearFactorGraph newFactors1; - newFactors1.add(PriorFactor(1, poseInitial, noisePrior)); - newFactors1.add(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + newFactors1.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors1.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues1; newValues1.insert(1, Pose3()); newValues1.insert(2, newValues1.at(1).compose(poseOdometry)); @@ -132,8 +131,8 @@ TEST( ConcurrentIncrementalSmootherGN, getFactors ) // Add some more factors to the smoother NonlinearFactorGraph newFactors2; - newFactors2.add(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - newFactors2.add(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + newFactors2.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + newFactors2.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); Values newValues2; newValues2.insert(3, newValues1.at(2).compose(poseOdometry)); newValues2.insert(4, newValues2.at(3).compose(poseOdometry)); @@ -166,8 +165,8 @@ TEST( ConcurrentIncrementalSmootherGN, getLinearizationPoint ) // Add some factors to the smoother NonlinearFactorGraph newFactors1; - newFactors1.add(PriorFactor(1, poseInitial, noisePrior)); - newFactors1.add(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + newFactors1.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors1.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues1; newValues1.insert(1, Pose3()); newValues1.insert(2, newValues1.at(1).compose(poseOdometry)); @@ -183,8 +182,8 @@ TEST( ConcurrentIncrementalSmootherGN, getLinearizationPoint ) // Add some more factors to the smoother NonlinearFactorGraph newFactors2; - newFactors2.add(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - newFactors2.add(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + newFactors2.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + newFactors2.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); Values newValues2; newValues2.insert(3, newValues1.at(2).compose(poseOdometry)); newValues2.insert(4, newValues2.at(3).compose(poseOdometry)); @@ -200,12 +199,6 @@ TEST( ConcurrentIncrementalSmootherGN, getLinearizationPoint ) CHECK(assert_equal(expected3, actual3)); } -/* ************************************************************************* */ -TEST( ConcurrentIncrementalSmootherGN, getOrdering ) -{ - // TODO: Think about how to check ordering... -} - /* ************************************************************************* */ TEST( ConcurrentIncrementalSmootherGN, getDelta ) { @@ -229,8 +222,8 @@ TEST( ConcurrentIncrementalSmootherGN, calculateEstimate ) // Add some factors to the smoother NonlinearFactorGraph newFactors2; - newFactors2.add(PriorFactor(1, poseInitial, noisePrior)); - newFactors2.add(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + newFactors2.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors2.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues2; newValues2.insert(1, Pose3().compose(poseError)); newValues2.insert(2, newValues2.at(1).compose(poseOdometry).compose(poseError)); @@ -249,8 +242,8 @@ TEST( ConcurrentIncrementalSmootherGN, calculateEstimate ) // Add some more factors to the smoother NonlinearFactorGraph newFactors3; - newFactors3.add(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - newFactors3.add(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + newFactors3.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + newFactors3.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); Values newValues3; newValues3.insert(3, newValues2.at(2).compose(poseOdometry).compose(poseError)); newValues3.insert(4, newValues3.at(3).compose(poseOdometry).compose(poseError)); @@ -316,8 +309,8 @@ TEST( ConcurrentIncrementalSmootherGN, update_multiple ) // Add some factors to the smoother NonlinearFactorGraph newFactors2; - newFactors2.add(PriorFactor(1, poseInitial, noisePrior)); - newFactors2.add(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + newFactors2.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors2.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues2; newValues2.insert(1, Pose3().compose(poseError)); newValues2.insert(2, newValues2.at(1).compose(poseOdometry).compose(poseError)); @@ -336,8 +329,8 @@ TEST( ConcurrentIncrementalSmootherGN, update_multiple ) // Add some more factors to the smoother NonlinearFactorGraph newFactors3; - newFactors3.add(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - newFactors3.add(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + newFactors3.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + newFactors3.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); Values newValues3; newValues3.insert(3, newValues2.at(2).compose(poseOdometry).compose(poseError)); newValues3.insert(4, newValues3.at(3).compose(poseOdometry).compose(poseError)); @@ -406,8 +399,8 @@ TEST( ConcurrentIncrementalSmootherGN, synchronize_1 ) Ordering ordering; ordering.push_back(1); ordering.push_back(2); - filterSumarization.add(LinearContainerFactor(PriorFactor(1, poseInitial, noisePrior).linearize(filterSeparatorValues, ordering), ordering, filterSeparatorValues)); - filterSumarization.add(LinearContainerFactor(BetweenFactor(1, 2, poseOdometry, noiseOdometery).linearize(filterSeparatorValues, ordering), ordering, filterSeparatorValues)); + filterSumarization.push_back(LinearContainerFactor(PriorFactor(1, poseInitial, noisePrior).linearize(filterSeparatorValues), filterSeparatorValues)); + filterSumarization.push_back(LinearContainerFactor(BetweenFactor(1, 2, poseOdometry, noiseOdometery).linearize(filterSeparatorValues), filterSeparatorValues)); // Create expected values: the smoother output will be empty for this case NonlinearFactorGraph expectedSmootherSummarization; @@ -467,15 +460,13 @@ TEST( ConcurrentIncrementalSmootherGN, synchronize_2 ) // Create a separator and cached smoother factors *from* the filter NonlinearFactorGraph smootherFactors, filterSumarization; Values smootherValues, filterSeparatorValues; - Ordering ordering; - ordering.push_back(1); - ordering.push_back(2); + filterSeparatorValues.insert(1, Pose3().compose(poseError)); filterSeparatorValues.insert(2, filterSeparatorValues.at(1).compose(poseOdometry).compose(poseError)); - filterSumarization.add(LinearContainerFactor(PriorFactor(1, poseInitial, noisePrior).linearize(filterSeparatorValues, ordering), ordering, filterSeparatorValues)); - filterSumarization.add(LinearContainerFactor(BetweenFactor(1, 2, poseOdometry, noiseOdometery).linearize(filterSeparatorValues, ordering), ordering, filterSeparatorValues)); - smootherFactors.add(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - smootherFactors.add(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + filterSumarization.push_back(LinearContainerFactor(PriorFactor(1, poseInitial, noisePrior).linearize(filterSeparatorValues), filterSeparatorValues)); + filterSumarization.push_back(LinearContainerFactor(BetweenFactor(1, 2, poseOdometry, noiseOdometery).linearize(filterSeparatorValues), filterSeparatorValues)); + smootherFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + smootherFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); smootherValues.insert(3, filterSeparatorValues.at(2).compose(poseOdometry).compose(poseError)); smootherValues.insert(4, smootherValues.at(3).compose(poseOdometry).compose(poseError)); @@ -548,16 +539,14 @@ TEST( ConcurrentIncrementalSmootherGN, synchronize_3 ) // Create a separator and cached smoother factors *from* the filter NonlinearFactorGraph smootherFactors, filterSumarization; Values smootherValues, filterSeparatorValues; - Ordering ordering; - ordering.push_back(1); - ordering.push_back(2); + filterSeparatorValues.insert(1, Pose3().compose(poseError)); filterSeparatorValues.insert(2, filterSeparatorValues.at(1).compose(poseOdometry).compose(poseError)); - filterSumarization.add(LinearContainerFactor(PriorFactor(1, poseInitial, noisePrior).linearize(filterSeparatorValues, ordering), ordering, filterSeparatorValues)); - filterSumarization.add(LinearContainerFactor(BetweenFactor(1, 2, poseOdometry, noiseOdometery).linearize(filterSeparatorValues, ordering), ordering, filterSeparatorValues)); - smootherFactors.add(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - smootherFactors.add(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - smootherFactors.add(PriorFactor(4, poseInitial, noisePrior)); + filterSumarization.push_back(LinearContainerFactor(PriorFactor(1, poseInitial, noisePrior).linearize(filterSeparatorValues), filterSeparatorValues)); + filterSumarization.push_back(LinearContainerFactor(BetweenFactor(1, 2, poseOdometry, noiseOdometery).linearize(filterSeparatorValues), filterSeparatorValues)); + smootherFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + smootherFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + smootherFactors.push_back(PriorFactor(4, poseInitial, noisePrior)); smootherValues.insert(3, filterSeparatorValues.at(2).compose(poseOdometry).compose(poseError)); smootherValues.insert(4, smootherValues.at(3).compose(poseOdometry).compose(poseError)); @@ -586,29 +575,249 @@ TEST( ConcurrentIncrementalSmootherGN, synchronize_3 ) // Check the optimized value of smoother state NonlinearFactorGraph allFactors = smootherFactors; Values allValues = smoother.getLinearizationPoint(); - ordering = smoother.getOrdering(); // I'm really hoping this is an acceptable ordering... +// ordering = smoother.getOrdering(); // I'm really hoping this is an acceptable ordering... - GaussianFactorGraph::shared_ptr LinFactorGraph = allFactors.linearize(allValues, ordering); + GaussianFactorGraph::shared_ptr LinFactorGraph = allFactors.linearize(allValues); // GaussianSequentialSolver GSS = GaussianSequentialSolver(*LinFactorGraph); // GaussianBayesNet::shared_ptr GBNsptr = GSS.eliminate(); - FastSet allkeys = LinFactorGraph->keys(); - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, filterSeparatorValues) { - Index index = ordering.at(key_value.key); - allkeys.erase(index); - } - std::vector variables(allkeys.begin(), allkeys.end()); - std::pair result = LinFactorGraph->eliminate(variables, EliminateCholesky); + FastSet allkeys = LinFactorGraph->keys(); + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, filterSeparatorValues) + allkeys.erase(key_value.key); + std::vector variables(allkeys.begin(), allkeys.end()); + std::pair result = LinFactorGraph->eliminatePartialSequential(variables, EliminateCholesky); expectedSmootherSummarization.resize(0); - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, result.second) { - expectedSmootherSummarization.add(LinearContainerFactor(factor, ordering, allValues)); + BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, *result.second) { + expectedSmootherSummarization.push_back(LinearContainerFactor(factor, allValues)); } CHECK(assert_equal(expectedSmootherSummarization, actualSmootherSummarization, 1e-6)); } +// ========================================================================================================= +///* ************************************************************************* */ +TEST( ConcurrentIncrementalSmoother, removeFactors_topology_1 ) +{ + std::cout << "*********************** removeFactors_topology_1 ************************" << std::endl; + + // Create a set of optimizer parameters + ISAM2Params parameters; + parameters.optimizationParams = ISAM2GaussNewtonParams(); + + // Create a Concurrent Batch Smoother + ConcurrentIncrementalSmoother smoother(parameters); + + // Add some factors to the smoother + NonlinearFactorGraph newFactors; + newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + + Values newValues; + newValues.insert(1, Pose3().compose(poseError)); + newValues.insert(2, newValues.at(1).compose(poseOdometry).compose(poseError)); + newValues.insert(3, newValues.at(2).compose(poseOdometry).compose(poseError)); + newValues.insert(4, newValues.at(3).compose(poseOdometry).compose(poseError)); + + // Update the smoother: add all factors + smoother.update(newFactors, newValues); + + // factor we want to remove + // NOTE: we can remove factors, paying attention that the remaining graph remains connected + // we remove a single factor, the number 1, which is a BetweenFactor(1, 2, poseOdometry, noiseOdometery); + std::vector removeFactorIndices(2,1); + + + + // Add no factors to the smoother (we only want to test the removal) + NonlinearFactorGraph noFactors; + Values noValues; + smoother.update(noFactors, noValues, removeFactorIndices); + + NonlinearFactorGraph actualGraph = smoother.getFactors(); + + actualGraph.print("actual graph: \n"); + + NonlinearFactorGraph expectedGraph; + expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); + // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + // we should add an empty one, so that the ordering and labeling of the factors is preserved + expectedGraph.push_back(NonlinearFactor::shared_ptr()); + expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + +// CHECK(assert_equal(expectedGraph, actualGraph, 1e-6)); +} + +/////* ************************************************************************* */ +//TEST( ConcurrentIncrementalSmoother, removeFactors_topology_2 ) +//{ +// std::cout << "*********************** removeFactors_topology_2 ************************" << std::endl; +// // we try removing the last factor +// +// // Create a set of optimizer parameters +// LevenbergMarquardtParams parameters; +// +// // Create a Concurrent Batch Smoother +// ConcurrentBatchSmoother smoother(parameters); +// +// // Add some factors to the smoother +// NonlinearFactorGraph newFactors; +// newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); +// newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); +// newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); +// newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); +// newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); +// +// Values newValues; +// newValues.insert(1, Pose3().compose(poseError)); +// newValues.insert(2, newValues.at(1).compose(poseOdometry).compose(poseError)); +// newValues.insert(3, newValues.at(2).compose(poseOdometry).compose(poseError)); +// newValues.insert(4, newValues.at(3).compose(poseOdometry).compose(poseError)); +// +// // Update the smoother: add all factors +// smoother.update(newFactors, newValues); +// +// // factor we want to remove +// // NOTE: we can remove factors, paying attention that the remaining graph remains connected +// // we remove a single factor, the number 1, which is a BetweenFactor(1, 2, poseOdometry, noiseOdometery); +// std::vector removeFactorIndices(1,4); +// +// // Add no factors to the smoother (we only want to test the removal) +// NonlinearFactorGraph noFactors; +// Values noValues; +// smoother.update(noFactors, noValues, removeFactorIndices); +// +// NonlinearFactorGraph actualGraph = smoother.getFactors(); +// +// NonlinearFactorGraph expectedGraph; +// expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); +// expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); +// expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); +// expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); +// // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); +// // we should add an empty one, so that the ordering and labeling of the factors is preserved +// expectedGraph.push_back(NonlinearFactor::shared_ptr()); +// +// CHECK(assert_equal(expectedGraph, actualGraph, 1e-6)); +//} +// +// +/////* ************************************************************************* */ +//TEST( ConcurrentBatchSmoother, removeFactors_topology_3 ) +//{ +// std::cout << "*********************** removeFactors_topology_3 ************************" << std::endl; +// // we try removing the first factor +// +// // Create a set of optimizer parameters +// LevenbergMarquardtParams parameters; +// ConcurrentBatchSmoother Smoother(parameters); +// +// // Add some factors to the Smoother +// NonlinearFactorGraph newFactors; +// newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); +// newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); +// newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); +// newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); +// newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); +// +// Values newValues; +// newValues.insert(1, Pose3().compose(poseError)); +// newValues.insert(2, newValues.at(1).compose(poseOdometry).compose(poseError)); +// newValues.insert(3, newValues.at(2).compose(poseOdometry).compose(poseError)); +// newValues.insert(4, newValues.at(3).compose(poseOdometry).compose(poseError)); +// +// // Update the Smoother: add all factors +// Smoother.update(newFactors, newValues); +// +// // factor we want to remove +// // NOTE: we can remove factors, paying attention that the remaining graph remains connected +// // we remove a single factor, the number 0, which is a BetweenFactor(1, 2, poseOdometry, noiseOdometery); +// std::vector removeFactorIndices(1,0); +// +// // Add no factors to the Smoother (we only want to test the removal) +// NonlinearFactorGraph noFactors; +// Values noValues; +// Smoother.update(noFactors, noValues, removeFactorIndices); +// +// NonlinearFactorGraph actualGraph = Smoother.getFactors(); +// +// NonlinearFactorGraph expectedGraph; +// // we should add an empty one, so that the ordering and labeling of the factors is preserved +// expectedGraph.push_back(NonlinearFactor::shared_ptr()); +// expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); +// expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); +// expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); +// expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); +// +// CHECK(assert_equal(expectedGraph, actualGraph, 1e-6)); +//} +// +/////* ************************************************************************* */ +//TEST( ConcurrentBatchSmoother, removeFactors_values ) +//{ +// std::cout << "*********************** removeFactors_values ************************" << std::endl; +// // we try removing the last factor +// +// // Create a set of optimizer parameters +// LevenbergMarquardtParams parameters; +// ConcurrentBatchSmoother Smoother(parameters); +// +// // Add some factors to the Smoother +// NonlinearFactorGraph newFactors; +// newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); +// newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); +// newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); +// newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); +// newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); +// +// Values newValues; +// newValues.insert(1, Pose3().compose(poseError)); +// newValues.insert(2, newValues.at(1).compose(poseOdometry).compose(poseError)); +// newValues.insert(3, newValues.at(2).compose(poseOdometry).compose(poseError)); +// newValues.insert(4, newValues.at(3).compose(poseOdometry).compose(poseError)); +// +// // Update the Smoother: add all factors +// Smoother.update(newFactors, newValues); +// +// // factor we want to remove +// // NOTE: we can remove factors, paying attention that the remaining graph remains connected +// // we remove a single factor, the number 4, which is a BetweenFactor(1, 2, poseOdometry, noiseOdometery); +// std::vector removeFactorIndices(1,4); +// +// // Add no factors to the Smoother (we only want to test the removal) +// NonlinearFactorGraph noFactors; +// Values noValues; +// Smoother.update(noFactors, noValues, removeFactorIndices); +// +// NonlinearFactorGraph actualGraph = Smoother.getFactors(); +// Values actualValues = Smoother.calculateEstimate(); +// +// // note: factors are removed before the optimization +// NonlinearFactorGraph expectedGraph; +// expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); +// expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); +// expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); +// expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); +// // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); +// // we should add an empty one, so that the ordering and labeling of the factors is preserved +// expectedGraph.push_back(NonlinearFactor::shared_ptr()); +// +// // Calculate expected factor graph and values +// Values expectedValues = BatchOptimize(expectedGraph, newValues); +// +// CHECK(assert_equal(expectedGraph, actualGraph, 1e-6)); +// CHECK(assert_equal(expectedValues, actualValues, 1e-6)); +//} + + + + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ diff --git a/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp index 73bb8c0a3..afcd60ae3 100644 --- a/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp @@ -19,17 +19,17 @@ #include #include +#include +#include +#include +#include +#include +#include #include #include -#include #include #include -#include -#include -#include -#include -#include -#include +#include using namespace std; using namespace gtsam; @@ -39,11 +39,9 @@ Key MakeKey(size_t index) { return Symbol('x', index); } /* ************************************************************************* */ bool check_smoother(const NonlinearFactorGraph& fullgraph, const Values& fullinit, const IncrementalFixedLagSmoother& smoother, const Key& key) { - Ordering ordering = *fullgraph.orderingCOLAMD(fullinit); - GaussianFactorGraph linearized = *fullgraph.linearize(fullinit, ordering); - GaussianBayesNet gbn = *GaussianSequentialSolver(linearized).eliminate(); - VectorValues delta = optimize(gbn); - Values fullfinal = fullinit.retract(delta, ordering); + GaussianFactorGraph linearized = *fullgraph.linearize(fullinit); + VectorValues delta = linearized.optimize(); + Values fullfinal = fullinit.retract(delta); Point2 expected = fullfinal.at(key); Point2 actual = smoother.calculateEstimate(key); @@ -52,7 +50,7 @@ bool check_smoother(const NonlinearFactorGraph& fullgraph, const Values& fullini } /* ************************************************************************* */ -TEST_UNSAFE( IncrementalFixedLagSmoother, Example ) +TEST( IncrementalFixedLagSmoother, Example ) { // Test the IncrementalFixedLagSmoother in a pure linear environment. Thus, full optimization and // the IncrementalFixedLagSmoother should be identical (even with the linearized approximations at @@ -61,8 +59,8 @@ TEST_UNSAFE( IncrementalFixedLagSmoother, Example ) SETDEBUG("IncrementalFixedLagSmoother update", true); // Set up parameters - SharedDiagonal odometerNoise = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); - SharedDiagonal loopNoise = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); + SharedDiagonal odometerNoise = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.1)); + SharedDiagonal loopNoise = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.1)); // Create a Fixed-Lag Smoother typedef IncrementalFixedLagSmoother::KeyTimestampMap Timestamps; @@ -85,7 +83,7 @@ TEST_UNSAFE( IncrementalFixedLagSmoother, Example ) Values newValues; Timestamps newTimestamps; - newFactors.add(PriorFactor(key0, Point2(0.0, 0.0), odometerNoise)); + newFactors.push_back(PriorFactor(key0, Point2(0.0, 0.0), odometerNoise)); newValues.insert(key0, Point2(0.01, 0.01)); newTimestamps[key0] = 0.0; @@ -110,7 +108,7 @@ TEST_UNSAFE( IncrementalFixedLagSmoother, Example ) Values newValues; Timestamps newTimestamps; - newFactors.add(BetweenFactor(key1, key2, Point2(1.0, 0.0), odometerNoise)); + newFactors.push_back(BetweenFactor(key1, key2, Point2(1.0, 0.0), odometerNoise)); newValues.insert(key2, Point2(double(i)+0.1, -0.1)); newTimestamps[key2] = double(i); @@ -136,8 +134,8 @@ TEST_UNSAFE( IncrementalFixedLagSmoother, Example ) Values newValues; Timestamps newTimestamps; - newFactors.add(BetweenFactor(key1, key2, Point2(1.0, 0.0), odometerNoise)); - newFactors.add(BetweenFactor(MakeKey(2), MakeKey(5), Point2(3.5, 0.0), loopNoise)); + newFactors.push_back(BetweenFactor(key1, key2, Point2(1.0, 0.0), odometerNoise)); + newFactors.push_back(BetweenFactor(MakeKey(2), MakeKey(5), Point2(3.5, 0.0), loopNoise)); newValues.insert(key2, Point2(double(i)+0.1, -0.1)); newTimestamps[key2] = double(i); @@ -162,7 +160,7 @@ TEST_UNSAFE( IncrementalFixedLagSmoother, Example ) Values newValues; Timestamps newTimestamps; - newFactors.add(BetweenFactor(key1, key2, Point2(1.0, 0.0), odometerNoise)); + newFactors.push_back(BetweenFactor(key1, key2, Point2(1.0, 0.0), odometerNoise)); newValues.insert(key2, Point2(double(i)+0.1, -0.1)); newTimestamps[key2] = double(i); diff --git a/gtsam_unstable/nonlinear/tests/testLinearizedFactor.cpp b/gtsam_unstable/nonlinear/tests/testLinearizedFactor.cpp index 426683806..21e271bd8 100644 --- a/gtsam_unstable/nonlinear/tests/testLinearizedFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testLinearizedFactor.cpp @@ -17,12 +17,12 @@ */ #include -#include +#include #include +#include #include #include -#include -#include +#include #include using namespace gtsam; @@ -33,9 +33,6 @@ TEST( LinearizedFactor, equals_jacobian ) // Create a Between Factor from a Point3. This is actually a linear factor. Key x1(1); Key x2(2); - Ordering ordering; - ordering.push_back(x1); - ordering.push_back(x2); Values values; values.insert(x1, Point3(-22.4, +8.5, +2.4)); values.insert(x2, Point3(-21.0, +5.0, +21.0)); @@ -46,9 +43,9 @@ TEST( LinearizedFactor, equals_jacobian ) // Create two identical factors and make sure they're equal - JacobianFactor::shared_ptr jf = boost::static_pointer_cast(betweenFactor.linearize(values, ordering)); - LinearizedJacobianFactor jacobian1(jf, ordering, values); - LinearizedJacobianFactor jacobian2(jf, ordering, values); + JacobianFactor::shared_ptr jf = boost::static_pointer_cast(betweenFactor.linearize(values)); + LinearizedJacobianFactor jacobian1(jf, values); + LinearizedJacobianFactor jacobian2(jf, values); CHECK(assert_equal(jacobian1, jacobian2)); } @@ -59,9 +56,6 @@ TEST( LinearizedFactor, clone_jacobian ) // Create a Between Factor from a Point3. This is actually a linear factor. Key x1(1); Key x2(2); - Ordering ordering; - ordering.push_back(x1); - ordering.push_back(x2); Values values; values.insert(x1, Point3(-22.4, +8.5, +2.4)); values.insert(x2, Point3(-21.0, +5.0, +21.0)); @@ -71,13 +65,13 @@ TEST( LinearizedFactor, clone_jacobian ) BetweenFactor betweenFactor(x1, x2, measured, model); // Create one factor that is a clone of the other and make sure they're equal - JacobianFactor::shared_ptr jf = boost::static_pointer_cast(betweenFactor.linearize(values, ordering)); - LinearizedJacobianFactor jacobian1(jf, ordering, values); + JacobianFactor::shared_ptr jf = boost::static_pointer_cast(betweenFactor.linearize(values)); + LinearizedJacobianFactor jacobian1(jf, values); LinearizedJacobianFactor::shared_ptr jacobian2 = boost::static_pointer_cast(jacobian1.clone()); CHECK(assert_equal(jacobian1, *jacobian2)); - JacobianFactor::shared_ptr jf1 = boost::static_pointer_cast(jacobian1.linearize(values, ordering)); - JacobianFactor::shared_ptr jf2 = boost::static_pointer_cast(jacobian2->linearize(values, ordering)); + JacobianFactor::shared_ptr jf1 = boost::static_pointer_cast(jacobian1.linearize(values)); + JacobianFactor::shared_ptr jf2 = boost::static_pointer_cast(jacobian2->linearize(values)); CHECK(assert_equal(*jf1, *jf2)); Matrix information1 = jf1->augmentedInformation(); @@ -91,9 +85,6 @@ TEST( LinearizedFactor, add_jacobian ) // Create a Between Factor from a Point3. This is actually a linear factor. Key x1(1); Key x2(2); - Ordering ordering; - ordering.push_back(x1); - ordering.push_back(x2); Values values; values.insert(x1, Point3(-22.4, +8.5, +2.4)); values.insert(x2, Point3(-21.0, +5.0, +21.0)); @@ -103,10 +94,10 @@ TEST( LinearizedFactor, add_jacobian ) BetweenFactor betweenFactor(x1, x2, measured, model); // Create two factor graphs, one using 'push_back' and one using 'add' and make sure they're equal - JacobianFactor::shared_ptr jf = boost::static_pointer_cast(betweenFactor.linearize(values, ordering)); - LinearizedJacobianFactor::shared_ptr jacobian(new LinearizedJacobianFactor(jf, ordering, values)); + JacobianFactor::shared_ptr jf = boost::static_pointer_cast(betweenFactor.linearize(values)); + LinearizedJacobianFactor::shared_ptr jacobian(new LinearizedJacobianFactor(jf, values)); NonlinearFactorGraph graph1; graph1.push_back(jacobian); - NonlinearFactorGraph graph2; graph2.add(*jacobian); + NonlinearFactorGraph graph2; graph2.push_back(*jacobian); // TODO: When creating a Jacobian from a cached factor, I experienced a problem in the 'add' version // However, I am currently unable to reproduce the error in this unit test. @@ -133,8 +124,8 @@ TEST( LinearizedFactor, add_jacobian ) // // // // Create a linearized jacobian factors -// JacobianFactor::shared_ptr jf = boost::static_pointer_cast(betweenFactor.linearize(values, ordering)); -// LinearizedJacobianFactor jacobian(jf, ordering, values); +// JacobianFactor::shared_ptr jf = boost::static_pointer_cast(betweenFactor.linearize(values)); +// LinearizedJacobianFactor jacobian(jf, values); // // // for(double x1 = -10; x1 < 10; x1 += 2.0) { @@ -156,8 +147,8 @@ TEST( LinearizedFactor, add_jacobian ) // EXPECT_DOUBLES_EQUAL(expected_error, actual_error, 1e-9 ); // // // Check that the linearized factors are identical -// GaussianFactor::shared_ptr expected_factor = betweenFactor.linearize(linpoint, ordering); -// GaussianFactor::shared_ptr actual_factor = jacobian.linearize(linpoint, ordering); +// GaussianFactor::shared_ptr expected_factor = betweenFactor.linearize(linpoint); +// GaussianFactor::shared_ptr actual_factor = jacobian.linearize(linpoint); // CHECK(assert_equal(*expected_factor, *actual_factor)); // } // } @@ -175,9 +166,6 @@ TEST( LinearizedFactor, equals_hessian ) // Create a Between Factor from a Point3. This is actually a linear factor. Key x1(1); Key x2(2); - Ordering ordering; - ordering.push_back(x1); - ordering.push_back(x2); Values values; values.insert(x1, Point3(-22.4, +8.5, +2.4)); values.insert(x2, Point3(-21.0, +5.0, +21.0)); @@ -188,10 +176,10 @@ TEST( LinearizedFactor, equals_hessian ) // Create two identical factors and make sure they're equal - JacobianFactor::shared_ptr jf = boost::static_pointer_cast(betweenFactor.linearize(values, ordering)); + JacobianFactor::shared_ptr jf = boost::static_pointer_cast(betweenFactor.linearize(values)); HessianFactor::shared_ptr hf(new HessianFactor(*jf)); - LinearizedHessianFactor hessian1(hf, ordering, values); - LinearizedHessianFactor hessian2(hf, ordering, values); + LinearizedHessianFactor hessian1(hf, values); + LinearizedHessianFactor hessian2(hf, values); CHECK(assert_equal(hessian1, hessian2)); } @@ -202,9 +190,6 @@ TEST( LinearizedFactor, clone_hessian ) // Create a Between Factor from a Point3. This is actually a linear factor. Key x1(1); Key x2(2); - Ordering ordering; - ordering.push_back(x1); - ordering.push_back(x2); Values values; values.insert(x1, Point3(-22.4, +8.5, +2.4)); values.insert(x2, Point3(-21.0, +5.0, +21.0)); @@ -215,9 +200,9 @@ TEST( LinearizedFactor, clone_hessian ) // Create two identical factors and make sure they're equal - JacobianFactor::shared_ptr jf = boost::static_pointer_cast(betweenFactor.linearize(values, ordering)); + JacobianFactor::shared_ptr jf = boost::static_pointer_cast(betweenFactor.linearize(values)); HessianFactor::shared_ptr hf(new HessianFactor(*jf)); - LinearizedHessianFactor hessian1(hf, ordering, values); + LinearizedHessianFactor hessian1(hf, values); LinearizedHessianFactor::shared_ptr hessian2 = boost::static_pointer_cast(hessian1.clone()); CHECK(assert_equal(hessian1, *hessian2)); @@ -229,9 +214,6 @@ TEST( LinearizedFactor, add_hessian ) // Create a Between Factor from a Point3. This is actually a linear factor. Key x1(1); Key x2(2); - Ordering ordering; - ordering.push_back(x1); - ordering.push_back(x2); Values values; values.insert(x1, Point3(-22.4, +8.5, +2.4)); values.insert(x2, Point3(-21.0, +5.0, +21.0)); @@ -242,11 +224,11 @@ TEST( LinearizedFactor, add_hessian ) // Create two identical factors and make sure they're equal - JacobianFactor::shared_ptr jf = boost::static_pointer_cast(betweenFactor.linearize(values, ordering)); + JacobianFactor::shared_ptr jf = boost::static_pointer_cast(betweenFactor.linearize(values)); HessianFactor::shared_ptr hf(new HessianFactor(*jf)); - LinearizedHessianFactor::shared_ptr hessian(new LinearizedHessianFactor(hf, ordering, values)); + LinearizedHessianFactor::shared_ptr hessian(new LinearizedHessianFactor(hf, values)); NonlinearFactorGraph graph1; graph1.push_back(hessian); - NonlinearFactorGraph graph2; graph2.add(*hessian); + NonlinearFactorGraph graph2; graph2.push_back(*hessian); CHECK(assert_equal(graph1, graph2)); } @@ -270,9 +252,9 @@ TEST( LinearizedFactor, add_hessian ) // // // // Create a linearized hessian factor -// JacobianFactor::shared_ptr jf = boost::static_pointer_cast(betweenFactor.linearize(values, ordering)); +// JacobianFactor::shared_ptr jf = boost::static_pointer_cast(betweenFactor.linearize(values)); // HessianFactor::shared_ptr hf(new HessianFactor(*jf)); -// LinearizedHessianFactor hessian(hf, ordering, values); +// LinearizedHessianFactor hessian(hf, values); // // // for(double x1 = -10; x1 < 10; x1 += 2.0) { @@ -294,8 +276,8 @@ TEST( LinearizedFactor, add_hessian ) // EXPECT_DOUBLES_EQUAL(expected_error, actual_error, 1e-9 ); // // // Check that the linearized factors are identical -// GaussianFactor::shared_ptr expected_factor = HessianFactor::shared_ptr(new HessianFactor(*betweenFactor.linearize(linpoint, ordering))); -// GaussianFactor::shared_ptr actual_factor = hessian.linearize(linpoint, ordering); +// GaussianFactor::shared_ptr expected_factor = HessianFactor::shared_ptr(new HessianFactor(*betweenFactor.linearize(linpoint))); +// GaussianFactor::shared_ptr actual_factor = hessian.linearize(linpoint); // CHECK(assert_equal(*expected_factor, *actual_factor)); // } // } diff --git a/gtsam_unstable/nonlinear/tests/testParticleFactor.cpp b/gtsam_unstable/nonlinear/tests/testParticleFactor.cpp new file mode 100644 index 000000000..cea3c0184 --- /dev/null +++ b/gtsam_unstable/nonlinear/tests/testParticleFactor.cpp @@ -0,0 +1,201 @@ +/* ---------------------------------------------------------------------------- + + * 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 testParticleFactor.cpp + * @brief Unit tests for the Particle factor + * @author Frank Dellaert + * @date Dec 9, 2013 + */ + +#include +#include + +namespace gtsam { + +/** + * A factor approximating a density on a variable as a set of weighted particles + */ +template +class ParticleFactor { + +public: + typedef ParticleFactor This; ///< Typedef to this class + typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class + +}; + +/** + * A particle filter class, styled on functional KalmanFilter + * A state is created, which is functionally updates through [predict] and [update] + */ +template +class ParticleFilter { + +public: + + /** + * The Particle filter state is simply a ParticleFactor + */ + typedef typename ParticleFactor::shared_ptr State; + + /** + * Create initial state, i.e., prior density at time k=0 + * In Bayes Filter notation, these are x_{0|0} and P_{0|0} + * @param x0 estimate at time 0 + * @param P0 covariance at time 0, given as a diagonal Gaussian 'model' + */ + State init(const Vector& x0, const SharedDiagonal& P0) { + return boost::make_shared >(); + } + +}; +// ParticleFilter + +}// namespace gtsam + +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +//****************************************************************************** + +TEST( particleFactor, constructor ) { +// ParticleFactor pf; + //CHECK(assert_equal(expected, actual)); +} + +//****************************************************************************** +// Tests to do: +// Take two variables pf-x-*-y, eliminate x, multiply and sample then marginalize +TEST( particleFactor, eliminate) { +// ParticleFactor fx; + BetweenFactor fxy; + +} + +//****************************************************************************** + +/** Small 2D point class implemented as a Vector */ +struct State: Vector { + State(double x, double y) : + Vector((Vector(2) << x, y)) { + } +}; + +//****************************************************************************** +TEST( ParticleFilter, constructor) { + +// Create a Kalman filter of dimension 2 + ParticleFilter pf1; + +// Create inital mean/covariance + State x_initial(0.0, 0.0); + SharedDiagonal P1 = noiseModel::Isotropic::Sigma(2, 0.1); + +// Get initial state by passing initial mean/covariance to the filter + ParticleFilter::State p1 = pf1.init(x_initial, P1); + +// // Assert it has the correct mean, covariance and information +// EXPECT(assert_equal(x_initial, p1->mean())); +// Matrix Sigma = (Mat(2, 2) << 0.01, 0.0, 0.0, 0.01); +// EXPECT(assert_equal(Sigma, p1->covariance())); +// EXPECT(assert_equal(inverse(Sigma), p1->information())); +// +// // Create one with a sharedGaussian +// KalmanFilter::State p2 = pf1.init(x_initial, Sigma); +// EXPECT(assert_equal(Sigma, p2->covariance())); +// +// // Now make sure both agree +// EXPECT(assert_equal(p1->covariance(), p2->covariance())); +} + +//****************************************************************************** +TEST( ParticleFilter, linear1 ) { + + // Create the controls and measurement properties for our example + Matrix F = eye(2, 2); + Matrix B = eye(2, 2); + Vector u = (Vector(2) << 1.0, 0.0); + SharedDiagonal modelQ = noiseModel::Isotropic::Sigma(2, 0.1); + Matrix Q = 0.01 * eye(2, 2); + Matrix H = eye(2, 2); + State z1(1.0, 0.0); + State z2(2.0, 0.0); + State z3(3.0, 0.0); + SharedDiagonal modelR = noiseModel::Isotropic::Sigma(2, 0.1); + Matrix R = 0.01 * eye(2, 2); + +// Create the set of expected output TestValues + State expected0(0.0, 0.0); + Matrix P00 = 0.01 * eye(2, 2); + + State expected1(1.0, 0.0); + Matrix P01 = P00 + Q; + Matrix I11 = inverse(P01) + inverse(R); + + State expected2(2.0, 0.0); + Matrix P12 = inverse(I11) + Q; + Matrix I22 = inverse(P12) + inverse(R); + + State expected3(3.0, 0.0); + Matrix P23 = inverse(I22) + Q; + Matrix I33 = inverse(P23) + inverse(R); + +// Create a Kalman filter of dimension 2 + KalmanFilter kf(2); + +// Create the Kalman Filter initialization point + State x_initial(0.0, 0.0); + SharedDiagonal P_initial = noiseModel::Isotropic::Sigma(2, 0.1); + +// Create initial KalmanFilter object + KalmanFilter::State p0 = kf.init(x_initial, P_initial); + EXPECT(assert_equal(expected0, p0->mean())); + EXPECT(assert_equal(P00, p0->covariance())); + +// Run iteration 1 + KalmanFilter::State p1p = kf.predict(p0, F, B, u, modelQ); + EXPECT(assert_equal(expected1, p1p->mean())); + EXPECT(assert_equal(P01, p1p->covariance())); + + KalmanFilter::State p1 = kf.update(p1p, H, z1, modelR); + EXPECT(assert_equal(expected1, p1->mean())); + EXPECT(assert_equal(I11, p1->information())); + +// Run iteration 2 (with full covariance) + KalmanFilter::State p2p = kf.predictQ(p1, F, B, u, Q); + EXPECT(assert_equal(expected2, p2p->mean())); + + KalmanFilter::State p2 = kf.update(p2p, H, z2, modelR); + EXPECT(assert_equal(expected2, p2->mean())); + +// Run iteration 3 + KalmanFilter::State p3p = kf.predict(p2, F, B, u, modelQ); + EXPECT(assert_equal(expected3, p3p->mean())); + LONGS_EQUAL(3, (long)KalmanFilter::step(p3p)); + + KalmanFilter::State p3 = kf.update(p3p, H, z3, modelR); + EXPECT(assert_equal(expected3, p3->mean())); + LONGS_EQUAL(3, (long)KalmanFilter::step(p3)); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** + diff --git a/gtsam_unstable/partition/CMakeLists.txt b/gtsam_unstable/partition/CMakeLists.txt new file mode 100644 index 000000000..d8d254513 --- /dev/null +++ b/gtsam_unstable/partition/CMakeLists.txt @@ -0,0 +1,7 @@ +# Install headers +file(GLOB partition_headers "*.h") +install(FILES ${partition_headers} DESTINATION include/gtsam_unstable/parition) + +set(ignore_test "tests/testNestedDissection.cpp") +# Add all tests +gtsamAddTestsGlob(partition_unstable "tests/*.cpp" "${ignore_test}" "gtsam_unstable;metis") diff --git a/gtsam_unstable/partition/FindSeparator-inl.h b/gtsam_unstable/partition/FindSeparator-inl.h new file mode 100644 index 000000000..dacbebc76 --- /dev/null +++ b/gtsam_unstable/partition/FindSeparator-inl.h @@ -0,0 +1,564 @@ +/* + * FindSeparator-inl.h + * + * Created on: Nov 23, 2010 + * Updated: Feb 20. 2014 + * Author: nikai + * Author: Andrew Melim + * Description: find the separator of bisectioning for a given graph + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +extern "C" { +#include +#include "metislib.h" +} + +#include "FindSeparator.h" + +namespace gtsam { namespace partition { + + typedef boost::shared_array sharedInts; + + /* ************************************************************************* */ + /** + * Return the size of the separator and the partiion indices {part} + * Part [j] is 0, 1, or 2, depending on + * whether node j is in the left part of the graph, the right part, or the + * separator, respectively + */ + std::pair separatorMetis(idx_t n, const sharedInts& xadj, + const sharedInts& adjncy, const sharedInts& adjwgt, bool verbose) { + + // control parameters + idx_t vwgt[n]; // the weights of the vertices + idx_t options[METIS_NOPTIONS]; + METIS_SetDefaultOptions(options); // use defaults + idx_t sepsize; // the size of the separator, output + sharedInts part_(new idx_t[n]); // the partition of each vertex, output + + // set uniform weights on the vertices + std::fill(vwgt, vwgt+n, 1); + + // TODO: Fix at later time + //boost::timer::cpu_timer TOTALTmr; + if (verbose) { + printf("**********************************************************************\n"); + printf("Graph Information ---------------------------------------------------\n"); + printf(" #Vertices: %d, #Edges: %u\n", n, *(xadj.get()+n) / 2); + printf("\nND Partitioning... -------------------------------------------\n"); + //TOTALTmr.start() + } + + // call metis parition routine + METIS_ComputeVertexSeparator(&n, xadj.get(), adjncy.get(), + vwgt, options, &sepsize, part_.get()); + + if (verbose) { + //boost::cpu_times const elapsed_times(timer.elapsed()); + //printf("\nTiming Information --------------------------------------------------\n"); + //printf(" Total: \t\t %7.3f\n", elapsed_times); + printf(" Sep size: \t\t %d\n", sepsize); + printf("**********************************************************************\n"); + } + + return std::make_pair(sepsize, part_); + } + + /* ************************************************************************* */ + void modefied_EdgeComputeSeparator(idx_t *nvtxs, idx_t *xadj, idx_t *adjncy, idx_t *vwgt, + idx_t *adjwgt, idx_t *options, idx_t *edgecut, idx_t *part) + { + idx_t i, ncon; + graph_t *graph; + real_t *tpwgts2; + ctrl_t *ctrl; + ctrl = SetupCtrl(METIS_OP_OMETIS, options, 1, 3, NULL, NULL); + ctrl->iptype = METIS_IPTYPE_GROW; + //if () == NULL) + // return METIS_ERROR_INPUT; + + InitRandom(ctrl->seed); + + graph = SetupGraph(ctrl, *nvtxs, 1, xadj, adjncy, vwgt, NULL, NULL); + + AllocateWorkSpace(ctrl, graph); + + ncon = graph->ncon; + ctrl->ncuts = 1; + + /* determine the weights of the two partitions as a function of the weight of the + target partition weights */ + + tpwgts2 = rwspacemalloc(ctrl, 2*ncon); + for (i=0; i>1), ctrl->tpwgts+i, ncon); + tpwgts2[ncon+i] = 1.0 - tpwgts2[i]; + } + /* perform the bisection */ + *edgecut = MultilevelBisect(ctrl, graph, tpwgts2); + + // ConstructMinCoverSeparator(&ctrl, &graph, 1.05); + // *edgecut = graph->mincut; + // *sepsize = graph.pwgts[2]; + icopy(*nvtxs, graph->where, part); + std::cout << "Finished bisection:" << *edgecut << std::endl; + FreeGraph(&graph); + + FreeCtrl(&ctrl); + } + + /* ************************************************************************* */ + /** + * Return the number of edge cuts and the partition indices {part} + * Part [j] is 0 or 1, depending on + * whether node j is in the left part of the graph or the right part respectively + */ + std::pair edgeMetis(idx_t n, const sharedInts& xadj, const sharedInts& adjncy, + const sharedInts& adjwgt, bool verbose) { + + // control parameters + idx_t vwgt[n]; // the weights of the vertices + idx_t options[METIS_NOPTIONS]; + METIS_SetDefaultOptions(options); // use defaults + idx_t edgecut; // the number of edge cuts, output + sharedInts part_(new idx_t[n]); // the partition of each vertex, output + + // set uniform weights on the vertices + std::fill(vwgt, vwgt+n, 1); + + //TODO: Fix later + //boost::timer TOTALTmr; + if (verbose) { + printf("**********************************************************************\n"); + printf("Graph Information ---------------------------------------------------\n"); + printf(" #Vertices: %d, #Edges: %u\n", n, *(xadj.get()+n) / 2); + printf("\nND Partitioning... -------------------------------------------\n"); + //cleartimer(TOTALTmr); + //starttimer(TOTALTmr); + } + + //int wgtflag = 1; // only edge weights + //int numflag = 0; // c style numbering starting from 0 + //int nparts = 2; // partition the graph to 2 submaps + modefied_EdgeComputeSeparator(&n, xadj.get(), adjncy.get(), vwgt, adjwgt.get(), + options, &edgecut, part_.get()); + + + if (verbose) { + //stoptimer(TOTALTmr); + printf("\nTiming Information --------------------------------------------------\n"); + //printf(" Total: \t\t %7.3f\n", gettimer(TOTALTmr)); + printf(" Edge cuts: \t\t %d\n", edgecut); + printf("**********************************************************************\n"); + } + + return std::make_pair(edgecut, part_); + } + + /* ************************************************************************* */ + /** + * Prepare the data structure {xadj} and {adjncy} required by metis + * xadj always has the size equal to the no. of the nodes plus 1 + * adjncy always has the size equal to two times of the no. of the edges in the Metis graph + */ + template + void prepareMetisGraph(const GenericGraph& graph, const std::vector& keys, WorkSpace& workspace, + sharedInts* ptr_xadj, sharedInts* ptr_adjncy, sharedInts* ptr_adjwgt) { + + typedef int Weight; + typedef std::vector Weights; + typedef std::vector Neighbors; + typedef std::pair NeighborsInfo; + + // set up dictionary + std::vector& dictionary = workspace.dictionary; + workspace.prepareDictionary(keys); + + // prepare for {adjacencyMap}, a pair of neighbor indices and the correponding edge weights + int numNodes = keys.size(); + int numEdges = 0; + std::vector adjacencyMap; + adjacencyMap.resize(numNodes); + std::cout << "Number of nodes: " << adjacencyMap.size() << std::endl; + int index1, index2; + + BOOST_FOREACH(const typename GenericGraph::value_type& factor, graph){ + index1 = dictionary[factor->key1.index]; + index2 = dictionary[factor->key2.index]; + std::cout << "index1: " << index1 << std::endl; + std::cout << "index2: " << index2 << std::endl; + // if both nodes are in the current graph, i.e. not a joint factor between frontal and separator + if (index1 >= 0 && index2 >= 0) { + std::pair& adjacencyMap1 = adjacencyMap[index1]; + std::pair& adjacencyMap2 = adjacencyMap[index2]; + try{ + adjacencyMap1.first.push_back(index2); + adjacencyMap1.second.push_back(factor->weight); + adjacencyMap2.first.push_back(index1); + adjacencyMap2.second.push_back(factor->weight); + }catch(std::exception& e){ + std::cout << e.what() << std::endl; + } + numEdges++; + } + } + + // prepare for {xadj}, {adjncy}, and {adjwgt} + *ptr_xadj = sharedInts(new idx_t[numNodes+1]); + *ptr_adjncy = sharedInts(new idx_t[numEdges*2]); + *ptr_adjwgt = sharedInts(new idx_t[numEdges*2]); + sharedInts& xadj = *ptr_xadj; + sharedInts& adjncy = *ptr_adjncy; + sharedInts& adjwgt = *ptr_adjwgt; + int ind_xadj = 0, ind_adjncy = 0; + BOOST_FOREACH(const NeighborsInfo& info, adjacencyMap) { + *(xadj.get() + ind_xadj) = ind_adjncy; + std::copy(info.first .begin(), info.first .end(), adjncy.get() + ind_adjncy); + std::copy(info.second.begin(), info.second.end(), adjwgt.get() + ind_adjncy); + assert(info.first.size() == info.second.size()); + ind_adjncy += info.first.size(); + ind_xadj ++; + } + if (ind_xadj != numNodes) throw std::runtime_error("prepareMetisGraph_: ind_xadj != numNodes"); + *(xadj.get() + ind_xadj) = ind_adjncy; + } + + /* ************************************************************************* */ + template + boost::optional separatorPartitionByMetis(const GenericGraph& graph, + const std::vector& keys, WorkSpace& workspace, bool verbose) { + // create a metis graph + size_t numKeys = keys.size(); + if (verbose) + std::cout << graph.size() << " factors,\t" << numKeys << " nodes;\t" << std::endl; + + sharedInts xadj, adjncy, adjwgt; + + prepareMetisGraph(graph, keys, workspace, &xadj, &adjncy, &adjwgt); + + // run ND on the graph + size_t sepsize; + sharedInts part; + boost::tie(sepsize, part) = separatorMetis(numKeys, xadj, adjncy, adjwgt, verbose); + if (!sepsize) return boost::optional(); + + // convert the 0-1-2 from Metis to 1-2-0, so that the separator is 0, as later + // we will have more submaps + MetisResult result; + result.C.reserve(sepsize); + result.A.reserve(numKeys - sepsize); + result.B.reserve(numKeys - sepsize); + int* ptr_part = part.get(); + std::vector::const_iterator itKey = keys.begin(); + std::vector::const_iterator itKeyLast = keys.end(); + while(itKey != itKeyLast) { + switch(*(ptr_part++)) { + case 0: result.A.push_back(*(itKey++)); break; + case 1: result.B.push_back(*(itKey++)); break; + case 2: result.C.push_back(*(itKey++)); break; + default: throw std::runtime_error("separatorPartitionByMetis: invalid results from Metis ND!"); + } + } + + if (verbose) { + std::cout << "total key: " << keys.size() + << " result(A,B,C) = " << result.A.size() << ", " << result.B.size() << ", " + << result.C.size() << "; sepsize from Metis = " << sepsize << std::endl; + //throw runtime_error("separatorPartitionByMetis:stop for debug"); + } + + if(result.C.size() != sepsize) { + std::cout << "total key: " << keys.size() + << " result(A,B,C) = " << result.A.size() << ", " << result.B.size() << ", " << result.C.size() + << "; sepsize from Metis = " << sepsize << std::endl; + throw std::runtime_error("separatorPartitionByMetis: invalid sepsize from Metis ND!"); + } + + return boost::make_optional(result); + } + + /* *************************************************************************/ + template + boost::optional edgePartitionByMetis(const GenericGraph& graph, + const std::vector& keys, WorkSpace& workspace, bool verbose) { + + // a small hack for handling the camera1-camera2 case used in the unit tests + if (graph.size() == 1 && keys.size() == 2) { + MetisResult result; + result.A.push_back(keys.front()); + result.B.push_back(keys.back()); + return result; + } + + // create a metis graph + size_t numKeys = keys.size(); + if (verbose) std::cout << graph.size() << " factors,\t" << numKeys << " nodes;\t" << std::endl; + sharedInts xadj, adjncy, adjwgt; + prepareMetisGraph(graph, keys, workspace, &xadj, &adjncy, &adjwgt); + + // run metis on the graph + int edgecut; + sharedInts part; + boost::tie(edgecut, part) = edgeMetis(numKeys, xadj, adjncy, adjwgt, verbose); + + // convert the 0-1-2 from Metis to 1-2-0, so that the separator is 0, as later we will have more submaps + MetisResult result; + result.A.reserve(numKeys); + result.B.reserve(numKeys); + int* ptr_part = part.get(); + std::vector::const_iterator itKey = keys.begin(); + std::vector::const_iterator itKeyLast = keys.end(); + while(itKey != itKeyLast) { + if (*ptr_part != 0 && *ptr_part != 1) + std::cout << *ptr_part << "!!!" << std::endl; + switch(*(ptr_part++)) { + case 0: result.A.push_back(*(itKey++)); break; + case 1: result.B.push_back(*(itKey++)); break; + default: throw std::runtime_error("edgePartitionByMetis: invalid results from Metis ND!"); + } + } + + if (verbose) { + std::cout << "the size of two submaps in the reduced graph: " << result.A.size() + << " " << result.B.size() << std::endl; + int edgeCut = 0; + + BOOST_FOREACH(const typename GenericGraph::value_type& factor, graph){ + int key1 = factor->key1.index; + int key2 = factor->key2.index; + // print keys and their subgraph assignment + std::cout << key1; + if (std::find(result.A.begin(), result.A.end(), key1) != result.A.end()) std::cout <<"A "; + if (std::find(result.B.begin(), result.B.end(), key1) != result.B.end()) std::cout <<"B "; + + std::cout << key2; + if (std::find(result.A.begin(), result.A.end(), key2) != result.A.end()) std::cout <<"A "; + if (std::find(result.B.begin(), result.B.end(), key2) != result.B.end()) std::cout <<"B "; + std::cout << "weight " << factor->weight;; + + // find vertices that were assigned to sets A & B. Their edge will be cut + if ((std::find(result.A.begin(), result.A.end(), key1) != result.A.end() && + std::find(result.B.begin(), result.B.end(), key2) != result.B.end()) || + (std::find(result.B.begin(), result.B.end(), key1) != result.B.end() && + std::find(result.A.begin(), result.A.end(), key2) != result.A.end())){ + edgeCut ++; + std::cout << " CUT "; + } + std::cout << std::endl; + } + std::cout << "edgeCut: " << edgeCut << std::endl; + } + + return boost::make_optional(result); + } + + /* ************************************************************************* */ + bool isLargerIsland(const std::vector& island1, const std::vector& island2) { + return island1.size() > island2.size(); + } + + /* ************************************************************************* */ + // debug functions + void printIsland(const std::vector& island) { + std::cout << "island: "; + BOOST_FOREACH(const size_t key, island) + std::cout << key << " "; + std::cout << std::endl; + } + + void printIslands(const std::list >& islands) { + BOOST_FOREACH(const std::vector& island, islands) + printIsland(island); + } + + void printNumCamerasLandmarks(const std::vector& keys, const std::vector& int2symbol) { + int numCamera = 0, numLandmark = 0; + BOOST_FOREACH(const size_t key, keys) + if (int2symbol[key].chr() == 'x') + numCamera++; + else + numLandmark++; + std::cout << "numCamera: " << numCamera << " numLandmark: " << numLandmark << std::endl; + } + + /* ************************************************************************* */ + template + void addLandmarkToPartitionResult(const GenericGraph& graph, const std::vector& landmarkKeys, + MetisResult& partitionResult, WorkSpace& workspace) { + + // set up cameras in the dictionary + std::vector& A = partitionResult.A; + std::vector& B = partitionResult.B; + std::vector& C = partitionResult.C; + std::vector& dictionary = workspace.dictionary; + std::fill(dictionary.begin(), dictionary.end(), -1); + BOOST_FOREACH(const size_t a, A) + dictionary[a] = 1; + BOOST_FOREACH(const size_t b, B) + dictionary[b] = 2; + if (!C.empty()) + throw std::runtime_error("addLandmarkToPartitionResult: C is not empty"); + + // set up landmarks + size_t i,j; + BOOST_FOREACH(const typename GenericGraph::value_type& factor, graph) { + i = factor->key1.index; + j = factor->key2.index; + if (dictionary[j] == 0) // if the landmark is already in the separator, continue + continue; + else if (dictionary[j] == -1) + dictionary[j] = dictionary[i]; + else { + if (dictionary[j] != dictionary[i]) + dictionary[j] = 0; + } +// if (j == 67980) +// std::cout << "dictionary[67980]" << dictionary[j] << std::endl; + } + + BOOST_FOREACH(const size_t j, landmarkKeys) { + switch(dictionary[j]) { + case 0: C.push_back(j); break; + case 1: A.push_back(j); break; + case 2: B.push_back(j); break; + default: std::cout << j << ": " << dictionary[j] << std::endl; + throw std::runtime_error("addLandmarkToPartitionResult: wrong status for landmark"); + } + } + } + +#define REDUCE_CAMERA_GRAPH + + /* ************************************************************************* */ + template + boost::optional findPartitoning(const GenericGraph& graph, const std::vector& keys, + WorkSpace& workspace, bool verbose, + const boost::optional >& int2symbol, const bool reduceGraph) { + boost::optional result; + GenericGraph reducedGraph; + std::vector keyToPartition; + std::vector cameraKeys, landmarkKeys; + if (reduceGraph) { + if (!int2symbol.is_initialized()) + throw std::invalid_argument("findSeparator: int2symbol must be valid!"); + + // find out all the landmark keys, which are to be eliminated + cameraKeys.reserve(keys.size()); + landmarkKeys.reserve(keys.size()); + BOOST_FOREACH(const size_t key, keys) { + if((*int2symbol)[key].chr() == 'x') + cameraKeys.push_back(key); + else + landmarkKeys.push_back(key); + } + + keyToPartition = cameraKeys; + workspace.prepareDictionary(keyToPartition); + const std::vector& dictionary = workspace.dictionary; + reduceGenericGraph(graph, cameraKeys, landmarkKeys, dictionary, reducedGraph); + std::cout << "original graph: V" << keys.size() << ", E" << graph.size() + << " --> reduced graph: V" << cameraKeys.size() << ", E" << reducedGraph.size() << std::endl; + result = edgePartitionByMetis(reducedGraph, keyToPartition, workspace, verbose); + } else // call Metis to partition the graph to A, B, C + result = separatorPartitionByMetis(graph, keys, workspace, verbose); + + if (!result.is_initialized()) { + std::cout << "metis failed!" << std::endl; + return 0; + } + + if (reduceGraph) { + addLandmarkToPartitionResult(graph, landmarkKeys, *result, workspace); + std::cout << "the separator size: " << result->C.size() << " landmarks" << std::endl; + } + + return result; + } + + /* ************************************************************************* */ + template + int findSeparator(const GenericGraph& graph, const std::vector& keys, + const int minNodesPerMap, WorkSpace& workspace, bool verbose, + const boost::optional >& int2symbol, const bool reduceGraph, + const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark) { + + boost::optional result = findPartitoning(graph, keys, workspace, + verbose, int2symbol, reduceGraph); + + // find the island in A and B, and make them separated submaps + typedef std::vector Island; + std::list islands; + + std::list islands_in_A = findIslands(graph, result->A, workspace, + minNrConstraintsPerCamera, minNrConstraintsPerLandmark); + + std::list islands_in_B = findIslands(graph, result->B, workspace, + minNrConstraintsPerCamera, minNrConstraintsPerLandmark); + + islands.insert(islands.end(), islands_in_A.begin(), islands_in_A.end()); + islands.insert(islands.end(), islands_in_B.begin(), islands_in_B.end()); + islands.sort(isLargerIsland); + size_t numIsland0 = islands.size(); + +#ifdef NDEBUG +// verbose = true; +// if (!int2symbol) throw std::invalid_argument("findSeparator: int2symbol is not set!"); +// std::cout << "sep size: " << result->C.size() << "; "; +// printNumCamerasLandmarks(result->C, *int2symbol); +// std::cout << "no. of island: " << islands.size() << "; "; +// std::cout << "island size: "; +// BOOST_FOREACH(const Island& island, islands) +// std::cout << island.size() << " "; +// std::cout << std::endl; + +// BOOST_FOREACH(const Island& island, islands) { +// printNumCamerasLandmarks(island, int2symbol); +// } +#endif + + // absorb small components into the separator + size_t oldSize = islands.size(); + while(true) { + if (islands.size() < 2) { + std::cout << "numIsland: " << numIsland0 << std::endl; + throw std::runtime_error("findSeparator: found fewer than 2 submaps!"); + } + + std::list::reference island = islands.back(); + if ((int)island.size() >= minNodesPerMap) break; + result->C.insert(result->C.end(), island.begin(), island.end()); + islands.pop_back(); + } + if (islands.size() != oldSize){ + if (verbose) std::cout << oldSize << "-" << oldSize - islands.size() << " submap(s);\t" << std::endl; + } + else{ + if (verbose) std::cout << oldSize << " submap(s);\t" << std::endl; + } + + // generate the node map + std::vector& partitionTable = workspace.partitionTable; + std::fill(partitionTable.begin(), partitionTable.end(), -1); + BOOST_FOREACH(const size_t key, result->C) + partitionTable[key] = 0; + int idx = 0; + BOOST_FOREACH(const Island& island, islands) { + idx++; + BOOST_FOREACH(const size_t key, island) { + partitionTable[key] = idx; + } + } + + return islands.size(); + } + +}} //namespace diff --git a/gtsam_unstable/partition/FindSeparator.h b/gtsam_unstable/partition/FindSeparator.h new file mode 100644 index 000000000..e52d40a12 --- /dev/null +++ b/gtsam_unstable/partition/FindSeparator.h @@ -0,0 +1,44 @@ +/* + * FindSeparator.h + * + * Created on: Nov 23, 2010 + * Author: nikai + * Description: find the separator of bisectioning for a given graph + */ + +#include +#include +#include +#include +#include + +#include "PartitionWorkSpace.h" + +namespace gtsam { namespace partition { + +// typedef std::map PartitionTable; // from the key to the partition: 0 - separator, > 1: submap id + + /** the metis Nest dissection result */ + struct MetisResult { + std::vector A, B; // frontals + std::vector C; // separator + }; + + /** + * use Metis library to partition, return the size of separator and the optional partition table + * the size of dictionary mush be equal to the number of variables in the original graph (the largest one) + */ + template + boost::optional separatorPartitionByMetis(const GenericGraph& graph, const std::vector& keys, + WorkSpace& workspace, bool verbose); + + /** + * return the number of submaps and the parition table of the partitioned graph (**stored in workspace.partitionTable**). + * return 0 if failed Note that the original output of Metis is 0,1 for submap, and 2 for the separator. + */ + template + int findSeparator(const GenericGraph& graph, const std::vector& keys, + const int minNodesPerMap, WorkSpace& workspace, bool verbose, const boost::optional >& int2symbol, + const bool reduceGraph, const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark); + +}} //namespace diff --git a/gtsam_unstable/partition/GenericGraph.cpp b/gtsam_unstable/partition/GenericGraph.cpp new file mode 100644 index 000000000..a3db6a9c1 --- /dev/null +++ b/gtsam_unstable/partition/GenericGraph.cpp @@ -0,0 +1,477 @@ +/* + * GenericGraph2D.cpp + * + * Created on: Nov 23, 2010 + * Author: nikai + * Description: generic graph types used in partitioning + */ +#include +#include +#include +#include +#include + +#include + +#include "GenericGraph.h" + +using namespace std; + +namespace gtsam { namespace partition { + + /** + * Note: Need to be able to handle a graph with factors that involve variables not in the given {keys} + */ + list > findIslands(const GenericGraph2D& graph, const vector& keys, WorkSpace& workspace, + const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark) + { + typedef pair IntPair; + typedef list FactorList; + typedef map Connections; + + // create disjoin set forest + DSFVector dsf(workspace.dsf, keys); + + FactorList factors(graph.begin(), graph.end()); + size_t nrFactors = factors.size(); + FactorList::iterator itEnd; + workspace.prepareDictionary(keys); + while (nrFactors) { + Connections connections; + bool succeed = false; + itEnd = factors.end(); + list toErase; + for (FactorList::iterator itFactor=factors.begin(); itFactor!=itEnd; itFactor++) { + + // remove invalid factors + GenericNode2D key1 = (*itFactor)->key1, key2 = (*itFactor)->key2; + if (workspace.dictionary[key1.index]==-1 || workspace.dictionary[key2.index]==-1) { + toErase.push_back(itFactor); nrFactors--; continue; + } + + size_t label1 = dsf.findSet(key1.index); + size_t label2 = dsf.findSet(key2.index); + if (label1 == label2) { toErase.push_back(itFactor); nrFactors--; continue; } + + // merge two trees if the connection is strong enough, otherwise cache it + // an odometry factor always merges two islands + if (key1.type == NODE_POSE_2D && key2.type == NODE_POSE_2D) { + toErase.push_back(itFactor); nrFactors--; + dsf.makeUnionInPlace(label1, label2); + succeed = true; + break; + } + + // single landmark island only need one measurement + if ((dsf.isSingleton(label1)==1 && key1.type == NODE_LANDMARK_2D) || + (dsf.isSingleton(label2)==1 && key2.type == NODE_LANDMARK_2D)) { + toErase.push_back(itFactor); nrFactors--; + dsf.makeUnionInPlace(label1, label2); + succeed = true; + break; + } + + // stack the current factor with the cached constraint + IntPair labels = (label1 < label2) ? make_pair(label1, label2) : make_pair(label2, label1); + Connections::iterator itCached = connections.find(labels); + if (itCached == connections.end()) { + connections.insert(make_pair(labels, itFactor)); + continue; + } else { + GenericNode2D key21 = (*itCached->second)->key1, key22 = (*itCached->second)->key2; + // if observe the same landmark, we can not merge, abandon the current factor + if ((key1.index == key21.index && key1.type == NODE_LANDMARK_2D) || + (key1.index == key22.index && key1.type == NODE_LANDMARK_2D) || + (key2.index == key21.index && key2.type == NODE_LANDMARK_2D) || + (key2.index == key22.index && key2.type == NODE_LANDMARK_2D)) { + toErase.push_back(itFactor); nrFactors--; + continue; + } else { + toErase.push_back(itFactor); nrFactors--; + toErase.push_back(itCached->second); nrFactors--; + dsf.makeUnionInPlace(label1, label2); + connections.erase(itCached); + succeed = true; + break; + } + } + } + + // erase unused factors + BOOST_FOREACH(const FactorList::iterator& it, toErase) + factors.erase(it); + + if (!succeed) break; + } + + list > islands; + map > arrays = dsf.arrays(); + size_t key; vector array; + BOOST_FOREACH(boost::tie(key, array), arrays) + islands.push_back(array); + return islands; + } + + + /* ************************************************************************* */ + void print(const GenericGraph2D& graph, const std::string name) { + cout << name << endl; + BOOST_FOREACH(const sharedGenericFactor2D& factor_, graph) + cout << factor_->key1.index << " " << factor_->key2.index << endl; + } + + /* ************************************************************************* */ + void print(const GenericGraph3D& graph, const std::string name) { + cout << name << endl; + BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) + cout << factor_->key1.index << " " << factor_->key2.index << " (" << + factor_->key1.type << ", " << factor_->key2.type <<")" << endl; + } + + /* ************************************************************************* */ + // create disjoin set forest + DSFVector createDSF(const GenericGraph3D& graph, const vector& keys, const WorkSpace& workspace) { + DSFVector dsf(workspace.dsf, keys); + typedef list FactorList; + + FactorList factors(graph.begin(), graph.end()); + size_t nrFactors = factors.size(); + FactorList::iterator itEnd; + while (nrFactors) { + + bool succeed = false; + itEnd = factors.end(); + list toErase; + for (FactorList::iterator itFactor=factors.begin(); itFactor!=itEnd; itFactor++) { + + // remove invalid factors + if (graph.size() == 178765) cout << "kai21" << endl; + GenericNode3D key1 = (*itFactor)->key1, key2 = (*itFactor)->key2; + if (graph.size() == 178765) cout << "kai21: " << key1.index << " " << key2.index << endl; + if (workspace.dictionary[key1.index]==-1 || workspace.dictionary[key2.index]==-1) { + toErase.push_back(itFactor); nrFactors--; continue; + } + + if (graph.size() == 178765) cout << "kai22" << endl; + size_t label1 = dsf.findSet(key1.index); + size_t label2 = dsf.findSet(key2.index); + if (label1 == label2) { toErase.push_back(itFactor); nrFactors--; continue; } + + if (graph.size() == 178765) cout << "kai23" << endl; + // merge two trees if the connection is strong enough, otherwise cache it + // an odometry factor always merges two islands + if ((key1.type == NODE_POSE_3D && key2.type == NODE_LANDMARK_3D) || + (key1.type == NODE_POSE_3D && key2.type == NODE_POSE_3D)) { + toErase.push_back(itFactor); nrFactors--; + dsf.makeUnionInPlace(label1, label2); + succeed = true; + break; + } + + if (graph.size() == 178765) cout << "kai24" << endl; + + + } + + // erase unused factors + BOOST_FOREACH(const FactorList::iterator& it, toErase) + factors.erase(it); + + if (!succeed) break; + } + return dsf; + } + + /* ************************************************************************* */ + // first check the type of the key (pose or landmark), and then check whether it is singular + inline bool isSingular(const set& singularCameras, const set& singularLandmarks, const GenericNode3D& node) { + switch(node.type) { + case NODE_POSE_3D: + return singularCameras.find(node.index) != singularCameras.end(); break; + case NODE_LANDMARK_3D: + return singularLandmarks.find(node.index) != singularLandmarks.end(); break; + default: + throw runtime_error("unrecognized key type!"); + } + } + + /* ************************************************************************* */ + void findSingularCamerasLandmarks(const GenericGraph3D& graph, const WorkSpace& workspace, + const vector& isCamera, const vector& isLandmark, + set& singularCameras, set& singularLandmarks, vector& nrConstraints, + bool& foundSingularCamera, bool& foundSingularLandmark, + const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark) { + + // compute the constraint number per camera + std::fill(nrConstraints.begin(), nrConstraints.end(), 0); + BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) { + const int& key1 = factor_->key1.index; + const int& key2 = factor_->key2.index; + if (workspace.dictionary[key1] != -1 && workspace.dictionary[key2] != -1 && + !isSingular(singularCameras, singularLandmarks, factor_->key1) && + !isSingular(singularCameras, singularLandmarks, factor_->key2)) { + nrConstraints[key1]++; + nrConstraints[key2]++; + + // a single pose constraint is sufficient for stereo, so we add 2 to the counter + // for a total of 3, i.e. the same as 3 landmarks fully constraining the camera + if(factor_->key1.type == NODE_POSE_3D && factor_->key2.type == NODE_POSE_3D){ + nrConstraints[key1]+=2; + nrConstraints[key2]+=2; + } + } + } + + // find singular cameras and landmarks + foundSingularCamera = false; + foundSingularLandmark = false; + for (size_t i=0; i > findIslands(const GenericGraph3D& graph, const vector& keys, WorkSpace& workspace, + const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark) { + + // create disjoint set forest + workspace.prepareDictionary(keys); + DSFVector dsf = createDSF(graph, keys, workspace); + + const bool verbose = false; + bool foundSingularCamera = true; + bool foundSingularLandmark = true; + + list > islands; + set singularCameras, singularLandmarks; + vector isCamera(workspace.dictionary.size(), false); + vector isLandmark(workspace.dictionary.size(), false); + + // check the constraint number of every variable + // find the camera and landmark keys + BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) { + //assert(factor_->key2.type == NODE_LANDMARK_3D); // only VisualSLAM should come here, not StereoSLAM + if (workspace.dictionary[factor_->key1.index] != -1) { + if (factor_->key1.type == NODE_POSE_3D) + isCamera[factor_->key1.index] = true; + else + isLandmark[factor_->key1.index] = true; + } + if (workspace.dictionary[factor_->key2.index] != -1) { + if (factor_->key2.type == NODE_POSE_3D) + isCamera[factor_->key2.index] = true; + else + isLandmark[factor_->key2.index] = true; + } + } + + vector nrConstraints(workspace.dictionary.size(), 0); + // iterate until all singular variables have been removed. Removing a singular variable + // can cause another to become singular, so this will probably run several times + while (foundSingularCamera || foundSingularLandmark) { + findSingularCamerasLandmarks(graph, workspace, isCamera, isLandmark, // input + singularCameras, singularLandmarks, nrConstraints, // output + foundSingularCamera, foundSingularLandmark, // output + minNrConstraintsPerCamera, minNrConstraintsPerLandmark); // input + } + + // add singular variables directly as islands + if (!singularCameras.empty()) { + if (verbose) cout << "singular cameras:"; + BOOST_FOREACH(const size_t i, singularCameras) { + islands.push_back(vector(1, i)); // <--------------------------- + if (verbose) cout << i << " "; + } + if (verbose) cout << endl; + } + if (!singularLandmarks.empty()) { + if (verbose) cout << "singular landmarks:"; + BOOST_FOREACH(const size_t i, singularLandmarks) { + islands.push_back(vector(1, i)); // <--------------------------- + if (verbose) cout << i << " "; + } + if (verbose) cout << endl; + } + + + // regenerating islands + map > labelIslands = dsf.arrays(); + size_t label; vector island; + BOOST_FOREACH(boost::tie(label, island), labelIslands) { + vector filteredIsland; // remove singular cameras from array + filteredIsland.reserve(island.size()); + BOOST_FOREACH(const size_t key, island) { + if ((isCamera[key] && singularCameras.find(key) == singularCameras.end()) || // not singular + (isLandmark[key] && singularLandmarks.find(key) == singularLandmarks.end()) || // not singular + (!isCamera[key] && !isLandmark[key])) { // the key is not involved in any factor, so the type is undertermined + filteredIsland.push_back(key); + } + } + islands.push_back(filteredIsland); + } + + // sanity check + size_t nrKeys = 0; + BOOST_FOREACH(const vector& island, islands) + nrKeys += island.size(); + if (nrKeys != keys.size()) { + cout << nrKeys << " vs " << keys.size() << endl; + throw runtime_error("findIslands: the number of keys is inconsistent!"); + } + + + if (verbose) cout << "found " << islands.size() << " islands!" << endl; + return islands; + } + + /* ************************************************************************* */ + // return the number of intersection between two **sorted** landmark vectors + inline int getNrCommonLandmarks(const vector& landmarks1, const vector& landmarks2){ + size_t i1 = 0, i2 = 0; + int nrCommonLandmarks = 0; + while (i1 < landmarks1.size() && i2 < landmarks2.size()) { + if (landmarks1[i1] < landmarks2[i2]) + i1 ++; + else if (landmarks1[i1] > landmarks2[i2]) + i2 ++; + else { + i1++; i2++; + nrCommonLandmarks ++; + } + } + return nrCommonLandmarks; + } + + /* ************************************************************************* */ + void reduceGenericGraph(const GenericGraph3D& graph, const std::vector& cameraKeys, const std::vector& landmarkKeys, + const std::vector& dictionary, GenericGraph3D& reducedGraph) { + + typedef size_t CameraKey; + typedef pair CameraPair; + typedef size_t LandmarkKey; + // get a mapping from each landmark to its connected cameras + vector > cameraToLandmarks(dictionary.size()); + // for odometry xi-xj where i cameraToCamera(dictionary.size(), -1); + size_t key_i, key_j; + BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) { + if (factor_->key1.type == NODE_POSE_3D) { + if (factor_->key2.type == NODE_LANDMARK_3D) {// projection factor + cameraToLandmarks[factor_->key1.index].push_back(factor_->key2.index); + } + else { // odometry factor + if (factor_->key1.index < factor_->key2.index) { + key_i = factor_->key1.index; + key_j = factor_->key2.index; + } else { + key_i = factor_->key2.index; + key_j = factor_->key1.index; + } + cameraToCamera[key_i] = key_j; + } + } + } + + // sort the landmark keys for the late getNrCommonLandmarks call + BOOST_FOREACH(vector &landmarks, cameraToLandmarks){ + if (!landmarks.empty()) + std::sort(landmarks.begin(), landmarks.end()); + } + + // generate the reduced graph + reducedGraph.clear(); + int factorIndex = 0; + int camera1, camera2, nrTotalConstraints; + bool hasOdometry; + for (size_t i1=0; i1 0 || hasOdometry) { + nrTotalConstraints = 2 * nrCommonLandmarks + (hasOdometry ? 6 : 0); + reducedGraph.push_back(boost::make_shared(camera1, camera2, + factorIndex++, NODE_POSE_3D, NODE_POSE_3D, nrTotalConstraints)); + } + } + } + } + + /* ************************************************************************* */ + void checkSingularity(const GenericGraph3D& graph, const std::vector& frontals, + WorkSpace& workspace, const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark) { + workspace.prepareDictionary(frontals); + vector nrConstraints(workspace.dictionary.size(), 0); + + // summarize the constraint number + const vector& dictionary = workspace.dictionary; + vector isValidCamera(workspace.dictionary.size(), false); + vector isValidLandmark(workspace.dictionary.size(), false); + BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) { + assert(factor_->key1.type == NODE_POSE_3D); + //assert(factor_->key2.type == NODE_LANDMARK_3D); + const size_t& key1 = factor_->key1.index; + const size_t& key2 = factor_->key2.index; + if (dictionary[key1] == -1 || dictionary[key2] == -1) + continue; + + isValidCamera[key1] = true; + if(factor_->key2.type == NODE_LANDMARK_3D) + isValidLandmark[key2] = true; + else + isValidCamera[key2] = true; + + nrConstraints[key1]++; + nrConstraints[key2]++; + + // a single pose constraint is sufficient for stereo, so we add 2 to the counter + // for a total of 3, i.e. the same as 3 landmarks fully constraining the camera + if(factor_->key1.type == NODE_POSE_3D && factor_->key2.type == NODE_POSE_3D){ + nrConstraints[key1]+=2; + nrConstraints[key2]+=2; + } + } + + // find the minimum constraint for cameras and landmarks + size_t minFoundConstraintsPerCamera = 10000; + size_t minFoundConstraintsPerLandmark = 10000; + + for (size_t i=0; i(minFoundConstraintsPerCamera)); + if (minFoundConstraintsPerLandmark < minNrConstraintsPerLandmark) + throw runtime_error("checkSingularity:minConstraintsPerLandmark < " + boost::lexical_cast(minFoundConstraintsPerLandmark)); + } + +}} // namespace diff --git a/gtsam_unstable/partition/GenericGraph.h b/gtsam_unstable/partition/GenericGraph.h new file mode 100644 index 000000000..c722721b7 --- /dev/null +++ b/gtsam_unstable/partition/GenericGraph.h @@ -0,0 +1,149 @@ +/* + * GenericGraph.h + * + * Created on: Nov 22, 2010 + * Author: nikai + * Description: generic graph types used in partitioning + */ + +#pragma once + +#include +#include +#include +#include + +#include "PartitionWorkSpace.h" + +namespace gtsam { namespace partition { + + /*************************************************** + * 2D generic factors and their factor graph + ***************************************************/ + enum GenericNode2DType { NODE_POSE_2D, NODE_LANDMARK_2D }; + + /** the index of the node and the type of the node */ + struct GenericNode2D { + std::size_t index; + GenericNode2DType type; + GenericNode2D (const std::size_t& index_in, const GenericNode2DType& type_in) : index(index_in), type(type_in) {} + }; + + /** a factor always involves two nodes/variables for now */ + struct GenericFactor2D { + GenericNode2D key1; + GenericNode2D key2; + int index; // the factor index in the original nonlinear factor graph + int weight; // the weight of the edge + GenericFactor2D(const size_t index1, const GenericNode2DType type1, const size_t index2, const GenericNode2DType type2, const int index_ = -1, const int weight_ = 1) + : key1(index1, type1), key2(index2, type2), index(index_), weight(weight_) {} + GenericFactor2D(const size_t index1, const char type1, const size_t index2, const char type2, const int index_ = -1, const int weight_ = 1) + : key1(index1, type1 == 'x' ? NODE_POSE_2D : NODE_LANDMARK_2D), + key2(index2, type2 == 'x' ? NODE_POSE_2D : NODE_LANDMARK_2D), index(index_), weight(weight_) {} + }; + + /** graph is a collection of factors */ + typedef boost::shared_ptr sharedGenericFactor2D; + typedef std::vector GenericGraph2D; + + /** merge nodes in DSF using constraints captured by the given graph */ + std::list > findIslands(const GenericGraph2D& graph, const std::vector& keys, WorkSpace& workspace, + const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark); + + /** eliminate the sensors from generic graph */ + inline void reduceGenericGraph(const GenericGraph2D& graph, const std::vector& cameraKeys, const std::vector& landmarkKeys, + const std::vector& dictionary, GenericGraph2D& reducedGraph) { + throw std::runtime_error("reduceGenericGraph 2d not implemented"); + } + + /** check whether the 2D graph is singular (under constrained) , Dummy function for 2D */ + inline void checkSingularity(const GenericGraph2D& graph, const std::vector& frontals, + WorkSpace& workspace, const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark) { return; } + + /** print the graph **/ + void print(const GenericGraph2D& graph, const std::string name = "GenericGraph2D"); + + /*************************************************** + * 3D generic factors and their factor graph + ***************************************************/ + enum GenericNode3DType { NODE_POSE_3D, NODE_LANDMARK_3D }; + +// const int minNrConstraintsPerCamera = 7; +// const int minNrConstraintsPerLandmark = 2; + + /** the index of the node and the type of the node */ + struct GenericNode3D { + std::size_t index; + GenericNode3DType type; + GenericNode3D (const std::size_t& index_in, const GenericNode3DType& type_in) : index(index_in), type(type_in) {} + }; + + /** a factor always involves two nodes/variables for now */ + struct GenericFactor3D { + GenericNode3D key1; + GenericNode3D key2; + int index; // the index in the entire graph, 0-based + int weight; // the weight of the edge + GenericFactor3D() :key1(-1, NODE_POSE_3D), key2(-1, NODE_LANDMARK_3D), index(-1), weight(1) {} + GenericFactor3D(const size_t index1, const size_t index2, const int index_ = -1, + const GenericNode3DType type1 = NODE_POSE_3D, const GenericNode3DType type2 = NODE_LANDMARK_3D, const int weight_ = 1) + : key1(index1, type1), key2(index2, type2), index(index_), weight(weight_) {} + }; + + /** graph is a collection of factors */ + typedef boost::shared_ptr sharedGenericFactor3D; + typedef std::vector GenericGraph3D; + + /** merge nodes in DSF using constraints captured by the given graph */ + std::list > findIslands(const GenericGraph3D& graph, const std::vector& keys, WorkSpace& workspace, + const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark); + + /** eliminate the sensors from generic graph */ + void reduceGenericGraph(const GenericGraph3D& graph, const std::vector& cameraKeys, const std::vector& landmarkKeys, + const std::vector& dictionary, GenericGraph3D& reducedGraph); + + /** check whether the 3D graph is singular (under constrained) */ + void checkSingularity(const GenericGraph3D& graph, const std::vector& frontals, + WorkSpace& workspace, const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark); + + + /** print the graph **/ + void print(const GenericGraph3D& graph, const std::string name = "GenericGraph3D"); + + /*************************************************** + * unary generic factors and their factor graph + ***************************************************/ + /** a factor involves a single variable */ + struct GenericUnaryFactor { + GenericNode2D key; + int index; // the factor index in the original nonlinear factor graph + GenericUnaryFactor(const size_t key_, const GenericNode2DType type_, const int index_ = -1) + : key(key_, type_), index(index_) {} + GenericUnaryFactor(const size_t key_, const char type_, const int index_ = -1) + : key(key_, type_ == 'x' ? NODE_POSE_2D : NODE_LANDMARK_2D), index(index_) {} + }; + + /** graph is a collection of factors */ + typedef boost::shared_ptr sharedGenericUnaryFactor; + typedef std::vector GenericUnaryGraph; + + /*************************************************** + * utility functions + ***************************************************/ + inline bool hasCommonCamera(const std::set& cameras1, const std::set& cameras2) { + if (cameras1.empty() || cameras2.empty()) + throw std::invalid_argument("hasCommonCamera: the input camera set is empty!"); + std::set::const_iterator it1 = cameras1.begin(); + std::set::const_iterator it2 = cameras2.begin(); + while (it1 != cameras1.end() && it2 != cameras2.end()) { + if (*it1 == *it2) + return true; + else if (*it1 < *it2) + it1++; + else + it2++; + } + return false; + } + +}} // namespace diff --git a/gtsam_unstable/partition/NestedDissection-inl.h b/gtsam_unstable/partition/NestedDissection-inl.h new file mode 100644 index 000000000..d6dafce49 --- /dev/null +++ b/gtsam_unstable/partition/NestedDissection-inl.h @@ -0,0 +1,251 @@ +/* + * NestedDissection-inl.h + * + * Created on: Nov 27, 2010 + * Author: nikai + * Description: + */ + +#pragma once + +#include + +#include "partition/FindSeparator-inl.h" +#include "OrderedSymbols.h" +#include "NestedDissection.h" + +namespace gtsam { namespace partition { + + /* ************************************************************************* */ + template + NestedDissection::NestedDissection( + const NLG& fg, const Ordering& ordering, const int numNodeStopPartition, const int minNodesPerMap, const bool verbose) : + fg_(fg), ordering_(ordering){ + GenericUnaryGraph unaryFactors; + GenericGraph gfg; + boost::tie(unaryFactors, gfg) = fg.createGenericGraph(ordering); + + // build reverse mapping from integer to symbol + int numNodes = ordering.size(); + int2symbol_.resize(numNodes); + Ordering::const_iterator it = ordering.begin(), itLast = ordering.end(); + while(it != itLast) + int2symbol_[it->second] = (it++)->first; + + vector keys; + keys.reserve(numNodes); + for(int i=0; i(), numNodeStopPartition, minNodesPerMap, boost::shared_ptr(), workspace, verbose); + } + + /* ************************************************************************* */ + template + NestedDissection::NestedDissection( + const NLG& fg, const Ordering& ordering, const boost::shared_ptr& cuts, const bool verbose) : fg_(fg), ordering_(ordering){ + GenericUnaryGraph unaryFactors; + GenericGraph gfg; + boost::tie(unaryFactors, gfg) = fg.createGenericGraph(ordering); + + // build reverse mapping from integer to symbol + int numNodes = ordering.size(); + int2symbol_.resize(numNodes); + Ordering::const_iterator it = ordering.begin(), itLast = ordering.end(); + while(it != itLast) + int2symbol_[it->second] = (it++)->first; + + vector keys; + keys.reserve(numNodes); + for(int i=0; i(), cuts, boost::shared_ptr(), workspace, verbose); + } + + /* ************************************************************************* */ + template + boost::shared_ptr NestedDissection::makeSubNLG( + const NLG& fg, const vector& frontals, const vector& sep, const boost::shared_ptr& parent) const { + OrderedSymbols frontalKeys; + BOOST_FOREACH(const size_t index, frontals) + frontalKeys.push_back(int2symbol_[index]); + + UnorderedSymbols sepKeys; + BOOST_FOREACH(const size_t index, sep) + sepKeys.insert(int2symbol_[index]); + + return boost::make_shared(fg, frontalKeys, sepKeys, parent); + } + + /* ************************************************************************* */ + template + void NestedDissection::processFactor( + const typename GenericGraph::value_type& factor, const std::vector& partitionTable, // input + vector& frontalFactors, NLG& sepFactors, vector >& childSeps, // output factor graphs + typename SubNLG::Weeklinks& weeklinks) const { // the links between child cliques + list sep_; // the separator variables involved in the current factor + int partition1 = partitionTable[factor->key1.index]; + int partition2 = partitionTable[factor->key2.index]; + if (partition1 <= 0 && partition2 <= 0) { // is a factor in the current clique + sepFactors.push_back(fg_[factor->index]); + } + else if (partition1 > 0 && partition2 > 0 && partition1 != partition2) { // is a weeklink (factor between two child cliques) + weeklinks.push_back(fg_[factor->index]); + } + else if (partition1 > 0 && partition2 > 0 && partition1 == partition2) { // is a local factor in one of the child cliques + frontalFactors[partition1 - 1].push_back(factor); + } + else { // is a joint factor in the child clique (involving varaibles in the current clique) + if (partition1 > 0 && partition2 <= 0) { + frontalFactors[partition1 - 1].push_back(factor); + childSeps[partition1 - 1].insert(factor->key2.index); + } else if (partition1 <= 0 && partition2 > 0) { + frontalFactors[partition2 - 1].push_back(factor); + childSeps[partition2 - 1].insert(factor->key1.index); + } else + throw runtime_error("processFactor: unexpected entries in the partition table!"); + } + } + + /* ************************************************************************* */ + /** + * given a factor graph and its partition {nodeMap}, split the factors between the child cliques ({frontalFactors}) + * and the current clique ({sepFactors}). Also split the variables between the child cliques ({childFrontals}) + * and the current clique ({localFrontals}). Those separator variables involved in {frontalFactors} are put into + * the correspoding ordering in {childSeps}. + */ + // TODO: frontalFactors and localFrontals should be generated in findSeparator + template + void NestedDissection::partitionFactorsAndVariables( + const GenericGraph& fg, const GenericUnaryGraph& unaryFactors, const std::vector& keys, //input + const std::vector& partitionTable, const int numSubmaps, // input + vector& frontalFactors, vector& frontalUnaryFactors, NLG& sepFactors, // output factor graphs + vector >& childFrontals, vector >& childSeps, vector& localFrontals, // output sub-orderings + typename SubNLG::Weeklinks& weeklinks) const { // the links between child cliques + + // make three lists of variables A, B, and C + int partition; + childFrontals.resize(numSubmaps); + BOOST_FOREACH(const size_t key, keys){ + partition = partitionTable[key]; + switch (partition) { + case -1: break; // the separator of the separator variables + case 0: localFrontals.push_back(key); break; // the separator variables + default: childFrontals[partition-1].push_back(key); // the frontal variables + } + } + + // group the factors to {frontalFactors} and {sepFactors},and find the joint variables + vector > childSeps_; + childSeps_.resize(numSubmaps); + childSeps.reserve(numSubmaps); + frontalFactors.resize(numSubmaps); + frontalUnaryFactors.resize(numSubmaps); + BOOST_FOREACH(typename GenericGraph::value_type factor, fg) + processFactor(factor, partitionTable, frontalFactors, sepFactors, childSeps_, weeklinks); + BOOST_FOREACH(const set& childSep, childSeps_) + childSeps.push_back(vector(childSep.begin(), childSep.end())); + + // add unary factor to the current cluster or pass it to one of the child clusters + BOOST_FOREACH(const sharedGenericUnaryFactor& unaryFactor_, unaryFactors) { + partition = partitionTable[unaryFactor_->key.index]; + if (!partition) sepFactors.push_back(fg_[unaryFactor_->index]); + else frontalUnaryFactors[partition-1].push_back(unaryFactor_); + } + } + + /* ************************************************************************* */ + template + NLG NestedDissection::collectOriginalFactors( + const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors) const { + NLG sepFactors; + typename GenericGraph::const_iterator it = gfg.begin(), itLast = gfg.end(); + while(it!=itLast) sepFactors.push_back(fg_[(*it++)->index]); + BOOST_FOREACH(const sharedGenericUnaryFactor& unaryFactor_, unaryFactors) + sepFactors.push_back(fg_[unaryFactor_->index]); + return sepFactors; + } + + /* ************************************************************************* */ + template + boost::shared_ptr NestedDissection::recursivePartition( + const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors, const vector& frontals, const vector& sep, + const int numNodeStopPartition, const int minNodesPerMap, const boost::shared_ptr& parent, WorkSpace& workspace, const bool verbose) const { + + // if no split needed + NLG sepFactors; // factors that should remain in the current cluster + if (frontals.size() <= numNodeStopPartition || gfg.size() <= numNodeStopPartition) { + sepFactors = collectOriginalFactors(gfg, unaryFactors); + return makeSubNLG(sepFactors, frontals, sep, parent); + } + + // find the nested dissection separator + int numSubmaps = findSeparator(gfg, frontals, minNodesPerMap, workspace, verbose, int2symbol_, NLG::reduceGraph(), + NLG::minNrConstraintsPerCamera(),NLG::minNrConstraintsPerLandmark()); + partition::PartitionTable& partitionTable = workspace.partitionTable; + if (numSubmaps == 0) throw runtime_error("recursivePartition: get zero submap after ND!"); + + // split the factors between child cliques and the current clique + vector frontalFactors; vector frontalUnaryFactors; typename SubNLG::Weeklinks weeklinks; + vector localFrontals; vector > childFrontals, childSeps; + partitionFactorsAndVariables(gfg, unaryFactors, frontals, partitionTable, numSubmaps, + frontalFactors, frontalUnaryFactors, sepFactors, childFrontals, childSeps, localFrontals, weeklinks); + + // make a new cluster + boost::shared_ptr current = makeSubNLG(sepFactors, localFrontals, sep, parent); + current->setWeeklinks(weeklinks); + + // check whether all the submaps are fully constrained + for (int i=0; i child = recursivePartition(frontalFactors[i], frontalUnaryFactors[i], childFrontals[i], childSeps[i], + numNodeStopPartition, minNodesPerMap, current, workspace, verbose); + current->addChild(child); + } + + return current; + } + + /* ************************************************************************* */ + template + boost::shared_ptr NestedDissection::recursivePartition( + const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors, const vector& frontals, const vector& sep, + const boost::shared_ptr& cuts, const boost::shared_ptr& parent, WorkSpace& workspace, const bool verbose) const { + + // if there is no need to cut any more + NLG sepFactors; // factors that should remain in the current cluster + if (!cuts.get()) { + sepFactors = collectOriginalFactors(gfg, unaryFactors); + return makeSubNLG(sepFactors, frontals, sep, parent); + } + + // retrieve the current partitioning info + int numSubmaps = 2; + partition::PartitionTable& partitionTable = cuts->partitionTable; + + // split the factors between child cliques and the current clique + vector frontalFactors; vector frontalUnaryFactors; typename SubNLG::Weeklinks weeklinks; + vector localFrontals; vector > childFrontals, childSeps; + partitionFactorsAndVariables(gfg, unaryFactors, frontals, partitionTable, numSubmaps, + frontalFactors, frontalUnaryFactors, sepFactors, childFrontals, childSeps, localFrontals, weeklinks); + + // make a new cluster + boost::shared_ptr current = makeSubNLG(sepFactors, localFrontals, sep, parent); + current->setWeeklinks(weeklinks); + + // create child clusters + for (int i=0; i<2; i++) { + boost::shared_ptr child = recursivePartition(frontalFactors[i], frontalUnaryFactors[i], childFrontals[i], childSeps[i], + cuts->children.empty() ? boost::shared_ptr() : cuts->children[i], current, workspace, verbose); + current->addChild(child); + } + return current; + } +}} //namespace diff --git a/gtsam_unstable/partition/NestedDissection.h b/gtsam_unstable/partition/NestedDissection.h new file mode 100644 index 000000000..3c294be64 --- /dev/null +++ b/gtsam_unstable/partition/NestedDissection.h @@ -0,0 +1,69 @@ +/* + * NestedDissection.h + * + * Created on: Nov 27, 2010 + * Author: nikai + * Description: apply nested dissection algorithm to the factor graph + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { namespace partition { + + /** + * Apply nested dissection algorithm to nonlinear factor graphs + */ + template + class NestedDissection { + public: + typedef boost::shared_ptr sharedSubNLG; + + private: + NLG fg_; // the original nonlinear factor graph + Ordering ordering_; // the variable ordering in the nonlinear factor graph + std::vector int2symbol_; // the mapping from integer key to symbol + sharedSubNLG root_; // the root of generated cluster tree + + public: + sharedSubNLG root() const { return root_; } + + public: + /* constructor with post-determined partitoning*/ + NestedDissection(const NLG& fg, const Ordering& ordering, const int numNodeStopPartition, const int minNodesPerMap, const bool verbose = false); + + /* constructor with pre-determined cuts*/ + NestedDissection(const NLG& fg, const Ordering& ordering, const boost::shared_ptr& cuts, const bool verbose = false); + + private: + /* convert generic subgraph to nonlinear subgraph */ + sharedSubNLG makeSubNLG(const NLG& fg, const std::vector& frontals, const std::vector& sep, const boost::shared_ptr& parent) const; + + void processFactor(const typename GenericGraph::value_type& factor, const std::vector& partitionTable, // input + std::vector& frontalFactors, NLG& sepFactors, std::vector >& childSeps, // output factor graphs + typename SubNLG::Weeklinks& weeklinks) const; + + /* recursively partition the generic graph */ + void partitionFactorsAndVariables( + const GenericGraph& fg, const GenericUnaryGraph& unaryFactors, + const std::vector& keys, const std::vector& partitionTable, const int numSubmaps, // input + std::vector& frontalFactors, vector& frontalUnaryFactors, NLG& sepFactors, // output factor graphs + std::vector >& childFrontals, std::vector >& childSeps, std::vector& localFrontals, // output sub-orderings + typename SubNLG::Weeklinks& weeklinks) const; + + NLG collectOriginalFactors(const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors) const; + + /* recursively partition the generic graph */ + sharedSubNLG recursivePartition(const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors, const std::vector& frontals, const std::vector& sep, + const int numNodeStopPartition, const int minNodesPerMap, const boost::shared_ptr& parent, WorkSpace& workspace, const bool verbose) const; + + /* recursively partition the generic graph */ + sharedSubNLG recursivePartition(const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors, const std::vector& frontals, const std::vector& sep, + const boost::shared_ptr& cuts, const boost::shared_ptr& parent, WorkSpace& workspace, const bool verbose) const; + + }; + +}} //namespace diff --git a/gtsam_unstable/partition/PartitionWorkSpace.h b/gtsam_unstable/partition/PartitionWorkSpace.h new file mode 100644 index 000000000..b268c0fb2 --- /dev/null +++ b/gtsam_unstable/partition/PartitionWorkSpace.h @@ -0,0 +1,44 @@ +/* + * PartitionWorkSpace.h + * + * Created on: Nov 24, 2010 + * Author: nikai + * Description: a preallocated memory space used in partitioning + */ + +#pragma once + +#include +#include + +namespace gtsam { namespace partition { + + typedef std::vector PartitionTable; + + // the work space, preallocated memory + struct WorkSpace { + std::vector dictionary; // a mapping from the integer key in the original graph to 0-based index in the subgraph, useful when handling a subset of keys and graphs + boost::shared_ptr > dsf; // a block memory pre-allocated for DSFVector + PartitionTable partitionTable; // a mapping from a key to the submap index, 0 means the separator, i means the ith submap + + // constructor + WorkSpace(const size_t numNodes) : dictionary(numNodes,0), + dsf(new std::vector(numNodes, 0)), partitionTable(numNodes, -1) { } + + // set up dictionary: -1: no such key, none-zero: the corresponding 0-based index + inline void prepareDictionary(const std::vector& keys) { + int index = 0; + std::fill(dictionary.begin(), dictionary.end(), -1); + std::vector::const_iterator it=keys.begin(), itLast=keys.end(); + while(it!=itLast) dictionary[*(it++)] = index++; + } + }; + + + // manually defined cuts + struct Cuts { + PartitionTable partitionTable; + std::vector > children; + }; + +}} // namespace diff --git a/gtsam_unstable/partition/tests/testFindSeparator.cpp b/gtsam_unstable/partition/tests/testFindSeparator.cpp new file mode 100644 index 000000000..4943f5456 --- /dev/null +++ b/gtsam_unstable/partition/tests/testFindSeparator.cpp @@ -0,0 +1,231 @@ +/* + * testFindSeparator.cpp + * + * Created on: Nov 23, 2010 + * Author: nikai + * Description: unit tests for FindSeparator + */ + +#include // for operator += +#include // for operator += +#include // for operator += +using namespace boost::assign; +#include +#include +#include + +#include +#include + +using namespace std; +using namespace gtsam; +using namespace gtsam::partition; + +/* ************************************************************************* */ +// x0 - x1 - x2 +// l3 l4 +TEST ( Partition, separatorPartitionByMetis ) +{ + GenericGraph2D graph; + graph.push_back(boost::make_shared(0, NODE_POSE_2D, 3, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(2, NODE_POSE_2D, 4, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(0, NODE_POSE_2D, 1, NODE_POSE_2D)); + graph.push_back(boost::make_shared(1, NODE_POSE_2D, 2, NODE_POSE_2D)); + std::vector keys; keys += 0, 1, 2, 3, 4; + + WorkSpace workspace(5); + boost::optional actual = separatorPartitionByMetis(graph, keys, + workspace, true); + + CHECK(actual.is_initialized()); + vector A_expected; A_expected += 0, 3; // frontal + vector B_expected; B_expected += 2, 4; // frontal + vector C_expected; C_expected += 1; // separator + CHECK(A_expected == actual->A); + CHECK(B_expected == actual->B); + CHECK(C_expected == actual->C); +} + +/* ************************************************************************* */ +// x1 - x2 - x3, variable not used x0, x4, l7 +// l5 l6 +TEST ( Partition, separatorPartitionByMetis2 ) +{ + GenericGraph2D graph; + graph.push_back(boost::make_shared(1, NODE_POSE_2D, 5, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(3, NODE_POSE_2D, 6, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(1, NODE_POSE_2D, 2, NODE_POSE_2D)); + graph.push_back(boost::make_shared(2, NODE_POSE_2D, 3, NODE_POSE_2D)); + std::vector keys; keys += 1, 2, 3, 5, 6; + + WorkSpace workspace(8); + boost::optional actual = separatorPartitionByMetis(graph, keys, + workspace, true); + + CHECK(actual.is_initialized()); + vector A_expected; A_expected += 1, 5; // frontal + vector B_expected; B_expected += 3, 6; // frontal + vector C_expected; C_expected += 2; // separator + CHECK(A_expected == actual->A); + CHECK(B_expected == actual->B); + CHECK(C_expected == actual->C); +} + +/* *************************************************************************/ +// x0 - x1 - x2 - x3 +TEST ( Partition, edgePartitionByMetis ) +{ + GenericGraph3D graph; + graph.push_back(boost::make_shared(0, 1, 0, NODE_POSE_3D, NODE_POSE_3D)); + graph.push_back(boost::make_shared(1, 2, 1, NODE_POSE_3D, NODE_POSE_3D)); + graph.push_back(boost::make_shared(2, 3, 2, NODE_POSE_3D, NODE_POSE_3D)); + std::vector keys; keys += 0, 1, 2, 3; + + WorkSpace workspace(6); + boost::optional actual = edgePartitionByMetis(graph, keys, + workspace, true); + + CHECK(actual.is_initialized()); + vector A_expected; A_expected += 0, 1; // frontal + vector B_expected; B_expected += 2, 3; // frontal + vector C_expected; // separator +// BOOST_FOREACH(const size_t a, actual->A) +// cout << a << " "; +// cout << endl; +// BOOST_FOREACH(const size_t b, actual->B) +// cout << b << " "; +// cout << endl; + + CHECK(A_expected == actual->A || A_expected == actual->B); + CHECK(B_expected == actual->B || B_expected == actual->A); + CHECK(C_expected == actual->C); +} + +/* *************************************************************************/ +// x0 - x1 - x2 - x3 - x4 +TEST ( Partition, edgePartitionByMetis2 ) +{ + GenericGraph3D graph; + graph.push_back(boost::make_shared(0, 1, 0, NODE_POSE_3D, NODE_POSE_3D, 1)); + graph.push_back(boost::make_shared(1, 2, 1, NODE_POSE_3D, NODE_POSE_3D, 1)); + graph.push_back(boost::make_shared(2, 3, 2, NODE_POSE_3D, NODE_POSE_3D, 20)); + graph.push_back(boost::make_shared(3, 4, 3, NODE_POSE_3D, NODE_POSE_3D, 1)); + std::vector keys; keys += 0, 1, 2, 3, 4; + + WorkSpace workspace(6); + boost::optional actual = edgePartitionByMetis(graph, keys, + workspace, true); + CHECK(actual.is_initialized()); + vector A_expected; A_expected += 0, 1; // frontal + vector B_expected; B_expected += 2, 3, 4; // frontal + vector C_expected; // separator + CHECK(A_expected == actual->A); + CHECK(B_expected == actual->B); + CHECK(C_expected == actual->C); +} + +/* ************************************************************************* */ +// x0 - x1 - x2 +// l3 l4 +TEST ( Partition, findSeparator ) +{ + GenericGraph2D graph; + graph.push_back(boost::make_shared(0, NODE_POSE_2D, 3, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(2, NODE_POSE_2D, 4, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(0, NODE_POSE_2D, 1, NODE_POSE_2D)); + graph.push_back(boost::make_shared(1, NODE_POSE_2D, 2, NODE_POSE_2D)); + std::vector keys; keys += 0, 1, 2, 3, 4; + + WorkSpace workspace(5); + int minNodesPerMap = -1; + bool reduceGraph = false; + int numSubmaps = findSeparator(graph, keys, minNodesPerMap, workspace, + false, boost::none, reduceGraph, 0, 0); + LONGS_EQUAL(2, numSubmaps); + LONGS_EQUAL(5, workspace.partitionTable.size()); + LONGS_EQUAL(1, workspace.partitionTable[0]); + LONGS_EQUAL(0, workspace.partitionTable[1]); + LONGS_EQUAL(2, workspace.partitionTable[2]); + LONGS_EQUAL(1, workspace.partitionTable[3]); + LONGS_EQUAL(2, workspace.partitionTable[4]); +} + +/* ************************************************************************* */ +// x1 - x2 - x3, variable not used x0, x4, l7 +// l5 l6 +TEST ( Partition, findSeparator2 ) +{ + GenericGraph2D graph; + graph.push_back(boost::make_shared(1, NODE_POSE_2D, 5, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(3, NODE_POSE_2D, 6, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(1, NODE_POSE_2D, 2, NODE_POSE_2D)); + graph.push_back(boost::make_shared(2, NODE_POSE_2D, 3, NODE_POSE_2D)); + std::vector keys; keys += 1, 2, 3, 5, 6; + + WorkSpace workspace(8); + int minNodesPerMap = -1; + bool reduceGraph = false; + int numSubmaps = findSeparator(graph, keys, minNodesPerMap, workspace, + false, boost::none, reduceGraph, 0, 0); + LONGS_EQUAL(2, numSubmaps); + LONGS_EQUAL(8, workspace.partitionTable.size()); + LONGS_EQUAL(-1,workspace.partitionTable[0]); + LONGS_EQUAL(1, workspace.partitionTable[1]); + LONGS_EQUAL(0, workspace.partitionTable[2]); + LONGS_EQUAL(2, workspace.partitionTable[3]); + LONGS_EQUAL(-1,workspace.partitionTable[4]); + LONGS_EQUAL(1, workspace.partitionTable[5]); + LONGS_EQUAL(2, workspace.partitionTable[6]); + LONGS_EQUAL(-1,workspace.partitionTable[7]); +} + +/* *************************************************************************/ +// l1-l8 l9-l16 l17-l24 +// / | / \ | \ +// x25 x26 x27 x28 +TEST ( Partition, findSeparator3_with_reduced_camera ) +{ + GenericGraph3D graph; + for (int j=1; j<=8; j++) + graph.push_back(boost::make_shared(25, j)); + for (int j=1; j<=16; j++) + graph.push_back(boost::make_shared(26, j)); + for (int j=9; j<=24; j++) + graph.push_back(boost::make_shared(27, j)); + for (int j=17; j<=24; j++) + graph.push_back(boost::make_shared(28, j)); + + std::vector keys; + for(int i=1; i<=28; i++) + keys.push_back(i); + + vector int2symbol; + int2symbol.push_back(Symbol('x',0)); // dummy + for(int i=1; i<=24; i++) + int2symbol.push_back(Symbol('l',i)); + int2symbol.push_back(Symbol('x',25)); + int2symbol.push_back(Symbol('x',26)); + int2symbol.push_back(Symbol('x',27)); + int2symbol.push_back(Symbol('x',28)); + + WorkSpace workspace(29); + bool reduceGraph = true; + int numIsland = findSeparator(graph, keys, 3, workspace, false, int2symbol, reduceGraph, 0, 0); + LONGS_EQUAL(2, numIsland); + + partition::PartitionTable& partitionTable = workspace.partitionTable; + for (int j=1; j<=8; j++) + LONGS_EQUAL(1, partitionTable[j]); + for (int j=9; j<=16; j++) + LONGS_EQUAL(0, partitionTable[j]); + for (int j=17; j<=24; j++) + LONGS_EQUAL(2, partitionTable[j]); + LONGS_EQUAL(1, partitionTable[25]); + LONGS_EQUAL(1, partitionTable[26]); + LONGS_EQUAL(2, partitionTable[27]); + LONGS_EQUAL(2, partitionTable[28]); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ diff --git a/gtsam_unstable/partition/tests/testGenericGraph.cpp b/gtsam_unstable/partition/tests/testGenericGraph.cpp new file mode 100644 index 000000000..7d9bf928f --- /dev/null +++ b/gtsam_unstable/partition/tests/testGenericGraph.cpp @@ -0,0 +1,254 @@ +/* + * testGenericGraph.cpp + * + * Created on: Nov 23, 2010 + * Author: nikai + * Description: unit tests for generic graph + */ + +#include +#include // for operator += +#include // for operator += +#include // for operator += +using namespace boost::assign; +#include +#include +#include + +#include + +using namespace std; +using namespace gtsam; +using namespace gtsam::partition; + +/* ************************************************************************* */ +/** + * l7 l9 + * / | \ / | + * x1 -x2-x3 - l8 - x4- x5-x6 + */ +TEST ( GenerciGraph, findIslands ) +{ + GenericGraph2D graph; + graph.push_back(boost::make_shared(1, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(2, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(3, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(3, NODE_POSE_2D, 8, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(4, NODE_POSE_2D, 8, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(4, NODE_POSE_2D, 9, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(5, NODE_POSE_2D, 9, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(6, NODE_POSE_2D, 9, NODE_LANDMARK_2D)); + + graph.push_back(boost::make_shared(1, NODE_POSE_2D, 2, NODE_POSE_2D)); + graph.push_back(boost::make_shared(2, NODE_POSE_2D, 3, NODE_POSE_2D)); + graph.push_back(boost::make_shared(4, NODE_POSE_2D, 5, NODE_POSE_2D)); + graph.push_back(boost::make_shared(5, NODE_POSE_2D, 6, NODE_POSE_2D)); + std::vector keys; keys += 1, 2, 3, 4, 5, 6, 7, 8, 9; + + WorkSpace workspace(10); // from 0 to 9 + list > islands = findIslands(graph, keys, workspace, 7, 2); + LONGS_EQUAL(2, islands.size()); + vector island1; island1 += 1, 2, 3, 7, 8; + vector island2; island2 += 4, 5, 6, 9; + CHECK(island1 == islands.front()); + CHECK(island2 == islands.back()); +} + +/* ************************************************************************* */ +/** + * l7 l8 + * / / | X | \ + * x1 -x2-x3 x4- x5-x6 + */ +TEST( GenerciGraph, findIslands2 ) +{ + GenericGraph2D graph; + graph.push_back(boost::make_shared(1, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(2, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(3, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(3, NODE_POSE_2D, 8, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(4, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(4, NODE_POSE_2D, 8, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(5, NODE_POSE_2D, 8, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(6, NODE_POSE_2D, 8, NODE_LANDMARK_2D)); + + graph.push_back(boost::make_shared(1, NODE_POSE_2D, 2, NODE_POSE_2D)); + graph.push_back(boost::make_shared(2, NODE_POSE_2D, 3, NODE_POSE_2D)); + graph.push_back(boost::make_shared(4, NODE_POSE_2D, 5, NODE_POSE_2D)); + graph.push_back(boost::make_shared(5, NODE_POSE_2D, 6, NODE_POSE_2D)); + std::vector keys; keys += 1, 2, 3, 4, 5, 6, 7, 8; + + WorkSpace workspace(15); // from 0 to 8, but testing over-allocation here + list > islands = findIslands(graph, keys, workspace, 7, 2); + LONGS_EQUAL(1, islands.size()); + vector island1; island1 += 1, 2, 3, 4, 5, 6, 7, 8; + CHECK(island1 == islands.front()); +} + +/* ************************************************************************* */ +/** + * x1 - l5 + * x2 - x3 - x4 - l6 + */ +TEST ( GenerciGraph, findIslands3 ) +{ + GenericGraph2D graph; + graph.push_back(boost::make_shared(1, NODE_POSE_2D, 5, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(4, NODE_POSE_2D, 6, NODE_LANDMARK_2D)); + + graph.push_back(boost::make_shared(2, NODE_POSE_2D, 3, NODE_POSE_2D)); + graph.push_back(boost::make_shared(3, NODE_POSE_2D, 4, NODE_POSE_2D)); + std::vector keys; keys += 1, 2, 3, 4, 5, 6; + + WorkSpace workspace(7); // from 0 to 9 + list > islands = findIslands(graph, keys, workspace, 7, 2); + LONGS_EQUAL(2, islands.size()); + vector island1; island1 += 1, 5; + vector island2; island2 += 2, 3, 4, 6; + CHECK(island1 == islands.front()); + CHECK(island2 == islands.back()); +} + +/* ************************************************************************* */ +/** + * x3 - l4 - x7 + */ +TEST ( GenerciGraph, findIslands4 ) +{ + GenericGraph2D graph; + graph.push_back(boost::make_shared(3, NODE_POSE_2D, 4, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(7, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); + std::vector keys; keys += 3, 4, 7; + + WorkSpace workspace(8); // from 0 to 7 + list > islands = findIslands(graph, keys, workspace, 7, 2); + LONGS_EQUAL(2, islands.size()); + vector island1; island1 += 3, 4; + vector island2; island2 += 7; + CHECK(island1 == islands.front()); + CHECK(island2 == islands.back()); +} + +/* ************************************************************************* */ +/** + * x1 - l5 - x2 + * | / \ | + * x3 x4 + */ +TEST ( GenerciGraph, findIslands5 ) +{ + GenericGraph2D graph; + graph.push_back(boost::make_shared(1, NODE_POSE_2D, 5, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(2, NODE_POSE_2D, 5, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(3, NODE_POSE_2D, 5, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(4, NODE_POSE_2D, 5, NODE_LANDMARK_2D)); + + graph.push_back(boost::make_shared(1, NODE_POSE_2D, 3, NODE_POSE_2D)); + graph.push_back(boost::make_shared(2, NODE_POSE_2D, 4, NODE_POSE_2D)); + + std::vector keys; keys += 1, 2, 3, 4, 5; + + WorkSpace workspace(6); // from 0 to 5 + list > islands = findIslands(graph, keys, workspace, 7, 2); + LONGS_EQUAL(2, islands.size()); + vector island1; island1 += 1, 3, 5; + vector island2; island2 += 2, 4; + CHECK(island1 == islands.front()); + CHECK(island2 == islands.back()); +} + +/* ************************************************************************* */ +/** + * l3 l4 l5 l6 + * \ | / \ / + * x1 x2 + */ +TEST ( GenerciGraph, reduceGenericGraph ) +{ + GenericGraph3D graph; + graph.push_back(boost::make_shared(1, 3)); + graph.push_back(boost::make_shared(1, 4)); + graph.push_back(boost::make_shared(1, 5)); + graph.push_back(boost::make_shared(2, 5)); + graph.push_back(boost::make_shared(2, 6)); + + std::vector cameraKeys, landmarkKeys; + cameraKeys.push_back(1); + cameraKeys.push_back(2); + landmarkKeys.push_back(3); + landmarkKeys.push_back(4); + landmarkKeys.push_back(5); + landmarkKeys.push_back(6); + + std::vector dictionary; + dictionary.resize(7, -1); // from 0 to 6 + dictionary[1] = 0; + dictionary[2] = 1; + + GenericGraph3D reduced; + std::map > cameraToLandmarks; + reduceGenericGraph(graph, cameraKeys, landmarkKeys, dictionary, reduced); + LONGS_EQUAL(1, reduced.size()); + LONGS_EQUAL(1, reduced[0]->key1.index); LONGS_EQUAL(2, reduced[0]->key2.index); +} + +/* ************************************************************************* */ +/** + * l3 l4 l5 l6 + * \ | / \ / + * x1 x2 - x7 + */ +TEST ( GenericGraph, reduceGenericGraph2 ) +{ + GenericGraph3D graph; + graph.push_back(boost::make_shared(1, 3, 0, NODE_POSE_3D, NODE_LANDMARK_3D)); + graph.push_back(boost::make_shared(1, 4, 1, NODE_POSE_3D, NODE_LANDMARK_3D)); + graph.push_back(boost::make_shared(1, 5, 2, NODE_POSE_3D, NODE_LANDMARK_3D)); + graph.push_back(boost::make_shared(2, 5, 3, NODE_POSE_3D, NODE_LANDMARK_3D)); + graph.push_back(boost::make_shared(2, 6, 4, NODE_POSE_3D, NODE_LANDMARK_3D)); + graph.push_back(boost::make_shared(2, 7, 5, NODE_POSE_3D, NODE_POSE_3D)); + + std::vector cameraKeys, landmarkKeys; + cameraKeys.push_back(1); + cameraKeys.push_back(2); + cameraKeys.push_back(7); + landmarkKeys.push_back(3); + landmarkKeys.push_back(4); + landmarkKeys.push_back(5); + landmarkKeys.push_back(6); + + std::vector dictionary; + dictionary.resize(8, -1); // from 0 to 7 + dictionary[1] = 0; + dictionary[2] = 1; + dictionary[7] = 6; + + GenericGraph3D reduced; + std::map > cameraToLandmarks; + reduceGenericGraph(graph, cameraKeys, landmarkKeys, dictionary, reduced); + LONGS_EQUAL(2, reduced.size()); + LONGS_EQUAL(1, reduced[0]->key1.index); LONGS_EQUAL(2, reduced[0]->key2.index); + LONGS_EQUAL(2, reduced[1]->key1.index); LONGS_EQUAL(7, reduced[1]->key2.index); +} + +/* ************************************************************************* */ +TEST ( GenerciGraph, hasCommonCamera ) +{ + std::set cameras1; cameras1 += 1, 2, 3, 4, 5; + std::set cameras2; cameras2 += 8, 7, 6, 5; + bool actual = hasCommonCamera(cameras1, cameras2); + CHECK(actual); +} + +/* ************************************************************************* */ +TEST ( GenerciGraph, hasCommonCamera2 ) +{ + std::set cameras1; cameras1 += 1, 3, 5, 7; + std::set cameras2; cameras2 += 2, 4, 6, 8, 10; + bool actual = hasCommonCamera(cameras1, cameras2); + CHECK(!actual); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ diff --git a/gtsam_unstable/partition/tests/testNestedDissection.cpp b/gtsam_unstable/partition/tests/testNestedDissection.cpp new file mode 100644 index 000000000..906178914 --- /dev/null +++ b/gtsam_unstable/partition/tests/testNestedDissection.cpp @@ -0,0 +1,339 @@ +/* + * testNestedDissection.cpp + * + * Created on: Nov 29, 2010 + * Author: nikai + * Description: unit tests for NestedDissection + */ + +#include // for operator += +#include // for operator += +#include // for operator += +using namespace boost::assign; +#include +#include +#include + +#include "SubmapPlanarSLAM.h" +#include "SubmapVisualSLAM.h" +#include "SubmapExamples.h" +#include "SubmapExamples3D.h" +#include "GenericGraph.h" +#include "NonlinearTSAM.h" +#include "partition/NestedDissection-inl.h" + +using namespace std; +using namespace gtsam; +using namespace gtsam::partition; + +/* ************************************************************************* */ +// x1 - x2 +// \ / +// l1 +TEST ( NestedDissection, oneIsland ) +{ + using namespace submapPlanarSLAM; + typedef TSAM2D::SubNLG SubNLG; + Graph fg; + fg.addOdometry(1, 2, Pose2(), odoNoise); + fg.addBearingRange(1, 1, Rot2(), 0., bearingRangeNoise); + fg.addBearingRange(2, 1, Rot2(), 0., bearingRangeNoise); + fg.addPoseConstraint(1, Pose2()); + + Ordering ordering; ordering += x1, x2, l1; + + int numNodeStopPartition = 1e3; + int minNodesPerMap = 1e3; + NestedDissection nd(fg, ordering, numNodeStopPartition, minNodesPerMap); + LONGS_EQUAL(4, nd.root()->size()); + LONGS_EQUAL(3, nd.root()->frontal().size()); + LONGS_EQUAL(0, nd.root()->children().size()); +} + +/* ************************************************************************* */ +// x1\ /x4 +// | x3 | +// x2/ \x5 +TEST ( NestedDissection, TwoIslands ) +{ + using namespace submapPlanarSLAM; + typedef TSAM2D::SubNLG SubNLG; + Graph fg; + fg.addOdometry(1, 2, Pose2(), odoNoise); + fg.addOdometry(1, 3, Pose2(), odoNoise); + fg.addOdometry(2, 3, Pose2(), odoNoise); + fg.addOdometry(3, 4, Pose2(), odoNoise); + fg.addOdometry(4, 5, Pose2(), odoNoise); + fg.addOdometry(3, 5, Pose2(), odoNoise); + fg.addPoseConstraint(1, Pose2()); + fg.addPoseConstraint(4, Pose2()); + Ordering ordering; ordering += x1, x2, x3, x4, x5; + + int numNodeStopPartition = 2; + int minNodesPerMap = 1; + NestedDissection nd(fg, ordering, numNodeStopPartition, minNodesPerMap); + // root submap + LONGS_EQUAL(0, nd.root()->size()); + LONGS_EQUAL(1, nd.root()->frontal().size()); + LONGS_EQUAL(0, nd.root()->separator().size()); + LONGS_EQUAL(2, nd.root()->children().size()); // 2 leaf submaps + + // the 1st submap + LONGS_EQUAL(2, nd.root()->children()[0]->frontal().size()); + LONGS_EQUAL(4, nd.root()->children()[0]->size()); + + // the 2nd submap + LONGS_EQUAL(2, nd.root()->children()[1]->frontal().size()); + LONGS_EQUAL(4, nd.root()->children()[1]->size()); +} + +/* ************************************************************************* */ +// x1\ /x4 +// x3 +// x2/ \x5 +TEST ( NestedDissection, FourIslands ) +{ + using namespace submapPlanarSLAM; + typedef TSAM2D::SubNLG SubNLG; + Graph fg; + fg.addOdometry(1, 3, Pose2(), odoNoise); + fg.addOdometry(2, 3, Pose2(), odoNoise); + fg.addOdometry(3, 4, Pose2(), odoNoise); + fg.addOdometry(3, 5, Pose2(), odoNoise); + fg.addPoseConstraint(1, Pose2()); + fg.addPoseConstraint(4, Pose2()); + Ordering ordering; ordering += x1, x2, x3, x4, x5; + + int numNodeStopPartition = 2; + int minNodesPerMap = 1; + NestedDissection nd(fg, ordering, numNodeStopPartition, minNodesPerMap); + LONGS_EQUAL(0, nd.root()->size()); + LONGS_EQUAL(1, nd.root()->frontal().size()); + LONGS_EQUAL(0, nd.root()->separator().size()); + LONGS_EQUAL(4, nd.root()->children().size()); // 4 leaf submaps + + // the 1st submap + LONGS_EQUAL(1, nd.root()->children()[0]->frontal().size()); + LONGS_EQUAL(2, nd.root()->children()[0]->size()); + + // the 2nd submap + LONGS_EQUAL(1, nd.root()->children()[1]->frontal().size()); + LONGS_EQUAL(2, nd.root()->children()[1]->size()); + + // the 3rd submap + LONGS_EQUAL(1, nd.root()->children()[2]->frontal().size()); + LONGS_EQUAL(1, nd.root()->children()[2]->size()); + + // the 4th submap + LONGS_EQUAL(1, nd.root()->children()[3]->frontal().size()); + LONGS_EQUAL(1, nd.root()->children()[3]->size()); +} + +/* ************************************************************************* */ +// x1\ /x3 +// | x2 | +// l6/ \x4 +// | +// x5 +TEST ( NestedDissection, weekLinks ) +{ + using namespace submapPlanarSLAM; + typedef TSAM2D::SubNLG SubNLG; + Graph fg; + fg.addOdometry(1, 2, Pose2(), odoNoise); + fg.addOdometry(2, 3, Pose2(), odoNoise); + fg.addOdometry(2, 4, Pose2(), odoNoise); + fg.addOdometry(3, 4, Pose2(), odoNoise); + fg.addBearingRange(1, 6, Rot2(), 0., bearingRangeNoise); + fg.addBearingRange(2, 6, Rot2(), 0., bearingRangeNoise); + fg.addBearingRange(5, 6, Rot2(), 0., bearingRangeNoise); + fg.addPoseConstraint(1, Pose2()); + fg.addPoseConstraint(4, Pose2()); + fg.addPoseConstraint(5, Pose2()); + Ordering ordering; ordering += x1, x2, x3, x4, x5, l6; + + int numNodeStopPartition = 2; + int minNodesPerMap = 1; + NestedDissection nd(fg, ordering, numNodeStopPartition, minNodesPerMap); + LONGS_EQUAL(0, nd.root()->size()); // one weeklink + LONGS_EQUAL(1, nd.root()->frontal().size()); + LONGS_EQUAL(0, nd.root()->separator().size()); + LONGS_EQUAL(3, nd.root()->children().size()); // 4 leaf submaps + LONGS_EQUAL(1, nd.root()->weeklinks().size()); + + // the 1st submap + LONGS_EQUAL(2, nd.root()->children()[0]->frontal().size()); // x3 and x4 + LONGS_EQUAL(4, nd.root()->children()[0]->size()); + + // the 2nd submap + LONGS_EQUAL(2, nd.root()->children()[1]->frontal().size()); // x1 and l6 + LONGS_EQUAL(4, nd.root()->children()[1]->size()); + // + // the 3rd submap + LONGS_EQUAL(1, nd.root()->children()[2]->frontal().size()); // x5 + LONGS_EQUAL(1, nd.root()->children()[2]->size()); +} + +/* ************************************************************************* */ +/** + * l1 l2 l3 + * | X | X | + * x0 - x1 - x2 + * | X | X | + * l4 l5 l6 + */ +TEST ( NestedDissection, manual_cuts ) +{ + using namespace submapPlanarSLAM; + typedef partition::Cuts Cuts; + typedef TSAM2D::SubNLG SubNLG; + typedef partition::PartitionTable PartitionTable; + Graph fg; + fg.addOdometry(x0, x1, Pose2(1.0, 0, 0), odoNoise); + fg.addOdometry(x1, x2, Pose2(1.0, 0, 0), odoNoise); + + fg.addBearingRange(x0, l1, Rot2::fromAngle( M_PI_2), 1, bearingRangeNoise); + fg.addBearingRange(x0, l4, Rot2::fromAngle(-M_PI_2), 1, bearingRangeNoise); + fg.addBearingRange(x0, l2, Rot2::fromAngle( M_PI_4), sqrt(2), bearingRangeNoise); + fg.addBearingRange(x0, l5, Rot2::fromAngle(-M_PI_4), sqrt(2), bearingRangeNoise); + + fg.addBearingRange(x1, l1, Rot2::fromAngle( M_PI_4 * 3), sqrt(2), bearingRangeNoise); + fg.addBearingRange(x1, l2, Rot2::fromAngle( M_PI_2), 1, bearingRangeNoise); + fg.addBearingRange(x1, l3, Rot2::fromAngle( M_PI_4), sqrt(2), bearingRangeNoise); + fg.addBearingRange(x1, l4, Rot2::fromAngle(-M_PI_4 * 3), sqrt(2), bearingRangeNoise); + fg.addBearingRange(x1, l5, Rot2::fromAngle( M_PI_2), 1, bearingRangeNoise); + fg.addBearingRange(x1, l6, Rot2::fromAngle(-M_PI_4), sqrt(2), bearingRangeNoise); + + fg.addBearingRange(x2, l2, Rot2::fromAngle( M_PI_4 * 3), sqrt(2), bearingRangeNoise); + fg.addBearingRange(x2, l5, Rot2::fromAngle(-M_PI_4 * 3), sqrt(2), bearingRangeNoise); + fg.addBearingRange(x2, l3, Rot2::fromAngle( M_PI_2), 1, bearingRangeNoise); + fg.addBearingRange(x2, l6, Rot2::fromAngle(-M_PI_2), 1, bearingRangeNoise); + + fg.addPrior(x0, Pose2(0.1, 0, 0), priorNoise); + + // generate ordering + Ordering ordering; ordering += x0, x1, x2, l1, l2, l3, l4, l5, l6; + + // define cuts + boost::shared_ptr cuts(new Cuts()); + cuts->partitionTable = PartitionTable(9, -1); PartitionTable* p = &cuts->partitionTable; + //x0 x1 x2 l1 l2 l3 l4 l5 l6 + (*p)[0]=1; (*p)[1]=0; (*p)[2]=2; (*p)[3]=1; (*p)[4]=0; (*p)[5]=2; (*p)[6]=1; (*p)[7]=0; (*p)[8]=2; + + cuts->children.push_back(boost::shared_ptr(new Cuts())); + cuts->children[0]->partitionTable = PartitionTable(9, -1); p = &cuts->children[0]->partitionTable; + //x0 x1 x2 l1 l2 l3 l4 l5 l6 + (*p)[0]=0; (*p)[1]=-1; (*p)[2]=-1; (*p)[3]=1; (*p)[4]=-1; (*p)[5]=-1; (*p)[6]=2; (*p)[7]=-1; (*p)[8]=-1; + + cuts->children.push_back(boost::shared_ptr(new Cuts())); + cuts->children[1]->partitionTable = PartitionTable(9, -1); p = &cuts->children[1]->partitionTable; + //x0 x1 x2 l1 l2 l3 l4 l5 l6 + (*p)[0]=-1; (*p)[1]=-1; (*p)[2]=0; (*p)[3]=-1; (*p)[4]=-1; (*p)[5]=1; (*p)[6]=-1; (*p)[7]=-1; (*p)[8]=2; + + + // nested dissection + NestedDissection nd(fg, ordering, cuts); + LONGS_EQUAL(2, nd.root()->size()); + LONGS_EQUAL(3, nd.root()->frontal().size()); + LONGS_EQUAL(0, nd.root()->separator().size()); + LONGS_EQUAL(2, nd.root()->children().size()); // 2 leaf submaps + LONGS_EQUAL(0, nd.root()->weeklinks().size()); + + // the 1st submap + LONGS_EQUAL(1, nd.root()->children()[0]->frontal().size()); // x0 + LONGS_EQUAL(4, nd.root()->children()[0]->size()); + LONGS_EQUAL(2, nd.root()->children()[0]->children().size()); + + // the 1-1st submap + LONGS_EQUAL(1, nd.root()->children()[0]->children()[0]->frontal().size()); // l1 + LONGS_EQUAL(2, nd.root()->children()[0]->children()[0]->size()); + + // the 1-2nd submap + LONGS_EQUAL(1, nd.root()->children()[0]->children()[1]->frontal().size()); // l4 + LONGS_EQUAL(2, nd.root()->children()[0]->children()[1]->size()); + + // the 2nd submap + LONGS_EQUAL(1, nd.root()->children()[1]->frontal().size()); // x2 + LONGS_EQUAL(3, nd.root()->children()[1]->size()); + LONGS_EQUAL(2, nd.root()->children()[1]->children().size()); + + // the 2-1st submap + LONGS_EQUAL(1, nd.root()->children()[1]->children()[0]->frontal().size()); // l3 + LONGS_EQUAL(2, nd.root()->children()[1]->children()[0]->size()); + + // the 2-2nd submap + LONGS_EQUAL(1, nd.root()->children()[1]->children()[1]->frontal().size()); // l6 + LONGS_EQUAL(2, nd.root()->children()[1]->children()[1]->size()); + +} + +/* ************************************************************************* */ +// l1-l8 l9-16 l17-124 +// / | / \ | \ +// x0 x1 x2 x3 +TEST( NestedDissection, Graph3D) { + using namespace gtsam::submapVisualSLAM; + typedef TSAM3D::SubNLG SubNLG; + typedef partition::PartitionTable PartitionTable; + vector cameras; + cameras.push_back(GeneralCamera(Pose3(Rot3(), Point3(-2., 0., 0.)))); + cameras.push_back(GeneralCamera(Pose3(Rot3(), Point3(-1., 0., 0.)))); + cameras.push_back(GeneralCamera(Pose3(Rot3(), Point3( 1., 0., 0.)))); + cameras.push_back(GeneralCamera(Pose3(Rot3(), Point3( 2., 0., 0.)))); + + vector points; + for (int cube_index = 0; cube_index <= 3; cube_index++) { + Point3 center((cube_index-1) * 3, 0.5, 10.); + points.push_back(center + Point3(-0.5, -0.5, -0.5)); + points.push_back(center + Point3(-0.5, 0.5, -0.5)); + points.push_back(center + Point3( 0.5, 0.5, -0.5)); + points.push_back(center + Point3( 0.5, -0.5, -0.5)); + points.push_back(center + Point3(-0.5, -0.5, 0.5)); + points.push_back(center + Point3(-0.5, 0.5, 0.5)); + points.push_back(center + Point3( 0.5, 0.5, 0.5)); + points.push_back(center + Point3( 0.5, 0.5, 0.5)); + } + + Graph graph; + SharedDiagonal measurementNoise(gtsam::Vector_(2, 1., 1.)); + SharedDiagonal measurementZeroNoise(gtsam::Vector_(2, 0., 0.)); + for (int j=1; j<=8; j++) + graph.addMeasurement(0, j, cameras[0].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise); + for (int j=1; j<=16; j++) + graph.addMeasurement(1, j, cameras[1].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise); + for (int j=9; j<=24; j++) + graph.addMeasurement(2, j, cameras[2].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise); + for (int j=17; j<=24; j++) + graph.addMeasurement(3, j, cameras[3].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise); + + // make an easy ordering + Ordering ordering; ordering += x0, x1, x2, x3; + for (int j=1; j<=24; j++) + ordering += Symbol('l', j); + + // nested dissection + const int numNodeStopPartition = 10; + const int minNodesPerMap = 5; + NestedDissection nd(graph, ordering, numNodeStopPartition, minNodesPerMap); + + LONGS_EQUAL(0, nd.root()->size()); + LONGS_EQUAL(8, nd.root()->frontal().size()); // l9-l16 + LONGS_EQUAL(0, nd.root()->separator().size()); + LONGS_EQUAL(2, nd.root()->children().size()); // 2 leaf submaps + LONGS_EQUAL(0, nd.root()->weeklinks().size()); + + // the 1st submap + LONGS_EQUAL(10, nd.root()->children()[0]->frontal().size()); // x0, x1, l1-l8 + LONGS_EQUAL(24, nd.root()->children()[0]->size()); // 8 + 16 + LONGS_EQUAL(0, nd.root()->children()[0]->children().size()); + + // the 2nd submap + LONGS_EQUAL(10, nd.root()->children()[1]->frontal().size()); // x2, x3, l1-l8 + LONGS_EQUAL(24, nd.root()->children()[1]->size()); // 16 + 8 + LONGS_EQUAL(0, nd.root()->children()[1]->children().size()); +} + + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ diff --git a/gtsam_unstable/slam/AHRS.cpp b/gtsam_unstable/slam/AHRS.cpp index 002c9e3e6..af562c1b2 100644 --- a/gtsam_unstable/slam/AHRS.cpp +++ b/gtsam_unstable/slam/AHRS.cpp @@ -25,7 +25,7 @@ Matrix Z3 = zeros(3, 3); /* ************************************************************************* */ AHRS::AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e, - bool flat) : + bool flat) : KF_(9) { // Initial state @@ -60,7 +60,7 @@ AHRS::AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e, sigmas_v_a_ = esqrt(T * Pa_.diagonal()); // gravity in nav frame - n_g_ = Vector_(3, 0.0, 0.0, g_e); + n_g_ = (Vector(3) << 0.0, 0.0, g_e); n_g_cross_ = skewSymmetric(n_g_); // nav frame has Z down !!! } @@ -70,9 +70,9 @@ std::pair AHRS::initialize(double g_e) // Calculate Omega_T, formula 2.80 in Farrell08book double cp = cos(mech0_.bRn().inverse().pitch()); double sp = sin(mech0_.bRn().inverse().pitch()); - double cy = cos(0); - double sy = sin(0); - Matrix Omega_T = Matrix_(3, 3, cy * cp, -sy, 0.0, sy * cp, cy, 0.0, -sp, 0.0, 1.0); + double cy = cos(0.0); + double sy = sin(0.0); + Matrix Omega_T = (Matrix(3, 3) << cy * cp, -sy, 0.0, sy * cp, cy, 0.0, -sp, 0.0, 1.0); // Calculate Jacobian of roll/pitch/yaw wrpt (g1,g2,g3), see doc/ypr.nb Vector b_g = mech0_.b_g(g_e); @@ -82,7 +82,7 @@ std::pair AHRS::initialize(double g_e) double g23 = g2 * g2 + g3 * g3; double g123 = g1 * g1 + g23; double f = 1 / (std::sqrt(g23) * g123); - Matrix H_g = Matrix_(3, 3, + Matrix H_g = (Matrix(3, 3) << 0.0, g3 / g23, -(g2 / g23), // roll std::sqrt(g23) / g123, -f * (g1 * g2), -f * (g1 * g3), // pitch 0.0, 0.0, 0.0); // we don't know anything on yaw @@ -94,17 +94,17 @@ std::pair AHRS::initialize(double g_e) Matrix P12 = -Omega_T * H_g * Pa; Matrix P_plus_k2 = Matrix(9, 9); - P_plus_k2.block(0, 0, 3, 3) = P11; - P_plus_k2.block(0, 3, 3, 3) = Z3; - P_plus_k2.block(0, 6, 3, 3) = P12; + P_plus_k2.block<3,3>(0, 0) = P11; + P_plus_k2.block<3,3>(0, 3) = Z3; + P_plus_k2.block<3,3>(0, 6) = P12; - P_plus_k2.block(3, 0, 3, 3) = Z3; - P_plus_k2.block(3, 3, 3, 3) = Pg_; - P_plus_k2.block(3, 6, 3, 3) = Z3; + P_plus_k2.block<3,3>(3, 0) = Z3; + P_plus_k2.block<3,3>(3, 3) = Pg_; + P_plus_k2.block<3,3>(3, 6) = Z3; - P_plus_k2.block(6, 0, 3, 3) = trans(P12); - P_plus_k2.block(6, 3, 3, 3) = Z3; - P_plus_k2.block(6, 6, 3, 3) = Pa; + P_plus_k2.block<3,3>(6, 0) = trans(P12); + P_plus_k2.block<3,3>(6, 3) = Z3; + P_plus_k2.block<3,3>(6, 6) = Pa; Vector dx = zero(9); KalmanFilter::State state = KF_.init(dx, P_plus_k2); @@ -127,19 +127,20 @@ std::pair AHRS::integrate( Matrix Z3 = zeros(3, 3); Matrix F_k = zeros(9, 9); - F_k.block(0, 3, 3, 3) = -bRn; - F_k.block(3, 3, 3, 3) = F_g_; - F_k.block(6, 6, 3, 3) = F_a_; + F_k.block<3,3>(0, 3) = -bRn; + F_k.block<3,3>(3, 3) = F_g_; + F_k.block<3,3>(6, 6) = F_a_; Matrix G_k = zeros(9, 12); - G_k.block(0, 0, 3, 3) = -bRn; - G_k.block(0, 6, 3, 3) = -bRn; - G_k.block(3, 3, 3, 3) = I3; - G_k.block(6, 9, 3, 3) = I3; + G_k.block<3,3>(0, 0) = -bRn; + G_k.block<3,3>(0, 6) = -bRn; + G_k.block<3,3>(3, 3) = I3; + G_k.block<3,3>(6, 9) = I3; Matrix Q_k = G_k * var_w_ * trans(G_k); Matrix Psi_k = eye(9) + dt * F_k; // +dt*dt*F_k*F_k/2; // transition matrix + // This implements update in section 10.6 Matrix B = zeros(9, 9); Vector u2 = zero(9); KalmanFilter::State newState = KF_.predictQ(state, Psi_k,B,u2,dt*Q_k); @@ -148,21 +149,21 @@ std::pair AHRS::integrate( /* ************************************************************************* */ bool AHRS::isAidingAvailable(const Mechanization_bRn2& mech, - const gtsam::Vector& f, const gtsam::Vector& u, double ge) { + const gtsam::Vector& f, const gtsam::Vector& u, double ge) const { // Subtract the biases Vector f_ = f - mech.x_a(); Vector u_ = u - mech.x_g(); - double mu_f = f_.norm() - ge; - double mu_u = u_.norm(); - return (fabs(mu_f)<0.5 && mu_u < 5.0 / 180.0 * 3.1415926); + double mu_f = f_.norm() - ge; // accelerometer same magnitude as local gravity ? + double mu_u = u_.norm(); // gyro says we are not maneuvering ? + return (fabs(mu_f)<0.5 && mu_u < 5.0 / 180.0 * M_PI); } /* ************************************************************************* */ std::pair AHRS::aid( const Mechanization_bRn2& mech, KalmanFilter::State state, - const Vector& f, bool Farrell) { + const Vector& f, bool Farrell) const { Matrix bRn = mech.bRn().matrix(); @@ -182,8 +183,8 @@ std::pair AHRS::aid( // F(:,k) = mech.x_a + dx_a - bRn*n_g; // F(:,k) = mech.x_a + dx_a - bRn*(I+P)*n_g; // F(:,k) = mech.x_a + dx_a - b_g - bRn*(rho x n_g); // P = [rho]_x - // Hence, the measurement z = b_g - (mech.x_a - F(:,k)) is predicted - // from the filter state (dx_a, rho) as dx_a + bRn*(n_g x rho) + // Hence, the measurement z = b_g - (mech.x_a - F(:,k)) is predicted + // from the filter state (dx_a, rho) as dx_a + bRn*(n_g x rho) // z = b_g - (mech.x_a - F(:,k)) = dx_a + bRn*(n_g x rho) z = bRn * n_g_ - measured_b_g; // Now the Jacobian H @@ -210,7 +211,7 @@ std::pair AHRS::aid( std::pair AHRS::aidGeneral( const Mechanization_bRn2& mech, KalmanFilter::State state, const Vector& f, const Vector& f_previous, - const Rot3& R_previous) { + const Rot3& R_previous) const { Matrix increment = R_previous.between(mech.bRn()).matrix(); @@ -220,8 +221,8 @@ std::pair AHRS::aidGeneral( Matrix b_g = skewSymmetric(increment* f_previous); Matrix H = collect(3, &b_g, &I3, &Z3); // Matrix R = diag(emul(sigmas_v_a_, sigmas_v_a_)); -// Matrix R = diag(Vector_(3, 1.0, 0.2, 1.0)); // good for L_twice - Matrix R = diag(Vector_(3, 0.01, 0.0001, 0.01)); +// Matrix R = diag((Vector(3) << 1.0, 0.2, 1.0)); // good for L_twice + Matrix R = diag((Vector(3) << 0.01, 0.0001, 0.01)); // update the Kalman filter KalmanFilter::State updatedState = KF_.updateQ(state, H, z, R); diff --git a/gtsam_unstable/slam/AHRS.h b/gtsam_unstable/slam/AHRS.h index 1d7eb85f5..81d74a9f5 100644 --- a/gtsam_unstable/slam/AHRS.h +++ b/gtsam_unstable/slam/AHRS.h @@ -52,15 +52,22 @@ public: const Vector& u, double dt); bool isAidingAvailable(const Mechanization_bRn2& mech, - const Vector& f, const Vector& u, double ge); + const Vector& f, const Vector& u, double ge) const; + /** + * Aid the AHRS with an accelerometer reading, typically called when isAidingAvailable == true + * @param mech current mechanization state + * @param state current Kalman filter state + * @param f accelerometer reading + * @param Farrell + */ std::pair aid( const Mechanization_bRn2& mech, KalmanFilter::State state, - const Vector& f, bool Farrell=0); + const Vector& f, bool Farrell=0) const; std::pair aidGeneral( const Mechanization_bRn2& mech, KalmanFilter::State state, - const Vector& f, const Vector& f_expected, const Rot3& R_previous); + const Vector& f, const Vector& f_expected, const Rot3& R_previous) const; /// print void print(const std::string& s = "") const; diff --git a/gtsam_unstable/slam/BetweenFactorEM.h b/gtsam_unstable/slam/BetweenFactorEM.h index e87f69a23..234ad25bc 100644 --- a/gtsam_unstable/slam/BetweenFactorEM.h +++ b/gtsam_unstable/slam/BetweenFactorEM.h @@ -52,6 +52,8 @@ namespace gtsam { double prior_inlier_; double prior_outlier_; + bool flag_bump_up_near_zero_probs_; + /** concept check by type */ GTSAM_CONCEPT_LIE_TYPE(T) GTSAM_CONCEPT_TESTABLE_TYPE(T) @@ -67,10 +69,10 @@ namespace gtsam { /** Constructor */ BetweenFactorEM(Key key1, Key key2, const VALUE& measured, const SharedGaussian& model_inlier, const SharedGaussian& model_outlier, - const double prior_inlier, const double prior_outlier) : - Base(key1, key2), key1_(key1), key2_(key2), measured_(measured), + const double prior_inlier, const double prior_outlier, const bool flag_bump_up_near_zero_probs = false) : + Base(cref_list_of<2>(key1)(key2)), key1_(key1), key2_(key2), measured_(measured), model_inlier_(model_inlier), model_outlier_(model_outlier), - prior_inlier_(prior_inlier), prior_outlier_(prior_outlier){ + prior_inlier_(prior_inlier), prior_outlier_(prior_outlier), flag_bump_up_near_zero_probs_(flag_bump_up_near_zero_probs){ } virtual ~BetweenFactorEM() {} @@ -119,7 +121,7 @@ namespace gtsam { * Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$ */ /* This version of linearize recalculates the noise model each time */ - virtual boost::shared_ptr linearize(const gtsam::Values& x, const gtsam::Ordering& ordering) const { + virtual boost::shared_ptr linearize(const gtsam::Values& x) const { // Only linearize if the factor is active if (!this->active(x)) return boost::shared_ptr(); @@ -132,7 +134,7 @@ namespace gtsam { A2 = A[1]; return gtsam::GaussianFactor::shared_ptr( - new gtsam::JacobianFactor(ordering[key1_], A1, ordering[key2_], A2, b, gtsam::noiseModel::Unit::Create(b.size()))); + new gtsam::JacobianFactor(key1_, A1, key2_, A2, b, gtsam::noiseModel::Unit::Create(b.size()))); } @@ -165,17 +167,17 @@ namespace gtsam { Vector err_wh_eq; err_wh_eq.resize(err_wh_inlier.rows()*2); - err_wh_eq << std::sqrt(p_inlier) * err_wh_inlier.array() , std::sqrt(p_outlier) * err_wh_outlier.array(); + err_wh_eq << sqrt(p_inlier) * err_wh_inlier.array() , sqrt(p_outlier) * err_wh_outlier.array(); if (H){ // stack Jacobians for the two indicators for each of the key - Matrix H1_inlier = std::sqrt(p_inlier)*model_inlier_->Whiten(H1); - Matrix H1_outlier = std::sqrt(p_outlier)*model_outlier_->Whiten(H1); + Matrix H1_inlier = sqrt(p_inlier)*model_inlier_->Whiten(H1); + Matrix H1_outlier = sqrt(p_outlier)*model_outlier_->Whiten(H1); Matrix H1_aug = gtsam::stack(2, &H1_inlier, &H1_outlier); - Matrix H2_inlier = std::sqrt(p_inlier)*model_inlier_->Whiten(H2); - Matrix H2_outlier = std::sqrt(p_outlier)*model_outlier_->Whiten(H2); + Matrix H2_inlier = sqrt(p_inlier)*model_inlier_->Whiten(H2); + Matrix H2_outlier = sqrt(p_outlier)*model_outlier_->Whiten(H2); Matrix H2_aug = gtsam::stack(2, &H2_inlier, &H2_outlier); (*H)[0].resize(H1_aug.rows(),H1_aug.cols()); @@ -223,6 +225,8 @@ namespace gtsam { /* ************************************************************************* */ gtsam::Vector calcIndicatorProb(const gtsam::Values& x) const { + bool debug = false; + Vector err = unwhitenedError(x); // Calculate indicator probabilities (inlier and outlier) @@ -232,33 +236,42 @@ namespace gtsam { Matrix invCov_inlier = model_inlier_->R().transpose() * model_inlier_->R(); Matrix invCov_outlier = model_outlier_->R().transpose() * model_outlier_->R(); - double p_inlier = prior_inlier_ * sqrt(invCov_inlier.norm()) * exp( -0.5 * err_wh_inlier.dot(err_wh_inlier) ); - double p_outlier = prior_outlier_ * sqrt(invCov_outlier.norm()) * exp( -0.5 * err_wh_outlier.dot(err_wh_outlier) ); + double p_inlier = prior_inlier_ * std::sqrt(invCov_inlier.norm()) * exp( -0.5 * err_wh_inlier.dot(err_wh_inlier) ); + double p_outlier = prior_outlier_ * std::sqrt(invCov_outlier.norm()) * exp( -0.5 * err_wh_outlier.dot(err_wh_outlier) ); + + if (debug){ + std::cout<<"in calcIndicatorProb. err_unwh: "<(key1_); const T& p2 = x.at(key2_); @@ -269,6 +282,16 @@ namespace gtsam { return measured_.localCoordinates(hx); } + /* ************************************************************************* */ + void set_flag_bump_up_near_zero_probs(bool flag) { + flag_bump_up_near_zero_probs_ = flag; + } + + /* ************************************************************************* */ + bool get_flag_bump_up_near_zero_probs() const { + return flag_bump_up_near_zero_probs_; + } + /* ************************************************************************* */ /** return the measured */ const VALUE& measured() const { diff --git a/gtsam_unstable/slam/CMakeLists.txt b/gtsam_unstable/slam/CMakeLists.txt index bfbac5734..a86beac63 100644 --- a/gtsam_unstable/slam/CMakeLists.txt +++ b/gtsam_unstable/slam/CMakeLists.txt @@ -7,28 +7,5 @@ file(GLOB slam_headers "*.h") list(REMOVE_ITEM slam_headers ${slam_excluded_headers}) install(FILES ${slam_headers} DESTINATION include/gtsam_unstable/slam) -# Components to link tests in this subfolder against -set(slam_local_libs - slam_unstable - slam - nonlinear - linear - inference - geometry - base - ccolamd -) - -set (slam_full_libs - ${gtsam-default} - ${gtsam_unstable-default}) - -# Exclude tests that don't work -set (slam_excluded_tests - "${CMAKE_CURRENT_SOURCE_DIR}/tests/testSerialization.cpp" - "${CMAKE_CURRENT_SOURCE_DIR}/tests/testSmartProjectionFactor.cpp" -# "" # Add to this list, with full path, to exclude -) # Add all tests -gtsam_add_subdir_tests(slam_unstable "${slam_local_libs}" "${slam_full_libs}" "${slam_excluded_tests}") -add_dependencies(check.unstable check.slam_unstable) +add_subdirectory(tests) diff --git a/gtsam_unstable/slam/DummyFactor.cpp b/gtsam_unstable/slam/DummyFactor.cpp index 6f4b3b109..1ce3a878d 100644 --- a/gtsam_unstable/slam/DummyFactor.cpp +++ b/gtsam_unstable/slam/DummyFactor.cpp @@ -7,13 +7,16 @@ #include +#include #include +using namespace boost::assign; + namespace gtsam { /* ************************************************************************* */ DummyFactor::DummyFactor(const Key& key1, size_t dim1, const Key& key2, size_t dim2) -: NonlinearFactor(key1, key2) +: NonlinearFactor(cref_list_of<2>(key1)(key2)) { dims_.push_back(dim1); dims_.push_back(dim2); @@ -38,15 +41,15 @@ bool DummyFactor::equals(const NonlinearFactor& f, double tol) const { /* ************************************************************************* */ boost::shared_ptr -DummyFactor::linearize(const Values& c, const Ordering& ordering) const { +DummyFactor::linearize(const Values& c) const { // Only linearize if the factor is active if (!this->active(c)) return boost::shared_ptr(); // Fill in terms with zero matrices - std::vector > terms(this->size()); + std::vector > terms(this->size()); for(size_t j=0; jsize(); ++j) { - terms[j].first = ordering[this->keys()[j]]; + terms[j].first = keys()[j]; terms[j].second = zeros(rowDim_, dims_[j]); } diff --git a/gtsam_unstable/slam/DummyFactor.h b/gtsam_unstable/slam/DummyFactor.h index d2ea2567f..0b0a1bd6c 100644 --- a/gtsam_unstable/slam/DummyFactor.h +++ b/gtsam_unstable/slam/DummyFactor.h @@ -54,8 +54,7 @@ public: virtual size_t dim() const { return rowDim_; } /** linearize to a GaussianFactor */ - virtual boost::shared_ptr - linearize(const Values& c, const Ordering& ordering) const; + virtual boost::shared_ptr linearize(const Values& c) const; /** * Creates a shared_ptr clone of the factor - needs to be specialized to allow diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h index 564f5a2c8..b2174f8a9 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h @@ -39,8 +39,8 @@ namespace gtsam { * ===== * Concept: Based on [Lupton12tro] * - Pre-integrate IMU measurements using the static function PreIntegrateIMUObservations. - * Pre-integrated quantities are expressed in the body system of t0 - the first time instant (in which pre-integration began). - * All sensor-to-body transformations are performed here. + * Pre-integrated quantities are expressed in the body system of t0 - the first time instant (in which pre-integration began). + * All sensor-to-body transformations are performed here. * - If required, calculate inertial solution by calling the static functions: predictPose_inertial, predictVelocity_inertial. * - When the time is right, incorporate pre-integrated IMU data by creating an EquivInertialNavFactor_GlobalVel factor, which will * relate between navigation variables at the two time instances (t0 and current time). @@ -54,11 +54,11 @@ namespace gtsam { * matrices and the process\modeling covariance matrix. The IneritalNavFactor converts this into a * discrete form using the supplied delta_t between sub-sequential measurements. * - Earth-rate correction: - * + Currently the user should supply R_ECEF_to_G, which is the rotation from ECEF to the global - * frame (Local-Level system: ENU or NED, see above). - * + R_ECEF_to_G can be calculated by approximated values of latitude and longitude of the system. - * + Currently it is assumed that a relatively small distance is traveled w.r.t. to initial pose, since R_ECEF_to_G is constant. - * Otherwise, R_ECEF_to_G should be updated each time using the current lat-lon. + * + Currently the user should supply R_ECEF_to_G, which is the rotation from ECEF to the global + * frame (Local-Level system: ENU or NED, see above). + * + R_ECEF_to_G can be calculated by approximated values of latitude and longitude of the system. + * + Currently it is assumed that a relatively small distance is traveled w.r.t. to initial pose, since R_ECEF_to_G is constant. + * Otherwise, R_ECEF_to_G should be updated each time using the current lat-lon. * * - Frame Notation: * Quantities are written as {Frame of Representation/Destination Frame}_{Quantity Type}_{Quatity Description/Origination Frame} @@ -92,256 +92,260 @@ class EquivInertialNavFactor_GlobalVel : public NoiseModelFactor5 This; - typedef NoiseModelFactor5 Base; + typedef EquivInertialNavFactor_GlobalVel This; + typedef NoiseModelFactor5 Base; - Vector delta_pos_in_t0_; - Vector delta_vel_in_t0_; - Vector3 delta_angles_; - double dt12_; + Vector delta_pos_in_t0_; + Vector delta_vel_in_t0_; + Vector3 delta_angles_; + double dt12_; - Vector world_g_; - Vector world_rho_; - Vector world_omega_earth_; + Vector world_g_; + Vector world_rho_; + Vector world_omega_earth_; - Matrix Jacobian_wrt_t0_Overall_; + Matrix Jacobian_wrt_t0_Overall_; - boost::optional Bias_initial_; // Bias used when pre-integrating IMU measurements - boost::optional body_P_sensor_; // The pose of the sensor in the body frame + boost::optional Bias_initial_; // Bias used when pre-integrating IMU measurements + boost::optional body_P_sensor_; // The pose of the sensor in the body frame public: - // shorthand for a smart pointer to a factor - typedef typename boost::shared_ptr shared_ptr; + // shorthand for a smart pointer to a factor + typedef typename boost::shared_ptr shared_ptr; - /** default constructor - only use for serialization */ - EquivInertialNavFactor_GlobalVel() {} + /** default constructor - only use for serialization */ + EquivInertialNavFactor_GlobalVel() {} - /** Constructor */ - EquivInertialNavFactor_GlobalVel(const Key& Pose1, const Key& Vel1, const Key& IMUBias1, const Key& Pose2, const Key& Vel2, - const Vector& delta_pos_in_t0, const Vector& delta_vel_in_t0, const Vector3& delta_angles, - double dt12, const Vector world_g, const Vector world_rho, - const Vector& world_omega_earth, const noiseModel::Gaussian::shared_ptr& model_equivalent, - const Matrix& Jacobian_wrt_t0_Overall, - boost::optional Bias_initial = boost::none, boost::optional body_P_sensor = boost::none) : - Base(model_equivalent, Pose1, Vel1, IMUBias1, Pose2, Vel2), - delta_pos_in_t0_(delta_pos_in_t0), delta_vel_in_t0_(delta_vel_in_t0), delta_angles_(delta_angles), - dt12_(dt12), world_g_(world_g), world_rho_(world_rho), world_omega_earth_(world_omega_earth), Jacobian_wrt_t0_Overall_(Jacobian_wrt_t0_Overall), - Bias_initial_(Bias_initial), body_P_sensor_(body_P_sensor) { } + /** Constructor */ + EquivInertialNavFactor_GlobalVel(const Key& Pose1, const Key& Vel1, const Key& IMUBias1, const Key& Pose2, const Key& Vel2, + const Vector& delta_pos_in_t0, const Vector& delta_vel_in_t0, const Vector3& delta_angles, + double dt12, const Vector world_g, const Vector world_rho, + const Vector& world_omega_earth, const noiseModel::Gaussian::shared_ptr& model_equivalent, + const Matrix& Jacobian_wrt_t0_Overall, + boost::optional Bias_initial = boost::none, boost::optional body_P_sensor = boost::none) : + Base(model_equivalent, Pose1, Vel1, IMUBias1, Pose2, Vel2), + delta_pos_in_t0_(delta_pos_in_t0), delta_vel_in_t0_(delta_vel_in_t0), delta_angles_(delta_angles), + dt12_(dt12), world_g_(world_g), world_rho_(world_rho), world_omega_earth_(world_omega_earth), Jacobian_wrt_t0_Overall_(Jacobian_wrt_t0_Overall), + Bias_initial_(Bias_initial), body_P_sensor_(body_P_sensor) { } - virtual ~EquivInertialNavFactor_GlobalVel() {} + virtual ~EquivInertialNavFactor_GlobalVel() {} - /** implement functions needed for Testable */ + /** implement functions needed for Testable */ - /** print */ - virtual void print(const std::string& s = "EquivInertialNavFactor_GlobalVel", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - std::cout << s << "(" - << keyFormatter(this->key1()) << "," - << keyFormatter(this->key2()) << "," - << keyFormatter(this->key3()) << "," - << keyFormatter(this->key4()) << "," - << keyFormatter(this->key5()) << "\n"; - std::cout << "delta_pos_in_t0: " << this->delta_pos_in_t0_.transpose() << std::endl; - std::cout << "delta_vel_in_t0: " << this->delta_vel_in_t0_.transpose() << std::endl; - std::cout << "delta_angles: " << this->delta_angles_ << std::endl; - std::cout << "dt12: " << this->dt12_ << std::endl; - std::cout << "gravity (in world frame): " << this->world_g_.transpose() << std::endl; - std::cout << "craft rate (in world frame): " << this->world_rho_.transpose() << std::endl; - std::cout << "earth's rotation (in world frame): " << this->world_omega_earth_.transpose() << std::endl; - if(this->body_P_sensor_) - this->body_P_sensor_->print(" sensor pose in body frame: "); - this->noiseModel_->print(" noise model"); - } + /** print */ + virtual void print(const std::string& s = "EquivInertialNavFactor_GlobalVel", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + std::cout << s << "(" + << keyFormatter(this->key1()) << "," + << keyFormatter(this->key2()) << "," + << keyFormatter(this->key3()) << "," + << keyFormatter(this->key4()) << "," + << keyFormatter(this->key5()) << "\n"; + std::cout << "delta_pos_in_t0: " << this->delta_pos_in_t0_.transpose() << std::endl; + std::cout << "delta_vel_in_t0: " << this->delta_vel_in_t0_.transpose() << std::endl; + std::cout << "delta_angles: " << this->delta_angles_ << std::endl; + std::cout << "dt12: " << this->dt12_ << std::endl; + std::cout << "gravity (in world frame): " << this->world_g_.transpose() << std::endl; + std::cout << "craft rate (in world frame): " << this->world_rho_.transpose() << std::endl; + std::cout << "earth's rotation (in world frame): " << this->world_omega_earth_.transpose() << std::endl; + if(this->body_P_sensor_) + this->body_P_sensor_->print(" sensor pose in body frame: "); + this->noiseModel_->print(" noise model"); + } - /** equals */ - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { - const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) - && (delta_pos_in_t0_ - e->delta_pos_in_t0_).norm() < tol - && (delta_vel_in_t0_ - e->delta_vel_in_t0_).norm() < tol - && (delta_angles_ - e->delta_angles_).norm() < tol - && (dt12_ - e->dt12_) < tol - && (world_g_ - e->world_g_).norm() < tol - && (world_rho_ - e->world_rho_).norm() < tol - && (world_omega_earth_ - e->world_omega_earth_).norm() < tol - && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); - } + /** equals */ + virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { + const This *e = dynamic_cast (&expected); + return e != NULL && Base::equals(*e, tol) + && (delta_pos_in_t0_ - e->delta_pos_in_t0_).norm() < tol + && (delta_vel_in_t0_ - e->delta_vel_in_t0_).norm() < tol + && (delta_angles_ - e->delta_angles_).norm() < tol + && (dt12_ - e->dt12_) < tol + && (world_g_ - e->world_g_).norm() < tol + && (world_rho_ - e->world_rho_).norm() < tol + && (world_omega_earth_ - e->world_omega_earth_).norm() < tol + && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); + } - POSE predictPose(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1) const { + POSE predictPose(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1) const { - // Correct delta_pos_in_t0_ using (Bias1 - Bias_t0) - Vector delta_BiasAcc = Bias1.accelerometer(); - Vector delta_BiasGyro = Bias1.gyroscope(); - if (Bias_initial_){ - delta_BiasAcc -= Bias_initial_->accelerometer(); - delta_BiasGyro -= Bias_initial_->gyroscope(); - } + // Correct delta_pos_in_t0_ using (Bias1 - Bias_t0) + Vector delta_BiasAcc = Bias1.accelerometer(); + Vector delta_BiasGyro = Bias1.gyroscope(); + if (Bias_initial_){ + delta_BiasAcc -= Bias_initial_->accelerometer(); + delta_BiasGyro -= Bias_initial_->gyroscope(); + } - Matrix J_Pos_wrt_BiasAcc = Jacobian_wrt_t0_Overall_.block(4,9,3,3); - Matrix J_Pos_wrt_BiasGyro = Jacobian_wrt_t0_Overall_.block(4,12,3,3); - Matrix J_angles_wrt_BiasGyro = Jacobian_wrt_t0_Overall_.block(0,12,3,3); + Matrix J_Pos_wrt_BiasAcc = Jacobian_wrt_t0_Overall_.block(4,9,3,3); + Matrix J_Pos_wrt_BiasGyro = Jacobian_wrt_t0_Overall_.block(4,12,3,3); + Matrix J_angles_wrt_BiasGyro = Jacobian_wrt_t0_Overall_.block(0,12,3,3); - /* Position term */ - Vector delta_pos_in_t0_corrected = delta_pos_in_t0_ + J_Pos_wrt_BiasAcc*delta_BiasAcc + J_Pos_wrt_BiasGyro*delta_BiasGyro; + /* Position term */ + Vector delta_pos_in_t0_corrected = delta_pos_in_t0_ + J_Pos_wrt_BiasAcc*delta_BiasAcc + J_Pos_wrt_BiasGyro*delta_BiasGyro; - /* Rotation term */ - Vector delta_angles_corrected = delta_angles_ + J_angles_wrt_BiasGyro*delta_BiasGyro; - // Another alternative: - // Vector delta_angles_corrected = Rot3::Logmap( Rot3::Expmap(delta_angles_)*Rot3::Expmap(J_angles_wrt_BiasGyro*delta_BiasGyro) ); + /* Rotation term */ + Vector delta_angles_corrected = delta_angles_ + J_angles_wrt_BiasGyro*delta_BiasGyro; + // Another alternative: + // Vector delta_angles_corrected = Rot3::Logmap( Rot3::Expmap(delta_angles_)*Rot3::Expmap(J_angles_wrt_BiasGyro*delta_BiasGyro) ); - return predictPose_inertial(Pose1, Vel1, - delta_pos_in_t0_corrected, delta_angles_corrected, - dt12_, world_g_, world_rho_, world_omega_earth_); - } + return predictPose_inertial(Pose1, Vel1, + delta_pos_in_t0_corrected, delta_angles_corrected, + dt12_, world_g_, world_rho_, world_omega_earth_); + } - static inline POSE predictPose_inertial(const POSE& Pose1, const VELOCITY& Vel1, - const Vector& delta_pos_in_t0, const Vector3& delta_angles, - const double dt12, const Vector& world_g, const Vector& world_rho, const Vector& world_omega_earth){ + static inline POSE predictPose_inertial(const POSE& Pose1, const VELOCITY& Vel1, + const Vector& delta_pos_in_t0, const Vector3& delta_angles, + const double dt12, const Vector& world_g, const Vector& world_rho, const Vector& world_omega_earth){ - const POSE& world_P1_body = Pose1; - const VELOCITY& world_V1_body = Vel1; + const POSE& world_P1_body = Pose1; + const VELOCITY& world_V1_body = Vel1; - /* Position term */ - Vector body_deltaPos_body = delta_pos_in_t0; + /* Position term */ + Vector body_deltaPos_body = delta_pos_in_t0; - Vector world_deltaPos_pls_body = world_P1_body.rotation().matrix() * body_deltaPos_body; - Vector world_deltaPos_body = world_V1_body * dt12 + 0.5*world_g*dt12*dt12 + world_deltaPos_pls_body; + Vector world_deltaPos_pls_body = world_P1_body.rotation().matrix() * body_deltaPos_body; + Vector world_deltaPos_body = world_V1_body * dt12 + 0.5*world_g*dt12*dt12 + world_deltaPos_pls_body; - // Incorporate earth-related terms. Note - these are assumed to be constant between t1 and t2. - world_deltaPos_body -= 2*skewSymmetric(world_rho + world_omega_earth)*world_V1_body * dt12*dt12; + // Incorporate earth-related terms. Note - these are assumed to be constant between t1 and t2. + world_deltaPos_body -= 2*skewSymmetric(world_rho + world_omega_earth)*world_V1_body * dt12*dt12; - /* TODO: the term dt12*dt12 in 0.5*world_g*dt12*dt12 is not entirely correct: - * the gravity should be canceled from the accelerometer measurements, bust since position - * is added with a delta velocity from a previous term, the actual delta time is more complicated. - * Need to figure out this in the future - currently because of this issue we'll get some more error - * in Z axis. - */ + /* TODO: the term dt12*dt12 in 0.5*world_g*dt12*dt12 is not entirely correct: + * the gravity should be canceled from the accelerometer measurements, bust since position + * is added with a delta velocity from a previous term, the actual delta time is more complicated. + * Need to figure out this in the future - currently because of this issue we'll get some more error + * in Z axis. + */ - /* Rotation term */ - Vector body_deltaAngles_body = delta_angles; + /* Rotation term */ + Vector body_deltaAngles_body = delta_angles; - // Convert earth-related terms into the body frame - Matrix body_R_world(world_P1_body.rotation().inverse().matrix()); - Vector body_rho = body_R_world * world_rho; - Vector body_omega_earth = body_R_world * world_omega_earth; + // Convert earth-related terms into the body frame + Matrix body_R_world(world_P1_body.rotation().inverse().matrix()); + Vector body_rho = body_R_world * world_rho; + Vector body_omega_earth = body_R_world * world_omega_earth; - // Incorporate earth-related terms. Note - these are assumed to be constant between t1 and t2. - body_deltaAngles_body -= (body_rho + body_omega_earth)*dt12; + // Incorporate earth-related terms. Note - these are assumed to be constant between t1 and t2. + body_deltaAngles_body -= (body_rho + body_omega_earth)*dt12; - return POSE(Pose1.rotation() * POSE::Rotation::Expmap(body_deltaAngles_body), Pose1.translation() + typename POSE::Translation(world_deltaPos_body)); + return POSE(Pose1.rotation() * POSE::Rotation::Expmap(body_deltaAngles_body), Pose1.translation() + typename POSE::Translation(world_deltaPos_body)); - } + } - VELOCITY predictVelocity(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1) const { + VELOCITY predictVelocity(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1) const { - // Correct delta_vel_in_t0_ using (Bias1 - Bias_t0) - Vector delta_BiasAcc = Bias1.accelerometer(); - Vector delta_BiasGyro = Bias1.gyroscope(); - if (Bias_initial_){ - delta_BiasAcc -= Bias_initial_->accelerometer(); - delta_BiasGyro -= Bias_initial_->gyroscope(); - } + // Correct delta_vel_in_t0_ using (Bias1 - Bias_t0) + Vector delta_BiasAcc = Bias1.accelerometer(); + Vector delta_BiasGyro = Bias1.gyroscope(); + if (Bias_initial_){ + delta_BiasAcc -= Bias_initial_->accelerometer(); + delta_BiasGyro -= Bias_initial_->gyroscope(); + } - Matrix J_Vel_wrt_BiasAcc = Jacobian_wrt_t0_Overall_.block(6,9,3,3); - Matrix J_Vel_wrt_BiasGyro = Jacobian_wrt_t0_Overall_.block(6,12,3,3); + Matrix J_Vel_wrt_BiasAcc = Jacobian_wrt_t0_Overall_.block(6,9,3,3); + Matrix J_Vel_wrt_BiasGyro = Jacobian_wrt_t0_Overall_.block(6,12,3,3); - Vector delta_vel_in_t0_corrected = delta_vel_in_t0_ + J_Vel_wrt_BiasAcc*delta_BiasAcc + J_Vel_wrt_BiasGyro*delta_BiasGyro; + Vector delta_vel_in_t0_corrected = delta_vel_in_t0_ + J_Vel_wrt_BiasAcc*delta_BiasAcc + J_Vel_wrt_BiasGyro*delta_BiasGyro; - return predictVelocity_inertial(Pose1, Vel1, - delta_vel_in_t0_corrected, - dt12_, world_g_, world_rho_, world_omega_earth_); - } + return predictVelocity_inertial(Pose1, Vel1, + delta_vel_in_t0_corrected, + dt12_, world_g_, world_rho_, world_omega_earth_); + } - static inline VELOCITY predictVelocity_inertial(const POSE& Pose1, const VELOCITY& Vel1, - const Vector& delta_vel_in_t0, - const double dt12, const Vector& world_g, const Vector& world_rho, const Vector& world_omega_earth) { + static inline VELOCITY predictVelocity_inertial(const POSE& Pose1, const VELOCITY& Vel1, + const Vector& delta_vel_in_t0, + const double dt12, const Vector& world_g, const Vector& world_rho, const Vector& world_omega_earth) { - const POSE& world_P1_body = Pose1; - const VELOCITY& world_V1_body = Vel1; + const POSE& world_P1_body = Pose1; + const VELOCITY& world_V1_body = Vel1; - Vector body_deltaVel_body = delta_vel_in_t0; - Vector world_deltaVel_body = world_P1_body.rotation().matrix() * body_deltaVel_body; + Vector body_deltaVel_body = delta_vel_in_t0; + Vector world_deltaVel_body = world_P1_body.rotation().matrix() * body_deltaVel_body; - VELOCITY VelDelta( world_deltaVel_body + world_g * dt12 ); + VELOCITY VelDelta( world_deltaVel_body + world_g * dt12 ); - // Incorporate earth-related terms. Note - these are assumed to be constant between t1 and t2. - VelDelta -= 2*skewSymmetric(world_rho + world_omega_earth)*world_V1_body * dt12; + // Incorporate earth-related terms. Note - these are assumed to be constant between t1 and t2. + VelDelta -= 2*skewSymmetric(world_rho + world_omega_earth)*world_V1_body * dt12; - // Predict - return Vel1.compose( VelDelta ); + // Predict + return Vel1.compose( VelDelta ); - } + } - void predict(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, POSE& Pose2, VELOCITY& Vel2) const { - Pose2 = predictPose(Pose1, Vel1, Bias1); - Vel2 = predictVelocity(Pose1, Vel1, Bias1); - } + void predict(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, POSE& Pose2, VELOCITY& Vel2) const { + Pose2 = predictPose(Pose1, Vel1, Bias1); + Vel2 = predictVelocity(Pose1, Vel1, Bias1); + } - POSE evaluatePoseError(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, const POSE& Pose2, const VELOCITY& Vel2) const { - // Predict - POSE Pose2Pred = predictPose(Pose1, Vel1, Bias1); + POSE evaluatePoseError(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, const POSE& Pose2, const VELOCITY& Vel2) const { + // Predict + POSE Pose2Pred = predictPose(Pose1, Vel1, Bias1); - // Calculate error - return Pose2.between(Pose2Pred); - } + // Luca: difference between Pose2 and Pose2Pred + POSE DiffPose( Pose2.rotation().between(Pose2Pred.rotation()), Pose2Pred.translation() - Pose2.translation() ); +// DiffPose = Pose2.between(Pose2Pred); + return DiffPose; + // Calculate error + //return Pose2.between(Pose2Pred); + } - VELOCITY evaluateVelocityError(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, const POSE& Pose2, const VELOCITY& Vel2) const { - // Predict - VELOCITY Vel2Pred = predictVelocity(Pose1, Vel1, Bias1); + VELOCITY evaluateVelocityError(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, const POSE& Pose2, const VELOCITY& Vel2) const { + // Predict + VELOCITY Vel2Pred = predictVelocity(Pose1, Vel1, Bias1); - // Calculate error - return Vel2.between(Vel2Pred); - } + // Calculate error + return Vel2.between(Vel2Pred); + } - Vector evaluateError(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, const POSE& Pose2, const VELOCITY& Vel2, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none, - boost::optional H3 = boost::none, - boost::optional H4 = boost::none, - boost::optional H5 = boost::none) const { + Vector evaluateError(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, const POSE& Pose2, const VELOCITY& Vel2, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none, + boost::optional H3 = boost::none, + boost::optional H4 = boost::none, + boost::optional H5 = boost::none) const { - // TODO: Write analytical derivative calculations - // Jacobian w.r.t. Pose1 - if (H1){ - Matrix H1_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, _1, Vel1, Bias1, Pose2, Vel2), Pose1); - Matrix H1_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, _1, Vel1, Bias1, Pose2, Vel2), Pose1); - *H1 = stack(2, &H1_Pose, &H1_Vel); - } + // TODO: Write analytical derivative calculations + // Jacobian w.r.t. Pose1 + if (H1){ + Matrix H1_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, _1, Vel1, Bias1, Pose2, Vel2), Pose1); + Matrix H1_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, _1, Vel1, Bias1, Pose2, Vel2), Pose1); + *H1 = stack(2, &H1_Pose, &H1_Vel); + } - // Jacobian w.r.t. Vel1 - if (H2){ - Matrix H2_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1); - Matrix H2_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1); - *H2 = stack(2, &H2_Pose, &H2_Vel); - } + // Jacobian w.r.t. Vel1 + if (H2){ + Matrix H2_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1); + Matrix H2_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1); + *H2 = stack(2, &H2_Pose, &H2_Vel); + } - // Jacobian w.r.t. IMUBias1 - if (H3){ - Matrix H3_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, Vel1, _1, Pose2, Vel2), Bias1); - Matrix H3_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, Vel1, _1, Pose2, Vel2), Bias1); - *H3 = stack(2, &H3_Pose, &H3_Vel); - } + // Jacobian w.r.t. IMUBias1 + if (H3){ + Matrix H3_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, Vel1, _1, Pose2, Vel2), Bias1); + Matrix H3_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, Vel1, _1, Pose2, Vel2), Bias1); + *H3 = stack(2, &H3_Pose, &H3_Vel); + } - // Jacobian w.r.t. Pose2 - if (H4){ - Matrix H4_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, Vel1, Bias1, _1, Vel2), Pose2); - Matrix H4_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, Vel1, Bias1, _1, Vel2), Pose2); - *H4 = stack(2, &H4_Pose, &H4_Vel); - } + // Jacobian w.r.t. Pose2 + if (H4){ + Matrix H4_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, Vel1, Bias1, _1, Vel2), Pose2); + Matrix H4_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, Vel1, Bias1, _1, Vel2), Pose2); + *H4 = stack(2, &H4_Pose, &H4_Vel); + } - // Jacobian w.r.t. Vel2 - if (H5){ - Matrix H5_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2); - Matrix H5_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2); - *H5 = stack(2, &H5_Pose, &H5_Vel); - } + // Jacobian w.r.t. Vel2 + if (H5){ + Matrix H5_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2); + Matrix H5_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2); + *H5 = stack(2, &H5_Pose, &H5_Vel); + } - Vector ErrPoseVector(POSE::Logmap(evaluatePoseError(Pose1, Vel1, Bias1, Pose2, Vel2))); - Vector ErrVelVector(VELOCITY::Logmap(evaluateVelocityError(Pose1, Vel1, Bias1, Pose2, Vel2))); + Vector ErrPoseVector(POSE::Logmap(evaluatePoseError(Pose1, Vel1, Bias1, Pose2, Vel2))); + Vector ErrVelVector(VELOCITY::Logmap(evaluateVelocityError(Pose1, Vel1, Bias1, Pose2, Vel2))); - return concatVectors(2, &ErrPoseVector, &ErrVelVector); - } + return concatVectors(2, &ErrPoseVector, &ErrVelVector); + } @@ -409,134 +413,137 @@ public: static inline void PreIntegrateIMUObservations(const Vector& msr_acc_t, const Vector& msr_gyro_t, const double msr_dt, Vector& delta_pos_in_t0, Vector3& delta_angles, Vector& delta_vel_in_t0, double& delta_t, - const noiseModel::Gaussian::shared_ptr& model_continuous_overall, - Matrix& EquivCov_Overall, Matrix& Jacobian_wrt_t0_Overall, const IMUBIAS Bias_t0 = IMUBIAS(), - boost::optional p_body_P_sensor = boost::none){ - // Note: all delta terms refer to an IMU\sensor system at t0 - // Note: Earth-related terms are not accounted here but are incorporated in predict functions. + const noiseModel::Gaussian::shared_ptr& model_continuous_overall, + Matrix& EquivCov_Overall, Matrix& Jacobian_wrt_t0_Overall, const IMUBIAS Bias_t0 = IMUBIAS(), + boost::optional p_body_P_sensor = boost::none){ + // Note: all delta terms refer to an IMU\sensor system at t0 + // Note: Earth-related terms are not accounted here but are incorporated in predict functions. - POSE body_P_sensor = POSE(); - bool flag_use_body_P_sensor = false; - if (p_body_P_sensor){ - body_P_sensor = *p_body_P_sensor; - flag_use_body_P_sensor = true; - } + POSE body_P_sensor = POSE(); + bool flag_use_body_P_sensor = false; + if (p_body_P_sensor){ + body_P_sensor = *p_body_P_sensor; + flag_use_body_P_sensor = true; + } - delta_pos_in_t0 = PreIntegrateIMUObservations_delta_pos(msr_dt, delta_pos_in_t0, delta_vel_in_t0); - delta_vel_in_t0 = PreIntegrateIMUObservations_delta_vel(msr_gyro_t, msr_acc_t, msr_dt, delta_angles, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor, Bias_t0); - delta_angles = PreIntegrateIMUObservations_delta_angles(msr_gyro_t, msr_dt, delta_angles, flag_use_body_P_sensor, body_P_sensor, Bias_t0); + delta_pos_in_t0 = PreIntegrateIMUObservations_delta_pos(msr_dt, delta_pos_in_t0, delta_vel_in_t0); + delta_vel_in_t0 = PreIntegrateIMUObservations_delta_vel(msr_gyro_t, msr_acc_t, msr_dt, delta_angles, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor, Bias_t0); + delta_angles = PreIntegrateIMUObservations_delta_angles(msr_gyro_t, msr_dt, delta_angles, flag_use_body_P_sensor, body_P_sensor, Bias_t0); - delta_t += msr_dt; + delta_t += msr_dt; - // Update EquivCov_Overall - Matrix Z_3x3 = zeros(3,3); - Matrix I_3x3 = eye(3,3); + // Update EquivCov_Overall + Matrix Z_3x3 = zeros(3,3); + Matrix I_3x3 = eye(3,3); - Matrix H_pos_pos = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, _1, delta_vel_in_t0), delta_pos_in_t0); - Matrix H_pos_vel = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, delta_pos_in_t0, _1), delta_vel_in_t0); - Matrix H_pos_angles = Z_3x3; - Matrix H_pos_bias = collect(2, &Z_3x3, &Z_3x3); + Matrix H_pos_pos = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, _1, delta_vel_in_t0), delta_pos_in_t0); + Matrix H_pos_vel = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, delta_pos_in_t0, _1), delta_vel_in_t0); + Matrix H_pos_angles = Z_3x3; + Matrix H_pos_bias = collect(2, &Z_3x3, &Z_3x3); - Matrix H_vel_vel = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, _1, flag_use_body_P_sensor, body_P_sensor, Bias_t0), delta_vel_in_t0); - Matrix H_vel_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, _1, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor, Bias_t0), delta_angles); - Matrix H_vel_bias = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor, _1), Bias_t0); - Matrix H_vel_pos = Z_3x3; + Matrix H_vel_vel = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, _1, flag_use_body_P_sensor, body_P_sensor, Bias_t0), delta_vel_in_t0); + Matrix H_vel_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, _1, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor, Bias_t0), delta_angles); + Matrix H_vel_bias = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor, _1), Bias_t0); + Matrix H_vel_pos = Z_3x3; - Matrix H_angles_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, _1, flag_use_body_P_sensor, body_P_sensor, Bias_t0), delta_angles); - Matrix H_angles_bias = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, delta_angles, flag_use_body_P_sensor, body_P_sensor, _1), Bias_t0); - Matrix H_angles_pos = Z_3x3; - Matrix H_angles_vel = Z_3x3; + Matrix H_angles_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, _1, flag_use_body_P_sensor, body_P_sensor, Bias_t0), delta_angles); + Matrix H_angles_bias = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, delta_angles, flag_use_body_P_sensor, body_P_sensor, _1), Bias_t0); + Matrix H_angles_pos = Z_3x3; + Matrix H_angles_vel = Z_3x3; - Matrix F_angles = collect(4, &H_angles_angles, &H_angles_pos, &H_angles_vel, &H_angles_bias); - Matrix F_pos = collect(4, &H_pos_angles, &H_pos_pos, &H_pos_vel, &H_pos_bias); - Matrix F_vel = collect(4, &H_vel_angles, &H_vel_pos, &H_vel_vel, &H_vel_bias); - Matrix F_bias_a = collect(5, &Z_3x3, &Z_3x3, &Z_3x3, &I_3x3, &Z_3x3); - Matrix F_bias_g = collect(5, &Z_3x3, &Z_3x3, &Z_3x3, &Z_3x3, &I_3x3); - Matrix F = stack(5, &F_angles, &F_pos, &F_vel, &F_bias_a, &F_bias_g); + Matrix F_angles = collect(4, &H_angles_angles, &H_angles_pos, &H_angles_vel, &H_angles_bias); + Matrix F_pos = collect(4, &H_pos_angles, &H_pos_pos, &H_pos_vel, &H_pos_bias); + Matrix F_vel = collect(4, &H_vel_angles, &H_vel_pos, &H_vel_vel, &H_vel_bias); + Matrix F_bias_a = collect(5, &Z_3x3, &Z_3x3, &Z_3x3, &I_3x3, &Z_3x3); + Matrix F_bias_g = collect(5, &Z_3x3, &Z_3x3, &Z_3x3, &Z_3x3, &I_3x3); + Matrix F = stack(5, &F_angles, &F_pos, &F_vel, &F_bias_a, &F_bias_g); - noiseModel::Gaussian::shared_ptr model_discrete_curr = calc_descrete_noise_model(model_continuous_overall, msr_dt ); - Matrix Q_d = inverse(model_discrete_curr->R().transpose() * model_discrete_curr->R() ); - EquivCov_Overall = F * EquivCov_Overall * F.transpose() + Q_d; + noiseModel::Gaussian::shared_ptr model_discrete_curr = calc_descrete_noise_model(model_continuous_overall, msr_dt ); + Matrix Q_d = inverse(model_discrete_curr->R().transpose() * model_discrete_curr->R() ); - // Update Jacobian_wrt_t0_Overall - Jacobian_wrt_t0_Overall = F * Jacobian_wrt_t0_Overall; - } + EquivCov_Overall = F * EquivCov_Overall * F.transpose() + Q_d; + // Luca: force identity covariance matrix (for testing purposes) + // EquivCov_Overall = Matrix::Identity(15,15); - static inline Vector PreIntegrateIMUObservations_delta_pos(const double msr_dt, - const Vector& delta_pos_in_t0, const Vector& delta_vel_in_t0){ + // Update Jacobian_wrt_t0_Overall + Jacobian_wrt_t0_Overall = F * Jacobian_wrt_t0_Overall; + } - // Note: all delta terms refer to an IMU\sensor system at t0 - // Note: delta_vel_in_t0 is already in body frame, so no need to use the body_P_sensor transformation here. + static inline Vector PreIntegrateIMUObservations_delta_pos(const double msr_dt, + const Vector& delta_pos_in_t0, const Vector& delta_vel_in_t0){ - return delta_pos_in_t0 + delta_vel_in_t0 * msr_dt; - } + // Note: all delta terms refer to an IMU\sensor system at t0 + // Note: delta_vel_in_t0 is already in body frame, so no need to use the body_P_sensor transformation here. + + return delta_pos_in_t0 + delta_vel_in_t0 * msr_dt; + } - static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt, - const Vector3& delta_angles, const Vector& delta_vel_in_t0, const bool flag_use_body_P_sensor, const POSE& body_P_sensor, - IMUBIAS Bias_t0 = IMUBIAS()){ + static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt, + const Vector3& delta_angles, const Vector& delta_vel_in_t0, const bool flag_use_body_P_sensor, const POSE& body_P_sensor, + IMUBIAS Bias_t0 = IMUBIAS()){ - // Note: all delta terms refer to an IMU\sensor system at t0 + // Note: all delta terms refer to an IMU\sensor system at t0 - // Calculate the corrected measurements using the Bias object - Vector AccCorrected = Bias_t0.correctAccelerometer(msr_acc_t); - Vector body_t_a_body; - if (flag_use_body_P_sensor){ - Matrix body_R_sensor = body_P_sensor.rotation().matrix(); + // Calculate the corrected measurements using the Bias object + Vector AccCorrected = Bias_t0.correctAccelerometer(msr_acc_t); + Vector body_t_a_body; + if (flag_use_body_P_sensor){ + Matrix body_R_sensor = body_P_sensor.rotation().matrix(); - Vector GyroCorrected(Bias_t0.correctGyroscope(msr_gyro_t)); + Vector GyroCorrected(Bias_t0.correctGyroscope(msr_gyro_t)); - Vector body_omega_body = body_R_sensor * GyroCorrected; - Matrix body_omega_body__cross = skewSymmetric(body_omega_body); + Vector body_omega_body = body_R_sensor * GyroCorrected; + Matrix body_omega_body__cross = skewSymmetric(body_omega_body); - body_t_a_body = body_R_sensor * AccCorrected - body_omega_body__cross * body_omega_body__cross * body_P_sensor.translation().vector(); - } else{ - body_t_a_body = AccCorrected; - } + body_t_a_body = body_R_sensor * AccCorrected - body_omega_body__cross * body_omega_body__cross * body_P_sensor.translation().vector(); + } else{ + body_t_a_body = AccCorrected; + } - Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); + Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); - return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt; - } + return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt; + } - static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt, - const Vector3& delta_angles, const bool flag_use_body_P_sensor, const POSE& body_P_sensor, - IMUBIAS Bias_t0 = IMUBIAS()){ + static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt, + const Vector3& delta_angles, const bool flag_use_body_P_sensor, const POSE& body_P_sensor, + IMUBIAS Bias_t0 = IMUBIAS()){ - // Note: all delta terms refer to an IMU\sensor system at t0 + // Note: all delta terms refer to an IMU\sensor system at t0 - // Calculate the corrected measurements using the Bias object - Vector GyroCorrected = Bias_t0.correctGyroscope(msr_gyro_t); + // Calculate the corrected measurements using the Bias object + Vector GyroCorrected = Bias_t0.correctGyroscope(msr_gyro_t); - Vector body_t_omega_body; - if (flag_use_body_P_sensor){ - body_t_omega_body = body_P_sensor.rotation().matrix() * GyroCorrected; - } else { - body_t_omega_body = GyroCorrected; - } + Vector body_t_omega_body; + if (flag_use_body_P_sensor){ + body_t_omega_body = body_P_sensor.rotation().matrix() * GyroCorrected; + } else { + body_t_omega_body = GyroCorrected; + } - Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); + Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); - R_t_to_t0 = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt ); - return Rot3::Logmap(R_t_to_t0); - } + R_t_to_t0 = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt ); + return Rot3::Logmap(R_t_to_t0); + } - static inline noiseModel::Gaussian::shared_ptr CalcEquivalentNoiseCov(const noiseModel::Gaussian::shared_ptr& gaussian_acc, const noiseModel::Gaussian::shared_ptr& gaussian_gyro, - const noiseModel::Gaussian::shared_ptr& gaussian_process){ + static inline noiseModel::Gaussian::shared_ptr CalcEquivalentNoiseCov(const noiseModel::Gaussian::shared_ptr& gaussian_acc, const noiseModel::Gaussian::shared_ptr& gaussian_gyro, + const noiseModel::Gaussian::shared_ptr& gaussian_process){ - Matrix cov_acc = inverse( gaussian_acc->R().transpose() * gaussian_acc->R() ); - Matrix cov_gyro = inverse( gaussian_gyro->R().transpose() * gaussian_gyro->R() ); - Matrix cov_process = inverse( gaussian_process->R().transpose() * gaussian_process->R() ); + Matrix cov_acc = inverse( gaussian_acc->R().transpose() * gaussian_acc->R() ); + Matrix cov_gyro = inverse( gaussian_gyro->R().transpose() * gaussian_gyro->R() ); + Matrix cov_process = inverse( gaussian_process->R().transpose() * gaussian_process->R() ); - cov_process.block(0,0, 3,3) += cov_gyro; - cov_process.block(6,6, 3,3) += cov_acc; + cov_process.block(0,0, 3,3) += cov_gyro; + cov_process.block(6,6, 3,3) += cov_acc; - return noiseModel::Gaussian::Covariance(cov_process); - } + return noiseModel::Gaussian::Covariance(cov_process); + } static inline void CalcEquivalentNoiseCov_DifferentParts(const noiseModel::Gaussian::shared_ptr& gaussian_acc, const noiseModel::Gaussian::shared_ptr& gaussian_gyro, const noiseModel::Gaussian::shared_ptr& gaussian_process, @@ -547,107 +554,107 @@ public: cov_process_without_acc_gyro = inverse( gaussian_process->R().transpose() * gaussian_process->R() ); } - static inline void Calc_g_rho_omega_earth_NED(const Vector& Pos_NED, const Vector& Vel_NED, const Vector& LatLonHeight_IC, const Vector& Pos_NED_Initial, - Vector& g_NED, Vector& rho_NED, Vector& omega_earth_NED) { + static inline void Calc_g_rho_omega_earth_NED(const Vector& Pos_NED, const Vector& Vel_NED, const Vector& LatLonHeight_IC, const Vector& Pos_NED_Initial, + Vector& g_NED, Vector& rho_NED, Vector& omega_earth_NED) { - Matrix ENU_to_NED = Matrix_(3, 3, - 0.0, 1.0, 0.0, - 1.0, 0.0, 0.0, - 0.0, 0.0, -1.0); + Matrix ENU_to_NED = (Matrix(3, 3) << + 0.0, 1.0, 0.0, + 1.0, 0.0, 0.0, + 0.0, 0.0, -1.0); - Matrix NED_to_ENU = Matrix_(3, 3, - 0.0, 1.0, 0.0, - 1.0, 0.0, 0.0, - 0.0, 0.0, -1.0); + Matrix NED_to_ENU = (Matrix(3, 3) << + 0.0, 1.0, 0.0, + 1.0, 0.0, 0.0, + 0.0, 0.0, -1.0); - // Convert incoming parameters to ENU - Vector Pos_ENU = NED_to_ENU * Pos_NED; - Vector Vel_ENU = NED_to_ENU * Vel_NED; - Vector Pos_ENU_Initial = NED_to_ENU * Pos_NED_Initial; + // Convert incoming parameters to ENU + Vector Pos_ENU = NED_to_ENU * Pos_NED; + Vector Vel_ENU = NED_to_ENU * Vel_NED; + Vector Pos_ENU_Initial = NED_to_ENU * Pos_NED_Initial; - // Call ENU version - Vector g_ENU; - Vector rho_ENU; - Vector omega_earth_ENU; - Calc_g_rho_omega_earth_ENU(Pos_ENU, Vel_ENU, LatLonHeight_IC, Pos_ENU_Initial, g_ENU, rho_ENU, omega_earth_ENU); + // Call ENU version + Vector g_ENU; + Vector rho_ENU; + Vector omega_earth_ENU; + Calc_g_rho_omega_earth_ENU(Pos_ENU, Vel_ENU, LatLonHeight_IC, Pos_ENU_Initial, g_ENU, rho_ENU, omega_earth_ENU); - // Convert output to NED - g_NED = ENU_to_NED * g_ENU; - rho_NED = ENU_to_NED * rho_ENU; - omega_earth_NED = ENU_to_NED * omega_earth_ENU; - } + // Convert output to NED + g_NED = ENU_to_NED * g_ENU; + rho_NED = ENU_to_NED * rho_ENU; + omega_earth_NED = ENU_to_NED * omega_earth_ENU; + } - static inline void Calc_g_rho_omega_earth_ENU(const Vector& Pos_ENU, const Vector& Vel_ENU, const Vector& LatLonHeight_IC, const Vector& Pos_ENU_Initial, - Vector& g_ENU, Vector& rho_ENU, Vector& omega_earth_ENU){ - double R0 = 6.378388e6; - double e = 1/297; - double Re( R0*( 1-e*(sin( LatLonHeight_IC(0) ))*(sin( LatLonHeight_IC(0) )) ) ); + static inline void Calc_g_rho_omega_earth_ENU(const Vector& Pos_ENU, const Vector& Vel_ENU, const Vector& LatLonHeight_IC, const Vector& Pos_ENU_Initial, + Vector& g_ENU, Vector& rho_ENU, Vector& omega_earth_ENU){ + double R0 = 6.378388e6; + double e = 1/297; + double Re( R0*( 1-e*(sin( LatLonHeight_IC(0) ))*(sin( LatLonHeight_IC(0) )) ) ); - // Calculate current lat, lon - Vector delta_Pos_ENU(Pos_ENU - Pos_ENU_Initial); - double delta_lat(delta_Pos_ENU(1)/Re); - double delta_lon(delta_Pos_ENU(0)/(Re*cos(LatLonHeight_IC(0)))); - double lat_new(LatLonHeight_IC(0) + delta_lat); - double lon_new(LatLonHeight_IC(1) + delta_lon); + // Calculate current lat, lon + Vector delta_Pos_ENU(Pos_ENU - Pos_ENU_Initial); + double delta_lat(delta_Pos_ENU(1)/Re); + double delta_lon(delta_Pos_ENU(0)/(Re*cos(LatLonHeight_IC(0)))); + double lat_new(LatLonHeight_IC(0) + delta_lat); + double lon_new(LatLonHeight_IC(1) + delta_lon); - // Rotation of lon about z axis - Rot3 C1(cos(lon_new), sin(lon_new), 0.0, - -sin(lon_new), cos(lon_new), 0.0, - 0.0, 0.0, 1.0); + // Rotation of lon about z axis + Rot3 C1(cos(lon_new), sin(lon_new), 0.0, + -sin(lon_new), cos(lon_new), 0.0, + 0.0, 0.0, 1.0); - // Rotation of lat about y axis - Rot3 C2(cos(lat_new), 0.0, sin(lat_new), - 0.0, 1.0, 0.0, - -sin(lat_new), 0.0, cos(lat_new)); + // Rotation of lat about y axis + Rot3 C2(cos(lat_new), 0.0, sin(lat_new), + 0.0, 1.0, 0.0, + -sin(lat_new), 0.0, cos(lat_new)); - Rot3 UEN_to_ENU(0, 1, 0, - 0, 0, 1, - 1, 0, 0); + Rot3 UEN_to_ENU(0, 1, 0, + 0, 0, 1, + 1, 0, 0); - Rot3 R_ECEF_to_ENU( UEN_to_ENU * C2 * C1 ); + Rot3 R_ECEF_to_ENU( UEN_to_ENU * C2 * C1 ); - Vector omega_earth_ECEF(Vector_(3, 0.0, 0.0, 7.292115e-5)); - omega_earth_ENU = R_ECEF_to_ENU.matrix() * omega_earth_ECEF; + Vector omega_earth_ECEF((Vector(3) << 0.0, 0.0, 7.292115e-5)); + omega_earth_ENU = R_ECEF_to_ENU.matrix() * omega_earth_ECEF; - // Calculating g - double height(LatLonHeight_IC(2)); - double EQUA_RADIUS = 6378137.0; // equatorial radius of the earth; WGS-84 - double ECCENTRICITY = 0.0818191908426; // eccentricity of the earth ellipsoid - double e2( pow(ECCENTRICITY,2) ); - double den( 1-e2*pow(sin(lat_new),2) ); - double Rm( (EQUA_RADIUS*(1-e2))/( pow(den,(3/2)) ) ); - double Rp( EQUA_RADIUS/( sqrt(den) ) ); - double Ro( sqrt(Rp*Rm) ); // mean earth radius of curvature - double g0( 9.780318*( 1 + 5.3024e-3 * pow(sin(lat_new),2) - 5.9e-6 * pow(sin(2*lat_new),2) ) ); - double g_calc( g0/( pow(1 + height/Ro, 2) ) ); - g_ENU = Vector_(3, 0.0, 0.0, -g_calc); + // Calculating g + double height(LatLonHeight_IC(2)); + double EQUA_RADIUS = 6378137.0; // equatorial radius of the earth; WGS-84 + double ECCENTRICITY = 0.0818191908426; // eccentricity of the earth ellipsoid + double e2( pow(ECCENTRICITY,2) ); + double den( 1-e2*pow(sin(lat_new),2) ); + double Rm( (EQUA_RADIUS*(1-e2))/( pow(den,(3/2)) ) ); + double Rp( EQUA_RADIUS/( sqrt(den) ) ); + double Ro( sqrt(Rp*Rm) ); // mean earth radius of curvature + double g0( 9.780318*( 1 + 5.3024e-3 * pow(sin(lat_new),2) - 5.9e-6 * pow(sin(2*lat_new),2) ) ); + double g_calc( g0/( pow(1 + height/Ro, 2) ) ); + g_ENU = (Vector(3) << 0.0, 0.0, -g_calc); - // Calculate rho - double Ve( Vel_ENU(0) ); - double Vn( Vel_ENU(1) ); - double rho_E = -Vn/(Rm + height); - double rho_N = Ve/(Rp + height); - double rho_U = Ve*tan(lat_new)/(Rp + height); - rho_ENU = Vector_(3, rho_E, rho_N, rho_U); - } + // Calculate rho + double Ve( Vel_ENU(0) ); + double Vn( Vel_ENU(1) ); + double rho_E = -Vn/(Rm + height); + double rho_N = Ve/(Rp + height); + double rho_U = Ve*tan(lat_new)/(Rp + height); + rho_ENU = (Vector(3) << rho_E, rho_N, rho_U); + } - static inline noiseModel::Gaussian::shared_ptr calc_descrete_noise_model(const noiseModel::Gaussian::shared_ptr& model, double delta_t){ - /* Q_d (approx)= Q * delta_t */ - /* In practice, square root of the information matrix is represented, so that: - * R_d (approx)= R / sqrt(delta_t) - * */ - return noiseModel::Gaussian::SqrtInformation(model->R()/sqrt(delta_t)); - } + static inline noiseModel::Gaussian::shared_ptr calc_descrete_noise_model(const noiseModel::Gaussian::shared_ptr& model, double delta_t){ + /* Q_d (approx)= Q * delta_t */ + /* In practice, square root of the information matrix is represented, so that: + * R_d (approx)= R / sqrt(delta_t) + * */ + return noiseModel::Gaussian::SqrtInformation(model->R()/sqrt(delta_t)); + } private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NonlinearFactor2", - boost::serialization::base_object(*this)); - } + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NonlinearFactor2", + boost::serialization::base_object(*this)); + } diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h index 2858b091e..dec1b2378 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h @@ -39,8 +39,8 @@ namespace gtsam { * ===== * Concept: Based on [Lupton12tro] * - Pre-integrate IMU measurements using the static function PreIntegrateIMUObservations. - * Pre-integrated quantities are expressed in the body system of t0 - the first time instant (in which pre-integration began). - * All sensor-to-body transformations are performed here. + * Pre-integrated quantities are expressed in the body system of t0 - the first time instant (in which pre-integration began). + * All sensor-to-body transformations are performed here. * - If required, calculate inertial solution by calling the static functions: predictPose_inertial, predictVelocity_inertial. * - When the time is right, incorporate pre-integrated IMU data by creating an EquivInertialNavFactor_GlobalVel_NoBias factor, which will * relate between navigation variables at the two time instances (t0 and current time). @@ -54,11 +54,11 @@ namespace gtsam { * matrices and the process\modeling covariance matrix. The IneritalNavFactor converts this into a * discrete form using the supplied delta_t between sub-sequential measurements. * - Earth-rate correction: - * + Currently the user should supply R_ECEF_to_G, which is the rotation from ECEF to the global - * frame (Local-Level system: ENU or NED, see above). - * + R_ECEF_to_G can be calculated by approximated values of latitude and longitude of the system. - * + Currently it is assumed that a relatively small distance is traveled w.r.t. to initial pose, since R_ECEF_to_G is constant. - * Otherwise, R_ECEF_to_G should be updated each time using the current lat-lon. + * + Currently the user should supply R_ECEF_to_G, which is the rotation from ECEF to the global + * frame (Local-Level system: ENU or NED, see above). + * + R_ECEF_to_G can be calculated by approximated values of latitude and longitude of the system. + * + Currently it is assumed that a relatively small distance is traveled w.r.t. to initial pose, since R_ECEF_to_G is constant. + * Otherwise, R_ECEF_to_G should be updated each time using the current lat-lon. * * - Frame Notation: * Quantities are written as {Frame of Representation/Destination Frame}_{Quantity Type}_{Quatity Description/Origination Frame} @@ -92,222 +92,222 @@ class EquivInertialNavFactor_GlobalVel_NoBias : public NoiseModelFactor4 This; - typedef NoiseModelFactor4 Base; + typedef EquivInertialNavFactor_GlobalVel_NoBias This; + typedef NoiseModelFactor4 Base; - Vector delta_pos_in_t0_; - Vector delta_vel_in_t0_; - Vector3 delta_angles_; - double dt12_; + Vector delta_pos_in_t0_; + Vector delta_vel_in_t0_; + Vector3 delta_angles_; + double dt12_; - Vector world_g_; - Vector world_rho_; - Vector world_omega_earth_; + Vector world_g_; + Vector world_rho_; + Vector world_omega_earth_; - Matrix Jacobian_wrt_t0_Overall_; + Matrix Jacobian_wrt_t0_Overall_; - boost::optional body_P_sensor_; // The pose of the sensor in the body frame + boost::optional body_P_sensor_; // The pose of the sensor in the body frame public: - // shorthand for a smart pointer to a factor - typedef typename boost::shared_ptr shared_ptr; + // shorthand for a smart pointer to a factor + typedef typename boost::shared_ptr shared_ptr; - /** default constructor - only use for serialization */ - EquivInertialNavFactor_GlobalVel_NoBias() {} + /** default constructor - only use for serialization */ + EquivInertialNavFactor_GlobalVel_NoBias() {} - /** Constructor */ - EquivInertialNavFactor_GlobalVel_NoBias(const Key& Pose1, const Key& Vel1, const Key& Pose2, const Key& Vel2, - const Vector& delta_pos_in_t0, const Vector& delta_vel_in_t0, const Vector3& delta_angles, - double dt12, const Vector world_g, const Vector world_rho, - const Vector& world_omega_earth, const noiseModel::Gaussian::shared_ptr& model_equivalent, - const Matrix& Jacobian_wrt_t0_Overall, - boost::optional body_P_sensor = boost::none) : - Base(model_equivalent, Pose1, Vel1, Pose2, Vel2), - delta_pos_in_t0_(delta_pos_in_t0), delta_vel_in_t0_(delta_vel_in_t0), delta_angles_(delta_angles), - dt12_(dt12), world_g_(world_g), world_rho_(world_rho), world_omega_earth_(world_omega_earth), Jacobian_wrt_t0_Overall_(Jacobian_wrt_t0_Overall), - body_P_sensor_(body_P_sensor) { } + /** Constructor */ + EquivInertialNavFactor_GlobalVel_NoBias(const Key& Pose1, const Key& Vel1, const Key& Pose2, const Key& Vel2, + const Vector& delta_pos_in_t0, const Vector& delta_vel_in_t0, const Vector3& delta_angles, + double dt12, const Vector world_g, const Vector world_rho, + const Vector& world_omega_earth, const noiseModel::Gaussian::shared_ptr& model_equivalent, + const Matrix& Jacobian_wrt_t0_Overall, + boost::optional body_P_sensor = boost::none) : + Base(model_equivalent, Pose1, Vel1, Pose2, Vel2), + delta_pos_in_t0_(delta_pos_in_t0), delta_vel_in_t0_(delta_vel_in_t0), delta_angles_(delta_angles), + dt12_(dt12), world_g_(world_g), world_rho_(world_rho), world_omega_earth_(world_omega_earth), Jacobian_wrt_t0_Overall_(Jacobian_wrt_t0_Overall), + body_P_sensor_(body_P_sensor) { } - virtual ~EquivInertialNavFactor_GlobalVel_NoBias() {} + virtual ~EquivInertialNavFactor_GlobalVel_NoBias() {} - /** implement functions needed for Testable */ + /** implement functions needed for Testable */ - /** print */ - virtual void print(const std::string& s = "EquivInertialNavFactor_GlobalVel_NoBias", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - std::cout << s << "(" - << keyFormatter(this->key1()) << "," - << keyFormatter(this->key2()) << "," - << keyFormatter(this->key3()) << "," - << keyFormatter(this->key4()) << "\n"; - std::cout << "delta_pos_in_t0: " << this->delta_pos_in_t0_.transpose() << std::endl; - std::cout << "delta_vel_in_t0: " << this->delta_vel_in_t0_.transpose() << std::endl; - std::cout << "delta_angles: " << this->delta_angles_ << std::endl; - std::cout << "dt12: " << this->dt12_ << std::endl; - std::cout << "gravity (in world frame): " << this->world_g_.transpose() << std::endl; - std::cout << "craft rate (in world frame): " << this->world_rho_.transpose() << std::endl; - std::cout << "earth's rotation (in world frame): " << this->world_omega_earth_.transpose() << std::endl; - if(this->body_P_sensor_) - this->body_P_sensor_->print(" sensor pose in body frame: "); - this->noiseModel_->print(" noise model"); - } + /** print */ + virtual void print(const std::string& s = "EquivInertialNavFactor_GlobalVel_NoBias", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + std::cout << s << "(" + << keyFormatter(this->key1()) << "," + << keyFormatter(this->key2()) << "," + << keyFormatter(this->key3()) << "," + << keyFormatter(this->key4()) << "\n"; + std::cout << "delta_pos_in_t0: " << this->delta_pos_in_t0_.transpose() << std::endl; + std::cout << "delta_vel_in_t0: " << this->delta_vel_in_t0_.transpose() << std::endl; + std::cout << "delta_angles: " << this->delta_angles_ << std::endl; + std::cout << "dt12: " << this->dt12_ << std::endl; + std::cout << "gravity (in world frame): " << this->world_g_.transpose() << std::endl; + std::cout << "craft rate (in world frame): " << this->world_rho_.transpose() << std::endl; + std::cout << "earth's rotation (in world frame): " << this->world_omega_earth_.transpose() << std::endl; + if(this->body_P_sensor_) + this->body_P_sensor_->print(" sensor pose in body frame: "); + this->noiseModel_->print(" noise model"); + } - /** equals */ - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { - const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) - && (delta_pos_in_t0_ - e->delta_pos_in_t0_).norm() < tol - && (delta_vel_in_t0_ - e->delta_vel_in_t0_).norm() < tol - && (delta_angles_ - e->delta_angles_).norm() < tol - && (dt12_ - e->dt12_) < tol - && (world_g_ - e->world_g_).norm() < tol - && (world_rho_ - e->world_rho_).norm() < tol - && (world_omega_earth_ - e->world_omega_earth_).norm() < tol - && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); - } + /** equals */ + virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { + const This *e = dynamic_cast (&expected); + return e != NULL && Base::equals(*e, tol) + && (delta_pos_in_t0_ - e->delta_pos_in_t0_).norm() < tol + && (delta_vel_in_t0_ - e->delta_vel_in_t0_).norm() < tol + && (delta_angles_ - e->delta_angles_).norm() < tol + && (dt12_ - e->dt12_) < tol + && (world_g_ - e->world_g_).norm() < tol + && (world_rho_ - e->world_rho_).norm() < tol + && (world_omega_earth_ - e->world_omega_earth_).norm() < tol + && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); + } - POSE predictPose(const POSE& Pose1, const VELOCITY& Vel1) const { + POSE predictPose(const POSE& Pose1, const VELOCITY& Vel1) const { - /* Position term */ - Vector delta_pos_in_t0_corrected = delta_pos_in_t0_; + /* Position term */ + Vector delta_pos_in_t0_corrected = delta_pos_in_t0_; - /* Rotation term */ - Vector delta_angles_corrected = delta_angles_; + /* Rotation term */ + Vector delta_angles_corrected = delta_angles_; - return predictPose_inertial(Pose1, Vel1, - delta_pos_in_t0_corrected, delta_angles_corrected, - dt12_, world_g_, world_rho_, world_omega_earth_); - } + return predictPose_inertial(Pose1, Vel1, + delta_pos_in_t0_corrected, delta_angles_corrected, + dt12_, world_g_, world_rho_, world_omega_earth_); + } - static inline POSE predictPose_inertial(const POSE& Pose1, const VELOCITY& Vel1, - const Vector& delta_pos_in_t0, const Vector3& delta_angles, - const double dt12, const Vector& world_g, const Vector& world_rho, const Vector& world_omega_earth){ + static inline POSE predictPose_inertial(const POSE& Pose1, const VELOCITY& Vel1, + const Vector& delta_pos_in_t0, const Vector3& delta_angles, + const double dt12, const Vector& world_g, const Vector& world_rho, const Vector& world_omega_earth){ - const POSE& world_P1_body = Pose1; - const VELOCITY& world_V1_body = Vel1; + const POSE& world_P1_body = Pose1; + const VELOCITY& world_V1_body = Vel1; - /* Position term */ - Vector body_deltaPos_body = delta_pos_in_t0; + /* Position term */ + Vector body_deltaPos_body = delta_pos_in_t0; - Vector world_deltaPos_pls_body = world_P1_body.rotation().matrix() * body_deltaPos_body; - Vector world_deltaPos_body = world_V1_body * dt12 + 0.5*world_g*dt12*dt12 + world_deltaPos_pls_body; + Vector world_deltaPos_pls_body = world_P1_body.rotation().matrix() * body_deltaPos_body; + Vector world_deltaPos_body = world_V1_body * dt12 + 0.5*world_g*dt12*dt12 + world_deltaPos_pls_body; - // Incorporate earth-related terms. Note - these are assumed to be constant between t1 and t2. - world_deltaPos_body -= 2*skewSymmetric(world_rho + world_omega_earth)*world_V1_body * dt12*dt12; + // Incorporate earth-related terms. Note - these are assumed to be constant between t1 and t2. + world_deltaPos_body -= 2*skewSymmetric(world_rho + world_omega_earth)*world_V1_body * dt12*dt12; - /* TODO: the term dt12*dt12 in 0.5*world_g*dt12*dt12 is not entirely correct: - * the gravity should be canceled from the accelerometer measurements, bust since position - * is added with a delta velocity from a previous term, the actual delta time is more complicated. - * Need to figure out this in the future - currently because of this issue we'll get some more error - * in Z axis. - */ + /* TODO: the term dt12*dt12 in 0.5*world_g*dt12*dt12 is not entirely correct: + * the gravity should be canceled from the accelerometer measurements, bust since position + * is added with a delta velocity from a previous term, the actual delta time is more complicated. + * Need to figure out this in the future - currently because of this issue we'll get some more error + * in Z axis. + */ - /* Rotation term */ - Vector body_deltaAngles_body = delta_angles; + /* Rotation term */ + Vector body_deltaAngles_body = delta_angles; - // Convert earth-related terms into the body frame - Matrix body_R_world(world_P1_body.rotation().inverse().matrix()); - Vector body_rho = body_R_world * world_rho; - Vector body_omega_earth = body_R_world * world_omega_earth; + // Convert earth-related terms into the body frame + Matrix body_R_world(world_P1_body.rotation().inverse().matrix()); + Vector body_rho = body_R_world * world_rho; + Vector body_omega_earth = body_R_world * world_omega_earth; - // Incorporate earth-related terms. Note - these are assumed to be constant between t1 and t2. - body_deltaAngles_body -= (body_rho + body_omega_earth)*dt12; + // Incorporate earth-related terms. Note - these are assumed to be constant between t1 and t2. + body_deltaAngles_body -= (body_rho + body_omega_earth)*dt12; - return POSE(Pose1.rotation() * POSE::Rotation::Expmap(body_deltaAngles_body), Pose1.translation() + typename POSE::Translation(world_deltaPos_body)); + return POSE(Pose1.rotation() * POSE::Rotation::Expmap(body_deltaAngles_body), Pose1.translation() + typename POSE::Translation(world_deltaPos_body)); - } + } - VELOCITY predictVelocity(const POSE& Pose1, const VELOCITY& Vel1) const { + VELOCITY predictVelocity(const POSE& Pose1, const VELOCITY& Vel1) const { - Vector delta_vel_in_t0_corrected = delta_vel_in_t0_; + Vector delta_vel_in_t0_corrected = delta_vel_in_t0_; - return predictVelocity_inertial(Pose1, Vel1, - delta_vel_in_t0_corrected, - dt12_, world_g_, world_rho_, world_omega_earth_); - } + return predictVelocity_inertial(Pose1, Vel1, + delta_vel_in_t0_corrected, + dt12_, world_g_, world_rho_, world_omega_earth_); + } - static inline VELOCITY predictVelocity_inertial(const POSE& Pose1, const VELOCITY& Vel1, - const Vector& delta_vel_in_t0, - const double dt12, const Vector& world_g, const Vector& world_rho, const Vector& world_omega_earth) { + static inline VELOCITY predictVelocity_inertial(const POSE& Pose1, const VELOCITY& Vel1, + const Vector& delta_vel_in_t0, + const double dt12, const Vector& world_g, const Vector& world_rho, const Vector& world_omega_earth) { - const POSE& world_P1_body = Pose1; - const VELOCITY& world_V1_body = Vel1; + const POSE& world_P1_body = Pose1; + const VELOCITY& world_V1_body = Vel1; - Vector body_deltaVel_body = delta_vel_in_t0; - Vector world_deltaVel_body = world_P1_body.rotation().matrix() * body_deltaVel_body; + Vector body_deltaVel_body = delta_vel_in_t0; + Vector world_deltaVel_body = world_P1_body.rotation().matrix() * body_deltaVel_body; - VELOCITY VelDelta( world_deltaVel_body + world_g * dt12 ); + VELOCITY VelDelta( world_deltaVel_body + world_g * dt12 ); - // Incorporate earth-related terms. Note - these are assumed to be constant between t1 and t2. - VelDelta -= 2*skewSymmetric(world_rho + world_omega_earth)*world_V1_body * dt12; + // Incorporate earth-related terms. Note - these are assumed to be constant between t1 and t2. + VelDelta -= 2*skewSymmetric(world_rho + world_omega_earth)*world_V1_body * dt12; - // Predict - return Vel1.compose( VelDelta ); + // Predict + return Vel1.compose( VelDelta ); - } + } - void predict(const POSE& Pose1, const VELOCITY& Vel1, POSE& Pose2, VELOCITY& Vel2) const { - Pose2 = predictPose(Pose1, Vel1); - Vel2 = predictVelocity(Pose1, Vel1); - } + void predict(const POSE& Pose1, const VELOCITY& Vel1, POSE& Pose2, VELOCITY& Vel2) const { + Pose2 = predictPose(Pose1, Vel1); + Vel2 = predictVelocity(Pose1, Vel1); + } - POSE evaluatePoseError(const POSE& Pose1, const VELOCITY& Vel1, const POSE& Pose2, const VELOCITY& Vel2) const { - // Predict - POSE Pose2Pred = predictPose(Pose1, Vel1); + POSE evaluatePoseError(const POSE& Pose1, const VELOCITY& Vel1, const POSE& Pose2, const VELOCITY& Vel2) const { + // Predict + POSE Pose2Pred = predictPose(Pose1, Vel1); - // Calculate error - return Pose2.between(Pose2Pred); - } + // Calculate error + return Pose2.between(Pose2Pred); + } - VELOCITY evaluateVelocityError(const POSE& Pose1, const VELOCITY& Vel1, const POSE& Pose2, const VELOCITY& Vel2) const { - // Predict - VELOCITY Vel2Pred = predictVelocity(Pose1, Vel1); + VELOCITY evaluateVelocityError(const POSE& Pose1, const VELOCITY& Vel1, const POSE& Pose2, const VELOCITY& Vel2) const { + // Predict + VELOCITY Vel2Pred = predictVelocity(Pose1, Vel1); - // Calculate error - return Vel2.between(Vel2Pred); - } + // Calculate error + return Vel2.between(Vel2Pred); + } - Vector evaluateError(const POSE& Pose1, const VELOCITY& Vel1, const POSE& Pose2, const VELOCITY& Vel2, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none, - boost::optional H3 = boost::none, - boost::optional H4 = boost::none) const { + Vector evaluateError(const POSE& Pose1, const VELOCITY& Vel1, const POSE& Pose2, const VELOCITY& Vel2, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none, + boost::optional H3 = boost::none, + boost::optional H4 = boost::none) const { - // TODO: Write analytical derivative calculations - // Jacobian w.r.t. Pose1 - if (H1){ - Matrix H1_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, _1, Vel1, Pose2, Vel2), Pose1); - Matrix H1_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, _1, Vel1, Pose2, Vel2), Pose1); - *H1 = stack(2, &H1_Pose, &H1_Vel); - } + // TODO: Write analytical derivative calculations + // Jacobian w.r.t. Pose1 + if (H1){ + Matrix H1_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, _1, Vel1, Pose2, Vel2), Pose1); + Matrix H1_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, _1, Vel1, Pose2, Vel2), Pose1); + *H1 = stack(2, &H1_Pose, &H1_Vel); + } - // Jacobian w.r.t. Vel1 - if (H2){ - Matrix H2_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, Pose1, _1, Pose2, Vel2), Vel1); - Matrix H2_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, Pose1, _1, Pose2, Vel2), Vel1); - *H2 = stack(2, &H2_Pose, &H2_Vel); - } + // Jacobian w.r.t. Vel1 + if (H2){ + Matrix H2_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, Pose1, _1, Pose2, Vel2), Vel1); + Matrix H2_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, Pose1, _1, Pose2, Vel2), Vel1); + *H2 = stack(2, &H2_Pose, &H2_Vel); + } - // Jacobian w.r.t. Pose2 - if (H3){ - Matrix H3_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, Pose1, Vel1, _1, Vel2), Pose2); - Matrix H3_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, Pose1, Vel1, _1, Vel2), Pose2); - *H3 = stack(2, &H3_Pose, &H3_Vel); - } + // Jacobian w.r.t. Pose2 + if (H3){ + Matrix H3_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, Pose1, Vel1, _1, Vel2), Pose2); + Matrix H3_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, Pose1, Vel1, _1, Vel2), Pose2); + *H3 = stack(2, &H3_Pose, &H3_Vel); + } - // Jacobian w.r.t. Vel2 - if (H4){ - Matrix H4_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, Pose1, Vel1, Pose2, _1), Vel2); - Matrix H4_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, Pose1, Vel1, Pose2, _1), Vel2); - *H4 = stack(2, &H4_Pose, &H4_Vel); - } + // Jacobian w.r.t. Vel2 + if (H4){ + Matrix H4_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, Pose1, Vel1, Pose2, _1), Vel2); + Matrix H4_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, Pose1, Vel1, Pose2, _1), Vel2); + *H4 = stack(2, &H4_Pose, &H4_Vel); + } - Vector ErrPoseVector(POSE::Logmap(evaluatePoseError(Pose1, Vel1, Pose2, Vel2))); - Vector ErrVelVector(VELOCITY::Logmap(evaluateVelocityError(Pose1, Vel1, Pose2, Vel2))); + Vector ErrPoseVector(POSE::Logmap(evaluatePoseError(Pose1, Vel1, Pose2, Vel2))); + Vector ErrVelVector(VELOCITY::Logmap(evaluateVelocityError(Pose1, Vel1, Pose2, Vel2))); - return concatVectors(2, &ErrPoseVector, &ErrVelVector); - } + return concatVectors(2, &ErrPoseVector, &ErrVelVector); + } @@ -348,126 +348,126 @@ public: static inline void PreIntegrateIMUObservations(const Vector& msr_acc_t, const Vector& msr_gyro_t, const double msr_dt, Vector& delta_pos_in_t0, Vector3& delta_angles, Vector& delta_vel_in_t0, double& delta_t, - const noiseModel::Gaussian::shared_ptr& model_continuous_overall, - Matrix& EquivCov_Overall, Matrix& Jacobian_wrt_t0_Overall, - boost::optional p_body_P_sensor = boost::none){ - // Note: all delta terms refer to an IMU\sensor system at t0 - // Note: Earth-related terms are not accounted here but are incorporated in predict functions. + const noiseModel::Gaussian::shared_ptr& model_continuous_overall, + Matrix& EquivCov_Overall, Matrix& Jacobian_wrt_t0_Overall, + boost::optional p_body_P_sensor = boost::none){ + // Note: all delta terms refer to an IMU\sensor system at t0 + // Note: Earth-related terms are not accounted here but are incorporated in predict functions. - POSE body_P_sensor = POSE(); - bool flag_use_body_P_sensor = false; - if (p_body_P_sensor){ - body_P_sensor = *p_body_P_sensor; - flag_use_body_P_sensor = true; - } + POSE body_P_sensor = POSE(); + bool flag_use_body_P_sensor = false; + if (p_body_P_sensor){ + body_P_sensor = *p_body_P_sensor; + flag_use_body_P_sensor = true; + } - delta_pos_in_t0 = PreIntegrateIMUObservations_delta_pos(msr_dt, delta_pos_in_t0, delta_vel_in_t0); - delta_vel_in_t0 = PreIntegrateIMUObservations_delta_vel(msr_gyro_t, msr_acc_t, msr_dt, delta_angles, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor); - delta_angles = PreIntegrateIMUObservations_delta_angles(msr_gyro_t, msr_dt, delta_angles, flag_use_body_P_sensor, body_P_sensor); + delta_pos_in_t0 = PreIntegrateIMUObservations_delta_pos(msr_dt, delta_pos_in_t0, delta_vel_in_t0); + delta_vel_in_t0 = PreIntegrateIMUObservations_delta_vel(msr_gyro_t, msr_acc_t, msr_dt, delta_angles, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor); + delta_angles = PreIntegrateIMUObservations_delta_angles(msr_gyro_t, msr_dt, delta_angles, flag_use_body_P_sensor, body_P_sensor); - delta_t += msr_dt; + delta_t += msr_dt; - // Update EquivCov_Overall - Matrix Z_3x3 = zeros(3,3); - Matrix I_3x3 = eye(3,3); + // Update EquivCov_Overall + Matrix Z_3x3 = zeros(3,3); + Matrix I_3x3 = eye(3,3); - Matrix H_pos_pos = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, _1, delta_vel_in_t0), delta_pos_in_t0); - Matrix H_pos_vel = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, delta_pos_in_t0, _1), delta_vel_in_t0); - Matrix H_pos_angles = Z_3x3; + Matrix H_pos_pos = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, _1, delta_vel_in_t0), delta_pos_in_t0); + Matrix H_pos_vel = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, delta_pos_in_t0, _1), delta_vel_in_t0); + Matrix H_pos_angles = Z_3x3; - Matrix H_vel_vel = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, _1, flag_use_body_P_sensor, body_P_sensor), delta_vel_in_t0); - Matrix H_vel_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, _1, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor), delta_angles); - Matrix H_vel_pos = Z_3x3; + Matrix H_vel_vel = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, _1, flag_use_body_P_sensor, body_P_sensor), delta_vel_in_t0); + Matrix H_vel_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, _1, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor), delta_angles); + Matrix H_vel_pos = Z_3x3; - Matrix H_angles_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, _1, flag_use_body_P_sensor, body_P_sensor), delta_angles); - Matrix H_angles_pos = Z_3x3; - Matrix H_angles_vel = Z_3x3; + Matrix H_angles_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, _1, flag_use_body_P_sensor, body_P_sensor), delta_angles); + Matrix H_angles_pos = Z_3x3; + Matrix H_angles_vel = Z_3x3; - Matrix F_angles = collect(3, &H_angles_angles, &H_angles_pos, &H_angles_vel); - Matrix F_pos = collect(3, &H_pos_angles, &H_pos_pos, &H_pos_vel); - Matrix F_vel = collect(3, &H_vel_angles, &H_vel_pos, &H_vel_vel); - Matrix F = stack(3, &F_angles, &F_pos, &F_vel); + Matrix F_angles = collect(3, &H_angles_angles, &H_angles_pos, &H_angles_vel); + Matrix F_pos = collect(3, &H_pos_angles, &H_pos_pos, &H_pos_vel); + Matrix F_vel = collect(3, &H_vel_angles, &H_vel_pos, &H_vel_vel); + Matrix F = stack(3, &F_angles, &F_pos, &F_vel); - noiseModel::Gaussian::shared_ptr model_discrete_curr = calc_descrete_noise_model(model_continuous_overall, msr_dt ); - Matrix Q_d = inverse(model_discrete_curr->R().transpose() * model_discrete_curr->R() ); + noiseModel::Gaussian::shared_ptr model_discrete_curr = calc_descrete_noise_model(model_continuous_overall, msr_dt ); + Matrix Q_d = inverse(model_discrete_curr->R().transpose() * model_discrete_curr->R() ); - EquivCov_Overall = F * EquivCov_Overall * F.transpose() + Q_d; + EquivCov_Overall = F * EquivCov_Overall * F.transpose() + Q_d; - // Update Jacobian_wrt_t0_Overall - Jacobian_wrt_t0_Overall = F * Jacobian_wrt_t0_Overall; - } + // Update Jacobian_wrt_t0_Overall + Jacobian_wrt_t0_Overall = F * Jacobian_wrt_t0_Overall; + } - static inline Vector PreIntegrateIMUObservations_delta_pos(const double msr_dt, - const Vector& delta_pos_in_t0, const Vector& delta_vel_in_t0){ + static inline Vector PreIntegrateIMUObservations_delta_pos(const double msr_dt, + const Vector& delta_pos_in_t0, const Vector& delta_vel_in_t0){ - // Note: all delta terms refer to an IMU\sensor system at t0 - // Note: delta_vel_in_t0 is already in body frame, so no need to use the body_P_sensor transformation here. + // Note: all delta terms refer to an IMU\sensor system at t0 + // Note: delta_vel_in_t0 is already in body frame, so no need to use the body_P_sensor transformation here. - return delta_pos_in_t0 + delta_vel_in_t0 * msr_dt; - } + return delta_pos_in_t0 + delta_vel_in_t0 * msr_dt; + } - static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt, - const Vector3& delta_angles, const Vector& delta_vel_in_t0, const bool flag_use_body_P_sensor, const POSE& body_P_sensor){ + static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt, + const Vector3& delta_angles, const Vector& delta_vel_in_t0, const bool flag_use_body_P_sensor, const POSE& body_P_sensor){ - // Note: all delta terms refer to an IMU\sensor system at t0 + // Note: all delta terms refer to an IMU\sensor system at t0 - // Calculate the corrected measurements using the Bias object - Vector AccCorrected = msr_acc_t; - Vector body_t_a_body; - if (flag_use_body_P_sensor){ - Matrix body_R_sensor = body_P_sensor.rotation().matrix(); + // Calculate the corrected measurements using the Bias object + Vector AccCorrected = msr_acc_t; + Vector body_t_a_body; + if (flag_use_body_P_sensor){ + Matrix body_R_sensor = body_P_sensor.rotation().matrix(); - Vector GyroCorrected(msr_gyro_t); + Vector GyroCorrected(msr_gyro_t); - Vector body_omega_body = body_R_sensor * GyroCorrected; - Matrix body_omega_body__cross = skewSymmetric(body_omega_body); + Vector body_omega_body = body_R_sensor * GyroCorrected; + Matrix body_omega_body__cross = skewSymmetric(body_omega_body); - body_t_a_body = body_R_sensor * AccCorrected - body_omega_body__cross * body_omega_body__cross * body_P_sensor.translation().vector(); - } else{ - body_t_a_body = AccCorrected; - } + body_t_a_body = body_R_sensor * AccCorrected - body_omega_body__cross * body_omega_body__cross * body_P_sensor.translation().vector(); + } else{ + body_t_a_body = AccCorrected; + } - Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); + Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); - return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt; - } + return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt; + } - static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt, - const Vector3& delta_angles, const bool flag_use_body_P_sensor, const POSE& body_P_sensor){ + static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt, + const Vector3& delta_angles, const bool flag_use_body_P_sensor, const POSE& body_P_sensor){ - // Note: all delta terms refer to an IMU\sensor system at t0 + // Note: all delta terms refer to an IMU\sensor system at t0 - // Calculate the corrected measurements using the Bias object - Vector GyroCorrected = msr_gyro_t; + // Calculate the corrected measurements using the Bias object + Vector GyroCorrected = msr_gyro_t; - Vector body_t_omega_body; - if (flag_use_body_P_sensor){ - body_t_omega_body = body_P_sensor.rotation().matrix() * GyroCorrected; - } else { - body_t_omega_body = GyroCorrected; - } + Vector body_t_omega_body; + if (flag_use_body_P_sensor){ + body_t_omega_body = body_P_sensor.rotation().matrix() * GyroCorrected; + } else { + body_t_omega_body = GyroCorrected; + } - Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); + Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); - R_t_to_t0 = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt ); - return Rot3::Logmap(R_t_to_t0); - } + R_t_to_t0 = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt ); + return Rot3::Logmap(R_t_to_t0); + } - static inline noiseModel::Gaussian::shared_ptr CalcEquivalentNoiseCov(const noiseModel::Gaussian::shared_ptr& gaussian_acc, const noiseModel::Gaussian::shared_ptr& gaussian_gyro, - const noiseModel::Gaussian::shared_ptr& gaussian_process){ + static inline noiseModel::Gaussian::shared_ptr CalcEquivalentNoiseCov(const noiseModel::Gaussian::shared_ptr& gaussian_acc, const noiseModel::Gaussian::shared_ptr& gaussian_gyro, + const noiseModel::Gaussian::shared_ptr& gaussian_process){ - Matrix cov_acc = inverse( gaussian_acc->R().transpose() * gaussian_acc->R() ); - Matrix cov_gyro = inverse( gaussian_gyro->R().transpose() * gaussian_gyro->R() ); - Matrix cov_process = inverse( gaussian_process->R().transpose() * gaussian_process->R() ); + Matrix cov_acc = inverse( gaussian_acc->R().transpose() * gaussian_acc->R() ); + Matrix cov_gyro = inverse( gaussian_gyro->R().transpose() * gaussian_gyro->R() ); + Matrix cov_process = inverse( gaussian_process->R().transpose() * gaussian_process->R() ); - cov_process.block(0,0, 3,3) += cov_gyro; - cov_process.block(6,6, 3,3) += cov_acc; + cov_process.block(0,0, 3,3) += cov_gyro; + cov_process.block(6,6, 3,3) += cov_acc; - return noiseModel::Gaussian::Covariance(cov_process); - } + return noiseModel::Gaussian::Covariance(cov_process); + } static inline void CalcEquivalentNoiseCov_DifferentParts(const noiseModel::Gaussian::shared_ptr& gaussian_acc, const noiseModel::Gaussian::shared_ptr& gaussian_gyro, const noiseModel::Gaussian::shared_ptr& gaussian_process, @@ -478,107 +478,107 @@ public: cov_process_without_acc_gyro = inverse( gaussian_process->R().transpose() * gaussian_process->R() ); } - static inline void Calc_g_rho_omega_earth_NED(const Vector& Pos_NED, const Vector& Vel_NED, const Vector& LatLonHeight_IC, const Vector& Pos_NED_Initial, - Vector& g_NED, Vector& rho_NED, Vector& omega_earth_NED) { + static inline void Calc_g_rho_omega_earth_NED(const Vector& Pos_NED, const Vector& Vel_NED, const Vector& LatLonHeight_IC, const Vector& Pos_NED_Initial, + Vector& g_NED, Vector& rho_NED, Vector& omega_earth_NED) { - Matrix ENU_to_NED = Matrix_(3, 3, - 0.0, 1.0, 0.0, - 1.0, 0.0, 0.0, - 0.0, 0.0, -1.0); + Matrix ENU_to_NED = (Matrix(3, 3) << + 0.0, 1.0, 0.0, + 1.0, 0.0, 0.0, + 0.0, 0.0, -1.0); - Matrix NED_to_ENU = Matrix_(3, 3, - 0.0, 1.0, 0.0, - 1.0, 0.0, 0.0, - 0.0, 0.0, -1.0); + Matrix NED_to_ENU = (Matrix(3, 3) << + 0.0, 1.0, 0.0, + 1.0, 0.0, 0.0, + 0.0, 0.0, -1.0); - // Convert incoming parameters to ENU - Vector Pos_ENU = NED_to_ENU * Pos_NED; - Vector Vel_ENU = NED_to_ENU * Vel_NED; - Vector Pos_ENU_Initial = NED_to_ENU * Pos_NED_Initial; + // Convert incoming parameters to ENU + Vector Pos_ENU = NED_to_ENU * Pos_NED; + Vector Vel_ENU = NED_to_ENU * Vel_NED; + Vector Pos_ENU_Initial = NED_to_ENU * Pos_NED_Initial; - // Call ENU version - Vector g_ENU; - Vector rho_ENU; - Vector omega_earth_ENU; - Calc_g_rho_omega_earth_ENU(Pos_ENU, Vel_ENU, LatLonHeight_IC, Pos_ENU_Initial, g_ENU, rho_ENU, omega_earth_ENU); + // Call ENU version + Vector g_ENU; + Vector rho_ENU; + Vector omega_earth_ENU; + Calc_g_rho_omega_earth_ENU(Pos_ENU, Vel_ENU, LatLonHeight_IC, Pos_ENU_Initial, g_ENU, rho_ENU, omega_earth_ENU); - // Convert output to NED - g_NED = ENU_to_NED * g_ENU; - rho_NED = ENU_to_NED * rho_ENU; - omega_earth_NED = ENU_to_NED * omega_earth_ENU; - } + // Convert output to NED + g_NED = ENU_to_NED * g_ENU; + rho_NED = ENU_to_NED * rho_ENU; + omega_earth_NED = ENU_to_NED * omega_earth_ENU; + } - static inline void Calc_g_rho_omega_earth_ENU(const Vector& Pos_ENU, const Vector& Vel_ENU, const Vector& LatLonHeight_IC, const Vector& Pos_ENU_Initial, - Vector& g_ENU, Vector& rho_ENU, Vector& omega_earth_ENU){ - double R0 = 6.378388e6; - double e = 1/297; - double Re( R0*( 1-e*(sin( LatLonHeight_IC(0) ))*(sin( LatLonHeight_IC(0) )) ) ); + static inline void Calc_g_rho_omega_earth_ENU(const Vector& Pos_ENU, const Vector& Vel_ENU, const Vector& LatLonHeight_IC, const Vector& Pos_ENU_Initial, + Vector& g_ENU, Vector& rho_ENU, Vector& omega_earth_ENU){ + double R0 = 6.378388e6; + double e = 1/297; + double Re( R0*( 1-e*(sin( LatLonHeight_IC(0) ))*(sin( LatLonHeight_IC(0) )) ) ); - // Calculate current lat, lon - Vector delta_Pos_ENU(Pos_ENU - Pos_ENU_Initial); - double delta_lat(delta_Pos_ENU(1)/Re); - double delta_lon(delta_Pos_ENU(0)/(Re*cos(LatLonHeight_IC(0)))); - double lat_new(LatLonHeight_IC(0) + delta_lat); - double lon_new(LatLonHeight_IC(1) + delta_lon); + // Calculate current lat, lon + Vector delta_Pos_ENU(Pos_ENU - Pos_ENU_Initial); + double delta_lat(delta_Pos_ENU(1)/Re); + double delta_lon(delta_Pos_ENU(0)/(Re*cos(LatLonHeight_IC(0)))); + double lat_new(LatLonHeight_IC(0) + delta_lat); + double lon_new(LatLonHeight_IC(1) + delta_lon); - // Rotation of lon about z axis - Rot3 C1(cos(lon_new), sin(lon_new), 0.0, - -sin(lon_new), cos(lon_new), 0.0, - 0.0, 0.0, 1.0); + // Rotation of lon about z axis + Rot3 C1(cos(lon_new), sin(lon_new), 0.0, + -sin(lon_new), cos(lon_new), 0.0, + 0.0, 0.0, 1.0); - // Rotation of lat about y axis - Rot3 C2(cos(lat_new), 0.0, sin(lat_new), - 0.0, 1.0, 0.0, - -sin(lat_new), 0.0, cos(lat_new)); + // Rotation of lat about y axis + Rot3 C2(cos(lat_new), 0.0, sin(lat_new), + 0.0, 1.0, 0.0, + -sin(lat_new), 0.0, cos(lat_new)); - Rot3 UEN_to_ENU(0, 1, 0, - 0, 0, 1, - 1, 0, 0); + Rot3 UEN_to_ENU(0, 1, 0, + 0, 0, 1, + 1, 0, 0); - Rot3 R_ECEF_to_ENU( UEN_to_ENU * C2 * C1 ); + Rot3 R_ECEF_to_ENU( UEN_to_ENU * C2 * C1 ); - Vector omega_earth_ECEF(Vector_(3, 0.0, 0.0, 7.292115e-5)); - omega_earth_ENU = R_ECEF_to_ENU.matrix() * omega_earth_ECEF; + Vector omega_earth_ECEF((Vector(3) << 0.0, 0.0, 7.292115e-5)); + omega_earth_ENU = R_ECEF_to_ENU.matrix() * omega_earth_ECEF; - // Calculating g - double height(LatLonHeight_IC(2)); - double EQUA_RADIUS = 6378137.0; // equatorial radius of the earth; WGS-84 - double ECCENTRICITY = 0.0818191908426; // eccentricity of the earth ellipsoid - double e2( pow(ECCENTRICITY,2) ); - double den( 1-e2*pow(sin(lat_new),2) ); - double Rm( (EQUA_RADIUS*(1-e2))/( pow(den,(3/2)) ) ); - double Rp( EQUA_RADIUS/( sqrt(den) ) ); - double Ro( sqrt(Rp*Rm) ); // mean earth radius of curvature - double g0( 9.780318*( 1 + 5.3024e-3 * pow(sin(lat_new),2) - 5.9e-6 * pow(sin(2*lat_new),2) ) ); - double g_calc( g0/( pow(1 + height/Ro, 2) ) ); - g_ENU = Vector_(3, 0.0, 0.0, -g_calc); + // Calculating g + double height(LatLonHeight_IC(2)); + double EQUA_RADIUS = 6378137.0; // equatorial radius of the earth; WGS-84 + double ECCENTRICITY = 0.0818191908426; // eccentricity of the earth ellipsoid + double e2( pow(ECCENTRICITY,2) ); + double den( 1-e2*pow(sin(lat_new),2) ); + double Rm( (EQUA_RADIUS*(1-e2))/( pow(den,(3/2)) ) ); + double Rp( EQUA_RADIUS/( sqrt(den) ) ); + double Ro( sqrt(Rp*Rm) ); // mean earth radius of curvature + double g0( 9.780318*( 1 + 5.3024e-3 * pow(sin(lat_new),2) - 5.9e-6 * pow(sin(2*lat_new),2) ) ); + double g_calc( g0/( pow(1 + height/Ro, 2) ) ); + g_ENU = (Vector(3) << 0.0, 0.0, -g_calc); - // Calculate rho - double Ve( Vel_ENU(0) ); - double Vn( Vel_ENU(1) ); - double rho_E = -Vn/(Rm + height); - double rho_N = Ve/(Rp + height); - double rho_U = Ve*tan(lat_new)/(Rp + height); - rho_ENU = Vector_(3, rho_E, rho_N, rho_U); - } + // Calculate rho + double Ve( Vel_ENU(0) ); + double Vn( Vel_ENU(1) ); + double rho_E = -Vn/(Rm + height); + double rho_N = Ve/(Rp + height); + double rho_U = Ve*tan(lat_new)/(Rp + height); + rho_ENU = (Vector(3) << rho_E, rho_N, rho_U); + } - static inline noiseModel::Gaussian::shared_ptr calc_descrete_noise_model(const noiseModel::Gaussian::shared_ptr& model, double delta_t){ - /* Q_d (approx)= Q * delta_t */ - /* In practice, square root of the information matrix is represented, so that: - * R_d (approx)= R / sqrt(delta_t) - * */ - return noiseModel::Gaussian::SqrtInformation(model->R()/sqrt(delta_t)); - } + static inline noiseModel::Gaussian::shared_ptr calc_descrete_noise_model(const noiseModel::Gaussian::shared_ptr& model, double delta_t){ + /* Q_d (approx)= Q * delta_t */ + /* In practice, square root of the information matrix is represented, so that: + * R_d (approx)= R / sqrt(delta_t) + * */ + return noiseModel::Gaussian::SqrtInformation(model->R()/sqrt(delta_t)); + } private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NonlinearFactor2", - boost::serialization::base_object(*this)); - } + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NonlinearFactor2", + boost::serialization::base_object(*this)); + } diff --git a/gtsam_unstable/slam/ImplicitSchurFactor.h b/gtsam_unstable/slam/ImplicitSchurFactor.h new file mode 100644 index 000000000..234d13f1f --- /dev/null +++ b/gtsam_unstable/slam/ImplicitSchurFactor.h @@ -0,0 +1,384 @@ +/** + * @file ImplicitSchurFactor.h + * @brief A new type of linear factor (GaussianFactor), which is subclass of GaussianFactor + * @author Frank Dellaert + * @author Luca Carlone + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace gtsam { + +/** + * ImplicitSchurFactor + */ +template // +class ImplicitSchurFactor: public GaussianFactor { + +public: + typedef ImplicitSchurFactor This; ///< Typedef to this class + typedef JacobianFactor Base; ///< Typedef to base class + typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class + +protected: + + typedef Eigen::Matrix Matrix2D; ///< type of an F block + typedef Eigen::Matrix Matrix23; + typedef Eigen::Matrix MatrixDD; ///< camera hessian + typedef std::pair KeyMatrix2D; ///< named F block + + std::vector Fblocks_; ///< All 2*D F blocks (one for each camera) + Matrix3 PointCovariance_; ///< the 3*3 matrix P = inv(E'E) (2*2 if degenerate) + Matrix E_; ///< The 2m*3 E Jacobian with respect to the point + Vector b_; ///< 2m-dimensional RHS vector + +public: + + /// Constructor + ImplicitSchurFactor() { + } + + /// Construct from blcoks of F, E, inv(E'*E), and RHS vector b + ImplicitSchurFactor(const std::vector& Fblocks, const Matrix& E, + const Matrix3& P, const Vector& b) : + Fblocks_(Fblocks), PointCovariance_(P), E_(E), b_(b) { + initKeys(); + } + + /// initialize keys from Fblocks + void initKeys() { + keys_.reserve(Fblocks_.size()); + BOOST_FOREACH(const KeyMatrix2D& it, Fblocks_) + keys_.push_back(it.first); + } + + /// Destructor + virtual ~ImplicitSchurFactor() { + } + + // Write access, only use for construction! + + inline std::vector& Fblocks() { + return Fblocks_; + } + + inline Matrix3& PointCovariance() { + return PointCovariance_; + } + + inline Matrix& E() { + return E_; + } + + inline Vector& b() { + return b_; + } + + /// Get matrix P + inline const Matrix& getPointCovariance() const { + return PointCovariance_; + } + + /// print + void print(const std::string& s, const KeyFormatter& formatter) const { + std::cout << " ImplicitSchurFactor " << std::endl; + Factor::print(s); + std::cout << " PointCovariance_ \n" << PointCovariance_ << std::endl; + std::cout << " E_ \n" << E_ << std::endl; + std::cout << " b_ \n" << b_.transpose() << std::endl; + } + + /// equals + bool equals(const GaussianFactor& lf, double tol) const { + if (!dynamic_cast(&lf)) + return false; + else { + return false; + } + } + + /// Degrees of freedom of camera + virtual DenseIndex getDim(const_iterator variable) const { + return D; + } + + virtual Matrix augmentedJacobian() const { + throw std::runtime_error( + "ImplicitSchurFactor::augmentedJacobian non implemented"); + return Matrix(); + } + virtual std::pair jacobian() const { + throw std::runtime_error("ImplicitSchurFactor::jacobian non implemented"); + return std::make_pair(Matrix(), Vector()); + } + virtual Matrix augmentedInformation() const { + throw std::runtime_error( + "ImplicitSchurFactor::augmentedInformation non implemented"); + return Matrix(); + } + virtual Matrix information() const { + throw std::runtime_error( + "ImplicitSchurFactor::information non implemented"); + return Matrix(); + } + + /// Return the diagonal of the Hessian for this factor + virtual VectorValues hessianDiagonal() const { + // diag(Hessian) = diag(F' * (I - E * PointCov * E') * F); + VectorValues d; + + for (size_t pos = 0; pos < size(); ++pos) { // for each camera + Key j = keys_[pos]; + + // Calculate Fj'*Ej for the current camera (observing a single point) + // D x 3 = (D x 2) * (2 x 3) + const Matrix2D& Fj = Fblocks_[pos].second; + Eigen::Matrix FtE = Fj.transpose() + * E_.block<2, 3>(2 * pos, 0); + + Eigen::Matrix dj; + for (size_t k = 0; k < D; ++k) { // for each diagonal element of the camera hessian + // Vector column_k_Fj = Fj.col(k); + dj(k) = Fj.col(k).squaredNorm(); // dot(column_k_Fj, column_k_Fj); + // Vector column_k_FtE = FtE.row(k); + // (1 x 1) = (1 x 3) * (3 * 3) * (3 x 1) + dj(k) -= FtE.row(k) * PointCovariance_ * FtE.row(k).transpose(); + } + d.insert(j, dj); + } + return d; + } + + /** + * @brief add the contribution of this factor to the diagonal of the hessian + * d(output) = d(input) + deltaHessianFactor + */ + void hessianDiagonal(double* d) const { + // diag(Hessian) = diag(F' * (I - E * PointCov * E') * F); + // Use eigen magic to access raw memory + typedef Eigen::Matrix DVector; + typedef Eigen::Map DMap; + + for (size_t pos = 0; pos < size(); ++pos) { // for each camera in the factor + Key j = keys_[pos]; + + // Calculate Fj'*Ej for the current camera (observing a single point) + // D x 3 = (D x 2) * (2 x 3) + const Matrix2D& Fj = Fblocks_[pos].second; + Eigen::Matrix FtE = Fj.transpose() + * E_.block<2, 3>(2 * pos, 0); + + DVector dj; + for (size_t k = 0; k < D; ++k) { // for each diagonal element of the camera hessian + dj(k) = Fj.col(k).squaredNorm(); + // (1 x 1) = (1 x 3) * (3 * 3) * (3 x 1) + dj(k) -= FtE.row(k) * PointCovariance_ * FtE.row(k).transpose(); + } + DMap(d + D * j) += dj; + } + } + + /// Return the block diagonal of the Hessian for this factor + virtual std::map hessianBlockDiagonal() const { + std::map blocks; + for (size_t pos = 0; pos < size(); ++pos) { + Key j = keys_[pos]; + const Matrix2D& Fj = Fblocks_[pos].second; + // F'*F - F'*E*P*E'*F (9*2)*(2*9) - (9*2)*(2*3)*(3*3)*(3*2)*(2*9) + Eigen::Matrix FtE = Fj.transpose() + * E_.block<2, 3>(2 * pos, 0); + blocks[j] = Fj.transpose() * Fj + - FtE * PointCovariance_ * FtE.transpose(); + // F'*(I - E*P*E')*F, TODO: this should work, but it does not :-( +// static const Eigen::Matrix I2 = eye(2); +// Eigen::Matrix Q = // +// I2 - E_.block<2, 3>(2 * pos, 0) * PointCovariance_ * E_.block<2, 3>(2 * pos, 0).transpose(); +// blocks[j] = Fj.transpose() * Q * Fj; + } + return blocks; + } + + virtual GaussianFactor::shared_ptr clone() const { + return boost::make_shared >(Fblocks_, + PointCovariance_, E_, b_); + throw std::runtime_error("ImplicitSchurFactor::clone non implemented"); + } + virtual bool empty() const { + return false; + } + + virtual GaussianFactor::shared_ptr negate() const { + return boost::make_shared >(Fblocks_, + PointCovariance_, E_, b_); + throw std::runtime_error("ImplicitSchurFactor::negate non implemented"); + } + + // Raw Vector version of y += F'*alpha*(I - E*P*E')*F*x, for testing + static + void multiplyHessianAdd(const Matrix& F, const Matrix& E, + const Matrix& PointCovariance, double alpha, const Vector& x, Vector& y) { + Vector e1 = F * x; + Vector d1 = E.transpose() * e1; + Vector d2 = PointCovariance * d1; + Vector e2 = E * d2; + Vector e3 = alpha * (e1 - e2); + y += F.transpose() * e3; + } + + typedef std::vector Error2s; + + /** + * @brief Calculate corrected error Q*e = (I - E*P*E')*e + */ + void projectError(const Error2s& e1, Error2s& e2) const { + + // d1 = E.transpose() * e1 = (3*2m)*2m + Vector3 d1; + d1.setZero(); + for (size_t k = 0; k < size(); k++) + d1 += E_.block < 2, 3 > (2 * k, 0).transpose() * e1[k]; + + // d2 = E.transpose() * e1 = (3*2m)*2m + Vector3 d2 = PointCovariance_ * d1; + + // e3 = alpha*(e1 - E*d2) = 1*[2m-(2m*3)*3] + for (size_t k = 0; k < size(); k++) + e2[k] = e1[k] - E_.block < 2, 3 > (2 * k, 0) * d2; + } + + /// needed to be GaussianFactor - (I - E*P*E')*(F*x - b) + virtual double error(const VectorValues& x) const { + + // resize does not do malloc if correct size + e1.resize(size()); + e2.resize(size()); + + // e1 = F * x - b = (2m*dm)*dm + for (size_t k = 0; k < size(); ++k) + e1[k] = Fblocks_[k].second * x.at(keys_[k]) - b_.segment < 2 > (k * 2); + projectError(e1, e2); + + double result = 0; + for (size_t k = 0; k < size(); ++k) + result += dot(e2[k], e2[k]); + return 0.5 * result; + } + + /// Scratch space for multiplyHessianAdd + mutable Error2s e1, e2; + + /** + * @brief double* Hessian-vector multiply, i.e. y += F'*alpha*(I - E*P*E')*F*x + * RAW memory access! Assumes keys start at 0 and go to M-1, and x and and y are laid out that way + */ + void multiplyHessianAdd(double alpha, const double* x, double* y) const { + + // Use eigen magic to access raw memory + typedef Eigen::Matrix DVector; + typedef Eigen::Map DMap; + typedef Eigen::Map ConstDMap; + + // resize does not do malloc if correct size + e1.resize(size()); + e2.resize(size()); + + // e1 = F * x = (2m*dm)*dm + size_t k = 0; + BOOST_FOREACH(const KeyMatrix2D& it, Fblocks_) { + Key key = it.first; + e1[k++] = it.second * ConstDMap(x + D * key); + } + + projectError(e1, e2); + + // y += F.transpose()*e2 = (2d*2m)*2m + k = 0; + BOOST_FOREACH(const KeyMatrix2D& it, Fblocks_) { + Key key = it.first; + DMap(y + D * key) += it.second.transpose() * alpha * e2[k++]; + } + } + + void multiplyHessianAdd(double alpha, const double* x, double* y, + std::vector keys) const { + } + ; + + /** + * @brief Hessian-vector multiply, i.e. y += F'*alpha*(I - E*P*E')*F*x + */ + void multiplyHessianAdd(double alpha, const VectorValues& x, + VectorValues& y) const { + + // resize does not do malloc if correct size + e1.resize(size()); + e2.resize(size()); + + // e1 = F * x = (2m*dm)*dm + for (size_t k = 0; k < size(); ++k) + e1[k] = Fblocks_[k].second * x.at(keys_[k]); + + projectError(e1, e2); + + // y += F.transpose()*e2 = (2d*2m)*2m + for (size_t k = 0; k < size(); ++k) { + Key key = keys_[k]; + static const Vector empty; + std::pair it = y.tryInsert(key, empty); + Vector& yi = it.first->second; + // Create the value as a zero vector if it does not exist. + if (it.second) + yi = Vector::Zero(Fblocks_[k].second.cols()); + yi += Fblocks_[k].second.transpose() * alpha * e2[k]; + } + } + + /** + * @brief Dummy version to measure overhead of key access + */ + void multiplyHessianDummy(double alpha, const VectorValues& x, + VectorValues& y) const { + + BOOST_FOREACH(const KeyMatrix2D& Fi, Fblocks_) { + static const Vector empty; + Key key = Fi.first; + std::pair it = y.tryInsert(key, empty); + Vector& yi = it.first->second; + yi = x.at(key); + } + } + + /** + * Calculate gradient, which is -F'Q*b, see paper + */ + VectorValues gradientAtZero() const { + // calculate Q*b + e1.resize(size()); + e2.resize(size()); + for (size_t k = 0; k < size(); k++) + e1[k] = b_.segment < 2 > (2 * k); + projectError(e1, e2); + + // g = F.transpose()*e2 + VectorValues g; + for (size_t k = 0; k < size(); ++k) { + Key key = keys_[k]; + g.insert(key, -Fblocks_[k].second.transpose() * e2[k]); + } + + // return it + return g; + } + +}; +// ImplicitSchurFactor + +} + diff --git a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h index 33eb8777d..5ca736c01 100644 --- a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h +++ b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h @@ -44,11 +44,11 @@ namespace gtsam { * matrices and the process\modeling covariance matrix. The IneritalNavFactor converts this into a * discrete form using the supplied delta_t between sub-sequential measurements. * - Earth-rate correction: - * + Currently the user should supply R_ECEF_to_G, which is the rotation from ECEF to the global - * frame (Local-Level system: ENU or NED, see above). - * + R_ECEF_to_G can be calculated by approximated values of latitude and longitude of the system. - * + Currently it is assumed that a relatively small distance is traveled w.r.t. to initial pose, since R_ECEF_to_G is constant. - * Otherwise, R_ECEF_to_G should be updated each time using the current lat-lon. + * + Currently the user should supply R_ECEF_to_G, which is the rotation from ECEF to the global + * frame (Local-Level system: ENU or NED, see above). + * + R_ECEF_to_G can be calculated by approximated values of latitude and longitude of the system. + * + Currently it is assumed that a relatively small distance is traveled w.r.t. to initial pose, since R_ECEF_to_G is constant. + * Otherwise, R_ECEF_to_G should be updated each time using the current lat-lon. * * - Frame Notation: * Quantities are written as {Frame of Representation/Destination Frame}_{Quantity Type}_{Quatity Description/Origination Frame} @@ -81,70 +81,70 @@ class InertialNavFactor_GlobalVelocity : public NoiseModelFactor5 This; - typedef NoiseModelFactor5 Base; + typedef InertialNavFactor_GlobalVelocity This; + typedef NoiseModelFactor5 Base; - Vector measurement_acc_; - Vector measurement_gyro_; - double dt_; + Vector measurement_acc_; + Vector measurement_gyro_; + double dt_; - Vector world_g_; - Vector world_rho_; + Vector world_g_; + Vector world_rho_; Vector world_omega_earth_; boost::optional body_P_sensor_; // The pose of the sensor in the body frame public: - // shorthand for a smart pointer to a factor - typedef typename boost::shared_ptr shared_ptr; + // shorthand for a smart pointer to a factor + typedef typename boost::shared_ptr shared_ptr; - /** default constructor - only use for serialization */ - InertialNavFactor_GlobalVelocity() {} + /** default constructor - only use for serialization */ + InertialNavFactor_GlobalVelocity() {} - /** Constructor */ - InertialNavFactor_GlobalVelocity(const Key& Pose1, const Key& Vel1, const Key& IMUBias1, const Key& Pose2, const Key& Vel2, + /** Constructor */ + InertialNavFactor_GlobalVelocity(const Key& Pose1, const Key& Vel1, const Key& IMUBias1, const Key& Pose2, const Key& Vel2, const Vector& measurement_acc, const Vector& measurement_gyro, const double measurement_dt, const Vector world_g, const Vector world_rho, const Vector& world_omega_earth, const noiseModel::Gaussian::shared_ptr& model_continuous, boost::optional body_P_sensor = boost::none) : Base(calc_descrete_noise_model(model_continuous, measurement_dt ), Pose1, Vel1, IMUBias1, Pose2, Vel2), measurement_acc_(measurement_acc), measurement_gyro_(measurement_gyro), - dt_(measurement_dt), world_g_(world_g), world_rho_(world_rho), world_omega_earth_(world_omega_earth), body_P_sensor_(body_P_sensor) { } + dt_(measurement_dt), world_g_(world_g), world_rho_(world_rho), world_omega_earth_(world_omega_earth), body_P_sensor_(body_P_sensor) { } - virtual ~InertialNavFactor_GlobalVelocity() {} + virtual ~InertialNavFactor_GlobalVelocity() {} - /** implement functions needed for Testable */ + /** implement functions needed for Testable */ - /** print */ - virtual void print(const std::string& s = "InertialNavFactor_GlobalVelocity", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - std::cout << s << "(" - << keyFormatter(this->key1()) << "," - << keyFormatter(this->key2()) << "," - << keyFormatter(this->key3()) << "," - << keyFormatter(this->key4()) << "," - << keyFormatter(this->key5()) << "\n"; - std::cout << "acc measurement: " << this->measurement_acc_.transpose() << std::endl; - std::cout << "gyro measurement: " << this->measurement_gyro_.transpose() << std::endl; - std::cout << "dt: " << this->dt_ << std::endl; - std::cout << "gravity (in world frame): " << this->world_g_.transpose() << std::endl; - std::cout << "craft rate (in world frame): " << this->world_rho_.transpose() << std::endl; - std::cout << "earth's rotation (in world frame): " << this->world_omega_earth_.transpose() << std::endl; - if(this->body_P_sensor_) - this->body_P_sensor_->print(" sensor pose in body frame: "); + /** print */ + virtual void print(const std::string& s = "InertialNavFactor_GlobalVelocity", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + std::cout << s << "(" + << keyFormatter(this->key1()) << "," + << keyFormatter(this->key2()) << "," + << keyFormatter(this->key3()) << "," + << keyFormatter(this->key4()) << "," + << keyFormatter(this->key5()) << "\n"; + std::cout << "acc measurement: " << this->measurement_acc_.transpose() << std::endl; + std::cout << "gyro measurement: " << this->measurement_gyro_.transpose() << std::endl; + std::cout << "dt: " << this->dt_ << std::endl; + std::cout << "gravity (in world frame): " << this->world_g_.transpose() << std::endl; + std::cout << "craft rate (in world frame): " << this->world_rho_.transpose() << std::endl; + std::cout << "earth's rotation (in world frame): " << this->world_omega_earth_.transpose() << std::endl; + if(this->body_P_sensor_) + this->body_P_sensor_->print(" sensor pose in body frame: "); this->noiseModel_->print(" noise model"); - } + } - /** equals */ - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { - const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) - && (measurement_acc_ - e->measurement_acc_).norm() < tol - && (measurement_gyro_ - e->measurement_gyro_).norm() < tol - && (dt_ - e->dt_) < tol - && (world_g_ - e->world_g_).norm() < tol - && (world_rho_ - e->world_rho_).norm() < tol - && (world_omega_earth_ - e->world_omega_earth_).norm() < tol - && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); - } + /** equals */ + virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { + const This *e = dynamic_cast (&expected); + return e != NULL && Base::equals(*e, tol) + && (measurement_acc_ - e->measurement_acc_).norm() < tol + && (measurement_gyro_ - e->measurement_gyro_).norm() < tol + && (dt_ - e->dt_) < tol + && (world_g_ - e->world_g_).norm() < tol + && (world_rho_ - e->world_rho_).norm() < tol + && (world_omega_earth_ - e->world_omega_earth_).norm() < tol + && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); + } POSE predictPose(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1) const { // Calculate the corrected measurements using the Bias object @@ -225,12 +225,12 @@ public: } /** implement functions needed to derive from Factor */ - Vector evaluateError(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, const POSE& Pose2, const VELOCITY& Vel2, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none, - boost::optional H3 = boost::none, - boost::optional H4 = boost::none, - boost::optional H5 = boost::none) const { + Vector evaluateError(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, const POSE& Pose2, const VELOCITY& Vel2, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none, + boost::optional H3 = boost::none, + boost::optional H4 = boost::none, + boost::optional H5 = boost::none) const { // TODO: Write analytical derivative calculations // Jacobian w.r.t. Pose1 @@ -268,34 +268,34 @@ public: *H5 = stack(2, &H5_Pose, &H5_Vel); } - Vector ErrPoseVector(POSE::Logmap(evaluatePoseError(Pose1, Vel1, Bias1, Pose2, Vel2))); - Vector ErrVelVector(VELOCITY::Logmap(evaluateVelocityError(Pose1, Vel1, Bias1, Pose2, Vel2))); + Vector ErrPoseVector(POSE::Logmap(evaluatePoseError(Pose1, Vel1, Bias1, Pose2, Vel2))); + Vector ErrVelVector(VELOCITY::Logmap(evaluateVelocityError(Pose1, Vel1, Bias1, Pose2, Vel2))); - return concatVectors(2, &ErrPoseVector, &ErrVelVector); - } + return concatVectors(2, &ErrPoseVector, &ErrVelVector); + } - static inline noiseModel::Gaussian::shared_ptr CalcEquivalentNoiseCov(const noiseModel::Gaussian::shared_ptr& gaussian_acc, const noiseModel::Gaussian::shared_ptr& gaussian_gyro, - const noiseModel::Gaussian::shared_ptr& gaussian_process){ + static inline noiseModel::Gaussian::shared_ptr CalcEquivalentNoiseCov(const noiseModel::Gaussian::shared_ptr& gaussian_acc, const noiseModel::Gaussian::shared_ptr& gaussian_gyro, + const noiseModel::Gaussian::shared_ptr& gaussian_process){ - Matrix cov_acc = inverse( gaussian_acc->R().transpose() * gaussian_acc->R() ); - Matrix cov_gyro = inverse( gaussian_gyro->R().transpose() * gaussian_gyro->R() ); - Matrix cov_process = inverse( gaussian_process->R().transpose() * gaussian_process->R() ); + Matrix cov_acc = inverse( gaussian_acc->R().transpose() * gaussian_acc->R() ); + Matrix cov_gyro = inverse( gaussian_gyro->R().transpose() * gaussian_gyro->R() ); + Matrix cov_process = inverse( gaussian_process->R().transpose() * gaussian_process->R() ); - cov_process.block(0,0, 3,3) += cov_gyro; - cov_process.block(6,6, 3,3) += cov_acc; + cov_process.block(0,0, 3,3) += cov_gyro; + cov_process.block(6,6, 3,3) += cov_acc; - return noiseModel::Gaussian::Covariance(cov_process); - } + return noiseModel::Gaussian::Covariance(cov_process); + } static inline void Calc_g_rho_omega_earth_NED(const Vector& Pos_NED, const Vector& Vel_NED, const Vector& LatLonHeight_IC, const Vector& Pos_NED_Initial, Vector& g_NED, Vector& rho_NED, Vector& omega_earth_NED) { - Matrix ENU_to_NED = Matrix_(3, 3, + Matrix ENU_to_NED = (Matrix(3, 3) << 0.0, 1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0); - Matrix NED_to_ENU = Matrix_(3, 3, + Matrix NED_to_ENU = (Matrix(3, 3) << 0.0, 1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0); @@ -317,78 +317,78 @@ public: omega_earth_NED = ENU_to_NED * omega_earth_ENU; } - static inline void Calc_g_rho_omega_earth_ENU(const Vector& Pos_ENU, const Vector& Vel_ENU, const Vector& LatLonHeight_IC, const Vector& Pos_ENU_Initial, - Vector& g_ENU, Vector& rho_ENU, Vector& omega_earth_ENU){ - double R0 = 6.378388e6; - double e = 1/297; - double Re( R0*( 1-e*(sin( LatLonHeight_IC(0) ))*(sin( LatLonHeight_IC(0) )) ) ); + static inline void Calc_g_rho_omega_earth_ENU(const Vector& Pos_ENU, const Vector& Vel_ENU, const Vector& LatLonHeight_IC, const Vector& Pos_ENU_Initial, + Vector& g_ENU, Vector& rho_ENU, Vector& omega_earth_ENU){ + double R0 = 6.378388e6; + double e = 1/297; + double Re( R0*( 1-e*(sin( LatLonHeight_IC(0) ))*(sin( LatLonHeight_IC(0) )) ) ); - // Calculate current lat, lon - Vector delta_Pos_ENU(Pos_ENU - Pos_ENU_Initial); - double delta_lat(delta_Pos_ENU(1)/Re); - double delta_lon(delta_Pos_ENU(0)/(Re*cos(LatLonHeight_IC(0)))); - double lat_new(LatLonHeight_IC(0) + delta_lat); - double lon_new(LatLonHeight_IC(1) + delta_lon); + // Calculate current lat, lon + Vector delta_Pos_ENU(Pos_ENU - Pos_ENU_Initial); + double delta_lat(delta_Pos_ENU(1)/Re); + double delta_lon(delta_Pos_ENU(0)/(Re*cos(LatLonHeight_IC(0)))); + double lat_new(LatLonHeight_IC(0) + delta_lat); + double lon_new(LatLonHeight_IC(1) + delta_lon); - // Rotation of lon about z axis - Rot3 C1(cos(lon_new), sin(lon_new), 0.0, - -sin(lon_new), cos(lon_new), 0.0, - 0.0, 0.0, 1.0); + // Rotation of lon about z axis + Rot3 C1(cos(lon_new), sin(lon_new), 0.0, + -sin(lon_new), cos(lon_new), 0.0, + 0.0, 0.0, 1.0); - // Rotation of lat about y axis - Rot3 C2(cos(lat_new), 0.0, sin(lat_new), - 0.0, 1.0, 0.0, - -sin(lat_new), 0.0, cos(lat_new)); + // Rotation of lat about y axis + Rot3 C2(cos(lat_new), 0.0, sin(lat_new), + 0.0, 1.0, 0.0, + -sin(lat_new), 0.0, cos(lat_new)); - Rot3 UEN_to_ENU(0, 1, 0, - 0, 0, 1, - 1, 0, 0); + Rot3 UEN_to_ENU(0, 1, 0, + 0, 0, 1, + 1, 0, 0); - Rot3 R_ECEF_to_ENU( UEN_to_ENU * C2 * C1 ); + Rot3 R_ECEF_to_ENU( UEN_to_ENU * C2 * C1 ); - Vector omega_earth_ECEF(Vector_(3, 0.0, 0.0, 7.292115e-5)); - omega_earth_ENU = R_ECEF_to_ENU.matrix() * omega_earth_ECEF; + Vector omega_earth_ECEF((Vector(3) << 0.0, 0.0, 7.292115e-5)); + omega_earth_ENU = R_ECEF_to_ENU.matrix() * omega_earth_ECEF; - // Calculating g - double height(LatLonHeight_IC(2)); - double EQUA_RADIUS = 6378137.0; // equatorial radius of the earth; WGS-84 - double ECCENTRICITY = 0.0818191908426; // eccentricity of the earth ellipsoid - double e2( pow(ECCENTRICITY,2) ); - double den( 1-e2*pow(sin(lat_new),2) ); - double Rm( (EQUA_RADIUS*(1-e2))/( pow(den,(3/2)) ) ); - double Rp( EQUA_RADIUS/( sqrt(den) ) ); - double Ro( sqrt(Rp*Rm) ); // mean earth radius of curvature - double g0( 9.780318*( 1 + 5.3024e-3 * pow(sin(lat_new),2) - 5.9e-6 * pow(sin(2*lat_new),2) ) ); - double g_calc( g0/( pow(1 + height/Ro, 2) ) ); - g_ENU = Vector_(3, 0.0, 0.0, -g_calc); + // Calculating g + double height(LatLonHeight_IC(2)); + double EQUA_RADIUS = 6378137.0; // equatorial radius of the earth; WGS-84 + double ECCENTRICITY = 0.0818191908426; // eccentricity of the earth ellipsoid + double e2( pow(ECCENTRICITY,2) ); + double den( 1-e2*pow(sin(lat_new),2) ); + double Rm( (EQUA_RADIUS*(1-e2))/( pow(den,(3/2)) ) ); + double Rp( EQUA_RADIUS/( sqrt(den) ) ); + double Ro( sqrt(Rp*Rm) ); // mean earth radius of curvature + double g0( 9.780318*( 1 + 5.3024e-3 * pow(sin(lat_new),2) - 5.9e-6 * pow(sin(2*lat_new),2) ) ); + double g_calc( g0/( pow(1 + height/Ro, 2) ) ); + g_ENU = (Vector(3) << 0.0, 0.0, -g_calc); - // Calculate rho - double Ve( Vel_ENU(0) ); - double Vn( Vel_ENU(1) ); - double rho_E = -Vn/(Rm + height); - double rho_N = Ve/(Rp + height); - double rho_U = Ve*tan(lat_new)/(Rp + height); - rho_ENU = Vector_(3, rho_E, rho_N, rho_U); - } + // Calculate rho + double Ve( Vel_ENU(0) ); + double Vn( Vel_ENU(1) ); + double rho_E = -Vn/(Rm + height); + double rho_N = Ve/(Rp + height); + double rho_U = Ve*tan(lat_new)/(Rp + height); + rho_ENU = (Vector(3) << rho_E, rho_N, rho_U); + } - static inline noiseModel::Gaussian::shared_ptr calc_descrete_noise_model(const noiseModel::Gaussian::shared_ptr& model, double delta_t){ - /* Q_d (approx)= Q * delta_t */ - /* In practice, square root of the information matrix is represented, so that: - * R_d (approx)= R / sqrt(delta_t) - * */ - return noiseModel::Gaussian::SqrtInformation(model->R()/std::sqrt(delta_t)); - } + static inline noiseModel::Gaussian::shared_ptr calc_descrete_noise_model(const noiseModel::Gaussian::shared_ptr& model, double delta_t){ + /* Q_d (approx)= Q * delta_t */ + /* In practice, square root of the information matrix is represented, so that: + * R_d (approx)= R / sqrt(delta_t) + * */ + return noiseModel::Gaussian::SqrtInformation(model->R()/std::sqrt(delta_t)); + } private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NonlinearFactor2", - boost::serialization::base_object(*this)); - } + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NonlinearFactor2", + boost::serialization::base_object(*this)); + } }; // \class GaussMarkov1stOrderFactor diff --git a/gtsam_unstable/slam/InvDepthFactor3.h b/gtsam_unstable/slam/InvDepthFactor3.h index 3cd87b8d2..29f9d4972 100644 --- a/gtsam_unstable/slam/InvDepthFactor3.h +++ b/gtsam_unstable/slam/InvDepthFactor3.h @@ -96,7 +96,7 @@ public: " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl; return gtsam::ones(2) * 2.0 * K_->fx(); } - return gtsam::Vector_(1, 0.0); + return (gtsam::Vector(1) << 0.0); } /** return the measurement */ diff --git a/gtsam_unstable/slam/InvDepthFactorVariant1.h b/gtsam_unstable/slam/InvDepthFactorVariant1.h index 5e67a39d3..d61787358 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant1.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant1.h @@ -96,7 +96,7 @@ public: << std::endl; return gtsam::ones(2) * 2.0 * K_->fx(); } - return gtsam::Vector_(1, 0.0); + return (gtsam::Vector(1) << 0.0); } /// Evaluate error h(x)-z and optionally derivatives diff --git a/gtsam_unstable/slam/InvDepthFactorVariant2.h b/gtsam_unstable/slam/InvDepthFactorVariant2.h index 2da440425..9d4113431 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant2.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant2.h @@ -99,7 +99,7 @@ public: << std::endl; return gtsam::ones(2) * 2.0 * K_->fx(); } - return gtsam::Vector_(1, 0.0); + return (gtsam::Vector(1) << 0.0); } /// Evaluate error h(x)-z and optionally derivatives diff --git a/gtsam_unstable/slam/InvDepthFactorVariant3.h b/gtsam_unstable/slam/InvDepthFactorVariant3.h index 0d75d5e6b..aef15638f 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant3.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant3.h @@ -99,7 +99,7 @@ public: << std::endl; return gtsam::ones(2) * 2.0 * K_->fx(); } - return gtsam::Vector_(1, 0.0); + return (Vector(1) << 0.0); } /// Evaluate error h(x)-z and optionally derivatives @@ -218,7 +218,7 @@ public: << std::endl; return gtsam::ones(2) * 2.0 * K_->fx(); } - return gtsam::Vector_(1, 0.0); + return (Vector(1) << 0.0); } /// Evaluate error h(x)-z and optionally derivatives diff --git a/gtsam_unstable/slam/JacobianFactorQ.h b/gtsam_unstable/slam/JacobianFactorQ.h new file mode 100644 index 000000000..fdbe0e231 --- /dev/null +++ b/gtsam_unstable/slam/JacobianFactorQ.h @@ -0,0 +1,47 @@ +/* + * @file JacobianFactorQ.h + * @date Oct 27, 2013 + * @uthor Frank Dellaert + */ + +#pragma once + +#include "JacobianSchurFactor.h" + +namespace gtsam { +/** + * JacobianFactor for Schur complement that uses Q noise model + */ +template +class JacobianFactorQ: public JacobianSchurFactor { + + typedef JacobianSchurFactor Base; + +public: + + /// Default constructor + JacobianFactorQ() { + } + + /// Constructor + JacobianFactorQ(const std::vector& Fblocks, + const Matrix& E, const Matrix3& P, const Vector& b, + const SharedDiagonal& model = SharedDiagonal()) : + JacobianSchurFactor() { + size_t j = 0, m2 = E.rows(), m = m2 / 2; + // Calculate projector Q + Matrix Q = eye(m2) - E * P * E.transpose(); + // Calculate pre-computed Jacobian matrices + // TODO: can we do better ? + typedef std::pair KeyMatrix; + std::vector < KeyMatrix > QF; + QF.reserve(m); + // Below, we compute each 2m*D block A_j = Q_j * F_j = (2m*2) * (2*D) + BOOST_FOREACH(const typename Base::KeyMatrix2D& it, Fblocks) + QF.push_back(KeyMatrix(it.first, Q.block(0, 2 * j++, m2, 2) * it.second)); + // Which is then passed to the normal JacobianFactor constructor + JacobianFactor::fillTerms(QF, Q * b, model); + } +}; + +} diff --git a/gtsam_unstable/slam/JacobianFactorQR.h b/gtsam_unstable/slam/JacobianFactorQR.h new file mode 100644 index 000000000..2d2d5b7a4 --- /dev/null +++ b/gtsam_unstable/slam/JacobianFactorQR.h @@ -0,0 +1,53 @@ +/* + * @file JacobianFactorQR.h + * @brief Jacobianfactor that combines and eliminates points + * @date Oct 27, 2013 + * @uthor Frank Dellaert + */ + +#pragma once +#include + +namespace gtsam { +/** + * JacobianFactor for Schur complement that uses Q noise model + */ +template +class JacobianFactorQR: public JacobianSchurFactor { + + typedef JacobianSchurFactor Base; + +public: + + /** + * Constructor + */ + JacobianFactorQR(const std::vector& Fblocks, + const Matrix& E, const Matrix3& P, const Vector& b, + const SharedDiagonal& model = SharedDiagonal()) : + JacobianSchurFactor() { + // Create a number of Jacobian factors in a factor graph + GaussianFactorGraph gfg; + Symbol pointKey('p', 0); + size_t i = 0; + BOOST_FOREACH(const typename Base::KeyMatrix2D& it, Fblocks) { + gfg.add(pointKey, E.block<2, 3>(2 * i, 0), it.first, it.second, + b.segment<2>(2 * i), model); + i += 1; + } + //gfg.print("gfg"); + + // eliminate the point + GaussianBayesNet::shared_ptr bn; + GaussianFactorGraph::shared_ptr fg; + std::vector < Key > variables; + variables.push_back(pointKey); + boost::tie(bn, fg) = gfg.eliminatePartialSequential(variables, EliminateQR); + //fg->print("fg"); + + JacobianFactor::operator=(JacobianFactor(*fg)); + } +}; +// class + +}// gtsam diff --git a/gtsam_unstable/slam/JacobianFactorSVD.h b/gtsam_unstable/slam/JacobianFactorSVD.h new file mode 100644 index 000000000..752a9f48e --- /dev/null +++ b/gtsam_unstable/slam/JacobianFactorSVD.h @@ -0,0 +1,44 @@ +/* + * @file JacobianFactorSVD.h + * @date Oct 27, 2013 + * @uthor Frank Dellaert + */ + +#pragma once +#include "gtsam_unstable/slam/JacobianSchurFactor.h" + +namespace gtsam { +/** + * JacobianFactor for Schur complement that uses Q noise model + */ +template +class JacobianFactorSVD: public JacobianSchurFactor { + +public: + + typedef Eigen::Matrix Matrix2D; + typedef std::pair KeyMatrix2D; + + /// Default constructor + JacobianFactorSVD() {} + + /// Constructor + JacobianFactorSVD(const std::vector& Fblocks, const Matrix& Enull, const Vector& b, + const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { + size_t numKeys = Enull.rows() / 2; + size_t j = 0, m2 = 2*numKeys-3; + // PLAIN NULL SPACE TRICK + // Matrix Q = Enull * Enull.transpose(); + // BOOST_FOREACH(const KeyMatrix2D& it, Fblocks) + // QF.push_back(KeyMatrix(it.first, Q.block(0, 2 * j++, m2, 2) * it.second)); + // JacobianFactor factor(QF, Q * b); + typedef std::pair KeyMatrix; + std::vector QF; + QF.reserve(numKeys); + BOOST_FOREACH(const KeyMatrix2D& it, Fblocks) + QF.push_back(KeyMatrix(it.first, (Enull.transpose()).block(0, 2 * j++, m2, 2) * it.second)); + JacobianFactor::fillTerms(QF, Enull.transpose() * b, model); + } +}; + +} diff --git a/gtsam_unstable/slam/JacobianSchurFactor.h b/gtsam_unstable/slam/JacobianSchurFactor.h new file mode 100644 index 000000000..2beecc264 --- /dev/null +++ b/gtsam_unstable/slam/JacobianSchurFactor.h @@ -0,0 +1,93 @@ +/* + * @file JacobianSchurFactor.h + * @brief Jacobianfactor that combines and eliminates points + * @date Oct 27, 2013 + * @uthor Frank Dellaert + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace gtsam { +/** + * JacobianFactor for Schur complement that uses Q noise model + */ +template +class JacobianSchurFactor: public JacobianFactor { + +public: + + typedef Eigen::Matrix Matrix2D; + typedef std::pair KeyMatrix2D; + + // Use eigen magic to access raw memory + typedef Eigen::Matrix DVector; + typedef Eigen::Map DMap; + typedef Eigen::Map ConstDMap; + + /** + * @brief double* Matrix-vector multiply, i.e. y = A*x + * RAW memory access! Assumes keys start at 0 and go to M-1, and x is laid out that way + */ + Vector operator*(const double* x) const { + Vector Ax = zero(Ab_.rows()); + if (empty()) return Ax; + + // Just iterate over all A matrices and multiply in correct config part + for(size_t pos=0; poswhiten(Ax) : Ax; + } + + /** + * @brief double* Transpose Matrix-vector multiply, i.e. x += A'*e + * RAW memory access! Assumes keys start at 0 and go to M-1, and y is laid out that way + */ + void transposeMultiplyAdd(double alpha, const Vector& e, double* x) const + { + Vector E = alpha * (model_ ? model_->whiten(e) : e); + // Just iterate over all A matrices and insert Ai^e into y + for(size_t pos=0; postransposeMultiplyAdd(alpha,Ax,y); + if (empty()) return; + Vector Ax = zero(Ab_.rows()); + + // Just iterate over all A matrices and multiply in correct config part + for(size_t pos=0; poswhitenInPlace(Ax); model_->whitenInPlace(Ax); } + + // multiply with alpha + Ax *= alpha; + + // Again iterate over all A matrices and insert Ai^e into y + for(size_t pos=0; pos& U, const std::list& F, const double g_e, bool flat) { - Matrix Umat = Matrix_(3,U.size(), concatVectors(U)); - Matrix Fmat = Matrix_(3,F.size(), concatVectors(F)); + Matrix Umat = (Matrix(3, U.size()) << concatVectors(U)); + Matrix Fmat = (Matrix(3, F.size()) << concatVectors(F)); return initialize(Umat, Fmat, g_e, flat); } @@ -33,14 +33,14 @@ Mechanization_bRn2 Mechanization_bRn2::initialize(const Matrix& U, if(g_e == 0) { if (flat) // acceleration measured is along the z-axis. - b_g = Vector_(3, 0.0, 0.0, norm_2(meanF)); + b_g = (Vector(3) << 0.0, 0.0, norm_2(meanF)); else // acceleration measured is the opposite of gravity (10.13) b_g = -meanF; } else { if (flat) // gravity is downward along the z-axis since we are flat on the ground - b_g = Vector_(3,0.0,0.0,g_e); + b_g = (Vector(3) << 0.0,0.0,g_e); else // normalize b_g and attribute remainder to biases b_g = - g_e * meanF/meanF.norm(); diff --git a/gtsam_unstable/slam/Mechanization_bRn2.h b/gtsam_unstable/slam/Mechanization_bRn2.h index 711a44bf9..76e4e9edd 100644 --- a/gtsam_unstable/slam/Mechanization_bRn2.h +++ b/gtsam_unstable/slam/Mechanization_bRn2.h @@ -34,14 +34,20 @@ public: bRn_(initial_bRn), x_g_(initial_x_g), x_a_(initial_x_a) { } - Vector b_g(double g_e) { - Vector n_g = Vector_(3, 0.0, 0.0, g_e); + /// Copy constructor + Mechanization_bRn2(const Mechanization_bRn2& other) : + bRn_(other.bRn_), x_g_(other.x_g_), x_a_(other.x_a_) { + } + + /// gravity in the body frame + Vector b_g(double g_e) const { + Vector n_g = (Vector(3) << 0, 0, g_e); return (bRn_ * n_g).vector(); } - Rot3 bRn() const {return bRn_; } - Vector x_g() const { return x_g_; } - Vector x_a() const { return x_a_; } + const Rot3& bRn() const {return bRn_; } + const Vector& x_g() const { return x_g_; } + const Vector& x_a() const { return x_a_; } /** * Initialize the first Mechanization state diff --git a/gtsam_unstable/slam/PartialPriorFactor.h b/gtsam_unstable/slam/PartialPriorFactor.h index bf0b2b6e3..1925a3fe4 100644 --- a/gtsam_unstable/slam/PartialPriorFactor.h +++ b/gtsam_unstable/slam/PartialPriorFactor.h @@ -70,7 +70,7 @@ namespace gtsam { /** Single Element Constructor: acts on a single parameter specified by idx */ PartialPriorFactor(Key key, size_t idx, double prior, const SharedNoiseModel& model) : - Base(model, key), prior_(Vector_(1, prior)), mask_(1, idx), H_(zeros(1, T::Dim())) { + Base(model, key), prior_((Vector(1) << prior)), mask_(1, idx), H_(zeros(1, T::Dim())) { assert(model->dim() == 1); this->fillH(); } diff --git a/gtsam_unstable/slam/RegularHessianFactor.h b/gtsam_unstable/slam/RegularHessianFactor.h new file mode 100644 index 000000000..0e20bb709 --- /dev/null +++ b/gtsam_unstable/slam/RegularHessianFactor.h @@ -0,0 +1,101 @@ +/* ---------------------------------------------------------------------------- + + * 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 RegularHessianFactor.h + * @brief HessianFactor class with constant sized blcoks + * @author Richard Roberts + * @date Dec 8, 2010 + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + +template +class RegularHessianFactor: public HessianFactor { + +private: + + typedef Eigen::Matrix MatrixDD; // camera hessian block + typedef Eigen::Matrix VectorD; + +public: + + /** Construct an n-way factor. Gs contains the upper-triangle blocks of the + * quadratic term (the Hessian matrix) provided in row-order, gs the pieces + * of the linear vector term, and f the constant term. + */ + RegularHessianFactor(const std::vector& js, + const std::vector& Gs, const std::vector& gs, double f) : + HessianFactor(js, Gs, gs, f) { + } + + /** Constructor with an arbitrary number of keys and with the augmented information matrix + * specified as a block matrix. */ + template + RegularHessianFactor(const KEYS& keys, + const SymmetricBlockMatrix& augmentedInformation) : + HessianFactor(keys, augmentedInformation) { + } + + /** y += alpha * A'*A*x */ + void multiplyHessianAdd(double alpha, const VectorValues& x, + VectorValues& y) const { + HessianFactor::multiplyHessianAdd(alpha, x, y); + } + + // Scratch space for multiplyHessianAdd + typedef Eigen::Matrix DVector; + mutable std::vector y; + + void multiplyHessianAdd(double alpha, const double* x, + double* yvalues) const { + // Create a vector of temporary y values, corresponding to rows i + y.resize(size()); + BOOST_FOREACH(DVector & yi, y) + yi.setZero(); + + typedef Eigen::Map DMap; + typedef Eigen::Map ConstDMap; + + // Accessing the VectorValues one by one is expensive + // So we will loop over columns to access x only once per column + // And fill the above temporary y values, to be added into yvalues after + DVector xj(D); + for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { + Key key = keys_[j]; + const double* xj = x + key * D; + DenseIndex i = 0; + for (; i < j; ++i) + y[i] += info_(i, j).knownOffDiagonal() * ConstDMap(xj); + // blocks on the diagonal are only half + y[i] += info_(j, j).selfadjointView() * ConstDMap(xj); + // for below diagonal, we take transpose block from upper triangular part + for (i = j + 1; i < (DenseIndex) size(); ++i) + y[i] += info_(i, j).knownOffDiagonal() * ConstDMap(xj); + } + + // copy to yvalues + for (DenseIndex i = 0; i < (DenseIndex) size(); ++i) { + Key key = keys_[i]; + DMap(yvalues + key * D) += alpha * y[i]; + } + } + +}; + +} + diff --git a/gtsam_unstable/slam/RelativeElevationFactor.cpp b/gtsam_unstable/slam/RelativeElevationFactor.cpp index 11d8ed3a3..292e3f68f 100644 --- a/gtsam_unstable/slam/RelativeElevationFactor.cpp +++ b/gtsam_unstable/slam/RelativeElevationFactor.cpp @@ -32,7 +32,7 @@ Vector RelativeElevationFactor::evaluateError(const Pose3& pose, const Point3& p *H2 = zeros(1, 3); (*H2)(0, 2) = -1.0; } - return Vector_(1, hx - measured_); + return (Vector(1) << hx - measured_); } /* ************************************************************************* */ diff --git a/gtsam_unstable/slam/SmartFactorBase.h b/gtsam_unstable/slam/SmartFactorBase.h new file mode 100644 index 000000000..d24a878bb --- /dev/null +++ b/gtsam_unstable/slam/SmartFactorBase.h @@ -0,0 +1,644 @@ +/* ---------------------------------------------------------------------------- + + * 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 SmartFactorBase.h + * @brief Base class to create smart factors on poses or cameras + * @author Luca Carlone + * @author Zsolt Kira + * @author Frank Dellaert + */ + +#pragma once + +#include "JacobianFactorQ.h" +#include "ImplicitSchurFactor.h" +#include "RegularHessianFactor.h" + +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace gtsam { +/// Base class with no internal point, completely functional +template +class SmartFactorBase: public NonlinearFactor { +protected: + + // Keep a copy of measurement and calibration for I/O + std::vector measured_; ///< 2D measurement for each of the m views + std::vector noise_; ///< noise model used + ///< (important that the order is the same as the keys that we use to create the factor) + + boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame (one for all cameras) + + /// Definitions for blocks of F + typedef Eigen::Matrix Matrix2D; // F + typedef Eigen::Matrix MatrixD2; // F' + typedef std::pair KeyMatrix2D; // Fblocks + typedef Eigen::Matrix MatrixDD; // camera hessian block + typedef Eigen::Matrix Matrix23; + typedef Eigen::Matrix VectorD; + typedef Eigen::Matrix Matrix2; + + /// shorthand for base class type + typedef NonlinearFactor Base; + + /// shorthand for this class + typedef SmartFactorBase This; + +public: + + /// shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; + + /// shorthand for a pinhole camera + typedef PinholeCamera Camera; + typedef std::vector Cameras; + + /** + * Constructor + * @param body_P_sensor is the transform from body to sensor frame (default identity) + */ + SmartFactorBase(boost::optional body_P_sensor = boost::none) : + body_P_sensor_(body_P_sensor) { + } + + /** Virtual destructor */ + virtual ~SmartFactorBase() { + } + + /** + * add a new measurement and pose key + * @param measured_i is the 2m dimensional projection of a single landmark + * @param poseKey is the index corresponding to the camera observing the landmark + * @param noise_i is the measurement noise + */ + void add(const Point2& measured_i, const Key& poseKey_i, + const SharedNoiseModel& noise_i) { + this->measured_.push_back(measured_i); + this->keys_.push_back(poseKey_i); + this->noise_.push_back(noise_i); + } + + /** + * variant of the previous add: adds a bunch of measurements, together with the camera keys and noises + */ + // **************************************************************************************************** + void add(std::vector& measurements, std::vector& poseKeys, + std::vector& noises) { + for (size_t i = 0; i < measurements.size(); i++) { + this->measured_.push_back(measurements.at(i)); + this->keys_.push_back(poseKeys.at(i)); + this->noise_.push_back(noises.at(i)); + } + } + + /** + * variant of the previous add: adds a bunch of measurements and uses the same noise model for all of them + */ + // **************************************************************************************************** + void add(std::vector& measurements, std::vector& poseKeys, + const SharedNoiseModel& noise) { + for (size_t i = 0; i < measurements.size(); i++) { + this->measured_.push_back(measurements.at(i)); + this->keys_.push_back(poseKeys.at(i)); + this->noise_.push_back(noise); + } + } + + /** + * Adds an entire SfM_track (collection of cameras observing a single point). + * The noise is assumed to be the same for all measurements + */ + // **************************************************************************************************** + void add(const SfM_Track& trackToAdd, const SharedNoiseModel& noise) { + for (size_t k = 0; k < trackToAdd.number_measurements(); k++) { + this->measured_.push_back(trackToAdd.measurements[k].second); + this->keys_.push_back(trackToAdd.measurements[k].first); + this->noise_.push_back(noise); + } + } + + /** return the measurements */ + const Vector& measured() const { + return measured_; + } + + /** return the noise model */ + const SharedNoiseModel& noise() const { + return noise_; + } + + /** + * print + * @param s optional string naming the factor + * @param keyFormatter optional formatter useful for printing Symbols + */ + void print(const std::string& s = "", const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const { + std::cout << s << "SmartFactorBase, z = \n"; + for (size_t k = 0; k < measured_.size(); ++k) { + std::cout << "measurement, p = " << measured_[k] << "\t"; + noise_[k]->print("noise model = "); + } + if (this->body_P_sensor_) + this->body_P_sensor_->print(" sensor pose in body frame: "); + Base::print("", keyFormatter); + } + + /// equals + virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { + const This *e = dynamic_cast(&p); + + bool areMeasurementsEqual = true; + for (size_t i = 0; i < measured_.size(); i++) { + if (this->measured_.at(i).equals(e->measured_.at(i), tol) == false) + areMeasurementsEqual = false; + break; + } + return e && Base::equals(p, tol) && areMeasurementsEqual + && ((!body_P_sensor_ && !e->body_P_sensor_) + || (body_P_sensor_ && e->body_P_sensor_ + && body_P_sensor_->equals(*e->body_P_sensor_))); + } + + // **************************************************************************************************** + /// Calculate vector of re-projection errors, before applying noise model + Vector reprojectionError(const Cameras& cameras, const Point3& point) const { + + Vector b = zero(2 * cameras.size()); + + size_t i = 0; + BOOST_FOREACH(const Camera& camera, cameras) { + const Point2& zi = this->measured_.at(i); + try { + Point2 e(camera.project(point) - zi); + b[2 * i] = e.x(); + b[2 * i + 1] = e.y(); + } catch (CheiralityException& e) { + std::cout << "Cheirality exception " << std::endl; + exit (EXIT_FAILURE); + } + i += 1; + } + + return b; + } + + // **************************************************************************************************** + /** + * Calculate the error of the factor. + * This is the log-likelihood, e.g. \f$ 0.5(h(x)-z)^2/\sigma^2 \f$ in case of Gaussian. + * In this class, we take the raw prediction error \f$ h(x)-z \f$, ask the noise model + * to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5. + * This is different from reprojectionError(cameras,point) as each point is whitened + */ + double totalReprojectionError(const Cameras& cameras, + const Point3& point) const { + + double overallError = 0; + + size_t i = 0; + BOOST_FOREACH(const Camera& camera, cameras) { + const Point2& zi = this->measured_.at(i); + try { + Point2 reprojectionError(camera.project(point) - zi); + overallError += 0.5 + * this->noise_.at(i)->distance(reprojectionError.vector()); + } catch (CheiralityException&) { + std::cout << "Cheirality exception " << std::endl; + exit (EXIT_FAILURE); + } + i += 1; + } + return overallError; + } + + // **************************************************************************************************** + /// Assumes non-degenerate ! + void computeEP(Matrix& E, Matrix& PointCov, const Cameras& cameras, + const Point3& point) const { + + int numKeys = this->keys_.size(); + E = zeros(2 * numKeys, 3); + Vector b = zero(2 * numKeys); + + Matrix Ei(2, 3); + for (size_t i = 0; i < this->measured_.size(); i++) { + try { + cameras[i].project(point, boost::none, Ei); + } catch (CheiralityException& e) { + std::cout << "Cheirality exception " << std::endl; + exit (EXIT_FAILURE); + } + this->noise_.at(i)->WhitenSystem(Ei, b); + E.block<2, 3>(2 * i, 0) = Ei; + } + + // Matrix PointCov; + PointCov.noalias() = (E.transpose() * E).inverse(); + } + + // **************************************************************************************************** + /// Compute F, E only (called below in both vanilla and SVD versions) + /// Given a Point3, assumes dimensionality is 3 + double computeJacobians(std::vector& Fblocks, Matrix& E, + Vector& b, const Cameras& cameras, const Point3& point) const { + + size_t numKeys = this->keys_.size(); + E = zeros(2 * numKeys, 3); + b = zero(2 * numKeys); + double f = 0; + + Matrix Fi(2, 6), Ei(2, 3), Hcali(2, D - 6), Hcam(2, D); + for (size_t i = 0; i < this->measured_.size(); i++) { + + Vector bi; + try { + bi = + -(cameras[i].project(point, Fi, Ei, Hcali) - this->measured_.at(i)).vector(); + } catch (CheiralityException&) { + std::cout << "Cheirality exception " << std::endl; + exit (EXIT_FAILURE); + } + this->noise_.at(i)->WhitenSystem(Fi, Ei, Hcali, bi); + + f += bi.squaredNorm(); + if (D == 6) { // optimize only camera pose + Fblocks.push_back(KeyMatrix2D(this->keys_[i], Fi)); + } else { + Hcam.block<2, 6>(0, 0) = Fi; // 2 x 6 block for the cameras + Hcam.block<2, D - 6>(0, 6) = Hcali; // 2 x nrCal block for the cameras + Fblocks.push_back(KeyMatrix2D(this->keys_[i], Hcam)); + } + E.block<2, 3>(2 * i, 0) = Ei; + subInsert(b, bi, 2 * i); + } + return f; + } + + // **************************************************************************************************** + /// Version that computes PointCov, with optional lambda parameter + double computeJacobians(std::vector& Fblocks, Matrix& E, + Matrix3& PointCov, Vector& b, const Cameras& cameras, const Point3& point, + double lambda = 0.0, bool diagonalDamping = false) const { + + double f = computeJacobians(Fblocks, E, b, cameras, point); + + // Point covariance inv(E'*E) + Matrix3 EtE = E.transpose() * E; + Matrix3 DMatrix = eye(E.cols()); // damping matrix + + if (diagonalDamping) { // diagonal of the hessian + DMatrix(0, 0) = EtE(0, 0); + DMatrix(1, 1) = EtE(1, 1); + DMatrix(2, 2) = EtE(2, 2); + } + + PointCov.noalias() = (EtE + lambda * DMatrix).inverse(); + + return f; + } + + // **************************************************************************************************** + // TODO, there should not be a Matrix version, really + double computeJacobians(Matrix& F, Matrix& E, Matrix3& PointCov, Vector& b, + const Cameras& cameras, const Point3& point, + const double lambda = 0.0) const { + + size_t numKeys = this->keys_.size(); + std::vector < KeyMatrix2D > Fblocks; + double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, + lambda); + F = zeros(2 * numKeys, D * numKeys); + + for (size_t i = 0; i < this->keys_.size(); ++i) { + F.block<2, D>(2 * i, D * i) = Fblocks.at(i).second; // 2 x 6 block for the cameras + } + return f; + } + + // **************************************************************************************************** + /// SVD version + double computeJacobiansSVD(std::vector& Fblocks, Matrix& Enull, + Vector& b, const Cameras& cameras, const Point3& point, double lambda = + 0.0, bool diagonalDamping = false) const { + + Matrix E; + Matrix3 PointCov; // useless + double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, + diagonalDamping); // diagonalDamping should have no effect (only on PointCov) + + // Do SVD on A + Eigen::JacobiSVD < Matrix > svd(E, Eigen::ComputeFullU); + Vector s = svd.singularValues(); + // Enull = zeros(2 * numKeys, 2 * numKeys - 3); + int numKeys = this->keys_.size(); + Enull = svd.matrixU().block(0, 3, 2 * numKeys, 2 * numKeys - 3); // last 2m-3 columns + + return f; + } + + // **************************************************************************************************** + /// Matrix version of SVD + // TODO, there should not be a Matrix version, really + double computeJacobiansSVD(Matrix& F, Matrix& Enull, Vector& b, + const Cameras& cameras, const Point3& point) const { + + int numKeys = this->keys_.size(); + std::vector < KeyMatrix2D > Fblocks; + double f = computeJacobiansSVD(Fblocks, Enull, b, cameras, point); + F.resize(2 * numKeys, D * numKeys); + F.setZero(); + + for (size_t i = 0; i < this->keys_.size(); ++i) + F.block<2, D>(2 * i, D * i) = Fblocks.at(i).second; // 2 x 6 block for the cameras + + return f; + } + + // **************************************************************************************************** + /// linearize returns a Hessianfactor that is an approximation of error(p) + boost::shared_ptr > createHessianFactor( + const Cameras& cameras, const Point3& point, const double lambda = 0.0, + bool diagonalDamping = false) const { + + int numKeys = this->keys_.size(); + + std::vector < KeyMatrix2D > Fblocks; + Matrix E; + Matrix3 PointCov; + Vector b; + double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, + diagonalDamping); + +//#define HESSIAN_BLOCKS +#ifdef HESSIAN_BLOCKS + // Create structures for Hessian Factors + std::vector < Matrix > Gs(numKeys * (numKeys + 1) / 2); + std::vector < Vector > gs(numKeys); + + sparseSchurComplement(Fblocks, E, PointCov, b, Gs, gs); + // schurComplement(Fblocks, E, PointCov, b, Gs, gs); + + //std::vector < Matrix > Gs2(Gs.begin(), Gs.end()); + //std::vector < Vector > gs2(gs.begin(), gs.end()); + + return boost::make_shared < RegularHessianFactor + > (this->keys_, Gs, gs, f); +#else + size_t n1 = D * numKeys + 1; + std::vector dims(numKeys + 1); // this also includes the b term + std::fill(dims.begin(), dims.end() - 1, D); + dims.back() = 1; + + SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(n1, n1)); // for 10 cameras, size should be (10*D+1 x 10*D+1) + sparseSchurComplement(Fblocks, E, PointCov, b, augmentedHessian); // augmentedHessian.matrix().block (i1,i2) = ... + augmentedHessian(numKeys,numKeys)(0,0) = f; + return boost::make_shared >( + this->keys_, augmentedHessian); + +#endif + } + + // **************************************************************************************************** + boost::shared_ptr > updateAugmentedHessian( + const Cameras& cameras, const Point3& point, const double lambda, + bool diagonalDamping, SymmetricBlockMatrix& augmentedHessian) const { + + int numKeys = this->keys_.size(); + + std::vector < KeyMatrix2D > Fblocks; + Matrix E; + Matrix3 PointCov; + Vector b; + double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, + diagonalDamping); + + std::vector dims(numKeys + 1); // this also includes the b term + std::fill(dims.begin(), dims.end() - 1, D); + dims.back() = 1; + + updateSparseSchurComplement(Fblocks, E, PointCov, b, augmentedHessian); // augmentedHessian.matrix().block (i1,i2) = ... + std::cout << "f "<< f <& Fblocks, const Matrix& E, + const Matrix& PointCov, const Vector& b, + /*output ->*/std::vector& Gs, std::vector& gs) const { + // Schur complement trick + // Gs = F' * F - F' * E * inv(E'*E) * E' * F + // gs = F' * (b - E * inv(E'*E) * E' * b) + // This version uses full matrices + + int numKeys = this->keys_.size(); + + /// Compute Full F ???? + Matrix F = zeros(2 * numKeys, D * numKeys); + for (size_t i = 0; i < this->keys_.size(); ++i) + F.block<2, D>(2 * i, D * i) = Fblocks.at(i).second; // 2 x 6 block for the cameras + + Matrix H(D * numKeys, D * numKeys); + Vector gs_vector; + + H.noalias() = F.transpose() * (F - (E * (PointCov * (E.transpose() * F)))); + gs_vector.noalias() = F.transpose() + * (b - (E * (PointCov * (E.transpose() * b)))); + + // Populate Gs and gs + int GsCount2 = 0; + for (DenseIndex i1 = 0; i1 < numKeys; i1++) { // for each camera + DenseIndex i1D = i1 * D; + gs.at(i1) = gs_vector.segment < D > (i1D); + for (DenseIndex i2 = 0; i2 < numKeys; i2++) { + if (i2 >= i1) { + Gs.at(GsCount2) = H.block(i1D, i2 * D); + GsCount2++; + } + } + } + } + + // **************************************************************************************************** + void updateSparseSchurComplement(const std::vector& Fblocks, + const Matrix& E, const Matrix& P /*Point Covariance*/, const Vector& b, + /*output ->*/SymmetricBlockMatrix& augmentedHessian) const { + // Schur complement trick + // Gs = F' * F - F' * E * P * E' * F + // gs = F' * (b - E * P * E' * b) + + // a single point is observed in numKeys cameras + size_t numKeys = this->keys_.size(); // cameras observing current point + size_t aug_numKeys = augmentedHessian.rows() - 1; // all cameras in the group + + // Blockwise Schur complement + for (size_t i1 = 0; i1 < numKeys; i1++) { // for each camera + + const Matrix2D& Fi1 = Fblocks.at(i1).second; + const Matrix23 Ei1_P = E.block<2, 3>(2 * i1, 0) * P; + + // D = (Dx2) * (2) + // (augmentedHessian.matrix()).block (i1,numKeys+1) = Fi1.transpose() * b.segment < 2 > (2 * i1); // F' * b + size_t aug_i1 = this->keys_[i1]; + std::cout << "i1 "<< i1 < (2 * i1) // F' * b + - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (Dx2) * (2x3) * (3*2m) * (2m x 1) + + // (DxD) = (Dx2) * ( (2xD) - (2x3) * (3x2) * (2xD) ) + std::cout << "filled 1 " <(2 * i1, 0).transpose() * Fi1); + + // upper triangular part of the hessian + for (size_t i2 = i1+1; i2 < numKeys; i2++) { // for each camera + const Matrix2D& Fi2 = Fblocks.at(i2).second; + size_t aug_i2 = this->keys_[i2]; + std::cout << "i2 "<< i2 <(2 * i2, 0).transpose() * Fi2); + } + } // end of for over cameras + } + + // **************************************************************************************************** + void sparseSchurComplement(const std::vector& Fblocks, + const Matrix& E, const Matrix& P /*Point Covariance*/, const Vector& b, + /*output ->*/SymmetricBlockMatrix& augmentedHessian) const { + // Schur complement trick + // Gs = F' * F - F' * E * P * E' * F + // gs = F' * (b - E * P * E' * b) + + // a single point is observed in numKeys cameras + size_t numKeys = this->keys_.size(); + + // Blockwise Schur complement + for (size_t i1 = 0; i1 < numKeys; i1++) { // for each camera + + const Matrix2D& Fi1 = Fblocks.at(i1).second; + const Matrix23 Ei1_P = E.block<2, 3>(2 * i1, 0) * P; + + // D = (Dx2) * (2) + // (augmentedHessian.matrix()).block (i1,numKeys+1) = Fi1.transpose() * b.segment < 2 > (2 * i1); // F' * b + augmentedHessian(i1,numKeys) = Fi1.transpose() * b.segment < 2 > (2 * i1) // F' * b + - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (Dx2) * (2x3) * (3*2m) * (2m x 1) + + // (DxD) = (Dx2) * ( (2xD) - (2x3) * (3x2) * (2xD) ) + augmentedHessian(i1,i1) = + Fi1.transpose() * (Fi1 - Ei1_P * E.block<2, 3>(2 * i1, 0).transpose() * Fi1); + + // upper triangular part of the hessian + for (size_t i2 = i1+1; i2 < numKeys; i2++) { // for each camera + const Matrix2D& Fi2 = Fblocks.at(i2).second; + + // (DxD) = (Dx2) * ( (2x2) * (2xD) ) + augmentedHessian(i1,i2) = + -Fi1.transpose() * (Ei1_P * E.block<2, 3>(2 * i2, 0).transpose() * Fi2); + } + } // end of for over cameras + } + + // **************************************************************************************************** + void sparseSchurComplement(const std::vector& Fblocks, + const Matrix& E, const Matrix& P /*Point Covariance*/, const Vector& b, + /*output ->*/std::vector& Gs, std::vector& gs) const { + // Schur complement trick + // Gs = F' * F - F' * E * P * E' * F + // gs = F' * (b - E * P * E' * b) + + // a single point is observed in numKeys cameras + size_t numKeys = this->keys_.size(); + + int GsIndex = 0; + // Blockwise Schur complement + for (size_t i1 = 0; i1 < numKeys; i1++) { // for each camera + // GsIndex points to the upper triangular blocks + // 0 1 2 3 4 + // X 5 6 7 8 + // X X 9 10 11 + // X X X 12 13 + // X X X X 14 + const Matrix2D& Fi1 = Fblocks.at(i1).second; + + const Matrix23 Ei1_P = E.block<2, 3>(2 * i1, 0) * P; + + { // for i1 = i2 + // D = (Dx2) * (2) + gs.at(i1) = Fi1.transpose() * b.segment < 2 > (2 * i1); // F' * b + // D = (Dx2) * (2x3) * (3*2m) * (2m x 1) + gs.at(i1) -= Fi1.transpose() * (Ei1_P * (E.transpose() * b)); + + // (DxD) = (Dx2) * ( (2xD) - (2x3) * (3x2) * (2xD) ) + Gs.at(GsIndex) = Fi1.transpose() * (Fi1 - Ei1_P * E.block<2, 3>(2 * i1, 0).transpose() * Fi1); + GsIndex++; + } + // upper triangular part of the hessian + for (size_t i2 = i1+1; i2 < numKeys; i2++) { // for each camera + const Matrix2D& Fi2 = Fblocks.at(i2).second; + + // (DxD) = (Dx2) * ( (2x2) * (2xD) ) + Gs.at(GsIndex) = -Fi1.transpose() * (Ei1_P * E.block<2, 3>(2 * i2, 0).transpose() * Fi2); + GsIndex++; + } + } // end of for over cameras + } + // **************************************************************************************************** + boost::shared_ptr > createImplicitSchurFactor( + const Cameras& cameras, const Point3& point, double lambda = 0.0, + bool diagonalDamping = false) const { + typename boost::shared_ptr > f( + new ImplicitSchurFactor()); + computeJacobians(f->Fblocks(), f->E(), f->PointCovariance(), f->b(), + cameras, point, lambda, diagonalDamping); + f->initKeys(); + return f; + } + + // **************************************************************************************************** + boost::shared_ptr > createJacobianQFactor( + const Cameras& cameras, const Point3& point, double lambda = 0.0, + bool diagonalDamping = false) const { + std::vector < KeyMatrix2D > Fblocks; + Matrix E; + Matrix3 PointCov; + Vector b; + computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, + diagonalDamping); + return boost::make_shared < JacobianFactorQ > (Fblocks, E, PointCov, b); + } + +private: + + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar & BOOST_SERIALIZATION_NVP(measured_); + ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); + } +}; + +} // \ namespace gtsam diff --git a/gtsam_unstable/slam/SmartProjectionFactor.h b/gtsam_unstable/slam/SmartProjectionFactor.h index 5572f04e8..59a841813 100644 --- a/gtsam_unstable/slam/SmartProjectionFactor.h +++ b/gtsam_unstable/slam/SmartProjectionFactor.h @@ -1,471 +1,663 @@ /* ---------------------------------------------------------------------------- - + * 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 ProjectionFactor.h - * @brief Basic bearing factor from 2D measurement - * @author Chris Beall + * @file SmartProjectionFactor.h + * @brief Base class to create smart factors on poses or cameras * @author Luca Carlone * @author Zsolt Kira + * @author Frank Dellaert */ #pragma once -#include -#include -#include -#include +#include "SmartFactorBase.h" + #include +#include +#include +#include +#include + #include -#include +#include +#include namespace gtsam { +/** + * Structure for storing some state memory, used to speed up optimization + * @addtogroup SLAM + */ +class SmartProjectionFactorState { + +protected: + +public: + + SmartProjectionFactorState() { + } + // Hessian representation (after Schur complement) + bool calculatedHessian; + Matrix H; + Vector gs_vector; + std::vector Gs; + std::vector gs; + double f; +}; + +/** + * SmartProjectionFactor: triangulates point + * TODO: why LANDMARK parameter? + */ +template +class SmartProjectionFactor: public SmartFactorBase { +protected: + + // Some triangulation parameters + const double rankTolerance_; ///< threshold to decide whether triangulation is degenerate_ + const double retriangulationThreshold_; ///< threshold to decide whether to re-triangulate + mutable std::vector cameraPosesTriangulation_; ///< current triangulation poses + + const bool manageDegeneracy_; ///< if set to true will use the rotation-only version for degenerate cases + + const bool enableEPI_; ///< if set to true, will refine triangulation using LM + + const double linearizationThreshold_; ///< threshold to decide whether to re-linearize + mutable std::vector cameraPosesLinearization_; ///< current linearization poses + + mutable Point3 point_; ///< Current estimate of the 3D point + + mutable bool degenerate_; + mutable bool cheiralityException_; + + // verbosity handling for Cheirality Exceptions + const bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false) + const bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false) + + boost::shared_ptr state_; + + /// shorthand for smart projection factor state variable + typedef boost::shared_ptr SmartFactorStatePtr; + + /// shorthand for base class type + typedef SmartFactorBase Base; + + /// shorthand for this class + typedef SmartProjectionFactor This; + +public: + + /// shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; + + /// shorthand for a pinhole camera + typedef PinholeCamera Camera; + typedef std::vector Cameras; + /** - * The calibration is known here. - * @addtogroup SLAM + * Constructor + * @param rankTol tolerance used to check if point triangulation is degenerate + * @param linThreshold threshold on relative pose changes used to decide whether to relinearize (selective relinearization) + * @param manageDegeneracy is true, in presence of degenerate triangulation, the factor is converted to a rotation-only constraint, + * otherwise the factor is simply neglected + * @param enableEPI if set to true linear triangulation is refined with embedded LM iterations + * @param body_P_sensor is the transform from body to sensor frame (default identity) */ - template - class SmartProjectionFactor: public NonlinearFactor { - protected: + SmartProjectionFactor(const double rankTol, const double linThreshold, + const bool manageDegeneracy, const bool enableEPI, + boost::optional body_P_sensor = boost::none, + SmartFactorStatePtr state = SmartFactorStatePtr( + new SmartProjectionFactorState())) : + Base(body_P_sensor), rankTolerance_(rankTol), retriangulationThreshold_( + 1e-5), manageDegeneracy_(manageDegeneracy), enableEPI_(enableEPI), linearizationThreshold_( + linThreshold), degenerate_(false), cheiralityException_(false), throwCheirality_( + false), verboseCheirality_(false), state_(state) { + } - // Keep a copy of measurement and calibration for I/O - std::vector measured_; ///< 2D measurement for each of the m views - ///< (important that the order is the same as the keys that we use to create the factor) - boost::shared_ptr K_; ///< shared pointer to calibration object - const SharedNoiseModel noise_; ///< noise model used - boost::optional point_; - boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame + /** Virtual destructor */ + virtual ~SmartProjectionFactor() { + } - // verbosity handling for Cheirality Exceptions - bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false) - bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false) + /** + * print + * @param s optional string naming the factor + * @param keyFormatter optional formatter useful for printing Symbols + */ + void print(const std::string& s = "", const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const { + std::cout << s << "SmartProjectionFactor, z = \n"; + std::cout << "rankTolerance_ = " << rankTolerance_ << std::endl; + std::cout << "degenerate_ = " << degenerate_ << std::endl; + std::cout << "cheiralityException_ = " << cheiralityException_ << std::endl; + Base::print("", keyFormatter); + } - public: + /// Check if the new linearization point_ is the same as the one used for previous triangulation + bool decideIfTriangulate(const Cameras& cameras) const { + // several calls to linearize will be done from the same linearization point_, hence it is not needed to re-triangulate + // Note that this is not yet "selecting linearization", that will come later, and we only check if the + // current linearization is the "same" (up to tolerance) w.r.t. the last time we triangulated the point_ - /// shorthand for base class type - typedef NonlinearFactor Base; + size_t m = cameras.size(); - /// shorthand for this class - typedef SmartProjectionFactor This; + bool retriangulate = false; - /// shorthand for a smart pointer to a factor - typedef boost::shared_ptr shared_ptr; + // if we do not have a previous linearization point_ or the new linearization point_ includes more poses + if (cameraPosesTriangulation_.empty() + || cameras.size() != cameraPosesTriangulation_.size()) + retriangulate = true; - /// Default constructor - SmartProjectionFactor() : throwCheirality_(false), verboseCheirality_(false) {} - - /** - * Constructor - * TODO: Mark argument order standard (keys, measurement, parameters) - * @param measured is the 2m dimensional location of the projection of a single landmark in the m views (the measurements) - * @param model is the standard deviation (current version assumes that the uncertainty is the same for all views) - * @param poseKeys is the set of indices corresponding to the cameras observing the same landmark - * @param K shared pointer to the constant calibration - * @param body_P_sensor is the transform from body to sensor frame (default identity) - */ - SmartProjectionFactor(const std::vector measured, const SharedNoiseModel& model, - std::vector poseKeys, const boost::shared_ptr& K, - boost::optional point = boost::none, - boost::optional body_P_sensor = boost::none) : - measured_(measured), K_(K), noise_(model), point_(point), body_P_sensor_(body_P_sensor), - throwCheirality_(false), verboseCheirality_(false) { - keys_.assign(poseKeys.begin(), poseKeys.end()); - } - - /** - * Constructor with exception-handling flags - * TODO: Mark argument order standard (keys, measurement, parameters) - * @param measured is the 2m dimensional location of the projection of a single landmark in the m views (the measurements) - * @param model is the standard deviation (current version assumes that the uncertainty is the same for all views) - * @param poseKeys is the set of indices corresponding to the cameras observing the same landmark - * @param K shared pointer to the constant calibration - * @param throwCheirality determines whether Cheirality exceptions are rethrown - * @param verboseCheirality determines whether exceptions are printed for Cheirality - * @param body_P_sensor is the transform from body to sensor frame (default identity) - */ - SmartProjectionFactor(const std::vector measured, const SharedNoiseModel& model, - std::vector poseKeys, const boost::shared_ptr& K, - bool throwCheirality, bool verboseCheirality, - boost::optional point = boost::none, - boost::optional body_P_sensor = boost::none) : - measured_(measured), K_(K), noise_(model), point_(point), body_P_sensor_(body_P_sensor), - throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) {} - - /** Virtual destructor */ - virtual ~SmartProjectionFactor() {} - - /// @return a deep copy of this factor -// virtual gtsam::NonlinearFactor::shared_ptr clone() const { -// return boost::static_pointer_cast( -// gtsam::NonlinearFactor::shared_ptr(new This(*this))); } - - /** - * print - * @param s optional string naming the factor - * @param keyFormatter optional formatter useful for printing Symbols - */ - void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - std::cout << s << "SmartProjectionFactor, z = "; - BOOST_FOREACH(const Point2& p, measured_) { - std::cout << "measurement, p = "<< p << std::endl; + if (!retriangulate) { + for (size_t i = 0; i < cameras.size(); i++) { + if (!cameras[i].pose().equals(cameraPosesTriangulation_[i], + retriangulationThreshold_)) { + retriangulate = true; // at least two poses are different, hence we retriangulate + break; + } } - if(this->body_P_sensor_) - this->body_P_sensor_->print(" sensor pose in body frame: "); - Base::print("", keyFormatter); } - /// equals - virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { - const This *e = dynamic_cast(&p); + if (retriangulate) { // we store the current poses used for triangulation + cameraPosesTriangulation_.clear(); + cameraPosesTriangulation_.reserve(m); + for (size_t i = 0; i < m; i++) + // cameraPosesTriangulation_[i] = cameras[i].pose(); + cameraPosesTriangulation_.push_back(cameras[i].pose()); + } - bool areMeasurementsEqual = true; - for(size_t i = 0; i < measured_.size(); i++) { - if(this->measured_.at(i).equals(e->measured_.at(i), tol) == false) - areMeasurementsEqual = false; - break; + return retriangulate; // if we arrive to this point_ all poses are the same and we don't need re-triangulation + } + + /// This function checks if the new linearization point_ is 'close' to the previous one used for linearization + bool decideIfLinearize(const Cameras& cameras) const { + // "selective linearization" + // The function evaluates how close are the old and the new poses, transformed in the ref frame of the first pose + // (we only care about the "rigidity" of the poses, not about their absolute pose) + + if (this->linearizationThreshold_ < 0) //by convention if linearizationThreshold is negative we always relinearize + return true; + + // if we do not have a previous linearization point_ or the new linearization point_ includes more poses + if (cameraPosesLinearization_.empty() + || (cameras.size() != cameraPosesLinearization_.size())) + return true; + + Pose3 firstCameraPose, firstCameraPoseOld; + for (size_t i = 0; i < cameras.size(); i++) { + + if (i == 0) { // we store the initial pose, this is useful for selective re-linearization + firstCameraPose = cameras[i].pose(); + firstCameraPoseOld = cameraPosesLinearization_[i]; + continue; } - return e - && Base::equals(p, tol) - && areMeasurementsEqual - && this->K_->equals(*e->K_, tol) - && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); + // we compare the poses in the frame of the first pose + Pose3 localCameraPose = firstCameraPose.between(cameras[i].pose()); + Pose3 localCameraPoseOld = firstCameraPoseOld.between( + cameraPosesLinearization_[i]); + if (!localCameraPose.equals(localCameraPoseOld, + this->linearizationThreshold_)) + return true; // at least two "relative" poses are different, hence we re-linearize } + return false; // if we arrive to this point_ all poses are the same and we don't need re-linearize + } - /// get the dimension of the factor (number of rows on linearization) - virtual size_t dim() const { - return 6*keys_.size(); + /// triangulateSafe + size_t triangulateSafe(const Values& values) const { + return triangulateSafe(this->cameras(values)); + } + + /// triangulateSafe + size_t triangulateSafe(const Cameras& cameras) const { + + size_t m = cameras.size(); + if (m < 2) { // if we have a single pose the corresponding factor is uninformative + degenerate_ = true; + return m; } + bool retriangulate = decideIfTriangulate(cameras); - /// linearize returns a Hessianfactor that is an approximation of error(p) - virtual boost::shared_ptr linearize(const Values& values, const Ordering& ordering) const { - - bool debug = false; - bool blockwise = true; - - // Collect all poses (Cameras) - std::vector cameraPoses; - BOOST_FOREACH(const Key& k, keys_) { - if(body_P_sensor_) - cameraPoses.push_back(values.at(k).compose(*body_P_sensor_)); - else - cameraPoses.push_back(values.at(k)); - } - + if (retriangulate) { // We triangulate the 3D position of the landmark - if (debug) { - BOOST_FOREACH(const Pose3& pose, cameraPoses) { - std::cout << "Pose: " << pose << std::endl; - } - BOOST_FOREACH(const Point2& point, measured_) { - std::cout << "Point: " << point << std::endl; - } - } - boost::optional point; - if (point_) { - point = point_; - //std::cout << "Using existing point " << *point << std::endl; - } else { - //std::cout << "Triangulating in linearize " << std::endl; - point = triangulatePoint3(cameraPoses, measured_, *K_); - } - if (debug) std::cout << "Result: " << *point << std::endl; - - - if (debug) { - std::cout << "point " << *point << std::endl; - } - - std::vector js; - std::vector Gs(keys_.size()*(keys_.size()+1)/2); - std::vector gs(keys_.size()); - double f=0; - - // fill in the keys - BOOST_FOREACH(const Key& k, keys_) { - js += ordering[k]; - } - - // point is behind one of the cameras, turn factor off by setting everything to 0 - if (!point) { - std::cout << "WARNING: Could not triangulate during linearize" << std::endl; - BOOST_FOREACH(gtsam::Matrix& m, Gs) m = zeros(6,6); - BOOST_FOREACH(Vector& v, gs) v = zero(6); - return HessianFactor::shared_ptr(new HessianFactor(js, Gs, gs, f)); - } - - // For debug only - std::vector Gs1; - std::vector gs1; - if (blockwise || debug){ - // ========================================================================================================== - std::vector Hx(keys_.size()); - std::vector Hl(keys_.size()); - std::vector b(keys_.size()); - - for(size_t i = 0; i < measured_.size(); i++) { - Pose3 pose = cameraPoses.at(i); - std::cout << "pose " << pose << std::endl; - PinholeCamera camera(pose, *K_); - b.at(i) = ( camera.project(*point,Hx.at(i),Hl.at(i)) - measured_.at(i) ).vector(); - noise_-> WhitenSystem(Hx.at(i), Hl.at(i), b.at(i)); - f += b.at(i).squaredNorm(); - } - - // Shur complement trick - - // Allocate m^2 matrix blocks - std::vector< std::vector > Hxl(keys_.size(), std::vector( keys_.size())); - - // Allocate inv(Hl'Hl) - Matrix3 C = zeros(3,3); - for(size_t i1 = 0; i1 < keys_.size(); i1++) { - C += Hl.at(i1).transpose() * Hl.at(i1); - } - - C = C.inverse().eval(); // this is very important: without eval, because of eigen aliasing the results will be incorrect - - // Calculate sub blocks - for(size_t i1 = 0; i1 < keys_.size(); i1++) { - for(size_t i2 = 0; i2 < keys_.size(); i2++) { - // we only need the upper triangular entries - Hxl[i1][i2] = Hx.at(i1).transpose() * Hl.at(i1) * C * Hl.at(i2).transpose(); - if (i1==0 && i2==0){ - if (debug) { - std::cout << "Hoff"<< i1 << i2 << "=[" << Hx.at(i1).transpose() * Hl.at(i1) * C * Hl.at(i2).transpose() << "];" << std::endl; - std::cout << "Hxoff"<< "=[" << Hx.at(i1) << "];" << std::endl; - std::cout << "Hloff"<< "=[" << Hl.at(i1) << "];" << std::endl; - std::cout << "Hloff2"<< "=[" << Hl.at(i2) << "];" << std::endl; - std::cout << "C"<< "=[" << C << "];" << std::endl; - } - } - } - } - // Populate Gs and gs - int GsCount = 0; - for(size_t i1 = 0; i1 < keys_.size(); i1++) { - gs.at(i1) = Hx.at(i1).transpose() * b.at(i1); - - for(size_t i2 = 0; i2 < keys_.size(); i2++) { - gs.at(i1) -= Hxl[i1][i2] * b.at(i2); - - if (i2 == i1){ - Gs.at(GsCount) = Hx.at(i1).transpose() * Hx.at(i1) - Hxl[i1][i2] * Hx.at(i2); - - if (debug) { - std::cout << "HxlH"<< GsCount << "=[" << Hxl[i1][i2] * Hx.at(i2) << "];" << std::endl; - std::cout << "Hx2_"<< GsCount << "=[" << Hx.at(i2) << "];" << std::endl; - std::cout << "H"<< GsCount << "=[" << Gs.at(GsCount) << "];" << std::endl; - } - GsCount++; - } - if (i2 > i1) { - Gs.at(GsCount) = - Hxl[i1][i2] * Hx.at(i2); - - if (debug) { - std::cout << "HxlH"<< GsCount << "=[" << Hxl[i1][i2] * Hx.at(i2) << "];" << std::endl; - std::cout << "Hx2_"<< GsCount << "=[" << Hx.at(i2) << "];" << std::endl; - std::cout << "H"<< GsCount << "=[" << Gs.at(GsCount) << "];" << std::endl; - } - GsCount++; - } - } - } - if (debug) { - // Copy result for later comparison - BOOST_FOREACH(const Matrix& m, Gs) { - Gs1.push_back(m); - } - // Copy result for later comparison - BOOST_FOREACH(const Matrix& m, gs) { - gs1.push_back(m); - } - } - } - - if (blockwise == false || debug){ // version with full matrix multiplication - // ========================================================================================================== - Matrix Hx2 = zeros(2*keys_.size(), 6*keys_.size()); - Matrix Hl2 = zeros(2*keys_.size(), 3); - Vector b2 = zero(2*keys_.size()); - - for(size_t i = 0; i < measured_.size(); i++) { - Pose3 pose = cameraPoses.at(i); - PinholeCamera camera(pose, *K_); - Matrix Hxi, Hli; - Vector bi = ( camera.project(*point,Hxi,Hli) - measured_.at(i) ).vector(); - - noise_-> WhitenSystem(Hxi, Hli, bi); - f += bi.squaredNorm(); - - Hx2.block( 2*i, 6*i, 2, 6 ) = Hxi; - Hl2.block( 2*i, 0, 2, 3 ) = Hli; - - if (debug) { - std::cout << "Hxi= \n" << Hxi << std::endl; - std::cout << "Hxi.transpose() * Hxi= \n" << Hxi.transpose() * Hxi << std::endl; - std::cout << "Hxl.transpose() * Hxl= \n" << Hli.transpose() * Hli << std::endl; - } - subInsert(b2,bi,2*i); - - } - - // Shur complement trick - Matrix H(6*keys_.size(), 6*keys_.size()); - Matrix3 C2 = (Hl2.transpose() * Hl2).inverse(); - H = Hx2.transpose() * Hx2 - Hx2.transpose() * Hl2 * C2 * Hl2.transpose() * Hx2; - - if (debug) { - std::cout << "Hx2" << "=[" << Hx2 << "];" << std::endl; - std::cout << "Hl2" << "=[" << Hl2 << "];" << std::endl; - std::cout << "H" << "=[" << H << "];" << std::endl; - - std::cout << "Cnoinv2"<< "=[" << Hl2.transpose() * Hl2 << "];" << std::endl; - std::cout << "C2"<< "=[" << C2 << "];" << std::endl; - std::cout << "================================================================================" << std::endl; - } - - Vector gs_vector = Hx2.transpose() * b2 - Hx2.transpose() * Hl2 * C2 * Hl2.transpose() * b2; - - - // Populate Gs and gs - int GsCount2 = 0; - for(size_t i1 = 0; i1 < keys_.size(); i1++) { - gs.at(i1) = sub(gs_vector, 6*i1, 6*i1 + 6); - - for(size_t i2 = 0; i2 < keys_.size(); i2++) { - if (i2 >= i1) { - Gs.at(GsCount2) = H.block(6*i1, 6*i2, 6, 6); - GsCount2++; - } - } - } - - } - - if (debug) { - // Compare blockwise and full version - bool gs1_equal_gs = true; - for(size_t i = 0; i < measured_.size(); i++) { - std::cout << "gs.at(i) " << gs.at(i).transpose() << std::endl; - std::cout << "gs1.at(i) " << gs1.at(i).transpose() << std::endl; - std::cout << "gs.error " << (gs.at(i)- gs1.at(i)).transpose() << std::endl; - if( !equal(gs.at(i), gs1.at(i)), 1e-7) { - gs1_equal_gs = false; - } - } - std::cout << "gs1_equal_gs " << gs1_equal_gs << std::endl; - - for(size_t i = 0; i < keys_.size()*(keys_.size()+1)/2; i++) { - std::cout << "Gs.at(i) " << Gs.at(i).transpose() << std::endl; - std::cout << "Gs1.at(i) " << Gs1.at(i).transpose() << std::endl; - std::cout << "Gs.error " << (Gs.at(i)- Gs1.at(i)).transpose() << std::endl; - } - std::cout << "Gs1_equal_Gs " << gs1_equal_gs << std::endl; - } - - // ========================================================================================================== - return HessianFactor::shared_ptr(new HessianFactor(js, Gs, gs, f)); - } - - /** - * Calculate the error of the factor. - * This is the log-likelihood, e.g. \f$ 0.5(h(x)-z)^2/\sigma^2 \f$ in case of Gaussian. - * In this class, we take the raw prediction error \f$ h(x)-z \f$, ask the noise model - * to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5. - */ - virtual double error(const Values& values) const { - bool debug = false; - if (this->active(values)) { - double overallError=0; - - // Collect all poses (Cameras) - std::vector cameraPoses; - - BOOST_FOREACH(const Key& k, keys_) { - if(body_P_sensor_) - cameraPoses.push_back(values.at(k).compose(*body_P_sensor_)); - else - cameraPoses.push_back(values.at(k)); - } - - // We triangulate the 3D position of the landmark - if (debug) { - BOOST_FOREACH(const Pose3& pose, cameraPoses) { - std::cout << "Pose: " << pose << std::endl; - } - BOOST_FOREACH(const Point2& point, measured_) { - std::cout << "Point: " << point << std::endl; - } - } - boost::optional point; - if (point_) { - point = point_; - //std::cout << "Using existing point " << *point << std::endl; - } else { - //std::cout << "Triangulate during error calc" << std::endl; - point = triangulatePoint3(cameraPoses, measured_, *K_); - } - if (debug) std::cout << "Result: " << *point << std::endl; - - if(point) - { // triangulation produced a good estimate of landmark position - - for(size_t i = 0; i < measured_.size(); i++) { - Pose3 pose = cameraPoses.at(i); - PinholeCamera camera(pose, *K_); - - Point2 reprojectionError(camera.project(*point) - measured_.at(i)); - overallError += noise_->distance( reprojectionError.vector() ); - } - return std::sqrt(overallError); - } else{ // triangulation failed: we deactivate the factor, then the error should not contribute to the overall error - std::cout << "WARNING: Could not triangulate during error calc" << std::endl; - return 0.0; - } - } else { - return 0.0; + try { + // std::cout << "triangulatePoint3 i \n" << rankTolerance << std::endl; + point_ = triangulatePoint3(cameras, this->measured_, + rankTolerance_, enableEPI_); + degenerate_ = false; + cheiralityException_ = false; + } catch (TriangulationUnderconstrainedException&) { + // if TriangulationUnderconstrainedException can be + // 1) There is a single pose for triangulation - this should not happen because we checked the number of poses before + // 2) The rank of the matrix used for triangulation is < 3: rotation-only, parallel cameras (or motion towards the landmark) + // in the second case we want to use a rotation-only smart factor + degenerate_ = true; + cheiralityException_ = false; + } catch (TriangulationCheiralityException&) { + // point is behind one of the cameras: can be the case of close-to-parallel cameras or may depend on outliers + // we manage this case by either discarding the smart factor, or imposing a rotation-only constraint + cheiralityException_ = true; } } + return m; + } - /** return the measurements */ - const Vector& measured() const { - return measured_; + /// triangulate + bool triangulateForLinearize(const Cameras& cameras) const { + + bool isDebug = false; + size_t nrCameras = this->triangulateSafe(cameras); + + if (nrCameras < 2 + || (!this->manageDegeneracy_ + && (this->cheiralityException_ || this->degenerate_))) { + if (isDebug) { + std::cout << "createImplicitSchurFactor: degenerate configuration" + << std::endl; + } + return false; + } else { + + // instead, if we want to manage the exception.. + if (this->cheiralityException_ || this->degenerate_) { // if we want to manage the exceptions with rotation-only factors + this->degenerate_ = true; + } + return true; + } + } + + /// linearize returns a Hessianfactor that is an approximation of error(p) + boost::shared_ptr > createHessianFactor( + const Cameras& cameras, const double lambda = 0.0) const { + + bool isDebug = false; + size_t numKeys = this->keys_.size(); + // Create structures for Hessian Factors + std::vector < Key > js; + std::vector < Matrix > Gs(numKeys * (numKeys + 1) / 2); + std::vector < Vector > gs(numKeys); + + if (this->measured_.size() != cameras.size()) { + std::cout + << "SmartProjectionHessianFactor: this->measured_.size() inconsistent with input" + << std::endl; + exit(1); } - /** return the calibration object */ - inline const boost::shared_ptr calibration() const { - return K_; + this->triangulateSafe(cameras); + + if (numKeys < 2 + || (!this->manageDegeneracy_ + && (this->cheiralityException_ || this->degenerate_))) { + // std::cout << "In linearize: exception" << std::endl; + BOOST_FOREACH(gtsam::Matrix& m, Gs) + m = zeros(D, D); + BOOST_FOREACH(Vector& v, gs) + v = zero(D); + return boost::make_shared >(this->keys_, Gs, gs, + 0.0); } - /** return verbosity */ - inline bool verboseCheirality() const { return verboseCheirality_; } - - /** return flag for throwing cheirality exceptions */ - inline bool throwCheirality() const { return throwCheirality_; } - - private: - - /// Serialization function - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar & BOOST_SERIALIZATION_NVP(measured_); - ar & BOOST_SERIALIZATION_NVP(K_); - ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); - ar & BOOST_SERIALIZATION_NVP(throwCheirality_); - ar & BOOST_SERIALIZATION_NVP(verboseCheirality_); + // instead, if we want to manage the exception.. + if (this->cheiralityException_ || this->degenerate_) { // if we want to manage the exceptions with rotation-only factors + this->degenerate_ = true; } - }; + bool doLinearize = this->decideIfLinearize(cameras); + + if (this->linearizationThreshold_ >= 0 && doLinearize) // if we apply selective relinearization and we need to relinearize + for (size_t i = 0; i < cameras.size(); i++) + this->cameraPosesLinearization_[i] = cameras[i].pose(); + + if (!doLinearize) { // return the previous Hessian factor + std::cout << "=============================" << std::endl; + std::cout << "doLinearize " << doLinearize << std::endl; + std::cout << "this->linearizationThreshold_ " + << this->linearizationThreshold_ << std::endl; + std::cout << "this->degenerate_ " << this->degenerate_ << std::endl; + std::cout + << "something wrong in SmartProjectionHessianFactor: selective relinearization should be disabled" + << std::endl; + exit(1); + return boost::make_shared >(this->keys_, + this->state_->Gs, this->state_->gs, this->state_->f); + } + + // ================================================================== + Matrix F, E; + Matrix3 PointCov; + Vector b; + double f = computeJacobians(F, E, PointCov, b, cameras, lambda); + + // Schur complement trick + // Frank says: should be possible to do this more efficiently? + // And we care, as in grouped factors this is called repeatedly + Matrix H(D * numKeys, D * numKeys); + Vector gs_vector; + + H.noalias() = F.transpose() * (F - (E * (PointCov * (E.transpose() * F)))); + gs_vector.noalias() = F.transpose() + * (b - (E * (PointCov * (E.transpose() * b)))); + if (isDebug) + std::cout << "gs_vector size " << gs_vector.size() << std::endl; + + // Populate Gs and gs + int GsCount2 = 0; + for (DenseIndex i1 = 0; i1 < (DenseIndex)numKeys; i1++) { // for each camera + DenseIndex i1D = i1 * D; + gs.at(i1) = gs_vector.segment < D > (i1D); + for (DenseIndex i2 = 0; i2 < (DenseIndex)numKeys; i2++) { + if (i2 >= i1) { + Gs.at(GsCount2) = H.block < D, D > (i1D, i2 * D); + GsCount2++; + } + } + } + // ================================================================== + if (this->linearizationThreshold_ >= 0) { // if we do not use selective relinearization we don't need to store these variables + this->state_->Gs = Gs; + this->state_->gs = gs; + this->state_->f = f; + } + return boost::make_shared >(this->keys_, Gs, gs, f); + } + + // create factor + boost::shared_ptr > createImplicitSchurFactor( + const Cameras& cameras, double lambda) const { + if (triangulateForLinearize(cameras)) + return Base::createImplicitSchurFactor(cameras, point_, lambda); + else + return boost::shared_ptr >(); + } + + /// create factor + boost::shared_ptr > createJacobianQFactor( + const Cameras& cameras, double lambda) const { + if (triangulateForLinearize(cameras)) + return Base::createJacobianQFactor(cameras, point_, lambda); + else + return boost::shared_ptr >(); + } + + /// Create a factor, takes values + boost::shared_ptr > createJacobianQFactor( + const Values& values, double lambda) const { + Cameras myCameras; + // TODO triangulate twice ?? + bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras); + if (nonDegenerate) + return createJacobianQFactor(myCameras, lambda); + else + return boost::shared_ptr >(); + } + + /// Returns true if nonDegenerate + bool computeCamerasAndTriangulate(const Values& values, + Cameras& myCameras) const { + Values valuesFactor; + + // Select only the cameras + BOOST_FOREACH(const Key key, this->keys_) + valuesFactor.insert(key, values.at(key)); + + myCameras = this->cameras(valuesFactor); + size_t nrCameras = this->triangulateSafe(myCameras); + + if (nrCameras < 2 + || (!this->manageDegeneracy_ + && (this->cheiralityException_ || this->degenerate_))) + return false; + + // instead, if we want to manage the exception.. + if (this->cheiralityException_ || this->degenerate_) // if we want to manage the exceptions with rotation-only factors + this->degenerate_ = true; + + if (this->degenerate_) { + std::cout << "SmartProjectionFactor: this is not ready" << std::endl; + std::cout << "this->cheiralityException_ " << this->cheiralityException_ + << std::endl; + std::cout << "this->degenerate_ " << this->degenerate_ << std::endl; + } + return true; + } + + /// Takes values + bool computeEP(Matrix& E, Matrix& PointCov, const Values& values) const { + Cameras myCameras; + bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras); + if (nonDegenerate) + computeEP(E, PointCov, myCameras); + return nonDegenerate; + } + + /// Assumes non-degenerate ! + void computeEP(Matrix& E, Matrix& PointCov, const Cameras& cameras) const { + return Base::computeEP(E, PointCov, cameras, point_); + } + + /// Version that takes values, and creates the point + bool computeJacobians(std::vector& Fblocks, + Matrix& E, Matrix& PointCov, Vector& b, const Values& values) const { + Cameras myCameras; + bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras); + if (nonDegenerate) + computeJacobians(Fblocks, E, PointCov, b, myCameras, 0.0); + return nonDegenerate; + } + + /// Compute F, E only (called below in both vanilla and SVD versions) + /// Assumes the point has been computed + /// Note E can be 2m*3 or 2m*2, in case point is degenerate + double computeJacobians(std::vector& Fblocks, + Matrix& E, Vector& b, const Cameras& cameras) const { + + if (this->degenerate_) { + std::cout << "manage degeneracy " << manageDegeneracy_ << std::endl; + std::cout << "point " << point_ << std::endl; + std::cout + << "SmartProjectionFactor: Management of degeneracy is disabled - not ready to be used" + << std::endl; + if (D > 6) { + std::cout + << "Management of degeneracy is not yet ready when one also optimizes for the calibration " + << std::endl; + } + + int numKeys = this->keys_.size(); + E = zeros(2 * numKeys, 2); + b = zero(2 * numKeys); + double f = 0; + for (size_t i = 0; i < this->measured_.size(); i++) { + if (i == 0) { // first pose + this->point_ = cameras[i].backprojectPointAtInfinity( + this->measured_.at(i)); + // 3D parametrization of point at infinity: [px py 1] + } + Matrix Fi, Ei; + Vector bi = -(cameras[i].projectPointAtInfinity(this->point_, Fi, Ei) + - this->measured_.at(i)).vector(); + + this->noise_.at(i)->WhitenSystem(Fi, Ei, bi); + f += bi.squaredNorm(); + Fblocks.push_back(typename Base::KeyMatrix2D(this->keys_[i], Fi)); + E.block < 2, 2 > (2 * i, 0) = Ei; + subInsert(b, bi, 2 * i); + } + return f; + } else { + // nondegenerate: just return Base version + return Base::computeJacobians(Fblocks, E, b, cameras, point_); + } // end else + } + + /// Version that computes PointCov, with optional lambda parameter + double computeJacobians(std::vector& Fblocks, + Matrix& E, Matrix& PointCov, Vector& b, const Cameras& cameras, + const double lambda = 0.0) const { + + double f = computeJacobians(Fblocks, E, b, cameras); + + // Point covariance inv(E'*E) + PointCov.noalias() = (E.transpose() * E + lambda * eye(E.cols())).inverse(); + + return f; + } + + /// takes values + bool computeJacobiansSVD(std::vector& Fblocks, + Matrix& Enull, Vector& b, const Values& values) const { + typename Base::Cameras myCameras; + double good = computeCamerasAndTriangulate(values, myCameras); + if (good) + computeJacobiansSVD(Fblocks, Enull, b, myCameras); + return true; + } + + /// SVD version + double computeJacobiansSVD(std::vector& Fblocks, + Matrix& Enull, Vector& b, const Cameras& cameras) const { + return Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, point_); + } + + /// Returns Matrix, TODO: maybe should not exist -> not sparse ! + // TODO should there be a lambda? + double computeJacobiansSVD(Matrix& F, Matrix& Enull, Vector& b, + const Cameras& cameras) const { + return Base::computeJacobiansSVD(F, Enull, b, cameras, point_); + } + + /// Returns Matrix, TODO: maybe should not exist -> not sparse ! + double computeJacobians(Matrix& F, Matrix& E, Matrix3& PointCov, Vector& b, + const Cameras& cameras, const double lambda) const { + return Base::computeJacobians(F, E, PointCov, b, cameras, point_, lambda); + } + + /// Calculate vector of re-projection errors, before applying noise model + /// Assumes triangulation was done and degeneracy handled + Vector reprojectionError(const Cameras& cameras) const { + return Base::reprojectionError(cameras, point_); + } + + /// Calculate vector of re-projection errors, before applying noise model + Vector reprojectionError(const Values& values) const { + Cameras myCameras; + bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras); + if (nonDegenerate) + return reprojectionError(myCameras); + else + return zero(myCameras.size() * 2); + } + + /** + * Calculate the error of the factor. + * This is the log-likelihood, e.g. \f$ 0.5(h(x)-z)^2/\sigma^2 \f$ in case of Gaussian. + * In this class, we take the raw prediction error \f$ h(x)-z \f$, ask the noise model + * to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5. + */ + double totalReprojectionError(const Cameras& cameras, + boost::optional externalPoint = boost::none) const { + + size_t nrCameras; + if (externalPoint) { + nrCameras = this->keys_.size(); + point_ = *externalPoint; + degenerate_ = false; + cheiralityException_ = false; + } else { + nrCameras = this->triangulateSafe(cameras); + } + + if (nrCameras < 2 + || (!this->manageDegeneracy_ + && (this->cheiralityException_ || this->degenerate_))) { + // if we don't want to manage the exceptions we discard the factor + // std::cout << "In error evaluation: exception" << std::endl; + return 0.0; + } + + if (this->cheiralityException_) { // if we want to manage the exceptions with rotation-only factors + std::cout + << "SmartProjectionHessianFactor: cheirality exception (this should not happen if CheiralityException is disabled)!" + << std::endl; + this->degenerate_ = true; + } + + if (this->degenerate_) { + // return 0.0; // TODO: this maybe should be zero? + std::cout + << "SmartProjectionHessianFactor: trying to manage degeneracy (this should not happen is manageDegeneracy is disabled)!" + << std::endl; + size_t i = 0; + double overallError = 0; + BOOST_FOREACH(const Camera& camera, cameras) { + const Point2& zi = this->measured_.at(i); + if (i == 0) // first pose + this->point_ = camera.backprojectPointAtInfinity(zi); // 3D parametrization of point at infinity + Point2 reprojectionError( + camera.projectPointAtInfinity(this->point_) - zi); + overallError += 0.5 + * this->noise_.at(i)->distance(reprojectionError.vector()); + i += 1; + } + return overallError; + } else { + // Just use version in base class + return Base::totalReprojectionError(cameras, point_); + } + } + + /// Cameras are computed in derived class + virtual Cameras cameras(const Values& values) const = 0; + + /** return the landmark */ + boost::optional point() const { + return point_; + } + + /** COMPUTE the landmark */ + boost::optional point(const Values& values) const { + triangulateSafe(values); + return point_; + } + + /** return the degenerate state */ + inline bool isDegenerate() const { + return (cheiralityException_ || degenerate_); + } + + /** return the cheirality status flag */ + inline bool isPointBehindCamera() const { + return cheiralityException_; + } + /** return chirality verbosity */ + inline bool verboseCheirality() const { + return verboseCheirality_; + } + + /** return flag for throwing cheirality exceptions */ + inline bool throwCheirality() const { + return throwCheirality_; + } + +private: + + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar & BOOST_SERIALIZATION_NVP(throwCheirality_); + ar & BOOST_SERIALIZATION_NVP(verboseCheirality_); + } +}; + } // \ namespace gtsam diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactor.h b/gtsam_unstable/slam/SmartProjectionPoseFactor.h new file mode 100644 index 000000000..879e5ab80 --- /dev/null +++ b/gtsam_unstable/slam/SmartProjectionPoseFactor.h @@ -0,0 +1,177 @@ +/* ---------------------------------------------------------------------------- + + * 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 SmartProjectionPoseFactor.h + * @brief Produces an Hessian factors on POSES from monocular measurements of a single landmark + * @author Luca Carlone + * @author Chris Beall + * @author Zsolt Kira + */ + +#pragma once + +#include "SmartProjectionFactor.h" + +namespace gtsam { + +/** + * The calibration is known here. The factor only constraints poses (variable dimension is 6) + * @addtogroup SLAM + */ +template +class SmartProjectionPoseFactor: public SmartProjectionFactor { +protected: + + // Known calibration + std::vector > K_all_; ///< shared pointer to calibration object (one for each camera) + +public: + + /// shorthand for base class type + typedef SmartProjectionFactor Base; + + /// shorthand for this class + typedef SmartProjectionPoseFactor This; + + /// shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; + + /** + * Constructor + * @param rankTol tolerance used to check if point triangulation is degenerate + * @param linThreshold threshold on relative pose changes used to decide whether to relinearize (selective relinearization) + * @param manageDegeneracy is true, in presence of degenerate triangulation, the factor is converted to a rotation-only constraint, + * otherwise the factor is simply neglected + * @param enableEPI if set to true linear triangulation is refined with embedded LM iterations + * @param body_P_sensor is the transform from body to sensor frame (default identity) + */ + SmartProjectionPoseFactor(const double rankTol = 1, + const double linThreshold = -1, const bool manageDegeneracy = false, + const bool enableEPI = false, boost::optional body_P_sensor = boost::none) : + Base(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor) {} + + /** Virtual destructor */ + virtual ~SmartProjectionPoseFactor() {} + + /** + * add a new measurement and pose key + * @param measured is the 2m dimensional location of the projection of a single landmark in the m view (the measurement) + * @param poseKey is the index corresponding to the camera observing the same landmark + * @param noise_i is the measurement noise + * @param K_i is the (known) camera calibration + */ + void add(const Point2 measured_i, const Key poseKey_i, + const SharedNoiseModel noise_i, + const boost::shared_ptr K_i) { + Base::add(measured_i, poseKey_i, noise_i); + K_all_.push_back(K_i); + } + + /** + * add a new measurements and pose keys + * Variant of the previous one in which we include a set of measurements + */ + void add(std::vector measurements, std::vector poseKeys, + std::vector noises, + std::vector > Ks) { + Base::add(measurements, poseKeys, noises); + for (size_t i = 0; i < measurements.size(); i++) { + K_all_.push_back(Ks.at(i)); + } + } + + /** + * add a new measurements and pose keys + * Variant of the previous one in which we include a set of measurements with the same noise and calibration + */ + void add(std::vector measurements, std::vector poseKeys, + const SharedNoiseModel noise, const boost::shared_ptr K) { + for (size_t i = 0; i < measurements.size(); i++) { + Base::add(measurements.at(i), poseKeys.at(i), noise); + K_all_.push_back(K); + } + } + + /** + * print + * @param s optional string naming the factor + * @param keyFormatter optional formatter useful for printing Symbols + */ + void print(const std::string& s = "", const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const { + std::cout << s << "SmartProjectionPoseFactor, z = \n "; + BOOST_FOREACH(const boost::shared_ptr& K, K_all_) + K->print("calibration = "); + Base::print("", keyFormatter); + } + + /// equals + virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { + const This *e = dynamic_cast(&p); + + return e && Base::equals(p, tol); + } + + /// get the dimension of the factor + virtual size_t dim() const { + return 6 * this->keys_.size(); + } + + // Collect all cameras + typename Base::Cameras cameras(const Values& values) const { + typename Base::Cameras cameras; + size_t i=0; + BOOST_FOREACH(const Key& k, this->keys_) { + Pose3 pose = values.at(k); + typename Base::Camera camera(pose, *K_all_[i++]); + cameras.push_back(camera); + } + return cameras; + } + + /** + * linearize returns a Hessian factor contraining the poses + */ + virtual boost::shared_ptr linearize( + const Values& values) const { + return this->createHessianFactor(cameras(values)); + } + + /** + * error calculates the error of the factor. + */ + virtual double error(const Values& values) const { + if (this->active(values)) { + return this->totalReprojectionError(cameras(values)); + } else { // else of active flag + return 0.0; + } + } + + /** return the calibration object */ + inline const boost::shared_ptr calibration() const { + return K_all_; + } + +private: + + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar & BOOST_SERIALIZATION_NVP(K_all_); + } + +}; // end of class declaration + +} // \ namespace gtsam diff --git a/gtsam_unstable/slam/SmartRangeFactor.h b/gtsam_unstable/slam/SmartRangeFactor.h index c2a43b019..84aed6ca8 100644 --- a/gtsam_unstable/slam/SmartRangeFactor.h +++ b/gtsam_unstable/slam/SmartRangeFactor.h @@ -11,8 +11,9 @@ #include #include -#include +#include #include +#include #include #include @@ -85,7 +86,7 @@ public: * Checks for best pair that includes first point */ Point2 triangulate(const Values& x) const { - gttic_(triangulate); + //gttic_(triangulate); // create n circles corresponding to measured range around each pose std::list circles; size_t n = size(); @@ -127,7 +128,7 @@ public: error2 += it.center.dist(p2); } return (error1 < error2) ? p1 : p2; - gttoc_(triangulate); + //gttoc_(triangulate); } /** diff --git a/gtsam_unstable/slam/TSAMFactors.h b/gtsam_unstable/slam/TSAMFactors.h new file mode 100644 index 000000000..d4c5f8172 --- /dev/null +++ b/gtsam_unstable/slam/TSAMFactors.h @@ -0,0 +1,167 @@ +/* ---------------------------------------------------------------------------- + + * 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 TSAMFactors.h + * @brief TSAM 1 Factors, simpler than the hierarchical TSAM 2 + * @author Frank Dellaert + * @date May 2014 + */ + +#pragma once + +#include +#include + +namespace gtsam { + +/** + * DeltaFactor: relative 2D measurement between Pose2 and Point2 + */ +class DeltaFactor: public NoiseModelFactor2 { + +public: + typedef DeltaFactor This; + typedef NoiseModelFactor2 Base; + typedef boost::shared_ptr shared_ptr; + +private: + Point2 measured_; ///< the measurement + +public: + + /// Constructor + DeltaFactor(Key i, Key j, const Point2& measured, + const SharedNoiseModel& model) : + Base(model, i, j), measured_(measured) { + } + + /// Evaluate measurement error h(x)-z + Vector evaluateError(const Pose2& pose, const Point2& point, + boost::optional H1 = boost::none, boost::optional H2 = + boost::none) const { + Point2 d = pose.transform_to(point, H1, H2); + Point2 e = measured_.between(d); + return e.vector(); + } +}; + +/** + * DeltaFactorBase: relative 2D measurement between Pose2 and Point2, with Basenodes + */ +class DeltaFactorBase: public NoiseModelFactor4 { + +public: + typedef DeltaFactorBase This; + typedef NoiseModelFactor4 Base; + typedef boost::shared_ptr shared_ptr; + +private: + Point2 measured_; ///< the measurement + +public: + + /// Constructor + DeltaFactorBase(Key b1, Key i, Key b2, Key j, const Point2& measured, + const SharedNoiseModel& model) : + Base(model, b1, i, b2, j), measured_(measured) { + } + + /// Evaluate measurement error h(x)-z + Vector evaluateError(const Pose2& base1, const Pose2& pose, + const Pose2& base2, const Point2& point, // + boost::optional H1 = boost::none, // + boost::optional H2 = boost::none, // + boost::optional H3 = boost::none, // + boost::optional H4 = boost::none) const { + if (H1 || H2 || H3 || H4) { + // TODO use fixed-size matrices + Matrix D_pose_g_base1, D_pose_g_pose; + Pose2 pose_g = base1.compose(pose, D_pose_g_base1, D_pose_g_pose); + Matrix D_point_g_base2, D_point_g_point; + Point2 point_g = base2.transform_from(point, D_point_g_base2, + D_point_g_point); + Matrix D_e_pose_g, D_e_point_g; + Point2 d = pose_g.transform_to(point_g, D_e_pose_g, D_e_point_g); + if (H1) + *H1 = D_e_pose_g * D_pose_g_base1; + if (H2) + *H2 = D_e_pose_g * D_pose_g_pose; + if (H3) + *H3 = D_e_point_g * D_point_g_base2; + if (H4) + *H4 = D_e_point_g * D_point_g_point; + return measured_.localCoordinates(d); + } else { + Pose2 pose_g = base1.compose(pose); + Point2 point_g = base2.transform_from(point); + Point2 d = pose_g.transform_to(point_g); + return measured_.localCoordinates(d); + } + } +}; + +/** + * OdometryFactorBase: Pose2 odometry, with Basenodes + */ +class OdometryFactorBase: public NoiseModelFactor4 { + +public: + typedef OdometryFactorBase This; + typedef NoiseModelFactor4 Base; + typedef boost::shared_ptr shared_ptr; + +private: + Pose2 measured_; ///< the measurement + +public: + + /// Constructor + OdometryFactorBase(Key b1, Key i, Key b2, Key j, const Pose2& measured, + const SharedNoiseModel& model) : + Base(model, b1, i, b2, j), measured_(measured) { + } + + /// Evaluate measurement error h(x)-z + Vector evaluateError(const Pose2& base1, const Pose2& pose1, + const Pose2& base2, const Pose2& pose2, // + boost::optional H1 = boost::none, // + boost::optional H2 = boost::none, // + boost::optional H3 = boost::none, // + boost::optional H4 = boost::none) const { + if (H1 || H2 || H3 || H4) { + // TODO use fixed-size matrices + Matrix D_pose1_g_base1, D_pose1_g_pose1; + Pose2 pose1_g = base1.compose(pose1, D_pose1_g_base1, D_pose1_g_pose1); + Matrix D_pose2_g_base2, D_pose2_g_pose2; + Pose2 pose2_g = base2.compose(pose2, D_pose2_g_base2, D_pose2_g_pose2); + Matrix D_e_pose1_g, D_e_pose2_g; + Pose2 d = pose1_g.between(pose2_g, D_e_pose1_g, D_e_pose2_g); + if (H1) + *H1 = D_e_pose1_g * D_pose1_g_base1; + if (H2) + *H2 = D_e_pose1_g * D_pose1_g_pose1; + if (H3) + *H3 = D_e_pose2_g * D_pose2_g_base2; + if (H4) + *H4 = D_e_pose2_g * D_pose2_g_pose2; + return measured_.localCoordinates(d); + } else { + Pose2 pose1_g = base1.compose(pose1); + Pose2 pose2_g = base2.compose(pose2); + Pose2 d = pose1_g.between(pose2_g); + return measured_.localCoordinates(d); + } + } +}; + +} + diff --git a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h new file mode 100644 index 000000000..c3564a748 --- /dev/null +++ b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h @@ -0,0 +1,245 @@ +/* ---------------------------------------------------------------------------- + + * 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 TransformBtwRobotsUnaryFactor.h + * @brief Unary factor for determining transformation between given trajectories of two robots + * @author Vadim Indelman + **/ +#pragma once + +#include + +#include +#include +#include +#include +#include + +namespace gtsam { + + /** + * A class for a measurement predicted by "between(config[key1],config[key2])" + * @tparam VALUE the Value type + * @addtogroup SLAM + */ + template + class TransformBtwRobotsUnaryFactor: public NonlinearFactor { + + public: + + typedef VALUE T; + + private: + + typedef TransformBtwRobotsUnaryFactor This; + typedef gtsam::NonlinearFactor Base; + + gtsam::Key key_; + + VALUE measured_; /** The measurement */ + + gtsam::Values valA_; // given values for robot A map\trajectory + gtsam::Values valB_; // given values for robot B map\trajectory + gtsam::Key keyA_; // key of robot A to which the measurement refers + gtsam::Key keyB_; // key of robot B to which the measurement refers + + SharedGaussian model_; + + /** concept check by type */ + GTSAM_CONCEPT_LIE_TYPE(T) + GTSAM_CONCEPT_TESTABLE_TYPE(T) + + public: + + // shorthand for a smart pointer to a factor + typedef typename boost::shared_ptr shared_ptr; + + /** default constructor - only use for serialization */ + TransformBtwRobotsUnaryFactor() {} + + /** Constructor */ + TransformBtwRobotsUnaryFactor(Key key, const VALUE& measured, Key keyA, Key keyB, + const gtsam::Values valA, const gtsam::Values valB, + const SharedGaussian& model) : + Base(cref_list_of<1>(key)), key_(key), measured_(measured), keyA_(keyA), keyB_(keyB), + model_(model){ + + setValAValB(valA, valB); + + } + + virtual ~TransformBtwRobotsUnaryFactor() {} + + + /** Clone */ + virtual gtsam::NonlinearFactor::shared_ptr clone() const { return boost::make_shared(*this); } + + + /** implement functions needed for Testable */ + + /** print */ + virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + std::cout << s << "TransformBtwRobotsUnaryFactor(" + << keyFormatter(key_) << ")\n"; + std::cout << "MR between factor keys: " + << keyFormatter(keyA_) << "," + << keyFormatter(keyB_) << "\n"; + measured_.print(" measured: "); + model_->print(" noise model: "); + // Base::print(s, keyFormatter); + } + + /** equals */ + virtual bool equals(const NonlinearFactor& f, double tol=1e-9) const { + const This *t = dynamic_cast (&f); + + if(t && Base::equals(f)) + return key_ == t->key_ && measured_.equals(t->measured_); + else + return false; + } + + /** implement functions needed to derive from Factor */ + + /* ************************************************************************* */ + void setValAValB(const gtsam::Values valA, const gtsam::Values valB){ + if ( (!valA.exists(keyA_)) && (!valB.exists(keyA_)) && (!valA.exists(keyB_)) && (!valB.exists(keyB_)) ) + throw("something is wrong!"); + + // TODO: make sure the two keys belong to different robots + + if (valA.exists(keyA_)){ + valA_ = valA; + valB_ = valB; + } + else { + valA_ = valB; + valB_ = valA; + } + } + + /* ************************************************************************* */ + virtual double error(const gtsam::Values& x) const { + return whitenedError(x).squaredNorm(); + } + + /* ************************************************************************* */ + /** + * Linearize a non-linearFactorN to get a gtsam::GaussianFactor, + * \f$ Ax-b \approx h(x+\delta x)-z = h(x) + A \delta x - z \f$ + * Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$ + */ + /* This version of linearize recalculates the noise model each time */ + virtual boost::shared_ptr linearize(const gtsam::Values& x) const { + // Only linearize if the factor is active + if (!this->active(x)) + return boost::shared_ptr(); + + //std::cout<<"About to linearize"< A(this->size()); + gtsam::Vector b = -whitenedError(x, A); + A1 = A[0]; + + return gtsam::GaussianFactor::shared_ptr( + new gtsam::JacobianFactor(key_, A1, b, gtsam::noiseModel::Unit::Create(b.size()))); + } + + + /* ************************************************************************* */ + gtsam::Vector whitenedError(const gtsam::Values& x, + boost::optional&> H = boost::none) const { + + bool debug = true; + + Matrix H_compose, H_between1, H_dummy; + + T orgA_T_currA = valA_.at(keyA_); + T orgB_T_currB = valB_.at(keyB_); + + T orgA_T_orgB = x.at(key_); + + T orgA_T_currB = orgA_T_orgB.compose(orgB_T_currB, H_compose, H_dummy); + + T currA_T_currB_pred = orgA_T_currA.between(orgA_T_currB, H_dummy, H_between1); + + T currA_T_currB_msr = measured_; + + Vector err_unw = currA_T_currB_msr.localCoordinates(currA_T_currB_pred); + + Vector err_wh = err_unw; + if (H) { + (*H)[0] = H_compose * H_between1; + model_->WhitenSystem(*H, err_wh); + } + else { + model_->whitenInPlace(err_wh); + } + + Vector err_wh2 = model_->whiten(err_wh); + + if (debug){ + // std::cout<<"err_wh: "<(keyA_); + T orgB_T_currB = valB_.at(keyB_); + + T orgA_T_orgB = x.at(key_); + + T orgA_T_currB = orgA_T_orgB.compose(orgB_T_currB); + + T currA_T_currB_pred = orgA_T_currA.between(orgA_T_currB); + + T currA_T_currB_msr = measured_; + + return currA_T_currB_msr.localCoordinates(currA_T_currB_pred); + } + + /* ************************************************************************* */ + + /** number of variables attached to this factor */ + std::size_t size() const { + return 1; + } + + virtual size_t dim() const { + return model_->R().rows() + model_->R().cols(); + } + + private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NonlinearFactor", + boost::serialization::base_object(*this)); + //ar & BOOST_SERIALIZATION_NVP(measured_); + } + }; // \class TransformBtwRobotsUnaryFactor + +} /// namespace gtsam diff --git a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h new file mode 100644 index 000000000..0411765e8 --- /dev/null +++ b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h @@ -0,0 +1,322 @@ +/* ---------------------------------------------------------------------------- + + * 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 TransformBtwRobotsUnaryFactorEM.h + * @brief Unary factor for determining transformation between given trajectories of two robots + * @author Vadim Indelman + **/ +#pragma once + +#include + +#include +#include +#include +#include +#include + +namespace gtsam { + + /** + * A class for a measurement predicted by "between(config[key1],config[key2])" + * @tparam VALUE the Value type + * @addtogroup SLAM + */ + template + class TransformBtwRobotsUnaryFactorEM: public NonlinearFactor { + + public: + + typedef VALUE T; + + private: + + typedef TransformBtwRobotsUnaryFactorEM This; + typedef gtsam::NonlinearFactor Base; + + gtsam::Key key_; + + VALUE measured_; /** The measurement */ + + gtsam::Values valA_; // given values for robot A map\trajectory + gtsam::Values valB_; // given values for robot B map\trajectory + gtsam::Key keyA_; // key of robot A to which the measurement refers + gtsam::Key keyB_; // key of robot B to which the measurement refers + + // TODO: create function to update valA_ and valB_ + + SharedGaussian model_inlier_; + SharedGaussian model_outlier_; + + double prior_inlier_; + double prior_outlier_; + + bool flag_bump_up_near_zero_probs_; + mutable bool start_with_M_step_; + + /** concept check by type */ + GTSAM_CONCEPT_LIE_TYPE(T) + GTSAM_CONCEPT_TESTABLE_TYPE(T) + + public: + + // shorthand for a smart pointer to a factor + typedef typename boost::shared_ptr shared_ptr; + + /** default constructor - only use for serialization */ + TransformBtwRobotsUnaryFactorEM() {} + + /** Constructor */ + TransformBtwRobotsUnaryFactorEM(Key key, const VALUE& measured, Key keyA, Key keyB, + const gtsam::Values valA, const gtsam::Values valB, + const SharedGaussian& model_inlier, const SharedGaussian& model_outlier, + const double prior_inlier, const double prior_outlier, + const bool flag_bump_up_near_zero_probs = false, + const bool start_with_M_step = false) : + Base(cref_list_of<1>(key)), key_(key), measured_(measured), keyA_(keyA), keyB_(keyB), + model_inlier_(model_inlier), model_outlier_(model_outlier), + prior_inlier_(prior_inlier), prior_outlier_(prior_outlier), flag_bump_up_near_zero_probs_(flag_bump_up_near_zero_probs), + start_with_M_step_(false){ + + setValAValB(valA, valB); + + } + + virtual ~TransformBtwRobotsUnaryFactorEM() {} + + + /** Clone */ + virtual gtsam::NonlinearFactor::shared_ptr clone() const { return boost::make_shared(*this); } + + + /** implement functions needed for Testable */ + + /** print */ + virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + std::cout << s << "TransformBtwRobotsUnaryFactorEM(" + << keyFormatter(key_) << ")\n"; + std::cout << "MR between factor keys: " + << keyFormatter(keyA_) << "," + << keyFormatter(keyB_) << "\n"; + measured_.print(" measured: "); + model_inlier_->print(" noise model inlier: "); + model_outlier_->print(" noise model outlier: "); + std::cout << "(prior_inlier, prior_outlier_) = (" + << prior_inlier_ << "," + << prior_outlier_ << ")\n"; + // Base::print(s, keyFormatter); + } + + /** equals */ + virtual bool equals(const NonlinearFactor& f, double tol=1e-9) const { + const This *t = dynamic_cast (&f); + + if(t && Base::equals(f)) + return key_ == t->key_ && measured_.equals(t->measured_) && + // model_inlier_->equals(t->model_inlier_ ) && // TODO: fix here + // model_outlier_->equals(t->model_outlier_ ) && + prior_outlier_ == t->prior_outlier_ && prior_inlier_ == t->prior_inlier_; + else + return false; + } + + /** implement functions needed to derive from Factor */ + + /* ************************************************************************* */ + void setValAValB(const gtsam::Values valA, const gtsam::Values valB){ + if ( (!valA.exists(keyA_)) && (!valB.exists(keyA_)) && (!valA.exists(keyB_)) && (!valB.exists(keyB_)) ) + throw("something is wrong!"); + + // TODO: make sure the two keys belong to different robots + + if (valA.exists(keyA_)){ + valA_ = valA; + valB_ = valB; + } + else { + valA_ = valB; + valB_ = valA; + } + } + + /* ************************************************************************* */ + virtual double error(const gtsam::Values& x) const { + return whitenedError(x).squaredNorm(); + } + + /* ************************************************************************* */ + /** + * Linearize a non-linearFactorN to get a gtsam::GaussianFactor, + * \f$ Ax-b \approx h(x+\delta x)-z = h(x) + A \delta x - z \f$ + * Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$ + */ + /* This version of linearize recalculates the noise model each time */ + virtual boost::shared_ptr linearize(const gtsam::Values& x) const { + // Only linearize if the factor is active + if (!this->active(x)) + return boost::shared_ptr(); + + //std::cout<<"About to linearize"< A(this->size()); + gtsam::Vector b = -whitenedError(x, A); + A1 = A[0]; + + return gtsam::GaussianFactor::shared_ptr( + new gtsam::JacobianFactor(key_, A1, b, gtsam::noiseModel::Unit::Create(b.size()))); + } + + + /* ************************************************************************* */ + gtsam::Vector whitenedError(const gtsam::Values& x, + boost::optional&> H = boost::none) const { + + bool debug = true; + + Matrix H_compose, H_between1, H_dummy; + + T orgA_T_currA = valA_.at(keyA_); + T orgB_T_currB = valB_.at(keyB_); + + T orgA_T_orgB = x.at(key_); + + T orgA_T_currB = orgA_T_orgB.compose(orgB_T_currB, H_compose, H_dummy); + + T currA_T_currB_pred = orgA_T_currA.between(orgA_T_currB, H_dummy, H_between1); + + T currA_T_currB_msr = measured_; + + Vector err = currA_T_currB_msr.localCoordinates(currA_T_currB_pred); + + // Calculate indicator probabilities (inlier and outlier) + Vector p_inlier_outlier = calcIndicatorProb(x); + double p_inlier = p_inlier_outlier[0]; + double p_outlier = p_inlier_outlier[1]; + + if (start_with_M_step_){ + start_with_M_step_ = false; + + p_inlier = 0.5; + p_outlier = 0.5; + } + + Vector err_wh_inlier = model_inlier_->whiten(err); + Vector err_wh_outlier = model_outlier_->whiten(err); + + Matrix invCov_inlier = model_inlier_->R().transpose() * model_inlier_->R(); + Matrix invCov_outlier = model_outlier_->R().transpose() * model_outlier_->R(); + + Vector err_wh_eq; + err_wh_eq.resize(err_wh_inlier.rows()*2); + err_wh_eq << sqrt(p_inlier) * err_wh_inlier.array() , sqrt(p_outlier) * err_wh_outlier.array(); + + Matrix H_unwh = H_compose * H_between1; + + if (H){ + + Matrix H_inlier = sqrt(p_inlier)*model_inlier_->Whiten(H_unwh); + Matrix H_outlier = sqrt(p_outlier)*model_outlier_->Whiten(H_unwh); + Matrix H_aug = gtsam::stack(2, &H_inlier, &H_outlier); + + (*H)[0].resize(H_aug.rows(),H_aug.cols()); + (*H)[0] = H_aug; + } + + if (debug){ + // std::cout<<"H_compose - rows, cols, : "<whiten(err); + Vector err_wh_outlier = model_outlier_->whiten(err); + + Matrix invCov_inlier = model_inlier_->R().transpose() * model_inlier_->R(); + Matrix invCov_outlier = model_outlier_->R().transpose() * model_outlier_->R(); + + double p_inlier = prior_inlier_ * sqrt(invCov_inlier.norm()) * exp( -0.5 * err_wh_inlier.dot(err_wh_inlier) ); + double p_outlier = prior_outlier_ * sqrt(invCov_outlier.norm()) * exp( -0.5 * err_wh_outlier.dot(err_wh_outlier) ); + + double sumP = p_inlier + p_outlier; + p_inlier /= sumP; + p_outlier /= sumP; + + if (flag_bump_up_near_zero_probs_){ + // Bump up near-zero probabilities (as in linerFlow.h) + double minP = 0.05; // == 0.1 / 2 indicator variables + if (p_inlier < minP || p_outlier < minP){ + if (p_inlier < minP) + p_inlier = minP; + if (p_outlier < minP) + p_outlier = minP; + sumP = p_inlier + p_outlier; + p_inlier /= sumP; + p_outlier /= sumP; + } + } + + return (Vector(2) << p_inlier, p_outlier); + } + + /* ************************************************************************* */ + gtsam::Vector unwhitenedError(const gtsam::Values& x) const { + + T orgA_T_currA = valA_.at(keyA_); + T orgB_T_currB = valB_.at(keyB_); + + T orgA_T_orgB = x.at(key_); + + T orgA_T_currB = orgA_T_orgB.compose(orgB_T_currB); + + T currA_T_currB_pred = orgA_T_currA.between(orgA_T_currB); + + T currA_T_currB_msr = measured_; + + return currA_T_currB_msr.localCoordinates(currA_T_currB_pred); + } + + /* ************************************************************************* */ + + /** number of variables attached to this factor */ + std::size_t size() const { + return 1; + } + + virtual size_t dim() const { + return model_inlier_->R().rows() + model_inlier_->R().cols(); + } + + private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NonlinearFactor", + boost::serialization::base_object(*this)); + //ar & BOOST_SERIALIZATION_NVP(measured_); + } + }; // \class TransformBtwRobotsUnaryFactorEM + +} /// namespace gtsam diff --git a/gtsam_unstable/slam/serialization.cpp b/gtsam_unstable/slam/serialization.cpp index 304b10b6d..2f54528b8 100644 --- a/gtsam_unstable/slam/serialization.cpp +++ b/gtsam_unstable/slam/serialization.cpp @@ -19,7 +19,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/gtsam_unstable/slam/tests/CMakeLists.txt b/gtsam_unstable/slam/tests/CMakeLists.txt new file mode 100644 index 000000000..bb5259ef2 --- /dev/null +++ b/gtsam_unstable/slam/tests/CMakeLists.txt @@ -0,0 +1,7 @@ + +# Exclude tests that don't work +set (slam_excluded_tests + testSerialization.cpp +) + +gtsamAddTestsGlob(slam_unstable "test*.cpp" "${slam_excluded_tests}" "gtsam_unstable") diff --git a/gtsam_unstable/slam/tests/testAHRS.cpp b/gtsam_unstable/slam/tests/testAHRS.cpp index d59098402..a3c963a58 100644 --- a/gtsam_unstable/slam/tests/testAHRS.cpp +++ b/gtsam_unstable/slam/tests/testAHRS.cpp @@ -16,22 +16,22 @@ using namespace std; using namespace gtsam; // stationary interval of gyro U and acc F -Matrix stationaryU = trans(Matrix_(3,3,-0.0004,-0.0002,-0.0014,0.0006,-0.0003,0.0007,0.0006,-0.0002,-0.0003)); -Matrix stationaryF = trans(Matrix_(3,3,0.1152,-0.0188,9.7419,-0.0163,0.0146,9.7753,-0.0283,-0.0428,9.9021)); +Matrix stationaryU = trans((Matrix(3, 3) << -0.0004,-0.0002,-0.0014,0.0006,-0.0003,0.0007,0.0006,-0.0002,-0.0003)); +Matrix stationaryF = trans((Matrix(3, 3) << 0.1152,-0.0188,9.7419,-0.0163,0.0146,9.7753,-0.0283,-0.0428,9.9021)); double g_e = 9.7963; // Atlanta /* ************************************************************************* */ TEST (AHRS, cov) { // samples stored by row - Matrix A = Matrix_(4, 3, + Matrix A = (Matrix(4, 3) << 1.0, 2.0, 3.0, 5.0, 7.0, 0.0, 9.0, 4.0, 7.0, 6.0, 3.0, 2.0); Matrix actual = cov(trans(A)); - Matrix expected = Matrix_(3, 3, + Matrix expected = (Matrix(3, 3) << 10.9167, 2.3333, 5.0000, 2.3333, 4.6667, -2.6667, 5.0000, -2.6667, 8.6667); @@ -43,7 +43,7 @@ TEST (AHRS, cov) { TEST (AHRS, covU) { Matrix actual = cov(10000*stationaryU); - Matrix expected = Matrix_(3, 3, + Matrix expected = (Matrix(3, 3) << 33.3333333, -1.66666667, 53.3333333, -1.66666667, 0.333333333, -5.16666667, 53.3333333, -5.16666667, 110.333333); @@ -55,7 +55,7 @@ TEST (AHRS, covU) { TEST (AHRS, covF) { Matrix actual = cov(100*stationaryF); - Matrix expected = Matrix_(3, 3, + Matrix expected = (Matrix(3, 3) << 63.3808333, -0.432166667, -48.1706667, -0.432166667, 8.31053333, -16.6792667, -48.1706667, -16.6792667, 71.4297333); @@ -75,7 +75,7 @@ TEST (AHRS, Mechanization_integrate) { Mechanization_bRn2 mech; KalmanFilter::State state; // boost::tie(mech,state) = ahrs.initialize(g_e); -// Vector u = Vector_(3,0.05,0.0,0.0); +// Vector u = (Vector(3) << 0.05,0.0,0.0); // double dt = 2; // Rot3 expected; // Mechanization_bRn2 mech2 = mech.integrate(u,dt); @@ -92,8 +92,8 @@ TEST (AHRS, init) { */ /* ************************************************************************* */ int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); + TestResult tr; + return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp b/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp index 3fb8473b1..a7934c622 100644 --- a/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp +++ b/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp @@ -15,10 +15,6 @@ #include -//#include -//#include -//#include - using namespace std; using namespace gtsam; @@ -59,8 +55,8 @@ TEST( BetweenFactorEM, ConstructorAndEquals) gtsam::Pose2 rel_pose_ideal = p1.between(p2); gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise); - SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::Vector_(3, 0.5, 0.5, 0.05))); - SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::Vector_(3, 5, 5, 1.0))); + SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::(Vector(3) << 0.5, 0.5, 0.05))); + SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::(Vector(3) << 5, 5, 1.0))); double prior_outlier = 0.5; double prior_inlier = 0.5; @@ -88,8 +84,8 @@ TEST( BetweenFactorEM, EvaluateError) gtsam::Pose2 rel_pose_ideal = p1.between(p2); gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise); - SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::Vector_(3, 0.5, 0.5, 0.05))); - SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::Vector_(3, 50.0, 50.0, 10.0))); + SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::(Vector(3) << 0.5, 0.5, 0.05))); + SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::(Vector(3) << 50.0, 50.0, 10.0))); gtsam::Values values; values.insert(key1, p1); @@ -103,15 +99,14 @@ TEST( BetweenFactorEM, EvaluateError) Vector actual_err_wh = f.whitenedError(values); - Vector actual_err_wh_inlier = Vector_(3, actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]); - Vector actual_err_wh_outlier = Vector_(3, actual_err_wh[3], actual_err_wh[4], actual_err_wh[5]); + Vector actual_err_wh_inlier = (Vector(3) << actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]); + Vector actual_err_wh_outlier = (Vector(3) << actual_err_wh[3], actual_err_wh[4], actual_err_wh[5]); + + // cout << "Inlier test. norm of actual_err_wh_inlier, actual_err_wh_outlier: "< 1000.0*actual_err_wh_outlier.norm()); - - cout << "Inlier test. norm of actual_err_wh_inlier, actual_err_wh_outlier: "< 1000.0*actual_err_wh_outlier.norm()); // Outlier test noise = gtsam::Pose2(10.5, 20.4, 2.01); @@ -122,14 +117,14 @@ TEST( BetweenFactorEM, EvaluateError) actual_err_wh = g.whitenedError(values); - actual_err_wh_inlier = Vector_(3, actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]); - actual_err_wh_outlier = Vector_(3, actual_err_wh[3], actual_err_wh[4], actual_err_wh[5]); + actual_err_wh_inlier = (Vector(3) << actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]); + actual_err_wh_outlier = (Vector(3) << actual_err_wh[3], actual_err_wh[4], actual_err_wh[5]); // in case of outlier, outlier-mode whitented error should be dominant - CHECK(actual_err_wh_inlier.norm() < 1000.0*actual_err_wh_outlier.norm()); - - cout << "Outlier test. norm of actual_err_wh_inlier, actual_err_wh_outlier: "< h_EM(key1, key2, rel_pose_msr, model_inlier, model_outlier, prior_inlier, prior_outlier); actual_err_wh = h_EM.whitenedError(values); - actual_err_wh_inlier = Vector_(3, actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]); + actual_err_wh_inlier = (Vector(3) << actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]); BetweenFactor h(key1, key2, rel_pose_msr, model_inlier ); Vector actual_err_wh_stnd = h.whitenedError(values); - cout<<"actual_err_wh: "< h(key1, key2, rel_pose_msr, model_inlier ); Vector actual_err_wh_stnd = h.whitenedError(values); - Vector actual_err_wh_inlier = Vector_(3, actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]); - CHECK( assert_equal(actual_err_wh_stnd, actual_err_wh_inlier, 1e-8)); + Vector actual_err_wh_inlier = (Vector(3) << actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]); +// CHECK( assert_equal(actual_err_wh_stnd, actual_err_wh_inlier, 1e-8)); std::vector H_actual_stnd_unwh(2); (void)h.unwhitenedError(values, H_actual_stnd_unwh); Matrix H1_actual_stnd_unwh = H_actual_stnd_unwh[0]; @@ -201,276 +196,59 @@ TEST (BetweenFactorEM, jacobian ) { // try to check numerical derivatives of a standard between factor Matrix H1_expected_stnd = gtsam::numericalDerivative11(boost::bind(&predictionError_standard, _1, p2, key1, key2, h), p1, stepsize); - CHECK( assert_equal(H1_expected_stnd, H1_actual_stnd, 1e-5)); - - - CHECK( assert_equal(H1_expected, H1_actual, 1e-8)); - CHECK( assert_equal(H2_expected, H2_actual, 1e-8)); +// CHECK( assert_equal(H1_expected_stnd, H1_actual_stnd, 1e-5)); +// +// +// CHECK( assert_equal(H1_expected, H1_actual, 1e-8)); +// CHECK( assert_equal(H2_expected, H2_actual, 1e-8)); } + /* ************************************************************************* */ -TEST( InertialNavFactor, Equals) +TEST( BetweenFactorEM, CaseStudy) { -// gtsam::Key Pose1(11); -// gtsam::Key Pose2(12); -// gtsam::Key Vel1(21); -// gtsam::Key Vel2(22); -// gtsam::Key Bias1(31); -// -// Vector measurement_acc(Vector_(3,0.1,0.2,0.4)); -// Vector measurement_gyro(Vector_(3, -0.2, 0.5, 0.03)); -// -// double measurement_dt(0.1); -// Vector world_g(Vector_(3, 0.0, 0.0, 9.81)); -// Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system -// gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); -// gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); -// -// SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); -// -// InertialNavFactor f(Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); -// InertialNavFactor g(Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); -// CHECK(assert_equal(f, g, 1e-5)); -} -/* ************************************************************************* */ -TEST( InertialNavFactor, Predict) -{ -// gtsam::Key PoseKey1(11); -// gtsam::Key PoseKey2(12); -// gtsam::Key VelKey1(21); -// gtsam::Key VelKey2(22); -// gtsam::Key BiasKey1(31); -// -// double measurement_dt(0.1); -// Vector world_g(Vector_(3, 0.0, 0.0, 9.81)); -// Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system -// gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); -// gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); -// -// SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); -// -// -// // First test: zero angular motion, some acceleration -// Vector measurement_acc(Vector_(3,0.1,0.2,0.3-9.81)); -// Vector measurement_gyro(Vector_(3, 0.0, 0.0, 0.0)); -// -// InertialNavFactor f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); -// -// Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00)); -// LieVector Vel1(3, 0.50, -0.50, 0.40); -// imuBias::ConstantBias Bias1; -// Pose3 expectedPose2(Rot3(), Point3(2.05, 0.95, 3.04)); -// LieVector expectedVel2(3, 0.51, -0.48, 0.43); -// Pose3 actualPose2; -// LieVector actualVel2; -// f.predict(Pose1, Vel1, Bias1, actualPose2, actualVel2); -// -// CHECK(assert_equal(expectedPose2, actualPose2, 1e-5)); -// CHECK(assert_equal(expectedVel2, actualVel2, 1e-5)); -} + bool debug = false; -/* ************************************************************************* */ -TEST( InertialNavFactor, ErrorPosVel) -{ -// gtsam::Key PoseKey1(11); -// gtsam::Key PoseKey2(12); -// gtsam::Key VelKey1(21); -// gtsam::Key VelKey2(22); -// gtsam::Key BiasKey1(31); -// -// double measurement_dt(0.1); -// Vector world_g(Vector_(3, 0.0, 0.0, 9.81)); -// Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system -// gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); -// gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); -// -// SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); -// -// -// // First test: zero angular motion, some acceleration -// Vector measurement_acc(Vector_(3,0.1,0.2,0.3-9.81)); -// Vector measurement_gyro(Vector_(3, 0.0, 0.0, 0.0)); -// -// InertialNavFactor f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); -// -// Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00)); -// Pose3 Pose2(Rot3(), Point3(2.05, 0.95, 3.04)); -// LieVector Vel1(3, 0.50, -0.50, 0.40); -// LieVector Vel2(3, 0.51, -0.48, 0.43); -// imuBias::ConstantBias Bias1; -// -// Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); -// Vector ExpectedErr(zero(9)); -// -// CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5)); -} + gtsam::Key key1(1); + gtsam::Key key2(2); -/* ************************************************************************* */ -TEST( InertialNavFactor, ErrorRot) -{ -// gtsam::Key PoseKey1(11); -// gtsam::Key PoseKey2(12); -// gtsam::Key VelKey1(21); -// gtsam::Key VelKey2(22); -// gtsam::Key BiasKey1(31); -// -// double measurement_dt(0.1); -// Vector world_g(Vector_(3, 0.0, 0.0, 9.81)); -// Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system -// gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); -// gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); -// -// SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); -// -// // Second test: zero angular motion, some acceleration -// Vector measurement_acc(Vector_(3,0.0,0.0,0.0-9.81)); -// Vector measurement_gyro(Vector_(3, 0.1, 0.2, 0.3)); -// -// InertialNavFactor f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); -// -// Pose3 Pose1(Rot3(), Point3(2.0,1.0,3.0)); -// Pose3 Pose2(Rot3::Expmap(measurement_gyro*measurement_dt), Point3(2.0,1.0,3.0)); -// LieVector Vel1(3,0.0,0.0,0.0); -// LieVector Vel2(3,0.0,0.0,0.0); -// imuBias::ConstantBias Bias1; -// -// Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); -// Vector ExpectedErr(zero(9)); -// -// CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5)); -} + // Inlier test + gtsam::Pose2 p1; + gtsam::Pose2 p2(-0.0491752554, -0.289649075, -0.328993962); + gtsam::Pose2 rel_pose_msr(0.0316191379, 0.0247539161, 0.004102182); -/* ************************************************************************* */ -TEST( InertialNavFactor, ErrorRotPosVel) -{ -// gtsam::Key PoseKey1(11); -// gtsam::Key PoseKey2(12); -// gtsam::Key VelKey1(21); -// gtsam::Key VelKey2(22); -// gtsam::Key BiasKey1(31); -// -// double measurement_dt(0.1); -// Vector world_g(Vector_(3, 0.0, 0.0, 9.81)); -// Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system -// gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); -// gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); -// -// SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); -// -// // Second test: zero angular motion, some acceleration - generated in matlab -// Vector measurement_acc(Vector_(3, 6.501390843381716, -6.763926150509185, -2.300389940090343)); -// Vector measurement_gyro(Vector_(3, 0.1, 0.2, 0.3)); -// -// InertialNavFactor f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); -// -// Rot3 R1(0.487316618, 0.125253866, 0.86419557, -// 0.580273724, 0.693095498, -0.427669306, -// -0.652537293, 0.709880342, 0.265075427); -// Point3 t1(2.0,1.0,3.0); -// Pose3 Pose1(R1, t1); -// LieVector Vel1(3,0.5,-0.5,0.4); -// Rot3 R2(0.473618898, 0.119523052, 0.872582019, -// 0.609241153, 0.67099888, -0.422594037, -// -0.636011287, 0.731761397, 0.244979388); -// Point3 t2(2.052670960415706, 0.977252139079380, 2.942482135362800); -// Pose3 Pose2(R2, t2); -// LieVector Vel2(3,0.510000000000000, -0.480000000000000, 0.430000000000000); -// imuBias::ConstantBias Bias1; -// -// Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); -// Vector ExpectedErr(zero(9)); -// -// CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5)); -} + SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::(Vector(3) << 0.4021, 0.286, 0.428))); + SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::(Vector(3) << 4.9821, 4.614, 1.8387))); + gtsam::Values values; + values.insert(key1, p1); + values.insert(key2, p2); -/* ************************************************************************* */ -TEST (InertialNavFactor, Jacobian ) { + double prior_outlier = 0.5; + double prior_inlier = 0.5; -// gtsam::Key PoseKey1(11); -// gtsam::Key PoseKey2(12); -// gtsam::Key VelKey1(21); -// gtsam::Key VelKey2(22); -// gtsam::Key BiasKey1(31); -// -// double measurement_dt(0.01); -// Vector world_g(Vector_(3, 0.0, 0.0, 9.81)); -// Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system -// gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); -// gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); -// -// SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); -// -// Vector measurement_acc(Vector_(3, 6.501390843381716, -6.763926150509185, -2.300389940090343)); -// Vector measurement_gyro(Vector_(3, 3.14, 3.14/2, -3.14)); -// -// InertialNavFactor factor(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); -// -// Rot3 R1(0.487316618, 0.125253866, 0.86419557, -// 0.580273724, 0.693095498, -0.427669306, -// -0.652537293, 0.709880342, 0.265075427); -// Point3 t1(2.0,1.0,3.0); -// Pose3 Pose1(R1, t1); -// LieVector Vel1(3,0.5,-0.5,0.4); -// Rot3 R2(0.473618898, 0.119523052, 0.872582019, -// 0.609241153, 0.67099888, -0.422594037, -// -0.636011287, 0.731761397, 0.244979388); -// Point3 t2(2.052670960415706, 0.977252139079380, 2.942482135362800); -// Pose3 Pose2(R2, t2); -// LieVector Vel2(3,0.510000000000000, -0.480000000000000, 0.430000000000000); -// imuBias::ConstantBias Bias1; -// -// Matrix H1_actual, H2_actual, H3_actual, H4_actual, H5_actual; -// -// Vector ActualErr(factor.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2, H1_actual, H2_actual, H3_actual, H4_actual, H5_actual)); -// -// // Checking for Pose part in the jacobians -// // ****** -// Matrix H1_actualPose(H1_actual.block(0,0,6,H1_actual.cols())); -// Matrix H2_actualPose(H2_actual.block(0,0,6,H2_actual.cols())); -// Matrix H3_actualPose(H3_actual.block(0,0,6,H3_actual.cols())); -// Matrix H4_actualPose(H4_actual.block(0,0,6,H4_actual.cols())); -// Matrix H5_actualPose(H5_actual.block(0,0,6,H5_actual.cols())); -// -// // Calculate the Jacobian matrices H1 until H5 using the numerical derivative function -// gtsam::Matrix H1_expectedPose, H2_expectedPose, H3_expectedPose, H4_expectedPose, H5_expectedPose; -// H1_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); -// H2_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1); -// H3_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1); -// H4_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2); -// H5_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2); -// -// // Verify they are equal for this choice of state -// CHECK( gtsam::assert_equal(H1_expectedPose, H1_actualPose, 1e-6)); -// CHECK( gtsam::assert_equal(H2_expectedPose, H2_actualPose, 1e-6)); -// CHECK( gtsam::assert_equal(H3_expectedPose, H3_actualPose, 1e-6)); -// CHECK( gtsam::assert_equal(H4_expectedPose, H4_actualPose, 1e-6)); -// CHECK( gtsam::assert_equal(H5_expectedPose, H5_actualPose, 1e-6)); -// -// // Checking for Vel part in the jacobians -// // ****** -// Matrix H1_actualVel(H1_actual.block(6,0,3,H1_actual.cols())); -// Matrix H2_actualVel(H2_actual.block(6,0,3,H2_actual.cols())); -// Matrix H3_actualVel(H3_actual.block(6,0,3,H3_actual.cols())); -// Matrix H4_actualVel(H4_actual.block(6,0,3,H4_actual.cols())); -// Matrix H5_actualVel(H5_actual.block(6,0,3,H5_actual.cols())); -// -// // Calculate the Jacobian matrices H1 until H5 using the numerical derivative function -// gtsam::Matrix H1_expectedVel, H2_expectedVel, H3_expectedVel, H4_expectedVel, H5_expectedVel; -// H1_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); -// H2_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1); -// H3_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1); -// H4_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2); -// H5_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2); -// -// // Verify they are equal for this choice of state -// CHECK( gtsam::assert_equal(H1_expectedVel, H1_actualVel, 1e-6)); -// CHECK( gtsam::assert_equal(H2_expectedVel, H2_actualVel, 1e-6)); -// CHECK( gtsam::assert_equal(H3_expectedVel, H3_actualVel, 1e-6)); -// CHECK( gtsam::assert_equal(H4_expectedVel, H4_actualVel, 1e-6)); -// CHECK( gtsam::assert_equal(H5_expectedVel, H5_actualVel, 1e-6)); + BetweenFactorEM f(key1, key2, rel_pose_msr, model_inlier, model_outlier, + prior_inlier, prior_outlier); + + if (debug) + cout << "==== inside CaseStudy ===="< -#include - #include +#include +#include + using namespace gtsam; const double tol = 1e-9; @@ -41,15 +42,12 @@ TEST( testDummyFactor, basics ) { // errors - all zeros DOUBLES_EQUAL(0.0, dummyfactor.error(c), tol); - Ordering ordering; - ordering += key1, key2; - // linearization: all zeros - GaussianFactor::shared_ptr actLinearization = dummyfactor.linearize(c, ordering); + GaussianFactor::shared_ptr actLinearization = dummyfactor.linearize(c); CHECK(actLinearization); noiseModel::Diagonal::shared_ptr model3 = noiseModel::Unit::Create(3); GaussianFactor::shared_ptr expLinearization(new JacobianFactor( - ordering[key1], zeros(3,3), ordering[key2], zeros(3,3), zero(3), model3)); + key1, zeros(3,3), key2, zeros(3,3), zero(3), model3)); EXPECT(assert_equal(*expLinearization, *actLinearization, tol)); } diff --git a/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp b/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp index 7db8e75f1..2a48aa73c 100644 --- a/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp +++ b/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp @@ -19,7 +19,7 @@ #include #include #include -#include +#include #include #include #include @@ -31,31 +31,31 @@ using namespace gtsam; /* ************************************************************************* */ TEST( EquivInertialNavFactor_GlobalVel, Constructor) { - Key poseKey1(11); - Key poseKey2(12); - Key velKey1(21); - Key velKey2(22); - Key biasKey1(31); + Key poseKey1(11); + Key poseKey2(12); + Key velKey1(21); + Key velKey2(22); + Key biasKey1(31); // IMU accumulation variables - Vector delta_pos_in_t0 = Vector_(3, 0.0, 0.0, 0.0); - Vector delta_vel_in_t0 = Vector_(3, 0.0, 0.0, 0.0); - Vector delta_angles = Vector_(3, 0.0, 0.0, 0.0); + Vector delta_pos_in_t0 = (Vector(3) << 0.0, 0.0, 0.0); + Vector delta_vel_in_t0 = (Vector(3) << 0.0, 0.0, 0.0); + Vector delta_angles = (Vector(3) << 0.0, 0.0, 0.0); double delta_t = 0.0; Matrix EquivCov_Overall = zeros(15,15); Matrix Jacobian_wrt_t0_Overall = eye(15); imuBias::ConstantBias bias1 = imuBias::ConstantBias(); - // Earth Terms (gravity, etc) + // Earth Terms (gravity, etc) Vector3 g(0.0, 0.0, -9.80); Vector3 rho(0.0, 0.0, 0.0); Vector3 omega_earth(0.0, 0.0, 0.0); - // IMU Noise Model - SharedGaussian imu_model = noiseModel::Gaussian::Covariance(EquivCov_Overall.block(0,0,9,9)); + // IMU Noise Model + SharedGaussian imu_model = noiseModel::Gaussian::Covariance(EquivCov_Overall.block(0,0,9,9)); - // Constructor - EquivInertialNavFactor_GlobalVel factor( + // Constructor + EquivInertialNavFactor_GlobalVel factor( poseKey1, velKey1, biasKey1, poseKey2, velKey2, delta_pos_in_t0, delta_vel_in_t0, delta_angles, delta_t, g, rho, omega_earth, imu_model, Jacobian_wrt_t0_Overall, bias1); @@ -63,5 +63,5 @@ TEST( EquivInertialNavFactor_GlobalVel, Constructor) } /* ************************************************************************* */ - int main() { TestResult tr; return TestRegistry::runAllTests(tr);} + int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ diff --git a/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp b/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp index 4bfd13e75..7756e79d3 100644 --- a/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp +++ b/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp @@ -21,19 +21,20 @@ #include #include #include -#include +#include #include #include +#include using namespace std; using namespace gtsam; gtsam::Rot3 world_R_ECEF( - 0.31686, 0.51505, 0.79645, - 0.85173, -0.52399, 0, - 0.41733, 0.67835, -0.60471); + 0.31686, 0.51505, 0.79645, + 0.85173, -0.52399, 0, + 0.41733, 0.67835, -0.60471); -gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); +gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); /* ************************************************************************* */ @@ -48,24 +49,24 @@ gtsam::LieVector predictionErrorVel(const Pose3& p1, const LieVector& v1, const /* ************************************************************************* */ TEST( InertialNavFactor_GlobalVelocity, Constructor) { - gtsam::Key Pose1(11); - gtsam::Key Pose2(12); - gtsam::Key Vel1(21); - gtsam::Key Vel2(22); - gtsam::Key Bias1(31); + gtsam::Key Pose1(11); + gtsam::Key Pose2(12); + gtsam::Key Vel1(21); + gtsam::Key Vel2(22); + gtsam::Key Bias1(31); - Vector measurement_acc(Vector_(3,0.1,0.2,0.4)); - Vector measurement_gyro(Vector_(3, -0.2, 0.5, 0.03)); + Vector measurement_acc((Vector(3) << 0.1,0.2,0.4)); + Vector measurement_gyro((Vector(3) << -0.2, 0.5, 0.03)); - double measurement_dt(0.1); - Vector world_g(Vector_(3, 0.0, 0.0, 9.81)); - Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); - gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); + double measurement_dt(0.1); + Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); + Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system + gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); + gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); - SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); + SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); - InertialNavFactor_GlobalVelocity f(Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); + InertialNavFactor_GlobalVelocity f(Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); } /* ************************************************************************* */ @@ -77,20 +78,20 @@ TEST( InertialNavFactor_GlobalVelocity, Equals) gtsam::Key Vel2(22); gtsam::Key Bias1(31); - Vector measurement_acc(Vector_(3,0.1,0.2,0.4)); - Vector measurement_gyro(Vector_(3, -0.2, 0.5, 0.03)); + Vector measurement_acc((Vector(3) << 0.1,0.2,0.4)); + Vector measurement_gyro((Vector(3) << -0.2, 0.5, 0.03)); - double measurement_dt(0.1); - Vector world_g(Vector_(3, 0.0, 0.0, 9.81)); - Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); + double measurement_dt(0.1); + Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); + Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system + gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); - SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); + SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); - InertialNavFactor_GlobalVelocity f(Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); - InertialNavFactor_GlobalVelocity g(Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); - CHECK(assert_equal(f, g, 1e-5)); + InertialNavFactor_GlobalVelocity f(Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); + InertialNavFactor_GlobalVelocity g(Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); + CHECK(assert_equal(f, g, 1e-5)); } /* ************************************************************************* */ @@ -103,25 +104,25 @@ TEST( InertialNavFactor_GlobalVelocity, Predict) gtsam::Key BiasKey1(31); double measurement_dt(0.1); - Vector world_g(Vector_(3, 0.0, 0.0, 9.81)); - Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); + Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); + Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system + gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); // First test: zero angular motion, some acceleration - Vector measurement_acc(Vector_(3,0.1,0.2,0.3-9.81)); - Vector measurement_gyro(Vector_(3, 0.0, 0.0, 0.0)); + Vector measurement_acc((Vector(3) <<0.1,0.2,0.3-9.81)); + Vector measurement_gyro((Vector(3) << 0.0, 0.0, 0.0)); InertialNavFactor_GlobalVelocity f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00)); - LieVector Vel1(3, 0.50, -0.50, 0.40); + LieVector Vel1((Vector(3) << 0.50, -0.50, 0.40)); imuBias::ConstantBias Bias1; Pose3 expectedPose2(Rot3(), Point3(2.05, 0.95, 3.04)); - LieVector expectedVel2(3, 0.51, -0.48, 0.43); + LieVector expectedVel2((Vector(3) << 0.51, -0.48, 0.43)); Pose3 actualPose2; LieVector actualVel2; f.predict(Pose1, Vel1, Bias1, actualPose2, actualVel2); @@ -139,31 +140,31 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorPosVel) gtsam::Key VelKey2(22); gtsam::Key BiasKey1(31); - double measurement_dt(0.1); - Vector world_g(Vector_(3, 0.0, 0.0, 9.81)); - Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); + double measurement_dt(0.1); + Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); + Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system + gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); - SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); + SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); - // First test: zero angular motion, some acceleration - Vector measurement_acc(Vector_(3,0.1,0.2,0.3-9.81)); - Vector measurement_gyro(Vector_(3, 0.0, 0.0, 0.0)); + // First test: zero angular motion, some acceleration + Vector measurement_acc((Vector(3) <<0.1,0.2,0.3-9.81)); + Vector measurement_gyro((Vector(3) << 0.0, 0.0, 0.0)); - InertialNavFactor_GlobalVelocity f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); + InertialNavFactor_GlobalVelocity f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); - Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00)); - Pose3 Pose2(Rot3(), Point3(2.05, 0.95, 3.04)); - LieVector Vel1(3, 0.50, -0.50, 0.40); - LieVector Vel2(3, 0.51, -0.48, 0.43); - imuBias::ConstantBias Bias1; + Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00)); + Pose3 Pose2(Rot3(), Point3(2.05, 0.95, 3.04)); + LieVector Vel1((Vector(3) << 0.50, -0.50, 0.40)); + LieVector Vel2((Vector(3) << 0.51, -0.48, 0.43)); + imuBias::ConstantBias Bias1; - Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); - Vector ExpectedErr(zero(9)); + Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); + Vector ExpectedErr(zero(9)); - CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5)); + CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5)); } /* ************************************************************************* */ @@ -175,30 +176,30 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRot) gtsam::Key VelKey2(22); gtsam::Key BiasKey1(31); - double measurement_dt(0.1); - Vector world_g(Vector_(3, 0.0, 0.0, 9.81)); - Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); + double measurement_dt(0.1); + Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); + Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system + gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); - SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); + SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); - // Second test: zero angular motion, some acceleration - Vector measurement_acc(Vector_(3,0.0,0.0,0.0-9.81)); - Vector measurement_gyro(Vector_(3, 0.1, 0.2, 0.3)); + // Second test: zero angular motion, some acceleration + Vector measurement_acc((Vector(3) <<0.0,0.0,0.0-9.81)); + Vector measurement_gyro((Vector(3) << 0.1, 0.2, 0.3)); - InertialNavFactor_GlobalVelocity f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); + InertialNavFactor_GlobalVelocity f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); - Pose3 Pose1(Rot3(), Point3(2.0,1.0,3.0)); - Pose3 Pose2(Rot3::Expmap(measurement_gyro*measurement_dt), Point3(2.0,1.0,3.0)); - LieVector Vel1(3,0.0,0.0,0.0); - LieVector Vel2(3,0.0,0.0,0.0); - imuBias::ConstantBias Bias1; + Pose3 Pose1(Rot3(), Point3(2.0,1.0,3.0)); + Pose3 Pose2(Rot3::Expmap(measurement_gyro*measurement_dt), Point3(2.0,1.0,3.0)); + LieVector Vel1((Vector(3) << 0.0, 0.0, 0.0)); + LieVector Vel2((Vector(3) << 0.0, 0.0, 0.0)); + imuBias::ConstantBias Bias1; - Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); - Vector ExpectedErr(zero(9)); + Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); + Vector ExpectedErr(zero(9)); - CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5)); + CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5)); } /* ************************************************************************* */ @@ -210,67 +211,67 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVel) gtsam::Key VelKey2(22); gtsam::Key BiasKey1(31); - double measurement_dt(0.1); - Vector world_g(Vector_(3, 0.0, 0.0, 9.81)); - Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); + double measurement_dt(0.1); + Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); + Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system + gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); - SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); + SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); - // Second test: zero angular motion, some acceleration - generated in matlab - Vector measurement_acc(Vector_(3, 6.501390843381716, -6.763926150509185, -2.300389940090343)); - Vector measurement_gyro(Vector_(3, 0.1, 0.2, 0.3)); + // Second test: zero angular motion, some acceleration - generated in matlab + Vector measurement_acc((Vector(3) << 6.501390843381716, -6.763926150509185, -2.300389940090343)); + Vector measurement_gyro((Vector(3) << 0.1, 0.2, 0.3)); - InertialNavFactor_GlobalVelocity f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); + InertialNavFactor_GlobalVelocity f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); - Rot3 R1(0.487316618, 0.125253866, 0.86419557, - 0.580273724, 0.693095498, -0.427669306, - -0.652537293, 0.709880342, 0.265075427); - Point3 t1(2.0,1.0,3.0); - Pose3 Pose1(R1, t1); - LieVector Vel1(3,0.5,-0.5,0.4); - Rot3 R2(0.473618898, 0.119523052, 0.872582019, - 0.609241153, 0.67099888, -0.422594037, - -0.636011287, 0.731761397, 0.244979388); - Point3 t2 = t1.compose( Point3(Vel1*measurement_dt) ); - Pose3 Pose2(R2, t2); - Vector dv = measurement_dt * (R1.matrix() * measurement_acc + world_g); - LieVector Vel2 = Vel1.compose( dv ); - imuBias::ConstantBias Bias1; + Rot3 R1(0.487316618, 0.125253866, 0.86419557, + 0.580273724, 0.693095498, -0.427669306, + -0.652537293, 0.709880342, 0.265075427); + Point3 t1(2.0,1.0,3.0); + Pose3 Pose1(R1, t1); + LieVector Vel1((Vector(3) << 0.5, -0.5, 0.4)); + Rot3 R2(0.473618898, 0.119523052, 0.872582019, + 0.609241153, 0.67099888, -0.422594037, + -0.636011287, 0.731761397, 0.244979388); + Point3 t2 = t1.compose( Point3(Vel1*measurement_dt) ); + Pose3 Pose2(R2, t2); + Vector dv = measurement_dt * (R1.matrix() * measurement_acc + world_g); + LieVector Vel2 = Vel1.compose( dv ); + imuBias::ConstantBias Bias1; - Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); - Vector ExpectedErr(zero(9)); + Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); + Vector ExpectedErr(zero(9)); - // TODO: Expected values need to be updated for global velocity version - CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5)); + // TODO: Expected values need to be updated for global velocity version + CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5)); } ///* VADIM - START ************************************************************************* */ //LieVector predictionRq(const LieVector angles, const LieVector q) { -// return (Rot3().RzRyRx(angles) * q).vector(); +// return (Rot3().RzRyRx(angles) * q).vector(); //} // //TEST (InertialNavFactor_GlobalVelocity, Rotation_Deriv ) { -// LieVector angles(Vector_(3, 3.001, -1.0004, 2.0005)); -// Rot3 R1(Rot3().RzRyRx(angles)); -// LieVector q(Vector_(3, 5.8, -2.2, 4.105)); -// Rot3 qx(0.0, -q[2], q[1], -// q[2], 0.0, -q[0], -// -q[1], q[0],0.0); -// Matrix J_hyp( -(R1*qx).matrix() ); +// LieVector angles((Vector(3) << 3.001, -1.0004, 2.0005)); +// Rot3 R1(Rot3().RzRyRx(angles)); +// LieVector q((Vector(3) << 5.8, -2.2, 4.105)); +// Rot3 qx(0.0, -q[2], q[1], +// q[2], 0.0, -q[0], +// -q[1], q[0],0.0); +// Matrix J_hyp( -(R1*qx).matrix() ); // -// gtsam::Matrix J_expected; +// gtsam::Matrix J_expected; // -// LieVector v(predictionRq(angles, q)); +// LieVector v(predictionRq(angles, q)); // -// J_expected = gtsam::numericalDerivative11(boost::bind(&predictionRq, _1, q), angles); +// J_expected = gtsam::numericalDerivative11(boost::bind(&predictionRq, _1, q), angles); // -// cout<<"J_hyp"< factor(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); + InertialNavFactor_GlobalVelocity factor(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); - Rot3 R1(0.487316618, 0.125253866, 0.86419557, - 0.580273724, 0.693095498, -0.427669306, - -0.652537293, 0.709880342, 0.265075427); - Point3 t1(2.0,1.0,3.0); - Pose3 Pose1(R1, t1); - LieVector Vel1(3,0.5,-0.5,0.4); - Rot3 R2(0.473618898, 0.119523052, 0.872582019, - 0.609241153, 0.67099888, -0.422594037, - -0.636011287, 0.731761397, 0.244979388); - Point3 t2(2.052670960415706, 0.977252139079380, 2.942482135362800); - Pose3 Pose2(R2, t2); - LieVector Vel2(3,0.510000000000000, -0.480000000000000, 0.430000000000000); - imuBias::ConstantBias Bias1; + Rot3 R1(0.487316618, 0.125253866, 0.86419557, + 0.580273724, 0.693095498, -0.427669306, + -0.652537293, 0.709880342, 0.265075427); + Point3 t1(2.0,1.0,3.0); + Pose3 Pose1(R1, t1); + LieVector Vel1((Vector(3) << 0.5, -0.5, 0.4)); + Rot3 R2(0.473618898, 0.119523052, 0.872582019, + 0.609241153, 0.67099888, -0.422594037, + -0.636011287, 0.731761397, 0.244979388); + Point3 t2(2.052670960415706, 0.977252139079380, 2.942482135362800); + Pose3 Pose2(R2, t2); + LieVector Vel2((Vector(3) << 0.510000000000000, -0.480000000000000, 0.430000000000000)); + imuBias::ConstantBias Bias1; - Matrix H1_actual, H2_actual, H3_actual, H4_actual, H5_actual; + Matrix H1_actual, H2_actual, H3_actual, H4_actual, H5_actual; - Vector ActualErr(factor.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2, H1_actual, H2_actual, H3_actual, H4_actual, H5_actual)); + Vector ActualErr(factor.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2, H1_actual, H2_actual, H3_actual, H4_actual, H5_actual)); - // Checking for Pose part in the jacobians - // ****** - Matrix H1_actualPose(H1_actual.block(0,0,6,H1_actual.cols())); - Matrix H2_actualPose(H2_actual.block(0,0,6,H2_actual.cols())); - Matrix H3_actualPose(H3_actual.block(0,0,6,H3_actual.cols())); - Matrix H4_actualPose(H4_actual.block(0,0,6,H4_actual.cols())); - Matrix H5_actualPose(H5_actual.block(0,0,6,H5_actual.cols())); + // Checking for Pose part in the jacobians + // ****** + Matrix H1_actualPose(H1_actual.block(0,0,6,H1_actual.cols())); + Matrix H2_actualPose(H2_actual.block(0,0,6,H2_actual.cols())); + Matrix H3_actualPose(H3_actual.block(0,0,6,H3_actual.cols())); + Matrix H4_actualPose(H4_actual.block(0,0,6,H4_actual.cols())); + Matrix H5_actualPose(H5_actual.block(0,0,6,H5_actual.cols())); - // Calculate the Jacobian matrices H1 until H5 using the numerical derivative function - gtsam::Matrix H1_expectedPose, H2_expectedPose, H3_expectedPose, H4_expectedPose, H5_expectedPose; - H1_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); - H2_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1); - H3_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1); - H4_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2); - H5_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2); + // Calculate the Jacobian matrices H1 until H5 using the numerical derivative function + gtsam::Matrix H1_expectedPose, H2_expectedPose, H3_expectedPose, H4_expectedPose, H5_expectedPose; + H1_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); + H2_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1); + H3_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1); + H4_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2); + H5_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2); - // Verify they are equal for this choice of state - CHECK( gtsam::assert_equal(H1_expectedPose, H1_actualPose, 1e-5)); - CHECK( gtsam::assert_equal(H2_expectedPose, H2_actualPose, 1e-5)); - CHECK( gtsam::assert_equal(H3_expectedPose, H3_actualPose, 2e-3)); - CHECK( gtsam::assert_equal(H4_expectedPose, H4_actualPose, 1e-5)); - CHECK( gtsam::assert_equal(H5_expectedPose, H5_actualPose, 1e-5)); + // Verify they are equal for this choice of state + CHECK( gtsam::assert_equal(H1_expectedPose, H1_actualPose, 1e-5)); + CHECK( gtsam::assert_equal(H2_expectedPose, H2_actualPose, 1e-5)); + CHECK( gtsam::assert_equal(H3_expectedPose, H3_actualPose, 2e-3)); + CHECK( gtsam::assert_equal(H4_expectedPose, H4_actualPose, 1e-5)); + CHECK( gtsam::assert_equal(H5_expectedPose, H5_actualPose, 1e-5)); - // Checking for Vel part in the jacobians - // ****** - Matrix H1_actualVel(H1_actual.block(6,0,3,H1_actual.cols())); - Matrix H2_actualVel(H2_actual.block(6,0,3,H2_actual.cols())); - Matrix H3_actualVel(H3_actual.block(6,0,3,H3_actual.cols())); - Matrix H4_actualVel(H4_actual.block(6,0,3,H4_actual.cols())); - Matrix H5_actualVel(H5_actual.block(6,0,3,H5_actual.cols())); + // Checking for Vel part in the jacobians + // ****** + Matrix H1_actualVel(H1_actual.block(6,0,3,H1_actual.cols())); + Matrix H2_actualVel(H2_actual.block(6,0,3,H2_actual.cols())); + Matrix H3_actualVel(H3_actual.block(6,0,3,H3_actual.cols())); + Matrix H4_actualVel(H4_actual.block(6,0,3,H4_actual.cols())); + Matrix H5_actualVel(H5_actual.block(6,0,3,H5_actual.cols())); - // Calculate the Jacobian matrices H1 until H5 using the numerical derivative function - gtsam::Matrix H1_expectedVel, H2_expectedVel, H3_expectedVel, H4_expectedVel, H5_expectedVel; - H1_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); - H2_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1); - H3_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1); - H4_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2); - H5_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2); + // Calculate the Jacobian matrices H1 until H5 using the numerical derivative function + gtsam::Matrix H1_expectedVel, H2_expectedVel, H3_expectedVel, H4_expectedVel, H5_expectedVel; + H1_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); + H2_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1); + H3_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1); + H4_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2); + H5_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2); - // Verify they are equal for this choice of state - CHECK( gtsam::assert_equal(H1_expectedVel, H1_actualVel, 1e-5)); - CHECK( gtsam::assert_equal(H2_expectedVel, H2_actualVel, 1e-5)); - CHECK( gtsam::assert_equal(H3_expectedVel, H3_actualVel, 1e-5)); - CHECK( gtsam::assert_equal(H4_expectedVel, H4_actualVel, 1e-5)); - CHECK( gtsam::assert_equal(H5_expectedVel, H5_actualVel, 1e-5)); + // Verify they are equal for this choice of state + CHECK( gtsam::assert_equal(H1_expectedVel, H1_actualVel, 1e-5)); + CHECK( gtsam::assert_equal(H2_expectedVel, H2_actualVel, 1e-5)); + CHECK( gtsam::assert_equal(H3_expectedVel, H3_actualVel, 1e-5)); + CHECK( gtsam::assert_equal(H4_expectedVel, H4_actualVel, 1e-5)); + CHECK( gtsam::assert_equal(H5_expectedVel, H5_actualVel, 1e-5)); } @@ -373,13 +374,13 @@ TEST( InertialNavFactor_GlobalVelocity, ConstructorWithTransform) gtsam::Key Vel2(22); gtsam::Key Bias1(31); - Vector measurement_acc(Vector_(3, 0.1, 0.2, 0.4)); - Vector measurement_gyro(Vector_(3, -0.2, 0.5, 0.03)); + Vector measurement_acc((Vector(3) << 0.1, 0.2, 0.4)); + Vector measurement_gyro((Vector(3) << -0.2, 0.5, 0.03)); double measurement_dt(0.1); - Vector world_g(Vector_(3, 0.0, 0.0, 9.81)); - Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); + Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); + Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system + gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); @@ -399,13 +400,13 @@ TEST( InertialNavFactor_GlobalVelocity, EqualsWithTransform) gtsam::Key Vel2(22); gtsam::Key Bias1(31); - Vector measurement_acc(Vector_(3, 0.1, 0.2, 0.4)); - Vector measurement_gyro(Vector_(3, -0.2, 0.5, 0.03)); + Vector measurement_acc((Vector(3) << 0.1, 0.2, 0.4)); + Vector measurement_gyro((Vector(3) << -0.2, 0.5, 0.03)); double measurement_dt(0.1); - Vector world_g(Vector_(3, 0.0, 0.0, 9.81)); - Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); + Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); + Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system + gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); @@ -428,9 +429,9 @@ TEST( InertialNavFactor_GlobalVelocity, PredictWithTransform) gtsam::Key BiasKey1(31); double measurement_dt(0.1); - Vector world_g(Vector_(3, 0.0, 0.0, 9.81)); - Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); + Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); + Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system + gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); @@ -439,17 +440,17 @@ TEST( InertialNavFactor_GlobalVelocity, PredictWithTransform) // First test: zero angular motion, some acceleration - Vector measurement_gyro(Vector_(3, 0.0, 0.0, 0.0)); // Measured in ENU orientation + Vector measurement_gyro((Vector(3) << 0.0, 0.0, 0.0)); // Measured in ENU orientation Matrix omega__cross = skewSymmetric(measurement_gyro); - Vector measurement_acc = Vector_(3, 0.2, 0.1, -0.3+9.81) + omega__cross*omega__cross*body_P_sensor.rotation().inverse().matrix()*body_P_sensor.translation().vector(); // Measured in ENU orientation + Vector measurement_acc = (Vector(3) << 0.2, 0.1, -0.3+9.81) + omega__cross*omega__cross*body_P_sensor.rotation().inverse().matrix()*body_P_sensor.translation().vector(); // Measured in ENU orientation InertialNavFactor_GlobalVelocity f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model, body_P_sensor); Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00)); - LieVector Vel1(3, 0.50, -0.50, 0.40); + LieVector Vel1((Vector(3) << 0.50, -0.50, 0.40)); imuBias::ConstantBias Bias1; Pose3 expectedPose2(Rot3(), Point3(2.05, 0.95, 3.04)); - LieVector expectedVel2(3, 0.51, -0.48, 0.43); + LieVector expectedVel2((Vector(3) << 0.51, -0.48, 0.43)); Pose3 actualPose2; LieVector actualVel2; f.predict(Pose1, Vel1, Bias1, actualPose2, actualVel2); @@ -468,9 +469,9 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorPosVelWithTransform) gtsam::Key BiasKey1(31); double measurement_dt(0.1); - Vector world_g(Vector_(3, 0.0, 0.0, 9.81)); - Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); + Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); + Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system + gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); @@ -479,16 +480,16 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorPosVelWithTransform) // First test: zero angular motion, some acceleration - Vector measurement_gyro(Vector_(3, 0.0, 0.0, 0.0)); // Measured in ENU orientation + Vector measurement_gyro((Vector(3) << 0.0, 0.0, 0.0)); // Measured in ENU orientation Matrix omega__cross = skewSymmetric(measurement_gyro); - Vector measurement_acc = Vector_(3, 0.2, 0.1, -0.3+9.81) + omega__cross*omega__cross*body_P_sensor.rotation().inverse().matrix()*body_P_sensor.translation().vector(); // Measured in ENU orientation + Vector measurement_acc = (Vector(3) << 0.2, 0.1, -0.3+9.81) + omega__cross*omega__cross*body_P_sensor.rotation().inverse().matrix()*body_P_sensor.translation().vector(); // Measured in ENU orientation InertialNavFactor_GlobalVelocity f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model, body_P_sensor); Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00)); Pose3 Pose2(Rot3(), Point3(2.05, 0.95, 3.04)); - LieVector Vel1(3, 0.50, -0.50, 0.40); - LieVector Vel2(3, 0.51, -0.48, 0.43); + LieVector Vel1((Vector(3) << 0.50, -0.50, 0.40)); + LieVector Vel2((Vector(3) << 0.51, -0.48, 0.43)); imuBias::ConstantBias Bias1; Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); @@ -507,9 +508,9 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotWithTransform) gtsam::Key BiasKey1(31); double measurement_dt(0.1); - Vector world_g(Vector_(3, 0.0, 0.0, 9.81)); - Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); + Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); + Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system + gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); @@ -518,16 +519,16 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotWithTransform) // Second test: zero angular motion, some acceleration - Vector measurement_gyro(Vector_(3, 0.2, 0.1, -0.3)); // Measured in ENU orientation + Vector measurement_gyro((Vector(3) << 0.2, 0.1, -0.3)); // Measured in ENU orientation Matrix omega__cross = skewSymmetric(measurement_gyro); - Vector measurement_acc = Vector_(3, 0.0, 0.0, 0.0+9.81) + omega__cross*omega__cross*body_P_sensor.rotation().inverse().matrix()*body_P_sensor.translation().vector(); // Measured in ENU orientation + Vector measurement_acc = (Vector(3) << 0.0, 0.0, 0.0+9.81) + omega__cross*omega__cross*body_P_sensor.rotation().inverse().matrix()*body_P_sensor.translation().vector(); // Measured in ENU orientation InertialNavFactor_GlobalVelocity f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model, body_P_sensor); Pose3 Pose1(Rot3(), Point3(2.0,1.0,3.0)); Pose3 Pose2(Rot3::Expmap(body_P_sensor.rotation().matrix()*measurement_gyro*measurement_dt), Point3(2.0, 1.0, 3.0)); - LieVector Vel1(3,0.0,0.0,0.0); - LieVector Vel2(3,0.0,0.0,0.0); + LieVector Vel1((Vector(3) << 0.0,0.0,0.0)); + LieVector Vel2((Vector(3) << 0.0,0.0,0.0)); imuBias::ConstantBias Bias1; Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); @@ -546,9 +547,9 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVelWithTransform) gtsam::Key BiasKey1(31); double measurement_dt(0.1); - Vector world_g(Vector_(3, 0.0, 0.0, 9.81)); - Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); + Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); + Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system + gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); @@ -557,9 +558,9 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVelWithTransform) // Second test: zero angular motion, some acceleration - generated in matlab - Vector measurement_gyro(Vector_(3, 0.2, 0.1, -0.3)); // Measured in ENU orientation + Vector measurement_gyro((Vector(3) << 0.2, 0.1, -0.3)); // Measured in ENU orientation Matrix omega__cross = skewSymmetric(measurement_gyro); - Vector measurement_acc = Vector_(3, -6.763926150509185, 6.501390843381716, +2.300389940090343) + omega__cross*omega__cross*body_P_sensor.rotation().inverse().matrix()*body_P_sensor.translation().vector(); // Measured in ENU orientation + Vector measurement_acc = (Vector(3) << -6.763926150509185, 6.501390843381716, +2.300389940090343) + omega__cross*omega__cross*body_P_sensor.rotation().inverse().matrix()*body_P_sensor.translation().vector(); // Measured in ENU orientation InertialNavFactor_GlobalVelocity f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model, body_P_sensor); @@ -568,13 +569,13 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVelWithTransform) -0.652537293, 0.709880342, 0.265075427); Point3 t1(2.0,1.0,3.0); Pose3 Pose1(R1, t1); - LieVector Vel1(3,0.5,-0.5,0.4); + LieVector Vel1((Vector(3) << 0.5,-0.5,0.4)); Rot3 R2(0.473618898, 0.119523052, 0.872582019, 0.609241153, 0.67099888, -0.422594037, -0.636011287, 0.731761397, 0.244979388); Point3 t2 = t1.compose( Point3(Vel1*measurement_dt) ); Pose3 Pose2(R2, t2); - Vector dv = measurement_dt * (R1.matrix() * body_P_sensor.rotation().matrix() * Vector_(3, -6.763926150509185, 6.501390843381716, +2.300389940090343) + world_g); + Vector dv = measurement_dt * (R1.matrix() * body_P_sensor.rotation().matrix() * (Vector(3) << -6.763926150509185, 6.501390843381716, +2.300389940090343) + world_g); LieVector Vel2 = Vel1.compose( dv ); imuBias::ConstantBias Bias1; @@ -595,9 +596,9 @@ TEST (InertialNavFactor_GlobalVelocity, JacobianWithTransform ) { gtsam::Key BiasKey1(31); double measurement_dt(0.01); - Vector world_g(Vector_(3, 0.0, 0.0, 9.81)); - Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); + Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); + Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system + gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); @@ -605,9 +606,9 @@ TEST (InertialNavFactor_GlobalVelocity, JacobianWithTransform ) { Pose3 body_P_sensor(Rot3(0, 1, 0, 1, 0, 0, 0, 0, -1), Point3(1.0, -2.0, 3.0)); // IMU is in ENU orientation - Vector measurement_gyro(Vector_(3, 3.14/2, 3.14, +3.14)); // Measured in ENU orientation + Vector measurement_gyro((Vector(3) << 3.14/2, 3.14, +3.14)); // Measured in ENU orientation Matrix omega__cross = skewSymmetric(measurement_gyro); - Vector measurement_acc = Vector_(3, -6.763926150509185, 6.501390843381716, +2.300389940090343) + omega__cross*omega__cross*body_P_sensor.rotation().inverse().matrix()*body_P_sensor.translation().vector(); // Measured in ENU orientation + Vector measurement_acc = (Vector(3) << -6.763926150509185, 6.501390843381716, +2.300389940090343) + omega__cross*omega__cross*body_P_sensor.rotation().inverse().matrix()*body_P_sensor.translation().vector(); // Measured in ENU orientation InertialNavFactor_GlobalVelocity factor(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model, body_P_sensor); @@ -617,13 +618,13 @@ TEST (InertialNavFactor_GlobalVelocity, JacobianWithTransform ) { -0.652537293, 0.709880342, 0.265075427); Point3 t1(2.0,1.0,3.0); Pose3 Pose1(R1, t1); - LieVector Vel1(3,0.5,-0.5,0.4); + LieVector Vel1((Vector(3) << 0.5,-0.5,0.4)); Rot3 R2(0.473618898, 0.119523052, 0.872582019, 0.609241153, 0.67099888, -0.422594037, -0.636011287, 0.731761397, 0.244979388); Point3 t2(2.052670960415706, 0.977252139079380, 2.942482135362800); Pose3 Pose2(R2, t2); - LieVector Vel2(3,0.510000000000000, -0.480000000000000, 0.430000000000000); + LieVector Vel2((Vector(3) << 0.510000000000000, -0.480000000000000, 0.430000000000000)); imuBias::ConstantBias Bias1; Matrix H1_actual, H2_actual, H3_actual, H4_actual, H5_actual; @@ -678,5 +679,5 @@ TEST (InertialNavFactor_GlobalVelocity, JacobianWithTransform ) { } /* ************************************************************************* */ - int main() { TestResult tr; return TestRegistry::runAllTests(tr);} + int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ diff --git a/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp b/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp index 9ab38bb37..5ea9fe29d 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp @@ -11,7 +11,7 @@ #include #include #include -#include +#include #include @@ -38,7 +38,7 @@ TEST( InvDepthFactor, optimize) { Point2 expected_uv = level_camera.project(landmark); InvDepthCamera3 inv_camera(level_pose, K); - LieVector inv_landmark(5, 0., 0., 1., 0., 0.); + LieVector inv_landmark((Vector(5) << 0., 0., 1., 0., 0.)); // initialize inverse depth with "incorrect" depth of 1/4 // in reality this is 1/5, but initial depth is guessed LieScalar inv_depth(1./4); @@ -49,7 +49,7 @@ TEST( InvDepthFactor, optimize) { InverseDepthFactor::shared_ptr factor(new InverseDepthFactor(expected_uv, sigma, Symbol('x',1), Symbol('l',1), Symbol('d',1), K)); graph.push_back(factor); - graph.add(PoseConstraint(Symbol('x',1),level_pose)); + graph += PoseConstraint(Symbol('x',1),level_pose); initial.insert(Symbol('x',1), level_pose); initial.insert(Symbol('l',1), inv_landmark); initial.insert(Symbol('d',1), inv_depth); @@ -75,7 +75,7 @@ TEST( InvDepthFactor, optimize) { Symbol('x',2), Symbol('l',1),Symbol('d',1),K)); graph.push_back(factor1); - graph.add(PoseConstraint(Symbol('x',2),right_pose)); + graph += PoseConstraint(Symbol('x',2),right_pose); initial.insert(Symbol('x',2), right_pose); diff --git a/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp b/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp index b8f24dda8..24535854d 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp @@ -10,7 +10,7 @@ #include #include #include -#include +#include #include #include #include @@ -45,7 +45,7 @@ TEST( InvDepthFactorVariant1, optimize) { double theta = atan2(ray.y(), ray.x()); double phi = atan2(ray.z(), sqrt(ray.x()*ray.x()+ray.y()*ray.y())); double rho = 1./ray.norm(); - LieVector expected(6, x, y, z, theta, phi, rho); + LieVector expected((Vector(6) << x, y, z, theta, phi, rho)); @@ -66,9 +66,9 @@ TEST( InvDepthFactorVariant1, optimize) { // Create a values with slightly incorrect initial conditions Values values; - values.insert(poseKey1, pose1.retract(Vector_(6, +0.01, -0.02, +0.03, -0.10, +0.20, -0.30))); - values.insert(poseKey2, pose2.retract(Vector_(6, +0.01, +0.02, -0.03, -0.10, +0.20, +0.30))); - values.insert(landmarkKey, expected.retract(Vector_(6, -0.20, +0.20, -0.35, +0.02, -0.04, +0.05))); + values.insert(poseKey1, pose1.retract((Vector(6) << +0.01, -0.02, +0.03, -0.10, +0.20, -0.30))); + values.insert(poseKey2, pose2.retract((Vector(6) << +0.01, +0.02, -0.03, -0.10, +0.20, +0.30))); + values.insert(landmarkKey, expected.retract((Vector(6) << -0.20, +0.20, -0.35, +0.02, -0.04, +0.05))); // Optimize the graph to recover the actual landmark position LevenbergMarquardtParams params; diff --git a/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp b/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp index 4706a9338..e99c9bcdf 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp @@ -10,7 +10,7 @@ #include #include #include -#include +#include #include #include #include @@ -43,7 +43,7 @@ TEST( InvDepthFactorVariant2, optimize) { double theta = atan2(ray.y(), ray.x()); double phi = atan2(ray.z(), sqrt(ray.x()*ray.x()+ray.y()*ray.y())); double rho = 1./ray.norm(); - LieVector expected(3, theta, phi, rho); + LieVector expected((Vector(3) << theta, phi, rho)); @@ -64,9 +64,9 @@ TEST( InvDepthFactorVariant2, optimize) { // Create a values with slightly incorrect initial conditions Values values; - values.insert(poseKey1, pose1.retract(Vector_(6, +0.01, -0.02, +0.03, -0.10, +0.20, -0.30))); - values.insert(poseKey2, pose2.retract(Vector_(6, +0.01, +0.02, -0.03, -0.10, +0.20, +0.30))); - values.insert(landmarkKey, expected.retract(Vector_(3, +0.02, -0.04, +0.05))); + values.insert(poseKey1, pose1.retract((Vector(6) << +0.01, -0.02, +0.03, -0.10, +0.20, -0.30))); + values.insert(poseKey2, pose2.retract((Vector(6) << +0.01, +0.02, -0.03, -0.10, +0.20, +0.30))); + values.insert(landmarkKey, expected.retract((Vector(3) << +0.02, -0.04, +0.05))); // Optimize the graph to recover the actual landmark position LevenbergMarquardtParams params; diff --git a/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp b/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp index c15998507..e65b7cacb 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp @@ -10,7 +10,7 @@ #include #include #include -#include +#include #include #include #include @@ -43,7 +43,7 @@ TEST( InvDepthFactorVariant3, optimize) { double theta = atan2(landmark_p1.x(), landmark_p1.z()); double phi = atan2(landmark_p1.y(), sqrt(landmark_p1.x()*landmark_p1.x()+landmark_p1.z()*landmark_p1.z())); double rho = 1./landmark_p1.norm(); - LieVector expected(3, theta, phi, rho); + LieVector expected((Vector(3) << theta, phi, rho)); @@ -64,9 +64,9 @@ TEST( InvDepthFactorVariant3, optimize) { // Create a values with slightly incorrect initial conditions Values values; - values.insert(poseKey1, pose1.retract(Vector_(6, +0.01, -0.02, +0.03, -0.10, +0.20, -0.30))); - values.insert(poseKey2, pose2.retract(Vector_(6, +0.01, +0.02, -0.03, -0.10, +0.20, +0.30))); - values.insert(landmarkKey, expected.retract(Vector_(3, +0.02, -0.04, +0.05))); + values.insert(poseKey1, pose1.retract((Vector(6) << +0.01, -0.02, +0.03, -0.10, +0.20, -0.30))); + values.insert(poseKey2, pose2.retract((Vector(6) << +0.01, +0.02, -0.03, -0.10, +0.20, +0.30))); + values.insert(landmarkKey, expected.retract((Vector(3) << +0.02, -0.04, +0.05))); // Optimize the graph to recover the actual landmark position LevenbergMarquardtParams params; diff --git a/gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp b/gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp index 1cb338b4b..cbc1022fc 100644 --- a/gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp +++ b/gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp @@ -16,7 +16,6 @@ * @date Nov 2009 */ -#include #include #include #include @@ -25,11 +24,10 @@ #include #include #include -#include +#include #include -#include -#include -#include +#include +#include #include #include #include @@ -44,7 +42,7 @@ using namespace gtsam; // make a realistic calibration matrix static double fov = 60; // degrees -static size_t w=640,h=480; +static int w=640,h=480; static Cal3_S2::shared_ptr K(new Cal3_S2(fov,w,h)); // Create a noise model for the pixel error @@ -77,7 +75,7 @@ TEST( MultiProjectionFactor, create ){ views.insert(x3); MultiProjectionFactor mpFactor(n_measPixel, noiseProjection, views, l1, K); - graph.add(mpFactor); + graph += mpFactor; } @@ -149,7 +147,7 @@ TEST( MultiProjectionFactor, create ){ // Vector actualError(factor.evaluateError(pose, point)); // // // The expected error is (-3.0, 0.0) pixels / UnitCovariance -// Vector expectedError = Vector_(2, -3.0, 0.0); +// Vector expectedError = (Vector(2) << -3.0, 0.0); // // // Verify we get the expected error // CHECK(assert_equal(expectedError, actualError, 1e-9)); @@ -172,7 +170,7 @@ TEST( MultiProjectionFactor, create ){ // Vector actualError(factor.evaluateError(pose, point)); // // // The expected error is (-3.0, 0.0) pixels / UnitCovariance -// Vector expectedError = Vector_(2, -3.0, 0.0); +// Vector expectedError = (Vector(2) << -3.0, 0.0); // // // Verify we get the expected error // CHECK(assert_equal(expectedError, actualError, 1e-9)); @@ -195,8 +193,8 @@ TEST( MultiProjectionFactor, create ){ // factor.evaluateError(pose, point, H1Actual, H2Actual); // // // The expected Jacobians -// Matrix H1Expected = Matrix_(2, 6, 0., -554.256, 0., -92.376, 0., 0., 554.256, 0., 0., 0., -92.376, 0.); -// Matrix H2Expected = Matrix_(2, 3, 92.376, 0., 0., 0., 92.376, 0.); +// Matrix H1Expected = (Matrix(2, 6) << 0., -554.256, 0., -92.376, 0., 0., 554.256, 0., 0., 0., -92.376, 0.); +// Matrix H2Expected = (Matrix(2, 3) << 92.376, 0., 0., 0., 92.376, 0.); // // // Verify the Jacobians are correct // CHECK(assert_equal(H1Expected, H1Actual, 1e-3)); @@ -221,8 +219,8 @@ TEST( MultiProjectionFactor, create ){ // factor.evaluateError(pose, point, H1Actual, H2Actual); // // // The expected Jacobians -// Matrix H1Expected = Matrix_(2, 6, -92.376, 0., 577.350, 0., 92.376, 0., -9.2376, -577.350, 0., 0., 0., 92.376); -// Matrix H2Expected = Matrix_(2, 3, 0., -92.376, 0., 0., 0., -92.376); +// Matrix H1Expected = (Matrix(2, 6) << -92.376, 0., 577.350, 0., 92.376, 0., -9.2376, -577.350, 0., 0., 0., 92.376); +// Matrix H2Expected = (Matrix(2, 3) << 0., -92.376, 0., 0., 0., -92.376); // // // Verify the Jacobians are correct // CHECK(assert_equal(H1Expected, H1Actual, 1e-3)); diff --git a/tests/testOccupancyGrid.cpp b/gtsam_unstable/slam/tests/testOccupancyGrid.cpp similarity index 99% rename from tests/testOccupancyGrid.cpp rename to gtsam_unstable/slam/tests/testOccupancyGrid.cpp index 3f24003c0..d92e5442e 100644 --- a/tests/testOccupancyGrid.cpp +++ b/gtsam_unstable/slam/tests/testOccupancyGrid.cpp @@ -5,10 +5,12 @@ * @author Frank Dellaert */ +#include + +#if 0 #include #include -#include #include //#include // FIXME: does not exist in boost 1.46 #include // Old header - should still exist @@ -282,7 +284,7 @@ public: }; /* ************************************************************************* */ -TEST_UNSAFE( OccupancyGrid, Test1) { +TEST( OccupancyGrid, Test1) { //Build a small grid and test optimization //Build small grid @@ -323,6 +325,8 @@ TEST_UNSAFE( OccupancyGrid, Test1) { } +#endif + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp b/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp index 3063571cc..bf9dc0e8e 100644 --- a/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp +++ b/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp @@ -17,7 +17,7 @@ */ #include -#include +#include #include #include #include @@ -95,12 +95,21 @@ TEST( PoseBetweenFactor, Error ) { // The expected error Vector expectedError(6); +#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) + expectedError << -0.0298135267953815, + 0.0131341515747393, + 0.0968868439682154, + -0.145701634472172, + -0.134898525569125, + -0.0421026389164264; +#else expectedError << -0.029839512616488, 0.013145599455949, 0.096971291682578, -0.139187549519821, -0.142346243063553, -0.039088532100977; +#endif // Create a factor and calculate the error Key poseKey1(1); @@ -123,12 +132,23 @@ TEST( PoseBetweenFactor, ErrorWithTransform ) { // The expected error Vector expectedError(6); + // TODO: The solution depends on choice of Pose3 and Rot3 Expmap mode! +#if defined(GTSAM_ROT3_EXPMAP) + expectedError << 0.0173358202010741, + 0.0222210698409755, + -0.0125032003886145, + 0.0263800787416566, + 0.00540285006310398, + 0.000175859555693563; +#else expectedError << 0.017337193670445, 0.022222830355243, -0.012504190982804, 0.026413288603739, 0.005237695303536, -0.000071612703633; +#endif + // Create a factor and calculate the error Key poseKey1(1); @@ -165,8 +185,8 @@ TEST( PoseBetweenFactor, Jacobian ) { factor.evaluateError(pose1, pose2, actualH1, actualH2); // Verify we get the expected error - CHECK(assert_equal(expectedH1, actualH1, 1e-9)); - CHECK(assert_equal(expectedH2, actualH2, 1e-9)); + CHECK(assert_equal(expectedH1, actualH1, 1e-5)); + CHECK(assert_equal(expectedH2, actualH2, 1e-6)); } /* ************************************************************************* */ @@ -194,8 +214,8 @@ TEST( PoseBetweenFactor, JacobianWithTransform ) { Vector error = factor.evaluateError(pose1, pose2, actualH1, actualH2); // Verify we get the expected error - CHECK(assert_equal(expectedH1, actualH1, 1e-9)); - CHECK(assert_equal(expectedH2, actualH2, 1e-9)); + CHECK(assert_equal(expectedH1, actualH1, 1e-6)); + CHECK(assert_equal(expectedH2, actualH2, 1e-5)); } /* ************************************************************************* */ diff --git a/gtsam_unstable/slam/tests/testPosePriorFactor.cpp b/gtsam_unstable/slam/tests/testPosePriorFactor.cpp index c9825d582..cbfa45431 100644 --- a/gtsam_unstable/slam/tests/testPosePriorFactor.cpp +++ b/gtsam_unstable/slam/tests/testPosePriorFactor.cpp @@ -17,7 +17,7 @@ */ #include -#include +#include #include #include #include @@ -90,12 +90,23 @@ TEST( PosePriorFactor, Error ) { // The expected error Vector expectedError(6); + // TODO: The solution depends on choice of Pose3 and Rot3 Expmap mode! +#if defined(GTSAM_ROT3_EXPMAP) + expectedError << -0.182948257976108, + 0.13851858011118, + -0.157375974517456, + 0.766913166076379, + -1.22976117053126, + 0.949345561430261; +#else expectedError << -0.184137861505414, 0.139419283914526, -0.158399296722242, 0.740211733817804, -1.198210282560946, 1.008156093015192; +#endif + // Create a factor and calculate the error Key poseKey(1); @@ -116,12 +127,22 @@ TEST( PosePriorFactor, ErrorWithTransform ) { // The expected error Vector expectedError(6); + // TODO: The solution depends on choice of Pose3 and Rot3 Expmap mode! +#if defined(GTSAM_ROT3_EXPMAP) + expectedError << -0.0224998729281528, + 0.191947887288328, + 0.273826035236257, + 1.36483391560855, + -0.754590051075035, + 0.585710674473659; +#else expectedError << -0.022712885347328, 0.193765110165872, 0.276418420819283, 1.497519863757366, -0.549375791422721, 0.452761203187666; +#endif // Create a factor and calculate the error Key poseKey(1); @@ -152,7 +173,7 @@ TEST( PosePriorFactor, Jacobian ) { factor.evaluateError(pose, actualH1); // Verify we get the expected error - CHECK(assert_equal(expectedH1, actualH1, 1e-9)); + CHECK(assert_equal(expectedH1, actualH1, 1e-5)); } /* ************************************************************************* */ @@ -176,7 +197,7 @@ TEST( PosePriorFactor, JacobianWithTransform ) { factor.evaluateError(pose, actualH1); // Verify we get the expected error - CHECK(assert_equal(expectedH1, actualH1, 1e-9)); + CHECK(assert_equal(expectedH1, actualH1, 1e-5)); } /* ************************************************************************* */ diff --git a/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp b/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp index be269be61..ffc7344cf 100644 --- a/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp +++ b/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp @@ -49,7 +49,7 @@ TEST( testRelativeElevationFactor, level_positive ) { double measured = 0.0; RelativeElevationFactor factor(poseKey, pointKey, measured, model1); Matrix actH1, actH2; - EXPECT(assert_equal(Vector_(1, 2.0), factor.evaluateError(pose1, point1, actH1, actH2))); + EXPECT(assert_equal((Vector(1) << 2.0), factor.evaluateError(pose1, point1, actH1, actH2))); Matrix expH1 = numericalDerivative21( boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5); Matrix expH2 = numericalDerivative22( @@ -64,7 +64,7 @@ TEST( testRelativeElevationFactor, level_negative ) { double measured = -1.0; RelativeElevationFactor factor(poseKey, pointKey, measured, model1); Matrix actH1, actH2; - EXPECT(assert_equal(Vector_(1, 3.0), factor.evaluateError(pose1, point1, actH1, actH2))); + EXPECT(assert_equal((Vector(1) << 3.0), factor.evaluateError(pose1, point1, actH1, actH2))); Matrix expH1 = numericalDerivative21( boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5); Matrix expH2 = numericalDerivative22( @@ -94,7 +94,7 @@ TEST( testRelativeElevationFactor, rotated_positive ) { double measured = 0.0; RelativeElevationFactor factor(poseKey, pointKey, measured, model1); Matrix actH1, actH2; - EXPECT(assert_equal(Vector_(1, 2.0), factor.evaluateError(pose2, point1, actH1, actH2))); + EXPECT(assert_equal((Vector(1) << 2.0), factor.evaluateError(pose2, point1, actH1, actH2))); Matrix expH1 = numericalDerivative21( boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5); Matrix expH2 = numericalDerivative22( @@ -109,7 +109,7 @@ TEST( testRelativeElevationFactor, rotated_negative1 ) { double measured = -1.0; RelativeElevationFactor factor(poseKey, pointKey, measured, model1); Matrix actH1, actH2; - EXPECT(assert_equal(Vector_(1, 3.0), factor.evaluateError(pose2, point1, actH1, actH2))); + EXPECT(assert_equal((Vector(1) << 3.0), factor.evaluateError(pose2, point1, actH1, actH2))); Matrix expH1 = numericalDerivative21( boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5); Matrix expH2 = numericalDerivative22( @@ -124,7 +124,7 @@ TEST( testRelativeElevationFactor, rotated_negative2 ) { double measured = -1.0; RelativeElevationFactor factor(poseKey, pointKey, measured, model1); Matrix actH1, actH2; - EXPECT(assert_equal(Vector_(1, 3.0), factor.evaluateError(pose3, point1, actH1, actH2))); + EXPECT(assert_equal((Vector(1) << 3.0), factor.evaluateError(pose3, point1, actH1, actH2))); Matrix expH1 = numericalDerivative21( boost::bind(evalFactorError, factor, _1, _2), pose3, point1, 1e-5); Matrix expH2 = numericalDerivative22( diff --git a/gtsam_unstable/slam/tests/testSerialization.cpp b/gtsam_unstable/slam/tests/testSerialization.cpp index 616b5b6b7..a5e09515c 100644 --- a/gtsam_unstable/slam/tests/testSerialization.cpp +++ b/gtsam_unstable/slam/tests/testSerialization.cpp @@ -8,6 +8,7 @@ */ #include +#include #include diff --git a/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp b/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp deleted file mode 100644 index cd24a9572..000000000 --- a/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp +++ /dev/null @@ -1,538 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 TestSmartProjectionFactor.cpp - * @brief Unit tests for ProjectionFactor Class - * @author Frank Dellaert - * @date Nov 2009 - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - - -using namespace std; -using namespace gtsam; - -// make a realistic calibration matrix -static double fov = 60; // degrees -static size_t w=640,h=480; -static Cal3_S2::shared_ptr K(new Cal3_S2(fov,w,h)); - -// Create a noise model for the pixel error -static SharedNoiseModel model(noiseModel::Unit::Create(2)); - -// Convenience for named keys -using symbol_shorthand::X; -using symbol_shorthand::L; - -typedef SmartProjectionFactor TestSmartProjectionFactor; - -/* ************************************************************************* */ -TEST( SmartProjectionFactor, Constructor) { - Key poseKey(X(1)); - - std::vector views; - views += poseKey; - - std::vector measurements; - measurements.push_back(Point2(323.0, 240.0)); - - TestSmartProjectionFactor factor(measurements, model, views, K); -} - -/* ************************************************************************* */ -TEST( SmartProjectionFactor, ConstructorWithTransform) { - Key poseKey(X(1)); - - std::vector views; - views += poseKey; - - std::vector measurements; - measurements.push_back(Point2(323.0, 240.0)); - Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); - - TestSmartProjectionFactor factor(measurements, model, views, K, boost::none, body_P_sensor); -} - -/* ************************************************************************* */ -TEST( SmartProjectionFactor, Equals ) { - // Create two identical factors and make sure they're equal - std::vector measurements; - measurements.push_back(Point2(323.0, 240.0)); - - std::vector views; - views += X(1); - TestSmartProjectionFactor factor1(measurements, model, views, K); - TestSmartProjectionFactor factor2(measurements, model, views, K); - - CHECK(assert_equal(factor1, factor2)); -} - -/* ************************************************************************* */ -TEST( SmartProjectionFactor, EqualsWithTransform ) { - // Create two identical factors and make sure they're equal - std::vector measurements; - measurements.push_back(Point2(323.0, 240.0)); - Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); - - std::vector views; - views += X(1); - TestSmartProjectionFactor factor1(measurements, model, views, K, boost::none, body_P_sensor); - TestSmartProjectionFactor factor2(measurements, model, views, K, boost::none, body_P_sensor); - - CHECK(assert_equal(factor1, factor2)); -} - - -/* ************************************************************************* */ -TEST( SmartProjectionFactor, noisy ){ - cout << " ************************ MultiProjectionFactor: noisy ****************************" << endl; - - Symbol x1('X', 1); - Symbol x2('X', 2); - - const SharedDiagonal noiseProjection = noiseModel::Isotropic::Sigma(2, 1); - - std::vector views; - views += x1, x2; //, x3; - - Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480)); - - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); - SimpleCamera level_camera(level_pose, *K); - - // create second camera 1 meter to the right of first camera - Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1,0,0)); - SimpleCamera level_camera_right(level_pose_right, *K); - - // landmark ~5 meters infront of camera - Point3 landmark(5, 0.5, 1.2); - - // 1. Project two landmarks into two cameras and triangulate - Point2 pixelError(0.2,0.2); - Point2 level_uv = level_camera.project(landmark) + pixelError; - Point2 level_uv_right = level_camera_right.project(landmark); - - Values values; - values.insert(x1, level_pose); - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); - values.insert(x2, level_pose_right.compose(noise_pose)); - - vector measurements; - measurements += level_uv, level_uv_right; - - SmartProjectionFactor::shared_ptr - smartFactor(new SmartProjectionFactor(measurements, noiseProjection, views, K)); - - double actualError = smartFactor->error(values); - std::cout << "Error: " << actualError << std::endl; - - // we do not expect to be able to predict the error, since the error on the pixel will change - // the triangulation of the landmark which is internal to the factor. - // DOUBLES_EQUAL(expectedError, actualError, 1e-7); -} - -/* ************************************************************************* */ -TEST( SmartProjectionFactor, 3poses_1iteration_projection_factor_comparison ){ - cout << " ************************ SmartProjectionFactor: 3 cams + 3 landmarks, 1 iteration, comparison b/w Generic and Smart Projection Factors **********************" << endl; - - Symbol x1('X', 1); - Symbol x2('X', 2); - Symbol x3('X', 3); - - const SharedDiagonal noiseProjection = noiseModel::Isotropic::Sigma(2, 1); - - std::vector views; - views += x1, x2, x3; - - Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480)); - - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); - SimpleCamera cam1(pose1, *K); - - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); - SimpleCamera cam2(pose2, *K); - - // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); - SimpleCamera cam3(pose3, *K); - - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); - Point3 landmark2(5, -0.5, 1.2); - Point3 landmark3(3, 0, 3.0); - - vector measurements_cam1, measurements_cam2, measurements_cam3; - - // 1. Project three landmarks into three cameras and triangulate - Point2 cam1_uv1 = cam1.project(landmark1); - Point2 cam2_uv1 = cam2.project(landmark1); - Point2 cam3_uv1 = cam3.project(landmark1); - measurements_cam1 += cam1_uv1, cam2_uv1, cam3_uv1; - - // - Point2 cam1_uv2 = cam1.project(landmark2); - Point2 cam2_uv2 = cam2.project(landmark2); - Point2 cam3_uv2 = cam3.project(landmark2); - measurements_cam2 += cam1_uv2, cam2_uv2, cam3_uv2; - - - Point2 cam1_uv3 = cam1.project(landmark3); - Point2 cam2_uv3 = cam2.project(landmark3); - Point2 cam3_uv3 = cam3.project(landmark3); - measurements_cam3 += cam1_uv3, cam2_uv3, cam3_uv3; - - typedef SmartProjectionFactor SmartFactor; - typedef GenericProjectionFactor ProjectionFactor; - - SmartFactor::shared_ptr smartFactor1(new SmartFactor(measurements_cam1, noiseProjection, views, K, boost::make_optional(landmark1) )); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(measurements_cam2, noiseProjection, views, K, boost::make_optional(landmark2) )); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(measurements_cam3, noiseProjection, views, K, boost::make_optional(landmark3) )); - - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - - NonlinearFactorGraph graphWithOriginalFactor; - graphWithOriginalFactor.add(ProjectionFactor(cam1.project(landmark1), noiseProjection, x1, L(1), K)); - graphWithOriginalFactor.add(ProjectionFactor(cam2.project(landmark1), noiseProjection, x2, L(1), K)); - graphWithOriginalFactor.add(ProjectionFactor(cam3.project(landmark1), noiseProjection, x3, L(1), K)); - - graphWithOriginalFactor.add(ProjectionFactor(cam1.project(landmark2), noiseProjection, x1, L(2), K)); - graphWithOriginalFactor.add(ProjectionFactor(cam2.project(landmark2), noiseProjection, x2, L(2), K)); - graphWithOriginalFactor.add(ProjectionFactor(cam3.project(landmark2), noiseProjection, x3, L(2), K)); - - graphWithOriginalFactor.add(ProjectionFactor(cam1.project(landmark3), noiseProjection, x1, L(3), K)); - graphWithOriginalFactor.add(ProjectionFactor(cam2.project(landmark3), noiseProjection, x2, L(3), K)); - graphWithOriginalFactor.add(ProjectionFactor(cam3.project(landmark3), noiseProjection, x3, L(3), K)); - - graphWithOriginalFactor.add(PriorFactor(x1, pose1, noisePrior)); - graphWithOriginalFactor.add(PriorFactor(x2, pose2, noisePrior)); - - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); - Values valuesOriginalFactor; - valuesOriginalFactor.insert(x1, pose1); - valuesOriginalFactor.insert(x2, pose2); - valuesOriginalFactor.insert(x3, pose3* noise_pose); - valuesOriginalFactor.insert(L(1), landmark1); - valuesOriginalFactor.insert(L(2), landmark2); - valuesOriginalFactor.insert(L(3), landmark3); - - NonlinearFactorGraph graphWithSmartFactor; - graphWithSmartFactor.push_back(smartFactor1); - graphWithSmartFactor.push_back(smartFactor2); - graphWithSmartFactor.push_back(smartFactor3); - graphWithSmartFactor.add(PriorFactor(x1, pose1, noisePrior)); - graphWithSmartFactor.add(PriorFactor(x2, pose2, noisePrior)); - - Values valuesSmartFactor; - valuesSmartFactor.insert(x1, pose1); - valuesSmartFactor.insert(x2, pose2); - // initialize third pose with some noise, we expect it to move back to original pose3 - valuesSmartFactor.insert(x3, pose3*noise_pose); - valuesSmartFactor.at(x3).print("Pose3 before optimization: "); - pose3.print("Pose3 ground truth: "); - - LevenbergMarquardtParams params; - params.maxIterations = 1; - params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; - params.verbosity = NonlinearOptimizerParams::ERROR; - - Values resultWithOriginalFactor; - std::cout << "\n=========================================" << std::endl; - std::cout << "Optimizing GenericProjectionFactor" << std::endl; - LevenbergMarquardtOptimizer optimizerForOriginalFactor(graphWithOriginalFactor, valuesOriginalFactor, params); - resultWithOriginalFactor = optimizerForOriginalFactor.optimize(); - - Values resultWithSmartFactor; - std::cout << "\n=========================================" << std::endl; - std::cout << "Optimizing SmartProjectionfactor" << std::endl; - LevenbergMarquardtOptimizer optimizerForSmartFactor(graphWithSmartFactor, valuesSmartFactor, params); - resultWithSmartFactor = optimizerForSmartFactor.optimize(); - - std::cout << "\n=========================================" << std::endl; - // result.print("results of 3 camera, 3 landmark optimization \n"); - resultWithOriginalFactor.at(x3).print("Original: Pose3 after optimization: "); - resultWithSmartFactor.at(x3).print("\nSmart: Pose3 after optimization: "); - EXPECT(assert_equal(resultWithOriginalFactor.at(x3),resultWithSmartFactor.at(x3))); - - GaussNewtonParams params2; - params2.maxIterations = 1; - Values resultWithOriginalFactor2; - GaussNewtonOptimizer optimizerForOriginalFactor2(graphWithOriginalFactor, valuesOriginalFactor, params2); - resultWithOriginalFactor2 = optimizerForOriginalFactor2.optimize(); - - Values resultWithSmartFactor2; - GaussNewtonOptimizer optimizerForSmartFactor2(graphWithSmartFactor, valuesSmartFactor, params2); - resultWithSmartFactor2 = optimizerForSmartFactor2.optimize(); - - resultWithOriginalFactor2.at(x3).print("Original: Pose3 after optimization (GaussNewton): "); - resultWithSmartFactor2.at(x3).print("\nSmart: Pose3 after optimization (GaussNewton): "); - -} - - -/* ************************************************************************* */ -TEST( SmartProjectionFactor, 3poses_smart_projection_factor ){ - cout << " ************************ SmartProjectionFactor: 3 cams + 3 landmarks **********************" << endl; - - Symbol x1('X', 1); - Symbol x2('X', 2); - Symbol x3('X', 3); - - const SharedDiagonal noiseProjection = noiseModel::Isotropic::Sigma(2, 1); - - std::vector views; - views += x1, x2, x3; - - Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480)); - - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); - SimpleCamera cam1(pose1, *K); - - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); - SimpleCamera cam2(pose2, *K); - - // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); - SimpleCamera cam3(pose3, *K); - - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); - Point3 landmark2(5, -0.5, 1.2); - Point3 landmark3(3, 0, 3.0); - - vector measurements_cam1, measurements_cam2, measurements_cam3; - - // 1. Project three landmarks into three cameras and triangulate - Point2 cam1_uv1 = cam1.project(landmark1); - Point2 cam2_uv1 = cam2.project(landmark1); - Point2 cam3_uv1 = cam3.project(landmark1); - measurements_cam1 += cam1_uv1, cam2_uv1, cam3_uv1; - - // - Point2 cam1_uv2 = cam1.project(landmark2); - Point2 cam2_uv2 = cam2.project(landmark2); - Point2 cam3_uv2 = cam3.project(landmark2); - measurements_cam2 += cam1_uv2, cam2_uv2, cam3_uv2; - - - Point2 cam1_uv3 = cam1.project(landmark3); - Point2 cam2_uv3 = cam2.project(landmark3); - Point2 cam3_uv3 = cam3.project(landmark3); - measurements_cam3 += cam1_uv3, cam2_uv3, cam3_uv3; - - typedef SmartProjectionFactor SmartFactor; - - SmartFactor::shared_ptr smartFactor1(new SmartFactor(measurements_cam1, noiseProjection, views, K)); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(measurements_cam2, noiseProjection, views, K)); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(measurements_cam3, noiseProjection, views, K)); - - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - - NonlinearFactorGraph graph; - graph.push_back(smartFactor1); - graph.push_back(smartFactor2); - graph.push_back(smartFactor3); - graph.add(PriorFactor(x1, pose1, noisePrior)); - graph.add(PriorFactor(x2, pose2, noisePrior)); - -// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise - Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); - // initialize third pose with some noise, we expect it to move back to original pose3 - values.insert(x3, pose3*noise_pose); - values.at(x3).print("Smart: Pose3 before optimization: "); - - LevenbergMarquardtParams params; - params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; - params.verbosity = NonlinearOptimizerParams::ERROR; - - Values result; - gttic_(SmartProjectionFactor); - LevenbergMarquardtOptimizer optimizer(graph, values, params); - result = optimizer.optimize(); - gttoc_(SmartProjectionFactor); - tictoc_finishedIteration_(); - - // result.print("results of 3 camera, 3 landmark optimization \n"); - result.at(x3).print("Smart: Pose3 after optimization: "); - EXPECT(assert_equal(pose3,result.at(x3))); - tictoc_print_(); - -} - - -/* ************************************************************************* */ -TEST( SmartProjectionFactor, 3poses_projection_factor ){ -// cout << " ************************ Normal ProjectionFactor: 3 cams + 3 landmarks **********************" << endl; - - Symbol x1('X', 1); - Symbol x2('X', 2); - Symbol x3('X', 3); - - const SharedDiagonal noiseProjection = noiseModel::Isotropic::Sigma(2, 1); - - std::vector views; - views += x1, x2, x3; - - Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480)); - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); - SimpleCamera cam1(pose1, *K); - - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); - SimpleCamera cam2(pose2, *K); - - // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); - SimpleCamera cam3(pose3, *K); - - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); - Point3 landmark2(5, -0.5, 1.2); - Point3 landmark3(3, 0, 3.0); - - typedef GenericProjectionFactor ProjectionFactor; - NonlinearFactorGraph graph; - - // 1. Project three landmarks into three cameras and triangulate - graph.add(ProjectionFactor(cam1.project(landmark1), noiseProjection, x1, L(1), K)); - graph.add(ProjectionFactor(cam2.project(landmark1), noiseProjection, x2, L(1), K)); - graph.add(ProjectionFactor(cam3.project(landmark1), noiseProjection, x3, L(1), K)); - - // - graph.add(ProjectionFactor(cam1.project(landmark2), noiseProjection, x1, L(2), K)); - graph.add(ProjectionFactor(cam2.project(landmark2), noiseProjection, x2, L(2), K)); - graph.add(ProjectionFactor(cam3.project(landmark2), noiseProjection, x3, L(2), K)); - - graph.add(ProjectionFactor(cam1.project(landmark3), noiseProjection, x1, L(3), K)); - graph.add(ProjectionFactor(cam2.project(landmark3), noiseProjection, x2, L(3), K)); - graph.add(ProjectionFactor(cam3.project(landmark3), noiseProjection, x3, L(3), K)); - - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - graph.add(PriorFactor(x1, pose1, noisePrior)); - graph.add(PriorFactor(x2, pose2, noisePrior)); - - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); - Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); - values.insert(x3, pose3* noise_pose); - values.insert(L(1), landmark1); - values.insert(L(2), landmark2); - values.insert(L(3), landmark3); -// values.at(x3).print("Pose3 before optimization: "); - - LevenbergMarquardtParams params; -// params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; -// params.verbosity = NonlinearOptimizerParams::ERROR; - LevenbergMarquardtOptimizer optimizer(graph, values, params); - Values result = optimizer.optimize(); - -// result.at(x3).print("Pose3 after optimization: "); - EXPECT(assert_equal(pose3,result.at(x3))); - -} - - - -/* ************************************************************************* */ -TEST( SmartProjectionFactor, Hessian ){ - cout << " ************************ SmartProjectionFactor: Hessian **********************" << endl; - - Symbol x1('X', 1); - Symbol x2('X', 2); - - const SharedDiagonal noiseProjection = noiseModel::Isotropic::Sigma(2, 1); - - std::vector views; - views += x1, x2; - - Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480)); - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); - SimpleCamera cam1(pose1, *K); - - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); - SimpleCamera cam2(pose2, *K); - - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); - - // 1. Project three landmarks into three cameras and triangulate - Point2 cam1_uv1 = cam1.project(landmark1); - Point2 cam2_uv1 = cam2.project(landmark1); - vector measurements_cam1; - measurements_cam1 += cam1_uv1, cam2_uv1; - - SmartProjectionFactor smartFactor(measurements_cam1, noiseProjection, views, K); - - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); - Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); - // values.insert(L(1), landmark1); - - Ordering ordering; - ordering.push_back(x1); - ordering.push_back(x2); - - boost::shared_ptr hessianFactor = smartFactor.linearize(values, ordering); - hessianFactor->print("Hessian factor \n"); - - // compute triangulation from linearization point - // compute reprojection errors (sum squared) - // compare with hessianFactor.info(): the bottom right element is the squared sum of the reprojection errors (normalized by the covariance) - // check that it is correctly scaled when using noiseProjection = [1/4 0; 0 1/4] - - - - - -} - - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ - diff --git a/gtsam_unstable/slam/tests/testSmartProjectionHessianPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartProjectionHessianPoseFactor.cpp new file mode 100644 index 000000000..9a3fe7f62 --- /dev/null +++ b/gtsam_unstable/slam/tests/testSmartProjectionHessianPoseFactor.cpp @@ -0,0 +1,1053 @@ +/* ---------------------------------------------------------------------------- + + * 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 TestSmartProjectionPoseFactor.cpp + * @brief Unit tests for ProjectionFactor Class + * @author Chris Beall + * @author Luca Carlone + * @author Zsolt Kira + * @date Sept 2013 + */ + +#include "../SmartProjectionPoseFactor.h" + +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace boost::assign; +using namespace gtsam; + +static bool isDebugTest = false; + +// make a realistic calibration matrix +static double fov = 60; // degrees +static size_t w=640,h=480; + +static Cal3_S2::shared_ptr K(new Cal3_S2(fov,w,h)); +static Cal3_S2::shared_ptr K2(new Cal3_S2(1500, 1200, 0, 640, 480)); +static boost::shared_ptr Kbundler(new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); + +static double rankTol = 1.0; +static double linThreshold = -1.0; +static bool manageDegeneracy = true; +// Create a noise model for the pixel error +static SharedNoiseModel model(noiseModel::Unit::Create(2)); + +// Convenience for named keys +using symbol_shorthand::X; +using symbol_shorthand::L; + +// tests data +Symbol x1('X', 1); +Symbol x2('X', 2); +Symbol x3('X', 3); + +static Key poseKey1(x1); +static Point2 measurement1(323.0, 240.0); +static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); + +typedef SmartProjectionPoseFactor SmartFactor; +typedef SmartProjectionPoseFactor SmartFactorBundler; + +void projectToMultipleCameras( + SimpleCamera cam1, SimpleCamera cam2, SimpleCamera cam3, Point3 landmark, + vector& measurements_cam){ + + Point2 cam1_uv1 = cam1.project(landmark); + Point2 cam2_uv1 = cam2.project(landmark); + Point2 cam3_uv1 = cam3.project(landmark); + measurements_cam.push_back(cam1_uv1); + measurements_cam.push_back(cam2_uv1); + measurements_cam.push_back(cam3_uv1); +} + +/* ************************************************************************* */ +TEST( SmartProjectionPoseFactor, Constructor) { + SmartFactor::shared_ptr factor1(new SmartFactor()); +} + +/* ************************************************************************* */ +TEST( SmartProjectionPoseFactor, Constructor2) { + SmartFactor factor1(rankTol, linThreshold); +} + +/* ************************************************************************* */ +TEST( SmartProjectionPoseFactor, Constructor3) { + SmartFactor::shared_ptr factor1(new SmartFactor()); + factor1->add(measurement1, poseKey1, model, K); +} + +/* ************************************************************************* */ +TEST( SmartProjectionPoseFactor, Constructor4) { + SmartFactor factor1(rankTol, linThreshold); + factor1.add(measurement1, poseKey1, model, K); +} + +/* ************************************************************************* */ +TEST( SmartProjectionPoseFactor, ConstructorWithTransform) { + bool manageDegeneracy = true; + bool enableEPI = false; + SmartFactor factor1(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor1); + factor1.add(measurement1, poseKey1, model, K); +} + +/* ************************************************************************* */ +TEST( SmartProjectionPoseFactor, Equals ) { + SmartFactor::shared_ptr factor1(new SmartFactor()); + factor1->add(measurement1, poseKey1, model, K); + + SmartFactor::shared_ptr factor2(new SmartFactor()); + factor2->add(measurement1, poseKey1, model, K); + + CHECK(assert_equal(*factor1, *factor2)); +} + +/* *************************************************************************/ +TEST_UNSAFE( SmartProjectionPoseFactor, noiseless ){ + // cout << " ************************ SmartProjectionPoseFactor: noisy ****************************" << endl; + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + SimpleCamera level_camera(level_pose, *K2); + + // create second camera 1 meter to the right of first camera + Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1,0,0)); + SimpleCamera level_camera_right(level_pose_right, *K2); + + // landmark ~5 meters infront of camera + Point3 landmark(5, 0.5, 1.2); + + // 1. Project two landmarks into two cameras and triangulate + Point2 level_uv = level_camera.project(landmark); + Point2 level_uv_right = level_camera_right.project(landmark); + + Values values; + values.insert(x1, level_pose); + values.insert(x2, level_pose_right); + + SmartFactor factor1; + factor1.add(level_uv, x1, model, K); + factor1.add(level_uv_right, x2, model, K); + + double actualError = factor1.error(values); + double expectedError = 0.0; + EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); + + SmartFactor::Cameras cameras = factor1.cameras(values); + double actualError2 = factor1.totalReprojectionError(cameras); + EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7); + + // test vector of errors + //Vector actual = factor1.unwhitenedError(values); + //EXPECT(assert_equal(zero(4),actual,1e-8)); +} + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, noisy ){ + // cout << " ************************ SmartProjectionPoseFactor: noisy ****************************" << endl; + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + SimpleCamera level_camera(level_pose, *K2); + + // create second camera 1 meter to the right of first camera + Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1,0,0)); + SimpleCamera level_camera_right(level_pose_right, *K2); + + // landmark ~5 meters infront of camera + Point3 landmark(5, 0.5, 1.2); + + // 1. Project two landmarks into two cameras and triangulate + Point2 pixelError(0.2,0.2); + Point2 level_uv = level_camera.project(landmark) + pixelError; + Point2 level_uv_right = level_camera_right.project(landmark); + + Values values; + values.insert(x1, level_pose); + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); + values.insert(x2, level_pose_right.compose(noise_pose)); + + SmartFactor::shared_ptr factor1(new SmartFactor()); + factor1->add(level_uv, x1, model, K); + factor1->add(level_uv_right, x2, model, K); + + double actualError1= factor1->error(values); + + SmartFactor::shared_ptr factor2(new SmartFactor()); + vector measurements; + measurements.push_back(level_uv); + measurements.push_back(level_uv_right); + + std::vector< SharedNoiseModel > noises; + noises.push_back(model); + noises.push_back(model); + + std::vector< boost::shared_ptr > Ks; ///< shared pointer to calibration object (one for each camera) + Ks.push_back(K); + Ks.push_back(K); + + std::vector views; + views.push_back(x1); + views.push_back(x2); + + factor2->add(measurements, views, noises, Ks); + + double actualError2= factor2->error(values); + + DOUBLES_EQUAL(actualError1, actualError2, 1e-7); +} + + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ){ + // cout << " ************************ SmartProjectionPoseFactor: 3 cams + 3 landmarks **********************" << endl; + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + SimpleCamera cam1(pose1, *K2); + + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + SimpleCamera cam2(pose2, *K2); + + // create third camera 1 meter above the first camera + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + SimpleCamera cam3(pose3, *K2); + + // three landmarks ~5 meters infront of camera + Point3 landmark1(5, 0.5, 1.2); + Point3 landmark2(5, -0.5, 1.2); + Point3 landmark3(3, 0, 3.0); + + vector measurements_cam1, measurements_cam2, measurements_cam3; + + // 1. Project three landmarks into three cameras and triangulate + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + std::vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + SmartFactor::shared_ptr smartFactor1(new SmartFactor()); + smartFactor1->add(measurements_cam1, views, model, K2); + + SmartFactor::shared_ptr smartFactor2(new SmartFactor()); + smartFactor2->add(measurements_cam2, views, model, K2); + + SmartFactor::shared_ptr smartFactor3(new SmartFactor()); + smartFactor3->add(measurements_cam3, views, model, K2); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.push_back(PriorFactor(x1, pose1, noisePrior)); + graph.push_back(PriorFactor(x2, pose2, noisePrior)); + + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + // initialize third pose with some noise, we expect it to move back to original pose3 + values.insert(x3, pose3*noise_pose); + if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); + + LevenbergMarquardtParams params; + if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; + + Values result; + gttic_(SmartProjectionPoseFactor); + LevenbergMarquardtOptimizer optimizer(graph, values, params); + result = optimizer.optimize(); + gttoc_(SmartProjectionPoseFactor); + tictoc_finishedIteration_(); + +// GaussianFactorGraph::shared_ptr GFG = graph.linearize(values); +// VectorValues delta = GFG->optimize(); + + // result.print("results of 3 camera, 3 landmark optimization \n"); + if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); + EXPECT(assert_equal(pose3,result.at(x3))); + if(isDebugTest) tictoc_print_(); +} + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ){ + // cout << " ************************ SmartProjectionPoseFactor: 3 cams + 3 landmarks **********************" << endl; + + std::vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + SimpleCamera cam1(pose1, *K); + + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + SimpleCamera cam2(pose2, *K); + + // create third camera 1 meter above the first camera + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + SimpleCamera cam3(pose3, *K); + + // three landmarks ~5 meters infront of camera + Point3 landmark1(5, 0.5, 1.2); + Point3 landmark2(5, -0.5, 1.2); + Point3 landmark3(3, 0, 3.0); + + vector measurements_cam1, measurements_cam2, measurements_cam3; + + // 1. Project three landmarks into three cameras and triangulate + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + SmartFactor::shared_ptr smartFactor1(new SmartFactor()); + smartFactor1->add(measurements_cam1, views, model, K); + + SmartFactor::shared_ptr smartFactor2(new SmartFactor()); + smartFactor2->add(measurements_cam2, views, model, K); + + SmartFactor::shared_ptr smartFactor3(new SmartFactor()); + smartFactor3->add(measurements_cam3, views, model, K); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.push_back(PriorFactor(x1, pose1, noisePrior)); + graph.push_back(PriorFactor(x2, pose2, noisePrior)); + + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + // initialize third pose with some noise, we expect it to move back to original pose3 + values.insert(x3, pose3*noise_pose); + if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); + + LevenbergMarquardtParams params; + if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; + + Values result; + gttic_(SmartProjectionPoseFactor); + LevenbergMarquardtOptimizer optimizer(graph, values, params); + result = optimizer.optimize(); + gttoc_(SmartProjectionPoseFactor); + tictoc_finishedIteration_(); + + // result.print("results of 3 camera, 3 landmark optimization \n"); + if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); + EXPECT(assert_equal(pose3,result.at(x3))); + if(isDebugTest) tictoc_print_(); +} + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, 3poses_projection_factor ){ + // cout << " ************************ Normal ProjectionFactor: 3 cams + 3 landmarks **********************" << endl; + + std::vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + SimpleCamera cam1(pose1, *K2); + + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + SimpleCamera cam2(pose2, *K2); + + // create third camera 1 meter above the first camera + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + SimpleCamera cam3(pose3, *K2); + + // three landmarks ~5 meters infront of camera + Point3 landmark1(5, 0.5, 1.2); + Point3 landmark2(5, -0.5, 1.2); + Point3 landmark3(3, 0, 3.0); + + typedef GenericProjectionFactor ProjectionFactor; + NonlinearFactorGraph graph; + + // 1. Project three landmarks into three cameras and triangulate + graph.push_back(ProjectionFactor(cam1.project(landmark1), model, x1, L(1), K2)); + graph.push_back(ProjectionFactor(cam2.project(landmark1), model, x2, L(1), K2)); + graph.push_back(ProjectionFactor(cam3.project(landmark1), model, x3, L(1), K2)); + + graph.push_back(ProjectionFactor(cam1.project(landmark2), model, x1, L(2), K2)); + graph.push_back(ProjectionFactor(cam2.project(landmark2), model, x2, L(2), K2)); + graph.push_back(ProjectionFactor(cam3.project(landmark2), model, x3, L(2), K2)); + + graph.push_back(ProjectionFactor(cam1.project(landmark3), model, x1, L(3), K2)); + graph.push_back(ProjectionFactor(cam2.project(landmark3), model, x2, L(3), K2)); + graph.push_back(ProjectionFactor(cam3.project(landmark3), model, x3, L(3), K2)); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + graph.push_back(PriorFactor(x1, pose1, noisePrior)); + graph.push_back(PriorFactor(x2, pose2, noisePrior)); + + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + values.insert(x3, pose3* noise_pose); + values.insert(L(1), landmark1); + values.insert(L(2), landmark2); + values.insert(L(3), landmark3); + if(isDebugTest) values.at(x3).print("Pose3 before optimization: "); + + LevenbergMarquardtParams params; + if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; + LevenbergMarquardtOptimizer optimizer(graph, values, params); + Values result = optimizer.optimize(); + + if(isDebugTest) result.at(x3).print("Pose3 after optimization: "); + EXPECT(assert_equal(pose3,result.at(x3))); +} + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, CheckHessian){ + + std::vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + SimpleCamera cam1(pose1, *K); + + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); + SimpleCamera cam2(pose2, *K); + + // create third camera 1 meter above the first camera + Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); + SimpleCamera cam3(pose3, *K); + + // three landmarks ~5 meters infront of camera + Point3 landmark1(5, 0.5, 1.2); + Point3 landmark2(5, -0.5, 1.2); + Point3 landmark3(3, 0, 3.0); + + vector measurements_cam1, measurements_cam2, measurements_cam3; + + // 1. Project three landmarks into three cameras and triangulate + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + double rankTol = 10; + + SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol)); + smartFactor1->add(measurements_cam1, views, model, K); + + SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol)); + smartFactor2->add(measurements_cam2, views, model, K); + + SmartFactor::shared_ptr smartFactor3(new SmartFactor(rankTol)); + smartFactor3->add(measurements_cam3, views, model, K); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + // initialize third pose with some noise, we expect it to move back to original pose3 + values.insert(x3, pose3*noise_pose); + if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); + + boost::shared_ptr hessianFactor1 = smartFactor1->linearize(values); + boost::shared_ptr hessianFactor2 = smartFactor2->linearize(values); + boost::shared_ptr hessianFactor3 = smartFactor3->linearize(values); + + Matrix CumulativeInformation = hessianFactor1->information() + hessianFactor2->information() + hessianFactor3->information(); + + boost::shared_ptr GaussianGraph = graph.linearize(values); + Matrix GraphInformation = GaussianGraph->hessian().first; + + // Check Hessian + EXPECT(assert_equal(GraphInformation, CumulativeInformation, 1e-8)); + + Matrix AugInformationMatrix = hessianFactor1->augmentedInformation() + + hessianFactor2->augmentedInformation() + hessianFactor3->augmentedInformation(); + + // Check Information vector + // cout << AugInformationMatrix.size() << endl; + Vector InfoVector = AugInformationMatrix.block(0,18,18,1); // 18x18 Hessian + information vector + + // Check Hessian + EXPECT(assert_equal(InfoVector, GaussianGraph->hessian().second, 1e-8)); +} + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_factor ){ + // cout << " ************************ SmartProjectionPoseFactor: 3 cams + 2 landmarks: Rotation Only**********************" << endl; + + std::vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + SimpleCamera cam1(pose1, *K2); + + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); + SimpleCamera cam2(pose2, *K2); + + // create third camera 1 meter above the first camera + Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); + SimpleCamera cam3(pose3, *K2); + + // three landmarks ~5 meters infront of camera + Point3 landmark1(5, 0.5, 1.2); + Point3 landmark2(5, -0.5, 1.2); + + vector measurements_cam1, measurements_cam2, measurements_cam3; + + // 1. Project three landmarks into three cameras and triangulate + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + + double rankTol = 50; + SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol, linThreshold, manageDegeneracy)); + smartFactor1->add(measurements_cam1, views, model, K2); + + SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol, linThreshold, manageDegeneracy)); + smartFactor2->add(measurements_cam2, views, model, K2); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10); + Point3 positionPrior = gtsam::Point3(0,0,1); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(PriorFactor(x1, pose1, noisePrior)); + graph.push_back(PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); + graph.push_back(PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); + + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2*noise_pose); + // initialize third pose with some noise, we expect it to move back to original pose3 + values.insert(x3, pose3*noise_pose*noise_pose); + if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); + + LevenbergMarquardtParams params; + if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYDELTA; + if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; + + Values result; + gttic_(SmartProjectionPoseFactor); + LevenbergMarquardtOptimizer optimizer(graph, values, params); + result = optimizer.optimize(); + gttoc_(SmartProjectionPoseFactor); + tictoc_finishedIteration_(); + + // result.print("results of 3 camera, 3 landmark optimization \n"); + if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); + std::cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << std::endl; + // EXPECT(assert_equal(pose3,result.at(x3))); + if(isDebugTest) tictoc_print_(); +} + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ){ + // cout << " ************************ SmartProjectionPoseFactor: 3 cams + 3 landmarks: Rotation Only**********************" << endl; + + std::vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + SimpleCamera cam1(pose1, *K); + + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); + SimpleCamera cam2(pose2, *K); + + // create third camera 1 meter above the first camera + Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); + SimpleCamera cam3(pose3, *K); + + // three landmarks ~5 meters infront of camera + Point3 landmark1(5, 0.5, 1.2); + Point3 landmark2(5, -0.5, 1.2); + Point3 landmark3(3, 0, 3.0); + + vector measurements_cam1, measurements_cam2, measurements_cam3; + + // 1. Project three landmarks into three cameras and triangulate + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + double rankTol = 10; + + SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol, linThreshold, manageDegeneracy)); + smartFactor1->add(measurements_cam1, views, model, K); + + SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol, linThreshold, manageDegeneracy)); + smartFactor2->add(measurements_cam2, views, model, K); + + SmartFactor::shared_ptr smartFactor3(new SmartFactor(rankTol, linThreshold, manageDegeneracy)); + smartFactor3->add(measurements_cam3, views, model, K); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10); + Point3 positionPrior = gtsam::Point3(0,0,1); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.push_back(PriorFactor(x1, pose1, noisePrior)); + graph.push_back(PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); + graph.push_back(PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); + + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + // initialize third pose with some noise, we expect it to move back to original pose3 + values.insert(x3, pose3*noise_pose); + if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); + + LevenbergMarquardtParams params; + if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYDELTA; + if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; + + Values result; + gttic_(SmartProjectionPoseFactor); + LevenbergMarquardtOptimizer optimizer(graph, values, params); + result = optimizer.optimize(); + gttoc_(SmartProjectionPoseFactor); + tictoc_finishedIteration_(); + + // result.print("results of 3 camera, 3 landmark optimization \n"); + if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); + std::cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << std::endl; + // EXPECT(assert_equal(pose3,result.at(x3))); + if(isDebugTest) tictoc_print_(); +} + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, Hessian ){ + // cout << " ************************ SmartProjectionPoseFactor: Hessian **********************" << endl; + + std::vector views; + views.push_back(x1); + views.push_back(x2); + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + SimpleCamera cam1(pose1, *K2); + + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + SimpleCamera cam2(pose2, *K2); + + // three landmarks ~5 meters infront of camera + Point3 landmark1(5, 0.5, 1.2); + + // 1. Project three landmarks into three cameras and triangulate + Point2 cam1_uv1 = cam1.project(landmark1); + Point2 cam2_uv1 = cam2.project(landmark1); + vector measurements_cam1; + measurements_cam1.push_back(cam1_uv1); + measurements_cam1.push_back(cam2_uv1); + + SmartFactor::shared_ptr smartFactor1(new SmartFactor()); + smartFactor1->add(measurements_cam1,views, model, K2); + + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + + boost::shared_ptr hessianFactor = smartFactor1->linearize(values); + if(isDebugTest) hessianFactor->print("Hessian factor \n"); + + // compute triangulation from linearization point + // compute reprojection errors (sum squared) + // compare with hessianFactor.info(): the bottom right element is the squared sum of the reprojection errors (normalized by the covariance) + // check that it is correctly scaled when using noiseProjection = [1/4 0; 0 1/4] +} + + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, HessianWithRotation ){ + // cout << " ************************ SmartProjectionPoseFactor: rotated Hessian **********************" << endl; + + std::vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + SimpleCamera cam1(pose1, *K); + + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + SimpleCamera cam2(pose2, *K); + + // create third camera 1 meter above the first camera + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + SimpleCamera cam3(pose3, *K); + + Point3 landmark1(5, 0.5, 1.2); + + vector measurements_cam1, measurements_cam2, measurements_cam3; + + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + + SmartFactor::shared_ptr smartFactorInstance(new SmartFactor()); + smartFactorInstance->add(measurements_cam1, views, model, K); + + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + values.insert(x3, pose3); + + boost::shared_ptr hessianFactor = smartFactorInstance->linearize(values); + // hessianFactor->print("Hessian factor \n"); + + Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,0)); + + Values rotValues; + rotValues.insert(x1, poseDrift.compose(pose1)); + rotValues.insert(x2, poseDrift.compose(pose2)); + rotValues.insert(x3, poseDrift.compose(pose3)); + + boost::shared_ptr hessianFactorRot = smartFactorInstance->linearize(rotValues); + // hessianFactorRot->print("Hessian factor \n"); + + // Hessian is invariant to rotations in the nondegenerate case + EXPECT(assert_equal(hessianFactor->information(), hessianFactorRot->information(), 1e-8) ); + + Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI/2, -M_PI/3, -M_PI/2), gtsam::Point3(10,-4,5)); + + Values tranValues; + tranValues.insert(x1, poseDrift2.compose(pose1)); + tranValues.insert(x2, poseDrift2.compose(pose2)); + tranValues.insert(x3, poseDrift2.compose(pose3)); + + boost::shared_ptr hessianFactorRotTran = smartFactorInstance->linearize(tranValues); + + // Hessian is invariant to rotations and translations in the nondegenerate case + EXPECT(assert_equal(hessianFactor->information(), hessianFactorRotTran->information(), 1e-8) ); +} + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ){ + // cout << " ************************ SmartProjectionPoseFactor: rotated Hessian (degenerate) **********************" << endl; + + std::vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + SimpleCamera cam1(pose1, *K2); + + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(0,0,0)); + SimpleCamera cam2(pose2, *K2); + + // create third camera 1 meter above the first camera + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,0,0)); + SimpleCamera cam3(pose3, *K2); + + Point3 landmark1(5, 0.5, 1.2); + + vector measurements_cam1, measurements_cam2, measurements_cam3; + + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + + SmartFactor::shared_ptr smartFactor(new SmartFactor()); + smartFactor->add(measurements_cam1, views, model, K2); + + + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + values.insert(x3, pose3); + + boost::shared_ptr hessianFactor = smartFactor->linearize(values); + if(isDebugTest) hessianFactor->print("Hessian factor \n"); + + Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,0)); + + Values rotValues; + rotValues.insert(x1, poseDrift.compose(pose1)); + rotValues.insert(x2, poseDrift.compose(pose2)); + rotValues.insert(x3, poseDrift.compose(pose3)); + + boost::shared_ptr hessianFactorRot = smartFactor->linearize(rotValues); + if(isDebugTest) hessianFactorRot->print("Hessian factor \n"); + + // Hessian is invariant to rotations in the nondegenerate case + EXPECT(assert_equal(hessianFactor->information(), hessianFactorRot->information(), 1e-8) ); + + Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI/2, -M_PI/3, -M_PI/2), gtsam::Point3(10,-4,5)); + + Values tranValues; + tranValues.insert(x1, poseDrift2.compose(pose1)); + tranValues.insert(x2, poseDrift2.compose(pose2)); + tranValues.insert(x3, poseDrift2.compose(pose3)); + + boost::shared_ptr hessianFactorRotTran = smartFactor->linearize(tranValues); + + // Hessian is invariant to rotations and translations in the nondegenerate case + EXPECT(assert_equal(hessianFactor->information(), hessianFactorRotTran->information(), 1e-8) ); +} + +/* ************************************************************************* */ +TEST( SmartProjectionPoseFactor, ConstructorWithCal3Bundler) { + SmartProjectionPoseFactor factor1(rankTol, linThreshold); + boost::shared_ptr Kbundler(new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); + factor1.add(measurement1, poseKey1, model, Kbundler); +} + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, Cal3Bundler ){ + // cout << " ************************ SmartProjectionPoseFactor: Cal3Bundler **********************" << endl; + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + PinholeCamera cam1(pose1, *Kbundler); + + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + PinholeCamera cam2(pose2, *Kbundler); + + // create third camera 1 meter above the first camera + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + PinholeCamera cam3(pose3, *Kbundler); + + // three landmarks ~5 meters infront of camera + Point3 landmark1(5, 0.5, 1.2); + Point3 landmark2(5, -0.5, 1.2); + Point3 landmark3(3, 0, 3.0); + + vector measurements_cam1, measurements_cam2, measurements_cam3; + + // 1. Project three landmarks into three cameras and triangulate + Point2 cam1_uv1 = cam1.project(landmark1); + Point2 cam2_uv1 = cam2.project(landmark1); + Point2 cam3_uv1 = cam3.project(landmark1); + measurements_cam1.push_back(cam1_uv1); + measurements_cam1.push_back(cam2_uv1); + measurements_cam1.push_back(cam3_uv1); + + Point2 cam1_uv2 = cam1.project(landmark2); + Point2 cam2_uv2 = cam2.project(landmark2); + Point2 cam3_uv2 = cam3.project(landmark2); + measurements_cam2.push_back(cam1_uv2); + measurements_cam2.push_back(cam2_uv2); + measurements_cam2.push_back(cam3_uv2); + + Point2 cam1_uv3 = cam1.project(landmark3); + Point2 cam2_uv3 = cam2.project(landmark3); + Point2 cam3_uv3 = cam3.project(landmark3); + measurements_cam3.push_back(cam1_uv3); + measurements_cam3.push_back(cam2_uv3); + measurements_cam3.push_back(cam3_uv3); + + std::vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + SmartFactorBundler::shared_ptr smartFactor1(new SmartFactorBundler()); + smartFactor1->add(measurements_cam1, views, model, Kbundler); + + SmartFactorBundler::shared_ptr smartFactor2(new SmartFactorBundler()); + smartFactor2->add(measurements_cam2, views, model, Kbundler); + + SmartFactorBundler::shared_ptr smartFactor3(new SmartFactorBundler()); + smartFactor3->add(measurements_cam3, views, model, Kbundler); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.push_back(PriorFactor(x1, pose1, noisePrior)); + graph.push_back(PriorFactor(x2, pose2, noisePrior)); + + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + // initialize third pose with some noise, we expect it to move back to original pose3 + values.insert(x3, pose3*noise_pose); + if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); + + LevenbergMarquardtParams params; + if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; + + Values result; + gttic_(SmartProjectionPoseFactor); + LevenbergMarquardtOptimizer optimizer(graph, values, params); + result = optimizer.optimize(); + gttoc_(SmartProjectionPoseFactor); + tictoc_finishedIteration_(); + + // result.print("results of 3 camera, 3 landmark optimization \n"); + if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); + EXPECT(assert_equal(pose3,result.at(x3), 1e-6)); + if(isDebugTest) tictoc_print_(); + } + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ){ + + std::vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + PinholeCamera cam1(pose1, *Kbundler); + + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); + PinholeCamera cam2(pose2, *Kbundler); + + // create third camera 1 meter above the first camera + Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); + PinholeCamera cam3(pose3, *Kbundler); + + // three landmarks ~5 meters infront of camera + Point3 landmark1(5, 0.5, 1.2); + Point3 landmark2(5, -0.5, 1.2); + Point3 landmark3(3, 0, 3.0); + + vector measurements_cam1, measurements_cam2, measurements_cam3; + + // 1. Project three landmarks into three cameras and triangulate + Point2 cam1_uv1 = cam1.project(landmark1); + Point2 cam2_uv1 = cam2.project(landmark1); + Point2 cam3_uv1 = cam3.project(landmark1); + measurements_cam1.push_back(cam1_uv1); + measurements_cam1.push_back(cam2_uv1); + measurements_cam1.push_back(cam3_uv1); + + Point2 cam1_uv2 = cam1.project(landmark2); + Point2 cam2_uv2 = cam2.project(landmark2); + Point2 cam3_uv2 = cam3.project(landmark2); + measurements_cam2.push_back(cam1_uv2); + measurements_cam2.push_back(cam2_uv2); + measurements_cam2.push_back(cam3_uv2); + + Point2 cam1_uv3 = cam1.project(landmark3); + Point2 cam2_uv3 = cam2.project(landmark3); + Point2 cam3_uv3 = cam3.project(landmark3); + measurements_cam3.push_back(cam1_uv3); + measurements_cam3.push_back(cam2_uv3); + measurements_cam3.push_back(cam3_uv3); + + double rankTol = 10; + + SmartFactorBundler::shared_ptr smartFactor1(new SmartFactorBundler(rankTol, linThreshold, manageDegeneracy)); + smartFactor1->add(measurements_cam1, views, model, Kbundler); + + SmartFactorBundler::shared_ptr smartFactor2(new SmartFactorBundler(rankTol, linThreshold, manageDegeneracy)); + smartFactor2->add(measurements_cam2, views, model, Kbundler); + + SmartFactorBundler::shared_ptr smartFactor3(new SmartFactorBundler(rankTol, linThreshold, manageDegeneracy)); + smartFactor3->add(measurements_cam3, views, model, Kbundler); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10); + Point3 positionPrior = gtsam::Point3(0,0,1); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.push_back(PriorFactor(x1, pose1, noisePrior)); + graph.push_back(PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); + graph.push_back(PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); + + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + // initialize third pose with some noise, we expect it to move back to original pose3 + values.insert(x3, pose3*noise_pose); + if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); + + LevenbergMarquardtParams params; + if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYDELTA; + if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; + + Values result; + gttic_(SmartProjectionPoseFactor); + LevenbergMarquardtOptimizer optimizer(graph, values, params); + result = optimizer.optimize(); + gttoc_(SmartProjectionPoseFactor); + tictoc_finishedIteration_(); + + // result.print("results of 3 camera, 3 landmark optimization \n"); + if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); + std::cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << std::endl; + // EXPECT(assert_equal(pose3,result.at(x3))); + if(isDebugTest) tictoc_print_(); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ + + diff --git a/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp b/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp index 8f485738b..f645f5086 100644 --- a/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp @@ -53,8 +53,8 @@ TEST( SmartRangeFactor, addRange ) { TEST( SmartRangeFactor, scenario ) { DOUBLES_EQUAL(10, r1, 1e-9); - DOUBLES_EQUAL(sqrt(100+25), r2, 1e-9); - DOUBLES_EQUAL(sqrt(50), r3, 1e-9); + DOUBLES_EQUAL(sqrt(100.0+25.0), r2, 1e-9); + DOUBLES_EQUAL(sqrt(50.0), r3, 1e-9); } /* ************************************************************************* */ @@ -74,22 +74,22 @@ TEST( SmartRangeFactor, unwhitenedError ) { // Whenever there are two ranges or less, error should be zero Vector actual1 = f.unwhitenedError(values); - EXPECT(assert_equal(Vector_(1,0.0), actual1)); + EXPECT(assert_equal((Vector(1) << 0.0), actual1)); f.addRange(2, r2); Vector actual2 = f.unwhitenedError(values); - EXPECT(assert_equal(Vector_(1,0.0), actual2)); + EXPECT(assert_equal((Vector(1) << 0.0), actual2)); f.addRange(3, r3); vector H(3); Vector actual3 = f.unwhitenedError(values); EXPECT_LONGS_EQUAL(3, f.keys().size()); - EXPECT(assert_equal(Vector_(1,0.0), actual3)); + EXPECT(assert_equal((Vector(1) << 0.0), actual3)); // Check keys and Jacobian Vector actual4 = f.unwhitenedError(values, H); // with H now ! - EXPECT(assert_equal(Vector_(1,0.0), actual4)); - CHECK(assert_equal(Matrix_(1,3, 0.0,-1.0,0.0), H.front())); - CHECK(assert_equal(Matrix_(1,3, sqrt(2)/2,-sqrt(2)/2,0.0), H.back())); + EXPECT(assert_equal((Vector(1) << 0.0), actual4)); + CHECK(assert_equal((Matrix(1, 3) << 0.0,-1.0,0.0), H.front())); + CHECK(assert_equal((Matrix(1, 3) << sqrt(2.0)/2,-sqrt(2.0)/2,0.0), H.back())); // Test clone NonlinearFactor::shared_ptr clone = f.clone(); @@ -109,15 +109,15 @@ TEST( SmartRangeFactor, optimization ) { initial.insert(2, pose2); initial.insert(3, Pose2(5, 6, 0)); // does not satisfy range measurement Vector actual5 = f.unwhitenedError(initial); - EXPECT(assert_equal(Vector_(1,sqrt(25+16)-sqrt(50)), actual5)); + EXPECT(assert_equal((Vector(1) << sqrt(25.0+16.0)-sqrt(50.0)), actual5)); // Create Factor graph NonlinearFactorGraph graph; - graph.add(f); + graph.push_back(f); const noiseModel::Base::shared_ptr // priorNoise = noiseModel::Diagonal::Sigmas(Vector3(1, 1, M_PI)); - graph.add(PriorFactor(1, pose1, priorNoise)); - graph.add(PriorFactor(2, pose2, priorNoise)); + graph.push_back(PriorFactor(1, pose1, priorNoise)); + graph.push_back(PriorFactor(2, pose2, priorNoise)); // Try optimizing LevenbergMarquardtParams params; diff --git a/gtsam_unstable/slam/tests/testTSAMFactors.cpp b/gtsam_unstable/slam/tests/testTSAMFactors.cpp new file mode 100644 index 000000000..44dca9b8e --- /dev/null +++ b/gtsam_unstable/slam/tests/testTSAMFactors.cpp @@ -0,0 +1,149 @@ +/* ---------------------------------------------------------------------------- + + * 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 testTSAMFactors.cpp + * @brief Unit tests for TSAM 1 Factors + * @author Frank Dellaert + * @date May 2014 + */ + +#include +#include +#include + +using namespace std; +using namespace gtsam; + +Key i(1), j(2); // Key for pose and point + +//************************************************************************* +TEST( DeltaFactor, all ) { + // Create a factor + Point2 measurement(1, 1); + static SharedNoiseModel model(noiseModel::Unit::Create(2)); + DeltaFactor factor(i, j, measurement, model); + + // Set the linearization point + Pose2 pose(1, 2, 0); + Point2 point(4, 11); + Vector2 expected(4 - 1 - 1, 11 - 2 - 1); + + // Use the factor to calculate the Jacobians + Matrix H1Actual, H2Actual; + Vector actual = factor.evaluateError(pose, point, H1Actual, H2Actual); + EXPECT(assert_equal(expected, actual, 1e-9)); + + // Use numerical derivatives to calculate the Jacobians + Matrix H1Expected, H2Expected; + H1Expected = numericalDerivative11( + boost::bind(&DeltaFactor::evaluateError, &factor, _1, point, boost::none, + boost::none), pose); + H2Expected = numericalDerivative11( + boost::bind(&DeltaFactor::evaluateError, &factor, pose, _1, boost::none, + boost::none), point); + + // Verify the Jacobians are correct + EXPECT(assert_equal(H1Expected, H1Actual, 1e-9)); + EXPECT(assert_equal(H2Expected, H2Actual, 1e-9)); +} + +//************************************************************************* +TEST( DeltaFactorBase, all ) { + // Create a factor + Key b1(10), b2(20); + Point2 measurement(1, 1); + static SharedNoiseModel model(noiseModel::Unit::Create(2)); + DeltaFactorBase factor(b1, i, b2, j, measurement, model); + + // Set the linearization point + Pose2 base1, base2(1, 0, 0); + Pose2 pose(1, 2, 0); + Point2 point(4, 11); + Vector2 expected(4 + 1 - 1 - 1, 11 - 2 - 1); + + // Use the factor to calculate the Jacobians + Matrix H1Actual, H2Actual, H3Actual, H4Actual; + Vector actual = factor.evaluateError(base1, pose, base2, point, H1Actual, + H2Actual, H3Actual, H4Actual); + EXPECT(assert_equal(expected, actual, 1e-9)); + + // Use numerical derivatives to calculate the Jacobians + Matrix H1Expected, H2Expected, H3Expected, H4Expected; + H1Expected = numericalDerivative11( + boost::bind(&DeltaFactorBase::evaluateError, &factor, _1, pose, base2, + point, boost::none, boost::none, boost::none, boost::none), base1); + H2Expected = numericalDerivative11( + boost::bind(&DeltaFactorBase::evaluateError, &factor, base1, _1, base2, + point, boost::none, boost::none, boost::none, boost::none), pose); + H3Expected = numericalDerivative11( + boost::bind(&DeltaFactorBase::evaluateError, &factor, base1, pose, _1, + point, boost::none, boost::none, boost::none, boost::none), base2); + H4Expected = numericalDerivative11( + boost::bind(&DeltaFactorBase::evaluateError, &factor, base1, pose, base2, + _1, boost::none, boost::none, boost::none, boost::none), point); + + // Verify the Jacobians are correct + EXPECT(assert_equal(H1Expected, H1Actual, 1e-9)); + EXPECT(assert_equal(H2Expected, H2Actual, 1e-9)); + EXPECT(assert_equal(H3Expected, H3Actual, 1e-9)); + EXPECT(assert_equal(H4Expected, H4Actual, 1e-9)); +} + +//************************************************************************* +TEST( OdometryFactorBase, all ) { + // Create a factor + Key b1(10), b2(20); + Pose2 measurement(1, 1, 0); + static SharedNoiseModel model(noiseModel::Unit::Create(2)); + OdometryFactorBase factor(b1, i, b2, j, measurement, model); + + // Set the linearization pose2 + Pose2 base1, base2(1, 0, 0); + Pose2 pose1(1, 2, 0), pose2(4, 11, 0); + Vector3 expected(4 + 1 - 1 - 1, 11 - 2 - 1, 0); + + // Use the factor to calculate the Jacobians + Matrix H1Actual, H2Actual, H3Actual, H4Actual; + Vector actual = factor.evaluateError(base1, pose1, base2, pose2, H1Actual, + H2Actual, H3Actual, H4Actual); + EXPECT(assert_equal(expected, actual, 1e-9)); + + // Use numerical derivatives to calculate the Jacobians + Matrix H1Expected, H2Expected, H3Expected, H4Expected; + H1Expected = numericalDerivative11( + boost::bind(&OdometryFactorBase::evaluateError, &factor, _1, pose1, base2, + pose2, boost::none, boost::none, boost::none, boost::none), base1); + H2Expected = numericalDerivative11( + boost::bind(&OdometryFactorBase::evaluateError, &factor, base1, _1, base2, + pose2, boost::none, boost::none, boost::none, boost::none), pose1); + H3Expected = numericalDerivative11( + boost::bind(&OdometryFactorBase::evaluateError, &factor, base1, pose1, _1, + pose2, boost::none, boost::none, boost::none, boost::none), base2); + H4Expected = numericalDerivative11( + boost::bind(&OdometryFactorBase::evaluateError, &factor, base1, pose1, + base2, _1, boost::none, boost::none, boost::none, boost::none), + pose2); + + // Verify the Jacobians are correct + EXPECT(assert_equal(H1Expected, H1Actual, 1e-9)); + EXPECT(assert_equal(H2Expected, H2Actual, 1e-9)); + EXPECT(assert_equal(H3Expected, H3Actual, 1e-9)); + EXPECT(assert_equal(H4Expected, H4Actual, 1e-9)); +} + +//************************************************************************* +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//************************************************************************* + diff --git a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp new file mode 100644 index 000000000..a063244a3 --- /dev/null +++ b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp @@ -0,0 +1,312 @@ +/** + * @file testBTransformBtwRobotsUnaryFactor.cpp + * @brief Unit test for the TransformBtwRobotsUnaryFactor + * @author Vadim Indelman + */ + +#include + + +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + + +using namespace std; +using namespace gtsam; + +// Disabled this test because it is currently failing - remove the lines "#if 0" and "#endif" below +// to reenable the test. +//#if 0 +/* ************************************************************************* */ +LieVector predictionError(const Pose2& org1_T_org2, const gtsam::Key& key, const TransformBtwRobotsUnaryFactor& factor){ + gtsam::Values values; + values.insert(key, org1_T_org2); + // LieVector err = factor.whitenedError(values); + // return err; + return LieVector::Expmap(factor.whitenedError(values)); +} + +/* ************************************************************************* */ +//LieVector predictionError_standard(const Pose2& p1, const Pose2& p2, const gtsam::Key& keyA, const gtsam::Key& keyB, const BetweenFactor& factor){ +// gtsam::Values values; +// values.insert(keyA, p1); +// values.insert(keyB, p2); +// // LieVector err = factor.whitenedError(values); +// // return err; +// return LieVector::Expmap(factor.whitenedError(values)); +//} + +/* ************************************************************************* */ +TEST( TransformBtwRobotsUnaryFactor, ConstructorAndEquals) +{ + gtsam::Key key(0); + gtsam::Key keyA(1); + gtsam::Key keyB(2); + + gtsam::Pose2 p1(10.0, 15.0, 0.1); + gtsam::Pose2 p2(15.0, 15.0, 0.3); + gtsam::Pose2 noise(0.5, 0.4, 0.01); + gtsam::Pose2 rel_pose_ideal = p1.between(p2); + gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise); + + SharedGaussian model(noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 0.05))); + + gtsam::Values valA, valB; + valA.insert(keyA, p1); + valB.insert(keyB, p2); + + // Constructor + TransformBtwRobotsUnaryFactor g(key, rel_pose_msr, keyA, keyB, valA, valB, model); + TransformBtwRobotsUnaryFactor h(key, rel_pose_msr, keyA, keyB, valA, valB, model); + + // Equals + CHECK(assert_equal(g, h, 1e-5)); +} + +/* ************************************************************************* */ +TEST( TransformBtwRobotsUnaryFactor, unwhitenedError) +{ + gtsam::Key key(0); + gtsam::Key keyA(1); + gtsam::Key keyB(2); + + gtsam::Pose2 orgA_T_1(10.0, 15.0, 0.1); + gtsam::Pose2 orgB_T_2(15.0, 15.0, 0.3); + + gtsam::Pose2 orgA_T_orgB(100.0, 45.0, 1.8); + + gtsam::Pose2 orgA_T_2 = orgA_T_orgB.compose(orgB_T_2); + + gtsam::Pose2 rel_pose_ideal = orgA_T_1.between(orgA_T_2); + gtsam::Pose2 rel_pose_msr = rel_pose_ideal; + + SharedGaussian model(noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 0.05))); + + gtsam::Values valA, valB; + valA.insert(keyA, orgA_T_1); + valB.insert(keyB, orgB_T_2); + + // Constructor + TransformBtwRobotsUnaryFactor g(key, rel_pose_msr, keyA, keyB, valA, valB, model); + + gtsam::Values values; + values.insert(key, orgA_T_orgB); + Vector err = g.unwhitenedError(values); + + // Equals + CHECK(assert_equal(err, zero(3), 1e-5)); +} + +/* ************************************************************************* */ +TEST( TransformBtwRobotsUnaryFactor, unwhitenedError2) +{ + gtsam::Key key(0); + gtsam::Key keyA(1); + gtsam::Key keyB(2); + + gtsam::Pose2 orgA_T_currA(0.0, 0.0, 0.0); + gtsam::Pose2 orgB_T_currB(-10.0, 15.0, 0.1); + + gtsam::Pose2 orgA_T_orgB(0.0, 0.0, 0.0); + + gtsam::Pose2 orgA_T_currB = orgA_T_orgB.compose(orgB_T_currB); + + gtsam::Pose2 rel_pose_ideal = orgA_T_currA.between(orgA_T_currB); + gtsam::Pose2 rel_pose_msr = rel_pose_ideal; + + SharedGaussian model(noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 0.05))); + + gtsam::Values valA, valB; + valA.insert(keyA, orgA_T_currA); + valB.insert(keyB, orgB_T_currB); + + // Constructor + TransformBtwRobotsUnaryFactor g(key, rel_pose_msr, keyA, keyB, valA, valB, model); + + gtsam::Values values; + values.insert(key, orgA_T_orgB); + Vector err = g.unwhitenedError(values); + + // Equals + CHECK(assert_equal(err, zero(3), 1e-5)); +} + +/* ************************************************************************* */ +TEST( TransformBtwRobotsUnaryFactor, Optimize) +{ + gtsam::Key key(0); + gtsam::Key keyA(1); + gtsam::Key keyB(2); + + gtsam::Pose2 orgA_T_currA(0.0, 0.0, 0.0); + gtsam::Pose2 orgB_T_currB(1.0, 2.0, 0.05); + + gtsam::Pose2 orgA_T_orgB_tr(10.0, -15.0, 0.0); + gtsam::Pose2 orgA_T_currB_tr = orgA_T_orgB_tr.compose(orgB_T_currB); + gtsam::Pose2 currA_T_currB_tr = orgA_T_currA.between(orgA_T_currB_tr); + + // some error in measurements + // gtsam::Pose2 currA_Tmsr_currB1 = currA_T_currB_tr.compose(gtsam::Pose2(0.1, 0.02, 0.01)); + // gtsam::Pose2 currA_Tmsr_currB2 = currA_T_currB_tr.compose(gtsam::Pose2(-0.1, 0.02, 0.01)); + // gtsam::Pose2 currA_Tmsr_currB3 = currA_T_currB_tr.compose(gtsam::Pose2(0.1, -0.02, 0.01)); + // gtsam::Pose2 currA_Tmsr_currB4 = currA_T_currB_tr.compose(gtsam::Pose2(0.1, 0.02, -0.01)); + + // ideal measurements + gtsam::Pose2 currA_Tmsr_currB1 = currA_T_currB_tr.compose(gtsam::Pose2(0.0, 0.0, 0.0)); + gtsam::Pose2 currA_Tmsr_currB2 = currA_Tmsr_currB1; + gtsam::Pose2 currA_Tmsr_currB3 = currA_Tmsr_currB1; + gtsam::Pose2 currA_Tmsr_currB4 = currA_Tmsr_currB1; + + SharedGaussian model(noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 0.05))); + + gtsam::Values valA, valB; + valA.insert(keyA, orgA_T_currA); + valB.insert(keyB, orgB_T_currB); + + // Constructor + TransformBtwRobotsUnaryFactor g1(key, currA_Tmsr_currB1, keyA, keyB, valA, valB, model); + + TransformBtwRobotsUnaryFactor g2(key, currA_Tmsr_currB2, keyA, keyB, valA, valB, model); + + TransformBtwRobotsUnaryFactor g3(key, currA_Tmsr_currB3, keyA, keyB, valA, valB, model); + + TransformBtwRobotsUnaryFactor g4(key, currA_Tmsr_currB4, keyA, keyB, valA, valB, model); + + gtsam::Values values; + values.insert(key, gtsam::Pose2()); + + gtsam::NonlinearFactorGraph graph; + graph.push_back(g1); + graph.push_back(g2); + graph.push_back(g3); + graph.push_back(g4); + + gtsam::GaussNewtonParams params; + gtsam::GaussNewtonOptimizer optimizer(graph, values, params); + gtsam::Values result = optimizer.optimize(); + + gtsam::Pose2 orgA_T_orgB_opt = result.at(key); + + CHECK(assert_equal(orgA_T_orgB_opt, orgA_T_orgB_tr, 1e-5)); +} + + +/* ************************************************************************* */ +TEST( TransformBtwRobotsUnaryFactor, Jacobian) +{ + gtsam::Key key(0); + gtsam::Key keyA(1); + gtsam::Key keyB(2); + + gtsam::Pose2 orgA_T_1(10.0, 15.0, 0.1); + gtsam::Pose2 orgB_T_2(15.0, 15.0, 0.3); + + gtsam::Pose2 orgA_T_orgB(100.0, 45.0, 1.8); + + gtsam::Pose2 orgA_T_2 = orgA_T_orgB.compose(orgB_T_2); + + gtsam::Pose2 noise(0.5, 0.4, 0.01); + + gtsam::Pose2 rel_pose_ideal = orgA_T_1.between(orgA_T_2); + gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise); + + SharedGaussian model(noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 0.05))); + + gtsam::Values valA, valB; + valA.insert(keyA, orgA_T_1); + valB.insert(keyB, orgB_T_2); + + // Constructor + TransformBtwRobotsUnaryFactor g(key, rel_pose_msr, keyA, keyB, valA, valB, model); + + gtsam::Values values; + values.insert(key, orgA_T_orgB); + + std::vector H_actual(1); + Vector actual_err_wh = g.whitenedError(values, H_actual); + + Matrix H1_actual = H_actual[0]; + + double stepsize = 1.0e-9; + Matrix H1_expected = gtsam::numericalDerivative11(boost::bind(&predictionError, _1, key, g), orgA_T_orgB, stepsize); +// CHECK( assert_equal(H1_expected, H1_actual, 1e-5)); +} + + + + +/////* ************************************************************************** */ +//TEST (TransformBtwRobotsUnaryFactor, jacobian ) { +// +// gtsam::Key keyA(1); +// gtsam::Key keyB(2); +// +// // Inlier test +// gtsam::Pose2 p1(10.0, 15.0, 0.1); +// gtsam::Pose2 p2(15.0, 15.0, 0.3); +// gtsam::Pose2 noise(0.5, 0.4, 0.01); +// gtsam::Pose2 rel_pose_ideal = p1.between(p2); +// gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise); +// +// SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::(Vector(3) << 0.5, 0.5, 0.05))); +// SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::(Vector(3) << 50.0, 50.0, 10.0))); +// +// gtsam::Values values; +// values.insert(keyA, p1); +// values.insert(keyB, p2); +// +// double prior_outlier = 0.0; +// double prior_inlier = 1.0; +// +// TransformBtwRobotsUnaryFactor f(keyA, keyB, rel_pose_msr, model_inlier, model_outlier, +// prior_inlier, prior_outlier); +// +// std::vector H_actual(2); +// Vector actual_err_wh = f.whitenedError(values, H_actual); +// +// Matrix H1_actual = H_actual[0]; +// Matrix H2_actual = H_actual[1]; +// +// // compare to standard between factor +// BetweenFactor h(keyA, keyB, rel_pose_msr, model_inlier ); +// Vector actual_err_wh_stnd = h.whitenedError(values); +// Vector actual_err_wh_inlier = (Vector(3) << actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]); +// CHECK( assert_equal(actual_err_wh_stnd, actual_err_wh_inlier, 1e-8)); +// std::vector H_actual_stnd_unwh(2); +// (void)h.unwhitenedError(values, H_actual_stnd_unwh); +// Matrix H1_actual_stnd_unwh = H_actual_stnd_unwh[0]; +// Matrix H2_actual_stnd_unwh = H_actual_stnd_unwh[1]; +// Matrix H1_actual_stnd = model_inlier->Whiten(H1_actual_stnd_unwh); +// Matrix H2_actual_stnd = model_inlier->Whiten(H2_actual_stnd_unwh); +//// CHECK( assert_equal(H1_actual_stnd, H1_actual, 1e-8)); +//// CHECK( assert_equal(H2_actual_stnd, H2_actual, 1e-8)); +// +// double stepsize = 1.0e-9; +// Matrix H1_expected = gtsam::numericalDerivative11(boost::bind(&predictionError, _1, p2, keyA, keyB, f), p1, stepsize); +// Matrix H2_expected = gtsam::numericalDerivative11(boost::bind(&predictionError, p1, _1, keyA, keyB, f), p2, stepsize); +// +// +// // try to check numerical derivatives of a standard between factor +// Matrix H1_expected_stnd = gtsam::numericalDerivative11(boost::bind(&predictionError_standard, _1, p2, keyA, keyB, h), p1, stepsize); +// CHECK( assert_equal(H1_expected_stnd, H1_actual_stnd, 1e-5)); +// +// +// CHECK( assert_equal(H1_expected, H1_actual, 1e-8)); +// CHECK( assert_equal(H2_expected, H2_actual, 1e-8)); +// +//} + +//#endif + +/* ************************************************************************* */ + int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ diff --git a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp new file mode 100644 index 000000000..1ffb2bebe --- /dev/null +++ b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp @@ -0,0 +1,337 @@ +/** + * @file testBTransformBtwRobotsUnaryFactorEM.cpp + * @brief Unit test for the TransformBtwRobotsUnaryFactorEM + * @author Vadim Indelman + */ + +#include + + +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + + +using namespace std; +using namespace gtsam; + +// Disabled this test because it is currently failing - remove the lines "#if 0" and "#endif" below +// to reenable the test. +//#if 0 +/* ************************************************************************* */ +LieVector predictionError(const Pose2& org1_T_org2, const gtsam::Key& key, const TransformBtwRobotsUnaryFactorEM& factor){ + gtsam::Values values; + values.insert(key, org1_T_org2); + // LieVector err = factor.whitenedError(values); + // return err; + return LieVector::Expmap(factor.whitenedError(values)); +} + +/* ************************************************************************* */ +//LieVector predictionError_standard(const Pose2& p1, const Pose2& p2, const gtsam::Key& keyA, const gtsam::Key& keyB, const BetweenFactor& factor){ +// gtsam::Values values; +// values.insert(keyA, p1); +// values.insert(keyB, p2); +// // LieVector err = factor.whitenedError(values); +// // return err; +// return LieVector::Expmap(factor.whitenedError(values)); +//} + +/* ************************************************************************* */ +TEST( TransformBtwRobotsUnaryFactorEM, ConstructorAndEquals) +{ + gtsam::Key key(0); + gtsam::Key keyA(1); + gtsam::Key keyB(2); + + gtsam::Pose2 p1(10.0, 15.0, 0.1); + gtsam::Pose2 p2(15.0, 15.0, 0.3); + gtsam::Pose2 noise(0.5, 0.4, 0.01); + gtsam::Pose2 rel_pose_ideal = p1.between(p2); + gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise); + + SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 0.05))); + SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas((Vector(3) << 5, 5, 1.0))); + + double prior_outlier = 0.5; + double prior_inlier = 0.5; + + gtsam::Values valA, valB; + valA.insert(keyA, p1); + valB.insert(keyB, p2); + + // Constructor + TransformBtwRobotsUnaryFactorEM g(key, rel_pose_msr, keyA, keyB, valA, valB, + model_inlier, model_outlier,prior_inlier, prior_outlier); + TransformBtwRobotsUnaryFactorEM h(key, rel_pose_msr, keyA, keyB, valA, valB, + model_inlier, model_outlier,prior_inlier, prior_outlier); + + // Equals + CHECK(assert_equal(g, h, 1e-5)); +} + +/* ************************************************************************* */ +TEST( TransformBtwRobotsUnaryFactorEM, unwhitenedError) +{ + gtsam::Key key(0); + gtsam::Key keyA(1); + gtsam::Key keyB(2); + + gtsam::Pose2 orgA_T_1(10.0, 15.0, 0.1); + gtsam::Pose2 orgB_T_2(15.0, 15.0, 0.3); + + gtsam::Pose2 orgA_T_orgB(100.0, 45.0, 1.8); + + gtsam::Pose2 orgA_T_2 = orgA_T_orgB.compose(orgB_T_2); + + gtsam::Pose2 rel_pose_ideal = orgA_T_1.between(orgA_T_2); + gtsam::Pose2 rel_pose_msr = rel_pose_ideal; + + SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 0.05))); + SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas((Vector(3) << 5, 5, 1.0))); + + double prior_outlier = 0.01; + double prior_inlier = 0.99; + + gtsam::Values valA, valB; + valA.insert(keyA, orgA_T_1); + valB.insert(keyB, orgB_T_2); + + // Constructor + TransformBtwRobotsUnaryFactorEM g(key, rel_pose_msr, keyA, keyB, valA, valB, + model_inlier, model_outlier,prior_inlier, prior_outlier); + + gtsam::Values values; + values.insert(key, orgA_T_orgB); + Vector err = g.unwhitenedError(values); + + // Equals + CHECK(assert_equal(err, zero(3), 1e-5)); +} + +/* ************************************************************************* */ +TEST( TransformBtwRobotsUnaryFactorEM, unwhitenedError2) +{ + gtsam::Key key(0); + gtsam::Key keyA(1); + gtsam::Key keyB(2); + + gtsam::Pose2 orgA_T_currA(0.0, 0.0, 0.0); + gtsam::Pose2 orgB_T_currB(-10.0, 15.0, 0.1); + + gtsam::Pose2 orgA_T_orgB(0.0, 0.0, 0.0); + + gtsam::Pose2 orgA_T_currB = orgA_T_orgB.compose(orgB_T_currB); + + gtsam::Pose2 rel_pose_ideal = orgA_T_currA.between(orgA_T_currB); + gtsam::Pose2 rel_pose_msr = rel_pose_ideal; + + SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 0.05))); + SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas((Vector(3) << 5, 5, 1.0))); + + double prior_outlier = 0.01; + double prior_inlier = 0.99; + + gtsam::Values valA, valB; + valA.insert(keyA, orgA_T_currA); + valB.insert(keyB, orgB_T_currB); + + // Constructor + TransformBtwRobotsUnaryFactorEM g(key, rel_pose_msr, keyA, keyB, valA, valB, + model_inlier, model_outlier,prior_inlier, prior_outlier); + + gtsam::Values values; + values.insert(key, orgA_T_orgB); + Vector err = g.unwhitenedError(values); + + // Equals + CHECK(assert_equal(err, zero(3), 1e-5)); +} + +/* ************************************************************************* */ +TEST( TransformBtwRobotsUnaryFactorEM, Optimize) +{ + gtsam::Key key(0); + gtsam::Key keyA(1); + gtsam::Key keyB(2); + + gtsam::Pose2 orgA_T_currA(0.0, 0.0, 0.0); + gtsam::Pose2 orgB_T_currB(1.0, 2.0, 0.05); + + gtsam::Pose2 orgA_T_orgB_tr(10.0, -15.0, 0.0); + gtsam::Pose2 orgA_T_currB_tr = orgA_T_orgB_tr.compose(orgB_T_currB); + gtsam::Pose2 currA_T_currB_tr = orgA_T_currA.between(orgA_T_currB_tr); + + // some error in measurements + // gtsam::Pose2 currA_Tmsr_currB1 = currA_T_currB_tr.compose(gtsam::Pose2(0.1, 0.02, 0.01)); + // gtsam::Pose2 currA_Tmsr_currB2 = currA_T_currB_tr.compose(gtsam::Pose2(-0.1, 0.02, 0.01)); + // gtsam::Pose2 currA_Tmsr_currB3 = currA_T_currB_tr.compose(gtsam::Pose2(0.1, -0.02, 0.01)); + // gtsam::Pose2 currA_Tmsr_currB4 = currA_T_currB_tr.compose(gtsam::Pose2(0.1, 0.02, -0.01)); + + // ideal measurements + gtsam::Pose2 currA_Tmsr_currB1 = currA_T_currB_tr.compose(gtsam::Pose2(0.0, 0.0, 0.0)); + gtsam::Pose2 currA_Tmsr_currB2 = currA_Tmsr_currB1; + gtsam::Pose2 currA_Tmsr_currB3 = currA_Tmsr_currB1; + gtsam::Pose2 currA_Tmsr_currB4 = currA_Tmsr_currB1; + + SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 0.05))); + SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas((Vector(3) << 5, 5, 1.0))); + + double prior_outlier = 0.01; + double prior_inlier = 0.99; + + gtsam::Values valA, valB; + valA.insert(keyA, orgA_T_currA); + valB.insert(keyB, orgB_T_currB); + + // Constructor + TransformBtwRobotsUnaryFactorEM g1(key, currA_Tmsr_currB1, keyA, keyB, valA, valB, + model_inlier, model_outlier,prior_inlier, prior_outlier); + + TransformBtwRobotsUnaryFactorEM g2(key, currA_Tmsr_currB2, keyA, keyB, valA, valB, + model_inlier, model_outlier,prior_inlier, prior_outlier); + + TransformBtwRobotsUnaryFactorEM g3(key, currA_Tmsr_currB3, keyA, keyB, valA, valB, + model_inlier, model_outlier,prior_inlier, prior_outlier); + + TransformBtwRobotsUnaryFactorEM g4(key, currA_Tmsr_currB4, keyA, keyB, valA, valB, + model_inlier, model_outlier,prior_inlier, prior_outlier); + + gtsam::Values values; + values.insert(key, gtsam::Pose2()); + + gtsam::NonlinearFactorGraph graph; + graph.push_back(g1); + graph.push_back(g2); + graph.push_back(g3); + graph.push_back(g4); + + gtsam::GaussNewtonParams params; + gtsam::GaussNewtonOptimizer optimizer(graph, values, params); + gtsam::Values result = optimizer.optimize(); + + gtsam::Pose2 orgA_T_orgB_opt = result.at(key); + + CHECK(assert_equal(orgA_T_orgB_opt, orgA_T_orgB_tr, 1e-5)); +} + + +/* ************************************************************************* */ +TEST( TransformBtwRobotsUnaryFactorEM, Jacobian) +{ + gtsam::Key key(0); + gtsam::Key keyA(1); + gtsam::Key keyB(2); + + gtsam::Pose2 orgA_T_1(10.0, 15.0, 0.1); + gtsam::Pose2 orgB_T_2(15.0, 15.0, 0.3); + + gtsam::Pose2 orgA_T_orgB(100.0, 45.0, 1.8); + + gtsam::Pose2 orgA_T_2 = orgA_T_orgB.compose(orgB_T_2); + + gtsam::Pose2 noise(0.5, 0.4, 0.01); + + gtsam::Pose2 rel_pose_ideal = orgA_T_1.between(orgA_T_2); + gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise); + + SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 0.05))); + SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas((Vector(3) << 5, 5, 1.0))); + + double prior_outlier = 0.5; + double prior_inlier = 0.5; + + gtsam::Values valA, valB; + valA.insert(keyA, orgA_T_1); + valB.insert(keyB, orgB_T_2); + + // Constructor + TransformBtwRobotsUnaryFactorEM g(key, rel_pose_msr, keyA, keyB, valA, valB, + model_inlier, model_outlier,prior_inlier, prior_outlier); + + gtsam::Values values; + values.insert(key, orgA_T_orgB); + + std::vector H_actual(1); + Vector actual_err_wh = g.whitenedError(values, H_actual); + + Matrix H1_actual = H_actual[0]; + + double stepsize = 1.0e-9; + Matrix H1_expected = gtsam::numericalDerivative11(boost::bind(&predictionError, _1, key, g), orgA_T_orgB, stepsize); +// CHECK( assert_equal(H1_expected, H1_actual, 1e-5)); +} +/////* ************************************************************************** */ +//TEST (TransformBtwRobotsUnaryFactorEM, jacobian ) { +// +// gtsam::Key keyA(1); +// gtsam::Key keyB(2); +// +// // Inlier test +// gtsam::Pose2 p1(10.0, 15.0, 0.1); +// gtsam::Pose2 p2(15.0, 15.0, 0.3); +// gtsam::Pose2 noise(0.5, 0.4, 0.01); +// gtsam::Pose2 rel_pose_ideal = p1.between(p2); +// gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise); +// +// SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::(Vector(3) << 0.5, 0.5, 0.05))); +// SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::(Vector(3) << 50.0, 50.0, 10.0))); +// +// gtsam::Values values; +// values.insert(keyA, p1); +// values.insert(keyB, p2); +// +// double prior_outlier = 0.0; +// double prior_inlier = 1.0; +// +// TransformBtwRobotsUnaryFactorEM f(keyA, keyB, rel_pose_msr, model_inlier, model_outlier, +// prior_inlier, prior_outlier); +// +// std::vector H_actual(2); +// Vector actual_err_wh = f.whitenedError(values, H_actual); +// +// Matrix H1_actual = H_actual[0]; +// Matrix H2_actual = H_actual[1]; +// +// // compare to standard between factor +// BetweenFactor h(keyA, keyB, rel_pose_msr, model_inlier ); +// Vector actual_err_wh_stnd = h.whitenedError(values); +// Vector actual_err_wh_inlier = (Vector(3) << actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]); +// CHECK( assert_equal(actual_err_wh_stnd, actual_err_wh_inlier, 1e-8)); +// std::vector H_actual_stnd_unwh(2); +// (void)h.unwhitenedError(values, H_actual_stnd_unwh); +// Matrix H1_actual_stnd_unwh = H_actual_stnd_unwh[0]; +// Matrix H2_actual_stnd_unwh = H_actual_stnd_unwh[1]; +// Matrix H1_actual_stnd = model_inlier->Whiten(H1_actual_stnd_unwh); +// Matrix H2_actual_stnd = model_inlier->Whiten(H2_actual_stnd_unwh); +//// CHECK( assert_equal(H1_actual_stnd, H1_actual, 1e-8)); +//// CHECK( assert_equal(H2_actual_stnd, H2_actual, 1e-8)); +// +// double stepsize = 1.0e-9; +// Matrix H1_expected = gtsam::numericalDerivative11(boost::bind(&predictionError, _1, p2, keyA, keyB, f), p1, stepsize); +// Matrix H2_expected = gtsam::numericalDerivative11(boost::bind(&predictionError, p1, _1, keyA, keyB, f), p2, stepsize); +// +// +// // try to check numerical derivatives of a standard between factor +// Matrix H1_expected_stnd = gtsam::numericalDerivative11(boost::bind(&predictionError_standard, _1, p2, keyA, keyB, h), p1, stepsize); +// CHECK( assert_equal(H1_expected_stnd, H1_actual_stnd, 1e-5)); +// +// +// CHECK( assert_equal(H1_expected, H1_actual, 1e-8)); +// CHECK( assert_equal(H2_expected, H2_actual, 1e-8)); +// +//} + +//#endif + +/* ************************************************************************* */ + int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ diff --git a/gtsam_unstable/slam/tests/timeInertialNavFactor_GlobalVelocity.cpp b/gtsam_unstable/slam/tests/timeInertialNavFactor_GlobalVelocity.cpp index f036eadeb..5132df658 100644 --- a/gtsam_unstable/slam/tests/timeInertialNavFactor_GlobalVelocity.cpp +++ b/gtsam_unstable/slam/tests/timeInertialNavFactor_GlobalVelocity.cpp @@ -28,11 +28,11 @@ using namespace std; using namespace gtsam; gtsam::Rot3 world_R_ECEF( - 0.31686, 0.51505, 0.79645, - 0.85173, -0.52399, 0, - 0.41733, 0.67835, -0.60471); + 0.31686, 0.51505, 0.79645, + 0.85173, -0.52399, 0, + 0.41733, 0.67835, -0.60471); -gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); +gtsam::Vector ECEF_omega_earth((Vec(3) << 0.0, 0.0, 7.292115e-5)); gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); /* ************************************************************************* */ @@ -54,16 +54,16 @@ int main() { gtsam::Key BiasKey1(31); double measurement_dt(0.1); - Vector world_g(Vector_(3, 0.0, 0.0, 9.81)); - Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); + Vector world_g((Vec(3) << 0.0, 0.0, 9.81)); + Vector world_rho((Vec(3) << 0.0, -1.5724e-05, 0.0)); // NED system + gtsam::Vector ECEF_omega_earth((Vec(3) << 0.0, 0.0, 7.292115e-5)); gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); // Second test: zero angular motion, some acceleration - generated in matlab - Vector measurement_acc(Vector_(3, 6.501390843381716, -6.763926150509185, -2.300389940090343)); - Vector measurement_gyro(Vector_(3, 0.1, 0.2, 0.3)); + Vector measurement_acc((Vec(3) << 6.501390843381716, -6.763926150509185, -2.300389940090343)); + Vector measurement_gyro((Vec(3) << 0.1, 0.2, 0.3)); InertialNavFactor_GlobalVelocity f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); diff --git a/matlab.h b/matlab.h index 6c2b137d7..100f5a242 100644 --- a/matlab.h +++ b/matlab.h @@ -84,7 +84,7 @@ namespace gtsam { return result; } - /// perturb all Point2 using normally distributed noise + /// Perturb all Point2 values using normally distributed noise void perturbPoint2(Values& values, double sigma, int32_t seed = 42u) { noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(2,sigma); Sampler sampler(model, seed); @@ -93,7 +93,17 @@ namespace gtsam { } } - /// perturb all Point3 using normally distributed noise + /// Perturb all Pose2 values using normally distributed noise + void perturbPose2(Values& values, double sigmaT, double sigmaR, int32_t seed = 42u) { + noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas( + Vector3(sigmaT, sigmaT, sigmaR)); + Sampler sampler(model, seed); + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, values.filter()) { + values.update(key_value.key, key_value.value.retract(sampler.sample())); + } + } + + /// Perturb all Point3 values using normally distributed noise void perturbPoint3(Values& values, double sigma, int32_t seed = 42u) { noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,sigma); Sampler sampler(model, seed); @@ -102,7 +112,7 @@ namespace gtsam { } } - /// insert a number of initial point values by backprojecting + /// Insert a number of initial point values by backprojecting void insertBackprojections(Values& values, const SimpleCamera& camera, const Vector& J, const Matrix& Z, double depth) { if (Z.rows() != 2) throw std::invalid_argument("insertBackProjections: Z must be 2*K"); if (Z.cols() != J.size()) throw std::invalid_argument("insertBackProjections: J and Z must have same number of entries"); @@ -113,7 +123,7 @@ namespace gtsam { } } - /// insert multiple projection factors for a single pose key + /// Insert multiple projection factors for a single pose key void insertProjectionFactors(NonlinearFactorGraph& graph, Key i, const Vector& J, const Matrix& Z, const SharedNoiseModel& model, const Cal3_S2::shared_ptr K, const Pose3& body_P_sensor = Pose3()) { if (Z.rows() != 2) throw std::invalid_argument("addMeasurements: Z must be 2*K"); @@ -126,7 +136,7 @@ namespace gtsam { } } - /// calculate the errors of all projection factors in a graph + /// Calculate the errors of all projection factors in a graph Matrix reprojectionErrors(const NonlinearFactorGraph& graph, const Values& values) { // first count size_t K = 0, k=0; diff --git a/matlab/+gtsam/Contents.m b/matlab/+gtsam/Contents.m index 8d15bc913..5fec7721c 100644 --- a/matlab/+gtsam/Contents.m +++ b/matlab/+gtsam/Contents.m @@ -173,3 +173,16 @@ % symbol - create a Symbol key % symbolChr - get character from a symbol key % symbolIndex - get index from a symbol key +% +%% Wrapped C++ Convenience Functions for use within MATLAB +% utilities.extractPoint2 - Extract all Point2 values into a single matrix [x y] +% utilities.extractPoint3 - Extract all Point3 values into a single matrix [x y z] +% utilities.extractPose2 - Extract all Pose2 values into a single matrix [x y theta] +% utilities.allPose3s - Extract all Pose3 values +% utilities.extractPose3 - Extract all Pose3 values into a single matrix [r11 r12 r13 r21 r22 r23 r31 r32 r33 x y z] +% utilities.perturbPoint2 - Perturb all Point2 values using normally distributed noise +% utilities.perturbPose2 - Perturb all Pose2 values using normally distributed noise +% utilities.perturbPoint3 - Perturb all Point3 values using normally distributed noise +% utilities.insertBackprojections - Insert a number of initial point values by backprojecting +% utilities.insertProjectionFactors - Insert multiple projection factors for a single pose key +% utilities.reprojectionErrors - Calculate the errors of all projection factors in a graph diff --git a/matlab/+gtsam/covarianceEllipse.m b/matlab/+gtsam/covarianceEllipse.m index 387d7cd12..cdc239bb2 100644 --- a/matlab/+gtsam/covarianceEllipse.m +++ b/matlab/+gtsam/covarianceEllipse.m @@ -1,4 +1,4 @@ -function covarianceEllipse(x,P,color, k) +function h = covarianceEllipse(x,P,color, k) % covarianceEllipse plots a Gaussian as an uncertainty ellipse % Based on Maybeck Vol 1, page 366 % k=2.296 corresponds to 1 std, 68.26% of all probability @@ -13,7 +13,7 @@ s1 = s(1,1); s2 = s(2,2); if nargin<4, k = 2.296; end; [ex,ey] = ellipse( sqrt(s1*k)*e(:,1), sqrt(s2*k)*e(:,2), x(1:2) ); -line(ex,ey,'color',color); +h = line(ex,ey,'color',color); function [x,y] = ellipse(a,b,c); % ellipse: return the x and y coordinates for an ellipse diff --git a/matlab/CMakeLists.txt b/matlab/CMakeLists.txt index 3e9e9d370..c7b7d6441 100644 --- a/matlab/CMakeLists.txt +++ b/matlab/CMakeLists.txt @@ -3,23 +3,39 @@ include(GtsamMatlabWrap) # Tests -message(STATUS "Installing Matlab Toolbox Tests") -install(DIRECTORY "${GTSAM_SOURCE_ROOT_DIR}/matlab/gtsam_tests" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING PATTERN "*.m" PATTERN ".svn" EXCLUDE) +#message(STATUS "Installing Matlab Toolbox") +install_matlab_scripts("${GTSAM_SOURCE_ROOT_DIR}/matlab/" "*.m;*.fig") +install_matlab_scripts("${GTSAM_SOURCE_ROOT_DIR}/matlab/" "README-gtsam-toolbox.txt") # Examples -message(STATUS "Installing Matlab Toolbox Examples") +#message(STATUS "Installing Matlab Toolbox Examples") # Matlab files: *.m and *.fig -install(DIRECTORY "${GTSAM_SOURCE_ROOT_DIR}/matlab/gtsam_examples" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING PATTERN "*.m" PATTERN "*.fig" PATTERN ".svn" EXCLUDE) +#install_matlab_scripts("${GTSAM_SOURCE_ROOT_DIR}/matlab/gtsam_examples" "*.m;*.fig") # Utilities -message(STATUS "Installing Matlab Toolbox Utilities") -install(DIRECTORY "${GTSAM_SOURCE_ROOT_DIR}/matlab/+gtsam" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING PATTERN "*.m" PATTERN ".svn" EXCLUDE) -install(FILES "${GTSAM_SOURCE_ROOT_DIR}/matlab/README-gtsam-toolbox.txt" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}") +#message(STATUS "Installing Matlab Toolbox Utilities") +#install_matlab_scripts("${GTSAM_SOURCE_ROOT_DIR}/matlab/+gtsam" "*.m") -message(STATUS "Installing Matlab Toolbox Examples (Data)") -# Data files: *.graph and *.txt +#message(STATUS "Installing Matlab Toolbox Example Data") +# Data files: *.graph, *.mat, and *.txt file(GLOB matlab_examples_data_graph "${GTSAM_SOURCE_ROOT_DIR}/examples/Data/*.graph") file(GLOB matlab_examples_data_mat "${GTSAM_SOURCE_ROOT_DIR}/examples/Data/*.mat") file(GLOB matlab_examples_data_txt "${GTSAM_SOURCE_ROOT_DIR}/examples/Data/*.txt") set(matlab_examples_data ${matlab_examples_data_graph} ${matlab_examples_data_mat} ${matlab_examples_data_txt}) -install(FILES ${matlab_examples_data} DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH}/gtsam_examples/Data) +if(GTSAM_BUILD_TYPE_POSTFIXES) + foreach(build_type ${CMAKE_CONFIGURATION_TYPES}) + string(TOUPPER "${build_type}" build_type_upper) + if("${build_type_upper}" STREQUAL "RELEASE") + set(build_type_tag "") # Don't create release mode tag on installed directory + else() + set(build_type_tag "${build_type}") + endif() + # Split up filename to strip trailing '/' in GTSAM_TOOLBOX_INSTALL_PATH if there is one + get_filename_component(location "${GTSAM_TOOLBOX_INSTALL_PATH}" PATH) + get_filename_component(name "${GTSAM_TOOLBOX_INSTALL_PATH}" NAME) + install(FILES ${matlab_examples_data} DESTINATION "${location}/${name}${build_type_tag}/gtsam_examples/Data" CONFIGURATIONS "${build_type}") + endforeach() +else() + install(FILES ${matlab_examples_data} DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH}/gtsam_examples/Data) +endif() + diff --git a/matlab/gtsam_tests/testJacobianFactor.m b/matlab/gtsam_tests/testJacobianFactor.m index 3bc8f4edd..bba6ca5ac 100644 --- a/matlab/gtsam_tests/testJacobianFactor.m +++ b/matlab/gtsam_tests/testJacobianFactor.m @@ -36,7 +36,9 @@ model4 = noiseModel.Diagonal.Sigmas(sigmas); combined = JacobianFactor(x2, Ax2, l1, Al1, x1, Ax1, b2, model4); % eliminate the first variable (x2) in the combined factor, destructive ! -actualCG = combined.eliminateFirst(); +ord=Ordering; +ord.push_back(x2); +actualCG = combined.eliminate(ord); % create expected Conditional Gaussian R11 = [ @@ -52,7 +54,7 @@ S13 = [ +0.00,-8.94427 ]; d=[2.23607;-1.56525]; -expectedCG = GaussianConditional(x2,d,R11,l1,S12,x1,S13,[1;1]); +expectedCG = GaussianConditional(x2,d,R11,l1,S12,x1,S13); % check if the result matches CHECK('actualCG.equals(expectedCG,1e-5)',actualCG.equals(expectedCG,1e-4)); diff --git a/matlab/unstable_examples/.gitignore b/matlab/unstable_examples/.gitignore new file mode 100644 index 000000000..6d725d9bc --- /dev/null +++ b/matlab/unstable_examples/.gitignore @@ -0,0 +1 @@ +*.m~ diff --git a/matlab/unstable_examples/SmartRangeFactorExample.m b/matlab/unstable_examples/SmartRangeFactorExample.m new file mode 100644 index 000000000..93e5dce0c --- /dev/null +++ b/matlab/unstable_examples/SmartRangeFactorExample.m @@ -0,0 +1,220 @@ +clear all; clear all; +clc +close all +import gtsam.*; + +%% Noise settings +t_noise = 0.1; % odometry tranlation error +r_noise = 0.05; % odometry rotation error +range_noise = 0.001; % range measurements error + +% Create noise models +noiseRange = noiseModel.Isotropic.Sigma(1, range_noise); % range measurements noise model +noiseOdom = noiseModel.Diagonal.Sigmas([t_noise; t_noise; r_noise]); % odometry noise model +noiseInit = noiseModel.Diagonal.Sigmas([0.01; 0.01; 0.0001]); % for prior on first pose + +%% Choose initial guess for landmarks +% if set to 1 we set the landmark guess to the true position +goodInitFlag_lmk1 = 0; +goodInitFlag_lmk2 = 1; +goodInitFlag_lmk3 = 1; +% true positions +lmk1 = Point2([0.5;0.5]); +lmk2 = Point2([1.5;0.5]); +lmk3 = Point2([2.5;0.5]); +% specular positions (to simulate ambiguity in triangulation) +lmk1_bad = Point2([0.5;-0.5]); +lmk2_bad = Point2([1.5;1.5]); +lmk3_bad = Point2([2.5;-0.5]); + +%% Create keys +num_poses = 7; +for ind_pose = 1:num_poses + poseKey(ind_pose) = symbol('x',ind_pose); % Key for each pose +end +lmkKey(1) = symbol('l',1); % Key for each pose +lmkKey(2) = symbol('l',2); % Key for each pose +lmkKey(3) = symbol('l',3); % Key for each pose + +%% Factor graphs +smartGraph = NonlinearFactorGraph; +smartValues = Values; + +fullGraph = NonlinearFactorGraph; +fullValues = Values; + +% Add prior on first pose +poseInit = Pose2; +smartValues.insert(poseKey(1), poseInit ); +smartGraph.add(PriorFactorPose2(poseKey(1), poseInit, noiseInit)); +fullValues.insert(poseKey(1), poseInit ); +fullGraph.add(PriorFactorPose2(poseKey(1), poseInit, noiseInit)); +currPose = poseInit; + +srf1 = SmartRangeFactor(range_noise); +srf2 = SmartRangeFactor(range_noise); +srf3 = SmartRangeFactor(range_noise); + +%% Main loop +for ind_pose = 2:7 + ind_pose + + %% apply command, depending on the time step + switch ind_pose + case 2 + v = [1; 0; pi/2]; + case 3 + v = [1; 0; -pi/2]; + case 4 + v = [1; 0; -pi/2]; + case 5 + v = [1; 0; pi/2]; + case 6 + v = [1; 0; pi/2]; + case 7 + v = [1; 0; 0]; + end + + %% compute intial guess for poses (initialized to ground truth) + currPose = currPose.retract(v); + smartValues.insert(poseKey(ind_pose), currPose ); + fullValues.insert(poseKey(ind_pose), currPose ); + + key_prev = poseKey(ind_pose-1); + key_curr = poseKey(ind_pose); + prev_pose = smartValues.at(key_prev); + curr_pose = smartValues.at(key_curr); + GTDeltaPose = prev_pose.between(curr_pose); + noisePose = Pose2([t_noise*rand; t_noise*rand; r_noise*rand]); + NoisyDeltaPose = GTDeltaPose.compose(noisePose); + + % add odometry factors + smartGraph.add(BetweenFactorPose2(key_prev, key_curr, NoisyDeltaPose, noiseOdom)); + fullGraph.add(BetweenFactorPose2(key_prev, key_curr, NoisyDeltaPose, noiseOdom)); + + % add range factors + switch ind_pose + %==================================================================== + case 2 + % x1-lmk1 + % x2-lmk1 + r1 = prev_pose.range(lmk1); % range of lmk1 wrt x1 + srf1.addRange(key_prev, r1); + r2 = curr_pose.range(lmk1); % range of lmk1 wrt x2 + srf1.addRange(key_curr, r2); + + rangef1 = RangeFactorPosePoint2(key_prev, lmkKey(1), r1, noiseRange); + fullGraph.add(rangef1); + rangef2 = RangeFactorPosePoint2(key_curr, lmkKey(1), r2, noiseRange); + fullGraph.add(rangef2); + + if goodInitFlag_lmk1==1 + fullValues.insert(lmkKey(1), lmk1); + else + fullValues.insert(lmkKey(1), lmk1_bad); + end + %==================================================================== + case 3 + % x3-lmk1 + % x3-lmk2 + r3 = curr_pose.range(lmk1); % range of lmk1 wrt x3 + srf1.addRange(key_curr, r3); + smartGraph.add(srf1); + r4 = curr_pose.range(lmk2); % range of lmk2 wrt x3 + srf2.addRange(key_curr, r4); + + rangef3 = RangeFactorPosePoint2(key_curr, lmkKey(1), r3, noiseRange); + fullGraph.add(rangef3); + rangef4 = RangeFactorPosePoint2(key_curr, lmkKey(2), r4, noiseRange); + % IF WE ADD FACTOR HERE IT CRASHES: fullGraph.add(rangef4); + %==================================================================== + case 4 + % x4-lmk2 + % x4-lmk3 + r5 = curr_pose.range(lmk2); % range of lmk2 wrt x4 + srf2.addRange(key_curr, r5); + r6 = curr_pose.range(lmk3); % range of lmk3 wrt x4 + srf3.addRange(key_curr, r6); + + % DELAYED INITIALIZATION: + fullGraph.add(rangef4); + rangef5 = RangeFactorPosePoint2(key_curr, lmkKey(2), r5, noiseRange); + fullGraph.add(rangef5); + rangef6 = RangeFactorPosePoint2(key_curr, lmkKey(3), r6, noiseRange); + % IF WE ADD FACTOR HERE IT CRASHES: fullGraph.add(rangef6); + + if goodInitFlag_lmk2==1 + fullValues.insert(lmkKey(2), lmk2); + else + fullValues.insert(lmkKey(2), lmk2_bad); + end + %==================================================================== + case 5 + % x5-lmk2 + % x4-lmk3 + r7 = curr_pose.range(lmk2); % range of lmk2 wrt x5 + srf2.addRange(key_curr, r7); + smartGraph.add(srf2); + r8 = curr_pose.range(lmk3); % range of lmk3 wrt x5 + srf3.addRange(key_curr, r8); + + % DELAYED INITIALIZATION: + fullGraph.add(rangef6); + rangef7 = RangeFactorPosePoint2(key_curr, lmkKey(2), r7, noiseRange); + fullGraph.add(rangef7); + rangef8 = RangeFactorPosePoint2(key_curr, lmkKey(3), r8, noiseRange); + fullGraph.add(rangef8); + + if goodInitFlag_lmk3==1 + fullValues.insert(lmkKey(3), lmk3); + else + fullValues.insert(lmkKey(3), lmk3_bad); + end + %==================================================================== + case 6 + % x6-lmk3 + r9 = curr_pose.range(lmk3); % range of lmk3 wrt x6 + srf3.addRange(key_curr, r9); + + rangef9 = RangeFactorPosePoint2(key_curr, lmkKey(3), r9, noiseRange); + fullGraph.add(rangef9); + case 7 + % x6-lmk3 + r10 = curr_pose.range(lmk3); % range of lmk3 wrt x7 + srf3.addRange(key_curr, r10); + smartGraph.add(srf3); + + rangef10 = RangeFactorPosePoint2(key_curr, lmkKey(3), r10, noiseRange); + fullGraph.add(rangef10); + end + + smartOpt = GaussNewtonOptimizer(smartGraph, smartValues); + smartSolution = smartOpt.optimize; + figure(1) + clf + plot2DTrajectory(smartSolution, 'b.-'); + title('Ground truth (g) VS smart estimate (b) VS full estimate (r)') + xlabel('x') + ylabel('y') + zlabel('z') + axis equal + hold on + + fullOpt = GaussNewtonOptimizer(fullGraph, fullValues); + fullSolution = fullOpt.optimize; + figure(1) + plot2DTrajectory(fullSolution, 'r.-'); + + figure(1) + plot2DTrajectory(smartValues, 'g.-'); + drawnow; +end + + + + + + + + + diff --git a/matlab/unstable_examples/testTSAMFactors.m b/matlab/unstable_examples/testTSAMFactors.m new file mode 100644 index 000000000..abdfc5922 --- /dev/null +++ b/matlab/unstable_examples/testTSAMFactors.m @@ -0,0 +1,62 @@ +% TSAMFactorsExample +% Frank Dellaert, May 2014 + +import gtsam.*; + +% noise models +noisePrior = noiseModel.Diagonal.Sigmas([100; 100; 100]); +noiseDelta = noiseModel.Isotropic.Sigma(2, 0.1); +noiseOdom = noiseModel.Diagonal.Sigmas([0.1; 0.1; 0.05]); + +% Example is 2 landmarks 1 and 2, 2 poses 10 and 20, 2 base nodes 100 and 200 +% l1 l2 +% + + +% - b - p - p - b +% +---+---+---+ + +% True values +b1 = Pose2(0,0,0); +b2 = Pose2(2,0,0); +l1 = Point2(0,1); +l2 = Point2(2,1); + +% Create a graph +graph = NonlinearFactorGraph; +origin = Pose2; +graph.add(gtsam.PriorFactorPose2(10, origin, noisePrior)) +graph.add(gtsam.PriorFactorPose2(20, origin, noisePrior)) +graph.add(gtsam.PriorFactorPose2(100, origin, noisePrior)) +graph.add(DeltaFactor(10, 1, b1.transform_to(l1), noiseDelta)) +graph.add(DeltaFactor(20, 1, b2.transform_to(l2), noiseDelta)) +graph.add(DeltaFactorBase(100,10, 200,2, b1.transform_to(l2), noiseDelta)) +graph.add(DeltaFactorBase(200,20, 100,1, b2.transform_to(l1), noiseDelta)) +graph.add(OdometryFactorBase(100,10,200,20, Pose2(2,0,0), noiseOdom)) + +% Initial values +initial = Values; +initial.insert(100,origin); +initial.insert(10,origin); +initial.insert(1,l1); +initial.insert(2,l2); +initial.insert(20,origin); +initial.insert(200,origin); + +% optimize +params = LevenbergMarquardtParams; +% params.setVerbosity('ERROR'); +optimizer = LevenbergMarquardtOptimizer(graph, initial, params); +result = optimizer.optimize(); + +% Check result +CHECK('error',result.at(100).equals(b1,1e-5)) +CHECK('error',result.at(10).equals(origin,1e-5)) +CHECK('error',result.at(1).equals(Point2(0,1),1e-5)) +CHECK('error',result.at(2).equals(Point2(0,1),1e-5)) +CHECK('error',result.at(20).equals(origin,1e-5)) +CHECK('error',result.at(200).equals(b2,1e-5)) + +% Check error +CHECK('error',abs(graph.error(result))<1e-9) +for i=0:7 + CHECK('error',abs(graph.at(i).error(result))<1e-9) +end diff --git a/package_scripts/toolbox_package_unix.sh b/package_scripts/toolbox_package_unix.sh index 67c458ae4..7ae0fa169 100755 --- a/package_scripts/toolbox_package_unix.sh +++ b/package_scripts/toolbox_package_unix.sh @@ -39,7 +39,7 @@ cmake -DCMAKE_BUILD_TYPE=Release \ -DBoost_USE_STATIC_LIBS:bool=true \ -DBOOST_ROOT="$1" \ -DGTSAM_BUILD_SHARED_LIBRARY:bool=false \ --DGTSAM_BUILD_STATIC_LIBRARY:bool=true \ +-DGTSAM_BUILD_STATIC_LIBRARY:bool=false \ -DGTSAM_BUILD_TESTS:bool=false \ -DGTSAM_BUILD_EXAMPLES:bool=false \ -DGTSAM_BUILD_UNSTABLE:bool=false \ @@ -54,7 +54,7 @@ if [ ! $? ]; then fi # Compile -make -j2 install +make -j8 install # Create package -tar czf gtsam-toolbox-2.3.1-$platform.tgz -C stage/borg toolbox +tar czf gtsam-toolbox-3.0.0-$platform.tgz -C stage/gtsam_toolbox toolbox diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index cc91ea4f3..cf81dc762 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -1,38 +1,19 @@ -# Assemble local libraries -set(tests_local_libs - slam - nonlinear - linear - discrete - inference - geometry - base - ccolamd - CppUnitLite) - # exclude certain files # note the source dir on each -set (tests_exclude - #"${CMAKE_CURRENT_SOURCE_DIR}/testOccupancyGrid.cpp" -) +set (tests_exclude "") if ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang") # might not be best test - Richard & Jason & Frank # clang linker segfaults on large testSerializationSLAM - list (APPEND tests_exclude "${CMAKE_CURRENT_SOURCE_DIR}/testSerializationSLAM.cpp") + list (APPEND tests_exclude "testSerializationSLAM.cpp") endif() # Build tests -if (GTSAM_BUILD_TESTS) - # Subdirectory target for tests - add_custom_target(check.tests COMMAND ${CMAKE_CTEST_COMMAND}) - set(is_test TRUE) +gtsamAddTestsGlob(tests "test*.cpp" "${tests_exclude}" "gtsam") - # Build grouped tests - gtsam_add_grouped_scripts("tests" # Use subdirectory as group label - "test*.cpp;*.h" check "Test" # Standard for all tests - "${tests_local_libs}" "${gtsam-default};CppUnitLite" "${tests_exclude}" # Pass in linking and exclusion lists - ${is_test}) # Set all as tests -endif (GTSAM_BUILD_TESTS) +if(MSVC) + set_property(SOURCE "${CMAKE_CURRENT_SOURCE_DIR}/testSerializationSLAM.cpp" + APPEND PROPERTY COMPILE_FLAGS "/bigobj") +endif() # Build timing scripts if (GTSAM_BUILD_TIMING) @@ -43,11 +24,6 @@ if (GTSAM_BUILD_TIMING) # Build grouped benchmarks gtsam_add_grouped_scripts("tests" # Use subdirectory as group label "time*.cpp" timing "Timing Benchmark" # Standard for all timing scripts - "${tests_local_libs}" "${gtsam-default};CppUnitLite" "${tests_exclude}" # Pass in linking and exclusion lists + "${tests_full_libs}" "${tests_full_libs}" "${tests_exclude}" # Pass in linking and exclusion lists ${is_test}) endif (GTSAM_BUILD_TIMING) - -if(MSVC) - set_property(SOURCE "${CMAKE_CURRENT_SOURCE_DIR}/testSerializationSLAM.cpp" - APPEND PROPERTY COMPILE_FLAGS "/bigobj") -endif() diff --git a/tests/simulated2D.h b/tests/simulated2D.h index 22f3ee776..b67ef0aef 100644 --- a/tests/simulated2D.h +++ b/tests/simulated2D.h @@ -83,43 +83,41 @@ namespace simulated2D { } }; - namespace { - /// Prior on a single pose - inline Point2 prior(const Point2& x) { - return x; - } + /// Prior on a single pose + inline Point2 prior(const Point2& x) { + return x; + } - /// Prior on a single pose, optionally returns derivative - Point2 prior(const Point2& x, boost::optional H = boost::none) { - if (H) *H = gtsam::eye(2); - return x; - } + /// Prior on a single pose, optionally returns derivative + inline Point2 prior(const Point2& x, boost::optional H = boost::none) { + if (H) *H = gtsam::eye(2); + return x; + } - /// odometry between two poses - inline Point2 odo(const Point2& x1, const Point2& x2) { + /// odometry between two poses + inline Point2 odo(const Point2& x1, const Point2& x2) { + return x2 - x1; + } + + /// odometry between two poses, optionally returns derivative + inline Point2 odo(const Point2& x1, const Point2& x2, boost::optional H1 = + boost::none, boost::optional H2 = boost::none) { + if (H1) *H1 = -gtsam::eye(2); + if (H2) *H2 = gtsam::eye(2); return x2 - x1; - } + } - /// odometry between two poses, optionally returns derivative - Point2 odo(const Point2& x1, const Point2& x2, boost::optional H1 = - boost::none, boost::optional H2 = boost::none) { - if (H1) *H1 = -gtsam::eye(2); - if (H2) *H2 = gtsam::eye(2); - return x2 - x1; - } + /// measurement between landmark and pose + inline Point2 mea(const Point2& x, const Point2& l) { + return l - x; + } - /// measurement between landmark and pose - inline Point2 mea(const Point2& x, const Point2& l) { + /// measurement between landmark and pose, optionally returns derivative + inline Point2 mea(const Point2& x, const Point2& l, boost::optional H1 = + boost::none, boost::optional H2 = boost::none) { + if (H1) *H1 = -gtsam::eye(2); + if (H2) *H2 = gtsam::eye(2); return l - x; - } - - /// measurement between landmark and pose, optionally returns derivative - Point2 mea(const Point2& x, const Point2& l, boost::optional H1 = - boost::none, boost::optional H2 = boost::none) { - if (H1) *H1 = -gtsam::eye(2); - if (H2) *H2 = gtsam::eye(2); - return l - x; - } } /** diff --git a/tests/smallExample.h b/tests/smallExample.h index eb87e84e2..7de553a68 100644 --- a/tests/smallExample.h +++ b/tests/smallExample.h @@ -22,76 +22,76 @@ #pragma once #include -#include +#include #include +#include +#include #include +#include namespace gtsam { namespace example { - namespace { - -typedef NonlinearFactorGraph Graph; /** * Create small example for non-linear factor graph */ -boost::shared_ptr sharedNonlinearFactorGraph(); -Graph createNonlinearFactorGraph(); +// inline boost::shared_ptr sharedNonlinearFactorGraph(); +// inline NonlinearFactorGraph createNonlinearFactorGraph(); /** * Create values structure to go with it * The ground truth values structure for the example above */ -Values createValues(); +// inline Values createValues(); /** Vector Values equivalent */ -VectorValues createVectorValues(); +// inline VectorValues createVectorValues(); /** * create a noisy values structure for a nonlinear factor graph */ -boost::shared_ptr sharedNoisyValues(); -Values createNoisyValues(); +// inline boost::shared_ptr sharedNoisyValues(); +// inline Values createNoisyValues(); /** * Zero delta config */ -VectorValues createZeroDelta(const Ordering& ordering); +// inline VectorValues createZeroDelta(); /** * Delta config that, when added to noisyValues, returns the ground truth */ -VectorValues createCorrectDelta(const Ordering& ordering); +// inline VectorValues createCorrectDelta(); /** * create a linear factor graph * The non-linear graph above evaluated at NoisyValues */ -GaussianFactorGraph createGaussianFactorGraph(const Ordering& ordering); +// inline GaussianFactorGraph createGaussianFactorGraph(); /** * create small Chordal Bayes Net x <- y */ -GaussianBayesNet createSmallGaussianBayesNet(); +// inline GaussianBayesNet createSmallGaussianBayesNet(); /** * Create really non-linear factor graph (cos/sin) */ -boost::shared_ptr -sharedReallyNonlinearFactorGraph(); -Graph createReallyNonlinearFactorGraph(); +// inline boost::shared_ptr +//sharedReallyNonlinearFactorGraph(); +// inline NonlinearFactorGraph createReallyNonlinearFactorGraph(); /** * Create a full nonlinear smoother * @param T number of time-steps */ -std::pair createNonlinearSmoother(int T); +// inline std::pair createNonlinearSmoother(int T); /** * Create a Kalman smoother by linearizing a non-linear factor graph * @param T number of time-steps */ -std::pair, Ordering> createSmoother(int T, boost::optional ordering = boost::none); +// inline GaussianFactorGraph createSmoother(int T); /* ******************************************************* */ // Linear Constrained Examples @@ -101,22 +101,22 @@ std::pair, Ordering> createSmoother(int T, boost::op * Creates a simple constrained graph with one linear factor and * one binary equality constraint that sets x = y */ -GaussianFactorGraph createSimpleConstraintGraph(); -VectorValues createSimpleConstraintValues(); +// inline GaussianFactorGraph createSimpleConstraintGraph(); +// inline VectorValues createSimpleConstraintValues(); /** * Creates a simple constrained graph with one linear factor and * one binary constraint. */ -GaussianFactorGraph createSingleConstraintGraph(); -VectorValues createSingleConstraintValues(); +// inline GaussianFactorGraph createSingleConstraintGraph(); +// inline VectorValues createSingleConstraintValues(); /** * Creates a constrained graph with a linear factor and two * binary constraints that share a node */ -GaussianFactorGraph createMultiConstraintGraph(); -VectorValues createMultiConstraintValues(); +// inline GaussianFactorGraph createMultiConstraintGraph(); +// inline VectorValues createMultiConstraintValues(); /* ******************************************************* */ // Planar graph with easy subtree for SubgraphPreconditioner @@ -131,14 +131,14 @@ VectorValues createMultiConstraintValues(); * -x11-x21-x31 * with x11 clamped at (1,1), and others related by 2D odometry. */ -boost::tuple planarGraph(size_t N); +// inline boost::tuple planarGraph(size_t N); /* * Create canonical ordering for planar graph that also works for tree * With x11 the root, e.g. for N=3 * x33 x23 x13 x32 x22 x12 x31 x21 x11 */ -Ordering planarOrdering(size_t N); +// inline Ordering planarOrdering(size_t N); /* * Split graph into tree and loop closing constraints, e.g., with N=3 @@ -148,8 +148,8 @@ Ordering planarOrdering(size_t N); * | * -x11-x21-x31 */ -std::pair splitOffPlanarTree( - size_t N, const GaussianFactorGraph& original); +// inline std::pair splitOffPlanarTree( +// size_t N, const GaussianFactorGraph& original); @@ -165,19 +165,19 @@ static SharedDiagonal sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1); static SharedDiagonal sigma0_2 = noiseModel::Isotropic::Sigma(2,0.2); static SharedDiagonal constraintModel = noiseModel::Constrained::All(2); -static const Index _l1_=0, _x1_=1, _x2_=2; -static const Index _x_=0, _y_=1, _z_=2; +static const Key _l1_=0, _x1_=1, _x2_=2; +static const Key _x_=0, _y_=1, _z_=2; } // \namespace impl /* ************************************************************************* */ -boost::shared_ptr sharedNonlinearFactorGraph() { +inline boost::shared_ptr sharedNonlinearFactorGraph() { using namespace impl; using symbol_shorthand::X; using symbol_shorthand::L; // Create - boost::shared_ptr nlfg( - new Graph); + boost::shared_ptr nlfg( + new NonlinearFactorGraph); // prior on x1 Point2 mu; @@ -203,12 +203,12 @@ boost::shared_ptr sharedNonlinearFactorGraph() { } /* ************************************************************************* */ -Graph createNonlinearFactorGraph() { +inline NonlinearFactorGraph createNonlinearFactorGraph() { return *sharedNonlinearFactorGraph(); } /* ************************************************************************* */ -Values createValues() { +inline Values createValues() { using symbol_shorthand::X; using symbol_shorthand::L; Values c; @@ -219,17 +219,17 @@ Values createValues() { } /* ************************************************************************* */ -VectorValues createVectorValues() { +inline VectorValues createVectorValues() { using namespace impl; - VectorValues c(std::vector(3, 2)); - c[_l1_] = Vector_(2, 0.0, -1.0); - c[_x1_] = Vector_(2, 0.0, 0.0); - c[_x2_] = Vector_(2, 1.5, 0.0); + VectorValues c = boost::assign::pair_list_of + (_l1_, (Vector(2) << 0.0, -1.0)) + (_x1_, (Vector(2) << 0.0, 0.0)) + (_x2_, (Vector(2) << 1.5, 0.0)); return c; } /* ************************************************************************* */ -boost::shared_ptr sharedNoisyValues() { +inline boost::shared_ptr sharedNoisyValues() { using symbol_shorthand::X; using symbol_shorthand::L; boost::shared_ptr c(new Values); @@ -240,52 +240,50 @@ boost::shared_ptr sharedNoisyValues() { } /* ************************************************************************* */ -Values createNoisyValues() { +inline Values createNoisyValues() { return *sharedNoisyValues(); } /* ************************************************************************* */ -VectorValues createCorrectDelta(const Ordering& ordering) { +inline VectorValues createCorrectDelta() { using symbol_shorthand::X; using symbol_shorthand::L; - VectorValues c(std::vector(3,2)); - c[ordering[L(1)]] = Vector_(2, -0.1, 0.1); - c[ordering[X(1)]] = Vector_(2, -0.1, -0.1); - c[ordering[X(2)]] = Vector_(2, 0.1, -0.2); + VectorValues c; + c.insert(L(1), (Vector(2) << -0.1, 0.1)); + c.insert(X(1), (Vector(2) << -0.1, -0.1)); + c.insert(X(2), (Vector(2) << 0.1, -0.2)); return c; } /* ************************************************************************* */ -VectorValues createZeroDelta(const Ordering& ordering) { +inline VectorValues createZeroDelta() { using symbol_shorthand::X; using symbol_shorthand::L; - VectorValues c(std::vector(3,2)); - c[ordering[L(1)]] = zero(2); - c[ordering[X(1)]] = zero(2); - c[ordering[X(2)]] = zero(2); + VectorValues c; + c.insert(L(1), zero(2)); + c.insert(X(1), zero(2)); + c.insert(X(2), zero(2)); return c; } /* ************************************************************************* */ -GaussianFactorGraph createGaussianFactorGraph(const Ordering& ordering) { +inline GaussianFactorGraph createGaussianFactorGraph() { using symbol_shorthand::X; using symbol_shorthand::L; // Create empty graph GaussianFactorGraph fg; - SharedDiagonal unit2 = noiseModel::Unit::Create(2); - // linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_] - fg.push_back(boost::make_shared(ordering[X(1)], 10*eye(2), -1.0*ones(2), unit2)); + fg += JacobianFactor(X(1), 10*eye(2), -1.0*ones(2)); // odometry between x1 and x2: x2-x1=[0.2;-0.1] - fg.push_back(boost::make_shared(ordering[X(1)], -10*eye(2),ordering[X(2)], 10*eye(2), Vector_(2, 2.0, -1.0), unit2)); + fg += JacobianFactor(X(1), -10*eye(2), X(2), 10*eye(2), (Vector(2) << 2.0, -1.0)); // measurement between x1 and l1: l1-x1=[0.0;0.2] - fg.push_back(boost::make_shared(ordering[X(1)], -5*eye(2), ordering[L(1)], 5*eye(2), Vector_(2, 0.0, 1.0), unit2)); + fg += JacobianFactor(X(1), -5*eye(2), L(1), 5*eye(2), (Vector(2) << 0.0, 1.0)); // measurement between x2 and l1: l1-x2=[-0.2;0.3] - fg.push_back(boost::make_shared(ordering[X(2)], -5*eye(2), ordering[L(1)], 5*eye(2), Vector_(2, -1.0, 1.5), unit2)); + fg += JacobianFactor(X(2), -5*eye(2), L(1), 5*eye(2), (Vector(2) << -1.0, 1.5)); return fg; } @@ -296,19 +294,17 @@ GaussianFactorGraph createGaussianFactorGraph(const Ordering& ordering) { * 1 1 9 * 1 5 */ -GaussianBayesNet createSmallGaussianBayesNet() { +inline GaussianBayesNet createSmallGaussianBayesNet() { using namespace impl; - Matrix R11 = Matrix_(1, 1, 1.0), S12 = Matrix_(1, 1, 1.0); - Matrix R22 = Matrix_(1, 1, 1.0); + Matrix R11 = (Matrix(1, 1) << 1.0), S12 = (Matrix(1, 1) << 1.0); + Matrix R22 = (Matrix(1, 1) << 1.0); Vector d1(1), d2(1); d1(0) = 9; d2(0) = 5; - Vector tau(1); - tau(0) = 1.0; // define nodes and specify in reverse topological sort (i.e. parents last) - GaussianConditional::shared_ptr Px_y(new GaussianConditional(_x_, d1, R11, _y_, S12, tau)); - GaussianConditional::shared_ptr Py(new GaussianConditional(_y_, d2, R22, tau)); + GaussianConditional::shared_ptr Px_y(new GaussianConditional(_x_, d1, R11, _y_, S12)); + GaussianConditional::shared_ptr Py(new GaussianConditional(_y_, d2, R22)); GaussianBayesNet cbn; cbn.push_back(Px_y); cbn.push_back(Py); @@ -321,12 +317,12 @@ GaussianBayesNet createSmallGaussianBayesNet() { /* ************************************************************************* */ namespace smallOptimize { -Point2 h(const Point2& v) { +inline Point2 h(const Point2& v) { return Point2(cos(v.x()), sin(v.y())); } -Matrix H(const Point2& v) { - return Matrix_(2, 2, +inline Matrix H(const Point2& v) { + return (Matrix(2, 2) << -sin(v.x()), 0.0, 0.0, cos(v.y())); } @@ -349,11 +345,11 @@ struct UnaryFactor: public gtsam::NoiseModelFactor1 { } /* ************************************************************************* */ -boost::shared_ptr sharedReallyNonlinearFactorGraph() { +inline boost::shared_ptr sharedReallyNonlinearFactorGraph() { using symbol_shorthand::X; using symbol_shorthand::L; - boost::shared_ptr fg(new Graph); - Vector z = Vector_(2, 1.0, 0.0); + boost::shared_ptr fg(new NonlinearFactorGraph); + Vector z = (Vector(2) << 1.0, 0.0); double sigma = 0.1; boost::shared_ptr factor( new smallOptimize::UnaryFactor(z, noiseModel::Isotropic::Sigma(2,sigma), X(1))); @@ -361,18 +357,18 @@ boost::shared_ptr sharedReallyNonlinearFactorGraph() { return fg; } -Graph createReallyNonlinearFactorGraph() { +inline NonlinearFactorGraph createReallyNonlinearFactorGraph() { return *sharedReallyNonlinearFactorGraph(); } /* ************************************************************************* */ -std::pair createNonlinearSmoother(int T) { +inline std::pair createNonlinearSmoother(int T) { using namespace impl; using symbol_shorthand::X; using symbol_shorthand::L; // Create - Graph nlfg; + NonlinearFactorGraph nlfg; Values poses; // prior on x1 @@ -400,17 +396,16 @@ std::pair createNonlinearSmoother(int T) { } /* ************************************************************************* */ -std::pair, Ordering> createSmoother(int T, boost::optional ordering) { - Graph nlfg; +inline GaussianFactorGraph createSmoother(int T) { + NonlinearFactorGraph nlfg; Values poses; boost::tie(nlfg, poses) = createNonlinearSmoother(T); - if(!ordering) ordering = *poses.orderingArbitrary(); - return std::make_pair(*nlfg.linearize(poses, *ordering), *ordering); + return *nlfg.linearize(poses); } /* ************************************************************************* */ -GaussianFactorGraph createSimpleConstraintGraph() { +inline GaussianFactorGraph createSimpleConstraintGraph() { using namespace impl; // create unary factor // prior on _x_, mean = [1,-1], sigma=0.1 @@ -426,7 +421,7 @@ GaussianFactorGraph createSimpleConstraintGraph() { // |0 1||x_2| | 0 -1||y_2| |0| Matrix Ax1 = eye(2); Matrix Ay1 = eye(2) * -1; - Vector b2 = Vector_(2, 0.0, 0.0); + Vector b2 = (Vector(2) << 0.0, 0.0); JacobianFactor::shared_ptr f2(new JacobianFactor(_x_, Ax1, _y_, Ay1, b2, constraintModel)); @@ -439,19 +434,19 @@ GaussianFactorGraph createSimpleConstraintGraph() { } /* ************************************************************************* */ -VectorValues createSimpleConstraintValues() { +inline VectorValues createSimpleConstraintValues() { using namespace impl; using symbol_shorthand::X; using symbol_shorthand::L; - VectorValues config(std::vector(2,2)); - Vector v = Vector_(2, 1.0, -1.0); - config[_x_] = v; - config[_y_] = v; + VectorValues config; + Vector v = (Vector(2) << 1.0, -1.0); + config.insert(_x_, v); + config.insert(_y_, v); return config; } /* ************************************************************************* */ -GaussianFactorGraph createSingleConstraintGraph() { +inline GaussianFactorGraph createSingleConstraintGraph() { using namespace impl; // create unary factor // prior on _x_, mean = [1,-1], sigma=0.1 @@ -472,7 +467,7 @@ GaussianFactorGraph createSingleConstraintGraph() { Ax1(1, 0) = 2.0; Ax1(1, 1) = 1.0; Matrix Ay1 = eye(2) * 10; - Vector b2 = Vector_(2, 1.0, 2.0); + Vector b2 = (Vector(2) << 1.0, 2.0); JacobianFactor::shared_ptr f2(new JacobianFactor(_x_, Ax1, _y_, Ay1, b2, constraintModel)); @@ -485,20 +480,20 @@ GaussianFactorGraph createSingleConstraintGraph() { } /* ************************************************************************* */ -VectorValues createSingleConstraintValues() { +inline VectorValues createSingleConstraintValues() { using namespace impl; - VectorValues config(std::vector(2,2)); - config[_x_] = Vector_(2, 1.0, -1.0); - config[_y_] = Vector_(2, 0.2, 0.1); + VectorValues config = boost::assign::pair_list_of + (_x_, (Vector(2) << 1.0, -1.0)) + (_y_, (Vector(2) << 0.2, 0.1)); return config; } /* ************************************************************************* */ -GaussianFactorGraph createMultiConstraintGraph() { +inline GaussianFactorGraph createMultiConstraintGraph() { using namespace impl; // unary factor 1 Matrix A = eye(2); - Vector b = Vector_(2, -2.0, 2.0); + Vector b = (Vector(2) << -2.0, 2.0); JacobianFactor::shared_ptr lf1(new JacobianFactor(_x_, A, b, sigma0_1)); // constraint 1 @@ -549,26 +544,26 @@ GaussianFactorGraph createMultiConstraintGraph() { } /* ************************************************************************* */ -VectorValues createMultiConstraintValues() { +inline VectorValues createMultiConstraintValues() { using namespace impl; - VectorValues config(std::vector(3,2)); - config[_x_] = Vector_(2, -2.0, 2.0); - config[_y_] = Vector_(2, -0.1, 0.4); - config[_z_] = Vector_(2, -4.0, 5.0); + VectorValues config = boost::assign::pair_list_of + (_x_, (Vector(2) << -2.0, 2.0)) + (_y_, (Vector(2) << -0.1, 0.4)) + (_z_, (Vector(2) <<-4.0, 5.0)); return config; } /* ************************************************************************* */ // Create key for simulated planar graph namespace impl { -Symbol key(int x, int y) { +inline Symbol key(size_t x, size_t y) { using symbol_shorthand::X; return X(1000*x+y); } } // \namespace impl /* ************************************************************************* */ -boost::tuple planarGraph(size_t N) { +inline boost::tuple planarGraph(size_t N) { using namespace impl; // create empty graph @@ -599,19 +594,18 @@ boost::tuple planarGraph(size_t N) { for (size_t x = 1; x <= N; x++) for (size_t y = 1; y <= N; y++) zeros.insert(key(x, y), Point2()); - Ordering ordering(planarOrdering(N)); - VectorValues xtrue(zeros.dims(ordering)); + VectorValues xtrue; for (size_t x = 1; x <= N; x++) for (size_t y = 1; y <= N; y++) - xtrue[ordering[key(x, y)]] = Point2(x,y).vector(); + xtrue.insert(key(x, y), Point2((double)x, (double)y).vector()); // linearize around zero - boost::shared_ptr gfg = nlfg.linearize(zeros, ordering); + boost::shared_ptr gfg = nlfg.linearize(zeros); return boost::make_tuple(*gfg, xtrue); } /* ************************************************************************* */ -Ordering planarOrdering(size_t N) { +inline Ordering planarOrdering(size_t N) { Ordering ordering; for (size_t y = N; y >= 1; y--) for (size_t x = N; x >= 1; x--) @@ -620,7 +614,7 @@ Ordering planarOrdering(size_t N) { } /* ************************************************************************* */ -std::pair splitOffPlanarTree(size_t N, +inline std::pair splitOffPlanarTree(size_t N, const GaussianFactorGraph& original) { GaussianFactorGraph T, C; @@ -646,6 +640,5 @@ std::pair splitOffPlanarTree(size_t N /* ************************************************************************* */ -} // anonymous namespace } // \namespace example } // \namespace gtsam diff --git a/tests/testBoundingConstraint.cpp b/tests/testBoundingConstraint.cpp index 516b7e070..7b5f31660 100644 --- a/tests/testBoundingConstraint.cpp +++ b/tests/testBoundingConstraint.cpp @@ -16,7 +16,7 @@ */ #include -#include +#include #include #include @@ -116,10 +116,8 @@ TEST( testBoundingConstraint, unary_linearization_inactive) { Point2 pt1(2.0, 3.0); Values config1; config1.insert(key, pt1); - Ordering ordering; - ordering += key; - GaussianFactor::shared_ptr actual1 = constraint1.linearize(config1, ordering); - GaussianFactor::shared_ptr actual2 = constraint2.linearize(config1, ordering); + GaussianFactor::shared_ptr actual1 = constraint1.linearize(config1); + GaussianFactor::shared_ptr actual2 = constraint2.linearize(config1); EXPECT(!actual1); EXPECT(!actual2); } @@ -129,12 +127,10 @@ TEST( testBoundingConstraint, unary_linearization_active) { Point2 pt2(-2.0, -3.0); Values config2; config2.insert(key, pt2); - Ordering ordering; - ordering += key; - GaussianFactor::shared_ptr actual1 = constraint1.linearize(config2, ordering); - GaussianFactor::shared_ptr actual2 = constraint2.linearize(config2, ordering); - JacobianFactor expected1(ordering[key], Matrix_(1, 2, 1.0, 0.0), repeat(1, 3.0), hard_model1); - JacobianFactor expected2(ordering[key], Matrix_(1, 2, 0.0, 1.0), repeat(1, 5.0), hard_model1); + GaussianFactor::shared_ptr actual1 = constraint1.linearize(config2); + GaussianFactor::shared_ptr actual2 = constraint2.linearize(config2); + JacobianFactor expected1(key, (Matrix(1, 2) << 1.0, 0.0), repeat(1, 3.0), hard_model1); + JacobianFactor expected2(key, (Matrix(1, 2) << 0.0, 1.0), repeat(1, 5.0), hard_model1); EXPECT(assert_equal((const GaussianFactor&)expected1, *actual1, tol)); EXPECT(assert_equal((const GaussianFactor&)expected2, *actual2, tol)); } @@ -148,9 +144,9 @@ TEST( testBoundingConstraint, unary_simple_optimization1) { NonlinearFactorGraph graph; Symbol x1('x',1); - graph.add(iq2D::PoseXInequality(x1, 1.0, true)); - graph.add(iq2D::PoseYInequality(x1, 2.0, true)); - graph.add(simulated2D::Prior(start_pt, soft_model2, x1)); + graph += iq2D::PoseXInequality(x1, 1.0, true); + graph += iq2D::PoseYInequality(x1, 2.0, true); + graph += simulated2D::Prior(start_pt, soft_model2, x1); Values initValues; initValues.insert(x1, start_pt); @@ -169,9 +165,9 @@ TEST( testBoundingConstraint, unary_simple_optimization2) { Point2 start_pt(2.0, 3.0); NonlinearFactorGraph graph; - graph.add(iq2D::PoseXInequality(key, 1.0, false)); - graph.add(iq2D::PoseYInequality(key, 2.0, false)); - graph.add(simulated2D::Prior(start_pt, soft_model2, key)); + graph += iq2D::PoseXInequality(key, 1.0, false); + graph += iq2D::PoseYInequality(key, 2.0, false); + graph += simulated2D::Prior(start_pt, soft_model2, key); Values initValues; initValues.insert(key, start_pt); @@ -191,7 +187,7 @@ TEST( testBoundingConstraint, MaxDistance_basics) { EXPECT(!rangeBound.isGreaterThan()); EXPECT(rangeBound.dim() == 1); - EXPECT(assert_equal(Vector_(1, 2.0), rangeBound.evaluateError(pt1, pt1))); + EXPECT(assert_equal(((Vector)Vector(1) << 2.0), rangeBound.evaluateError(pt1, pt1))); EXPECT(assert_equal(ones(1), rangeBound.evaluateError(pt1, pt2))); EXPECT(assert_equal(zero(1), rangeBound.evaluateError(pt1, pt3))); EXPECT(assert_equal(-1.0*ones(1), rangeBound.evaluateError(pt1, pt4))); @@ -199,16 +195,15 @@ TEST( testBoundingConstraint, MaxDistance_basics) { Values config1; config1.insert(key1, pt1); config1.insert(key2, pt1); - Ordering ordering; ordering += key1, key2; EXPECT(!rangeBound.active(config1)); EXPECT(assert_equal(zero(1), rangeBound.unwhitenedError(config1))); - EXPECT(!rangeBound.linearize(config1, ordering)); + EXPECT(!rangeBound.linearize(config1)); EXPECT_DOUBLES_EQUAL(0.0, rangeBound.error(config1), tol); config1.update(key2, pt2); EXPECT(!rangeBound.active(config1)); EXPECT(assert_equal(zero(1), rangeBound.unwhitenedError(config1))); - EXPECT(!rangeBound.linearize(config1, ordering)); + EXPECT(!rangeBound.linearize(config1)); EXPECT_DOUBLES_EQUAL(0.0, rangeBound.error(config1), tol); config1.update(key2, pt3); @@ -229,9 +224,9 @@ TEST( testBoundingConstraint, MaxDistance_simple_optimization) { Symbol x1('x',1), x2('x',2); NonlinearFactorGraph graph; - graph.add(simulated2D::equality_constraints::UnaryEqualityConstraint(pt1, x1)); - graph.add(simulated2D::Prior(pt2_init, soft_model2_alt, x2)); - graph.add(iq2D::PoseMaxDistConstraint(x1, x2, 2.0)); + graph += simulated2D::equality_constraints::UnaryEqualityConstraint(pt1, x1); + graph += simulated2D::Prior(pt2_init, soft_model2_alt, x2); + graph += iq2D::PoseMaxDistConstraint(x1, x2, 2.0); Values initial_state; initial_state.insert(x1, pt1); @@ -255,12 +250,12 @@ TEST( testBoundingConstraint, avoid_demo) { Point2 odo(2.0, 0.0); NonlinearFactorGraph graph; - graph.add(simulated2D::equality_constraints::UnaryEqualityConstraint(x1_pt, x1)); - graph.add(simulated2D::Odometry(odo, soft_model2_alt, x1, x2)); - graph.add(iq2D::LandmarkAvoid(x2, l1, radius)); - graph.add(simulated2D::equality_constraints::UnaryEqualityPointConstraint(l1_pt, l1)); - graph.add(simulated2D::Odometry(odo, soft_model2_alt, x2, x3)); - graph.add(simulated2D::equality_constraints::UnaryEqualityConstraint(x3_pt, x3)); + graph += simulated2D::equality_constraints::UnaryEqualityConstraint(x1_pt, x1); + graph += simulated2D::Odometry(odo, soft_model2_alt, x1, x2); + graph += iq2D::LandmarkAvoid(x2, l1, radius); + graph += simulated2D::equality_constraints::UnaryEqualityPointConstraint(l1_pt, l1); + graph += simulated2D::Odometry(odo, soft_model2_alt, x2, x3); + graph += simulated2D::equality_constraints::UnaryEqualityConstraint(x3_pt, x3); Values init, expected; init.insert(x1, x1_pt); diff --git a/tests/testDoglegOptimizer.cpp b/tests/testDoglegOptimizer.cpp index 008ea443a..c4bf0480c 100644 --- a/tests/testDoglegOptimizer.cpp +++ b/tests/testDoglegOptimizer.cpp @@ -15,17 +15,15 @@ * @author Richard Roberts */ +#include + #include #include -#include +#include #include -#include #include -#include #include -#include - #ifdef __GNUC__ #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wunused-variable" @@ -45,316 +43,40 @@ using namespace gtsam; using symbol_shorthand::X; using symbol_shorthand::L; -/* ************************************************************************* */ -double computeError(const GaussianBayesNet& gbn, const LieVector& values) { - - // Convert Vector to VectorValues - VectorValues vv = *allocateVectorValues(gbn); - internal::writeVectorValuesSlices(values, vv, - boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(vv.size())); - - // Convert to factor graph - GaussianFactorGraph gfg(gbn); - return gfg.error(vv); -} - -/* ************************************************************************* */ -double computeErrorBt(const BayesTree& gbt, const LieVector& values) { - - // Convert Vector to VectorValues - VectorValues vv = *allocateVectorValues(gbt); - internal::writeVectorValuesSlices(values, vv, - boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(vv.size())); - - // Convert to factor graph - GaussianFactorGraph gfg(gbt); - return gfg.error(vv); -} - -/* ************************************************************************* */ -TEST(DoglegOptimizer, ComputeSteepestDescentPoint) { - - // Create an arbitrary Bayes Net - GaussianBayesNet gbn; - gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 0, Vector_(2, 1.0,2.0), Matrix_(2,2, 3.0,4.0,0.0,6.0), - 3, Matrix_(2,2, 7.0,8.0,9.0,10.0), - 4, Matrix_(2,2, 11.0,12.0,13.0,14.0), ones(2))); - gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 1, Vector_(2, 15.0,16.0), Matrix_(2,2, 17.0,18.0,0.0,20.0), - 2, Matrix_(2,2, 21.0,22.0,23.0,24.0), - 4, Matrix_(2,2, 25.0,26.0,27.0,28.0), ones(2))); - gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 2, Vector_(2, 29.0,30.0), Matrix_(2,2, 31.0,32.0,0.0,34.0), - 3, Matrix_(2,2, 35.0,36.0,37.0,38.0), ones(2))); - gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 3, Vector_(2, 39.0,40.0), Matrix_(2,2, 41.0,42.0,0.0,44.0), - 4, Matrix_(2,2, 45.0,46.0,47.0,48.0), ones(2))); - gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 4, Vector_(2, 49.0,50.0), Matrix_(2,2, 51.0,52.0,0.0,54.0), ones(2))); - - // Compute the Hessian numerically - Matrix hessian = numericalHessian( - boost::function(boost::bind(&computeError, gbn, _1)), - LieVector(VectorValues::Zero(*allocateVectorValues(gbn)).asVector())); - - // Compute the gradient numerically - VectorValues gradientValues = *allocateVectorValues(gbn); - Vector gradient = numericalGradient( - boost::function(boost::bind(&computeError, gbn, _1)), - LieVector(VectorValues::Zero(gradientValues).asVector())); - internal::writeVectorValuesSlices(gradient, gradientValues, - boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(gradientValues.size())); - - // Compute the gradient using dense matrices - Matrix augmentedHessian = GaussianFactorGraph(gbn).augmentedHessian(); - LONGS_EQUAL(11, augmentedHessian.cols()); - VectorValues denseMatrixGradient = *allocateVectorValues(gbn); - internal::writeVectorValuesSlices(-augmentedHessian.col(10).segment(0,10), denseMatrixGradient, - boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(gradientValues.size())); - EXPECT(assert_equal(gradientValues, denseMatrixGradient, 1e-5)); - - // Compute the steepest descent point - double step = -gradient.squaredNorm() / (gradient.transpose() * hessian * gradient)(0); - VectorValues expected = gradientValues; scal(step, expected); - - // Compute the steepest descent point with the dogleg function - VectorValues actual = optimizeGradientSearch(gbn); - - // Check that points agree - EXPECT(assert_equal(expected, actual, 1e-5)); - - // Check that point causes a decrease in error - double origError = GaussianFactorGraph(gbn).error(VectorValues::Zero(*allocateVectorValues(gbn))); - double newError = GaussianFactorGraph(gbn).error(actual); - EXPECT(newError < origError); -} - -/* ************************************************************************* */ -TEST(DoglegOptimizer, BT_BN_equivalency) { - - // Create an arbitrary Bayes Tree - BayesTree bt; - bt.insert(BayesTree::sharedClique(new BayesTree::Clique( - GaussianConditional::shared_ptr(new GaussianConditional( - boost::assign::pair_list_of - (2, Matrix_(6,2, - 31.0,32.0, - 0.0,34.0, - 0.0,0.0, - 0.0,0.0, - 0.0,0.0, - 0.0,0.0)) - (3, Matrix_(6,2, - 35.0,36.0, - 37.0,38.0, - 41.0,42.0, - 0.0,44.0, - 0.0,0.0, - 0.0,0.0)) - (4, Matrix_(6,2, - 0.0,0.0, - 0.0,0.0, - 45.0,46.0, - 47.0,48.0, - 51.0,52.0, - 0.0,54.0)), - 3, Vector_(6, 29.0,30.0,39.0,40.0,49.0,50.0), ones(6)))))); - bt.insert(BayesTree::sharedClique(new BayesTree::Clique( - GaussianConditional::shared_ptr(new GaussianConditional( - boost::assign::pair_list_of - (0, Matrix_(4,2, - 3.0,4.0, - 0.0,6.0, - 0.0,0.0, - 0.0,0.0)) - (1, Matrix_(4,2, - 0.0,0.0, - 0.0,0.0, - 17.0,18.0, - 0.0,20.0)) - (2, Matrix_(4,2, - 0.0,0.0, - 0.0,0.0, - 21.0,22.0, - 23.0,24.0)) - (3, Matrix_(4,2, - 7.0,8.0, - 9.0,10.0, - 0.0,0.0, - 0.0,0.0)) - (4, Matrix_(4,2, - 11.0,12.0, - 13.0,14.0, - 25.0,26.0, - 27.0,28.0)), - 2, Vector_(4, 1.0,2.0,15.0,16.0), ones(4)))))); - - // Create an arbitrary Bayes Net - GaussianBayesNet gbn; - gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 0, Vector_(2, 1.0,2.0), Matrix_(2,2, 3.0,4.0,0.0,6.0), - 3, Matrix_(2,2, 7.0,8.0,9.0,10.0), - 4, Matrix_(2,2, 11.0,12.0,13.0,14.0), ones(2))); - gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 1, Vector_(2, 15.0,16.0), Matrix_(2,2, 17.0,18.0,0.0,20.0), - 2, Matrix_(2,2, 21.0,22.0,23.0,24.0), - 4, Matrix_(2,2, 25.0,26.0,27.0,28.0), ones(2))); - gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 2, Vector_(2, 29.0,30.0), Matrix_(2,2, 31.0,32.0,0.0,34.0), - 3, Matrix_(2,2, 35.0,36.0,37.0,38.0), ones(2))); - gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 3, Vector_(2, 39.0,40.0), Matrix_(2,2, 41.0,42.0,0.0,44.0), - 4, Matrix_(2,2, 45.0,46.0,47.0,48.0), ones(2))); - gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 4, Vector_(2, 49.0,50.0), Matrix_(2,2, 51.0,52.0,0.0,54.0), ones(2))); - - GaussianFactorGraph expected(gbn); - GaussianFactorGraph actual(bt); - - EXPECT(assert_equal(expected.augmentedHessian(), actual.augmentedHessian())); -} - -/* ************************************************************************* */ -TEST(DoglegOptimizer, ComputeSteepestDescentPointBT) { - - // Create an arbitrary Bayes Tree - BayesTree bt; - bt.insert(BayesTree::sharedClique(new BayesTree::Clique( - GaussianConditional::shared_ptr(new GaussianConditional( - boost::assign::pair_list_of - (2, Matrix_(6,2, - 31.0,32.0, - 0.0,34.0, - 0.0,0.0, - 0.0,0.0, - 0.0,0.0, - 0.0,0.0)) - (3, Matrix_(6,2, - 35.0,36.0, - 37.0,38.0, - 41.0,42.0, - 0.0,44.0, - 0.0,0.0, - 0.0,0.0)) - (4, Matrix_(6,2, - 0.0,0.0, - 0.0,0.0, - 45.0,46.0, - 47.0,48.0, - 51.0,52.0, - 0.0,54.0)), - 3, Vector_(6, 29.0,30.0,39.0,40.0,49.0,50.0), ones(6)))))); - bt.insert(BayesTree::sharedClique(new BayesTree::Clique( - GaussianConditional::shared_ptr(new GaussianConditional( - boost::assign::pair_list_of - (0, Matrix_(4,2, - 3.0,4.0, - 0.0,6.0, - 0.0,0.0, - 0.0,0.0)) - (1, Matrix_(4,2, - 0.0,0.0, - 0.0,0.0, - 17.0,18.0, - 0.0,20.0)) - (2, Matrix_(4,2, - 0.0,0.0, - 0.0,0.0, - 21.0,22.0, - 23.0,24.0)) - (3, Matrix_(4,2, - 7.0,8.0, - 9.0,10.0, - 0.0,0.0, - 0.0,0.0)) - (4, Matrix_(4,2, - 11.0,12.0, - 13.0,14.0, - 25.0,26.0, - 27.0,28.0)), - 2, Vector_(4, 1.0,2.0,15.0,16.0), ones(4)))))); - - // Compute the Hessian numerically - Matrix hessian = numericalHessian( - boost::function(boost::bind(&computeErrorBt, bt, _1)), - LieVector(VectorValues::Zero(*allocateVectorValues(bt)).asVector())); - - // Compute the gradient numerically - VectorValues gradientValues = *allocateVectorValues(bt); - Vector gradient = numericalGradient( - boost::function(boost::bind(&computeErrorBt, bt, _1)), - LieVector(VectorValues::Zero(gradientValues).asVector())); - internal::writeVectorValuesSlices(gradient, gradientValues, - boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(gradientValues.size())); - - // Compute the gradient using dense matrices - Matrix augmentedHessian = GaussianFactorGraph(bt).augmentedHessian(); - LONGS_EQUAL(11, augmentedHessian.cols()); - VectorValues denseMatrixGradient = *allocateVectorValues(bt); - internal::writeVectorValuesSlices(-augmentedHessian.col(10).segment(0,10), denseMatrixGradient, - boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(gradientValues.size())); - EXPECT(assert_equal(gradientValues, denseMatrixGradient, 1e-5)); - - // Compute the steepest descent point - double step = -gradient.squaredNorm() / (gradient.transpose() * hessian * gradient)(0); - VectorValues expected = gradientValues; scal(step, expected); - - // Known steepest descent point from Bayes' net version - VectorValues expectedFromBN(5,2); - expectedFromBN[0] = Vector_(2, 0.000129034, 0.000688183); - expectedFromBN[1] = Vector_(2, 0.0109679, 0.0253767); - expectedFromBN[2] = Vector_(2, 0.0680441, 0.114496); - expectedFromBN[3] = Vector_(2, 0.16125, 0.241294); - expectedFromBN[4] = Vector_(2, 0.300134, 0.423233); - - // Compute the steepest descent point with the dogleg function - VectorValues actual = optimizeGradientSearch(bt); - - // Check that points agree - EXPECT(assert_equal(expected, actual, 1e-5)); - EXPECT(assert_equal(expectedFromBN, actual, 1e-5)); - - // Check that point causes a decrease in error - double origError = GaussianFactorGraph(bt).error(VectorValues::Zero(*allocateVectorValues(bt))); - double newError = GaussianFactorGraph(bt).error(actual); - EXPECT(newError < origError); -} - /* ************************************************************************* */ TEST(DoglegOptimizer, ComputeBlend) { // Create an arbitrary Bayes Net GaussianBayesNet gbn; gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 0, Vector_(2, 1.0,2.0), Matrix_(2,2, 3.0,4.0,0.0,6.0), - 3, Matrix_(2,2, 7.0,8.0,9.0,10.0), - 4, Matrix_(2,2, 11.0,12.0,13.0,14.0), ones(2))); + 0, (Vector(2) << 1.0,2.0), (Matrix(2, 2) << 3.0,4.0,0.0,6.0), + 3, (Matrix(2, 2) << 7.0,8.0,9.0,10.0), + 4, (Matrix(2, 2) << 11.0,12.0,13.0,14.0))); gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 1, Vector_(2, 15.0,16.0), Matrix_(2,2, 17.0,18.0,0.0,20.0), - 2, Matrix_(2,2, 21.0,22.0,23.0,24.0), - 4, Matrix_(2,2, 25.0,26.0,27.0,28.0), ones(2))); + 1, (Vector(2) << 15.0,16.0), (Matrix(2, 2) << 17.0,18.0,0.0,20.0), + 2, (Matrix(2, 2) << 21.0,22.0,23.0,24.0), + 4, (Matrix(2, 2) << 25.0,26.0,27.0,28.0))); gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 2, Vector_(2, 29.0,30.0), Matrix_(2,2, 31.0,32.0,0.0,34.0), - 3, Matrix_(2,2, 35.0,36.0,37.0,38.0), ones(2))); + 2, (Vector(2) << 29.0,30.0), (Matrix(2, 2) << 31.0,32.0,0.0,34.0), + 3, (Matrix(2, 2) << 35.0,36.0,37.0,38.0))); gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 3, Vector_(2, 39.0,40.0), Matrix_(2,2, 41.0,42.0,0.0,44.0), - 4, Matrix_(2,2, 45.0,46.0,47.0,48.0), ones(2))); + 3, (Vector(2) << 39.0,40.0), (Matrix(2, 2) << 41.0,42.0,0.0,44.0), + 4, (Matrix(2, 2) << 45.0,46.0,47.0,48.0))); gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 4, Vector_(2, 49.0,50.0), Matrix_(2,2, 51.0,52.0,0.0,54.0), ones(2))); + 4, (Vector(2) << 49.0,50.0), (Matrix(2, 2) << 51.0,52.0,0.0,54.0))); // Compute steepest descent point - VectorValues xu = optimizeGradientSearch(gbn); + VectorValues xu = gbn.optimizeGradientSearch(); // Compute Newton's method point - VectorValues xn = optimize(gbn); + VectorValues xn = gbn.optimize(); // The Newton's method point should be more "adventurous", i.e. larger, than the steepest descent point - EXPECT(xu.asVector().norm() < xn.asVector().norm()); + EXPECT(xu.vector().norm() < xn.vector().norm()); // Compute blend double Delta = 1.5; VectorValues xb = DoglegOptimizerImpl::ComputeBlend(Delta, xu, xn); - DOUBLES_EQUAL(Delta, xb.asVector().norm(), 1e-10); + DOUBLES_EQUAL(Delta, xb.vector().norm(), 1e-10); } /* ************************************************************************* */ @@ -362,70 +84,66 @@ TEST(DoglegOptimizer, ComputeDoglegPoint) { // Create an arbitrary Bayes Net GaussianBayesNet gbn; gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 0, Vector_(2, 1.0,2.0), Matrix_(2,2, 3.0,4.0,0.0,6.0), - 3, Matrix_(2,2, 7.0,8.0,9.0,10.0), - 4, Matrix_(2,2, 11.0,12.0,13.0,14.0), ones(2))); + 0, (Vector(2) << 1.0,2.0), (Matrix(2, 2) << 3.0,4.0,0.0,6.0), + 3, (Matrix(2, 2) << 7.0,8.0,9.0,10.0), + 4, (Matrix(2, 2) << 11.0,12.0,13.0,14.0))); gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 1, Vector_(2, 15.0,16.0), Matrix_(2,2, 17.0,18.0,0.0,20.0), - 2, Matrix_(2,2, 21.0,22.0,23.0,24.0), - 4, Matrix_(2,2, 25.0,26.0,27.0,28.0), ones(2))); + 1, (Vector(2) << 15.0,16.0), (Matrix(2, 2) << 17.0,18.0,0.0,20.0), + 2, (Matrix(2, 2) << 21.0,22.0,23.0,24.0), + 4, (Matrix(2, 2) << 25.0,26.0,27.0,28.0))); gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 2, Vector_(2, 29.0,30.0), Matrix_(2,2, 31.0,32.0,0.0,34.0), - 3, Matrix_(2,2, 35.0,36.0,37.0,38.0), ones(2))); + 2, (Vector(2) << 29.0,30.0), (Matrix(2, 2) << 31.0,32.0,0.0,34.0), + 3, (Matrix(2, 2) << 35.0,36.0,37.0,38.0))); gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 3, Vector_(2, 39.0,40.0), Matrix_(2,2, 41.0,42.0,0.0,44.0), - 4, Matrix_(2,2, 45.0,46.0,47.0,48.0), ones(2))); + 3, (Vector(2) << 39.0,40.0), (Matrix(2, 2) << 41.0,42.0,0.0,44.0), + 4, (Matrix(2, 2) << 45.0,46.0,47.0,48.0))); gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 4, Vector_(2, 49.0,50.0), Matrix_(2,2, 51.0,52.0,0.0,54.0), ones(2))); + 4, (Vector(2) << 49.0,50.0), (Matrix(2, 2) << 51.0,52.0,0.0,54.0))); // Compute dogleg point for different deltas double Delta1 = 0.5; // Less than steepest descent - VectorValues actual1 = DoglegOptimizerImpl::ComputeDoglegPoint(Delta1, optimizeGradientSearch(gbn), optimize(gbn)); - DOUBLES_EQUAL(Delta1, actual1.asVector().norm(), 1e-5); + VectorValues actual1 = DoglegOptimizerImpl::ComputeDoglegPoint(Delta1, gbn.optimizeGradientSearch(), gbn.optimize()); + DOUBLES_EQUAL(Delta1, actual1.vector().norm(), 1e-5); double Delta2 = 1.5; // Between steepest descent and Newton's method - VectorValues expected2 = DoglegOptimizerImpl::ComputeBlend(Delta2, optimizeGradientSearch(gbn), optimize(gbn)); - VectorValues actual2 = DoglegOptimizerImpl::ComputeDoglegPoint(Delta2, optimizeGradientSearch(gbn), optimize(gbn)); - DOUBLES_EQUAL(Delta2, actual2.asVector().norm(), 1e-5); + VectorValues expected2 = DoglegOptimizerImpl::ComputeBlend(Delta2, gbn.optimizeGradientSearch(), gbn.optimize()); + VectorValues actual2 = DoglegOptimizerImpl::ComputeDoglegPoint(Delta2, gbn.optimizeGradientSearch(), gbn.optimize()); + DOUBLES_EQUAL(Delta2, actual2.vector().norm(), 1e-5); EXPECT(assert_equal(expected2, actual2)); double Delta3 = 5.0; // Larger than Newton's method point - VectorValues expected3 = optimize(gbn); - VectorValues actual3 = DoglegOptimizerImpl::ComputeDoglegPoint(Delta3, optimizeGradientSearch(gbn), optimize(gbn)); + VectorValues expected3 = gbn.optimize(); + VectorValues actual3 = DoglegOptimizerImpl::ComputeDoglegPoint(Delta3, gbn.optimizeGradientSearch(), gbn.optimize()); EXPECT(assert_equal(expected3, actual3)); } /* ************************************************************************* */ TEST(DoglegOptimizer, Iterate) { // really non-linear factor graph - boost::shared_ptr fg(new example::Graph( - example::createReallyNonlinearFactorGraph())); + NonlinearFactorGraph fg = example::createReallyNonlinearFactorGraph(); // config far from minimum Point2 x0(3,0); - boost::shared_ptr config(new Values); - config->insert(X(1), x0); - - // ordering - boost::shared_ptr ord(new Ordering()); - ord->push_back(X(1)); + Values config; + config.insert(X(1), x0); double Delta = 1.0; for(size_t it=0; it<10; ++it) { - GaussianSequentialSolver solver(*fg->linearize(*config, *ord)); - GaussianBayesNet gbn = *solver.eliminate(); + GaussianBayesNet gbn = *fg.linearize(config)->eliminateSequential(); // Iterate assumes that linear error = nonlinear error at the linearization point, and this should be true - double nonlinearError = fg->error(*config); - double linearError = GaussianFactorGraph(gbn).error(VectorValues::Zero(*allocateVectorValues(gbn))); + double nonlinearError = fg.error(config); + double linearError = GaussianFactorGraph(gbn).error(config.zeroVectors()); DOUBLES_EQUAL(nonlinearError, linearError, 1e-5); // cout << "it " << it << ", Delta = " << Delta << ", error = " << fg->error(*config) << endl; - DoglegOptimizerImpl::IterationResult result = DoglegOptimizerImpl::Iterate(Delta, DoglegOptimizerImpl::SEARCH_EACH_ITERATION, gbn, *fg, *config, *ord, fg->error(*config)); + VectorValues dx_u = gbn.optimizeGradientSearch(); + VectorValues dx_n = gbn.optimize(); + DoglegOptimizerImpl::IterationResult result = DoglegOptimizerImpl::Iterate(Delta, DoglegOptimizerImpl::SEARCH_EACH_ITERATION, dx_u, dx_n, gbn, fg, config, fg.error(config)); Delta = result.Delta; - EXPECT(result.f_error < fg->error(*config)); // Check that error decreases - Values newConfig(config->retract(result.dx_d, *ord)); - (*config) = newConfig; - DOUBLES_EQUAL(fg->error(*config), result.f_error, 1e-5); // Check that error is correctly filled in + EXPECT(result.f_error < fg.error(config)); // Check that error decreases + Values newConfig(config.retract(result.dx_d)); + config = newConfig; + DOUBLES_EQUAL(fg.error(config), result.f_error, 1e-5); // Check that error is correctly filled in } } diff --git a/tests/testExtendedKalmanFilter.cpp b/tests/testExtendedKalmanFilter.cpp index 6bdfa7eee..7b27293df 100644 --- a/tests/testExtendedKalmanFilter.cpp +++ b/tests/testExtendedKalmanFilter.cpp @@ -18,7 +18,7 @@ #include #include #include -#include +#include #include #include @@ -32,25 +32,25 @@ using symbol_shorthand::L; /* ************************************************************************* */ TEST( ExtendedKalmanFilter, linear ) { - // Create the Kalman Filter initialization point - Point2 x_initial(0.0, 0.0); - SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); - - // Create an ExtendedKalmanFilter object - ExtendedKalmanFilter ekf(x_initial, P_initial); - // Create the TestKeys for our example Symbol x0('x',0), x1('x',1), x2('x',2), x3('x',3); + // Create the Kalman Filter initialization point + Point2 x_initial(0.0, 0.0); + SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.1)); + + // Create an ExtendedKalmanFilter object + ExtendedKalmanFilter ekf(x0, x_initial, P_initial); + // Create the controls and measurement properties for our example double dt = 1.0; - Vector u = Vector_(2, 1.0, 0.0); + Vector u = (Vector(2) << 1.0, 0.0); Point2 difference(u*dt); - SharedDiagonal Q = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1), true); + SharedDiagonal Q = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.1), true); Point2 z1(1.0, 0.0); Point2 z2(2.0, 0.0); Point2 z3(3.0, 0.0); - SharedDiagonal R = noiseModel::Diagonal::Sigmas(Vector_(2, 0.25, 0.25), true); + SharedDiagonal R = noiseModel::Diagonal::Sigmas((Vector(2) << 0.25, 0.25), true); // Create the set of expected output TestValues Point2 expected1(1.0, 0.0); @@ -107,7 +107,7 @@ public: NonlinearMotionModel(){} NonlinearMotionModel(const Symbol& TestKey1, const Symbol& TestKey2) : - Base(noiseModel::Diagonal::Sigmas(Vector_(2, 1.0, 1.0)), TestKey1, TestKey2), Q_(2,2) { + Base(noiseModel::Diagonal::Sigmas((Vector(2) << 1.0, 1.0)), TestKey1, TestKey2), Q_(2,2) { // Initialize motion model parameters: // w is vector of motion noise sigmas. The final covariance is calculated as G*w*w'*G' @@ -192,16 +192,15 @@ public: * Ax-b \approx h(x1+dx1,x2+dx2)-z = h(x1,x2) + A2*dx1 + A2*dx2 - z * Hence b = z - h(x1,x2) = - error_vector(x) */ - boost::shared_ptr linearize(const Values& c, const Ordering& ordering) const { + boost::shared_ptr linearize(const Values& c) const { const X1& x1 = c.at(key1()); const X2& x2 = c.at(key2()); Matrix A1, A2; Vector b = -evaluateError(x1, x2, A1, A2); - const Index var1 = ordering[key1()], var2 = ordering[key2()]; SharedDiagonal constrained = boost::dynamic_pointer_cast(this->noiseModel_); if (constrained.get() != NULL) { - return JacobianFactor::shared_ptr(new JacobianFactor(var1, A1, var2, + return JacobianFactor::shared_ptr(new JacobianFactor(key1(), A1, key2(), A2, b, constrained)); } // "Whiten" the system before converting to a Gaussian Factor @@ -209,7 +208,7 @@ public: A1 = Qinvsqrt*A1; A2 = Qinvsqrt*A2; b = Qinvsqrt*b; - return GaussianFactor::shared_ptr(new JacobianFactor(var1, A1, var2, + return GaussianFactor::shared_ptr(new JacobianFactor(key1(), A1, key2(), A2, b, noiseModel::Unit::Create(b.size()))); } @@ -256,7 +255,7 @@ public: NonlinearMeasurementModel(){} NonlinearMeasurementModel(const Symbol& TestKey, Vector z) : - Base(noiseModel::Diagonal::Sigmas(Vector_(2, 1.0, 1.0)), TestKey), z_(z), R_(1,1) { + Base(noiseModel::Unit::Create(z.size()), TestKey), z_(z), R_(1,1) { // Initialize nonlinear measurement model parameters: // z(t) = H*x(t) + v @@ -329,21 +328,20 @@ public: * Ax-b \approx h(x1+dx1)-z = h(x1) + A1*dx1 - z * Hence b = z - h(x1) = - error_vector(x) */ - boost::shared_ptr linearize(const Values& c, const Ordering& ordering) const { + boost::shared_ptr linearize(const Values& c) const { const X& x1 = c.at(key()); Matrix A1; Vector b = -evaluateError(x1, A1); - const Index var1 = ordering[key()]; SharedDiagonal constrained = boost::dynamic_pointer_cast(this->noiseModel_); if (constrained.get() != NULL) { - return JacobianFactor::shared_ptr(new JacobianFactor(var1, A1, b, constrained)); + return JacobianFactor::shared_ptr(new JacobianFactor(key(), A1, b, constrained)); } // "Whiten" the system before converting to a Gaussian Factor Matrix Rinvsqrt = RInvSqrt(x1); A1 = Rinvsqrt*A1; b = Rinvsqrt*b; - return GaussianFactor::shared_ptr(new JacobianFactor(var1, A1, b, noiseModel::Unit::Create(b.size()))); + return GaussianFactor::shared_ptr(new JacobianFactor(key(), A1, b, noiseModel::Unit::Create(b.size()))); } /** vector of errors */ @@ -405,10 +403,10 @@ TEST( ExtendedKalmanFilter, nonlinear ) { // Create the Kalman Filter initialization point Point2 x_initial(0.90, 1.10); - SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); + SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.1)); // Create an ExtendedKalmanFilter object - ExtendedKalmanFilter ekf(x_initial, P_initial); + ExtendedKalmanFilter ekf(X(0), x_initial, P_initial); // Enter Predict-Update Loop Point2 x_predict, x_update; @@ -418,7 +416,7 @@ TEST( ExtendedKalmanFilter, nonlinear ) { x_predict = ekf.predict(motionFactor); // Create a measurement factor - NonlinearMeasurementModel measurementFactor(X(i+1), Vector_(1, z[i])); + NonlinearMeasurementModel measurementFactor(X(i+1), (Vector(1) << z[i])); x_update = ekf.update(measurementFactor); EXPECT(assert_equal(expected_predict[i],x_predict, 1e-6)); diff --git a/tests/testGaussianBayesNet.cpp b/tests/testGaussianBayesNet.cpp deleted file mode 100644 index 1cef17341..000000000 --- a/tests/testGaussianBayesNet.cpp +++ /dev/null @@ -1,189 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 testGaussianBayesNet.cpp - * @brief Unit tests for GaussianBayesNet - * @author Frank Dellaert - */ - -// STL/C++ -#include -#include -#include -#include -#include - -#include // for operator += -using namespace boost::assign; - -#include -#include -#include -#include -#include - -using namespace std; -using namespace gtsam; -using namespace example; - -static const Index _x_=0, _y_=1, _z_=2; - -/* ************************************************************************* */ -TEST( GaussianBayesNet, constructor ) -{ - // small Bayes Net x <- y - // x y d - // 1 1 9 - // 1 5 - Matrix R11 = Matrix_(1,1,1.0), S12 = Matrix_(1,1,1.0); - Matrix R22 = Matrix_(1,1,1.0); - Vector d1(1), d2(1); - d1(0) = 9; d2(0) = 5; - Vector sigmas(1); - sigmas(0) = 1.; - - // define nodes and specify in reverse topological sort (i.e. parents last) - GaussianConditional x(_x_,d1,R11,_y_,S12, sigmas), y(_y_,d2,R22, sigmas); - - // check small example which uses constructor - GaussianBayesNet cbn = createSmallGaussianBayesNet(); - EXPECT( x.equals(*cbn[_x_]) ); - EXPECT( y.equals(*cbn[_y_]) ); -} - -/* ************************************************************************* */ -TEST( GaussianBayesNet, matrix ) -{ - // Create a test graph - GaussianBayesNet cbn = createSmallGaussianBayesNet(); - - Matrix R; Vector d; - boost::tie(R,d) = matrix(cbn); // find matrix and RHS - - Matrix R1 = Matrix_(2,2, - 1.0, 1.0, - 0.0, 1.0 - ); - Vector d1 = Vector_(2, 9.0, 5.0); - - EXPECT(assert_equal(R,R1)); - EXPECT(assert_equal(d,d1)); -} - -/* ************************************************************************* */ -TEST( GaussianBayesNet, optimize ) -{ - GaussianBayesNet cbn = createSmallGaussianBayesNet(); - VectorValues actual = optimize(cbn); - - VectorValues expected(vector(2,1)); - expected[_x_] = Vector_(1,4.); - expected[_y_] = Vector_(1,5.); - - EXPECT(assert_equal(expected,actual)); -} - -/* ************************************************************************* */ -TEST( GaussianBayesNet, optimize2 ) -{ - - // Create empty graph - GaussianFactorGraph fg; - SharedDiagonal noise = noiseModel::Unit::Create(1); - - fg.add(_y_, eye(1), 2*ones(1), noise); - - fg.add(_x_, eye(1),_y_, -eye(1), -ones(1), noise); - - fg.add(_y_, eye(1),_z_, -eye(1), -ones(1), noise); - - fg.add(_x_, -eye(1), _z_, eye(1), 2*ones(1), noise); - - VectorValues actual = *GaussianSequentialSolver(fg).optimize(); - - VectorValues expected(vector(3,1)); - expected[_x_] = Vector_(1,1.); - expected[_y_] = Vector_(1,2.); - expected[_z_] = Vector_(1,3.); - - EXPECT(assert_equal(expected,actual)); -} - -/* ************************************************************************* */ -TEST( GaussianBayesNet, optimize3 ) -{ - // y=R*x, x=inv(R)*y - // 9 = 1 1 4 - // 5 1 5 - // NOTE: we are supplying a new RHS here - GaussianBayesNet cbn = createSmallGaussianBayesNet(); - - VectorValues expected(vector(2,1)), x(vector(2,1)); - expected[_x_] = Vector_(1, 4.); - expected[_y_] = Vector_(1, 5.); - - // test functional version - VectorValues actual = optimize(cbn); - EXPECT(assert_equal(expected,actual)); - - // test imperative version - optimizeInPlace(cbn,x); - EXPECT(assert_equal(expected,x)); -} - -/* ************************************************************************* */ -TEST( GaussianBayesNet, backSubstituteTranspose ) -{ - // x=R'*y, y=inv(R')*x - // 2 = 1 2 - // 5 1 1 3 - GaussianBayesNet cbn = createSmallGaussianBayesNet(); - - VectorValues y(vector(2,1)), x(vector(2,1)); - x[_x_] = Vector_(1,2.); - x[_y_] = Vector_(1,5.); - y[_x_] = Vector_(1,2.); - y[_y_] = Vector_(1,3.); - - // test functional version - VectorValues actual = backSubstituteTranspose(cbn,x); - EXPECT(assert_equal(y,actual)); -} - -/* ************************************************************************* */ -// Tests computing Determinant -TEST( GaussianBayesNet, DeterminantTest ) -{ - GaussianBayesNet cbn; - cbn += boost::shared_ptr(new GaussianConditional( - 0, Vector_( 2, 3.0, 4.0 ), Matrix_(2, 2, 1.0, 3.0, 0.0, 4.0 ), - 1, Matrix_(2, 2, 2.0, 1.0, 2.0, 3.0), - ones(2))); - - cbn += boost::shared_ptr(new GaussianConditional( - 1, Vector_( 2, 5.0, 6.0 ), Matrix_(2, 2, 1.0, 1.0, 0.0, 3.0 ), - 2, Matrix_(2, 2, 1.0, 0.0, 5.0, 2.0), - ones(2))); - - cbn += boost::shared_ptr(new GaussianConditional( - 3, Vector_( 2, 7.0, 8.0 ), Matrix_(2, 2, 1.0, 1.0, 0.0, 5.0 ), - ones(2))); - - double expectedDeterminant = 60; - double actualDeterminant = determinant(cbn); - - EXPECT_DOUBLES_EQUAL( expectedDeterminant, actualDeterminant, 1e-9); -} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr);} -/* ************************************************************************* */ diff --git a/tests/testGaussianBayesTree.cpp b/tests/testGaussianBayesTreeB.cpp similarity index 54% rename from tests/testGaussianBayesTree.cpp rename to tests/testGaussianBayesTreeB.cpp index 85ea792fe..97bd5bd57 100644 --- a/tests/testGaussianBayesTree.cpp +++ b/tests/testGaussianBayesTreeB.cpp @@ -16,10 +16,12 @@ */ #include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include #include @@ -55,42 +57,40 @@ C6 x1 : x2 TEST( GaussianBayesTree, linear_smoother_shortcuts ) { // Create smoother with 7 nodes - Ordering ordering; - GaussianFactorGraph smoother; - boost::tie(smoother, ordering) = createSmoother(7); + GaussianFactorGraph smoother = createSmoother(7); - GaussianBayesTree bayesTree = *GaussianMultifrontalSolver(smoother).eliminate(); + GaussianBayesTree bayesTree = *smoother.eliminateMultifrontal(); // Create the Bayes tree - LONGS_EQUAL(6, bayesTree.size()); + LONGS_EQUAL(6, (long)bayesTree.size()); // Check the conditional P(Root|Root) GaussianBayesNet empty; - GaussianBayesTree::sharedClique R = bayesTree.root(); - GaussianBayesNet actual1 = R->shortcut(R, EliminateCholesky); + GaussianBayesTree::sharedClique R = bayesTree.roots().front(); + GaussianBayesNet actual1 = R->shortcut(R); EXPECT(assert_equal(empty,actual1,tol)); // Check the conditional P(C2|Root) - GaussianBayesTree::sharedClique C2 = bayesTree[ordering[X(5)]]; - GaussianBayesNet actual2 = C2->shortcut(R, EliminateCholesky); + GaussianBayesTree::sharedClique C2 = bayesTree[X(5)]; + GaussianBayesNet actual2 = C2->shortcut(R); EXPECT(assert_equal(empty,actual2,tol)); // Check the conditional P(C3|Root) double sigma3 = 0.61808; - Matrix A56 = Matrix_(2,2,-0.382022,0.,0.,-0.382022); + Matrix A56 = (Matrix(2,2) << -0.382022,0.,0.,-0.382022); GaussianBayesNet expected3; - push_front(expected3,ordering[X(5)], zero(2), eye(2)/sigma3, ordering[X(6)], A56/sigma3, ones(2)); - GaussianBayesTree::sharedClique C3 = bayesTree[ordering[X(4)]]; - GaussianBayesNet actual3 = C3->shortcut(R, EliminateCholesky); + expected3 += GaussianConditional(X(5), zero(2), eye(2)/sigma3, X(6), A56/sigma3); + GaussianBayesTree::sharedClique C3 = bayesTree[X(4)]; + GaussianBayesNet actual3 = C3->shortcut(R); EXPECT(assert_equal(expected3,actual3,tol)); // Check the conditional P(C4|Root) double sigma4 = 0.661968; - Matrix A46 = Matrix_(2,2,-0.146067,0.,0.,-0.146067); + Matrix A46 = (Matrix(2,2) << -0.146067,0.,0.,-0.146067); GaussianBayesNet expected4; - push_front(expected4, ordering[X(4)], zero(2), eye(2)/sigma4, ordering[X(6)], A46/sigma4, ones(2)); - GaussianBayesTree::sharedClique C4 = bayesTree[ordering[X(3)]]; - GaussianBayesNet actual4 = C4->shortcut(R, EliminateCholesky); + expected4 += GaussianConditional(X(4), zero(2), eye(2)/sigma4, X(6), A46/sigma4); + GaussianBayesTree::sharedClique C4 = bayesTree[X(3)]; + GaussianBayesNet actual4 = C4->shortcut(R); EXPECT(assert_equal(expected4,actual4,tol)); } @@ -116,66 +116,50 @@ TEST( GaussianBayesTree, linear_smoother_shortcuts ) TEST( GaussianBayesTree, balanced_smoother_marginals ) { // Create smoother with 7 nodes - Ordering ordering; - ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4); - GaussianFactorGraph smoother = createSmoother(7, ordering).first; + GaussianFactorGraph smoother = createSmoother(7); // Create the Bayes tree - GaussianBayesTree bayesTree = *GaussianMultifrontalSolver(smoother).eliminate(); + Ordering ordering; + ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4); + GaussianBayesTree bayesTree = *smoother.eliminateMultifrontal(ordering); - VectorValues expectedSolution(VectorValues::Zero(7,2)); - VectorValues actualSolution = optimize(bayesTree); + VectorValues actualSolution = bayesTree.optimize(); + VectorValues expectedSolution = VectorValues::Zero(actualSolution); EXPECT(assert_equal(expectedSolution,actualSolution,tol)); - LONGS_EQUAL(4,bayesTree.size()); + LONGS_EQUAL(4, (long)bayesTree.size()); double tol=1e-5; // Check marginal on x1 - GaussianBayesNet expected1 = simpleGaussian(ordering[X(1)], zero(2), sigmax1); - GaussianBayesNet actual1 = *bayesTree.marginalBayesNet(ordering[X(1)], EliminateCholesky); + JacobianFactor expected1 = GaussianDensity::FromMeanAndStddev(X(1), zero(2), sigmax1); + JacobianFactor actual1 = *bayesTree.marginalFactor(X(1)); Matrix expectedCovarianceX1 = eye(2,2) * (sigmax1 * sigmax1); Matrix actualCovarianceX1; - GaussianFactor::shared_ptr m = bayesTree.marginalFactor(ordering[X(1)], EliminateCholesky); - actualCovarianceX1 = bayesTree.marginalFactor(ordering[X(1)], EliminateCholesky)->information().inverse(); + GaussianFactor::shared_ptr m = bayesTree.marginalFactor(X(1), EliminateCholesky); + actualCovarianceX1 = bayesTree.marginalFactor(X(1), EliminateCholesky)->information().inverse(); EXPECT(assert_equal(expectedCovarianceX1, actualCovarianceX1, tol)); EXPECT(assert_equal(expected1,actual1,tol)); // Check marginal on x2 double sigx2 = 0.68712938; // FIXME: this should be corrected analytically - GaussianBayesNet expected2 = simpleGaussian(ordering[X(2)], zero(2), sigx2); - GaussianBayesNet actual2 = *bayesTree.marginalBayesNet(ordering[X(2)], EliminateCholesky); - Matrix expectedCovarianceX2 = eye(2,2) * (sigx2 * sigx2); - Matrix actualCovarianceX2; - actualCovarianceX2 = bayesTree.marginalFactor(ordering[X(2)], EliminateCholesky)->information().inverse(); - EXPECT(assert_equal(expectedCovarianceX2, actualCovarianceX2, tol)); + JacobianFactor expected2 = GaussianDensity::FromMeanAndStddev(X(2), zero(2), sigx2); + JacobianFactor actual2 = *bayesTree.marginalFactor(X(2)); EXPECT(assert_equal(expected2,actual2,tol)); // Check marginal on x3 - GaussianBayesNet expected3 = simpleGaussian(ordering[X(3)], zero(2), sigmax3); - GaussianBayesNet actual3 = *bayesTree.marginalBayesNet(ordering[X(3)], EliminateCholesky); - Matrix expectedCovarianceX3 = eye(2,2) * (sigmax3 * sigmax3); - Matrix actualCovarianceX3; - actualCovarianceX3 = bayesTree.marginalFactor(ordering[X(3)], EliminateCholesky)->information().inverse(); - EXPECT(assert_equal(expectedCovarianceX3, actualCovarianceX3, tol)); + JacobianFactor expected3 = GaussianDensity::FromMeanAndStddev(X(3), zero(2), sigmax3); + JacobianFactor actual3 = *bayesTree.marginalFactor(X(3)); EXPECT(assert_equal(expected3,actual3,tol)); // Check marginal on x4 - GaussianBayesNet expected4 = simpleGaussian(ordering[X(4)], zero(2), sigmax4); - GaussianBayesNet actual4 = *bayesTree.marginalBayesNet(ordering[X(4)], EliminateCholesky); - Matrix expectedCovarianceX4 = eye(2,2) * (sigmax4 * sigmax4); - Matrix actualCovarianceX4; - actualCovarianceX4 = bayesTree.marginalFactor(ordering[X(4)], EliminateCholesky)->information().inverse(); - EXPECT(assert_equal(expectedCovarianceX4, actualCovarianceX4, tol)); + JacobianFactor expected4 = GaussianDensity::FromMeanAndStddev(X(4), zero(2), sigmax4); + JacobianFactor actual4 = *bayesTree.marginalFactor(X(4)); EXPECT(assert_equal(expected4,actual4,tol)); // Check marginal on x7 (should be equal to x1) - GaussianBayesNet expected7 = simpleGaussian(ordering[X(7)], zero(2), sigmax7); - GaussianBayesNet actual7 = *bayesTree.marginalBayesNet(ordering[X(7)], EliminateCholesky); - Matrix expectedCovarianceX7 = eye(2,2) * (sigmax7 * sigmax7); - Matrix actualCovarianceX7; - actualCovarianceX7 = bayesTree.marginalFactor(ordering[X(7)], EliminateCholesky)->information().inverse(); - EXPECT(assert_equal(expectedCovarianceX7, actualCovarianceX7, tol)); + JacobianFactor expected7 = GaussianDensity::FromMeanAndStddev(X(7), zero(2), sigmax7); + JacobianFactor actual7 = *bayesTree.marginalFactor(X(7)); EXPECT(assert_equal(expected7,actual7,tol)); } @@ -183,22 +167,22 @@ TEST( GaussianBayesTree, balanced_smoother_marginals ) TEST( GaussianBayesTree, balanced_smoother_shortcuts ) { // Create smoother with 7 nodes - Ordering ordering; - ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4); - GaussianFactorGraph smoother = createSmoother(7, ordering).first; + GaussianFactorGraph smoother = createSmoother(7); // Create the Bayes tree - GaussianBayesTree bayesTree = *GaussianMultifrontalSolver(smoother).eliminate(); + Ordering ordering; + ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4); + GaussianBayesTree bayesTree = *smoother.eliminateMultifrontal(ordering); // Check the conditional P(Root|Root) GaussianBayesNet empty; - GaussianBayesTree::sharedClique R = bayesTree.root(); - GaussianBayesNet actual1 = R->shortcut(R, EliminateCholesky); + GaussianBayesTree::sharedClique R = bayesTree.roots().front(); + GaussianBayesNet actual1 = R->shortcut(R); EXPECT(assert_equal(empty,actual1,tol)); // Check the conditional P(C2|Root) - GaussianBayesTree::sharedClique C2 = bayesTree[ordering[X(3)]]; - GaussianBayesNet actual2 = C2->shortcut(R, EliminateCholesky); + GaussianBayesTree::sharedClique C2 = bayesTree[X(3)]; + GaussianBayesNet actual2 = C2->shortcut(R); EXPECT(assert_equal(empty,actual2,tol)); // Check the conditional P(C3|Root), which should be equal to P(x2|x4) @@ -249,78 +233,56 @@ TEST( GaussianBayesTree, balanced_smoother_joint ) // Create smoother with 7 nodes Ordering ordering; ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4); - GaussianFactorGraph smoother = createSmoother(7, ordering).first; + GaussianFactorGraph smoother = createSmoother(7); // Create the Bayes tree, expected to look like: // x5 x6 x4 // x3 x2 : x4 // x1 : x2 // x7 : x6 - GaussianBayesTree bayesTree = *GaussianMultifrontalSolver(smoother).eliminate(); + GaussianBayesTree bayesTree = *smoother.eliminateMultifrontal(ordering); // Conditional density elements reused by both tests - const Vector sigma = ones(2); const Matrix I = eye(2), A = -0.00429185*I; // Check the joint density P(x1,x7) factored as P(x1|x7)P(x7) - GaussianBayesNet expected1; - // Why does the sign get flipped on the prior? - GaussianConditional::shared_ptr - parent1(new GaussianConditional(ordering[X(7)], zero(2), -1*I/sigmax7, ones(2))); - expected1.push_front(parent1); - push_front(expected1,ordering[X(1)], zero(2), I/sigmax7, ordering[X(7)], A/sigmax7, sigma); - GaussianBayesNet actual1 = *bayesTree.jointBayesNet(ordering[X(1)],ordering[X(7)], EliminateCholesky); - EXPECT(assert_equal(expected1,actual1,tol)); + GaussianBayesNet expected1 = list_of + // Why does the sign get flipped on the prior? + (GaussianConditional(X(1), zero(2), I/sigmax7, X(7), A/sigmax7)) + (GaussianConditional(X(7), zero(2), -1*I/sigmax7)); + GaussianBayesNet actual1 = *bayesTree.jointBayesNet(X(1),X(7)); + EXPECT(assert_equal(expected1, actual1, tol)); // // Check the joint density P(x7,x1) factored as P(x7|x1)P(x1) // GaussianBayesNet expected2; // GaussianConditional::shared_ptr - // parent2(new GaussianConditional(ordering[X(1)], zero(2), -1*I/sigmax1, ones(2))); + // parent2(new GaussianConditional(X(1), zero(2), -1*I/sigmax1, ones(2))); // expected2.push_front(parent2); - // push_front(expected2,ordering[X(7)], zero(2), I/sigmax1, ordering[X(1)], A/sigmax1, sigma); - // GaussianBayesNet actual2 = *bayesTree.jointBayesNet(ordering[X(7)],ordering[X(1)]); + // push_front(expected2,X(7), zero(2), I/sigmax1, X(1), A/sigmax1, sigma); + // GaussianBayesNet actual2 = *bayesTree.jointBayesNet(X(7),X(1)); // EXPECT(assert_equal(expected2,actual2,tol)); // Check the joint density P(x1,x4), i.e. with a root variable - GaussianBayesNet expected3; - GaussianConditional::shared_ptr - parent3(new GaussianConditional(ordering[X(4)], zero(2), I/sigmax4, ones(2))); - expected3.push_front(parent3); double sig14 = 0.784465; Matrix A14 = -0.0769231*I; - push_front(expected3,ordering[X(1)], zero(2), I/sig14, ordering[X(4)], A14/sig14, sigma); - GaussianBayesNet actual3 = *bayesTree.jointBayesNet(ordering[X(1)],ordering[X(4)], EliminateCholesky); + GaussianBayesNet expected3 = list_of + (GaussianConditional(X(1), zero(2), I/sig14, X(4), A14/sig14)) + (GaussianConditional(X(4), zero(2), I/sigmax4)); + GaussianBayesNet actual3 = *bayesTree.jointBayesNet(X(1),X(4)); EXPECT(assert_equal(expected3,actual3,tol)); // // Check the joint density P(x4,x1), i.e. with a root variable, factored the other way // GaussianBayesNet expected4; // GaussianConditional::shared_ptr - // parent4(new GaussianConditional(ordering[X(1)], zero(2), -1.0*I/sigmax1, ones(2))); + // parent4(new GaussianConditional(X(1), zero(2), -1.0*I/sigmax1, ones(2))); // expected4.push_front(parent4); // double sig41 = 0.668096; // Matrix A41 = -0.055794*I; - // push_front(expected4,ordering[X(4)], zero(2), I/sig41, ordering[X(1)], A41/sig41, sigma); - // GaussianBayesNet actual4 = *bayesTree.jointBayesNet(ordering[X(4)],ordering[X(1)]); + // push_front(expected4,X(4), zero(2), I/sig41, X(1), A41/sig41, sigma); + // GaussianBayesNet actual4 = *bayesTree.jointBayesNet(X(4),X(1)); // EXPECT(assert_equal(expected4,actual4,tol)); } -/* ************************************************************************* */ -TEST(GaussianBayesTree, simpleMarginal) -{ - GaussianFactorGraph gfg; - - Matrix A12 = Rot2::fromDegrees(45.0).matrix(); - - gfg.add(0, eye(2), zero(2), noiseModel::Isotropic::Sigma(2, 1.0)); - gfg.add(0, -eye(2), 1, eye(2), ones(2), noiseModel::Isotropic::Sigma(2, 1.0)); - gfg.add(1, -eye(2), 2, A12, ones(2), noiseModel::Isotropic::Sigma(2, 1.0)); - - Matrix expected(GaussianSequentialSolver(gfg).marginalCovariance(2)); - Matrix actual(GaussianMultifrontalSolver(gfg).marginalCovariance(2)); - - EXPECT(assert_equal(expected, actual)); -} - /* ************************************************************************* */ TEST(GaussianBayesTree, shortcut_overlapping_separator) { @@ -334,26 +296,26 @@ TEST(GaussianBayesTree, shortcut_overlapping_separator) // f(6,7) GaussianFactorGraph fg; noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(1); - fg.add(1, Matrix_(1,1, 1.0), 3, Matrix_(1,1, 2.0), 5, Matrix_(1,1, 3.0), Vector_(1, 4.0), model); - fg.add(1, Matrix_(1,1, 5.0), Vector_(1, 6.0), model); - fg.add(2, Matrix_(1,1, 7.0), 4, Matrix_(1,1, 8.0), 5, Matrix_(1,1, 9.0), Vector_(1, 10.0), model); - fg.add(2, Matrix_(1,1, 11.0), Vector_(1, 12.0), model); - fg.add(5, Matrix_(1,1, 13.0), 6, Matrix_(1,1, 14.0), Vector_(1, 15.0), model); - fg.add(6, Matrix_(1,1, 17.0), 7, Matrix_(1,1, 18.0), Vector_(1, 19.0), model); - fg.add(7, Matrix_(1,1, 20.0), Vector_(1, 21.0), model); + fg.add(1, (Matrix(1, 1) << 1.0), 3, (Matrix(1, 1) << 2.0), 5, (Matrix(1, 1) << 3.0), (Vector(1) << 4.0), model); + fg.add(1, (Matrix(1, 1) << 5.0), (Vector(1) << 6.0), model); + fg.add(2, (Matrix(1, 1) << 7.0), 4, (Matrix(1, 1) << 8.0), 5, (Matrix(1, 1) << 9.0), (Vector(1) << 10.0), model); + fg.add(2, (Matrix(1, 1) << 11.0), (Vector(1) << 12.0), model); + fg.add(5, (Matrix(1, 1) << 13.0), 6, (Matrix(1, 1) << 14.0), (Vector(1) << 15.0), model); + fg.add(6, (Matrix(1, 1) << 17.0), 7, (Matrix(1, 1) << 18.0), (Vector(1) << 19.0), model); + fg.add(7, (Matrix(1, 1) << 20.0), (Vector(1) << 21.0), model); // Eliminate into BayesTree // c(6,7) // c(5|6) // c(1,2|5) // c(3,4|5) - GaussianBayesTree bt = *GaussianMultifrontalSolver(fg).eliminate(); + GaussianBayesTree bt = *fg.eliminateMultifrontal(Ordering(fg.keys())); // eliminate in increasing key order, fg.keys() is sorted. GaussianFactorGraph joint = *bt.joint(1,2, EliminateQR); Matrix expectedJointJ = (Matrix(2,3) << - 0, 11, 12, - -5, 0, -6 + 5, 0, 6, + 0, -11, -12 ).finished(); Matrix actualJointJ = joint.augmentedJacobian(); diff --git a/tests/testGaussianFactor.cpp b/tests/testGaussianFactor.cpp deleted file mode 100644 index 62cad80ca..000000000 --- a/tests/testGaussianFactor.cpp +++ /dev/null @@ -1,228 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 testGaussianFactor.cpp - * @brief Unit tests for Linear Factor - * @author Christian Potthast - * @author Frank Dellaert - **/ - -#include -#include -#include -#include -#include -#include - -#include - -#include -#include // for operator += -#include -#include // for insert -using namespace boost::assign; - -#include - -using namespace std; -using namespace gtsam; - -// Convenience for named keys -using symbol_shorthand::X; -using symbol_shorthand::L; - -static SharedDiagonal - sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1), sigma_02 = noiseModel::Isotropic::Sigma(2,0.2), - constraintModel = noiseModel::Constrained::All(2); - -//const Key kx1 = X(1), kx2 = X(2), kl1 = L(1); // FIXME: throws exception - -/* ************************************************************************* */ -TEST( GaussianFactor, linearFactor ) -{ - const Key kx1 = X(1), kx2 = X(2), kl1 = L(1); - Ordering ordering; ordering += kx1,kx2,kl1; - - Matrix I = eye(2); - Vector b = Vector_(2, 2.0, -1.0); - JacobianFactor expected(ordering[kx1], -10*I,ordering[kx2], 10*I, b, noiseModel::Unit::Create(2)); - - // create a small linear factor graph - GaussianFactorGraph fg = example::createGaussianFactorGraph(ordering); - - // get the factor kf2 from the factor graph - JacobianFactor::shared_ptr lf = - boost::dynamic_pointer_cast(fg[1]); - - // check if the two factors are the same - EXPECT(assert_equal(expected,*lf)); -} - -/* ************************************************************************* */ -TEST( GaussianFactor, getDim ) -{ - const Key kx1 = X(1), kx2 = X(2), kl1 = L(1); - // get a factor - Ordering ordering; ordering += kx1,kx2,kl1; - GaussianFactorGraph fg = example::createGaussianFactorGraph(ordering); - GaussianFactor::shared_ptr factor = fg[0]; - - // get the size of a variable - size_t actual = factor->getDim(factor->find(ordering[kx1])); - - // verify - size_t expected = 2; - EXPECT_LONGS_EQUAL(expected, actual); -} - -/* ************************************************************************* */ -TEST( GaussianFactor, error ) -{ - const Key kx1 = X(1), kx2 = X(2), kl1 = L(1); - // create a small linear factor graph - Ordering ordering; ordering += kx1,kx2,kl1; - GaussianFactorGraph fg = example::createGaussianFactorGraph(ordering); - - // get the first factor from the factor graph - GaussianFactor::shared_ptr lf = fg[0]; - - // check the error of the first factor with noisy config - VectorValues cfg = example::createZeroDelta(ordering); - - // calculate the error from the factor kf1 - // note the error is the same as in testNonlinearFactor - double actual = lf->error(cfg); - DOUBLES_EQUAL( 1.0, actual, 0.00000001 ); -} - -/* ************************************************************************* */ -TEST( GaussianFactor, matrix ) -{ - const Key kx1 = X(1), kx2 = X(2), kl1 = L(1); - // create a small linear factor graph - Ordering ordering; ordering += kx1,kx2,kl1; - GaussianFactorGraph fg = example::createGaussianFactorGraph(ordering); - - // get the factor kf2 from the factor graph - //GaussianFactor::shared_ptr lf = fg[1]; // NOTE: using the older version - Vector b2 = Vector_(2, 0.2, -0.1); - Matrix I = eye(2); - // render with a given ordering - Ordering ord; - ord += kx1,kx2; - JacobianFactor::shared_ptr lf(new JacobianFactor(ord[kx1], -I, ord[kx2], I, b2, sigma0_1)); - - // Test whitened version - Matrix A_act1; Vector b_act1; - boost::tie(A_act1,b_act1) = lf->matrix(true); - - Matrix A1 = Matrix_(2,4, - -10.0, 0.0, 10.0, 0.0, - 000.0,-10.0, 0.0, 10.0 ); - Vector b1 = Vector_(2, 2.0, -1.0); - - EQUALITY(A_act1,A1); - EQUALITY(b_act1,b1); - - // Test unwhitened version - Matrix A_act2; Vector b_act2; - boost::tie(A_act2,b_act2) = lf->matrix(false); - - - Matrix A2 = Matrix_(2,4, - -1.0, 0.0, 1.0, 0.0, - 000.0,-1.0, 0.0, 1.0 ); - //Vector b2 = Vector_(2, 2.0, -1.0); - - EQUALITY(A_act2,A2); - EQUALITY(b_act2,b2); - - // Ensure that whitening is consistent - boost::shared_ptr model = lf->get_model(); - model->WhitenSystem(A_act2, b_act2); - EQUALITY(A_act1, A_act2); - EQUALITY(b_act1, b_act2); -} - -/* ************************************************************************* */ -TEST( GaussianFactor, matrix_aug ) -{ - const Key kx1 = X(1), kx2 = X(2), kl1 = L(1); - // create a small linear factor graph - Ordering ordering; ordering += kx1,kx2,kl1; - GaussianFactorGraph fg = example::createGaussianFactorGraph(ordering); - - // get the factor kf2 from the factor graph - //GaussianFactor::shared_ptr lf = fg[1]; - Vector b2 = Vector_(2, 0.2, -0.1); - Matrix I = eye(2); - // render with a given ordering - Ordering ord; - ord += kx1,kx2; - JacobianFactor::shared_ptr lf(new JacobianFactor(ord[kx1], -I, ord[kx2], I, b2, sigma0_1)); - - - // Test unwhitened version - Matrix Ab_act1; - Ab_act1 = lf->matrix_augmented(false); - - Matrix Ab1 = Matrix_(2,5, - -1.0, 0.0, 1.0, 0.0, 0.2, - 00.0,- 1.0, 0.0, 1.0, -0.1 ); - - EQUALITY(Ab_act1,Ab1); - - // Test whitened version - Matrix Ab_act2; - Ab_act2 = lf->matrix_augmented(true); - - Matrix Ab2 = Matrix_(2,5, - -10.0, 0.0, 10.0, 0.0, 2.0, - 00.0, -10.0, 0.0, 10.0, -1.0 ); - - EQUALITY(Ab_act2,Ab2); - - // Ensure that whitening is consistent - boost::shared_ptr model = lf->get_model(); - model->WhitenInPlace(Ab_act1); - EQUALITY(Ab_act1, Ab_act2); -} - -/* ************************************************************************* */ -// small aux. function to print out lists of anything -template -void print(const list& i) { - copy(i.begin(), i.end(), ostream_iterator (cout, ",")); - cout << endl; -} - -/* ************************************************************************* */ -TEST( GaussianFactor, size ) -{ - // create a linear factor graph - const Key kx1 = X(1), kx2 = X(2), kl1 = L(1); - Ordering ordering; ordering += kx1,kx2,kl1; - GaussianFactorGraph fg = example::createGaussianFactorGraph(ordering); - - // get some factors from the graph - boost::shared_ptr factor1 = fg[0]; - boost::shared_ptr factor2 = fg[1]; - boost::shared_ptr factor3 = fg[2]; - - EXPECT_LONGS_EQUAL(1, factor1->size()); - EXPECT_LONGS_EQUAL(2, factor2->size()); - EXPECT_LONGS_EQUAL(2, factor3->size()); -} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr);} -/* ************************************************************************* */ diff --git a/tests/testGaussianFactorGraphB.cpp b/tests/testGaussianFactorGraphB.cpp index b8bea8c6e..962d8b893 100644 --- a/tests/testGaussianFactorGraphB.cpp +++ b/tests/testGaussianFactorGraphB.cpp @@ -16,11 +16,10 @@ **/ #include -#include +#include #include -#include +#include #include -#include #include #include #include @@ -33,6 +32,8 @@ #include // for operator += #include // for operator += using namespace boost::assign; +#include +namespace br { using namespace boost::range; using namespace boost::adaptors; } #include #include @@ -49,17 +50,15 @@ using symbol_shorthand::L; /* ************************************************************************* */ TEST( GaussianFactorGraph, equals ) { - Ordering ordering; ordering += X(1),X(2),L(1); - GaussianFactorGraph fg = createGaussianFactorGraph(ordering); - GaussianFactorGraph fg2 = createGaussianFactorGraph(ordering); + GaussianFactorGraph fg = createGaussianFactorGraph(); + GaussianFactorGraph fg2 = createGaussianFactorGraph(); EXPECT(fg.equals(fg2)); } /* ************************************************************************* */ TEST( GaussianFactorGraph, error ) { - Ordering ordering; ordering += X(1),X(2),L(1); - GaussianFactorGraph fg = createGaussianFactorGraph(ordering); - VectorValues cfg = createZeroDelta(ordering); + GaussianFactorGraph fg = createGaussianFactorGraph(); + VectorValues cfg = createZeroDelta(); // note the error is the same as in testNonlinearFactorGraph as a // zero delta config in the linear graph is equivalent to noisy in @@ -71,21 +70,23 @@ TEST( GaussianFactorGraph, error ) { /* ************************************************************************* */ TEST( GaussianFactorGraph, eliminateOne_x1 ) { - Ordering ordering; ordering += X(1),L(1),X(2); - GaussianFactorGraph fg = createGaussianFactorGraph(ordering); + GaussianFactorGraph fg = createGaussianFactorGraph(); GaussianConditional::shared_ptr conditional; - GaussianFactorGraph remaining; - boost::tie(conditional,remaining) = fg.eliminateOne(0, EliminateQR); + pair result = + fg.eliminatePartialSequential(Ordering(list_of(X(1)))); + conditional = result.first->front(); // create expected Conditional Gaussian Matrix I = 15*eye(2), R11 = I, S12 = -0.111111*I, S13 = -0.444444*I; - Vector d = Vector_(2, -0.133333, -0.0222222), sigma = ones(2); - GaussianConditional expected(ordering[X(1)],15*d,R11,ordering[L(1)],S12,ordering[X(2)],S13,sigma); + Vector d = (Vector(2) << -0.133333, -0.0222222); + GaussianConditional expected(X(1),15*d,R11,L(1),S12,X(2),S13); EXPECT(assert_equal(expected,*conditional,tol)); } +#if 0 + /* ************************************************************************* */ TEST( GaussianFactorGraph, eliminateOne_x2 ) { @@ -96,7 +97,7 @@ TEST( GaussianFactorGraph, eliminateOne_x2 ) // create expected Conditional Gaussian double sig = 0.0894427; Matrix I = eye(2)/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I; - Vector d = Vector_(2, 0.2, -0.14)/sig, sigma = ones(2); + Vector d = (Vector(2) << 0.2, -0.14)/sig, sigma = ones(2); GaussianConditional expected(ordering[X(2)],d,R11,ordering[L(1)],S12,ordering[X(1)],S13,sigma); EXPECT(assert_equal(expected,*actual,tol)); @@ -112,7 +113,7 @@ TEST( GaussianFactorGraph, eliminateOne_l1 ) // create expected Conditional Gaussian double sig = sqrt(2.0)/10.; Matrix I = eye(2)/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I; - Vector d = Vector_(2, -0.1, 0.25)/sig, sigma = ones(2); + Vector d = (Vector(2) << -0.1, 0.25)/sig, sigma = ones(2); GaussianConditional expected(ordering[L(1)],d,R11,ordering[X(1)],S12,ordering[X(2)],S13,sigma); EXPECT(assert_equal(expected,*actual,tol)); @@ -129,21 +130,21 @@ TEST( GaussianFactorGraph, eliminateOne_x1_fast ) // create expected Conditional Gaussian Matrix I = 15*eye(2), R11 = I, S12 = -0.111111*I, S13 = -0.444444*I; - Vector d = Vector_(2, -0.133333, -0.0222222), sigma = ones(2); + Vector d = (Vector(2) << -0.133333, -0.0222222), sigma = ones(2); GaussianConditional expected(ordering[X(1)],15*d,R11,ordering[L(1)],S12,ordering[X(2)],S13,sigma); // Create expected remaining new factor - JacobianFactor expectedFactor(1, Matrix_(4,2, + JacobianFactor expectedFactor(1, (Matrix(4,2) << 4.714045207910318, 0., 0., 4.714045207910318, 0., 0., 0., 0.), - 2, Matrix_(4,2, + 2, (Matrix(4,2) << -2.357022603955159, 0., 0., -2.357022603955159, 7.071067811865475, 0., 0., 7.071067811865475), - Vector_(4, -0.707106781186547, 0.942809041582063, 0.707106781186547, -1.414213562373094), noiseModel::Unit::Create(4)); + (Vector(4) << -0.707106781186547, 0.942809041582063, 0.707106781186547, -1.414213562373094), noiseModel::Unit::Create(4)); EXPECT(assert_equal(expected,*conditional,tol)); EXPECT(assert_equal((const GaussianFactor&)expectedFactor,*remaining.back(),tol)); @@ -159,7 +160,7 @@ TEST( GaussianFactorGraph, eliminateOne_x2_fast ) // create expected Conditional Gaussian double sig = 0.0894427; Matrix I = eye(2)/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I; - Vector d = Vector_(2, 0.2, -0.14)/sig, sigma = ones(2); + Vector d = (Vector(2) << 0.2, -0.14)/sig, sigma = ones(2); GaussianConditional expected(ordering[X(2)],d,R11,ordering[X(1)],S13,ordering[L(1)],S12,sigma); EXPECT(assert_equal(expected,*actual,tol)); @@ -175,7 +176,7 @@ TEST( GaussianFactorGraph, eliminateOne_l1_fast ) // create expected Conditional Gaussian double sig = sqrt(2.0)/10.; Matrix I = eye(2)/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I; - Vector d = Vector_(2, -0.1, 0.25)/sig, sigma = ones(2); + Vector d = (Vector(2) << -0.1, 0.25)/sig, sigma = ones(2); GaussianConditional expected(ordering[L(1)],d,R11,ordering[X(1)],S12,ordering[X(2)],S13,sigma); EXPECT(assert_equal(expected,*actual,tol)); @@ -190,15 +191,15 @@ TEST( GaussianFactorGraph, eliminateAll ) Ordering ordering; ordering += X(2),L(1),X(1); - Vector d1 = Vector_(2, -0.1,-0.1); + Vector d1 = (Vector(2) << -0.1,-0.1); GaussianBayesNet expected = simpleGaussian(ordering[X(1)],d1,0.1); double sig1 = 0.149071; - Vector d2 = Vector_(2, 0.0, 0.2)/sig1, sigma2 = ones(2); + Vector d2 = (Vector(2) << 0.0, 0.2)/sig1, sigma2 = ones(2); push_front(expected,ordering[L(1)],d2, I/sig1,ordering[X(1)], (-1)*I/sig1,sigma2); double sig2 = 0.0894427; - Vector d3 = Vector_(2, 0.2, -0.14)/sig2, sigma3 = ones(2); + Vector d3 = (Vector(2) << 0.2, -0.14)/sig2, sigma3 = ones(2); push_front(expected,ordering[X(2)],d3, I/sig2,ordering[L(1)], (-0.2)*I/sig2, ordering[X(1)], (-0.8)*I/sig2, sigma3); // Check one ordering @@ -373,10 +374,10 @@ TEST( GaussianFactorGraph, multiplication ) VectorValues x = createCorrectDelta(ord); Errors actual = A * x; Errors expected; - expected += Vector_(2,-1.0,-1.0); - expected += Vector_(2, 2.0,-1.0); - expected += Vector_(2, 0.0, 1.0); - expected += Vector_(2,-1.0, 1.5); + expected += (Vector(2) << -1.0,-1.0); + expected += (Vector(2) << 2.0,-1.0); + expected += (Vector(2) << 0.0, 1.0); + expected += (Vector(2) << -1.0, 1.5); EXPECT(assert_equal(expected,actual)); } @@ -389,11 +390,11 @@ TEST( GaussianFactorGraph, elimination ) // Create Gaussian Factor Graph GaussianFactorGraph fg; Matrix Ap = eye(1), An = eye(1) * -1; - Vector b = Vector_(1, 0.0); + Vector b = (Vector(1) << 0.0); SharedDiagonal sigma = noiseModel::Isotropic::Sigma(1,2.0); - fg.add(ord[X(1)], An, ord[X(2)], Ap, b, sigma); - fg.add(ord[X(1)], Ap, b, sigma); - fg.add(ord[X(2)], Ap, b, sigma); + fg += ord[X(1)], An, ord[X(2)], Ap, b, sigma; + fg += ord[X(1)], Ap, b, sigma; + fg += ord[X(2)], Ap, b, sigma; // Eliminate GaussianBayesNet bayesNet = *GaussianSequentialSolver(fg).eliminate(); @@ -404,10 +405,10 @@ TEST( GaussianFactorGraph, elimination ) // Check matrix Matrix R;Vector d; boost::tie(R,d) = matrix(bayesNet); - Matrix expected = Matrix_(2,2, + Matrix expected = (Matrix(2, 2) << 0.707107, -0.353553, 0.0, 0.612372); - Matrix expected2 = Matrix_(2,2, + Matrix expected2 = (Matrix(2, 2) << 0.707107, -0.353553, 0.0, -0.612372); EXPECT(equal_with_abs_tol(expected, R, 1e-6) || equal_with_abs_tol(expected2, R, 1e-6)); @@ -513,6 +514,8 @@ TEST(GaussianFactorGraph, createSmoother2) CHECK(assert_equal(*p_x1.back(),*p_x3.front())); // should be the same because of symmetry } +#endif + /* ************************************************************************* */ TEST(GaussianFactorGraph, hasConstraints) { @@ -522,8 +525,7 @@ TEST(GaussianFactorGraph, hasConstraints) FactorGraph fgc2 = createSimpleConstraintGraph() ; EXPECT(hasConstraints(fgc2)); - Ordering ordering; ordering += X(1), X(2), L(1); - GaussianFactorGraph fg = createGaussianFactorGraph(ordering); + GaussianFactorGraph fg = createGaussianFactorGraph(); EXPECT(!hasConstraints(fg)); } @@ -531,7 +533,6 @@ TEST(GaussianFactorGraph, hasConstraints) #include #include #include -#include /* ************************************************************************* */ TEST( GaussianFactorGraph, conditional_sigma_failure) { @@ -543,13 +544,13 @@ TEST( GaussianFactorGraph, conditional_sigma_failure) { gtsam::Key xC1 = 0, l32 = 1, l41 = 2; // noisemodels at nonlinear level - gtsam::SharedNoiseModel priorModel = noiseModel::Diagonal::Sigmas(Vector_(6, 0.05, 0.05, 3.0, 0.2, 0.2, 0.2)); + gtsam::SharedNoiseModel priorModel = noiseModel::Diagonal::Sigmas((Vector(6) << 0.05, 0.05, 3.0, 0.2, 0.2, 0.2)); gtsam::SharedNoiseModel measModel = noiseModel::Unit::Create(2); gtsam::SharedNoiseModel elevationModel = noiseModel::Isotropic::Sigma(1, 3.0); double fov = 60; // degrees - double imgW = 640; // pixels - double imgH = 480; // pixels + int imgW = 640; // pixels + int imgH = 480; // pixels gtsam::Cal3_S2::shared_ptr K(new gtsam::Cal3_S2(fov, imgW, imgH)); typedef GenericProjectionFactor ProjectionFactor; @@ -567,30 +568,28 @@ TEST( GaussianFactorGraph, conditional_sigma_failure) { initValues.insert(l41, Point3(1.61051523, 6.7373052, -0.231582751) ); NonlinearFactorGraph factors; - factors.add(PriorFactor(xC1, + factors += PriorFactor(xC1, Pose3(Rot3( -1., 0.0, 1.2246468e-16, 0.0, 1., 0.0, -1.2246468e-16, 0.0, -1), - Point3(0.511832102, 8.42819594, 5.76841725)), priorModel)); - factors.add(ProjectionFactor(Point2(333.648615, 98.61535), measModel, xC1, l32, K)); - factors.add(ProjectionFactor(Point2(218.508, 83.8022039), measModel, xC1, l41, K)); - factors.add(RangeFactor(xC1, l32, relElevation, elevationModel)); - factors.add(RangeFactor(xC1, l41, relElevation, elevationModel)); - - Ordering orderingC; orderingC += xC1, l32, l41; + Point3(0.511832102, 8.42819594, 5.76841725)), priorModel); + factors += ProjectionFactor(Point2(333.648615, 98.61535), measModel, xC1, l32, K); + factors += ProjectionFactor(Point2(218.508, 83.8022039), measModel, xC1, l41, K); + factors += RangeFactor(xC1, l32, relElevation, elevationModel); + factors += RangeFactor(xC1, l41, relElevation, elevationModel); // Check that sigmas are correct (i.e., unit) - GaussianFactorGraph lfg = *factors.linearize(initValues, orderingC); + GaussianFactorGraph lfg = *factors.linearize(initValues); - GaussianMultifrontalSolver solver(lfg, false); - GaussianBayesTree actBT = *solver.eliminate(); + GaussianBayesTree actBT = *lfg.eliminateMultifrontal(); // Check that all sigmas in an unconstrained bayes tree are set to one - BOOST_FOREACH(const GaussianBayesTree::sharedClique& clique, actBT.nodes()) { + BOOST_FOREACH(const GaussianBayesTree::sharedClique& clique, actBT.nodes() | br::map_values) { GaussianConditional::shared_ptr conditional = clique->conditional(); - size_t dim = conditional->dim(); - EXPECT(assert_equal(gtsam::ones(dim), conditional->get_sigmas(), tol)); + //size_t dim = conditional->rows(); + //EXPECT(assert_equal(gtsam::ones(dim), conditional->get_model()->sigmas(), tol)); + EXPECT(!conditional->get_model()); } } diff --git a/tests/testGaussianISAM.cpp b/tests/testGaussianISAM.cpp index 60d5db672..7f53406c8 100644 --- a/tests/testGaussianISAM.cpp +++ b/tests/testGaussianISAM.cpp @@ -15,21 +15,18 @@ * @author Michael Kaess */ -#include -#include -#include -#include -#include -#include -#include -#include -#include - #include +#include +#include +#include +#include + #include #include // for operator += using namespace boost::assign; +#include +namespace br { using namespace boost::adaptors; using namespace boost::range; } using namespace std; using namespace gtsam; @@ -38,11 +35,6 @@ using namespace example; using symbol_shorthand::X; using symbol_shorthand::L; -/* ************************************************************************* */ -// Some numbers that should be consistent among all smoother tests - -static const double tol = 1e-4; - /* ************************************************************************* */ TEST( ISAM, iSAM_smoother ) { @@ -50,7 +42,7 @@ TEST( ISAM, iSAM_smoother ) for (int t = 1; t <= 7; t++) ordering += X(t); // Create smoother with 7 nodes - GaussianFactorGraph smoother = createSmoother(7, ordering).first; + GaussianFactorGraph smoother = createSmoother(7); // run iSAM for every factor GaussianISAM actual; @@ -61,22 +53,21 @@ TEST( ISAM, iSAM_smoother ) } // Create expected Bayes Tree by solving smoother with "natural" ordering - BayesTree::shared_ptr bayesTree = GaussianMultifrontalSolver(smoother).eliminate(); - GaussianISAM expected(*bayesTree); + GaussianBayesTree expected = *smoother.eliminateMultifrontal(ordering); // Verify sigmas in the bayes tree - BOOST_FOREACH(const GaussianBayesTree::sharedClique& clique, bayesTree->nodes()) { + BOOST_FOREACH(const GaussianBayesTree::sharedClique& clique, expected.nodes() | br::map_values) { GaussianConditional::shared_ptr conditional = clique->conditional(); - size_t dim = conditional->dim(); - EXPECT(assert_equal(gtsam::ones(dim), conditional->get_sigmas(), tol)); + EXPECT(!conditional->get_model()); } // Check whether BayesTree is correct - EXPECT(assert_equal(expected, actual)); + EXPECT(assert_equal(GaussianFactorGraph(expected).augmentedHessian(), GaussianFactorGraph(actual).augmentedHessian())); // obtain solution - VectorValues e(VectorValues::Zero(7,2)); // expected solution - VectorValues optimized = optimize(actual); // actual solution + VectorValues e; // expected solution + for (int t = 1; t <= 7; t++) e.insert(X(t), Vector::Zero(2)); + VectorValues optimized = actual.optimize(); // actual solution EXPECT(assert_equal(e, optimized)); } diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index aa169c3a0..55329d8e9 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -1,1045 +1,853 @@ -/** - * @file testGaussianISAM2.cpp - * @brief Unit tests for GaussianISAM2 - * @author Michael Kaess - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include -#include // for operator += -#include -using namespace boost::assign; - -using namespace std; -using namespace gtsam; -using boost::shared_ptr; - -const double tol = 1e-4; - -// SETDEBUG("ISAM2 update", true); -// SETDEBUG("ISAM2 update verbose", true); -// SETDEBUG("ISAM2 recalculate", true); - -// Set up parameters -SharedDiagonal odoNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.1, 0.1, M_PI/100.0)); -SharedDiagonal brNoise = noiseModel::Diagonal::Sigmas(Vector_(2, M_PI/100.0, 0.1)); - -ISAM2 createSlamlikeISAM2( - boost::optional init_values = boost::none, - boost::optional full_graph = boost::none, - const ISAM2Params& params = ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, true)) { - - // These variables will be reused and accumulate factors and values - ISAM2 isam(params); - Values fullinit; - NonlinearFactorGraph fullgraph; - - // i keeps track of the time step - size_t i = 0; - - // Add a prior at time 0 and update isam - { - NonlinearFactorGraph newfactors; - newfactors.add(PriorFactor(0, Pose2(0.0, 0.0, 0.0), odoNoise)); - fullgraph.push_back(newfactors); - - Values init; - init.insert((0), Pose2(0.01, 0.01, 0.01)); - fullinit.insert((0), Pose2(0.01, 0.01, 0.01)); - - isam.update(newfactors, init); - } - - // Add odometry from time 0 to time 5 - for( ; i<5; ++i) { - NonlinearFactorGraph newfactors; - newfactors.add(BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise)); - fullgraph.push_back(newfactors); - - Values init; - init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - - isam.update(newfactors, init); - } - - // Add odometry from time 5 to 6 and landmark measurement at time 5 - { - NonlinearFactorGraph newfactors; - newfactors.add(BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise)); - newfactors.add(BearingRangeFactor(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise)); - newfactors.add(BearingRangeFactor(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise)); - fullgraph.push_back(newfactors); - - Values init; - init.insert((i+1), Pose2(1.01, 0.01, 0.01)); - init.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); - init.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); - fullinit.insert((i+1), Pose2(1.01, 0.01, 0.01)); - fullinit.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); - fullinit.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); - - isam.update(newfactors, init); - ++ i; - } - - // Add odometry from time 6 to time 10 - for( ; i<10; ++i) { - NonlinearFactorGraph newfactors; - newfactors.add(BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise)); - fullgraph.push_back(newfactors); - - Values init; - init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - - isam.update(newfactors, init); - } - - // Add odometry from time 10 to 11 and landmark measurement at time 10 - { - NonlinearFactorGraph newfactors; - newfactors.add(BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise)); - newfactors.add(BearingRangeFactor(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise)); - newfactors.add(BearingRangeFactor(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise)); - fullgraph.push_back(newfactors); - - Values init; - init.insert((i+1), Pose2(6.9, 0.1, 0.01)); - fullinit.insert((i+1), Pose2(6.9, 0.1, 0.01)); - - isam.update(newfactors, init); - ++ i; - } - - if (full_graph) - *full_graph = fullgraph; - - if (init_values) - *init_values = fullinit; - - return isam; -} - -/* ************************************************************************* */ -TEST_UNSAFE(ISAM2, ImplAddVariables) { - - // Create initial state - Values theta; - theta.insert(0, Pose2(.1, .2, .3)); - theta.insert(100, Point2(.4, .5)); - Values newTheta; - newTheta.insert(1, Pose2(.6, .7, .8)); - - VectorValues delta; - delta.insert(0, Vector_(3, .1, .2, .3)); - delta.insert(1, Vector_(2, .4, .5)); - - VectorValues deltaNewton; - deltaNewton.insert(0, Vector_(3, .1, .2, .3)); - deltaNewton.insert(1, Vector_(2, .4, .5)); - - VectorValues deltaRg; - deltaRg.insert(0, Vector_(3, .1, .2, .3)); - deltaRg.insert(1, Vector_(2, .4, .5)); - - vector replacedKeys(2, false); - - Ordering ordering; ordering += 100, 0; - - // Verify initial state - LONGS_EQUAL(0, ordering[100]); - LONGS_EQUAL(1, ordering[0]); - EXPECT(assert_equal(delta[0], delta[ordering[100]])); - EXPECT(assert_equal(delta[1], delta[ordering[0]])); - - // Create expected state - Values thetaExpected; - thetaExpected.insert(0, Pose2(.1, .2, .3)); - thetaExpected.insert(100, Point2(.4, .5)); - thetaExpected.insert(1, Pose2(.6, .7, .8)); - - VectorValues deltaExpected; - deltaExpected.insert(0, Vector_(3, .1, .2, .3)); - deltaExpected.insert(1, Vector_(2, .4, .5)); - deltaExpected.insert(2, Vector_(3, 0.0, 0.0, 0.0)); - - VectorValues deltaNewtonExpected; - deltaNewtonExpected.insert(0, Vector_(3, .1, .2, .3)); - deltaNewtonExpected.insert(1, Vector_(2, .4, .5)); - deltaNewtonExpected.insert(2, Vector_(3, 0.0, 0.0, 0.0)); - - VectorValues deltaRgExpected; - deltaRgExpected.insert(0, Vector_(3, .1, .2, .3)); - deltaRgExpected.insert(1, Vector_(2, .4, .5)); - deltaRgExpected.insert(2, Vector_(3, 0.0, 0.0, 0.0)); - - vector replacedKeysExpected(3, false); - - Ordering orderingExpected; orderingExpected += 100, 0, 1; - - // Expand initial state - ISAM2::Impl::AddVariables(newTheta, theta, delta, deltaNewton, deltaRg, replacedKeys, ordering); - - EXPECT(assert_equal(thetaExpected, theta)); - EXPECT(assert_equal(deltaExpected, delta)); - EXPECT(assert_equal(deltaNewtonExpected, deltaNewton)); - EXPECT(assert_equal(deltaRgExpected, deltaRg)); - EXPECT(assert_container_equality(replacedKeysExpected, replacedKeys)); - EXPECT(assert_equal(orderingExpected, ordering)); -} - -/* ************************************************************************* */ -TEST_UNSAFE(ISAM2, ImplRemoveVariables) { - - // Create initial state - Values theta; - theta.insert(0, Pose2(.1, .2, .3)); - theta.insert(1, Pose2(.6, .7, .8)); - theta.insert(100, Point2(.4, .5)); - - SymbolicFactorGraph sfg; - sfg.push_back(boost::make_shared(Index(0), Index(2))); - sfg.push_back(boost::make_shared(Index(0), Index(1))); - VariableIndex variableIndex(sfg); - - VectorValues delta; - delta.insert(0, Vector_(3, .1, .2, .3)); - delta.insert(1, Vector_(3, .4, .5, .6)); - delta.insert(2, Vector_(2, .7, .8)); - - VectorValues deltaNewton; - deltaNewton.insert(0, Vector_(3, .1, .2, .3)); - deltaNewton.insert(1, Vector_(3, .4, .5, .6)); - deltaNewton.insert(2, Vector_(2, .7, .8)); - - VectorValues deltaRg; - deltaRg.insert(0, Vector_(3, .1, .2, .3)); - deltaRg.insert(1, Vector_(3, .4, .5, .6)); - deltaRg.insert(2, Vector_(2, .7, .8)); - - vector replacedKeys(3, false); - replacedKeys[0] = true; - replacedKeys[1] = false; - replacedKeys[2] = true; - - Ordering ordering; ordering += 100, 1, 0; - - ISAM2::Nodes nodes(3); - - // Verify initial state - LONGS_EQUAL(0, ordering[100]); - LONGS_EQUAL(1, ordering[1]); - LONGS_EQUAL(2, ordering[0]); - - // Create expected state - Values thetaExpected; - thetaExpected.insert(0, Pose2(.1, .2, .3)); - thetaExpected.insert(100, Point2(.4, .5)); - - SymbolicFactorGraph sfgRemoved; - sfgRemoved.push_back(boost::make_shared(Index(0), Index(1))); - sfgRemoved.push_back(SymbolicFactorGraph::sharedFactor()); // Add empty factor to keep factor indices consistent - VariableIndex variableIndexExpected(sfgRemoved); - - VectorValues deltaExpected; - deltaExpected.insert(0, Vector_(3, .1, .2, .3)); - deltaExpected.insert(1, Vector_(2, .7, .8)); - - VectorValues deltaNewtonExpected; - deltaNewtonExpected.insert(0, Vector_(3, .1, .2, .3)); - deltaNewtonExpected.insert(1, Vector_(2, .7, .8)); - - VectorValues deltaRgExpected; - deltaRgExpected.insert(0, Vector_(3, .1, .2, .3)); - deltaRgExpected.insert(1, Vector_(2, .7, .8)); - - vector replacedKeysExpected(2, true); - - Ordering orderingExpected; orderingExpected += 100, 0; - - ISAM2::Nodes nodesExpected(2); - - // Reduce initial state - FastSet unusedKeys; - unusedKeys.insert(1); - vector removedFactorsI; removedFactorsI.push_back(1); - SymbolicFactorGraph removedFactors; removedFactors.push_back(sfg[1]); - variableIndex.remove(removedFactorsI, removedFactors); - GaussianFactorGraph linearFactors; - FastSet fixedVariables; - ISAM2::Impl::RemoveVariables(unusedKeys, ISAM2::sharedClique(), theta, variableIndex, delta, deltaNewton, deltaRg, - replacedKeys, ordering, nodes, linearFactors, fixedVariables); - - EXPECT(assert_equal(thetaExpected, theta)); - EXPECT(assert_equal(variableIndexExpected, variableIndex)); - EXPECT(assert_equal(deltaExpected, delta)); - EXPECT(assert_equal(deltaNewtonExpected, deltaNewton)); - EXPECT(assert_equal(deltaRgExpected, deltaRg)); - EXPECT(assert_container_equality(replacedKeysExpected, replacedKeys)); - EXPECT(assert_equal(orderingExpected, ordering)); -} - -/* ************************************************************************* */ -//TEST(ISAM2, IndicesFromFactors) { -// -// using namespace gtsam::planarSLAM; -// typedef GaussianISAM2::Impl Impl; -// -// Ordering ordering; ordering += (0), (0), (1); -// NonlinearFactorGraph graph; -// graph.add(PriorFactor((0), Pose2(), noiseModel::Unit::Create(Pose2::dimension)); -// graph.addRange((0), (0), 1.0, noiseModel::Unit::Create(1)); -// -// FastSet expected; -// expected.insert(0); -// expected.insert(1); -// -// FastSet actual = Impl::IndicesFromFactors(ordering, graph); -// -// EXPECT(assert_equal(expected, actual)); -//} - -/* ************************************************************************* */ -//TEST(ISAM2, CheckRelinearization) { -// -// typedef GaussianISAM2::Impl Impl; -// -// // Create values where indices 1 and 3 are above the threshold of 0.1 -// VectorValues values; -// values.reserve(4, 10); -// values.push_back_preallocated(Vector_(2, 0.09, 0.09)); -// values.push_back_preallocated(Vector_(3, 0.11, 0.11, 0.09)); -// values.push_back_preallocated(Vector_(3, 0.09, 0.09, 0.09)); -// values.push_back_preallocated(Vector_(2, 0.11, 0.11)); -// -// // Create a permutation -// Permutation permutation(4); -// permutation[0] = 2; -// permutation[1] = 0; -// permutation[2] = 1; -// permutation[3] = 3; -// -// Permuted permuted(permutation, values); -// -// // After permutation, the indices above the threshold are 2 and 2 -// FastSet expected; -// expected.insert(2); -// expected.insert(3); -// -// // Indices checked by CheckRelinearization -// FastSet actual = Impl::CheckRelinearization(permuted, 0.1); -// -// EXPECT(assert_equal(expected, actual)); -//} - -/* ************************************************************************* */ -TEST(ISAM2, optimize2) { - - // Create initialization - Values theta; - theta.insert((0), Pose2(0.01, 0.01, 0.01)); - - // Create conditional - Vector d(3); d << -0.1, -0.1, -0.31831; - Matrix R(3,3); R << - 10, 0.0, 0.0, - 0.0, 10, 0.0, - 0.0, 0.0, 31.8309886; - GaussianConditional::shared_ptr conditional(new GaussianConditional(0, d, R, Vector::Ones(3))); - - // Create ordering - Ordering ordering; ordering += (0); - - // Expected vector - VectorValues expected(1, 3); - conditional->solveInPlace(expected); - - // Clique - ISAM2::sharedClique clique( - ISAM2::Clique::Create(make_pair(conditional,GaussianFactor::shared_ptr()))); - VectorValues actual(theta.dims(ordering)); - internal::optimizeInPlace(clique, actual); - -// expected.print("expected: "); -// actual.print("actual: "); - EXPECT(assert_equal(expected, actual)); -} - -/* ************************************************************************* */ -bool isam_check(const NonlinearFactorGraph& fullgraph, const Values& fullinit, const ISAM2& isam, Test& test, TestResult& result) { - - TestResult& result_ = result; - const std::string name_ = test.getName(); - - Values actual = isam.calculateEstimate(); - Ordering ordering = isam.getOrdering(); // *fullgraph.orderingCOLAMD(fullinit).first; - GaussianFactorGraph linearized = *fullgraph.linearize(fullinit, ordering); -// linearized.print("Expected linearized: "); - GaussianBayesNet gbn = *GaussianSequentialSolver(linearized).eliminate(); -// gbn.print("Expected bayesnet: "); - VectorValues delta = optimize(gbn); - Values expected = fullinit.retract(delta, ordering); - - bool isamEqual = assert_equal(expected, actual); - - // The following two checks make sure that the cached gradients are maintained and used correctly - - // Check gradient at each node - bool nodeGradientsOk = true; - typedef ISAM2::sharedClique sharedClique; - BOOST_FOREACH(const sharedClique& clique, isam.nodes()) { - // Compute expected gradient - FactorGraph jfg; - jfg.push_back(JacobianFactor::shared_ptr(new JacobianFactor(*clique->conditional()))); - VectorValues expectedGradient(*allocateVectorValues(isam)); - gradientAtZero(jfg, expectedGradient); - // Compare with actual gradients - int variablePosition = 0; - for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) { - const int dim = clique->conditional()->dim(jit); - Vector actual = clique->gradientContribution().segment(variablePosition, dim); - bool gradOk = assert_equal(expectedGradient[*jit], actual); - EXPECT(gradOk); - nodeGradientsOk = nodeGradientsOk && gradOk; - variablePosition += dim; - } - bool dimOk = clique->gradientContribution().rows() == variablePosition; - EXPECT(dimOk); - nodeGradientsOk = nodeGradientsOk && dimOk; - } - - // Check gradient - VectorValues expectedGradient(*allocateVectorValues(isam)); - gradientAtZero(FactorGraph(isam), expectedGradient); - VectorValues expectedGradient2(gradient(FactorGraph(isam), VectorValues::Zero(expectedGradient))); - VectorValues actualGradient(*allocateVectorValues(isam)); - gradientAtZero(isam, actualGradient); - bool expectedGradOk = assert_equal(expectedGradient2, expectedGradient); - EXPECT(expectedGradOk); - bool totalGradOk = assert_equal(expectedGradient, actualGradient); - EXPECT(totalGradOk); - - return nodeGradientsOk && expectedGradOk && totalGradOk && isamEqual; -} - -/* ************************************************************************* */ -TEST(ISAM2, slamlike_solution_gaussnewton) -{ - // These variables will be reused and accumulate factors and values - Values fullinit; - NonlinearFactorGraph fullgraph; - ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); - - // Compare solutions - CHECK(isam_check(fullgraph, fullinit, isam, *this, result_)); -} - -/* ************************************************************************* */ -TEST(ISAM2, slamlike_solution_dogleg) -{ - // These variables will be reused and accumulate factors and values - Values fullinit; - NonlinearFactorGraph fullgraph; - ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false)); - - // Compare solutions - CHECK(isam_check(fullgraph, fullinit, isam, *this, result_)); -} - -/* ************************************************************************* */ -TEST(ISAM2, slamlike_solution_gaussnewton_qr) -{ - // These variables will be reused and accumulate factors and values - Values fullinit; - NonlinearFactorGraph fullgraph; - ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, false, ISAM2Params::QR)); - - // Compare solutions - CHECK(isam_check(fullgraph, fullinit, isam, *this, result_)); -} - -/* ************************************************************************* */ -TEST(ISAM2, slamlike_solution_dogleg_qr) -{ - // These variables will be reused and accumulate factors and values - Values fullinit; - NonlinearFactorGraph fullgraph; - ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false, false, ISAM2Params::QR)); - - // Compare solutions - CHECK(isam_check(fullgraph, fullinit, isam, *this, result_)); -} - -/* ************************************************************************* */ -TEST(ISAM2, clone) { - - ISAM2 clone1; - - { - ISAM2 isam = createSlamlikeISAM2(); - clone1 = isam; - - ISAM2 clone2(isam); - - // Modify original isam - NonlinearFactorGraph factors; - factors.add(BetweenFactor(0, 10, - isam.calculateEstimate(0).between(isam.calculateEstimate(10)), noiseModel::Unit::Create(3))); - isam.update(factors); - - CHECK(assert_equal(createSlamlikeISAM2(), clone2)); - } - - // This is to (perhaps unsuccessfully) try to currupt unallocated memory referenced - // if the references in the iSAM2 copy point to the old instance which deleted at - // the end of the {...} section above. - ISAM2 temp = createSlamlikeISAM2(); - - CHECK(assert_equal(createSlamlikeISAM2(), clone1)); - CHECK(assert_equal(clone1, temp)); - - // Check clone empty - ISAM2 isam; - clone1 = isam; - CHECK(assert_equal(ISAM2(), clone1)); -} - -/* ************************************************************************* */ -TEST(ISAM2, permute_cached) { - typedef boost::shared_ptr sharedISAM2Clique; - - // Construct expected permuted BayesTree (variable 2 has been changed to 1) - BayesTree expected; - expected.insert(sharedISAM2Clique(new ISAM2Clique(make_pair( - boost::make_shared(pair_list_of - (3, Matrix_(1,1,1.0)) - (4, Matrix_(1,1,2.0)), - 2, Vector_(1,1.0), Vector_(1,1.0)), // p(3,4) - HessianFactor::shared_ptr())))); // Cached: empty - expected.insert(sharedISAM2Clique(new ISAM2Clique(make_pair( - boost::make_shared(pair_list_of - (2, Matrix_(1,1,1.0)) - (3, Matrix_(1,1,2.0)), - 1, Vector_(1,1.0), Vector_(1,1.0)), // p(2|3) - boost::make_shared(3, Matrix_(1,1,1.0), Vector_(1,1.0), 0.0))))); // Cached: p(3) - expected.insert(sharedISAM2Clique(new ISAM2Clique(make_pair( - boost::make_shared(pair_list_of - (0, Matrix_(1,1,1.0)) - (2, Matrix_(1,1,2.0)), - 1, Vector_(1,1.0), Vector_(1,1.0)), // p(0|2) - boost::make_shared(1, Matrix_(1,1,1.0), Vector_(1,1.0), 0.0))))); // Cached: p(1) - // Change variable 2 to 1 - expected.root()->children().front()->conditional()->keys()[0] = 1; - expected.root()->children().front()->children().front()->conditional()->keys()[1] = 1; - - // Construct unpermuted BayesTree - BayesTree actual; - actual.insert(sharedISAM2Clique(new ISAM2Clique(make_pair( - boost::make_shared(pair_list_of - (3, Matrix_(1,1,1.0)) - (4, Matrix_(1,1,2.0)), - 2, Vector_(1,1.0), Vector_(1,1.0)), // p(3,4) - HessianFactor::shared_ptr())))); // Cached: empty - actual.insert(sharedISAM2Clique(new ISAM2Clique(make_pair( - boost::make_shared(pair_list_of - (2, Matrix_(1,1,1.0)) - (3, Matrix_(1,1,2.0)), - 1, Vector_(1,1.0), Vector_(1,1.0)), // p(2|3) - boost::make_shared(3, Matrix_(1,1,1.0), Vector_(1,1.0), 0.0))))); // Cached: p(3) - actual.insert(sharedISAM2Clique(new ISAM2Clique(make_pair( - boost::make_shared(pair_list_of - (0, Matrix_(1,1,1.0)) - (2, Matrix_(1,1,2.0)), - 1, Vector_(1,1.0), Vector_(1,1.0)), // p(0|2) - boost::make_shared(2, Matrix_(1,1,1.0), Vector_(1,1.0), 0.0))))); // Cached: p(2) - - // Create permutation that changes variable 2 -> 0 - Permutation permutation = Permutation::Identity(5); - permutation[2] = 1; - - // Permute BayesTree - actual.root()->permuteWithInverse(permutation); - - // Check - EXPECT(assert_equal(expected, actual)); -} - -/* ************************************************************************* */ -TEST(ISAM2, removeFactors) -{ - // This test builds a graph in the same way as the "slamlike" test above, but - // then removes the 2nd-to-last landmark measurement - - // These variables will be reused and accumulate factors and values - Values fullinit; - NonlinearFactorGraph fullgraph; - ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); - - // Remove the 2nd measurement on landmark 0 (Key 100) - FastVector toRemove; - toRemove.push_back(12); - isam.update(NonlinearFactorGraph(), Values(), toRemove); - - // Remove the factor from the full system - fullgraph.remove(12); - - // Compare solutions - CHECK(isam_check(fullgraph, fullinit, isam, *this, result_)); -} - -/* ************************************************************************* */ -TEST_UNSAFE(ISAM2, removeVariables) -{ - // These variables will be reused and accumulate factors and values - Values fullinit; - NonlinearFactorGraph fullgraph; - ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); - - // Remove the measurement on landmark 0 (Key 100) - FastVector toRemove; - toRemove.push_back(7); - toRemove.push_back(14); - isam.update(NonlinearFactorGraph(), Values(), toRemove); - - // Remove the factors and variable from the full system - fullgraph.remove(7); - fullgraph.remove(14); - fullinit.erase(100); - - // Compare solutions - CHECK(isam_check(fullgraph, fullinit, isam, *this, result_)); -} - -/* ************************************************************************* */ -TEST_UNSAFE(ISAM2, swapFactors) -{ - // This test builds a graph in the same way as the "slamlike" test above, but - // then swaps the 2nd-to-last landmark measurement with a different one - - Values fullinit; - NonlinearFactorGraph fullgraph; - ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph); - - // Remove the measurement on landmark 0 and replace with a different one - { - size_t swap_idx = isam.getFactorsUnsafe().size()-2; - FastVector toRemove; - toRemove.push_back(swap_idx); - fullgraph.remove(swap_idx); - - NonlinearFactorGraph swapfactors; -// swapfactors.add(BearingRange(10, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); // original factor - swapfactors.add(BearingRangeFactor(10, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 5.0, brNoise)); - fullgraph.push_back(swapfactors); - isam.update(swapfactors, Values(), toRemove); - } - - // Compare solutions - EXPECT(assert_equal(fullgraph, NonlinearFactorGraph(isam.getFactorsUnsafe()))); - EXPECT(isam_check(fullgraph, fullinit, isam, *this, result_)); - - // Check gradient at each node - typedef ISAM2::sharedClique sharedClique; - BOOST_FOREACH(const sharedClique& clique, isam.nodes()) { - // Compute expected gradient - FactorGraph jfg; - jfg.push_back(JacobianFactor::shared_ptr(new JacobianFactor(*clique->conditional()))); - VectorValues expectedGradient(*allocateVectorValues(isam)); - gradientAtZero(jfg, expectedGradient); - // Compare with actual gradients - int variablePosition = 0; - for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) { - const int dim = clique->conditional()->dim(jit); - Vector actual = clique->gradientContribution().segment(variablePosition, dim); - EXPECT(assert_equal(expectedGradient[*jit], actual)); - variablePosition += dim; - } - EXPECT_LONGS_EQUAL(clique->gradientContribution().rows(), variablePosition); - } - - // Check gradient - VectorValues expectedGradient(*allocateVectorValues(isam)); - gradientAtZero(FactorGraph(isam), expectedGradient); - VectorValues expectedGradient2(gradient(FactorGraph(isam), VectorValues::Zero(expectedGradient))); - VectorValues actualGradient(*allocateVectorValues(isam)); - gradientAtZero(isam, actualGradient); - EXPECT(assert_equal(expectedGradient2, expectedGradient)); - EXPECT(assert_equal(expectedGradient, actualGradient)); -} - -/* ************************************************************************* */ -TEST(ISAM2, constrained_ordering) -{ - // These variables will be reused and accumulate factors and values - ISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); - Values fullinit; - NonlinearFactorGraph fullgraph; - - // We will constrain x3 and x4 to the end - FastMap constrained; - constrained.insert(make_pair((3), 1)); - constrained.insert(make_pair((4), 2)); - - // i keeps track of the time step - size_t i = 0; - - // Add a prior at time 0 and update isam - { - NonlinearFactorGraph newfactors; - newfactors.add(PriorFactor(0, Pose2(0.0, 0.0, 0.0), odoNoise)); - fullgraph.push_back(newfactors); - - Values init; - init.insert((0), Pose2(0.01, 0.01, 0.01)); - fullinit.insert((0), Pose2(0.01, 0.01, 0.01)); - - isam.update(newfactors, init); - } - - CHECK(isam_check(fullgraph, fullinit, isam, *this, result_)); - - // Add odometry from time 0 to time 5 - for( ; i<5; ++i) { - NonlinearFactorGraph newfactors; - newfactors.add(BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise)); - fullgraph.push_back(newfactors); - - Values init; - init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - - if(i >= 3) - isam.update(newfactors, init, FastVector(), constrained); - else - isam.update(newfactors, init); - } - - // Add odometry from time 5 to 6 and landmark measurement at time 5 - { - NonlinearFactorGraph newfactors; - newfactors.add(BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise)); - newfactors.add(BearingRangeFactor(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise)); - newfactors.add(BearingRangeFactor(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise)); - fullgraph.push_back(newfactors); - - Values init; - init.insert((i+1), Pose2(1.01, 0.01, 0.01)); - init.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); - init.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); - fullinit.insert((i+1), Pose2(1.01, 0.01, 0.01)); - fullinit.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); - fullinit.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); - - isam.update(newfactors, init, FastVector(), constrained); - ++ i; - } - - // Add odometry from time 6 to time 10 - for( ; i<10; ++i) { - NonlinearFactorGraph newfactors; - newfactors.add(BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise)); - fullgraph.push_back(newfactors); - - Values init; - init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - - isam.update(newfactors, init, FastVector(), constrained); - } - - // Add odometry from time 10 to 11 and landmark measurement at time 10 - { - NonlinearFactorGraph newfactors; - newfactors.add(BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise)); - newfactors.add(BearingRangeFactor(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise)); - newfactors.add(BearingRangeFactor(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise)); - fullgraph.push_back(newfactors); - - Values init; - init.insert((i+1), Pose2(6.9, 0.1, 0.01)); - fullinit.insert((i+1), Pose2(6.9, 0.1, 0.01)); - - isam.update(newfactors, init, FastVector(), constrained); - ++ i; - } - - // Compare solutions - EXPECT(isam_check(fullgraph, fullinit, isam, *this, result_)); - - // Check that x3 and x4 are last, but either can come before the other - EXPECT(isam.getOrdering()[(3)] == 12 && isam.getOrdering()[(4)] == 13); - - // Check gradient at each node - typedef ISAM2::sharedClique sharedClique; - BOOST_FOREACH(const sharedClique& clique, isam.nodes()) { - // Compute expected gradient - FactorGraph jfg; - jfg.push_back(JacobianFactor::shared_ptr(new JacobianFactor(*clique->conditional()))); - VectorValues expectedGradient(*allocateVectorValues(isam)); - gradientAtZero(jfg, expectedGradient); - // Compare with actual gradients - int variablePosition = 0; - for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) { - const int dim = clique->conditional()->dim(jit); - Vector actual = clique->gradientContribution().segment(variablePosition, dim); - EXPECT(assert_equal(expectedGradient[*jit], actual)); - variablePosition += dim; - } - LONGS_EQUAL(clique->gradientContribution().rows(), variablePosition); - } - - // Check gradient - VectorValues expectedGradient(*allocateVectorValues(isam)); - gradientAtZero(FactorGraph(isam), expectedGradient); - VectorValues expectedGradient2(gradient(FactorGraph(isam), VectorValues::Zero(expectedGradient))); - VectorValues actualGradient(*allocateVectorValues(isam)); - gradientAtZero(isam, actualGradient); - EXPECT(assert_equal(expectedGradient2, expectedGradient)); - EXPECT(assert_equal(expectedGradient, actualGradient)); -} - -/* ************************************************************************* */ -TEST(ISAM2, slamlike_solution_partial_relinearization_check) -{ - // These variables will be reused and accumulate factors and values - Values fullinit; - NonlinearFactorGraph fullgraph; - ISAM2Params params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false); - params.enablePartialRelinearizationCheck = true; - ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, params); - - // Compare solutions - CHECK(isam_check(fullgraph, fullinit, isam, *this, result_)); -} - -namespace { - bool checkMarginalizeLeaves(ISAM2& isam, const FastList& leafKeys) { - Matrix expectedAugmentedHessian, expected3AugmentedHessian; - vector toKeep; - const Index lastVar = isam.getOrdering().size() - 1; - for(Index i=0; i<=lastVar; ++i) - if(find(leafKeys.begin(), leafKeys.end(), isam.getOrdering().key(i)) == leafKeys.end()) - toKeep.push_back(i); - - // Calculate expected marginal from iSAM2 tree - GaussianFactorGraph isamAsGraph(isam); - GaussianSequentialSolver solver(isamAsGraph); - GaussianFactorGraph marginalgfg = *solver.jointFactorGraph(toKeep); - expectedAugmentedHessian = marginalgfg.augmentedHessian(); - - //// Calculate expected marginal from cached linear factors - //assert(isam.params().cacheLinearizedFactors); - //GaussianSequentialSolver solver2(isam.linearFactors_, isam.params().factorization == ISAM2Params::QR); - //expected2AugmentedHessian = solver2.jointFactorGraph(toKeep)->augmentedHessian(); - - // Calculate expected marginal from original nonlinear factors - GaussianSequentialSolver solver3( - *isam.getFactorsUnsafe().linearize(isam.getLinearizationPoint(), isam.getOrdering()), - isam.params().factorization == ISAM2Params::QR); - expected3AugmentedHessian = solver3.jointFactorGraph(toKeep)->augmentedHessian(); - - // Do marginalization - isam.marginalizeLeaves(leafKeys); - - // Check - GaussianFactorGraph actualMarginalGraph(isam); - Matrix actualAugmentedHessian = actualMarginalGraph.augmentedHessian(); - //Matrix actual2AugmentedHessian = linearFactors_.augmentedHessian(); - Matrix actual3AugmentedHessian = isam.getFactorsUnsafe().linearize( - isam.getLinearizationPoint(), isam.getOrdering())->augmentedHessian(); - assert(actualAugmentedHessian.unaryExpr(std::ptr_fun(&std::isfinite)).all()); - - // Check full marginalization - //cout << "treeEqual" << endl; - bool treeEqual = assert_equal(expectedAugmentedHessian, actualAugmentedHessian, 1e-6); - //bool linEqual = assert_equal(expected2AugmentedHessian, actualAugmentedHessian, 1e-6); - //cout << "nonlinEqual" << endl; - bool nonlinEqual = assert_equal(expected3AugmentedHessian, actualAugmentedHessian, 1e-6); - //bool linCorrect = assert_equal(expected3AugmentedHessian, expected2AugmentedHessian, 1e-6); - //actual2AugmentedHessian.bottomRightCorner(1,1) = expected3AugmentedHessian.bottomRightCorner(1,1); bool afterLinCorrect = assert_equal(expected3AugmentedHessian, actual2AugmentedHessian, 1e-6); - //cout << "nonlinCorrect" << endl; - actual3AugmentedHessian.bottomRightCorner(1,1) = expected3AugmentedHessian.bottomRightCorner(1,1); bool afterNonlinCorrect = assert_equal(expected3AugmentedHessian, actual3AugmentedHessian, 1e-6); - - bool ok = treeEqual && /*linEqual &&*/ nonlinEqual && /*linCorrect &&*/ /*afterLinCorrect &&*/ afterNonlinCorrect; - return ok; - } -} - -/* ************************************************************************* */ -TEST_UNSAFE(ISAM2, marginalizeLeaves1) -{ - ISAM2 isam; - - NonlinearFactorGraph factors; - factors.add(PriorFactor(0, LieVector(0.0), noiseModel::Unit::Create(1))); - - factors.add(BetweenFactor(0, 1, LieVector(0.0), noiseModel::Unit::Create(1))); - factors.add(BetweenFactor(1, 2, LieVector(0.0), noiseModel::Unit::Create(1))); - factors.add(BetweenFactor(0, 2, LieVector(0.0), noiseModel::Unit::Create(1))); - - Values values; - values.insert(0, LieVector(0.0)); - values.insert(1, LieVector(0.0)); - values.insert(2, LieVector(0.0)); - - FastMap constrainedKeys; - constrainedKeys.insert(make_pair(0,0)); - constrainedKeys.insert(make_pair(1,1)); - constrainedKeys.insert(make_pair(2,2)); - - isam.update(factors, values, FastVector(), constrainedKeys); - - FastList leafKeys; - leafKeys.push_back(isam.getOrdering().key(0)); - EXPECT(checkMarginalizeLeaves(isam, leafKeys)); -} - -/* ************************************************************************* */ -TEST_UNSAFE(ISAM2, marginalizeLeaves2) -{ - ISAM2 isam; - - NonlinearFactorGraph factors; - factors.add(PriorFactor(0, LieVector(0.0), noiseModel::Unit::Create(1))); - - factors.add(BetweenFactor(0, 1, LieVector(0.0), noiseModel::Unit::Create(1))); - factors.add(BetweenFactor(1, 2, LieVector(0.0), noiseModel::Unit::Create(1))); - factors.add(BetweenFactor(0, 2, LieVector(0.0), noiseModel::Unit::Create(1))); - factors.add(BetweenFactor(2, 3, LieVector(0.0), noiseModel::Unit::Create(1))); - - Values values; - values.insert(0, LieVector(0.0)); - values.insert(1, LieVector(0.0)); - values.insert(2, LieVector(0.0)); - values.insert(3, LieVector(0.0)); - - FastMap constrainedKeys; - constrainedKeys.insert(make_pair(0,0)); - constrainedKeys.insert(make_pair(1,1)); - constrainedKeys.insert(make_pair(2,2)); - constrainedKeys.insert(make_pair(3,3)); - - isam.update(factors, values, FastVector(), constrainedKeys); - - FastList leafKeys; - leafKeys.push_back(isam.getOrdering().key(0)); - EXPECT(checkMarginalizeLeaves(isam, leafKeys)); -} - -/* ************************************************************************* */ -TEST_UNSAFE(ISAM2, marginalizeLeaves3) -{ - ISAM2 isam; - - NonlinearFactorGraph factors; - factors.add(PriorFactor(0, LieVector(0.0), noiseModel::Unit::Create(1))); - - factors.add(BetweenFactor(0, 1, LieVector(0.0), noiseModel::Unit::Create(1))); - factors.add(BetweenFactor(1, 2, LieVector(0.0), noiseModel::Unit::Create(1))); - factors.add(BetweenFactor(0, 2, LieVector(0.0), noiseModel::Unit::Create(1))); - - factors.add(BetweenFactor(2, 3, LieVector(0.0), noiseModel::Unit::Create(1))); - - factors.add(BetweenFactor(3, 4, LieVector(0.0), noiseModel::Unit::Create(1))); - factors.add(BetweenFactor(4, 5, LieVector(0.0), noiseModel::Unit::Create(1))); - factors.add(BetweenFactor(3, 5, LieVector(0.0), noiseModel::Unit::Create(1))); - - Values values; - values.insert(0, LieVector(0.0)); - values.insert(1, LieVector(0.0)); - values.insert(2, LieVector(0.0)); - values.insert(3, LieVector(0.0)); - values.insert(4, LieVector(0.0)); - values.insert(5, LieVector(0.0)); - - FastMap constrainedKeys; - constrainedKeys.insert(make_pair(0,0)); - constrainedKeys.insert(make_pair(1,1)); - constrainedKeys.insert(make_pair(2,2)); - constrainedKeys.insert(make_pair(3,3)); - constrainedKeys.insert(make_pair(4,4)); - constrainedKeys.insert(make_pair(5,5)); - - isam.update(factors, values, FastVector(), constrainedKeys); - - FastList leafKeys; - leafKeys.push_back(isam.getOrdering().key(0)); - EXPECT(checkMarginalizeLeaves(isam, leafKeys)); -} - -/* ************************************************************************* */ -TEST_UNSAFE(ISAM2, marginalizeLeaves4) -{ - ISAM2 isam; - - NonlinearFactorGraph factors; - factors.add(PriorFactor(0, LieVector(0.0), noiseModel::Unit::Create(1))); - factors.add(BetweenFactor(0, 2, LieVector(0.0), noiseModel::Unit::Create(1))); - factors.add(BetweenFactor(1, 2, LieVector(0.0), noiseModel::Unit::Create(1))); - - Values values; - values.insert(0, LieVector(0.0)); - values.insert(1, LieVector(0.0)); - values.insert(2, LieVector(0.0)); - - FastMap constrainedKeys; - constrainedKeys.insert(make_pair(0,0)); - constrainedKeys.insert(make_pair(1,1)); - constrainedKeys.insert(make_pair(2,2)); - - isam.update(factors, values, FastVector(), constrainedKeys); - - FastList leafKeys; - leafKeys.push_back(isam.getOrdering().key(1)); - EXPECT(checkMarginalizeLeaves(isam, leafKeys)); -} - -/* ************************************************************************* */ -TEST_UNSAFE(ISAM2, marginalizeLeaves5) -{ - // Create isam2 - ISAM2 isam = createSlamlikeISAM2(); - - // Marginalize - FastList marginalizeKeys; - marginalizeKeys.push_back(isam.getOrdering().key(0)); - EXPECT(checkMarginalizeLeaves(isam, marginalizeKeys)); -} - -/* ************************************************************************* */ -TEST(ISAM2, marginalCovariance) -{ - // Create isam2 - ISAM2 isam = createSlamlikeISAM2(); - - // Check marginal - Matrix expected = Marginals(isam.getFactorsUnsafe(), isam.getLinearizationPoint()).marginalCovariance(5); - Matrix actual = isam.marginalCovariance(5); - EXPECT(assert_equal(expected, actual)); -} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr);} -/* ************************************************************************* */ +/** + * @file testGaussianISAM2.cpp + * @brief Unit tests for GaussianISAM2 + * @author Michael Kaess + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +using namespace boost::assign; +#include +namespace br { using namespace boost::adaptors; using namespace boost::range; } + +using namespace std; +using namespace gtsam; +using boost::shared_ptr; + +// SETDEBUG("ISAM2 update", true); +// SETDEBUG("ISAM2 update verbose", true); +// SETDEBUG("ISAM2 recalculate", true); + +// Set up parameters +SharedDiagonal odoNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.1, 0.1, M_PI/100.0)); +SharedDiagonal brNoise = noiseModel::Diagonal::Sigmas((Vector(2) << M_PI/100.0, 0.1)); + +ISAM2 createSlamlikeISAM2( + boost::optional init_values = boost::none, + boost::optional full_graph = boost::none, + const ISAM2Params& params = ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, true), + size_t maxPoses = 10) { + + // These variables will be reused and accumulate factors and values + ISAM2 isam(params); + Values fullinit; + NonlinearFactorGraph fullgraph; + + // i keeps track of the time step + size_t i = 0; + + // Add a prior at time 0 and update isam + { + NonlinearFactorGraph newfactors; + newfactors += PriorFactor(0, Pose2(0.0, 0.0, 0.0), odoNoise); + fullgraph.push_back(newfactors); + + Values init; + init.insert((0), Pose2(0.01, 0.01, 0.01)); + fullinit.insert((0), Pose2(0.01, 0.01, 0.01)); + + isam.update(newfactors, init); + } + + if(i > maxPoses) + goto done; + + // Add odometry from time 0 to time 5 + for( ; i<5; ++i) { + NonlinearFactorGraph newfactors; + newfactors += BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + fullgraph.push_back(newfactors); + + Values init; + init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); + fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); + + isam.update(newfactors, init); + + if(i > maxPoses) + goto done; + } + + if(i > maxPoses) + goto done; + + // Add odometry from time 5 to 6 and landmark measurement at time 5 + { + NonlinearFactorGraph newfactors; + newfactors += BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors += BearingRangeFactor(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise); + newfactors += BearingRangeFactor(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); + fullgraph.push_back(newfactors); + + Values init; + init.insert((i+1), Pose2(1.01, 0.01, 0.01)); + init.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); + init.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); + fullinit.insert((i+1), Pose2(1.01, 0.01, 0.01)); + fullinit.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); + fullinit.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); + + isam.update(newfactors, init); + ++ i; + } + + if(i > maxPoses) + goto done; + + // Add odometry from time 6 to time 10 + for( ; i<10; ++i) { + NonlinearFactorGraph newfactors; + newfactors += BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + fullgraph.push_back(newfactors); + + Values init; + init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); + fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); + + isam.update(newfactors, init); + + if(i > maxPoses) + goto done; + } + + if(i > maxPoses) + goto done; + + // Add odometry from time 10 to 11 and landmark measurement at time 10 + { + NonlinearFactorGraph newfactors; + newfactors += BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors += BearingRangeFactor(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); + newfactors += BearingRangeFactor(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise); + fullgraph.push_back(newfactors); + + Values init; + init.insert((i+1), Pose2(6.9, 0.1, 0.01)); + fullinit.insert((i+1), Pose2(6.9, 0.1, 0.01)); + + isam.update(newfactors, init); + ++ i; + } + +done: + + if (full_graph) + *full_graph = fullgraph; + + if (init_values) + *init_values = fullinit; + + return isam; +} + +/* ************************************************************************* */ +//TEST(ISAM2, CheckRelinearization) { +// +// typedef GaussianISAM2::Impl Impl; +// +// // Create values where indices 1 and 3 are above the threshold of 0.1 +// VectorValues values; +// values.reserve(4, 10); +// values.push_back_preallocated((Vector(2) << 0.09, 0.09)); +// values.push_back_preallocated((Vector(3) << 0.11, 0.11, 0.09)); +// values.push_back_preallocated((Vector(3) << 0.09, 0.09, 0.09)); +// values.push_back_preallocated((Vector(2) << 0.11, 0.11)); +// +// // Create a permutation +// Permutation permutation(4); +// permutation[0] = 2; +// permutation[1] = 0; +// permutation[2] = 1; +// permutation[3] = 3; +// +// Permuted permuted(permutation, values); +// +// // After permutation, the indices above the threshold are 2 and 2 +// FastSet expected; +// expected.insert(2); +// expected.insert(3); +// +// // Indices checked by CheckRelinearization +// FastSet actual = Impl::CheckRelinearization(permuted, 0.1); +// +// EXPECT(assert_equal(expected, actual)); +//} + +/* ************************************************************************* */ +struct ConsistencyVisitor +{ + bool consistent; + const ISAM2& isam; + ConsistencyVisitor(const ISAM2& isam) : + consistent(true), isam(isam) {} + int operator()(const ISAM2::sharedClique& node, int& parentData) + { + if(std::find(isam.roots().begin(), isam.roots().end(), node) == isam.roots().end()) + { + if(node->parent_.expired()) + consistent = false; + if(std::find(node->parent()->children.begin(), node->parent()->children.end(), node) == node->parent()->children.end()) + consistent = false; + } + BOOST_FOREACH(Key j, node->conditional()->frontals()) + { + if(isam.nodes().at(j).get() != node.get()) + consistent = false; + } + return 0; + } +}; + +/* ************************************************************************* */ +bool isam_check(const NonlinearFactorGraph& fullgraph, const Values& fullinit, const ISAM2& isam, Test& test, TestResult& result) { + + TestResult& result_ = result; + const std::string name_ = test.getName(); + + Values actual = isam.calculateEstimate(); + Values expected = fullinit.retract(fullgraph.linearize(fullinit)->optimize()); + + bool isamEqual = assert_equal(expected, actual); + + // Check information + GaussianFactorGraph isamGraph(isam); + isamGraph += isam.roots().front()->cachedFactor_; + Matrix expectedHessian = fullgraph.linearize(isam.getLinearizationPoint())->augmentedHessian(); + Matrix actualHessian = isamGraph.augmentedHessian(); + expectedHessian.bottomRightCorner(1,1) = actualHessian.bottomRightCorner(1,1); + bool isamTreeEqual = assert_equal(expectedHessian, actualHessian); + + // Check consistency + ConsistencyVisitor visitor(isam); + int data; // Unused + treeTraversal::DepthFirstForest(isam, data, visitor); + bool consistent = visitor.consistent; + + // The following two checks make sure that the cached gradients are maintained and used correctly + + // Check gradient at each node + bool nodeGradientsOk = true; + typedef ISAM2::sharedClique sharedClique; + BOOST_FOREACH(const sharedClique& clique, isam.nodes() | br::map_values) { + // Compute expected gradient + GaussianFactorGraph jfg; + jfg += clique->conditional(); + VectorValues expectedGradient = jfg.gradientAtZero(); + // Compare with actual gradients + DenseIndex variablePosition = 0; + for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) { + const DenseIndex dim = clique->conditional()->getDim(jit); + Vector actual = clique->gradientContribution().segment(variablePosition, dim); + bool gradOk = assert_equal(expectedGradient[*jit], actual); + EXPECT(gradOk); + nodeGradientsOk = nodeGradientsOk && gradOk; + variablePosition += dim; + } + bool dimOk = clique->gradientContribution().rows() == variablePosition; + EXPECT(dimOk); + nodeGradientsOk = nodeGradientsOk && dimOk; + } + + // Check gradient + VectorValues expectedGradient = GaussianFactorGraph(isam).gradientAtZero(); + VectorValues expectedGradient2 = GaussianFactorGraph(isam).gradient(VectorValues::Zero(expectedGradient)); + VectorValues actualGradient = isam.gradientAtZero(); + bool expectedGradOk = assert_equal(expectedGradient2, expectedGradient); + EXPECT(expectedGradOk); + bool totalGradOk = assert_equal(expectedGradient, actualGradient); + EXPECT(totalGradOk); + + return nodeGradientsOk && expectedGradOk && totalGradOk && isamEqual && isamTreeEqual && consistent; +} + +/* ************************************************************************* */ +TEST(ISAM2, AddFactorsStep1) +{ + NonlinearFactorGraph nonlinearFactors; + nonlinearFactors += PriorFactor(10, LieVector(), gtsam::SharedNoiseModel()); + nonlinearFactors += NonlinearFactor::shared_ptr(); + nonlinearFactors += PriorFactor(11, LieVector(), gtsam::SharedNoiseModel()); + + NonlinearFactorGraph newFactors; + newFactors += PriorFactor(1, LieVector(), gtsam::SharedNoiseModel()); + newFactors += PriorFactor(2, LieVector(), gtsam::SharedNoiseModel()); + + NonlinearFactorGraph expectedNonlinearFactors; + expectedNonlinearFactors += PriorFactor(10, LieVector(), gtsam::SharedNoiseModel()); + expectedNonlinearFactors += PriorFactor(1, LieVector(), gtsam::SharedNoiseModel()); + expectedNonlinearFactors += PriorFactor(11, LieVector(), gtsam::SharedNoiseModel()); + expectedNonlinearFactors += PriorFactor(2, LieVector(), gtsam::SharedNoiseModel()); + + const FastVector expectedNewFactorIndices = list_of(1)(3); + + FastVector actualNewFactorIndices; + + ISAM2::Impl::AddFactorsStep1(newFactors, true, nonlinearFactors, actualNewFactorIndices); + + EXPECT(assert_equal(expectedNonlinearFactors, nonlinearFactors)); + EXPECT(assert_container_equality(expectedNewFactorIndices, actualNewFactorIndices)); +} + +/* ************************************************************************* */ +TEST(ISAM2, simple) +{ + for(size_t i = 0; i < 10; ++i) { + // These variables will be reused and accumulate factors and values + Values fullinit; + NonlinearFactorGraph fullgraph; + ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false), i); + + // Compare solutions + EXPECT(isam_check(fullgraph, fullinit, isam, *this, result_)); + } +} + +/* ************************************************************************* */ +TEST(ISAM2, slamlike_solution_gaussnewton) +{ + // These variables will be reused and accumulate factors and values + Values fullinit; + NonlinearFactorGraph fullgraph; + ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); + + // Compare solutions + CHECK(isam_check(fullgraph, fullinit, isam, *this, result_)); +} + +/* ************************************************************************* */ +TEST(ISAM2, slamlike_solution_dogleg) +{ + // These variables will be reused and accumulate factors and values + Values fullinit; + NonlinearFactorGraph fullgraph; + ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false)); + + // Compare solutions + CHECK(isam_check(fullgraph, fullinit, isam, *this, result_)); +} + +/* ************************************************************************* */ +TEST(ISAM2, slamlike_solution_gaussnewton_qr) +{ + // These variables will be reused and accumulate factors and values + Values fullinit; + NonlinearFactorGraph fullgraph; + ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, false, ISAM2Params::QR)); + + // Compare solutions + CHECK(isam_check(fullgraph, fullinit, isam, *this, result_)); +} + +/* ************************************************************************* */ +TEST(ISAM2, slamlike_solution_dogleg_qr) +{ + // These variables will be reused and accumulate factors and values + Values fullinit; + NonlinearFactorGraph fullgraph; + ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false, false, ISAM2Params::QR)); + + // Compare solutions + CHECK(isam_check(fullgraph, fullinit, isam, *this, result_)); +} + +/* ************************************************************************* */ +TEST(ISAM2, clone) { + + ISAM2 clone1; + + { + ISAM2 isam = createSlamlikeISAM2(); + clone1 = isam; + + ISAM2 clone2(isam); + + // Modify original isam + NonlinearFactorGraph factors; + factors += BetweenFactor(0, 10, + isam.calculateEstimate(0).between(isam.calculateEstimate(10)), noiseModel::Unit::Create(3)); + isam.update(factors); + + CHECK(assert_equal(createSlamlikeISAM2(), clone2)); + } + + // This is to (perhaps unsuccessfully) try to currupt unallocated memory referenced + // if the references in the iSAM2 copy point to the old instance which deleted at + // the end of the {...} section above. + ISAM2 temp = createSlamlikeISAM2(); + + CHECK(assert_equal(createSlamlikeISAM2(), clone1)); + CHECK(assert_equal(clone1, temp)); + + // Check clone empty + ISAM2 isam; + clone1 = isam; + CHECK(assert_equal(ISAM2(), clone1)); +} + +/* ************************************************************************* */ +TEST(ISAM2, removeFactors) +{ + // This test builds a graph in the same way as the "slamlike" test above, but + // then removes the 2nd-to-last landmark measurement + + // These variables will be reused and accumulate factors and values + Values fullinit; + NonlinearFactorGraph fullgraph; + ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); + + // Remove the 2nd measurement on landmark 0 (Key 100) + FastVector toRemove; + toRemove.push_back(12); + isam.update(NonlinearFactorGraph(), Values(), toRemove); + + // Remove the factor from the full system + fullgraph.remove(12); + + // Compare solutions + CHECK(isam_check(fullgraph, fullinit, isam, *this, result_)); +} + +/* ************************************************************************* */ +TEST(ISAM2, removeVariables) +{ + // These variables will be reused and accumulate factors and values + Values fullinit; + NonlinearFactorGraph fullgraph; + ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); + + // Remove the measurement on landmark 0 (Key 100) + FastVector toRemove; + toRemove.push_back(7); + toRemove.push_back(14); + isam.update(NonlinearFactorGraph(), Values(), toRemove); + + // Remove the factors and variable from the full system + fullgraph.remove(7); + fullgraph.remove(14); + fullinit.erase(100); + + // Compare solutions + CHECK(isam_check(fullgraph, fullinit, isam, *this, result_)); +} + +/* ************************************************************************* */ +TEST(ISAM2, swapFactors) +{ + // This test builds a graph in the same way as the "slamlike" test above, but + // then swaps the 2nd-to-last landmark measurement with a different one + + Values fullinit; + NonlinearFactorGraph fullgraph; + ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph); + + // Remove the measurement on landmark 0 and replace with a different one + { + size_t swap_idx = isam.getFactorsUnsafe().size()-2; + FastVector toRemove; + toRemove.push_back(swap_idx); + fullgraph.remove(swap_idx); + + NonlinearFactorGraph swapfactors; +// swapfactors += BearingRange(10, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise; // original factor + swapfactors += BearingRangeFactor(10, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 5.0, brNoise); + fullgraph.push_back(swapfactors); + isam.update(swapfactors, Values(), toRemove); + } + + // Compare solutions + EXPECT(assert_equal(fullgraph, NonlinearFactorGraph(isam.getFactorsUnsafe()))); + EXPECT(isam_check(fullgraph, fullinit, isam, *this, result_)); + + // Check gradient at each node + typedef ISAM2::sharedClique sharedClique; + BOOST_FOREACH(const sharedClique& clique, isam.nodes() | br::map_values) { + // Compute expected gradient + GaussianFactorGraph jfg; + jfg += clique->conditional(); + VectorValues expectedGradient = jfg.gradientAtZero(); + // Compare with actual gradients + DenseIndex variablePosition = 0; + for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) { + const DenseIndex dim = clique->conditional()->getDim(jit); + Vector actual = clique->gradientContribution().segment(variablePosition, dim); + EXPECT(assert_equal(expectedGradient[*jit], actual)); + variablePosition += dim; + } + EXPECT_LONGS_EQUAL((long)clique->gradientContribution().rows(), (long)variablePosition); + } + + // Check gradient + VectorValues expectedGradient = GaussianFactorGraph(isam).gradientAtZero(); + VectorValues expectedGradient2 = GaussianFactorGraph(isam).gradient(VectorValues::Zero(expectedGradient)); + VectorValues actualGradient = isam.gradientAtZero(); + EXPECT(assert_equal(expectedGradient2, expectedGradient)); + EXPECT(assert_equal(expectedGradient, actualGradient)); +} + +/* ************************************************************************* */ +TEST(ISAM2, constrained_ordering) +{ + // These variables will be reused and accumulate factors and values + ISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); + Values fullinit; + NonlinearFactorGraph fullgraph; + + // We will constrain x3 and x4 to the end + FastMap constrained; + constrained.insert(make_pair((3), 1)); + constrained.insert(make_pair((4), 2)); + + // i keeps track of the time step + size_t i = 0; + + // Add a prior at time 0 and update isam + { + NonlinearFactorGraph newfactors; + newfactors += PriorFactor(0, Pose2(0.0, 0.0, 0.0), odoNoise); + fullgraph.push_back(newfactors); + + Values init; + init.insert((0), Pose2(0.01, 0.01, 0.01)); + fullinit.insert((0), Pose2(0.01, 0.01, 0.01)); + + isam.update(newfactors, init); + } + + CHECK(isam_check(fullgraph, fullinit, isam, *this, result_)); + + // Add odometry from time 0 to time 5 + for( ; i<5; ++i) { + NonlinearFactorGraph newfactors; + newfactors += BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + fullgraph.push_back(newfactors); + + Values init; + init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); + fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); + + if(i >= 3) + isam.update(newfactors, init, FastVector(), constrained); + else + isam.update(newfactors, init); + } + + // Add odometry from time 5 to 6 and landmark measurement at time 5 + { + NonlinearFactorGraph newfactors; + newfactors += BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors += BearingRangeFactor(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise); + newfactors += BearingRangeFactor(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); + fullgraph.push_back(newfactors); + + Values init; + init.insert((i+1), Pose2(1.01, 0.01, 0.01)); + init.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); + init.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); + fullinit.insert((i+1), Pose2(1.01, 0.01, 0.01)); + fullinit.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); + fullinit.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); + + isam.update(newfactors, init, FastVector(), constrained); + ++ i; + } + + // Add odometry from time 6 to time 10 + for( ; i<10; ++i) { + NonlinearFactorGraph newfactors; + newfactors += BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + fullgraph.push_back(newfactors); + + Values init; + init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); + fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); + + isam.update(newfactors, init, FastVector(), constrained); + } + + // Add odometry from time 10 to 11 and landmark measurement at time 10 + { + NonlinearFactorGraph newfactors; + newfactors += BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors += BearingRangeFactor(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); + newfactors += BearingRangeFactor(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise); + fullgraph.push_back(newfactors); + + Values init; + init.insert((i+1), Pose2(6.9, 0.1, 0.01)); + fullinit.insert((i+1), Pose2(6.9, 0.1, 0.01)); + + isam.update(newfactors, init, FastVector(), constrained); + ++ i; + } + + // Compare solutions + EXPECT(isam_check(fullgraph, fullinit, isam, *this, result_)); + + // Check gradient at each node + typedef ISAM2::sharedClique sharedClique; + BOOST_FOREACH(const sharedClique& clique, isam.nodes() | br::map_values) { + // Compute expected gradient + GaussianFactorGraph jfg; + jfg += clique->conditional(); + VectorValues expectedGradient = jfg.gradientAtZero(); + // Compare with actual gradients + DenseIndex variablePosition = 0; + for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) { + const DenseIndex dim = clique->conditional()->getDim(jit); + Vector actual = clique->gradientContribution().segment(variablePosition, dim); + EXPECT(assert_equal(expectedGradient[*jit], actual)); + variablePosition += dim; + } + LONGS_EQUAL((long)clique->gradientContribution().rows(), (long)variablePosition); + } + + // Check gradient + VectorValues expectedGradient = GaussianFactorGraph(isam).gradientAtZero(); + VectorValues expectedGradient2 = GaussianFactorGraph(isam).gradient(VectorValues::Zero(expectedGradient)); + VectorValues actualGradient = isam.gradientAtZero(); + EXPECT(assert_equal(expectedGradient2, expectedGradient)); + EXPECT(assert_equal(expectedGradient, actualGradient)); +} + +/* ************************************************************************* */ +TEST(ISAM2, slamlike_solution_partial_relinearization_check) +{ + // These variables will be reused and accumulate factors and values + Values fullinit; + NonlinearFactorGraph fullgraph; + ISAM2Params params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false); + params.enablePartialRelinearizationCheck = true; + ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, params); + + // Compare solutions + CHECK(isam_check(fullgraph, fullinit, isam, *this, result_)); +} + +namespace { + bool checkMarginalizeLeaves(ISAM2& isam, const FastList& leafKeys) { + Matrix expectedAugmentedHessian, expected3AugmentedHessian; + vector toKeep; + BOOST_FOREACH(Key j, isam.getDelta() | br::map_keys) + if(find(leafKeys.begin(), leafKeys.end(), j) == leafKeys.end()) + toKeep.push_back(j); + + // Calculate expected marginal from iSAM2 tree + expectedAugmentedHessian = GaussianFactorGraph(isam).marginal(toKeep, EliminateQR)->augmentedHessian(); + + // Calculate expected marginal from cached linear factors + //assert(isam.params().cacheLinearizedFactors); + //Matrix expected2AugmentedHessian = isam.linearFactors_.marginal(toKeep, EliminateQR)->augmentedHessian(); + + // Calculate expected marginal from original nonlinear factors + expected3AugmentedHessian = isam.getFactorsUnsafe().linearize(isam.getLinearizationPoint()) + ->marginal(toKeep, EliminateQR)->augmentedHessian(); + + // Do marginalization + isam.marginalizeLeaves(leafKeys); + + // Check + GaussianFactorGraph actualMarginalGraph(isam); + Matrix actualAugmentedHessian = actualMarginalGraph.augmentedHessian(); + //Matrix actual2AugmentedHessian = linearFactors_.augmentedHessian(); + Matrix actual3AugmentedHessian = isam.getFactorsUnsafe().linearize( + isam.getLinearizationPoint())->augmentedHessian(); + assert(actualAugmentedHessian.allFinite()); + + // Check full marginalization + //cout << "treeEqual" << endl; + bool treeEqual = assert_equal(expectedAugmentedHessian, actualAugmentedHessian, 1e-6); + //actualAugmentedHessian.bottomRightCorner(1,1) = expected2AugmentedHessian.bottomRightCorner(1,1); bool linEqual = assert_equal(expected2AugmentedHessian, actualAugmentedHessian, 1e-6); + //cout << "nonlinEqual" << endl; + actualAugmentedHessian.bottomRightCorner(1,1) = expected3AugmentedHessian.bottomRightCorner(1,1); bool nonlinEqual = assert_equal(expected3AugmentedHessian, actualAugmentedHessian, 1e-6); + //bool linCorrect = assert_equal(expected3AugmentedHessian, expected2AugmentedHessian, 1e-6); + //actual2AugmentedHessian.bottomRightCorner(1,1) = expected3AugmentedHessian.bottomRightCorner(1,1); bool afterLinCorrect = assert_equal(expected3AugmentedHessian, actual2AugmentedHessian, 1e-6); + //cout << "nonlinCorrect" << endl; + bool afterNonlinCorrect = assert_equal(expected3AugmentedHessian, actual3AugmentedHessian, 1e-6); + + bool ok = treeEqual && /*linEqual &&*/ nonlinEqual && /*linCorrect &&*/ /*afterLinCorrect &&*/ afterNonlinCorrect; + return ok; + } +} + +/* ************************************************************************* */ +TEST(ISAM2, marginalizeLeaves1) +{ + ISAM2 isam; + + NonlinearFactorGraph factors; + factors += PriorFactor(0, LieVector(0.0), noiseModel::Unit::Create(1)); + + factors += BetweenFactor(0, 1, LieVector(0.0), noiseModel::Unit::Create(1)); + factors += BetweenFactor(1, 2, LieVector(0.0), noiseModel::Unit::Create(1)); + factors += BetweenFactor(0, 2, LieVector(0.0), noiseModel::Unit::Create(1)); + + Values values; + values.insert(0, LieVector(0.0)); + values.insert(1, LieVector(0.0)); + values.insert(2, LieVector(0.0)); + + FastMap constrainedKeys; + constrainedKeys.insert(make_pair(0,0)); + constrainedKeys.insert(make_pair(1,1)); + constrainedKeys.insert(make_pair(2,2)); + + isam.update(factors, values, FastVector(), constrainedKeys); + + FastList leafKeys = list_of(0); + EXPECT(checkMarginalizeLeaves(isam, leafKeys)); +} + +/* ************************************************************************* */ +TEST(ISAM2, marginalizeLeaves2) +{ + ISAM2 isam; + + NonlinearFactorGraph factors; + factors += PriorFactor(0, LieVector(0.0), noiseModel::Unit::Create(1)); + + factors += BetweenFactor(0, 1, LieVector(0.0), noiseModel::Unit::Create(1)); + factors += BetweenFactor(1, 2, LieVector(0.0), noiseModel::Unit::Create(1)); + factors += BetweenFactor(0, 2, LieVector(0.0), noiseModel::Unit::Create(1)); + factors += BetweenFactor(2, 3, LieVector(0.0), noiseModel::Unit::Create(1)); + + Values values; + values.insert(0, LieVector(0.0)); + values.insert(1, LieVector(0.0)); + values.insert(2, LieVector(0.0)); + values.insert(3, LieVector(0.0)); + + FastMap constrainedKeys; + constrainedKeys.insert(make_pair(0,0)); + constrainedKeys.insert(make_pair(1,1)); + constrainedKeys.insert(make_pair(2,2)); + constrainedKeys.insert(make_pair(3,3)); + + isam.update(factors, values, FastVector(), constrainedKeys); + + FastList leafKeys = list_of(0); + EXPECT(checkMarginalizeLeaves(isam, leafKeys)); +} + +/* ************************************************************************* */ +TEST(ISAM2, marginalizeLeaves3) +{ + ISAM2 isam; + + NonlinearFactorGraph factors; + factors += PriorFactor(0, LieVector(0.0), noiseModel::Unit::Create(1)); + + factors += BetweenFactor(0, 1, LieVector(0.0), noiseModel::Unit::Create(1)); + factors += BetweenFactor(1, 2, LieVector(0.0), noiseModel::Unit::Create(1)); + factors += BetweenFactor(0, 2, LieVector(0.0), noiseModel::Unit::Create(1)); + + factors += BetweenFactor(2, 3, LieVector(0.0), noiseModel::Unit::Create(1)); + + factors += BetweenFactor(3, 4, LieVector(0.0), noiseModel::Unit::Create(1)); + factors += BetweenFactor(4, 5, LieVector(0.0), noiseModel::Unit::Create(1)); + factors += BetweenFactor(3, 5, LieVector(0.0), noiseModel::Unit::Create(1)); + + Values values; + values.insert(0, LieVector(0.0)); + values.insert(1, LieVector(0.0)); + values.insert(2, LieVector(0.0)); + values.insert(3, LieVector(0.0)); + values.insert(4, LieVector(0.0)); + values.insert(5, LieVector(0.0)); + + FastMap constrainedKeys; + constrainedKeys.insert(make_pair(0,0)); + constrainedKeys.insert(make_pair(1,1)); + constrainedKeys.insert(make_pair(2,2)); + constrainedKeys.insert(make_pair(3,3)); + constrainedKeys.insert(make_pair(4,4)); + constrainedKeys.insert(make_pair(5,5)); + + isam.update(factors, values, FastVector(), constrainedKeys); + + FastList leafKeys = list_of(0); + EXPECT(checkMarginalizeLeaves(isam, leafKeys)); +} + +/* ************************************************************************* */ +TEST(ISAM2, marginalizeLeaves4) +{ + ISAM2 isam; + + NonlinearFactorGraph factors; + factors += PriorFactor(0, LieVector(0.0), noiseModel::Unit::Create(1)); + factors += BetweenFactor(0, 2, LieVector(0.0), noiseModel::Unit::Create(1)); + factors += BetweenFactor(1, 2, LieVector(0.0), noiseModel::Unit::Create(1)); + + Values values; + values.insert(0, LieVector(0.0)); + values.insert(1, LieVector(0.0)); + values.insert(2, LieVector(0.0)); + + FastMap constrainedKeys; + constrainedKeys.insert(make_pair(0,0)); + constrainedKeys.insert(make_pair(1,1)); + constrainedKeys.insert(make_pair(2,2)); + + isam.update(factors, values, FastVector(), constrainedKeys); + + FastList leafKeys = list_of(1); + EXPECT(checkMarginalizeLeaves(isam, leafKeys)); +} + +/* ************************************************************************* */ +TEST(ISAM2, marginalizeLeaves5) +{ + // Create isam2 + ISAM2 isam = createSlamlikeISAM2(); + + // Marginalize + FastList marginalizeKeys = list_of(0); + EXPECT(checkMarginalizeLeaves(isam, marginalizeKeys)); +} + +/* ************************************************************************* */ +TEST(ISAM2, marginalCovariance) +{ + // Create isam2 + ISAM2 isam = createSlamlikeISAM2(); + + // Check marginal + Matrix expected = Marginals(isam.getFactorsUnsafe(), isam.getLinearizationPoint()).marginalCovariance(5); + Matrix actual = isam.marginalCovariance(5); + EXPECT(assert_equal(expected, actual)); +} + +/* ************************************************************************* */ +TEST(ISAM2, calculate_nnz) +{ + ISAM2 isam = createSlamlikeISAM2(); + int expected = 241; + int actual = calculate_nnz(isam.roots().front()); + + EXPECT_LONGS_EQUAL(expected, actual); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ diff --git a/tests/testGaussianJunctionTreeB.cpp b/tests/testGaussianJunctionTreeB.cpp index b24b2b901..0b84f137d 100644 --- a/tests/testGaussianJunctionTreeB.cpp +++ b/tests/testGaussianJunctionTreeB.cpp @@ -15,6 +15,10 @@ * @author nikai */ +#include + +#if 0 + #include #include #include @@ -22,18 +26,14 @@ #include #include #include -#include +#include #include -#include -#include #include #include #include #include #include -#include - #include #include // for operator += #include // for operator += @@ -105,7 +105,7 @@ TEST( GaussianJunctionTreeB, optimizeMultiFrontal ) // verify VectorValues expected(vector(7,2)); // expected solution - Vector v = Vector_(2, 0., 0.); + Vector v = (Vector(2) << 0., 0.); for (int i=1; i<=7; i++) expected[ordering[X(i)]] = v; EXPECT(assert_equal(expected,actual)); @@ -134,8 +134,8 @@ TEST(GaussianJunctionTreeB, slamlike) { Values init; NonlinearFactorGraph newfactors; NonlinearFactorGraph fullgraph; - SharedDiagonal odoNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.1, 0.1, M_PI/100.0)); - SharedDiagonal brNoise = noiseModel::Diagonal::Sigmas(Vector_(2, M_PI/100.0, 0.1)); + SharedDiagonal odoNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.1, 0.1, M_PI/100.0)); + SharedDiagonal brNoise = noiseModel::Diagonal::Sigmas((Vector(2) << M_PI/100.0, 0.1)); size_t i = 0; @@ -199,7 +199,7 @@ TEST(GaussianJunctionTreeB, simpleMarginal) { // Create a simple graph NonlinearFactorGraph fg; fg.add(PriorFactor(X(0), Pose2(), noiseModel::Isotropic::Sigma(3, 10.0))); - fg.add(BetweenFactor(X(0), X(1), Pose2(1.0, 0.0, 0.0), noiseModel::Diagonal::Sigmas(Vector_(3, 10.0, 1.0, 1.0)))); + fg.add(BetweenFactor(X(0), X(1), Pose2(1.0, 0.0, 0.0), noiseModel::Diagonal::Sigmas((Vector(3) << 10.0, 1.0, 1.0)))); Values init; init.insert(X(0), Pose2()); @@ -232,6 +232,9 @@ TEST(GaussianJunctionTreeB, simpleMarginal) { EXPECT(assert_equal(expected, actual3)); } +#endif + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ + diff --git a/tests/testGradientDescentOptimizer.cpp b/tests/testGradientDescentOptimizer.cpp index 4f556eb0f..2a84248a3 100644 --- a/tests/testGradientDescentOptimizer.cpp +++ b/tests/testGradientDescentOptimizer.cpp @@ -30,21 +30,21 @@ boost::tuple generateProblem() { // 2a. Add Gaussian prior Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin - SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); - graph.add(PriorFactor(1, priorMean, priorNoise)); + SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1)); + graph += PriorFactor(1, priorMean, priorNoise); // 2b. Add odometry factors - SharedDiagonal odometryNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); - graph.add(BetweenFactor(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise)); - graph.add(BetweenFactor(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); - graph.add(BetweenFactor(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); - graph.add(BetweenFactor(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); + SharedDiagonal odometryNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1)); + graph += BetweenFactor(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise); + graph += BetweenFactor(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise); + graph += BetweenFactor(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise); + graph += BetweenFactor(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise); // 2c. Add pose constraint - SharedDiagonal constraintUncertainty = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); - graph.add(BetweenFactor(5, 2, Pose2(2.0, 0.0, M_PI_2), constraintUncertainty)); + SharedDiagonal constraintUncertainty = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1)); + graph += BetweenFactor(5, 2, Pose2(2.0, 0.0, M_PI_2), constraintUncertainty); - // 3. Create the data structure to hold the initialEstimate estinmate to the solution + // 3. Create the data structure to hold the initialEstimate estimate to the solution Values initialEstimate; Pose2 x1(0.5, 0.0, 0.2 ); initialEstimate.insert(1, x1); Pose2 x2(2.3, 0.1,-0.2 ); initialEstimate.insert(2, x2); diff --git a/tests/testGraph.cpp b/tests/testGraph.cpp index dc00b3f4a..842f2bd67 100644 --- a/tests/testGraph.cpp +++ b/tests/testGraph.cpp @@ -48,7 +48,7 @@ TEST ( Ordering, predecessorMap2Keys ) { expected += 4,5,3,2,1; list actual = predecessorMap2Keys(p_map); - LONGS_EQUAL(expected.size(), actual.size()); + LONGS_EQUAL((long)expected.size(), (long)actual.size()); list::const_iterator it1 = expected.begin(); list::const_iterator it2 = actual.begin(); @@ -70,7 +70,7 @@ TEST( Graph, predecessorMap2Graph ) p_map.insert(3, 2); boost::tie(graph, root, key2vertex) = predecessorMap2Graph, SVertex, Key>(p_map); - LONGS_EQUAL(3, boost::num_vertices(graph)); + LONGS_EQUAL(3, (long)boost::num_vertices(graph)); CHECK(root == key2vertex[2]); } @@ -81,9 +81,9 @@ TEST( Graph, composePoses ) SharedNoiseModel cov = noiseModel::Unit::Create(3); Pose2 p1(1.0, 2.0, 0.3), p2(4.0, 5.0, 0.6), p3(7.0, 8.0, 0.9), p4(2.0, 2.0, 2.9); Pose2 p12=p1.between(p2), p23=p2.between(p3), p43=p4.between(p3); - graph.add(BetweenFactor(1,2, p12, cov)); - graph.add(BetweenFactor(2,3, p23, cov)); - graph.add(BetweenFactor(4,3, p43, cov)); + graph += BetweenFactor(1,2, p12, cov); + graph += BetweenFactor(2,3, p23, cov); + graph += BetweenFactor(4,3, p43, cov); PredecessorMap tree; tree.insert(1,2); @@ -101,7 +101,7 @@ TEST( Graph, composePoses ) expected.insert(3, p3); expected.insert(4, p4); - LONGS_EQUAL(4, actual->size()); + LONGS_EQUAL(4, (long)actual->size()); CHECK(assert_equal(expected, *actual)); } @@ -110,12 +110,12 @@ TEST( Graph, composePoses ) // GaussianFactorGraph g; // Matrix I = eye(2); // Vector b = Vector_(0, 0, 0); -// g.add(X(1), I, X(2), I, b, model); -// g.add(X(1), I, X(3), I, b, model); -// g.add(X(1), I, X(4), I, b, model); -// g.add(X(2), I, X(3), I, b, model); -// g.add(X(2), I, X(4), I, b, model); -// g.add(X(3), I, X(4), I, b, model); +// g += X(1), I, X(2), I, b, model; +// g += X(1), I, X(3), I, b, model; +// g += X(1), I, X(4), I, b, model; +// g += X(2), I, X(3), I, b, model; +// g += X(2), I, X(4), I, b, model; +// g += X(3), I, X(4), I, b, model; // // map tree = g.findMinimumSpanningTree(); // EXPECT(tree[X(1)].compare(X(1))==0); @@ -130,11 +130,11 @@ TEST( Graph, composePoses ) // GaussianFactorGraph g; // Matrix I = eye(2); // Vector b = Vector_(0, 0, 0); -// g.add(X(1), I, X(2), I, b, model); -// g.add(X(1), I, X(3), I, b, model); -// g.add(X(1), I, X(4), I, b, model); -// g.add(X(2), I, X(3), I, b, model); -// g.add(X(2), I, X(4), I, b, model); +// g += X(1), I, X(2), I, b, model; +// g += X(1), I, X(3), I, b, model; +// g += X(1), I, X(4), I, b, model; +// g += X(2), I, X(3), I, b, model; +// g += X(2), I, X(4), I, b, model; // // PredecessorMap tree; // tree[X(1)] = X(1); diff --git a/tests/testInferenceB.cpp b/tests/testInferenceB.cpp deleted file mode 100644 index 0df25d082..000000000 --- a/tests/testInferenceB.cpp +++ /dev/null @@ -1,89 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 testInferenceB.cpp - * @brief Unit tests for functionality declared in inference.h - * @author Frank Dellaert - */ - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -using namespace std; -using namespace gtsam; - -// Convenience for named keys -using symbol_shorthand::X; -using symbol_shorthand::L; - -/* ************************************************************************* */ -// The tests below test the *generic* inference algorithms. Some of these have -// specialized versions in the derived classes GaussianFactorGraph etc... -/* ************************************************************************* */ - -/* ************************************************************************* */ -TEST( inference, marginals ) -{ - using namespace example; - // create and marginalize a small Bayes net on "x" - GaussianBayesNet cbn = createSmallGaussianBayesNet(); - vector xvar; xvar.push_back(0); - GaussianBayesNet actual = *GaussianSequentialSolver( - *GaussianSequentialSolver(GaussianFactorGraph(cbn)).jointFactorGraph(xvar)).eliminate(); - - // expected is just scalar Gaussian on x - GaussianBayesNet expected = scalarGaussian(0, 4, sqrt(2.0)); - CHECK(assert_equal(expected,actual)); -} - -/* ************************************************************************* */ -TEST( inference, marginals2) -{ - NonlinearFactorGraph fg; - SharedDiagonal poseModel(noiseModel::Isotropic::Sigma(3, 0.1)); - SharedDiagonal pointModel(noiseModel::Isotropic::Sigma(2, 0.1)); - - fg.add(PriorFactor(X(0), Pose2(), poseModel)); - fg.add(BetweenFactor(X(0), X(1), Pose2(1.0,0.0,0.0), poseModel)); - fg.add(BetweenFactor(X(1), X(2), Pose2(1.0,0.0,0.0), poseModel)); - fg.add(BearingRangeFactor(X(0), L(0), Rot2(), 1.0, pointModel)); - fg.add(BearingRangeFactor(X(1), L(0), Rot2(), 1.0, pointModel)); - fg.add(BearingRangeFactor(X(2), L(0), Rot2(), 1.0, pointModel)); - - Values init; - init.insert(X(0), Pose2(0.0,0.0,0.0)); - init.insert(X(1), Pose2(1.0,0.0,0.0)); - init.insert(X(2), Pose2(2.0,0.0,0.0)); - init.insert(L(0), Point2(1.0,1.0)); - - Ordering ordering(*fg.orderingCOLAMD(init)); - FactorGraph::shared_ptr gfg(fg.linearize(init, ordering)); - GaussianMultifrontalSolver solver(*gfg); - solver.marginalFactor(ordering[L(0)]); -} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr);} -/* ************************************************************************* */ diff --git a/tests/testIterative.cpp b/tests/testIterative.cpp index 1484052e5..bb50d2afa 100644 --- a/tests/testIterative.cpp +++ b/tests/testIterative.cpp @@ -19,9 +19,7 @@ #include #include #include -#include -#include -#include +#include #include #include @@ -41,12 +39,10 @@ static ConjugateGradientParameters parameters; TEST( Iterative, steepestDescent ) { // Create factor graph - Ordering ordering; - ordering += L(1), X(1), X(2); - GaussianFactorGraph fg = createGaussianFactorGraph(ordering); + GaussianFactorGraph fg = createGaussianFactorGraph(); // eliminate and solve - VectorValues expected = *GaussianSequentialSolver(fg).optimize(); + VectorValues expected = fg.optimize(); // Do gradient descent VectorValues zero = VectorValues::Zero(expected); // TODO, how do we do this normally? @@ -58,19 +54,17 @@ TEST( Iterative, steepestDescent ) TEST( Iterative, conjugateGradientDescent ) { // Create factor graph - Ordering ordering; - ordering += L(1), X(1), X(2); - GaussianFactorGraph fg = createGaussianFactorGraph(ordering); + GaussianFactorGraph fg = createGaussianFactorGraph(); // eliminate and solve - VectorValues expected = *GaussianSequentialSolver(fg).optimize(); + VectorValues expected = fg.optimize(); // get matrices Matrix A; Vector b; Vector x0 = gtsam::zero(6); boost::tie(A, b) = fg.jacobian(); - Vector expectedX = Vector_(6, -0.1, 0.1, -0.1, -0.1, 0.1, -0.2); + Vector expectedX = (Vector(6) << -0.1, 0.1, -0.1, -0.1, 0.1, -0.2); // Do conjugate gradient descent, System version System Ab(A, b); @@ -96,14 +90,12 @@ TEST( Iterative, conjugateGradientDescent_hard_constraint ) config.insert(X(2), Pose2(1.5,0.,0.)); NonlinearFactorGraph graph; - graph.add(NonlinearEquality(X(1), pose1)); - graph.add(BetweenFactor(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1))); + graph += NonlinearEquality(X(1), pose1); + graph += BetweenFactor(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1)); - Ordering ordering; - ordering += X(1), X(2); - boost::shared_ptr fg = graph.linearize(config,ordering); + boost::shared_ptr fg = graph.linearize(config); - VectorValues zeros = VectorValues::Zero(2, 3); + VectorValues zeros = config.zeroVectors(); ConjugateGradientParameters parameters; parameters.setEpsilon_abs(1e-3); @@ -112,8 +104,8 @@ TEST( Iterative, conjugateGradientDescent_hard_constraint ) VectorValues actual = conjugateGradientDescent(*fg, zeros, parameters); VectorValues expected; - expected.insert(0, zero(3)); - expected.insert(1, Vector_(3,-0.5,0.,0.)); + expected.insert(X(1), zero(3)); + expected.insert(X(2), (Vector(3) << -0.5,0.,0.)); CHECK(assert_equal(expected, actual)); } @@ -125,14 +117,12 @@ TEST( Iterative, conjugateGradientDescent_soft_constraint ) config.insert(X(2), Pose2(1.5,0.,0.)); NonlinearFactorGraph graph; - graph.add(PriorFactor(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10))); - graph.add(BetweenFactor(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1))); + graph += PriorFactor(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10)); + graph += BetweenFactor(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1)); - Ordering ordering; - ordering += X(1), X(2); - boost::shared_ptr fg = graph.linearize(config,ordering); + boost::shared_ptr fg = graph.linearize(config); - VectorValues zeros = VectorValues::Zero(2, 3); + VectorValues zeros = config.zeroVectors(); ConjugateGradientParameters parameters; parameters.setEpsilon_abs(1e-3); @@ -141,8 +131,8 @@ TEST( Iterative, conjugateGradientDescent_soft_constraint ) VectorValues actual = conjugateGradientDescent(*fg, zeros, parameters); VectorValues expected; - expected.insert(0, zero(3)); - expected.insert(1, Vector_(3,-0.5,0.,0.)); + expected.insert(X(1), zero(3)); + expected.insert(X(2), (Vector(3) << -0.5,0.,0.)); CHECK(assert_equal(expected, actual)); } diff --git a/tests/testMarginals.cpp b/tests/testMarginals.cpp index 93f1431a1..5478ce38e 100644 --- a/tests/testMarginals.cpp +++ b/tests/testMarginals.cpp @@ -19,7 +19,7 @@ #include // for all nonlinear keys -#include +#include // for points and poses #include @@ -51,21 +51,21 @@ TEST(Marginals, planarSLAMmarginals) { /* add prior */ // gaussian for prior - SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); + SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1)); Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin - graph.add(PriorFactor(x1, priorMean, priorNoise)); // add the factor to the graph + graph += PriorFactor(x1, priorMean, priorNoise); // add the factor to the graph /* add odometry */ // general noisemodel for odometry - SharedDiagonal odometryNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); + SharedDiagonal odometryNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1)); Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case) // create between factors to represent odometry - graph.add(BetweenFactor(x1, x2, odometry, odometryNoise)); - graph.add(BetweenFactor(x2, x3, odometry, odometryNoise)); + graph += BetweenFactor(x1, x2, odometry, odometryNoise); + graph += BetweenFactor(x2, x3, odometry, odometryNoise); /* add measurements */ // general noisemodel for measurements - SharedDiagonal measurementNoise = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.2)); + SharedDiagonal measurementNoise = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.2)); // create the measurement values - indices are (pose id, landmark id) Rot2 bearing11 = Rot2::fromDegrees(45), @@ -76,9 +76,9 @@ TEST(Marginals, planarSLAMmarginals) { range32 = 2.0; // create bearing/range factors - graph.add(BearingRangeFactor(x1, l1, bearing11, range11, measurementNoise)); - graph.add(BearingRangeFactor(x2, l1, bearing21, range21, measurementNoise)); - graph.add(BearingRangeFactor(x3, l2, bearing32, range32, measurementNoise)); + graph += BearingRangeFactor(x1, l1, bearing11, range11, measurementNoise); + graph += BearingRangeFactor(x2, l1, bearing21, range21, measurementNoise); + graph += BearingRangeFactor(x3, l2, bearing32, range32, measurementNoise); // linearization point for marginals Values soln; @@ -140,8 +140,8 @@ TEST(Marginals, planarSLAMmarginals) { -0.104516127560770, 0.351935664055174, 0.000000000000000, 0.090000180000270, 0.040000000000000, 0.007741936219615, 0.351935664055174, 0.056129031890193, -0.050967744878460, 0.056129031890193, 0.000000000000000, 0.000000000000000, 0.010000000000000, 0.004516127560770, 0.056129031890193, 0.027741936219615; vector variables(3); - variables[0] = l2; - variables[1] = x1; + variables[0] = x1; + variables[1] = l2; variables[2] = x3; JointMarginal joint_l2x1x3 = marginals.jointMarginalCovariance(variables); EXPECT(assert_equal(Matrix(expected_l2x1x3.block(0,0,2,2)), Matrix(joint_l2x1x3(l2,l2)), 1e-6)); @@ -183,10 +183,10 @@ TEST(Marginals, planarSLAMmarginals) { /* ************************************************************************* */ TEST(Marginals, order) { NonlinearFactorGraph fg; - fg.add(PriorFactor(0, Pose2(), noiseModel::Unit::Create(3))); - fg.add(BetweenFactor(0, 1, Pose2(1,0,0), noiseModel::Unit::Create(3))); - fg.add(BetweenFactor(1, 2, Pose2(1,0,0), noiseModel::Unit::Create(3))); - fg.add(BetweenFactor(2, 3, Pose2(1,0,0), noiseModel::Unit::Create(3))); + fg += PriorFactor(0, Pose2(), noiseModel::Unit::Create(3)); + fg += BetweenFactor(0, 1, Pose2(1,0,0), noiseModel::Unit::Create(3)); + fg += BetweenFactor(1, 2, Pose2(1,0,0), noiseModel::Unit::Create(3)); + fg += BetweenFactor(2, 3, Pose2(1,0,0), noiseModel::Unit::Create(3)); Values vals; vals.insert(0, Pose2()); @@ -197,44 +197,44 @@ TEST(Marginals, order) { vals.insert(100, Point2(0,1)); vals.insert(101, Point2(1,1)); - fg.add(BearingRangeFactor(0, 100, + fg += BearingRangeFactor(0, 100, vals.at(0).bearing(vals.at(100)), - vals.at(0).range(vals.at(100)), noiseModel::Unit::Create(2))); - fg.add(BearingRangeFactor(0, 101, + vals.at(0).range(vals.at(100)), noiseModel::Unit::Create(2)); + fg += BearingRangeFactor(0, 101, vals.at(0).bearing(vals.at(101)), - vals.at(0).range(vals.at(101)), noiseModel::Unit::Create(2))); + vals.at(0).range(vals.at(101)), noiseModel::Unit::Create(2)); - fg.add(BearingRangeFactor(1, 100, + fg += BearingRangeFactor(1, 100, vals.at(1).bearing(vals.at(100)), - vals.at(1).range(vals.at(100)), noiseModel::Unit::Create(2))); - fg.add(BearingRangeFactor(1, 101, + vals.at(1).range(vals.at(100)), noiseModel::Unit::Create(2)); + fg += BearingRangeFactor(1, 101, vals.at(1).bearing(vals.at(101)), - vals.at(1).range(vals.at(101)), noiseModel::Unit::Create(2))); + vals.at(1).range(vals.at(101)), noiseModel::Unit::Create(2)); - fg.add(BearingRangeFactor(2, 100, + fg += BearingRangeFactor(2, 100, vals.at(2).bearing(vals.at(100)), - vals.at(2).range(vals.at(100)), noiseModel::Unit::Create(2))); - fg.add(BearingRangeFactor(2, 101, + vals.at(2).range(vals.at(100)), noiseModel::Unit::Create(2)); + fg += BearingRangeFactor(2, 101, vals.at(2).bearing(vals.at(101)), - vals.at(2).range(vals.at(101)), noiseModel::Unit::Create(2))); + vals.at(2).range(vals.at(101)), noiseModel::Unit::Create(2)); - fg.add(BearingRangeFactor(3, 100, + fg += BearingRangeFactor(3, 100, vals.at(3).bearing(vals.at(100)), - vals.at(3).range(vals.at(100)), noiseModel::Unit::Create(2))); - fg.add(BearingRangeFactor(3, 101, + vals.at(3).range(vals.at(100)), noiseModel::Unit::Create(2)); + fg += BearingRangeFactor(3, 101, vals.at(3).bearing(vals.at(101)), - vals.at(3).range(vals.at(101)), noiseModel::Unit::Create(2))); + vals.at(3).range(vals.at(101)), noiseModel::Unit::Create(2)); Marginals marginals(fg, vals); FastVector keys(fg.keys()); JointMarginal joint = marginals.jointMarginalCovariance(keys); - LONGS_EQUAL(3, joint(0,0).rows()); - LONGS_EQUAL(3, joint(1,1).rows()); - LONGS_EQUAL(3, joint(2,2).rows()); - LONGS_EQUAL(3, joint(3,3).rows()); - LONGS_EQUAL(2, joint(100,100).rows()); - LONGS_EQUAL(2, joint(101,101).rows()); + LONGS_EQUAL(3, (long)joint(0,0).rows()); + LONGS_EQUAL(3, (long)joint(1,1).rows()); + LONGS_EQUAL(3, (long)joint(2,2).rows()); + LONGS_EQUAL(3, (long)joint(3,3).rows()); + LONGS_EQUAL(2, (long)joint(100,100).rows()); + LONGS_EQUAL(2, (long)joint(101,101).rows()); } /* ************************************************************************* */ diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp index f10ce1b35..b55f6f144 100644 --- a/tests/testNonlinearEquality.cpp +++ b/tests/testNonlinearEquality.cpp @@ -18,7 +18,7 @@ #include #include -#include +#include #include #include #include @@ -55,9 +55,9 @@ TEST ( NonlinearEquality, linearization ) { // check linearize SharedDiagonal constraintModel = noiseModel::Constrained::All(3); - JacobianFactor expLF(0, eye(3), zero(3), constraintModel); - GaussianFactor::shared_ptr actualLF = nle->linearize(linearize, *linearize.orderingArbitrary()); - EXPECT(assert_equal(*actualLF, (const GaussianFactor&)expLF)); + JacobianFactor expLF(key, eye(3), zero(3), constraintModel); + GaussianFactor::shared_ptr actualLF = nle->linearize(linearize); + EXPECT(assert_equal((const GaussianFactor&)expLF, *actualLF)); } /* ********************************************************************** */ @@ -71,7 +71,7 @@ TEST ( NonlinearEquality, linearization_pose ) { // create a nonlinear equality constraint shared_poseNLE nle(new PoseNLE(key, value)); - GaussianFactor::shared_ptr actualLF = nle->linearize(config, *config.orderingArbitrary()); + GaussianFactor::shared_ptr actualLF = nle->linearize(config); EXPECT(true); } @@ -86,7 +86,7 @@ TEST ( NonlinearEquality, linearization_fail ) { shared_poseNLE nle(new PoseNLE(key, value)); // check linearize to ensure that it fails for bad linearization points - CHECK_EXCEPTION(nle->linearize(bad_linearize, *bad_linearize.orderingArbitrary()), std::invalid_argument); + CHECK_EXCEPTION(nle->linearize(bad_linearize), std::invalid_argument); } /* ********************************************************************** */ @@ -102,7 +102,7 @@ TEST ( NonlinearEquality, linearization_fail_pose ) { shared_poseNLE nle(new PoseNLE(key, value)); // check linearize to ensure that it fails for bad linearization points - CHECK_EXCEPTION(nle->linearize(bad_linearize, *bad_linearize.orderingArbitrary()), std::invalid_argument); + CHECK_EXCEPTION(nle->linearize(bad_linearize), std::invalid_argument); } /* ********************************************************************** */ @@ -118,7 +118,7 @@ TEST ( NonlinearEquality, linearization_fail_pose_origin ) { shared_poseNLE nle(new PoseNLE(key, value)); // check linearize to ensure that it fails for bad linearization points - CHECK_EXCEPTION(nle->linearize(bad_linearize, *bad_linearize.orderingArbitrary()), std::invalid_argument); + CHECK_EXCEPTION(nle->linearize(bad_linearize), std::invalid_argument); } /* ************************************************************************* */ @@ -166,7 +166,7 @@ TEST ( NonlinearEquality, allow_error_pose ) { // the unwhitened error should provide logmap to the feasible state Pose2 badPoint1(0.0, 2.0, 3.0); Vector actVec = nle.evaluateError(badPoint1); - Vector expVec = Vector_(3, -0.989992, -0.14112, 0.0); + Vector expVec = (Vector(3) << -0.989992, -0.14112, 0.0); EXPECT(assert_equal(expVec, actVec, 1e-5)); // the actual error should have a gain on it @@ -176,11 +176,11 @@ TEST ( NonlinearEquality, allow_error_pose ) { DOUBLES_EQUAL(500.0, actError, 1e-9); // check linearization - GaussianFactor::shared_ptr actLinFactor = nle.linearize(config, *config.orderingArbitrary()); + GaussianFactor::shared_ptr actLinFactor = nle.linearize(config); Matrix A1 = eye(3,3); Vector b = expVec; SharedDiagonal model = noiseModel::Constrained::All(3); - GaussianFactor::shared_ptr expLinFactor(new JacobianFactor(0, A1, b, model)); + GaussianFactor::shared_ptr expLinFactor(new JacobianFactor(key1, A1, b, model)); EXPECT(assert_equal(*expLinFactor, *actLinFactor, 1e-5)); } @@ -193,7 +193,7 @@ TEST ( NonlinearEquality, allow_error_optimize ) { // add to a graph NonlinearFactorGraph graph; - graph.add(nle); + graph += nle; // initialize away from the ideal Pose2 initPose(0.0, 2.0, 3.0); @@ -231,8 +231,8 @@ TEST ( NonlinearEquality, allow_error_optimize_with_factors ) { // add to a graph NonlinearFactorGraph graph; - graph.add(nle); - graph.add(prior); + graph += nle; + graph += prior; // optimize Ordering ordering; @@ -267,8 +267,8 @@ TEST( testNonlinearEqualityConstraint, unary_basics ) { Point2 ptBad1(2.0, 2.0); config2.insert(key, ptBad1); EXPECT(constraint.active(config2)); - EXPECT(assert_equal(Vector_(2, 1.0, 0.0), constraint.evaluateError(ptBad1), tol)); - EXPECT(assert_equal(Vector_(2, 1.0, 0.0), constraint.unwhitenedError(config2), tol)); + EXPECT(assert_equal((Vector(2) << 1.0, 0.0), constraint.evaluateError(ptBad1), tol)); + EXPECT(assert_equal((Vector(2) << 1.0, 0.0), constraint.unwhitenedError(config2), tol)); EXPECT_DOUBLES_EQUAL(500.0, constraint.error(config2), tol); } @@ -277,21 +277,19 @@ TEST( testNonlinearEqualityConstraint, unary_linearization ) { Point2 pt(1.0, 2.0); Symbol key1('x',1); double mu = 1000.0; - Ordering ordering; - ordering += key; eq2D::UnaryEqualityConstraint constraint(pt, key, mu); Values config1; config1.insert(key, pt); - GaussianFactor::shared_ptr actual1 = constraint.linearize(config1, ordering); - GaussianFactor::shared_ptr expected1(new JacobianFactor(ordering[key], eye(2,2), zero(2), hard_model)); + GaussianFactor::shared_ptr actual1 = constraint.linearize(config1); + GaussianFactor::shared_ptr expected1(new JacobianFactor(key, eye(2,2), zero(2), hard_model)); EXPECT(assert_equal(*expected1, *actual1, tol)); Values config2; Point2 ptBad(2.0, 2.0); config2.insert(key, ptBad); - GaussianFactor::shared_ptr actual2 = constraint.linearize(config2, ordering); - GaussianFactor::shared_ptr expected2(new JacobianFactor(ordering[key], eye(2,2), Vector_(2,-1.0,0.0), hard_model)); + GaussianFactor::shared_ptr actual2 = constraint.linearize(config2); + GaussianFactor::shared_ptr expected2(new JacobianFactor(key, eye(2,2), (Vector(2) <<-1.0,0.0), hard_model)); EXPECT(assert_equal(*expected2, *actual2, tol)); } @@ -349,8 +347,8 @@ TEST( testNonlinearEqualityConstraint, odo_basics ) { config2.insert(key1, x1bad); config2.insert(key2, x2bad); EXPECT(constraint.active(config2)); - EXPECT(assert_equal(Vector_(2, -1.0, -1.0), constraint.evaluateError(x1bad, x2bad), tol)); - EXPECT(assert_equal(Vector_(2, -1.0, -1.0), constraint.unwhitenedError(config2), tol)); + EXPECT(assert_equal((Vector(2) << -1.0, -1.0), constraint.evaluateError(x1bad, x2bad), tol)); + EXPECT(assert_equal((Vector(2) << -1.0, -1.0), constraint.unwhitenedError(config2), tol)); EXPECT_DOUBLES_EQUAL(1000.0, constraint.error(config2), tol); } @@ -359,16 +357,14 @@ TEST( testNonlinearEqualityConstraint, odo_linearization ) { Point2 x1(1.0, 2.0), x2(2.0, 3.0), odom(1.0, 1.0); Symbol key1('x',1), key2('x',2); double mu = 1000.0; - Ordering ordering; - ordering += key1, key2; eq2D::OdoEqualityConstraint constraint(odom, key1, key2, mu); Values config1; config1.insert(key1, x1); config1.insert(key2, x2); - GaussianFactor::shared_ptr actual1 = constraint.linearize(config1, ordering); + GaussianFactor::shared_ptr actual1 = constraint.linearize(config1); GaussianFactor::shared_ptr expected1( - new JacobianFactor(ordering[key1], -eye(2,2), ordering[key2], + new JacobianFactor(key1, -eye(2,2), key2, eye(2,2), zero(2), hard_model)); EXPECT(assert_equal(*expected1, *actual1, tol)); @@ -377,10 +373,10 @@ TEST( testNonlinearEqualityConstraint, odo_linearization ) { Point2 x2bad(2.0, 2.0); config2.insert(key1, x1bad); config2.insert(key2, x2bad); - GaussianFactor::shared_ptr actual2 = constraint.linearize(config2, ordering); + GaussianFactor::shared_ptr actual2 = constraint.linearize(config2); GaussianFactor::shared_ptr expected2( - new JacobianFactor(ordering[key1], -eye(2,2), ordering[key2], - eye(2,2), Vector_(2, 1.0, 1.0), hard_model)); + new JacobianFactor(key1, -eye(2,2), key2, + eye(2,2), (Vector(2) << 1.0, 1.0), hard_model)); EXPECT(assert_equal(*expected2, *actual2, tol)); } @@ -436,17 +432,17 @@ TEST (testNonlinearEqualityConstraint, two_pose ) { Symbol l1('l',1), l2('l',2); Point2 pt_x1(1.0, 1.0), pt_x2(5.0, 6.0); - graph.add(eq2D::UnaryEqualityConstraint(pt_x1, x1)); - graph.add(eq2D::UnaryEqualityConstraint(pt_x2, x2)); + graph += eq2D::UnaryEqualityConstraint(pt_x1, x1); + graph += eq2D::UnaryEqualityConstraint(pt_x2, x2); Point2 z1(0.0, 5.0); SharedNoiseModel sigma(noiseModel::Isotropic::Sigma(2, 0.1)); - graph.add(simulated2D::Measurement(z1, sigma, x1,l1)); + graph += simulated2D::Measurement(z1, sigma, x1,l1); Point2 z2(-4.0, 0.0); - graph.add(simulated2D::Measurement(z2, sigma, x2,l2)); + graph += simulated2D::Measurement(z2, sigma, x2,l2); - graph.add(eq2D::PointEqualityConstraint(l1, l2)); + graph += eq2D::PointEqualityConstraint(l1, l2); Values initialEstimate; initialEstimate.insert(x1, pt_x1); @@ -475,20 +471,20 @@ TEST (testNonlinearEqualityConstraint, map_warp ) { // constant constraint on x1 Point2 pose1(1.0, 1.0); - graph.add(eq2D::UnaryEqualityConstraint(pose1, x1)); + graph += eq2D::UnaryEqualityConstraint(pose1, x1); SharedDiagonal sigma = noiseModel::Isotropic::Sigma(2, 0.1); // measurement from x1 to l1 Point2 z1(0.0, 5.0); - graph.add(simulated2D::Measurement(z1, sigma, x1, l1)); + graph += simulated2D::Measurement(z1, sigma, x1, l1); // measurement from x2 to l2 Point2 z2(-4.0, 0.0); - graph.add(simulated2D::Measurement(z2, sigma, x2, l2)); + graph += simulated2D::Measurement(z2, sigma, x2, l2); // equality constraint between l1 and l2 - graph.add(eq2D::PointEqualityConstraint(l1, l2)); + graph += eq2D::PointEqualityConstraint(l1, l2); // create an initial estimate Values initialEstimate; @@ -510,7 +506,7 @@ TEST (testNonlinearEqualityConstraint, map_warp ) { // make a realistic calibration matrix static double fov = 60; // degrees -static size_t w=640,h=480; +static int w=640,h=480; static Cal3_S2 K(fov,w,h); static boost::shared_ptr shK(new Cal3_S2(K)); @@ -524,7 +520,7 @@ typedef NonlinearEquality2 Point3Equality; TEST (testNonlinearEqualityConstraint, stereo_constrained ) { // create initial estimates - Rot3 faceDownY(Matrix_(3,3, + Rot3 faceDownY((Matrix)(Matrix(3,3) << 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, -1.0, 0.0)); @@ -542,16 +538,16 @@ TEST (testNonlinearEqualityConstraint, stereo_constrained ) { VGraph graph; // create equality constraints for poses - graph.add(NonlinearEquality(x1, camera1.pose())); - graph.add(NonlinearEquality(x2, camera2.pose())); + graph += NonlinearEquality(x1, camera1.pose()); + graph += NonlinearEquality(x2, camera2.pose()); // create factors SharedDiagonal vmodel = noiseModel::Unit::Create(2); - graph.add(GenericProjectionFactor(camera1.project(landmark), vmodel, x1, l1, shK)); - graph.add(GenericProjectionFactor(camera2.project(landmark), vmodel, x2, l2, shK)); + graph += GenericProjectionFactor(camera1.project(landmark), vmodel, x1, l1, shK); + graph += GenericProjectionFactor(camera2.project(landmark), vmodel, x2, l2, shK); // add equality constraint - graph.add(Point3Equality(l1, l2)); + graph += Point3Equality(l1, l2); // create initial data Point3 landmark1(0.5, 5.0, 0.0); diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index c490cbc6d..390257f02 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -32,7 +32,7 @@ #include #include #include -#include +#include using namespace std; using namespace gtsam; @@ -67,10 +67,10 @@ TEST( NonlinearFactor, equals ) TEST( NonlinearFactor, equals2 ) { // create a non linear factor graph - Graph fg = createNonlinearFactorGraph(); + NonlinearFactorGraph fg = createNonlinearFactorGraph(); // get two factors - Graph::sharedFactor f0 = fg[0], f1 = fg[1]; + NonlinearFactorGraph::sharedFactor f0 = fg[0], f1 = fg[1]; CHECK(f0->equals(*f0)); CHECK(!f0->equals(*f1)); @@ -81,13 +81,13 @@ TEST( NonlinearFactor, equals2 ) TEST( NonlinearFactor, NonlinearFactor ) { // create a non linear factor graph - Graph fg = createNonlinearFactorGraph(); + NonlinearFactorGraph fg = createNonlinearFactorGraph(); // create a values structure for the non linear factor graph Values cfg = createNoisyValues(); // get the factor "f1" from the factor graph - Graph::sharedFactor factor = fg[0]; + NonlinearFactorGraph::sharedFactor factor = fg[0]; // calculate the error_vector from the factor "f1" // error_vector = [0.1 0.1] @@ -108,13 +108,13 @@ TEST( NonlinearFactor, linearize_f1 ) Values c = createNoisyValues(); // Grab a non-linear factor - Graph nfg = createNonlinearFactorGraph(); - Graph::sharedFactor nlf = nfg[0]; + NonlinearFactorGraph nfg = createNonlinearFactorGraph(); + NonlinearFactorGraph::sharedFactor nlf = nfg[0]; // We linearize at noisy config from SmallExample - GaussianFactor::shared_ptr actual = nlf->linearize(c, *c.orderingArbitrary()); + GaussianFactor::shared_ptr actual = nlf->linearize(c); - GaussianFactorGraph lfg = createGaussianFactorGraph(*c.orderingArbitrary()); + GaussianFactorGraph lfg = createGaussianFactorGraph(); GaussianFactor::shared_ptr expected = lfg[0]; CHECK(assert_equal(*expected,*actual)); @@ -130,13 +130,13 @@ TEST( NonlinearFactor, linearize_f2 ) Values c = createNoisyValues(); // Grab a non-linear factor - Graph nfg = createNonlinearFactorGraph(); - Graph::sharedFactor nlf = nfg[1]; + NonlinearFactorGraph nfg = createNonlinearFactorGraph(); + NonlinearFactorGraph::sharedFactor nlf = nfg[1]; // We linearize at noisy config from SmallExample - GaussianFactor::shared_ptr actual = nlf->linearize(c, *c.orderingArbitrary()); + GaussianFactor::shared_ptr actual = nlf->linearize(c); - GaussianFactorGraph lfg = createGaussianFactorGraph(*c.orderingArbitrary()); + GaussianFactorGraph lfg = createGaussianFactorGraph(); GaussianFactor::shared_ptr expected = lfg[1]; CHECK(assert_equal(*expected,*actual)); @@ -146,14 +146,14 @@ TEST( NonlinearFactor, linearize_f2 ) TEST( NonlinearFactor, linearize_f3 ) { // Grab a non-linear factor - Graph nfg = createNonlinearFactorGraph(); - Graph::sharedFactor nlf = nfg[2]; + NonlinearFactorGraph nfg = createNonlinearFactorGraph(); + NonlinearFactorGraph::sharedFactor nlf = nfg[2]; // We linearize at noisy config from SmallExample Values c = createNoisyValues(); - GaussianFactor::shared_ptr actual = nlf->linearize(c, *c.orderingArbitrary()); + GaussianFactor::shared_ptr actual = nlf->linearize(c); - GaussianFactorGraph lfg = createGaussianFactorGraph(*c.orderingArbitrary()); + GaussianFactorGraph lfg = createGaussianFactorGraph(); GaussianFactor::shared_ptr expected = lfg[2]; CHECK(assert_equal(*expected,*actual)); @@ -163,14 +163,14 @@ TEST( NonlinearFactor, linearize_f3 ) TEST( NonlinearFactor, linearize_f4 ) { // Grab a non-linear factor - Graph nfg = createNonlinearFactorGraph(); - Graph::sharedFactor nlf = nfg[3]; + NonlinearFactorGraph nfg = createNonlinearFactorGraph(); + NonlinearFactorGraph::sharedFactor nlf = nfg[3]; // We linearize at noisy config from SmallExample Values c = createNoisyValues(); - GaussianFactor::shared_ptr actual = nlf->linearize(c, *c.orderingArbitrary()); + GaussianFactor::shared_ptr actual = nlf->linearize(c); - GaussianFactorGraph lfg = createGaussianFactorGraph(*c.orderingArbitrary()); + GaussianFactorGraph lfg = createGaussianFactorGraph(); GaussianFactor::shared_ptr expected = lfg[3]; CHECK(assert_equal(*expected,*actual)); @@ -180,13 +180,13 @@ TEST( NonlinearFactor, linearize_f4 ) TEST( NonlinearFactor, size ) { // create a non linear factor graph - Graph fg = createNonlinearFactorGraph(); + NonlinearFactorGraph fg = createNonlinearFactorGraph(); // create a values structure for the non linear factor graph Values cfg = createNoisyValues(); // get some factors from the graph - Graph::sharedFactor factor1 = fg[0], factor2 = fg[1], + NonlinearFactorGraph::sharedFactor factor1 = fg[0], factor2 = fg[1], factor3 = fg[2]; CHECK(factor1->size() == 1); @@ -197,27 +197,27 @@ TEST( NonlinearFactor, size ) /* ************************************************************************* */ TEST( NonlinearFactor, linearize_constraint1 ) { - Vector sigmas = Vector_(2, 0.2, 0.0); + Vector sigmas = (Vector(2) << 0.2, 0.0); SharedDiagonal constraint = noiseModel::Constrained::MixedSigmas(sigmas); Point2 mu(1., -1.); - Graph::sharedFactor f0(new simulated2D::Prior(mu, constraint, X(1))); + NonlinearFactorGraph::sharedFactor f0(new simulated2D::Prior(mu, constraint, X(1))); Values config; config.insert(X(1), Point2(1.0, 2.0)); - GaussianFactor::shared_ptr actual = f0->linearize(config, *config.orderingArbitrary()); + GaussianFactor::shared_ptr actual = f0->linearize(config); // create expected - Ordering ord(*config.orderingArbitrary()); - Vector b = Vector_(2, 0., -3.); - JacobianFactor expected(ord[X(1)], Matrix_(2,2, 5.0, 0.0, 0.0, 1.0), b, constraint); + Vector b = (Vector(2) << 0., -3.); + JacobianFactor expected(X(1), (Matrix(2, 2) << 5.0, 0.0, 0.0, 1.0), b, + noiseModel::Constrained::MixedSigmas((Vector(2) << 1.0, 0.0))); CHECK(assert_equal((const GaussianFactor&)expected, *actual)); } /* ************************************************************************* */ TEST( NonlinearFactor, linearize_constraint2 ) { - Vector sigmas = Vector_(2, 0.2, 0.0); + Vector sigmas = (Vector(2) << 0.2, 0.0); SharedDiagonal constraint = noiseModel::Constrained::MixedSigmas(sigmas); Point2 z3(1.,-1.); @@ -226,13 +226,13 @@ TEST( NonlinearFactor, linearize_constraint2 ) Values config; config.insert(X(1), Point2(1.0, 2.0)); config.insert(L(1), Point2(5.0, 4.0)); - GaussianFactor::shared_ptr actual = f0.linearize(config, *config.orderingArbitrary()); + GaussianFactor::shared_ptr actual = f0.linearize(config); // create expected - Ordering ord(*config.orderingArbitrary()); - Matrix A = Matrix_(2,2, 5.0, 0.0, 0.0, 1.0); - Vector b = Vector_(2, -15., -3.); - JacobianFactor expected(ord[X(1)], -1*A, ord[L(1)], A, b, constraint); + Matrix A = (Matrix(2, 2) << 5.0, 0.0, 0.0, 1.0); + Vector b = (Vector(2) << -15., -3.); + JacobianFactor expected(X(1), -1*A, L(1), A, b, + noiseModel::Constrained::MixedSigmas((Vector(2) << 1.0, 0.0))); CHECK(assert_equal((const GaussianFactor&)expected, *actual)); } @@ -240,7 +240,7 @@ TEST( NonlinearFactor, linearize_constraint2 ) class TestFactor4 : public NoiseModelFactor4 { public: typedef NoiseModelFactor4 Base; - TestFactor4() : Base(noiseModel::Diagonal::Sigmas(Vector_(1, 2.0)), X(1), X(2), X(3), X(4)) {} + TestFactor4() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0)), X(1), X(2), X(3), X(4)) {} virtual Vector evaluateError(const LieVector& x1, const LieVector& x2, const LieVector& x3, const LieVector& x4, @@ -249,10 +249,10 @@ public: boost::optional H3 = boost::none, boost::optional H4 = boost::none) const { if(H1) { - *H1 = Matrix_(1,1, 1.0); - *H2 = Matrix_(1,1, 2.0); - *H3 = Matrix_(1,1, 3.0); - *H4 = Matrix_(1,1, 4.0); + *H1 = (Matrix(1, 1) << 1.0); + *H2 = (Matrix(1, 1) << 2.0); + *H3 = (Matrix(1, 1) << 3.0); + *H4 = (Matrix(1, 1) << 4.0); } return (Vector(1) << x1 + x2 + x3 + x4).finished(); } @@ -266,30 +266,29 @@ public: TEST(NonlinearFactor, NoiseModelFactor4) { TestFactor4 tf; Values tv; - tv.insert(X(1), LieVector(1, 1.0)); - tv.insert(X(2), LieVector(1, 2.0)); - tv.insert(X(3), LieVector(1, 3.0)); - tv.insert(X(4), LieVector(1, 4.0)); - EXPECT(assert_equal(Vector_(1, 10.0), tf.unwhitenedError(tv))); + tv.insert(X(1), LieVector((Vector(1) << 1.0))); + tv.insert(X(2), LieVector((Vector(1) << 2.0))); + tv.insert(X(3), LieVector((Vector(1) << 3.0))); + tv.insert(X(4), LieVector((Vector(1) << 4.0))); + EXPECT(assert_equal((Vector(1) << 10.0), tf.unwhitenedError(tv))); DOUBLES_EQUAL(25.0/2.0, tf.error(tv), 1e-9); - Ordering ordering; ordering += X(1), X(2), X(3), X(4); - JacobianFactor jf(*boost::dynamic_pointer_cast(tf.linearize(tv, ordering))); - LONGS_EQUAL(jf.keys()[0], 0); - LONGS_EQUAL(jf.keys()[1], 1); - LONGS_EQUAL(jf.keys()[2], 2); - LONGS_EQUAL(jf.keys()[3], 3); - EXPECT(assert_equal(Matrix_(1,1, 0.5), jf.getA(jf.begin()))); - EXPECT(assert_equal(Matrix_(1,1, 1.0), jf.getA(jf.begin()+1))); - EXPECT(assert_equal(Matrix_(1,1, 1.5), jf.getA(jf.begin()+2))); - EXPECT(assert_equal(Matrix_(1,1, 2.0), jf.getA(jf.begin()+3))); - EXPECT(assert_equal(Vector_(1, -5.0), jf.getb())); + JacobianFactor jf(*boost::dynamic_pointer_cast(tf.linearize(tv))); + LONGS_EQUAL((long)X(1), (long)jf.keys()[0]); + LONGS_EQUAL((long)X(2), (long)jf.keys()[1]); + LONGS_EQUAL((long)X(3), (long)jf.keys()[2]); + LONGS_EQUAL((long)X(4), (long)jf.keys()[3]); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 0.5), jf.getA(jf.begin()))); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 1.0), jf.getA(jf.begin()+1))); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 1.5), jf.getA(jf.begin()+2))); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 2.0), jf.getA(jf.begin()+3))); + EXPECT(assert_equal((Vector)(Vector(1) << -5.0), jf.getb())); } /* ************************************************************************* */ class TestFactor5 : public NoiseModelFactor5 { public: typedef NoiseModelFactor5 Base; - TestFactor5() : Base(noiseModel::Diagonal::Sigmas(Vector_(1, 2.0)), X(1), X(2), X(3), X(4), X(5)) {} + TestFactor5() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0)), X(1), X(2), X(3), X(4), X(5)) {} virtual Vector evaluateError(const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, @@ -299,11 +298,11 @@ public: boost::optional H4 = boost::none, boost::optional H5 = boost::none) const { if(H1) { - *H1 = Matrix_(1,1, 1.0); - *H2 = Matrix_(1,1, 2.0); - *H3 = Matrix_(1,1, 3.0); - *H4 = Matrix_(1,1, 4.0); - *H5 = Matrix_(1,1, 5.0); + *H1 = (Matrix(1, 1) << 1.0); + *H2 = (Matrix(1, 1) << 2.0); + *H3 = (Matrix(1, 1) << 3.0); + *H4 = (Matrix(1, 1) << 4.0); + *H5 = (Matrix(1, 1) << 5.0); } return (Vector(1) << x1 + x2 + x3 + x4 + x5).finished(); } @@ -313,33 +312,32 @@ public: TEST(NonlinearFactor, NoiseModelFactor5) { TestFactor5 tf; Values tv; - tv.insert(X(1), LieVector(1, 1.0)); - tv.insert(X(2), LieVector(1, 2.0)); - tv.insert(X(3), LieVector(1, 3.0)); - tv.insert(X(4), LieVector(1, 4.0)); - tv.insert(X(5), LieVector(1, 5.0)); - EXPECT(assert_equal(Vector_(1, 15.0), tf.unwhitenedError(tv))); + tv.insert(X(1), LieVector((Vector(1) << 1.0))); + tv.insert(X(2), LieVector((Vector(1) << 2.0))); + tv.insert(X(3), LieVector((Vector(1) << 3.0))); + tv.insert(X(4), LieVector((Vector(1) << 4.0))); + tv.insert(X(5), LieVector((Vector(1) << 5.0))); + EXPECT(assert_equal((Vector(1) << 15.0), tf.unwhitenedError(tv))); DOUBLES_EQUAL(56.25/2.0, tf.error(tv), 1e-9); - Ordering ordering; ordering += X(1), X(2), X(3), X(4), X(5); - JacobianFactor jf(*boost::dynamic_pointer_cast(tf.linearize(tv, ordering))); - LONGS_EQUAL(jf.keys()[0], 0); - LONGS_EQUAL(jf.keys()[1], 1); - LONGS_EQUAL(jf.keys()[2], 2); - LONGS_EQUAL(jf.keys()[3], 3); - LONGS_EQUAL(jf.keys()[4], 4); - EXPECT(assert_equal(Matrix_(1,1, 0.5), jf.getA(jf.begin()))); - EXPECT(assert_equal(Matrix_(1,1, 1.0), jf.getA(jf.begin()+1))); - EXPECT(assert_equal(Matrix_(1,1, 1.5), jf.getA(jf.begin()+2))); - EXPECT(assert_equal(Matrix_(1,1, 2.0), jf.getA(jf.begin()+3))); - EXPECT(assert_equal(Matrix_(1,1, 2.5), jf.getA(jf.begin()+4))); - EXPECT(assert_equal(Vector_(1, -7.5), jf.getb())); + JacobianFactor jf(*boost::dynamic_pointer_cast(tf.linearize(tv))); + LONGS_EQUAL((long)X(1), (long)jf.keys()[0]); + LONGS_EQUAL((long)X(2), (long)jf.keys()[1]); + LONGS_EQUAL((long)X(3), (long)jf.keys()[2]); + LONGS_EQUAL((long)X(4), (long)jf.keys()[3]); + LONGS_EQUAL((long)X(5), (long)jf.keys()[4]); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 0.5), jf.getA(jf.begin()))); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 1.0), jf.getA(jf.begin()+1))); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 1.5), jf.getA(jf.begin()+2))); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 2.0), jf.getA(jf.begin()+3))); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 2.5), jf.getA(jf.begin()+4))); + EXPECT(assert_equal((Vector)(Vector(1) << -7.5), jf.getb())); } /* ************************************************************************* */ class TestFactor6 : public NoiseModelFactor6 { public: typedef NoiseModelFactor6 Base; - TestFactor6() : Base(noiseModel::Diagonal::Sigmas(Vector_(1, 2.0)), X(1), X(2), X(3), X(4), X(5), X(6)) {} + TestFactor6() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0)), X(1), X(2), X(3), X(4), X(5), X(6)) {} virtual Vector evaluateError(const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, @@ -350,12 +348,12 @@ public: boost::optional H5 = boost::none, boost::optional H6 = boost::none) const { if(H1) { - *H1 = Matrix_(1,1, 1.0); - *H2 = Matrix_(1,1, 2.0); - *H3 = Matrix_(1,1, 3.0); - *H4 = Matrix_(1,1, 4.0); - *H5 = Matrix_(1,1, 5.0); - *H6 = Matrix_(1,1, 6.0); + *H1 = (Matrix(1, 1) << 1.0); + *H2 = (Matrix(1, 1) << 2.0); + *H3 = (Matrix(1, 1) << 3.0); + *H4 = (Matrix(1, 1) << 4.0); + *H5 = (Matrix(1, 1) << 5.0); + *H6 = (Matrix(1, 1) << 6.0); } return (Vector(1) << x1 + x2 + x3 + x4 + x5 + x6).finished(); } @@ -366,29 +364,28 @@ public: TEST(NonlinearFactor, NoiseModelFactor6) { TestFactor6 tf; Values tv; - tv.insert(X(1), LieVector(1, 1.0)); - tv.insert(X(2), LieVector(1, 2.0)); - tv.insert(X(3), LieVector(1, 3.0)); - tv.insert(X(4), LieVector(1, 4.0)); - tv.insert(X(5), LieVector(1, 5.0)); - tv.insert(X(6), LieVector(1, 6.0)); - EXPECT(assert_equal(Vector_(1, 21.0), tf.unwhitenedError(tv))); + tv.insert(X(1), LieVector((Vector(1) << 1.0))); + tv.insert(X(2), LieVector((Vector(1) << 2.0))); + tv.insert(X(3), LieVector((Vector(1) << 3.0))); + tv.insert(X(4), LieVector((Vector(1) << 4.0))); + tv.insert(X(5), LieVector((Vector(1) << 5.0))); + tv.insert(X(6), LieVector((Vector(1) << 6.0))); + EXPECT(assert_equal((Vector(1) << 21.0), tf.unwhitenedError(tv))); DOUBLES_EQUAL(110.25/2.0, tf.error(tv), 1e-9); - Ordering ordering; ordering += X(1), X(2), X(3), X(4), X(5), X(6); - JacobianFactor jf(*boost::dynamic_pointer_cast(tf.linearize(tv, ordering))); - LONGS_EQUAL(jf.keys()[0], 0); - LONGS_EQUAL(jf.keys()[1], 1); - LONGS_EQUAL(jf.keys()[2], 2); - LONGS_EQUAL(jf.keys()[3], 3); - LONGS_EQUAL(jf.keys()[4], 4); - LONGS_EQUAL(jf.keys()[5], 5); - EXPECT(assert_equal(Matrix_(1,1, 0.5), jf.getA(jf.begin()))); - EXPECT(assert_equal(Matrix_(1,1, 1.0), jf.getA(jf.begin()+1))); - EXPECT(assert_equal(Matrix_(1,1, 1.5), jf.getA(jf.begin()+2))); - EXPECT(assert_equal(Matrix_(1,1, 2.0), jf.getA(jf.begin()+3))); - EXPECT(assert_equal(Matrix_(1,1, 2.5), jf.getA(jf.begin()+4))); - EXPECT(assert_equal(Matrix_(1,1, 3.0), jf.getA(jf.begin()+5))); - EXPECT(assert_equal(Vector_(1, -10.5), jf.getb())); + JacobianFactor jf(*boost::dynamic_pointer_cast(tf.linearize(tv))); + LONGS_EQUAL((long)X(1), (long)jf.keys()[0]); + LONGS_EQUAL((long)X(2), (long)jf.keys()[1]); + LONGS_EQUAL((long)X(3), (long)jf.keys()[2]); + LONGS_EQUAL((long)X(4), (long)jf.keys()[3]); + LONGS_EQUAL((long)X(5), (long)jf.keys()[4]); + LONGS_EQUAL((long)X(6), (long)jf.keys()[5]); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 0.5), jf.getA(jf.begin()))); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 1.0), jf.getA(jf.begin()+1))); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 1.5), jf.getA(jf.begin()+2))); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 2.0), jf.getA(jf.begin()+3))); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 2.5), jf.getA(jf.begin()+4))); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 3.0), jf.getA(jf.begin()+5))); + EXPECT(assert_equal((Vector)(Vector(1) << -10.5), jf.getb())); } @@ -396,10 +393,10 @@ TEST(NonlinearFactor, NoiseModelFactor6) { TEST( NonlinearFactor, clone_rekey ) { shared_nlf init(new TestFactor4()); - EXPECT_LONGS_EQUAL(X(1), init->keys()[0]); - EXPECT_LONGS_EQUAL(X(2), init->keys()[1]); - EXPECT_LONGS_EQUAL(X(3), init->keys()[2]); - EXPECT_LONGS_EQUAL(X(4), init->keys()[3]); + EXPECT_LONGS_EQUAL((long)X(1), (long)init->keys()[0]); + EXPECT_LONGS_EQUAL((long)X(2), (long)init->keys()[1]); + EXPECT_LONGS_EQUAL((long)X(3), (long)init->keys()[2]); + EXPECT_LONGS_EQUAL((long)X(4), (long)init->keys()[3]); // Standard clone shared_nlf actClone = init->clone(); @@ -416,16 +413,16 @@ TEST( NonlinearFactor, clone_rekey ) EXPECT(actRekey.get() != init.get()); // Ensure different pointers // Ensure init is unchanged - EXPECT_LONGS_EQUAL(X(1), init->keys()[0]); - EXPECT_LONGS_EQUAL(X(2), init->keys()[1]); - EXPECT_LONGS_EQUAL(X(3), init->keys()[2]); - EXPECT_LONGS_EQUAL(X(4), init->keys()[3]); + EXPECT_LONGS_EQUAL((long)X(1), (long)init->keys()[0]); + EXPECT_LONGS_EQUAL((long)X(2), (long)init->keys()[1]); + EXPECT_LONGS_EQUAL((long)X(3), (long)init->keys()[2]); + EXPECT_LONGS_EQUAL((long)X(4), (long)init->keys()[3]); // Check new keys - EXPECT_LONGS_EQUAL(X(5), actRekey->keys()[0]); - EXPECT_LONGS_EQUAL(X(6), actRekey->keys()[1]); - EXPECT_LONGS_EQUAL(X(7), actRekey->keys()[2]); - EXPECT_LONGS_EQUAL(X(8), actRekey->keys()[3]); + EXPECT_LONGS_EQUAL((long)X(5), (long)actRekey->keys()[0]); + EXPECT_LONGS_EQUAL((long)X(6), (long)actRekey->keys()[1]); + EXPECT_LONGS_EQUAL((long)X(7), (long)actRekey->keys()[2]); + EXPECT_LONGS_EQUAL((long)X(8), (long)actRekey->keys()[3]); } /* ************************************************************************* */ diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp index 5a35cbd94..9c2eddcc3 100644 --- a/tests/testNonlinearFactorGraph.cpp +++ b/tests/testNonlinearFactorGraph.cpp @@ -11,7 +11,7 @@ /** * @file testNonlinearFactorGraph.cpp - * @brief Unit tests for Non-Linear Factor Graph + * @brief Unit tests for Non-Linear Factor NonlinearFactorGraph * @brief testNonlinearFactorGraph * @author Carlos Nieto * @author Christian Potthast @@ -31,8 +31,9 @@ using namespace boost::assign; #include #include #include +#include +#include #include -#include using namespace gtsam; using namespace example; @@ -41,17 +42,17 @@ using symbol_shorthand::X; using symbol_shorthand::L; /* ************************************************************************* */ -TEST( Graph, equals ) +TEST( NonlinearFactorGraph, equals ) { - Graph fg = createNonlinearFactorGraph(); - Graph fg2 = createNonlinearFactorGraph(); + NonlinearFactorGraph fg = createNonlinearFactorGraph(); + NonlinearFactorGraph fg2 = createNonlinearFactorGraph(); CHECK( fg.equals(fg2) ); } /* ************************************************************************* */ -TEST( Graph, error ) +TEST( NonlinearFactorGraph, error ) { - Graph fg = createNonlinearFactorGraph(); + NonlinearFactorGraph fg = createNonlinearFactorGraph(); Values c1 = createValues(); double actual1 = fg.error(c1); DOUBLES_EQUAL( 0.0, actual1, 1e-9 ); @@ -62,41 +63,37 @@ TEST( Graph, error ) } /* ************************************************************************* */ -TEST( Graph, keys ) +TEST( NonlinearFactorGraph, keys ) { - Graph fg = createNonlinearFactorGraph(); + NonlinearFactorGraph fg = createNonlinearFactorGraph(); FastSet actual = fg.keys(); - LONGS_EQUAL(3, actual.size()); + LONGS_EQUAL(3, (long)actual.size()); FastSet::const_iterator it = actual.begin(); - LONGS_EQUAL(L(1), *(it++)); - LONGS_EQUAL(X(1), *(it++)); - LONGS_EQUAL(X(2), *(it++)); + LONGS_EQUAL((long)L(1), (long)*(it++)); + LONGS_EQUAL((long)X(1), (long)*(it++)); + LONGS_EQUAL((long)X(2), (long)*(it++)); } /* ************************************************************************* */ -TEST( Graph, GET_ORDERING) +TEST( NonlinearFactorGraph, GET_ORDERING) { -// Ordering expected; expected += "x1","l1","x2"; // For starting with x1,x2,l1 Ordering expected; expected += L(1), X(2), X(1); // For starting with l1,x1,x2 - Graph nlfg = createNonlinearFactorGraph(); - SymbolicFactorGraph::shared_ptr symbolic; - Ordering::shared_ptr ordering; - boost::tie(symbolic, ordering) = nlfg.symbolic(createNoisyValues()); - Ordering actual = *nlfg.orderingCOLAMD(createNoisyValues()); + NonlinearFactorGraph nlfg = createNonlinearFactorGraph(); + Ordering actual = Ordering::COLAMD(nlfg); EXPECT(assert_equal(expected,actual)); // Constrained ordering - put x2 at the end - std::map constraints; - constraints[X(2)] = 1; - Ordering actualConstrained = *nlfg.orderingCOLAMDConstrained(createNoisyValues(), constraints); Ordering expectedConstrained; expectedConstrained += L(1), X(1), X(2); + FastMap constraints; + constraints[X(2)] = 1; + Ordering actualConstrained = Ordering::COLAMDConstrained(nlfg, constraints); EXPECT(assert_equal(expectedConstrained, actualConstrained)); } /* ************************************************************************* */ -TEST( Graph, probPrime ) +TEST( NonlinearFactorGraph, probPrime ) { - Graph fg = createNonlinearFactorGraph(); + NonlinearFactorGraph fg = createNonlinearFactorGraph(); Values cfg = createValues(); // evaluate the probability of the factor graph @@ -106,39 +103,39 @@ TEST( Graph, probPrime ) } /* ************************************************************************* */ -TEST( Graph, linearize ) +TEST( NonlinearFactorGraph, linearize ) { - Graph fg = createNonlinearFactorGraph(); + NonlinearFactorGraph fg = createNonlinearFactorGraph(); Values initial = createNoisyValues(); - boost::shared_ptr > linearized = fg.linearize(initial, *initial.orderingArbitrary()); - FactorGraph expected = createGaussianFactorGraph(*initial.orderingArbitrary()); - CHECK(assert_equal(expected,*linearized)); // Needs correct linearizations + GaussianFactorGraph linearized = *fg.linearize(initial); + GaussianFactorGraph expected = createGaussianFactorGraph(); + CHECK(assert_equal(expected,linearized)); // Needs correct linearizations } /* ************************************************************************* */ -TEST( Graph, clone ) +TEST( NonlinearFactorGraph, clone ) { - Graph fg = createNonlinearFactorGraph(); - Graph actClone = fg.clone(); + NonlinearFactorGraph fg = createNonlinearFactorGraph(); + NonlinearFactorGraph actClone = fg.clone(); EXPECT(assert_equal(fg, actClone)); for (size_t i=0; i rekey_mapping; rekey_mapping.insert(make_pair(L(1), L(4))); - Graph actRekey = init.rekey(rekey_mapping); + NonlinearFactorGraph actRekey = init.rekey(rekey_mapping); // ensure deep clone - LONGS_EQUAL(init.size(), actRekey.size()); + LONGS_EQUAL((long)init.size(), (long)actRekey.size()); for (size_t i=0; i #include +#include +#include #include #include #include #include -#include +#include #include #include using namespace gtsam; -typedef NonlinearISAM PlanarISAM; - const double tol=1e-5; /* ************************************************************************* */ TEST(testNonlinearISAM, markov_chain ) { int reorder_interval = 2; - PlanarISAM isam(reorder_interval); // create an ISAM object + NonlinearISAM isamChol(reorder_interval, EliminatePreferCholesky); // create an ISAM object + NonlinearISAM isamQR(reorder_interval, EliminateQR); // create an ISAM object - SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector_(3, 3.0, 3.0, 0.5)); + SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(3) << 3.0, 3.0, 0.5)); Sampler sampler(model, 42u); // create initial graph Pose2 cur_pose; // start at origin NonlinearFactorGraph start_factors; - start_factors.add(NonlinearEquality(0, cur_pose)); + start_factors += NonlinearEquality(0, cur_pose); Values init; Values expected; init.insert(0, cur_pose); expected.insert(0, cur_pose); - isam.update(start_factors, init); + isamChol.update(start_factors, init); + isamQR.update(start_factors, init); // loop for a period of time to verify memory usage size_t nrPoses = 21; Pose2 z(1.0, 2.0, 0.1); for (size_t i=1; i<=nrPoses; ++i) { NonlinearFactorGraph new_factors; - new_factors.add(BetweenFactor(i-1, i, z, model)); + new_factors += BetweenFactor(i-1, i, z, model); Values new_init; - // perform a check on changing orderings - if (i == 5) { - Ordering ordering = isam.getOrdering(); - - // swap last two elements - Key last = ordering.pop_back().first; - Key secondLast = ordering.pop_back().first; - ordering.push_back(last); - ordering.push_back(secondLast); - isam.setOrdering(ordering); - - Ordering expected; expected += (0), (1), (2), (4), (3); - EXPECT(assert_equal(expected, isam.getOrdering())); - } - cur_pose = cur_pose.compose(z); new_init.insert(i, cur_pose.retract(sampler.sample())); expected.insert(i, cur_pose); - isam.update(new_factors, new_init); + isamChol.update(new_factors, new_init); + isamQR.update(new_factors, new_init); } // verify values - all but the last one should be very close - Values actual = isam.estimate(); + Values actualChol = isamChol.estimate(); for (size_t i=0; i(i), actual.at(i), tol)); + EXPECT(assert_equal(expected.at(i), actualChol.at(i), tol)); } + Values actualQR = isamQR.estimate(); + for (size_t i=0; i(i), actualQR.at(i), tol)); + } +} + +/* ************************************************************************* */ +TEST(testNonlinearISAM, markov_chain_with_disconnects ) { + int reorder_interval = 2; + NonlinearISAM isamChol(reorder_interval, EliminatePreferCholesky); // create an ISAM object + NonlinearISAM isamQR(reorder_interval, EliminateQR); // create an ISAM object + + SharedDiagonal model3 = noiseModel::Diagonal::Sigmas((Vector(3) << 3.0, 3.0, 0.5)); + SharedDiagonal model2 = noiseModel::Diagonal::Sigmas((Vector(2) << 2.0, 2.0)); + Sampler sampler(model3, 42u); + + // create initial graph + Pose2 cur_pose; // start at origin + NonlinearFactorGraph start_factors; + start_factors += NonlinearEquality(0, cur_pose); + + Values init; + Values expected; + init.insert(0, cur_pose); + expected.insert(0, cur_pose); + isamChol.update(start_factors, init); + isamQR.update(start_factors, init); + + size_t nrPoses = 21; + + // create a constrained constellation of landmarks + Key lm1 = nrPoses+1, lm2 = nrPoses+2, lm3 = nrPoses+3; + Point2 landmark1(3., 4.), landmark2(6., 4.), landmark3(6., 9.); + expected.insert(lm1, landmark1); + expected.insert(lm2, landmark2); + expected.insert(lm3, landmark3); + + // loop for a period of time to verify memory usage + Pose2 z(1.0, 2.0, 0.1); + for (size_t i=1; i<=nrPoses; ++i) { + NonlinearFactorGraph new_factors; + new_factors += BetweenFactor(i-1, i, z, model3); + Values new_init; + + cur_pose = cur_pose.compose(z); + new_init.insert(i, cur_pose.retract(sampler.sample())); + expected.insert(i, cur_pose); + + // Add a floating landmark constellation + if (i == 7) { + new_factors += PriorFactor(lm1, landmark1, model2); + new_factors += PriorFactor(lm2, landmark2, model2); + new_factors += PriorFactor(lm3, landmark3, model2); + + // Initialize to origin + new_init.insert(lm1, Point2()); + new_init.insert(lm2, Point2()); + new_init.insert(lm3, Point2()); + } + + isamChol.update(new_factors, new_init); + isamQR.update(new_factors, new_init); + } + + // verify values - all but the last one should be very close + Values actualChol = isamChol.estimate(); + for (size_t i=0; i(i), actualChol.at(i), tol)); + + Values actualQR = isamQR.estimate(); + for (size_t i=0; i(i), actualQR.at(i), tol)); + + // Check landmarks + EXPECT(assert_equal(expected.at(lm1), actualChol.at(lm1), tol)); + EXPECT(assert_equal(expected.at(lm2), actualChol.at(lm2), tol)); + EXPECT(assert_equal(expected.at(lm3), actualChol.at(lm3), tol)); + + EXPECT(assert_equal(expected.at(lm1), actualQR.at(lm1), tol)); + EXPECT(assert_equal(expected.at(lm2), actualQR.at(lm2), tol)); + EXPECT(assert_equal(expected.at(lm3), actualQR.at(lm3), tol)); +} + +/* ************************************************************************* */ +TEST(testNonlinearISAM, markov_chain_with_reconnect ) { + int reorder_interval = 2; + NonlinearISAM isamChol(reorder_interval, EliminatePreferCholesky); // create an ISAM object + NonlinearISAM isamQR(reorder_interval, EliminateQR); // create an ISAM object + + SharedDiagonal model3 = noiseModel::Diagonal::Sigmas((Vector(3) << 3.0, 3.0, 0.5)); + SharedDiagonal model2 = noiseModel::Diagonal::Sigmas((Vector(2) << 2.0, 2.0)); + Sampler sampler(model3, 42u); + + // create initial graph + Pose2 cur_pose; // start at origin + NonlinearFactorGraph start_factors; + start_factors += NonlinearEquality(0, cur_pose); + + Values init; + Values expected; + init.insert(0, cur_pose); + expected.insert(0, cur_pose); + isamChol.update(start_factors, init); + isamQR.update(start_factors, init); + + size_t nrPoses = 21; + + // create a constrained constellation of landmarks + Key lm1 = nrPoses+1, lm2 = nrPoses+2, lm3 = nrPoses+3; + Point2 landmark1(3., 4.), landmark2(6., 4.), landmark3(6., 9.); + expected.insert(lm1, landmark1); + expected.insert(lm2, landmark2); + expected.insert(lm3, landmark3); + + // loop for a period of time to verify memory usage + Pose2 z(1.0, 2.0, 0.1); + for (size_t i=1; i<=nrPoses; ++i) { + NonlinearFactorGraph new_factors; + new_factors += BetweenFactor(i-1, i, z, model3); + Values new_init; + + cur_pose = cur_pose.compose(z); + new_init.insert(i, cur_pose.retract(sampler.sample())); + expected.insert(i, cur_pose); + + // Add a floating landmark constellation + if (i == 7) { + new_factors += PriorFactor(lm1, landmark1, model2); + new_factors += PriorFactor(lm2, landmark2, model2); + new_factors += PriorFactor(lm3, landmark3, model2); + + // Initialize to origin + new_init.insert(lm1, Point2()); + new_init.insert(lm2, Point2()); + new_init.insert(lm3, Point2()); + } + + // Reconnect with observation later + if (i == 15) { + new_factors += BearingRangeFactor( + i, lm1, cur_pose.bearing(landmark1), cur_pose.range(landmark1), model2); + } + + isamChol.update(new_factors, new_init); + isamQR.update(new_factors, new_init); + } + + // verify values - all but the last one should be very close + Values actualChol = isamChol.estimate(); + for (size_t i=0; i(i), actualChol.at(i), tol)); + + Values actualQR = isamQR.estimate(); + for (size_t i=0; i(i), actualQR.at(i), tol)); + + // Check landmarks + EXPECT(assert_equal(expected.at(lm1), actualChol.at(lm1), tol)); + EXPECT(assert_equal(expected.at(lm2), actualChol.at(lm2), tol)); + EXPECT(assert_equal(expected.at(lm3), actualChol.at(lm3), tol)); + + EXPECT(assert_equal(expected.at(lm1), actualQR.at(lm1), tol)); + EXPECT(assert_equal(expected.at(lm2), actualQR.at(lm2), tol)); + EXPECT(assert_equal(expected.at(lm3), actualQR.at(lm3), tol)); } /* ************************************************************************* */ diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index aff07c15b..b976cca90 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -20,7 +20,7 @@ #include #include #include -#include +#include #include #include #include @@ -37,6 +37,7 @@ using namespace boost::assign; #include +#include using namespace std; using namespace gtsam; @@ -50,7 +51,7 @@ using symbol_shorthand::L; TEST( NonlinearOptimizer, iterateLM ) { // really non-linear factor graph - example::Graph fg(example::createReallyNonlinearFactorGraph()); + NonlinearFactorGraph fg(example::createReallyNonlinearFactorGraph()); // config far from minimum Point2 x0(3,0); @@ -74,7 +75,7 @@ TEST( NonlinearOptimizer, iterateLM ) /* ************************************************************************* */ TEST( NonlinearOptimizer, optimize ) { - example::Graph fg(example::createReallyNonlinearFactorGraph()); + NonlinearFactorGraph fg(example::createReallyNonlinearFactorGraph()); // test error at minimum Point2 xstar(0,0); @@ -114,7 +115,7 @@ TEST( NonlinearOptimizer, optimize ) /* ************************************************************************* */ TEST( NonlinearOptimizer, SimpleLMOptimizer ) { - example::Graph fg(example::createReallyNonlinearFactorGraph()); + NonlinearFactorGraph fg(example::createReallyNonlinearFactorGraph()); Point2 x0(3,3); Values c0; @@ -127,7 +128,7 @@ TEST( NonlinearOptimizer, SimpleLMOptimizer ) /* ************************************************************************* */ TEST( NonlinearOptimizer, SimpleGNOptimizer ) { - example::Graph fg(example::createReallyNonlinearFactorGraph()); + NonlinearFactorGraph fg(example::createReallyNonlinearFactorGraph()); Point2 x0(3,3); Values c0; @@ -140,7 +141,7 @@ TEST( NonlinearOptimizer, SimpleGNOptimizer ) /* ************************************************************************* */ TEST( NonlinearOptimizer, SimpleDLOptimizer ) { - example::Graph fg(example::createReallyNonlinearFactorGraph()); + NonlinearFactorGraph fg(example::createReallyNonlinearFactorGraph()); Point2 x0(3,3); Values c0; @@ -158,7 +159,7 @@ TEST( NonlinearOptimizer, optimization_method ) LevenbergMarquardtParams paramsChol; paramsChol.linearSolverType = LevenbergMarquardtParams::MULTIFRONTAL_CHOLESKY; - example::Graph fg = example::createReallyNonlinearFactorGraph(); + NonlinearFactorGraph fg = example::createReallyNonlinearFactorGraph(); Point2 x0(3,3); Values c0; @@ -179,8 +180,8 @@ TEST( NonlinearOptimizer, Factorization ) config.insert(X(2), Pose2(1.5,0.,0.)); NonlinearFactorGraph graph; - graph.add(PriorFactor(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10))); - graph.add(BetweenFactor(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1))); + graph += PriorFactor(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10)); + graph += BetweenFactor(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1)); Ordering ordering; ordering.push_back(X(1)); @@ -198,10 +199,10 @@ TEST( NonlinearOptimizer, Factorization ) /* ************************************************************************* */ TEST(NonlinearOptimizer, NullFactor) { - example::Graph fg = example::createReallyNonlinearFactorGraph(); + NonlinearFactorGraph fg = example::createReallyNonlinearFactorGraph(); // Add null factor - fg.push_back(example::Graph::sharedFactor()); + fg.push_back(NonlinearFactorGraph::sharedFactor()); // test error at minimum Point2 xstar(0,0); @@ -233,39 +234,123 @@ TEST(NonlinearOptimizer, NullFactor) { } /* ************************************************************************* */ -TEST(NonlinearOptimizer, MoreOptimization) { +TEST_UNSAFE(NonlinearOptimizer, MoreOptimization) { NonlinearFactorGraph fg; - fg.add(PriorFactor(0, Pose2(0,0,0), noiseModel::Isotropic::Sigma(3,1))); - fg.add(BetweenFactor(0, 1, Pose2(1,0,M_PI/2), noiseModel::Isotropic::Sigma(3,1))); - fg.add(BetweenFactor(1, 2, Pose2(1,0,M_PI/2), noiseModel::Isotropic::Sigma(3,1))); + fg += PriorFactor(0, Pose2(0, 0, 0), + noiseModel::Isotropic::Sigma(3, 1)); + fg += BetweenFactor(0, 1, Pose2(1, 0, M_PI / 2), + noiseModel::Isotropic::Sigma(3, 1)); + fg += BetweenFactor(1, 2, Pose2(1, 0, M_PI / 2), + noiseModel::Isotropic::Sigma(3, 1)); Values init; - init.insert(0, Pose2(3,4,-M_PI)); - init.insert(1, Pose2(10,2,-M_PI)); - init.insert(2, Pose2(11,7,-M_PI)); + init.insert(0, Pose2(3, 4, -M_PI)); + init.insert(1, Pose2(10, 2, -M_PI)); + init.insert(2, Pose2(11, 7, -M_PI)); Values expected; - expected.insert(0, Pose2(0,0,0)); - expected.insert(1, Pose2(1,0,M_PI/2)); - expected.insert(2, Pose2(1,1,M_PI)); + expected.insert(0, Pose2(0, 0, 0)); + expected.insert(1, Pose2(1, 0, M_PI / 2)); + expected.insert(2, Pose2(1, 1, M_PI)); + + VectorValues expectedGradient; + expectedGradient.insert(0,zero(3)); + expectedGradient.insert(1,zero(3)); + expectedGradient.insert(2,zero(3)); // Try LM and Dogleg - EXPECT(assert_equal(expected, LevenbergMarquardtOptimizer(fg, init).optimize())); + LevenbergMarquardtParams params; +// params.setVerbosityLM("TRYDELTA"); +// params.setVerbosity("TERMINATION"); + params.setlambdaUpperBound(1e9); +// params.setRelativeErrorTol(0); +// params.setAbsoluteErrorTol(0); + //params.setlambdaInitial(10); + + { + LevenbergMarquardtOptimizer optimizer(fg, init, params); + + // test convergence + Values actual = optimizer.optimize(); + EXPECT(assert_equal(expected, actual)); + + // Check that the gradient is zero + GaussianFactorGraph::shared_ptr linear = optimizer.linearize(); + EXPECT(assert_equal(expectedGradient,linear->gradientAtZero())); + } EXPECT(assert_equal(expected, DoglegOptimizer(fg, init).optimize())); + +// cout << "===================================================================================" << endl; + + // Try LM with diagonal damping + Values initBetter; + initBetter.insert(0, Pose2(3,4,0)); + initBetter.insert(1, Pose2(10,2,M_PI/3)); + initBetter.insert(2, Pose2(11,7,M_PI/2)); + + { + params.setDiagonalDamping(true); + LevenbergMarquardtOptimizer optimizer(fg, initBetter, params); + + // test the diagonal + GaussianFactorGraph::shared_ptr linear = optimizer.linearize(); + GaussianFactorGraph damped = optimizer.buildDampedSystem(*linear); + VectorValues d = linear->hessianDiagonal(), // + expectedDiagonal = d + params.lambdaInitial * d; + EXPECT(assert_equal(expectedDiagonal, damped.hessianDiagonal())); + + // test convergence (does not!) + Values actual = optimizer.optimize(); + EXPECT(assert_equal(expected, actual)); + + // Check that the gradient is zero (it is not!) + linear = optimizer.linearize(); + EXPECT(assert_equal(expectedGradient,linear->gradientAtZero())); + + // Check that the gradient is zero for damped system (it is not!) + damped = optimizer.buildDampedSystem(*linear); + VectorValues actualGradient = damped.gradientAtZero(); + EXPECT(assert_equal(expectedGradient,actualGradient)); + + /* This block was made to test the original initial guess "init" + // Check errors at convergence and errors in direction of gradient (decreases!) + EXPECT_DOUBLES_EQUAL(46.02558,fg.error(actual),1e-5); + EXPECT_DOUBLES_EQUAL(44.742237,fg.error(actual.retract(-0.01*actualGradient)),1e-5); + + // Check that solve yields gradient (it's not equal to gradient, as predicted) + VectorValues delta = damped.optimize(); + double factor = actualGradient[0][0]/delta[0][0]; + EXPECT(assert_equal(actualGradient,factor*delta)); + + // Still pointing downhill wrt actual gradient ! + EXPECT_DOUBLES_EQUAL( 0.1056157,dot(-1*actualGradient,delta),1e-3); + + // delta.print("This is the delta value computed by LM with diagonal damping"); + + // Still pointing downhill wrt expected gradient (IT DOES NOT! actually they are perpendicular) + EXPECT_DOUBLES_EQUAL( 0.0,dot(-1*expectedGradient,delta),1e-5); + + // Check errors at convergence and errors in direction of solution (does not decrease!) + EXPECT_DOUBLES_EQUAL(46.0254859,fg.error(actual.retract(delta)),1e-5); + + // Check errors at convergence and errors at a small step in direction of solution (does not decrease!) + EXPECT_DOUBLES_EQUAL(46.0255,fg.error(actual.retract(0.01*delta)),1e-3); + */ + } } /* ************************************************************************* */ TEST(NonlinearOptimizer, MoreOptimizationWithHuber) { NonlinearFactorGraph fg; - fg.add(PriorFactor(0, Pose2(0,0,0), noiseModel::Isotropic::Sigma(3,1))); - fg.add(BetweenFactor(0, 1, Pose2(1,0,M_PI/2), + fg += PriorFactor(0, Pose2(0,0,0), noiseModel::Isotropic::Sigma(3,1)); + fg += BetweenFactor(0, 1, Pose2(1,0,M_PI/2), noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(2.0), - noiseModel::Isotropic::Sigma(3,1)))); - fg.add(BetweenFactor(1, 2, Pose2(1,0,M_PI/2), + noiseModel::Isotropic::Sigma(3,1))); + fg += BetweenFactor(1, 2, Pose2(1,0,M_PI/2), noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(3.0), - noiseModel::Isotropic::Sigma(3,1)))); + noiseModel::Isotropic::Sigma(3,1))); Values init; init.insert(0, Pose2(10,10,0)); @@ -282,6 +367,100 @@ TEST(NonlinearOptimizer, MoreOptimizationWithHuber) { EXPECT(assert_equal(expected, DoglegOptimizer(fg, init).optimize())); } +/* ************************************************************************* */ +TEST(NonlinearOptimizer, disconnected_graph) { + Values expected; + expected.insert(X(1), Pose2(0.,0.,0.)); + expected.insert(X(2), Pose2(1.5,0.,0.)); + expected.insert(X(3), Pose2(3.0,0.,0.)); + + Values init; + init.insert(X(1), Pose2(0.,0.,0.)); + init.insert(X(2), Pose2(0.,0.,0.)); + init.insert(X(3), Pose2(0.,0.,0.)); + + NonlinearFactorGraph graph; + graph += PriorFactor(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3,1)); + graph += BetweenFactor(X(1),X(2), Pose2(1.5,0.,0.), noiseModel::Isotropic::Sigma(3,1)); + graph += PriorFactor(X(3), Pose2(3.,0.,0.), noiseModel::Isotropic::Sigma(3,1)); + + EXPECT(assert_equal(expected, LevenbergMarquardtOptimizer(graph, init).optimize())); +} + +/* ************************************************************************* */ +#include + +class IterativeLM: public LevenbergMarquardtOptimizer { + + /// Solver specific parameters + ConjugateGradientParameters cgParams_; + +public: + /// Constructor + IterativeLM(const NonlinearFactorGraph& graph, const Values& initialValues, + const ConjugateGradientParameters &p, + const LevenbergMarquardtParams& params = LevenbergMarquardtParams()) : + LevenbergMarquardtOptimizer(graph, initialValues, params), cgParams_(p) { + } + + /// Solve that uses conjugate gradient + virtual VectorValues solve(const GaussianFactorGraph &gfg, + const Values& initial, const NonlinearOptimizerParams& params) const { + VectorValues zeros = initial.zeroVectors(); + return conjugateGradientDescent(gfg, zeros, cgParams_); + } +}; + +/* ************************************************************************* */ +TEST(NonlinearOptimizer, subclass_solver) { + Values expected; + expected.insert(X(1), Pose2(0.,0.,0.)); + expected.insert(X(2), Pose2(1.5,0.,0.)); + expected.insert(X(3), Pose2(3.0,0.,0.)); + + Values init; + init.insert(X(1), Pose2(0.,0.,0.)); + init.insert(X(2), Pose2(0.,0.,0.)); + init.insert(X(3), Pose2(0.,0.,0.)); + + NonlinearFactorGraph graph; + graph += PriorFactor(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3,1)); + graph += BetweenFactor(X(1),X(2), Pose2(1.5,0.,0.), noiseModel::Isotropic::Sigma(3,1)); + graph += PriorFactor(X(3), Pose2(3.,0.,0.), noiseModel::Isotropic::Sigma(3,1)); + + ConjugateGradientParameters p; + Values actual = IterativeLM(graph, init, p).optimize(); + EXPECT(assert_equal(expected, actual, 1e-4)); +} + +/* ************************************************************************* */ +#include +TEST( NonlinearOptimizer, logfile ) +{ + NonlinearFactorGraph fg(example::createReallyNonlinearFactorGraph()); + + Point2 x0(3,3); + Values c0; + c0.insert(X(1), x0); + + // Levenberg-Marquardt + LevenbergMarquardtParams lmParams; + static const string filename("testNonlinearOptimizer.log"); + lmParams.setLogFile(filename); + CHECK(lmParams.getLogFile()==filename); + LevenbergMarquardtOptimizer(fg, c0, lmParams).optimize(); + +// stringstream expected,actual; +// ifstream ifs(("../../gtsam/tests/" + filename).c_str()); +// if(!ifs) throw std::runtime_error(filename); +// expected << ifs.rdbuf(); +// ifs.close(); +// ifstream ifs2(filename.c_str()); +// if(!ifs2) throw std::runtime_error(filename); +// actual << ifs2.rdbuf(); +// EXPECT(actual.str()==expected.str()); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/tests/testRot3Optimization.cpp b/tests/testRot3Optimization.cpp index 89b621ed2..31cf68ebd 100644 --- a/tests/testRot3Optimization.cpp +++ b/tests/testRot3Optimization.cpp @@ -24,7 +24,7 @@ #include #include #include -#include +#include #include #include @@ -41,11 +41,11 @@ TEST(Rot3, optimize) { Values truth; Values initial; Graph fg; - fg.add(Prior(Symbol('r',0), Rot3(), noiseModel::Isotropic::Sigma(3, 0.01))); + fg += Prior(Symbol('r',0), Rot3(), noiseModel::Isotropic::Sigma(3, 0.01)); for(int j=0; j<6; ++j) { truth.insert(Symbol('r',j), Rot3::Rz(M_PI/3.0 * double(j))); initial.insert(Symbol('r',j), Rot3::Rz(M_PI/3.0 * double(j) + 0.1 * double(j%2))); - fg.add(Between(Symbol('r',j), Symbol('r',(j+1)%6), Rot3::Rz(M_PI/3.0), noiseModel::Isotropic::Sigma(3, 0.01))); + fg += Between(Symbol('r',j), Symbol('r',(j+1)%6), Rot3::Rz(M_PI/3.0), noiseModel::Isotropic::Sigma(3, 0.01)); } Values final = GaussNewtonOptimizer(fg, initial).optimize(); diff --git a/tests/testSerializationSLAM.cpp b/tests/testSerializationSLAM.cpp index 57a1e5cb3..c9f434da2 100644 --- a/tests/testSerializationSLAM.cpp +++ b/tests/testSerializationSLAM.cpp @@ -16,6 +16,10 @@ * @date Feb 7, 2012 */ +#include + +#if 0 + #include //#include #include @@ -29,9 +33,8 @@ #include #include #include -#include +#include #include -#include #include #include #include @@ -49,7 +52,6 @@ #include #include -#include using namespace std; using namespace gtsam; @@ -235,11 +237,11 @@ TEST (testSerializationSLAM, smallExample_linear) { using namespace example; Ordering ordering; ordering += X(1),X(2),L(1); - GaussianFactorGraph fg = createGaussianFactorGraph(ordering); EXPECT(equalsObj(ordering)); EXPECT(equalsXML(ordering)); EXPECT(equalsBinary(ordering)); + GaussianFactorGraph fg = createGaussianFactorGraph(); EXPECT(equalsObj(fg)); EXPECT(equalsXML(fg)); EXPECT(equalsBinary(fg)); @@ -253,10 +255,8 @@ TEST (testSerializationSLAM, smallExample_linear) { /* ************************************************************************* */ TEST (testSerializationSLAM, gaussianISAM) { using namespace example; - Ordering ordering; - GaussianFactorGraph smoother; - boost::tie(smoother, ordering) = createSmoother(7); - BayesTree bayesTree = *GaussianMultifrontalSolver(smoother).eliminate(); + GaussianFactorGraph smoother = createSmoother(7); + GaussianBayesTree bayesTree = *smoother.eliminateMultifrontal(); GaussianISAM isam(bayesTree); EXPECT(equalsObj(isam)); @@ -287,8 +287,8 @@ TEST (testSerializationSLAM, smallExample_nonlinear) { /* ************************************************************************* */ TEST (testSerializationSLAM, factors) { - LieVector lieVector(4, 1.0, 2.0, 3.0, 4.0); - LieMatrix lieMatrix(2, 3, 1.0, 2.0, 3.0, 4.0, 5.0 ,6.0); + LieVector lieVector((Vector(4) << 1.0, 2.0, 3.0, 4.0)); + LieMatrix lieMatrix((Matrix(2, 3) << 1.0, 2.0, 3.0, 4.0, 5.0 ,6.0)); Point2 point2(1.0, 2.0); StereoPoint2 stereoPoint2(1.0, 2.0, 3.0); Point3 point3(1.0, 2.0, 3.0); @@ -408,66 +408,66 @@ TEST (testSerializationSLAM, factors) { NonlinearFactorGraph graph; - graph.add(priorFactorLieVector); - graph.add(priorFactorLieMatrix); - graph.add(priorFactorPoint2); - graph.add(priorFactorStereoPoint2); - graph.add(priorFactorPoint3); - graph.add(priorFactorRot2); - graph.add(priorFactorRot3); - graph.add(priorFactorPose2); - graph.add(priorFactorPose3); - graph.add(priorFactorCal3_S2); - graph.add(priorFactorCal3DS2); - graph.add(priorFactorCalibratedCamera); - graph.add(priorFactorSimpleCamera); - graph.add(priorFactorStereoCamera); + graph += priorFactorLieVector; + graph += priorFactorLieMatrix; + graph += priorFactorPoint2; + graph += priorFactorStereoPoint2; + graph += priorFactorPoint3; + graph += priorFactorRot2; + graph += priorFactorRot3; + graph += priorFactorPose2; + graph += priorFactorPose3; + graph += priorFactorCal3_S2; + graph += priorFactorCal3DS2; + graph += priorFactorCalibratedCamera; + graph += priorFactorSimpleCamera; + graph += priorFactorStereoCamera; - graph.add(betweenFactorLieVector); - graph.add(betweenFactorLieMatrix); - graph.add(betweenFactorPoint2); - graph.add(betweenFactorPoint3); - graph.add(betweenFactorRot2); - graph.add(betweenFactorRot3); - graph.add(betweenFactorPose2); - graph.add(betweenFactorPose3); + graph += betweenFactorLieVector; + graph += betweenFactorLieMatrix; + graph += betweenFactorPoint2; + graph += betweenFactorPoint3; + graph += betweenFactorRot2; + graph += betweenFactorRot3; + graph += betweenFactorPose2; + graph += betweenFactorPose3; - graph.add(nonlinearEqualityLieVector); - graph.add(nonlinearEqualityLieMatrix); - graph.add(nonlinearEqualityPoint2); - graph.add(nonlinearEqualityStereoPoint2); - graph.add(nonlinearEqualityPoint3); - graph.add(nonlinearEqualityRot2); - graph.add(nonlinearEqualityRot3); - graph.add(nonlinearEqualityPose2); - graph.add(nonlinearEqualityPose3); - graph.add(nonlinearEqualityCal3_S2); - graph.add(nonlinearEqualityCal3DS2); - graph.add(nonlinearEqualityCalibratedCamera); - graph.add(nonlinearEqualitySimpleCamera); - graph.add(nonlinearEqualityStereoCamera); + graph += nonlinearEqualityLieVector; + graph += nonlinearEqualityLieMatrix; + graph += nonlinearEqualityPoint2; + graph += nonlinearEqualityStereoPoint2; + graph += nonlinearEqualityPoint3; + graph += nonlinearEqualityRot2; + graph += nonlinearEqualityRot3; + graph += nonlinearEqualityPose2; + graph += nonlinearEqualityPose3; + graph += nonlinearEqualityCal3_S2; + graph += nonlinearEqualityCal3DS2; + graph += nonlinearEqualityCalibratedCamera; + graph += nonlinearEqualitySimpleCamera; + graph += nonlinearEqualityStereoCamera; - graph.add(rangeFactorPosePoint2); - graph.add(rangeFactorPosePoint3); - graph.add(rangeFactorPose2); - graph.add(rangeFactorPose3); - graph.add(rangeFactorCalibratedCameraPoint); - graph.add(rangeFactorSimpleCameraPoint); - graph.add(rangeFactorCalibratedCamera); - graph.add(rangeFactorSimpleCamera); + graph += rangeFactorPosePoint2; + graph += rangeFactorPosePoint3; + graph += rangeFactorPose2; + graph += rangeFactorPose3; + graph += rangeFactorCalibratedCameraPoint; + graph += rangeFactorSimpleCameraPoint; + graph += rangeFactorCalibratedCamera; + graph += rangeFactorSimpleCamera; - graph.add(bearingFactor2D); + graph += bearingFactor2D; - graph.add(bearingRangeFactor2D); + graph += bearingRangeFactor2D; - graph.add(genericProjectionFactorCal3_S2); - graph.add(genericProjectionFactorCal3DS2); + graph += genericProjectionFactorCal3_S2; + graph += genericProjectionFactorCal3DS2; - graph.add(generalSFMFactorCal3_S2); + graph += generalSFMFactorCal3_S2; - graph.add(generalSFMFactor2Cal3_S2); + graph += generalSFMFactor2Cal3_S2; - graph.add(genericStereoFactor3D); + graph += genericStereoFactor3D; // text @@ -674,6 +674,8 @@ TEST (testSerializationSLAM, factors) { EXPECT(equalsBinary(genericStereoFactor3D)); } +#endif + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/tests/testSimulated2DOriented.cpp b/tests/testSimulated2DOriented.cpp index 05d64653d..dcc31e0ec 100644 --- a/tests/testSimulated2DOriented.cpp +++ b/tests/testSimulated2DOriented.cpp @@ -18,7 +18,7 @@ #include #include -#include +#include #include #include @@ -58,13 +58,13 @@ TEST( simulated2DOriented, Dprior ) TEST( simulated2DOriented, constructor ) { Pose2 measurement(0.2, 0.3, 0.1); - SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector_(3, 1., 1., 1.)); + SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(3) << 1., 1., 1.)); simulated2DOriented::Odometry factor(measurement, model, X(1), X(2)); simulated2DOriented::Values config; config.insert(X(1), Pose2(1., 0., 0.2)); config.insert(X(2), Pose2(2., 0., 0.1)); - boost::shared_ptr lf = factor.linearize(config, *config.orderingArbitrary()); + boost::shared_ptr lf = factor.linearize(config); } /* ************************************************************************* */ diff --git a/tests/testSimulated3D.cpp b/tests/testSimulated3D.cpp index 9599dfbf3..dcfcc735b 100644 --- a/tests/testSimulated3D.cpp +++ b/tests/testSimulated3D.cpp @@ -16,7 +16,7 @@ **/ #include -#include +#include #include #include #include diff --git a/tests/testSubgraphPreconditioner.cpp b/tests/testSubgraphPreconditioner.cpp index 53ba92331..765c1e1ca 100644 --- a/tests/testSubgraphPreconditioner.cpp +++ b/tests/testSubgraphPreconditioner.cpp @@ -15,17 +15,18 @@ * @author Frank Dellaert **/ +#include + +#if 0 + #include -#include -#include +#include #include #include -#include #include +#include #include -#include - #include #include #include @@ -130,12 +131,12 @@ TEST( SubgraphPreconditioner, system ) // y1 = perturbed y0 VectorValues y1 = zeros; - y1[1] = Vector_(2, 1.0, -1.0); + y1[1] = (Vector(2) << 1.0, -1.0); // Check corresponding x values VectorValues expected_x1 = xtrue, x1 = system.x(y1); - expected_x1[1] = Vector_(2, 2.01, 2.99); - expected_x1[0] = Vector_(2, 3.01, 2.99); + expected_x1[1] = (Vector(2) << 2.01, 2.99); + expected_x1[0] = (Vector(2) << 3.01, 2.99); CHECK(assert_equal(xtrue, system.x(y0))); CHECK(assert_equal(expected_x1,system.x(y1))); @@ -149,28 +150,28 @@ TEST( SubgraphPreconditioner, system ) VectorValues expected_gx0 = zeros; VectorValues expected_gx1 = zeros; CHECK(assert_equal(expected_gx0,gradient(Ab,xtrue))); - expected_gx1[2] = Vector_(2, -100., 100.); - expected_gx1[4] = Vector_(2, -100., 100.); - expected_gx1[1] = Vector_(2, 200., -200.); - expected_gx1[3] = Vector_(2, -100., 100.); - expected_gx1[0] = Vector_(2, 100., -100.); + expected_gx1[2] = (Vector(2) << -100., 100.); + expected_gx1[4] = (Vector(2) << -100., 100.); + expected_gx1[1] = (Vector(2) << 200., -200.); + expected_gx1[3] = (Vector(2) << -100., 100.); + expected_gx1[0] = (Vector(2) << 100., -100.); CHECK(assert_equal(expected_gx1,gradient(Ab,x1))); // Test gradient in y VectorValues expected_gy0 = zeros; VectorValues expected_gy1 = zeros; - expected_gy1[2] = Vector_(2, 2., -2.); - expected_gy1[4] = Vector_(2, -2., 2.); - expected_gy1[1] = Vector_(2, 3., -3.); - expected_gy1[3] = Vector_(2, -1., 1.); - expected_gy1[0] = Vector_(2, 1., -1.); + expected_gy1[2] = (Vector(2) << 2., -2.); + expected_gy1[4] = (Vector(2) << -2., 2.); + expected_gy1[1] = (Vector(2) << 3., -3.); + expected_gy1[3] = (Vector(2) << -1., 1.); + expected_gy1[0] = (Vector(2) << 1., -1.); CHECK(assert_equal(expected_gy0,gradient(system,y0))); CHECK(assert_equal(expected_gy1,gradient(system,y1))); // Check it numerically for good measure // TODO use boost::bind(&SubgraphPreconditioner::error,&system,_1) // Vector numerical_g1 = numericalGradient (error, y1, 0.001); - // Vector expected_g1 = Vector_(18, 0., 0., 0., 0., 2., -2., 0., 0., -2., 2., + // Vector expected_g1 = (Vector(18) << 0., 0., 0., 0., 2., -2., 0., 0., -2., 2., // 3., -3., 0., 0., -1., 1., 1., -1.); // CHECK(assert_equal(expected_g1,numerical_g1)); } @@ -203,7 +204,7 @@ TEST( SubgraphPreconditioner, conjugateGradients ) VectorValues y0 = VectorValues::Zero(xbar); VectorValues y1 = y0; - y1[1] = Vector_(2, 1.0, -1.0); + y1[1] = (Vector(2) << 1.0, -1.0); VectorValues x1 = system.x(y1); // Solve for the remaining constraints using PCG @@ -217,6 +218,8 @@ TEST( SubgraphPreconditioner, conjugateGradients ) CHECK(assert_equal(xtrue,actual2,1e-4)); } +#endif + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/tests/testSubgraphSolver.cpp b/tests/testSubgraphSolver.cpp index 700f35261..f5d0384f2 100644 --- a/tests/testSubgraphSolver.cpp +++ b/tests/testSubgraphSolver.cpp @@ -15,18 +15,19 @@ * @author Yong-Dian Jian **/ +#include + +#if 0 + #include -#include -#include +#include #include #include #include #include -#include +#include #include -#include - #include #include #include @@ -109,6 +110,8 @@ TEST( SubgraphSolver, constructor3 ) DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5); } +#endif + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/tests/testSummarization.cpp b/tests/testSummarization.cpp deleted file mode 100644 index bf7342202..000000000 --- a/tests/testSummarization.cpp +++ /dev/null @@ -1,242 +0,0 @@ -/** - * @file testSummarization.cpp - * - * @brief Test ported from MastSLAM for a simple batch summarization technique - * - * @date May 7, 2013 - * @author Alex Cunningham - */ - -#include -#include - -#include - -#include - -#include - -#include -#include -#include - -#include -#include -#include - -using namespace std; -using namespace boost::assign; -using namespace gtsam; - -const double tol=1e-5; - -typedef gtsam::PriorFactor PosePrior; -typedef gtsam::BetweenFactor PoseBetween; -typedef gtsam::BearingRangeFactor PosePointBearingRange; - -gtsam::noiseModel::Base::shared_ptr model2 = noiseModel::Unit::Create(2); -gtsam::noiseModel::Base::shared_ptr model3 = noiseModel::Unit::Create(3); - -/* ************************************************************************* */ -TEST( testSummarization, example_from_ddf1 ) { - Key xA0 = LabeledSymbol('x', 'A', 0), - xA1 = LabeledSymbol('x', 'A', 1), - xA2 = LabeledSymbol('x', 'A', 2); - Key lA3 = LabeledSymbol('l', 'A', 3), lA5 = LabeledSymbol('l', 'A', 5); - - SharedDiagonal diagmodel2 = noiseModel::Unit::Create(2); - SharedDiagonal diagmodel4 = noiseModel::Unit::Create(4); - - Pose2 pose0; - Pose2 pose1(1.0, 0.0, 0.0); - Pose2 pose2(2.0, 0.0, 0.0); - Point2 landmark3(3.0, 3.0); - Point2 landmark5(5.0, 5.0); - - Values values; - values.insert(xA0, pose0); - values.insert(xA1, pose1); - values.insert(xA2, pose2); - values.insert(lA3, landmark3); - values.insert(lA5, landmark5); - - // build from nonlinear graph/values - NonlinearFactorGraph graph; - graph.add(PosePrior(xA0, Pose2(), model3)); - graph.add(PoseBetween(xA0, xA1, pose0.between(pose1), model3)); - graph.add(PoseBetween(xA1, xA2, pose1.between(pose2), model3)); - graph.add(PosePointBearingRange(xA0, lA3, pose0.bearing(landmark3), pose0.range(landmark3), model2)); - graph.add(PosePointBearingRange(xA1, lA3, pose1.bearing(landmark3), pose1.range(landmark3), model2)); - graph.add(PosePointBearingRange(xA2, lA5, pose2.bearing(landmark5), pose2.range(landmark5), model2)); - - KeySet saved_keys; - saved_keys += lA3, lA5; - - { - // Summarize to a linear system - GaussianFactorGraph actLinGraph; Ordering actOrdering; - SummarizationMode mode = PARTIAL_QR; - boost::tie(actLinGraph, actOrdering) = summarize(graph, values, saved_keys, mode); - - Ordering expSumOrdering; expSumOrdering += xA0, xA1, xA2, lA3, lA5; - EXPECT(assert_equal(expSumOrdering, actOrdering)); - - // Does not split out subfactors where possible - GaussianFactorGraph expLinGraph; - expLinGraph.add( - expSumOrdering[lA3], - Matrix_(4,2, - 0.595867, 0.605092, - 0.0, -0.406109, - 0.0, 0.0, - 0.0, 0.0), - expSumOrdering[lA5], - Matrix_(4,2, - -0.125971, -0.160052, - 0.13586, 0.301096, - 0.268667, 0.31703, - 0.0, -0.131698), - zero(4), diagmodel4); - EXPECT(assert_equal(expLinGraph, actLinGraph, tol)); - - // Summarize directly from a nonlinear graph to another nonlinear graph - NonlinearFactorGraph actContainerGraph = summarizeAsNonlinearContainer(graph, values, saved_keys, mode); - NonlinearFactorGraph expContainerGraph = LinearContainerFactor::convertLinearGraph(expLinGraph, expSumOrdering); - - EXPECT(assert_equal(expContainerGraph, actContainerGraph, tol)); - } - - { - // Summarize to a linear system using cholesky - compare to previous version - GaussianFactorGraph actLinGraph; Ordering actOrdering; - SummarizationMode mode = PARTIAL_CHOLESKY; - boost::tie(actLinGraph, actOrdering) = summarize(graph, values, saved_keys, mode); - - Ordering expSumOrdering; expSumOrdering += xA0, xA1, xA2, lA3, lA5; - EXPECT(assert_equal(expSumOrdering, actOrdering)); - - // Does not split out subfactors where possible - GaussianFactorGraph expLinGraph; - expLinGraph.add(HessianFactor(JacobianFactor( - expSumOrdering[lA3], - Matrix_(4,2, - 0.595867, 0.605092, - 0.0, -0.406109, - 0.0, 0.0, - 0.0, 0.0), - expSumOrdering[lA5], - Matrix_(4,2, - -0.125971, -0.160052, - 0.13586, 0.301096, - 0.268667, 0.31703, - 0.0, -0.131698), - zero(4), diagmodel4))); - EXPECT(assert_equal(expLinGraph, actLinGraph, tol)); - - // Summarize directly from a nonlinear graph to another nonlinear graph - NonlinearFactorGraph actContainerGraph = summarizeAsNonlinearContainer(graph, values, saved_keys, mode); - NonlinearFactorGraph expContainerGraph = LinearContainerFactor::convertLinearGraph(expLinGraph, expSumOrdering); - - EXPECT(assert_equal(expContainerGraph, actContainerGraph, tol)); - } - - { - // Summarize to a linear system with joint factor graph version - GaussianFactorGraph actLinGraph; Ordering actOrdering; - SummarizationMode mode = SEQUENTIAL_QR; - boost::tie(actLinGraph, actOrdering) = summarize(graph, values, saved_keys, mode); - - Ordering expSumOrdering; expSumOrdering += xA0, xA1, xA2, lA3, lA5; - EXPECT(assert_equal(expSumOrdering, actOrdering)); - - // Does not split out subfactors where possible - GaussianFactorGraph expLinGraph; - expLinGraph.add( - expSumOrdering[lA3], - Matrix_(2,2, - 0.595867, 0.605092, - 0.0, 0.406109), - expSumOrdering[lA5], - Matrix_(2,2, - -0.125971, -0.160052, - -0.13586, -0.301096), - zero(2), diagmodel2); - - expLinGraph.add( - expSumOrdering[lA5], - Matrix_(2,2, - 0.268667, 0.31703, - 0.0, 0.131698), - zero(2), diagmodel2); - - EXPECT(assert_equal(expLinGraph, actLinGraph, tol)); - - // Summarize directly from a nonlinear graph to another nonlinear graph - NonlinearFactorGraph actContainerGraph = summarizeAsNonlinearContainer(graph, values, saved_keys, mode); - NonlinearFactorGraph expContainerGraph = LinearContainerFactor::convertLinearGraph(expLinGraph, expSumOrdering); - - EXPECT(assert_equal(expContainerGraph, actContainerGraph, tol)); - } - - { - // Summarize to a linear system with joint factor graph version - GaussianFactorGraph actLinGraph; Ordering actOrdering; - SummarizationMode mode = SEQUENTIAL_CHOLESKY; - boost::tie(actLinGraph, actOrdering) = summarize(graph, values, saved_keys, mode); - - Ordering expSumOrdering; expSumOrdering += xA0, xA1, xA2, lA3, lA5; - EXPECT(assert_equal(expSumOrdering, actOrdering)); - - // Does not split out subfactors where possible - GaussianFactorGraph expLinGraph; - expLinGraph.add( - expSumOrdering[lA3], - Matrix_(2,2, - 0.595867, 0.605092, - 0.0, 0.406109), - expSumOrdering[lA5], - Matrix_(2,2, - -0.125971, -0.160052, - -0.13586, -0.301096), - zero(2), diagmodel2); - - expLinGraph.add( - expSumOrdering[lA5], - Matrix_(2,2, - 0.268667, 0.31703, - 0.0, 0.131698), - zero(2), diagmodel2); - - EXPECT(assert_equal(expLinGraph, actLinGraph, tol)); - - // Summarize directly from a nonlinear graph to another nonlinear graph - NonlinearFactorGraph actContainerGraph = summarizeAsNonlinearContainer(graph, values, saved_keys, mode); - NonlinearFactorGraph expContainerGraph = LinearContainerFactor::convertLinearGraph(expLinGraph, expSumOrdering); - - EXPECT(assert_equal(expContainerGraph, actContainerGraph, tol)); - } -} - -/* ************************************************************************* */ -TEST( testSummarization, no_summarize_case ) { - // Checks a corner case in which no variables are being eliminated - gtsam::Key key = 7; - gtsam::KeySet saved_keys; saved_keys.insert(key); - NonlinearFactorGraph graph; - graph.add(PosePrior(key, Pose2(1.0, 2.0, 0.3), model3)); - graph.add(PosePrior(key, Pose2(2.0, 3.0, 0.4), model3)); - Values values; - values.insert(key, Pose2(0.0, 0.0, 0.1)); - - SummarizationMode mode = SEQUENTIAL_CHOLESKY; - GaussianFactorGraph actLinGraph; Ordering actOrdering; - boost::tie(actLinGraph, actOrdering) = summarize(graph, values, saved_keys, mode); - Ordering expOrdering; expOrdering += key; - GaussianFactorGraph expLinGraph = *graph.linearize(values, expOrdering); - EXPECT(assert_equal(expOrdering, actOrdering)); - EXPECT(assert_equal(expLinGraph, actLinGraph)); -} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ diff --git a/tests/testSymbolicBayesNetB.cpp b/tests/testSymbolicBayesNetB.cpp deleted file mode 100644 index e046d6ba4..000000000 --- a/tests/testSymbolicBayesNetB.cpp +++ /dev/null @@ -1,79 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 testSymbolicBayesNetB.cpp - * @brief Unit tests for a symbolic Bayes chain - * @author Frank Dellaert - */ - -#include // for 'insert()' -#include // for operator += -using namespace boost::assign; - -#include - -#include -#include -#include -#include -#include -#include - -using namespace std; -using namespace gtsam; -using namespace example; - -using symbol_shorthand::X; -using symbol_shorthand::L; - -/* ************************************************************************* */ -TEST( SymbolicBayesNet, constructor ) -{ - Ordering o; o += X(2),L(1),X(1); - // Create manually - IndexConditional::shared_ptr - x2(new IndexConditional(o[X(2)],o[L(1)], o[X(1)])), - l1(new IndexConditional(o[L(1)],o[X(1)])), - x1(new IndexConditional(o[X(1)])); - BayesNet expected; - expected.push_back(x2); - expected.push_back(l1); - expected.push_back(x1); - - // Create from a factor graph - GaussianFactorGraph factorGraph = createGaussianFactorGraph(o); - SymbolicFactorGraph fg(factorGraph); - - // eliminate it - SymbolicBayesNet actual = *SymbolicSequentialSolver(fg).eliminate(); - - CHECK(assert_equal(expected, actual)); -} - -/* ************************************************************************* */ -TEST( SymbolicBayesNet, FromGaussian) { - SymbolicBayesNet expected; - expected.push_back(IndexConditional::shared_ptr(new IndexConditional(0, 1))); - expected.push_back(IndexConditional::shared_ptr(new IndexConditional(1))); - - GaussianBayesNet gbn = createSmallGaussianBayesNet(); - SymbolicBayesNet actual(gbn); - - EXPECT(assert_equal(expected, actual)); -} - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ diff --git a/tests/testSymbolicFactorGraphB.cpp b/tests/testSymbolicFactorGraphB.cpp deleted file mode 100644 index 5d6b478bf..000000000 --- a/tests/testSymbolicFactorGraphB.cpp +++ /dev/null @@ -1,156 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 testSymbolicFactorGraphB.cpp - * @brief Unit tests for a symbolic Factor Graph - * @author Frank Dellaert - */ - -#include -#include -#include -#include -#include -#include - -#include - -#include // for operator += -using namespace boost::assign; - -using namespace std; -using namespace gtsam; - -using symbol_shorthand::X; -using symbol_shorthand::L; - -/* ************************************************************************* */ -TEST( SymbolicFactorGraph, symbolicFactorGraph ) -{ - Ordering o; o += X(1),L(1),X(2); - // construct expected symbolic graph - SymbolicFactorGraph expected; - expected.push_factor(o[X(1)]); - expected.push_factor(o[X(1)],o[X(2)]); - expected.push_factor(o[X(1)],o[L(1)]); - expected.push_factor(o[X(2)],o[L(1)]); - - // construct it from the factor graph - GaussianFactorGraph factorGraph = example::createGaussianFactorGraph(o); - SymbolicFactorGraph actual(factorGraph); - - CHECK(assert_equal(expected, actual)); -} - -///* ************************************************************************* */ -//TEST( SymbolicFactorGraph, findAndRemoveFactors ) -//{ -// // construct it from the factor graph graph -// GaussianFactorGraph factorGraph = example::createGaussianFactorGraph(); -// SymbolicFactorGraph actual(factorGraph); -// SymbolicFactor::shared_ptr f1 = actual[0]; -// SymbolicFactor::shared_ptr f3 = actual[2]; -// actual.findAndRemoveFactors(X(2)); -// -// // construct expected graph after find_factors_and_remove -// SymbolicFactorGraph expected; -// SymbolicFactor::shared_ptr null; -// expected.push_back(f1); -// expected.push_back(null); -// expected.push_back(f3); -// expected.push_back(null); -// -// CHECK(assert_equal(expected, actual)); -//} -///* ************************************************************************* */ -//TEST( SymbolicFactorGraph, factors) -//{ -// // create a test graph -// GaussianFactorGraph factorGraph = example::createGaussianFactorGraph(); -// SymbolicFactorGraph fg(factorGraph); -// -// // ask for all factor indices connected to x1 -// list x1_factors = fg.factors(X(1)); -// int x1_indices[] = { 0, 1, 2 }; -// list x1_expected(x1_indices, x1_indices + 3); -// CHECK(x1_factors==x1_expected); -// -// // ask for all factor indices connected to x2 -// list x2_factors = fg.factors(X(2)); -// int x2_indices[] = { 1, 3 }; -// list x2_expected(x2_indices, x2_indices + 2); -// CHECK(x2_factors==x2_expected); -//} - -///* ************************************************************************* */ -//TEST( SymbolicFactorGraph, removeAndCombineFactors ) -//{ -// // create a test graph -// GaussianFactorGraph factorGraph = example::createGaussianFactorGraph(); -// SymbolicFactorGraph fg(factorGraph); -// -// // combine all factors connected to x1 -// SymbolicFactor::shared_ptr actual = removeAndCombineFactors(fg,X(1)); -// -// // check result -// SymbolicFactor expected(L(1),X(1),X(2)); -// CHECK(assert_equal(expected,*actual)); -//} - -///* ************************************************************************* */ -//TEST( SymbolicFactorGraph, eliminateOne ) -//{ -// Ordering o; o += X(1),L(1),X(2); -// // create a test graph -// GaussianFactorGraph factorGraph = example::createGaussianFactorGraph(o); -// SymbolicFactorGraph fg(factorGraph); -// -// // eliminate -// IndexConditional::shared_ptr actual = GaussianSequentialSolver::EliminateUntil(fg, o[X(1)]+1); -// -// // create expected symbolic IndexConditional -// IndexConditional expected(o[X(1)],o[L(1)],o[X(2)]); -// -// CHECK(assert_equal(expected,*actual)); -//} - -/* ************************************************************************* */ -TEST( SymbolicFactorGraph, eliminate ) -{ - Ordering o; o += X(2),L(1),X(1); - - // create expected Chordal bayes Net - IndexConditional::shared_ptr x2(new IndexConditional(o[X(2)], o[L(1)], o[X(1)])); - IndexConditional::shared_ptr l1(new IndexConditional(o[L(1)], o[X(1)])); - IndexConditional::shared_ptr x1(new IndexConditional(o[X(1)])); - - SymbolicBayesNet expected; - expected.push_back(x2); - expected.push_back(l1); - expected.push_back(x1); - - // create a test graph - GaussianFactorGraph factorGraph = example::createGaussianFactorGraph(o); - SymbolicFactorGraph fg(factorGraph); - - // eliminate it - SymbolicBayesNet actual = *SymbolicSequentialSolver(fg).eliminate(); - - CHECK(assert_equal(expected,actual)); -} - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ diff --git a/tests/timeGaussianFactorGraph.cpp b/tests/timeGaussianFactorGraph.cpp index f7dd8601d..00d24bd13 100644 --- a/tests/timeGaussianFactorGraph.cpp +++ b/tests/timeGaussianFactorGraph.cpp @@ -20,7 +20,6 @@ #include // for operator += in Ordering #include #include -#include using namespace std; using namespace gtsam; diff --git a/tests/timeIncremental.cpp b/tests/timeIncremental.cpp index 3a8f26b09..6f2909b7a 100644 --- a/tests/timeIncremental.cpp +++ b/tests/timeIncremental.cpp @@ -20,7 +20,7 @@ #include #include #include -#include +#include #include #include diff --git a/tests/timeiSAM2Chain.cpp b/tests/timeiSAM2Chain.cpp index 9ed18a1ef..d15ccfaf4 100644 --- a/tests/timeiSAM2Chain.cpp +++ b/tests/timeiSAM2Chain.cpp @@ -19,7 +19,7 @@ #include #include #include -#include +#include #include #include diff --git a/wrap/CMakeLists.txt b/wrap/CMakeLists.txt index 28f8bea48..03915a662 100644 --- a/wrap/CMakeLists.txt +++ b/wrap/CMakeLists.txt @@ -31,9 +31,5 @@ set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE) install(FILES matlab.h DESTINATION include/wrap) # Build tests -if (GTSAM_BUILD_TESTS) - set(wrap_local_libs wrap_lib ${WRAP_BOOST_LIBRARIES}) - gtsam_add_subdir_tests("wrap" "${wrap_local_libs}" "${wrap_local_libs}" "") -endif(GTSAM_BUILD_TESTS) - +add_subdirectory(tests) diff --git a/wrap/Constructor.cpp b/wrap/Constructor.cpp index 4529d45b3..fdbbf0e42 100644 --- a/wrap/Constructor.cpp +++ b/wrap/Constructor.cpp @@ -96,7 +96,7 @@ string Constructor::wrapper_fragment(FileWriter& file, if(!cppBaseClassName.empty()) { file.oss << "\n"; file.oss << " typedef boost::shared_ptr<" << cppBaseClassName << "> SharedBase;\n"; - file.oss << " out[1] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);\n"; + file.oss << " out[1] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);\n"; file.oss << " *reinterpret_cast(mxGetData(out[1])) = new SharedBase(*self);\n"; } diff --git a/wrap/Module.cpp b/wrap/Module.cpp index 3cfdf2903..6870d5178 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -321,7 +321,12 @@ void Module::parseMarkup(const std::string& data) { [assign_a(args,args0)] [assign_a(retVal,retVal0)]; - Rule include_p = str_p("#include") >> ch_p('<') >> (*(anychar_p - '>'))[push_back_a(includes)] >> ch_p('>'); + Rule include_p = str_p("#include") >> ch_p('<') >> (*(anychar_p - '>'))[push_back_a(includes)] >> ch_p('>'); + +#ifdef __clang__ +#pragma clang diagnostic push +#pragma clang diagnostic ignored "-Wuninitialized" +#endif Rule namespace_def_p = (str_p("namespace") @@ -330,8 +335,12 @@ void Module::parseMarkup(const std::string& data) { >> *(include_p | class_p | templateSingleInstantiation_p | global_function_p | namespace_def_p | comments_p) >> ch_p('}')) [pop_a(namespaces)]; - - Rule forward_declaration_p = + +#ifdef __clang__ +#pragma clang diagnostic pop +#endif + + Rule forward_declaration_p = !(str_p("virtual")[assign_a(fwDec.isVirtual, true)]) >> str_p("class") >> (*(namespace_name_p >> str_p("::")) >> className_p)[assign_a(fwDec.name)] diff --git a/wrap/matlab.h b/wrap/matlab.h index c84d6fdec..23f391903 100644 --- a/wrap/matlab.h +++ b/wrap/matlab.h @@ -1,427 +1,427 @@ -/* ---------------------------------------------------------------------------- - - * 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 matlab.h - * @brief header file to be included in MATLAB wrappers - * @date 2008 - * @author Frank Dellaert - * @author Alex Cunningham - * @author Andrew Melim +/* ---------------------------------------------------------------------------- + + * 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 matlab.h + * @brief header file to be included in MATLAB wrappers + * @date 2008 + * @author Frank Dellaert + * @author Alex Cunningham + * @author Andrew Melim * @author Richard Roberts - * - * wrapping and unwrapping is done using specialized templates, see - * http://www.cplusplus.com/doc/tutorial/templates.html - */ - -#include -#include - -using gtsam::Vector; -using gtsam::Matrix; - -extern "C" { -#include -} - -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -using namespace std; -using namespace boost; // not usual, but for conciseness of generated code - -// start GTSAM Specifics ///////////////////////////////////////////////// -// to enable Matrix and Vector constructor for SharedGaussian: -#define GTSAM_MAGIC_GAUSSIAN -// end GTSAM Specifics ///////////////////////////////////////////////// - -#if defined(__LP64__) || defined(_WIN64) -// 64-bit -#define mxUINT32OR64_CLASS mxUINT64_CLASS -#else -#define mxUINT32OR64_CLASS mxUINT32_CLASS -#endif - -// "Unique" key to signal calling the matlab object constructor with a raw pointer -// to a shared pointer of the same C++ object type as the MATLAB type. -// Also present in utilities.h -static const uint64_t ptr_constructor_key = - (uint64_t('G') << 56) | - (uint64_t('T') << 48) | - (uint64_t('S') << 40) | - (uint64_t('A') << 32) | - (uint64_t('M') << 24) | - (uint64_t('p') << 16) | - (uint64_t('t') << 8) | - (uint64_t('r')); - -//***************************************************************************** -// Utilities -//***************************************************************************** - -void error(const char* str) { - mexErrMsgIdAndTxt("wrap:error", str); -} - -mxArray *scalar(mxClassID classid) { - mwSize dims[1]; dims[0]=1; - return mxCreateNumericArray(1, dims, classid, mxREAL); -} - -void checkScalar(const mxArray* array, const char* str) { - int m = mxGetM(array), n = mxGetN(array); - if (m!=1 || n!=1) - mexErrMsgIdAndTxt("wrap: not a scalar in ", str); -} - -// Replacement streambuf for cout that writes to the MATLAB console -// Thanks to http://stackoverflow.com/a/249008 -class mstream : public std::streambuf { -protected: - virtual std::streamsize xsputn(const char *s, std::streamsize n) { - mexPrintf("%.*s",n,s); - return n; - } - virtual int overflow(int c = EOF) { - if (c != EOF) { - mexPrintf("%.1s",&c); - } - return 1; - } -}; - -//***************************************************************************** -// Check arguments -//***************************************************************************** - -void checkArguments(const string& name, int nargout, int nargin, int expected) { - stringstream err; - err << name << " expects " << expected << " arguments, not " << nargin; - if (nargin!=expected) - error(err.str().c_str()); -} - -//***************************************************************************** -// wrapping C++ basis types in MATLAB arrays -//***************************************************************************** - -// default wrapping throws an error: only basis types are allowed in wrap -template -mxArray* wrap(const Class& value) { - error("wrap internal error: attempted wrap of invalid type"); - return 0; -} - -// specialization to string -// wraps into a character array -template<> -mxArray* wrap(const string& value) { - return mxCreateString(value.c_str()); -} - -// specialization to char -template<> -mxArray* wrap(const char& value) { - mxArray *result = scalar(mxUINT32OR64_CLASS); - *(char*)mxGetData(result) = value; - return result; -} - -// specialization to unsigned char -template<> -mxArray* wrap(const unsigned char& value) { - mxArray *result = scalar(mxUINT32OR64_CLASS); - *(unsigned char*)mxGetData(result) = value; - return result; -} - -// specialization to bool -template<> -mxArray* wrap(const bool& value) { - mxArray *result = scalar(mxUINT32OR64_CLASS); - *(bool*)mxGetData(result) = value; - return result; -} - -// specialization to size_t -template<> -mxArray* wrap(const size_t& value) { - mxArray *result = scalar(mxUINT32OR64_CLASS); - *(size_t*)mxGetData(result) = value; - return result; -} - -// specialization to int -template<> -mxArray* wrap(const int& value) { - mxArray *result = scalar(mxUINT32OR64_CLASS); - *(int*)mxGetData(result) = value; - return result; -} - -// specialization to double -> just double -template<> -mxArray* wrap(const double& value) { - return mxCreateDoubleScalar(value); -} - -// wrap a const Eigen vector into a double vector -mxArray* wrap_Vector(const gtsam::Vector& v) { - int m = v.size(); - mxArray *result = mxCreateDoubleMatrix(m, 1, mxREAL); - double *data = mxGetPr(result); - for (int i=0;i double vector -template<> -mxArray* wrap(const gtsam::Vector& v) { - return wrap_Vector(v); -} - -// wrap a const Eigen MATRIX into a double matrix -mxArray* wrap_Matrix(const gtsam::Matrix& A) { - int m = A.rows(), n = A.cols(); -#ifdef DEBUG_WRAP - mexPrintf("wrap_Matrix called with A = \n", m,n); - gtsam::print(A); -#endif - mxArray *result = mxCreateDoubleMatrix(m, n, mxREAL); - double *data = mxGetPr(result); - // converts from column-major to row-major - for (int j=0;j double matrix -template<> -mxArray* wrap(const gtsam::Matrix& A) { - return wrap_Matrix(A); -} - -//***************************************************************************** -// unwrapping MATLAB arrays into C++ basis types -//***************************************************************************** - -// default unwrapping throws an error -// as wrap only supports passing a reference or one of the basic types -template -T unwrap(const mxArray* array) { - error("wrap internal error: attempted unwrap of invalid type"); - return T(); -} - -// specialization to string -// expects a character array -// Warning: relies on mxChar==char -template<> -string unwrap(const mxArray* array) { - char *data = mxArrayToString(array); - if (data==NULL) error("unwrap: not a character array"); - string str(data); - mxFree(data); - return str; -} - -// Check for 64-bit, as Mathworks says mxGetScalar only good for 32 bit -template -T myGetScalar(const mxArray* array) { - switch (mxGetClassID(array)) { - case mxINT64_CLASS: - return (T) *(int64_t*) mxGetData(array); - case mxUINT64_CLASS: - return (T) *(uint64_t*) mxGetData(array); - default: - // hope for the best! - return (T) mxGetScalar(array); - } -} - -// specialization to bool -template<> -bool unwrap(const mxArray* array) { - checkScalar(array,"unwrap"); - return myGetScalar(array); -} - -// specialization to char -template<> -char unwrap(const mxArray* array) { - checkScalar(array,"unwrap"); - return myGetScalar(array); -} - -// specialization to unsigned char -template<> -unsigned char unwrap(const mxArray* array) { - checkScalar(array,"unwrap"); - return myGetScalar(array); -} - -// specialization to int -template<> -int unwrap(const mxArray* array) { - checkScalar(array,"unwrap"); - return myGetScalar(array); -} - -// specialization to size_t -template<> -size_t unwrap(const mxArray* array) { - checkScalar(array, "unwrap"); - return myGetScalar(array); -} - -// specialization to double -template<> -double unwrap(const mxArray* array) { - checkScalar(array,"unwrap"); - return myGetScalar(array); -} - -// specialization to Eigen vector -template<> -gtsam::Vector unwrap< gtsam::Vector >(const mxArray* array) { - int m = mxGetM(array), n = mxGetN(array); - if (mxIsDouble(array)==false || n!=1) error("unwrap: not a vector"); -#ifdef DEBUG_WRAP - mexPrintf("unwrap< gtsam::Vector > called with %dx%d argument\n", m,n); -#endif - double* data = (double*)mxGetData(array); - gtsam::Vector v(m); - for (int i=0;i -gtsam::Matrix unwrap< gtsam::Matrix >(const mxArray* array) { - if (mxIsDouble(array)==false) error("unwrap: not a matrix"); - int m = mxGetM(array), n = mxGetN(array); -#ifdef DEBUG_WRAP - mexPrintf("unwrap< gtsam::Matrix > called with %dx%d argument\n", m,n); -#endif - double* data = (double*)mxGetData(array); - gtsam::Matrix A(m,n); - // converts from row-major to column-major - for (int j=0;j(mxGetData(input[0])) = ptr_constructor_key; - // Second input argument is the pointer - input[1] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); - *reinterpret_cast(mxGetData(input[1])) = pointer; - // If the class is virtual, use the RTTI name to look up the derived matlab type - const char *derivedClassName; - if(isVirtual) { - const mxArray *rttiRegistry = mexGetVariablePtr("global", "gtsamwrap_rttiRegistry"); - if(!rttiRegistry) - mexErrMsgTxt( - "gtsam wrap: RTTI registry is missing - it could have been cleared from the workspace." - " You can issue 'clear all' to completely clear the workspace, and next time a wrapped object is" - " created the RTTI registry will be recreated."); - const mxArray *derivedNameMx = mxGetField(rttiRegistry, 0, rttiName); - if(!derivedNameMx) - mexErrMsgTxt(( - "gtsam wrap: The derived class type " + string(rttiName) + " was not found in the RTTI registry. " - "Try calling 'clear all' twice consecutively - we have seen things not get unloaded properly the " - "first time. If this does not work, this may indicate an inconsistency in your wrap interface file. " - "The most likely cause for this is that a base class was marked virtual in the wrap interface " - "definition header file for gtsam or for your module, but a derived type was returned by a C++ " - "function and that derived type was not marked virtual (or was not specified in the wrap interface " - "definition header at all).").c_str()); - size_t strLen = mxGetN(derivedNameMx); - char *buf = new char[strLen+1]; - if(mxGetString(derivedNameMx, buf, strLen+1)) - mexErrMsgTxt("gtsam wrap: Internal error reading RTTI table, try 'clear all' to clear your workspace and reinitialize the toolbox."); - derivedClassName = buf; - input[2] = mxCreateString("void"); - nargin = 3; - } else { - derivedClassName = classname.c_str(); - } - // Call special pointer constructor, which sets 'self' - mexCallMATLAB(1,&result, nargin, input, derivedClassName); - // Deallocate our memory - mxDestroyArray(input[0]); - mxDestroyArray(input[1]); - if(isVirtual) { - mxDestroyArray(input[2]); - delete[] derivedClassName; - } - return result; -} - -/* - When the user calls a method that returns a shared pointer, we create - an ObjectHandle from the shared_pointer and return it as a proxy - class to matlab. -*/ -template -mxArray* wrap_shared_ptr(boost::shared_ptr< Class > shared_ptr, const std::string& matlabName, bool isVirtual) { - // Create actual class object from out pointer - mxArray* result; - if(isVirtual) { - boost::shared_ptr void_ptr(shared_ptr); - result = create_object(matlabName, &void_ptr, isVirtual, typeid(*shared_ptr).name()); - } else { - boost::shared_ptr *heapPtr = new boost::shared_ptr(shared_ptr); - result = create_object(matlabName, heapPtr, isVirtual, ""); - } - return result; -} - -template -boost::shared_ptr unwrap_shared_ptr(const mxArray* obj, const string& propertyName) { - - mxArray* mxh = mxGetProperty(obj,0, propertyName.c_str()); - if (mxGetClassID(mxh) != mxUINT32OR64_CLASS || mxIsComplex(mxh) - || mxGetM(mxh) != 1 || mxGetN(mxh) != 1) error( - "Parameter is not an Shared type."); - - boost::shared_ptr* spp = *reinterpret_cast**> (mxGetData(mxh)); - return *spp; -} + * + * wrapping and unwrapping is done using specialized templates, see + * http://www.cplusplus.com/doc/tutorial/templates.html + */ + +#include +#include + +using gtsam::Vector; +using gtsam::Matrix; + +extern "C" { +#include +} + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace boost; // not usual, but for conciseness of generated code + +// start GTSAM Specifics ///////////////////////////////////////////////// +// to enable Matrix and Vector constructor for SharedGaussian: +#define GTSAM_MAGIC_GAUSSIAN +// end GTSAM Specifics ///////////////////////////////////////////////// + +#if defined(__LP64__) || defined(_WIN64) +// 64-bit +#define mxUINT32OR64_CLASS mxUINT64_CLASS +#else +#define mxUINT32OR64_CLASS mxUINT32_CLASS +#endif + +// "Unique" key to signal calling the matlab object constructor with a raw pointer +// to a shared pointer of the same C++ object type as the MATLAB type. +// Also present in utilities.h +static const uint64_t ptr_constructor_key = + (uint64_t('G') << 56) | + (uint64_t('T') << 48) | + (uint64_t('S') << 40) | + (uint64_t('A') << 32) | + (uint64_t('M') << 24) | + (uint64_t('p') << 16) | + (uint64_t('t') << 8) | + (uint64_t('r')); + +//***************************************************************************** +// Utilities +//***************************************************************************** + +void error(const char* str) { + mexErrMsgIdAndTxt("wrap:error", str); +} + +mxArray *scalar(mxClassID classid) { + mwSize dims[1]; dims[0]=1; + return mxCreateNumericArray(1, dims, classid, mxREAL); +} + +void checkScalar(const mxArray* array, const char* str) { + int m = mxGetM(array), n = mxGetN(array); + if (m!=1 || n!=1) + mexErrMsgIdAndTxt("wrap: not a scalar in ", str); +} + +// Replacement streambuf for cout that writes to the MATLAB console +// Thanks to http://stackoverflow.com/a/249008 +class mstream : public std::streambuf { +protected: + virtual std::streamsize xsputn(const char *s, std::streamsize n) { + mexPrintf("%.*s",n,s); + return n; + } + virtual int overflow(int c = EOF) { + if (c != EOF) { + mexPrintf("%.1s",&c); + } + return 1; + } +}; + +//***************************************************************************** +// Check arguments +//***************************************************************************** + +void checkArguments(const string& name, int nargout, int nargin, int expected) { + stringstream err; + err << name << " expects " << expected << " arguments, not " << nargin; + if (nargin!=expected) + error(err.str().c_str()); +} + +//***************************************************************************** +// wrapping C++ basis types in MATLAB arrays +//***************************************************************************** + +// default wrapping throws an error: only basis types are allowed in wrap +template +mxArray* wrap(const Class& value) { + error("wrap internal error: attempted wrap of invalid type"); + return 0; +} + +// specialization to string +// wraps into a character array +template<> +mxArray* wrap(const string& value) { + return mxCreateString(value.c_str()); +} + +// specialization to char +template<> +mxArray* wrap(const char& value) { + mxArray *result = scalar(mxUINT32OR64_CLASS); + *(char*)mxGetData(result) = value; + return result; +} + +// specialization to unsigned char +template<> +mxArray* wrap(const unsigned char& value) { + mxArray *result = scalar(mxUINT32OR64_CLASS); + *(unsigned char*)mxGetData(result) = value; + return result; +} + +// specialization to bool +template<> +mxArray* wrap(const bool& value) { + mxArray *result = scalar(mxUINT32OR64_CLASS); + *(bool*)mxGetData(result) = value; + return result; +} + +// specialization to size_t +template<> +mxArray* wrap(const size_t& value) { + mxArray *result = scalar(mxUINT32OR64_CLASS); + *(size_t*)mxGetData(result) = value; + return result; +} + +// specialization to int +template<> +mxArray* wrap(const int& value) { + mxArray *result = scalar(mxUINT32OR64_CLASS); + *(int*)mxGetData(result) = value; + return result; +} + +// specialization to double -> just double +template<> +mxArray* wrap(const double& value) { + return mxCreateDoubleScalar(value); +} + +// wrap a const Eigen vector into a double vector +mxArray* wrap_Vector(const gtsam::Vector& v) { + int m = v.size(); + mxArray *result = mxCreateDoubleMatrix(m, 1, mxREAL); + double *data = mxGetPr(result); + for (int i=0;i double vector +template<> +mxArray* wrap(const gtsam::Vector& v) { + return wrap_Vector(v); +} + +// wrap a const Eigen MATRIX into a double matrix +mxArray* wrap_Matrix(const gtsam::Matrix& A) { + int m = A.rows(), n = A.cols(); +#ifdef DEBUG_WRAP + mexPrintf("wrap_Matrix called with A = \n", m,n); + gtsam::print(A); +#endif + mxArray *result = mxCreateDoubleMatrix(m, n, mxREAL); + double *data = mxGetPr(result); + // converts from column-major to row-major + for (int j=0;j double matrix +template<> +mxArray* wrap(const gtsam::Matrix& A) { + return wrap_Matrix(A); +} + +//***************************************************************************** +// unwrapping MATLAB arrays into C++ basis types +//***************************************************************************** + +// default unwrapping throws an error +// as wrap only supports passing a reference or one of the basic types +template +T unwrap(const mxArray* array) { + error("wrap internal error: attempted unwrap of invalid type"); + return T(); +} + +// specialization to string +// expects a character array +// Warning: relies on mxChar==char +template<> +string unwrap(const mxArray* array) { + char *data = mxArrayToString(array); + if (data==NULL) error("unwrap: not a character array"); + string str(data); + mxFree(data); + return str; +} + +// Check for 64-bit, as Mathworks says mxGetScalar only good for 32 bit +template +T myGetScalar(const mxArray* array) { + switch (mxGetClassID(array)) { + case mxINT64_CLASS: + return (T) *(int64_t*) mxGetData(array); + case mxUINT64_CLASS: + return (T) *(uint64_t*) mxGetData(array); + default: + // hope for the best! + return (T) mxGetScalar(array); + } +} + +// specialization to bool +template<> +bool unwrap(const mxArray* array) { + checkScalar(array,"unwrap"); + return myGetScalar(array); +} + +// specialization to char +template<> +char unwrap(const mxArray* array) { + checkScalar(array,"unwrap"); + return myGetScalar(array); +} + +// specialization to unsigned char +template<> +unsigned char unwrap(const mxArray* array) { + checkScalar(array,"unwrap"); + return myGetScalar(array); +} + +// specialization to int +template<> +int unwrap(const mxArray* array) { + checkScalar(array,"unwrap"); + return myGetScalar(array); +} + +// specialization to size_t +template<> +size_t unwrap(const mxArray* array) { + checkScalar(array, "unwrap"); + return myGetScalar(array); +} + +// specialization to double +template<> +double unwrap(const mxArray* array) { + checkScalar(array,"unwrap"); + return myGetScalar(array); +} + +// specialization to Eigen vector +template<> +gtsam::Vector unwrap< gtsam::Vector >(const mxArray* array) { + int m = mxGetM(array), n = mxGetN(array); + if (mxIsDouble(array)==false || n!=1) error("unwrap: not a vector"); +#ifdef DEBUG_WRAP + mexPrintf("unwrap< gtsam::Vector > called with %dx%d argument\n", m,n); +#endif + double* data = (double*)mxGetData(array); + gtsam::Vector v(m); + for (int i=0;i +gtsam::Matrix unwrap< gtsam::Matrix >(const mxArray* array) { + if (mxIsDouble(array)==false) error("unwrap: not a matrix"); + int m = mxGetM(array), n = mxGetN(array); +#ifdef DEBUG_WRAP + mexPrintf("unwrap< gtsam::Matrix > called with %dx%d argument\n", m,n); +#endif + double* data = (double*)mxGetData(array); + gtsam::Matrix A(m,n); + // converts from row-major to column-major + for (int j=0;j(mxGetData(input[0])) = ptr_constructor_key; + // Second input argument is the pointer + input[1] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast(mxGetData(input[1])) = pointer; + // If the class is virtual, use the RTTI name to look up the derived matlab type + const char *derivedClassName; + if(isVirtual) { + const mxArray *rttiRegistry = mexGetVariablePtr("global", "gtsamwrap_rttiRegistry"); + if(!rttiRegistry) + mexErrMsgTxt( + "gtsam wrap: RTTI registry is missing - it could have been cleared from the workspace." + " You can issue 'clear all' to completely clear the workspace, and next time a wrapped object is" + " created the RTTI registry will be recreated."); + const mxArray *derivedNameMx = mxGetField(rttiRegistry, 0, rttiName); + if(!derivedNameMx) + mexErrMsgTxt(( + "gtsam wrap: The derived class type " + string(rttiName) + " was not found in the RTTI registry. " + "Try calling 'clear all' twice consecutively - we have seen things not get unloaded properly the " + "first time. If this does not work, this may indicate an inconsistency in your wrap interface file. " + "The most likely cause for this is that a base class was marked virtual in the wrap interface " + "definition header file for gtsam or for your module, but a derived type was returned by a C++ " + "function and that derived type was not marked virtual (or was not specified in the wrap interface " + "definition header at all).").c_str()); + size_t strLen = mxGetN(derivedNameMx); + char *buf = new char[strLen+1]; + if(mxGetString(derivedNameMx, buf, strLen+1)) + mexErrMsgTxt("gtsam wrap: Internal error reading RTTI table, try 'clear all' to clear your workspace and reinitialize the toolbox."); + derivedClassName = buf; + input[2] = mxCreateString("void"); + nargin = 3; + } else { + derivedClassName = classname.c_str(); + } + // Call special pointer constructor, which sets 'self' + mexCallMATLAB(1,&result, nargin, input, derivedClassName); + // Deallocate our memory + mxDestroyArray(input[0]); + mxDestroyArray(input[1]); + if(isVirtual) { + mxDestroyArray(input[2]); + delete[] derivedClassName; + } + return result; +} + +/* + When the user calls a method that returns a shared pointer, we create + an ObjectHandle from the shared_pointer and return it as a proxy + class to matlab. +*/ +template +mxArray* wrap_shared_ptr(boost::shared_ptr< Class > shared_ptr, const std::string& matlabName, bool isVirtual) { + // Create actual class object from out pointer + mxArray* result; + if(isVirtual) { + boost::shared_ptr void_ptr(shared_ptr); + result = create_object(matlabName, &void_ptr, isVirtual, typeid(*shared_ptr).name()); + } else { + boost::shared_ptr *heapPtr = new boost::shared_ptr(shared_ptr); + result = create_object(matlabName, heapPtr, isVirtual, ""); + } + return result; +} + +template +boost::shared_ptr unwrap_shared_ptr(const mxArray* obj, const string& propertyName) { + + mxArray* mxh = mxGetProperty(obj,0, propertyName.c_str()); + if (mxGetClassID(mxh) != mxUINT32OR64_CLASS || mxIsComplex(mxh) + || mxGetM(mxh) != 1 || mxGetN(mxh) != 1) error( + "Parameter is not an Shared type."); + + boost::shared_ptr* spp = *reinterpret_cast**> (mxGetData(mxh)); + return *spp; +} diff --git a/wrap/tests/CMakeLists.txt b/wrap/tests/CMakeLists.txt new file mode 100644 index 000000000..f3ac1df49 --- /dev/null +++ b/wrap/tests/CMakeLists.txt @@ -0,0 +1 @@ +gtsamAddTestsGlob(wrap "test*.cpp" "" "wrap_lib") diff --git a/wrap/utilities.h b/wrap/utilities.h index 54e1d008e..7280dd297 100644 --- a/wrap/utilities.h +++ b/wrap/utilities.h @@ -89,8 +89,8 @@ public: virtual const char* what() const throw() { return what_.c_str(); } }; -// "Unique" key to signal calling the matlab object constructor with a raw pointer -// to a shared pointer of the same C++ object type as the MATLAB type. +// "Unique" key to signal calling the matlab object constructor with a raw pointer +// to a shared pointer of the same C++ object type as the MATLAB type. // Also present in matlab.h static const uint64_t ptr_constructor_key = (uint64_t('G') << 56) |