From 8dba25f532000e274815b3995f89f483a46eecce Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 28 May 2014 10:36:26 -0400 Subject: [PATCH] Rationalized some cholesky-related code as I was looking at it. --- .cproject | 1848 +++++++++++++++---------------- gtsam/base/Matrix.cpp | 47 +- gtsam/base/tests/testMatrix.cpp | 36 +- 3 files changed, 952 insertions(+), 979 deletions(-) diff --git a/.cproject b/.cproject index da776f062..af6b32cd2 100644 --- a/.cproject +++ b/.cproject @@ -568,7 +568,6 @@ make - tests/testBayesTree.run true false @@ -576,7 +575,6 @@ make - testBinaryBayesNet.run true false @@ -624,7 +622,6 @@ make - testSymbolicBayesNet.run true false @@ -632,7 +629,6 @@ make - tests/testSymbolicFactor.run true false @@ -640,7 +636,6 @@ make - testSymbolicFactorGraph.run true false @@ -656,20 +651,11 @@ make - tests/testBayesTree true false true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j5 @@ -686,134 +672,6 @@ true true - - make - -j5 - testCal3Bundler.run - true - true - true - - - make - -j5 - testCal3DS2.run - true - true - true - - - make - -j5 - testCalibratedCamera.run - true - true - true - - - make - -j5 - testEssentialMatrix.run - true - true - true - - - make - -j1 VERBOSE=1 - testHomography2.run - true - false - true - - - make - -j5 - testPinholeCamera.run - true - true - true - - - make - -j5 - testPoint2.run - true - true - true - - - make - -j5 - testPoint3.run - true - true - true - - - make - -j5 - testPose2.run - true - true - true - - - make - -j5 - testPose3.run - true - true - true - - - make - -j5 - testRot3M.run - true - true - true - - - make - -j5 - testSphere2.run - true - true - true - - - make - -j5 - testStereoCamera.run - true - true - true - - - make - -j5 - timeCalibratedCamera.run - true - true - true - - - make - -j5 - timePinholeCamera.run - true - true - true - - - make - -j5 - timeStereoCamera.run - true - true - true - make -j5 @@ -854,46 +712,6 @@ 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 @@ -910,134 +728,6 @@ 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 - -j2 - clean all - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j5 - testGeneralSFMFactor.run - true - true - true - - - make - -j5 - testProjectionFactor.run - true - true - true - - - make - -j5 - testGeneralSFMFactor_Cal3Bundler.run - true - true - true - - - make - -j6 -j8 - testAntiFactor.run - true - true - true - - - make - -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 -j5 @@ -1078,6 +768,30 @@ true true + + make + -j2 + all + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + make -j5 @@ -1126,46 +840,6 @@ true true - - make - -j5 - testWrap.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 @@ -1334,294 +1008,6 @@ true true - - make - -j5 - testDiscreteFactor.run - true - true - true - - - make - -j1 - testDiscreteBayesTree.run - true - false - true - - - make - -j5 - testDiscreteFactorGraph.run - true - true - true - - - make - -j5 - testDiscreteConditional.run - true - true - true - - - make - -j5 - testDiscreteMarginals.run - true - true - true - - - make - -j2 - vSFMexample.run - true - true - true - - - make - -j5 - testInvDepthCamera3.run - true - true - true - - - make - -j5 - testTriangulation.run - true - true - true - - - make - -j2 - testVSLAMGraph - true - true - true - - - make - -j5 - check.tests - true - true - true - - - make - -j2 - timeGaussianFactorGraph.run - true - true - true - - - make - -j5 - testMarginals.run - true - true - true - - - make - -j5 - testGaussianISAM2.run - true - true - true - - - make - -j5 - testSymbolicFactorGraphB.run - true - true - true - - - make - -j2 - timeSequentialOnDataset.run - true - true - true - - - make - -j5 - testGradientDescentOptimizer.run - true - true - true - - - make - -j2 - testGaussianFactor.run - true - true - true - - - make - -j2 - testNonlinearOptimizer.run - true - true - true - - - make - -j2 - testGaussianBayesNet.run - true - true - true - - - make - -j2 - testNonlinearISAM.run - true - true - true - - - make - -j2 - testNonlinearEquality.run - true - true - true - - - make - -j2 - testExtendedKalmanFilter.run - true - true - true - - - make - -j5 - timing.tests - true - true - true - - - make - -j5 - testNonlinearFactor.run - true - true - true - - - make - -j5 - clean - true - true - true - - - make - -j5 - testGaussianJunctionTreeB.run - true - true - true - - - make - - testGraph.run - true - false - true - - - make - - testJunctionTree.run - true - false - true - - - make - - testSymbolicBayesNetB.run - true - false - true - - - make - -j5 - testGaussianISAM.run - true - true - true - - - make - -j5 - testDoglegOptimizer.run - true - true - true - - - make - -j5 - testNonlinearFactorGraph.run - true - true - true - - - make - -j5 - testIterative.run - true - true - true - - - make - -j5 - testSubgraphSolver.run - true - true - true - - - make - -j5 - testGaussianFactorGraphB.run - true - true - true - - - make - -j5 - testSummarization.run - true - true - true - make -j5 @@ -1728,7 +1114,6 @@ make - testErrors.run true false @@ -1862,14 +1247,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -1966,54 +1343,6 @@ 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 -j2 @@ -2104,6 +1433,7 @@ make + testSimulated2DOriented.run true false @@ -2143,6 +1473,7 @@ make + testSimulated2D.run true false @@ -2150,6 +1481,7 @@ make + testSimulated3D.run true false @@ -2179,22 +1511,6 @@ true true - - make - -j5 - testVector.run - true - true - true - - - make - -j5 - testMatrix.run - true - true - true - make -j5 @@ -2235,181 +1551,6 @@ false 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 @@ -2627,7 +1768,6 @@ cpack - -G DEB true false @@ -2635,7 +1775,6 @@ cpack - -G RPM true false @@ -2643,7 +1782,6 @@ cpack - -G TGZ true false @@ -2651,7 +1789,6 @@ cpack - --config CPackSourceConfig.cmake true false @@ -2825,6 +1962,898 @@ 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 + testGaussianFactor.run + true + true + true + + + make + -j5 + testCal3Bundler.run + true + true + true + + + make + -j5 + testCal3DS2.run + true + true + true + + + make + -j5 + testCalibratedCamera.run + true + true + true + + + make + -j5 + testEssentialMatrix.run + true + true + true + + + make + -j1 VERBOSE=1 + testHomography2.run + true + false + true + + + make + -j5 + testPinholeCamera.run + true + true + true + + + make + -j5 + testPoint2.run + true + true + true + + + make + -j5 + testPoint3.run + true + true + true + + + make + -j5 + testPose2.run + true + true + true + + + make + -j5 + testPose3.run + true + true + true + + + make + -j5 + testRot3M.run + true + true + true + + + make + -j5 + testSphere2.run + true + true + true + + + make + -j5 + testStereoCamera.run + true + true + true + + + make + -j5 + timeCalibratedCamera.run + true + true + true + + + make + -j5 + timePinholeCamera.run + true + true + true + + + make + -j5 + timeStereoCamera.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 + true + true + true + + + make + -j5 + testWrap.run + true + true + true + + + make + -j5 + testDiscreteFactor.run + true + true + true + + + make + -j1 + testDiscreteBayesTree.run + true + false + true + + + make + -j5 + testDiscreteFactorGraph.run + true + true + true + + + make + -j5 + testDiscreteConditional.run + true + true + true + + + make + -j5 + testDiscreteMarginals.run + true + true + true + + + make + -j2 + vSFMexample.run + true + true + true + + + make + -j2 + testVSLAMGraph + true + true + true + + + make + -j5 + testInvDepthCamera3.run + true + true + true + + + make + -j5 + testTriangulation.run + true + true + true + + + make + -j5 + testMatrix.run + true + true + true + + + make + -j5 + testVector.run + true + true + true + + + make + -j5 + check.tests + true + true + true + + + make + -j2 + timeGaussianFactorGraph.run + true + true + true + + + make + -j5 + testMarginals.run + true + true + true + + + make + -j5 + testGaussianISAM2.run + true + true + true + + + make + -j5 + testSymbolicFactorGraphB.run + true + true + true + + + make + -j2 + timeSequentialOnDataset.run + true + true + true + + + make + -j5 + testGradientDescentOptimizer.run + true + true + true + + + make + -j2 + testGaussianFactor.run + true + true + true + + + make + -j2 + testNonlinearOptimizer.run + true + true + true + + + make + -j2 + testGaussianBayesNet.run + true + true + true + + + make + -j2 + testNonlinearISAM.run + true + true + true + + + make + -j2 + testNonlinearEquality.run + true + true + true + + + make + -j2 + testExtendedKalmanFilter.run + true + true + true + + + make + -j5 + timing.tests + true + true + true + + + make + -j5 + testNonlinearFactor.run + true + true + true + + + make + -j5 + clean + true + true + true + + + make + -j5 + testGaussianJunctionTreeB.run + true + true + true + + + make + testGraph.run + true + false + true + + + make + testJunctionTree.run + true + false + true + + + make + testSymbolicBayesNetB.run + true + false + true + + + make + -j5 + testGaussianISAM.run + true + true + true + + + make + -j5 + testDoglegOptimizer.run + true + true + true + + + make + -j5 + testNonlinearFactorGraph.run + true + true + true + + + make + -j5 + testIterative.run + true + true + true + + + make + -j5 + testSubgraphSolver.run + true + true + true + + + make + -j5 + testGaussianFactorGraphB.run + true + true + true + + + make + -j5 + testSummarization.run + true + true + true + + + make + -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 + -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 @@ -2921,45 +2950,6 @@ 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 -j5 diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index d9ab7d71c..f51197cf2 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -591,41 +591,17 @@ Matrix3 skewSymmetric(double wx, double wy, double wz) } /* ************************************************************************* */ -/** Numerical Recipes in C wrappers - * create Numerical Recipes in C structure - * pointers are subtracted by one to provide base 1 access - */ -/* ************************************************************************* */ -// FIXME: assumes row major, rather than column major -//double** createNRC(Matrix& A) { -// const size_t m=A.rows(); -// double** a = new double* [m]; -// for(size_t i = 0; i < m; i++) -// a[i] = &A(i,0)-1; -// return a; -//} - -/* ****************************************** - * - * Modified from Justin's codebase - * - * Idea came from other public domain code. Takes a S.P.D. matrix - * and computes the LL^t decomposition. returns L, which is lower - * triangular. Note this is the opposite convention from Matlab, - * which calculates Q'Q where Q is upper triangular. - * - * ******************************************/ Matrix LLt(const Matrix& A) { - Matrix L = zeros(A.rows(), A.rows()); - Eigen::LLT llt; - llt.compute(A); + Eigen::LLT llt(A); return llt.matrixL(); } +/* ************************************************************************* */ Matrix RtR(const Matrix &A) { - return LLt(A).transpose(); + Eigen::LLT llt(A); + return llt.matrixU(); } /* @@ -633,13 +609,10 @@ Matrix RtR(const Matrix &A) */ Matrix cholesky_inverse(const Matrix &A) { - // FIXME: replace with real algorithm - return A.inverse(); - -// Matrix L = LLt(A); -// Matrix inv(eye(A.rows())); -// inplace_solve (L, inv, BNU::lower_tag ()); -// return BNU::prod(trans(inv), inv); + Eigen::LLT llt(A); + Matrix inv = eye(A.rows()); + llt.matrixU().solveInPlace(inv); + return inv*inv.transpose(); } /* ************************************************************************* */ @@ -648,9 +621,9 @@ Matrix cholesky_inverse(const Matrix &A) // inv(B) * inv(B)' == A // inv(B' * B) == A Matrix inverse_square_root(const Matrix& A) { - Matrix R = RtR(A); + Eigen::LLT llt(A); Matrix inv = eye(A.rows()); - R.triangularView().solveInPlace(inv); + llt.matrixU().solveInPlace(inv); return inv.transpose(); } diff --git a/gtsam/base/tests/testMatrix.cpp b/gtsam/base/tests/testMatrix.cpp index 9d84e5ab7..4e857b143 100644 --- a/gtsam/base/tests/testMatrix.cpp +++ b/gtsam/base/tests/testMatrix.cpp @@ -1022,22 +1022,32 @@ TEST( matrix, inverse_square_root ) /* *********************************************************************** */ // M was generated as the covariance of a set of random numbers. L that // we are checking against was generated via chol(M)' on octave +namespace cholesky { +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) << + 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, + 0.027552165831157, 0.043423266737274, 0.021695600982708, 0.267613525371710, 0.000000000000000, + 0.016485031422565, -0.012072546984405, -0.006621889326331, 0.014405837566082, 0.300462176944247); +} TEST( matrix, LLt ) { - 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); + EQUALITY(cholesky::expected, LLt(cholesky::M)); +} +TEST( matrix, RtR ) +{ + EQUALITY(cholesky::expected.transpose(), RtR(cholesky::M)); +} - 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, - 0.027552165831157, 0.043423266737274, 0.021695600982708, 0.267613525371710, 0.000000000000000, - 0.016485031422565, -0.012072546984405, -0.006621889326331, 0.014405837566082, 0.300462176944247); - - EQUALITY(expected, LLt(M)); +TEST( matrix, cholesky_inverse ) +{ + EQUALITY(cholesky::M.inverse(), cholesky_inverse(cholesky::M)); } /* ************************************************************************* */