12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge branch 'eigen'

release/4.3a0
Alex Cunningham 2011-05-20 13:52:08 +00:00
parent d35eb581ee
commit e20561be73
119 changed files with 4427 additions and 3312 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

564
.cproject
View File

@ -322,14 +322,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianFactor.run" path="linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testGaussianFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -356,6 +348,7 @@
</target>
<target name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testBayesTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -363,6 +356,7 @@
</target>
<target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testBinaryBayesNet.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -410,6 +404,7 @@
</target>
<target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicBayesNet.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -417,6 +412,7 @@
</target>
<target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testSymbolicFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -424,6 +420,7 @@
</target>
<target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -439,11 +436,20 @@
</target>
<target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testBayesTree</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianFactor.run" path="linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testGaussianFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -478,7 +484,6 @@
</target>
<target name="testGraph.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -574,7 +579,6 @@
</target>
<target name="testInference.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testInference.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -582,7 +586,6 @@
</target>
<target name="testGaussianBayesNet.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testGaussianBayesNet.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -590,7 +593,6 @@
</target>
<target name="testGaussianFactor.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testGaussianFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -598,7 +600,6 @@
</target>
<target name="testJunctionTree.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testJunctionTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -606,7 +607,6 @@
</target>
<target name="testSymbolicBayesNet.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicBayesNet.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -614,7 +614,6 @@
</target>
<target name="testSymbolicFactorGraph.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -692,6 +691,22 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -748,14 +763,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/gtsam/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="vSFMexample.run" path="build/examples/vSLAMexample" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -764,6 +771,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/gtsam/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testVSLAMGraph" path="build/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -820,22 +835,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianFactorGraph" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testGaussianFactorGraph</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianJunctionTree" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testGaussianJunctionTree</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testInference.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -876,6 +875,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianFactor.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testGaussianFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="SimpleRotation.run" path="examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -974,6 +981,7 @@
</target>
<target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testErrors.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1267,6 +1275,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -1349,7 +1365,6 @@
</target>
<target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated2DOriented.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1389,7 +1404,6 @@
</target>
<target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated2D.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1397,7 +1411,6 @@
</target>
<target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated3D.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1427,6 +1440,62 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testVector.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testVector.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testMatrix.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testMatrix.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testNumericalDerivative.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testNumericalDerivative.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testBlockMatrices.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testBlockMatrices.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="clean" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>clean</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testCholesky.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testCholesky.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testVectorValues.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -1459,6 +1528,38 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testGaussianConditional.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testGaussianConditional.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testGaussianFactorGraph.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testGaussianFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testGaussianJunctionTree.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testGaussianJunctionTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="SimpleRotation.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -1501,7 +1602,6 @@
</target>
<target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testGaussianISAM2</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1523,6 +1623,46 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>install</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="clean" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>clean</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>all</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="dist" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>dist</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -1619,54 +1759,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>install</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="clean" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>clean</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>all</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="dist" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>dist</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -1699,6 +1791,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
</buildTargets>
</storageModule>
</cconfiguration>
@ -2021,14 +2121,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianFactor.run" path="linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testGaussianFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -2055,6 +2147,7 @@
</target>
<target name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testBayesTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2062,6 +2155,7 @@
</target>
<target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testBinaryBayesNet.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2109,6 +2203,7 @@
</target>
<target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicBayesNet.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2116,6 +2211,7 @@
</target>
<target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testSymbolicFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2123,6 +2219,7 @@
</target>
<target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2138,11 +2235,20 @@
</target>
<target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testBayesTree</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianFactor.run" path="linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testGaussianFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -2177,7 +2283,6 @@
</target>
<target name="testGraph.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2273,7 +2378,6 @@
</target>
<target name="testInference.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testInference.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2281,7 +2385,6 @@
</target>
<target name="testGaussianBayesNet.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testGaussianBayesNet.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2289,7 +2392,6 @@
</target>
<target name="testGaussianFactor.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testGaussianFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2297,7 +2399,6 @@
</target>
<target name="testJunctionTree.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testJunctionTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2305,7 +2406,6 @@
</target>
<target name="testSymbolicBayesNet.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicBayesNet.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2313,7 +2413,6 @@
</target>
<target name="testSymbolicFactorGraph.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2391,6 +2490,22 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -2447,14 +2562,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/gtsam/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="vSFMexample.run" path="build/examples/vSLAMexample" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -2463,6 +2570,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/gtsam/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testVSLAMGraph" path="build/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -2519,22 +2634,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianFactorGraph" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testGaussianFactorGraph</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianJunctionTree" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testGaussianJunctionTree</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testInference.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -2575,6 +2674,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianFactor.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testGaussianFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="SimpleRotation.run" path="examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -2673,6 +2780,7 @@
</target>
<target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testErrors.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2966,6 +3074,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -3048,7 +3164,6 @@
</target>
<target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated2DOriented.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -3088,7 +3203,6 @@
</target>
<target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated2D.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -3096,7 +3210,6 @@
</target>
<target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated3D.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -3126,6 +3239,62 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testVector.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testVector.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testMatrix.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testMatrix.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testNumericalDerivative.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testNumericalDerivative.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testBlockMatrices.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testBlockMatrices.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="clean" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>clean</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testCholesky.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testCholesky.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testVectorValues.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -3158,6 +3327,38 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testGaussianConditional.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testGaussianConditional.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testGaussianFactorGraph.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testGaussianFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testGaussianJunctionTree.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testGaussianJunctionTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="SimpleRotation.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -3200,7 +3401,6 @@
</target>
<target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testGaussianISAM2</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -3222,6 +3422,46 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>install</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="clean" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>clean</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>all</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="dist" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>dist</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -3318,54 +3558,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>install</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="clean" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>clean</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>all</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="dist" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>dist</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -3398,6 +3590,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
</buildTargets>
</storageModule>
</cconfiguration>

118
configure.ac
View File

@ -13,9 +13,38 @@ AC_CONFIG_SRCDIR([gtsam/inference/SymbolicFactorGraph.cpp])
AC_CONFIG_SRCDIR([gtsam/linear/GaussianFactor.cpp])
AC_CONFIG_SRCDIR([gtsam/nonlinear/NonlinearOptimizer.cpp])
AC_CONFIG_SRCDIR([gtsam/slam/pose2SLAM.cpp])
AC_CONFIG_SRCDIR([tests/testSQP.cpp])
AC_CONFIG_SRCDIR([tests/testTupleValues.cpp])
AC_CONFIG_SRCDIR([examples/SimpleRotation.cpp])
# pull in all subdirectories of Eigen - FIXME: find a better way of handling this
AC_CONFIG_SRCDIR([gtsam/3rdparty/Makefile.am])
AC_CONFIG_SRCDIR([gtsam/3rdparty/Eigen/Makefile.am])
AC_CONFIG_SRCDIR([gtsam/3rdparty/Eigen/src/Makefile.am])
AC_CONFIG_SRCDIR([gtsam/3rdparty/Eigen/src/Cholesky/Makefile.am])
AC_CONFIG_SRCDIR([gtsam/3rdparty/Eigen/src/Core/Makefile.am])
AC_CONFIG_SRCDIR([gtsam/3rdparty/Eigen/src/Core/arch/Makefile.am])
AC_CONFIG_SRCDIR([gtsam/3rdparty/Eigen/src/Core/arch/AltiVec/Makefile.am])
AC_CONFIG_SRCDIR([gtsam/3rdparty/Eigen/src/Core/arch/Default/Makefile.am])
AC_CONFIG_SRCDIR([gtsam/3rdparty/Eigen/src/Core/arch/NEON/Makefile.am])
AC_CONFIG_SRCDIR([gtsam/3rdparty/Eigen/src/Core/arch/SSE/Makefile.am])
AC_CONFIG_SRCDIR([gtsam/3rdparty/Eigen/src/Core/products/Makefile.am])
AC_CONFIG_SRCDIR([gtsam/3rdparty/Eigen/src/Core/util/Makefile.am])
AC_CONFIG_SRCDIR([gtsam/3rdparty/Eigen/src/Eigen2Support/Makefile.am])
AC_CONFIG_SRCDIR([gtsam/3rdparty/Eigen/src/Eigen2Support/Geometry/Makefile.am])
AC_CONFIG_SRCDIR([gtsam/3rdparty/Eigen/src/Eigenvalues/Makefile.am])
AC_CONFIG_SRCDIR([gtsam/3rdparty/Eigen/src/Geometry/Makefile.am])
AC_CONFIG_SRCDIR([gtsam/3rdparty/Eigen/src/Geometry/arch/Makefile.am])
AC_CONFIG_SRCDIR([gtsam/3rdparty/Eigen/src/Householder/Makefile.am])
AC_CONFIG_SRCDIR([gtsam/3rdparty/Eigen/src/QR/Makefile.am])
AC_CONFIG_SRCDIR([gtsam/3rdparty/Eigen/src/SVD/Makefile.am])
AC_CONFIG_SRCDIR([gtsam/3rdparty/Eigen/src/Jacobi/Makefile.am])
AC_CONFIG_SRCDIR([gtsam/3rdparty/Eigen/src/misc/Makefile.am])
AC_CONFIG_SRCDIR([gtsam/3rdparty/Eigen/src/Sparse/Makefile.am])
AC_CONFIG_SRCDIR([gtsam/3rdparty/Eigen/src/LU/Makefile.am])
AC_CONFIG_SRCDIR([gtsam/3rdparty/Eigen/src/LU/arch/Makefile.am])
AC_CONFIG_SRCDIR([gtsam/3rdparty/Eigen/src/plugins/Makefile.am])
AC_CONFIG_SRCDIR([gtsam/3rdparty/Eigen/src/StlSupport/Makefile.am])
# Check for OS
AC_CANONICAL_HOST # needs to be called at some point earlier
AM_CONDITIONAL([DARWIN], [case $host_os in darwin*) true;; *) false;; esac])
@ -45,46 +74,6 @@ case $host_os in
;;
esac
# enable BLAS with general purpose script
AC_ARG_ENABLE([blas],
[ --enable-blas Enable external BLAS library],
[case "${enableval}" in
yes) blas=true ;;
no) blas=false ;;
*) AC_MSG_ERROR([bad value ${enableval} for --enable-blas]) ;;
esac],[blas=true])
ak
AM_CONDITIONAL([USE_BLAS], test x$blas = xtrue)
AM_CONDITIONAL([USE_BLAS_MACOS], [test x$blas = xtrue && test x$ISMAC = xtrue])
AM_CONDITIONAL([USE_BLAS_LINUX], [test x$blas = xtrue && test x$ISMAC = xfalse])
# enable LAPACK
AC_ARG_ENABLE([lapack],
[ --enable-lapack Enable external LAPACK library],
[case "${enableval}" in
yes) lapack=true ;;
no) lapack=false ;;
*) AC_MSG_ERROR([bad value ${enableval} for --enable-lapack]) ;;
esac],[lapack=true])
AM_CONDITIONAL([USE_LAPACK], test x$lapack = xtrue)
AM_CONDITIONAL([USE_LAPACK_MACOS], [test x$lapack = xtrue && test x$ISMAC = xtrue])
AM_CONDITIONAL([USE_LAPACK_LINUX], [test x$lapack = xtrue && test x$ISMAC = xfalse])
# On Mac, we use the Accelerate framework for BLAS/LAPACK
AM_CONDITIONAL([USE_ACCELERATE_MACOS], [(test x$lapack = xtrue || test x$blas = xtrue) && test x$ISMAC = xtrue])
#enable SparseQR for linear solving
AC_ARG_ENABLE([spqr],
[ --enable-spqr Enable SparseQR library support],
[case "${enableval}" in
yes) spqr=true ;;
no) spqr=false ;;
*) AC_MSG_ERROR([bad value ${enableval} for --enable-spqr]) ;;
esac],[spqr=false])
AM_CONDITIONAL([USE_SPQR], [test x$spqr = xtrue])
# enable profiling
AC_ARG_ENABLE([profiling],
[ --enable-profiling Enable profiling],
@ -167,13 +156,42 @@ AC_ARG_WITH([ccolamd-lib],
[CCOLAMDLib=${HOME}/lib])
AC_SUBST([CCOLAMDLib])
# For now we require blas, atlas, and lapack
#AM_COND_IF([test x$ISMAC = xtrue],
# [LINALG_CPPFLAGS="-I/System/Library/Frameworks/vecLib.framework/Headers ${CCOLAMDInc} -DGT_USE_LAPACK"],
# [LINALG_CPPFLAGS="${CCOLAMDInc} -DGT_USE_LAPACK"])
#AM_COND_IF([test x$ISMAC = xtrue],
# [LINALG_LDFLAGS="-Wl,/System/Library/Frameworks/Accelerate.framework/Accelerate ${CCOLAMDLib}"],
# [LINALG_LDFLAGS="-lcblas -latlas -llapack ${CCOLAMDLib}"])
AC_CONFIG_FILES([CppUnitLite/Makefile gtsam/base/Makefile gtsam/geometry/Makefile gtsam/inference/Makefile gtsam/linear/Makefile gtsam/nonlinear/Makefile gtsam/slam/Makefile gtsam/Makefile tests/Makefile examples/Makefile examples/vSLAMexample/Makefile Makefile])
AC_CONFIG_FILES([CppUnitLite/Makefile \
gtsam/3rdparty/Eigen/src/Cholesky/Makefile \
gtsam/3rdparty/Eigen/src/Core/Makefile \
gtsam/3rdparty/Eigen/src/Core/arch/Makefile \
gtsam/3rdparty/Eigen/src/Core/arch/AltiVec/Makefile \
gtsam/3rdparty/Eigen/src/Core/arch/Default/Makefile \
gtsam/3rdparty/Eigen/src/Core/arch/NEON/Makefile \
gtsam/3rdparty/Eigen/src/Core/arch/SSE/Makefile \
gtsam/3rdparty/Eigen/src/Core/products/Makefile \
gtsam/3rdparty/Eigen/src/Core/util/Makefile \
gtsam/3rdparty/Eigen/src/Eigenvalues/Makefile \
gtsam/3rdparty/Eigen/src/Geometry/Makefile \
gtsam/3rdparty/Eigen/src/Geometry/arch/Makefile \
gtsam/3rdparty/Eigen/src/Householder/Makefile \
gtsam/3rdparty/Eigen/src/QR/Makefile \
gtsam/3rdparty/Eigen/src/SVD/Makefile \
gtsam/3rdparty/Eigen/src/Jacobi/Makefile \
gtsam/3rdparty/Eigen/src/misc/Makefile \
gtsam/3rdparty/Eigen/src/Sparse/Makefile \
gtsam/3rdparty/Eigen/src/LU/Makefile \
gtsam/3rdparty/Eigen/src/LU/arch/Makefile \
gtsam/3rdparty/Eigen/src/plugins/Makefile \
gtsam/3rdparty/Eigen/src/StlSupport/Makefile \
gtsam/3rdparty/Eigen/src/Eigen2Support/Makefile \
gtsam/3rdparty/Eigen/src/Eigen2Support/Geometry/Makefile \
gtsam/3rdparty/Eigen/src/Makefile \
gtsam/3rdparty/Eigen/Makefile \
gtsam/3rdparty/Makefile \
gtsam/base/Makefile \
gtsam/geometry/Makefile \
gtsam/inference/Makefile \
gtsam/linear/Makefile \
gtsam/nonlinear/Makefile \
gtsam/slam/Makefile gtsam/Makefile \
tests/Makefile \
examples/Makefile \
examples/vSLAMexample/Makefile \
Makefile])
AC_OUTPUT

16
examples/Makefile.am
View File

@ -27,22 +27,6 @@ AM_LDFLAGS = $(BOOST_LDFLAGS)
LDADD = ../gtsam/libgtsam.la
AM_DEFAULT_SOURCE_EXT = .cpp
if USE_BLAS_LINUX
AM_LDFLAGS += -lcblas -latlas
endif
if USE_LAPACK
AM_CPPFLAGS += -DGT_USE_LAPACK
endif
if USE_LAPACK_LINUX
AM_LDFLAGS += -llapack
endif
if USE_ACCELERATE_MACOS
AM_LDFLAGS += -Wl,/System/Library/Frameworks/Accelerate.framework/Accelerate
endif
# rule to run an executable
%.run: % $(LDADD)
./$^

16
examples/vSLAMexample/Makefile.am
View File

@ -33,22 +33,6 @@ AM_LDFLAGS = $(BOOST_LDFLAGS)
LDADD = ../../gtsam/libgtsam.la
AM_DEFAULT_SOURCE_EXT = .cpp
if USE_BLAS_LINUX
AM_LDFLAGS += -lcblas -latlas
endif
if USE_LAPACK
AM_CPPFLAGS += -DGT_USE_LAPACK
endif
if USE_LAPACK_LINUX
AM_LDFLAGS += -llapack
endif
if USE_ACCELERATE_MACOS
AM_LDFLAGS += -Wl,/System/Library/Frameworks/Accelerate.framework/Accelerate
endif
# rule to run an executable
%.run: % $(LDADD)
./$^

21
gtsam/3rdparty/Eigen/Makefile.am vendored Normal file
View File

@ -0,0 +1,21 @@
# Eigen build/install - just need to copy header files into correct place
# All the sub-directories that need to be built
SUBDIRS = src
# And the corresponding libraries produced
# TODO add sublibs when something in 3rdparty actually needs compilation
SUBLIBS =
# use nostdinc to turn off -I. and -I.., we do not need them because
# header files are qualified so they can be included in external projects.
AUTOMAKE_OPTIONS = nostdinc
# add primary headers
headers = Array Cholesky Core Dense Eigen Eigen2Support Eigenvalues
headers += Geometry Householder Jacobi LeastSquares LU QR QtAlignedMalloc
headers += Sparse StdDeque StdList StdVector SVD
Eigendir = $(pkgincludedir)/3rdparty/Eigen
Eigen_includedir = $(includedir)/gtsam/3rdparty/Eigen
Eigen_HEADERS = $(headers)

7
gtsam/3rdparty/Eigen/src/CMakeLists.txt vendored
View File

@ -1,7 +0,0 @@
file(GLOB Eigen_src_subdirectories "*")
escape_string_as_regex(ESCAPED_CMAKE_CURRENT_SOURCE_DIR "${CMAKE_CURRENT_SOURCE_DIR}")
foreach(f ${Eigen_src_subdirectories})
if(NOT f MATCHES "\\.txt" AND NOT f MATCHES "${ESCAPED_CMAKE_CURRENT_SOURCE_DIR}/[.].+" )
add_subdirectory(${f})
endif()
endforeach()

6
gtsam/3rdparty/Eigen/src/Cholesky/CMakeLists.txt vendored
View File

@ -1,6 +0,0 @@
FILE(GLOB Eigen_Cholesky_SRCS "*.h")
INSTALL(FILES
${Eigen_Cholesky_SRCS}
DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen/src/Cholesky COMPONENT Devel
)

21
gtsam/3rdparty/Eigen/src/Cholesky/Makefile.am vendored Normal file
View File

@ -0,0 +1,21 @@
# Eigen build/install - just need to copy header files into correct place
# All the sub-directories that need to be built
SUBDIRS =
# And the corresponding libraries produced
# TODO add sublibs when something in 3rdparty actually needs compilation
SUBLIBS =
# use nostdinc to turn off -I. and -I.., we do not need them because
# header files are qualified so they can be included in external projects.
AUTOMAKE_OPTIONS = nostdinc
# add headers from src folder for Eigen
#Cholesky:
headers = LDLT.h
headers += LLT.h
Choleskydir = $(pkgincludedir)/3rdparty/Eigen/src/Cholesky
Cholesky_includedir = $(includedir)/gtsam/3rdparty/Eigen/src/Cholesky
Cholesky_HEADERS = $(headers)

10
gtsam/3rdparty/Eigen/src/Core/CMakeLists.txt vendored
View File

@ -1,10 +0,0 @@
FILE(GLOB Eigen_Core_SRCS "*.h")
INSTALL(FILES
${Eigen_Core_SRCS}
DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen/src/Core COMPONENT Devel
)
ADD_SUBDIRECTORY(products)
ADD_SUBDIRECTORY(util)
ADD_SUBDIRECTORY(arch)

78
gtsam/3rdparty/Eigen/src/Core/Makefile.am vendored Normal file
View File

@ -0,0 +1,78 @@
# Eigen build/install - just need to copy header files into correct place
# All the sub-directories that need to be built
SUBDIRS = arch products util
# And the corresponding libraries produced
# TODO add sublibs when something in 3rdparty actually needs compilation
SUBLIBS =
# use nostdinc to turn off -I. and -I.., we do not need them because
# header files are qualified so they can be included in external projects.
AUTOMAKE_OPTIONS = nostdinc
# add headers from src folder for Eigen
headers =
#Core:
headers += ArrayBase.h
headers += Array.h
headers += ArrayWrapper.h
headers += Assign.h
headers += BandMatrix.h
headers += Block.h
headers += BooleanRedux.h
headers += CommaInitializer.h
headers += CwiseBinaryOp.h
headers += CwiseNullaryOp.h
headers += CwiseUnaryOp.h
headers += CwiseUnaryView.h
headers += DenseBase.h
headers += DenseCoeffsBase.h
headers += DenseStorage.h
headers += Diagonal.h
headers += DiagonalMatrix.h
headers += DiagonalProduct.h
headers += Dot.h
headers += EigenBase.h
headers += Flagged.h
headers += ForceAlignedAccess.h
headers += Functors.h
headers += Fuzzy.h
headers += GenericPacketMath.h
headers += GlobalFunctions.h
headers += IO.h
headers += MapBase.h
headers += Map.h
headers += MathFunctions.h
headers += MatrixBase.h
headers += Matrix.h
headers += NestByValue.h
headers += NoAlias.h
headers += NumTraits.h
headers += PermutationMatrix.h
headers += PlainObjectBase.h
headers += ProductBase.h
headers += Product.h
headers += Random.h
headers += Redux.h
headers += Replicate.h
headers += ReturnByValue.h
headers += Reverse.h
headers += Select.h
headers += SelfAdjointView.h
headers += SelfCwiseBinaryOp.h
headers += SolveTriangular.h
headers += StableNorm.h
headers += Stride.h
headers += Swap.h
headers += Transpose.h
headers += Transpositions.h
headers += TriangularMatrix.h
headers += VectorBlock.h
headers += VectorwiseOp.h
headers += Visitor.h
Coredir = $(pkgincludedir)/3rdparty/Eigen/src/Core
Core_includedir = $(includedir)/gtsam/3rdparty/Eigen/src/Core
Core_HEADERS = $(headers)

23
gtsam/3rdparty/Eigen/src/Core/arch/AltiVec/Makefile.am vendored Normal file
View File

@ -0,0 +1,23 @@
# Eigen build/install - just need to copy header files into correct place
# All the sub-directories that need to be built
SUBDIRS =
# And the corresponding libraries produced
# TODO add sublibs when something in 3rdparty actually needs compilation
SUBLIBS =
# use nostdinc to turn off -I. and -I.., we do not need them because
# header files are qualified so they can be included in external projects.
AUTOMAKE_OPTIONS = nostdinc
headers =
# Core/arch/AltiVec:
headers += Complex.h
headers += PacketMath.h
AltiVecdir = $(pkgincludedir)/3rdparty/Eigen/src/Core/arch/AltiVec
AltiVec_includedir = $(includedir)/gtsam/3rdparty/Eigen/src/Core/arch/AltiVec
AltiVec_HEADERS = $(headers)

4
gtsam/3rdparty/Eigen/src/Core/arch/CMakeLists.txt vendored
View File

@ -1,4 +0,0 @@
ADD_SUBDIRECTORY(SSE)
ADD_SUBDIRECTORY(AltiVec)
ADD_SUBDIRECTORY(NEON)
ADD_SUBDIRECTORY(Default)

21
gtsam/3rdparty/Eigen/src/Core/arch/Default/Makefile.am vendored Normal file
View File

@ -0,0 +1,21 @@
# Eigen build/install - just need to copy header files into correct place
# All the sub-directories that need to be built
SUBDIRS =
# And the corresponding libraries produced
# TODO add sublibs when something in 3rdparty actually needs compilation
SUBLIBS =
# use nostdinc to turn off -I. and -I.., we do not need them because
# header files are qualified so they can be included in external projects.
AUTOMAKE_OPTIONS = nostdinc
headers =
# Core/arch/Default:
headers += Settings.h
Defaultdir = $(pkgincludedir)/3rdparty/Eigen/src/Core/arch/Default
Default_includedir = $(includedir)/gtsam/3rdparty/Eigen/src/Core/arch/Default
Default_HEADERS = $(headers)

4
gtsam/3rdparty/Eigen/src/Core/arch/Makefile.am vendored Normal file
View File

@ -0,0 +1,4 @@
# Eigen build/install - just need to copy header files into correct place
# All the sub-directories that need to be built
SUBDIRS = AltiVec Default NEON SSE

22
gtsam/3rdparty/Eigen/src/Core/arch/NEON/Makefile.am vendored Normal file
View File

@ -0,0 +1,22 @@
# Eigen build/install - just need to copy header files into correct place
# All the sub-directories that need to be built
SUBDIRS =
# And the corresponding libraries produced
# TODO add sublibs when something in 3rdparty actually needs compilation
SUBLIBS =
# use nostdinc to turn off -I. and -I.., we do not need them because
# header files are qualified so they can be included in external projects.
AUTOMAKE_OPTIONS = nostdinc
headers =
# Core/arch/NEON:
headers += Complex.h
headers += PacketMath.h
NEONdir = $(pkgincludedir)/3rdparty/Eigen/src/Core/arch/NEON
NEON_includedir = $(includedir)/gtsam/3rdparty/Eigen/src/Core/arch/NEON
NEON_HEADERS = $(headers)

22
gtsam/3rdparty/Eigen/src/Core/arch/SSE/Makefile.am vendored Normal file
View File

@ -0,0 +1,22 @@
# Eigen build/install - just need to copy header files into correct place
# All the sub-directories that need to be built
SUBDIRS =
# And the corresponding libraries produced
# TODO add sublibs when something in 3rdparty actually needs compilation
SUBLIBS =
# use nostdinc to turn off -I. and -I.., we do not need them because
# header files are qualified so they can be included in external projects.
AUTOMAKE_OPTIONS = nostdinc
headers =
# Core/arch/SSE:
headers += Complex.h
headers += MathFunctions.h
headers += PacketMath.h
SSEdir = $(pkgincludedir)/3rdparty/Eigen/src/Core/arch/SSE
SSE_includedir = $(includedir)/gtsam/3rdparty/Eigen/src/Core/arch/SSE
SSE_HEADERS = $(headers)

34
gtsam/3rdparty/Eigen/src/Core/products/Makefile.am vendored Normal file
View File

@ -0,0 +1,34 @@
# Eigen build/install - just need to copy header files into correct place
# All the sub-directories that need to be built
SUBDIRS =
# And the corresponding libraries produced
# TODO add sublibs when something in 3rdparty actually needs compilation
SUBLIBS =
# use nostdinc to turn off -I. and -I.., we do not need them because
# header files are qualified so they can be included in external projects.
AUTOMAKE_OPTIONS = nostdinc
headers =
# Core/products:
headers += CoeffBasedProduct.h
headers += GeneralBlockPanelKernel.h
headers += GeneralMatrixMatrix.h
headers += GeneralMatrixMatrixTriangular.h
headers += GeneralMatrixVector.h
headers += Parallelizer.h
headers += SelfadjointMatrixMatrix.h
headers += SelfadjointMatrixVector.h
headers += SelfadjointProduct.h
headers += SelfadjointRank2Update.h
headers += TriangularMatrixMatrix.h
headers += TriangularMatrixVector.h
headers += TriangularSolverMatrix.h
headers += TriangularSolverVector.h
productsdir = $(pkgincludedir)/3rdparty/Eigen/src/Core/products
products_includedir = $(includedir)/gtsam/3rdparty/Eigen/src/Core/products
products_HEADERS = $(headers)

30
gtsam/3rdparty/Eigen/src/Core/util/Makefile.am vendored Normal file
View File

@ -0,0 +1,30 @@
# Eigen build/install - just need to copy header files into correct place
# All the sub-directories that need to be built
SUBDIRS =
# And the corresponding libraries produced
# TODO add sublibs when something in 3rdparty actually needs compilation
SUBLIBS =
# use nostdinc to turn off -I. and -I.., we do not need them because
# header files are qualified so they can be included in external projects.
AUTOMAKE_OPTIONS = nostdinc
headers =
# Core/util:
headers += BlasUtil.h
headers += Constants.h
headers += DisableStupidWarnings.h
headers += ForwardDeclarations.h
headers += Macros.h
headers += Memory.h
headers += Meta.h
headers += ReenableStupidWarnings.h
headers += StaticAssert.h
headers += XprHelper.h
utildir = $(pkgincludedir)/3rdparty/Eigen/src/Core/util
util_includedir = $(includedir)/gtsam/3rdparty/Eigen/src/Core/util
util_HEADERS = $(headers)

8
gtsam/3rdparty/Eigen/src/Eigen2Support/CMakeLists.txt vendored
View File

@ -1,8 +0,0 @@
FILE(GLOB Eigen_Eigen2Support_SRCS "*.h")
INSTALL(FILES
${Eigen_Eigen2Support_SRCS}
DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen/src/Eigen2Support COMPONENT Devel
)
ADD_SUBDIRECTORY(Geometry)

6
gtsam/3rdparty/Eigen/src/Eigen2Support/Geometry/CMakeLists.txt vendored
View File

@ -1,6 +0,0 @@
FILE(GLOB Eigen_Eigen2Support_Geometry_SRCS "*.h")
INSTALL(FILES
${Eigen_Eigen2Support_Geometry_SRCS}
DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen/src/Eigen2Support/Geometry
)

31
gtsam/3rdparty/Eigen/src/Eigen2Support/Geometry/Makefile.am vendored Normal file
View File

@ -0,0 +1,31 @@
# Eigen build/install - just need to copy header files into correct place
# All the sub-directories that need to be built
SUBDIRS =
# And the corresponding libraries produced
# TODO add sublibs when something in 3rdparty actually needs compilation
SUBLIBS =
# use nostdinc to turn off -I. and -I.., we do not need them because
# header files are qualified so they can be included in external projects.
AUTOMAKE_OPTIONS = nostdinc
headers =
# Eigen2SupportGeometry:
headers += AlignedBox.h
headers += All.h
headers += AngleAxis.h
headers += Hyperplane.h
headers += ParametrizedLine.h
headers += Quaternion.h
headers += Rotation2D.h
headers += RotationBase.h
headers += Scaling.h
headers += Transform.h
headers += Translation.h
Geometrydir = $(pkgincludedir)/3rdparty/Eigen/src/Eigen2Support/Geometry
Geometry_includedir = $(includedir)/gtsam/3rdparty/Eigen/src/Eigen2Support/Geometry
Geometry_HEADERS = $(headers)

35
gtsam/3rdparty/Eigen/src/Eigen2Support/Makefile.am vendored Normal file
View File

@ -0,0 +1,35 @@
# Eigen build/install - just need to copy header files into correct place
# All the sub-directories that need to be built
SUBDIRS = Geometry
# And the corresponding libraries produced
# TODO add sublibs when something in 3rdparty actually needs compilation
SUBLIBS =
# use nostdinc to turn off -I. and -I.., we do not need them because
# header files are qualified so they can be included in external projects.
AUTOMAKE_OPTIONS = nostdinc
headers =
# Eigen2Support:
headers += Block.h
headers += Cwise.h
headers += CwiseOperators.h
headers += Lazy.h
headers += LeastSquares.h
headers += LU.h
headers += Macros.h
headers += MathFunctions.h
headers += Memory.h
headers += Meta.h
headers += Minor.h
headers += QR.h
headers += SVD.h
headers += TriangularSolver.h
headers += VectorBlock.h
Eigen2Supportdir = $(pkgincludedir)/3rdparty/Eigen/src/Eigen2Support
Eigen2Support_includedir = $(includedir)/gtsam/3rdparty/Eigen/src/Eigen2Support
Eigen2Support_HEADERS = $(headers)

30
gtsam/3rdparty/Eigen/src/Eigenvalues/Makefile.am vendored Normal file
View File

@ -0,0 +1,30 @@
# Eigen build/install - just need to copy header files into correct place
# All the sub-directories that need to be built
SUBDIRS =
# And the corresponding libraries produced
# TODO add sublibs when something in 3rdparty actually needs compilation
SUBLIBS =
# use nostdinc to turn off -I. and -I.., we do not need them because
# header files are qualified so they can be included in external projects.
AUTOMAKE_OPTIONS = nostdinc
headers =
# Eigenvalues:
headers += ComplexEigenSolver.h
headers += ComplexSchur.h
headers += EigenSolver.h
headers += EigenvaluesCommon.h
headers += GeneralizedSelfAdjointEigenSolver.h
headers += HessenbergDecomposition.h
headers += MatrixBaseEigenvalues.h
headers += RealSchur.h
headers += SelfAdjointEigenSolver.h
headers += Tridiagonalization.h
Eigenvaluesdir = $(pkgincludedir)/3rdparty/Eigen/src/Eigenvalues
Eigenvalues_includedir = $(includedir)/gtsam/3rdparty/Eigen/src/Eigenvalues
Eigenvalues_HEADERS = $(headers)

34
gtsam/3rdparty/Eigen/src/Geometry/Makefile.am vendored Normal file
View File

@ -0,0 +1,34 @@
# Eigen build/install - just need to copy header files into correct place
# All the sub-directories that need to be built
SUBDIRS = arch
# And the corresponding libraries produced
# TODO add sublibs when something in 3rdparty actually needs compilation
SUBLIBS =
# use nostdinc to turn off -I. and -I.., we do not need them because
# header files are qualified so they can be included in external projects.
AUTOMAKE_OPTIONS = nostdinc
headers =
# Geometry:
headers += AlignedBox.h
headers += AngleAxis.h
headers += EulerAngles.h
headers += Homogeneous.h
headers += Hyperplane.h
headers += OrthoMethods.h
headers += ParametrizedLine.h
headers += Quaternion.h
headers += Rotation2D.h
headers += RotationBase.h
headers += Scaling.h
headers += Transform.h
headers += Translation.h
headers += Umeyama.h
Geometrydir = $(pkgincludedir)/3rdparty/Eigen/src/Geometry
Geometry_includedir = $(includedir)/gtsam/3rdparty/Eigen/src/Geometry
Geometry_HEADERS = $(headers)

6
gtsam/3rdparty/Eigen/src/Geometry/arch/CMakeLists.txt vendored
View File

@ -1,6 +0,0 @@
FILE(GLOB Eigen_Geometry_arch_SRCS "*.h")
INSTALL(FILES
${Eigen_Geometry_arch_SRCS}
DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen/src/Geometry/arch COMPONENT Devel
)

22
gtsam/3rdparty/Eigen/src/Geometry/arch/Makefile.am vendored Normal file
View File

@ -0,0 +1,22 @@
# Eigen build/install - just need to copy header files into correct place
# All the sub-directories that need to be built
SUBDIRS =
# And the corresponding libraries produced
# TODO add sublibs when something in 3rdparty actually needs compilation
SUBLIBS =
# use nostdinc to turn off -I. and -I.., we do not need them because
# header files are qualified so they can be included in external projects.
AUTOMAKE_OPTIONS = nostdinc
# add primary headers
headers =
# Geometry/arch:
headers += Geometry_SSE.h
archdir = $(pkgincludedir)/3rdparty/Eigen/src/Geometry/arch
arch_includedir = $(includedir)/gtsam/3rdparty/Eigen/src/Geometry/arch
arch_HEADERS = $(headers)

6
gtsam/3rdparty/Eigen/src/Householder/CMakeLists.txt vendored
View File

@ -1,6 +0,0 @@
FILE(GLOB Eigen_Householder_SRCS "*.h")
INSTALL(FILES
${Eigen_Householder_SRCS}
DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen/src/Householder COMPONENT Devel
)

23
gtsam/3rdparty/Eigen/src/Householder/Makefile.am vendored Normal file
View File

@ -0,0 +1,23 @@
# Eigen build/install - just need to copy header files into correct place
# All the sub-directories that need to be built
SUBDIRS =
# And the corresponding libraries produced
# TODO add sublibs when something in 3rdparty actually needs compilation
SUBLIBS =
# use nostdinc to turn off -I. and -I.., we do not need them because
# header files are qualified so they can be included in external projects.
AUTOMAKE_OPTIONS = nostdinc
headers =
# Householder:
headers += BlockHouseholder.h
headers += Householder.h
headers += HouseholderSequence.h
Householderdir = $(pkgincludedir)/3rdparty/Eigen/src/Householder
Householder_includedir = $(includedir)/gtsam/3rdparty/Eigen/src/Householder
Householder_HEADERS = $(headers)

6
gtsam/3rdparty/Eigen/src/Jacobi/CMakeLists.txt vendored
View File

@ -1,6 +0,0 @@
FILE(GLOB Eigen_Jacobi_SRCS "*.h")
INSTALL(FILES
${Eigen_Jacobi_SRCS}
DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen/src/Jacobi COMPONENT Devel
)

20
gtsam/3rdparty/Eigen/src/Jacobi/Makefile.am vendored Normal file
View File

@ -0,0 +1,20 @@
# Eigen build/install - just need to copy header files into correct place
# All the sub-directories that need to be built
SUBDIRS =
# And the corresponding libraries produced
# TODO add sublibs when something in 3rdparty actually needs compilation
SUBLIBS =
# use nostdinc to turn off -I. and -I.., we do not need them because
# header files are qualified so they can be included in external projects.
AUTOMAKE_OPTIONS = nostdinc
# add primary headers
# Jacobi
headers = Jacobi.h
Jacobidir = $(pkgincludedir)/3rdparty/Eigen/src/Jacobi
Jacobi_includedir = $(includedir)/gtsam/3rdparty/Eigen/src/Jacobi
Jacobi_HEADERS = $(headers)

8
gtsam/3rdparty/Eigen/src/LU/CMakeLists.txt vendored
View File

@ -1,8 +0,0 @@
FILE(GLOB Eigen_LU_SRCS "*.h")
INSTALL(FILES
${Eigen_LU_SRCS}
DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen/src/LU COMPONENT Devel
)
ADD_SUBDIRECTORY(arch)

24
gtsam/3rdparty/Eigen/src/LU/Makefile.am vendored Normal file
View File

@ -0,0 +1,24 @@
# Eigen build/install - just need to copy header files into correct place
# All the sub-directories that need to be built
SUBDIRS = arch
# And the corresponding libraries produced
# TODO add sublibs when something in 3rdparty actually needs compilation
SUBLIBS =
# use nostdinc to turn off -I. and -I.., we do not need them because
# header files are qualified so they can be included in external projects.
AUTOMAKE_OPTIONS = nostdinc
headers =
# LU:
headers += Determinant.h
headers += FullPivLU.h
headers += Inverse.h
headers += PartialPivLU.h
LUdir = $(pkgincludedir)/3rdparty/Eigen/src/LU
LU_includedir = $(includedir)/gtsam/3rdparty/Eigen/src/LU
LU_HEADERS = $(headers)

6
gtsam/3rdparty/Eigen/src/LU/arch/CMakeLists.txt vendored
View File

@ -1,6 +0,0 @@
FILE(GLOB Eigen_LU_arch_SRCS "*.h")
INSTALL(FILES
${Eigen_LU_arch_SRCS}
DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen/src/LU/arch COMPONENT Devel
)

19
gtsam/3rdparty/Eigen/src/LU/arch/Makefile.am vendored Normal file
View File

@ -0,0 +1,19 @@
# Eigen build/install - just need to copy header files into correct place
# All the sub-directories that need to be built
SUBDIRS =
# And the corresponding libraries produced
# TODO add sublibs when something in 3rdparty actually needs compilation
SUBLIBS =
# use nostdinc to turn off -I. and -I.., we do not need them because
# header files are qualified so they can be included in external projects.
AUTOMAKE_OPTIONS = nostdinc
# LU/arch:
headers = Inverse_SSE.h
archdir = $(pkgincludedir)/3rdparty/Eigen/src/LU/arch
arch_includedir = $(includedir)/gtsam/3rdparty/Eigen/src/LU/arch
arch_HEADERS = $(headers)

9
gtsam/3rdparty/Eigen/src/Makefile.am vendored Normal file
View File

@ -0,0 +1,9 @@
# Eigen build/install - just need to copy header files into correct place
# All the sub-directories that need to be built
SUBDIRS = Cholesky Core Eigenvalues
SUBDIRS += Geometry
SUBDIRS += Jacobi misc Sparse
SUBDIRS += LU plugins StlSupport
SUBDIRS += Householder QR SVD
SUBDIRS += Eigen2Support

6
gtsam/3rdparty/Eigen/src/QR/CMakeLists.txt vendored
View File

@ -1,6 +0,0 @@
FILE(GLOB Eigen_QR_SRCS "*.h")
INSTALL(FILES
${Eigen_QR_SRCS}
DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen/src/QR COMPONENT Devel
)

23
gtsam/3rdparty/Eigen/src/QR/Makefile.am vendored Normal file
View File

@ -0,0 +1,23 @@
# Eigen build/install - just need to copy header files into correct place
# All the sub-directories that need to be built
SUBDIRS =
# And the corresponding libraries produced
# TODO add sublibs when something in 3rdparty actually needs compilation
SUBLIBS =
# use nostdinc to turn off -I. and -I.., we do not need them because
# header files are qualified so they can be included in external projects.
AUTOMAKE_OPTIONS = nostdinc
headers =
# QR:
headers += ColPivHouseholderQR.h
headers += FullPivHouseholderQR.h
headers += HouseholderQR.h
QRdir = $(pkgincludedir)/3rdparty/Eigen/src/QR
QR_includedir = $(includedir)/gtsam/3rdparty/Eigen/src/QR
QR_HEADERS = $(headers)

6
gtsam/3rdparty/Eigen/src/SVD/CMakeLists.txt vendored
View File

@ -1,6 +0,0 @@
FILE(GLOB Eigen_SVD_SRCS "*.h")
INSTALL(FILES
${Eigen_SVD_SRCS}
DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen/src/SVD COMPONENT Devel
)

22
gtsam/3rdparty/Eigen/src/SVD/Makefile.am vendored Normal file
View File

@ -0,0 +1,22 @@
# Eigen build/install - just need to copy header files into correct place
# All the sub-directories that need to be built
SUBDIRS =
# And the corresponding libraries produced
# TODO add sublibs when something in 3rdparty actually needs compilation
SUBLIBS =
# use nostdinc to turn off -I. and -I.., we do not need them because
# header files are qualified so they can be included in external projects.
AUTOMAKE_OPTIONS = nostdinc
headers =
#SVD:
headers += JacobiSVD.h
headers += UpperBidiagonalization.h
SVDdir = $(pkgincludedir)/3rdparty/Eigen/src/SVD
SVD_includedir = $(includedir)/gtsam/3rdparty/Eigen/src/SVD
SVD_HEADERS = $(headers)

6
gtsam/3rdparty/Eigen/src/Sparse/CMakeLists.txt vendored
View File

@ -1,6 +0,0 @@
FILE(GLOB Eigen_Sparse_SRCS "*.h")
INSTALL(FILES
${Eigen_Sparse_SRCS}
DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen/src/Sparse COMPONENT Devel
)

45
gtsam/3rdparty/Eigen/src/Sparse/Makefile.am vendored Normal file
View File

@ -0,0 +1,45 @@
# Eigen build/install - just need to copy header files into correct place
# All the sub-directories that need to be built
SUBDIRS =
# And the corresponding libraries produced
# TODO add sublibs when something in 3rdparty actually needs compilation
SUBLIBS =
# use nostdinc to turn off -I. and -I.., we do not need them because
# header files are qualified so they can be included in external projects.
AUTOMAKE_OPTIONS = nostdinc
headers =
# Sparse:
headers += AmbiVector.h
headers += CompressedStorage.h
headers += CoreIterators.h
headers += DynamicSparseMatrix.h
headers += MappedSparseMatrix.h
headers += SparseAssign.h
headers += SparseBlock.h
headers += SparseCwiseBinaryOp.h
headers += SparseCwiseUnaryOp.h
headers += SparseDenseProduct.h
headers += SparseDiagonalProduct.h
headers += SparseDot.h
headers += SparseFuzzy.h
headers += SparseMatrixBase.h
headers += SparseMatrix.h
headers += SparseProduct.h
headers += SparseRedux.h
headers += SparseSelfAdjointView.h
headers += SparseSparseProduct.h
headers += SparseTranspose.h
headers += SparseTriangularView.h
headers += SparseUtil.h
headers += SparseVector.h
headers += SparseView.h
headers += TriangularSolver.h
Sparsedir = $(pkgincludedir)/3rdparty/Eigen/src/Sparse
Sparse_includedir = $(includedir)/gtsam/3rdparty/Eigen/src/Sparse
Sparse_HEADERS = $(headers)

6
gtsam/3rdparty/Eigen/src/StlSupport/CMakeLists.txt vendored
View File

@ -1,6 +0,0 @@
FILE(GLOB Eigen_StlSupport_SRCS "*.h")
INSTALL(FILES
${Eigen_StlSupport_SRCS}
DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen/src/StlSupport COMPONENT Devel
)

24
gtsam/3rdparty/Eigen/src/StlSupport/Makefile.am vendored Normal file
View File

@ -0,0 +1,24 @@
# Eigen build/install - just need to copy header files into correct place
# All the sub-directories that need to be built
SUBDIRS =
# And the corresponding libraries produced
# TODO add sublibs when something in 3rdparty actually needs compilation
SUBLIBS =
# use nostdinc to turn off -I. and -I.., we do not need them because
# header files are qualified so they can be included in external projects.
AUTOMAKE_OPTIONS = nostdinc
headers =
# StlSupport:
headers += details.h
headers += StdDeque.h
headers += StdList.h
headers += StdVector.h
StlSupportdir = $(pkgincludedir)/3rdparty/Eigen/src/StlSupport
StlSupport_includedir = $(includedir)/gtsam/3rdparty/Eigen/src/StlSupport
StlSupport_HEADERS = $(headers)

6
gtsam/3rdparty/Eigen/src/misc/CMakeLists.txt vendored
View File

@ -1,6 +0,0 @@
FILE(GLOB Eigen_misc_SRCS "*.h")
INSTALL(FILES
${Eigen_misc_SRCS}
DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen/src/misc COMPONENT Devel
)

23
gtsam/3rdparty/Eigen/src/misc/Makefile.am vendored Normal file
View File

@ -0,0 +1,23 @@
# Eigen build/install - just need to copy header files into correct place
# All the sub-directories that need to be built
SUBDIRS =
# And the corresponding libraries produced
# TODO add sublibs when something in 3rdparty actually needs compilation
SUBLIBS =
# use nostdinc to turn off -I. and -I.., we do not need them because
# header files are qualified so they can be included in external projects.
AUTOMAKE_OPTIONS = nostdinc
headers =
# misc:
headers += Image.h
headers += Kernel.h
headers += Solve.h
miscdir = $(pkgincludedir)/3rdparty/Eigen/src/misc
misc_includedir = $(includedir)/gtsam/3rdparty/Eigen/src/misc
misc_HEADERS = $(headers)

6
gtsam/3rdparty/Eigen/src/plugins/CMakeLists.txt vendored
View File

@ -1,6 +0,0 @@
FILE(GLOB Eigen_plugins_SRCS "*.h")
INSTALL(FILES
${Eigen_plugins_SRCS}
DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen/src/plugins COMPONENT Devel
)

27
gtsam/3rdparty/Eigen/src/plugins/Makefile.am vendored Normal file
View File

@ -0,0 +1,27 @@
# Eigen build/install - just need to copy header files into correct place
# All the sub-directories that need to be built
SUBDIRS =
# And the corresponding libraries produced
# TODO add sublibs when something in 3rdparty actually needs compilation
SUBLIBS =
# use nostdinc to turn off -I. and -I.., we do not need them because
# header files are qualified so they can be included in external projects.
AUTOMAKE_OPTIONS = nostdinc
headers =
# plugins:
headers += ArrayCwiseBinaryOps.h
headers += ArrayCwiseUnaryOps.h
headers += BlockMethods.h
headers += CommonCwiseBinaryOps.h
headers += CommonCwiseUnaryOps.h
headers += MatrixCwiseBinaryOps.h
headers += MatrixCwiseUnaryOps.h
pluginsdir = $(pkgincludedir)/3rdparty/Eigen/src/plugins
plugins_includedir = $(includedir)/gtsam/3rdparty/Eigen/src/plugins
plugins_HEADERS = $(headers)

8
gtsam/3rdparty/Makefile.am vendored Normal file
View File

@ -0,0 +1,8 @@
# 3rd Party libraries to be built and installed along with gtsam
# All the sub-directories that need to be built
SUBDIRS = Eigen
# And the corresponding libraries produced
# TODO add sublibs when something in 3rdparty actually needs compilation
# SUBLIBS =

6
gtsam/Makefile.am
View File

@ -1,6 +1,6 @@
# All the sub-directories that need to be built
SUBDIRS = base geometry inference linear nonlinear slam
SUBDIRS = base geometry inference linear nonlinear slam 3rdparty
# And the corresponding libraries produced
SUBLIBS = base/libbase.la geometry/libgeometry.la inference/libinference.la \
@ -12,7 +12,3 @@ libgtsam_la_SOURCES =
nodist_EXTRA_libgtsam_la_SOURCES = dummy.cxx
libgtsam_la_LIBADD = $(SUBLIBS) -L$(CCOLAMDLib) $(BOOST_LDFLAGS) -lccolamd
libgtsam_la_LDFLAGS = -no-undefined -version-info 0:0:0
if USE_ACCELERATE_MACOS
libgtsam_la_LDFLAGS += -Wl,/System/Library/Frameworks/Accelerate.framework/Accelerate
endif

279
gtsam/base/DenseQRUtil.cpp
View File

@ -19,149 +19,146 @@
#include <gtsam/base/timing.h>
#include <gtsam/base/DenseQRUtil.h>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <string.h> // for memcpy and memset
using namespace std;
namespace ublas = boost::numeric::ublas;
//namespace ublas = boost::numeric::ublas;
#ifdef GT_USE_LAPACK
namespace gtsam {
/* ************************************************************************* */
int* MakeStairs(Matrix& A) {
const int m = A.size1();
const int n = A.size2();
int* Stair = new int[n];
// record the starting pointer of each row
double* a[m];
list<int> remained;
a[0] = A.data().begin();
for(int i=0; i<m-1; i++) {
a[i+1] = a[i] + n;
remained.push_back(i);
}
remained.push_back(m-1);
// reorder the rows
int j;
vector<int> sorted;
list<int>::iterator itRemained;
for(j = 0; j < n; ) {
// remove the non-zero rows in the current column
for(itRemained = remained.begin(); itRemained!=remained.end(); ) {
if (*(a[*itRemained]) != 0) {
sorted.push_back(*itRemained);
itRemained = remained.erase(itRemained);
} else {
a[*itRemained]++;
itRemained ++;
}
}
// record the stair number
Stair[j++] = m - remained.size();
if(remained.empty()) break;
}
// all the remained columns have maximum stair
for (; j<n; j++)
Stair[j] = m;
// copy the new row
Matrix A_new = zeros(m,n);
int offset[m];
offset[0] = 0;
for(int i=1; i<m; i++)
offset[i] = offset[i-1] +n;
vector<int>::const_iterator itSorted;
double* ptr1 = A.data().begin();
double* ptr2 = A_new.data().begin();
int row = 0, sizeOfRow = n * sizeof(double);
for(itSorted=sorted.begin(); itSorted!=sorted.end(); itSorted++, row++)
memcpy(ptr2+offset[row], ptr1+offset[*itSorted], sizeOfRow);
A = A_new;
return Stair;
}
void printColumnMajor(const double* a, const int m, const int n) {
for(int i=0; i<m; i++) {
for(int j=0; j<n; j++)
cout << a[j*m+i] << "\t";
cout << endl;
}
}
/* ************************************************************************* */
void householder_denseqr(Matrix &A, int* Stair) {
tic("householder_denseqr");
int m = A.size1();
int n = A.size2();
bool allocedStair = false;
if (Stair == NULL) {
allocedStair = true;
Stair = new int[n];
for(int j=0; j<n; j++)
Stair[j] = m;
}
tic("householder_denseqr: row->col");
// convert from row major to column major
ublas::matrix<double, ublas::column_major> Acolwise(A);
double *a = Acolwise.data().begin();
toc("householder_denseqr: row->col");
tic("householder_denseqr: denseqr_front");
int npiv = min(m,n);
int b = min(min(m,n),32);
double W[b*(n+b)];
DenseQR(m, n, npiv, a, Stair, W);
toc("householder_denseqr: denseqr_front");
tic("householder_denseqr: col->row");
int k0 = 0;
int j0;
int k;
memset(A.data().begin(), 0, m*n*sizeof(double));
for(int j=0; j<n; j++, k0+=m) {
k = k0;
j0 = min(j+1,m);
for(int i=0; i<j0; i++, k++)
A(i,j) = a[k];
}
toc("householder_denseqr: col->row");
if(allocedStair) delete[] Stair;
toc("householder_denseqr");
}
void householder_denseqr_colmajor(ublas::matrix<double, ublas::column_major>& A, int *Stair) {
int m = A.size1();
int n = A.size2();
assert(Stair != NULL);
tic(1, "householder_denseqr");
int npiv = min(m,n);
int b = min(min(m,n),32);
double W[b*(n+b)];
DenseQR(m, n, npiv, A.data().begin(), Stair, W);
toc(1, "householder_denseqr");
}
} // namespace gtsam
#endif
//#ifdef GT_USE_LAPACK
//namespace gtsam {
//
// /* ************************************************************************* */
// int* MakeStairs(Matrix& A) {
//
// const int m = A.rows();
// const int n = A.cols();
// int* Stair = new int[n];
//
// // record the starting pointer of each row
// double* a[m];
// list<int> remained;
// a[0] = A.data();
// for(int i=0; i<m-1; i++) {
// a[i+1] = a[i] + n;
// remained.push_back(i);
// }
// remained.push_back(m-1);
//
// // reorder the rows
// int j;
// vector<int> sorted;
// list<int>::iterator itRemained;
// for(j = 0; j < n; ) {
// // remove the non-zero rows in the current column
// for(itRemained = remained.begin(); itRemained!=remained.end(); ) {
// if (*(a[*itRemained]) != 0) {
// sorted.push_back(*itRemained);
// itRemained = remained.erase(itRemained);
// } else {
// a[*itRemained]++;
// itRemained ++;
// }
// }
//
// // record the stair number
// Stair[j++] = m - remained.size();
//
// if(remained.empty()) break;
// }
//
// // all the remained columns have maximum stair
// for (; j<n; j++)
// Stair[j] = m;
//
// // copy the new row
// Matrix A_new = zeros(m,n);
// int offset[m];
// offset[0] = 0;
// for(int i=1; i<m; i++)
// offset[i] = offset[i-1] +n;
// vector<int>::const_iterator itSorted;
// double* ptr1 = A.data();
// double* ptr2 = A_new.data();
// int row = 0, sizeOfRow = n * sizeof(double);
// for(itSorted=sorted.begin(); itSorted!=sorted.end(); itSorted++, row++)
// memcpy(ptr2+offset[row], ptr1+offset[*itSorted], sizeOfRow);
//
// A = A_new;
//
// return Stair;
// }
//
// void printColumnMajor(const double* a, const int m, const int n) {
// for(int i=0; i<m; i++) {
// for(int j=0; j<n; j++)
// cout << a[j*m+i] << "\t";
// cout << endl;
// }
// }
//
// /* ************************************************************************* */
// void householder_denseqr(Matrix &A, int* Stair) {
//
// tic("householder_denseqr");
//
// int m = A.rows();
// int n = A.cols();
//
// bool allocedStair = false;
// if (Stair == NULL) {
// allocedStair = true;
// Stair = new int[n];
// for(int j=0; j<n; j++)
// Stair[j] = m;
// }
//
// tic("householder_denseqr: row->col");
// // convert from row major to column major
// MatrixColMajor Acolwise(A);
// double *a = Acolwise.data();
// toc("householder_denseqr: row->col");
//
// tic("householder_denseqr: denseqr_front");
// int npiv = min(m,n);
// int b = min(min(m,n),32);
// double W[b*(n+b)];
// DenseQR(m, n, npiv, a, Stair, W);
// toc("householder_denseqr: denseqr_front");
//
// tic("householder_denseqr: col->row");
// int k0 = 0;
// int j0;
// int k;
// memset(A.data(), 0, m*n*sizeof(double));
// for(int j=0; j<n; j++, k0+=m) {
// k = k0;
// j0 = min(j+1,m);
// for(int i=0; i<j0; i++, k++)
// A(i,j) = a[k];
// }
//
// toc("householder_denseqr: col->row");
//
//
// if(allocedStair) delete[] Stair;
//
// toc("householder_denseqr");
// }
//
// void householder_denseqr_colmajor(MatrixColMajor& A, int *Stair) {
// int m = A.rows();
// int n = A.cols();
//
// assert(Stair != NULL);
//
// tic(1, "householder_denseqr");
// int npiv = min(m,n);
// int b = min(min(m,n),32);
// double W[b*(n+b)];
// DenseQR(m, n, npiv, A.data(), Stair, W);
// toc(1, "householder_denseqr");
// }
//
//} // namespace gtsam
//#endif

28
gtsam/base/DenseQRUtil.h
View File

@ -21,17 +21,17 @@
#include <gtsam/base/Matrix.h>
#ifdef GT_USE_LAPACK
#include <gtsam/base/DenseQR.h>
namespace gtsam {
/** make stairs and speed up householder_denseqr. Stair is defined as the row index of where zero entries start in each column */
int* MakeStairs(Matrix &A);
/** Householder tranformation, zeros below diagonal */
void householder_denseqr(Matrix &A, int* Stair = NULL);
void householder_denseqr_colmajor(boost::numeric::ublas::matrix<double, boost::numeric::ublas::column_major>& A, int *Stair);
}
#endif
//#ifdef GT_USE_LAPACK
//#include <gtsam/base/DenseQR.h>
//
//namespace gtsam {
//
// /** make stairs and speed up householder_denseqr. Stair is defined as the row index of where zero entries start in each column */
// int* MakeStairs(Matrix &A);
//
// /** Householder tranformation, zeros below diagonal */
// void householder_denseqr(Matrix &A, int* Stair = NULL);
//
// void householder_denseqr_colmajor(MatrixColMajor& A, int *Stair);
//}
//#endif

28
gtsam/base/FixedVector.h
View File

@ -28,10 +28,10 @@ namespace gtsam {
* size checking.
*/
template<size_t N>
class FixedVector : public boost::numeric::ublas::bounded_vector<double, N> ,
class FixedVector : public Eigen::Matrix<double, N, 1>,
public Testable<FixedVector<N> > {
public:
typedef boost::numeric::ublas::bounded_vector<double, N> Base;
typedef Eigen::Matrix<double, N, 1> Base;
/** default constructor */
FixedVector() {}
@ -44,7 +44,7 @@ public:
/** Initialize with a C-style array */
FixedVector(const double* values) {
std::copy(values, values+N, this->data().begin());
std::copy(values, values+N, this->data());
}
/**
@ -69,10 +69,11 @@ public:
* @param constant value
*/
inline static FixedVector repeat(double value) {
FixedVector v;
for (size_t i=0; i<N; ++i)
v(i) = value;
return v;
return FixedVector(Base::Constant(value));
// FixedVector v;
// for (size_t i=0; i<N; ++i)
// v(i) = value;
// return v;
}
/**
@ -83,9 +84,10 @@ public:
* @return delta vector
*/
inline static FixedVector delta(size_t i, double value) {
FixedVector v = zero();
v(i) = value;
return v;
return FixedVector(Base::Unit(i) * value);
// FixedVector v = zero();
// v(i) = value;
// return v;
}
/**
@ -94,17 +96,17 @@ public:
* @param index of the one
* @return basis vector
*/
inline static FixedVector basis(size_t i) { return delta(i, 1.0); }
inline static FixedVector basis(size_t i) { return FixedVector(Base::Unit(i)); }
/**
* Create zero vector
*/
inline static FixedVector zero() { return repeat(0.0);}
inline static FixedVector zero() { return FixedVector(Base::Zero());}
/**
* Create vector initialized to ones
*/
inline static FixedVector ones() { return repeat(1.0);}
inline static FixedVector ones() { return FixedVector(FixedVector::Ones());}
static size_t dim() { return Base::max_size; }

140
gtsam/base/LieVector.h
View File

@ -23,83 +23,83 @@
namespace gtsam {
/**
* LieVector is a wrapper around vector to allow it to be a Lie type
*/
struct LieVector : public Vector, public Lie<LieVector>, Testable<LieVector> {
/** default constructor - should be unnecessary */
LieVector() {}
/** initialize from a normal vector */
LieVector(const Vector& v) : Vector(v) {}
/** wrap a double */
LieVector(double d) : Vector(Vector_(1, d)) {}
/** constructor with size and initial data, row order ! */
LieVector(size_t m, const double* const data);
/** Specify arguments directly, as in Vector_() - always force these to be doubles */
LieVector(size_t m, ...);
/** get the underlying vector */
inline Vector vector() const {
return static_cast<Vector>(*this);
}
/** print @param s optional string naming the object */
inline void print(const std::string& name="") const {
gtsam::print(vector(), name);
}
/** equality up to tolerance */
inline bool equals(const LieVector& expected, double tol=1e-5) const {
return gtsam::equal(vector(), expected.vector(), tol);
}
/**
* LieVector is a wrapper around vector to allow it to be a Lie type
* Returns dimensionality of the tangent space
*/
struct LieVector : public Vector, public Lie<LieVector>, Testable<LieVector> {
inline size_t dim() const { return this->size(); }
/** default constructor - should be unnecessary */
LieVector() {}
/**
* Returns Exponential map update of T
* Default implementation calls global binary function
*/
inline LieVector expmap(const Vector& v) const { return LieVector(vector() + v); }
/** initialize from a normal vector */
LieVector(const Vector& v) : Vector(v) {}
/** expmap around identity */
static inline LieVector Expmap(const Vector& v) { return LieVector(v); }
/** wrap a double */
LieVector(double d) : Vector(Vector_(1, d)) {}
/**
* Returns Log map
* Default Implementation calls global binary function
*/
inline Vector logmap(const LieVector& lp) const {
return lp.vector() - vector();
}
/** constructor with size and initial data, row order ! */
LieVector(size_t m, const double* const data);
/** Logmap around identity - just returns with default cast back */
static inline Vector Logmap(const LieVector& p) { return p; }
/** Specify arguments directly, as in Vector_() - always force these to be doubles */
LieVector(size_t m, ...);
/** compose with another object */
inline LieVector compose(const LieVector& p) const {
return LieVector(vector() + p);
}
/** get the underlying vector */
inline Vector vector() const {
return static_cast<Vector>(*this);
}
/** between operation */
inline LieVector between(const LieVector& l2,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const {
if(H1) *H1 = -eye(dim());
if(H2) *H2 = eye(l2.dim());
return LieVector(l2.vector() - vector());
}
/** print @param s optional string naming the object */
inline void print(const std::string& name="") const {
gtsam::print(vector(), name);
}
/** equality up to tolerance */
inline bool equals(const LieVector& expected, double tol=1e-5) const {
return gtsam::equal(vector(), expected.vector(), tol);
}
/**
* Returns dimensionality of the tangent space
*/
inline size_t dim() const { return this->size(); }
/**
* Returns Exponential map update of T
* Default implementation calls global binary function
*/
inline LieVector expmap(const Vector& v) const { return LieVector(vector() + v); }
/** expmap around identity */
static inline LieVector Expmap(const Vector& v) { return LieVector(v); }
/**
* Returns Log map
* Default Implementation calls global binary function
*/
inline Vector logmap(const LieVector& lp) const {
return lp.vector() - vector();
}
/** Logmap around identity - just returns with default cast back */
static inline Vector Logmap(const LieVector& p) { return p; }
/** compose with another object */
inline LieVector compose(const LieVector& p) const {
return LieVector(vector() + p);
}
/** between operation */
inline LieVector between(const LieVector& l2,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const {
if(H1) *H1 = -eye(dim());
if(H2) *H2 = eye(l2.dim());
return LieVector(l2.vector() - vector());
}
/** invert the object and yield a new one */
inline LieVector inverse() const {
return LieVector(-1.0 * vector());
}
};
/** invert the object and yield a new one */
inline LieVector inverse() const {
return LieVector(-1.0 * vector());
}
};
} // \namespace gtsam

42
gtsam/base/Makefile.am
View File

@ -13,15 +13,15 @@ check_PROGRAMS =
# base Math
headers += FixedVector.h types.h blockMatrices.h Matrix-inl.h lapack.h
sources += Vector.cpp Matrix.cpp cholesky.cpp
check_PROGRAMS += tests/testFixedVector tests/testVector tests/testMatrix tests/testCholesky
headers += FixedVector.h types.h blockMatrices.h Matrix-inl.h
sources += Vector.cpp Matrix.cpp
sources += cholesky.cpp
check_PROGRAMS += tests/testFixedVector tests/testVector tests/testMatrix tests/testBlockMatrices
check_PROGRAMS += tests/testCholesky
check_PROGRAMS += tests/testNumericalDerivative
if USE_LAPACK
sources += DenseQR.cpp DenseQRUtil.cpp
check_PROGRAMS += tests/testDenseQRUtil
endif
#sources += DenseQR.cpp DenseQRUtil.cpp
#check_PROGRAMS += tests/testDenseQRUtil
# Testing
headers += Testable.h TestableAssertions.h numericalDerivative.h
@ -39,7 +39,8 @@ sources += DSFVector.cpp
check_PROGRAMS += tests/testBTree tests/testDSF tests/testDSFVector
# Timing tests
noinst_PROGRAMS = tests/timeMatrix tests/timeublas tests/timeVirtual tests/timeTest
noinst_PROGRAMS = tests/timeMatrix tests/timeVirtual tests/timeTest
#noinst_PROGRAMS += tests/timeublas
#----------------------------------------------------------------------------------------------------
# Create a libtool library that is not installed
@ -54,18 +55,9 @@ libbase_la_SOURCES = $(sources)
AM_CPPFLAGS =
# On Mac, we compile using the BLAS/LAPACK headers in the Accelerate framework
if USE_ACCELERATE_MACOS
AM_CPPFLAGS += -I/System/Library/Frameworks/vecLib.framework/Headers
endif
AM_CPPFLAGS += $(BOOST_CPPFLAGS) -I$(CCOLAMDInc) -I$(top_srcdir)
AM_LDFLAGS = $(BOOST_LDFLAGS)
if USE_BLAS
AM_CPPFLAGS += -DGT_USE_CBLAS
endif
#----------------------------------------------------------------------------------------------------
# rules to build local programs
#----------------------------------------------------------------------------------------------------
@ -74,22 +66,6 @@ AM_DEFAULT_SOURCE_EXT = .cpp
AM_LDFLAGS += $(boost_serialization) -L$(CCOLAMDLib) -lccolamd
LDADD = libbase.la ../../CppUnitLite/libCppUnitLite.a
if USE_BLAS_LINUX
AM_LDFLAGS += -lcblas -latlas
endif
if USE_LAPACK
AM_CPPFLAGS += -DGT_USE_LAPACK
endif
if USE_LAPACK_LINUX
AM_LDFLAGS += -llapack
endif
if USE_ACCELERATE_MACOS
AM_LDFLAGS += -Wl,/System/Library/Frameworks/Accelerate.framework/Accelerate
endif
# rule to run an executable
%.run: % $(LDADD)
./$^

156
gtsam/base/Matrix-inl.h
View File

@ -26,84 +26,82 @@ namespace gtsam {
using namespace std;
/* ************************************************************************* */
/**
* backSubstitute U*x=b
* @param U an upper triangular matrix
* @param b an RHS vector
* @return the solution x of U*x=b
*/
template<class MATRIX, class VECTOR>
Vector backSubstituteUpper(const boost::numeric::ublas::matrix_expression<MATRIX>& _U,
const boost::numeric::ublas::vector_expression<VECTOR>& _b, bool unit=false) {
const MATRIX& U(*static_cast<const MATRIX*>(&_U));
const VECTOR& b(*static_cast<const VECTOR*>(&_b));
size_t n = U.size2();
#ifndef NDEBUG
size_t m = U.size1();
if (m!=n)
throw invalid_argument("backSubstituteUpper: U must be square");
#endif
Vector result(n);
for (size_t i = n; i > 0; i--) {
double zi = b(i-1);
for (size_t j = i+1; j <= n; j++)
zi -= U(i-1,j-1) * result(j-1);
#ifndef NDEBUG
if(!unit && fabs(U(i-1,i-1)) <= numeric_limits<double>::epsilon()) {
stringstream ss;
ss << "backSubstituteUpper: U is singular,\n";
print(U, "U: ", ss);
throw invalid_argument(ss.str());
}
#endif
if (!unit) zi /= U(i-1,i-1);
result(i-1) = zi;
}
return result;
}
/* ************************************************************************* */
/**
* backSubstitute x'*U=b'
* @param U an upper triangular matrix
* @param b an RHS vector
* @param unit, set true if unit triangular
* @return the solution x of x'*U=b'
* TODO: use boost
*/
template<class VECTOR, class MATRIX>
Vector backSubstituteUpper(const boost::numeric::ublas::vector_expression<VECTOR>& _b,
const boost::numeric::ublas::matrix_expression<MATRIX>& _U, bool unit=false) {
const VECTOR& b(*static_cast<const VECTOR*>(&_b));
const MATRIX& U(*static_cast<const MATRIX*>(&_U));
size_t n = U.size2();
#ifndef NDEBUG
size_t m = U.size1();
if (m!=n)
throw invalid_argument("backSubstituteUpper: U must be square");
#endif
Vector result(n);
for (size_t i = 1; i <= n; i++) {
double zi = b(i-1);
for (size_t j = 1; j < i; j++)
zi -= U(j-1,i-1) * result(j-1);
#ifndef NDEBUG
if(!unit && fabs(U(i-1,i-1)) <= numeric_limits<double>::epsilon()) {
stringstream ss;
ss << "backSubstituteUpper: U is singular,\n";
print(U, "U: ", ss);
throw invalid_argument(ss.str());
}
#endif
if (!unit) zi /= U(i-1,i-1);
result(i-1) = zi;
}
return result;
}
///* ************************************************************************* */
///**
// * backSubstitute U*x=b
// * @param U an upper triangular matrix
// * @param b an RHS vector
// * @return the solution x of U*x=b
// */
//template<class MATRIX, class VECTOR>
//Vector backSubstituteUpper(const MATRIX& U, const VECTOR& b, bool unit=false) {
//// const MATRIX& U(*static_cast<const MATRIX*>(&_U));
//// const VECTOR& b(*static_cast<const VECTOR*>(&_b));
// size_t n = U.cols();
//#ifndef NDEBUG
// size_t m = U.rows();
// if (m!=n)
// throw invalid_argument("backSubstituteUpper: U must be square");
//#endif
//
// Vector result(n);
// for (size_t i = n; i > 0; i--) {
// double zi = b(i-1);
// for (size_t j = i+1; j <= n; j++)
// zi -= U(i-1,j-1) * result(j-1);
//#ifndef NDEBUG
// if(!unit && fabs(U(i-1,i-1)) <= numeric_limits<double>::epsilon()) {
// stringstream ss;
// ss << "backSubstituteUpper: U is singular,\n";
// print(U, "U: ", ss);
// throw invalid_argument(ss.str());
// }
//#endif
// if (!unit) zi /= U(i-1,i-1);
// result(i-1) = zi;
// }
//
// return result;
//}
//
///* ************************************************************************* */
///**
// * backSubstitute x'*U=b'
// * @param U an upper triangular matrix
// * @param b an RHS vector
// * @param unit, set true if unit triangular
// * @return the solution x of x'*U=b'
// * TODO: use boost
// */
//template<class VECTOR, class MATRIX>
//Vector backSubstituteUpper(const VECTOR& b, const MATRIX& U, bool unit=false) {
//// const VECTOR& b(*static_cast<const VECTOR*>(&_b));
//// const MATRIX& U(*static_cast<const MATRIX*>(&_U));
// size_t n = U.cols();
//#ifndef NDEBUG
// size_t m = U.rows();
// if (m!=n)
// throw invalid_argument("backSubstituteUpper: U must be square");
//#endif
//
// Vector result(n);
// for (size_t i = 1; i <= n; i++) {
// double zi = b(i-1);
// for (size_t j = 1; j < i; j++)
// zi -= U(j-1,i-1) * result(j-1);
//#ifndef NDEBUG
// if(!unit && fabs(U(i-1,i-1)) <= numeric_limits<double>::epsilon()) {
// stringstream ss;
// ss << "backSubstituteUpper: U is singular,\n";
// print(U, "U: ", ss);
// throw invalid_argument(ss.str());
// }
//#endif
// if (!unit) zi /= U(i-1,i-1);
// result(i-1) = zi;
// }
//
// return result;
//}
}

1169
gtsam/base/Matrix.cpp
View File

File diff suppressed because it is too large Load Diff

263
gtsam/base/Matrix.h
View File

@ -11,10 +11,11 @@
/**
* @file Matrix.h
* @brief typedef and functions to augment Boost's ublas::matrix<double>
* @brief typedef and functions to augment Eigen's MatrixXd
* @author Christian Potthast
* @author Kai Ni
* @author Frank Dellaert
* @author Alex Cunningham
*/
// \callgraph
@ -23,20 +24,28 @@
#include <list>
#include <boost/format.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/tuple/tuple.hpp>
#include <gtsam/3rdparty/Eigen/QR>
#include <gtsam/base/Vector.h>
/**
* Vector is a *global* typedef
* Matrix is a *global* typedef
* wrap-matlab does this typedef as well
* we use the default < double,row_major,unbounded_array<double> >
*/
#if ! defined (MEX_H)
typedef boost::numeric::ublas::matrix<double> Matrix;
typedef boost::numeric::ublas::matrix<double, boost::numeric::ublas::column_major> MatrixColMajor;
#endif
// FIXME: replace to handle matlab wrapper
//#if ! defined (MEX_H)
//typedef boost::numeric::ublas::matrix<double> Matrix;
//typedef boost::numeric::ublas::matrix<double, boost::numeric::ublas::column_major> MatrixColMajor;
//#endif
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> Matrix;
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor> MatrixColMajor;
// Matrix expressions for accessing parts of matrices
typedef Eigen::Block<Matrix> SubMatrix;
typedef Eigen::Block<const Matrix> ConstSubMatrix;
namespace gtsam {
@ -72,7 +81,23 @@ Matrix diag(const Vector& v);
/**
* equals with an tolerance
*/
bool equal_with_abs_tol(const Matrix& A, const Matrix& B, double tol = 1e-9);
template <class MATRIX>
bool equal_with_abs_tol(const Eigen::DenseBase<MATRIX>& A, const Eigen::DenseBase<MATRIX>& B, double tol = 1e-9) {
const size_t n1 = A.cols(), m1 = A.rows();
const size_t n2 = B.cols(), m2 = B.rows();
if(m1!=m2 || n1!=n2) return false;
for(size_t i=0; i<m1; i++)
for(size_t j=0; j<n1; j++) {
if(std::isnan(A(i,j)) xor std::isnan(B(i,j)))
return false;
else if(fabs(A(i,j) - B(i,j)) > tol)
return false;
}
return true;
}
/**
* equality is just equal_with_abs_tol 1e-9
@ -91,7 +116,9 @@ inline bool operator!=(const Matrix& A, const Matrix& B) {
/**
* equals with an tolerance, prints out message if unequal
*/
// FIXME: make better use of templates to test these properly
bool assert_equal(const Matrix& A, const Matrix& B, double tol = 1e-9);
bool assert_equal(const MatrixColMajor& A, const MatrixColMajor& B, double tol = 1e-9);
/**
* equals with an tolerance, prints out message if unequal
@ -108,11 +135,6 @@ bool linear_independent(const Matrix& A, const Matrix& B, double tol = 1e-9);
*/
bool linear_dependent(const Matrix& A, const Matrix& B, double tol = 1e-9);
/**
* overload * for matrix-vector multiplication (as BOOST does not)
*/
inline Vector operator*(const Matrix& A, const Vector & v) { return prod(A,v);}
/**
* BLAS Level-2 style e <- e + alpha*A*x
*/
@ -144,15 +166,12 @@ void transposeMultiplyAdd(const Matrix& A, const Vector& e, Vector& x);
*/
void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, SubVector x);
/**
* overload * for vector*matrix multiplication (as BOOST does not)
*/
inline Vector operator*(const Vector & v, const Matrix& A) { return prod(v,A);}
/**
* overload * for matrix multiplication (as BOOST does not)
*/
inline Matrix operator*(const Matrix& A, const Matrix& B) { return prod(A,B);}
/** products using old-style format to improve compatibility */
template<class MATRIX>
inline MATRIX prod(const MATRIX& A, const MATRIX&B) {
MATRIX result = A * B;
return result;
}
/**
* convert to column vector, column order !!!
@ -163,6 +182,7 @@ Vector Vector_(const Matrix& A);
* print a matrix
*/
void print(const Matrix& A, const std::string& s = "", std::ostream& stream = std::cout);
void print(const MatrixColMajor& A, const std::string& s = "", std::ostream& stream = std::cout);
/**
* save a matrix to file, which can be loaded by matlab
@ -178,7 +198,11 @@ void save(const Matrix& A, const std::string &s, const std::string& filename);
* @param j2 last col index + 1
* @return submatrix A(i1:i2-1,j1:j2-1)
*/
Matrix sub(const Matrix& A, size_t i1, size_t i2, size_t j1, size_t j2);
template<class MATRIX>
Eigen::Block<const MATRIX> sub(const MATRIX& A, size_t i1, size_t i2, size_t j1, size_t j2) {
size_t m=i2-i1, n=j2-j1;
return A.block(i1,j1,m,n);
}
/**
* insert a submatrix IN PLACE at a specified location in a larger matrix
@ -191,13 +215,26 @@ Matrix sub(const Matrix& A, size_t i1, size_t i2, size_t j1, size_t j2);
void insertSub(Matrix& big, const Matrix& small, size_t i, size_t j);
/**
* extracts a column from a matrix
* NOTE: using this without the underscore is the ublas version!
* Extracts a column view from a matrix that avoids a copy
* @param matrix to extract column from
* @param index of the column
* @return the column in vector form
* @return a const view of the matrix
*/
Vector column_(const Matrix& A, size_t j);
template<class MATRIX>
const typename MATRIX::ConstColXpr column(const MATRIX& A, size_t j) {
return A.col(j);
}
/**
* Extracts a row view from a matrix that avoids a copy
* @param matrix to extract row from
* @param index of the row
* @return a const view of the matrix
*/
template<class MATRIX>
const typename MATRIX::ConstRowXpr row(const MATRIX& A, size_t j) {
return A.row(j);
}
/**
* inserts a column into a matrix IN PLACE
@ -210,15 +247,25 @@ Vector column_(const Matrix& A, size_t j);
void insertColumn(Matrix& A, const Vector& col, size_t j);
void insertColumn(Matrix& A, const Vector& col, size_t i, size_t j);
Vector columnNormSquare(const MatrixColMajor &A) ;
Vector columnNormSquare(const MatrixColMajor &A);
/**
* extracts a row from a matrix
* @param matrix to extract row from
* @param index of the row
* @return the row in vector form
* Zeros all of the elements below the diagonal of a matrix, in place
* @param A is a matrix, to be modified in place
* @param cols is the number of columns to zero, use zero for all columns
*/
Vector row_(const Matrix& A, size_t j);
template<class MATRIX>
void zeroBelowDiagonal(MATRIX& A, size_t cols=0) {
const size_t m = A.rows(), n = A.cols();
const size_t k = (cols) ? std::min(cols, std::min(m,n)) : std::min(m,n);
for (size_t j=0; j<k; ++j)
A.col(j).segment(j+1, m-(j+1)).setZero();
}
/**
* static transpose function, just calls Eigen transpose member function
*/
inline Matrix trans(const Matrix& A) { return A.transpose(); }
/**
* solve AX=B via in-place Lu factorization and backsubstitution
@ -239,10 +286,31 @@ Matrix inverse(const Matrix& A);
*/
std::pair<Matrix,Matrix> qr(const Matrix& A);
/**
* QR factorization using Eigen's internal block QR algorithm
* @param A is the input matrix, and is the output
* @param clear_below_diagonal enables zeroing out below diagonal
*/
template <class MATRIX>
void inplace_QR(MATRIX& A, bool clear_below_diagonal=true) {
size_t rows = A.rows();
size_t cols = A.cols();
size_t size = std::min(rows,cols);
typedef Eigen::internal::plain_diag_type<MatrixColMajor>::type HCoeffsType;
typedef Eigen::internal::plain_row_type<MatrixColMajor>::type RowVectorType;
HCoeffsType hCoeffs(size);
RowVectorType temp(cols);
Eigen::internal::householder_qr_inplace_blocked(A, hCoeffs, 48, temp.data());
zeroBelowDiagonal(A);
}
/**
* Imperative version of Householder rank 1 update
*/
void householder_update(Matrix &A, size_t j, double beta, const Vector& vjm);
//void householder_update(Matrix &A, size_t j, double beta, const Vector& vjm);
/**
* Imperative algorithm for in-place full elimination with
@ -271,24 +339,43 @@ void householder_(Matrix& A, size_t k);
*/
void householder(Matrix& A, size_t k);
#ifdef GT_USE_LAPACK
#ifdef USE_LAPACK_QR
/**
* Householder tranformation, zeros below diagonal
* Householder tranformation, Householder vectors below diagonal
* Column Major Version
* @param k number of columns to zero out below diagonal
* @param A matrix
* @param copy_vectors - true to copy Householder vectors below diagonal
* @return nothing: in place !!!
*/
void householder(Matrix& A);
void householder_(MatrixColMajor& A, size_t k, bool copy_vectors=true);
/**
* Householder tranformation directly on a column-major matrix. Does not zero
* below the diagonal, so it will contain Householder vectors.
* Householder tranformation, zeros below diagonal
* Column Major version
* @param k number of columns to zero out below diagonal
* @param A matrix
* @return nothing: in place !!!
* FIXME: this uses the LAPACK QR function, so it cannot be used on Ubuntu (without
* lots of hacks, at least)
*/
void householderColMajor(MatrixColMajor& A);
#endif
#endif
void householder(MatrixColMajor& A, size_t k);
//#ifdef GT_USE_LAPACK
//#ifdef USE_LAPACK_QR
///**
// * Householder tranformation, zeros below diagonal
// * @return nothing: in place !!!
// */
//void householder(Matrix& A);
//
///**
// * Householder tranformation directly on a column-major matrix. Does not zero
// * below the diagonal, so it will contain Householder vectors.
// * @return nothing: in place !!!
// * FIXME: this uses the LAPACK QR function, so it cannot be used on Ubuntu (without
// * lots of hacks, at least)
// */
//void householderColMajor(MatrixColMajor& A);
//#endif
//#endif
/**
* backSubstitute U*x=b
@ -296,11 +383,11 @@ void householderColMajor(MatrixColMajor& A);
* @param b an RHS vector
* @param unit, set true if unit triangular
* @return the solution x of U*x=b
* TODO: use boost
*/
template<class MATRIX, class VECTOR>
Vector backSubstituteUpper(const boost::numeric::ublas::matrix_expression<MATRIX>& U,
const boost::numeric::ublas::vector_expression<VECTOR>& b, bool unit=false);
//FIXME: add back expression form
//template<class MATRIX, class VECTOR>
//Vector backSubstituteUpper(const MATRIX& U, const VECTOR& b, bool unit=false);
Vector backSubstituteUpper(const Matrix& U, const Vector& b, bool unit=false);
/**
* backSubstitute x'*U=b'
@ -308,11 +395,11 @@ Vector backSubstituteUpper(const boost::numeric::ublas::matrix_expression<MATRIX
* @param b an RHS vector
* @param unit, set true if unit triangular
* @return the solution x of x'*U=b'
* TODO: use boost
*/
template<class VECTOR, class MATRIX>
Vector backSubstituteUpper(const boost::numeric::ublas::vector_expression<VECTOR>& b,
const boost::numeric::ublas::matrix_expression<MATRIX>& U, bool unit=false);
//FIXME: add back expression form
//template<class VECTOR, class MATRIX>
//Vector backSubstituteUpper(const VECTOR& b, const MATRIX& U, bool unit=false);
Vector backSubstituteUpper(const Vector& b, const Matrix& U, bool unit=false);
/**
* backSubstitute L*x=b
@ -320,7 +407,6 @@ Vector backSubstituteUpper(const boost::numeric::ublas::vector_expression<VECTOR
* @param b an RHS vector
* @param unit, set true if unit triangular
* @return the solution x of L*x=b
* TODO: use boost
*/
Vector backSubstituteLower(const Matrix& L, const Vector& d, bool unit=false);
@ -379,6 +465,40 @@ Matrix RtR(const Matrix& A);
/** Return the inverse of a S.P.D. matrix. Inversion is done via Cholesky decomposition. */
Matrix cholesky_inverse(const Matrix &A);
/**
* SVD computes economy SVD A=U*S*V'
* @param A an m*n matrix
* @param U output argument: rotation matrix
* @param S output argument: vector of singular values, sorted by default, pass false as last argument to avoid sorting!!!
* @param V output argument: rotation matrix
* if m > n then U*S*V' = (m*n)*(n*n)*(n*n) (the m-n columns of U are of no use)
* if m < n then U*S*V' = (m*m)*(m*m)*(m*n) (the n-m columns of V are of no use)
* Careful! The dimensions above reflect V', not V, which is n*m if m<n.
* U is a basis in R^m, V is a basis in R^n
* You can just pass empty matrices U,V, and vector S, they will be re-allocated.
*/
void svd(const Matrix& A, Matrix& U, Vector& S, Matrix& V);
/*
* In place SVD, will throw an exception when m < n
* @param A an m*n matrix is modified to contain U
* @param S output argument: vector of singular values, sorted by default, pass false as last argument to avoid sorting!!!
* @param V output argument: rotation matrix
* if m > n then U*S*V' = (m*n)*(n*n)*(n*n) (the m-n columns of U are of no use)
* You can just pass empty matrix V and vector S, they will be re-allocated.
*/
void svd(Matrix& A, Vector& S, Matrix& V);
/**
* Direct linear transform algorithm that calls svd
* to find a vector v that minimizes the algebraic error A*v
* @param A of size m*n, where m>=n (pad with zero rows if not!)
* Returns rank of A, minimum error (singular value),
* and corresponding eigenvector (column of V, with A=U*S*V')
*/
boost::tuple<int, double, Vector>
DLT(const Matrix& A, double rank_tol = 1e-9);
/**
* Numerical exponential map, naive approach, not industrial strength !!!
* @param A matrix to exponentiate
@ -392,3 +512,38 @@ Matrix expm(const Matrix& A, size_t K=7);
result_.addFailure(Failure(name_, __FILE__, __LINE__, #expected, #actual)); }
} // namespace gtsam
#include <boost/serialization/nvp.hpp>
#include <boost/serialization/split_free.hpp>
namespace boost {
namespace serialization {
// split version - sends sizes ahead
template<class Archive>
void save(Archive & ar, const Matrix & m, unsigned int version)
{
const int rows = m.rows(), cols = m.cols(), elements = rows*cols;
std::vector<double> raw_data(elements);
std::copy(m.data(), m.data()+elements, raw_data.begin());
ar << make_nvp("rows", rows);
ar << make_nvp("cols", cols);
ar << make_nvp("data", raw_data);
}
template<class Archive>
void load(Archive & ar, Matrix & m, unsigned int version)
{
size_t rows, cols;
std::vector<double> raw_data;
ar >> make_nvp("rows", rows);
ar >> make_nvp("cols", cols);
ar >> make_nvp("data", raw_data);
m = Matrix(rows, cols);
std::copy(raw_data.begin(), raw_data.end(), m.data());
}
} // namespace serialization
} // namespace boost
BOOST_SERIALIZATION_SPLIT_FREE(Matrix)

852
gtsam/base/Vector.cpp
View File

@ -10,11 +10,11 @@
* -------------------------------------------------------------------------- */
/**
* @file Vector.cpp
* @brief typedef and functions to augment Boost's ublas::vector<double>
* @author Kai Ni
* @author Frank Dellaert
*/
* @file Vector.cpp
* @brief typedef and functions to augment Boost's ublas::vector<double>
* @author Kai Ni
* @author Frank Dellaert
*/
#include <stdarg.h>
#include <limits>
@ -31,460 +31,456 @@
#include <Windows.h>
#endif
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <gtsam/base/Vector.h>
using namespace std;
namespace ublas = boost::numeric::ublas;
namespace gtsam {
/* ************************************************************************* */
void odprintf_(const char *format, ostream& stream, ...) {
char buf[4096], *p = buf;
int n;
va_list args;
va_start(args, stream);
#ifdef WIN32
n = _vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL
#else
n = vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL
#endif
va_end(args);
#ifdef WIN32
OutputDebugString(buf);
#else
stream << buf;
#endif
}
/* ************************************************************************* */
void odprintf(const char *format, ...)
{
char buf[4096], *p = buf;
int n;
/* ************************************************************************* */
void odprintf_(const char *format, ostream& stream, ...) {
char buf[4096], *p = buf;
int n;
va_list args;
va_start(args, format);
#ifdef WIN32
n = _vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL
#else
n = vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL
#endif
va_end(args);
va_list args;
va_start(args, stream);
#ifdef WIN32
n = _vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL
#else
n = vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL
#endif
va_end(args);
#ifdef WIN32
OutputDebugString(buf);
#else
cout << buf;
#endif
}
#ifdef WIN32
OutputDebugString(buf);
#else
stream << buf;
#endif
}
/* ************************************************************************* */
Vector Vector_( size_t m, const double* const data) {
Vector v(m);
copy(data, data+m, v.data().begin());
return v;
}
/* ************************************************************************* */
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;
}
/* ************************************************************************* */
bool zero(const Vector& v) {
bool result = true;
size_t n = v.size();
for( size_t j = 0 ; j < n ; j++)
result = result && (v(j) == 0.0);
return result;
}
/* ************************************************************************* */
Vector repeat(size_t n, double value) {
Vector v(n, value);
return v;
}
/* ************************************************************************* */
/* ************************************************************************* */
Vector delta(size_t n, size_t i, double value) {
Vector v = zero(n);
v(i) = value;
return v;
}
void odprintf(const char *format, ...)
{
char buf[4096], *p = buf;
int n;
/* ************************************************************************* */
void print(const Vector& v, const string& s, ostream& stream) {
size_t n = v.size();
odprintf_("%s [", stream, s.c_str());
for(size_t i=0; i<n; i++)
odprintf_("%g%s", stream, v[i], (i<n-1 ? "; " : ""));
odprintf_("];\n", stream);
}
/* ************************************************************************* */
void save(const Vector& v, const string &s, const string& filename) {
fstream stream(filename.c_str(), fstream::out | fstream::app);
print(v, s + "=", stream);
stream.close();
}
va_list args;
va_start(args, format);
#ifdef WIN32
n = _vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL
#else
n = vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL
#endif
va_end(args);
/* ************************************************************************* */
bool operator==(const Vector& vec1,const Vector& vec2) {
Vector::const_iterator it1 = vec1.begin();
Vector::const_iterator it2 = vec2.begin();
size_t m = vec1.size();
for(size_t i=0; i<m; i++)
if(it1[i] != it2[i])
return false;
return true;
}
/* ************************************************************************* */
bool greaterThanOrEqual(const Vector& vec1, const Vector& vec2) {
Vector::const_iterator it1 = vec1.begin();
Vector::const_iterator it2 = vec2.begin();
size_t m = vec1.size();
for(size_t i=0; i<m; i++)
if(!(it1[i] >= it2[i]))
return false;
return true;
}
#ifdef WIN32
OutputDebugString(buf);
#else
cout << buf;
#endif
}
/* ************************************************************************* */
bool equal_with_abs_tol(const Vector& vec1, const Vector& vec2, double tol) {
Vector::const_iterator it1 = vec1.begin();
Vector::const_iterator it2 = vec2.begin();
if (vec1.size()!=vec2.size()) return false;
for(size_t i=0; i<vec1.size(); i++) {
if(isnan(it1[i]) xor isnan(it2[i]))
return false;
if(fabs(it1[i] - it2[i]) > tol)
return false;
}
return true;
}
/* ************************************************************************* */
bool assert_equal(const Vector& expected, const Vector& actual, double tol) {
if (equal_with_abs_tol(expected,actual,tol)) return true;
cout << "not equal:" << endl;
print(expected, "expected");
print(actual, "actual");
return false;
}
/* ************************************************************************* */
Vector Vector_( size_t m, const double* const data) {
// Vector v(m);
// for (size_t i=0; i<m; ++i)
// v(i) = data[i];
// return v;
/* ************************************************************************* */
bool assert_equal(SubVector expected, SubVector actual, double tol) {
if (equal_with_abs_tol(expected,actual,tol)) return true;
cout << "not equal:" << endl;
print(expected, "expected");
print(actual, "actual");
return false;
}
Vector A(m);
copy(data, data+m, A.data());
return A;
}
/* ************************************************************************* */
bool assert_equal(ConstSubVector expected, ConstSubVector actual, double tol) {
if (equal_with_abs_tol(expected,actual,tol)) return true;
cout << "not equal:" << endl;
print(expected, "expected");
print(actual, "actual");
return false;
}
/* ************************************************************************* */
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;
}
/* ************************************************************************* */
bool linear_dependent(const Vector& vec1, const Vector& vec2, double tol) {
if (vec1.size()!=vec2.size()) return false;
Vector::const_iterator it1 = vec1.begin();
Vector::const_iterator it2 = vec2.begin();
boost::optional<double> scale;
for(size_t i=0; i<vec1.size(); i++) {
if((fabs(it1[i])>tol&&fabs(it2[i])<tol) || (fabs(it1[i])<tol&&fabs(it2[i])>tol))
return false;
if(it1[i] == 0 && it2[i] == 0)
continue;
if (!scale)
scale = it1[i] / it2[i];
else if (fabs(it1[i] - it2[i] * (*scale)) > tol)
return false;
}
return scale.is_initialized();
}
/* ************************************************************************* */
Vector Vector_(const std::vector<double>& d) {
Vector v(d.size());
copy(d.begin(), d.end(), v.data());
return v;
}
/* ************************************************************************* */
Vector sub(const Vector &v, size_t i1, size_t i2) {
size_t n = i2-i1;
Vector v_return(n);
for( size_t i = 0; i < n; i++ )
v_return(i) = v(i1 + i);
return v_return;
}
/* ************************************************************************* */
void subInsert(Vector& big, const Vector& small, size_t i) {
ublas::vector_range<Vector> colsubproxy(big, ublas::range (i, i+small.size()));
colsubproxy = small;
}
/* ************************************************************************* */
bool zero(const Vector& v) {
bool result = true;
size_t n = v.size();
for( size_t j = 0 ; j < n ; j++)
result = result && (v(j) == 0.0);
return result;
}
/* ************************************************************************* */
Vector emul(const Vector &a, const Vector &b) {
size_t n = a.size();
assert (b.size()==n);
Vector c(n);
for( size_t i = 0; i < n; i++ )
c(i) = a(i)*b(i);
return c;
/* ************************************************************************* */
Vector repeat(size_t n, double value) {
return Vector::Constant(n, value);
}
/* ************************************************************************* */
Vector delta(size_t n, size_t i, double value) {
return Vector::Unit(n, i) * value;
// Vector v = zero(n);
// v(i) = value;
// return v;
}
/* ************************************************************************* */
void print(const Vector& v, const string& s, ostream& stream) {
size_t n = v.size();
odprintf_("%s [", stream, s.c_str());
for(size_t i=0; i<n; i++)
odprintf_("%g%s", stream, v[i], (i<n-1 ? "; " : ""));
odprintf_("];\n", stream);
}
/* ************************************************************************* */
void save(const Vector& v, const string &s, const string& filename) {
fstream stream(filename.c_str(), fstream::out | fstream::app);
print(v, s + "=", stream);
stream.close();
}
/* ************************************************************************* */
bool operator==(const Vector& vec1,const Vector& vec2) {
if (vec1.size() != vec2.size()) return false;
size_t m = vec1.size();
for(size_t i=0; i<m; i++)
if(vec1[i] != vec2[i])
return false;
return true;
}
/* ************************************************************************* */
bool greaterThanOrEqual(const Vector& vec1, const Vector& vec2) {
size_t m = vec1.size();
for(size_t i=0; i<m; i++)
if(!(vec1[i] >= vec2[i]))
return false;
return true;
}
/* ************************************************************************* */
bool equal_with_abs_tol(const Vector& vec1, const Vector& vec2, double tol) {
if (vec1.size()!=vec2.size()) return false;
size_t m = vec1.size();
for(size_t i=0; i<m; ++i) {
if(isnan(vec1[i]) xor isnan(vec2[i]))
return false;
if(fabs(vec1[i] - vec2[i]) > tol)
return false;
}
return true;
}
/* ************************************************************************* */
bool equal_with_abs_tol(const SubVector& vec1, const SubVector& vec2, double tol) {
if (vec1.size()!=vec2.size()) return false;
size_t m = vec1.size();
for(size_t i=0; i<m; ++i) {
if(isnan(vec1[i]) xor isnan(vec2[i]))
return false;
if(fabs(vec1[i] - vec2[i]) > tol)
return false;
}
return true;
}
/* ************************************************************************* */
bool assert_equal(const Vector& expected, const Vector& actual, double tol) {
if (equal_with_abs_tol(expected,actual,tol)) return true;
cout << "not equal:" << endl;
print(expected, "expected");
print(actual, "actual");
return false;
}
/* ************************************************************************* */
bool assert_equal(SubVector expected, SubVector actual, double tol) {
if (equal_with_abs_tol(expected,actual,tol)) return true;
cout << "not equal:" << endl;
print(expected, "expected");
print(actual, "actual");
return false;
}
///* ************************************************************************* */
//bool assert_equal(ConstSubVector expected, ConstSubVector actual, double tol) {
// if (equal_with_abs_tol(Vector(expected),Vector(actual),tol)) return true;
// cout << "not equal:" << endl;
// print(Vector(expected), "expected");
// print(Vector(actual), "actual");
// return false;
//}
/* ************************************************************************* */
bool linear_dependent(const Vector& vec1, const Vector& vec2, double tol) {
if (vec1.size()!=vec2.size()) return false;
boost::optional<double> scale;
size_t m = vec1.size();
for(size_t i=0; i<m; i++) {
if((fabs(vec1[i])>tol&&fabs(vec2[i])<tol) || (fabs(vec1[i])<tol&&fabs(vec2[i])>tol))
return false;
if(vec1[i] == 0 && vec2[i] == 0)
continue;
if (!scale)
scale = vec1[i] / vec2[i];
else if (fabs(vec1[i] - vec2[i] * (*scale)) > tol)
return false;
}
return scale.is_initialized();
}
/* ************************************************************************* */
ConstSubVector sub(const Vector &v, size_t i1, size_t i2) {
return v.segment(i1,i2-i1);
}
/* ************************************************************************* */
void subInsert(Vector& big, const Vector& small, size_t i) {
big.segment(i, small.size()) = small;
}
/* ************************************************************************* */
Vector emul(const Vector &a, const Vector &b) {
assert (b.size()==a.size());
return a.cwiseProduct(b);
}
/* ************************************************************************* */
Vector ediv(const Vector &a, const Vector &b) {
assert (b.size()==a.size());
return a.cwiseQuotient(b);
}
/* ************************************************************************* */
Vector ediv_(const Vector &a, const Vector &b) {
size_t n = a.size();
assert (b.size()==a.size());
Vector c(n);
for( size_t i = 0; i < n; i++ ) {
double ai = a(i), bi = b(i);
c(i) = (bi==0.0 && ai==0.0) ? 0.0 : a(i)/b(i);
}
return c;
}
/* ************************************************************************* */
double sum(const Vector &a) {
return a.sum();
// double result = 0.0;
// size_t n = a.size();
// for( size_t i = 0; i < n; i++ )
// result += a(i);
// return result;
}
/* ************************************************************************* */
double norm_2(const Vector& v) {
return v.norm();
}
/* ************************************************************************* */
Vector reciprocal(const Vector &a) {
size_t n = a.size();
Vector b(n);
for( size_t i = 0; i < n; i++ )
b(i) = 1.0/a(i);
return b;
}
/* ************************************************************************* */
Vector esqrt(const Vector& v) {
return v.cwiseSqrt();
// Vector s(v.size());
// transform(v.begin(), v.end(), s.begin(), ::sqrt);
// return s;
}
/* ************************************************************************* */
Vector abs(const Vector& v) {
return v.cwiseAbs();
}
/* ************************************************************************* */
double max(const Vector &a) {
return a.maxCoeff();
}
/* ************************************************************************* */
double dot(const Vector& a, const Vector& b) {
assert (b.size()==a.size());
return a.dot(b);
}
/* ************************************************************************* */
void scal(double alpha, Vector& x) {
x *= alpha;
}
/* ************************************************************************* */
void axpy(double alpha, const Vector& x, Vector& y) {
assert (y.size()==x.size());
y += alpha * x;
// size_t n = x.size();
// for (size_t i = 0; i < n; i++)
// y[i] += alpha * x[i];
}
/* ************************************************************************* */
void axpy(double alpha, const Vector& x, SubVector y) {
assert (y.size()==x.size());
y += alpha * x;
// size_t n = x.size();
// for (size_t i = 0; i < n; i++)
// y[i] += alpha * x[i];
}
/* ************************************************************************* */
// imperative version, pass in x
double houseInPlace(Vector &v) {
const double x0 = v(0);
const double x02 = x0*x0;
// old code - GSL verison was actually a bit slower
const double sigma = v.squaredNorm() - x02;
v(0) = 1.0;
if( sigma == 0.0 )
return 0.0;
else {
double mu = sqrt(x02 + sigma);
if( x0 <= 0.0 )
v(0) = x0 - mu;
else
v(0) = -sigma / (x0 + mu);
const double v02 = v(0)*v(0);
v = v / v(0);
return 2.0 * v02 / (sigma + v02);
}
}
/* ************************************************************************* */
pair<double, Vector > house(const Vector &x) {
Vector v(x);
double beta = houseInPlace(v);
return make_pair(beta, v);
}
/* ************************************************************************* */
// Fast version *no error checking* !
// Pass in initialized vector of size m or will crash !
double weightedPseudoinverse(const Vector& a, const Vector& weights,
Vector& pseudo) {
size_t m = weights.size();
static const double inf = std::numeric_limits<double>::infinity();
// Check once for zero entries of a. TODO: really needed ?
vector<bool> isZero;
for (size_t i = 0; i < m; ++i) isZero.push_back(fabs(a[i]) < 1e-9);
// If there is a valid (a!=0) constraint (sigma==0) return the first one
for (size_t i = 0; i < m; ++i)
if (weights[i] == inf && !isZero[i]) {
pseudo = delta(m, i, 1 / a[i]);
return inf;
}
/* ************************************************************************* */
Vector ediv(const Vector &a, const Vector &b) {
size_t n = a.size();
assert (b.size()==n);
Vector c(n);
for( size_t i = 0; i < n; i++ )
c(i) = a(i)/b(i);
return c;
}
/* ************************************************************************* */
Vector ediv_(const Vector &a, const Vector &b) {
size_t n = a.size();
assert (b.size()==n);
Vector c(n);
for( size_t i = 0; i < n; i++ ) {
double ai = a(i), bi = b(i);
c(i) = (bi==0.0 && ai==0.0) ? 0.0 : a(i)/b(i);
}
return c;
}
/* ************************************************************************* */
double sum(const Vector &a) {
double result = 0.0;
size_t n = a.size();
for( size_t i = 0; i < n; i++ )
result += a(i);
return result;
}
/* ************************************************************************* */
Vector reciprocal(const Vector &a) {
size_t n = a.size();
Vector b(n);
for( size_t i = 0; i < n; i++ )
b(i) = 1.0/a(i);
return b;
}
/* ************************************************************************* */
Vector esqrt(const Vector& v) {
Vector s(v.size());
transform(v.begin(), v.end(), s.begin(), ::sqrt);
return s;
// Form psuedo-inverse inv(a'inv(Sigma)a)a'inv(Sigma)
// For diagonal Sigma, inv(Sigma) = diag(precisions)
double precision = 0;
for (size_t i = 0; i < m; i++) {
double ai = a[i];
if (!isZero[i]) // also catches remaining sigma==0 rows
precision += weights[i] * (ai * ai);
}
/* ************************************************************************* */
Vector abs(const Vector& v) {
Vector s(v.size());
transform(v.begin(), v.end(), s.begin(), ::fabs);
return s;
// precision = a'inv(Sigma)a
if (precision < 1e-9)
for (size_t i = 0; i < m; i++)
pseudo[i] = 0;
else {
// emul(precisions,a)/precision
double variance = 1.0 / precision;
for (size_t i = 0; i < m; i++)
pseudo[i] = isZero[i] ? 0 : variance * weights[i] * a[i];
}
return precision; // sum of weights
}
/* ************************************************************************* */
// Slow version with error checking
pair<Vector, double>
weightedPseudoinverse(const Vector& a, const Vector& weights) {
int m = weights.size();
if (a.size() != m)
throw invalid_argument("a and weights have different sizes!");
Vector pseudo(m);
double precision = weightedPseudoinverse(a, weights, pseudo);
return make_pair(pseudo, precision);
}
/* ************************************************************************* */
Vector concatVectors(const std::list<Vector>& vs)
{
size_t dim = 0;
BOOST_FOREACH(Vector v, vs)
dim += v.size();
Vector A(dim);
size_t index = 0;
BOOST_FOREACH(Vector v, vs) {
for(int d = 0; d < v.size(); d++)
A(d+index) = v(d);
index += v.size();
}
/* ************************************************************************* */
double max(const Vector &a) {
return *(std::max_element(a.begin(), a.end()));
return A;
}
/* ************************************************************************* */
Vector concatVectors(size_t nrVectors, ...)
{
va_list ap;
list<Vector> vs;
va_start(ap, nrVectors);
for( size_t i = 0 ; i < nrVectors ; i++) {
Vector* V = va_arg(ap, Vector*);
vs.push_back(*V);
}
va_end(ap);
return concatVectors(vs);
}
/* ************************************************************************* */
double dot(const Vector& a, const Vector& b) {
size_t n = a.size();
assert (b.size()==n);
double result = 0.0;
for (size_t i = 0; i < n; i++)
result += a[i] * b[i];
return result;
}
/* ************************************************************************* */
Vector rand_vector_norm(size_t dim, double mean, double sigma)
{
boost::normal_distribution<double> norm_dist(mean, sigma);
boost::variate_generator<boost::minstd_rand&, boost::normal_distribution<double> > norm(generator, norm_dist);
/* ************************************************************************* */
void scal(double alpha, Vector& x) {
size_t n = x.size();
for (size_t i = 0; i < n; i++)
x[i] *= alpha;
}
Vector v(dim);
for(int i = 0; i<v.size(); ++i)
v(i) = norm();
/* ************************************************************************* */
void axpy(double alpha, const Vector& x, Vector& y) {
size_t n = x.size();
assert (y.size()==n);
for (size_t i = 0; i < n; i++)
y[i] += alpha * x[i];
}
return v;
}
/* ************************************************************************* */
void axpy(double alpha, const Vector& x, SubVector y) {
size_t n = x.size();
assert (y.size()==n);
for (size_t i = 0; i < n; i++)
y[i] += alpha * x[i];
}
/* ************************************************************************* */
/* ************************************************************************* */
Vector operator/(double s, const Vector& v) {
Vector result(v.size());
for(size_t i = 0; i < v.size(); i++)
result[i] = s / v[i];
return result;
}
/* ************************************************************************* */
// imperative version, pass in x
double houseInPlace(Vector &v) {
const double x0 = v(0);
const double x02 = x0*x0;
// old code - GSL verison was actually a bit slower
const double sigma = inner_prod(v,v) - x02;
v(0) = 1.0;
if( sigma == 0.0 )
return 0.0;
else {
double mu = sqrt(x02 + sigma);
if( x0 <= 0.0 )
v(0) = x0 - mu;
else
v(0) = -sigma / (x0 + mu);
const double v02 = v(0)*v(0);
v = v / v(0);
return 2.0 * v02 / (sigma + v02);
}
}
/* ************************************************************************* */
pair<double, Vector > house(const Vector &x) {
Vector v(x);
double beta = houseInPlace(v);
return make_pair(beta, v);
}
/* ************************************************************************* */
// Fast version *no error checking* !
// Pass in initialized vector of size m or will crash !
double weightedPseudoinverse(const Vector& a, const Vector& weights,
Vector& pseudo) {
size_t m = weights.size();
static const double inf = std::numeric_limits<double>::infinity();
// Check once for zero entries of a. TODO: really needed ?
vector<bool> isZero;
for (size_t i = 0; i < m; ++i) isZero.push_back(fabs(a[i]) < 1e-9);
// If there is a valid (a!=0) constraint (sigma==0) return the first one
for (size_t i = 0; i < m; ++i)
if (weights[i] == inf && !isZero[i]) {
pseudo = delta(m, i, 1 / a[i]);
return inf;
}
// Form psuedo-inverse inv(a'inv(Sigma)a)a'inv(Sigma)
// For diagonal Sigma, inv(Sigma) = diag(precisions)
double precision = 0;
for (size_t i = 0; i < m; i++) {
double ai = a[i];
if (!isZero[i]) // also catches remaining sigma==0 rows
precision += weights[i] * (ai * ai);
}
// precision = a'inv(Sigma)a
if (precision < 1e-9)
for (size_t i = 0; i < m; i++)
pseudo[i] = 0;
else {
// emul(precisions,a)/precision
double variance = 1.0 / precision;
for (size_t i = 0; i < m; i++)
pseudo[i] = isZero[i] ? 0 : variance * weights[i] * a[i];
}
return precision; // sum of weights
}
/* ************************************************************************* */
// Slow version with error checking
pair<Vector, double>
weightedPseudoinverse(const Vector& a, const Vector& weights) {
size_t m = weights.size();
if (a.size() != m)
throw invalid_argument("a and weights have different sizes!");
Vector pseudo(m);
double precision = weightedPseudoinverse(a, weights, pseudo);
return make_pair(pseudo, precision);
}
/* ************************************************************************* */
Vector concatVectors(const std::list<Vector>& vs)
{
size_t dim = 0;
BOOST_FOREACH(Vector v, vs)
dim += v.size();
Vector A(dim);
size_t index = 0;
BOOST_FOREACH(Vector v, vs) {
for(size_t d = 0; d < v.size(); d++)
A(d+index) = v(d);
index += v.size();
}
return A;
}
/* ************************************************************************* */
Vector concatVectors(size_t nrVectors, ...)
{
va_list ap;
list<Vector> vs;
va_start(ap, nrVectors);
for( size_t i = 0 ; i < nrVectors ; i++) {
Vector* V = va_arg(ap, Vector*);
vs.push_back(*V);
}
va_end(ap);
return concatVectors(vs);
}
/* ************************************************************************* */
Vector rand_vector_norm(size_t dim, double mean, double sigma)
{
boost::normal_distribution<double> norm_dist(mean, sigma);
boost::variate_generator<boost::minstd_rand&, boost::normal_distribution<double> > norm(generator, norm_dist);
Vector v(dim);
Vector::iterator it_v;
for(it_v=v.begin(); it_v!=v.end(); it_v++)
*it_v = norm();
return v;
}
/* ************************************************************************* */
} // namespace gtsam

76
gtsam/base/Vector.h
View File

@ -21,17 +21,22 @@
#pragma once
#include <list>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <vector>
#include <gtsam/3rdparty/Eigen/Core>
#include <boost/random/linear_congruential.hpp>
// Vector is a *global* typedef
// wrap-matlab does this typedef as well
#if ! defined (MEX_H)
typedef boost::numeric::ublas::vector<double> Vector;
#endif
typedef boost::numeric::ublas::vector_range<Vector> SubVector;
typedef boost::numeric::ublas::vector_range<const Vector> ConstSubVector;
// TODO: fix matlab wrapper
//#if ! defined (MEX_H)
//typedef boost::numeric::ublas::vector<double> Vector;
//#endif
// Typedef arbitary length vector
typedef Eigen::VectorXd Vector;
typedef Eigen::VectorBlock<Vector> SubVector;
typedef Eigen::VectorBlock<const Vector> ConstSubVector;
namespace gtsam {
@ -51,6 +56,11 @@ Vector Vector_( size_t m, const double* const data);
*/
Vector Vector_(size_t m, ...);
/**
* Create a numeric vector from an STL vector of doubles
*/
Vector Vector_(const std::vector<double>& data);
/**
* Create vector initialized to a constant value
* @param size
@ -81,13 +91,13 @@ inline Vector basis(size_t n, size_t i) { return delta(n, i, 1.0); }
* Create zero vector
* @param size
*/
inline Vector zero(size_t n) { return repeat(n,0.0);}
inline Vector zero(size_t n) { return Vector::Zero(n);}
/**
* Create vector initialized to ones
* @param size
*/
inline Vector ones(size_t n) { return repeat(n,1.0);}
inline Vector ones(size_t n) { return Vector::Ones(n); }
/**
* check if all zero
@ -125,6 +135,7 @@ bool greaterThanOrEqual(const Vector& v1, const Vector& v2);
* VecA == VecB up to tolerance
*/
bool equal_with_abs_tol(const Vector& vec1, const Vector& vec2, double tol=1e-9);
bool equal_with_abs_tol(const SubVector& vec1, const SubVector& vec2, double tol=1e-9);
/**
* Override of equal in Lie.h
@ -157,7 +168,7 @@ bool assert_equal(const Vector& vec1, const Vector& vec2, double tol=1e-9);
* @return bool
*/
bool assert_equal(SubVector vec1, SubVector vec2, double tol=1e-9);
bool assert_equal(ConstSubVector vec1, ConstSubVector vec2, double tol=1e-9);
//bool assert_equal(ConstSubVector vec1, ConstSubVector vec2, double tol=1e-9);
/**
* check whether two vectors are linearly dependent
@ -175,7 +186,7 @@ bool linear_dependent(const Vector& vec1, const Vector& vec2, double tol=1e-9);
* @param i2 last row index + 1
* @return subvector v(i1:i2)
*/
Vector sub(const Vector &v, size_t i1, size_t i2);
ConstSubVector sub(const Vector &v, size_t i1, size_t i2);
/**
* Inserts a subvector into a vector IN PLACE
@ -216,6 +227,14 @@ Vector ediv_(const Vector &a, const Vector &b);
*/
double sum(const Vector &a);
/**
* Calculates L2 norm for a vector
* modeled after boost.ublas for compatibility
* @param vector
* @return the L2 norm
*/
double norm_2(const Vector& v);
/**
* elementwise reciprocal of vector elements
* @param a vector
@ -247,6 +266,7 @@ double max(const Vector &a);
/** Dot product */
double dot(const Vector &a, const Vector& b);
// TODO: remove simple blas functions - these are one-liners with Eigen
/**
* BLAS Level 1 scal: x <- alpha*x
*/
@ -258,11 +278,6 @@ void scal(double alpha, Vector& x);
void axpy(double alpha, const Vector& x, Vector& y);
void axpy(double alpha, const Vector& x, SubVector y);
/**
* Divide every element of a Vector into a scalar
*/
Vector operator/(double s, const Vector& v);
/**
* house(x,j) computes HouseHolder vector v and scaling factor beta
* from x, such that the corresponding Householder reflection zeroes out
@ -310,5 +325,34 @@ Vector rand_vector_norm(size_t dim, double mean = 0, double sigma = 1);
} // namespace gtsam
// FIXME: make this go away - use the Sampler class instead
static boost::minstd_rand generator(42u);
#include <boost/serialization/nvp.hpp>
#include <boost/serialization/split_free.hpp>
namespace boost {
namespace serialization {
// split version - copies into an STL vector for serialization
template<class Archive>
void save(Archive & ar, const Vector & v, unsigned int version)
{
const size_t n = v.size();
std::vector<double> raw_data(n);
copy(v.data(), v.data()+n, raw_data.begin());
ar << make_nvp("data", raw_data);
}
template<class Archive>
void load(Archive & ar, Vector & v, unsigned int version)
{
std::vector<double> raw_data;
ar >> make_nvp("data", raw_data);
v = gtsam::Vector_(raw_data);
}
} // namespace serialization
} // namespace boost
BOOST_SERIALIZATION_SPLIT_FREE(Vector)

274
gtsam/base/blockMatrices.h
View File

@ -18,60 +18,12 @@
#pragma once
#include <vector>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <gtsam/3rdparty/Eigen/Dense>
#include <gtsam/3rdparty/Eigen/Core>
#include <gtsam/base/Matrix.h>
namespace gtsam {
/** This is a wrapper around ublas::matrix_column that stores a copy of a
* ublas::matrix_range. This does not copy the matrix data itself. The
* purpose of this class is to be allow a column-of-a-range to be returned
* from a function, given that a standard column-of-a-range just stores a
* reference to the range. The stored range stores a reference to the original
* matrix.
*/
template<class MATRIX>
class BlockColumn : public boost::numeric::ublas::vector_expression<BlockColumn<MATRIX> > {
protected:
typedef boost::numeric::ublas::matrix_range<MATRIX> Range;
typedef boost::numeric::ublas::matrix_column<Range> Base;
Range range_;
Base base_;
public:
typedef BlockColumn<MATRIX> Self;
typedef typename Base::matrix_type matrix_type;
typedef typename Base::size_type size_type;
typedef typename Base::difference_type difference_type;
typedef typename Base::value_type value_type;
typedef typename Base::const_reference const_reference;
typedef typename Base::reference reference;
typedef typename Base::storage_category storage_category;
typedef Self closure_type;
typedef const Self const_closure_type;
typedef typename Base::iterator iterator;
typedef typename Base::const_iterator const_iterator;
BlockColumn(const boost::numeric::ublas::matrix_range<MATRIX>& block, size_t column) :
range_(block), base_(range_, column) {}
BlockColumn(const BlockColumn& rhs) :
range_(rhs.range_), base_(rhs.base_) {}
BlockColumn& operator=(const BlockColumn& rhs) { base_.operator=(rhs.base_); return *this; }
template<class AE> BlockColumn& operator=(const boost::numeric::ublas::vector_expression<AE>& rhs) { base_.operator=(rhs); return *this; }
typename Base::size_type size() const { return base_.size(); }
const typename Base::matrix_closure_type& data() const { return base_.data(); }
typename Base::matrix_closure_type& data() { return base_.data(); }
typename Base::const_reference operator()(typename Base::size_type i) const { return base_(i); }
typename Base::reference operator()(typename Base::size_type i) { return base_(i); }
BlockColumn& assign_temporary(BlockColumn& rhs) { base_.assign_temporary(rhs.base_); return *this; }
BlockColumn& assign_temporary(Base& rhs) { base_.assign_temporary(rhs); return *this; }
bool same_closure(const BlockColumn& rhs) { return base_.same_closure(rhs.base_); }
bool same_closure(const Base& rhs) { return base_.same_closure(rhs); }
template<class AE> BlockColumn& assign(const boost::numeric::ublas::vector_expression<AE>& rhs) { base_.assign(rhs); return *this; }
iterator begin() { return base_.begin(); }
const_iterator begin() const { return base_.begin(); }
iterator end() { return base_.end(); }
const_iterator end() const { return base_.end(); }
};
template<class MATRIX> class SymmetricBlockView;
/**
@ -95,10 +47,12 @@ template<class MATRIX>
class VerticalBlockView {
public:
typedef MATRIX FullMatrix;
typedef typename boost::numeric::ublas::matrix_range<MATRIX> Block;
typedef typename boost::numeric::ublas::matrix_range<const MATRIX> constBlock;
typedef BlockColumn<MATRIX> Column;
typedef BlockColumn<const MATRIX> constColumn;
typedef Eigen::Block<MATRIX> Block;
typedef Eigen::Block<const MATRIX> constBlock;
// columns of blocks
typedef Eigen::VectorBlock<typename MATRIX::ColXpr> Column;
typedef Eigen::VectorBlock<const typename MATRIX::ConstColXpr> constColumn;
protected:
FullMatrix& matrix_; // the reference to the full matrix
@ -112,18 +66,19 @@ protected:
public:
/** Construct from an empty matrix (asserts that the matrix is empty) */
VerticalBlockView(FullMatrix& matrix) :
matrix_(matrix), rowStart_(0), rowEnd_(matrix_.size1()), blockStart_(0) {
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. */
* another block view.
*/
template<class RHS>
VerticalBlockView(FullMatrix& matrix, const RHS& rhs) :
matrix_(matrix) {
if(matrix_.size1() != rhs.size1() || matrix_.size2() != rhs.size2())
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"
@ -136,12 +91,13 @@ public:
/** Construct from iterators over the sizes of each vertical block */
template<typename ITERATOR>
VerticalBlockView(FullMatrix& matrix, ITERATOR firstBlockDim, ITERATOR lastBlockDim) :
matrix_(matrix), rowStart_(0), rowEnd_(matrix_.size1()), blockStart_(0) {
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
/**
* 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.
*/
@ -149,17 +105,17 @@ public:
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(), false);
matrix_.resize(matrixNewHeight, variableColOffsets_.back());
assertInvariants();
}
/** Row size
*/
size_t size1() const { assertInvariants(); return rowEnd_ - rowStart_; }
size_t rows() const { assertInvariants(); return rowEnd_ - rowStart_; }
/** Column size
*/
size_t size2() const { assertInvariants(); return variableColOffsets_.back() - variableColOffsets_[blockStart_]; }
size_t cols() const { assertInvariants(); return variableColOffsets_.back() - variableColOffsets_[blockStart_]; }
/** Block count
@ -167,60 +123,62 @@ public:
size_t nBlocks() const { assertInvariants(); return variableColOffsets_.size() - 1 - blockStart_; }
Block operator()(size_t block) {
/** Access a single block in the underlying matrix with read/write access */
inline Block operator()(size_t block) {
return range(block, block+1);
}
constBlock operator()(size_t block) const {
/** Access a const block view */
inline const constBlock operator()(size_t block) const {
return range(block, block+1);
}
Block range(size_t startBlock, size_t endBlock) {
/** 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_;
checkBlock(actualStartBlock);
assert(actualEndBlock < variableColOffsets_.size());
return Block(matrix_,
boost::numeric::ublas::range(rowStart_, rowEnd_),
boost::numeric::ublas::range(variableColOffsets_[actualStartBlock], variableColOffsets_[actualEndBlock]));
const size_t& startCol = variableColOffsets_[actualStartBlock];
const size_t& endCol = variableColOffsets_[actualEndBlock];
return matrix_.block(rowStart_, startCol, rowEnd_-rowStart_, endCol-startCol);
}
constBlock range(size_t startBlock, size_t endBlock) const {
inline const constBlock range(size_t startBlock, size_t endBlock) const {
assertInvariants();
size_t actualStartBlock = startBlock + blockStart_;
size_t actualEndBlock = endBlock + blockStart_;
checkBlock(actualStartBlock);
assert(actualEndBlock < variableColOffsets_.size());
return constBlock(matrix_,
boost::numeric::ublas::range(rowStart_, rowEnd_),
boost::numeric::ublas::range(variableColOffsets_[actualStartBlock], variableColOffsets_[actualEndBlock]));
const size_t& startCol = variableColOffsets_[actualStartBlock];
const size_t& endCol = variableColOffsets_[actualEndBlock];
return ((const FullMatrix&)matrix_).block(rowStart_, startCol, rowEnd_-rowStart_, endCol-startCol);
}
Block full() {
inline Block full() {
return range(0,nBlocks());
}
constBlock full() const {
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]);
Block blockMat(operator()(block));
return Column(blockMat, columnOffset);
return matrix_.col(variableColOffsets_[actualBlock] + columnOffset).segment(rowStart_, rowEnd_-rowStart_);
}
constColumn column(size_t block, size_t columnOffset) const {
const constColumn column(size_t block, size_t columnOffset) const {
assertInvariants();
size_t actualBlock = block + blockStart_;
checkBlock(actualBlock);
assert(variableColOffsets_[actualBlock] + columnOffset < matrix_.size2());
constBlock blockMat(operator()(block));
return constColumn(blockMat, columnOffset);
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 {
@ -237,17 +195,21 @@ public:
size_t rowEnd() const { return rowEnd_; }
size_t firstBlock() const { return blockStart_; }
/** Copy the block structure and resize the underlying matrix, but do not
/** access to full matrix */
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 size2() of the source VerticalBlockView, and
* 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<class RHS>
void copyStructureFrom(const RHS& rhs) {
if(matrix_.size1() != rhs.size1() || matrix_.size2() != rhs.size2())
matrix_.resize(rhs.size1(), rhs.size2(), false);
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 {
@ -261,7 +223,7 @@ public:
}
}
rowStart_ = 0;
rowEnd_ = matrix_.size1();
rowEnd_ = matrix_.rows();
blockStart_ = 0;
assertInvariants();
}
@ -275,13 +237,14 @@ public:
* 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 size2() of the source VerticalBlockView, and the underlying matrix
* will be cols() of the source VerticalBlockView, and the underlying matrix
* will be the size of the view of the source matrix.
*/
template<class RHS>
VerticalBlockView<MATRIX>& assignNoalias(const RHS& rhs) {
copyStructureFrom(rhs);
boost::numeric::ublas::noalias(matrix_) = rhs.full();
// boost::numeric::ublas::noalias(matrix_) = rhs.full();
matrix_ = rhs.full(); // FIXME: check if noalias is necessary here
return *this;
}
@ -300,17 +263,17 @@ public:
protected:
void assertInvariants() const {
assert(matrix_.size2() == variableColOffsets_.back());
assert((size_t) matrix_.cols() == variableColOffsets_.back());
assert(blockStart_ < variableColOffsets_.size());
assert(rowStart_ <= matrix_.size1());
assert(rowEnd_ <= matrix_.size1());
assert(rowStart_ <= (size_t) matrix_.rows());
assert(rowEnd_ <= (size_t) matrix_.rows());
assert(rowStart_ <= rowEnd_);
}
void checkBlock(size_t block) const {
assert(matrix_.size2() == variableColOffsets_.back());
assert((size_t) matrix_.cols() == variableColOffsets_.back());
assert(block < variableColOffsets_.size()-1);
assert(variableColOffsets_[block] < matrix_.size2() && variableColOffsets_[block+1] <= matrix_.size2());
assert(variableColOffsets_[block] < (size_t) matrix_.cols() && variableColOffsets_[block+1] <= (size_t) matrix_.cols());
}
template<typename ITERATOR>
@ -328,7 +291,6 @@ protected:
template<class RELATED> friend class VerticalBlockView;
};
/**
* 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
@ -348,12 +310,14 @@ template<class MATRIX>
class SymmetricBlockView {
public:
typedef MATRIX FullMatrix;
typedef typename boost::numeric::ublas::matrix_range<MATRIX> Block;
typedef typename boost::numeric::ublas::matrix_range<const MATRIX> constBlock;
typedef BlockColumn<MATRIX> Column;
typedef BlockColumn<const MATRIX> constColumn;
typedef Eigen::Block<MATRIX> Block;
typedef Eigen::Block<const MATRIX> constBlock;
// typedef typename MATRIX::ColXpr Column;
// typedef typename MATRIX::ConstColXpr constColumn;
protected:
static FullMatrix matrixTemp_; // the reference to the full matrix
FullMatrix& matrix_; // the reference to the full matrix
std::vector<size_t> variableColOffsets_; // the starting columns of each block (0-based)
@ -385,16 +349,19 @@ public:
void resize(ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool preserve) {
blockStart_ = 0;
fillOffsets(firstBlockDim, lastBlockDim);
matrix_.resize(variableColOffsets_.back(), variableColOffsets_.back(), preserve);
if (preserve)
matrix_.conservativeResize(variableColOffsets_.back(), variableColOffsets_.back());
else
matrix_.resize(variableColOffsets_.back(), variableColOffsets_.back());
}
/** Row size
*/
size_t size1() const { assertInvariants(); return variableColOffsets_.back() - variableColOffsets_[blockStart_]; }
size_t rows() const { assertInvariants(); return variableColOffsets_.back() - variableColOffsets_[blockStart_]; }
/** Column size
*/
size_t size2() const { return size1(); }
size_t cols() const { return rows(); }
/** Block count
@ -420,9 +387,10 @@ public:
checkBlock(j_actualStartBlock);
assert(i_actualEndBlock < variableColOffsets_.size());
assert(j_actualEndBlock < variableColOffsets_.size());
return Block(matrix_,
boost::numeric::ublas::range(variableColOffsets_[i_actualStartBlock], variableColOffsets_[i_actualEndBlock]),
boost::numeric::ublas::range(variableColOffsets_[j_actualStartBlock], variableColOffsets_[j_actualEndBlock]));
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 {
@ -435,9 +403,10 @@ public:
checkBlock(j_actualStartBlock);
assert(i_actualEndBlock < variableColOffsets_.size());
assert(j_actualEndBlock < variableColOffsets_.size());
return constBlock(matrix_,
boost::numeric::ublas::range(variableColOffsets_[i_actualStartBlock], variableColOffsets_[i_actualEndBlock]),
boost::numeric::ublas::range(variableColOffsets_[j_actualStartBlock], variableColOffsets_[j_actualEndBlock]));
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() {
@ -448,12 +417,32 @@ public:
return range(0,nBlocks(), 0,nBlocks());
}
/** access to full matrix */
const FullMatrix& fullMatrix() const { return matrix_; }
// typedef typename MATRIX::ColXpr Column;
typedef typeof(matrixTemp_.col(0).segment(0, 1)) Column;
typedef typeof(((const FullMatrix&)matrixTemp_).col(0).segment(0, 1)) constColumn;
Column column(size_t i_block, size_t j_block, size_t columnOffset) {
assertInvariants();
size_t j_actualBlock = j_block + blockStart_;
assert(variableColOffsets_[j_actualBlock] + columnOffset < variableColOffsets_[j_actualBlock+1]);
Block blockMat(operator()(i_block, j_block));
return Column(blockMat, columnOffset);
assertInvariants();
// size_t j_actualBlock = j_block + blockStart_;
// assert(variableColOffsets_[j_actualBlock] + columnOffset < variableColOffsets_[j_actualBlock+1]);
// Block blockMat(operator()(i_block, j_block));
// return blockMat.col(columnOffset);
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 {
@ -466,18 +455,48 @@ public:
Column rangeColumn(size_t i_startBlock, size_t i_endBlock, size_t j_block, size_t columnOffset) {
assertInvariants();
size_t j_actualBlock = j_block + blockStart_;
assert(variableColOffsets_[j_actualBlock] + columnOffset < variableColOffsets_[j_actualBlock+1]);
Block blockMat(this->range(i_startBlock, i_endBlock, j_block));
return Column(blockMat, columnOffset);
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]);
// column of a block approach
// size_t j_actualBlock = j_block + blockStart_;
// assert(variableColOffsets_[j_actualBlock] + columnOffset < variableColOffsets_[j_actualBlock+1]);
// Block blockMat = this->range(i_startBlock, i_endBlock, j_block, j_block+1);
// return Column(blockMat, columnOffset);
}
constColumn rangeColumn(size_t i_startBlock, size_t i_endBlock, size_t j_block, size_t columnOffset) const {
assertInvariants();
size_t j_actualBlock = j_block + blockStart_;
assert(variableColOffsets_[j_actualBlock] + columnOffset < variableColOffsets_[j_actualBlock+1]);
constBlock blockMat(this->range(i_startBlock, i_endBlock, j_block, j_block+1));
return constColumn(blockMat, columnOffset);
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]);
// column of a block approach
// size_t j_actualBlock = j_block + blockStart_;
// assert(variableColOffsets_[j_actualBlock] + columnOffset < variableColOffsets_[j_actualBlock+1]);
// constBlock blockMat = this->range(i_startBlock, i_endBlock, j_block, j_block+1);
// return constColumn(blockMat, columnOffset);
}
size_t offset(size_t block) const {
@ -498,7 +517,7 @@ public:
*/
template<class RHS>
void copyStructureFrom(const RHS& rhs) {
matrix_.resize(rhs.size2(), rhs.size2(), false);
matrix_.resize(rhs.cols(), rhs.cols());
if(rhs.blockStart_ == 0)
variableColOffsets_ = rhs.variableColOffsets_;
else {
@ -529,7 +548,8 @@ public:
template<class RHSMATRIX>
SymmetricBlockView<MATRIX>& assignNoalias(const SymmetricBlockView<RHSMATRIX>& rhs) {
copyStructureFrom(rhs);
boost::numeric::ublas::noalias(matrix_) = rhs.range(0, rhs.nBlocks(), 0, rhs.nBlocks());
// boost::numeric::ublas::noalias(matrix_) = rhs.range(0, rhs.nBlocks(), 0, rhs.nBlocks());
matrix_ = rhs.matrix_.block(0, 0, rhs.nBlocks(), rhs.nBlocks());
return *this;
}
@ -546,16 +566,16 @@ public:
protected:
void assertInvariants() const {
assert(matrix_.size1() == matrix_.size2());
assert(matrix_.size2() == variableColOffsets_.back());
assert(matrix_.rows() == matrix_.cols());
assert((size_t) matrix_.cols() == variableColOffsets_.back());
assert(blockStart_ < variableColOffsets_.size());
}
void checkBlock(size_t block) const {
assert(matrix_.size1() == matrix_.size2());
assert(matrix_.size2() == variableColOffsets_.back());
assert(matrix_.rows() == matrix_.cols());
assert((size_t) matrix_.cols() == variableColOffsets_.back());
assert(block < variableColOffsets_.size()-1);
assert(variableColOffsets_[block] < matrix_.size2() && variableColOffsets_[block+1] <= matrix_.size2());
assert(variableColOffsets_[block] < (size_t) matrix_.cols() && variableColOffsets_[block+1] <= (size_t) matrix_.cols());
}
template<typename ITERATOR>

111
gtsam/base/cholesky.cpp
View File

@ -18,18 +18,14 @@
#include <gtsam/base/debug.h>
#include <gtsam/base/cholesky.h>
#include <gtsam/base/lapack.h>
#include <gtsam/base/timing.h>
#include <gtsam/3rdparty/Eigen/Core>
#include <gtsam/3rdparty/Eigen/Dense>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/symmetric.hpp>
#include <boost/numeric/ublas/blas.hpp>
#include <boost/format.hpp>
namespace ublas = boost::numeric::ublas;
//namespace ublas = boost::numeric::ublas;
using namespace std;
@ -39,90 +35,6 @@ namespace gtsam {
static const double zeroPivotThreshold = 1e-6;
static const double underconstrainedPrior = 1e-5;
/* ************************************************************************* */
void cholesky_inplace(MatrixColMajor& I) {
// We do not check for symmetry but we do check for squareness
assert(I.size1() == I.size2());
// Do Cholesky, return value info as follows (from dpotrf.f):
// 00054 * INFO (output) INTEGER
// 00055 * = 0: successful exit
// 00056 * < 0: if INFO = -i, the i-th argument had an illegal value
// 00057 * > 0: if INFO = i, the leading minor of order i is not
// 00058 * positive definite, and the factorization could not be
// 00059 * completed.
int info = lapack_dpotrf('U', I.size1(), &I(0,0), I.size1());
if(info != 0) {
if(info < 0)
throw std::domain_error(boost::str(boost::format(
"Bad input to cholesky_inplace, dpotrf returned %d.\n")%info));
else
throw std::domain_error("The matrix passed into cholesky_inplace is rank-deficient");
}
}
/* ************************************************************************* */
size_t choleskyFactorUnderdetermined(MatrixColMajor& Ab, size_t nFrontal) {
static const bool debug = false;
size_t m = Ab.size1();
size_t n = Ab.size2();
// If m >= n, this function will just compute the plain Cholesky
// factorization of A'A. If m < n, A'A is rank-deficient this function
// instead computes the upper-trapazoidal factor [ R S ], as described in the
// header file comment.
// size_t rank = std::min(m,n-1);
size_t rank = nFrontal;
if(rank > 0) {
// F is the first 'rank' columns of Ab, G is the remaining columns
ublas::matrix_range<MatrixColMajor> F(ublas::project(Ab, ublas::range(0,m), ublas::range(0,rank)));
ublas::matrix_range<MatrixColMajor> G(ublas::project(Ab, ublas::range(0,m), ublas::range(rank,n)));
if(debug) {
print(F, "F: ");
print(G, "G: ");
}
ublas::matrix_range<MatrixColMajor> R(ublas::project(Ab, ublas::range(0,rank), ublas::range(0,rank)));
ublas::matrix_range<MatrixColMajor> S(ublas::project(Ab, ublas::range(0,rank), ublas::range(rank,n)));
// First compute F' * G (ublas makes a copy here to avoid aliasing)
if(S.size2() > 0)
S = ublas::prod(ublas::trans(F), G);
// ublas makes a copy to avoid aliasing on this assignment
R = ublas::prod(ublas::trans(F), F);
// Compute the values of R from F'F
int info = lapack_dpotrf('U', rank, &R(0,0), Ab.size1());
if(info != 0) {
if(info < 0)
throw std::domain_error(boost::str(boost::format(
"Bad input to choleskyFactorUnderdetermined, dpotrf returned %d.\n")%info));
else
throw std::domain_error(boost::str(boost::format(
"The matrix passed into choleskyFactorUnderdetermined is numerically rank-deficient, dpotrf returned rank=%d, expected rank was %d.\n")%(info-1)%rank));
}
// Compute S = inv(R') * F' * G, i.e. solve S when R'S = F'G
if(S.size2() > 0)
cblas_dtrsm(CblasColMajor, CblasLeft, CblasUpper, CblasTrans, CblasNonUnit, S.size1(), S.size2(), 1.0, &R(0,0), m, &S(0,0), m);
if(debug) {
print(R, "R: ");
print(S, "S: ");
}
return m;
} else
return 0;
}
/* ************************************************************************* */
static inline bool choleskyStep(MatrixColMajor& ATA, size_t k, size_t order) {
@ -146,15 +58,18 @@ static inline bool choleskyStep(MatrixColMajor& ATA, size_t k, size_t order) {
ATA(k,k) = beta;
if(k < (order-1)) {
ublas::matrix_row<MatrixColMajor> Vf(ublas::row(ATA, k));
ublas::vector_range<typeof(Vf)> V(ublas::subrange(Vf, k+1,order));
// ublas::matrix_row<MatrixColMajor> Vf(ublas::row(ATA, k));
// ublas::vector_range<typeof(Vf)> V(ublas::subrange(Vf, k+1,order));
// Update A(k,k+1:end) <- A(k,k+1:end) / beta
V *= betainv;
typedef typeof(ATA.row(k).segment(k+1, order-(k+1))) BlockRow;
BlockRow V = ATA.row(k).segment(k+1, order-(k+1));
V *= betainv;
// Update A(k+1:end, k+1:end) <- A(k+1:end, k+1:end) - v*v' / alpha
ublas::matrix_range<MatrixColMajor> L(ublas::subrange(ATA, k+1,order, k+1,order));
L -= ublas::outer_prod(V, V);
// ublas::matrix_range<MatrixColMajor> L(ublas::subrange(ATA, k+1,order, k+1,order));
// L -= ublas::outer_prod(V, V);
ATA.block(k+1, k+1, order-(k+1), order-(k+1)) -= V.transpose() * V;
}
return true;
@ -173,10 +88,10 @@ pair<size_t,bool> choleskyCareful(MatrixColMajor& ATA, int order) {
const bool debug = ISDEBUG("choleskyCareful");
// Check that the matrix is square (we do not check for symmetry)
assert(ATA.size1() == ATA.size2());
assert(ATA.rows() == ATA.cols());
// Number of rows/columns
const size_t n = ATA.size1();
const size_t n = ATA.rows();
// Negative order means factor the entire matrix
if(order < 0)
@ -204,11 +119,11 @@ pair<size_t,bool> choleskyCareful(MatrixColMajor& ATA, int order) {
}
/* ************************************************************************* */
void choleskyPartial(MatrixColMajor& ABCublas, size_t nFrontal) {
void choleskyPartial(MatrixColMajor& ABC, size_t nFrontal) {
const bool debug = ISDEBUG("choleskyPartial");
Eigen::Map<Eigen::MatrixXd> ABC(&ABCublas(0,0), ABCublas.size1(), ABCublas.size2());
// Eigen::Map<Eigen::MatrixXd> ABC(&ABCublas(0,0), ABCublas.rows(), ABCublas.cols());
assert(ABC.rows() == ABC.cols());
assert(ABC.rows() >= 0 && nFrontal <= size_t(ABC.rows()));

23
gtsam/base/cholesky.h
View File

@ -21,29 +21,6 @@
namespace gtsam {
/** Plain Cholesky on a symmetric positive semi-definite matrix, in place. */
void cholesky_inplace(MatrixColMajor& I);
/**
* Factor an underdetermined Gaussian into a Gaussian conditional. This means
* for a Gaussian exp(-1/2 * ||Ax - b||^2), with a "wide" A, i.e. m < n, to
* find an upper-triangular m x m R, rectangular m x (n-m) S, and m-vector d,
* such that ||Ax - b||^2 == || [R S]x - d ||^2.
*
* The matrices [ R S ] and [ R S d ] are each upper-trapazoidal.
*
* This returns the same upper-trapazoidal factor as QR, but uses Cholesky for
* efficiency. Given a matrix [ F G b ], with F square, this first computes
* the upper-triangular R = chol(F), i.e. R'R == F'F. It then computes the
* upper-trapazoidal factor [ R S d ], with [ S d ] = inv(R') * F' * [ G b ].
*
* Note that this operates on the Jacobian A matrix, not the symmetric
* information matrix like plain Cholesky.
*
* This function returns the rank of the factor.
*/
size_t choleskyFactorUnderdetermined(MatrixColMajor& Ab, size_t nFrontal);
/**
* "Careful" Cholesky computes the positive square-root of a positive symmetric
* semi-definite matrix (i.e. that may be rank-deficient). Unlike standard

45
gtsam/base/lapack.h
View File

@ -1,45 +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 lapack.h
* @brief Handles wrapping of LAPACK functions that we use
* @author Richard Roberts
* @created Nov 6, 2010
*/
#pragma once
// Prototypes of LAPACK functions that we use
extern "C" {
#include <cblas.h>
/* FORTRAN subroutine */
int dpotrf_(char *uplo, int *n, double *a, int *lda, int *info);
/* C wrapper */
inline int lapack_dpotrf(char uplo, int n, double* a, int lda) {
int info;
dpotrf_(&uplo, &n, a, &lda, &info);
return info;
}
int dpotf2_(char *uplo, int *n, double *a, int *lda, int *info);
inline int lapack_dpotf2(char uplo, int n, double* a, int lda) {
int info;
dpotf2_(&uplo, &n, a, &lda, &info);
return info;
}
}

14
gtsam/base/numericalDerivative.h
View File

@ -66,7 +66,7 @@ namespace gtsam {
Vector numericalGradient(boost::function<double(const X&)> h, const X& x, double delta=1e-5) {
double factor = 1.0/(2.0*delta);
const size_t n = x.dim();
Vector d(n,0.0), g(n,0.0);
Vector d = zero(n), g = zero(n);
for (size_t j=0;j<n;j++) {
d(j) += delta; double hxplus = h(x.expmap(d));
d(j) -= 2*delta; double hxmin = h(x.expmap(d));
@ -95,7 +95,7 @@ namespace gtsam {
Y hx = h(x);
double factor = 1.0/(2.0*delta);
const size_t m = hx.dim(), n = x.dim();
Vector d(n,0.0);
Vector d = zero(n);
Matrix H = zeros(m,n);
for (size_t j=0;j<n;j++) {
d(j) += delta; Vector hxplus = hx.logmap(h(x.expmap(d)));
@ -149,7 +149,7 @@ namespace gtsam {
Y hx = h(x1,x2);
double factor = 1.0/(2.0*delta);
const size_t m = hx.dim(), n = x1.dim();
Vector d(n,0.0);
Vector d = zero(n);
Matrix H = zeros(m,n);
for (size_t j=0;j<n;j++) {
d(j) += delta; Vector hxplus = hx.logmap(h(x1.expmap(d),x2));
@ -213,7 +213,7 @@ namespace gtsam {
Y hx = h(x1,x2);
double factor = 1.0/(2.0*delta);
const size_t m = hx.dim(), n = x2.dim();
Vector d(n,0.0);
Vector d = zero(n);
Matrix H = zeros(m,n);
for (size_t j=0;j<n;j++) {
d(j) += delta; Vector hxplus = hx.logmap(h(x1,x2.expmap(d)));
@ -279,7 +279,7 @@ namespace gtsam {
Y hx = h(x1,x2,x3);
double factor = 1.0/(2.0*delta);
const size_t m = hx.dim(), n = x1.dim();
Vector d(n,0.0);
Vector d = zero(n);
Matrix H = zeros(m,n);
for (size_t j=0;j<n;j++) {
d(j) += delta; Vector hxplus = hx.logmap(h(x1.expmap(d),x2,x3));
@ -344,7 +344,7 @@ namespace gtsam {
Y hx = h(x1,x2,x3);
double factor = 1.0/(2.0*delta);
const size_t m = hx.dim(), n = x2.dim();
Vector d(n,0.0);
Vector d = zero(n);
Matrix H = zeros(m,n);
for (size_t j=0;j<n;j++) {
d(j) += delta; Vector hxplus = hx.logmap(h(x1, x2.expmap(d),x3));
@ -409,7 +409,7 @@ namespace gtsam {
Y hx = h(x1,x2,x3);
double factor = 1.0/(2.0*delta);
const size_t m = hx.dim(), n = x3.dim();
Vector d(n,0.0);
Vector d = zero(n);
Matrix H = zeros(m,n);
for (size_t j=0;j<n;j++) {
d(j) += delta; Vector hxplus = hx.logmap(h(x1, x2, x3.expmap(d)));

122
gtsam/base/tests/testBlockMatrices.cpp Normal file
View File

@ -0,0 +1,122 @@
/**
* @file testBlockMatrices
* @author Alex Cunningham
*/
#include <iostream>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/blockMatrices.h>
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
TEST(testBlockMatrices, jacobian_factor1) {
typedef MatrixColMajor AbMatrix;
typedef VerticalBlockView<AbMatrix> 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 MatrixColMajor AbMatrix;
typedef VerticalBlockView<AbMatrix> 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 MatrixColMajor InfoMatrix;
typedef SymmetricBlockView<InfoMatrix> 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); }
/* ************************************************************************* */

110
gtsam/base/tests/testCholesky.cpp
View File

@ -20,54 +20,55 @@
#include <gtsam/base/cholesky.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/symmetric.hpp>
//#include <boost/numeric/ublas/triangular.hpp>
//#include <boost/numeric/ublas/matrix_proxy.hpp>
//#include <boost/numeric/ublas/symmetric.hpp>
using namespace gtsam;
namespace ublas = boost::numeric::ublas;
//namespace ublas = boost::numeric::ublas;
/* ************************************************************************* */
TEST(cholesky, cholesky_inplace) {
Matrix I = Matrix_(5,5,
1.739214152118470, 0.714164147286383, 1.893437990040579, 1.469485110232169, 1.549910412077052,
0.714164147286383, 0.587767128354794, 0.738548568260523, 0.588775778746414, 0.645015863153235,
1.893437990040579, 0.738548568260523, 2.165530165155413, 1.631203698494913, 1.617901992621506,
1.469485110232169, 0.588775778746414, 1.631203698494913, 1.591363675348797, 1.584779118350080,
1.549910412077052, 0.645015863153235, 1.617901992621506, 1.584779118350080, 1.928887183904716);
///* ************************************************************************* */
//TEST(cholesky, cholesky_inplace) {
// Matrix I = Matrix_(5,5,
// 1.739214152118470, 0.714164147286383, 1.893437990040579, 1.469485110232169, 1.549910412077052,
// 0.714164147286383, 0.587767128354794, 0.738548568260523, 0.588775778746414, 0.645015863153235,
// 1.893437990040579, 0.738548568260523, 2.165530165155413, 1.631203698494913, 1.617901992621506,
// 1.469485110232169, 0.588775778746414, 1.631203698494913, 1.591363675348797, 1.584779118350080,
// 1.549910412077052, 0.645015863153235, 1.617901992621506, 1.584779118350080, 1.928887183904716);
//
// Matrix expected = Matrix_(5,5,
// 1.318792687316119, 0.541528743793524, 1.435735888021887, 1.114265437142152, 1.175249473995251,
// 0.0, 0.542691208699940, -0.071760299365346, -0.026960052875681, 0.015818372803848,
// 0.0, 0.0, 0.314711112667495, 0.093667363355578, -0.217058504307151,
// 0.0, 0.0, 0.0, 0.583331630832890, 0.507424929100112,
// 0.0, 0.0, 0.0, 0.0, 0.492779041656733);
//
// MatrixColMajor actualColMaj = I;
// cholesky_inplace(actualColMaj);
// Matrix actual = ublas::triangular_adaptor<MatrixColMajor, ublas::upper>(actualColMaj);
// CHECK(assert_equal(expected, actual, 1e-7));
//}
Matrix expected = Matrix_(5,5,
1.318792687316119, 0.541528743793524, 1.435735888021887, 1.114265437142152, 1.175249473995251,
0.0, 0.542691208699940, -0.071760299365346, -0.026960052875681, 0.015818372803848,
0.0, 0.0, 0.314711112667495, 0.093667363355578, -0.217058504307151,
0.0, 0.0, 0.0, 0.583331630832890, 0.507424929100112,
0.0, 0.0, 0.0, 0.0, 0.492779041656733);
MatrixColMajor actualColMaj = I;
cholesky_inplace(actualColMaj);
Matrix actual = ublas::triangular_adaptor<MatrixColMajor, ublas::upper>(actualColMaj);
CHECK(assert_equal(expected, actual, 1e-7));
}
/* ************************************************************************* */
TEST(cholesky, choleskyFactorUnderdetermined) {
Matrix Ab = Matrix_(3,7,
0.0357, 0.6787, 0.3922, 0.7060, 0.0462, 0.6948, 0.0344,
0.8491, 0.7577, 0.6555, 0.0318, 0.0971, 0.3171, 0.4387,
0.9340, 0.7431, 0.1712, 0.2769, 0.8235, 0.9502, 0.3816);
Matrix expected = Matrix_(3,7,
1.2628, 1.0784, 0.5785, 0.2462, 0.6757, 0.9357, 0.5782,
0.0, 0.6513, 0.4089, 0.6811, -0.0180, 0.6280, 0.0244,
0.0, 0.0, 0.3332, -0.2273, -0.4825, -0.4652, 0.0660);
MatrixColMajor actualColmaj = Ab;
choleskyFactorUnderdetermined(actualColmaj, 3);
Matrix actual = ublas::triangular_adaptor<MatrixColMajor, ublas::upper>(actualColmaj);
CHECK(assert_equal(expected, actual, 1e-3));
}
///* ************************************************************************* */
//TEST(cholesky, choleskyFactorUnderdetermined) {
//
// Matrix Ab = Matrix_(3,7,
// 0.0357, 0.6787, 0.3922, 0.7060, 0.0462, 0.6948, 0.0344,
// 0.8491, 0.7577, 0.6555, 0.0318, 0.0971, 0.3171, 0.4387,
// 0.9340, 0.7431, 0.1712, 0.2769, 0.8235, 0.9502, 0.3816);
//
// Matrix expected = Matrix_(3,7,
// 1.2628, 1.0784, 0.5785, 0.2462, 0.6757, 0.9357, 0.5782,
// 0.0, 0.6513, 0.4089, 0.6811, -0.0180, 0.6280, 0.0244,
// 0.0, 0.0, 0.3332, -0.2273, -0.4825, -0.4652, 0.0660);
//
// MatrixColMajor actualColmaj = Ab;
// choleskyFactorUnderdetermined(actualColmaj, 3);
//
// Matrix actual = ublas::triangular_adaptor<MatrixColMajor, ublas::upper>(actualColmaj);
//
// CHECK(assert_equal(expected, actual, 1e-3));
//}
/* ************************************************************************* */
TEST(cholesky, choleskyPartial) {
@ -88,15 +89,20 @@ TEST(cholesky, choleskyPartial) {
choleskyPartial(RSL, 3);
// See the function comment for choleskyPartial, this decomposition should hold.
Matrix R1(ublas::trans(RSL));
Matrix R2(RSL);
ublas::project(R1, ublas::range(3,7), ublas::range(3,7)) = eye(4,4);
ublas::project(R2, ublas::range(3,7), ublas::range(3,7)) =
ublas::symmetric_adaptor<const ublas::matrix_range<Matrix>,ublas::upper>(
ublas::project(R2, ublas::range(3,7), ublas::range(3,7)));
Matrix actual(R1 * R2);
MatrixColMajor R1 = RSL.transpose();
MatrixColMajor R2 = RSL;
// ublas::project(R1, ublas::range(3,7), ublas::range(3,7)) = eye(4,4);
R1.block(3, 3, 4, 4).setIdentity();
EXPECT(assert_equal(ublas::symmetric_adaptor<Matrix,ublas::upper>(ABC), actual, 1e-9));
// ublas::project(R2, ublas::range(3,7), ublas::range(3,7)) =
// ublas::symmetric_adaptor<const ublas::matrix_range<Matrix>,ublas::upper>(
// ublas::project(R2, ublas::range(3,7), ublas::range(3,7)));
R2.block(3, 3, 4, 4) = R2.block(3, 3, 4, 4).selfadjointView<Eigen::Upper>();
MatrixColMajor actual = R1 * R2;
MatrixColMajor expected = ABC.selfadjointView<Eigen::Upper>();
EXPECT(assert_equal(expected, actual, 1e-9));
}
/* ************************************************************************* */

437
gtsam/base/tests/testMatrix.cpp
View File

@ -20,8 +20,6 @@
#include <CppUnitLite/TestHarness.h>
#include <boost/tuple/tuple.hpp>
#include <boost/foreach.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <gtsam/base/Matrix.h>
using namespace std;
@ -50,7 +48,7 @@ TEST( matrix, constructor_vector )
double data[] = { -5, 3, 0, -5 };
Matrix A = Matrix_(2, 2, data);
Vector v(4);
copy(data, data + 4, v.begin());
copy(data, data + 4, v.data());
Matrix B = Matrix_(2, 2, v); // this one is column order !
EQUALITY(A,trans(B));
}
@ -74,10 +72,10 @@ TEST( matrix, row_major )
{
Matrix A = Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0);
const double * const a = &A(0, 0);
CHECK(a[0] == 1);
CHECK(a[1] == 2);
CHECK(a[2] == 3);
CHECK(a[3] == 4);
EXPECT(a[0] == 1);
EXPECT(a[1] == 2);
EXPECT(a[2] == 3);
EXPECT(a[3] == 4);
}
/* ************************************************************************* */
@ -95,7 +93,6 @@ TEST( matrix, collect1 )
C(i, j + 2) = B(i, j);
EQUALITY(C,AB);
}
/* ************************************************************************* */
@ -116,7 +113,6 @@ TEST( matrix, collect2 )
C(i, j + 2) = B(i, j);
EQUALITY(C,AB);
}
/* ************************************************************************* */
@ -129,8 +125,9 @@ TEST( matrix, collect3 )
matrices.push_back(&A);
matrices.push_back(&B);
Matrix AB = collect(matrices, 2, 3);
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);
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);
EQUALITY(exp,AB);
}
@ -158,17 +155,17 @@ TEST( matrix, column )
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 a1 = column(A, 0);
Vector exp1 = Vector_(4, -1., 0., 1., 0.);
CHECK(assert_equal(a1, exp1));
EXPECT(assert_equal(a1, exp1));
Vector a2 = column_(A, 3);
Vector a2 = column(A, 3);
Vector exp2 = Vector_(4, 0., 1., 0., 0.);
CHECK(assert_equal(a2, exp2));
EXPECT(assert_equal(a2, exp2));
Vector a3 = column_(A, 6);
Vector a3 = column(A, 6);
Vector exp3 = Vector_(4, -0.2, 0.3, 0.2, -0.1);
CHECK(assert_equal(a3, exp3));
EXPECT(assert_equal(a3, exp3));
}
/* ************************************************************************* */
@ -187,7 +184,7 @@ TEST( matrix, insert_column )
0.0, 0.0, 0.0, 1.0, 0.0, 0.0,
0.0, 0.0, 0.0, 1.0, 0.0, 0.0);
CHECK(assert_equal(expected, big));
EXPECT(assert_equal(expected, big));
}
/* ************************************************************************* */
@ -210,7 +207,7 @@ TEST( matrix, insert_subcolumn )
0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 1.0);
CHECK(assert_equal(expected, big));
EXPECT(assert_equal(expected, big));
}
/* ************************************************************************* */
@ -219,17 +216,17 @@ TEST( matrix, row )
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 a1 = row(A, 0);
Vector exp1 = Vector_(7, -1., 0., 1., 0., 0., 0., -0.2);
CHECK(assert_equal(a1, exp1));
EXPECT(assert_equal(a1, exp1));
Vector a2 = row_(A, 2);
Vector a2 = row(A, 2);
Vector exp2 = Vector_(7, 1., 0., 0., 0., -1., 0., 0.2);
CHECK(assert_equal(a2, exp2));
EXPECT(assert_equal(a2, exp2));
Vector a3 = row_(A, 3);
Vector a3 = row(A, 3);
Vector exp3 = Vector_(7, 0., 1., 0., 0., 0., -1., -0.1);
CHECK(assert_equal(a3, exp3));
EXPECT(assert_equal(a3, exp3));
}
/* ************************************************************************* */
@ -260,7 +257,7 @@ TEST( matrix, insert_sub )
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);
CHECK(assert_equal(expected, big));
EXPECT(assert_equal(expected, big));
}
/* ************************************************************************* */
@ -298,7 +295,7 @@ TEST( matrix, scale_columns )
expected(2, 2) = 4.;
expected(2, 3) = 5.;
CHECK(assert_equal(actual, expected));
EXPECT(assert_equal(actual, expected));
}
/* ************************************************************************* */
@ -336,7 +333,7 @@ TEST( matrix, scale_rows )
expected(2, 2) = 4.;
expected(2, 3) = 4.;
CHECK(assert_equal(actual, expected));
EXPECT(assert_equal(actual, expected));
}
/* ************************************************************************* */
@ -365,8 +362,8 @@ TEST( matrix, equal )
Matrix A3(A);
A3(3, 3) = -2.1;
CHECK(A==A2);
CHECK(A!=A3);
EXPECT(A==A2);
EXPECT(A!=A3);
}
/* ************************************************************************* */
@ -395,7 +392,7 @@ TEST( matrix, equal_nan )
Matrix A3(A);
A3(3, 3) = inf;
CHECK(A!=A3);
EXPECT(A!=A3);
}
/* ************************************************************************* */
@ -494,8 +491,8 @@ TEST( matrix, matrix_vector_multiplication )
TEST( matrix, nrRowsAndnrCols )
{
Matrix A(3, 6);
LONGS_EQUAL( A.size1() , 3 );
LONGS_EQUAL( A.size2() , 6 );
LONGS_EQUAL( A.rows() , 3 );
LONGS_EQUAL( A.cols() , 6 );
}
/* ************************************************************************* */
@ -516,6 +513,61 @@ TEST( matrix, scalar_divide )
EQUALITY(B,A/10);
}
/* ************************************************************************* */
TEST( matrix, zero_below_diagonal ) {
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,
1.0, 2.0, 3.0, 4.0,
0.0, 2.0, 3.0, 4.0,
0.0, 0.0, 3.0, 4.0);
Matrix actual1r = A1;
zeroBelowDiagonal(actual1r);
EXPECT(assert_equal(expected1, actual1r, 1e-10));
MatrixColMajor actual1c = A1;
zeroBelowDiagonal(actual1c);
EXPECT(assert_equal(MatrixColMajor(expected1), actual1c, 1e-10));
actual1c = A1;
zeroBelowDiagonal(actual1c, 4);
EXPECT(assert_equal(MatrixColMajor(expected1), actual1c, 1e-10));
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,
1.0, 2.0, 3.0,
0.0, 2.0, 3.0,
0.0, 0.0, 3.0,
0.0, 0.0, 0.0,
0.0, 0.0, 0.0);
Matrix actual2r = A2;
zeroBelowDiagonal(actual2r);
EXPECT(assert_equal(expected2, actual2r, 1e-10));
MatrixColMajor actual2c = A2;
zeroBelowDiagonal(actual2c);
EXPECT(assert_equal(MatrixColMajor(expected2), actual2c, 1e-10));
Matrix expected2_partial = Matrix_(5, 3,
1.0, 2.0, 3.0,
0.0, 2.0, 3.0,
0.0, 2.0, 3.0,
0.0, 2.0, 3.0,
0.0, 2.0, 3.0);
actual2c = A2;
zeroBelowDiagonal(actual2c, 1);
EXPECT(assert_equal(MatrixColMajor(expected2_partial), actual2c, 1e-10));
}
/* ************************************************************************* */
TEST( matrix, inverse )
{
@ -531,8 +583,8 @@ TEST( matrix, inverse )
A(2, 2) = 6;
Matrix Ainv = inverse(A);
CHECK(assert_equal(eye(3), A*Ainv));
CHECK(assert_equal(eye(3), Ainv*A));
EXPECT(assert_equal(eye(3), A*Ainv));
EXPECT(assert_equal(eye(3), Ainv*A));
Matrix expected(3, 3);
expected(0, 0) = 1.0909;
@ -545,21 +597,21 @@ TEST( matrix, inverse )
expected(2, 1) = 0.0909;
expected(2, 2) = 0.1818;
CHECK(assert_equal(expected, Ainv, 1e-4));
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));
CHECK(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)));
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));
CHECK(assert_equal(Matrix_(3,3,
0.0, 1.0,-2.0,
-1.0, 0.0, 1.0,
0.0, 0.0, 1.0),
inverse(gMl)));
EXPECT(assert_equal(Matrix_(3,3,
0.0, 1.0,-2.0,
-1.0, 0.0, 1.0,
0.0, 0.0, 1.0),
inverse(gMl)));
}
/* ************************************************************************* */
@ -589,7 +641,7 @@ TEST( matrix, inverse2 )
expected(2, 1) = 0;
expected(2, 2) = 1;
CHECK(assert_equal(expected, Ainv, 1e-4));
EXPECT(assert_equal(expected, Ainv, 1e-4));
}
/* ************************************************************************* */
@ -599,78 +651,137 @@ TEST( matrix, backsubtitution )
Vector expected1 = Vector_(2, 3.6250, -0.75);
Matrix U22 = Matrix_(2, 2, 2., 3., 0., 4.);
Vector b1 = U22 * expected1;
CHECK( assert_equal(expected1 , backSubstituteUpper(U22, b1), 0.000001));
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 b2 = U33 * expected2;
CHECK( assert_equal(expected2 , backSubstituteUpper(U33, b2), 0.000001));
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.);
Matrix L3 = trans(U33);
Vector b3 = L3 * expected3;
CHECK( assert_equal(expected3 , backSubstituteLower(L3, b3), 0.000001));
EXPECT( assert_equal(expected3 , backSubstituteLower(L3, b3), 0.000001));
// TEST FOUR Try the above with transpose backSubstituteUpper
CHECK( assert_equal(expected3 , backSubstituteUpper(b3,U33), 0.000001));
EXPECT( assert_equal(expected3 , backSubstituteUpper(b3,U33), 0.000001));
}
/* ************************************************************************* */
// unit tests for housholder transformation
/* ************************************************************************* */
TEST( matrix, houseHolder )
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 };
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,
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 };
double data1[] = {
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);
householder_(A1, 3);
CHECK(assert_equal(expected1, A1, 1e-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,
double data2[] = {
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);
householder(A2, 3);
CHECK(assert_equal(expected, A2, 1e-3));
EXPECT(assert_equal(expected, A2, 1e-3));
}
/* ************************************************************************* */
// unit tests for housholder transformation
/* ************************************************************************* */
#ifdef GT_USE_LAPACK
#ifdef YA_BLAS
TEST( matrix, houseHolder2 )
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 };
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,
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 expected1 = Matrix_(4, 7, data1);
Matrix A1 = Matrix_(4, 7, data);
householder(A1);
CHECK(assert_equal(expected1, A1, 1e-3));
double data1[] = {
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 };
MatrixColMajor expected1(Matrix_(4, 7, data1));
MatrixColMajor A1(Matrix_(4, 7, data));
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,
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 };
MatrixColMajor expected(Matrix_(4, 7, data2));
MatrixColMajor A2(Matrix_(4, 7, data));
householder(A2, 3);
EXPECT(assert_equal(expected, A2, 1e-3));
}
#endif
#endif
/* ************************************************************************* */
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,
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 };
MatrixColMajor expected(Matrix_(4, 7, data2));
MatrixColMajor A(Matrix_(4, 7, data));
MatrixColMajor actual = A.householderQr().matrixQR();
zeroBelowDiagonal(actual);
EXPECT(assert_equal(expected, actual, 1e-3));
// use shiny new in place QR inside gtsam
A = MatrixColMajor(Matrix_(4, 7, data));
inplace_QR(A);
EXPECT(assert_equal(expected, A, 1e-3));
}
///* ************************************************************************* */
//// unit tests for householder transformation
///* ************************************************************************* */
//#ifdef GT_USE_LAPACK
//#ifdef YA_BLAS
//TEST( matrix, houseHolder2 )
//{
// 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,
// 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 expected1 = Matrix_(4, 7, data1);
// Matrix A1 = Matrix_(4, 7, data);
// householder(A1);
// EXPECT(assert_equal(expected1, A1, 1e-3));
//}
//#endif
//#endif
/* ************************************************************************* */
// unit test for qr factorization (and hence householder)
@ -694,9 +805,9 @@ TEST( matrix, qr )
Matrix Q, R;
boost::tie(Q, R) = qr(A);
CHECK(assert_equal(expectedQ, Q, 1e-4));
CHECK(assert_equal(expectedR, R, 1e-4));
CHECK(assert_equal(A, Q*R, 1e-14));
EXPECT(assert_equal(expectedQ, Q, 1e-4));
EXPECT(assert_equal(expectedR, R, 1e-4));
EXPECT(assert_equal(A, Q*R, 1e-14));
}
/* ************************************************************************* */
@ -735,7 +846,7 @@ TEST( matrix, row_major_access )
// __attribute__ ((noinline)) // uncomment to prevent inlining when profiling
//static void updateAb(Matrix& A, Vector& b, int j, const Vector& a,
// const Vector& r, double d) {
// const size_t m = A.size1(), n = A.size2();
// const size_t m = A.rows(), n = A.cols();
// for (int i = 0; i < m; i++) { // update all rows
// double ai = a(i);
// b(i) -= ai * d;
@ -774,12 +885,12 @@ TEST( matrix, weighted_elimination )
// unpack and verify
i = 0;
BOOST_FOREACH(boost::tie(r, di, sigma), solution)
{ CHECK(assert_equal(r, row(expectedR, i))); // verify r
DOUBLES_EQUAL(d(i), di, 1e-8); // verify d
DOUBLES_EQUAL(newSigmas(i), sigma, 1e-5); // verify sigma
i += 1;
}
BOOST_FOREACH(boost::tie(r, di, sigma), solution){
EXPECT(assert_equal(r, expectedR.row(i))); // verify r
DOUBLES_EQUAL(d(i), di, 1e-8); // verify d
DOUBLES_EQUAL(newSigmas(i), sigma, 1e-5); // verify sigma
i += 1;
}
}
/* ************************************************************************* */
@ -795,25 +906,25 @@ TEST( matrix, inverse_square_root )
EQUALITY(expected,actual);
EQUALITY(measurement_covariance,inverse(actual*actual));
// Randomly generated test. This test really requires inverse to
// be working well; if it's not, there's the possibility of a
// Randomly generated test. This test really requires inverse to
// be working well; if it's not, there's the possibility of a
// bug in inverse masking a bug in this routine since we
// use the same inverse routing inside inverse_square_root()
// as we use here to check it.
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);
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 = trans(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,
0.683045380655496, 0.283773848115276, -0.099969232183396, 3.433537147891568, 0.000000000000000,
-0.006740136923185, -0.669325697387650, -0.169716689114923, 0.171493059476284, 3.583921085468937));
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,
0.683045380655496, 0.283773848115276, -0.099969232183396, 3.433537147891568, 0.000000000000000,
-0.006740136923185, -0.669325697387650, -0.169716689114923, 0.171493059476284, 3.583921085468937));
EQUALITY(expected, inverse_square_root(M));
}
@ -830,11 +941,11 @@ TEST( matrix, LLt )
-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);
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));
}
@ -844,10 +955,10 @@ 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.),
expected = e + prod(A, x);
expected = e + A * x;
multiplyAdd(1, A, x, e);
CHECK(assert_equal(expected, e));
EXPECT(assert_equal(expected, e));
}
/* ************************************************************************* */
@ -855,10 +966,10 @@ 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.),
expected = x + prod(trans(A), e);
expected = x + trans(A) * e;
transposeMultiplyAdd(1, A, e, x);
CHECK(assert_equal(expected, x));
EXPECT(assert_equal(expected, x));
}
/* ************************************************************************* */
@ -866,7 +977,7 @@ 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);
CHECK(linear_dependent(A, B));
EXPECT(linear_dependent(A, B));
}
/* ************************************************************************* */
@ -874,7 +985,7 @@ 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);
CHECK(linear_dependent(A, B));
EXPECT(linear_dependent(A, B));
}
/* ************************************************************************* */
@ -882,7 +993,97 @@ 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);
CHECK(linear_independent(A, B));
EXPECT(linear_independent(A, B));
}
/* ************************************************************************* */
TEST( matrix, svd1 )
{
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;
Vector s;
svd(A, U, s, V);
Matrix S = diag(s);
EXPECT(assert_equal(U*S*Matrix(trans(V)),A));
EXPECT(assert_equal(S,S1));
}
/* ************************************************************************* */
/// Sample A matrix for SVD
static Matrix sampleA = Matrix_(3, 2, 0.,-2., 0., 0., 3., 0.);
static Matrix sampleAt = trans(sampleA);
/* ************************************************************************* */
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.);
svd(sampleA, U, s, V);
EXPECT(assert_equal(expectedU,U));
EXPECT(assert_equal(expected_s,s,1e-9));
EXPECT(assert_equal(expectedV,V));
}
/* ************************************************************************* */
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.);
svd(sampleAt, U, s, V);
Matrix S = diag(s);
Matrix t = U * S;
Matrix Vt = trans(V);
EXPECT(assert_equal(sampleAt, prod(t, Vt)));
EXPECT(assert_equal(expectedU,U));
EXPECT(assert_equal(expected_s,s,1e-9));
EXPECT(assert_equal(expectedV,V));
}
/* ************************************************************************* */
TEST( matrix, svd4 )
{
Matrix U, V;
Vector s;
Matrix A = Matrix_(3,2,
0.8147, 0.9134,
0.9058, 0.6324,
0.1270, 0.0975);
Matrix expectedU = Matrix_(3,2,
/// right column - originally had opposite sign
-0.7397, -0.6724,
-0.6659, 0.7370,
-0.0970, 0.0689);
Vector expected_s = Vector_(2, 1.6455, 0.1910);
Matrix expectedV = Matrix_(2,2,
/// right column - originally had opposite sign
-0.7403, 0.6723,
-0.6723, -0.7403);
svd(A, U, s, V);
Matrix reconstructed = U * diag(s) * trans(V);
EXPECT(assert_equal(A, reconstructed, 1e-4));
EXPECT(assert_equal(expectedU,U, 1e-3));
EXPECT(assert_equal(expected_s,s, 1e-4));
EXPECT(assert_equal(expectedV,V, 1e-4));
}
/* ************************************************************************* */

2
gtsam/base/tests/testNumericalDerivative.cpp
View File

@ -30,7 +30,7 @@ double f(const LieVector& x) {
/* ************************************************************************* */
TEST_UNSAFE(testNumericalDerivative, numericalHessian) {
LieVector center(2, 1.0, 1.0);
LieVector center = ones(2);
Matrix expected = Matrix_(2,2,
-sin(center(0)), 0.0,

94
gtsam/base/tests/testVector.cpp
View File

@ -29,7 +29,7 @@ TEST( TestVector, Vector_variants )
Vector a = Vector_(2,10.0,20.0);
double data[] = {10,20};
Vector b = Vector_(2,data);
CHECK(a==b);
EXPECT(assert_equal(a, b));
}
/* ************************************************************************* */
@ -37,23 +37,25 @@ TEST( TestVector, copy )
{
Vector a(2); a(0) = 10; a(1) = 20;
double data[] = {10,20};
Vector b(2); copy(data,data+2,b.begin());
CHECK(a==b);
Vector b(2);
copy(data,data+2,b.data());
EXPECT(assert_equal(a, b));
}
/* ************************************************************************* */
TEST( TestVector, zero1 )
{
Vector v(2,0.0);
CHECK(zero(v)==true);
Vector v = Vector::Zero(2);
EXPECT(zero(v));
}
/* ************************************************************************* */
TEST( TestVector, zero2 )
{
Vector a = zero(2);
Vector b(2,0.0);
CHECK(a==b);
Vector b = Vector::Zero(2);
EXPECT(a==b);
EXPECT(assert_equal(a, b));
}
/* ************************************************************************* */
@ -61,7 +63,7 @@ TEST( TestVector, scalar_multiply )
{
Vector a(2); a(0) = 10; a(1) = 20;
Vector b(2); b(0) = 1; b(1) = 2;
CHECK(a==b*10.0);
EXPECT(assert_equal(a,b*10.0));
}
/* ************************************************************************* */
@ -69,7 +71,7 @@ TEST( TestVector, scalar_divide )
{
Vector a(2); a(0) = 10; a(1) = 20;
Vector b(2); b(0) = 1; b(1) = 2;
CHECK(b==a/10.0);
EXPECT(assert_equal(b,a/10.0));
}
/* ************************************************************************* */
@ -77,22 +79,23 @@ TEST( TestVector, negate )
{
Vector a(2); a(0) = 10; a(1) = 20;
Vector b(2); b(0) = -10; b(1) = -20;
CHECK(b==-a);
EXPECT(assert_equal(b, -a));
}
/* ************************************************************************* */
TEST( TestVector, sub )
{
Vector a(6);
a(0) = 10; a(1) = 20; a(2) = 3;
a(0) = 10; a(1) = 20; a(2) = 3;
a(3) = 34; a(4) = 11; a(5) = 2;
Vector result(sub(a,2,5));
Vector b(3);
b(0) = 3; b(1) = 34; b(2) =11;
CHECK(b==result);
Vector result(sub(a,2,5));
Vector b(3);
b(0) = 3; b(1) = 34; b(2) =11;
EXPECT(b==result);
EXPECT(assert_equal(b, result));
}
/* ************************************************************************* */
@ -106,30 +109,22 @@ TEST( TestVector, subInsert )
Vector expected = Vector_(6, 0.0, 0.0, 1.0, 1.0, 1.0, 0.0);
CHECK(assert_equal(expected, big));
EXPECT(assert_equal(expected, big));
}
/* ************************************************************************* */
TEST( TestVector, householder )
{
Vector x(4);
Vector x(4);
x(0) = 3; x(1) = 1; x(2) = 5; x(3) = 1;
Vector expected(4);
expected(0) = 1.0; expected(1) = -0.333333; expected(2) = -1.66667; expected(3) = -0.333333;
pair<double, Vector> result = house(x);
CHECK(result.first==0.5);
CHECK(equal_with_abs_tol(expected,result.second,1e-5));
}
/* ************************************************************************* */
TEST( TestVector, zeros )
{
Vector a(2); a(0) = 0; a(1) = 0;
Vector b(2,0.0);
CHECK(b==a);
pair<double, Vector> result = house(x);
EXPECT(result.first==0.5);
EXPECT(equal_with_abs_tol(expected,result.second,1e-5));
}
/* ************************************************************************* */
@ -141,7 +136,7 @@ TEST( TestVector, concatVectors)
Vector B(5);
for(int i = 0; i < 5; i++)
B(i) = i;
Vector C(7);
for(int i = 0; i < 2; i++) C(i) = A(i);
for(int i = 0; i < 5; i++) C(i+2) = B(i);
@ -150,10 +145,10 @@ TEST( TestVector, concatVectors)
vs.push_back(A);
vs.push_back(B);
Vector AB1 = concatVectors(vs);
CHECK(AB1 == C);
EXPECT(AB1 == C);
Vector AB2 = concatVectors(2, &A, &B);
CHECK(AB2 == C);
EXPECT(AB2 == C);
}
/* ************************************************************************* */
@ -178,8 +173,8 @@ TEST( TestVector, weightedPseudoinverse )
double expPrecision = 200.0;
// verify
CHECK(assert_equal(expected,actual));
CHECK(fabs(expPrecision-precision) < 1e-5);
EXPECT(assert_equal(expected,actual));
EXPECT(fabs(expPrecision-precision) < 1e-5);
}
/* ************************************************************************* */
@ -203,8 +198,8 @@ TEST( TestVector, weightedPseudoinverse_constraint )
expected(0) = 1.0; expected(1) = 0.0;
// verify
CHECK(assert_equal(expected,actual));
CHECK(isinf(precision));
EXPECT(assert_equal(expected,actual));
EXPECT(isinf(precision));
}
/* ************************************************************************* */
@ -217,11 +212,10 @@ TEST( TestVector, weightedPseudoinverse_nan )
boost::tie(pseudo, precision) = weightedPseudoinverse(a, weights);
Vector expected = Vector_(4, 1., 0., 0.,0.);
CHECK(assert_equal(expected, pseudo));
EXPECT(assert_equal(expected, pseudo));
DOUBLES_EQUAL(100, precision, 1e-5);
}
/* ************************************************************************* */
TEST( TestVector, ediv )
{
@ -230,7 +224,7 @@ TEST( TestVector, ediv )
Vector actual(ediv(a,b));
Vector c = Vector_(3,5.0,4.0,5.0);
CHECK(assert_equal(c,actual));
EXPECT(assert_equal(c,actual));
}
/* ************************************************************************* */
@ -248,7 +242,7 @@ TEST( TestVector, axpy )
Vector y = Vector_(3,2.0,5.0,6.0);
axpy(0.1,x,y);
Vector expected = Vector_(3,3.0,7.0,9.0);
CHECK(assert_equal(expected,y));
EXPECT(assert_equal(expected,y));
}
/* ************************************************************************* */
@ -257,7 +251,7 @@ TEST( TestVector, equals )
Vector v1 = Vector_(1, 0.0/0.0); //testing nan
Vector v2 = Vector_(1, 1.0);
double tol = 1.;
CHECK(!equal_with_abs_tol(v1, v2, tol));
EXPECT(!equal_with_abs_tol(v1, v2, tol));
}
/* ************************************************************************* */
@ -265,15 +259,15 @@ TEST( TestVector, greater_than )
{
Vector v1 = Vector_(3, 1.0, 2.0, 3.0),
v2 = zero(3);
CHECK(greaterThanOrEqual(v1, v1)); // test basic greater than
CHECK(greaterThanOrEqual(v1, v2)); // test equals
EXPECT(greaterThanOrEqual(v1, v1)); // test basic greater than
EXPECT(greaterThanOrEqual(v1, v2)); // test equals
}
/* ************************************************************************* */
TEST( TestVector, reciprocal )
{
Vector v = Vector_(3, 1.0, 2.0, 4.0);
CHECK(assert_equal(Vector_(3, 1.0, 0.5, 0.25),reciprocal(v)));
EXPECT(assert_equal(Vector_(3, 1.0, 0.5, 0.25),reciprocal(v)));
}
/* ************************************************************************* */
@ -281,7 +275,7 @@ TEST( TestVector, linear_dependent )
{
Vector v1 = Vector_(3, 1.0, 2.0, 3.0);
Vector v2 = Vector_(3, -2.0, -4.0, -6.0);
CHECK(linear_dependent(v1, v2));
EXPECT(linear_dependent(v1, v2));
}
/* ************************************************************************* */
@ -289,7 +283,7 @@ TEST( TestVector, linear_dependent2 )
{
Vector v1 = Vector_(3, 0.0, 2.0, 0.0);
Vector v2 = Vector_(3, 0.0, -4.0, 0.0);
CHECK(linear_dependent(v1, v2));
EXPECT(linear_dependent(v1, v2));
}
/* ************************************************************************* */
@ -297,7 +291,7 @@ TEST( TestVector, linear_dependent3 )
{
Vector v1 = Vector_(3, 0.0, 2.0, 0.0);
Vector v2 = Vector_(3, 0.1, -4.1, 0.0);
CHECK(!linear_dependent(v1, v2));
EXPECT(!linear_dependent(v1, v2));
}
/* ************************************************************************* */

6
gtsam/base/tests/timeMatrix.cpp
View File

@ -162,7 +162,7 @@ double timeColumn(size_t reps) {
for (size_t i=0; i<reps; ++i)
for (size_t j = 0; j<n; ++j)
//result = ublas::matrix_column<Matrix>(M, j);
result = column_(M, j);
result = column(M, j);
elapsed = t.elapsed();
}
return elapsed;
@ -257,8 +257,8 @@ int main(int argc, char ** argv) {
double vsRow_time = timeVScaleRow(m1, n1, reps1);
cout << "Elapsed time for vector_scale(row) [(" << m1 << ", " << n1 << ") matrix] : " << vsRow_time << endl;
// Time column_() NOTE: using the ublas version
cout << "Starting column_() Timing" << endl;
// Time column() NOTE: using the ublas version
cout << "Starting column() Timing" << endl;
size_t reps2 = 2000000;
double column_time = timeColumn(reps2);
cout << "Time: " << column_time << " sec" << endl;

2
gtsam/geometry/Cal3Bundler.cpp
View File

@ -93,7 +93,7 @@ Matrix Cal3Bundler::D2d_intrinsic(const Point2& p) const {
Matrix DR = Matrix_(2, 2,
g + x*dg_dx, x*dg_dy,
y*dg_dx , g + y*dg_dy) ;
return prod(DK,DR) ;
return DK * DR;
}
Matrix Cal3Bundler::D2d_calibration(const Point2& p) const {

2
gtsam/geometry/Cal3DS2.cpp
View File

@ -92,7 +92,7 @@ Matrix Cal3DS2::D2d_intrinsic(const Point2& p) const {
Matrix DR = Matrix_(2, 2, g + x*dg_dx + dDx_dx, x*dg_dy + dDx_dy,
y*dg_dx + dDy_dx, g + y*dg_dy + dDy_dy) ;
return prod(DK,DR) ;
return DK * DR;
}

2
gtsam/geometry/Cal3_S2.h
View File

@ -93,7 +93,7 @@ namespace gtsam {
Vector vector() const {
double r[] = { fx_, fy_, s_, u0_, v0_ };
Vector v(5);
copy(r, r + 5, v.begin());
std::copy(r, r + 5, v.data());
return v;
}

32
gtsam/geometry/GeneralCameraT.h
View File

@ -18,8 +18,6 @@
#pragma once
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/serialization/nvp.hpp>
#include <gtsam/base/Vector.h>
#include <gtsam/base/Matrix.h>
@ -52,8 +50,8 @@ class GeneralCameraT {
// Vector Initialization
GeneralCameraT(const Vector &v) :
calibrated_(subrange(v, 0, Camera::Dim())),
calibration_(subrange(v, Camera::Dim(), Camera::Dim() + Calibration::Dim() )) {}
calibrated_(sub(v, 0, Camera::Dim())),
calibration_(sub(v, Camera::Dim(), Camera::Dim() + Calibration::Dim() )) {}
inline const Pose3& pose() const { return calibrated_.pose(); }
inline const Camera& calibrated() const { return calibrated_; }
@ -123,8 +121,8 @@ class GeneralCameraT {
}
GeneralCameraT expmap(const Vector &v) const {
Vector v1 = subrange(v,0,Camera::Dim());
Vector v2 = subrange(v,Camera::Dim(),Camera::Dim()+Calibration::Dim());
Vector v1 = sub(v,0,Camera::Dim());
Vector v2 = sub(v,Camera::Dim(),Camera::Dim()+Calibration::Dim());
return GeneralCameraT(calibrated_.expmap(v1), calibration_.expmap(v2));
}
@ -138,8 +136,8 @@ class GeneralCameraT {
static GeneralCameraT Expmap(const Vector& v) {
//std::cout << "Expmap" << std::endl;
return GeneralCameraT(
Camera::Expmap(subrange(v,0,Camera::Dim())),
Calibration::Expmap(subrange(v,Camera::Dim(), Camera::Dim()+Calibration::Dim()))
Camera::Expmap(sub(v,0,Camera::Dim())),
Calibration::Expmap(sub(v,Camera::Dim(), Camera::Dim()+Calibration::Dim()))
);
}
@ -159,15 +157,15 @@ class GeneralCameraT {
Point2 intrinsic = calibrated_.project(point);
Matrix D_intrinsic_pose = Dproject_pose(calibrated_, point);
Matrix D_2d_intrinsic = calibration_.D2d_intrinsic(intrinsic);
Matrix D_2d_pose = prod(D_2d_intrinsic,D_intrinsic_pose);
Matrix D_2d_pose = D_2d_intrinsic * D_intrinsic_pose;
Matrix D_2d_calibration = calibration_.D2d_calibration(intrinsic);
const int n1 = calibrated_.dim() ;
const int n2 = calibration_.dim() ;
Matrix D(2,n1+n2) ;
subrange(D,0,2,0,n1) = D_2d_pose ;
subrange(D,0,2,n1,n1+n2) = D_2d_calibration ;
sub(D,0,2,0,n1) = D_2d_pose ;
sub(D,0,2,n1,n1+n2) = D_2d_calibration ;
return D;
}
@ -175,27 +173,27 @@ class GeneralCameraT {
Point2 intrinsic = calibrated_.project(point);
Matrix D_intrinsic_3d = Dproject_point(calibrated_, point);
Matrix D_2d_intrinsic = calibration_.D2d_intrinsic(intrinsic);
return prod(D_2d_intrinsic,D_intrinsic_3d);
return D_2d_intrinsic * D_intrinsic_3d;
}
Matrix D2d_camera_3d(const Point3& point) const {
Point2 intrinsic = calibrated_.project(point);
Matrix D_intrinsic_pose = Dproject_pose(calibrated_, point);
Matrix D_2d_intrinsic = calibration_.D2d_intrinsic(intrinsic);
Matrix D_2d_pose = prod(D_2d_intrinsic,D_intrinsic_pose);
Matrix D_2d_pose = D_2d_intrinsic * D_intrinsic_pose;
Matrix D_2d_calibration = calibration_.D2d_calibration(intrinsic);
Matrix D_intrinsic_3d = Dproject_point(calibrated_, point);
Matrix D_2d_3d = prod(D_2d_intrinsic,D_intrinsic_3d);
Matrix D_2d_3d = D_2d_intrinsic * D_intrinsic_3d;
const int n1 = calibrated_.dim() ;
const int n2 = calibration_.dim() ;
Matrix D(2,n1+n2+3) ;
subrange(D,0,2,0,n1) = D_2d_pose ;
subrange(D,0,2,n1,n1+n2) = D_2d_calibration ;
subrange(D,0,2,n1+n2,n1+n2+3) = D_2d_3d ;
sub(D,0,2,0,n1) = D_2d_pose ;
sub(D,0,2,n1,n1+n2) = D_2d_calibration ;
sub(D,0,2,n1+n2,n1+n2+3) = D_2d_3d ;
return D;
}

4
gtsam/geometry/Makefile.am
View File

@ -53,10 +53,6 @@ AM_LDFLAGS += $(boost_serialization)
LDADD = libgeometry.la ../base/libbase.la ../../CppUnitLite/libCppUnitLite.a
AM_DEFAULT_SOURCE_EXT = .cpp
if USE_ACCELERATE_MACOS
AM_LDFLAGS += -Wl,/System/Library/Frameworks/Accelerate.framework/Accelerate
endif
# rule to run an executable
%.run: % $(LDADD)
./$^

2
gtsam/geometry/Pose2.cpp
View File

@ -190,7 +190,7 @@ namespace gtsam {
-c, -s, dt1,
s, -c, dt2,
0.0, 0.0, -1.0};
copy(data, data+9, H1->data().begin());
copy(data, data+9, H1->data());
}
if (H2) *H2 = I3;

2
gtsam/geometry/Pose2.h
View File

@ -66,7 +66,7 @@ namespace gtsam {
/** Constructor from 3*3 matrix */
Pose2(const Matrix &T) :
r_(Rot2::atan2(T(1, 0), T(0, 0))), t_(T(0, 2), T(1, 2)) { assert(T.size1()==3 && T.size2()==3); }
r_(Rot2::atan2(T(1, 0), T(0, 0))), t_(T(0, 2), T(1, 2)) { assert(T.rows()==3 && T.cols()==3); }
/** print with optional string */
void print(const std::string& s = "") const;

3
gtsam/geometry/Pose3.cpp
View File

@ -19,7 +19,6 @@
#include <gtsam/base/Lie-inl.h>
using namespace std;
using namespace boost::numeric::ublas;
namespace gtsam {
@ -77,7 +76,7 @@ namespace gtsam {
/* ************************************************************************* */
Vector Pose3::LogmapFull(const Pose3& p) {
Vector w = Rot3::Logmap(p.rotation()), T = p.translation().vector();
double t = norm_2(w);
double t = w.norm();
if (t < 1e-10)
return concatVectors(2, &w, &T);
else {

2
gtsam/geometry/Pose3.h
View File

@ -18,8 +18,6 @@
#pragma once
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/base/Testable.h>

2
gtsam/geometry/Rot3.cpp
View File

@ -104,7 +104,7 @@ namespace gtsam {
/* ************************************************************************* */
Rot3 Rot3::rodriguez(const Vector& w) {
double t = norm_2(w);
double t = w.norm();
if (t < 1e-10) return Rot3();
return rodriguez(w/t, t);
}

3
gtsam/geometry/projectiveGeometry.cpp
View File

@ -17,10 +17,7 @@
*/
#include <boost/foreach.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <gtsam/base/Matrix.h>
typedef boost::numeric::ublas::matrix_row<Matrix> Row;
#include <gtsam/geometry/tensorInterface.h>
#include <gtsam/geometry/projectiveGeometry.h>

15
gtsam/geometry/tests/testPose3.cpp
View File

@ -43,8 +43,7 @@ TEST( Pose3, equals)
TEST( Pose3, expmap_a)
{
Pose3 id;
Vector v(6);
fill(v.begin(), v.end(), 0);
Vector v = zero(6);
v(0) = 0.3;
CHECK(assert_equal(id.expmap(v), Pose3(R, Point3())));
#ifdef CORRECT_POSE3_EXMAP
@ -60,8 +59,7 @@ TEST( Pose3, expmap_a)
TEST( Pose3, expmap_a_full)
{
Pose3 id;
Vector v(6);
fill(v.begin(), v.end(), 0);
Vector v = zero(6);
v(0) = 0.3;
CHECK(assert_equal(id.expmapFull(v), Pose3(R, Point3())));
v(3)=0.2;v(4)=0.394742;v(5)=-2.08998;
@ -72,8 +70,7 @@ TEST( Pose3, expmap_a_full)
TEST( Pose3, expmap_a_full2)
{
Pose3 id;
Vector v(6);
fill(v.begin(), v.end(), 0);
Vector v = zero(6);
v(0) = 0.3;
CHECK(assert_equal(expmapFull<Pose3>(id,v), Pose3(R, Point3())));
v(3)=0.2;v(4)=0.394742;v(5)=-2.08998;
@ -126,7 +123,6 @@ TEST(Pose3, Adjoint)
/* ************************************************************************* */
/** Agrawal06iros version */
using namespace boost::numeric::ublas;
Pose3 Agrawal06iros(const Vector& xi) {
Vector w = vector_range<const Vector>(xi, range(0,3));
Vector v = vector_range<const Vector>(xi, range(3,6));
@ -209,10 +205,9 @@ TEST(Pose3, Adjoint_full)
/* ************************************************************************* */
/** Agrawal06iros version */
using namespace boost::numeric::ublas;
Pose3 Agrawal06iros(const Vector& xi) {
Vector w = vector_range<const Vector>(xi, range(0,3));
Vector v = vector_range<const Vector>(xi, range(3,6));
Vector w = xi.head(3);
Vector v = xi.tail(3);
double t = norm_2(w);
if (t < 1e-5)
return Pose3(Rot3(), Point3::Expmap(v));

3
gtsam/geometry/tests/testRot3.cpp
View File

@ -136,8 +136,7 @@ TEST( Rot3, rodriguez4)
/* ************************************************************************* */
TEST( Rot3, expmap)
{
Vector v(3);
fill(v.begin(), v.end(), 0);
Vector v = zero(3);
CHECK(assert_equal(R.expmap(v), R));
}

8
gtsam/inference/Makefile.am
View File

@ -77,10 +77,6 @@ LDADD = libinference.la ../base/libbase.la
LDADD += ../../CppUnitLite/libCppUnitLite.a
AM_DEFAULT_SOURCE_EXT = .cpp
if USE_ACCELERATE_MACOS
AM_LDFLAGS += -Wl,/System/Library/Frameworks/Accelerate.framework/Accelerate
endif
# rule to run an executable
%.run: % $(LDADD)
./$^
@ -89,7 +85,3 @@ endif
%.valgrind: % $(LDADD)
valgrind ./$^
if USE_LAPACK
AM_CXXFLAGS += -DGT_USE_LAPACK
endif

6
gtsam/linear/GaussianBayesNet.cpp
View File

@ -74,7 +74,7 @@ boost::shared_ptr<VectorValues> allocateVectorValues(const GaussianBayesNet& bn)
vector<size_t> dimensions(bn.size());
Index var = 0;
BOOST_FOREACH(const boost::shared_ptr<const GaussianConditional> conditional, bn) {
dimensions[var++] = conditional->get_R().size1();
dimensions[var++] = conditional->get_R().rows();
}
return boost::shared_ptr<VectorValues>(new VectorValues(dimensions));
}
@ -138,7 +138,7 @@ VectorValues backSubstituteTranspose(const GaussianBayesNet& bn,
// i^th part of L*gy=gx is done block-column by block-column of L
BOOST_FOREACH(const boost::shared_ptr<const GaussianConditional> cg, bn) {
Index j = cg->key();
gy[j] = gtsam::backSubstituteUpper(gy[j],cg->get_R());
gy[j] = gtsam::backSubstituteUpper(gy[j],Matrix(cg->get_R()));
GaussianConditional::const_iterator it;
for (it = cg->beginParents(); it!= cg->endParents(); it++) {
const Index i = *it;
@ -193,7 +193,7 @@ pair<Matrix,Vector> matrix(const GaussianBayesNet& bn) {
GaussianConditional::const_iterator keyS = cg->beginParents();
for (; keyS!=cg->endParents(); keyS++) {
Matrix S = cg->get_S(keyS); // get S matrix
const size_t m = S.size1(), n = S.size2(); // find S size
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<m;i++)
for(size_t j=0;j<n;j++)

77
gtsam/linear/GaussianConditional.cpp
View File

@ -16,8 +16,6 @@
*/
#include <string.h>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/operation.hpp>
#include <boost/format.hpp>
#include <boost/lambda/bind.hpp>
@ -27,7 +25,6 @@
#include <gtsam/base/Matrix-inl.h>
using namespace std;
namespace ublas = boost::numeric::ublas;
namespace gtsam {
@ -40,81 +37,82 @@ GaussianConditional::GaussianConditional(Index key) : IndexConditional(key), rsd
/* ************************************************************************* */
GaussianConditional::GaussianConditional(Index key,const Vector& d, const Matrix& R, const Vector& sigmas) :
IndexConditional(key), rsd_(matrix_), sigmas_(sigmas) {
assert(R.size1() <= R.size2());
size_t dims[] = { R.size2(), 1 };
assert(R.rows() <= R.cols());
size_t dims[] = { R.cols(), 1 };
rsd_.copyStructureFrom(rsd_type(matrix_, dims, dims+2, d.size()));
ublas::noalias(rsd_(0)) = ublas::triangular_adaptor<const Matrix, ublas::upper>(R);
ublas::noalias(get_d_()) = d;
rsd_(0) = R; //.triangularView<Eigen::Upper>();
get_d_() = d;
}
/* ************************************************************************* */
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.size1() <= R.size2());
size_t dims[] = { R.size2(), S.size2(), 1 };
assert(R.rows() <= R.cols());
size_t dims[] = { R.cols(), S.cols(), 1 };
rsd_.copyStructureFrom(rsd_type(matrix_, dims, dims+3, d.size()));
ublas::noalias(rsd_(0)) = ublas::triangular_adaptor<const Matrix, ublas::upper>(R);
ublas::noalias(rsd_(1)) = S;
ublas::noalias(get_d_()) = d;
rsd_(0) = R.triangularView<Eigen::Upper>();
rsd_(1) = S;
get_d_() = d;
}
/* ************************************************************************* */
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.size1() <= R.size2());
size_t dims[] = { R.size2(), S.size2(), T.size2(), 1 };
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()));
ublas::noalias(rsd_(0)) = ublas::triangular_adaptor<const Matrix, ublas::upper>(R);
ublas::noalias(rsd_(1)) = S;
ublas::noalias(rsd_(2)) = T;
ublas::noalias(get_d_()) = d;
rsd_(0) = R.triangularView<Eigen::Upper>();
rsd_(1) = S;
rsd_(2) = T;
get_d_() = d;
}
/* ************************************************************************* */
GaussianConditional::GaussianConditional(Index key, const Vector& d,
const Matrix& R, const list<pair<Index, Matrix> >& parents, const Vector& sigmas) :
IndexConditional(key, GetKeys(parents.size(), parents.begin(), parents.end())), rsd_(matrix_), sigmas_(sigmas) {
assert(R.size1() <= R.size2());
assert(R.rows() <= R.cols());
size_t dims[1+parents.size()+1];
dims[0] = R.size2();
dims[0] = R.cols();
size_t j=1;
std::list<std::pair<Index, Matrix> >::const_iterator parent=parents.begin();
for(; parent!=parents.end(); ++parent,++j)
dims[j] = parent->second.size2();
dims[j] = parent->second.cols();
dims[j] = 1;
rsd_.copyStructureFrom(rsd_type(matrix_, dims, dims+1+parents.size()+1, d.size()));
ublas::noalias(rsd_(0)) = ublas::triangular_adaptor<const Matrix, ublas::upper>(R);
rsd_(0) = R.triangularView<Eigen::Upper>();
j = 1;
for(std::list<std::pair<Index, Matrix> >::const_iterator parent=parents.begin(); parent!=parents.end(); ++parent) {
ublas::noalias(rsd_(j)) = parent->second;
rsd_(j).noalias() = parent->second;
++ j;
}
ublas::noalias(get_d_()) = d;
get_d_().noalias() = d;
}
/* ************************************************************************* */
void GaussianConditional::print(const string &s) const
{
cout << s << ": density on " << key() << endl;
gtsam::print(get_R(),"R");
gtsam::print(Matrix(get_R()),"R");
for(const_iterator it = beginParents() ; it != endParents() ; it++ ) {
gtsam::print(get_S(it), (boost::format("A[%1%]")%(*it)).str());
gtsam::print(Matrix(get_S(it)), (boost::format("A[%1%]")%(*it)).str());
}
gtsam::print(get_d(),"d");
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;
if (parents().size() != c.parents().size())
return false;
// check if R_ and d_ are linear independent
for (size_t i=0; i<rsd_.size1(); i++) {
list<Vector> rows1; rows1.push_back(row_(get_R(), i));
list<Vector> rows2; rows2.push_back(row_(c.get_R(), i));
for (size_t i=0; i<rsd_.rows(); i++) {
list<Vector> rows1; rows1.push_back(Vector(get_R().row(i)));
list<Vector> rows2; rows2.push_back(Vector(c.get_R().row(i)));
// check if the matrices are the same
// iterate over the parents_ map
@ -122,13 +120,14 @@ bool GaussianConditional::equals(const GaussianConditional &c, double tol) const
const_iterator it2 = c.beginParents() + (it-beginParents());
if(*it != *(it2))
return false;
rows1.push_back(row_(get_S(it), i));
rows2.push_back(row_(c.get_S(it2), i));
rows1.push_back(row(get_S(it), i));
rows2.push_back(row(c.get_S(it2), i));
}
Vector row1 = concatVectors(rows1);
Vector row2 = concatVectors(rows2);
if (!linear_dependent(row1, row2, tol)) return false;
if (!linear_dependent(row1, row2, tol))
return false;
}
// check if sigmas are equal
@ -148,10 +147,10 @@ Vector GaussianConditional::solve(const VectorValues& x) const {
if(debug) print("Solving conditional ");
Vector rhs(get_d());
for (const_iterator parent = beginParents(); parent != endParents(); ++parent) {
ublas::axpy_prod(-get_S(parent), x[*parent], rhs, false);
rhs += -get_S(parent) * x[*parent];
// ublas::axpy_prod(-get_S(parent), x[*parent], rhs, false);
}
if(debug) gtsam::print(get_R(), "Calling backSubstituteUpper on ");
if(debug) gtsam::print(Matrix(get_R()), "Calling backSubstituteUpper on ");
if(debug) gtsam::print(rhs, "rhs: ");
if(debug) {
Vector soln = backSubstituteUpper(get_R(), rhs, false);
@ -165,8 +164,8 @@ Vector GaussianConditional::solve(const VectorValues& x) const {
Vector GaussianConditional::solve(const Permuted<VectorValues>& x) const {
Vector rhs(get_d());
for (const_iterator parent = beginParents(); parent != endParents(); ++parent) {
ublas::axpy_prod(-get_S(parent), x[*parent], rhs, false);
rhs += -get_S(parent) * x[*parent];
// ublas::axpy_prod(-get_S(parent), x[*parent], rhs, false);
}
return backSubstituteUpper(get_R(), rhs, false);
}

10
gtsam/linear/GaussianConditional.h
View File

@ -23,7 +23,6 @@
#include <vector>
#include <boost/utility.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <gtsam/base/types.h>
#include <gtsam/inference/IndexConditional.h>
@ -48,7 +47,8 @@ public:
typedef boost::shared_ptr<GaussianConditional> shared_ptr;
/** Store the conditional matrix as upper-triangular column-major */
typedef boost::numeric::ublas::triangular_matrix<double, boost::numeric::ublas::upper, boost::numeric::ublas::column_major> AbMatrix;
// typedef boost::numeric::ublas::triangular_matrix<double, boost::numeric::ublas::upper, boost::numeric::ublas::column_major> AbMatrix;
typedef MatrixColMajor AbMatrix;
typedef VerticalBlockView<AbMatrix> rsd_type;
typedef rsd_type::Block r_type;
@ -112,13 +112,15 @@ public:
bool equals(const GaussianConditional &cg, double tol = 1e-9) const;
/** dimension of multivariate variable */
size_t dim() const { return rsd_.size1(); }
size_t dim() const { return rsd_.rows(); }
/** return stuff contained in GaussianConditional */
rsd_type::constColumn get_d() const { return rsd_.column(1+nrParents(), 0); }
const_d_type get_d() const { return rsd_.column(1+nrParents(), 0); }
rsd_type::constBlock get_R() const { return rsd_(0); }
rsd_type::constBlock get_S(const_iterator variable) const { return rsd_(variable - this->begin()); }
const Vector& get_sigmas() const {return sigmas_;}
const AbMatrix& matrix() const { return matrix_; }
const rsd_type& rsd() const { return rsd_; } /// DEBUGGING ONLY
/**
* Copy to a Factor (this creates a JacobianFactor and returns it as its

7
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -107,7 +107,7 @@ namespace gtsam {
1> (), factorEntries[entry].get<2> ()));
// Increment row index
i += jacobianFactor->size1();
i += jacobianFactor->rows();
}
return entries;
}
@ -175,7 +175,7 @@ namespace gtsam {
++ jointVarpos;
}
BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, factors) {
m += factor->size1();
m += factor->rows();
}
}
return boost::make_tuple(varDims, m, n);
@ -275,7 +275,8 @@ namespace gtsam {
}
/* ************************************************************************* */
static FastMap<Index, SlotEntry> findScatterAndDims//
//static
FastMap<Index, SlotEntry> findScatterAndDims
(const FactorGraph<GaussianFactor>& factors) {
static const bool debug = false;

3
gtsam/linear/GaussianFactorGraph.h
View File

@ -61,6 +61,9 @@ namespace gtsam {
return e;
}
/** DEBUG ONLY: TODO: hide again later */
FastMap<Index, SlotEntry> findScatterAndDims(const FactorGraph<GaussianFactor>& factors);
/**
* A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
* Factor == GaussianFactor

4
gtsam/linear/GaussianISAM.cpp
View File

@ -24,7 +24,7 @@ using namespace gtsam;
#include <gtsam/inference/ISAM-inl.h>
template class ISAM<GaussianConditional>;
namespace ublas = boost::numeric::ublas;
//namespace ublas = boost::numeric::ublas;
namespace gtsam {
@ -42,7 +42,7 @@ BayesNet<GaussianConditional>::shared_ptr GaussianISAM::marginalBayesNet(Index j
std::pair<Vector, Matrix> GaussianISAM::marginal(Index j) const {
GaussianConditional::shared_ptr conditional = marginalBayesNet(j)->front();
Matrix R = conditional->get_R();
return make_pair(conditional->get_d(), inverse(ublas::prod(ublas::trans(R), R)));
return make_pair(conditional->get_d(), (R.transpose() * R).inverse());
}
/* ************************************************************************* */

7
gtsam/linear/GaussianMultifrontalSolver.cpp
View File

@ -20,10 +20,6 @@
#include <gtsam/inference/GenericMultifrontalSolver-inl.h>
#include <boost/numeric/ublas/matrix.hpp>
namespace ublas = boost::numeric::ublas;
namespace gtsam {
/* ************************************************************************* */
@ -75,7 +71,8 @@ std::pair<Vector, Matrix> GaussianMultifrontalSolver::marginalCovariance(Index j
fg.push_back(Base::marginalFactor(j,&EliminateQR));
GaussianConditional::shared_ptr conditional = EliminateQR(fg,1).first->front();
Matrix R = conditional->get_R();
return make_pair(conditional->get_d(), inverse(ublas::prod(ublas::trans(R), R)));
return make_pair(conditional->get_d(), (R.transpose() * R).inverse());
// return make_pair(conditional->get_d(), inverse(ublas::prod(ublas::trans(R), R)));
}
}

7
gtsam/linear/GaussianSequentialSolver.cpp
View File

@ -21,10 +21,6 @@
#include <gtsam/inference/GenericSequentialSolver-inl.h>
#include <boost/numeric/ublas/matrix.hpp>
namespace ublas = boost::numeric::ublas;
namespace gtsam {
/* ************************************************************************* */
@ -91,7 +87,8 @@ std::pair<Vector, Matrix> GaussianSequentialSolver::marginalCovariance(Index j)
fg.push_back(Base::marginalFactor(j, &EliminateQR));
GaussianConditional::shared_ptr conditional = EliminateQR(fg, 1).first->front();
Matrix R = conditional->get_R();
return make_pair(conditional->get_d(), inverse(ublas::prod(ublas::trans(R),R)));
return make_pair(conditional->get_d(), (R.transpose() * R).inverse());
// return make_pair(conditional->get_d(), inverse(ublas::prod(ublas::trans(R),R)));
}
/* ************************************************************************* */

821
gtsam/linear/HessianFactor.cpp
View File

@ -34,390 +34,406 @@
#include <boost/lambda/lambda.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/symmetric.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/blas.hpp>
#include <gtsam/3rdparty/Eigen/Core>
#include <gtsam/3rdparty/Eigen/Dense>
#include <sstream>
using namespace std;
namespace ublas = boost::numeric::ublas;
using namespace boost::lambda;
namespace gtsam {
/* ************************************************************************* */
void HessianFactor::assertInvariants() const {
/* ************************************************************************* */
void HessianFactor::assertInvariants() const {
#ifndef NDEBUG
// Check for non-finite values
for(size_t i=0; i<matrix_.size1(); ++i)
for(size_t j=i; j<matrix_.size2(); ++j)
if(!isfinite(matrix_(i,j)))
throw invalid_argument("HessianFactor contains non-finite matrix entries.");
// Check for non-finite values
for(size_t i=0; i<(size_t) matrix_.rows(); ++i)
for(size_t j=i; j<(size_t) matrix_.cols(); ++j)
if(!isfinite(matrix_(i,j)))
throw invalid_argument("HessianFactor contains non-finite matrix entries.");
#endif
}
/* ************************************************************************* */
HessianFactor::HessianFactor(const HessianFactor& gf) :
GaussianFactor(gf), info_(matrix_) {
info_.assignNoalias(gf.info_);
assertInvariants();
}
/* ************************************************************************* */
HessianFactor::HessianFactor() : info_(matrix_) {
assertInvariants();
}
/* ************************************************************************* */
HessianFactor::HessianFactor(Index j1, const Matrix& G, const Vector& g, double f) :
GaussianFactor(j1), info_(matrix_) {
if(G.size1() != G.size2() || G.size1() != g.size())
throw invalid_argument("Inconsistent matrix and/or vector dimensions in HessianFactor constructor");
size_t dims[] = { G.size1(), 1 };
InfoMatrix fullMatrix(G.size1() + 1, G.size1() + 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(Index j1, Index 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.size1() != G11.size2() || G11.size1() != G12.size1() || G11.size1() != g1.size() ||
G22.size2() != G12.size2() || G22.size2() != g2.size())
throw invalid_argument("Inconsistent matrix and/or vector dimensions in HessianFactor constructor");
size_t dims[] = { G11.size1(), G22.size1(), 1 };
InfoMatrix fullMatrix(G11.size1() + G22.size1() + 1, G11.size1() + G22.size1() + 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();
}
/* ************************************************************************* */
HessianFactor::HessianFactor(const GaussianConditional& cg) : GaussianFactor(cg), info_(matrix_) {
JacobianFactor jf(cg);
info_.copyStructureFrom(jf.Ab_);
ublas::noalias(ublas::symmetric_adaptor<MatrixColMajor,ublas::upper>(matrix_)) =
ublas::prod(ublas::trans(jf.matrix_), jf.matrix_);
assertInvariants();
}
/* ************************************************************************* */
HessianFactor::HessianFactor(const GaussianFactor& gf) : info_(matrix_) {
// Copy the variable indices
(GaussianFactor&)(*this) = gf;
// Copy the matrix data depending on what type of factor we're copying from
if(dynamic_cast<const JacobianFactor*>(&gf)) {
const JacobianFactor& jf(static_cast<const JacobianFactor&>(gf));
if(jf.model_->isConstrained())
throw invalid_argument("Cannot construct HessianFactor from JacobianFactor with constrained noise model");
else {
Vector invsigmas = jf.model_->invsigmas();
typedef Eigen::Map<const Eigen::MatrixXd> ConstEigenMap;
typedef Eigen::Map<Eigen::MatrixXd> EigenMap;
typedef typeof(ConstEigenMap(&jf.matrix_(0,0),0,0).block(0,0,0,0)) EigenBlock;
EigenBlock A(ConstEigenMap(&jf.matrix_(0,0),jf.matrix_.size1(),jf.matrix_.size2()).block(
jf.Ab_.rowStart(),jf.Ab_.offset(0), jf.Ab_.full().size1(), jf.Ab_.full().size2()));
typedef typeof(Eigen::Map<const Eigen::VectorXd>(&invsigmas(0),0).asDiagonal()) EigenDiagonal;
EigenDiagonal R(Eigen::Map<const Eigen::VectorXd>(&invsigmas(0),jf.model_->dim()).asDiagonal());
info_.copyStructureFrom(jf.Ab_);
EigenMap L(EigenMap(&matrix_(0,0), matrix_.size1(), matrix_.size2()));
L.noalias() = A.transpose() * R * R * A;
}
} else if(dynamic_cast<const HessianFactor*>(&gf)) {
const HessianFactor& hf(static_cast<const HessianFactor&>(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<GaussianFactor>& factors,
const vector<size_t>& dimensions, const Scatter& scatter) :
info_(matrix_) {
const bool debug = ISDEBUG("EliminateCholesky");
// Form Ab' * Ab
tic(1, "allocate");
info_.resize(dimensions.begin(), dimensions.end(), false);
toc(1, "allocate");
tic(2, "zero");
ublas::noalias(matrix_) = ublas::zero_matrix<double>(matrix_.size1(),matrix_.size2());
toc(2, "zero");
tic(3, "update");
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors)
{
shared_ptr hessian(boost::dynamic_pointer_cast<HessianFactor>(factor));
if (hessian)
updateATA(*hessian, scatter);
else {
JacobianFactor::shared_ptr jacobianFactor(boost::dynamic_pointer_cast<JacobianFactor>(factor));
if (jacobianFactor)
updateATA(*jacobianFactor, scatter);
else
throw invalid_argument("GaussianFactor is neither Hessian nor Jacobian");
}
}
toc(3, "update");
if (debug) gtsam::print(matrix_, "Ab' * Ab: ");
assertInvariants();
}
/* ************************************************************************* */
void HessianFactor::print(const std::string& s) const {
cout << s << "\n";
cout << " keys: ";
for(const_iterator key=this->begin(); key!=this->end(); ++key)
cout << *key << "(" << this->getDim(key) << ") ";
cout << "\n";
gtsam::print(ublas::symmetric_adaptor<const constBlock,ublas::upper>(
info_.range(0,info_.nBlocks(), 0,info_.nBlocks())), "Ab^T * Ab: ");
}
/* ************************************************************************* */
bool HessianFactor::equals(const GaussianFactor& lf, double tol) const {
if(!dynamic_cast<const HessianFactor*>(&lf))
return false;
else {
MatrixColMajor thisMatrix = ublas::symmetric_adaptor<const MatrixColMajor,ublas::upper>(this->info_.full());
thisMatrix(thisMatrix.size1()-1, thisMatrix.size2()-1) = 0.0;
MatrixColMajor rhsMatrix = ublas::symmetric_adaptor<const MatrixColMajor,ublas::upper>(static_cast<const HessianFactor&>(lf).info_.full());
rhsMatrix(rhsMatrix.size1()-1, rhsMatrix.size2()-1) = 0.0;
return equal_with_abs_tol(thisMatrix, rhsMatrix, tol);
}
}
/* ************************************************************************* */
double HessianFactor::error(const VectorValues& c) const {
return 0.5 * (ublas::inner_prod(c.vector(),
ublas::prod(
ublas::symmetric_adaptor<const constBlock,ublas::upper>(info_.range(0, this->size(), 0, this->size())),
c.vector())) -
2.0*ublas::inner_prod(c.vector(), info_.rangeColumn(0, this->size(), this->size(), 0)) +
info_(this->size(), this->size())(0,0));
}
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");
// First build an array of slots
tic(1, "slots");
size_t slots[update.size()];
size_t slot = 0;
BOOST_FOREACH(Index j, update) {
slots[slot] = scatter.find(j)->second.slot;
++ slot;
}
toc(1, "slots");
if(debug) {
this->print("Updating this: ");
update.print("with: ");
}
Eigen::Map<Eigen::MatrixXd> information(&matrix_(0,0), matrix_.size1(), matrix_.size2());
Eigen::Map<const Eigen::MatrixXd> updateInform(&update.matrix_(0,0), update.matrix_.size1(), update.matrix_.size2());
// Apply updates to the upper triangle
tic(3, "update");
assert(this->info_.nBlocks() - 1 == scatter.size());
for(size_t j2=0; j2<update.info_.nBlocks(); ++j2) {
size_t slot2 = (j2 == update.size()) ? this->info_.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;
information.block(info_.offset(slot1), info_.offset(slot2), info_(slot1,slot2).size1(), info_(slot1,slot2).size2()).noalias() +=
updateInform.block(update.info_.offset(j1), update.info_.offset(j2), update.info_(j1,j2).size1(), update.info_(j1,j2).size2());
} else if(slot1 > slot2) {
if(debug)
cout << "Updating (" << slot2 << "," << slot1 << ") from (" << j1 << "," << j2 << ")" << endl;
information.block(info_.offset(slot2), info_.offset(slot1), info_(slot2,slot1).size1(), info_(slot2,slot1).size2()).noalias() +=
updateInform.block(update.info_.offset(j1), update.info_.offset(j2), update.info_(j1,j2).size1(), update.info_(j1,j2).size2()).transpose();
} else {
if(debug)
cout << "Updating (" << slot1 << "," << slot2 << ") from (" << j1 << "," << j2 << ")" << endl;
information.block(info_.offset(slot1), info_.offset(slot2), info_(slot1,slot2).size1(), info_(slot1,slot2).size2()).triangularView<Eigen::Upper>() +=
updateInform.block(update.info_.offset(j1), update.info_.offset(j2), update.info_(j1,j2).size1(), update.info_(j1,j2).size2());
}
if(debug) cout << "Updating block " << slot1 << "," << slot2 << " from block " << j1 << "," << j2 << "\n";
if(debug) this->print();
}
}
toc(3, "update");
}
/* ************************************************************************* */
HessianFactor::HessianFactor(const HessianFactor& gf) :
GaussianFactor(gf), info_(matrix_) {
info_.assignNoalias(gf.info_);
assertInvariants();
}
/* ************************************************************************* */
HessianFactor::HessianFactor() : info_(matrix_) {
assertInvariants();
}
/* ************************************************************************* */
HessianFactor::HessianFactor(Index j1, const Matrix& G, const Vector& g, double f) :
GaussianFactor(j1), 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(Index j1, Index 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();
}
/* ************************************************************************* */
HessianFactor::HessianFactor(const GaussianConditional& cg) : GaussianFactor(cg), info_(matrix_) {
JacobianFactor jf(cg);
info_.copyStructureFrom(jf.Ab_);
// ublas::noalias(ublas::symmetric_adaptor<MatrixColMajor,ublas::upper>(matrix_)) =
// ublas::prod(ublas::trans(jf.matrix_), jf.matrix_);
matrix_.noalias() = jf.matrix_.transpose() * jf.matrix_;
assertInvariants();
}
/* ************************************************************************* */
HessianFactor::HessianFactor(const GaussianFactor& gf) : info_(matrix_) {
// Copy the variable indices
(GaussianFactor&)(*this) = gf;
// Copy the matrix data depending on what type of factor we're copying from
if(dynamic_cast<const JacobianFactor*>(&gf)) {
const JacobianFactor& jf(static_cast<const JacobianFactor&>(gf));
if(jf.model_->isConstrained())
throw invalid_argument("Cannot construct HessianFactor from JacobianFactor with constrained noise model");
else {
Vector invsigmas = jf.model_->invsigmas().cwiseProduct(jf.model_->invsigmas());
info_.copyStructureFrom(jf.Ab_);
BlockInfo::constBlock A = jf.Ab_.full();
matrix_.noalias() = A.transpose() * invsigmas.asDiagonal() * A;
// typedef Eigen::Map<Eigen::MatrixXd> EigenMap;
// typedef typeof(EigenMap(&jf.matrix_(0,0),0,0).block(0,0,0,0)) EigenBlock;
// EigenBlock A(EigenMap(&jf.matrix_(0,0),jf.matrix_.rows(),jf.matrix_.cols()).block(jf.Ab_.rowStart(),jf.Ab_.offset(0), jf.Ab_.full().rows(), jf.Ab_.full().cols()));
// typedef typeof(Eigen::Map<Eigen::VectorXd>(&invsigmas(0),0).asDiagonal()) EigenDiagonal;
// EigenDiagonal R(Eigen::Map<Eigen::VectorXd>(&invsigmas(0),jf.model_->dim()).asDiagonal());
// info_.copyStructureFrom(jf.Ab_);
// EigenMap L(EigenMap(&matrix_(0,0), matrix_.rows(), matrix_.cols()));
// L.noalias() = A.transpose() * R * R * A;
}
} else if(dynamic_cast<const HessianFactor*>(&gf)) {
const HessianFactor& hf(static_cast<const HessianFactor&>(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<GaussianFactor>& factors,
const vector<size_t>& dimensions, const Scatter& scatter) :
info_(matrix_) {
const bool debug = ISDEBUG("EliminateCholesky");
// Form Ab' * Ab
tic(1, "allocate");
info_.resize(dimensions.begin(), dimensions.end(), false);
toc(1, "allocate");
tic(2, "zero");
matrix_.noalias() = Matrix::Zero(matrix_.rows(),matrix_.cols());
toc(2, "zero");
tic(3, "update");
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors)
{
shared_ptr hessian(boost::dynamic_pointer_cast<HessianFactor>(factor));
if (hessian)
updateATA(*hessian, scatter);
else {
JacobianFactor::shared_ptr jacobianFactor(boost::dynamic_pointer_cast<JacobianFactor>(factor));
if (jacobianFactor)
updateATA(*jacobianFactor, scatter);
else
throw invalid_argument("GaussianFactor is neither Hessian nor Jacobian");
}
}
toc(3, "update");
if (debug) gtsam::print(matrix_, "Ab' * Ab: ");
assertInvariants();
}
/* ************************************************************************* */
void HessianFactor::print(const std::string& s) const {
cout << s << "\n";
cout << " keys: ";
for(const_iterator key=this->begin(); key!=this->end(); ++key)
cout << *key << "(" << this->getDim(key) << ") ";
cout << "\n";
// gtsam::print(ublas::symmetric_adaptor<const constBlock,ublas::upper>(
// info_.range(0,info_.nBlocks(), 0,info_.nBlocks())), "Ab^T * Ab: ");
gtsam::print(MatrixColMajor(info_.range(0,info_.nBlocks(), 0,info_.nBlocks()).selfadjointView<Eigen::Upper>()), "Ab^T * Ab: ");
}
/* ************************************************************************* */
bool HessianFactor::equals(const GaussianFactor& lf, double tol) const {
if(!dynamic_cast<const HessianFactor*>(&lf))
return false;
else {
MatrixColMajor thisMatrix = this->info_.full().selfadjointView<Eigen::Upper>();
// MatrixColMajor thisMatrix = ublas::symmetric_adaptor<const MatrixColMajor,ublas::upper>(this->info_.full());
thisMatrix(thisMatrix.rows()-1, thisMatrix.cols()-1) = 0.0;
MatrixColMajor rhsMatrix = static_cast<const HessianFactor&>(lf).info_.full().selfadjointView<Eigen::Upper>();
// MatrixColMajor rhsMatrix = ublas::symmetric_adaptor<const MatrixColMajor,ublas::upper>(static_cast<const HessianFactor&>(lf).info_.full());
rhsMatrix(rhsMatrix.rows()-1, rhsMatrix.cols()-1) = 0.0;
return equal_with_abs_tol(thisMatrix, rhsMatrix, tol);
}
}
/* ************************************************************************* */
double HessianFactor::constant_term() const {
return info_(this->size(), this->size())(0,0);
}
/* ************************************************************************* */
HessianFactor::constColumn HessianFactor::linear_term() const {
return info_.rangeColumn(0, this->size(), this->size(), 0);
}
/* ************************************************************************* */
double HessianFactor::error(const VectorValues& c) const {
// error 0.5*(f - 2*x'*g + x'*G*x)
// return 0.5 * (ublas::inner_prod(c.vector(),
// ublas::prod(
// ublas::symmetric_adaptor<const constBlock,ublas::upper>(info_.range(0, this->size(), 0, this->size())),
// c.vector())) -
// 2.0*ublas::inner_prod(c.vector(), info_.rangeColumn(0, this->size(), this->size(), 0)) +
// info_(this->size(), this->size())(0,0));
// double expected_manual = 0.5 * (f - 2.0 * dxv.dot(g) + dxv.transpose() * G.selfadjointView<Eigen::Upper>() * dxv);
const double f = constant_term();
const double xtg = c.vector().dot(linear_term());
const double xGx = c.vector().transpose() * info_.range(0, this->size(), 0, this->size()).selfadjointView<Eigen::Upper>() * c.vector();
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");
// First build an array of slots
tic(1, "slots");
size_t slots[update.size()];
size_t slot = 0;
BOOST_FOREACH(Index j, update) {
slots[slot] = scatter.find(j)->second.slot;
++ slot;
}
toc(1, "slots");
if(debug) {
this->print("Updating this: ");
update.print("with: ");
}
// Eigen::Map<Eigen::MatrixXd> information(&matrix_(0,0), matrix_.rows(), matrix_.cols());
// Eigen::Map<Eigen::MatrixXd> updateInform(&update.matrix_(0,0), update.matrix_.rows(), update.matrix_.cols());
// Apply updates to the upper triangle
tic(3, "update");
assert(this->info_.nBlocks() - 1 == scatter.size());
for(size_t j2=0; j2<update.info_.nBlocks(); ++j2) {
size_t slot2 = (j2 == update.size()) ? this->info_.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<Eigen::Upper>() +=
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();
}
}
toc(3, "update");
}
/* ************************************************************************* */
void HessianFactor::updateATA(const JacobianFactor& 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.
// 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");
const bool debug = ISDEBUG("updateATA");
// First build an array of slots
tic(1, "slots");
size_t slots[update.size()];
size_t slot = 0;
BOOST_FOREACH(Index j, update) {
slots[slot] = scatter.find(j)->second.slot;
++ slot;
}
toc(1, "slots");
// First build an array of slots
tic(1, "slots");
size_t slots[update.size()];
size_t slot = 0;
BOOST_FOREACH(Index j, update) {
slots[slot] = scatter.find(j)->second.slot;
++ slot;
}
toc(1, "slots");
tic(2, "form A^T*A");
if(update.model_->isConstrained())
throw invalid_argument("Cannot update HessianFactor from JacobianFactor with constrained noise model");
tic(2, "form A^T*A");
if(update.model_->isConstrained())
throw invalid_argument("Cannot update HessianFactor from JacobianFactor with constrained noise model");
if(debug) {
this->print("Updating this: ");
update.print("with: ");
}
if(debug) {
this->print("Updating this: ");
update.print("with: ");
}
Eigen::Map<Eigen::MatrixXd> information(&matrix_(0,0), matrix_.size1(), matrix_.size2());
Eigen::Map<const Eigen::MatrixXd> updateAf(&update.matrix_(0,0), update.matrix_.size1(), update.matrix_.size2());
Eigen::Block<typeof(updateAf)> updateA(updateAf.block(
update.Ab_.rowStart(),update.Ab_.offset(0), update.Ab_.full().size1(), update.Ab_.full().size2()));
// Eigen::Map<Eigen::MatrixXd> information(&matrix_(0,0), matrix_.rows(), matrix_.cols());
// Eigen::Map<Eigen::MatrixXd> updateAf(&update.matrix_(0,0), update.matrix_.rows(), update.matrix_.cols());
Eigen::Block<typeof(update.matrix_)> updateA(update.matrix_.block(
update.Ab_.rowStart(),update.Ab_.offset(0), update.Ab_.full().rows(), update.Ab_.full().cols()));
Eigen::MatrixXd updateInform;
if(boost::dynamic_pointer_cast<noiseModel::Unit>(update.model_)) {
updateInform.noalias() = updateA.transpose() * updateA;
} else {
noiseModel::Diagonal::shared_ptr diagonal(boost::dynamic_pointer_cast<noiseModel::Diagonal>(update.model_));
if(diagonal) {
typeof(Eigen::Map<Eigen::VectorXd>(&update.model_->invsigmas()(0),0).asDiagonal()) R(
Eigen::Map<Eigen::VectorXd>(&update.model_->invsigmas()(0),update.model_->dim()).asDiagonal());
updateInform.noalias() = updateA.transpose() * R * R * updateA;
} else
throw invalid_argument("In HessianFactor::updateATA, JacobianFactor noise model is neither Unit nor Diagonal");
}
toc(2, "form A^T*A");
Eigen::MatrixXd updateInform;
if(boost::dynamic_pointer_cast<noiseModel::Unit>(update.model_)) {
updateInform.noalias() = updateA.transpose() * updateA;
} else {
noiseModel::Diagonal::shared_ptr diagonal(boost::dynamic_pointer_cast<noiseModel::Diagonal>(update.model_));
if(diagonal) {
typeof(Eigen::Map<Eigen::VectorXd>(&update.model_->invsigmas()(0),0).asDiagonal()) R(
Eigen::Map<Eigen::VectorXd>(&update.model_->invsigmas()(0),update.model_->dim()).asDiagonal());
updateInform.noalias() = updateA.transpose() * R * R * updateA;
} else
throw invalid_argument("In HessianFactor::updateATA, JacobianFactor noise model is neither Unit nor Diagonal");
}
toc(2, "form A^T*A");
// Apply updates to the upper triangle
tic(3, "update");
assert(this->info_.nBlocks() - 1 == scatter.size());
for(size_t j2=0; j2<update.Ab_.nBlocks(); ++j2) {
size_t slot2 = (j2 == update.size()) ? this->info_.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;
information.block(info_.offset(slot1), info_.offset(slot2), info_(slot1,slot2).size1(), info_(slot1,slot2).size2()).noalias() +=
updateInform.block(update.Ab_.offset(j1)-off0, update.Ab_.offset(j2)-off0, update.Ab_(j1).size2(), update.Ab_(j2).size2());
} else if(slot1 > slot2) {
if(debug)
cout << "Updating (" << slot2 << "," << slot1 << ") from (" << j1 << "," << j2 << ")" << endl;
information.block(info_.offset(slot2), info_.offset(slot1), info_(slot2,slot1).size1(), info_(slot2,slot1).size2()).noalias() +=
updateInform.block(update.Ab_.offset(j1)-off0, update.Ab_.offset(j2)-off0, update.Ab_(j1).size2(), update.Ab_(j2).size2()).transpose();
} else {
if(debug)
cout << "Updating (" << slot1 << "," << slot2 << ") from (" << j1 << "," << j2 << ")" << endl;
information.block(info_.offset(slot1), info_.offset(slot2), info_(slot1,slot2).size1(), info_(slot1,slot2).size2()).triangularView<Eigen::Upper>() +=
updateInform.block(update.Ab_.offset(j1)-off0, update.Ab_.offset(j2)-off0, update.Ab_(j1).size2(), update.Ab_(j2).size2());
}
if(debug) cout << "Updating block " << slot1 << "," << slot2 << " from block " << j1 << "," << j2 << "\n";
if(debug) this->print();
}
}
toc(3, "update");
// Apply updates to the upper triangle
tic(3, "update");
assert(this->info_.nBlocks() - 1 == scatter.size());
for(size_t j2=0; j2<update.Ab_.nBlocks(); ++j2) {
size_t slot2 = (j2 == update.size()) ? this->info_.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<Eigen::Upper>() +=
updateInform.block(update.Ab_.offset(j1)-off0, update.Ab_.offset(j2)-off0, update.Ab_(j1).cols(), update.Ab_(j2).cols());
}
if(debug) cout << "Updating block " << slot1 << "," << slot2 << " from block " << j1 << "," << j2 << "\n";
if(debug) this->print();
}
}
toc(3, "update");
// Eigen::Map<Eigen::MatrixXd> information(&matrix_(0,0), matrix_.size1(), matrix_.size2());
// Eigen::Map<Eigen::MatrixXd> updateA(&update.matrix_(0,0), update.matrix_.size1(), update.matrix_.size2());
//
// // Apply updates to the upper triangle
// tic(2, "update");
// assert(this->info_.nBlocks() - 1 == scatter.size());
// for(size_t j2=0; j2<update.Ab_.nBlocks(); ++j2) {
// size_t slot2 = (j2 == update.size()) ? this->info_.nBlocks()-1 : slots[j2];
// for(size_t j1=0; j1<=j2; ++j1) {
// size_t slot1 = (j1 == update.size()) ? this->info_.nBlocks()-1 : slots[j1];
// typedef typeof(updateA.block(0,0,0,0)) ABlock;
// ABlock A1(updateA.block(update.Ab_.rowStart(),update.Ab_.offset(j1), update.Ab_(j1).size1(),update.Ab_(j1).size2()));
// ABlock A2(updateA.block(update.Ab_.rowStart(),update.Ab_.offset(j2), update.Ab_(j2).size1(),update.Ab_(j2).size2()));
// if(slot2 > slot1) {
// if(debug)
// cout << "Updating (" << slot1 << "," << slot2 << ") from (" << j1 << "," << j2 << ")" << endl;
// if(boost::dynamic_pointer_cast<noiseModel::Unit>(update.model_)) {
// information.block(info_.offset(slot1), info_.offset(slot2), info_(slot1,slot2).size1(), info_(slot1,slot2).size2()) +=
// A1.transpose() * A2;
// } else {
// noiseModel::Diagonal::shared_ptr diagonal(boost::dynamic_pointer_cast<noiseModel::Diagonal>(update.model_));
// if(diagonal) {
// typeof(Eigen::Map<Eigen::VectorXd>(&update.model_->invsigmas()(0),0).asDiagonal()) R(
// Eigen::Map<Eigen::VectorXd>(&update.model_->invsigmas()(0),update.model_->dim()).asDiagonal());
// information.block(info_.offset(slot1), info_.offset(slot2), info_(slot1,slot2).size1(), info_(slot1,slot2).size2()).noalias() +=
// A1.transpose() * R * R * A2;
// } else
// throw invalid_argument("In HessianFactor::updateATA, JacobianFactor noise model is neither Unit nor Diagonal");
// }
// } else if(slot1 > slot2) {
// if(debug)
// cout << "Updating (" << slot2 << "," << slot1 << ") from (" << j1 << "," << j2 << ")" << endl;
// if(boost::dynamic_pointer_cast<noiseModel::Unit>(update.model_)) {
// information.block(info_.offset(slot2), info_.offset(slot1), info_(slot2,slot1).size1(), info_(slot2,slot1).size2()).noalias() +=
// A2.transpose() * A1;
// } else {
// noiseModel::Diagonal::shared_ptr diagonal(boost::dynamic_pointer_cast<noiseModel::Diagonal>(update.model_));
// if(diagonal) {
// typeof(Eigen::Map<Eigen::VectorXd>(&update.model_->invsigmas()(0),0).asDiagonal()) R(
// Eigen::Map<Eigen::VectorXd>(&update.model_->invsigmas()(0),update.model_->dim()).asDiagonal());
// information.block(info_.offset(slot1), info_.offset(slot2), info_(slot1,slot2).size1(), info_(slot1,slot2).size2()).noalias() +=
// A2.transpose() * R * R * A1;
// } else
// throw invalid_argument("In HessianFactor::updateATA, JacobianFactor noise model is neither Unit nor Diagonal");
// }
// } else {
// if(debug)
// cout << "Updating (" << slot1 << "," << slot2 << ") from (" << j1 << "," << j2 << ")" << endl;
// if(boost::dynamic_pointer_cast<noiseModel::Unit>(update.model_)) {
// information.block(info_.offset(slot2), info_.offset(slot1), info_(slot2,slot1).size1(), info_(slot2,slot1).size2()).triangularView<Eigen::Upper>() +=
// A1.transpose() * A1;
// } else {
// noiseModel::Diagonal::shared_ptr diagonal(boost::dynamic_pointer_cast<noiseModel::Diagonal>(update.model_));
// if(diagonal) {
// typeof(Eigen::Map<Eigen::VectorXd>(&update.model_->invsigmas()(0),0).asDiagonal()) R(
// Eigen::Map<Eigen::VectorXd>(&update.model_->invsigmas()(0),update.model_->dim()).asDiagonal());
// information.block(info_.offset(slot2), info_.offset(slot1), info_(slot2,slot1).size1(), info_(slot2,slot1).size2()).triangularView<Eigen::Upper>() +=
// A1.transpose() * R * R * A1;
// } else
// throw invalid_argument("In HessianFactor::updateATA, JacobianFactor noise model is neither Unit nor Diagonal");
// }
// }
// if(debug) cout << "Updating block " << slot1 << "," << slot2 << " from block " << j1 << "," << j2 << "\n";
// if(debug) this->print();
// }
// }
// toc(2, "update");
// Eigen::Map<Eigen::MatrixXd> information(&matrix_(0,0), matrix_.rows(), matrix_.cols());
// Eigen::Map<Eigen::MatrixXd> updateA(&update.matrix_(0,0), update.matrix_.rows(), update.matrix_.cols());
//
// // Apply updates to the upper triangle
// tic(2, "update");
// assert(this->info_.nBlocks() - 1 == scatter.size());
// for(size_t j2=0; j2<update.Ab_.nBlocks(); ++j2) {
// size_t slot2 = (j2 == update.size()) ? this->info_.nBlocks()-1 : slots[j2];
// for(size_t j1=0; j1<=j2; ++j1) {
// size_t slot1 = (j1 == update.size()) ? this->info_.nBlocks()-1 : slots[j1];
// typedef typeof(updateA.block(0,0,0,0)) ABlock;
// ABlock A1(updateA.block(update.Ab_.rowStart(),update.Ab_.offset(j1), update.Ab_(j1).rows(),update.Ab_(j1).cols()));
// ABlock A2(updateA.block(update.Ab_.rowStart(),update.Ab_.offset(j2), update.Ab_(j2).rows(),update.Ab_(j2).cols()));
// if(slot2 > slot1) {
// if(debug)
// cout << "Updating (" << slot1 << "," << slot2 << ") from (" << j1 << "," << j2 << ")" << endl;
// if(boost::dynamic_pointer_cast<noiseModel::Unit>(update.model_)) {
// information.block(info_.offset(slot1), info_.offset(slot2), info_(slot1,slot2).rows(), info_(slot1,slot2).cols()) +=
// A1.transpose() * A2;
// } else {
// noiseModel::Diagonal::shared_ptr diagonal(boost::dynamic_pointer_cast<noiseModel::Diagonal>(update.model_));
// if(diagonal) {
// typeof(Eigen::Map<Eigen::VectorXd>(&update.model_->invsigmas()(0),0).asDiagonal()) R(
// Eigen::Map<Eigen::VectorXd>(&update.model_->invsigmas()(0),update.model_->dim()).asDiagonal());
// information.block(info_.offset(slot1), info_.offset(slot2), info_(slot1,slot2).rows(), info_(slot1,slot2).cols()).noalias() +=
// A1.transpose() * R * R * A2;
// } else
// throw invalid_argument("In HessianFactor::updateATA, JacobianFactor noise model is neither Unit nor Diagonal");
// }
// } else if(slot1 > slot2) {
// if(debug)
// cout << "Updating (" << slot2 << "," << slot1 << ") from (" << j1 << "," << j2 << ")" << endl;
// if(boost::dynamic_pointer_cast<noiseModel::Unit>(update.model_)) {
// information.block(info_.offset(slot2), info_.offset(slot1), info_(slot2,slot1).rows(), info_(slot2,slot1).cols()).noalias() +=
// A2.transpose() * A1;
// } else {
// noiseModel::Diagonal::shared_ptr diagonal(boost::dynamic_pointer_cast<noiseModel::Diagonal>(update.model_));
// if(diagonal) {
// typeof(Eigen::Map<Eigen::VectorXd>(&update.model_->invsigmas()(0),0).asDiagonal()) R(
// Eigen::Map<Eigen::VectorXd>(&update.model_->invsigmas()(0),update.model_->dim()).asDiagonal());
// information.block(info_.offset(slot1), info_.offset(slot2), info_(slot1,slot2).rows(), info_(slot1,slot2).cols()).noalias() +=
// A2.transpose() * R * R * A1;
// } else
// throw invalid_argument("In HessianFactor::updateATA, JacobianFactor noise model is neither Unit nor Diagonal");
// }
// } else {
// if(debug)
// cout << "Updating (" << slot1 << "," << slot2 << ") from (" << j1 << "," << j2 << ")" << endl;
// if(boost::dynamic_pointer_cast<noiseModel::Unit>(update.model_)) {
// information.block(info_.offset(slot2), info_.offset(slot1), info_(slot2,slot1).rows(), info_(slot2,slot1).cols()).triangularView<Eigen::Upper>() +=
// A1.transpose() * A1;
// } else {
// noiseModel::Diagonal::shared_ptr diagonal(boost::dynamic_pointer_cast<noiseModel::Diagonal>(update.model_));
// if(diagonal) {
// typeof(Eigen::Map<Eigen::VectorXd>(&update.model_->invsigmas()(0),0).asDiagonal()) R(
// Eigen::Map<Eigen::VectorXd>(&update.model_->invsigmas()(0),update.model_->dim()).asDiagonal());
// information.block(info_.offset(slot2), info_.offset(slot1), info_(slot2,slot1).rows(), info_(slot2,slot1).cols()).triangularView<Eigen::Upper>() +=
// A1.transpose() * R * R * A1;
// } else
// throw invalid_argument("In HessianFactor::updateATA, JacobianFactor noise model is neither Unit nor Diagonal");
// }
// }
// if(debug) cout << "Updating block " << slot1 << "," << slot2 << " from block " << j1 << "," << j2 << "\n";
// if(debug) this->print();
// }
// }
// toc(2, "update");
}
/* ************************************************************************* */
@ -429,51 +445,52 @@ void HessianFactor::partialCholesky(size_t nrFrontals) {
GaussianBayesNet::shared_ptr
HessianFactor::splitEliminatedFactor(size_t nrFrontals, const vector<Index>& keys) {
static const bool debug = false;
static const bool debug = false;
// Extract conditionals
tic(1, "extract conditionals");
GaussianBayesNet::shared_ptr conditionals(new GaussianBayesNet());
typedef VerticalBlockView<MatrixColMajor> BlockAb;
BlockAb Ab(matrix_, info_);
for(size_t j=0; j<nrFrontals; ++j) {
// Temporarily restrict the matrix view to the conditional blocks of the
// eliminated Ab_ matrix to create the GaussianConditional from it.
size_t varDim = Ab(0).size2();
Ab.rowEnd() = Ab.rowStart() + varDim;
// Extract conditionals
tic(1, "extract conditionals");
GaussianBayesNet::shared_ptr conditionals(new GaussianBayesNet());
typedef VerticalBlockView<MatrixColMajor> BlockAb;
BlockAb Ab(matrix_, info_);
for(size_t j=0; j<nrFrontals; ++j) {
// Temporarily restrict the matrix view to the conditional blocks of the
// eliminated Ab_ matrix to create the GaussianConditional from it.
size_t varDim = Ab(0).cols();
Ab.rowEnd() = Ab.rowStart() + varDim;
// Zero the entries below the diagonal (this relies on the matrix being
// column-major).
{
tic(1, "zero");
BlockAb::Block remainingMatrix(Ab.range(0, Ab.nBlocks()));
if(remainingMatrix.size1() > 1)
for(size_t j = 0; j < remainingMatrix.size1() - 1; ++j) {
ublas::matrix_column<BlockAb::Block> col(ublas::column(remainingMatrix, j));
std::fill(col.begin() + j+1, col.end(), 0.0);
}
toc(1, "zero");
}
// Zero the entries below the diagonal
{
tic(1, "zero");
BlockAb::Block remainingMatrix(Ab.range(0, Ab.nBlocks()));
zeroBelowDiagonal(remainingMatrix);
// for(size_t j = 0; j < (size_t) remainingMatrix.rows() - 1; ++j) {
// remainingMatrix.col(j).tail(remainingMatrix.rows() - (j+1)).setZero();
// ublas::matrix_column<BlockAb::Block> col(ublas::column(remainingMatrix, j));
// std::fill(col.begin() + j+1, col.end(), 0.0);
// }
toc(1, "zero");
}
tic(2, "construct cond");
const ublas::scalar_vector<double> sigmas(varDim, 1.0);
conditionals->push_back(boost::make_shared<ConditionalType>(keys.begin()+j, keys.end(), 1, Ab, sigmas));
toc(2, "construct cond");
if(debug) conditionals->back()->print("Extracted conditional: ");
Ab.rowStart() += varDim;
Ab.firstBlock() += 1;
if(debug) cout << "rowStart = " << Ab.rowStart() << ", rowEnd = " << Ab.rowEnd() << endl;
}
toc(1, "extract conditionals");
tic(2, "construct cond");
// const ublas::scalar_vector<double> sigmas(varDim, 1.0);
Vector sigmas = Vector::Ones(varDim);
conditionals->push_back(boost::make_shared<ConditionalType>(keys.begin()+j, keys.end(), 1, Ab, sigmas));
toc(2, "construct cond");
if(debug) conditionals->back()->print("Extracted conditional: ");
Ab.rowStart() += varDim;
Ab.firstBlock() += 1;
if(debug) cout << "rowStart = " << Ab.rowStart() << ", rowEnd = " << Ab.rowEnd() << endl;
}
toc(1, "extract conditionals");
// Take lower-right block of Ab_ to get the new factor
tic(2, "remaining factor");
info_.blockStart() = nrFrontals;
// Assign the keys
keys_.assign(keys.begin() + nrFrontals, keys.end());
toc(2, "remaining factor");
// Take lower-right block of Ab_ to get the new factor
tic(2, "remaining factor");
info_.blockStart() = nrFrontals;
// Assign the keys
keys_.assign(keys.begin() + nrFrontals, keys.end());
toc(2, "remaining factor");
return conditionals;
return conditionals;
}
} // gtsam

20
gtsam/linear/HessianFactor.h
View File

@ -58,6 +58,8 @@ namespace gtsam {
typedef boost::shared_ptr<HessianFactor> shared_ptr;
typedef BlockInfo::Block Block;
typedef BlockInfo::constBlock constBlock;
typedef BlockInfo::Column Column;
typedef BlockInfo::constColumn constColumn;
/** Copy constructor */
HessianFactor(const HessianFactor& gf);
@ -91,8 +93,7 @@ namespace gtsam {
HessianFactor(const FactorGraph<GaussianFactor>& factors,
const std::vector<size_t>& dimensions, const Scatter& scatter);
virtual ~HessianFactor() {
}
virtual ~HessianFactor() {}
// Implementing Testable interface
virtual void print(const std::string& s = "") const;
@ -103,14 +104,25 @@ namespace gtsam {
/** 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 info_(variable-this->begin(), 0).size1(); }
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 size1() const { return info_.size1(); }
size_t rows() const { return info_.rows(); }
/** Return a view of a block of the information matrix */
constBlock info(const_iterator j1, const_iterator j2) const { return info_(j1-begin(), j2-begin()); }
/** returns the constant term - f from the constructors */
double constant_term() const;
/** returns the full linear term - g from the constructors */
constColumn linear_term() const;
/** const access to the full matrix DEBUG ONLY*/
const InfoMatrix& raw_matrix() const { return matrix_; }
const BlockInfo& raw_info() const { return info_; } /// DEBUG ONLY
/**
* Permutes the GaussianFactor, but for efficiency requires the permutation
* to already be inverted. This acts just as a change-of-name for each

162
gtsam/linear/JacobianFactor.cpp
View File

@ -34,17 +34,10 @@
#include <boost/lambda/bind.hpp>
#include <boost/lambda/lambda.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/blas.hpp>
#include <sstream>
#include <stdexcept>
using namespace std;
namespace ublas = boost::numeric::ublas;
using namespace boost::lambda;
namespace gtsam {
@ -53,15 +46,15 @@ namespace gtsam {
inline void JacobianFactor::assertInvariants() const {
#ifndef NDEBUG
IndexFactor::assertInvariants(); // The base class checks for sorted keys
assert((size() == 0 && Ab_.size1() == 0 && Ab_.nBlocks() == 0) || size()+1 == Ab_.nBlocks());
assert(firstNonzeroBlocks_.size() == Ab_.size1());
assert((size() == 0 && Ab_.rows() == 0 && Ab_.nBlocks() == 0) || size()+1 == Ab_.nBlocks());
assert(firstNonzeroBlocks_.size() == Ab_.rows());
for(size_t i=0; i<firstNonzeroBlocks_.size(); ++i)
assert(firstNonzeroBlocks_[i] < Ab_.nBlocks());
#endif
// Check for non-finite values
for(size_t i=0; i<Ab_.size1(); ++i)
for(size_t j=0; j<Ab_.size2(); ++j)
for(size_t i=0; i<Ab_.rows(); ++i)
for(size_t j=0; j<Ab_.cols(); ++j)
if(isnan(matrix_(i,j)))
throw invalid_argument("JacobianFactor contains NaN matrix entries.");
}
@ -88,7 +81,7 @@ namespace gtsam {
JacobianFactor::JacobianFactor(Index i1, const Matrix& A1,
const Vector& b, const SharedDiagonal& model) :
GaussianFactor(i1), model_(model), firstNonzeroBlocks_(b.size(), 0), Ab_(matrix_) {
size_t dims[] = { A1.size2(), 1};
size_t dims[] = { A1.cols(), 1};
Ab_.copyStructureFrom(BlockAb(matrix_, dims, dims+2, b.size()));
Ab_(0) = A1;
getb() = b;
@ -99,7 +92,7 @@ namespace gtsam {
JacobianFactor::JacobianFactor(Index i1, const Matrix& A1, Index i2, const Matrix& A2,
const Vector& b, const SharedDiagonal& model) :
GaussianFactor(i1,i2), model_(model), firstNonzeroBlocks_(b.size(), 0), Ab_(matrix_) {
size_t dims[] = { A1.size2(), A2.size2(), 1};
size_t dims[] = { A1.cols(), A2.cols(), 1};
Ab_.copyStructureFrom(BlockAb(matrix_, dims, dims+3, b.size()));
Ab_(0) = A1;
Ab_(1) = A2;
@ -111,7 +104,7 @@ namespace gtsam {
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), firstNonzeroBlocks_(b.size(), 0), Ab_(matrix_) {
size_t dims[] = { A1.size2(), A2.size2(), A3.size2(), 1};
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;
@ -128,7 +121,7 @@ namespace gtsam {
{
size_t dims[terms.size()+1];
for(size_t j=0; j<terms.size(); ++j)
dims[j] = terms[j].second.size2();
dims[j] = terms[j].second.cols();
dims[terms.size()] = 1;
Ab_.copyStructureFrom(BlockAb(matrix_, dims, dims+terms.size()+1, b.size()));
for(size_t j=0; j<terms.size(); ++j)
@ -147,7 +140,7 @@ namespace gtsam {
size_t j=0;
std::list<std::pair<Index, Matrix> >::const_iterator term=terms.begin();
for(; term!=terms.end(); ++term,++j)
dims[j] = term->second.size2();
dims[j] = term->second.cols();
dims[j] = 1;
Ab_.copyStructureFrom(BlockAb(matrix_, dims, dims+terms.size()+1, b.size()));
j = 0;
@ -170,11 +163,12 @@ namespace gtsam {
keys_ = factor.keys_;
Ab_.assignNoalias(factor.info_);
size_t maxrank = choleskyCareful(matrix_).first;
matrix_ = ublas::triangular_adaptor<AbMatrix, ublas::upper>(matrix_);
// FIXME: replace with triangular system
// matrix_ = ublas::triangular_adaptor<AbMatrix, ublas::upper>(matrix_);
Ab_.rowEnd() = maxrank;
model_ = noiseModel::Unit::Create(maxrank);
firstNonzeroBlocks_.resize(this->size1(), 0);
firstNonzeroBlocks_.resize(this->rows(), 0);
// Sort keys
set<Index> vars;
@ -203,8 +197,9 @@ namespace gtsam {
cout << endl;
} else {
for(const_iterator key=begin(); key!=end(); ++key)
gtsam::print(getA(key), (boost::format("A[%1%]=\n")%*key).str());
gtsam::print(getb(),"b=");
cout << boost::format("A[%1%]=\n")%*key << getA(key) << endl;
// gtsam::print(getA(key), (boost::format("A[%1%]=\n")%*key).str());
cout << "b=" << getb() << endl;
model_->print("model");
}
}
@ -220,13 +215,14 @@ namespace gtsam {
if(keys()!=f.keys() /*|| !model_->equals(lf->model_, tol)*/)
return false;
assert(Ab_.size1() == f.Ab_.size1() && Ab_.size2() == f.Ab_.size2());
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<Ab1.size1(); ++row)
if(!equal_with_abs_tol(ublas::row(Ab1, row), ublas::row(Ab2, row), tol) &&
!equal_with_abs_tol(-ublas::row(Ab1, row), ublas::row(Ab2, row), tol))
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;
@ -246,7 +242,7 @@ namespace gtsam {
vector<size_t> dimensions(size() + 1);
size_t j = 0;
BOOST_FOREACH(const SourceSlots::value_type& sourceSlot, sourceSlots) {
dimensions[j++] = Ab_(sourceSlot.second).size2();
dimensions[j++] = Ab_(sourceSlot.second).cols();
}
assert(j == size());
dimensions.back() = 1;
@ -255,14 +251,16 @@ namespace gtsam {
vector<Index> oldKeys(size());
keys_.swap(oldKeys);
AbMatrix oldMatrix;
BlockAb oldAb(oldMatrix, dimensions.begin(), dimensions.end(), Ab_.size1());
BlockAb oldAb(oldMatrix, dimensions.begin(), dimensions.end(), Ab_.rows());
Ab_.swap(oldAb);
j = 0;
BOOST_FOREACH(const SourceSlots::value_type& sourceSlot, sourceSlots) {
keys_[j] = sourceSlot.first;
ublas::noalias(Ab_(j++)) = oldAb(sourceSlot.second);
Ab_(j++) = oldAb(sourceSlot.second);
// ublas::noalias(Ab_(j++)) = oldAb(sourceSlot.second);
}
ublas::noalias(Ab_(j)) = oldAb(j);
Ab_(j) = oldAb(j);
// ublas::noalias(Ab_(j)) = oldAb(j);
// Since we're permuting the variables, ensure that entire rows from this
// factor are copied when Combine is called
@ -275,7 +273,7 @@ namespace gtsam {
Vector e = -getb();
if (empty()) return e;
for(size_t pos=0; pos<size(); ++pos)
e += ublas::prod(Ab_(pos), c[keys_[pos]]);
e += Ab_(pos) * c[keys_[pos]];
return e;
}
@ -289,18 +287,18 @@ namespace gtsam {
double JacobianFactor::error(const VectorValues& c) const {
if (empty()) return 0;
Vector weighted = error_vector(c);
return 0.5 * inner_prod(weighted,weighted);
return 0.5 * weighted.dot(weighted);
}
/* ************************************************************************* */
Vector JacobianFactor::operator*(const VectorValues& x) const {
Vector Ax = zero(Ab_.size1());
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 += ublas::prod(Ab_(pos), x[keys_[pos]]);
Ax += Ab_(pos) * x[keys_[pos]];
return model_->whiten(Ax);
}
@ -341,14 +339,14 @@ namespace gtsam {
Matrix whitenedA(model_->Whiten(getA(var)));
// find first column index for this key
size_t column_start = columnIndices[*var];
for (size_t i = 0; i < whitenedA.size1(); i++)
for (size_t j = 0; j < whitenedA.size2(); j++)
for (size_t i = 0; i < (size_t) whitenedA.rows(); i++)
for (size_t j = 0; j < (size_t) whitenedA.cols(); j++)
entries.push_back(boost::make_tuple(i, column_start+j, whitenedA(i,j)));
}
Vector whitenedb(model_->whiten(getb()));
size_t bcolumn = columnIndices.back();
for (size_t i = 0; i < whitenedb.size(); i++)
for (size_t i = 0; i < (size_t) whitenedb.size(); i++)
entries.push_back(boost::make_tuple(i, bcolumn, whitenedb(i)));
// return the result
@ -371,7 +369,7 @@ namespace gtsam {
/* ************************************************************************* */
GaussianBayesNet::shared_ptr JacobianFactor::eliminate(size_t nrFrontals) {
assert(Ab_.rowStart() == 0 && Ab_.rowEnd() == matrix_.size1() && Ab_.firstBlock() == 0);
assert(Ab_.rowStart() == 0 && Ab_.rowEnd() == (size_t) matrix_.rows() && Ab_.firstBlock() == 0);
assert(size() >= nrFrontals);
assertInvariants();
@ -380,50 +378,49 @@ namespace gtsam {
if(debug) cout << "Eliminating " << nrFrontals << " frontal variables" << endl;
if(debug) this->print("Eliminating JacobianFactor: ");
tic(1, "stairs");
// Translate the left-most nonzero column indices into top-most zero row indices
vector<int> firstZeroRows(Ab_.size2());
{
size_t lastNonzeroRow = 0;
vector<int>::iterator firstZeroRowsIt = firstZeroRows.begin();
for(size_t var=0; var<keys().size(); ++var) {
while(lastNonzeroRow < this->size1() && firstNonzeroBlocks_[lastNonzeroRow] <= var)
++ lastNonzeroRow;
fill(firstZeroRowsIt, firstZeroRowsIt+Ab_(var).size2(), lastNonzeroRow);
firstZeroRowsIt += Ab_(var).size2();
}
assert(firstZeroRowsIt+1 == firstZeroRows.end());
*firstZeroRowsIt = this->size1();
}
toc(1, "stairs");
// tic(1, "stairs");
// // Translate the left-most nonzero column indices into top-most zero row indices
// vector<int> firstZeroRows(Ab_.cols());
// {
// size_t lastNonzeroRow = 0;
// vector<int>::iterator firstZeroRowsIt = firstZeroRows.begin();
// for(size_t var=0; var<keys().size(); ++var) {
// while(lastNonzeroRow < this->rows() && firstNonzeroBlocks_[lastNonzeroRow] <= var)
// ++ lastNonzeroRow;
// fill(firstZeroRowsIt, firstZeroRowsIt+Ab_(var).cols(), lastNonzeroRow);
// firstZeroRowsIt += Ab_(var).cols();
// }
// assert(firstZeroRowsIt+1 == firstZeroRows.end());
// *firstZeroRowsIt = this->rows();
// }
// toc(1, "stairs");
#ifndef NDEBUG
for(size_t col=0; col<Ab_.size2(); ++col) {
if(debug) cout << "Staircase[" << col << "] = " << firstZeroRows[col] << endl;
if(col != 0) assert(firstZeroRows[col] >= firstZeroRows[col-1]);
assert(firstZeroRows[col] <= (long)this->size1());
}
#endif
// #ifndef NDEBUG
// for(size_t col=0; col<Ab_.cols(); ++col) {
// if(debug) cout << "Staircase[" << col << "] = " << firstZeroRows[col] << endl;
// if(col != 0) assert(firstZeroRows[col] >= firstZeroRows[col-1]);
// assert(firstZeroRows[col] <= (long)this->rows());
// }
// #endif
if(debug) gtsam::print(matrix_, "Augmented Ab: ");
size_t frontalDim = Ab_.range(0,nrFrontals).size2();
size_t frontalDim = Ab_.range(0,nrFrontals).cols();
if(debug) cout << "frontalDim = " << frontalDim << endl;
// Use in-place QR or Cholesky on dense Ab appropriate to NoiseModel
tic(2, "QR");
SharedDiagonal noiseModel = model_->QRColumnWise(matrix_, firstZeroRows);
SharedDiagonal noiseModel = model_->QR(matrix_);
toc(2, "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_.size1() > 0) {
for(size_t j=0; j<matrix_.size2(); ++j)
if(matrix_.rows() > 0)
for(size_t j=0; j<(size_t) matrix_.cols(); ++j)
for(size_t i=j+1; i<noiseModel->dim(); ++i)
matrix_(i,j) = 0.0;
}
if(debug) gtsam::print(matrix_, "QR result: ");
if(debug) noiseModel->print("QR result noise model: ");
@ -441,9 +438,10 @@ namespace gtsam {
for(size_t j=0; j<nrFrontals; ++j) {
// Temporarily restrict the matrix view to the conditional blocks of the
// eliminated Ab matrix to create the GaussianConditional from it.
size_t varDim = Ab_(0).size2();
size_t varDim = Ab_(0).cols();
Ab_.rowEnd() = Ab_.rowStart() + varDim;
const ublas::vector_range<const Vector> sigmas(noiseModel->sigmas(), ublas::range(Ab_.rowStart(), Ab_.rowEnd()));
const Eigen::VectorBlock<const Vector> sigmas = noiseModel->sigmas().segment(Ab_.rowStart(), Ab_.rowEnd()-Ab_.rowStart());
// const ublas::vector_range<const Vector> sigmas(noiseModel->sigmas(), ublas::range(Ab_.rowStart(), Ab_.rowEnd()));
conditionals->push_back(boost::make_shared<ConditionalType>(begin()+j, end(), 1, Ab_, sigmas));
if(debug) conditionals->back()->print("Extracted conditional: ");
Ab_.rowStart() += varDim;
@ -463,14 +461,14 @@ namespace gtsam {
else
model_ = noiseModel::Diagonal::Sigmas(sub(noiseModel->sigmas(), frontalDim, noiseModel->dim()));
if(debug) this->print("Eliminated factor: ");
assert(Ab_.size1() <= Ab_.size2()-1);
assert(Ab_.rows() <= Ab_.cols()-1);
toc(4, "remaining factor");
// todo SL: deal with "dead" pivot columns!!!
tic(5, "rowstarts");
size_t varpos = 0;
firstNonzeroBlocks_.resize(this->size1());
for(size_t row=0; row<size1(); ++row) {
firstNonzeroBlocks_.resize(this->rows());
for(size_t row=0; row<rows(); ++row) {
if(debug) cout << "row " << row << " varpos " << varpos << " Ab_.offset(varpos)=" << Ab_.offset(varpos) << " Ab_.offset(varpos+1)=" << Ab_.offset(varpos+1) << endl;
while(varpos < this->size() && Ab_.offset(varpos+1)-Ab_.offset(0) <= row)
++ varpos;
@ -490,7 +488,7 @@ namespace gtsam {
/* ************************************************************************* */
void JacobianFactor::collectInfo(size_t index, vector<_RowSource>& rowSources) const {
assertInvariants();
for (size_t row = 0; row < size1(); ++row) {
for (size_t row = 0; row < rows(); ++row) {
Index firstNonzeroVar;
if (firstNonzeroBlocks_[row] < size()) {
firstNonzeroVar = keys_[firstNonzeroBlocks_[row]];
@ -518,18 +516,20 @@ namespace gtsam {
/* ************************************************************************* */
void JacobianFactor::copyRow(const JacobianFactor& source, Index sourceRow,
size_t sourceSlot, size_t row, Index slot) {
ABlock combinedBlock(Ab_(slot));
ABlock combinedBlock = Ab_(slot);
if (sourceSlot != numeric_limits<Index>::max()) {
if (source.firstNonzeroBlocks_[sourceRow] <= sourceSlot) {
const constABlock sourceBlock(source.Ab_(sourceSlot));
ublas::noalias(ublas::row(combinedBlock, row)) = ublas::row(
sourceBlock, sourceRow);
} else
ublas::noalias(ublas::row(combinedBlock, row)) = ublas::zero_vector<
double>(combinedBlock.size2());
} else
ublas::noalias(ublas::row(combinedBlock, row)) = ublas::zero_vector<
double>(combinedBlock.size2());
combinedBlock.row(row).noalias() = sourceBlock.row(sourceRow);
// ublas::noalias(ublas::row(combinedBlock, row)) = ublas::row(sourceBlock, sourceRow);
} else {
combinedBlock.row(row).setZero();
// ublas::noalias(ublas::row(combinedBlock, row)) = ublas::zero_vector<double>(combinedBlock.cols());
}
} else {
combinedBlock.row(row).setZero();
// ublas::noalias(ublas::row(combinedBlock, row)) = ublas::zero_vector<double>(combinedBlock.cols());
}
}
/* ************************************************************************* */
@ -615,7 +615,7 @@ namespace gtsam {
Index i = 0;
BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) {
for(JacobianFactor::const_iterator j = factor->begin(); j != factor->end(); ++j) {
r[i] += prod(factor->getA(j), x[*j]);
r[i] += factor->getA(j) * x[*j];
}
++i;
}
@ -627,7 +627,7 @@ namespace gtsam {
Index i = 0;
BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) {
for(JacobianFactor::const_iterator j = factor->begin(); j != factor->end(); ++j) {
x[*j] += prod(trans(factor->getA(j)), r[i]);
x[*j] += factor->getA(j).transpose() * r[i];
}
++i;
}

33
gtsam/linear/JacobianFactor.h
View File

@ -42,10 +42,11 @@ namespace gtsam {
template<class C> class BayesNet;
class JacobianFactor : public GaussianFactor {
public:
typedef MatrixColMajor AbMatrix;
typedef VerticalBlockView<AbMatrix> BlockAb;
protected:
typedef MatrixColMajor AbMatrix;
typedef VerticalBlockView<AbMatrix> BlockAb;
SharedDiagonal model_; // Gaussian noise model with diagonal covariance matrix
std::vector<size_t> firstNonzeroBlocks_;
@ -70,6 +71,8 @@ namespace gtsam {
/** Construct Null factor */
JacobianFactor(const Vector& b_in);
// FIXME: make these constructors use other matrix types - column major and blocks
/** Construct unary factor */
JacobianFactor(Index i1, const Matrix& A1,
const Vector& b, const SharedDiagonal& model);
@ -111,7 +114,7 @@ namespace gtsam {
* not necessarily mean that the factor involves no variables (to check for
* involving no variables use keys().empty()).
*/
bool empty() const { return Ab_.size1() == 0;}
bool empty() const { return Ab_.rows() == 0;}
/** is noise model constrained ? */
bool isConstrained() const { return model_->isConstrained();}
@ -119,7 +122,7 @@ namespace gtsam {
/** 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()).size2(); }
virtual size_t getDim(const_iterator variable) const { return Ab_(variable - begin()).cols(); }
/**
* Permutes the GaussianFactor, but for efficiency requires the permutation
@ -131,17 +134,17 @@ namespace gtsam {
/**
* return the number of columns in the corresponding linear system
*/
size_t size1() const { return Ab_.size1(); }
size_t rows() const { return Ab_.rows(); }
/**
* return the number of rows in the corresponding linear system
*/
size_t numberOfRows() const { return size1(); }
size_t numberOfRows() const { return rows(); }
/**
* return the number of columns in the corresponding linear system
*/
size_t size2() const { return Ab_.size2(); }
size_t cols() const { return Ab_.cols(); }
/** get a copy of model */
const SharedDiagonal& get_model() const { return model_; }
@ -150,12 +153,12 @@ namespace gtsam {
SharedDiagonal& get_model() { return model_; }
/** Get a view of the r.h.s. vector b */
constBVector getb() const { return Ab_.column(size(), 0); }
inline const constBVector getb() const { return Ab_.column(size(), 0); }
/** Get a view of the A matrix for the variable pointed to be the given key iterator */
constABlock getA(const_iterator variable) const { return Ab_(variable - begin()); }
BVector getb() { return Ab_.column(size(), 0); }
inline BVector getb() { return Ab_.column(size(), 0); }
ABlock getA(iterator variable) { return Ab_(variable - begin()); }
@ -180,6 +183,18 @@ namespace gtsam {
*/
Matrix matrix_augmented(bool weight = true) const;
/**
* Returns full dense matrix underying factor
* TESTING ONLY
*/
const AbMatrix& raw_matrix() const { return matrix_; }
/**
* Returns the block indexing view
* TESTING ONLY
*/
const BlockAb& Ab() const { return Ab_; }
/**
* 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.

8
gtsam/linear/Makefile.am
View File

@ -68,10 +68,6 @@ LDADD = liblinear.la ../inference/libinference.la ../base/libbase.la
LDADD += ../../CppUnitLite/libCppUnitLite.a
AM_DEFAULT_SOURCE_EXT = .cpp
if USE_ACCELERATE_MACOS
AM_LDFLAGS += -Wl,/System/Library/Frameworks/Accelerate.framework/Accelerate
endif
# rule to run an executable
%.run: % $(LDADD)
./$^
@ -80,7 +76,3 @@ endif
%.valgrind: % $(LDADD)
valgrind ./$^
if USE_LAPACK
AM_CXXFLAGS += -DGT_USE_LAPACK
endif

213
gtsam/linear/NoiseModel.cpp
View File

@ -22,8 +22,6 @@
#include <typeinfo>
#include <stdexcept>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/foreach.hpp>
#include <boost/random/linear_congruential.hpp>
#include <boost/random/normal_distribution.hpp>
@ -33,10 +31,6 @@
#include <gtsam/base/cholesky.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/linear/SharedDiagonal.h>
#include <gtsam/base/DenseQRUtil.h>
namespace ublas = boost::numeric::ublas;
typedef ublas::matrix_column<Matrix> column;
static double inf = std::numeric_limits<double>::infinity();
using namespace std;
@ -59,23 +53,25 @@ namespace noiseModel {
// Linear algebra: takes away projection on latest orthogonal
// Graph: make a new factor on the separator S
// __attribute__ ((noinline)) // uncomment to prevent inlining when profiling
static void updateAb(Matrix& Ab, int j, const Vector& a, const Vector& rd) {
size_t m = Ab.size1(), n = Ab.size2()-1;
for (size_t i = 0; i < m; i++) { // update all rows
double ai = a(i);
double *Aij = Ab.data().begin() + i * (n+1) + j + 1;
const double *rptr = rd.data().begin() + j + 1;
// Ab(i,j+1:end) -= ai*rd(j+1:end)
for (size_t j2 = j + 1; j2 < n+1; j2++, Aij++, rptr++)
*Aij -= ai * (*rptr);
}
template<class MATRIX>
void updateAb(MATRIX& Ab, int j, const Vector& a, const Vector& rd) {
size_t n = Ab.cols()-1;
Ab.middleCols(j+1,n-j) -= a * rd.segment(j+1, n-j).transpose();
// size_t m = Ab.rows(), n = Ab.cols()-1;
// for (size_t i = 0; i < m; i++) { // update all rows
// double ai = a(i);
// double *Aij = Ab.data() + i * (n+1) + j + 1;
// const double *rptr = rd.data() + j + 1;
// // Ab(i,j+1:end) -= ai*rd(j+1:end)
// for (size_t j2 = j + 1; j2 < n+1; j2++, Aij++, rptr++)
// *Aij -= ai * (*rptr);
// }
}
/* ************************************************************************* */
Gaussian::shared_ptr Gaussian::Covariance(const Matrix& covariance, bool smart) {
size_t m = covariance.size1(), n = covariance.size2();
size_t m = covariance.rows(), n = covariance.cols();
if (m != n) throw invalid_argument("Gaussian::Covariance: covariance not square");
if (smart) {
// check all non-diagonal entries
@ -118,7 +114,7 @@ Vector Gaussian::unwhiten(const Vector& v) const {
double Gaussian::Mahalanobis(const Vector& v) const {
// Note: for Diagonal, which does ediv_, will be correct for constraints
Vector w = whiten(v);
return inner_prod(w, w);
return w.dot(w);
}
/* ************************************************************************* */
@ -133,18 +129,18 @@ void Gaussian::WhitenInPlace(Matrix& H) const {
/* ************************************************************************* */
void Gaussian::WhitenInPlace(MatrixColMajor& H) const {
H = ublas::prod(thisR(), H);
H = thisR() * H;
}
/* ************************************************************************* */
// General QR, see also special version in Constrained
SharedDiagonal Gaussian::QR(Matrix& Ab, boost::optional<vector<int>&> firstZeroRows) const {
SharedDiagonal Gaussian::QR(Matrix& Ab) const {
static const bool debug = false;
// get size(A) and maxRank
// TODO: really no rank problems ?
size_t m = Ab.size1(), n = Ab.size2()-1;
size_t m = Ab.rows(), n = Ab.cols()-1;
size_t maxRank = min(m,n);
// pre-whiten everything (cheaply if possible)
@ -152,126 +148,43 @@ SharedDiagonal Gaussian::QR(Matrix& Ab, boost::optional<vector<int>&> firstZeroR
if(debug) gtsam::print(Ab, "Whitened Ab: ");
// Perform in-place Householder
#ifdef GT_USE_LAPACK
if(firstZeroRows)
householder_denseqr(Ab, &(*firstZeroRows)[0]);
else
householder_denseqr(Ab);
#else
householder(Ab, maxRank);
#endif
// Eigen QR - much faster than older householder approach
inplace_QR(Ab, false);
// hand-coded householder implementation
// TODO: necessary to isolate last column?
// householder(Ab, maxRank);
return Unit::Create(maxRank);
}
// Special version of QR for Constrained calls slower but smarter code
// that deals with possibly zero sigmas
// It is Gram-Schmidt orthogonalization rather than Householder
// Previously Diagonal::QR
/*SharedDiagonal Gaussian::QR(Matrix& Ab, boost::optional<std::vector<long>&> firstZeroRows) const {
WhitenInPlace(Ab);
// get size(A) and maxRank
size_t m = Ab.size1(), n = Ab.size2()-1;
size_t maxRank = min(m,n);
// create storage for [R d]
typedef boost::tuple<size_t, Vector, double> Triple;
list<Triple> Rd;
Vector pseudo(m); // allocate storage for pseudo-inverse
Vector weights = ones(m); // 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
// scalar variable x as d-rS, with S the separator (remaining columns).
// Then update A and b by substituting x with d-rS, zero-ing out x's column.
for (size_t j=0; j<n; ++j) {
// extract the first column of A
// ublas::matrix_column is slower ! TODO Really, why ????
// AGC: if you use column() you will automatically call ublas, use
// column_() to actually use the one in our library
Vector a(column(Ab, j));
// Calculate weighted pseudo-inverse and corresponding precision
double precision = weightedPseudoinverse(a, weights, pseudo);
// If precision is zero, no information on this column
// This is actually not limited to constraints, could happen in Gaussian::QR
// In that case, we're probably hosed. TODO: make sure Householder is rank-revealing
if (precision < 1e-8) continue;
// create solution [r d], rhs is automatically r(n)
Vector rd(n+1); // uninitialized !
rd(j)=1.0; // put 1 on diagonal
for (size_t j2=j+1; j2<n+1; ++j2) // and fill in remainder with dot-products
rd(j2) = inner_prod(pseudo, ublas::matrix_column<Matrix>(Ab, j2));
// construct solution (r, d, sigma)
Rd.push_back(boost::make_tuple(j, rd, precision));
// exit after rank exhausted
if (Rd.size()>=maxRank) break;
// update Ab, expensive, using outer product
updateAb(Ab, j, a, rd);
}
// Create storage for precisions
Vector precisions(Rd.size());
// Write back result in Ab, imperative as we are
// TODO: test that is correct if a column was skipped !!!!
size_t i = 0; // start with first row
bool mixed = false;
BOOST_FOREACH(const Triple& t, Rd) {
const size_t& j = t.get<0>();
const Vector& rd = t.get<1>();
precisions(i) = t.get<2>();
if (precisions(i)==inf) mixed = true;
for (size_t j2=0; j2<j; ++j2) Ab(i,j2) = 0.0; // fill in zeros below diagonal anway
for (size_t j2=j; j2<n+1; ++j2) // copy the j-the row TODO memcpy
Ab(i,j2) = rd(j2);
i+=1;
}
return Unit::Create(maxRank);
}*/
/* ************************************************************************* */
// General QR, see also special version in Constrained
SharedDiagonal Gaussian::QRColumnWise(MatrixColMajor& Ab, vector<int>& firstZeroRows) const {
SharedDiagonal Gaussian::QR(MatrixColMajor& Ab) const {
static const bool debug = false;
// get size(A) and maxRank
// TODO: really no rank problems ?
size_t m = Ab.size1(), n = Ab.size2()-1;
size_t m = Ab.rows(), n = Ab.cols()-1;
size_t maxRank = min(m,n);
// pre-whiten everything (cheaply if possible)
WhitenInPlace(Ab);
if(debug) gtsam::print(Ab, "Whitened Ab: ");
if(debug) gtsam::print(Matrix(Ab), "Whitened Ab: ");
// Perform in-place Householder
#ifdef GT_USE_LAPACK
#ifdef USE_LAPACK_QR
householderColMajor(Ab);
#elif USE_DAVIS_QR
#else
householder_denseqr_colmajor(Ab, &firstZeroRows[0]);
#endif
#else
Matrix Ab_rowWise = Ab;
householder(Ab_rowWise, maxRank);
Ab = Ab_rowWise; // FIXME: this is a really silly way of doing this
#endif
// Eigen QR - much faster than older householder approach
inplace_QR(Ab, false);
// hand-coded householder implementation
// TODO: necessary to isolate last column?
// householder(Ab, maxRank);
return Unit::Create(maxRank);
}
/* ************************************************************************* */
SharedDiagonal Gaussian::Cholesky(MatrixColMajor& Ab, size_t nFrontals) const {
// get size(A) and maxRank
@ -284,7 +197,7 @@ SharedDiagonal Gaussian::Cholesky(MatrixColMajor& Ab, size_t nFrontals) const {
// Form A'*A (todo: this is probably less efficient than possible)
tic("Cholesky: 2 A' * A");
Ab = ublas::prod(ublas::trans(Ab), Ab);
Ab = Ab.transpose() * Ab;
toc("Cholesky: 2 A' * A");
// Use Cholesky to factor Ab
@ -295,7 +208,7 @@ SharedDiagonal Gaussian::Cholesky(MatrixColMajor& Ab, size_t nFrontals) const {
// Due to numerical error the rank could appear to be more than the number
// of variables. The important part is that it does not includes the
// augmented b column.
if(maxrank == Ab.size2())
if(maxrank == (size_t) Ab.cols())
-- maxrank;
return Unit::Create(maxrank);
@ -332,7 +245,7 @@ Diagonal::shared_ptr Diagonal::Variances(const Vector& variances, bool smart) {
Diagonal::shared_ptr Diagonal::Sigmas(const Vector& sigmas, bool smart) {
if (smart) {
// look for zeros to make a constraint
for (size_t i=0; i<sigmas.size(); ++i)
for (size_t i=0; i< (size_t) sigmas.size(); ++i)
if (sigmas(i)<1e-8)
return Constrained::MixedSigmas(sigmas);
}
@ -422,16 +335,15 @@ void Constrained::WhitenInPlace(MatrixColMajor& H) const {
}
/* ************************************************************************* */
// Special version of QR for Constrained calls slower but smarter code
// that deals with possibly zero sigmas
// It is Gram-Schmidt orthogonalization rather than Householder
// Previously Diagonal::QR
SharedDiagonal Constrained::QR(Matrix& Ab, boost::optional<std::vector<int>&> firstZeroRows) const {
// templated generic constrained QR
template<class MATRIX>
SharedDiagonal constrained_QR(MATRIX& Ab, const Vector& sigmas) {
bool verbose = false;
if (verbose) cout << "\nStarting Constrained::QR" << endl;
// get size(A) and maxRank
size_t m = Ab.size1(), n = Ab.size2()-1;
size_t m = Ab.rows(), n = Ab.cols()-1;
size_t maxRank = min(m,n);
// create storage for [R d]
@ -439,7 +351,7 @@ SharedDiagonal Constrained::QR(Matrix& Ab, boost::optional<std::vector<int>&> fi
list<Triple> Rd;
Vector pseudo(m); // allocate storage for pseudo-inverse
Vector invsigmas = reciprocal(sigmas_);
Vector invsigmas = reciprocal(sigmas);
Vector weights = emul(invsigmas,invsigmas); // calculate weights once
// We loop over all columns, because the columns that can be eliminated
@ -448,24 +360,25 @@ SharedDiagonal Constrained::QR(Matrix& Ab, boost::optional<std::vector<int>&> fi
// Then update A and b by substituting x with d-rS, zero-ing out x's column.
for (size_t j=0; j<n; ++j) {
// extract the first column of A
// ublas::matrix_column is slower ! TODO Really, why ????
// AGC: if you use column() you will automatically call ublas, use
// column_() to actually use the one in our library
Vector a(column(Ab, j));
Vector a = Ab.col(j);
// Calculate weighted pseudo-inverse and corresponding precision
tic(1, "constrained_QR weightedPseudoinverse");
double precision = weightedPseudoinverse(a, weights, pseudo);
toc(1, "constrained_QR weightedPseudoinverse");
// If precision is zero, no information on this column
// This is actually not limited to constraints, could happen in Gaussian::QR
// In that case, we're probably hosed. TODO: make sure Householder is rank-revealing
if (precision < 1e-8) continue;
tic(2, "constrained_QR create rd");
// create solution [r d], rhs is automatically r(n)
Vector rd(n+1); // uninitialized !
rd(j)=1.0; // put 1 on diagonal
for (size_t j2=j+1; j2<n+1; ++j2) // and fill in remainder with dot-products
rd(j2) = inner_prod(pseudo, ublas::matrix_column<Matrix>(Ab, j2));
rd(j2) = pseudo.dot(Ab.col(j2));
toc(2, "constrained_QR create rd");
// construct solution (r, d, sigma)
Rd.push_back(boost::make_tuple(j, rd, precision));
@ -474,12 +387,16 @@ SharedDiagonal Constrained::QR(Matrix& Ab, boost::optional<std::vector<int>&> fi
if (Rd.size()>=maxRank) break;
// update Ab, expensive, using outer product
updateAb(Ab, j, a, rd);
tic(3, "constrained_QR update Ab");
Ab.middleCols(j+1,n-j) -= a * rd.segment(j+1, n-j).transpose();
toc(3, "constrained_QR update Ab");
// updateAb(Ab, j, a, rd);
}
// Create storage for precisions
Vector precisions(Rd.size());
tic(4, "constrained_QR write back into Ab");
// Write back result in Ab, imperative as we are
// TODO: test that is correct if a column was skipped !!!!
size_t i = 0; // start with first row
@ -494,16 +411,23 @@ SharedDiagonal Constrained::QR(Matrix& Ab, boost::optional<std::vector<int>&> fi
Ab(i,j2) = rd(j2);
i+=1;
}
toc(4, "constrained_QR write back into Ab");
return mixed ? Constrained::MixedPrecisions(precisions) : Diagonal::Precisions(precisions);
}
/* ************************************************************************* */
SharedDiagonal Constrained::QRColumnWise(ublas::matrix<double, ublas::column_major>& Ab, vector<int>& firstZeroRows) const {
Matrix AbRowWise(Ab);
SharedDiagonal result = this->QR(AbRowWise, firstZeroRows);
Ab = AbRowWise;
return result;
// Special version of QR for Constrained calls slower but smarter code
// that deals with possibly zero sigmas
// It is Gram-Schmidt orthogonalization rather than Householder
// Previously Diagonal::QR
SharedDiagonal Constrained::QR(Matrix& Ab) const {
return constrained_QR(Ab, sigmas_);
}
/* ************************************************************************* */
SharedDiagonal Constrained::QR(MatrixColMajor& Ab) const {
return constrained_QR(Ab, sigmas_);
}
/* ************************************************************************* */
@ -521,8 +445,7 @@ void Isotropic::print(const string& name) const {
/* ************************************************************************* */
double Isotropic::Mahalanobis(const Vector& v) const {
double dot = inner_prod(v, v);
return dot * invsigma_ * invsigma_;
return v.dot(v) * invsigma_ * invsigma_;
}
/* ************************************************************************* */

25
gtsam/linear/NoiseModel.h
View File

@ -20,6 +20,7 @@
#pragma once
#include <boost/shared_ptr.hpp>
#include <boost/serialization/nvp.hpp>
#include <gtsam/base/Testable.h>
#include <gtsam/base/Vector.h>
#include <gtsam/base/Matrix.h>
@ -135,7 +136,7 @@ namespace gtsam {
* @param smart, check if can be simplified to derived class
*/
static shared_ptr SqrtInformation(const Matrix& R) {
return shared_ptr(new Gaussian(R.size1(),R));
return shared_ptr(new Gaussian(R.rows(),R));
}
/**
@ -181,12 +182,11 @@ namespace gtsam {
* @param Ab is the m*(n+1) augmented system matrix [A b]
* @return in-place QR factorization [R d]. Below-diagonal is undefined !!!!!
*/
virtual SharedDiagonal QR(Matrix& Ab, boost::optional<std::vector<int>&> firstZeroRows = boost::none) const;
/**
* Version for column-wise matrices
*/
virtual SharedDiagonal QRColumnWise(MatrixColMajor& Ab, std::vector<int>& firstZeroRows) const;
virtual SharedDiagonal QR(Matrix& Ab) const;
virtual SharedDiagonal QR(MatrixColMajor& Ab) const;
// FIXME: these previously had firstZeroRows - what did this do?
// virtual SharedDiagonal QRColumnWise(MatrixColMajor& Ab, std::vector<int>& firstZeroRows) const;
// virtual SharedDiagonal QR(Matrix& Ab, boost::optional<std::vector<int>&> firstZeroRows = boost::none) const;
/**
* Cholesky factorization
@ -374,13 +374,8 @@ namespace gtsam {
/**
* Apply QR factorization to the system [A b], taking into account constraints
*/
virtual SharedDiagonal QR(Matrix& Ab, boost::optional<std::vector<int>&> firstZeroRows = boost::none) const;
/**
* Version for column-wise matrices
*/
virtual SharedDiagonal QRColumnWise(boost::numeric::ublas::matrix<double, boost::numeric::ublas::column_major>& Ab,
std::vector<int>& firstZeroRows) const;
virtual SharedDiagonal QR(Matrix& Ab) const;
virtual SharedDiagonal QR(MatrixColMajor& Ab) const;
/**
* Check constrained is always true
@ -490,7 +485,7 @@ namespace gtsam {
}
virtual void print(const std::string& name) const;
virtual double Mahalanobis(const Vector& v) const {return inner_prod(v,v); }
virtual double Mahalanobis(const Vector& v) const {return v.dot(v); }
virtual Vector whiten(const Vector& v) const { return v; }
virtual Vector unwhiten(const Vector& v) const { return v; }
virtual Matrix Whiten(const Matrix& H) const { return H; }

64
gtsam/linear/VectorValues.h
View File

@ -11,7 +11,7 @@
/**
* @file VectorValues.h
* @brief Factor Graph Valuesuration
* @brief Factor Graph Values
* @author Richard Roberts
*/
@ -22,8 +22,6 @@
#include <gtsam/base/types.h>
#include <boost/foreach.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/shared_ptr.hpp>
#include <vector>
@ -40,11 +38,10 @@ public:
typedef boost::shared_ptr<VectorValues> shared_ptr;
typedef _impl_iterator<VectorValues> iterator;
typedef _impl_iterator<const VectorValues> const_iterator;
typedef boost::numeric::ublas::vector_range<Vector> value_reference_type;
typedef boost::numeric::ublas::vector_range<const Vector> const_value_reference_type;
typedef boost::numeric::ublas::vector_range<Vector> mapped_type;
typedef boost::numeric::ublas::vector_range<const Vector> const_mapped_type;
typedef SubVector value_reference_type;
typedef ConstSubVector const_value_reference_type;
typedef SubVector mapped_type;
typedef ConstSubVector const_mapped_type;
/**
* Default constructor creates an empty VectorValues. reserve(...) must be
@ -105,7 +102,7 @@ public:
Vector& vector() { return values_; }
/** Reserve space for a total number of variables and dimensionality */
void reserve(Index nVars, size_t totalDims) { values_.resize(std::max(totalDims, values_.size())); varStarts_.reserve(nVars+1); }
void reserve(Index nVars, size_t totalDims) { values_.resize(totalDims); varStarts_.reserve(nVars+1); }
/**
* Append a variable using the next variable ID, and return that ID. Space
@ -119,7 +116,7 @@ public:
}
/** Set all elements to zero */
void makeZero() { boost::numeric::ublas::noalias(values_) = boost::numeric::ublas::zero_vector<double>(values_.size()); }
void makeZero() { values_.setZero(); }
/** print required by Testable for unit testing */
void print(const std::string& str = "VectorValues: ") const {
@ -131,13 +128,8 @@ public:
}
/** equals required by Testable for unit testing */
bool equals(const VectorValues& expected, double tol=1e-9) const {
if(size() != expected.size()) return false;
// iterate over all elements
for(Index var=0; var<size(); ++var)
if(!equal_with_abs_tol(expected[var],operator[](var),tol))
return false;
return true;
bool equals(const VectorValues& x, double tol=1e-9) const {
return varStarts_ == x.varStarts_ && equal_with_abs_tol(values_, x.values_, tol);
}
/** + operator simply adds Vectors. This checks for structural equivalence
@ -147,15 +139,17 @@ public:
assert(varStarts_ == c.varStarts_);
VectorValues result;
result.varStarts_ = varStarts_;
result.values_ = boost::numeric::ublas::project(values_, boost::numeric::ublas::range(0, varStarts_.back())) +
boost::numeric::ublas::project(c.values_, boost::numeric::ublas::range(0, c.varStarts_.back()));
result.values_ = values_.head(varStarts_.back()) + c.values_.head(varStarts_.back());
// result.values_ = boost::numeric::ublas::project(values_, boost::numeric::ublas::range(0, varStarts_.back())) +
// boost::numeric::ublas::project(c.values_, boost::numeric::ublas::range(0, c.varStarts_.back()));
return result;
}
void operator+=(const VectorValues& c) {
assert(varStarts_ == c.varStarts_);
this->values_ = boost::numeric::ublas::project(this->values_, boost::numeric::ublas::range(0, varStarts_.back())) +
boost::numeric::ublas::project(c.values_, boost::numeric::ublas::range(0, c.varStarts_.back()));
this->values_ += c.values_.head(varStarts_.back());
// this->values_ = boost::numeric::ublas::project(this->values_, boost::numeric::ublas::range(0, varStarts_.back())) +
// boost::numeric::ublas::project(c.values_, boost::numeric::ublas::range(0, c.varStarts_.back()));
}
@ -219,17 +213,21 @@ public:
// check whether there's a zero in the vector
friend bool anyZero(const VectorValues& x, double tol=1e-5) {
bool flag = false ;
BOOST_FOREACH(const double &v, x.values_) {
if ( v < tol && v > -tol) {
size_t i=0;
for (const double *v = x.values_.data(); i< (size_t) x.values_.size(); ++v) {
// BOOST_FOREACH(const double &v, x.values_) {
// double v = x(i);
if ( *v < tol && *v > -tol) {
flag = true ;
break;
}
++i;
}
return flag;
}
};
/// Implementations of functions
template<class CONTAINER>
inline VectorValues::VectorValues(const CONTAINER& dimensions) : varStarts_(dimensions.size()+1) {
@ -239,7 +237,7 @@ inline VectorValues::VectorValues(const CONTAINER& dimensions) : varStarts_(dime
BOOST_FOREACH(size_t dim, dimensions) {
varStarts_[++var] = (varStart += dim);
}
values_.resize(varStarts_.back(), false);
values_.resize(varStarts_.back());
}
inline VectorValues::VectorValues(Index nVars, size_t varDim) : varStarts_(nVars+1) {
@ -247,7 +245,7 @@ inline VectorValues::VectorValues(Index nVars, size_t varDim) : varStarts_(nVars
size_t varStart = 0;
for(Index j=1; j<=nVars; ++j)
varStarts_[j] = (varStart += varDim);
values_.resize(varStarts_.back(), false);
values_.resize(varStarts_.back());
}
inline VectorValues::VectorValues(const std::vector<size_t>& dimensions, const Vector& values) :
@ -258,7 +256,7 @@ inline VectorValues::VectorValues(const std::vector<size_t>& dimensions, const V
BOOST_FOREACH(size_t dim, dimensions) {
varStarts_[++var] = (varStart += dim);
}
assert(varStarts_.back() == values.size());
assert(varStarts_.back() == (size_t) values.size());
}
inline VectorValues::VectorValues(const std::vector<size_t>& dimensions, const double* values) :
@ -281,14 +279,18 @@ inline VectorValues VectorValues::SameStructure(const VectorValues& otherValues)
inline VectorValues::mapped_type VectorValues::operator[](Index variable) {
checkVariable(variable);
return boost::numeric::ublas::project(values_,
boost::numeric::ublas::range(varStarts_[variable], varStarts_[variable+1]));
const size_t start = varStarts_[variable], n = varStarts_[variable+1] - start;
return values_.segment(start, n);
// return boost::numeric::ublas::project(values_,
// boost::numeric::ublas::range(varStarts_[variable], varStarts_[variable+1]));
}
inline VectorValues::const_mapped_type VectorValues::operator[](Index variable) const {
checkVariable(variable);
return boost::numeric::ublas::project(values_,
boost::numeric::ublas::range(varStarts_[variable], varStarts_[variable+1]));
const size_t start = varStarts_[variable], n = varStarts_[variable+1] - start;
return values_.segment(start, n);
// return boost::numeric::ublas::project(values_,
// boost::numeric::ublas::range(varStarts_[variable], varStarts_[variable+1]));
}
struct DimSpec: public std::vector<size_t> {

Some files were not shown because too many files have changed in this diff Show More