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));
}
/* ************************************************************************* */